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