armsrc/i2c.c

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