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