git.zerfleddert.de Git - proxmark3-svn/blob - armsrc/mifarecmd.c

   1 //-----------------------------------------------------------------------------
   2 // Merlok - June 2011, 2012
   3 // Gerhard de Koning Gans - May 2008
   4 // Hagen Fritsch - June 2010
   5 //
   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[10];
  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[10];
 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[10];
 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[10];
 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       cmd_send(CMD_ACK,0,ncount,targetBlockNo + (targetKeyType * 0x100),buf,48);
 497 //                      UsbSendPacket((uint8_t *)&ack, sizeof(UsbCommand));
 498                         LED_B_OFF();
 499                 }
 500         }
 501
 502         // finalize list
 503 //      ack.arg[0] = 1; // isEOF = 1
 504 //      ack.arg[1] = 0;
 505 //      ack.arg[2] = 0;
 506 //      memset(ack.d.asBytes, 0x00, sizeof(ack.d.asBytes));
 507
 508         LED_B_ON();
 509 //      UsbSendPacket((uint8_t *)&ack, sizeof(UsbCommand));
 510   cmd_send(CMD_ACK,1,0,0,0,0);
 511         LED_B_OFF();
 512
 513         if (MF_DBGLEVEL >= 4)   DbpString("NESTED FINISHED");
 514
 515         // Thats it...
 516         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 517         LEDsoff();
 518
 519   iso14a_set_tracing(TRUE);
 520 }
 521
 522 //-----------------------------------------------------------------------------
 523 // MIFARE check keys. key count up to 8.
 524 //
 525 //-----------------------------------------------------------------------------
 526 void MifareChkKeys(uint8_t arg0, uint8_t arg1, uint8_t arg2, uint8_t *datain)
 527 {
 528   // params
 529         uint8_t blockNo = arg0;
 530         uint8_t keyType = arg1;
 531         uint8_t keyCount = arg2;
 532         uint64_t ui64Key = 0;
 533
 534         // variables
 535         int i;
 536         byte_t isOK = 0;
 537         uint8_t uid[10];
 538         uint32_t cuid;
 539         struct Crypto1State mpcs = {0, 0};
 540         struct Crypto1State *pcs;
 541         pcs = &mpcs;
 542
 543         // clear debug level
 544         int OLD_MF_DBGLEVEL = MF_DBGLEVEL;
 545         MF_DBGLEVEL = MF_DBG_NONE;
 546
 547         // clear trace
 548         iso14a_clear_trace();
 549   iso14a_set_tracing(TRUE);
 550
 551         iso14443a_setup();
 552
 553         LED_A_ON();
 554         LED_B_OFF();
 555         LED_C_OFF();
 556
 557         SpinDelay(300);
 558         for (i = 0; i < keyCount; i++) {
 559                 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 560     SpinDelay(100);
 561     FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_READER_MOD);
 562
 563                 if(!iso14443a_select_card(uid, NULL, &cuid)) {
 564                         if (OLD_MF_DBGLEVEL >= 1)       Dbprintf("Can't select card");
 565                         break;
 566                 };
 567
 568                 ui64Key = bytes_to_num(datain + i * 6, 6);
 569                 if(mifare_classic_auth(pcs, cuid, blockNo, keyType, ui64Key, AUTH_FIRST)) {
 570                         continue;
 571                 };
 572
 573                 isOK = 1;
 574                 break;
 575         }
 576
 577         //  ----------------------------- crypto1 destroy
 578         crypto1_destroy(pcs);
 579
 580         // add trace trailer
 581         memset(uid, 0x44, 4);
 582         LogTrace(uid, 4, 0, 0, TRUE);
 583
 584 //      UsbCommand ack = {CMD_ACK, {isOK, 0, 0}};
 585 //      if (isOK) memcpy(ack.d.asBytes, datain + i * 6, 6);
 586
 587         LED_B_ON();
 588     cmd_send(CMD_ACK,isOK,0,0,datain + i * 6,6);
 589 //      UsbSendPacket((uint8_t *)&ack, sizeof(UsbCommand));
 590         LED_B_OFF();
 591
 592   // Thats it...
 593         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 594         LEDsoff();
 595
 596         // restore debug level
 597         MF_DBGLEVEL = OLD_MF_DBGLEVEL;
 598 }
 599
 600 //-----------------------------------------------------------------------------
 601 // MIFARE commands set debug level
 602 //
 603 //-----------------------------------------------------------------------------
 604 void MifareSetDbgLvl(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){
 605         MF_DBGLEVEL = arg0;
 606         Dbprintf("Debug level: %d", MF_DBGLEVEL);
 607 }
 608
 609 //-----------------------------------------------------------------------------
 610 // Work with emulator memory
 611 //
 612 //-----------------------------------------------------------------------------
 613 void MifareEMemClr(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){
 614         emlClearMem();
 615 }
 616
 617 void MifareEMemSet(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){
 618         emlSetMem(datain, arg0, arg1); // data, block num, blocks count
 619 }
 620
 621 void MifareEMemGet(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){
 622 //      UsbCommand ack = {CMD_ACK, {arg0, arg1, 0}};
 623
 624   byte_t buf[48];
 625         emlGetMem(buf, arg0, arg1); // data, block num, blocks count
 626
 627         LED_B_ON();
 628   cmd_send(CMD_ACK,arg0,arg1,0,buf,48);
 629 //      UsbSendPacket((uint8_t *)&ack, sizeof(UsbCommand));
 630         LED_B_OFF();
 631 }
 632
 633 //-----------------------------------------------------------------------------
 634 // Load a card into the emulator memory
 635 //
 636 //-----------------------------------------------------------------------------
 637 void MifareECardLoad(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){
 638         int i;
 639         uint8_t sectorNo = 0;
 640         uint8_t keyType = arg1;
 641         uint64_t ui64Key = 0;
 642         uint32_t cuid;
 643         struct Crypto1State mpcs = {0, 0};
 644         struct Crypto1State *pcs;
 645         pcs = &mpcs;
 646
 647         // variables
 648         byte_t dataoutbuf[16];
 649         byte_t dataoutbuf2[16];
 650         uint8_t uid[10];
 651
 652         // clear trace
 653         iso14a_clear_trace();
 654         iso14a_set_tracing(false);
 655
 656         iso14443a_setup();
 657
 658         LED_A_ON();
 659         LED_B_OFF();
 660         LED_C_OFF();
 661
 662         while (true) {
 663                 if(!iso14443a_select_card(uid, NULL, &cuid)) {
 664                         if (MF_DBGLEVEL >= 1)   Dbprintf("Can't select card");
 665                         break;
 666                 };
 667
 668                 for (i = 0; i < 16; i++) {
 669                         sectorNo = i;
 670                         ui64Key = emlGetKey(sectorNo, keyType);
 671
 672                         if (!i){
 673                                 if(mifare_classic_auth(pcs, cuid, sectorNo * 4, keyType, ui64Key, AUTH_FIRST)) {
 674                                         if (MF_DBGLEVEL >= 1)   Dbprintf("Sector[%d]. Auth error", i);
 675                                         break;
 676                                 }
 677                         } else {
 678                                 if(mifare_classic_auth(pcs, cuid, sectorNo * 4, keyType, ui64Key, AUTH_NESTED)) {
 679                                         if (MF_DBGLEVEL >= 1)   Dbprintf("Sector[%d]. Auth nested error", i);
 680                                         break;
 681                                 }
 682                         }
 683
 684                         if(mifare_classic_readblock(pcs, cuid, sectorNo * 4 + 0, dataoutbuf)) {
 685                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Read block 0 error");
 686                                 break;
 687                         };
 688                         emlSetMem(dataoutbuf, sectorNo * 4 + 0, 1);
 689
 690                         if(mifare_classic_readblock(pcs, cuid, sectorNo * 4 + 1, dataoutbuf)) {
 691                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Read block 1 error");
 692                                 break;
 693                         };
 694                         emlSetMem(dataoutbuf, sectorNo * 4 + 1, 1);
 695
 696                         if(mifare_classic_readblock(pcs, cuid, sectorNo * 4 + 2, dataoutbuf)) {
 697                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Read block 2 error");
 698                                 break;
 699                         };
 700                         emlSetMem(dataoutbuf, sectorNo * 4 + 2, 1);
 701
 702                         // get block 3 bytes 6-9
 703                         if(mifare_classic_readblock(pcs, cuid, sectorNo * 4 + 3, dataoutbuf)) {
 704                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Read block 3 error");
 705                                 break;
 706                         };
 707                         emlGetMem(dataoutbuf2, sectorNo * 4 + 3, 1);
 708                         memcpy(&dataoutbuf2[6], &dataoutbuf[6], 4);
 709                         emlSetMem(dataoutbuf2,  sectorNo * 4 + 3, 1);
 710                 }
 711
 712                 if(mifare_classic_halt(pcs, cuid)) {
 713                         if (MF_DBGLEVEL >= 1)   Dbprintf("Halt error");
 714                         break;
 715                 };
 716
 717                 break;
 718         }
 719
 720         //  ----------------------------- crypto1 destroy
 721         crypto1_destroy(pcs);
 722
 723         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 724         LEDsoff();
 725
 726         if (MF_DBGLEVEL >= 2) DbpString("EMUL FILL SECTORS FINISHED");
 727
 728         // add trace trailer
 729         memset(uid, 0x44, 4);
 730         LogTrace(uid, 4, 0, 0, TRUE);
 731 }
 732
 733 //-----------------------------------------------------------------------------
 734 // MIFARE 1k emulator
 735 //
 736 //-----------------------------------------------------------------------------
 737
 738
 739 //-----------------------------------------------------------------------------
 740 // Work with "magic Chinese" card (email him: ouyangweidaxian@live.cn)
 741 //
 742 //-----------------------------------------------------------------------------
 743 void MifareCSetBlock(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){
 744
 745   // params
 746         uint8_t needWipe = arg0;
 747         // bit 0 - need get UID
 748         // bit 1 - need wupC
 749         // bit 2 - need HALT after sequence
 750         // bit 3 - need init FPGA and field before sequence
 751         // bit 4 - need reset FPGA and LED
 752         uint8_t workFlags = arg1;
 753         uint8_t blockNo = arg2;
 754
 755         // card commands
 756         uint8_t wupC1[]       = { 0x40 };
 757         uint8_t wupC2[]       = { 0x43 };
 758         uint8_t wipeC[]       = { 0x41 };
 759
 760         // variables
 761         byte_t isOK = 0;
 762         uint8_t uid[10];
 763         uint8_t d_block[18];
 764         uint32_t cuid;
 765
 766         memset(uid, 0x00, 10);
 767         uint8_t* receivedAnswer = mifare_get_bigbufptr();
 768
 769         if (workFlags & 0x08) {
 770                 // clear trace
 771                 iso14a_clear_trace();
 772                 iso14a_set_tracing(TRUE);
 773
 774                 iso14443a_setup();
 775
 776                 LED_A_ON();
 777                 LED_B_OFF();
 778                 LED_C_OFF();
 779
 780                 SpinDelay(300);
 781                 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 782                 SpinDelay(100);
 783                 FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_READER_MOD);
 784         }
 785
 786         while (true) {
 787                 // get UID from chip
 788                 if (workFlags & 0x01) {
 789                         if(!iso14443a_select_card(uid, NULL, &cuid)) {
 790                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Can't select card");
 791                                 break;
 792                         };
 793
 794                         if(mifare_classic_halt(NULL, cuid)) {
 795                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Halt error");
 796                                 break;
 797                         };
 798                 };
 799
 800                 // reset chip
 801                 if (needWipe){
 802       ReaderTransmitBitsPar(wupC1,7,0);
 803                         if(!ReaderReceive(receivedAnswer) || (receivedAnswer[0] != 0x0a)) {
 804                                 if (MF_DBGLEVEL >= 1)   Dbprintf("wupC1 error");
 805                                 break;
 806                         };
 807
 808                         ReaderTransmit(wipeC, sizeof(wipeC));
 809                         if(!ReaderReceive(receivedAnswer) || (receivedAnswer[0] != 0x0a)) {
 810                                 if (MF_DBGLEVEL >= 1)   Dbprintf("wipeC error");
 811                                 break;
 812                         };
 813
 814                         if(mifare_classic_halt(NULL, cuid)) {
 815                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Halt error");
 816                                 break;
 817                         };
 818                 };
 819
 820                 // write block
 821                 if (workFlags & 0x02) {
 822       ReaderTransmitBitsPar(wupC1,7,0);
 823                         if(!ReaderReceive(receivedAnswer) || (receivedAnswer[0] != 0x0a)) {
 824                                 if (MF_DBGLEVEL >= 1)   Dbprintf("wupC1 error");
 825                                 break;
 826                         };
 827
 828                         ReaderTransmit(wupC2, sizeof(wupC2));
 829                         if(!ReaderReceive(receivedAnswer) || (receivedAnswer[0] != 0x0a)) {
 830                                 if (MF_DBGLEVEL >= 1)   Dbprintf("wupC2 error");
 831                                 break;
 832                         };
 833                 }
 834
 835                 if ((mifare_sendcmd_short(NULL, 0, 0xA0, blockNo, receivedAnswer) != 1) || (receivedAnswer[0] != 0x0a)) {
 836                         if (MF_DBGLEVEL >= 1)   Dbprintf("write block send command error");
 837                         break;
 838                 };
 839
 840                 memcpy(d_block, datain, 16);
 841                 AppendCrc14443a(d_block, 16);
 842
 843                 ReaderTransmit(d_block, sizeof(d_block));
 844                 if ((ReaderReceive(receivedAnswer) != 1) || (receivedAnswer[0] != 0x0a)) {
 845                         if (MF_DBGLEVEL >= 1)   Dbprintf("write block send data error");
 846                         break;
 847                 };
 848
 849                 if (workFlags & 0x04) {
 850                         if (mifare_classic_halt(NULL, cuid)) {
 851                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Halt error");
 852                                 break;
 853                         };
 854                 }
 855
 856                 isOK = 1;
 857                 break;
 858         }
 859
 860 //      UsbCommand ack = {CMD_ACK, {isOK, 0, 0}};
 861 //      if (isOK) memcpy(ack.d.asBytes, uid, 4);
 862
 863         // add trace trailer
 864         /**
 865         *       Removed by Martin, the uid is overwritten with 0x44,
 866         *       which can 't be intended.
 867         *
 868         *       memset(uid, 0x44, 4);
 869         *       LogTrace(uid, 4, 0, 0, TRUE);
 870         **/
 871
 872
 873         LED_B_ON();
 874   cmd_send(CMD_ACK,isOK,0,0,uid,4);
 875 //      UsbSendPacket((uint8_t *)&ack, sizeof(UsbCommand));
 876         LED_B_OFF();
 877
 878         if ((workFlags & 0x10) || (!isOK)) {
 879                 // Thats it...
 880                 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 881                 LEDsoff();
 882         }
 883 }
 884
 885 void MifareCGetBlock(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain){
 886
 887   // params
 888         // bit 1 - need wupC
 889         // bit 2 - need HALT after sequence
 890         // bit 3 - need init FPGA and field before sequence
 891         // bit 4 - need reset FPGA and LED
 892         uint8_t workFlags = arg0;
 893         uint8_t blockNo = arg2;
 894
 895         // card commands
 896         uint8_t wupC1[]       = { 0x40 };
 897         uint8_t wupC2[]       = { 0x43 };
 898
 899         // variables
 900         byte_t isOK = 0;
 901         uint8_t data[18];
 902         uint32_t cuid = 0;
 903
 904         memset(data, 0x00, 18);
 905         uint8_t* receivedAnswer = mifare_get_bigbufptr();
 906
 907         if (workFlags & 0x08) {
 908                 // clear trace
 909                 iso14a_clear_trace();
 910                 iso14a_set_tracing(TRUE);
 911
 912                 iso14443a_setup();
 913
 914                 LED_A_ON();
 915                 LED_B_OFF();
 916                 LED_C_OFF();
 917
 918                 SpinDelay(300);
 919                 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 920                 SpinDelay(100);
 921                 FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_READER_MOD);
 922         }
 923
 924         while (true) {
 925                 if (workFlags & 0x02) {
 926       ReaderTransmitBitsPar(wupC1,7,0);
 927                         if(!ReaderReceive(receivedAnswer) || (receivedAnswer[0] != 0x0a)) {
 928                                 if (MF_DBGLEVEL >= 1)   Dbprintf("wupC1 error");
 929                                 break;
 930                         };
 931
 932                         ReaderTransmit(wupC2, sizeof(wupC2));
 933                         if(!ReaderReceive(receivedAnswer) || (receivedAnswer[0] != 0x0a)) {
 934                                 if (MF_DBGLEVEL >= 1)   Dbprintf("wupC2 error");
 935                                 break;
 936                         };
 937                 }
 938
 939                 // read block
 940                 if ((mifare_sendcmd_short(NULL, 0, 0x30, blockNo, receivedAnswer) != 18)) {
 941                         if (MF_DBGLEVEL >= 1)   Dbprintf("read block send command error");
 942                         break;
 943                 };
 944                 memcpy(data, receivedAnswer, 18);
 945
 946                 if (workFlags & 0x04) {
 947                         if (mifare_classic_halt(NULL, cuid)) {
 948                                 if (MF_DBGLEVEL >= 1)   Dbprintf("Halt error");
 949                                 break;
 950                         };
 951                 }
 952
 953                 isOK = 1;
 954                 break;
 955         }
 956
 957 //      UsbCommand ack = {CMD_ACK, {isOK, 0, 0}};
 958 //      if (isOK) memcpy(ack.d.asBytes, data, 18);
 959
 960         // add trace trailer
 961         /*
 962         * Removed by Martin, this piece of overwrites the 'data' variable
 963         * which is sent two lines down, and is obviously not correct.
 964         *
 965         * memset(data, 0x44, 4);
 966         * LogTrace(data, 4, 0, 0, TRUE);
 967         */
 968         LED_B_ON();
 969   cmd_send(CMD_ACK,isOK,0,0,data,18);
 970 //      UsbSendPacket((uint8_t *)&ack, sizeof(UsbCommand));
 971         LED_B_OFF();
 972
 973         if ((workFlags & 0x10) || (!isOK)) {
 974                 // Thats it...
 975                 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 976                 LEDsoff();
 977         }
 978 }
 979