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 //
   6 // This code is licensed to you under the terms of the GNU GPL, version 2 or,
   7 // at your option, any later version. See the LICENSE.txt file for the text of
   8 // the license.
   9 //-----------------------------------------------------------------------------
  10 // Routines to support ISO 14443 type A.
  11 //-----------------------------------------------------------------------------
  12
  13 #include "mifarecmd.h"
  14 #include "apps.h"
  15
  16 //-----------------------------------------------------------------------------
  17 // Select, Authenticaate, Read an MIFARE tag.
  18 // read block
  19 //-----------------------------------------------------------------------------
  20 void MifareReadBlock(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain)
  21 {
  22   // params
  23         uint8_t blockNo = arg0;
  24         uint8_t keyType = arg1;
  25         uint64_t ui64Key = 0;
  26         ui64Key = bytes_to_num(datain, 6);
  27
  28         // variables
  29         byte_t isOK = 0;
  30         byte_t dataoutbuf[16];
  31         uint8_t uid[8];
  32         uint32_t cuid;
  33         struct Crypto1State mpcs = {0, 0};
  34         struct Crypto1State *pcs;
  35         pcs = &mpcs;
  36
  37         // clear trace
  38         iso14a_clear_trace();
  39 //      iso14a_set_tracing(false);
  40
  41         iso14443a_setup();
  42
  43         LED_A_ON();
  44         LED_B_OFF();
  45         LED_C_OFF();
  46
  47         while (true) {
  48                 if(!iso14443a_select_card(uid, NULL, &cuid)) {
  49                 if (MF_DBGLEVEL >= 1)   Dbprintf("Can't select card");
  50                         break;
  51                 };
  52
  53                 if(mifare_classic_auth(pcs, cuid, blockNo, keyType, ui64Key, AUTH_FIRST)) {
  54                 if (MF_DBGLEVEL >= 1)   Dbprintf("Auth error");
  55                         break;
  56                 };
  57
  58                 if(mifare_classic_readblock(pcs, cuid, blockNo, dataoutbuf)) {
  59                 if (MF_DBGLEVEL >= 1)   Dbprintf("Read block error");
  60                         break;
  61                 };
  62
  63                 if(mifare_classic_halt(pcs, cuid)) {
  64                 if (MF_DBGLEVEL >= 1)   Dbprintf("Halt error");
  65                         break;
  66                 };
  67
  68                 isOK = 1;
  69                 break;
  70         }
  71
  72         //  ----------------------------- crypto1 destroy
  73         crypto1_destroy(pcs);
  74
  75         if (MF_DBGLEVEL >= 2)   DbpString("READ BLOCK FINISHED");
  76
  77         // add trace trailer
  78         memset(uid, 0x44, 4);
  79         LogTrace(uid, 4, 0, 0, TRUE);
  80
  81 //      UsbCommand ack = {CMD_ACK, {isOK, 0, 0}};
  82 //      memcpy(ack.d.asBytes, dataoutbuf, 16);
  83
  84         LED_B_ON();
  85   cmd_send(CMD_ACK,isOK,0,0,dataoutbuf,16);
  86 //      UsbSendPacket((uint8_t *)&ack, sizeof(UsbCommand));
  87         LED_B_OFF();
  88
  89
  90   // Thats it...
  91         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
  92         LEDsoff();
  93 //  iso14a_set_tracing(TRUE);
  94
  95 }
  96
  97 //-----------------------------------------------------------------------------
  98 // Select, Authenticaate, Read an MIFARE tag.
  99 // read sector (data = 4 x 16 bytes = 64 bytes)
 100 //-----------------------------------------------------------------------------
 101 void MifareReadSector(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain)
 102 {
 103   // params
 104         uint8_t sectorNo = arg0;
 105         uint8_t keyType = arg1;
 106         uint64_t ui64Key = 0;
 107         ui64Key = bytes_to_num(datain, 6);
 108
 109         // variables
 110         byte_t isOK = 0;
 111         byte_t dataoutbuf[16 * 4];
 112         uint8_t uid[8];
 113         uint32_t cuid;
 114         struct Crypto1State mpcs = {0, 0};
 115         struct Crypto1State *pcs;
 116         pcs = &mpcs;
 117
 118         // clear trace
 119         iso14a_clear_trace();
 120 //      iso14a_set_tracing(false);
 121
 122         iso14443a_setup();
 123
 124         LED_A_ON();
 125         LED_B_OFF();
 126         LED_C_OFF();
 127
 128         while (true) {
 129                 if(!iso14443a_select_card(uid, NULL, &cuid)) {
 130                 if (MF_DBGLEVEL >= 1)   Dbprintf("Can't select card");
 131                         break;
 132                 };
 133
 134                 if(mifare_classic_auth(pcs, cuid, sectorNo * 4, keyType, ui64Key, AUTH_FIRST)) {
 135                 if (MF_DBGLEVEL >= 1)   Dbprintf("Auth error");
 136                         break;
 137                 };
 138
 139                 if(mifare_classic_readblock(pcs, cuid, sectorNo * 4 + 0, dataoutbuf + 16 * 0)) {
 140                 if (MF_DBGLEVEL >= 1)   Dbprintf("Read block 0 error");
 141                         break;
 142                 };
 143                 if(mifare_classic_readblock(pcs, cuid, sectorNo * 4 + 1, dataoutbuf + 16 * 1)) {
 144                 if (MF_DBGLEVEL >= 1)   Dbprintf("Read block 1 error");
 145                         break;
 146                 };
 147                 if(mifare_classic_readblock(pcs, cuid, sectorNo * 4 + 2, dataoutbuf + 16 * 2)) {
 148                 if (MF_DBGLEVEL >= 1)   Dbprintf("Read block 2 error");
 149                         break;
 150                 };
 151                 if(mifare_classic_readblock(pcs, cuid, sectorNo * 4 + 3, dataoutbuf + 16 * 3)) {
 152                 if (MF_DBGLEVEL >= 1)   Dbprintf("Read block 3 error");
 153                         break;
 154                 };
 155
 156                 if(mifare_classic_halt(pcs, cuid)) {
 157                 if (MF_DBGLEVEL >= 1)   Dbprintf("Halt error");
 158                         break;
 159                 };
 160
 161                 isOK = 1;
 162                 break;
 163         }
 164
 165         //  ----------------------------- crypto1 destroy
 166         crypto1_destroy(pcs);
 167
 168         if (MF_DBGLEVEL >= 2) DbpString("READ SECTOR FINISHED");
 169
 170         // add trace trailer
 171         memset(uid, 0x44, 4);
 172         LogTrace(uid, 4, 0, 0, TRUE);
 173
 174 //      UsbCommand ack = {CMD_ACK, {isOK, 0, 0}};
 175 //      memcpy(ack.d.asBytes, dataoutbuf, 16 * 2);
 176
 177         LED_B_ON();
 178   cmd_send(CMD_ACK,isOK,0,0,dataoutbuf,32);
 179 //      UsbSendPacket((uint8_t *)&ack, sizeof(UsbCommand));
 180 //      SpinDelay(100);
 181
 182 //      memcpy(ack.d.asBytes, dataoutbuf + 16 * 2, 16 * 2);
 183 //      UsbSendPacket((uint8_t *)&ack, sizeof(UsbCommand));
 184   cmd_send(CMD_ACK,isOK,0,0,dataoutbuf+32, 32);
 185         LED_B_OFF();
 186
 187         // Thats it...
 188         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 189         LEDsoff();
 190 //  iso14a_set_tracing(TRUE);
 191
 192 }
 193
 194 //-----------------------------------------------------------------------------
 195 // Select, Authenticaate, Read an MIFARE tag.
 196 // read block
 197 //-----------------------------------------------------------------------------
 198 void MifareWriteBlock(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain)
 199 {
 200         // params
 201         uint8_t blockNo = arg0;
 202         uint8_t keyType = arg1;
 203         uint64_t ui64Key = 0;
 204         byte_t blockdata[16];
 205
 206         ui64Key = bytes_to_num(datain, 6);
 207         memcpy(blockdata, datain + 10, 16);
 208
 209         // variables
 210         byte_t isOK = 0;
 211         uint8_t uid[8];
 212         uint32_t cuid;
 213         struct Crypto1State mpcs = {0, 0};
 214         struct Crypto1State *pcs;
 215         pcs = &mpcs;
 216
 217         // clear trace
 218         iso14a_clear_trace();
 219 //  iso14a_set_tracing(false);
 220
 221         iso14443a_setup();
 222
 223         LED_A_ON();
 224         LED_B_OFF();
 225         LED_C_OFF();
 226
 227         while (true) {
 228                         if(!iso14443a_select_card(uid, NULL, &cuid)) {
 229                         if (MF_DBGLEVEL >= 1)   Dbprintf("Can't select card");
 230                         break;
 231                 };
 232
 233                 if(mifare_classic_auth(pcs, cuid, blockNo, keyType, ui64Key, AUTH_FIRST)) {
 234                         if (MF_DBGLEVEL >= 1)   Dbprintf("Auth error");
 235                         break;
 236                 };
 237
 238                 if(mifare_classic_writeblock(pcs, cuid, blockNo, blockdata)) {
 239                         if (MF_DBGLEVEL >= 1)   Dbprintf("Write block error");
 240                         break;
 241                 };
 242
 243                 if(mifare_classic_halt(pcs, cuid)) {
 244                         if (MF_DBGLEVEL >= 1)   Dbprintf("Halt error");
 245                         break;
 246                 };
 247
 248                 isOK = 1;
 249                 break;
 250         }
 251
 252         //  ----------------------------- crypto1 destroy
 253         crypto1_destroy(pcs);
 254
 255         if (MF_DBGLEVEL >= 2)   DbpString("WRITE BLOCK FINISHED");
 256
 257         // add trace trailer
 258         memset(uid, 0x44, 4);
 259         LogTrace(uid, 4, 0, 0, TRUE);
 260
 261 //      UsbCommand ack = {CMD_ACK, {isOK, 0, 0}};
 262
 263         LED_B_ON();
 264   cmd_send(CMD_ACK,isOK,0,0,0,0);
 265 //      UsbSendPacket((uint8_t *)&ack, sizeof(UsbCommand));
 266         LED_B_OFF();
 267
 268
 269         // Thats it...
 270         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 271         LEDsoff();
 272 //  iso14a_set_tracing(TRUE);
 273
 274 }
 275
 276 // Return 1 if the nonce is invalid else return 0
 277 int valid_nonce(uint32_t Nt, uint32_t NtEnc, uint32_t Ks1, byte_t * parity) {
 278         return ((oddparity((Nt >> 24) & 0xFF) == ((parity[0]) ^ oddparity((NtEnc >> 24) & 0xFF) ^ BIT(Ks1,16))) & \
 279         (oddparity((Nt >> 16) & 0xFF) == ((parity[1]) ^ oddparity((NtEnc >> 16) & 0xFF) ^ BIT(Ks1,8))) & \
 280         (oddparity((Nt >> 8) & 0xFF) == ((parity[2]) ^ oddparity((NtEnc >> 8) & 0xFF) ^ BIT(Ks1,0)))) ? 1 : 0;
 281 }
 282
 283 //-----------------------------------------------------------------------------
 284 // MIFARE nested authentication.
 285 //
 286 //-----------------------------------------------------------------------------
 287 void MifareNested(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain)
 288 {
 289         // params
 290         uint8_t blockNo = arg0;
 291         uint8_t keyType = arg1;
 292         uint8_t targetBlockNo = arg2 & 0xff;
 293         uint8_t targetKeyType = (arg2 >> 8) & 0xff;
 294         uint64_t ui64Key = 0;
 295
 296         ui64Key = bytes_to_num(datain, 6);
 297
 298         // variables
 299         int rtr, i, j, m, len;
 300         int davg, dmin, dmax;
 301         uint8_t uid[8];
 302         uint32_t cuid, nt1, nt2, nttmp, nttest, par, ks1;
 303         uint8_t par_array[4];
 304         nestedVector nvector[NES_MAX_INFO + 1][11];
 305         int nvectorcount[NES_MAX_INFO + 1];
 306         int ncount = 0;
 307         struct Crypto1State mpcs = {0, 0};
 308         struct Crypto1State *pcs;
 309         pcs = &mpcs;
 310         uint8_t* receivedAnswer = mifare_get_bigbufptr();
 311
 312         //init
 313         for (i = 0; i < NES_MAX_INFO + 1; i++) nvectorcount[i] = 11;  //  11 - empty block;
 314
 315         // clear trace
 316         iso14a_clear_trace();
 317   iso14a_set_tracing(false);
 318
 319         iso14443a_setup();
 320
 321         LED_A_ON();
 322         LED_B_ON();
 323         LED_C_OFF();
 324
 325   FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 326   SpinDelay(200);
 327
 328         davg = dmax = 0;
 329         dmin = 2000;
 330
 331         // test nonce distance
 332         for (rtr = 0; rtr < 10; rtr++) {
 333     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 334     SpinDelay(100);
 335     FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_READER_MOD);
 336
 337     // Test if the action was cancelled
 338     if(BUTTON_PRESS()) {
 339       break;
 340     }
 341
 342                 if(!iso14443a_select_card(uid, NULL, &cuid)) {
 343                         if (MF_DBGLEVEL >= 1)   Dbprintf("Can't select card");
 344                         break;
 345                 };
 346
 347                 if(mifare_classic_authex(pcs, cuid, blockNo, keyType, ui64Key, AUTH_FIRST, &nt1)) {
 348                         if (MF_DBGLEVEL >= 1)   Dbprintf("Auth1 error");
 349                         break;
 350                 };
 351
 352                 if(mifare_classic_authex(pcs, cuid, blockNo, keyType, ui64Key, AUTH_NESTED, &nt2)) {
 353                         if (MF_DBGLEVEL >= 1)   Dbprintf("Auth2 error");
 354                         break;
 355                 };
 356
 357                 nttmp = prng_successor(nt1, 500);
 358                 for (i = 501; i < 2000; i++) {
 359                         nttmp = prng_successor(nttmp, 1);
 360                         if (nttmp == nt2) break;
 361                 }
 362
 363                 if (i != 2000) {
 364                         davg += i;
 365                         if (dmin > i) dmin = i;
 366                         if (dmax < i) dmax = i;
 367                         if (MF_DBGLEVEL >= 4)   Dbprintf("r=%d nt1=%08x nt2=%08x distance=%d", rtr, nt1, nt2, i);
 368                 }
 369         }
 370
 371         if (rtr == 0)   return;
 372
 373         davg = davg / rtr;
 374         if (MF_DBGLEVEL >= 3)   Dbprintf("distance: min=%d max=%d avg=%d", dmin, dmax, davg);
 375
 376         LED_B_OFF();
 377
 378 //  -------------------------------------------------------------------------------------------------
 379
 380         LED_C_ON();
 381
 382         //  get crypted nonces for target sector
 383         for (rtr = 0; rtr < NS_RETRIES_GETNONCE; rtr++) {
 384         if (MF_DBGLEVEL >= 4)                   Dbprintf("------------------------------");
 385
 386                 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 387     SpinDelay(100);
 388     FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_READER_MOD);
 389
 390     // Test if the action was cancelled
 391     if(BUTTON_PRESS()) {
 392       break;
 393     }
 394
 395                 if(!iso14443a_select_card(uid, NULL, &cuid)) {
 396                         if (MF_DBGLEVEL >= 1)   Dbprintf("Can't select card");
 397                         break;
 398                 };
 399
 400                 if(mifare_classic_authex(pcs, cuid, blockNo, keyType, ui64Key, AUTH_FIRST, &nt1)) {
 401                         if (MF_DBGLEVEL >= 1)   Dbprintf("Auth1 error");
 402                         break;
 403                 };
 404
 405                 // nested authentication
 406                 len = mifare_sendcmd_shortex(pcs, AUTH_NESTED, 0x60 + (targetKeyType & 0x01), targetBlockNo, receivedAnswer, &par);
 407                 if (len != 4) {
 408                         if (MF_DBGLEVEL >= 1)   Dbprintf("Auth2 error len=%d", len);
 409                         break;
 410                 };
 411
 412                 nt2 = bytes_to_num(receivedAnswer, 4);
 413                 if (MF_DBGLEVEL >= 4)   Dbprintf("r=%d nt1=%08x nt2enc=%08x nt2par=%08x", rtr, nt1, nt2, par);
 414
 415                 // Parity validity check
 416                 for (i = 0; i < 4; i++) {
 417                         par_array[i] = (oddparity(receivedAnswer[i]) != ((par & 0x08) >> 3));
 418                         par = par << 1;
 419                 }
 420
 421                 ncount = 0;
 422                 nttest = prng_successor(nt1, dmin - NS_TOLERANCE);
 423                 for (m = dmin - NS_TOLERANCE + 1; m < dmax + NS_TOLERANCE; m++) {
 424                         nttest = prng_successor(nttest, 1);
 425                         ks1 = nt2 ^ nttest;
 426
 427                         if (valid_nonce(nttest, nt2, ks1, par_array) && (ncount < 11)){
 428
 429                                 nvector[NES_MAX_INFO][ncount].nt = nttest;
 430                                 nvector[NES_MAX_INFO][ncount].ks1 = ks1;
 431                                 ncount++;
 432                                 nvectorcount[NES_MAX_INFO] = ncount;
 433                                 if (MF_DBGLEVEL >= 4)   Dbprintf("valid m=%d ks1=%08x nttest=%08x", m, ks1, nttest);
 434                         }
 435
 436                 }
 437
 438                 // select vector with length less than got
 439                 if (nvectorcount[NES_MAX_INFO] != 0) {
 440                         m = NES_MAX_INFO;
 441
 442                         for (i = 0; i < NES_MAX_INFO; i++)
 443                                 if (nvectorcount[i] > 10) {
 444                                         m = i;
 445                                         break;
 446                                 }
 447
 448                         if (m == NES_MAX_INFO)
 449                                 for (i = 0; i < NES_MAX_INFO; i++)
 450                                         if (nvectorcount[NES_MAX_INFO] < nvectorcount[i]) {
 451                                                 m = i;
 452                                                 break;
 453                                         }
 454
 455                         if (m != NES_MAX_INFO) {
 456                                 for (i = 0; i < nvectorcount[m]; i++) {
 457                                         nvector[m][i] = nvector[NES_MAX_INFO][i];
 458                                 }
 459                                 nvectorcount[m] = nvectorcount[NES_MAX_INFO];
 460                         }
 461                 }
 462         }
 463
 464         LED_C_OFF();
 465
 466         //  ----------------------------- crypto1 destroy
 467         crypto1_destroy(pcs);
 468
 469         // add trace trailer
 470         memset(uid, 0x44, 4);
 471         LogTrace(uid, 4, 0, 0, TRUE);
 472
 473 //  UsbCommand ack = {CMD_ACK, {0, 0, 0}};
 474
 475         for (i = 0; i < NES_MAX_INFO; i++) {
 476                 if (nvectorcount[i] > 10) continue;
 477
 478                 for (j = 0; j < nvectorcount[i]; j += 5) {
 479                         ncount = nvectorcount[i] - j;
 480                         if (ncount > 5) ncount = 5;
 481
 482 //                      ack.arg[0] = 0; // isEOF = 0
 483 //                      ack.arg[1] = ncount;
 484 //                      ack.arg[2] = targetBlockNo + (targetKeyType * 0x100);
 485 //                      memset(ack.d.asBytes, 0x00, sizeof(ack.d.asBytes));
 486
 487       byte_t buf[48];
 488       memset(buf, 0x00, sizeof(buf));
 489                         memcpy(buf, &cuid, 4);
 490                         for (m = 0; m < ncount; m++) {
 491                                 memcpy(buf + 8 + m * 8 + 0, &nvector[i][m + j].nt, 4);
 492                                 memcpy(buf + 8 + m * 8 + 4, &nvector[i][m + j].ks1, 4);
 493                         }
 494
 495                         LED_B_ON();
 496 //                      SpinDelay(100);
 497       cmd_send(CMD_ACK,0,ncount,targetBlockNo + (targetKeyType * 0x100),buf,48);
 498 //                      UsbSendPacket((uint8_t *)&ack, sizeof(UsbCommand));
 499                         LED_B_OFF();
 500                 }
 501         }
 502
 503         // finalize list
 504 //      ack.arg[0] = 1; // isEOF = 1
 505 //      ack.arg[1] = 0;
 506 //      ack.arg[2] = 0;
 507 //      memset(ack.d.asBytes, 0x00, sizeof(ack.d.asBytes));
 508
 509         LED_B_ON();
 510 //      SpinDelay(300);
 511 //      UsbSendPacket((uint8_t *)&ack, sizeof(UsbCommand));
 512   cmd_send(CMD_ACK,1,0,0,0,0);
 513         LED_B_OFF();
 514
 515         if (MF_DBGLEVEL >= 4)   DbpString("NESTED FINISHED");
 516
 517         // Thats it...
 518         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 519         LEDsoff();
 520
 521   iso14a_set_tracing(TRUE);
 522 }
 523
 524 //-----------------------------------------------------------------------------
 525 // MIFARE check keys. key count up to 8.
 526 //
 527 //-----------------------------------------------------------------------------
 528 void MifareChkKeys(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain)
 529 {
 530   // params
 531         uint8_t blockNo = arg0;
 532         uint8_t keyType = arg1;
 533         uint8_t keyCount = arg2;
 534         uint64_t ui64Key = 0;
 535
 536         // variables
 537         int i;
 538         byte_t isOK = 0;
 539         uint8_t uid[8];
 540         uint32_t cuid;
 541         struct Crypto1State mpcs = {0, 0};
 542         struct Crypto1State *pcs;
 543         pcs = &mpcs;
 544
 545         // clear debug level
 546         int OLD_MF_DBGLEVEL = MF_DBGLEVEL;
 547         MF_DBGLEVEL = MF_DBG_NONE;
 548
 549         // clear trace
 550         iso14a_clear_trace();
 551   iso14a_set_tracing(TRUE);
 552
 553         iso14443a_setup();
 554
 555         LED_A_ON();
 556         LED_B_OFF();
 557         LED_C_OFF();
 558
 559         SpinDelay(300);
 560         for (i = 0; i < keyCount; i++) {
 561                 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 562     SpinDelay(100);
 563     FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_READER_MOD);
 564
 565                 if(!iso14443a_select_card(uid, NULL, &cuid)) {
 566                         if (OLD_MF_DBGLEVEL >= 1)       Dbprintf("Can't select card");
 567                         break;
 568                 };
 569
 570                 ui64Key = bytes_to_num(datain + i * 6, 6);
 571                 if(mifare_classic_auth(pcs, cuid, blockNo, keyType, ui64Key, AUTH_FIRST)) {
 572                         continue;
 573                 };
 574
 575                 isOK = 1;
 576                 break;
 577         }
 578
 579         //  ----------------------------- crypto1 destroy
 580         crypto1_destroy(pcs);
 581
 582         // add trace trailer
 583         memset(uid, 0x44, 4);
 584         LogTrace(uid, 4, 0, 0, TRUE);
 585
 586 //      UsbCommand ack = {CMD_ACK, {isOK, 0, 0}};
 587 //      if (isOK) memcpy(ack.d.asBytes, datain + i * 6, 6);
 588
 589         LED_B_ON();
 590     cmd_send(CMD_ACK,isOK,0,0,datain + i * 6,6);
 591 //      UsbSendPacket((uint8_t *)&ack, sizeof(UsbCommand));
 592         LED_B_OFF();
 593
 594   // Thats it...
 595         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 596         LEDsoff();
 597
 598         // restore debug level
 599         MF_DBGLEVEL = OLD_MF_DBGLEVEL;
 600 }
 601
 602 //-----------------------------------------------------------------------------
 603 // MIFARE commands set debug level
 604 //
 605 //-----------------------------------------------------------------------------
 606 void MifareSetDbgLvl(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){
 607         MF_DBGLEVEL = arg0;
 608         Dbprintf("Debug level: %d", MF_DBGLEVEL);
 609 }
 610
 611 //-----------------------------------------------------------------------------
 612 // Work with emulator memory
 613 //
 614 //-----------------------------------------------------------------------------
 615 void MifareEMemClr(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){
 616         emlClearMem();
 617 }
 618
 619 void MifareEMemSet(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){
 620         emlSetMem(datain, arg0, arg1); // data, block num, blocks count
 621 }
 622
 623 void MifareEMemGet(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){
 624 //      UsbCommand ack = {CMD_ACK, {arg0, arg1, 0}};
 625
 626   byte_t buf[48];
 627         emlGetMem(buf, arg0, arg1); // data, block num, blocks count
 628
 629         LED_B_ON();
 630   cmd_send(CMD_ACK,arg0,arg1,0,buf,48);
 631 //      UsbSendPacket((uint8_t *)&ack, sizeof(UsbCommand));
 632         LED_B_OFF();
 633 }
 634
 635 //-----------------------------------------------------------------------------
 636 // Load a card into the emulator memory
 637 //
 638 //-----------------------------------------------------------------------------
 639 void MifareECardLoad(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){
 640         int i;
 641         uint8_t sectorNo = 0;
 642         uint8_t keyType = arg1;
 643         uint64_t ui64Key = 0;
 644         uint32_t cuid;
 645         struct Crypto1State mpcs = {0, 0};
 646         struct Crypto1State *pcs;
 647         pcs = &mpcs;
 648
 649         // variables
 650         byte_t dataoutbuf[16];
 651         byte_t dataoutbuf2[16];
 652         uint8_t uid[8];
 653
 654         // clear trace
 655         iso14a_clear_trace();
 656         iso14a_set_tracing(false);
 657
 658         iso14443a_setup();
 659
 660         LED_A_ON();
 661         LED_B_OFF();
 662         LED_C_OFF();
 663
 664         while (true) {
 665                 if(!iso14443a_select_card(uid, NULL, &cuid)) {
 666                         if (MF_DBGLEVEL >= 1)   Dbprintf("Can't select card");
 667                         break;
 668                 };
 669
 670                 for (i = 0; i < 16; i++) {
 671                         sectorNo = i;
 672                         ui64Key = emlGetKey(sectorNo, keyType);
 673
 674                         if (!i){
 675                                 if(mifare_classic_auth(pcs, cuid, sectorNo * 4, keyType, ui64Key, AUTH_FIRST)) {
 676                                         if (MF_DBGLEVEL >= 1)   Dbprintf("Sector[%d]. Auth error", i);
 677                                         break;
 678                                 }
 679                         } else {
 680                                 if(mifare_classic_auth(pcs, cuid, sectorNo * 4, keyType, ui64Key, AUTH_NESTED)) {
 681                                         if (MF_DBGLEVEL >= 1)   Dbprintf("Sector[%d]. Auth nested error", i);
 682                                         break;
 683                                 }
 684                         }
 685
 686                         if(mifare_classic_readblock(pcs, cuid, sectorNo * 4 + 0, dataoutbuf)) {
 687                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Read block 0 error");
 688                                 break;
 689                         };
 690                         emlSetMem(dataoutbuf, sectorNo * 4 + 0, 1);
 691
 692                         if(mifare_classic_readblock(pcs, cuid, sectorNo * 4 + 1, dataoutbuf)) {
 693                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Read block 1 error");
 694                                 break;
 695                         };
 696                         emlSetMem(dataoutbuf, sectorNo * 4 + 1, 1);
 697
 698                         if(mifare_classic_readblock(pcs, cuid, sectorNo * 4 + 2, dataoutbuf)) {
 699                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Read block 2 error");
 700                                 break;
 701                         };
 702                         emlSetMem(dataoutbuf, sectorNo * 4 + 2, 1);
 703
 704                         // get block 3 bytes 6-9
 705                         if(mifare_classic_readblock(pcs, cuid, sectorNo * 4 + 3, dataoutbuf)) {
 706                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Read block 3 error");
 707                                 break;
 708                         };
 709                         emlGetMem(dataoutbuf2, sectorNo * 4 + 3, 1);
 710                         memcpy(&dataoutbuf2[6], &dataoutbuf[6], 4);
 711                         emlSetMem(dataoutbuf2,  sectorNo * 4 + 3, 1);
 712                 }
 713
 714                 if(mifare_classic_halt(pcs, cuid)) {
 715                         if (MF_DBGLEVEL >= 1)   Dbprintf("Halt error");
 716                         break;
 717                 };
 718
 719                 break;
 720         }
 721
 722         //  ----------------------------- crypto1 destroy
 723         crypto1_destroy(pcs);
 724
 725         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 726         LEDsoff();
 727
 728         if (MF_DBGLEVEL >= 2) DbpString("EMUL FILL SECTORS FINISHED");
 729
 730         // add trace trailer
 731         memset(uid, 0x44, 4);
 732         LogTrace(uid, 4, 0, 0, TRUE);
 733 }
 734
 735 //-----------------------------------------------------------------------------
 736 // MIFARE 1k emulator
 737 //
 738 //-----------------------------------------------------------------------------
 739
 740
 741 //-----------------------------------------------------------------------------
 742 // Work with "magic Chinese" card (email him: ouyangweidaxian@live.cn)
 743 //
 744 //-----------------------------------------------------------------------------
 745 void MifareCSetBlock(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){
 746
 747   // params
 748         uint8_t needWipe = arg0;
 749         // bit 0 - need get UID
 750         // bit 1 - need wupC
 751         // bit 2 - need HALT after sequence
 752         // bit 3 - need init FPGA and field before sequence
 753         // bit 4 - need reset FPGA and LED
 754         uint8_t workFlags = arg1;
 755         uint8_t blockNo = arg2;
 756
 757         // card commands
 758         uint8_t wupC1[]       = { 0x40 };
 759         uint8_t wupC2[]       = { 0x43 };
 760         uint8_t wipeC[]       = { 0x41 };
 761
 762         // variables
 763         byte_t isOK = 0;
 764         uint8_t uid[8];
 765         uint8_t d_block[18];
 766         uint32_t cuid;
 767
 768         memset(uid, 0x00, 8);
 769         uint8_t* receivedAnswer = mifare_get_bigbufptr();
 770
 771         if (workFlags & 0x08) {
 772                 // clear trace
 773                 iso14a_clear_trace();
 774                 iso14a_set_tracing(TRUE);
 775
 776                 iso14443a_setup();
 777
 778                 LED_A_ON();
 779                 LED_B_OFF();
 780                 LED_C_OFF();
 781
 782                 SpinDelay(300);
 783                 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 784                 SpinDelay(100);
 785                 FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_READER_MOD);
 786         }
 787
 788         while (true) {
 789                 // get UID from chip
 790                 if (workFlags & 0x01) {
 791                         if(!iso14443a_select_card(uid, NULL, &cuid)) {
 792                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Can't select card");
 793                                 break;
 794                         };
 795
 796                         if(mifare_classic_halt(NULL, cuid)) {
 797                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Halt error");
 798                                 break;
 799                         };
 800                 };
 801
 802                 // reset chip
 803                 if (needWipe){
 804       ReaderTransmitBitsPar(wupC1,7,0);
 805                         if(!ReaderReceive(receivedAnswer) || (receivedAnswer[0] != 0x0a)) {
 806                                 if (MF_DBGLEVEL >= 1)   Dbprintf("wupC1 error");
 807                                 break;
 808                         };
 809
 810                         ReaderTransmit(wipeC, sizeof(wipeC));
 811                         if(!ReaderReceive(receivedAnswer) || (receivedAnswer[0] != 0x0a)) {
 812                                 if (MF_DBGLEVEL >= 1)   Dbprintf("wipeC error");
 813                                 break;
 814                         };
 815
 816                         if(mifare_classic_halt(NULL, cuid)) {
 817                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Halt error");
 818                                 break;
 819                         };
 820                 };
 821
 822                 // write block
 823                 if (workFlags & 0x02) {
 824       ReaderTransmitBitsPar(wupC1,7,0);
 825                         if(!ReaderReceive(receivedAnswer) || (receivedAnswer[0] != 0x0a)) {
 826                                 if (MF_DBGLEVEL >= 1)   Dbprintf("wupC1 error");
 827                                 break;
 828                         };
 829
 830                         ReaderTransmit(wupC2, sizeof(wupC2));
 831                         if(!ReaderReceive(receivedAnswer) || (receivedAnswer[0] != 0x0a)) {
 832                                 if (MF_DBGLEVEL >= 1)   Dbprintf("wupC2 error");
 833                                 break;
 834                         };
 835                 }
 836
 837                 if ((mifare_sendcmd_short(NULL, 0, 0xA0, blockNo, receivedAnswer) != 1) || (receivedAnswer[0] != 0x0a)) {
 838                         if (MF_DBGLEVEL >= 1)   Dbprintf("write block send command error");
 839                         break;
 840                 };
 841
 842                 memcpy(d_block, datain, 16);
 843                 AppendCrc14443a(d_block, 16);
 844
 845                 ReaderTransmit(d_block, sizeof(d_block));
 846                 if ((ReaderReceive(receivedAnswer) != 1) || (receivedAnswer[0] != 0x0a)) {
 847                         if (MF_DBGLEVEL >= 1)   Dbprintf("write block send data error");
 848                         break;
 849                 };
 850
 851                 if (workFlags & 0x04) {
 852                         if (mifare_classic_halt(NULL, cuid)) {
 853                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Halt error");
 854                                 break;
 855                         };
 856                 }
 857
 858                 isOK = 1;
 859                 break;
 860         }
 861
 862 //      UsbCommand ack = {CMD_ACK, {isOK, 0, 0}};
 863 //      if (isOK) memcpy(ack.d.asBytes, uid, 4);
 864
 865         // add trace trailer
 866         memset(uid, 0x44, 4);
 867         LogTrace(uid, 4, 0, 0, TRUE);
 868
 869         LED_B_ON();
 870   cmd_send(CMD_ACK,isOK,0,0,uid,4);
 871 //      UsbSendPacket((uint8_t *)&ack, sizeof(UsbCommand));
 872         LED_B_OFF();
 873
 874         if ((workFlags & 0x10) || (!isOK)) {
 875                 // Thats it...
 876                 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 877                 LEDsoff();
 878         }
 879 }
 880
 881 void MifareCGetBlock(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){
 882
 883   // params
 884         // bit 1 - need wupC
 885         // bit 2 - need HALT after sequence
 886         // bit 3 - need init FPGA and field before sequence
 887         // bit 4 - need reset FPGA and LED
 888         uint8_t workFlags = arg0;
 889         uint8_t blockNo = arg2;
 890
 891         // card commands
 892         uint8_t wupC1[]       = { 0x40 };
 893         uint8_t wupC2[]       = { 0x43 };
 894
 895         // variables
 896         byte_t isOK = 0;
 897         uint8_t data[18];
 898         uint32_t cuid = 0;
 899
 900         memset(data, 0x00, 18);
 901         uint8_t* receivedAnswer = mifare_get_bigbufptr();
 902
 903         if (workFlags & 0x08) {
 904                 // clear trace
 905                 iso14a_clear_trace();
 906                 iso14a_set_tracing(TRUE);
 907
 908                 iso14443a_setup();
 909
 910                 LED_A_ON();
 911                 LED_B_OFF();
 912                 LED_C_OFF();
 913
 914                 SpinDelay(300);
 915                 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 916                 SpinDelay(100);
 917                 FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_READER_MOD);
 918         }
 919
 920         while (true) {
 921                 if (workFlags & 0x02) {
 922       ReaderTransmitBitsPar(wupC1,7,0);
 923                         if(!ReaderReceive(receivedAnswer) || (receivedAnswer[0] != 0x0a)) {
 924                                 if (MF_DBGLEVEL >= 1)   Dbprintf("wupC1 error");
 925                                 break;
 926                         };
 927
 928                         ReaderTransmit(wupC2, sizeof(wupC2));
 929                         if(!ReaderReceive(receivedAnswer) || (receivedAnswer[0] != 0x0a)) {
 930                                 if (MF_DBGLEVEL >= 1)   Dbprintf("wupC2 error");
 931                                 break;
 932                         };
 933                 }
 934
 935                 // read block
 936                 if ((mifare_sendcmd_short(NULL, 0, 0x30, blockNo, receivedAnswer) != 18)) {
 937                         if (MF_DBGLEVEL >= 1)   Dbprintf("read block send command error");
 938                         break;
 939                 };
 940                 memcpy(data, receivedAnswer, 18);
 941
 942                 if (workFlags & 0x04) {
 943                         if (mifare_classic_halt(NULL, cuid)) {
 944                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Halt error");
 945                                 break;
 946                         };
 947                 }
 948
 949                 isOK = 1;
 950                 break;
 951         }
 952
 953 //      UsbCommand ack = {CMD_ACK, {isOK, 0, 0}};
 954 //      if (isOK) memcpy(ack.d.asBytes, data, 18);
 955
 956         // add trace trailer
 957         memset(data, 0x44, 4);
 958         LogTrace(data, 4, 0, 0, TRUE);
 959
 960         LED_B_ON();
 961   cmd_send(CMD_ACK,isOK,0,0,data,18);
 962 //      UsbSendPacket((uint8_t *)&ack, sizeof(UsbCommand));
 963         LED_B_OFF();
 964
 965         if ((workFlags & 0x10) || (!isOK)) {
 966                 // Thats it...
 967                 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 968                 LEDsoff();
 969         }
 970 }
 971