Nico De Witte / PN532

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

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 }