MifareClassic.cpp

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