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

   1 //-----------------------------------------------------------------------------
   2 // This code is licensed to you under the terms of the GNU GPL, version 2 or,
   3 // at your option, any later version. See the LICENSE.txt file for the text of
   4 // the license.
   5 //-----------------------------------------------------------------------------
   6 // Miscellaneous routines for low frequency tag operations.
   7 // Tags supported here so far are Texas Instruments (TI), HID
   8 // Also routines for raw mode reading/simulating of LF waveform
   9 //-----------------------------------------------------------------------------
  10
  11 #include "proxmark3.h"
  12 #include "apps.h"
  13 #include "util.h"
  14 #include "hitag2.h"
  15 #include "crc16.h"
  16 #include "string.h"
  17 #include "lfdemod.h"
  18
  19
  20 /**
  21 * Does the sample acquisition. If threshold is specified, the actual sampling
  22 * is not commenced until the threshold has been reached.
  23 * @param trigger_threshold - the threshold
  24 * @param silent - is true, now outputs are made. If false, dbprints the status
  25 */
  26 void DoAcquisition125k_internal(int trigger_threshold,bool silent)
  27 {
  28     uint8_t *dest = BigBuf_get_addr();
  29     int n = BigBuf_max_traceLen();
  30     int i;
  31
  32     memset(dest, 0, n);
  33     i = 0;
  34     for(;;) {
  35         if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_TXRDY) {
  36             AT91C_BASE_SSC->SSC_THR = 0x43;
  37             LED_D_ON();
  38         }
  39         if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) {
  40             dest[i] = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
  41             LED_D_OFF();
  42             if (trigger_threshold != -1 && dest[i] < trigger_threshold)
  43                 continue;
  44             else
  45                 trigger_threshold = -1;
  46             if (++i >= n) break;
  47         }
  48     }
  49     if(!silent)
  50     {
  51         Dbprintf("buffer samples: %02x %02x %02x %02x %02x %02x %02x %02x ...",
  52                  dest[0], dest[1], dest[2], dest[3], dest[4], dest[5], dest[6], dest[7]);
  53
  54     }
  55 }
  56 /**
  57 * Perform sample aquisition.
  58 */
  59 void DoAcquisition125k(int trigger_threshold)
  60 {
  61     DoAcquisition125k_internal(trigger_threshold, false);
  62 }
  63
  64 /**
  65 * Setup the FPGA to listen for samples. This method downloads the FPGA bitstream
  66 * if not already loaded, sets divisor and starts up the antenna.
  67 * @param divisor : 1, 88> 255 or negative ==> 134.8 KHz
  68 *                                  0 or 95 ==> 125 KHz
  69 *
  70 **/
  71 void LFSetupFPGAForADC(int divisor, bool lf_field)
  72 {
  73     FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
  74     if ( (divisor == 1) || (divisor < 0) || (divisor > 255) )
  75         FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 88); //134.8Khz
  76     else if (divisor == 0)
  77         FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
  78     else
  79         FpgaSendCommand(FPGA_CMD_SET_DIVISOR, divisor);
  80
  81     FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | (lf_field ? FPGA_LF_ADC_READER_FIELD : 0));
  82
  83     // Connect the A/D to the peak-detected low-frequency path.
  84     SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
  85
  86     // Give it a bit of time for the resonant antenna to settle.
  87         SpinDelay(150);
  88
  89     // Now set up the SSC to get the ADC samples that are now streaming at us.
  90     FpgaSetupSsc();
  91 }
  92 /**
  93 * Initializes the FPGA, and acquires the samples.
  94 **/
  95 void AcquireRawAdcSamples125k(int divisor)
  96 {
  97     LFSetupFPGAForADC(divisor, true);
  98     // Now call the acquisition routine
  99     DoAcquisition125k_internal(-1,false);
 100 }
 101 /**
 102 * Initializes the FPGA for snoop-mode, and acquires the samples.
 103 **/
 104
 105 void SnoopLFRawAdcSamples(int divisor, int trigger_threshold)
 106 {
 107     LFSetupFPGAForADC(divisor, false);
 108     DoAcquisition125k(trigger_threshold);
 109 }
 110
 111 void ModThenAcquireRawAdcSamples125k(int delay_off, int period_0, int period_1, uint8_t *command)
 112 {
 113
 114     /* Make sure the tag is reset */
 115     FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
 116     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 117     SpinDelay(2500);
 118
 119
 120     int divisor_used = 95; // 125 KHz
 121     // see if 'h' was specified
 122
 123     if (command[strlen((char *) command) - 1] == 'h')
 124         divisor_used = 88; // 134.8 KHz
 125
 126
 127     FpgaSendCommand(FPGA_CMD_SET_DIVISOR, divisor_used);
 128     FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
 129     // Give it a bit of time for the resonant antenna to settle.
 130     SpinDelay(50);
 131
 132     // And a little more time for the tag to fully power up
 133     SpinDelay(2000);
 134
 135     // Now set up the SSC to get the ADC samples that are now streaming at us.
 136     FpgaSetupSsc();
 137
 138     // now modulate the reader field
 139     while(*command != '\0' && *command != ' ') {
 140         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 141         LED_D_OFF();
 142         SpinDelayUs(delay_off);
 143         FpgaSendCommand(FPGA_CMD_SET_DIVISOR, divisor_used);
 144
 145         FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
 146         LED_D_ON();
 147         if(*(command++) == '0')
 148             SpinDelayUs(period_0);
 149         else
 150             SpinDelayUs(period_1);
 151     }
 152     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 153     LED_D_OFF();
 154     SpinDelayUs(delay_off);
 155     FpgaSendCommand(FPGA_CMD_SET_DIVISOR, divisor_used);
 156
 157     FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
 158
 159     // now do the read
 160     DoAcquisition125k(-1);
 161 }
 162
 163 /* blank r/w tag data stream
 164 ...0000000000000000 01111111
 165 1010101010101010101010101010101010101010101010101010101010101010
 166 0011010010100001
 167 01111111
 168 101010101010101[0]000...
 169
 170 [5555fe852c5555555555555555fe0000]
 171 */
 172 void ReadTItag(void)
 173 {
 174     // some hardcoded initial params
 175     // when we read a TI tag we sample the zerocross line at 2Mhz
 176     // TI tags modulate a 1 as 16 cycles of 123.2Khz
 177     // TI tags modulate a 0 as 16 cycles of 134.2Khz
 178  #define FSAMPLE 2000000
 179  #define FREQLO 123200
 180  #define FREQHI 134200
 181
 182     signed char *dest = (signed char *)BigBuf_get_addr();
 183     uint16_t n = BigBuf_max_traceLen();
 184     // 128 bit shift register [shift3:shift2:shift1:shift0]
 185     uint32_t shift3 = 0, shift2 = 0, shift1 = 0, shift0 = 0;
 186
 187     int i, cycles=0, samples=0;
 188     // how many sample points fit in 16 cycles of each frequency
 189     uint32_t sampleslo = (FSAMPLE<<4)/FREQLO, sampleshi = (FSAMPLE<<4)/FREQHI;
 190     // when to tell if we're close enough to one freq or another
 191     uint32_t threshold = (sampleslo - sampleshi + 1)>>1;
 192
 193     // TI tags charge at 134.2Khz
 194     FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
 195     FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 88); //134.8Khz
 196
 197     // Place FPGA in passthrough mode, in this mode the CROSS_LO line
 198     // connects to SSP_DIN and the SSP_DOUT logic level controls
 199     // whether we're modulating the antenna (high)
 200     // or listening to the antenna (low)
 201     FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_PASSTHRU);
 202
 203     // get TI tag data into the buffer
 204     AcquireTiType();
 205
 206     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 207
 208     for (i=0; i<n-1; i++) {
 209         // count cycles by looking for lo to hi zero crossings
 210         if ( (dest[i]<0) && (dest[i+1]>0) ) {
 211             cycles++;
 212             // after 16 cycles, measure the frequency
 213             if (cycles>15) {
 214                 cycles=0;
 215                 samples=i-samples; // number of samples in these 16 cycles
 216
 217                 // TI bits are coming to us lsb first so shift them
 218                 // right through our 128 bit right shift register
 219                 shift0 = (shift0>>1) | (shift1 << 31);
 220                 shift1 = (shift1>>1) | (shift2 << 31);
 221                 shift2 = (shift2>>1) | (shift3 << 31);
 222                 shift3 >>= 1;
 223
 224                 // check if the cycles fall close to the number
 225                 // expected for either the low or high frequency
 226                 if ( (samples>(sampleslo-threshold)) && (samples<(sampleslo+threshold)) ) {
 227                     // low frequency represents a 1
 228                     shift3 |= (1<<31);
 229                 } else if ( (samples>(sampleshi-threshold)) && (samples<(sampleshi+threshold)) ) {
 230                     // high frequency represents a 0
 231                 } else {
 232                     // probably detected a gay waveform or noise
 233                     // use this as gaydar or discard shift register and start again
 234                     shift3 = shift2 = shift1 = shift0 = 0;
 235                 }
 236                 samples = i;
 237
 238                 // for each bit we receive, test if we've detected a valid tag
 239
 240                 // if we see 17 zeroes followed by 6 ones, we might have a tag
 241                 // remember the bits are backwards
 242                 if ( ((shift0 & 0x7fffff) == 0x7e0000) ) {
 243                     // if start and end bytes match, we have a tag so break out of the loop
 244                     if ( ((shift0>>16)&0xff) == ((shift3>>8)&0xff) ) {
 245                         cycles = 0xF0B; //use this as a flag (ugly but whatever)
 246                         break;
 247                     }
 248                 }
 249             }
 250         }
 251     }
 252
 253     // if flag is set we have a tag
 254     if (cycles!=0xF0B) {
 255         DbpString("Info: No valid tag detected.");
 256     } else {
 257         // put 64 bit data into shift1 and shift0
 258         shift0 = (shift0>>24) | (shift1 << 8);
 259         shift1 = (shift1>>24) | (shift2 << 8);
 260
 261         // align 16 bit crc into lower half of shift2
 262         shift2 = ((shift2>>24) | (shift3 << 8)) & 0x0ffff;
 263
 264         // if r/w tag, check ident match
 265                 if (shift3 & (1<<15) ) {
 266             DbpString("Info: TI tag is rewriteable");
 267             // only 15 bits compare, last bit of ident is not valid
 268                         if (((shift3 >> 16) ^ shift0) & 0x7fff ) {
 269                 DbpString("Error: Ident mismatch!");
 270             } else {
 271                 DbpString("Info: TI tag ident is valid");
 272             }
 273         } else {
 274             DbpString("Info: TI tag is readonly");
 275         }
 276
 277         // WARNING the order of the bytes in which we calc crc below needs checking
 278         // i'm 99% sure the crc algorithm is correct, but it may need to eat the
 279         // bytes in reverse or something
 280         // calculate CRC
 281         uint32_t crc=0;
 282
 283         crc = update_crc16(crc, (shift0)&0xff);
 284         crc = update_crc16(crc, (shift0>>8)&0xff);
 285         crc = update_crc16(crc, (shift0>>16)&0xff);
 286         crc = update_crc16(crc, (shift0>>24)&0xff);
 287         crc = update_crc16(crc, (shift1)&0xff);
 288         crc = update_crc16(crc, (shift1>>8)&0xff);
 289         crc = update_crc16(crc, (shift1>>16)&0xff);
 290         crc = update_crc16(crc, (shift1>>24)&0xff);
 291
 292         Dbprintf("Info: Tag data: %x%08x, crc=%x",
 293                  (unsigned int)shift1, (unsigned int)shift0, (unsigned int)shift2 & 0xFFFF);
 294         if (crc != (shift2&0xffff)) {
 295             Dbprintf("Error: CRC mismatch, expected %x", (unsigned int)crc);
 296         } else {
 297             DbpString("Info: CRC is good");
 298         }
 299     }
 300 }
 301
 302 void WriteTIbyte(uint8_t b)
 303 {
 304     int i = 0;
 305
 306     // modulate 8 bits out to the antenna
 307     for (i=0; i<8; i++)
 308     {
 309         if (b&(1<<i)) {
 310             // stop modulating antenna
 311             LOW(GPIO_SSC_DOUT);
 312             SpinDelayUs(1000);
 313             // modulate antenna
 314             HIGH(GPIO_SSC_DOUT);
 315             SpinDelayUs(1000);
 316         } else {
 317             // stop modulating antenna
 318             LOW(GPIO_SSC_DOUT);
 319             SpinDelayUs(300);
 320             // modulate antenna
 321             HIGH(GPIO_SSC_DOUT);
 322             SpinDelayUs(1700);
 323         }
 324     }
 325 }
 326
 327 void AcquireTiType(void)
 328 {
 329     int i, j, n;
 330     // tag transmission is <20ms, sampling at 2M gives us 40K samples max
 331     // each sample is 1 bit stuffed into a uint32_t so we need 1250 uint32_t
 332  #define TIBUFLEN 1250
 333
 334     // clear buffer
 335         uint32_t *BigBuf = (uint32_t *)BigBuf_get_addr();
 336     memset(BigBuf,0,BigBuf_max_traceLen()/sizeof(uint32_t));
 337
 338     // Set up the synchronous serial port
 339     AT91C_BASE_PIOA->PIO_PDR = GPIO_SSC_DIN;
 340     AT91C_BASE_PIOA->PIO_ASR = GPIO_SSC_DIN;
 341
 342     // steal this pin from the SSP and use it to control the modulation
 343     AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT;
 344     AT91C_BASE_PIOA->PIO_OER = GPIO_SSC_DOUT;
 345
 346     AT91C_BASE_SSC->SSC_CR = AT91C_SSC_SWRST;
 347     AT91C_BASE_SSC->SSC_CR = AT91C_SSC_RXEN | AT91C_SSC_TXEN;
 348
 349     // Sample at 2 Mbit/s, so TI tags are 16.2 vs. 14.9 clocks long
 350     // 48/2 = 24 MHz clock must be divided by 12
 351     AT91C_BASE_SSC->SSC_CMR = 12;
 352
 353     AT91C_BASE_SSC->SSC_RCMR = SSC_CLOCK_MODE_SELECT(0);
 354     AT91C_BASE_SSC->SSC_RFMR = SSC_FRAME_MODE_BITS_IN_WORD(32) | AT91C_SSC_MSBF;
 355     AT91C_BASE_SSC->SSC_TCMR = 0;
 356     AT91C_BASE_SSC->SSC_TFMR = 0;
 357
 358     LED_D_ON();
 359
 360     // modulate antenna
 361     HIGH(GPIO_SSC_DOUT);
 362
 363     // Charge TI tag for 50ms.
 364     SpinDelay(50);
 365
 366     // stop modulating antenna and listen
 367     LOW(GPIO_SSC_DOUT);
 368
 369     LED_D_OFF();
 370
 371     i = 0;
 372     for(;;) {
 373         if(AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) {
 374             BigBuf[i] = AT91C_BASE_SSC->SSC_RHR;        // store 32 bit values in buffer
 375             i++; if(i >= TIBUFLEN) break;
 376         }
 377         WDT_HIT();
 378     }
 379
 380     // return stolen pin to SSP
 381     AT91C_BASE_PIOA->PIO_PDR = GPIO_SSC_DOUT;
 382     AT91C_BASE_PIOA->PIO_ASR = GPIO_SSC_DIN | GPIO_SSC_DOUT;
 383
 384     char *dest = (char *)BigBuf_get_addr();
 385     n = TIBUFLEN*32;
 386     // unpack buffer
 387     for (i=TIBUFLEN-1; i>=0; i--) {
 388         for (j=0; j<32; j++) {
 389             if(BigBuf[i] & (1 << j)) {
 390                 dest[--n] = 1;
 391             } else {
 392                 dest[--n] = -1;
 393             }
 394         }
 395     }
 396 }
 397
 398 // arguments: 64bit data split into 32bit idhi:idlo and optional 16bit crc
 399 // if crc provided, it will be written with the data verbatim (even if bogus)
 400 // if not provided a valid crc will be computed from the data and written.
 401 void WriteTItag(uint32_t idhi, uint32_t idlo, uint16_t crc)
 402 {
 403     FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
 404     if(crc == 0) {
 405         crc = update_crc16(crc, (idlo)&0xff);
 406         crc = update_crc16(crc, (idlo>>8)&0xff);
 407         crc = update_crc16(crc, (idlo>>16)&0xff);
 408         crc = update_crc16(crc, (idlo>>24)&0xff);
 409         crc = update_crc16(crc, (idhi)&0xff);
 410         crc = update_crc16(crc, (idhi>>8)&0xff);
 411         crc = update_crc16(crc, (idhi>>16)&0xff);
 412         crc = update_crc16(crc, (idhi>>24)&0xff);
 413     }
 414     Dbprintf("Writing to tag: %x%08x, crc=%x",
 415              (unsigned int) idhi, (unsigned int) idlo, crc);
 416
 417     // TI tags charge at 134.2Khz
 418     FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 88); //134.8Khz
 419     // Place FPGA in passthrough mode, in this mode the CROSS_LO line
 420     // connects to SSP_DIN and the SSP_DOUT logic level controls
 421     // whether we're modulating the antenna (high)
 422     // or listening to the antenna (low)
 423     FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_PASSTHRU);
 424     LED_A_ON();
 425
 426     // steal this pin from the SSP and use it to control the modulation
 427     AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT;
 428     AT91C_BASE_PIOA->PIO_OER = GPIO_SSC_DOUT;
 429
 430     // writing algorithm:
 431     // a high bit consists of a field off for 1ms and field on for 1ms
 432     // a low bit consists of a field off for 0.3ms and field on for 1.7ms
 433     // initiate a charge time of 50ms (field on) then immediately start writing bits
 434     // start by writing 0xBB (keyword) and 0xEB (password)
 435     // then write 80 bits of data (or 64 bit data + 16 bit crc if you prefer)
 436     // finally end with 0x0300 (write frame)
 437     // all data is sent lsb firts
 438     // finish with 15ms programming time
 439
 440     // modulate antenna
 441     HIGH(GPIO_SSC_DOUT);
 442     SpinDelay(50);      // charge time
 443
 444     WriteTIbyte(0xbb); // keyword
 445     WriteTIbyte(0xeb); // password
 446     WriteTIbyte( (idlo    )&0xff );
 447     WriteTIbyte( (idlo>>8 )&0xff );
 448     WriteTIbyte( (idlo>>16)&0xff );
 449     WriteTIbyte( (idlo>>24)&0xff );
 450     WriteTIbyte( (idhi    )&0xff );
 451     WriteTIbyte( (idhi>>8 )&0xff );
 452     WriteTIbyte( (idhi>>16)&0xff );
 453     WriteTIbyte( (idhi>>24)&0xff ); // data hi to lo
 454     WriteTIbyte( (crc     )&0xff ); // crc lo
 455     WriteTIbyte( (crc>>8  )&0xff ); // crc hi
 456     WriteTIbyte(0x00); // write frame lo
 457     WriteTIbyte(0x03); // write frame hi
 458     HIGH(GPIO_SSC_DOUT);
 459     SpinDelay(50);      // programming time
 460
 461     LED_A_OFF();
 462
 463     // get TI tag data into the buffer
 464     AcquireTiType();
 465
 466     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 467     DbpString("Now use tiread to check");
 468 }
 469
 470 void SimulateTagLowFrequency(uint16_t period, uint32_t gap, uint8_t ledcontrol)
 471 {
 472     int i;
 473     uint8_t *tab = BigBuf_get_addr();
 474
 475     FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
 476     FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_EDGE_DETECT);
 477
 478     AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT | GPIO_SSC_CLK;
 479
 480     AT91C_BASE_PIOA->PIO_OER = GPIO_SSC_DOUT;
 481     AT91C_BASE_PIOA->PIO_ODR = GPIO_SSC_CLK;
 482
 483 #define SHORT_COIL()    LOW(GPIO_SSC_DOUT)
 484 #define OPEN_COIL()             HIGH(GPIO_SSC_DOUT)
 485
 486     i = 0;
 487     for(;;) {
 488         while(!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK)) {
 489             if(BUTTON_PRESS()) {
 490                 DbpString("Stopped");
 491                 return;
 492             }
 493             WDT_HIT();
 494         }
 495
 496         if (ledcontrol)
 497             LED_D_ON();
 498
 499         if(tab[i])
 500             OPEN_COIL();
 501         else
 502             SHORT_COIL();
 503
 504         if (ledcontrol)
 505             LED_D_OFF();
 506
 507         while(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK) {
 508             if(BUTTON_PRESS()) {
 509                 DbpString("Stopped");
 510                 return;
 511             }
 512             WDT_HIT();
 513         }
 514
 515         i++;
 516         if(i == period) {
 517             i = 0;
 518             if (gap) {
 519                 SHORT_COIL();
 520                 SpinDelayUs(gap);
 521             }
 522         }
 523     }
 524 }
 525
 526 #define DEBUG_FRAME_CONTENTS 1
 527 void SimulateTagLowFrequencyBidir(int divisor, int t0)
 528 {
 529 }
 530
 531 // compose fc/8 fc/10 waveform
 532 static void fc(int c, int *n) {
 533     uint8_t *dest = BigBuf_get_addr();
 534     int idx;
 535
 536     // for when we want an fc8 pattern every 4 logical bits
 537     if(c==0) {
 538         dest[((*n)++)]=1;
 539         dest[((*n)++)]=1;
 540         dest[((*n)++)]=0;
 541         dest[((*n)++)]=0;
 542         dest[((*n)++)]=0;
 543         dest[((*n)++)]=0;
 544         dest[((*n)++)]=0;
 545         dest[((*n)++)]=0;
 546     }
 547     //  an fc/8  encoded bit is a bit pattern of  11000000  x6 = 48 samples
 548     if(c==8) {
 549         for (idx=0; idx<6; idx++) {
 550             dest[((*n)++)]=1;
 551             dest[((*n)++)]=1;
 552             dest[((*n)++)]=0;
 553             dest[((*n)++)]=0;
 554             dest[((*n)++)]=0;
 555             dest[((*n)++)]=0;
 556             dest[((*n)++)]=0;
 557             dest[((*n)++)]=0;
 558         }
 559     }
 560
 561     //  an fc/10 encoded bit is a bit pattern of 1110000000 x5 = 50 samples
 562     if(c==10) {
 563         for (idx=0; idx<5; idx++) {
 564             dest[((*n)++)]=1;
 565             dest[((*n)++)]=1;
 566             dest[((*n)++)]=1;
 567             dest[((*n)++)]=0;
 568             dest[((*n)++)]=0;
 569             dest[((*n)++)]=0;
 570             dest[((*n)++)]=0;
 571             dest[((*n)++)]=0;
 572             dest[((*n)++)]=0;
 573             dest[((*n)++)]=0;
 574         }
 575     }
 576 }
 577
 578 // prepare a waveform pattern in the buffer based on the ID given then
 579 // simulate a HID tag until the button is pressed
 580 void CmdHIDsimTAG(int hi, int lo, int ledcontrol)
 581 {
 582     int n=0, i=0;
 583     /*
 584      HID tag bitstream format
 585      The tag contains a 44bit unique code. This is sent out MSB first in sets of 4 bits
 586      A 1 bit is represented as 6 fc8 and 5 fc10 patterns
 587      A 0 bit is represented as 5 fc10 and 6 fc8 patterns
 588      A fc8 is inserted before every 4 bits
 589      A special start of frame pattern is used consisting a0b0 where a and b are neither 0
 590      nor 1 bits, they are special patterns (a = set of 12 fc8 and b = set of 10 fc10)
 591     */
 592
 593     if (hi>0xFFF) {
 594         DbpString("Tags can only have 44 bits.");
 595         return;
 596     }
 597     fc(0,&n);
 598     // special start of frame marker containing invalid bit sequences
 599     fc(8,  &n); fc(8,  &n);     // invalid
 600     fc(8,  &n); fc(10, &n); // logical 0
 601     fc(10, &n); fc(10, &n); // invalid
 602     fc(8,  &n); fc(10, &n); // logical 0
 603
 604     WDT_HIT();
 605     // manchester encode bits 43 to 32
 606     for (i=11; i>=0; i--) {
 607         if ((i%4)==3) fc(0,&n);
 608         if ((hi>>i)&1) {
 609             fc(10, &n); fc(8,  &n);             // low-high transition
 610         } else {
 611             fc(8,  &n); fc(10, &n);             // high-low transition
 612         }
 613     }
 614
 615     WDT_HIT();
 616     // manchester encode bits 31 to 0
 617     for (i=31; i>=0; i--) {
 618         if ((i%4)==3) fc(0,&n);
 619         if ((lo>>i)&1) {
 620             fc(10, &n); fc(8,  &n);             // low-high transition
 621         } else {
 622             fc(8,  &n); fc(10, &n);             // high-low transition
 623         }
 624     }
 625
 626     if (ledcontrol)
 627         LED_A_ON();
 628     SimulateTagLowFrequency(n, 0, ledcontrol);
 629
 630     if (ledcontrol)
 631         LED_A_OFF();
 632 }
 633
 634 // loop to get raw HID waveform then FSK demodulate the TAG ID from it
 635 void CmdHIDdemodFSK(int findone, int *high, int *low, int ledcontrol)
 636 {
 637     uint8_t *dest = BigBuf_get_addr();
 638     const size_t sizeOfBigBuff = BigBuf_max_traceLen();
 639     size_t size = 0;
 640     uint32_t hi2=0, hi=0, lo=0;
 641     int idx=0;
 642     // Configure to go in 125Khz listen mode
 643     LFSetupFPGAForADC(95, true);
 644
 645     while(!BUTTON_PRESS()) {
 646
 647         WDT_HIT();
 648         if (ledcontrol) LED_A_ON();
 649
 650         DoAcquisition125k_internal(-1,true);
 651         // FSK demodulator
 652         size = sizeOfBigBuff;  //variable size will change after demod so re initialize it before use
 653                 idx = HIDdemodFSK(dest, &size, &hi2, &hi, &lo);
 654
 655                 if (idx>0 && lo>0){
 656             // final loop, go over previously decoded manchester data and decode into usable tag ID
 657             // 111000 bit pattern represent start of frame, 01 pattern represents a 1 and 10 represents a 0
 658             if (hi2 != 0){ //extra large HID tags
 659                 Dbprintf("TAG ID: %x%08x%08x (%d)",
 660                          (unsigned int) hi2, (unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF);
 661             }else {  //standard HID tags <38 bits
 662                 //Dbprintf("TAG ID: %x%08x (%d)",(unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF); //old print cmd
 663                 uint8_t bitlen = 0;
 664                 uint32_t fc = 0;
 665                 uint32_t cardnum = 0;
 666                                 if (((hi>>5)&1) == 1){//if bit 38 is set then < 37 bit format is used
 667                     uint32_t lo2=0;
 668                     lo2=(((hi & 31) << 12) | (lo>>20)); //get bits 21-37 to check for format len bit
 669                     uint8_t idx3 = 1;
 670                                         while(lo2 > 1){ //find last bit set to 1 (format len bit)
 671                                                 lo2=lo2 >> 1;
 672                         idx3++;
 673                     }
 674                                         bitlen = idx3+19;
 675                     fc =0;
 676                     cardnum=0;
 677                                         if(bitlen == 26){
 678                         cardnum = (lo>>1)&0xFFFF;
 679                         fc = (lo>>17)&0xFF;
 680                     }
 681                                         if(bitlen == 37){
 682                         cardnum = (lo>>1)&0x7FFFF;
 683                         fc = ((hi&0xF)<<12)|(lo>>20);
 684                     }
 685                                         if(bitlen == 34){
 686                         cardnum = (lo>>1)&0xFFFF;
 687                         fc= ((hi&1)<<15)|(lo>>17);
 688                     }
 689                                         if(bitlen == 35){
 690                         cardnum = (lo>>1)&0xFFFFF;
 691                         fc = ((hi&1)<<11)|(lo>>21);
 692                     }
 693                 }
 694                 else { //if bit 38 is not set then 37 bit format is used
 695                     bitlen= 37;
 696                     fc =0;
 697                     cardnum=0;
 698                     if(bitlen==37){
 699                         cardnum = (lo>>1)&0x7FFFF;
 700                         fc = ((hi&0xF)<<12)|(lo>>20);
 701                     }
 702                 }
 703                 //Dbprintf("TAG ID: %x%08x (%d)",
 704                 // (unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF);
 705                 Dbprintf("TAG ID: %x%08x (%d) - Format Len: %dbit - FC: %d - Card: %d",
 706                          (unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF,
 707                          (unsigned int) bitlen, (unsigned int) fc, (unsigned int) cardnum);
 708             }
 709             if (findone){
 710                 if (ledcontrol) LED_A_OFF();
 711                 *high = hi;
 712                 *low = lo;
 713                 return;
 714             }
 715             // reset
 716             hi2 = hi = lo = 0;
 717         }
 718         WDT_HIT();
 719     }
 720     DbpString("Stopped");
 721     if (ledcontrol) LED_A_OFF();
 722 }
 723
 724 void CmdEM410xdemod(int findone, int *high, int *low, int ledcontrol)
 725 {
 726     uint8_t *dest = BigBuf_get_addr();
 727
 728         size_t size=0, idx=0;
 729     int clk=0, invert=0, errCnt=0;
 730     uint64_t lo=0;
 731     // Configure to go in 125Khz listen mode
 732     LFSetupFPGAForADC(95, true);
 733
 734     while(!BUTTON_PRESS()) {
 735
 736         WDT_HIT();
 737         if (ledcontrol) LED_A_ON();
 738
 739         DoAcquisition125k_internal(-1,true);
 740         size  = BigBuf_max_traceLen();
 741         //Dbprintf("DEBUG: Buffer got");
 742                 //askdemod and manchester decode
 743                 errCnt = askmandemod(dest, &size, &clk, &invert);
 744         //Dbprintf("DEBUG: ASK Got");
 745         WDT_HIT();
 746
 747         if (errCnt>=0){
 748                         lo = Em410xDecode(dest, &size, &idx);
 749             //Dbprintf("DEBUG: EM GOT");
 750             if (lo>0){
 751                                 Dbprintf("EM TAG ID: %02x%08x - (%05d_%03d_%08d)",
 752                                     (uint32_t)(lo>>32),
 753                                     (uint32_t)lo,
 754                                     (uint32_t)(lo&0xFFFF),
 755                                     (uint32_t)((lo>>16LL) & 0xFF),
 756                                     (uint32_t)(lo & 0xFFFFFF));
 757             }
 758             if (findone){
 759                 if (ledcontrol) LED_A_OFF();
 760                 *high=lo>>32;
 761                 *low=lo & 0xFFFFFFFF;
 762                 return;
 763             }
 764         } else{
 765             //Dbprintf("DEBUG: No Tag");
 766         }
 767         WDT_HIT();
 768         lo = 0;
 769         clk=0;
 770         invert=0;
 771         errCnt=0;
 772         size=0;
 773     }
 774     DbpString("Stopped");
 775     if (ledcontrol) LED_A_OFF();
 776 }
 777
 778 void CmdIOdemodFSK(int findone, int *high, int *low, int ledcontrol)
 779 {
 780     uint8_t *dest = BigBuf_get_addr();
 781     int idx=0;
 782     uint32_t code=0, code2=0;
 783     uint8_t version=0;
 784     uint8_t facilitycode=0;
 785     uint16_t number=0;
 786     // Configure to go in 125Khz listen mode
 787     LFSetupFPGAForADC(95, true);
 788
 789     while(!BUTTON_PRESS()) {
 790         WDT_HIT();
 791         if (ledcontrol) LED_A_ON();
 792         DoAcquisition125k_internal(-1,true);
 793         //fskdemod and get start index
 794         WDT_HIT();
 795         idx = IOdemodFSK(dest, BigBuf_max_traceLen());
 796         if (idx>0){
 797             //valid tag found
 798
 799             //Index map
 800             //0           10          20          30          40          50          60
 801             //|           |           |           |           |           |           |
 802             //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23
 803             //-----------------------------------------------------------------------------
 804             //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11
 805             //
 806             //XSF(version)facility:codeone+codetwo
 807             //Handle the data
 808             if(findone){ //only print binary if we are doing one
 809                 Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx],   dest[idx+1],   dest[idx+2],dest[idx+3],dest[idx+4],dest[idx+5],dest[idx+6],dest[idx+7],dest[idx+8]);
 810                 Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+9], dest[idx+10],dest[idx+11],dest[idx+12],dest[idx+13],dest[idx+14],dest[idx+15],dest[idx+16],dest[idx+17]);
 811                 Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+18],dest[idx+19],dest[idx+20],dest[idx+21],dest[idx+22],dest[idx+23],dest[idx+24],dest[idx+25],dest[idx+26]);
 812                 Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+27],dest[idx+28],dest[idx+29],dest[idx+30],dest[idx+31],dest[idx+32],dest[idx+33],dest[idx+34],dest[idx+35]);
 813                 Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+36],dest[idx+37],dest[idx+38],dest[idx+39],dest[idx+40],dest[idx+41],dest[idx+42],dest[idx+43],dest[idx+44]);
 814                 Dbprintf("%d%d%d%d%d%d%d%d %d",dest[idx+45],dest[idx+46],dest[idx+47],dest[idx+48],dest[idx+49],dest[idx+50],dest[idx+51],dest[idx+52],dest[idx+53]);
 815                 Dbprintf("%d%d%d%d%d%d%d%d %d%d",dest[idx+54],dest[idx+55],dest[idx+56],dest[idx+57],dest[idx+58],dest[idx+59],dest[idx+60],dest[idx+61],dest[idx+62],dest[idx+63]);
 816             }
 817             code = bytebits_to_byte(dest+idx,32);
 818             code2 = bytebits_to_byte(dest+idx+32,32);
 819             version = bytebits_to_byte(dest+idx+27,8); //14,4
 820             facilitycode = bytebits_to_byte(dest+idx+18,8) ;
 821             number = (bytebits_to_byte(dest+idx+36,8)<<8)|(bytebits_to_byte(dest+idx+45,8)); //36,9
 822
 823             Dbprintf("XSF(%02d)%02x:%05d (%08x%08x)",version,facilitycode,number,code,code2);
 824             // if we're only looking for one tag
 825             if (findone){
 826                 if (ledcontrol) LED_A_OFF();
 827                 //LED_A_OFF();
 828                 *high=code;
 829                 *low=code2;
 830                 return;
 831             }
 832             code=code2=0;
 833             version=facilitycode=0;
 834             number=0;
 835             idx=0;
 836         }
 837         WDT_HIT();
 838     }
 839     DbpString("Stopped");
 840     if (ledcontrol) LED_A_OFF();
 841 }
 842
 843 /*------------------------------
 844  * T5555/T5557/T5567 routines
 845  *------------------------------
 846  */
 847
 848 /* T55x7 configuration register definitions */
 849 #define T55x7_POR_DELAY                 0x00000001
 850 #define T55x7_ST_TERMINATOR             0x00000008
 851 #define T55x7_PWD                       0x00000010
 852 #define T55x7_MAXBLOCK_SHIFT            5
 853 #define T55x7_AOR                       0x00000200
 854 #define T55x7_PSKCF_RF_2                0
 855 #define T55x7_PSKCF_RF_4                0x00000400
 856 #define T55x7_PSKCF_RF_8                0x00000800
 857 #define T55x7_MODULATION_DIRECT         0
 858 #define T55x7_MODULATION_PSK1           0x00001000
 859 #define T55x7_MODULATION_PSK2           0x00002000
 860 #define T55x7_MODULATION_PSK3           0x00003000
 861 #define T55x7_MODULATION_FSK1           0x00004000
 862 #define T55x7_MODULATION_FSK2           0x00005000
 863 #define T55x7_MODULATION_FSK1a          0x00006000
 864 #define T55x7_MODULATION_FSK2a          0x00007000
 865 #define T55x7_MODULATION_MANCHESTER     0x00008000
 866 #define T55x7_MODULATION_BIPHASE        0x00010000
 867 #define T55x7_BITRATE_RF_8              0
 868 #define T55x7_BITRATE_RF_16             0x00040000
 869 #define T55x7_BITRATE_RF_32             0x00080000
 870 #define T55x7_BITRATE_RF_40             0x000C0000
 871 #define T55x7_BITRATE_RF_50             0x00100000
 872 #define T55x7_BITRATE_RF_64             0x00140000
 873 #define T55x7_BITRATE_RF_100            0x00180000
 874 #define T55x7_BITRATE_RF_128            0x001C0000
 875
 876 /* T5555 (Q5) configuration register definitions */
 877 #define T5555_ST_TERMINATOR             0x00000001
 878 #define T5555_MAXBLOCK_SHIFT            0x00000001
 879 #define T5555_MODULATION_MANCHESTER     0
 880 #define T5555_MODULATION_PSK1           0x00000010
 881 #define T5555_MODULATION_PSK2           0x00000020
 882 #define T5555_MODULATION_PSK3           0x00000030
 883 #define T5555_MODULATION_FSK1           0x00000040
 884 #define T5555_MODULATION_FSK2           0x00000050
 885 #define T5555_MODULATION_BIPHASE        0x00000060
 886 #define T5555_MODULATION_DIRECT         0x00000070
 887 #define T5555_INVERT_OUTPUT             0x00000080
 888 #define T5555_PSK_RF_2                  0
 889 #define T5555_PSK_RF_4                  0x00000100
 890 #define T5555_PSK_RF_8                  0x00000200
 891 #define T5555_USE_PWD                   0x00000400
 892 #define T5555_USE_AOR                   0x00000800
 893 #define T5555_BITRATE_SHIFT             12
 894 #define T5555_FAST_WRITE                0x00004000
 895 #define T5555_PAGE_SELECT               0x00008000
 896
 897 /*
 898  * Relevant times in microsecond
 899  * To compensate antenna falling times shorten the write times
 900  * and enlarge the gap ones.
 901  */
 902 #define START_GAP 30*8 // 10 - 50fc 250
 903 #define WRITE_GAP 20*8 //  8 - 30fc
 904 #define WRITE_0   24*8 // 16 - 31fc 24fc 192
 905 #define WRITE_1   54*8 // 48 - 63fc 54fc 432 for T55x7; 448 for E5550
 906
 907 //  VALUES TAKEN FROM EM4x function: SendForward
 908 //  START_GAP = 440;       (55*8) cycles at 125Khz (8us = 1cycle)
 909 //  WRITE_GAP = 128;       (16*8)
 910 //  WRITE_1   = 256 32*8;  (32*8)
 911
 912 //  These timings work for 4469/4269/4305 (with the 55*8 above)
 913 //  WRITE_0 = 23*8 , 9*8  SpinDelayUs(23*8);
 914
 915 #define T55xx_SAMPLES_SIZE              12000 // 32 x 32 x 10  (32 bit times numofblock (7), times clock skip..)
 916
 917 // Write one bit to card
 918 void T55xxWriteBit(int bit)
 919 {
 920     FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
 921     FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
 922     FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
 923         if (!bit)
 924         SpinDelayUs(WRITE_0);
 925     else
 926         SpinDelayUs(WRITE_1);
 927     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 928     SpinDelayUs(WRITE_GAP);
 929 }
 930
 931 // Write one card block in page 0, no lock
 932 void T55xxWriteBlock(uint32_t Data, uint32_t Block, uint32_t Pwd, uint8_t PwdMode)
 933 {
 934         uint32_t i = 0;
 935
 936         // Set up FPGA, 125kHz
 937         // Wait for config.. (192+8190xPOW)x8 == 67ms
 938         LFSetupFPGAForADC(0, true);
 939
 940     // Now start writting
 941     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 942     SpinDelayUs(START_GAP);
 943
 944     // Opcode
 945     T55xxWriteBit(1);
 946     T55xxWriteBit(0); //Page 0
 947     if (PwdMode == 1){
 948         // Pwd
 949         for (i = 0x80000000; i != 0; i >>= 1)
 950             T55xxWriteBit(Pwd & i);
 951     }
 952     // Lock bit
 953     T55xxWriteBit(0);
 954
 955     // Data
 956     for (i = 0x80000000; i != 0; i >>= 1)
 957         T55xxWriteBit(Data & i);
 958
 959     // Block
 960     for (i = 0x04; i != 0; i >>= 1)
 961         T55xxWriteBit(Block & i);
 962
 963     // Now perform write (nominal is 5.6 ms for T55x7 and 18ms for E5550,
 964     // so wait a little more)
 965     FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
 966     FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
 967     SpinDelay(20);
 968     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 969 }
 970
 971 // Read one card block in page 0
 972 void T55xxReadBlock(uint32_t Block, uint32_t Pwd, uint8_t PwdMode)
 973 {
 974     uint8_t *dest = BigBuf_get_addr();
 975     //uint16_t bufferlength = BigBuf_max_traceLen();
 976         uint16_t bufferlength = T55xx_SAMPLES_SIZE;
 977     uint32_t i = 0;
 978         // Clear destination buffer before sending the command  0x80 = average.
 979         memset(dest, 0x80, bufferlength);
 980
 981         // Set up FPGA, 125kHz
 982         // Wait for config.. (192+8190xPOW)x8 == 67ms
 983         LFSetupFPGAForADC(0, true);
 984     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 985     SpinDelayUs(START_GAP);
 986
 987     // Opcode
 988     T55xxWriteBit(1);
 989     T55xxWriteBit(0); //Page 0
 990     if (PwdMode == 1){
 991         // Pwd
 992         for (i = 0x80000000; i != 0; i >>= 1)
 993             T55xxWriteBit(Pwd & i);
 994     }
 995     // Lock bit
 996     T55xxWriteBit(0);
 997     // Block
 998     for (i = 0x04; i != 0; i >>= 1)
 999         T55xxWriteBit(Block & i);
1000
1001     // Turn field on to read the response
1002         TurnReadLFOn();
1003
1004     // Now do the acquisition
1005     i = 0;
1006     for(;;) {
1007         if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_TXRDY) {
1008             AT91C_BASE_SSC->SSC_THR = 0x43;
1009                         //AT91C_BASE_SSC->SSC_THR = 0xff;
1010                         LED_D_ON();
1011         }
1012         if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) {
1013             dest[i] = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
1014                         ++i;
1015                         LED_D_OFF();
1016                         if (i >= bufferlength) break;
1017         }
1018     }
1019
1020         cmd_send(CMD_ACK,0,0,0,0,0);
1021     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1022     LED_D_OFF();
1023 }
1024
1025 // Read card traceability data (page 1)
1026 void T55xxReadTrace(void){
1027     uint8_t *dest = BigBuf_get_addr();
1028     //uint16_t bufferlength = BigBuf_max_traceLen();
1029         uint16_t bufferlength = T55xx_SAMPLES_SIZE;
1030         uint32_t i = 0;
1031
1032         // Clear destination buffer before sending the command 0x80 = average
1033         memset(dest, 0x80, bufferlength);
1034
1035         LFSetupFPGAForADC(0, true);
1036     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1037     SpinDelayUs(START_GAP);
1038
1039     // Opcode
1040     T55xxWriteBit(1);
1041     T55xxWriteBit(1); //Page 1
1042
1043     // Turn field on to read the response
1044         TurnReadLFOn();
1045
1046     // Now do the acquisition
1047     for(;;) {
1048         if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_TXRDY) {
1049             AT91C_BASE_SSC->SSC_THR = 0x43;
1050                         LED_D_ON();
1051         }
1052         if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) {
1053             dest[i] = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
1054                         ++i;
1055                         LED_D_OFF();
1056
1057                         if (i >= bufferlength) break;
1058                 }
1059         }
1060
1061         cmd_send(CMD_ACK,0,0,0,0,0);
1062     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1063     LED_D_OFF();
1064 }
1065
1066 void TurnReadLFOn(){
1067         FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
1068         FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
1069         // Give it a bit of time for the resonant antenna to settle.
1070         //SpinDelay(30);
1071         SpinDelayUs(8*150);
1072 }
1073
1074 /*-------------- Cloning routines -----------*/
1075 // Copy HID id to card and setup block 0 config
1076 void CopyHIDtoT55x7(uint32_t hi2, uint32_t hi, uint32_t lo, uint8_t longFMT)
1077 {
1078     int data1=0, data2=0, data3=0, data4=0, data5=0, data6=0; //up to six blocks for long format
1079     int last_block = 0;
1080
1081     if (longFMT){
1082         // Ensure no more than 84 bits supplied
1083         if (hi2>0xFFFFF) {
1084             DbpString("Tags can only have 84 bits.");
1085             return;
1086         }
1087         // Build the 6 data blocks for supplied 84bit ID
1088         last_block = 6;
1089         data1 = 0x1D96A900; // load preamble (1D) & long format identifier (9E manchester encoded)
1090         for (int i=0;i<4;i++) {
1091             if (hi2 & (1<<(19-i)))
1092                 data1 |= (1<<(((3-i)*2)+1)); // 1 -> 10
1093             else
1094                 data1 |= (1<<((3-i)*2)); // 0 -> 01
1095         }
1096
1097         data2 = 0;
1098         for (int i=0;i<16;i++) {
1099             if (hi2 & (1<<(15-i)))
1100                 data2 |= (1<<(((15-i)*2)+1)); // 1 -> 10
1101             else
1102                 data2 |= (1<<((15-i)*2)); // 0 -> 01
1103         }
1104
1105         data3 = 0;
1106         for (int i=0;i<16;i++) {
1107             if (hi & (1<<(31-i)))
1108                 data3 |= (1<<(((15-i)*2)+1)); // 1 -> 10
1109             else
1110                 data3 |= (1<<((15-i)*2)); // 0 -> 01
1111         }
1112
1113         data4 = 0;
1114         for (int i=0;i<16;i++) {
1115             if (hi & (1<<(15-i)))
1116                 data4 |= (1<<(((15-i)*2)+1)); // 1 -> 10
1117             else
1118                 data4 |= (1<<((15-i)*2)); // 0 -> 01
1119         }
1120
1121         data5 = 0;
1122         for (int i=0;i<16;i++) {
1123             if (lo & (1<<(31-i)))
1124                 data5 |= (1<<(((15-i)*2)+1)); // 1 -> 10
1125             else
1126                 data5 |= (1<<((15-i)*2)); // 0 -> 01
1127         }
1128
1129         data6 = 0;
1130         for (int i=0;i<16;i++) {
1131             if (lo & (1<<(15-i)))
1132                 data6 |= (1<<(((15-i)*2)+1)); // 1 -> 10
1133             else
1134                 data6 |= (1<<((15-i)*2)); // 0 -> 01
1135         }
1136     }
1137     else {
1138         // Ensure no more than 44 bits supplied
1139         if (hi>0xFFF) {
1140             DbpString("Tags can only have 44 bits.");
1141             return;
1142         }
1143
1144         // Build the 3 data blocks for supplied 44bit ID
1145         last_block = 3;
1146
1147         data1 = 0x1D000000; // load preamble
1148
1149         for (int i=0;i<12;i++) {
1150             if (hi & (1<<(11-i)))
1151                 data1 |= (1<<(((11-i)*2)+1)); // 1 -> 10
1152             else
1153                 data1 |= (1<<((11-i)*2)); // 0 -> 01
1154         }
1155
1156         data2 = 0;
1157         for (int i=0;i<16;i++) {
1158             if (lo & (1<<(31-i)))
1159                 data2 |= (1<<(((15-i)*2)+1)); // 1 -> 10
1160             else
1161                 data2 |= (1<<((15-i)*2)); // 0 -> 01
1162         }
1163
1164         data3 = 0;
1165         for (int i=0;i<16;i++) {
1166             if (lo & (1<<(15-i)))
1167                 data3 |= (1<<(((15-i)*2)+1)); // 1 -> 10
1168             else
1169                 data3 |= (1<<((15-i)*2)); // 0 -> 01
1170         }
1171     }
1172
1173     LED_D_ON();
1174     // Program the data blocks for supplied ID
1175     // and the block 0 for HID format
1176     T55xxWriteBlock(data1,1,0,0);
1177     T55xxWriteBlock(data2,2,0,0);
1178     T55xxWriteBlock(data3,3,0,0);
1179
1180     if (longFMT) { // if long format there are 6 blocks
1181         T55xxWriteBlock(data4,4,0,0);
1182         T55xxWriteBlock(data5,5,0,0);
1183         T55xxWriteBlock(data6,6,0,0);
1184     }
1185
1186     // Config for HID (RF/50, FSK2a, Maxblock=3 for short/6 for long)
1187     T55xxWriteBlock(T55x7_BITRATE_RF_50    |
1188                     T55x7_MODULATION_FSK2a |
1189                     last_block << T55x7_MAXBLOCK_SHIFT,
1190                     0,0,0);
1191
1192     LED_D_OFF();
1193
1194     DbpString("DONE!");
1195 }
1196
1197 void CopyIOtoT55x7(uint32_t hi, uint32_t lo, uint8_t longFMT)
1198 {
1199     int data1=0, data2=0; //up to six blocks for long format
1200
1201     data1 = hi;  // load preamble
1202     data2 = lo;
1203
1204     LED_D_ON();
1205     // Program the data blocks for supplied ID
1206     // and the block 0 for HID format
1207     T55xxWriteBlock(data1,1,0,0);
1208     T55xxWriteBlock(data2,2,0,0);
1209
1210     //Config Block
1211     T55xxWriteBlock(0x00147040,0,0,0);
1212     LED_D_OFF();
1213
1214     DbpString("DONE!");
1215 }
1216
1217 // Define 9bit header for EM410x tags
1218 #define EM410X_HEADER           0x1FF
1219 #define EM410X_ID_LENGTH        40
1220
1221 void WriteEM410x(uint32_t card, uint32_t id_hi, uint32_t id_lo)
1222 {
1223     int i, id_bit;
1224     uint64_t id = EM410X_HEADER;
1225     uint64_t rev_id = 0;        // reversed ID
1226     int c_parity[4];    // column parity
1227     int r_parity = 0;   // row parity
1228     uint32_t clock = 0;
1229
1230     // Reverse ID bits given as parameter (for simpler operations)
1231     for (i = 0; i < EM410X_ID_LENGTH; ++i) {
1232         if (i < 32) {
1233             rev_id = (rev_id << 1) | (id_lo & 1);
1234             id_lo >>= 1;
1235         } else {
1236             rev_id = (rev_id << 1) | (id_hi & 1);
1237             id_hi >>= 1;
1238         }
1239     }
1240
1241     for (i = 0; i < EM410X_ID_LENGTH; ++i) {
1242         id_bit = rev_id & 1;
1243
1244         if (i % 4 == 0) {
1245             // Don't write row parity bit at start of parsing
1246             if (i)
1247                 id = (id << 1) | r_parity;
1248             // Start counting parity for new row
1249             r_parity = id_bit;
1250         } else {
1251             // Count row parity
1252             r_parity ^= id_bit;
1253         }
1254
1255         // First elements in column?
1256         if (i < 4)
1257             // Fill out first elements
1258             c_parity[i] = id_bit;
1259         else
1260             // Count column parity
1261             c_parity[i % 4] ^= id_bit;
1262
1263         // Insert ID bit
1264         id = (id << 1) | id_bit;
1265         rev_id >>= 1;
1266     }
1267
1268     // Insert parity bit of last row
1269     id = (id << 1) | r_parity;
1270
1271     // Fill out column parity at the end of tag
1272     for (i = 0; i < 4; ++i)
1273         id = (id << 1) | c_parity[i];
1274
1275     // Add stop bit
1276     id <<= 1;
1277
1278     Dbprintf("Started writing %s tag ...", card ? "T55x7":"T5555");
1279     LED_D_ON();
1280
1281     // Write EM410x ID
1282     T55xxWriteBlock((uint32_t)(id >> 32), 1, 0, 0);
1283     T55xxWriteBlock((uint32_t)id, 2, 0, 0);
1284
1285     // Config for EM410x (RF/64, Manchester, Maxblock=2)
1286     if (card) {
1287         // Clock rate is stored in bits 8-15 of the card value
1288         clock = (card & 0xFF00) >> 8;
1289         Dbprintf("Clock rate: %d", clock);
1290         switch (clock)
1291         {
1292         case 32:
1293             clock = T55x7_BITRATE_RF_32;
1294             break;
1295         case 16:
1296             clock = T55x7_BITRATE_RF_16;
1297             break;
1298         case 0:
1299             // A value of 0 is assumed to be 64 for backwards-compatibility
1300             // Fall through...
1301         case 64:
1302             clock = T55x7_BITRATE_RF_64;
1303             break;
1304         default:
1305             Dbprintf("Invalid clock rate: %d", clock);
1306             return;
1307         }
1308
1309         // Writing configuration for T55x7 tag
1310         T55xxWriteBlock(clock       |
1311                         T55x7_MODULATION_MANCHESTER |
1312                         2 << T55x7_MAXBLOCK_SHIFT,
1313                         0, 0, 0);
1314     }
1315     else
1316         // Writing configuration for T5555(Q5) tag
1317         T55xxWriteBlock(0x1F << T5555_BITRATE_SHIFT |
1318                         T5555_MODULATION_MANCHESTER   |
1319                         2 << T5555_MAXBLOCK_SHIFT,
1320                         0, 0, 0);
1321
1322     LED_D_OFF();
1323     Dbprintf("Tag %s written with 0x%08x%08x\n", card ? "T55x7":"T5555",
1324              (uint32_t)(id >> 32), (uint32_t)id);
1325 }
1326
1327 // Clone Indala 64-bit tag by UID to T55x7
1328 void CopyIndala64toT55x7(int hi, int lo)
1329 {
1330
1331     //Program the 2 data blocks for supplied 64bit UID
1332     // and the block 0 for Indala64 format
1333     T55xxWriteBlock(hi,1,0,0);
1334     T55xxWriteBlock(lo,2,0,0);
1335     //Config for Indala (RF/32;PSK1 with RF/2;Maxblock=2)
1336     T55xxWriteBlock(T55x7_BITRATE_RF_32    |
1337                     T55x7_MODULATION_PSK1 |
1338                     2 << T55x7_MAXBLOCK_SHIFT,
1339                     0, 0, 0);
1340     //Alternative config for Indala (Extended mode;RF/32;PSK1 with RF/2;Maxblock=2;Inverse data)
1341     //  T5567WriteBlock(0x603E1042,0);
1342
1343     DbpString("DONE!");
1344
1345 }
1346
1347 void CopyIndala224toT55x7(int uid1, int uid2, int uid3, int uid4, int uid5, int uid6, int uid7)
1348 {
1349
1350     //Program the 7 data blocks for supplied 224bit UID
1351     // and the block 0 for Indala224 format
1352     T55xxWriteBlock(uid1,1,0,0);
1353     T55xxWriteBlock(uid2,2,0,0);
1354     T55xxWriteBlock(uid3,3,0,0);
1355     T55xxWriteBlock(uid4,4,0,0);
1356     T55xxWriteBlock(uid5,5,0,0);
1357     T55xxWriteBlock(uid6,6,0,0);
1358     T55xxWriteBlock(uid7,7,0,0);
1359     //Config for Indala (RF/32;PSK1 with RF/2;Maxblock=7)
1360     T55xxWriteBlock(T55x7_BITRATE_RF_32    |
1361                     T55x7_MODULATION_PSK1 |
1362                     7 << T55x7_MAXBLOCK_SHIFT,
1363                     0,0,0);
1364     //Alternative config for Indala (Extended mode;RF/32;PSK1 with RF/2;Maxblock=7;Inverse data)
1365     //  T5567WriteBlock(0x603E10E2,0);
1366
1367     DbpString("DONE!");
1368
1369 }
1370
1371
1372 #define abs(x) ( ((x)<0) ? -(x) : (x) )
1373 #define max(x,y) ( x<y ? y:x)
1374
1375 int DemodPCF7931(uint8_t **outBlocks) {
1376     uint8_t BitStream[256];
1377     uint8_t Blocks[8][16];
1378     uint8_t *GraphBuffer = BigBuf_get_addr();
1379     int GraphTraceLen = BigBuf_max_traceLen();
1380     int i, j, lastval, bitidx, half_switch;
1381     int clock = 64;
1382     int tolerance = clock / 8;
1383     int pmc, block_done;
1384     int lc, warnings = 0;
1385     int num_blocks = 0;
1386     int lmin=128, lmax=128;
1387     uint8_t dir;
1388
1389     AcquireRawAdcSamples125k(0);
1390
1391     lmin = 64;
1392     lmax = 192;
1393
1394     i = 2;
1395
1396     /* Find first local max/min */
1397     if(GraphBuffer[1] > GraphBuffer[0]) {
1398         while(i < GraphTraceLen) {
1399             if( !(GraphBuffer[i] > GraphBuffer[i-1]) && GraphBuffer[i] > lmax)
1400                 break;
1401             i++;
1402         }
1403         dir = 0;
1404     }
1405     else {
1406         while(i < GraphTraceLen) {
1407             if( !(GraphBuffer[i] < GraphBuffer[i-1]) && GraphBuffer[i] < lmin)
1408                 break;
1409             i++;
1410         }
1411         dir = 1;
1412     }
1413
1414     lastval = i++;
1415     half_switch = 0;
1416     pmc = 0;
1417     block_done = 0;
1418
1419     for (bitidx = 0; i < GraphTraceLen; i++)
1420     {
1421         if ( (GraphBuffer[i-1] > GraphBuffer[i] && dir == 1 && GraphBuffer[i] > lmax) || (GraphBuffer[i-1] < GraphBuffer[i] && dir == 0 && GraphBuffer[i] < lmin))
1422         {
1423             lc = i - lastval;
1424             lastval = i;
1425
1426             // Switch depending on lc length:
1427             // Tolerance is 1/8 of clock rate (arbitrary)
1428             if (abs(lc-clock/4) < tolerance) {
1429                 // 16T0
1430                 if((i - pmc) == lc) { /* 16T0 was previous one */
1431                     /* It's a PMC ! */
1432                     i += (128+127+16+32+33+16)-1;
1433                     lastval = i;
1434                     pmc = 0;
1435                     block_done = 1;
1436                 }
1437                 else {
1438                     pmc = i;
1439                 }
1440             } else if (abs(lc-clock/2) < tolerance) {
1441                 // 32TO
1442                 if((i - pmc) == lc) { /* 16T0 was previous one */
1443                     /* It's a PMC ! */
1444                     i += (128+127+16+32+33)-1;
1445                     lastval = i;
1446                     pmc = 0;
1447                     block_done = 1;
1448                 }
1449                 else if(half_switch == 1) {
1450                     BitStream[bitidx++] = 0;
1451                     half_switch = 0;
1452                 }
1453                 else
1454                     half_switch++;
1455             } else if (abs(lc-clock) < tolerance) {
1456                 // 64TO
1457                 BitStream[bitidx++] = 1;
1458             } else {
1459                 // Error
1460                 warnings++;
1461                 if (warnings > 10)
1462                 {
1463                     Dbprintf("Error: too many detection errors, aborting.");
1464                     return 0;
1465                 }
1466             }
1467
1468             if(block_done == 1) {
1469                 if(bitidx == 128) {
1470                     for(j=0; j<16; j++) {
1471                         Blocks[num_blocks][j] = 128*BitStream[j*8+7]+
1472                                 64*BitStream[j*8+6]+
1473                                 32*BitStream[j*8+5]+
1474                                 16*BitStream[j*8+4]+
1475                                 8*BitStream[j*8+3]+
1476                                 4*BitStream[j*8+2]+
1477                                 2*BitStream[j*8+1]+
1478                                 BitStream[j*8];
1479                     }
1480                     num_blocks++;
1481                 }
1482                 bitidx = 0;
1483                 block_done = 0;
1484                 half_switch = 0;
1485             }
1486             if(i < GraphTraceLen)
1487             {
1488                 if (GraphBuffer[i-1] > GraphBuffer[i]) dir=0;
1489                 else dir = 1;
1490             }
1491         }
1492         if(bitidx==255)
1493             bitidx=0;
1494         warnings = 0;
1495         if(num_blocks == 4) break;
1496     }
1497     memcpy(outBlocks, Blocks, 16*num_blocks);
1498     return num_blocks;
1499 }
1500
1501 int IsBlock0PCF7931(uint8_t *Block) {
1502     // Assume RFU means 0 :)
1503     if((memcmp(Block, "\x00\x00\x00\x00\x00\x00\x00\x01", 8) == 0) && memcmp(Block+9, "\x00\x00\x00\x00\x00\x00\x00", 7) == 0) // PAC enabled
1504         return 1;
1505     if((memcmp(Block+9, "\x00\x00\x00\x00\x00\x00\x00", 7) == 0) && Block[7] == 0) // PAC disabled, can it *really* happen ?
1506         return 1;
1507     return 0;
1508 }
1509
1510 int IsBlock1PCF7931(uint8_t *Block) {
1511     // Assume RFU means 0 :)
1512     if(Block[10] == 0 && Block[11] == 0 && Block[12] == 0 && Block[13] == 0)
1513         if((Block[14] & 0x7f) <= 9 && Block[15] <= 9)
1514             return 1;
1515
1516     return 0;
1517 }
1518
1519 #define ALLOC 16
1520
1521 void ReadPCF7931() {
1522     uint8_t Blocks[8][17];
1523     uint8_t tmpBlocks[4][16];
1524     int i, j, ind, ind2, n;
1525     int num_blocks = 0;
1526     int max_blocks = 8;
1527     int ident = 0;
1528     int error = 0;
1529     int tries = 0;
1530
1531     memset(Blocks, 0, 8*17*sizeof(uint8_t));
1532
1533     do {
1534         memset(tmpBlocks, 0, 4*16*sizeof(uint8_t));
1535         n = DemodPCF7931((uint8_t**)tmpBlocks);
1536         if(!n)
1537             error++;
1538         if(error==10 && num_blocks == 0) {
1539             Dbprintf("Error, no tag or bad tag");
1540             return;
1541         }
1542         else if (tries==20 || error==10) {
1543             Dbprintf("Error reading the tag");
1544             Dbprintf("Here is the partial content");
1545             goto end;
1546         }
1547
1548         for(i=0; i<n; i++)
1549             Dbprintf("(dbg) %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x",
1550                      tmpBlocks[i][0], tmpBlocks[i][1], tmpBlocks[i][2], tmpBlocks[i][3], tmpBlocks[i][4], tmpBlocks[i][5], tmpBlocks[i][6], tmpBlocks[i][7],
1551                     tmpBlocks[i][8], tmpBlocks[i][9], tmpBlocks[i][10], tmpBlocks[i][11], tmpBlocks[i][12], tmpBlocks[i][13], tmpBlocks[i][14], tmpBlocks[i][15]);
1552         if(!ident) {
1553             for(i=0; i<n; i++) {
1554                 if(IsBlock0PCF7931(tmpBlocks[i])) {
1555                     // Found block 0 ?
1556                     if(i < n-1 && IsBlock1PCF7931(tmpBlocks[i+1])) {
1557                         // Found block 1!
1558                         // \o/
1559                         ident = 1;
1560                         memcpy(Blocks[0], tmpBlocks[i], 16);
1561                         Blocks[0][ALLOC] = 1;
1562                         memcpy(Blocks[1], tmpBlocks[i+1], 16);
1563                         Blocks[1][ALLOC] = 1;
1564                         max_blocks = max((Blocks[1][14] & 0x7f), Blocks[1][15]) + 1;
1565                         // Debug print
1566                         Dbprintf("(dbg) Max blocks: %d", max_blocks);
1567                         num_blocks = 2;
1568                         // Handle following blocks
1569                         for(j=i+2, ind2=2; j!=i; j++, ind2++, num_blocks++) {
1570                             if(j==n) j=0;
1571                             if(j==i) break;
1572                             memcpy(Blocks[ind2], tmpBlocks[j], 16);
1573                             Blocks[ind2][ALLOC] = 1;
1574                         }
1575                         break;
1576                     }
1577                 }
1578             }
1579         }
1580         else {
1581             for(i=0; i<n; i++) { // Look for identical block in known blocks
1582                 if(memcmp(tmpBlocks[i], "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", 16)) { // Block is not full of 00
1583                     for(j=0; j<max_blocks; j++) {
1584                         if(Blocks[j][ALLOC] == 1 && !memcmp(tmpBlocks[i], Blocks[j], 16)) {
1585                             // Found an identical block
1586                             for(ind=i-1,ind2=j-1; ind >= 0; ind--,ind2--) {
1587                                 if(ind2 < 0)
1588                                     ind2 = max_blocks;
1589                                 if(!Blocks[ind2][ALLOC]) { // Block ind2 not already found
1590                                     // Dbprintf("Tmp %d -> Block %d", ind, ind2);
1591                                     memcpy(Blocks[ind2], tmpBlocks[ind], 16);
1592                                     Blocks[ind2][ALLOC] = 1;
1593                                     num_blocks++;
1594                                     if(num_blocks == max_blocks) goto end;
1595                                 }
1596                             }
1597                             for(ind=i+1,ind2=j+1; ind < n; ind++,ind2++) {
1598                                 if(ind2 > max_blocks)
1599                                     ind2 = 0;
1600                                 if(!Blocks[ind2][ALLOC]) { // Block ind2 not already found
1601                                     // Dbprintf("Tmp %d -> Block %d", ind, ind2);
1602                                     memcpy(Blocks[ind2], tmpBlocks[ind], 16);
1603                                     Blocks[ind2][ALLOC] = 1;
1604                                     num_blocks++;
1605                                     if(num_blocks == max_blocks) goto end;
1606                                 }
1607                             }
1608                         }
1609                     }
1610                 }
1611             }
1612         }
1613         tries++;
1614         if (BUTTON_PRESS()) return;
1615     } while (num_blocks != max_blocks);
1616 end:
1617     Dbprintf("-----------------------------------------");
1618     Dbprintf("Memory content:");
1619     Dbprintf("-----------------------------------------");
1620     for(i=0; i<max_blocks; i++) {
1621         if(Blocks[i][ALLOC]==1)
1622             Dbprintf("%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x",
1623                      Blocks[i][0], Blocks[i][1], Blocks[i][2], Blocks[i][3], Blocks[i][4], Blocks[i][5], Blocks[i][6], Blocks[i][7],
1624                     Blocks[i][8], Blocks[i][9], Blocks[i][10], Blocks[i][11], Blocks[i][12], Blocks[i][13], Blocks[i][14], Blocks[i][15]);
1625         else
1626             Dbprintf("<missing block %d>", i);
1627     }
1628     Dbprintf("-----------------------------------------");
1629
1630     return ;
1631 }
1632
1633
1634 //-----------------------------------
1635 // EM4469 / EM4305 routines
1636 //-----------------------------------
1637 #define FWD_CMD_LOGIN 0xC //including the even parity, binary mirrored
1638 #define FWD_CMD_WRITE 0xA
1639 #define FWD_CMD_READ 0x9
1640 #define FWD_CMD_DISABLE 0x5
1641
1642
1643 uint8_t forwardLink_data[64]; //array of forwarded bits
1644 uint8_t * forward_ptr; //ptr for forward message preparation
1645 uint8_t fwd_bit_sz; //forwardlink bit counter
1646 uint8_t * fwd_write_ptr; //forwardlink bit pointer
1647
1648 //====================================================================
1649 // prepares command bits
1650 // see EM4469 spec
1651 //====================================================================
1652 //--------------------------------------------------------------------
1653 uint8_t Prepare_Cmd( uint8_t cmd ) {
1654     //--------------------------------------------------------------------
1655
1656     *forward_ptr++ = 0; //start bit
1657     *forward_ptr++ = 0; //second pause for 4050 code
1658
1659     *forward_ptr++ = cmd;
1660     cmd >>= 1;
1661     *forward_ptr++ = cmd;
1662     cmd >>= 1;
1663     *forward_ptr++ = cmd;
1664     cmd >>= 1;
1665     *forward_ptr++ = cmd;
1666
1667     return 6; //return number of emited bits
1668 }
1669
1670 //====================================================================
1671 // prepares address bits
1672 // see EM4469 spec
1673 //====================================================================
1674
1675 //--------------------------------------------------------------------
1676 uint8_t Prepare_Addr( uint8_t addr ) {
1677     //--------------------------------------------------------------------
1678
1679     register uint8_t line_parity;
1680
1681     uint8_t i;
1682     line_parity = 0;
1683     for(i=0;i<6;i++) {
1684         *forward_ptr++ = addr;
1685         line_parity ^= addr;
1686         addr >>= 1;
1687     }
1688
1689     *forward_ptr++ = (line_parity & 1);
1690
1691     return 7; //return number of emited bits
1692 }
1693
1694 //====================================================================
1695 // prepares data bits intreleaved with parity bits
1696 // see EM4469 spec
1697 //====================================================================
1698
1699 //--------------------------------------------------------------------
1700 uint8_t Prepare_Data( uint16_t data_low, uint16_t data_hi) {
1701     //--------------------------------------------------------------------
1702
1703     register uint8_t line_parity;
1704     register uint8_t column_parity;
1705     register uint8_t i, j;
1706     register uint16_t data;
1707
1708     data = data_low;
1709     column_parity = 0;
1710
1711     for(i=0; i<4; i++) {
1712         line_parity = 0;
1713         for(j=0; j<8; j++) {
1714             line_parity ^= data;
1715             column_parity ^= (data & 1) << j;
1716             *forward_ptr++ = data;
1717             data >>= 1;
1718         }
1719         *forward_ptr++ = line_parity;
1720         if(i == 1)
1721             data = data_hi;
1722     }
1723
1724     for(j=0; j<8; j++) {
1725         *forward_ptr++ = column_parity;
1726         column_parity >>= 1;
1727     }
1728     *forward_ptr = 0;
1729
1730     return 45; //return number of emited bits
1731 }
1732
1733 //====================================================================
1734 // Forward Link send function
1735 // Requires: forwarLink_data filled with valid bits (1 bit per byte)
1736 // fwd_bit_count set with number of bits to be sent
1737 //====================================================================
1738 void SendForward(uint8_t fwd_bit_count) {
1739
1740     fwd_write_ptr = forwardLink_data;
1741     fwd_bit_sz = fwd_bit_count;
1742
1743     LED_D_ON();
1744
1745     //Field on
1746     FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
1747     FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
1748     FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
1749
1750     // Give it a bit of time for the resonant antenna to settle.
1751     // And for the tag to fully power up
1752     SpinDelay(150);
1753
1754     // force 1st mod pulse (start gap must be longer for 4305)
1755     fwd_bit_sz--; //prepare next bit modulation
1756     fwd_write_ptr++;
1757     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1758     SpinDelayUs(55*8); //55 cycles off (8us each)for 4305
1759     FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
1760     FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);//field on
1761     SpinDelayUs(16*8); //16 cycles on (8us each)
1762
1763     // now start writting
1764     while(fwd_bit_sz-- > 0) { //prepare next bit modulation
1765         if(((*fwd_write_ptr++) & 1) == 1)
1766             SpinDelayUs(32*8); //32 cycles at 125Khz (8us each)
1767         else {
1768             //These timings work for 4469/4269/4305 (with the 55*8 above)
1769             FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1770             SpinDelayUs(23*8); //16-4 cycles off (8us each)
1771             FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
1772             FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);//field on
1773             SpinDelayUs(9*8); //16 cycles on (8us each)
1774         }
1775     }
1776 }
1777
1778 void EM4xLogin(uint32_t Password) {
1779
1780     uint8_t fwd_bit_count;
1781
1782     forward_ptr = forwardLink_data;
1783     fwd_bit_count = Prepare_Cmd( FWD_CMD_LOGIN );
1784     fwd_bit_count += Prepare_Data( Password&0xFFFF, Password>>16 );
1785
1786     SendForward(fwd_bit_count);
1787
1788     //Wait for command to complete
1789     SpinDelay(20);
1790
1791 }
1792
1793 void EM4xReadWord(uint8_t Address, uint32_t Pwd, uint8_t PwdMode) {
1794
1795         uint8_t *dest =  BigBuf_get_addr();
1796         uint16_t bufferlength = BigBuf_max_traceLen();
1797         uint32_t i = 0;
1798
1799         // Clear destination buffer before sending the command  0x80 = average.
1800         memset(dest, 0x80, bufferlength);
1801
1802     uint8_t fwd_bit_count;
1803
1804     //If password mode do login
1805     if (PwdMode == 1) EM4xLogin(Pwd);
1806
1807     forward_ptr = forwardLink_data;
1808     fwd_bit_count = Prepare_Cmd( FWD_CMD_READ );
1809     fwd_bit_count += Prepare_Addr( Address );
1810
1811     // Connect the A/D to the peak-detected low-frequency path.
1812     SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
1813     // Now set up the SSC to get the ADC samples that are now streaming at us.
1814     FpgaSetupSsc();
1815
1816     SendForward(fwd_bit_count);
1817
1818     // Now do the acquisition
1819     i = 0;
1820     for(;;) {
1821         if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_TXRDY) {
1822             AT91C_BASE_SSC->SSC_THR = 0x43;
1823         }
1824         if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) {
1825             dest[i] = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
1826                         ++i;
1827                         if (i >= bufferlength) break;
1828                 }
1829         }
1830
1831         cmd_send(CMD_ACK,0,0,0,0,0);
1832     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1833     LED_D_OFF();
1834 }
1835
1836 void EM4xWriteWord(uint32_t Data, uint8_t Address, uint32_t Pwd, uint8_t PwdMode) {
1837
1838     uint8_t fwd_bit_count;
1839
1840     //If password mode do login
1841     if (PwdMode == 1) EM4xLogin(Pwd);
1842
1843     forward_ptr = forwardLink_data;
1844     fwd_bit_count = Prepare_Cmd( FWD_CMD_WRITE );
1845     fwd_bit_count += Prepare_Addr( Address );
1846     fwd_bit_count += Prepare_Data( Data&0xFFFF, Data>>16 );
1847
1848     SendForward(fwd_bit_count);
1849
1850     //Wait for write to complete
1851     SpinDelay(20);
1852     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1853     LED_D_OFF();
1854 }