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