MifareClassic.cpp

00001 #include "MifareClassic.h"
00002 #include "PN532_debug.h"
00003 
00004 #include <string.h>
00005 
00006 #define BLOCK_SIZE 16
00007 #define LONG_TLV_SIZE 4
00008 #define SHORT_TLV_SIZE 2
00009 
00010 #define MIFARE_CLASSIC ("Mifare Classic")
00011 
00012 MifareClassic::MifareClassic(PN532& nfcShield)
00013 {
00014   _nfcShield = &nfcShield;
00015 }
00016 
00017 MifareClassic::~MifareClassic()
00018 {
00019 }
00020 
00021 NfcTag MifareClassic::read(uint8_t *uid, unsigned int uidLength)
00022 {
00023     uint8_t key[6] = { 0xD3, 0xF7, 0xD3, 0xF7, 0xD3, 0xF7 };
00024     int currentBlock = 4;
00025     int messageStartIndex = 0;
00026     int messageLength = 0;
00027     uint8_t data[BLOCK_SIZE];
00028 
00029     // read first block to get message length
00030     int success = _nfcShield->mifareclassic_AuthenticateBlock (uid, uidLength, currentBlock, 0, key);
00031     if (success)
00032     {
00033         success = _nfcShield->mifareclassic_ReadDataBlock(currentBlock, data);
00034         if (success)
00035         {
00036             if (!decodeTlv(data, messageLength, messageStartIndex)) {
00037                 return NfcTag(uid, uidLength, "ERROR"); // TODO should the error message go in NfcTag?
00038             }
00039         }
00040         else
00041         {
00042             DMSG("Error. Failed read block ");
00043             DMSG_INT(currentBlock);
00044             return NfcTag(uid, uidLength, MIFARE_CLASSIC);
00045         }
00046     }
00047     else
00048     {
00049         DMSG("Tag is not NDEF formatted.");
00050         // TODO set tag.isFormatted = false
00051         return NfcTag(uid, uidLength, MIFARE_CLASSIC);
00052     }
00053 
00054     // this should be nested in the message length loop
00055     int index = 0;
00056     int bufferSize = getBufferSize(messageLength);
00057     uint8_t buffer[bufferSize];
00058 
00059     #ifdef MIFARE_CLASSIC_DEBUG
00060     DMSG("Message Length ");
00061     DMSG_INT(messageLength);
00062     DMSG("Buffer Size ");
00063     DMSG_INT(bufferSize);
00064     #endif
00065 
00066     while (index < bufferSize)
00067     {
00068 
00069         // authenticate on every sector
00070         if (_nfcShield->mifareclassic_IsFirstBlock(currentBlock))
00071         {
00072             success = _nfcShield->mifareclassic_AuthenticateBlock(uid, uidLength, currentBlock, 0, key);
00073             if (!success)
00074             {
00075                 DMSG("Error. Block Authentication failed for ");
00076                 DMSG_INT(currentBlock);
00077                 // TODO error handling
00078             }
00079         }
00080 
00081         // read the data
00082         success = _nfcShield->mifareclassic_ReadDataBlock(currentBlock, &buffer[index]);
00083         if (success)
00084         {
00085             #ifdef MIFARE_CLASSIC_DEBUG
00086             DMSG("Block ");
00087             DMSG_INT(currentBlock);
00088             _nfcShield->PrintHexChar(&buffer[index], BLOCK_SIZE);
00089             #endif
00090         }
00091         else
00092         {
00093             DMSG("Read failed ");
00094             DMSG_INT(currentBlock);
00095             // TODO handle errors here
00096         }
00097 
00098         index += BLOCK_SIZE;
00099         currentBlock++;
00100 
00101         // skip the trailer block
00102         if (_nfcShield->mifareclassic_IsTrailerBlock(currentBlock))
00103         {
00104             #ifdef MIFARE_CLASSIC_DEBUG
00105             DMSG("Skipping block ");
00106             DMSG_INT(currentBlock);
00107             #endif
00108             currentBlock++;
00109         }
00110     }
00111 
00112     return NfcTag(uid, uidLength, MIFARE_CLASSIC, &buffer[messageStartIndex], messageLength);
00113 }
00114 
00115 int MifareClassic::getBufferSize(int messageLength)
00116 {
00117 
00118     int bufferSize = messageLength;
00119 
00120     // TLV header is 2 or 4 uint8_ts, TLV terminator is 1 uint8_t.
00121     if (messageLength < 0xFF)
00122     {
00123         bufferSize += SHORT_TLV_SIZE + 1;
00124     }
00125     else
00126     {
00127         bufferSize += LONG_TLV_SIZE + 1;
00128     }
00129 
00130     // bufferSize needs to be a multiple of BLOCK_SIZE
00131     if (bufferSize % BLOCK_SIZE != 0)
00132     {
00133         bufferSize = ((bufferSize / BLOCK_SIZE) + 1) * BLOCK_SIZE;
00134     }
00135 
00136     return bufferSize;
00137 }
00138 
00139 // skip null tlvs (0x0) before the real message
00140 // technically unlimited null tlvs, but we assume
00141 // T & L of TLV in the first block we read
00142 int MifareClassic::getNdefStartIndex(uint8_t *data)
00143 {
00144 
00145     for (int i = 0; i < BLOCK_SIZE; i++)
00146     {
00147         if (data[i] == 0x0)
00148         {
00149             // do nothing, skip
00150         }
00151         else if (data[i] == 0x3)
00152         {
00153             return i;
00154         }
00155         else
00156         {
00157             DMSG("Unknown TLV ");
00158             DMSG_HEX(data[i]);
00159             return -2;
00160         }
00161     }
00162 
00163     return -1;
00164 }
00165 
00166 // Decode the NDEF data length from the Mifare TLV
00167 // Leading null TLVs (0x0) are skipped
00168 // Assuming T & L of TLV will be in the first block
00169 // messageLength and messageStartIndex written to the parameters
00170 // success or failure status is returned
00171 //
00172 // { 0x3, LENGTH }
00173 // { 0x3, 0xFF, LENGTH, LENGTH }
00174 bool MifareClassic::decodeTlv(uint8_t *data, int &messageLength, int &messageStartIndex)
00175 {
00176     int i = getNdefStartIndex(data);
00177 
00178     if (i < 0 || data[i] != 0x3)
00179     {
00180         DMSG("Error. Can't decode message length.");
00181         return false;
00182     }
00183     else
00184     {
00185         if (data[i+1] == 0xFF)
00186         {
00187             messageLength = ((0xFF & data[i+2]) << 8) | (0xFF & data[i+3]);
00188             messageStartIndex = i + LONG_TLV_SIZE;
00189         }
00190         else
00191         {
00192             messageLength = data[i+1];
00193             messageStartIndex = i + SHORT_TLV_SIZE;
00194         }
00195     }
00196 
00197     return true;
00198 }
00199 
00200 bool MifareClassic::write(NdefMessage& m, uint8_t * uid, unsigned int uidLength)
00201 {
00202 
00203     uint8_t encoded[m.getEncodedSize()];
00204     m.encode(encoded);
00205 
00206     uint8_t buffer[getBufferSize(sizeof(encoded))];
00207     memset(buffer, 0, sizeof(buffer));
00208 
00209     #ifdef MIFARE_CLASSIC_DEBUG
00210     DMSG("sizeof(encoded) "));DMSG(sizeof(encoded);
00211     DMSG("sizeof(buffer) "));DMSG(sizeof(buffer);
00212     #endif
00213 
00214     if (sizeof(encoded) < 0xFF)
00215     {
00216         buffer[0] = 0x3;
00217         buffer[1] = sizeof(encoded);
00218         memcpy(&buffer[2], encoded, sizeof(encoded));
00219         buffer[2+sizeof(encoded)] = 0xFE; // terminator
00220     }
00221     else
00222     {
00223         buffer[0] = 0x3;
00224         buffer[1] = 0xFF;
00225         buffer[2] = ((sizeof(encoded) >> 8) & 0xFF);
00226         buffer[3] = (sizeof(encoded) & 0xFF);
00227         memcpy(&buffer[4], encoded, sizeof(encoded));
00228         buffer[4+sizeof(encoded)] = 0xFE; // terminator
00229     }
00230 
00231     // Write to tag
00232     int index = 0;
00233     int currentBlock = 4;
00234     uint8_t key[6] = { 0xD3, 0xF7, 0xD3, 0xF7, 0xD3, 0xF7 }; // this is Sector 1 - 15 key
00235 
00236     while (index < sizeof(buffer))
00237     {
00238 
00239         if (_nfcShield->mifareclassic_IsFirstBlock(currentBlock))
00240         {
00241             int success = _nfcShield->mifareclassic_AuthenticateBlock(uid, uidLength, currentBlock, 0, key);
00242             if (!success)
00243             {
00244                 DMSG("Error. Block Authentication failed for ");DMSG_INT(currentBlock);
00245                 return false;
00246             }
00247         }
00248         int write_success = _nfcShield->mifareclassic_WriteDataBlock (currentBlock, &buffer[index]);
00249         if (write_success)
00250         {
00251             #ifdef MIFARE_CLASSIC_DEBUG
00252             DMSG("Wrote block ");Serial.print(currentBlock);DMSG(" - ");
00253             _nfcShield->PrintHexChar(&buffer[index], BLOCK_SIZE);
00254             #endif
00255         }
00256         else
00257         {
00258             DMSG("Write failed ");DMSG_INT(currentBlock);
00259             return false;
00260         }
00261         index += BLOCK_SIZE;
00262         currentBlock++;
00263 
00264         if (_nfcShield->mifareclassic_IsTrailerBlock(currentBlock))
00265         {
00266             // can't write to trailer block
00267             #ifdef MIFARE_CLASSIC_DEBUG
00268             DMSG("Skipping block ");DMSG(currentBlock);
00269             #endif
00270             currentBlock++;
00271         }
00272 
00273     }
00274 
00275     return true;
00276 }