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

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