git.zerfleddert.de Git - proxmark3-svn/blob - 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 #include "mifareutil.h" //for mf_dbglevel
  13 #include "string.h"  //for memset memcmp
  14
  15 //      ¶¨ÒåÁ¬½ÓÒý½Å
  16 #define GPIO_RST  AT91C_PIO_PA1
  17 #define GPIO_SCL  AT91C_PIO_PA5
  18 #define GPIO_SDA  AT91C_PIO_PA7
  19
  20 #define SCL_H   HIGH(GPIO_SCL)
  21 #define SCL_L   LOW(GPIO_SCL)
  22 #define SDA_H   HIGH(GPIO_SDA)
  23 #define SDA_L   LOW(GPIO_SDA)
  24
  25 #define SCL_read  (AT91C_BASE_PIOA->PIO_PDSR & GPIO_SCL)
  26 #define SDA_read  (AT91C_BASE_PIOA->PIO_PDSR & GPIO_SDA)
  27
  28 #define I2C_ERROR  "I2C_WaitAck Error"
  29
  30 volatile unsigned long c;
  31
  32 //      Ö±½ÓÊ¹ÓÃÑ»·À´ÑÓÊ±£¬Ò»¸öÑ»· 6 ÌõÖ¸Áî£¬48M£¬ Delay=1 ´ó¸ÅÎª 200kbps
  33 // timer.
  34 // I2CSpinDelayClk(4) = 12.31us
  35 // I2CSpinDelayClk(1) = 3.07us
  36 void __attribute__((optimize("O0"))) I2CSpinDelayClk(uint16_t delay) {
  37         for (c = delay * 2; c; c--) {};
  38 }
  39
  40 //      Í¨Ñ¶ÑÓ³Ùº¯Êý     communication delay function
  41 #define I2C_DELAY_1CLK    I2CSpinDelayClk(1)
  42 #define I2C_DELAY_2CLK    I2CSpinDelayClk(2)
  43 #define I2C_DELAY_XCLK(x) I2CSpinDelayClk((x))
  44
  45
  46 #define  ISO7618_MAX_FRAME 255
  47
  48 void I2C_init(void) {
  49         // ÅäÖÃ¸´Î»Òý½Å£¬¹Ø±ÕÉÏÀ£¬ÍÆÍìÊä³ö£¬Ä¬ÈÏ¸ß
  50         // Configure reset pin, close up pull up, push-pull output, default high
  51         AT91C_BASE_PIOA->PIO_PPUDR = GPIO_RST;
  52         AT91C_BASE_PIOA->PIO_MDDR = GPIO_RST;
  53
  54         // ÅäÖÃ I2C Òý½Å£¬¿ªÆôÉÏÀ£¬¿ªÂ©Êä³ö
  55         // Configure I2C pin, open up, open leakage
  56         AT91C_BASE_PIOA->PIO_PPUER |= (GPIO_SCL | GPIO_SDA);  // ´ò¿ªÉÏÀ  Open up the pull up
  57         AT91C_BASE_PIOA->PIO_MDER |= (GPIO_SCL | GPIO_SDA);
  58
  59         // Ä¬ÈÏÈý¸ùÏßÈ«²¿À¸ß
  60         // default three lines all pull up
  61         AT91C_BASE_PIOA->PIO_SODR |= (GPIO_SCL | GPIO_SDA | GPIO_RST);
  62
  63         // ÔÊÐíÊä³ö
  64         // allow output
  65         AT91C_BASE_PIOA->PIO_OER |= (GPIO_SCL | GPIO_SDA | GPIO_RST);
  66         AT91C_BASE_PIOA->PIO_PER |= (GPIO_SCL | GPIO_SDA | GPIO_RST);
  67 }
  68
  69
  70 // ÉèÖÃ¸´Î»×´Ì¬
  71 // set the reset state
  72 void I2C_SetResetStatus(uint8_t LineRST, uint8_t LineSCK, uint8_t LineSDA) {
  73         if (LineRST)
  74                 HIGH(GPIO_RST);
  75         else
  76                 LOW(GPIO_RST);
  77
  78         if (LineSCK)
  79                 HIGH(GPIO_SCL);
  80         else
  81                 LOW(GPIO_SCL);
  82
  83         if (LineSDA)
  84                 HIGH(GPIO_SDA);
  85         else
  86                 LOW(GPIO_SDA);
  87 }
  88
  89 // ¸´Î»½øÈëÖ÷³ÌÐò
  90 // Reset the SIM_Adapter, then  enter the main program
  91 // Note: the SIM_Adapter will not enter the main program after power up. Please run this function before use SIM_Adapter.
  92 void I2C_Reset_EnterMainProgram(void) {
  93         I2C_SetResetStatus(0, 0, 0);    // ÀµÍ¸´Î»Ïß
  94         SpinDelay(30);
  95         I2C_SetResetStatus(1, 0, 0);    // ½â³ý¸´Î»
  96         SpinDelay(30);
  97         I2C_SetResetStatus(1, 1, 1);    // À¸ßÊý¾ÝÏß
  98         SpinDelay(10);
  99 }
 100
 101 // ¸´Î»½øÈëÒýµ¼Ä£Ê½
 102 // Reset the SIM_Adapter, then enter the bootloader program
 103 // Reserve£ºFor firmware update.
 104 void I2C_Reset_EnterBootloader(void) {
 105         I2C_SetResetStatus(0, 1, 1);    // ÀµÍ¸´Î»Ïß
 106         SpinDelay(100);
 107         I2C_SetResetStatus(1, 1, 1);    // ½â³ý¸´Î»
 108         SpinDelay(10);
 109 }
 110
 111 //      µÈ´ýÊ±ÖÓ±ä¸ß
 112 // Wait for the clock to go High.
 113 bool WaitSCL_H_delay(uint32_t delay) {
 114         while (delay--) {
 115                 if (SCL_read) {
 116                         return true;
 117                 }
 118                 I2C_DELAY_1CLK;
 119         }
 120         return false;
 121 }
 122
 123 // 5000 * 3.07us = 15350us. 15.35ms
 124 bool WaitSCL_H(void) {
 125         return WaitSCL_H_delay(5000);
 126 }
 127
 128 // Wait max 300ms or until SCL goes LOW.
 129 // Which ever comes first
 130 bool WaitSCL_L_300ms(void) {
 131         volatile uint16_t delay = 300;
 132         while ( delay-- ) {
 133                 // exit on SCL LOW
 134                 if (!SCL_read)
 135                         return true;
 136
 137                 SpinDelay(1);
 138         }
 139         return (delay == 0);
 140 }
 141
 142 bool I2C_Start(void) {
 143
 144         I2C_DELAY_XCLK(4);
 145         SDA_H; I2C_DELAY_1CLK;
 146         SCL_H;
 147         if (!WaitSCL_H()) return false;
 148
 149         I2C_DELAY_2CLK;
 150
 151         if (!SCL_read) return false;
 152         if (!SDA_read) return false;
 153
 154         SDA_L; I2C_DELAY_2CLK;
 155         return true;
 156 }
 157
 158 bool I2C_WaitForSim() {
 159         // variable delay here.
 160         if (!WaitSCL_L_300ms())
 161                 return false;
 162
 163         // 8051 speaks with smart card.
 164         // 1000*50*3.07 = 153.5ms
 165         // 1byte transfer == 1ms
 166         if (!WaitSCL_H_delay(2000*50) )
 167                 return false;
 168
 169         return true;
 170 }
 171
 172 // send i2c STOP
 173 void I2C_Stop(void) {
 174         SCL_L; I2C_DELAY_2CLK;
 175         SDA_L; I2C_DELAY_2CLK;
 176         SCL_H; I2C_DELAY_2CLK;
 177         if (!WaitSCL_H()) return;
 178         SDA_H;
 179         I2C_DELAY_XCLK(8);
 180 }
 181
 182 // Send i2c ACK
 183 void I2C_Ack(void) {
 184         SCL_L; I2C_DELAY_2CLK;
 185         SDA_L; I2C_DELAY_2CLK;
 186         SCL_H; I2C_DELAY_2CLK;
 187         SCL_L; I2C_DELAY_2CLK;
 188 }
 189
 190 // Send i2c NACK
 191 void I2C_NoAck(void) {
 192         SCL_L; I2C_DELAY_2CLK;
 193         SDA_H; I2C_DELAY_2CLK;
 194         SCL_H; I2C_DELAY_2CLK;
 195         SCL_L; I2C_DELAY_2CLK;
 196 }
 197
 198 bool I2C_WaitAck(void) {
 199         SCL_L; I2C_DELAY_1CLK;
 200         SDA_H; I2C_DELAY_1CLK;
 201         SCL_H;
 202         if (!WaitSCL_H())
 203                 return false;
 204
 205         I2C_DELAY_2CLK;
 206         if (SDA_read) {
 207                 SCL_L;
 208                 return false;
 209         }
 210         SCL_L;
 211         return true;
 212 }
 213
 214 void I2C_SendByte(uint8_t data) {
 215         uint8_t i = 8;
 216
 217         while (i--) {
 218                 SCL_L; I2C_DELAY_1CLK;
 219
 220                 if (data & 0x80)
 221                         SDA_H;
 222                 else
 223                         SDA_L;
 224
 225                 data <<= 1;
 226                 I2C_DELAY_1CLK;
 227
 228                 SCL_H;
 229                 if (!WaitSCL_H())
 230                         return;
 231
 232                 I2C_DELAY_2CLK;
 233         }
 234         SCL_L;
 235 }
 236
 237 uint8_t I2C_ReadByte(void) {
 238         uint8_t i = 8, b = 0;
 239
 240         SDA_H;
 241         while (i--) {
 242                 b <<= 1;
 243                 SCL_L; I2C_DELAY_2CLK;
 244                 SCL_H;
 245                 if (!WaitSCL_H())
 246                         return 0;
 247
 248                 I2C_DELAY_2CLK;
 249                 if (SDA_read)
 250                         b |= 0x01;
 251         }
 252         SCL_L;
 253         return b;
 254 }
 255
 256 // Sends one byte  ( command to be written, SlaveDevice address)
 257 bool I2C_WriteCmd(uint8_t device_cmd, uint8_t device_address) {
 258         bool bBreak = true;
 259         do {
 260                 if (!I2C_Start())
 261                         return false;
 262                 //[C0]
 263                 I2C_SendByte(device_address & 0xFE);
 264                 if (!I2C_WaitAck())
 265                         break;
 266
 267                 I2C_SendByte(device_cmd);
 268                 if (!I2C_WaitAck())
 269                         break;
 270
 271                 bBreak = false;
 272         } while (false);
 273
 274         I2C_Stop();
 275         if (bBreak) {
 276                 if ( MF_DBGLEVEL > 3 ) DbpString(I2C_ERROR);
 277                 return false;
 278         }
 279         return true;
 280 }
 281
 282 // Ð´Èë1×Ö½ÚÊý¾Ý £¨´ýÐ´ÈëÊý¾Ý£¬´ýÐ´ÈëµØÖ·£¬Æ÷¼þÀàÐÍ£©
 283 // Sends 1 byte data (Data to be written, command to be written , SlaveDevice address  ).
 284 bool I2C_WriteByte(uint8_t data, uint8_t device_cmd, uint8_t device_address) {
 285         bool bBreak = true;
 286         do {
 287                 if (!I2C_Start())
 288                         return false;
 289
 290                 I2C_SendByte(device_address & 0xFE);
 291                 if (!I2C_WaitAck())
 292                         break;
 293
 294                 I2C_SendByte(device_cmd);
 295                 if (!I2C_WaitAck())
 296                         break;
 297
 298                 I2C_SendByte(data);
 299                 if (!I2C_WaitAck())
 300                         break;
 301
 302                 bBreak = false;
 303         } while (false);
 304
 305         I2C_Stop();
 306         if (bBreak) {
 307                 if ( MF_DBGLEVEL > 3 ) DbpString(I2C_ERROR);
 308                 return false;
 309         }
 310         return true;
 311 }
 312
 313 //      Ð´Èë1´®Êý¾Ý£¨´ýÐ´ÈëÊý×éµØÖ·£¬´ýÐ´Èë³¤¶È£¬´ýÐ´ÈëµØÖ·£¬Æ÷¼þÀàÐÍ£©
 314 //Sends a string of data (Array, length, command to be written , SlaveDevice address  ).
 315 // len = uint8 (max buffer to write 256bytes)
 316 bool I2C_BufferWrite(uint8_t *data, uint8_t len, uint8_t device_cmd, uint8_t device_address) {
 317         bool bBreak = true;
 318         do {
 319                 if (!I2C_Start())
 320                         return false;
 321
 322                 I2C_SendByte(device_address & 0xFE);
 323                 if (!I2C_WaitAck())
 324                         break;
 325
 326                 I2C_SendByte(device_cmd);
 327                 if (!I2C_WaitAck())
 328                         break;
 329
 330                 while (len) {
 331
 332                         I2C_SendByte(*data);
 333                         if (!I2C_WaitAck())
 334                                 break;
 335
 336                         len--;
 337                         data++;
 338                 }
 339
 340                 if (len == 0)
 341                         bBreak = false;
 342         } while (false);
 343
 344         I2C_Stop();
 345         if (bBreak) {
 346                 if ( MF_DBGLEVEL > 3 ) DbpString(I2C_ERROR);
 347                 return false;
 348         }
 349         return true;
 350 }
 351
 352 // ¶Á³ö1´®Êý¾Ý£¨´æ·Å¶Á³öÊý¾Ý£¬´ý¶Á³ö³¤¶È£¬´ø¶Á³öµØÖ·£¬Æ÷¼þÀàÐÍ£©
 353 // read 1 strings of data (Data array, Readout length, command to be written , SlaveDevice address  ).
 354 // len = uint8 (max buffer to read 256bytes)
 355 uint8_t I2C_BufferRead(uint8_t *data, uint8_t len, uint8_t device_cmd, uint8_t device_address) {
 356
 357         if ( !data || len == 0 )
 358                 return 0;
 359
 360         // extra wait  500us (514us measured)
 361         // 200us  (xx measured)
 362         SpinDelayUs(200);
 363         bool bBreak = true;
 364         uint8_t readcount = 0;
 365
 366         do {
 367                 if (!I2C_Start())
 368                         return 0;
 369
 370                 // 0xB0 / 0xC0  == i2c write
 371                 I2C_SendByte(device_address & 0xFE);
 372                 if (!I2C_WaitAck())
 373                         break;
 374
 375                 I2C_SendByte(device_cmd);
 376                 if (!I2C_WaitAck())
 377                         break;
 378
 379                 // 0xB1 / 0xC1 == i2c read
 380                 I2C_Start();
 381                 I2C_SendByte(device_address | 1);
 382                 if (!I2C_WaitAck())
 383                         break;
 384
 385                 bBreak = false;
 386         } while (false);
 387
 388         if (bBreak) {
 389                 I2C_Stop();
 390                 if ( MF_DBGLEVEL > 3 ) DbpString(I2C_ERROR);
 391                 return 0;
 392         }
 393
 394         // reading
 395         while (len) {
 396
 397                 *data = I2C_ReadByte();
 398
 399                 len--;
 400
 401                 // ¶ÁÈ¡µÄµÚÒ»¸ö×Ö½ÚÎªºóÐø³¤¶È
 402                 // The first byte in response is the message length
 403                 if (!readcount && (len > *data)) {
 404                         len = *data;
 405                 } else {
 406                         data++;
 407                 }
 408                 readcount++;
 409
 410                 // acknowledgements. After last byte send NACK.
 411                 if (len == 0)
 412                         I2C_NoAck();
 413                 else
 414                         I2C_Ack();
 415         }
 416
 417         I2C_Stop();
 418         // return bytecount - first byte (which is length byte)
 419         return (readcount) ? --readcount : 0;
 420 }
 421
 422 uint8_t I2C_ReadFW(uint8_t *data, uint8_t len, uint8_t msb, uint8_t lsb, uint8_t device_address) {
 423         //START, 0xB0, 0x00, 0x00, START, 0xB1, xx, yy, zz, ......, STOP
 424         bool bBreak = true;
 425         uint8_t readcount = 0;
 426
 427         // sending
 428         do {
 429                 if (!I2C_Start())
 430                         return 0;
 431
 432                 // 0xB0 / 0xC0  i2c write
 433                 I2C_SendByte(device_address & 0xFE);
 434                 if (!I2C_WaitAck())
 435                         break;
 436
 437                 // msb
 438                 I2C_SendByte(msb);
 439                 if (!I2C_WaitAck())
 440                         break;
 441
 442                 // lsb
 443                 I2C_SendByte(lsb);
 444                 if (!I2C_WaitAck())
 445                         break;
 446
 447                 // 0xB1 / 0xC1  i2c read
 448                 I2C_Start();
 449                 I2C_SendByte(device_address | 1);
 450                 if (!I2C_WaitAck())
 451                         break;
 452
 453                 bBreak = false;
 454         } while (false);
 455
 456         if (bBreak) {
 457                 I2C_Stop();
 458                 if ( MF_DBGLEVEL > 3 ) DbpString(I2C_ERROR);
 459                 return 0;
 460         }
 461
 462         // reading
 463         while (len) {
 464                 *data = I2C_ReadByte();
 465
 466                 data++;
 467                 readcount++;
 468                 len--;
 469
 470                 // acknowledgements. After last byte send NACK.
 471                 if (len == 0)
 472                         I2C_NoAck();
 473                 else
 474                         I2C_Ack();
 475         }
 476
 477         I2C_Stop();
 478         return readcount;
 479 }
 480
 481 bool I2C_WriteFW(uint8_t *data, uint8_t len, uint8_t msb, uint8_t lsb, uint8_t device_address) {
 482         //START, 0xB0, 0x00, 0x00, xx, yy, zz, ......, STOP
 483         bool bBreak = true;
 484
 485         do {
 486                 if (!I2C_Start())
 487                         return false;
 488
 489                 // 0xB0  == i2c write
 490                 I2C_SendByte(device_address & 0xFE);
 491                 if (!I2C_WaitAck())
 492                         break;
 493
 494                 // msb
 495                 I2C_SendByte(msb);
 496                 if (!I2C_WaitAck())
 497                         break;
 498
 499                 // lsb
 500                 I2C_SendByte(lsb);
 501                 if (!I2C_WaitAck())
 502                         break;
 503
 504                 while (len) {
 505                         I2C_SendByte(*data);
 506                         if (!I2C_WaitAck())
 507                                 break;
 508
 509                         len--;
 510                         data++;
 511                 }
 512
 513                 if (len == 0)
 514                         bBreak = false;
 515         } while (false);
 516
 517         I2C_Stop();
 518         if (bBreak) {
 519                 if ( MF_DBGLEVEL > 3 ) DbpString(I2C_ERROR);
 520                 return false;
 521         }
 522         return true;
 523 }
 524
 525 void I2C_print_status(void) {
 526         DbpString("Smart card module (ISO 7816)");
 527         uint8_t resp[] = {0,0,0,0};
 528         I2C_init();
 529         I2C_Reset_EnterMainProgram();
 530         uint8_t len = I2C_BufferRead(resp, sizeof(resp), I2C_DEVICE_CMD_GETVERSION, I2C_DEVICE_ADDRESS_MAIN);
 531         if ( len > 0 )
 532                 Dbprintf("  version.................v%x.%02x", resp[0], resp[1]);
 533         else
 534                 DbpString("  version.................FAILED");
 535 }
 536
 537 bool GetATR(smart_card_atr_t *card_ptr) {
 538
 539         // clear
 540         if ( card_ptr ) {
 541                 card_ptr->atr_len = 0;
 542                 memset(card_ptr->atr, 0, sizeof(card_ptr->atr));
 543         }
 544
 545         // Send ATR
 546         // start [C0 01] stop start C1 len aa bb cc stop]
 547         I2C_WriteCmd(I2C_DEVICE_CMD_GENERATE_ATR, I2C_DEVICE_ADDRESS_MAIN);
 548         uint8_t cmd[1] = {1};
 549         LogTrace(cmd, 1, 0, 0, NULL, true);
 550
 551         //wait for sim card to answer.
 552         if (!I2C_WaitForSim())
 553                 return false;
 554
 555         // read answer
 556         uint8_t len = I2C_BufferRead(card_ptr->atr, sizeof(card_ptr->atr), I2C_DEVICE_CMD_READ, I2C_DEVICE_ADDRESS_MAIN);
 557
 558         if ( len == 0 )
 559                 return false;
 560
 561         // for some reason we only get first byte of atr, if that is so, send dummy command to retrieve the rest of the atr
 562         if (len == 1) {
 563
 564                 uint8_t data[1] = {0};
 565                 I2C_BufferWrite(data, len, I2C_DEVICE_CMD_SEND, I2C_DEVICE_ADDRESS_MAIN);
 566
 567                 if ( !I2C_WaitForSim() )
 568                         return false;
 569
 570                 uint8_t len2 = I2C_BufferRead(card_ptr->atr + len, sizeof(card_ptr->atr) - len, I2C_DEVICE_CMD_READ, I2C_DEVICE_ADDRESS_MAIN);
 571                 len = len + len2;
 572         }
 573
 574         if ( card_ptr ) {
 575                 card_ptr->atr_len = len;
 576                 LogTrace(card_ptr->atr, card_ptr->atr_len, 0, 0, NULL, false);
 577         }
 578
 579         return true;
 580 }
 581
 582 void SmartCardAtr(void) {
 583         smart_card_atr_t card;
 584         LED_D_ON();
 585         clear_trace();
 586         set_tracing(true);
 587         I2C_init();
 588         I2C_Reset_EnterMainProgram();
 589         bool isOK = GetATR( &card );
 590         cmd_send(CMD_ACK, isOK, sizeof(smart_card_atr_t), 0, &card, sizeof(smart_card_atr_t));
 591         set_tracing(false);
 592         LEDsoff();
 593 }
 594
 595 void SmartCardRaw( uint64_t arg0, uint64_t arg1, uint8_t *data ) {
 596
 597         LED_D_ON();
 598
 599         uint8_t len = 0;
 600         uint8_t *resp = BigBuf_malloc(ISO7618_MAX_FRAME);
 601         smartcard_command_t flags = arg0;
 602
 603         if ((flags & SC_CONNECT))
 604                 clear_trace();
 605
 606         set_tracing(true);
 607
 608         if ((flags & SC_CONNECT)) {
 609
 610                 I2C_init();
 611                 I2C_Reset_EnterMainProgram();
 612
 613                 if ( !(flags & SC_NO_SELECT) ) {
 614                         smart_card_atr_t card;
 615                         bool gotATR = GetATR( &card );
 616                         //cmd_send(CMD_ACK, gotATR, sizeof(smart_card_atr_t), 0, &card, sizeof(smart_card_atr_t));
 617                         if ( !gotATR )
 618                                 goto OUT;
 619                 }
 620         }
 621
 622         if ((flags & SC_RAW)) {
 623
 624                 LogTrace(data, arg1, 0, 0, NULL, true);
 625
 626                 // Send raw bytes
 627                 // asBytes = A0 A4 00 00 02
 628                 // arg1 = len 5
 629                 I2C_BufferWrite(data, arg1, I2C_DEVICE_CMD_SEND, I2C_DEVICE_ADDRESS_MAIN);
 630
 631                 if ( !I2C_WaitForSim() )
 632                         goto OUT;
 633
 634                 len = I2C_BufferRead(resp, ISO7618_MAX_FRAME, I2C_DEVICE_CMD_READ, I2C_DEVICE_ADDRESS_MAIN);
 635                 LogTrace(resp, len, 0, 0, NULL, false);
 636         }
 637 OUT:
 638         cmd_send(CMD_ACK, len, 0, 0, resp, len);
 639         set_tracing(false);
 640         LEDsoff();
 641 }
 642
 643 void SmartCardUpgrade(uint64_t arg0) {
 644
 645         LED_C_ON();
 646
 647         #define I2C_BLOCK_SIZE 128
 648         // write.   Sector0,  with 11,22,33,44
 649         // erase is 128bytes, and takes 50ms to execute
 650
 651         I2C_init();
 652         I2C_Reset_EnterBootloader();
 653
 654         bool isOK = true;
 655         uint8_t res = 0;
 656         uint16_t length = arg0;
 657         uint16_t pos = 0;
 658         uint8_t *fwdata = BigBuf_get_addr();
 659         uint8_t *verfiydata = BigBuf_malloc(I2C_BLOCK_SIZE);
 660
 661         while (length) {
 662
 663                 uint8_t msb = (pos >> 8) & 0xFF;
 664                 uint8_t lsb = pos & 0xFF;
 665
 666                 Dbprintf("FW %02X%02X", msb, lsb);
 667
 668                 size_t size = MIN(I2C_BLOCK_SIZE, length);
 669
 670                 // write
 671                 res = I2C_WriteFW(fwdata+pos, size, msb, lsb, I2C_DEVICE_ADDRESS_BOOT);
 672                 if ( !res ) {
 673                         DbpString("Writing failed");
 674                         isOK = false;
 675                         break;
 676                 }
 677
 678                 // writing takes time.
 679                 SpinDelay(50);
 680
 681                 // read
 682                 res = I2C_ReadFW(verfiydata, size, msb, lsb, I2C_DEVICE_ADDRESS_BOOT);
 683                 if ( res == 0) {
 684                         DbpString("Reading back failed");
 685                         isOK = false;
 686                         break;
 687                 }
 688
 689                 // cmp
 690                 if ( 0 != memcmp(fwdata+pos, verfiydata, size)) {
 691                         DbpString("not equal data");
 692                         isOK = false;
 693                         break;
 694                 }
 695
 696                 length -= size;
 697                 pos += size;
 698         }
 699         cmd_send(CMD_ACK, isOK, pos, 0, 0, 0);
 700         LED_C_OFF();
 701 }
 702
 703 // unfinished (or not needed?)
 704 //void SmartCardSetBaud(uint64_t arg0) {
 705 //}
 706
 707 void SmartCardSetClock(uint64_t arg0) {
 708         LED_D_ON();
 709         set_tracing(true);
 710         I2C_init();
 711         I2C_Reset_EnterMainProgram();
 712
 713         // Send SIM CLC
 714         // start [C0 05 xx] stop
 715         I2C_WriteByte(arg0, I2C_DEVICE_CMD_SIM_CLC, I2C_DEVICE_ADDRESS_MAIN);
 716
 717         cmd_send(CMD_ACK, 1, 0, 0, 0, 0);
 718         set_tracing(false);
 719         LEDsoff();
 720 }