Nico De Witte / PN532

PN532 NFC shield of Adafruit based on PN532 of Seeed.

Fork of PN532 by Seeed

MifareClassic.cpp@11:e7bbcfa61859, 2016-11-17 (annotated)

Committer:
dwini
Date:
Thu Nov 17 11:32:02 2016 +0000
Revision:
11:e7bbcfa61859
Parent:
10:59298dea711a 
Fix use of DMSG instead of printf

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yihui 3:4189a10038e6 1 #include "MifareClassic.h"
yihui 3:4189a10038e6 2 #include "PN532_debug.h"
yihui 3:4189a10038e6 3
dwini 10:59298dea711a 4 #include <string.h>
dwini 10:59298dea711a 5
yihui 3:4189a10038e6 6 #define BLOCK_SIZE 16
yihui 3:4189a10038e6 7 #define LONG_TLV_SIZE 4
yihui 3:4189a10038e6 8 #define SHORT_TLV_SIZE 2
yihui 3:4189a10038e6 9
yihui 3:4189a10038e6 10 #define MIFARE_CLASSIC ("Mifare Classic")
yihui 3:4189a10038e6 11
yihui 3:4189a10038e6 12 MifareClassic::MifareClassic(PN532& nfcShield)
yihui 3:4189a10038e6 13 {
yihui 3:4189a10038e6 14 _nfcShield = &nfcShield;
yihui 3:4189a10038e6 15 }
yihui 3:4189a10038e6 16
yihui 3:4189a10038e6 17 MifareClassic::~MifareClassic()
yihui 3:4189a10038e6 18 {
yihui 3:4189a10038e6 19 }
yihui 3:4189a10038e6 20
yihui 3:4189a10038e6 21 NfcTag MifareClassic::read(uint8_t *uid, unsigned int uidLength)
yihui 3:4189a10038e6 22 {
yihui 3:4189a10038e6 23 uint8_t key[6] = { 0xD3, 0xF7, 0xD3, 0xF7, 0xD3, 0xF7 };
yihui 3:4189a10038e6 24 int currentBlock = 4;
yihui 3:4189a10038e6 25 int messageStartIndex = 0;
yihui 3:4189a10038e6 26 int messageLength = 0;
yihui 3:4189a10038e6 27 uint8_t data[BLOCK_SIZE];
yihui 3:4189a10038e6 28
yihui 3:4189a10038e6 29 // read first block to get message length
yihui 3:4189a10038e6 30 int success = _nfcShield->mifareclassic_AuthenticateBlock(uid, uidLength, currentBlock, 0, key);
yihui 3:4189a10038e6 31 if (success)
yihui 3:4189a10038e6 32 {
yihui 3:4189a10038e6 33 success = _nfcShield->mifareclassic_ReadDataBlock(currentBlock, data);
yihui 3:4189a10038e6 34 if (success)
yihui 3:4189a10038e6 35 {
yihui 3:4189a10038e6 36 if (!decodeTlv(data, messageLength, messageStartIndex)) {
yihui 3:4189a10038e6 37 return NfcTag(uid, uidLength, "ERROR"); // TODO should the error message go in NfcTag?
yihui 3:4189a10038e6 38 }
yihui 3:4189a10038e6 39 }
yihui 3:4189a10038e6 40 else
yihui 3:4189a10038e6 41 {
yihui 3:4189a10038e6 42 DMSG("Error. Failed read block ");
yihui 3:4189a10038e6 43 DMSG_INT(currentBlock);
yihui 3:4189a10038e6 44 return NfcTag(uid, uidLength, MIFARE_CLASSIC);
yihui 3:4189a10038e6 45 }
yihui 3:4189a10038e6 46 }
yihui 3:4189a10038e6 47 else
yihui 3:4189a10038e6 48 {
yihui 3:4189a10038e6 49 DMSG("Tag is not NDEF formatted.");
yihui 3:4189a10038e6 50 // TODO set tag.isFormatted = false
yihui 3:4189a10038e6 51 return NfcTag(uid, uidLength, MIFARE_CLASSIC);
yihui 3:4189a10038e6 52 }
yihui 3:4189a10038e6 53
yihui 3:4189a10038e6 54 // this should be nested in the message length loop
yihui 3:4189a10038e6 55 int index = 0;
yihui 3:4189a10038e6 56 int bufferSize = getBufferSize(messageLength);
yihui 3:4189a10038e6 57 uint8_t buffer[bufferSize];
yihui 3:4189a10038e6 58
yihui 3:4189a10038e6 59 #ifdef MIFARE_CLASSIC_DEBUG
yihui 3:4189a10038e6 60 DMSG("Message Length ");
yihui 3:4189a10038e6 61 DMSG_INT(messageLength);
yihui 3:4189a10038e6 62 DMSG("Buffer Size ");
yihui 3:4189a10038e6 63 DMSG_INT(bufferSize);
yihui 3:4189a10038e6 64 #endif
yihui 3:4189a10038e6 65
yihui 3:4189a10038e6 66 while (index < bufferSize)
yihui 3:4189a10038e6 67 {
yihui 3:4189a10038e6 68
yihui 3:4189a10038e6 69 // authenticate on every sector
yihui 3:4189a10038e6 70 if (_nfcShield->mifareclassic_IsFirstBlock(currentBlock))
yihui 3:4189a10038e6 71 {
yihui 3:4189a10038e6 72 success = _nfcShield->mifareclassic_AuthenticateBlock(uid, uidLength, currentBlock, 0, key);
yihui 3:4189a10038e6 73 if (!success)
yihui 3:4189a10038e6 74 {
yihui 3:4189a10038e6 75 DMSG("Error. Block Authentication failed for ");
yihui 3:4189a10038e6 76 DMSG_INT(currentBlock);
yihui 3:4189a10038e6 77 // TODO error handling
yihui 3:4189a10038e6 78 }
yihui 3:4189a10038e6 79 }
yihui 3:4189a10038e6 80
yihui 3:4189a10038e6 81 // read the data
yihui 3:4189a10038e6 82 success = _nfcShield->mifareclassic_ReadDataBlock(currentBlock, &buffer[index]);
yihui 3:4189a10038e6 83 if (success)
yihui 3:4189a10038e6 84 {
yihui 3:4189a10038e6 85 #ifdef MIFARE_CLASSIC_DEBUG
yihui 3:4189a10038e6 86 DMSG("Block ");
yihui 3:4189a10038e6 87 DMSG_INT(currentBlock);
yihui 3:4189a10038e6 88 _nfcShield->PrintHexChar(&buffer[index], BLOCK_SIZE);
yihui 3:4189a10038e6 89 #endif
yihui 3:4189a10038e6 90 }
yihui 3:4189a10038e6 91 else
yihui 3:4189a10038e6 92 {
yihui 3:4189a10038e6 93 DMSG("Read failed ");
yihui 3:4189a10038e6 94 DMSG_INT(currentBlock);
yihui 3:4189a10038e6 95 // TODO handle errors here
yihui 3:4189a10038e6 96 }
yihui 3:4189a10038e6 97
yihui 3:4189a10038e6 98 index += BLOCK_SIZE;
yihui 3:4189a10038e6 99 currentBlock++;
yihui 3:4189a10038e6 100
yihui 3:4189a10038e6 101 // skip the trailer block
yihui 3:4189a10038e6 102 if (_nfcShield->mifareclassic_IsTrailerBlock(currentBlock))
yihui 3:4189a10038e6 103 {
yihui 3:4189a10038e6 104 #ifdef MIFARE_CLASSIC_DEBUG
yihui 3:4189a10038e6 105 DMSG("Skipping block ");
yihui 3:4189a10038e6 106 DMSG_INT(currentBlock);
yihui 3:4189a10038e6 107 #endif
yihui 3:4189a10038e6 108 currentBlock++;
yihui 3:4189a10038e6 109 }
yihui 3:4189a10038e6 110 }
yihui 3:4189a10038e6 111
yihui 3:4189a10038e6 112 return NfcTag(uid, uidLength, MIFARE_CLASSIC, &buffer[messageStartIndex], messageLength);
yihui 3:4189a10038e6 113 }
yihui 3:4189a10038e6 114
yihui 3:4189a10038e6 115 int MifareClassic::getBufferSize(int messageLength)
yihui 3:4189a10038e6 116 {
yihui 3:4189a10038e6 117
yihui 3:4189a10038e6 118 int bufferSize = messageLength;
yihui 3:4189a10038e6 119
yihui 3:4189a10038e6 120 // TLV header is 2 or 4 uint8_ts, TLV terminator is 1 uint8_t.
yihui 3:4189a10038e6 121 if (messageLength < 0xFF)
yihui 3:4189a10038e6 122 {
yihui 3:4189a10038e6 123 bufferSize += SHORT_TLV_SIZE + 1;
yihui 3:4189a10038e6 124 }
yihui 3:4189a10038e6 125 else
yihui 3:4189a10038e6 126 {
yihui 3:4189a10038e6 127 bufferSize += LONG_TLV_SIZE + 1;
yihui 3:4189a10038e6 128 }
yihui 3:4189a10038e6 129
yihui 3:4189a10038e6 130 // bufferSize needs to be a multiple of BLOCK_SIZE
yihui 3:4189a10038e6 131 if (bufferSize % BLOCK_SIZE != 0)
yihui 3:4189a10038e6 132 {
yihui 3:4189a10038e6 133 bufferSize = ((bufferSize / BLOCK_SIZE) + 1) * BLOCK_SIZE;
yihui 3:4189a10038e6 134 }
yihui 3:4189a10038e6 135
yihui 3:4189a10038e6 136 return bufferSize;
yihui 3:4189a10038e6 137 }
yihui 3:4189a10038e6 138
yihui 3:4189a10038e6 139 // skip null tlvs (0x0) before the real message
yihui 3:4189a10038e6 140 // technically unlimited null tlvs, but we assume
yihui 3:4189a10038e6 141 // T & L of TLV in the first block we read
yihui 3:4189a10038e6 142 int MifareClassic::getNdefStartIndex(uint8_t *data)
yihui 3:4189a10038e6 143 {
yihui 3:4189a10038e6 144
yihui 3:4189a10038e6 145 for (int i = 0; i < BLOCK_SIZE; i++)
yihui 3:4189a10038e6 146 {
yihui 3:4189a10038e6 147 if (data[i] == 0x0)
yihui 3:4189a10038e6 148 {
yihui 3:4189a10038e6 149 // do nothing, skip
yihui 3:4189a10038e6 150 }
yihui 3:4189a10038e6 151 else if (data[i] == 0x3)
yihui 3:4189a10038e6 152 {
yihui 3:4189a10038e6 153 return i;
yihui 3:4189a10038e6 154 }
yihui 3:4189a10038e6 155 else
yihui 3:4189a10038e6 156 {
yihui 3:4189a10038e6 157 DMSG("Unknown TLV ");
yihui 3:4189a10038e6 158 DMSG_HEX(data[i]);
yihui 3:4189a10038e6 159 return -2;
yihui 3:4189a10038e6 160 }
yihui 3:4189a10038e6 161 }
yihui 3:4189a10038e6 162
yihui 3:4189a10038e6 163 return -1;
yihui 3:4189a10038e6 164 }
yihui 3:4189a10038e6 165
yihui 3:4189a10038e6 166 // Decode the NDEF data length from the Mifare TLV
yihui 3:4189a10038e6 167 // Leading null TLVs (0x0) are skipped
yihui 3:4189a10038e6 168 // Assuming T & L of TLV will be in the first block
yihui 3:4189a10038e6 169 // messageLength and messageStartIndex written to the parameters
yihui 3:4189a10038e6 170 // success or failure status is returned
yihui 3:4189a10038e6 171 //
yihui 3:4189a10038e6 172 // { 0x3, LENGTH }
yihui 3:4189a10038e6 173 // { 0x3, 0xFF, LENGTH, LENGTH }
yihui 3:4189a10038e6 174 bool MifareClassic::decodeTlv(uint8_t *data, int &messageLength, int &messageStartIndex)
yihui 3:4189a10038e6 175 {
yihui 3:4189a10038e6 176 int i = getNdefStartIndex(data);
yihui 3:4189a10038e6 177
yihui 3:4189a10038e6 178 if (i < 0 || data[i] != 0x3)
yihui 3:4189a10038e6 179 {
yihui 3:4189a10038e6 180 DMSG("Error. Can't decode message length.");
yihui 3:4189a10038e6 181 return false;
yihui 3:4189a10038e6 182 }
yihui 3:4189a10038e6 183 else
yihui 3:4189a10038e6 184 {
yihui 3:4189a10038e6 185 if (data[i+1] == 0xFF)
yihui 3:4189a10038e6 186 {
yihui 3:4189a10038e6 187 messageLength = ((0xFF & data[i+2]) << 8) | (0xFF & data[i+3]);
yihui 3:4189a10038e6 188 messageStartIndex = i + LONG_TLV_SIZE;
yihui 3:4189a10038e6 189 }
yihui 3:4189a10038e6 190 else
yihui 3:4189a10038e6 191 {
yihui 3:4189a10038e6 192 messageLength = data[i+1];
yihui 3:4189a10038e6 193 messageStartIndex = i + SHORT_TLV_SIZE;
yihui 3:4189a10038e6 194 }
yihui 3:4189a10038e6 195 }
yihui 3:4189a10038e6 196
yihui 3:4189a10038e6 197 return true;
yihui 3:4189a10038e6 198 }
yihui 3:4189a10038e6 199
yihui 3:4189a10038e6 200 bool MifareClassic::write(NdefMessage& m, uint8_t * uid, unsigned int uidLength)
yihui 3:4189a10038e6 201 {
yihui 3:4189a10038e6 202
yihui 3:4189a10038e6 203 uint8_t encoded[m.getEncodedSize()];
yihui 3:4189a10038e6 204 m.encode(encoded);
yihui 3:4189a10038e6 205
yihui 3:4189a10038e6 206 uint8_t buffer[getBufferSize(sizeof(encoded))];
yihui 3:4189a10038e6 207 memset(buffer, 0, sizeof(buffer));
yihui 3:4189a10038e6 208
yihui 3:4189a10038e6 209 #ifdef MIFARE_CLASSIC_DEBUG
yihui 3:4189a10038e6 210 DMSG("sizeof(encoded) "));DMSG(sizeof(encoded);
yihui 3:4189a10038e6 211 DMSG("sizeof(buffer) "));DMSG(sizeof(buffer);
yihui 3:4189a10038e6 212 #endif
yihui 3:4189a10038e6 213
yihui 3:4189a10038e6 214 if (sizeof(encoded) < 0xFF)
yihui 3:4189a10038e6 215 {
yihui 3:4189a10038e6 216 buffer[0] = 0x3;
yihui 3:4189a10038e6 217 buffer[1] = sizeof(encoded);
yihui 3:4189a10038e6 218 memcpy(&buffer[2], encoded, sizeof(encoded));
yihui 3:4189a10038e6 219 buffer[2+sizeof(encoded)] = 0xFE; // terminator
yihui 3:4189a10038e6 220 }
yihui 3:4189a10038e6 221 else
yihui 3:4189a10038e6 222 {
yihui 3:4189a10038e6 223 buffer[0] = 0x3;
yihui 3:4189a10038e6 224 buffer[1] = 0xFF;
yihui 3:4189a10038e6 225 buffer[2] = ((sizeof(encoded) >> 8) & 0xFF);
yihui 3:4189a10038e6 226 buffer[3] = (sizeof(encoded) & 0xFF);
yihui 3:4189a10038e6 227 memcpy(&buffer[4], encoded, sizeof(encoded));
yihui 3:4189a10038e6 228 buffer[4+sizeof(encoded)] = 0xFE; // terminator
yihui 3:4189a10038e6 229 }
yihui 3:4189a10038e6 230
yihui 3:4189a10038e6 231 // Write to tag
yihui 3:4189a10038e6 232 int index = 0;
yihui 3:4189a10038e6 233 int currentBlock = 4;
yihui 3:4189a10038e6 234 uint8_t key[6] = { 0xD3, 0xF7, 0xD3, 0xF7, 0xD3, 0xF7 }; // this is Sector 1 - 15 key
yihui 3:4189a10038e6 235
yihui 3:4189a10038e6 236 while (index < sizeof(buffer))
yihui 3:4189a10038e6 237 {
yihui 3:4189a10038e6 238
yihui 3:4189a10038e6 239 if (_nfcShield->mifareclassic_IsFirstBlock(currentBlock))
yihui 3:4189a10038e6 240 {
yihui 3:4189a10038e6 241 int success = _nfcShield->mifareclassic_AuthenticateBlock(uid, uidLength, currentBlock, 0, key);
yihui 3:4189a10038e6 242 if (!success)
yihui 3:4189a10038e6 243 {
yihui 3:4189a10038e6 244 DMSG("Error. Block Authentication failed for ");DMSG_INT(currentBlock);
yihui 3:4189a10038e6 245 return false;
yihui 3:4189a10038e6 246 }
yihui 3:4189a10038e6 247 }
yihui 3:4189a10038e6 248 int write_success = _nfcShield->mifareclassic_WriteDataBlock (currentBlock, &buffer[index]);
yihui 3:4189a10038e6 249 if (write_success)
yihui 3:4189a10038e6 250 {
yihui 3:4189a10038e6 251 #ifdef MIFARE_CLASSIC_DEBUG
yihui 3:4189a10038e6 252 DMSG("Wrote block ");Serial.print(currentBlock);DMSG(" - ");
yihui 3:4189a10038e6 253 _nfcShield->PrintHexChar(&buffer[index], BLOCK_SIZE);
yihui 3:4189a10038e6 254 #endif
yihui 3:4189a10038e6 255 }
yihui 3:4189a10038e6 256 else
yihui 3:4189a10038e6 257 {
yihui 3:4189a10038e6 258 DMSG("Write failed ");DMSG_INT(currentBlock);
yihui 3:4189a10038e6 259 return false;
yihui 3:4189a10038e6 260 }
yihui 3:4189a10038e6 261 index += BLOCK_SIZE;
yihui 3:4189a10038e6 262 currentBlock++;
yihui 3:4189a10038e6 263
yihui 3:4189a10038e6 264 if (_nfcShield->mifareclassic_IsTrailerBlock(currentBlock))
yihui 3:4189a10038e6 265 {
yihui 3:4189a10038e6 266 // can't write to trailer block
yihui 3:4189a10038e6 267 #ifdef MIFARE_CLASSIC_DEBUG
yihui 3:4189a10038e6 268 DMSG("Skipping block ");DMSG(currentBlock);
yihui 3:4189a10038e6 269 #endif
yihui 3:4189a10038e6 270 currentBlock++;
yihui 3:4189a10038e6 271 }
yihui 3:4189a10038e6 272
yihui 3:4189a10038e6 273 }
yihui 3:4189a10038e6 274
yihui 3:4189a10038e6 275 return true;
yihui 3:4189a10038e6 276 }