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

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