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