cvs.zerfleddert.de Git - proxmark3-svn/blob - armsrc/legicrf.c

   1 //-----------------------------------------------------------------------------
   2 // (c) 2009 Henryk Plötz <henryk@ploetzli.ch>
   3 //
   4 // This code is licensed to you under the terms of the GNU GPL, version 2 or,
   5 // at your option, any later version. See the LICENSE.txt file for the text of
   6 // the license.
   7 //-----------------------------------------------------------------------------
   8 // LEGIC RF simulation code
   9 //-----------------------------------------------------------------------------
  10
  11 #include "proxmark3.h"
  12 #include "apps.h"
  13 #include "util.h"
  14 #include "string.h"
  15
  16 #include "legicrf.h"
  17 #include "legic_prng.h"
  18 #include "crc.h"
  19
  20 static struct legic_frame {
  21         int bits;
  22         uint32_t data;
  23 } current_frame;
  24
  25 static enum {
  26   STATE_DISCON,
  27   STATE_IV,
  28   STATE_CON,
  29 } legic_state;
  30
  31 static crc_t    legic_crc;
  32 static int      legic_read_count;
  33 static uint32_t legic_prng_bc;
  34 static uint32_t legic_prng_iv;
  35
  36 static int      legic_phase_drift;
  37 static int      legic_frame_drift;
  38 static int      legic_reqresp_drift;
  39
  40 AT91PS_TC timer;
  41 AT91PS_TC prng_timer;
  42
  43 static void setup_timer(void)
  44 {
  45         /* Set up Timer 1 to use for measuring time between pulses. Since we're bit-banging
  46          * this it won't be terribly accurate but should be good enough.
  47          */
  48         AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_TC1);
  49         timer = AT91C_BASE_TC1;
  50         timer->TC_CCR = AT91C_TC_CLKDIS;
  51         timer->TC_CMR = AT91C_TC_CLKS_TIMER_DIV3_CLOCK;
  52         timer->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
  53
  54         /*
  55      * Set up Timer 2 to use for measuring time between frames in
  56      * tag simulation mode. Runs 4x faster as Timer 1
  57          */
  58     AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_TC2);
  59     prng_timer = AT91C_BASE_TC2;
  60     prng_timer->TC_CCR = AT91C_TC_CLKDIS;
  61         prng_timer->TC_CMR = AT91C_TC_CLKS_TIMER_DIV2_CLOCK;
  62     prng_timer->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
  63 }
  64
  65 /* At TIMER_CLOCK3 (MCK/32) */
  66 #define RWD_TIME_1 150     /* RWD_TIME_PAUSE off, 80us on = 100us */
  67 #define RWD_TIME_0 90      /* RWD_TIME_PAUSE off, 40us on = 60us */
  68 #define RWD_TIME_PAUSE 30  /* 20us */
  69 #define RWD_TIME_FUZZ 20   /* rather generous 13us, since the peak detector + hysteresis fuzz quite a bit */
  70 #define TAG_TIME_BIT 150   /* 100us for every bit */
  71 #define TAG_TIME_WAIT 490  /* time from RWD frame end to tag frame start, experimentally determined */
  72
  73 #define SIM_DIVISOR  586   /* prng_time/SIM_DIVISOR count prng needs to be forwared */
  74 #define SIM_SHIFT    900   /* prng_time+SIM_SHIFT shift of delayed start */
  75
  76 #define SESSION_IV 0x55
  77 #define OFFSET_LOG 1024
  78
  79 #define FUZZ_EQUAL(value, target, fuzz) ((value) > ((target)-(fuzz)) && (value) < ((target)+(fuzz)))
  80
  81 /* Generate Keystream */
  82 static uint32_t get_key_stream(int skip, int count)
  83 {
  84   uint32_t key=0; int i;
  85
  86   /* Use int to enlarge timer tc to 32bit */
  87   legic_prng_bc += prng_timer->TC_CV;
  88   prng_timer->TC_CCR = AT91C_TC_SWTRG;
  89
  90   /* If skip == -1, forward prng time based */
  91   if(skip == -1) {
  92      i  = (legic_prng_bc+SIM_SHIFT)/SIM_DIVISOR; /* Calculate Cycles based on timer */
  93      i -= legic_prng_count(); /* substract cycles of finished frames */
  94      i -= count; /* substract current frame length, rewidn to bedinning */
  95      legic_prng_forward(i);
  96   } else {
  97      legic_prng_forward(skip);
  98   }
  99
 100   /* Write Time Data into LOG */
 101   if(count == 6) { i = -1; } else { i = legic_read_count; }
 102   ((uint8_t*)BigBuf)[OFFSET_LOG+128+i] = legic_prng_count();
 103   ((uint8_t*)BigBuf)[OFFSET_LOG+256+i*4]   = (legic_prng_bc >> 0) & 0xff;
 104   ((uint8_t*)BigBuf)[OFFSET_LOG+256+i*4+1] = (legic_prng_bc >> 8) & 0xff;
 105   ((uint8_t*)BigBuf)[OFFSET_LOG+256+i*4+2] = (legic_prng_bc >>16) & 0xff;
 106   ((uint8_t*)BigBuf)[OFFSET_LOG+256+i*4+3] = (legic_prng_bc >>24) & 0xff;
 107   ((uint8_t*)BigBuf)[OFFSET_LOG+384+i] = count;
 108
 109   /* Generate KeyStream */
 110   for(i=0; i<count; i++) {
 111     key |= legic_prng_get_bit() << i;
 112     legic_prng_forward(1);
 113   }
 114   return key;
 115 }
 116
 117 /* Send a frame in tag mode, the FPGA must have been set up by
 118  * LegicRfSimulate
 119  */
 120 static void frame_send_tag(uint16_t response, int bits, int crypt)
 121 {
 122    /* Bitbang the response */
 123    AT91C_BASE_PIOA->PIO_CODR = GPIO_SSC_DOUT;
 124    AT91C_BASE_PIOA->PIO_OER = GPIO_SSC_DOUT;
 125    AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT;
 126
 127    /* Use time to crypt frame */
 128    if(crypt) {
 129       legic_prng_forward(2); /* TAG_TIME_WAIT -> shift by 2 */
 130       int i; int key = 0;
 131       for(i=0; i<bits; i++) {
 132          key |= legic_prng_get_bit() << i;
 133          legic_prng_forward(1);
 134       }
 135       //Dbprintf("key = 0x%x", key);
 136       response = response ^ key;
 137    }
 138
 139    /* Wait for the frame start */
 140    while(timer->TC_CV < (TAG_TIME_WAIT - 30)) ;
 141
 142    int i;
 143    for(i=0; i<bits; i++) {
 144       int nextbit = timer->TC_CV + TAG_TIME_BIT;
 145       int bit = response & 1;
 146       response = response >> 1;
 147       if(bit) {
 148          AT91C_BASE_PIOA->PIO_SODR = GPIO_SSC_DOUT;
 149       } else {
 150          AT91C_BASE_PIOA->PIO_CODR = GPIO_SSC_DOUT;
 151       }
 152       while(timer->TC_CV < nextbit) ;
 153    }
 154    AT91C_BASE_PIOA->PIO_CODR = GPIO_SSC_DOUT;
 155 }
 156
 157 /* Send a frame in reader mode, the FPGA must have been set up by
 158  * LegicRfReader
 159  */
 160 static void frame_send_rwd(uint32_t data, int bits)
 161 {
 162         /* Start clock */
 163         timer->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
 164         while(timer->TC_CV > 1) ; /* Wait till the clock has reset */
 165
 166         int i;
 167         for(i=0; i<bits; i++) {
 168                 int starttime = timer->TC_CV;
 169                 int pause_end = starttime + RWD_TIME_PAUSE, bit_end;
 170                 int bit = data & 1;
 171                 data = data >> 1;
 172
 173                 if(bit ^ legic_prng_get_bit()) {
 174                         bit_end = starttime + RWD_TIME_1;
 175                 } else {
 176                         bit_end = starttime + RWD_TIME_0;
 177                 }
 178
 179                 /* RWD_TIME_PAUSE time off, then some time on, so that the complete bit time is
 180                  * RWD_TIME_x, where x is the bit to be transmitted */
 181                 AT91C_BASE_PIOA->PIO_CODR = GPIO_SSC_DOUT;
 182                 while(timer->TC_CV < pause_end) ;
 183                 AT91C_BASE_PIOA->PIO_SODR = GPIO_SSC_DOUT;
 184                 legic_prng_forward(1); /* bit duration is longest. use this time to forward the lfsr */
 185
 186                 while(timer->TC_CV < bit_end) ;
 187         }
 188
 189         {
 190                 /* One final pause to mark the end of the frame */
 191                 int pause_end = timer->TC_CV + RWD_TIME_PAUSE;
 192                 AT91C_BASE_PIOA->PIO_CODR = GPIO_SSC_DOUT;
 193                 while(timer->TC_CV < pause_end) ;
 194                 AT91C_BASE_PIOA->PIO_SODR = GPIO_SSC_DOUT;
 195         }
 196
 197         /* Reset the timer, to measure time until the start of the tag frame */
 198         timer->TC_CCR = AT91C_TC_SWTRG;
 199         while(timer->TC_CV > 1) ; /* Wait till the clock has reset */
 200 }
 201
 202 /* Receive a frame from the card in reader emulation mode, the FPGA and
 203  * timer must have been set up by LegicRfReader and frame_send_rwd.
 204  *
 205  * The LEGIC RF protocol from card to reader does not include explicit
 206  * frame start/stop information or length information. The reader must
 207  * know beforehand how many bits it wants to receive. (Notably: a card
 208  * sending a stream of 0-bits is indistinguishable from no card present.)
 209  *
 210  * Receive methodology: There is a fancy correlator in hi_read_rx_xcorr, but
 211  * I'm not smart enough to use it. Instead I have patched hi_read_tx to output
 212  * the ADC signal with hysteresis on SSP_DIN. Bit-bang that signal and look
 213  * for edges. Count the edges in each bit interval. If they are approximately
 214  * 0 this was a 0-bit, if they are approximately equal to the number of edges
 215  * expected for a 212kHz subcarrier, this was a 1-bit. For timing we use the
 216  * timer that's still running from frame_send_rwd in order to get a synchronization
 217  * with the frame that we just sent.
 218  *
 219  * FIXME: Because we're relying on the hysteresis to just do the right thing
 220  * the range is severely reduced (and you'll probably also need a good antenna).
 221  * So this should be fixed some time in the future for a proper receiver.
 222  */
 223 static void frame_receive_rwd(struct legic_frame * const f, int bits, int crypt)
 224 {
 225         uint32_t the_bit = 1;  /* Use a bitmask to save on shifts */
 226         uint32_t data=0;
 227         int i, old_level=0, edges=0;
 228         int next_bit_at = TAG_TIME_WAIT;
 229
 230         if(bits > 32) {
 231                 bits = 32;
 232     }
 233
 234         AT91C_BASE_PIOA->PIO_ODR = GPIO_SSC_DIN;
 235         AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DIN;
 236
 237         /* we have some time now, precompute the cipher
 238      * since we cannot compute it on the fly while reading */
 239         legic_prng_forward(2);
 240
 241         if(crypt)
 242         {
 243                 for(i=0; i<bits; i++) {
 244                         data |= legic_prng_get_bit() << i;
 245                         legic_prng_forward(1);
 246                 }
 247         }
 248
 249         while(timer->TC_CV < next_bit_at) ;
 250
 251         next_bit_at += TAG_TIME_BIT;
 252
 253         for(i=0; i<bits; i++) {
 254                 edges = 0;
 255                 while(timer->TC_CV < next_bit_at) {
 256                         int level = (AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_DIN);
 257                         if(level != old_level)
 258                                 edges++;
 259                         old_level = level;
 260                 }
 261                 next_bit_at += TAG_TIME_BIT;
 262
 263                 if(edges > 20 && edges < 60) { /* expected are 42 edges */
 264                         data ^= the_bit;
 265                 }
 266                 the_bit <<= 1;
 267         }
 268
 269         f->data = data;
 270         f->bits = bits;
 271
 272         /* Reset the timer, to synchronize the next frame */
 273         timer->TC_CCR = AT91C_TC_SWTRG;
 274         while(timer->TC_CV > 1) ; /* Wait till the clock has reset */
 275 }
 276
 277 static void frame_append_bit(struct legic_frame * const f, int bit)
 278 {
 279    if(f->bits >= 31) {
 280        return; /* Overflow, won't happen */
 281    }
 282    f->data |= (bit<<f->bits);
 283    f->bits++;
 284 }
 285
 286 static void frame_clean(struct legic_frame * const f)
 287 {
 288         f->data = 0;
 289         f->bits = 0;
 290 }
 291
 292 static uint32_t perform_setup_phase_rwd(int iv)
 293 {
 294
 295         /* Switch on carrier and let the tag charge for 1ms */
 296         AT91C_BASE_PIOA->PIO_SODR = GPIO_SSC_DOUT;
 297         SpinDelay(1);
 298
 299         legic_prng_init(0); /* no keystream yet */
 300         frame_send_rwd(iv, 7);
 301         legic_prng_init(iv);
 302
 303         frame_clean(&current_frame);
 304         frame_receive_rwd(&current_frame, 6, 1);
 305         legic_prng_forward(1); /* we wait anyways */
 306         while(timer->TC_CV < 387) ; /* ~ 258us */
 307         frame_send_rwd(0x19, 6);
 308
 309         return current_frame.data;
 310 }
 311
 312 static void LegicCommonInit(void) {
 313         SetAdcMuxFor(GPIO_MUXSEL_HIPKD);
 314         FpgaSetupSsc();
 315         FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_READER_TX);
 316
 317         /* Bitbang the transmitter */
 318         AT91C_BASE_PIOA->PIO_CODR = GPIO_SSC_DOUT;
 319         AT91C_BASE_PIOA->PIO_OER = GPIO_SSC_DOUT;
 320         AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT;
 321
 322         setup_timer();
 323
 324         crc_init(&legic_crc, 4, 0x19 >> 1, 0x5, 0);
 325 }
 326
 327 static void switch_off_tag_rwd(void)
 328 {
 329         /* Switch off carrier, make sure tag is reset */
 330         AT91C_BASE_PIOA->PIO_CODR = GPIO_SSC_DOUT;
 331         SpinDelay(10);
 332
 333         WDT_HIT();
 334 }
 335 /* calculate crc for a legic command */
 336 static int LegicCRC(int byte_index, int value, int cmd_sz) {
 337         crc_clear(&legic_crc);
 338         crc_update(&legic_crc, 1, 1); /* CMD_READ */
 339         crc_update(&legic_crc, byte_index, cmd_sz-1);
 340         crc_update(&legic_crc, value, 8);
 341         return crc_finish(&legic_crc);
 342 }
 343
 344 int legic_read_byte(int byte_index, int cmd_sz) {
 345         int byte;
 346
 347         legic_prng_forward(4); /* we wait anyways */
 348         while(timer->TC_CV < 387) ; /* ~ 258us + 100us*delay */
 349
 350         frame_send_rwd(1 | (byte_index << 1), cmd_sz);
 351         frame_clean(&current_frame);
 352
 353         frame_receive_rwd(&current_frame, 12, 1);
 354
 355         byte = current_frame.data & 0xff;
 356         if( LegicCRC(byte_index, byte, cmd_sz) != (current_frame.data >> 8) ) {
 357                 Dbprintf("!!! crc mismatch: expected %x but got %x !!!",
 358            LegicCRC(byte_index, current_frame.data & 0xff, cmd_sz), current_frame.data >> 8);
 359                 return -1;
 360         }
 361
 362         return byte;
 363 }
 364
 365 /* legic_write_byte() is not included, however it's trivial to implement
 366  * and here are some hints on what remains to be done:
 367  *
 368  *  * assemble a write_cmd_frame with crc and send it
 369  *  * wait until the tag sends back an ACK ('1' bit unencrypted)
 370  *  * forward the prng based on the timing
 371  */
 372 int legic_write_byte(int byte, int addr, int addr_sz) {
 373     //do not write UID, CRC, DCF
 374     if(addr <= 0x06) {
 375                 return 0;
 376         }
 377
 378         //== send write command ==============================
 379         crc_clear(&legic_crc);
 380         crc_update(&legic_crc, 0, 1); /* CMD_WRITE */
 381         crc_update(&legic_crc, addr, addr_sz);
 382         crc_update(&legic_crc, byte, 8);
 383
 384         uint32_t crc = crc_finish(&legic_crc);
 385         uint32_t cmd = ((crc     <<(addr_sz+1+8)) //CRC
 386                    |(byte    <<(addr_sz+1))   //Data
 387                    |(addr    <<1)             //Address
 388                    |(0x00    <<0));           //CMD = W
 389     uint32_t cmd_sz = addr_sz+1+8+4;          //crc+data+cmd
 390
 391     legic_prng_forward(2); /* we wait anyways */
 392     while(timer->TC_CV < 387) ; /* ~ 258us */
 393         frame_send_rwd(cmd, cmd_sz);
 394
 395         //== wait for ack ====================================
 396     int t, old_level=0, edges=0;
 397     int next_bit_at =0;
 398         while(timer->TC_CV < 387) ; /* ~ 258us */
 399     for(t=0; t<80; t++) {
 400         edges = 0;
 401                 next_bit_at += TAG_TIME_BIT;
 402         while(timer->TC_CV < next_bit_at) {
 403             int level = (AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_DIN);
 404             if(level != old_level) {
 405                 edges++;
 406                         }
 407             old_level = level;
 408         }
 409         if(edges > 20 && edges < 60) { /* expected are 42 edges */
 410                         int t = timer->TC_CV;
 411                         int c = t/TAG_TIME_BIT;
 412                         timer->TC_CCR = AT91C_TC_SWTRG;
 413                         while(timer->TC_CV > 1) ; /* Wait till the clock has reset */
 414                         legic_prng_forward(c);
 415                 return 0;
 416         }
 417     }
 418     timer->TC_CCR = AT91C_TC_SWTRG;
 419     while(timer->TC_CV > 1) ; /* Wait till the clock has reset */
 420         return -1;
 421 }
 422
 423 int LegicRfReader(int offset, int bytes) {
 424         int byte_index=0, cmd_sz=0, card_sz=0;
 425
 426         LegicCommonInit();
 427
 428         memset(BigBuf, 0, 1024);
 429
 430         DbpString("setting up legic card");
 431         uint32_t tag_type = perform_setup_phase_rwd(SESSION_IV);
 432         switch_off_tag_rwd(); //we lose to mutch time with dprintf
 433         switch(tag_type) {
 434                 case 0x1d:
 435                         DbpString("MIM 256 card found, reading card ...");
 436             cmd_sz = 9;
 437                         card_sz = 256;
 438                         break;
 439                 case 0x3d:
 440                         DbpString("MIM 1024 card found, reading card ...");
 441             cmd_sz = 11;
 442                         card_sz = 1024;
 443                         break;
 444                 default:
 445                         Dbprintf("Unknown card format: %x",tag_type);
 446                         return -1;
 447         }
 448         if(bytes == -1) {
 449                 bytes = card_sz;
 450         }
 451         if(bytes+offset >= card_sz) {
 452                 bytes = card_sz-offset;
 453         }
 454
 455         perform_setup_phase_rwd(SESSION_IV);
 456
 457         LED_B_ON();
 458         while(byte_index < bytes) {
 459                 int r = legic_read_byte(byte_index+offset, cmd_sz);
 460                 if(r == -1 ||BUTTON_PRESS()) {
 461                 DbpString("operation aborted");
 462                         switch_off_tag_rwd();
 463                 LED_B_OFF();
 464                 LED_C_OFF();
 465                 return -1;
 466                 }
 467                 ((uint8_t*)BigBuf)[byte_index] = r;
 468         WDT_HIT();
 469                 byte_index++;
 470                 if(byte_index & 0x10) LED_C_ON(); else LED_C_OFF();
 471         }
 472         LED_B_OFF();
 473     LED_C_OFF();
 474         switch_off_tag_rwd();
 475         Dbprintf("Card read, use 'hf legic decode' or");
 476     Dbprintf("'data hexsamples %d' to view results", (bytes+7) & ~7);
 477     return 0;
 478 }
 479
 480 void LegicRfWriter(int bytes, int offset) {
 481         int byte_index=0, addr_sz=0;
 482
 483         LegicCommonInit();
 484
 485         DbpString("setting up legic card");
 486         uint32_t tag_type = perform_setup_phase_rwd(SESSION_IV);
 487         switch_off_tag_rwd();
 488         switch(tag_type) {
 489                 case 0x1d:
 490                         if(offset+bytes > 0x100) {
 491                                 Dbprintf("Error: can not write to 0x%03.3x on MIM 256", offset+bytes);
 492                                 return;
 493                         }
 494                         addr_sz = 8;
 495                         Dbprintf("MIM 256 card found, writing 0x%02.2x - 0x%02.2x ...", offset, offset+bytes);
 496                         break;
 497                 case 0x3d:
 498                         if(offset+bytes > 0x400) {
 499                         Dbprintf("Error: can not write to 0x%03.3x on MIM 1024", offset+bytes);
 500                         return;
 501                 }
 502                         addr_sz = 10;
 503                         Dbprintf("MIM 1024 card found, writing 0x%03.3x - 0x%03.3x ...", offset, offset+bytes);
 504                         break;
 505                 default:
 506                         Dbprintf("No or unknown card found, aborting");
 507             return;
 508         }
 509
 510     LED_B_ON();
 511         perform_setup_phase_rwd(SESSION_IV);
 512     legic_prng_forward(2);
 513         while(byte_index < bytes) {
 514                 int r = legic_write_byte(((uint8_t*)BigBuf)[byte_index+offset], byte_index+offset, addr_sz);
 515                 if((r != 0) || BUTTON_PRESS()) {
 516                         Dbprintf("operation aborted @ 0x%03.3x", byte_index);
 517                         switch_off_tag_rwd();
 518                         LED_B_OFF();
 519                         LED_C_OFF();
 520                         return;
 521                 }
 522         WDT_HIT();
 523                 byte_index++;
 524         if(byte_index & 0x10) LED_C_ON(); else LED_C_OFF();
 525         }
 526     LED_B_OFF();
 527     LED_C_OFF();
 528     DbpString("write successful");
 529 }
 530
 531 int timestamp;
 532
 533 /* Handle (whether to respond) a frame in tag mode */
 534 static void frame_handle_tag(struct legic_frame const * const f)
 535 {
 536    /* First Part of Handshake (IV) */
 537    if(f->bits == 7) {
 538      if(f->data == SESSION_IV) {
 539         LED_C_ON();
 540         prng_timer->TC_CCR = AT91C_TC_SWTRG;
 541         legic_prng_init(f->data);
 542         frame_send_tag(0x3d, 6, 1); /* 0x3d^0x26 = 0x1b */
 543         legic_state = STATE_IV;
 544         legic_read_count = 0;
 545         legic_prng_bc = 0;
 546         legic_prng_iv = f->data;
 547
 548         /* TIMEOUT */
 549         timer->TC_CCR = AT91C_TC_SWTRG;
 550         while(timer->TC_CV > 1);
 551         while(timer->TC_CV < 280);
 552         return;
 553       } else if((prng_timer->TC_CV % 50) > 40) {
 554         legic_prng_init(f->data);
 555         frame_send_tag(0x3d, 6, 1);
 556         SpinDelay(20);
 557         return;
 558      }
 559    }
 560
 561    /* 0x19==??? */
 562    if(legic_state == STATE_IV) {
 563       if((f->bits == 6) && (f->data == (0x19 ^ get_key_stream(1, 6)))) {
 564          legic_state = STATE_CON;
 565
 566          /* TIMEOUT */
 567          timer->TC_CCR = AT91C_TC_SWTRG;
 568          while(timer->TC_CV > 1);
 569          while(timer->TC_CV < 200);
 570          return;
 571       } else {
 572          legic_state = STATE_DISCON;
 573          LED_C_OFF();
 574          Dbprintf("0x19 - Frame: %03.3x", f->data);
 575          return;
 576       }
 577    }
 578
 579    /* Read */
 580    if(f->bits == 11) {
 581       if(legic_state == STATE_CON) {
 582          int key   = get_key_stream(-1, 11); //legic_phase_drift, 11);
 583          int addr  = f->data ^ key; addr = addr >> 1;
 584          int data = ((uint8_t*)BigBuf)[addr];
 585          int hash = LegicCRC(addr, data, 11) << 8;
 586          ((uint8_t*)BigBuf)[OFFSET_LOG+legic_read_count] = (uint8_t)addr;
 587          legic_read_count++;
 588
 589          //Dbprintf("Data:%03.3x, key:%03.3x, addr: %03.3x, read_c:%u", f->data, key, addr, read_c);
 590          legic_prng_forward(legic_reqresp_drift);
 591
 592          frame_send_tag(hash | data, 12, 1);
 593
 594          /* SHORT TIMEOUT */
 595          timer->TC_CCR = AT91C_TC_SWTRG;
 596          while(timer->TC_CV > 1);
 597          legic_prng_forward(legic_frame_drift);
 598          while(timer->TC_CV < 180);
 599          return;
 600       }
 601    }
 602
 603    /* Write */
 604    if(f->bits == 23) {
 605       int key   = get_key_stream(-1, 23); //legic_frame_drift, 23);
 606       int addr  = f->data ^ key; addr = addr >> 1; addr = addr & 0x3ff;
 607       int data  = f->data ^ key; data = data >> 11; data = data & 0xff;
 608
 609       /* write command */
 610       legic_state = STATE_DISCON;
 611       LED_C_OFF();
 612       Dbprintf("write - addr: %x, data: %x", addr, data);
 613       return;
 614    }
 615
 616    if(legic_state != STATE_DISCON) {
 617       Dbprintf("Unexpected: sz:%u, Data:%03.3x, State:%u, Count:%u", f->bits, f->data, legic_state, legic_read_count);
 618       int i;
 619       Dbprintf("IV: %03.3x", legic_prng_iv);
 620       for(i = 0; i<legic_read_count; i++) {
 621          Dbprintf("Read Nb: %u, Addr: %u", i, ((uint8_t*)BigBuf)[OFFSET_LOG+i]);
 622       }
 623
 624       for(i = -1; i<legic_read_count; i++) {
 625          uint32_t t;
 626          t  = ((uint8_t*)BigBuf)[OFFSET_LOG+256+i*4];
 627          t |= ((uint8_t*)BigBuf)[OFFSET_LOG+256+i*4+1] << 8;
 628          t |= ((uint8_t*)BigBuf)[OFFSET_LOG+256+i*4+2] <<16;
 629          t |= ((uint8_t*)BigBuf)[OFFSET_LOG+256+i*4+3] <<24;
 630
 631          Dbprintf("Cycles: %u, Frame Length: %u, Time: %u",
 632             ((uint8_t*)BigBuf)[OFFSET_LOG+128+i],
 633             ((uint8_t*)BigBuf)[OFFSET_LOG+384+i],
 634             t);
 635       }
 636    }
 637    legic_state = STATE_DISCON;
 638    legic_read_count = 0;
 639    SpinDelay(10);
 640    LED_C_OFF();
 641    return;
 642 }
 643
 644 /* Read bit by bit untill full frame is received
 645  * Call to process frame end answer
 646  */
 647 static void emit(int bit)
 648 {
 649   if(bit == -1) {
 650      if(current_frame.bits <= 4) {
 651         frame_clean(&current_frame);
 652      } else {
 653         frame_handle_tag(&current_frame);
 654         frame_clean(&current_frame);
 655      }
 656      WDT_HIT();
 657   } else if(bit == 0) {
 658     frame_append_bit(&current_frame, 0);
 659   } else if(bit == 1) {
 660     frame_append_bit(&current_frame, 1);
 661   }
 662 }
 663
 664 void LegicRfSimulate(int phase, int frame, int reqresp)
 665 {
 666   /* ADC path high-frequency peak detector, FPGA in high-frequency simulator mode,
 667    * modulation mode set to 212kHz subcarrier. We are getting the incoming raw
 668    * envelope waveform on DIN and should send our response on DOUT.
 669    *
 670    * The LEGIC RF protocol is pulse-pause-encoding from reader to card, so we'll
 671    * measure the time between two rising edges on DIN, and no encoding on the
 672    * subcarrier from card to reader, so we'll just shift out our verbatim data
 673    * on DOUT, 1 bit is 100us. The time from reader to card frame is still unclear,
 674    * seems to be 300us-ish.
 675    */
 676
 677    if(phase < 0) {
 678       int i;
 679       for(i=0; i<=reqresp; i++) {
 680          legic_prng_init(SESSION_IV);
 681          Dbprintf("i=%u, key 0x%3.3x", i, get_key_stream(i, frame));
 682       }
 683       return;
 684    }
 685
 686    legic_phase_drift = phase;
 687    legic_frame_drift = frame;
 688    legic_reqresp_drift = reqresp;
 689
 690    SetAdcMuxFor(GPIO_MUXSEL_HIPKD);
 691    FpgaSetupSsc();
 692    FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_SIMULATOR | FPGA_HF_SIMULATOR_MODULATE_212K);
 693
 694    /* Bitbang the receiver */
 695    AT91C_BASE_PIOA->PIO_ODR = GPIO_SSC_DIN;
 696    AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DIN;
 697
 698    setup_timer();
 699    crc_init(&legic_crc, 4, 0x19 >> 1, 0x5, 0);
 700
 701    int old_level = 0;
 702    int active = 0;
 703    legic_state = STATE_DISCON;
 704
 705    LED_B_ON();
 706    DbpString("Starting Legic emulator, press button to end");
 707    while(!BUTTON_PRESS()) {
 708       int level = !!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_DIN);
 709       int time = timer->TC_CV;
 710
 711       if(level != old_level) {
 712          if(level == 1) {
 713             timer->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
 714             if(FUZZ_EQUAL(time, RWD_TIME_1, RWD_TIME_FUZZ)) {
 715                /* 1 bit */
 716                emit(1);
 717                active = 1;
 718                LED_A_ON();
 719             } else if(FUZZ_EQUAL(time, RWD_TIME_0, RWD_TIME_FUZZ)) {
 720                /* 0 bit */
 721                emit(0);
 722                active = 1;
 723                LED_A_ON();
 724             } else if(active) {
 725                /* invalid */
 726                emit(-1);
 727                active = 0;
 728                LED_A_OFF();
 729             }
 730          }
 731       }
 732
 733       if(time >= (RWD_TIME_1+RWD_TIME_FUZZ) && active) {
 734          /* Frame end */
 735          emit(-1);
 736          active = 0;
 737          LED_A_OFF();
 738       }
 739
 740       if(time >= (20*RWD_TIME_1) && (timer->TC_SR & AT91C_TC_CLKSTA)) {
 741          timer->TC_CCR = AT91C_TC_CLKDIS;
 742       }
 743
 744       old_level = level;
 745       WDT_HIT();
 746    }
 747    DbpString("Stopped");
 748    LED_B_OFF();
 749    LED_A_OFF();
 750    LED_C_OFF();
 751 }
 752