MifareClassic.cpp

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