armsrc/mifarecmd.c

   1 //-----------------------------------------------------------------------------
   2 // Merlok - June 2011, 2012
   3 // Gerhard de Koning Gans - May 2008
   4 // Hagen Fritsch - June 2010
   5 // Midnitesnake - Dec 2013
   6 // Andy Davies  - Apr 2014
   7 // Iceman - May 2014
   8 //
   9 // This code is licensed to you under the terms of the GNU GPL, version 2 or,
  10 // at your option, any later version. See the LICENSE.txt file for the text of
  11 // the license.
  12 //-----------------------------------------------------------------------------
  13 // Routines to support ISO 14443 type A.
  14 //-----------------------------------------------------------------------------
  15
  16 #include "mifarecmd.h"
  17 #include "apps.h"
  18 #include "util.h"
  19
  20 #include "crc.h"
  21
  22 //-----------------------------------------------------------------------------
  23 // Select, Authenticate, Read a MIFARE tag.
  24 // read block
  25 //-----------------------------------------------------------------------------
  26 void MifareReadBlock(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain)
  27 {
  28   // params
  29         uint8_t blockNo = arg0;
  30         uint8_t keyType = arg1;
  31         uint64_t ui64Key = 0;
  32         ui64Key = bytes_to_num(datain, 6);
  33
  34         // variables
  35         byte_t isOK = 0;
  36         byte_t dataoutbuf[16];
  37         uint8_t uid[10];
  38         uint32_t cuid;
  39         struct Crypto1State mpcs = {0, 0};
  40         struct Crypto1State *pcs;
  41         pcs = &mpcs;
  42
  43         // clear trace
  44         clear_trace();
  45         iso14443a_setup(FPGA_HF_ISO14443A_READER_LISTEN);
  46
  47         LED_A_ON();
  48         LED_B_OFF();
  49         LED_C_OFF();
  50
  51         while (true) {
  52                 if(!iso14443a_select_card(uid, NULL, &cuid)) {
  53                         if (MF_DBGLEVEL >= 1)   Dbprintf("Can't select card");
  54                         break;
  55                 };
  56
  57                 if(mifare_classic_auth(pcs, cuid, blockNo, keyType, ui64Key, AUTH_FIRST)) {
  58                         if (MF_DBGLEVEL >= 1)   Dbprintf("Auth error");
  59                         break;
  60                 };
  61
  62                 if(mifare_classic_readblock(pcs, cuid, blockNo, dataoutbuf)) {
  63                         if (MF_DBGLEVEL >= 1)   Dbprintf("Read block error");
  64                         break;
  65                 };
  66
  67                 if(mifare_classic_halt(pcs, cuid)) {
  68                         if (MF_DBGLEVEL >= 1)   Dbprintf("Halt error");
  69                         break;
  70                 };
  71
  72                 isOK = 1;
  73                 break;
  74         }
  75
  76         //  ----------------------------- crypto1 destroy
  77         crypto1_destroy(pcs);
  78
  79         if (MF_DBGLEVEL >= 2)   DbpString("READ BLOCK FINISHED");
  80
  81         LED_B_ON();
  82         cmd_send(CMD_ACK,isOK,0,0,dataoutbuf,16);
  83         LED_B_OFF();
  84
  85         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
  86         LEDsoff();
  87 }
  88
  89
  90 void MifareUC_Auth1(uint8_t arg0, uint8_t *datain){
  91
  92         byte_t isOK = 0;
  93         byte_t dataoutbuf[16] = {0x00};
  94         uint8_t uid[10] = {0x00};
  95         uint32_t cuid;
  96
  97         LED_A_ON();
  98         LED_B_OFF();
  99         LED_C_OFF();
 100
 101         clear_trace();
 102         iso14443a_setup(FPGA_HF_ISO14443A_READER_LISTEN);
 103
 104         if(!iso14443a_select_card(uid, NULL, &cuid)) {
 105                 if (MF_DBGLEVEL >= MF_DBG_ERROR)
 106                         Dbprintf("Can't select card");
 107                 //OnError(0);
 108                 return;
 109         };
 110
 111         if(mifare_ultra_auth1(cuid, dataoutbuf)){
 112                 if (MF_DBGLEVEL >= MF_DBG_ERROR)
 113                         Dbprintf("Authentication part1: Fail.");
 114                 //OnError(1);
 115                 return;
 116         }
 117
 118         isOK = 1;
 119         if (MF_DBGLEVEL >= MF_DBG_EXTENDED)
 120                 DbpString("AUTH 1 FINISHED");
 121
 122     cmd_send(CMD_ACK,isOK,cuid,0,dataoutbuf,11);
 123         LEDsoff();
 124 }
 125 void MifareUC_Auth2(uint32_t arg0, uint8_t *datain){
 126
 127         uint32_t cuid = arg0;
 128         uint8_t key[16] = {0x00};
 129         byte_t isOK = 0;
 130         byte_t dataoutbuf[16] = {0x00};
 131
 132         memcpy(key, datain, 16);
 133
 134         LED_A_ON();
 135         LED_B_OFF();
 136         LED_C_OFF();
 137
 138         if(mifare_ultra_auth2(cuid, key, dataoutbuf)){
 139             if (MF_DBGLEVEL >= MF_DBG_ERROR)
 140                         Dbprintf("Authentication part2: Fail...");
 141                 //OnError(1);
 142                 return;
 143         }
 144
 145         isOK = 1;
 146         if (MF_DBGLEVEL >= MF_DBG_EXTENDED)
 147                 DbpString("AUTH 2 FINISHED");
 148
 149         cmd_send(CMD_ACK,isOK,0,0,dataoutbuf,11);
 150         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 151         LEDsoff();
 152 }
 153
 154 void MifareUReadBlock(uint8_t arg0,uint8_t *datain)
 155 {
 156         uint8_t blockNo = arg0;
 157         byte_t dataout[16] = {0x00};
 158         uint8_t uid[10] = {0x00};
 159         uint32_t cuid;
 160
 161         LED_A_ON();
 162         LED_B_OFF();
 163         LED_C_OFF();
 164
 165         clear_trace();
 166         iso14443a_setup(FPGA_HF_ISO14443A_READER_LISTEN);
 167
 168         int len = iso14443a_select_card(uid, NULL, &cuid);
 169         if(!len) {
 170                 if (MF_DBGLEVEL >= MF_DBG_ERROR)        Dbprintf("Can't select card");
 171                 //OnError(1);
 172                 return;
 173                 };
 174
 175         len = mifare_ultra_readblock(cuid, blockNo, dataout);
 176         if(len) {
 177                 if (MF_DBGLEVEL >= MF_DBG_ERROR)        Dbprintf("Read block error");
 178                 //OnError(2);
 179                 return;
 180                 };
 181
 182         len = mifare_ultra_halt(cuid);
 183         if(len) {
 184                 if (MF_DBGLEVEL >= MF_DBG_ERROR)        Dbprintf("Halt error");
 185                 //OnError(3);
 186                 return;
 187                 };
 188
 189     cmd_send(CMD_ACK,1,0,0,dataout,16);
 190         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 191         LEDsoff();
 192 }
 193
 194 //-----------------------------------------------------------------------------
 195 // Select, Authenticate, Read a MIFARE tag.
 196 // read sector (data = 4 x 16 bytes = 64 bytes, or 16 x 16 bytes = 256 bytes)
 197 //-----------------------------------------------------------------------------
 198 void MifareReadSector(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain)
 199 {
 200   // params
 201         uint8_t sectorNo = arg0;
 202         uint8_t keyType = arg1;
 203         uint64_t ui64Key = 0;
 204         ui64Key = bytes_to_num(datain, 6);
 205
 206         // variables
 207         byte_t isOK = 0;
 208         byte_t dataoutbuf[16 * 16];
 209         uint8_t uid[10];
 210         uint32_t cuid;
 211         struct Crypto1State mpcs = {0, 0};
 212         struct Crypto1State *pcs;
 213         pcs = &mpcs;
 214
 215         // clear trace
 216         clear_trace();
 217
 218         iso14443a_setup(FPGA_HF_ISO14443A_READER_LISTEN);
 219
 220         LED_A_ON();
 221         LED_B_OFF();
 222         LED_C_OFF();
 223
 224         isOK = 1;
 225         if(!iso14443a_select_card(uid, NULL, &cuid)) {
 226                 isOK = 0;
 227                 if (MF_DBGLEVEL >= 1)   Dbprintf("Can't select card");
 228         }
 229
 230
 231         if(isOK && mifare_classic_auth(pcs, cuid, FirstBlockOfSector(sectorNo), keyType, ui64Key, AUTH_FIRST)) {
 232                 isOK = 0;
 233                 if (MF_DBGLEVEL >= 1)   Dbprintf("Auth error");
 234         }
 235
 236         for (uint8_t blockNo = 0; isOK && blockNo < NumBlocksPerSector(sectorNo); blockNo++) {
 237                 if(mifare_classic_readblock(pcs, cuid, FirstBlockOfSector(sectorNo) + blockNo, dataoutbuf + 16 * blockNo)) {
 238                         isOK = 0;
 239                         if (MF_DBGLEVEL >= 1)   Dbprintf("Read sector %2d block %2d error", sectorNo, blockNo);
 240                         break;
 241                 }
 242         }
 243
 244         if(mifare_classic_halt(pcs, cuid)) {
 245                 if (MF_DBGLEVEL >= 1)   Dbprintf("Halt error");
 246         }
 247
 248         //  ----------------------------- crypto1 destroy
 249         crypto1_destroy(pcs);
 250
 251         if (MF_DBGLEVEL >= 2) DbpString("READ SECTOR FINISHED");
 252
 253         LED_B_ON();
 254         cmd_send(CMD_ACK,isOK,0,0,dataoutbuf,16*NumBlocksPerSector(sectorNo));
 255         LED_B_OFF();
 256
 257         // Thats it...
 258         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 259         LEDsoff();
 260 }
 261
 262 void MifareUReadCard(uint8_t arg0, int arg1, uint8_t *datain)
 263 {
 264   // params
 265         uint8_t sectorNo = arg0;
 266         int Pages = arg1;
 267         int count_Pages = 0;
 268         byte_t dataout[176] = {0x00};;
 269         uint8_t uid[10] = {0x00};
 270         uint32_t cuid;
 271
 272         LED_A_ON();
 273         LED_B_OFF();
 274         LED_C_OFF();
 275
 276         if (MF_DBGLEVEL >= MF_DBG_ALL)
 277                 Dbprintf("Pages %d",Pages);
 278
 279         clear_trace();
 280         iso14443a_setup(FPGA_HF_ISO14443A_READER_LISTEN);
 281
 282         int len = iso14443a_select_card(uid, NULL, &cuid);
 283
 284         if (!len) {
 285                 if (MF_DBGLEVEL >= MF_DBG_ERROR)
 286                         Dbprintf("Can't select card");
 287                 //OnError(1);
 288                 return;
 289         }
 290
 291         for (int i = 0; i < Pages; i++){
 292
 293                 len = mifare_ultra_readblock(cuid, sectorNo * 4 + i, dataout + 4 * i);
 294
 295                 if (len) {
 296                         if (MF_DBGLEVEL >= MF_DBG_ERROR)
 297                                 Dbprintf("Read block %d error",i);
 298                         //OnError(2);
 299                         return;
 300                 } else {
 301                         count_Pages++;
 302                 }
 303         }
 304
 305         len = mifare_ultra_halt(cuid);
 306         if (len) {
 307                 if (MF_DBGLEVEL >= MF_DBG_ERROR)
 308                         Dbprintf("Halt error");
 309                 //OnError(3);
 310                 return;
 311         }
 312
 313         if (MF_DBGLEVEL >= MF_DBG_ALL) {
 314                 Dbprintf("Pages read %d", count_Pages);
 315         }
 316
 317         len = 16*4; //64 bytes
 318
 319         // Read a UL-C
 320         if (Pages == 44 && count_Pages > 16)
 321                 len = 176;
 322
 323         cmd_send(CMD_ACK, 1, 0, 0, dataout, len);
 324         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 325         LEDsoff();
 326 }
 327
 328
 329 //-----------------------------------------------------------------------------
 330 // Select, Authenticate, Write a MIFARE tag.
 331 // read block
 332 //-----------------------------------------------------------------------------
 333 void MifareWriteBlock(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain)
 334 {
 335         // params
 336         uint8_t blockNo = arg0;
 337         uint8_t keyType = arg1;
 338         uint64_t ui64Key = 0;
 339         byte_t blockdata[16];
 340
 341         ui64Key = bytes_to_num(datain, 6);
 342         memcpy(blockdata, datain + 10, 16);
 343
 344         // variables
 345         byte_t isOK = 0;
 346         uint8_t uid[10];
 347         uint32_t cuid;
 348         struct Crypto1State mpcs = {0, 0};
 349         struct Crypto1State *pcs;
 350         pcs = &mpcs;
 351
 352         // clear trace
 353         clear_trace();
 354
 355         iso14443a_setup(FPGA_HF_ISO14443A_READER_LISTEN);
 356
 357         LED_A_ON();
 358         LED_B_OFF();
 359         LED_C_OFF();
 360
 361         while (true) {
 362                         if(!iso14443a_select_card(uid, NULL, &cuid)) {
 363                         if (MF_DBGLEVEL >= 1)   Dbprintf("Can't select card");
 364                         break;
 365                 };
 366
 367                 if(mifare_classic_auth(pcs, cuid, blockNo, keyType, ui64Key, AUTH_FIRST)) {
 368                         if (MF_DBGLEVEL >= 1)   Dbprintf("Auth error");
 369                         break;
 370                 };
 371
 372                 if(mifare_classic_writeblock(pcs, cuid, blockNo, blockdata)) {
 373                         if (MF_DBGLEVEL >= 1)   Dbprintf("Write block error");
 374                         break;
 375                 };
 376
 377                 if(mifare_classic_halt(pcs, cuid)) {
 378                         if (MF_DBGLEVEL >= 1)   Dbprintf("Halt error");
 379                         break;
 380                 };
 381
 382                 isOK = 1;
 383                 break;
 384         }
 385
 386         //  ----------------------------- crypto1 destroy
 387         crypto1_destroy(pcs);
 388
 389         if (MF_DBGLEVEL >= 2)   DbpString("WRITE BLOCK FINISHED");
 390
 391         LED_B_ON();
 392         cmd_send(CMD_ACK,isOK,0,0,0,0);
 393         LED_B_OFF();
 394
 395
 396         // Thats it...
 397         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 398         LEDsoff();
 399 }
 400
 401 void MifareUWriteBlock(uint8_t arg0, uint8_t *datain)
 402 {
 403         // params
 404         uint8_t blockNo = arg0;
 405         byte_t blockdata[16] = {0x00};
 406
 407         memcpy(blockdata, datain,16);
 408
 409         // variables
 410         byte_t isOK = 0;
 411         uint8_t uid[10] = {0x00};
 412         uint32_t cuid;
 413
 414                 clear_trace();
 415                 iso14443a_setup(FPGA_HF_ISO14443A_READER_LISTEN);
 416
 417         LED_A_ON();
 418         LED_B_OFF();
 419         LED_C_OFF();
 420
 421         while (true) {
 422                 if(!iso14443a_select_card(uid, NULL, &cuid)) {
 423                         if (MF_DBGLEVEL >= 1)   Dbprintf("Can't select card");
 424                         break;
 425                 };
 426
 427                 if(mifare_ultra_writeblock(cuid, blockNo, blockdata)) {
 428                         if (MF_DBGLEVEL >= 1)   Dbprintf("Write block error");
 429                         break;
 430                 };
 431
 432                 if(mifare_ultra_halt(cuid)) {
 433                         if (MF_DBGLEVEL >= 1)   Dbprintf("Halt error");
 434                         break;
 435                 };
 436
 437                 isOK = 1;
 438                 break;
 439         }
 440
 441         if (MF_DBGLEVEL >= 2)   DbpString("WRITE BLOCK FINISHED");
 442
 443                 cmd_send(CMD_ACK,isOK,0,0,0,0);
 444         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 445         LEDsoff();
 446 }
 447
 448 void MifareUWriteBlock_Special(uint8_t arg0, uint8_t *datain)
 449 {
 450         // params
 451         uint8_t blockNo = arg0;
 452         byte_t blockdata[4] = {0x00};
 453
 454         memcpy(blockdata, datain,4);
 455
 456         // variables
 457         byte_t isOK = 0;
 458         uint8_t uid[10] = {0x00};
 459         uint32_t cuid;
 460
 461         clear_trace();
 462         iso14443a_setup(FPGA_HF_ISO14443A_READER_LISTEN);
 463
 464         LED_A_ON();
 465         LED_B_OFF();
 466         LED_C_OFF();
 467
 468         while (true) {
 469                 if(!iso14443a_select_card(uid, NULL, &cuid)) {
 470                         if (MF_DBGLEVEL >= 1)   Dbprintf("Can't select card");
 471                         break;
 472                 };
 473
 474                 if(mifare_ultra_special_writeblock(cuid, blockNo, blockdata)) {
 475                         if (MF_DBGLEVEL >= 1)   Dbprintf("Write block error");
 476                         break;
 477                 };
 478
 479                 if(mifare_ultra_halt(cuid)) {
 480                         if (MF_DBGLEVEL >= 1)   Dbprintf("Halt error");
 481                         break;
 482                 };
 483
 484                 isOK = 1;
 485                 break;
 486         }
 487
 488         if (MF_DBGLEVEL >= 2)   DbpString("WRITE BLOCK FINISHED");
 489
 490         cmd_send(CMD_ACK,isOK,0,0,0,0);
 491         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 492         LEDsoff();
 493 }
 494
 495 // Return 1 if the nonce is invalid else return 0
 496 int valid_nonce(uint32_t Nt, uint32_t NtEnc, uint32_t Ks1, uint8_t *parity) {
 497         return ((oddparity((Nt >> 24) & 0xFF) == ((parity[0]) ^ oddparity((NtEnc >> 24) & 0xFF) ^ BIT(Ks1,16))) & \
 498         (oddparity((Nt >> 16) & 0xFF) == ((parity[1]) ^ oddparity((NtEnc >> 16) & 0xFF) ^ BIT(Ks1,8))) & \
 499         (oddparity((Nt >> 8) & 0xFF) == ((parity[2]) ^ oddparity((NtEnc >> 8) & 0xFF) ^ BIT(Ks1,0)))) ? 1 : 0;
 500 }
 501
 502
 503 //-----------------------------------------------------------------------------
 504 // MIFARE nested authentication.
 505 //
 506 //-----------------------------------------------------------------------------
 507 void MifareNested(uint32_t arg0, uint32_t arg1, uint32_t calibrate, uint8_t *datain)
 508 {
 509         // params
 510         uint8_t blockNo = arg0 & 0xff;
 511         uint8_t keyType = (arg0 >> 8) & 0xff;
 512         uint8_t targetBlockNo = arg1 & 0xff;
 513         uint8_t targetKeyType = (arg1 >> 8) & 0xff;
 514         uint64_t ui64Key = 0;
 515
 516         ui64Key = bytes_to_num(datain, 6);
 517
 518         // variables
 519         uint16_t rtr, i, j, len;
 520         uint16_t davg;
 521         static uint16_t dmin, dmax;
 522         uint8_t uid[10];
 523         uint32_t cuid, nt1, nt2, nttmp, nttest, ks1;
 524         uint8_t par[1];
 525         uint32_t target_nt[2], target_ks[2];
 526
 527         uint8_t par_array[4];
 528         uint16_t ncount = 0;
 529         struct Crypto1State mpcs = {0, 0};
 530         struct Crypto1State *pcs;
 531         pcs = &mpcs;
 532         uint8_t receivedAnswer[MAX_MIFARE_FRAME_SIZE];
 533
 534         uint32_t auth1_time, auth2_time;
 535         static uint16_t delta_time;
 536
 537         // free eventually allocated BigBuf memory
 538         BigBuf_free();
 539         // clear trace
 540         clear_trace();
 541         set_tracing(false);
 542
 543         iso14443a_setup(FPGA_HF_ISO14443A_READER_LISTEN);
 544
 545         LED_A_ON();
 546         LED_C_OFF();
 547
 548
 549         // statistics on nonce distance
 550         if (calibrate) {        // for first call only. Otherwise reuse previous calibration
 551                 LED_B_ON();
 552                 WDT_HIT();
 553
 554                 davg = dmax = 0;
 555                 dmin = 2000;
 556                 delta_time = 0;
 557
 558                 for (rtr = 0; rtr < 17; rtr++) {
 559
 560                         // prepare next select. No need to power down the card.
 561                         if(mifare_classic_halt(pcs, cuid)) {
 562                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Nested: Halt error");
 563                                 rtr--;
 564                                 continue;
 565                         }
 566
 567                         if(!iso14443a_select_card(uid, NULL, &cuid)) {
 568                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Nested: Can't select card");
 569                                 rtr--;
 570                                 continue;
 571                         };
 572
 573                         auth1_time = 0;
 574                         if(mifare_classic_authex(pcs, cuid, blockNo, keyType, ui64Key, AUTH_FIRST, &nt1, &auth1_time)) {
 575                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Nested: Auth1 error");
 576                                 rtr--;
 577                                 continue;
 578                         };
 579
 580                         if (delta_time) {
 581                                 auth2_time = auth1_time + delta_time;
 582                         } else {
 583                                 auth2_time = 0;
 584                         }
 585                         if(mifare_classic_authex(pcs, cuid, blockNo, keyType, ui64Key, AUTH_NESTED, &nt2, &auth2_time)) {
 586                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Nested: Auth2 error");
 587                                 rtr--;
 588                                 continue;
 589                         };
 590
 591                         nttmp = prng_successor(nt1, 100);                               //NXP Mifare is typical around 840,but for some unlicensed/compatible mifare card this can be 160
 592                         for (i = 101; i < 1200; i++) {
 593                                 nttmp = prng_successor(nttmp, 1);
 594                                 if (nttmp == nt2) break;
 595                         }
 596
 597                         if (i != 1200) {
 598                                 if (rtr != 0) {
 599                                         davg += i;
 600                                         dmin = MIN(dmin, i);
 601                                         dmax = MAX(dmax, i);
 602                                 }
 603                                 else {
 604                                         delta_time = auth2_time - auth1_time + 32;  // allow some slack for proper timing
 605                                 }
 606                                 if (MF_DBGLEVEL >= 3) Dbprintf("Nested: calibrating... ntdist=%d", i);
 607                         }
 608                 }
 609
 610                 if (rtr <= 1)   return;
 611
 612                 davg = (davg + (rtr - 1)/2) / (rtr - 1);
 613
 614                 if (MF_DBGLEVEL >= 3) Dbprintf("min=%d max=%d avg=%d, delta_time=%d", dmin, dmax, davg, delta_time);
 615
 616                 dmin = davg - 2;
 617                 dmax = davg + 2;
 618
 619                 LED_B_OFF();
 620
 621         }
 622 //  -------------------------------------------------------------------------------------------------
 623
 624         LED_C_ON();
 625
 626         //  get crypted nonces for target sector
 627         for(i=0; i < 2; i++) { // look for exactly two different nonces
 628
 629                 target_nt[i] = 0;
 630                 while(target_nt[i] == 0) { // continue until we have an unambiguous nonce
 631
 632                         // prepare next select. No need to power down the card.
 633                         if(mifare_classic_halt(pcs, cuid)) {
 634                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Nested: Halt error");
 635                                 continue;
 636                         }
 637
 638                         if(!iso14443a_select_card(uid, NULL, &cuid)) {
 639                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Nested: Can't select card");
 640                                 continue;
 641                         };
 642
 643                         auth1_time = 0;
 644                         if(mifare_classic_authex(pcs, cuid, blockNo, keyType, ui64Key, AUTH_FIRST, &nt1, &auth1_time)) {
 645                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Nested: Auth1 error");
 646                                 continue;
 647                         };
 648
 649                         // nested authentication
 650                         auth2_time = auth1_time + delta_time;
 651                         len = mifare_sendcmd_shortex(pcs, AUTH_NESTED, 0x60 + (targetKeyType & 0x01), targetBlockNo, receivedAnswer, par, &auth2_time);
 652                         if (len != 4) {
 653                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Nested: Auth2 error len=%d", len);
 654                                 continue;
 655                         };
 656
 657                         nt2 = bytes_to_num(receivedAnswer, 4);
 658                         if (MF_DBGLEVEL >= 3) Dbprintf("Nonce#%d: Testing nt1=%08x nt2enc=%08x nt2par=%02x", i+1, nt1, nt2, par[0]);
 659
 660                         // Parity validity check
 661                         for (j = 0; j < 4; j++) {
 662                                 par_array[j] = (oddparity(receivedAnswer[j]) != ((par[0] >> (7-j)) & 0x01));
 663                         }
 664
 665                         ncount = 0;
 666                         nttest = prng_successor(nt1, dmin - 1);
 667                         for (j = dmin; j < dmax + 1; j++) {
 668                                 nttest = prng_successor(nttest, 1);
 669                                 ks1 = nt2 ^ nttest;
 670
 671                                 if (valid_nonce(nttest, nt2, ks1, par_array)){
 672                                         if (ncount > 0) {               // we are only interested in disambiguous nonces, try again
 673                                                 if (MF_DBGLEVEL >= 3) Dbprintf("Nonce#%d: dismissed (ambigous), ntdist=%d", i+1, j);
 674                                                 target_nt[i] = 0;
 675                                                 break;
 676                                         }
 677                                         target_nt[i] = nttest;
 678                                         target_ks[i] = ks1;
 679                                         ncount++;
 680                                         if (i == 1 && target_nt[1] == target_nt[0]) { // we need two different nonces
 681                                                 target_nt[i] = 0;
 682                                                 if (MF_DBGLEVEL >= 3) Dbprintf("Nonce#2: dismissed (= nonce#1), ntdist=%d", j);
 683                                                 break;
 684                                         }
 685                                         if (MF_DBGLEVEL >= 3) Dbprintf("Nonce#%d: valid, ntdist=%d", i+1, j);
 686                                 }
 687                         }
 688                         if (target_nt[i] == 0 && j == dmax+1 && MF_DBGLEVEL >= 3) Dbprintf("Nonce#%d: dismissed (all invalid)", i+1);
 689                 }
 690         }
 691
 692         LED_C_OFF();
 693
 694         //  ----------------------------- crypto1 destroy
 695         crypto1_destroy(pcs);
 696
 697         byte_t buf[4 + 4 * 4];
 698         memcpy(buf, &cuid, 4);
 699         memcpy(buf+4, &target_nt[0], 4);
 700         memcpy(buf+8, &target_ks[0], 4);
 701         memcpy(buf+12, &target_nt[1], 4);
 702         memcpy(buf+16, &target_ks[1], 4);
 703
 704         LED_B_ON();
 705         cmd_send(CMD_ACK, 0, 2, targetBlockNo + (targetKeyType * 0x100), buf, sizeof(buf));
 706         LED_B_OFF();
 707
 708         if (MF_DBGLEVEL >= 3)   DbpString("NESTED FINISHED");
 709
 710         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 711         LEDsoff();
 712         set_tracing(TRUE);
 713 }
 714
 715 //-----------------------------------------------------------------------------
 716 // MIFARE check keys. key count up to 85.
 717 //
 718 //-----------------------------------------------------------------------------
 719 void MifareChkKeys(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain)
 720 {
 721   // params
 722         uint8_t blockNo = arg0;
 723         uint8_t keyType = arg1;
 724         uint8_t keyCount = arg2;
 725         uint64_t ui64Key = 0;
 726
 727         // variables
 728         int i;
 729         byte_t isOK = 0;
 730         uint8_t uid[10];
 731         uint32_t cuid;
 732         struct Crypto1State mpcs = {0, 0};
 733         struct Crypto1State *pcs;
 734         pcs = &mpcs;
 735
 736         // clear debug level
 737         int OLD_MF_DBGLEVEL = MF_DBGLEVEL;
 738         MF_DBGLEVEL = MF_DBG_NONE;
 739
 740         // clear trace
 741         clear_trace();
 742         set_tracing(TRUE);
 743
 744         iso14443a_setup(FPGA_HF_ISO14443A_READER_LISTEN);
 745
 746         LED_A_ON();
 747         LED_B_OFF();
 748         LED_C_OFF();
 749
 750         for (i = 0; i < keyCount; i++) {
 751                 if(mifare_classic_halt(pcs, cuid)) {
 752                         if (MF_DBGLEVEL >= 1)   Dbprintf("ChkKeys: Halt error");
 753                 }
 754
 755                 if(!iso14443a_select_card(uid, NULL, &cuid)) {
 756                         if (OLD_MF_DBGLEVEL >= 1)       Dbprintf("ChkKeys: Can't select card");
 757                         break;
 758                 };
 759
 760                 ui64Key = bytes_to_num(datain + i * 6, 6);
 761                 if(mifare_classic_auth(pcs, cuid, blockNo, keyType, ui64Key, AUTH_FIRST)) {
 762                         continue;
 763                 };
 764
 765                 isOK = 1;
 766                 break;
 767         }
 768
 769         //  ----------------------------- crypto1 destroy
 770         crypto1_destroy(pcs);
 771
 772         LED_B_ON();
 773     cmd_send(CMD_ACK,isOK,0,0,datain + i * 6,6);
 774         LED_B_OFF();
 775
 776         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 777         LEDsoff();
 778
 779         // restore debug level
 780         MF_DBGLEVEL = OLD_MF_DBGLEVEL;
 781 }
 782
 783 //-----------------------------------------------------------------------------
 784 // MIFARE commands set debug level
 785 //
 786 //-----------------------------------------------------------------------------
 787 void MifareSetDbgLvl(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){
 788         MF_DBGLEVEL = arg0;
 789         Dbprintf("Debug level: %d", MF_DBGLEVEL);
 790 }
 791
 792 //-----------------------------------------------------------------------------
 793 // Work with emulator memory
 794 //
 795 //-----------------------------------------------------------------------------
 796 void MifareEMemClr(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){
 797         emlClearMem();
 798 }
 799
 800 void MifareEMemSet(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){
 801         emlSetMem(datain, arg0, arg1); // data, block num, blocks count
 802 }
 803
 804 void MifareEMemGet(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){
 805         byte_t buf[USB_CMD_DATA_SIZE];
 806         emlGetMem(buf, arg0, arg1); // data, block num, blocks count (max 4)
 807
 808         LED_B_ON();
 809         cmd_send(CMD_ACK,arg0,arg1,0,buf,USB_CMD_DATA_SIZE);
 810         LED_B_OFF();
 811 }
 812
 813 //-----------------------------------------------------------------------------
 814 // Load a card into the emulator memory
 815 //
 816 //-----------------------------------------------------------------------------
 817 void MifareECardLoad(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){
 818         uint8_t numSectors = arg0;
 819         uint8_t keyType = arg1;
 820         uint64_t ui64Key = 0;
 821         uint32_t cuid;
 822         struct Crypto1State mpcs = {0, 0};
 823         struct Crypto1State *pcs;
 824         pcs = &mpcs;
 825
 826         // variables
 827         byte_t dataoutbuf[16];
 828         byte_t dataoutbuf2[16];
 829         uint8_t uid[10];
 830
 831         // clear trace
 832         clear_trace();
 833         set_tracing(false);
 834
 835         iso14443a_setup(FPGA_HF_ISO14443A_READER_LISTEN);
 836
 837         LED_A_ON();
 838         LED_B_OFF();
 839         LED_C_OFF();
 840
 841         bool isOK = true;
 842
 843         if(!iso14443a_select_card(uid, NULL, &cuid)) {
 844                 isOK = false;
 845                 if (MF_DBGLEVEL >= 1)   Dbprintf("Can't select card");
 846         }
 847
 848         for (uint8_t sectorNo = 0; isOK && sectorNo < numSectors; sectorNo++) {
 849                 ui64Key = emlGetKey(sectorNo, keyType);
 850                 if (sectorNo == 0){
 851                         if(isOK && mifare_classic_auth(pcs, cuid, FirstBlockOfSector(sectorNo), keyType, ui64Key, AUTH_FIRST)) {
 852                                 isOK = false;
 853                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Sector[%2d]. Auth error", sectorNo);
 854                                 break;
 855                         }
 856                 } else {
 857                         if(isOK && mifare_classic_auth(pcs, cuid, FirstBlockOfSector(sectorNo), keyType, ui64Key, AUTH_NESTED)) {
 858                                 isOK = false;
 859                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Sector[%2d]. Auth nested error", sectorNo);
 860                                 break;
 861                         }
 862                 }
 863
 864                 for (uint8_t blockNo = 0; isOK && blockNo < NumBlocksPerSector(sectorNo); blockNo++) {
 865                         if(isOK && mifare_classic_readblock(pcs, cuid, FirstBlockOfSector(sectorNo) + blockNo, dataoutbuf)) {
 866                                 isOK = false;
 867                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Error reading sector %2d block %2d", sectorNo, blockNo);
 868                                 break;
 869                         };
 870                         if (isOK) {
 871                                 if (blockNo < NumBlocksPerSector(sectorNo) - 1) {
 872                                         emlSetMem(dataoutbuf, FirstBlockOfSector(sectorNo) + blockNo, 1);
 873                                 } else {        // sector trailer, keep the keys, set only the AC
 874                                         emlGetMem(dataoutbuf2, FirstBlockOfSector(sectorNo) + blockNo, 1);
 875                                         memcpy(&dataoutbuf2[6], &dataoutbuf[6], 4);
 876                                         emlSetMem(dataoutbuf2,  FirstBlockOfSector(sectorNo) + blockNo, 1);
 877                                 }
 878                         }
 879                 }
 880
 881         }
 882
 883         if(mifare_classic_halt(pcs, cuid)) {
 884                 if (MF_DBGLEVEL >= 1)   Dbprintf("Halt error");
 885         };
 886
 887         //  ----------------------------- crypto1 destroy
 888         crypto1_destroy(pcs);
 889
 890         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 891         LEDsoff();
 892
 893         if (MF_DBGLEVEL >= 2) DbpString("EMUL FILL SECTORS FINISHED");
 894
 895 }
 896
 897
 898 //-----------------------------------------------------------------------------
 899 // Work with "magic Chinese" card (email him: ouyangweidaxian@live.cn)
 900 //
 901 //-----------------------------------------------------------------------------
 902 void MifareCSetBlock(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){
 903
 904   // params
 905         uint8_t needWipe = arg0;
 906         // bit 0 - need get UID
 907         // bit 1 - need wupC
 908         // bit 2 - need HALT after sequence
 909         // bit 3 - need init FPGA and field before sequence
 910         // bit 4 - need reset FPGA and LED
 911         uint8_t workFlags = arg1;
 912         uint8_t blockNo = arg2;
 913
 914         // card commands
 915         uint8_t wupC1[]       = { 0x40 };
 916         uint8_t wupC2[]       = { 0x43 };
 917         uint8_t wipeC[]       = { 0x41 };
 918
 919         // variables
 920         byte_t isOK = 0;
 921         uint8_t uid[10] = {0x00};
 922         uint8_t d_block[18] = {0x00};
 923         uint32_t cuid;
 924
 925         uint8_t receivedAnswer[MAX_MIFARE_FRAME_SIZE];
 926         uint8_t receivedAnswerPar[MAX_MIFARE_PARITY_SIZE];
 927
 928         // reset FPGA and LED
 929         if (workFlags & 0x08) {
 930                 LED_A_ON();
 931                 LED_B_OFF();
 932                 LED_C_OFF();
 933
 934                 clear_trace();
 935                 set_tracing(TRUE);
 936                 iso14443a_setup(FPGA_HF_ISO14443A_READER_LISTEN);
 937         }
 938
 939         while (true) {
 940
 941                 // get UID from chip
 942                 if (workFlags & 0x01) {
 943                         if(!iso14443a_select_card(uid, NULL, &cuid)) {
 944                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Can't select card");
 945                                 break;
 946                         };
 947
 948                         if(mifare_classic_halt(NULL, cuid)) {
 949                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Halt error");
 950                                 break;
 951                         };
 952                 };
 953
 954                 // reset chip
 955                 if (needWipe){
 956                         ReaderTransmitBitsPar(wupC1,7,0, NULL);
 957                         if(!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) {
 958                                 if (MF_DBGLEVEL >= 1)   Dbprintf("wupC1 error");
 959                                 break;
 960                         };
 961
 962                         ReaderTransmit(wipeC, sizeof(wipeC), NULL);
 963                         if(!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) {
 964                                 if (MF_DBGLEVEL >= 1)   Dbprintf("wipeC error");
 965                                 break;
 966                         };
 967
 968                         if(mifare_classic_halt(NULL, cuid)) {
 969                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Halt error");
 970                                 break;
 971                         };
 972                 };
 973
 974                 // write block
 975                 if (workFlags & 0x02) {
 976                         ReaderTransmitBitsPar(wupC1,7,0, NULL);
 977                         if(!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) {
 978                                 if (MF_DBGLEVEL >= 1)   Dbprintf("wupC1 error");
 979                                 break;
 980                         };
 981
 982                         ReaderTransmit(wupC2, sizeof(wupC2), NULL);
 983                         if(!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) {
 984                                 if (MF_DBGLEVEL >= 1)   Dbprintf("wupC2 error");
 985                                 break;
 986                         };
 987                 }
 988
 989                 if ((mifare_sendcmd_short(NULL, 0, 0xA0, blockNo, receivedAnswer, receivedAnswerPar, NULL) != 1) || (receivedAnswer[0] != 0x0a)) {
 990                         if (MF_DBGLEVEL >= 1)   Dbprintf("write block send command error");
 991                         break;
 992                 };
 993
 994                 memcpy(d_block, datain, 16);
 995                 AppendCrc14443a(d_block, 16);
 996
 997                 ReaderTransmit(d_block, sizeof(d_block), NULL);
 998                 if ((ReaderReceive(receivedAnswer, receivedAnswerPar) != 1) || (receivedAnswer[0] != 0x0a)) {
 999                         if (MF_DBGLEVEL >= 1)   Dbprintf("write block send data error");
1000                         break;
1001                 };
1002
1003                 if (workFlags & 0x04) {
1004                         if (mifare_classic_halt(NULL, cuid)) {
1005                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Halt error");
1006                                 break;
1007                         };
1008                 }
1009
1010                 isOK = 1;
1011                 break;
1012         }
1013
1014         LED_B_ON();
1015         cmd_send(CMD_ACK,isOK,0,0,uid,4);
1016         LED_B_OFF();
1017
1018         if ((workFlags & 0x10) || (!isOK)) {
1019                 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1020                 LEDsoff();
1021         }
1022 }
1023
1024
1025 void MifareCGetBlock(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){
1026
1027   // params
1028         // bit 1 - need wupC
1029         // bit 2 - need HALT after sequence
1030         // bit 3 - need init FPGA and field before sequence
1031         // bit 4 - need reset FPGA and LED
1032         uint8_t workFlags = arg0;
1033         uint8_t blockNo = arg2;
1034
1035         // card commands
1036         uint8_t wupC1[]       = { 0x40 };
1037         uint8_t wupC2[]       = { 0x43 };
1038
1039         // variables
1040         byte_t isOK = 0;
1041         uint8_t data[18] = {0x00};
1042         uint32_t cuid = 0;
1043
1044         uint8_t receivedAnswer[MAX_MIFARE_FRAME_SIZE];
1045         uint8_t receivedAnswerPar[MAX_MIFARE_PARITY_SIZE];
1046
1047         if (workFlags & 0x08) {
1048                 LED_A_ON();
1049                 LED_B_OFF();
1050                 LED_C_OFF();
1051
1052                 clear_trace();
1053                 set_tracing(TRUE);
1054                 iso14443a_setup(FPGA_HF_ISO14443A_READER_LISTEN);
1055         }
1056
1057         while (true) {
1058                 if (workFlags & 0x02) {
1059                         ReaderTransmitBitsPar(wupC1,7,0, NULL);
1060                         if(!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) {
1061                                 if (MF_DBGLEVEL >= 1)   Dbprintf("wupC1 error");
1062                                 break;
1063                         };
1064
1065                         ReaderTransmit(wupC2, sizeof(wupC2), NULL);
1066                         if(!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) {
1067                                 if (MF_DBGLEVEL >= 1)   Dbprintf("wupC2 error");
1068                                 break;
1069                         };
1070                 }
1071
1072                 // read block
1073                 if ((mifare_sendcmd_short(NULL, 0, 0x30, blockNo, receivedAnswer, receivedAnswerPar, NULL) != 18)) {
1074                         if (MF_DBGLEVEL >= 1)   Dbprintf("read block send command error");
1075                         break;
1076                 };
1077                 memcpy(data, receivedAnswer, 18);
1078
1079                 if (workFlags & 0x04) {
1080                         if (mifare_classic_halt(NULL, cuid)) {
1081                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Halt error");
1082                                 break;
1083                         };
1084                 }
1085
1086                 isOK = 1;
1087                 break;
1088         }
1089
1090         LED_B_ON();
1091         cmd_send(CMD_ACK,isOK,0,0,data,18);
1092         LED_B_OFF();
1093
1094         if ((workFlags & 0x10) || (!isOK)) {
1095                 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1096                 LEDsoff();
1097         }
1098 }
1099
1100 void MifareCIdent(){
1101
1102         // card commands
1103         uint8_t wupC1[]       = { 0x40 };
1104         uint8_t wupC2[]       = { 0x43 };
1105
1106         // variables
1107         byte_t isOK = 1;
1108
1109         uint8_t receivedAnswer[MAX_MIFARE_FRAME_SIZE];
1110         uint8_t receivedAnswerPar[MAX_MIFARE_PARITY_SIZE];
1111
1112         ReaderTransmitBitsPar(wupC1,7,0, NULL);
1113         if(!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) {
1114                 isOK = 0;
1115         };
1116
1117         ReaderTransmit(wupC2, sizeof(wupC2), NULL);
1118         if(!ReaderReceive(receivedAnswer, receivedAnswerPar) || (receivedAnswer[0] != 0x0a)) {
1119                 isOK = 0;
1120         };
1121
1122         if (mifare_classic_halt(NULL, 0)) {
1123                 isOK = 0;
1124         };
1125
1126         cmd_send(CMD_ACK,isOK,0,0,0,0);
1127 }
1128
1129                         //
1130 // DESFIRE
1131 //
1132
1133 void Mifare_DES_Auth1(uint8_t arg0, uint8_t *datain){
1134
1135         byte_t dataout[11] = {0x00};
1136         uint8_t uid[10] = {0x00};
1137         uint32_t cuid;
1138
1139         clear_trace();
1140         iso14443a_setup(FPGA_HF_ISO14443A_READER_LISTEN);
1141
1142         int len = iso14443a_select_card(uid, NULL, &cuid);
1143         if(!len) {
1144                 if (MF_DBGLEVEL >= MF_DBG_ERROR)
1145                         Dbprintf("Can't select card");
1146                 //OnError(1);
1147                 return;
1148         };
1149
1150         if(mifare_desfire_des_auth1(cuid, dataout)){
1151                 if (MF_DBGLEVEL >= MF_DBG_ERROR)
1152                         Dbprintf("Authentication part1: Fail.");
1153                 //OnError(4);
1154                 return;
1155         }
1156
1157         if (MF_DBGLEVEL >= MF_DBG_EXTENDED) DbpString("AUTH 1 FINISHED");
1158
1159     cmd_send(CMD_ACK,1,cuid,0,dataout, sizeof(dataout));
1160 }
1161
1162 void Mifare_DES_Auth2(uint32_t arg0, uint8_t *datain){
1163
1164         uint32_t cuid = arg0;
1165         uint8_t key[16] = {0x00};
1166         byte_t isOK = 0;
1167         byte_t dataout[12] = {0x00};
1168
1169         memcpy(key, datain, 16);
1170
1171         isOK = mifare_desfire_des_auth2(cuid, key, dataout);
1172
1173         if( isOK) {
1174             if (MF_DBGLEVEL >= MF_DBG_EXTENDED)
1175                         Dbprintf("Authentication part2: Failed");
1176                 //OnError(4);
1177                 return;
1178         }
1179
1180         if (MF_DBGLEVEL >= MF_DBG_EXTENDED)
1181                 DbpString("AUTH 2 FINISHED");
1182
1183         cmd_send(CMD_ACK, isOK, 0, 0, dataout, sizeof(dataout));
1184         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1185         LEDsoff();
1186 }