git.zerfleddert.de Git - proxmark3-svn/blob - armsrc/lfops.c

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