armsrc/mifareutil.c

   1 // Merlok, May 2011, 2012
   2 // Many authors, whom made it possible
   3 //
   4 // This code is licensed to you under the terms of the GNU GPL, version 2 or,
   5 // at your option, any later version. See the LICENSE.txt file for the text of
   6 // the license.
   7 //-----------------------------------------------------------------------------
   8 // Work with mifare cards.
   9 //-----------------------------------------------------------------------------
  10 #include "mifareutil.h"
  11
  12 #include "string.h"
  13 #include "BigBuf.h"
  14 #include "iso14443a.h"
  15 #include "ticks.h"
  16 #include "dbprint.h"
  17 #include "parity.h"
  18 #include "commonutil.h"
  19 #include "crc16.h"
  20 #include "protocols.h"
  21 #include "desfire_crypto.h"
  22
  23 // crypto1 helpers
  24 void mf_crypto1_decryptEx(struct Crypto1State *pcs, uint8_t *data_in, int len, uint8_t *data_out) {
  25     if (len != 1) {
  26         for (int i = 0; i < len; i++)
  27             data_out[i] = crypto1_byte(pcs, 0x00, 0) ^ data_in[i];
  28     } else {
  29         uint8_t bt = 0;
  30         bt |= (crypto1_bit(pcs, 0, 0) ^ BIT(data_in[0], 0)) << 0;
  31         bt |= (crypto1_bit(pcs, 0, 0) ^ BIT(data_in[0], 1)) << 1;
  32         bt |= (crypto1_bit(pcs, 0, 0) ^ BIT(data_in[0], 2)) << 2;
  33         bt |= (crypto1_bit(pcs, 0, 0) ^ BIT(data_in[0], 3)) << 3;
  34         data_out[0] = bt;
  35     }
  36     return;
  37 }
  38
  39 void mf_crypto1_decrypt(struct Crypto1State *pcs, uint8_t *data, int len) {
  40     mf_crypto1_decryptEx(pcs, data, len, data);
  41 }
  42
  43 void mf_crypto1_encrypt(struct Crypto1State *pcs, uint8_t *data, uint16_t len, uint8_t *par) {
  44     mf_crypto1_encryptEx(pcs, data, NULL, data, len, par);
  45 }
  46
  47 void mf_crypto1_encryptEx(struct Crypto1State *pcs, uint8_t *data_in, uint8_t *keystream, uint8_t *data_out, uint16_t len, uint8_t *par) {
  48     int i;
  49     par[0] = 0;
  50
  51     for (i = 0; i < len; i++) {
  52         uint8_t bt = data_in[i];
  53         data_out[i] = crypto1_byte(pcs, keystream ? keystream[i] : 0x00, 0) ^ data_in[i];
  54         if ((i & 0x0007) == 0)
  55             par[ i >> 3 ] = 0;
  56         par[ i >> 3 ] |= (((filter(pcs->odd) ^ oddparity8(bt)) & 0x01) << (7 - (i & 0x0007)));
  57     }
  58 }
  59
  60 uint8_t mf_crypto1_encrypt4bit(struct Crypto1State *pcs, uint8_t data) {
  61     uint8_t bt = 0;
  62     bt |= (crypto1_bit(pcs, 0, 0) ^ BIT(data, 0)) << 0;
  63     bt |= (crypto1_bit(pcs, 0, 0) ^ BIT(data, 1)) << 1;
  64     bt |= (crypto1_bit(pcs, 0, 0) ^ BIT(data, 2)) << 2;
  65     bt |= (crypto1_bit(pcs, 0, 0) ^ BIT(data, 3)) << 3;
  66     return bt;
  67 }
  68
  69 // send X byte basic commands
  70 int mifare_sendcmd(uint8_t cmd, uint8_t *data, uint8_t data_size, uint8_t *answer, uint8_t *answer_parity, uint32_t *timing) {
  71
  72     uint8_t dcmd[data_size + 3];
  73     dcmd[0] = cmd;
  74     if (data_size > 0)
  75         memcpy(dcmd + 1, data, data_size);
  76
  77     AddCrc14A(dcmd, data_size + 1);
  78     ReaderTransmit(dcmd, sizeof(dcmd), timing);
  79     int len = ReaderReceive(answer, answer_parity);
  80     if (!len) {
  81         if (DBGLEVEL >= DBG_ERROR) Dbprintf("%02X Cmd failed. Card timeout.", cmd);
  82         len = ReaderReceive(answer, answer_parity);
  83     }
  84     return len;
  85 }
  86
  87 // send 2 byte commands
  88 int mifare_sendcmd_short(struct Crypto1State *pcs, uint8_t crypted, uint8_t cmd, uint8_t data, uint8_t *answer, uint8_t *answer_parity, uint32_t *timing) {
  89     uint16_t pos;
  90     uint8_t dcmd[4] = {cmd, data, 0x00, 0x00};
  91     uint8_t ecmd[4] = {0x00, 0x00, 0x00, 0x00};
  92     uint8_t par[1] = {0x00}; // 1 Byte parity is enough here
  93     AddCrc14A(dcmd, 2);
  94     memcpy(ecmd, dcmd, sizeof(dcmd));
  95
  96     if (pcs && crypted) {
  97         par[0] = 0;
  98         for (pos = 0; pos < 4; pos++) {
  99             ecmd[pos] = crypto1_byte(pcs, 0x00, 0) ^ dcmd[pos];
 100             par[0] |= (((filter(pcs->odd) ^ oddparity8(dcmd[pos])) & 0x01) << (7 - pos));
 101         }
 102         ReaderTransmitPar(ecmd, sizeof(ecmd), par, timing);
 103     } else {
 104         ReaderTransmit(dcmd, sizeof(dcmd), timing);
 105     }
 106
 107     int len = ReaderReceive(answer, par);
 108
 109     if (answer_parity) *answer_parity = par[0];
 110
 111     if (crypted == CRYPT_ALL) {
 112         if (len == 1) {
 113             uint16_t res = 0;
 114             res |= (crypto1_bit(pcs, 0, 0) ^ BIT(answer[0], 0)) << 0;
 115             res |= (crypto1_bit(pcs, 0, 0) ^ BIT(answer[0], 1)) << 1;
 116             res |= (crypto1_bit(pcs, 0, 0) ^ BIT(answer[0], 2)) << 2;
 117             res |= (crypto1_bit(pcs, 0, 0) ^ BIT(answer[0], 3)) << 3;
 118             answer[0] = res;
 119         } else {
 120             for (pos = 0; pos < len; pos++)
 121                 answer[pos] = crypto1_byte(pcs, 0x00, 0) ^ answer[pos];
 122         }
 123     }
 124     return len;
 125 }
 126
 127 // mifare classic commands
 128 int mifare_classic_auth(struct Crypto1State *pcs, uint32_t uid, uint8_t blockNo, uint8_t keyType, uint64_t ui64Key, uint8_t isNested) {
 129     return mifare_classic_authex(pcs, uid, blockNo, keyType, ui64Key, isNested, NULL, NULL);
 130 }
 131
 132 int mifare_classic_authex(struct Crypto1State *pcs, uint32_t uid, uint8_t blockNo, uint8_t keyType, uint64_t ui64Key, uint8_t isNested, uint32_t *ntptr, uint32_t *timing) {
 133     int len;
 134     uint32_t pos, nt, ntpp; // Supplied tag nonce
 135     uint8_t par[1] = {0x00};
 136     uint8_t nr[4];
 137     uint8_t mf_nr_ar[] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
 138     uint8_t receivedAnswer[MAX_MIFARE_FRAME_SIZE] = {0x00};
 139     uint8_t receivedAnswerPar[MAX_MIFARE_PARITY_SIZE] = {0x00};
 140
 141     // "random" reader nonce:
 142     num_to_bytes(prng_successor(GetTickCount(), 32), 4, nr);
 143
 144     // Transmit MIFARE_CLASSIC_AUTH
 145     len = mifare_sendcmd_short(pcs, isNested, 0x60 + (keyType & 0x01), blockNo, receivedAnswer, receivedAnswerPar, timing);
 146     if (len != 4) return 1;
 147
 148     // Save the tag nonce (nt)
 149     nt = bytes_to_num(receivedAnswer, 4);
 150
 151     //  ----------------------------- crypto1 create
 152     if (isNested)
 153         crypto1_deinit(pcs);
 154
 155     // Init cipher with key
 156     crypto1_init(pcs, ui64Key);
 157
 158     if (isNested == AUTH_NESTED) {
 159         // decrypt nt with help of new key
 160         nt = crypto1_word(pcs, nt ^ uid, 1) ^ nt;
 161     } else {
 162         // Load (plain) uid^nt into the cipher
 163         crypto1_word(pcs, nt ^ uid, 0);
 164     }
 165
 166     // some statistic
 167     if (!ntptr && (DBGLEVEL >= DBG_EXTENDED))
 168         Dbprintf("auth uid: %08x | nr: %08x | nt: %08x", uid, nr, nt);
 169
 170     // save Nt
 171     if (ntptr)
 172         *ntptr = nt;
 173
 174     // Generate (encrypted) nr+parity by loading it into the cipher (Nr)
 175     par[0] = 0;
 176     for (pos = 0; pos < 4; pos++) {
 177         mf_nr_ar[pos] = crypto1_byte(pcs, nr[pos], 0) ^ nr[pos];
 178         par[0] |= (((filter(pcs->odd) ^ oddparity8(nr[pos])) & 0x01) << (7 - pos));
 179     }
 180
 181     // Skip 32 bits in pseudo random generator
 182     nt = prng_successor(nt, 32);
 183
 184     //  ar+parity
 185     for (pos = 4; pos < 8; pos++) {
 186         nt = prng_successor(nt, 8);
 187         mf_nr_ar[pos] = crypto1_byte(pcs, 0x00, 0) ^ (nt & 0xff);
 188         par[0] |= (((filter(pcs->odd) ^ oddparity8(nt & 0xff)) & 0x01) << (7 - pos));
 189     }
 190
 191     // Transmit reader nonce and reader answer
 192     ReaderTransmitPar(mf_nr_ar, sizeof(mf_nr_ar), par, NULL);
 193
 194     // save standard timeout
 195     uint32_t save_timeout = iso14a_get_timeout();
 196
 197     // set timeout for authentication response
 198     if (save_timeout > 103)
 199         iso14a_set_timeout(103);
 200
 201     // Receive 4 byte tag answer
 202     len = ReaderReceive(receivedAnswer, receivedAnswerPar);
 203
 204     iso14a_set_timeout(save_timeout);
 205
 206     if (!len) {
 207         if (DBGLEVEL >= DBG_EXTENDED) Dbprintf("Authentication failed. Card timeout");
 208         return 2;
 209     }
 210
 211     ntpp = prng_successor(nt, 32) ^ crypto1_word(pcs, 0, 0);
 212
 213     if (ntpp != bytes_to_num(receivedAnswer, 4)) {
 214         if (DBGLEVEL >= DBG_EXTENDED) Dbprintf("Authentication failed. Error card response");
 215         return 3;
 216     }
 217     return 0;
 218 }
 219
 220 int mifare_classic_readblock(struct Crypto1State *pcs, uint32_t uid, uint8_t blockNo, uint8_t *blockData) {
 221
 222     int len;
 223     uint8_t bt[2] = {0x00, 0x00};
 224     uint8_t receivedAnswer[MAX_MIFARE_FRAME_SIZE] = {0x00};
 225     uint8_t receivedAnswerPar[MAX_MIFARE_PARITY_SIZE] = {0x00};
 226
 227     len = mifare_sendcmd_short(pcs, 1, ISO14443A_CMD_READBLOCK, blockNo, receivedAnswer, receivedAnswerPar, NULL);
 228     if (len == 1) {
 229         if (DBGLEVEL >= DBG_ERROR) Dbprintf("Cmd Error %02x", receivedAnswer[0]);
 230         return 1;
 231     }
 232     if (len != 18) {
 233         if (DBGLEVEL >= DBG_ERROR) Dbprintf("wrong response len %d (expected 18)", len);
 234         return 2;
 235     }
 236
 237     memcpy(bt, receivedAnswer + 16, 2);
 238     AddCrc14A(receivedAnswer, 16);
 239     if (bt[0] != receivedAnswer[16] || bt[1] != receivedAnswer[17]) {
 240         if (DBGLEVEL >= DBG_INFO) Dbprintf("CRC response error");
 241         return 3;
 242     }
 243
 244     memcpy(blockData, receivedAnswer, 16);
 245     return 0;
 246 }
 247
 248 // mifare ultralight commands
 249 int mifare_ul_ev1_auth(uint8_t *keybytes, uint8_t *pack) {
 250
 251     uint16_t len = 0;
 252     uint8_t resp[4] = {0x00, 0x00, 0x00, 0x00};
 253     uint8_t respPar[1] = {0x00};
 254     uint8_t key[4] = {0x00, 0x00, 0x00, 0x00};
 255     memcpy(key, keybytes, 4);
 256
 257     if (DBGLEVEL >= DBG_EXTENDED)
 258         Dbprintf("EV1 Auth : %02x%02x%02x%02x", key[0], key[1], key[2], key[3]);
 259
 260     len = mifare_sendcmd(MIFARE_ULEV1_AUTH, key, sizeof(key), resp, respPar, NULL);
 261
 262     if (len != 4) {
 263         if (DBGLEVEL >= DBG_ERROR) Dbprintf("Cmd Error: %02x %u", resp[0], len);
 264         return 0;
 265     }
 266
 267     if (DBGLEVEL >= DBG_EXTENDED)
 268         Dbprintf("Auth Resp: %02x%02x%02x%02x", resp[0], resp[1], resp[2], resp[3]);
 269
 270     memcpy(pack, resp, 4);
 271     return 1;
 272 }
 273
 274 int mifare_ultra_auth(uint8_t *keybytes) {
 275
 276     /// 3des2k
 277     uint8_t random_a[8] = {1, 1, 1, 1, 1, 1, 1, 1};
 278     uint8_t random_b[8] = {0x00};
 279     uint8_t enc_random_b[8] = {0x00};
 280     uint8_t rnd_ab[16] = {0x00};
 281     uint8_t IV[8] = {0x00};
 282     uint8_t key[16] = {0x00};
 283     memcpy(key, keybytes, 16);
 284
 285     uint16_t len = 0;
 286     uint8_t resp[19] = {0x00};
 287     uint8_t respPar[3] = {0, 0, 0};
 288
 289     // REQUEST AUTHENTICATION
 290     len = mifare_sendcmd_short(NULL, CRYPT_NONE, MIFARE_ULC_AUTH_1, 0x00, resp, respPar, NULL);
 291     if (len != 11) {
 292         if (DBGLEVEL >= DBG_ERROR) Dbprintf("Cmd Error: %02x", resp[0]);
 293         return 0;
 294     }
 295
 296     // tag nonce.
 297     memcpy(enc_random_b, resp + 1, 8);
 298
 299     // decrypt nonce.
 300     tdes_nxp_receive((void *)enc_random_b, (void *)random_b, sizeof(random_b), (const void *)key, IV, 2);
 301     rol(random_b, 8);
 302     memcpy(rnd_ab, random_a, 8);
 303     memcpy(rnd_ab + 8, random_b, 8);
 304
 305     if (DBGLEVEL >= DBG_EXTENDED) {
 306         Dbprintf("enc_B: %02x %02x %02x %02x %02x %02x %02x %02x",
 307                  enc_random_b[0], enc_random_b[1], enc_random_b[2], enc_random_b[3], enc_random_b[4], enc_random_b[5], enc_random_b[6], enc_random_b[7]);
 308
 309         Dbprintf("    B: %02x %02x %02x %02x %02x %02x %02x %02x",
 310                  random_b[0], random_b[1], random_b[2], random_b[3], random_b[4], random_b[5], random_b[6], random_b[7]);
 311
 312         Dbprintf("rnd_ab: %02x %02x %02x %02x %02x %02x %02x %02x",
 313                  rnd_ab[0], rnd_ab[1], rnd_ab[2], rnd_ab[3], rnd_ab[4], rnd_ab[5], rnd_ab[6], rnd_ab[7]);
 314
 315         Dbprintf("rnd_ab: %02x %02x %02x %02x %02x %02x %02x %02x",
 316                  rnd_ab[8], rnd_ab[9], rnd_ab[10], rnd_ab[11], rnd_ab[12], rnd_ab[13], rnd_ab[14], rnd_ab[15]);
 317     }
 318
 319     // encrypt    out, in, length, key, iv
 320     tdes_nxp_send(rnd_ab, rnd_ab, sizeof(rnd_ab), key, enc_random_b, 2);
 321
 322     len = mifare_sendcmd(MIFARE_ULC_AUTH_2, rnd_ab, sizeof(rnd_ab), resp, respPar, NULL);
 323     if (len != 11) {
 324         if (DBGLEVEL >= DBG_ERROR) Dbprintf("Cmd Error: %02x", resp[0]);
 325         return 0;
 326     }
 327
 328     uint8_t enc_resp[8] = { 0, 0, 0, 0, 0, 0, 0, 0 };
 329     uint8_t resp_random_a[8] = { 0, 0, 0, 0, 0, 0, 0, 0 };
 330     memcpy(enc_resp, resp + 1, 8);
 331
 332     // decrypt    out, in, length, key, iv
 333     tdes_nxp_receive(enc_resp, resp_random_a, 8, key, enc_random_b, 2);
 334     if (memcmp(resp_random_a, random_a, 8) != 0) {
 335         if (DBGLEVEL >= DBG_ERROR) Dbprintf("failed authentication");
 336         return 0;
 337     }
 338
 339     if (DBGLEVEL >= DBG_EXTENDED) {
 340         Dbprintf("e_AB: %02x %02x %02x %02x %02x %02x %02x %02x",
 341                  rnd_ab[0], rnd_ab[1], rnd_ab[2], rnd_ab[3],
 342                  rnd_ab[4], rnd_ab[5], rnd_ab[6], rnd_ab[7]);
 343
 344         Dbprintf("e_AB: %02x %02x %02x %02x %02x %02x %02x %02x",
 345                  rnd_ab[8], rnd_ab[9], rnd_ab[10], rnd_ab[11],
 346                  rnd_ab[12], rnd_ab[13], rnd_ab[14], rnd_ab[15]);
 347
 348         Dbprintf("a: %02x %02x %02x %02x %02x %02x %02x %02x",
 349                  random_a[0], random_a[1], random_a[2], random_a[3],
 350                  random_a[4], random_a[5], random_a[6], random_a[7]);
 351
 352         Dbprintf("b: %02x %02x %02x %02x %02x %02x %02x %02x",
 353                  resp_random_a[0], resp_random_a[1], resp_random_a[2], resp_random_a[3],
 354                  resp_random_a[4], resp_random_a[5], resp_random_a[6], resp_random_a[7]);
 355     }
 356     return 1;
 357 }
 358
 359 static int mifare_ultra_readblockEx(uint8_t blockNo, uint8_t *blockData) {
 360     uint16_t len = 0;
 361     uint8_t bt[2] = {0x00, 0x00};
 362     uint8_t receivedAnswer[MAX_FRAME_SIZE] = {0x00};
 363     uint8_t receivedAnswerPar[MAX_PARITY_SIZE] = {0x00};
 364
 365     len = mifare_sendcmd_short(NULL, CRYPT_NONE, ISO14443A_CMD_READBLOCK, blockNo, receivedAnswer, receivedAnswerPar, NULL);
 366     if (len == 1) {
 367         if (DBGLEVEL >= DBG_ERROR) Dbprintf("Cmd Error: %02x", receivedAnswer[0]);
 368         return 1;
 369     }
 370     if (len != 18) {
 371         if (DBGLEVEL >= DBG_ERROR) Dbprintf("Cmd Error: card timeout. len: %x", len);
 372         return 2;
 373     }
 374
 375     memcpy(bt, receivedAnswer + 16, 2);
 376     AddCrc14A(receivedAnswer, 16);
 377     if (bt[0] != receivedAnswer[16] || bt[1] != receivedAnswer[17]) {
 378         if (DBGLEVEL >= DBG_ERROR) Dbprintf("Cmd CRC response error.");
 379         return 3;
 380     }
 381
 382     memcpy(blockData, receivedAnswer, 16);
 383     return 0;
 384 }
 385 int mifare_ultra_readblock(uint8_t blockNo, uint8_t *blockData) {
 386 #define MFU_MAX_RETRIES 5
 387     uint8_t res;
 388
 389     for (uint8_t retries = 0; retries < MFU_MAX_RETRIES; ++retries) {
 390         res = mifare_ultra_readblockEx(blockNo, blockData);
 391
 392         // break if OK,  or NACK.
 393         switch (res) {
 394             case 0:
 395             case 1:
 396                 return res;
 397             default:
 398                 continue;
 399         }
 400     }
 401     return res;
 402 }
 403
 404 int mifare_classic_writeblock(struct Crypto1State *pcs, uint32_t uid, uint8_t blockNo, uint8_t *blockData) {
 405     // variables
 406     uint16_t len = 0;
 407     uint32_t pos = 0;
 408     uint8_t par[3] = {0x00, 0x00, 0x00}; // enough for 18 Bytes to send
 409     uint8_t res = 0;
 410
 411     uint8_t d_block[18], d_block_enc[18];
 412     uint8_t receivedAnswer[MAX_MIFARE_FRAME_SIZE] = {0x00};
 413     uint8_t receivedAnswerPar[MAX_MIFARE_PARITY_SIZE] = {0x00};
 414
 415     // command MIFARE_CLASSIC_WRITEBLOCK
 416     len = mifare_sendcmd_short(pcs, 1, ISO14443A_CMD_WRITEBLOCK, blockNo, receivedAnswer, receivedAnswerPar, NULL);
 417
 418     if ((len != 1) || (receivedAnswer[0] != 0x0A)) {   //  0x0a - ACK
 419         if (DBGLEVEL >= DBG_ERROR) Dbprintf("Cmd Error: %02x", receivedAnswer[0]);
 420         return 1;
 421     }
 422
 423     memcpy(d_block, blockData, 16);
 424     AddCrc14A(d_block, 16);
 425
 426     // crypto
 427     for (pos = 0; pos < 18; pos++) {
 428         d_block_enc[pos] = crypto1_byte(pcs, 0x00, 0) ^ d_block[pos];
 429         par[pos >> 3] |= (((filter(pcs->odd) ^ oddparity8(d_block[pos])) & 0x01) << (7 - (pos & 0x0007)));
 430     }
 431
 432     ReaderTransmitPar(d_block_enc, sizeof(d_block_enc), par, NULL);
 433
 434     // Receive the response
 435     len = ReaderReceive(receivedAnswer, receivedAnswerPar);
 436
 437     res = 0;
 438     res |= (crypto1_bit(pcs, 0, 0) ^ BIT(receivedAnswer[0], 0)) << 0;
 439     res |= (crypto1_bit(pcs, 0, 0) ^ BIT(receivedAnswer[0], 1)) << 1;
 440     res |= (crypto1_bit(pcs, 0, 0) ^ BIT(receivedAnswer[0], 2)) << 2;
 441     res |= (crypto1_bit(pcs, 0, 0) ^ BIT(receivedAnswer[0], 3)) << 3;
 442
 443     if ((len != 1) || (res != 0x0A)) {
 444         if (DBGLEVEL >= DBG_ERROR) Dbprintf("Cmd send data2 Error: %02x", res);
 445         return 2;
 446     }
 447     return 0;
 448 }
 449
 450 int mifare_ultra_writeblock_compat(uint8_t blockNo, uint8_t *blockData) {
 451     // variables
 452     uint16_t len = 0;
 453
 454     uint8_t d_block[18];
 455     uint8_t receivedAnswer[MAX_MIFARE_FRAME_SIZE] = {0x00};
 456     uint8_t receivedAnswerPar[MAX_MIFARE_PARITY_SIZE] = {0x00};
 457
 458     len = mifare_sendcmd_short(NULL, CRYPT_NONE, ISO14443A_CMD_WRITEBLOCK, blockNo, receivedAnswer, receivedAnswerPar, NULL);
 459
 460     if (receivedAnswer[0] != 0x0A) {   //  0x0a - ACK
 461         if (DBGLEVEL >= DBG_ERROR)
 462             Dbprintf("Cmd Send Error: %02x %d", receivedAnswer[0], len);
 463         return 1;
 464     }
 465
 466     memcpy(d_block, blockData, 16);
 467     AddCrc14A(d_block, 16);
 468
 469     ReaderTransmit(d_block, sizeof(d_block), NULL);
 470
 471     // Receive the response
 472     len = ReaderReceive(receivedAnswer, receivedAnswerPar);
 473
 474     if (receivedAnswer[0] != 0x0A) {   //  0x0a - ACK
 475         if (DBGLEVEL >= DBG_ERROR)
 476             Dbprintf("Cmd Send Data Error: %02x %d", receivedAnswer[0], len);
 477         return 2;
 478     }
 479     return 0;
 480 }
 481
 482 int mifare_ultra_writeblock(uint8_t blockNo, uint8_t *blockData) {
 483     uint16_t len = 0;
 484     uint8_t block[5] = {blockNo, 0x00, 0x00, 0x00, 0x00 };
 485     uint8_t receivedAnswer[MAX_MIFARE_FRAME_SIZE] = {0x00};
 486     uint8_t receivedAnswerPar[MAX_MIFARE_PARITY_SIZE] = {0x00};
 487
 488     // command MIFARE_CLASSIC_WRITEBLOCK
 489     memcpy(block + 1, blockData, 4);
 490
 491     len = mifare_sendcmd(MIFARE_ULC_WRITE, block, sizeof(block), receivedAnswer, receivedAnswerPar, NULL);
 492
 493     if (receivedAnswer[0] != 0x0A) {   //  0x0a - ACK
 494         if (DBGLEVEL >= DBG_ERROR)
 495             Dbprintf("Cmd Send Error: %02x %d", receivedAnswer[0], len);
 496         return 1;
 497     }
 498     return 0;
 499 }
 500 int mifare_classic_halt_ex(struct Crypto1State *pcs) {
 501     uint8_t receivedAnswer[4] = {0x00, 0x00, 0x00, 0x00};
 502     uint16_t len = mifare_sendcmd_short(pcs, (pcs == NULL) ? CRYPT_NONE : CRYPT_ALL, ISO14443A_CMD_HALT, 0x00, receivedAnswer, NULL, NULL);
 503     if (len != 0) {
 504         if (DBGLEVEL >= DBG_EXTENDED) Dbprintf("halt warning. response len: %x", len);
 505         return 1;
 506     }
 507     return 0;
 508 }
 509 int mifare_classic_halt(struct Crypto1State *pcs, uint32_t uid) {
 510     return mifare_classic_halt_ex(pcs);
 511 }
 512
 513 int mifare_ultra_halt(void) {
 514     uint16_t len = 0;
 515     uint8_t receivedAnswer[4] = {0x00, 0x00, 0x00, 0x00};
 516     len = mifare_sendcmd_short(NULL, CRYPT_NONE, ISO14443A_CMD_HALT, 0x00, receivedAnswer, NULL, NULL);
 517     if (len != 0) {
 518         if (DBGLEVEL >= DBG_EXTENDED) Dbprintf("halt warning. response len: %x", len);
 519         return 1;
 520     }
 521     return 0;
 522 }
 523
 524
 525 // Mifare Memory Structure: up to 32 Sectors with 4 blocks each (1k and 2k cards),
 526 // plus evtl. 8 sectors with 16 blocks each (4k cards)
 527 uint8_t NumBlocksPerSector(uint8_t sectorNo) {
 528     return (sectorNo < 32) ? 4 : 16;
 529 }
 530
 531 uint8_t FirstBlockOfSector(uint8_t sectorNo) {
 532     if (sectorNo < 32)
 533         return sectorNo * 4;
 534     else
 535         return 32 * 4 + (sectorNo - 32) * 16;
 536
 537 }
 538
 539 // work with emulator memory
 540 void emlSetMem(uint8_t *data, int blockNum, int blocksCount) {
 541     emlSetMem_xt(data, blockNum, blocksCount, 16);
 542 }
 543
 544 void emlSetMem_xt(uint8_t *data, int blockNum, int blocksCount, int blockBtWidth) {
 545     uint8_t *emCARD = BigBuf_get_EM_addr();
 546     memcpy(emCARD + blockNum * blockBtWidth, data, blocksCount * blockBtWidth);
 547 }
 548
 549 void emlGetMem(uint8_t *data, int blockNum, int blocksCount) {
 550     uint8_t *emCARD = BigBuf_get_EM_addr();
 551     memcpy(data, emCARD + blockNum * 16, blocksCount * 16);
 552 }
 553
 554 void emlGetMemBt(uint8_t *data, int offset, int byteCount) {
 555     uint8_t *emCARD = BigBuf_get_EM_addr();
 556     memcpy(data, emCARD + offset, byteCount);
 557 }
 558
 559 int emlCheckValBl(int blockNum) {
 560     uint8_t *emCARD = BigBuf_get_EM_addr();
 561     uint8_t *data = emCARD + blockNum * 16;
 562
 563     if ((data[0] != (data[4] ^ 0xff)) || (data[0] != data[8]) ||
 564             (data[1] != (data[5] ^ 0xff)) || (data[1] != data[9]) ||
 565             (data[2] != (data[6] ^ 0xff)) || (data[2] != data[10]) ||
 566             (data[3] != (data[7] ^ 0xff)) || (data[3] != data[11]) ||
 567             (data[12] != (data[13] ^ 0xff)) || (data[12] != data[14]) ||
 568             (data[12] != (data[15] ^ 0xff))
 569        )
 570         return 1;
 571     return 0;
 572 }
 573
 574 int emlGetValBl(uint32_t *blReg, uint8_t *blBlock, int blockNum) {
 575     uint8_t *emCARD = BigBuf_get_EM_addr();
 576     uint8_t *data = emCARD + blockNum * 16;
 577
 578     if (emlCheckValBl(blockNum))
 579         return 1;
 580
 581     memcpy(blReg, data, 4);
 582     *blBlock = data[12];
 583     return 0;
 584 }
 585
 586 int emlSetValBl(uint32_t blReg, uint8_t blBlock, int blockNum) {
 587     uint8_t *emCARD = BigBuf_get_EM_addr();
 588     uint8_t *data = emCARD + blockNum * 16;
 589
 590     memcpy(data + 0, &blReg, 4);
 591     memcpy(data + 8, &blReg, 4);
 592     blReg = blReg ^ 0xffffffff;
 593     memcpy(data + 4, &blReg, 4);
 594
 595     data[12] = blBlock;
 596     data[13] = blBlock ^ 0xff;
 597     data[14] = blBlock;
 598     data[15] = blBlock ^ 0xff;
 599
 600     return 0;
 601 }
 602
 603 uint64_t emlGetKey(int sectorNum, int keyType) {
 604     uint8_t key[6] = {0x00};
 605     uint8_t *emCARD = BigBuf_get_EM_addr();
 606     memcpy(key, emCARD + 16 * (FirstBlockOfSector(sectorNum) + NumBlocksPerSector(sectorNum) - 1) + keyType * 10, 6);
 607     return bytes_to_num(key, 6);
 608 }
 609
 610 void emlClearMem(void) {
 611     const uint8_t trailer[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x07, 0x80, 0x69, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 612     const uint8_t uid[]   =   {0xe6, 0x84, 0x87, 0xf3, 0x16, 0x88, 0x04, 0x00, 0x46, 0x8e, 0x45, 0x55, 0x4d, 0x70, 0x41, 0x04};
 613     uint8_t *emCARD = BigBuf_get_EM_addr();
 614     memset(emCARD, 0, CARD_MEMORY_SIZE);
 615
 616     // fill sectors trailer data
 617     for (uint16_t b = 3; b < MIFARE_4K_MAXBLOCK; ((b < MIFARE_2K_MAXBLOCK - 4) ? (b += 4) : (b += 16)))
 618         emlSetMem((uint8_t *)trailer, b, 1);
 619
 620     // uid
 621     emlSetMem((uint8_t *)uid, 0, 1);
 622     return;
 623 }
 624
 625 uint8_t SectorTrailer(uint8_t blockNo) {
 626     if (blockNo <= MIFARE_2K_MAXBLOCK) {
 627         if (DBGLEVEL >= DBG_EXTENDED)
 628             Dbprintf("Sector Trailer for block %d : %d", blockNo, (blockNo | 0x03));
 629         return (blockNo | 0x03);
 630     } else {
 631         if (DBGLEVEL >= DBG_EXTENDED)
 632             Dbprintf("Sector Trailer for block %d : %d", blockNo, (blockNo | 0x0f));
 633         return (blockNo | 0x0f);
 634     }
 635 }
 636
 637 bool IsSectorTrailer(uint8_t blockNo) {
 638     return (blockNo == SectorTrailer(blockNo));
 639 }
 640
 641 // Mifare desfire commands
 642 int mifare_sendcmd_special(struct Crypto1State *pcs, uint8_t crypted, uint8_t cmd, uint8_t *data, uint8_t *answer, uint8_t *answer_parity, uint32_t *timing) {
 643     uint8_t dcmd[5] = {cmd, data[0], data[1], 0x00, 0x00};
 644     AddCrc14A(dcmd, 3);
 645
 646     ReaderTransmit(dcmd, sizeof(dcmd), NULL);
 647     int len = ReaderReceive(answer, answer_parity);
 648     if (!len) {
 649         if (DBGLEVEL >= DBG_ERROR) Dbprintf("Authentication failed. Card timeout.");
 650         return 1;
 651     }
 652     return len;
 653 }
 654
 655 int mifare_sendcmd_special2(struct Crypto1State *pcs, uint8_t crypted, uint8_t cmd, uint8_t *data, uint8_t *answer, uint8_t *answer_parity, uint32_t *timing) {
 656     uint8_t dcmd[20] = {0x00};
 657     dcmd[0] = cmd;
 658     memcpy(dcmd + 1, data, 17);
 659     AddCrc14A(dcmd, 18);
 660
 661     ReaderTransmit(dcmd, sizeof(dcmd), NULL);
 662     int len = ReaderReceive(answer, answer_parity);
 663     if (!len) {
 664         if (DBGLEVEL >= DBG_ERROR) Dbprintf("Authentication failed. Card timeout.");
 665         return 1;
 666     }
 667     return len;
 668 }
 669
 670 int mifare_desfire_des_auth1(uint32_t uid, uint8_t *blockData) {
 671
 672     int len;
 673     // load key, keynumber
 674     uint8_t data[2] = {MFDES_AUTHENTICATE, 0x00};
 675     uint8_t receivedAnswer[MAX_FRAME_SIZE] = {0x00};
 676     uint8_t receivedAnswerPar[MAX_PARITY_SIZE] = {0x00};
 677
 678     len = mifare_sendcmd_special(NULL, 1, 0x02, data, receivedAnswer, receivedAnswerPar, NULL);
 679     if (len == 1) {
 680         if (DBGLEVEL >= DBG_ERROR)
 681             Dbprintf("Cmd Error: %02x", receivedAnswer[0]);
 682         return 1;
 683     }
 684
 685     if (len == 12) {
 686         if (DBGLEVEL >= DBG_EXTENDED) {
 687             Dbprintf("Auth1 Resp: %02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x",
 688                      receivedAnswer[0], receivedAnswer[1], receivedAnswer[2], receivedAnswer[3], receivedAnswer[4],
 689                      receivedAnswer[5], receivedAnswer[6], receivedAnswer[7], receivedAnswer[8], receivedAnswer[9],
 690                      receivedAnswer[10], receivedAnswer[11]);
 691         }
 692         memcpy(blockData, receivedAnswer, 12);
 693         return 0;
 694     }
 695     return 1;
 696 }
 697
 698 int mifare_desfire_des_auth2(uint32_t uid, uint8_t *key, uint8_t *blockData) {
 699
 700     int len;
 701     uint8_t data[17] = {MFDES_ADDITIONAL_FRAME};
 702     memcpy(data + 1, key, 16);
 703
 704     uint8_t receivedAnswer[MAX_FRAME_SIZE] = {0x00};
 705     uint8_t receivedAnswerPar[MAX_PARITY_SIZE] = {0x00};
 706
 707     len = mifare_sendcmd_special2(NULL, 1, 0x03, data, receivedAnswer, receivedAnswerPar, NULL);
 708
 709     if ((receivedAnswer[0] == 0x03) && (receivedAnswer[1] == 0xae)) {
 710         if (DBGLEVEL >= DBG_ERROR)
 711             Dbprintf("Auth Error: %02x %02x", receivedAnswer[0], receivedAnswer[1]);
 712         return 1;
 713     }
 714
 715     if (len == 12) {
 716         if (DBGLEVEL >= DBG_EXTENDED) {
 717             Dbprintf("Auth2 Resp: %02x%02x%02x%02x%02x%02x%02x%02x%02x%02x",
 718                      receivedAnswer[0], receivedAnswer[1], receivedAnswer[2], receivedAnswer[3], receivedAnswer[4],
 719                      receivedAnswer[5], receivedAnswer[6], receivedAnswer[7], receivedAnswer[8], receivedAnswer[9],
 720                      receivedAnswer[10], receivedAnswer[11]);
 721         }
 722         memcpy(blockData, receivedAnswer, 12);
 723         return 0;
 724     }
 725     return 1;
 726 }