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