PN532 NFC library for Seeed Studio's NFC Shield
Fork of PN532 by
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Tue Jul 12 2022 17:32:26 by 1.7.2