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