armsrc/i2c.c

   1 // //-----------------------------------------------------------------------------
   2 // Copyright (C) Proxmark3 contributors. See AUTHORS.md for details.
   3 //
   4 // This program is free software: you can redistribute it and/or modify
   5 // it under the terms of the GNU General Public License as published by
   6 // the Free Software Foundation, either version 3 of the License, or
   7 // (at your option) any later version.
   8 //
   9 // This program is distributed in the hope that it will be useful,
  10 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12 // GNU General Public License for more details.
  13 //
  14 // See LICENSE.txt for the text of the license.
  15 //-----------------------------------------------------------------------------
  16 // The main i2c code, for communications with smart card module
  17 //-----------------------------------------------------------------------------
  18 #include "i2c.h"
  19
  20 #include "proxmark3_arm.h"
  21 #include "cmd.h"
  22 #include "BigBuf.h"
  23 #include "ticks.h"
  24 #include "dbprint.h"
  25 #include "util.h"
  26 #include "string.h"
  27
  28 #define GPIO_RST AT91C_PIO_PA1
  29 #define GPIO_SCL AT91C_PIO_PA5
  30 #define GPIO_SDA AT91C_PIO_PA7
  31
  32 #define SCL_H    HIGH(GPIO_SCL)
  33 #define SCL_L    LOW(GPIO_SCL)
  34 #define SDA_H    HIGH(GPIO_SDA)
  35 #define SDA_L    LOW(GPIO_SDA)
  36
  37 #define SCL_read ((AT91C_BASE_PIOA->PIO_PDSR & GPIO_SCL) == GPIO_SCL)
  38 #define SDA_read ((AT91C_BASE_PIOA->PIO_PDSR & GPIO_SDA) == GPIO_SDA)
  39
  40 #define I2C_ERROR  "I2C_WaitAck Error"
  41
  42 // Direct use the loop to delay. 6 instructions loop, Masterclock 48MHz,
  43 // delay=1 is about 200kbps
  44 // timer.
  45 // I2CSpinDelayClk(4) = 12.31us
  46 // I2CSpinDelayClk(1) = 3.07us
  47 static volatile uint32_t c;
  48 static void __attribute__((optimize("O0"))) I2CSpinDelayClk(uint16_t delay) {
  49     for (c = delay * 2; c; c--) {};
  50 }
  51
  52 #define I2C_DELAY_1CLK    I2CSpinDelayClk(1)
  53 #define I2C_DELAY_2CLK    I2CSpinDelayClk(2)
  54 #define I2C_DELAY_XCLK(x) I2CSpinDelayClk((x))
  55
  56 // try i2c bus recovery at 100kHz = 5us high, 5us low
  57 void I2C_recovery(void) {
  58
  59     DbpString("Performing i2c bus recovery");
  60
  61     // reset I2C
  62     SDA_H;
  63     SCL_H;
  64
  65     //9nth cycle acts as NACK
  66     for (int i = 0; i < 10; i++)  {
  67         SCL_H;
  68         WaitUS(5);
  69         SCL_L;
  70         WaitUS(5);
  71     }
  72
  73     //a STOP signal (SDA from low to high while CLK is high)
  74     SDA_L;
  75     WaitUS(5);
  76
  77     SCL_H;
  78     WaitUS(2);
  79     SDA_H;
  80     WaitUS(2);
  81
  82     bool isok = (SCL_read && SDA_read);
  83     if (!SDA_read)
  84         DbpString("I2C bus recovery  error: SDA still LOW");
  85     if (!SCL_read)
  86         DbpString("I2C bus recovery  error: SCL still LOW");
  87     if (isok)
  88         DbpString("I2C bus recovery complete");
  89 }
  90
  91 void I2C_init(bool has_ticks) {
  92     // Configure reset pin, close up pull up, push-pull output, default high
  93     AT91C_BASE_PIOA->PIO_PPUDR = GPIO_RST;
  94     AT91C_BASE_PIOA->PIO_MDDR = GPIO_RST;
  95
  96     // Configure I2C pin, open up, open leakage
  97     AT91C_BASE_PIOA->PIO_PPUER |= (GPIO_SCL | GPIO_SDA);
  98     AT91C_BASE_PIOA->PIO_MDER |= (GPIO_SCL | GPIO_SDA);
  99
 100     // default three lines all pull up
 101     AT91C_BASE_PIOA->PIO_SODR |= (GPIO_SCL | GPIO_SDA | GPIO_RST);
 102
 103     AT91C_BASE_PIOA->PIO_OER |= (GPIO_SCL | GPIO_SDA | GPIO_RST);
 104     AT91C_BASE_PIOA->PIO_PER |= (GPIO_SCL | GPIO_SDA | GPIO_RST);
 105
 106     if (has_ticks) {
 107         WaitMS(2);
 108     }
 109
 110     bool isok = (SCL_read && SDA_read);
 111     if (isok == false)
 112         I2C_recovery();
 113 }
 114
 115 // set the reset state
 116 void I2C_SetResetStatus(uint8_t LineRST, uint8_t LineSCK, uint8_t LineSDA) {
 117     if (LineRST)
 118         HIGH(GPIO_RST);
 119     else
 120         LOW(GPIO_RST);
 121
 122     if (LineSCK)
 123         HIGH(GPIO_SCL);
 124     else
 125         LOW(GPIO_SCL);
 126
 127     if (LineSDA)
 128         HIGH(GPIO_SDA);
 129     else
 130         LOW(GPIO_SDA);
 131 }
 132
 133 // Reset the SIM_Adapter, then  enter the main program
 134 // Note: the SIM_Adapter will not enter the main program after power up. Please run this function before use SIM_Adapter.
 135 void I2C_Reset_EnterMainProgram(void) {
 136     StartTicks();
 137     I2C_init(true);
 138     I2C_SetResetStatus(0, 0, 0);
 139     WaitMS(30);
 140     I2C_SetResetStatus(1, 0, 0);
 141     WaitMS(30);
 142     I2C_SetResetStatus(1, 1, 1);
 143     WaitMS(10);
 144 }
 145
 146 // Reset the SIM_Adapter, then enter the bootloader program
 147 // Reserve for firmware update.
 148 void I2C_Reset_EnterBootloader(void) {
 149     StartTicks();
 150     I2C_init(true);
 151     I2C_SetResetStatus(0, 1, 1);
 152     WaitMS(100);
 153     I2C_SetResetStatus(1, 1, 1);
 154     WaitMS(10);
 155 }
 156
 157 // Wait for the clock to go High.
 158 static bool WaitSCL_H_delay(uint32_t delay) {
 159     while (delay--) {
 160         if (SCL_read) {
 161             return true;
 162         }
 163         I2C_DELAY_1CLK;
 164     }
 165     return false;
 166 }
 167
 168 // 5000 * 3.07us = 15350 us = 15.35 ms
 169 // 15000 * 3.07us = 46050 us = 46.05 ms
 170 static bool WaitSCL_H(void) {
 171     return WaitSCL_H_delay(5000);
 172 }
 173
 174 static bool WaitSCL_L_delay(uint32_t delay) {
 175     while (delay--) {
 176         if (SCL_read == false) {
 177             return true;
 178         }
 179         I2C_DELAY_1CLK;
 180     }
 181     return false;
 182 }
 183
 184 // 5000 * 3.07us = 15350us. 15.35ms
 185 // 15000 * 3.07us = 46050us. 46.05ms
 186 static bool WaitSCL_L(void) {
 187     return WaitSCL_L_delay(5000);
 188 }
 189
 190 // Wait max 1800ms or until SCL goes LOW.
 191 // It timeout reading response from card
 192 // Which ever comes first
 193 static bool WaitSCL_L_timeout(void) {
 194     volatile uint32_t delay = 1200;
 195     while (delay--) {
 196         // exit on SCL LOW
 197         if (SCL_read == false)
 198             return true;
 199
 200         WaitMS(1);
 201     }
 202     return (delay == 0);
 203 }
 204
 205 static bool I2C_Start(void) {
 206
 207     I2C_DELAY_2CLK;
 208     I2C_DELAY_2CLK;
 209     SDA_H;
 210     I2C_DELAY_1CLK;
 211     SCL_H;
 212     if (WaitSCL_H() == false) {
 213         return false;
 214     }
 215
 216     I2C_DELAY_2CLK;
 217
 218     if (SCL_read == false) {
 219         return false;
 220     }
 221
 222     if (SDA_read == false) {
 223         return false;
 224     }
 225
 226     SDA_L;
 227     I2C_DELAY_2CLK;
 228     return true;
 229 }
 230
 231 static bool I2C_WaitForSim(uint32_t wait) {
 232
 233     // wait for data from card
 234     if (WaitSCL_L_timeout() == false) {
 235         return false;
 236     }
 237
 238     // 8051 speaks with smart card.
 239     // 1000*50*3.07   = 153.5ms
 240     // 1000*110*3.07  = 337.7ms  (337700)
 241     // 4 560 000 * 3.07 = 13999,2ms (13999200)
 242     // 1byte transfer == 1ms with max frame being 256bytes
 243
 244     // fct WaitSCL_H_delay uses a I2C_DELAY_1CLK in the loop with "wait" as number of iterations.
 245     // I2C_DELAY_1CLK == I2CSpinDelayClk(1) = 3.07us
 246     return WaitSCL_H_delay(wait);
 247 }
 248
 249 // send i2c STOP
 250 static void I2C_Stop(void) {
 251     SCL_L;
 252     I2C_DELAY_2CLK;
 253     SDA_L;
 254     I2C_DELAY_2CLK;
 255     SCL_H;
 256     I2C_DELAY_2CLK;
 257
 258     if (WaitSCL_H() == false) {
 259         return;
 260     }
 261
 262     SDA_H;
 263     I2C_DELAY_2CLK;
 264     I2C_DELAY_2CLK;
 265     I2C_DELAY_2CLK;
 266     I2C_DELAY_2CLK;
 267 }
 268
 269 // Send i2c ACK
 270 static void I2C_Ack(void) {
 271     SCL_L;
 272     I2C_DELAY_2CLK;
 273     SDA_L;
 274     I2C_DELAY_2CLK;
 275     SCL_H;
 276     I2C_DELAY_2CLK;
 277
 278     if (WaitSCL_H() == false) {
 279         return;
 280     }
 281
 282     SCL_L;
 283     I2C_DELAY_2CLK;
 284 }
 285
 286 // Send i2c NACK
 287 static void I2C_NoAck(void) {
 288     SCL_L;
 289     I2C_DELAY_2CLK;
 290     SDA_H;
 291     I2C_DELAY_2CLK;
 292     SCL_H;
 293     I2C_DELAY_2CLK;
 294
 295     if (WaitSCL_H() == false) {
 296         return;
 297     }
 298
 299     SCL_L;
 300     I2C_DELAY_2CLK;
 301 }
 302
 303 static bool I2C_WaitAck(void) {
 304     SCL_L;
 305     I2C_DELAY_1CLK;
 306     SDA_H;
 307     I2C_DELAY_1CLK;
 308     SCL_H;
 309
 310     if (WaitSCL_H() == false) {
 311         return false;
 312     }
 313
 314     I2C_DELAY_2CLK;
 315     I2C_DELAY_2CLK;
 316     if (SDA_read) {
 317         SCL_L;
 318         return false;
 319     }
 320     SCL_L;
 321     return true;
 322 }
 323
 324 static void I2C_SendByte(uint8_t data) {
 325
 326     uint8_t bits = 8;
 327
 328     while (bits--) {
 329         SCL_L;
 330
 331         I2C_DELAY_1CLK;
 332
 333         if (data & 0x80)
 334             SDA_H;
 335         else
 336             SDA_L;
 337
 338         data <<= 1;
 339
 340         I2C_DELAY_1CLK;
 341
 342         SCL_H;
 343         if (WaitSCL_H() == false) {
 344             return;
 345         }
 346
 347         I2C_DELAY_2CLK;
 348     }
 349     SCL_L;
 350 }
 351
 352 static int16_t I2C_ReadByte(void) {
 353     uint8_t bits = 8, b = 0;
 354
 355     SDA_H;
 356     while (bits--) {
 357
 358         b <<= 1;
 359         SCL_L;
 360         if (WaitSCL_L() == false) {
 361             return -2;
 362         }
 363
 364         I2C_DELAY_1CLK;
 365         SCL_H;
 366         if (WaitSCL_H() == false) {
 367             return -1;
 368         }
 369
 370         I2C_DELAY_1CLK;
 371         if (SDA_read) {
 372             b |= 0x01;
 373         }
 374     }
 375     SCL_L;
 376     return b;
 377 }
 378
 379 // Sends one byte  (command to be written, SlaveDevice address)
 380 bool I2C_WriteCmd(uint8_t device_cmd, uint8_t device_address) {
 381     bool _break = true;
 382     do {
 383         if (I2C_Start() == false) {
 384             return false;
 385         }
 386
 387         I2C_SendByte(device_address & 0xFE);
 388         if (I2C_WaitAck() == false) {
 389             break;
 390         }
 391
 392         I2C_SendByte(device_cmd);
 393         if (I2C_WaitAck() == false) {
 394             break;
 395         }
 396
 397         _break = false;
 398     } while (false);
 399
 400     I2C_Stop();
 401
 402     if (_break) {
 403
 404         if (g_dbglevel > 3) DbpString(I2C_ERROR);
 405
 406         return false;
 407     }
 408     return true;
 409 }
 410
 411 // Sends 1 byte data (data to be written, command to be written , SlaveDevice address)
 412 bool I2C_WriteByte(uint8_t data, uint8_t device_cmd, uint8_t device_address) {
 413     bool _break = true;
 414     do {
 415         if (I2C_Start() == false) {
 416             return false;
 417         }
 418
 419         I2C_SendByte(device_address & 0xFE);
 420         if (I2C_WaitAck() == false) {
 421             break;
 422         }
 423
 424         I2C_SendByte(device_cmd);
 425         if (I2C_WaitAck() == false) {
 426             break;
 427         }
 428
 429         I2C_SendByte(data);
 430         if (I2C_WaitAck() == false) {
 431             break;
 432         }
 433
 434         _break = false;
 435     } while (false);
 436
 437     I2C_Stop();
 438     if (_break) {
 439         if (g_dbglevel > 3) DbpString(I2C_ERROR);
 440         return false;
 441     }
 442     return true;
 443 }
 444
 445 // Sends array of data (array, length, command to be written , SlaveDevice address)
 446 // len = uint16 because we need to write up to 256 bytes
 447 bool I2C_BufferWrite(const uint8_t *data, uint16_t len, uint8_t device_cmd, uint8_t device_address) {
 448     bool _break = true;
 449     do {
 450         if (I2C_Start() == false) {
 451             return false;
 452         }
 453
 454         I2C_SendByte(device_address & 0xFE);
 455         if (I2C_WaitAck() == false) {
 456             break;
 457         }
 458
 459         I2C_SendByte(device_cmd);
 460         if (I2C_WaitAck() == false) {
 461             break;
 462         }
 463
 464         while (len) {
 465
 466             I2C_SendByte(*data);
 467             if (I2C_WaitAck() == false)
 468                 break;
 469
 470             len--;
 471             data++;
 472         }
 473
 474         if (len == 0) {
 475             _break = false;
 476         }
 477
 478     } while (false);
 479
 480     I2C_Stop();
 481     if (_break) {
 482         if (g_dbglevel > 3) DbpString(I2C_ERROR);
 483         return false;
 484     }
 485     return true;
 486 }
 487
 488 // read one array of data (Data array, Readout length, command to be written , SlaveDevice address  ).
 489 // len = uint16 because we need to read up to 256bytes
 490 int16_t I2C_BufferRead(uint8_t *data, uint16_t len, uint8_t device_cmd, uint8_t device_address) {
 491
 492     // sanity check
 493     if (data == NULL || len == 0) {
 494         return 0;
 495     }
 496
 497 //    uint8_t *pd = data;
 498
 499     // extra wait  500us (514us measured)
 500     // 200us  (xx measured)
 501     WaitUS(600);
 502
 503     bool _break = true;
 504
 505     do {
 506         if (I2C_Start() == false) {
 507             return 0;
 508         }
 509
 510         // 0xB0 / 0xC0  == i2c write
 511         I2C_SendByte(device_address & 0xFE);
 512         if (I2C_WaitAck() == false) {
 513             break;
 514         }
 515
 516         I2C_SendByte(device_cmd);
 517         if (I2C_WaitAck() == false) {
 518             break;
 519         }
 520
 521         // 0xB1 / 0xC1 == i2c read
 522         I2C_Start();
 523         I2C_SendByte(device_address | 1);
 524         if (I2C_WaitAck() == false) {
 525             break;
 526         }
 527
 528         _break = false;
 529     } while (false);
 530
 531     if (_break) {
 532         I2C_Stop();
 533         if (g_dbglevel > 3) DbpString(I2C_ERROR);
 534         return 0;
 535     }
 536
 537     uint16_t readcount = 0;
 538     uint16_t recv_len = 0;
 539
 540     while (len) {
 541
 542         int16_t tmp = I2C_ReadByte();
 543         if (tmp < 0) {
 544             return tmp;
 545         }
 546
 547         *data = (uint8_t)tmp & 0xFF;
 548
 549         len--;
 550
 551         // Starting firmware v4 the length is encoded on the first two bytes.
 552         switch (readcount) {
 553             case 0: {
 554                 // Length (MSB)
 555                 recv_len = (*data) << 8;
 556                 break;
 557             }
 558             case 1: {
 559                 // Length (LSB)
 560                 recv_len += *data;
 561
 562                 // old packages..
 563                 if (recv_len > 0x0200) {
 564                     // [0] = len
 565                     // [1] = data
 566                     recv_len >>= 8;
 567                     data++;
 568                 }
 569
 570                 // Adjust len if needed
 571                 if (len > recv_len) {
 572                     len = recv_len;
 573                 }
 574                 break;
 575             }
 576             default: {
 577                 // Data byte received
 578                 data++;
 579                 break;
 580             }
 581         }
 582
 583         readcount++;
 584
 585         // acknowledgements. After last byte send NACK.
 586         if (len == 0) {
 587             I2C_NoAck();
 588         } else {
 589             I2C_Ack();
 590         }
 591     }
 592
 593     I2C_Stop();
 594
 595 //    Dbprintf("rec len...  %u  readcount... %u", recv_len, readcount);
 596 //    Dbhexdump(readcount, pd, false);
 597
 598     if (readcount < 2) {
 599         return 0;
 600     }
 601
 602     // return bytecount - bytes encoding length
 603     return readcount - 2;
 604 }
 605
 606 int16_t I2C_ReadFW(uint8_t *data, uint8_t len, uint8_t msb, uint8_t lsb, uint8_t device_address) {
 607     //START, 0xB0, 0x00, 0x00, START, 0xB1, xx, yy, zz, ......, STOP
 608     bool _break = true;
 609     uint8_t readcount = 0;
 610
 611     // sending
 612     do {
 613         if (I2C_Start() == false) {
 614             return 0;
 615         }
 616
 617         // 0xB0 / 0xC0  i2c write
 618         I2C_SendByte(device_address & 0xFE);
 619         if (I2C_WaitAck() == false)
 620             break;
 621
 622         I2C_SendByte(msb);
 623         if (I2C_WaitAck() == false) {
 624             break;
 625         }
 626
 627         I2C_SendByte(lsb);
 628         if (I2C_WaitAck() == false) {
 629             break;
 630         }
 631
 632         // 0xB1 / 0xC1  i2c read
 633         I2C_Start();
 634         I2C_SendByte(device_address | 1);
 635         if (I2C_WaitAck() == false) {
 636             break;
 637         }
 638
 639         _break = false;
 640     } while (false);
 641
 642     if (_break) {
 643         I2C_Stop();
 644         if (g_dbglevel > 3) DbpString(I2C_ERROR);
 645         return 0;
 646     }
 647
 648     // reading
 649     while (len) {
 650
 651         int16_t tmp = I2C_ReadByte();
 652         if (tmp < 0) {
 653             return tmp;
 654         }
 655
 656         *data = (uint8_t)tmp & 0xFF;
 657
 658         data++;
 659         readcount++;
 660         len--;
 661
 662         // acknowledgements. After last byte send NACK.
 663         if (len == 0)
 664             I2C_NoAck();
 665         else
 666             I2C_Ack();
 667     }
 668
 669     I2C_Stop();
 670     return readcount;
 671 }
 672
 673 bool I2C_WriteFW(const uint8_t *data, uint8_t len, uint8_t msb, uint8_t lsb, uint8_t device_address) {
 674     //START, 0xB0, 0x00, 0x00, xx, yy, zz, ......, STOP
 675     bool _break = true;
 676
 677     do {
 678         if (I2C_Start() == false) {
 679             return false;
 680         }
 681
 682         // 0xB0  == i2c write
 683         I2C_SendByte(device_address & 0xFE);
 684         if (I2C_WaitAck() == false) {
 685             break;
 686         }
 687
 688         I2C_SendByte(msb);
 689         if (I2C_WaitAck() == false) {
 690             break;
 691         }
 692
 693         I2C_SendByte(lsb);
 694         if (I2C_WaitAck() == false) {
 695             break;
 696         }
 697
 698         while (len) {
 699             I2C_SendByte(*data);
 700             if (I2C_WaitAck() == false) {
 701                 break;
 702             }
 703             len--;
 704             data++;
 705         }
 706
 707         if (len == 0) {
 708             _break = false;
 709         }
 710
 711     } while (false);
 712
 713     I2C_Stop();
 714
 715     if (_break) {
 716         if (g_dbglevel > 3) DbpString(I2C_ERROR);
 717         return false;
 718     }
 719     return true;
 720 }
 721
 722 void I2C_print_status(void) {
 723     DbpString(_CYAN_("Smart card module (ISO 7816)"));
 724
 725     uint8_t major, minor;
 726     if (I2C_get_version(&major, &minor) == PM3_SUCCESS) {
 727
 728         Dbprintf("  version................. v%d.%02d ( %s )"
 729                  , major
 730                  , minor
 731                  , ((major == 4) && (minor == 42)) ? _GREEN_("ok") : _RED_("Outdated")
 732                 );
 733     } else {
 734         DbpString("  version................. ( " _RED_("fail") " )");
 735     }
 736 }
 737
 738 int I2C_get_version(uint8_t *major, uint8_t *minor) {
 739     uint8_t resp[] = {0, 0, 0, 0};
 740     I2C_Reset_EnterMainProgram();
 741     uint8_t len = I2C_BufferRead(resp, sizeof(resp), I2C_DEVICE_CMD_GETVERSION, I2C_DEVICE_ADDRESS_MAIN);
 742     if (len > 1) {
 743         *major = resp[0];
 744         *minor = resp[1];
 745         return PM3_SUCCESS;
 746     }
 747     return PM3_EDEVNOTSUPP;
 748 }
 749
 750 // Will read response from smart card module,  retries 3 times to get the data.
 751 bool sc_rx_bytes(uint8_t *dest, uint16_t *destlen, uint32_t wait) {
 752
 753     uint8_t i = 10;
 754     int16_t len = 0;
 755     while (i--) {
 756
 757         I2C_WaitForSim(wait);
 758
 759         len = I2C_BufferRead(dest, *destlen, I2C_DEVICE_CMD_READ, I2C_DEVICE_ADDRESS_MAIN);
 760
 761         LED_C_ON();
 762
 763         if (len > 1) {
 764             break;
 765         } else if (len == 1) {
 766             continue;
 767         } else {
 768             return false;
 769         }
 770     }
 771
 772     if (len < 1) {
 773         return false;
 774     }
 775
 776     *destlen = len;
 777     return true;
 778 }
 779
 780 bool GetATR(smart_card_atr_t *card_ptr, bool verbose) {
 781
 782     if (card_ptr == NULL) {
 783         return false;
 784     }
 785
 786
 787     card_ptr->atr_len = 0;
 788     memset(card_ptr->atr, 0, sizeof(card_ptr->atr));
 789
 790     // Send ATR
 791     // start [C0 01] stop start C1 len aa bb cc stop]
 792     I2C_WriteCmd(I2C_DEVICE_CMD_GENERATE_ATR, I2C_DEVICE_ADDRESS_MAIN);
 793
 794     // wait for sim card to answer.
 795     // 1byte = 1ms ,  max frame 256bytes.  Should wait 256ms atleast just in case.
 796     if (I2C_WaitForSim(SIM_WAIT_DELAY) == false) {
 797         return false;
 798     }
 799
 800     // read bytes from module
 801     uint16_t len = sizeof(card_ptr->atr);
 802     if (sc_rx_bytes(card_ptr->atr, &len, SIM_WAIT_DELAY) == false) {
 803         return false;
 804     }
 805
 806     if (len > sizeof(card_ptr->atr)) {
 807         len = sizeof(card_ptr->atr);
 808     }
 809
 810     uint8_t pos_td = 1;
 811     if ((card_ptr->atr[1] & 0x10) == 0x10) pos_td++;
 812     if ((card_ptr->atr[1] & 0x20) == 0x20) pos_td++;
 813     if ((card_ptr->atr[1] & 0x40) == 0x40) pos_td++;
 814
 815     // T0 indicate presence T=0 vs T=1.  T=1 has checksum TCK
 816     if ((card_ptr->atr[1] & 0x80) == 0x80) {
 817
 818         pos_td++;
 819
 820         // 1 == T1 ,  presence of checksum TCK
 821         if ((card_ptr->atr[pos_td] & 0x01) == 0x01) {
 822
 823             uint8_t chksum = 0;
 824             // xor property.  will be zero when xored with chksum.
 825             for (uint16_t i = 1; i < len; ++i)
 826                 chksum ^= card_ptr->atr[i];
 827
 828             if (chksum) {
 829                 if (g_dbglevel > 2) DbpString("Wrong ATR checksum");
 830             }
 831         }
 832     }
 833
 834     card_ptr->atr_len = (uint8_t)(len & 0xff);
 835     if (verbose) {
 836         LogTrace(card_ptr->atr, card_ptr->atr_len, 0, 0, NULL, false);
 837     }
 838
 839     return true;
 840 }
 841
 842 void SmartCardAtr(void) {
 843     LED_D_ON();
 844     set_tracing(true);
 845     I2C_Reset_EnterMainProgram();
 846     smart_card_atr_t card;
 847     if (GetATR(&card, true)) {
 848         reply_ng(CMD_SMART_ATR, PM3_SUCCESS, (uint8_t *)&card, sizeof(smart_card_atr_t));
 849     } else {
 850         reply_ng(CMD_SMART_ATR, PM3_ETIMEOUT, NULL, 0);
 851     }
 852     set_tracing(false);
 853     LEDsoff();
 854 //    StopTicks();
 855 }
 856
 857 void SmartCardRaw(const smart_card_raw_t *p) {
 858     LED_D_ON();
 859
 860     uint16_t len = 0;
 861     uint8_t *resp = BigBuf_malloc(ISO7816_MAX_FRAME);
 862     // check if alloacted...
 863     smartcard_command_t flags = p->flags;
 864
 865     if ((flags & SC_CLEARLOG) == SC_CLEARLOG)
 866         clear_trace();
 867
 868     if ((flags & SC_LOG) == SC_LOG)
 869         set_tracing(true);
 870     else
 871         set_tracing(false);
 872
 873     if ((flags & SC_CONNECT) == SC_CONNECT) {
 874
 875         I2C_Reset_EnterMainProgram();
 876
 877         if ((flags & SC_SELECT) == SC_SELECT) {
 878             smart_card_atr_t card;
 879             bool gotATR = GetATR(&card, true);
 880             //reply_old(CMD_ACK, gotATR, sizeof(smart_card_atr_t), 0, &card, sizeof(smart_card_atr_t));
 881             if (gotATR == false) {
 882                 reply_ng(CMD_SMART_RAW, PM3_ESOFT, NULL, 0);
 883                 goto OUT;
 884             }
 885         }
 886     }
 887
 888     if (((flags & SC_RAW) == SC_RAW) || ((flags & SC_RAW_T0) == SC_RAW_T0)) {
 889
 890         uint32_t wait = SIM_WAIT_DELAY;
 891         if ((flags & SC_WAIT) == SC_WAIT) {
 892             wait = (uint32_t)((p->wait_delay * 1000) / 3.07);
 893         }
 894
 895         LogTrace(p->data, p->len, 0, 0, NULL, true);
 896
 897         bool res = I2C_BufferWrite(
 898                        p->data,
 899                        p->len,
 900                        (((flags & SC_RAW_T0) == SC_RAW_T0) ? I2C_DEVICE_CMD_SEND_T0 : I2C_DEVICE_CMD_SEND),
 901                        I2C_DEVICE_ADDRESS_MAIN
 902                    );
 903
 904         if (res == false && g_dbglevel > 3) {
 905             DbpString(I2C_ERROR);
 906             reply_ng(CMD_SMART_RAW, PM3_ESOFT, NULL, 0);
 907             goto OUT;
 908         }
 909
 910         // read bytes from module
 911         len = ISO7816_MAX_FRAME;
 912         res = sc_rx_bytes(resp, &len, wait);
 913         if (res) {
 914             LogTrace(resp, len, 0, 0, NULL, false);
 915         } else {
 916             len = 0;
 917         }
 918     }
 919
 920     reply_ng(CMD_SMART_RAW, PM3_SUCCESS, resp, len);
 921
 922 OUT:
 923     BigBuf_free();
 924     set_tracing(false);
 925     LEDsoff();
 926 }
 927
 928 void SmartCardUpgrade(uint64_t arg0) {
 929
 930     LED_C_ON();
 931
 932 #define I2C_BLOCK_SIZE 128
 933     // write.   Sector0,  with 11,22,33,44
 934     // erase is 128bytes, and takes 50ms to execute
 935
 936     I2C_Reset_EnterBootloader();
 937
 938     bool isOK = true;
 939     uint16_t length = arg0, pos = 0;
 940     const uint8_t *fwdata = BigBuf_get_addr();
 941     uint8_t *verfiydata = BigBuf_malloc(I2C_BLOCK_SIZE);
 942
 943     while (length) {
 944
 945         uint8_t msb = (pos >> 8) & 0xFF;
 946         uint8_t lsb = pos & 0xFF;
 947
 948         Dbprintf("FW %02X%02X", msb, lsb);
 949
 950         size_t size = MIN(I2C_BLOCK_SIZE, length);
 951
 952         // write
 953         int16_t res = I2C_WriteFW(fwdata + pos, size, msb, lsb, I2C_DEVICE_ADDRESS_BOOT);
 954         if (!res) {
 955             DbpString("Writing failed");
 956             isOK = false;
 957             break;
 958         }
 959
 960         // writing takes time.
 961         WaitMS(50);
 962
 963         // read
 964         res = I2C_ReadFW(verfiydata, size, msb, lsb, I2C_DEVICE_ADDRESS_BOOT);
 965         if (res <= 0) {
 966             DbpString("Reading back failed");
 967             isOK = false;
 968             break;
 969         }
 970
 971         // cmp
 972         if (0 != memcmp(fwdata + pos, verfiydata, size)) {
 973             DbpString("not equal data");
 974             isOK = false;
 975             break;
 976         }
 977
 978         length -= size;
 979         pos += size;
 980     }
 981
 982     reply_ng(CMD_SMART_UPGRADE, (isOK) ? PM3_SUCCESS : PM3_ESOFT, NULL, 0);
 983     LED_C_OFF();
 984     BigBuf_free();
 985 }
 986
 987 void SmartCardSetBaud(uint64_t arg0) {
 988 }
 989
 990 void SmartCardSetClock(uint64_t arg0) {
 991     LED_D_ON();
 992     set_tracing(true);
 993     I2C_Reset_EnterMainProgram();
 994     // Send SIM CLC
 995     // start [C0 05 xx] stop
 996     I2C_WriteByte(arg0, I2C_DEVICE_CMD_SIM_CLC, I2C_DEVICE_ADDRESS_MAIN);
 997     reply_ng(CMD_SMART_SETCLOCK, PM3_SUCCESS, NULL, 0);
 998     set_tracing(false);
 999     LEDsoff();
1000 }