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