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 by marshmellow (MM)
 442 void SimulateTagLowFrequencyMM(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){
 467             if (tab[i]!=tab[i-1]){
 468                 // transition
 469                 if (ledcontrol)
 470                     LED_D_ON();
 471
 472                 // modulate coil
 473                 if(tab[i])
 474                     OPEN_COIL();
 475                 else
 476                     SHORT_COIL();
 477
 478                 if (ledcontrol)
 479                     LED_D_OFF();
 480
 481             } else { //no transition
 482                 //NOTE: it appears the COIL transition messes with the detection of the carrier, so if a transition happened
 483                 //  skip test for readers Carrier = LOW, otherwise we get a bit behind
 484
 485                 //wait until reader carrier is LOW
 486                 while(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK) {
 487                     WDT_HIT();
 488                 }
 489             }
 490         } else {
 491             // transition
 492             if (ledcontrol)
 493                 LED_D_ON();
 494
 495             // modulate coil
 496             if(tab[i])
 497                 OPEN_COIL();
 498             else
 499                 SHORT_COIL();
 500
 501             if (ledcontrol)
 502                 LED_D_OFF();
 503         }
 504         WDT_HIT();
 505
 506
 507         i++;
 508         if(i == period) {
 509             // end of data stream, gap then repeat
 510             i = 0;
 511             if (gap) {
 512                 SHORT_COIL();
 513                 SpinDelayUs(gap);
 514             }
 515         }
 516     }
 517     DbpString("Stopped");
 518     return;
 519 }
 520
 521 #define DEBUG_FRAME_CONTENTS 1
 522 void SimulateTagLowFrequencyBidir(int divisor, int t0)
 523 {
 524 }
 525
 526 // compose fc/8 fc/10 waveform (FSK2)
 527 static void fc(int c, int *n)
 528 {
 529     uint8_t *dest = BigBuf_get_addr();
 530     int idx;
 531
 532     // for when we want an fc8 pattern every 4 logical bits
 533     if(c==0) {
 534         dest[((*n)++)]=1;
 535         dest[((*n)++)]=1;
 536         dest[((*n)++)]=1;
 537         dest[((*n)++)]=1;
 538         dest[((*n)++)]=0;
 539         dest[((*n)++)]=0;
 540         dest[((*n)++)]=0;
 541         dest[((*n)++)]=0;
 542     }
 543
 544     //  an fc/8  encoded bit is a bit pattern of  11000000  x6 = 48 samples
 545     if(c==8) {
 546         for (idx=0; idx<6; idx++) {
 547             dest[((*n)++)]=1;
 548             dest[((*n)++)]=1;
 549             dest[((*n)++)]=1;
 550             dest[((*n)++)]=1;
 551             dest[((*n)++)]=0;
 552             dest[((*n)++)]=0;
 553             dest[((*n)++)]=0;
 554             dest[((*n)++)]=0;
 555         }
 556     }
 557
 558     //  an fc/10 encoded bit is a bit pattern of 1110000000 x5 = 50 samples
 559     if(c==10) {
 560         for (idx=0; idx<5; idx++) {
 561             dest[((*n)++)]=1;
 562             dest[((*n)++)]=1;
 563             dest[((*n)++)]=1;
 564             dest[((*n)++)]=1;
 565             dest[((*n)++)]=1;
 566             dest[((*n)++)]=0;
 567             dest[((*n)++)]=0;
 568             dest[((*n)++)]=0;
 569             dest[((*n)++)]=0;
 570             dest[((*n)++)]=0;
 571         }
 572     }
 573 }
 574 // compose fc/X fc/Y waveform (FSKx)
 575 static void fcAll(uint8_t c, int *n, uint8_t clock, uint16_t *modCnt)
 576 {
 577     uint8_t *dest = BigBuf_get_addr();
 578     uint8_t idx;
 579     uint8_t fcCnt;
 580     // c = count of field clock for this bit
 581     uint8_t mod = clock % c;
 582     uint8_t modAdj = c/mod;
 583     bool modAdjOk=FALSE;
 584     if (c % mod==0) modAdjOk=TRUE;
 585     // loop through clock - step field clock
 586     for (idx=0; idx < (uint8_t) clock/c; idx++){
 587         // 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)
 588         for (fcCnt=0; fcCnt < c; fcCnt++){  //fudge slow transition from low to high - shorten wave by 1
 589             if (fcCnt < c/2+1){
 590                 dest[((*n)++)]=0;
 591             } else {
 592                 //fudge low to high transition
 593                 //if (idx==clock/c && dest[*n-1]==1 && mod>0) dest[((*n++))]=0;
 594                 //if (c==8 && fcCnt==5) continue;
 595                 dest[((*n)++)]=1;
 596             }
 597         }
 598     }
 599     if (mod>0) (*modCnt)++;
 600     if ((mod>0) && modAdjOk){  //fsk2
 601         if ((*modCnt % modAdj) == 0){ //if 4th 8 length wave in a rf/50 add extra 8 length wave
 602             for (fcCnt=0; fcCnt < c; fcCnt++){  //fudge slow transition from low to high - shorten wave by 1
 603                 if (fcCnt < c/2+1){
 604                     dest[((*n)++)]=0;
 605                 } else {
 606                     //if (c==8 && fcCnt==5) continue;
 607                     dest[((*n)++)]=1;
 608                 }
 609             }
 610         }
 611     }
 612     //Dbprintf("mod: %d, modAdj %d, modc %d",mod, modAdj, c % mod);
 613     if (mod>0 && !modAdjOk){  //fsk1
 614         for (idx=0; idx < mod; idx++){
 615             if (idx < mod/2) {
 616                 dest[((*n)++)]=0;
 617             } else {
 618                 dest[((*n)++)]=1;
 619             }
 620         }
 621     }
 622 }
 623
 624 // prepare a waveform pattern in the buffer based on the ID given then
 625 // simulate a HID tag until the button is pressed
 626 void CmdHIDsimTAG(int hi, int lo, int ledcontrol)
 627 {
 628     int n=0, i=0;
 629     /*
 630      HID tag bitstream format
 631      The tag contains a 44bit unique code. This is sent out MSB first in sets of 4 bits
 632      A 1 bit is represented as 6 fc8 and 5 fc10 patterns
 633      A 0 bit is represented as 5 fc10 and 6 fc8 patterns
 634      A fc8 is inserted before every 4 bits
 635      A special start of frame pattern is used consisting a0b0 where a and b are neither 0
 636      nor 1 bits, they are special patterns (a = set of 12 fc8 and b = set of 10 fc10)
 637     */
 638
 639     if (hi>0xFFF) {
 640         DbpString("Tags can only have 44 bits.");
 641         return;
 642     }
 643     fc(0,&n);
 644     // special start of frame marker containing invalid bit sequences
 645     fc(8,  &n); fc(8,  &n);     // invalid
 646     fc(8,  &n); fc(10, &n); // logical 0
 647     fc(10, &n); fc(10, &n); // invalid
 648     fc(8,  &n); fc(10, &n); // logical 0
 649
 650     WDT_HIT();
 651     // manchester encode bits 43 to 32
 652     for (i=11; i>=0; i--) {
 653         if ((i%4)==3) fc(0,&n);
 654         if ((hi>>i)&1) {
 655             fc(10, &n); fc(8,  &n);             // low-high transition
 656         } else {
 657             fc(8,  &n); fc(10, &n);             // high-low transition
 658         }
 659     }
 660
 661     WDT_HIT();
 662     // manchester encode bits 31 to 0
 663     for (i=31; i>=0; i--) {
 664         if ((i%4)==3) fc(0,&n);
 665         if ((lo>>i)&1) {
 666             fc(10, &n); fc(8,  &n);             // low-high transition
 667         } else {
 668             fc(8,  &n); fc(10, &n);             // high-low transition
 669         }
 670     }
 671
 672     if (ledcontrol)
 673         LED_A_ON();
 674     SimulateTagLowFrequency(n, 0, ledcontrol);
 675
 676     if (ledcontrol)
 677         LED_A_OFF();
 678 }
 679
 680 // prepare a waveform pattern in the buffer based on the ID given then
 681 // simulate a FSK tag until the button is pressed
 682 // arg1 contains fcHigh and fcLow, arg2 contains invert and clock
 683 void CmdFSKsimTAG(uint16_t arg1, uint16_t arg2, size_t size, uint8_t *BitStream)
 684 {
 685     int ledcontrol=1;
 686     int n=0, i=0;
 687     uint8_t fcHigh = arg1 >> 8;
 688     uint8_t fcLow = arg1 & 0xFF;
 689     uint16_t modCnt = 0;
 690     //spacer bit
 691     uint8_t clk = arg2 & 0xFF;
 692     uint8_t invert = (arg2 >> 8) & 1;
 693     //fcAll(0, &n, clk);
 694
 695     WDT_HIT();
 696     for (i=0; i<size; i++){
 697         if (BitStream[i] == invert){
 698             fcAll(fcLow, &n, clk, &modCnt);
 699         } else {
 700             fcAll(fcHigh, &n, clk, &modCnt);
 701         }
 702     }
 703     Dbprintf("Simulating with fcHigh: %d, fcLow: %d, clk: %d, invert: %d, n: %d",fcHigh, fcLow, clk, invert, n);
 704     Dbprintf("First 64:");
 705     uint8_t *dest = BigBuf_get_addr();
 706     i=0;
 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     i+=16;
 717     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]);
 718     i+=16;
 719     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]);
 720     i+=16;
 721     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]);
 722     i+=16;
 723     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]);
 724     i+=16;
 725     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]);
 726     i+=16;
 727     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]);
 728     i+=16;
 729     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]);
 730     i+=16;
 731     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]);
 732     i+=16;
 733     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]);
 734     i+=16;
 735     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]);
 736
 737     if (ledcontrol)
 738         LED_A_ON();
 739     SimulateTagLowFrequencyMM(n, 0, ledcontrol);
 740
 741     if (ledcontrol)
 742         LED_A_OFF();
 743 }
 744
 745 // compose ask waveform for one bit(ASK)
 746 static void askSimBit(uint8_t c, int *n, uint8_t clock, uint8_t manchester)
 747 {
 748     uint8_t *dest = BigBuf_get_addr();
 749     uint8_t idx;
 750     // c = current bit 1 or 0
 751     int i = 0;
 752     // for when we want a separator
 753     if (c==2) { //separator
 754         for (i=0; i<clock/2; i++){
 755             dest[((*n)++)]=0;
 756         }
 757     } else {
 758         if (manchester){
 759             for (idx=0; idx < (uint8_t) clock/2; idx++){
 760                 dest[((*n)++)]=c;
 761             }
 762             for (idx=0; idx < (uint8_t) clock/2; idx++){
 763                 dest[((*n)++)]=c^1;
 764             }
 765         } else {
 766             for (idx=0; idx < (uint8_t) clock; idx++){
 767                 dest[((*n)++)]=c;
 768             }
 769         }
 770     }
 771 }
 772
 773 // args clock, ask/man or askraw, invert, transmission separator
 774 void CmdASKsimTag(uint16_t arg1, uint16_t arg2, size_t size, uint8_t *BitStream)
 775 {
 776     int ledcontrol = 1;
 777     int n=0, i=0;
 778     uint8_t clk = (arg1 >> 8) & 0xFF;
 779     uint8_t manchester = arg1 & 1;
 780     uint8_t separator = arg2 & 1;
 781     uint8_t invert = (arg2 >> 8) & 1;
 782     WDT_HIT();
 783     for (i=0; i<size; i++){
 784         askSimBit(BitStream[i]^invert, &n, clk, manchester);
 785     }
 786     if (separator==1) Dbprintf("sorry but separator option not yet available"); //askSimBit(2, &n, clk, manchester);
 787
 788     Dbprintf("Simulating with clk: %d, invert: %d, manchester: %d, separator: %d, n: %d",clk, invert, manchester, separator, n);
 789     //DEBUG
 790     //Dbprintf("First 64:");
 791     //uint8_t *dest = BigBuf_get_addr();
 792     //i=0;
 793     //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]);
 794     //i+=16;
 795     //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]);
 796     //i+=16;
 797     //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]);
 798     //i+=16;
 799     //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]);
 800
 801
 802     if (ledcontrol)
 803         LED_A_ON();
 804     SimulateTagLowFrequencyMM(n, 0, ledcontrol);
 805
 806     if (ledcontrol)
 807         LED_A_OFF();
 808 }
 809
 810 //carrier can be 2,4 or 8
 811 static void pskSimBit(uint8_t waveLen, int *n, uint8_t clk, uint8_t *curPhase, bool phaseChg)
 812 {
 813     uint8_t *dest = BigBuf_get_addr();
 814     uint8_t idx;
 815     int i = 0;
 816     if (phaseChg){
 817         // write phase change
 818         for (i=0; i < waveLen/2; i++){
 819             dest[((*n)++)] = *curPhase^1;
 820         }
 821         for (i=0; i < waveLen/2; i++){
 822             dest[((*n)++)] = *curPhase;
 823         }
 824         *curPhase ^= 1;
 825     }
 826     //write each normal clock wave for the clock duration
 827     for (; i < clk; i+=waveLen){
 828         for (idx=0; idx<waveLen/2; idx++){
 829             dest[((*n)++)] = *curPhase;
 830         }
 831         for (idx=0; idx<waveLen/2; idx++){
 832             dest[((*n)++)] = *curPhase^1;
 833         }
 834     }
 835 }
 836
 837 // args clock, carrier, invert,
 838 void CmdPSKsimTag(uint16_t arg1, uint16_t arg2, size_t size, uint8_t *BitStream)
 839 {
 840     int ledcontrol=1;
 841     int n=0, i=0;
 842     uint8_t clk = arg1 >> 8;
 843     uint8_t carrier = arg1 & 0xFF;
 844     uint8_t invert = arg2 & 0xFF;
 845     //uint8_t phase = carrier/2; //extra phase changing bits = 1/2 a carrier wave to change the phase
 846     //uint8_t invert = (arg2 >> 8) & 1;
 847     uint8_t curPhase = 0;
 848     WDT_HIT();
 849     for (i=0; i<size; i++){
 850         if (BitStream[i] == curPhase){
 851             pskSimBit(carrier, &n, clk, &curPhase, FALSE);
 852         } else {
 853             pskSimBit(carrier, &n, clk, &curPhase, TRUE);
 854         }
 855     }
 856     Dbprintf("Simulating with Carrier: %d, clk: %d, invert: %d, n: %d",carrier, clk, invert, n);
 857     Dbprintf("First 128:");
 858     uint8_t *dest = BigBuf_get_addr();
 859     i=0;
 860     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]);
 861     i+=16;
 862     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]);
 863     i+=16;
 864     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]);
 865     i+=16;
 866     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]);
 867     i+=16;
 868     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]);
 869     i+=16;
 870     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]);
 871     i+=16;
 872     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]);
 873     i+=16;
 874     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]);
 875
 876     if (ledcontrol)
 877         LED_A_ON();
 878     SimulateTagLowFrequencyMM(n, 0, ledcontrol);
 879
 880     if (ledcontrol)
 881         LED_A_OFF();
 882 }
 883
 884 // loop to get raw HID waveform then FSK demodulate the TAG ID from it
 885 void CmdHIDdemodFSK(int findone, int *high, int *low, int ledcontrol)
 886 {
 887     uint8_t *dest = BigBuf_get_addr();
 888     const size_t sizeOfBigBuff = BigBuf_max_traceLen();
 889     size_t size = 0;
 890     uint32_t hi2=0, hi=0, lo=0;
 891     int idx=0;
 892     // Configure to go in 125Khz listen mode
 893     LFSetupFPGAForADC(95, true);
 894
 895     while(!BUTTON_PRESS()) {
 896
 897         WDT_HIT();
 898         if (ledcontrol) LED_A_ON();
 899
 900                 DoAcquisition_default(-1,true);
 901                 // FSK demodulator
 902         size = sizeOfBigBuff;  //variable size will change after demod so re initialize it before use
 903                 idx = HIDdemodFSK(dest, &size, &hi2, &hi, &lo);
 904
 905                 if (idx>0 && lo>0){
 906             // final loop, go over previously decoded manchester data and decode into usable tag ID
 907             // 111000 bit pattern represent start of frame, 01 pattern represents a 1 and 10 represents a 0
 908             if (hi2 != 0){ //extra large HID tags
 909                 Dbprintf("TAG ID: %x%08x%08x (%d)",
 910                          (unsigned int) hi2, (unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF);
 911             }else {  //standard HID tags <38 bits
 912                 //Dbprintf("TAG ID: %x%08x (%d)",(unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF); //old print cmd
 913                 uint8_t bitlen = 0;
 914                 uint32_t fc = 0;
 915                 uint32_t cardnum = 0;
 916                                 if (((hi>>5)&1) == 1){//if bit 38 is set then < 37 bit format is used
 917                     uint32_t lo2=0;
 918                     lo2=(((hi & 31) << 12) | (lo>>20)); //get bits 21-37 to check for format len bit
 919                     uint8_t idx3 = 1;
 920                                         while(lo2 > 1){ //find last bit set to 1 (format len bit)
 921                                                 lo2=lo2 >> 1;
 922                         idx3++;
 923                     }
 924                                         bitlen = idx3+19;
 925                     fc =0;
 926                     cardnum=0;
 927                                         if(bitlen == 26){
 928                         cardnum = (lo>>1)&0xFFFF;
 929                         fc = (lo>>17)&0xFF;
 930                     }
 931                                         if(bitlen == 37){
 932                         cardnum = (lo>>1)&0x7FFFF;
 933                         fc = ((hi&0xF)<<12)|(lo>>20);
 934                     }
 935                                         if(bitlen == 34){
 936                         cardnum = (lo>>1)&0xFFFF;
 937                         fc= ((hi&1)<<15)|(lo>>17);
 938                     }
 939                                         if(bitlen == 35){
 940                         cardnum = (lo>>1)&0xFFFFF;
 941                         fc = ((hi&1)<<11)|(lo>>21);
 942                     }
 943                 }
 944                 else { //if bit 38 is not set then 37 bit format is used
 945                     bitlen= 37;
 946                     fc =0;
 947                     cardnum=0;
 948                     if(bitlen==37){
 949                         cardnum = (lo>>1)&0x7FFFF;
 950                         fc = ((hi&0xF)<<12)|(lo>>20);
 951                     }
 952                 }
 953                 //Dbprintf("TAG ID: %x%08x (%d)",
 954                 // (unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF);
 955                 Dbprintf("TAG ID: %x%08x (%d) - Format Len: %dbit - FC: %d - Card: %d",
 956                          (unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF,
 957                          (unsigned int) bitlen, (unsigned int) fc, (unsigned int) cardnum);
 958             }
 959             if (findone){
 960                 if (ledcontrol) LED_A_OFF();
 961                 *high = hi;
 962                 *low = lo;
 963                 return;
 964             }
 965             // reset
 966             hi2 = hi = lo = 0;
 967         }
 968         WDT_HIT();
 969     }
 970     DbpString("Stopped");
 971     if (ledcontrol) LED_A_OFF();
 972 }
 973
 974 void CmdEM410xdemod(int findone, int *high, int *low, int ledcontrol)
 975 {
 976     uint8_t *dest = BigBuf_get_addr();
 977
 978         size_t size=0, idx=0;
 979     int clk=0, invert=0, errCnt=0, maxErr=20;
 980     uint64_t lo=0;
 981     // Configure to go in 125Khz listen mode
 982     LFSetupFPGAForADC(95, true);
 983
 984     while(!BUTTON_PRESS()) {
 985
 986         WDT_HIT();
 987         if (ledcontrol) LED_A_ON();
 988
 989                 DoAcquisition_default(-1,true);
 990                 size  = BigBuf_max_traceLen();
 991         //Dbprintf("DEBUG: Buffer got");
 992                 //askdemod and manchester decode
 993                 errCnt = askmandemod(dest, &size, &clk, &invert, maxErr);
 994         //Dbprintf("DEBUG: ASK Got");
 995         WDT_HIT();
 996
 997         if (errCnt>=0){
 998                         lo = Em410xDecode(dest, &size, &idx);
 999             //Dbprintf("DEBUG: EM GOT");
1000             if (lo>0){
1001                                 Dbprintf("EM TAG ID: %02x%08x - (%05d_%03d_%08d)",
1002                                     (uint32_t)(lo>>32),
1003                                     (uint32_t)lo,
1004                                     (uint32_t)(lo&0xFFFF),
1005                                     (uint32_t)((lo>>16LL) & 0xFF),
1006                                     (uint32_t)(lo & 0xFFFFFF));
1007             }
1008             if (findone){
1009                 if (ledcontrol) LED_A_OFF();
1010                 *high=lo>>32;
1011                 *low=lo & 0xFFFFFFFF;
1012                 return;
1013             }
1014         } else{
1015             //Dbprintf("DEBUG: No Tag");
1016         }
1017         WDT_HIT();
1018         lo = 0;
1019         clk=0;
1020         invert=0;
1021         errCnt=0;
1022         size=0;
1023     }
1024     DbpString("Stopped");
1025     if (ledcontrol) LED_A_OFF();
1026 }
1027
1028 void CmdIOdemodFSK(int findone, int *high, int *low, int ledcontrol)
1029 {
1030     uint8_t *dest = BigBuf_get_addr();
1031     int idx=0;
1032     uint32_t code=0, code2=0;
1033     uint8_t version=0;
1034     uint8_t facilitycode=0;
1035     uint16_t number=0;
1036     // Configure to go in 125Khz listen mode
1037     LFSetupFPGAForADC(95, true);
1038
1039     while(!BUTTON_PRESS()) {
1040         WDT_HIT();
1041         if (ledcontrol) LED_A_ON();
1042                 DoAcquisition_default(-1,true);
1043                 //fskdemod and get start index
1044         WDT_HIT();
1045         idx = IOdemodFSK(dest, BigBuf_max_traceLen());
1046         if (idx>0){
1047             //valid tag found
1048
1049             //Index map
1050             //0           10          20          30          40          50          60
1051             //|           |           |           |           |           |           |
1052             //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23
1053             //-----------------------------------------------------------------------------
1054             //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11
1055             //
1056             //XSF(version)facility:codeone+codetwo
1057             //Handle the data
1058             if(findone){ //only print binary if we are doing one
1059                 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]);
1060                 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]);
1061                 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]);
1062                 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]);
1063                 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]);
1064                 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]);
1065                 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]);
1066             }
1067             code = bytebits_to_byte(dest+idx,32);
1068             code2 = bytebits_to_byte(dest+idx+32,32);
1069             version = bytebits_to_byte(dest+idx+27,8); //14,4
1070             facilitycode = bytebits_to_byte(dest+idx+18,8) ;
1071             number = (bytebits_to_byte(dest+idx+36,8)<<8)|(bytebits_to_byte(dest+idx+45,8)); //36,9
1072
1073             Dbprintf("XSF(%02d)%02x:%05d (%08x%08x)",version,facilitycode,number,code,code2);
1074             // if we're only looking for one tag
1075             if (findone){
1076                 if (ledcontrol) LED_A_OFF();
1077                 //LED_A_OFF();
1078                 *high=code;
1079                 *low=code2;
1080                 return;
1081             }
1082             code=code2=0;
1083             version=facilitycode=0;
1084             number=0;
1085             idx=0;
1086         }
1087         WDT_HIT();
1088     }
1089     DbpString("Stopped");
1090     if (ledcontrol) LED_A_OFF();
1091 }
1092
1093 /*------------------------------
1094  * T5555/T5557/T5567 routines
1095  *------------------------------
1096  */
1097
1098 /* T55x7 configuration register definitions */
1099 #define T55x7_POR_DELAY                 0x00000001
1100 #define T55x7_ST_TERMINATOR             0x00000008
1101 #define T55x7_PWD                       0x00000010
1102 #define T55x7_MAXBLOCK_SHIFT            5
1103 #define T55x7_AOR                       0x00000200
1104 #define T55x7_PSKCF_RF_2                0
1105 #define T55x7_PSKCF_RF_4                0x00000400
1106 #define T55x7_PSKCF_RF_8                0x00000800
1107 #define T55x7_MODULATION_DIRECT         0
1108 #define T55x7_MODULATION_PSK1           0x00001000
1109 #define T55x7_MODULATION_PSK2           0x00002000
1110 #define T55x7_MODULATION_PSK3           0x00003000
1111 #define T55x7_MODULATION_FSK1           0x00004000
1112 #define T55x7_MODULATION_FSK2           0x00005000
1113 #define T55x7_MODULATION_FSK1a          0x00006000
1114 #define T55x7_MODULATION_FSK2a          0x00007000
1115 #define T55x7_MODULATION_MANCHESTER     0x00008000
1116 #define T55x7_MODULATION_BIPHASE        0x00010000
1117 #define T55x7_BITRATE_RF_8              0
1118 #define T55x7_BITRATE_RF_16             0x00040000
1119 #define T55x7_BITRATE_RF_32             0x00080000
1120 #define T55x7_BITRATE_RF_40             0x000C0000
1121 #define T55x7_BITRATE_RF_50             0x00100000
1122 #define T55x7_BITRATE_RF_64             0x00140000
1123 #define T55x7_BITRATE_RF_100            0x00180000
1124 #define T55x7_BITRATE_RF_128            0x001C0000
1125
1126 /* T5555 (Q5) configuration register definitions */
1127 #define T5555_ST_TERMINATOR             0x00000001
1128 #define T5555_MAXBLOCK_SHIFT            0x00000001
1129 #define T5555_MODULATION_MANCHESTER     0
1130 #define T5555_MODULATION_PSK1           0x00000010
1131 #define T5555_MODULATION_PSK2           0x00000020
1132 #define T5555_MODULATION_PSK3           0x00000030
1133 #define T5555_MODULATION_FSK1           0x00000040
1134 #define T5555_MODULATION_FSK2           0x00000050
1135 #define T5555_MODULATION_BIPHASE        0x00000060
1136 #define T5555_MODULATION_DIRECT         0x00000070
1137 #define T5555_INVERT_OUTPUT             0x00000080
1138 #define T5555_PSK_RF_2                  0
1139 #define T5555_PSK_RF_4                  0x00000100
1140 #define T5555_PSK_RF_8                  0x00000200
1141 #define T5555_USE_PWD                   0x00000400
1142 #define T5555_USE_AOR                   0x00000800
1143 #define T5555_BITRATE_SHIFT             12
1144 #define T5555_FAST_WRITE                0x00004000
1145 #define T5555_PAGE_SELECT               0x00008000
1146
1147 /*
1148  * Relevant times in microsecond
1149  * To compensate antenna falling times shorten the write times
1150  * and enlarge the gap ones.
1151  */
1152 #define START_GAP 250
1153 #define WRITE_GAP 160
1154 #define WRITE_0   144 // 192
1155 #define WRITE_1   400 // 432 for T55x7; 448 for E5550
1156
1157 // Write one bit to card
1158 void T55xxWriteBit(int bit)
1159 {
1160     FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
1161     FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
1162     FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
1163     if (bit == 0)
1164         SpinDelayUs(WRITE_0);
1165     else
1166         SpinDelayUs(WRITE_1);
1167     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1168     SpinDelayUs(WRITE_GAP);
1169 }
1170
1171 // Write one card block in page 0, no lock
1172 void T55xxWriteBlock(uint32_t Data, uint32_t Block, uint32_t Pwd, uint8_t PwdMode)
1173 {
1174     //unsigned int i;  //enio adjustment 12/10/14
1175     uint32_t i;
1176
1177     FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
1178     FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
1179     FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
1180
1181     // Give it a bit of time for the resonant antenna to settle.
1182     // And for the tag to fully power up
1183     SpinDelay(150);
1184
1185     // Now start writting
1186     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1187     SpinDelayUs(START_GAP);
1188
1189     // Opcode
1190     T55xxWriteBit(1);
1191     T55xxWriteBit(0); //Page 0
1192     if (PwdMode == 1){
1193         // Pwd
1194         for (i = 0x80000000; i != 0; i >>= 1)
1195             T55xxWriteBit(Pwd & i);
1196     }
1197     // Lock bit
1198     T55xxWriteBit(0);
1199
1200     // Data
1201     for (i = 0x80000000; i != 0; i >>= 1)
1202         T55xxWriteBit(Data & i);
1203
1204     // Block
1205     for (i = 0x04; i != 0; i >>= 1)
1206         T55xxWriteBit(Block & i);
1207
1208     // Now perform write (nominal is 5.6 ms for T55x7 and 18ms for E5550,
1209     // so wait a little more)
1210     FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
1211     FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
1212     SpinDelay(20);
1213     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1214 }
1215
1216 // Read one card block in page 0
1217 void T55xxReadBlock(uint32_t Block, uint32_t Pwd, uint8_t PwdMode)
1218 {
1219     uint8_t *dest = BigBuf_get_addr();
1220     //int m=0, i=0; //enio adjustment 12/10/14
1221     uint32_t m=0, i=0;
1222     FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
1223     m = BigBuf_max_traceLen();
1224     // Clear destination buffer before sending the command
1225     memset(dest, 128, m);
1226     // Connect the A/D to the peak-detected low-frequency path.
1227     SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
1228     // Now set up the SSC to get the ADC samples that are now streaming at us.
1229     FpgaSetupSsc();
1230
1231     LED_D_ON();
1232     FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
1233     FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
1234
1235     // Give it a bit of time for the resonant antenna to settle.
1236     // And for the tag to fully power up
1237     SpinDelay(150);
1238
1239     // Now start writting
1240     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1241     SpinDelayUs(START_GAP);
1242
1243     // Opcode
1244     T55xxWriteBit(1);
1245     T55xxWriteBit(0); //Page 0
1246     if (PwdMode == 1){
1247         // Pwd
1248         for (i = 0x80000000; i != 0; i >>= 1)
1249             T55xxWriteBit(Pwd & i);
1250     }
1251     // Lock bit
1252     T55xxWriteBit(0);
1253     // Block
1254     for (i = 0x04; i != 0; i >>= 1)
1255         T55xxWriteBit(Block & i);
1256
1257     // Turn field on to read the response
1258     FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
1259     FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
1260
1261     // Now do the acquisition
1262     i = 0;
1263     for(;;) {
1264         if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_TXRDY) {
1265             AT91C_BASE_SSC->SSC_THR = 0x43;
1266         }
1267         if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) {
1268             dest[i] = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
1269             // we don't care about actual value, only if it's more or less than a
1270             // threshold essentially we capture zero crossings for later analysis
1271             //                  if(dest[i] < 127) dest[i] = 0; else dest[i] = 1;
1272             i++;
1273             if (i >= m) break;
1274         }
1275     }
1276
1277     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1278     LED_D_OFF();
1279     DbpString("DONE!");
1280 }
1281
1282 // Read card traceability data (page 1)
1283 void T55xxReadTrace(void){
1284     uint8_t *dest = BigBuf_get_addr();
1285     int m=0, i=0;
1286
1287     FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
1288     m = BigBuf_max_traceLen();
1289     // Clear destination buffer before sending the command
1290     memset(dest, 128, m);
1291     // Connect the A/D to the peak-detected low-frequency path.
1292     SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
1293     // Now set up the SSC to get the ADC samples that are now streaming at us.
1294     FpgaSetupSsc();
1295
1296     LED_D_ON();
1297     FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
1298     FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
1299
1300     // Give it a bit of time for the resonant antenna to settle.
1301     // And for the tag to fully power up
1302     SpinDelay(150);
1303
1304     // Now start writting
1305     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1306     SpinDelayUs(START_GAP);
1307
1308     // Opcode
1309     T55xxWriteBit(1);
1310     T55xxWriteBit(1); //Page 1
1311
1312     // Turn field on to read the response
1313     FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
1314     FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
1315
1316     // Now do the acquisition
1317     i = 0;
1318     for(;;) {
1319         if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_TXRDY) {
1320             AT91C_BASE_SSC->SSC_THR = 0x43;
1321         }
1322         if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) {
1323             dest[i] = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
1324             i++;
1325             if (i >= m) break;
1326         }
1327     }
1328
1329     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1330     LED_D_OFF();
1331     DbpString("DONE!");
1332 }
1333
1334 /*-------------- Cloning routines -----------*/
1335 // Copy HID id to card and setup block 0 config
1336 void CopyHIDtoT55x7(uint32_t hi2, uint32_t hi, uint32_t lo, uint8_t longFMT)
1337 {
1338     int data1=0, data2=0, data3=0, data4=0, data5=0, data6=0; //up to six blocks for long format
1339     int last_block = 0;
1340
1341     if (longFMT){
1342         // Ensure no more than 84 bits supplied
1343         if (hi2>0xFFFFF) {
1344             DbpString("Tags can only have 84 bits.");
1345             return;
1346         }
1347         // Build the 6 data blocks for supplied 84bit ID
1348         last_block = 6;
1349         data1 = 0x1D96A900; // load preamble (1D) & long format identifier (9E manchester encoded)
1350         for (int i=0;i<4;i++) {
1351             if (hi2 & (1<<(19-i)))
1352                 data1 |= (1<<(((3-i)*2)+1)); // 1 -> 10
1353             else
1354                 data1 |= (1<<((3-i)*2)); // 0 -> 01
1355         }
1356
1357         data2 = 0;
1358         for (int i=0;i<16;i++) {
1359             if (hi2 & (1<<(15-i)))
1360                 data2 |= (1<<(((15-i)*2)+1)); // 1 -> 10
1361             else
1362                 data2 |= (1<<((15-i)*2)); // 0 -> 01
1363         }
1364
1365         data3 = 0;
1366         for (int i=0;i<16;i++) {
1367             if (hi & (1<<(31-i)))
1368                 data3 |= (1<<(((15-i)*2)+1)); // 1 -> 10
1369             else
1370                 data3 |= (1<<((15-i)*2)); // 0 -> 01
1371         }
1372
1373         data4 = 0;
1374         for (int i=0;i<16;i++) {
1375             if (hi & (1<<(15-i)))
1376                 data4 |= (1<<(((15-i)*2)+1)); // 1 -> 10
1377             else
1378                 data4 |= (1<<((15-i)*2)); // 0 -> 01
1379         }
1380
1381         data5 = 0;
1382         for (int i=0;i<16;i++) {
1383             if (lo & (1<<(31-i)))
1384                 data5 |= (1<<(((15-i)*2)+1)); // 1 -> 10
1385             else
1386                 data5 |= (1<<((15-i)*2)); // 0 -> 01
1387         }
1388
1389         data6 = 0;
1390         for (int i=0;i<16;i++) {
1391             if (lo & (1<<(15-i)))
1392                 data6 |= (1<<(((15-i)*2)+1)); // 1 -> 10
1393             else
1394                 data6 |= (1<<((15-i)*2)); // 0 -> 01
1395         }
1396     }
1397     else {
1398         // Ensure no more than 44 bits supplied
1399         if (hi>0xFFF) {
1400             DbpString("Tags can only have 44 bits.");
1401             return;
1402         }
1403
1404         // Build the 3 data blocks for supplied 44bit ID
1405         last_block = 3;
1406
1407         data1 = 0x1D000000; // load preamble
1408
1409         for (int i=0;i<12;i++) {
1410             if (hi & (1<<(11-i)))
1411                 data1 |= (1<<(((11-i)*2)+1)); // 1 -> 10
1412             else
1413                 data1 |= (1<<((11-i)*2)); // 0 -> 01
1414         }
1415
1416         data2 = 0;
1417         for (int i=0;i<16;i++) {
1418             if (lo & (1<<(31-i)))
1419                 data2 |= (1<<(((15-i)*2)+1)); // 1 -> 10
1420             else
1421                 data2 |= (1<<((15-i)*2)); // 0 -> 01
1422         }
1423
1424         data3 = 0;
1425         for (int i=0;i<16;i++) {
1426             if (lo & (1<<(15-i)))
1427                 data3 |= (1<<(((15-i)*2)+1)); // 1 -> 10
1428             else
1429                 data3 |= (1<<((15-i)*2)); // 0 -> 01
1430         }
1431     }
1432
1433     LED_D_ON();
1434     // Program the data blocks for supplied ID
1435     // and the block 0 for HID format
1436     T55xxWriteBlock(data1,1,0,0);
1437     T55xxWriteBlock(data2,2,0,0);
1438     T55xxWriteBlock(data3,3,0,0);
1439
1440     if (longFMT) { // if long format there are 6 blocks
1441         T55xxWriteBlock(data4,4,0,0);
1442         T55xxWriteBlock(data5,5,0,0);
1443         T55xxWriteBlock(data6,6,0,0);
1444     }
1445
1446     // Config for HID (RF/50, FSK2a, Maxblock=3 for short/6 for long)
1447     T55xxWriteBlock(T55x7_BITRATE_RF_50    |
1448                     T55x7_MODULATION_FSK2a |
1449                     last_block << T55x7_MAXBLOCK_SHIFT,
1450                     0,0,0);
1451
1452     LED_D_OFF();
1453
1454     DbpString("DONE!");
1455 }
1456
1457 void CopyIOtoT55x7(uint32_t hi, uint32_t lo, uint8_t longFMT)
1458 {
1459     int data1=0, data2=0; //up to six blocks for long format
1460
1461     data1 = hi;  // load preamble
1462     data2 = lo;
1463
1464     LED_D_ON();
1465     // Program the data blocks for supplied ID
1466     // and the block 0 for HID format
1467     T55xxWriteBlock(data1,1,0,0);
1468     T55xxWriteBlock(data2,2,0,0);
1469
1470     //Config Block
1471     T55xxWriteBlock(0x00147040,0,0,0);
1472     LED_D_OFF();
1473
1474     DbpString("DONE!");
1475 }
1476
1477 // Define 9bit header for EM410x tags
1478 #define EM410X_HEADER           0x1FF
1479 #define EM410X_ID_LENGTH        40
1480
1481 void WriteEM410x(uint32_t card, uint32_t id_hi, uint32_t id_lo)
1482 {
1483     int i, id_bit;
1484     uint64_t id = EM410X_HEADER;
1485     uint64_t rev_id = 0;        // reversed ID
1486     int c_parity[4];    // column parity
1487     int r_parity = 0;   // row parity
1488     uint32_t clock = 0;
1489
1490     // Reverse ID bits given as parameter (for simpler operations)
1491     for (i = 0; i < EM410X_ID_LENGTH; ++i) {
1492         if (i < 32) {
1493             rev_id = (rev_id << 1) | (id_lo & 1);
1494             id_lo >>= 1;
1495         } else {
1496             rev_id = (rev_id << 1) | (id_hi & 1);
1497             id_hi >>= 1;
1498         }
1499     }
1500
1501     for (i = 0; i < EM410X_ID_LENGTH; ++i) {
1502         id_bit = rev_id & 1;
1503
1504         if (i % 4 == 0) {
1505             // Don't write row parity bit at start of parsing
1506             if (i)
1507                 id = (id << 1) | r_parity;
1508             // Start counting parity for new row
1509             r_parity = id_bit;
1510         } else {
1511             // Count row parity
1512             r_parity ^= id_bit;
1513         }
1514
1515         // First elements in column?
1516         if (i < 4)
1517             // Fill out first elements
1518             c_parity[i] = id_bit;
1519         else
1520             // Count column parity
1521             c_parity[i % 4] ^= id_bit;
1522
1523         // Insert ID bit
1524         id = (id << 1) | id_bit;
1525         rev_id >>= 1;
1526     }
1527
1528     // Insert parity bit of last row
1529     id = (id << 1) | r_parity;
1530
1531     // Fill out column parity at the end of tag
1532     for (i = 0; i < 4; ++i)
1533         id = (id << 1) | c_parity[i];
1534
1535     // Add stop bit
1536     id <<= 1;
1537
1538     Dbprintf("Started writing %s tag ...", card ? "T55x7":"T5555");
1539     LED_D_ON();
1540
1541     // Write EM410x ID
1542     T55xxWriteBlock((uint32_t)(id >> 32), 1, 0, 0);
1543     T55xxWriteBlock((uint32_t)id, 2, 0, 0);
1544
1545     // Config for EM410x (RF/64, Manchester, Maxblock=2)
1546     if (card) {
1547         // Clock rate is stored in bits 8-15 of the card value
1548         clock = (card & 0xFF00) >> 8;
1549         Dbprintf("Clock rate: %d", clock);
1550         switch (clock)
1551         {
1552         case 32:
1553             clock = T55x7_BITRATE_RF_32;
1554             break;
1555         case 16:
1556             clock = T55x7_BITRATE_RF_16;
1557             break;
1558         case 0:
1559             // A value of 0 is assumed to be 64 for backwards-compatibility
1560             // Fall through...
1561         case 64:
1562             clock = T55x7_BITRATE_RF_64;
1563             break;
1564         default:
1565             Dbprintf("Invalid clock rate: %d", clock);
1566             return;
1567         }
1568
1569         // Writing configuration for T55x7 tag
1570         T55xxWriteBlock(clock       |
1571                         T55x7_MODULATION_MANCHESTER |
1572                         2 << T55x7_MAXBLOCK_SHIFT,
1573                         0, 0, 0);
1574     }
1575     else
1576         // Writing configuration for T5555(Q5) tag
1577         T55xxWriteBlock(0x1F << T5555_BITRATE_SHIFT |
1578                         T5555_MODULATION_MANCHESTER   |
1579                         2 << T5555_MAXBLOCK_SHIFT,
1580                         0, 0, 0);
1581
1582     LED_D_OFF();
1583     Dbprintf("Tag %s written with 0x%08x%08x\n", card ? "T55x7":"T5555",
1584              (uint32_t)(id >> 32), (uint32_t)id);
1585 }
1586
1587 // Clone Indala 64-bit tag by UID to T55x7
1588 void CopyIndala64toT55x7(int hi, int lo)
1589 {
1590
1591     //Program the 2 data blocks for supplied 64bit UID
1592     // and the block 0 for Indala64 format
1593     T55xxWriteBlock(hi,1,0,0);
1594     T55xxWriteBlock(lo,2,0,0);
1595     //Config for Indala (RF/32;PSK1 with RF/2;Maxblock=2)
1596     T55xxWriteBlock(T55x7_BITRATE_RF_32    |
1597                     T55x7_MODULATION_PSK1 |
1598                     2 << T55x7_MAXBLOCK_SHIFT,
1599                     0, 0, 0);
1600     //Alternative config for Indala (Extended mode;RF/32;PSK1 with RF/2;Maxblock=2;Inverse data)
1601     //  T5567WriteBlock(0x603E1042,0);
1602
1603     DbpString("DONE!");
1604
1605 }
1606
1607 void CopyIndala224toT55x7(int uid1, int uid2, int uid3, int uid4, int uid5, int uid6, int uid7)
1608 {
1609
1610     //Program the 7 data blocks for supplied 224bit UID
1611     // and the block 0 for Indala224 format
1612     T55xxWriteBlock(uid1,1,0,0);
1613     T55xxWriteBlock(uid2,2,0,0);
1614     T55xxWriteBlock(uid3,3,0,0);
1615     T55xxWriteBlock(uid4,4,0,0);
1616     T55xxWriteBlock(uid5,5,0,0);
1617     T55xxWriteBlock(uid6,6,0,0);
1618     T55xxWriteBlock(uid7,7,0,0);
1619     //Config for Indala (RF/32;PSK1 with RF/2;Maxblock=7)
1620     T55xxWriteBlock(T55x7_BITRATE_RF_32    |
1621                     T55x7_MODULATION_PSK1 |
1622                     7 << T55x7_MAXBLOCK_SHIFT,
1623                     0,0,0);
1624     //Alternative config for Indala (Extended mode;RF/32;PSK1 with RF/2;Maxblock=7;Inverse data)
1625     //  T5567WriteBlock(0x603E10E2,0);
1626
1627     DbpString("DONE!");
1628
1629 }
1630
1631
1632 #define abs(x) ( ((x)<0) ? -(x) : (x) )
1633 #define max(x,y) ( x<y ? y:x)
1634
1635 int DemodPCF7931(uint8_t **outBlocks) {
1636     uint8_t BitStream[256];
1637     uint8_t Blocks[8][16];
1638     uint8_t *GraphBuffer = BigBuf_get_addr();
1639     int GraphTraceLen = BigBuf_max_traceLen();
1640     int i, j, lastval, bitidx, half_switch;
1641     int clock = 64;
1642     int tolerance = clock / 8;
1643     int pmc, block_done;
1644     int lc, warnings = 0;
1645     int num_blocks = 0;
1646     int lmin=128, lmax=128;
1647     uint8_t dir;
1648
1649         LFSetupFPGAForADC(95, true);
1650         DoAcquisition_default(0, 0);
1651
1652
1653     lmin = 64;
1654     lmax = 192;
1655
1656     i = 2;
1657
1658     /* Find first local max/min */
1659     if(GraphBuffer[1] > GraphBuffer[0]) {
1660         while(i < GraphTraceLen) {
1661             if( !(GraphBuffer[i] > GraphBuffer[i-1]) && GraphBuffer[i] > lmax)
1662                 break;
1663             i++;
1664         }
1665         dir = 0;
1666     }
1667     else {
1668         while(i < GraphTraceLen) {
1669             if( !(GraphBuffer[i] < GraphBuffer[i-1]) && GraphBuffer[i] < lmin)
1670                 break;
1671             i++;
1672         }
1673         dir = 1;
1674     }
1675
1676     lastval = i++;
1677     half_switch = 0;
1678     pmc = 0;
1679     block_done = 0;
1680
1681     for (bitidx = 0; i < GraphTraceLen; i++)
1682     {
1683         if ( (GraphBuffer[i-1] > GraphBuffer[i] && dir == 1 && GraphBuffer[i] > lmax) || (GraphBuffer[i-1] < GraphBuffer[i] && dir == 0 && GraphBuffer[i] < lmin))
1684         {
1685             lc = i - lastval;
1686             lastval = i;
1687
1688             // Switch depending on lc length:
1689             // Tolerance is 1/8 of clock rate (arbitrary)
1690             if (abs(lc-clock/4) < tolerance) {
1691                 // 16T0
1692                 if((i - pmc) == lc) { /* 16T0 was previous one */
1693                     /* It's a PMC ! */
1694                     i += (128+127+16+32+33+16)-1;
1695                     lastval = i;
1696                     pmc = 0;
1697                     block_done = 1;
1698                 }
1699                 else {
1700                     pmc = i;
1701                 }
1702             } else if (abs(lc-clock/2) < tolerance) {
1703                 // 32TO
1704                 if((i - pmc) == lc) { /* 16T0 was previous one */
1705                     /* It's a PMC ! */
1706                     i += (128+127+16+32+33)-1;
1707                     lastval = i;
1708                     pmc = 0;
1709                     block_done = 1;
1710                 }
1711                 else if(half_switch == 1) {
1712                     BitStream[bitidx++] = 0;
1713                     half_switch = 0;
1714                 }
1715                 else
1716                     half_switch++;
1717             } else if (abs(lc-clock) < tolerance) {
1718                 // 64TO
1719                 BitStream[bitidx++] = 1;
1720             } else {
1721                 // Error
1722                 warnings++;
1723                 if (warnings > 10)
1724                 {
1725                     Dbprintf("Error: too many detection errors, aborting.");
1726                     return 0;
1727                 }
1728             }
1729
1730             if(block_done == 1) {
1731                 if(bitidx == 128) {
1732                     for(j=0; j<16; j++) {
1733                         Blocks[num_blocks][j] = 128*BitStream[j*8+7]+
1734                                 64*BitStream[j*8+6]+
1735                                 32*BitStream[j*8+5]+
1736                                 16*BitStream[j*8+4]+
1737                                 8*BitStream[j*8+3]+
1738                                 4*BitStream[j*8+2]+
1739                                 2*BitStream[j*8+1]+
1740                                 BitStream[j*8];
1741                     }
1742                     num_blocks++;
1743                 }
1744                 bitidx = 0;
1745                 block_done = 0;
1746                 half_switch = 0;
1747             }
1748             if(i < GraphTraceLen)
1749             {
1750                 if (GraphBuffer[i-1] > GraphBuffer[i]) dir=0;
1751                 else dir = 1;
1752             }
1753         }
1754         if(bitidx==255)
1755             bitidx=0;
1756         warnings = 0;
1757         if(num_blocks == 4) break;
1758     }
1759     memcpy(outBlocks, Blocks, 16*num_blocks);
1760     return num_blocks;
1761 }
1762
1763 int IsBlock0PCF7931(uint8_t *Block) {
1764     // Assume RFU means 0 :)
1765     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
1766         return 1;
1767     if((memcmp(Block+9, "\x00\x00\x00\x00\x00\x00\x00", 7) == 0) && Block[7] == 0) // PAC disabled, can it *really* happen ?
1768         return 1;
1769     return 0;
1770 }
1771
1772 int IsBlock1PCF7931(uint8_t *Block) {
1773     // Assume RFU means 0 :)
1774     if(Block[10] == 0 && Block[11] == 0 && Block[12] == 0 && Block[13] == 0)
1775         if((Block[14] & 0x7f) <= 9 && Block[15] <= 9)
1776             return 1;
1777
1778     return 0;
1779 }
1780
1781 #define ALLOC 16
1782
1783 void ReadPCF7931() {
1784     uint8_t Blocks[8][17];
1785     uint8_t tmpBlocks[4][16];
1786     int i, j, ind, ind2, n;
1787     int num_blocks = 0;
1788     int max_blocks = 8;
1789     int ident = 0;
1790     int error = 0;
1791     int tries = 0;
1792
1793     memset(Blocks, 0, 8*17*sizeof(uint8_t));
1794
1795     do {
1796         memset(tmpBlocks, 0, 4*16*sizeof(uint8_t));
1797         n = DemodPCF7931((uint8_t**)tmpBlocks);
1798         if(!n)
1799             error++;
1800         if(error==10 && num_blocks == 0) {
1801             Dbprintf("Error, no tag or bad tag");
1802             return;
1803         }
1804         else if (tries==20 || error==10) {
1805             Dbprintf("Error reading the tag");
1806             Dbprintf("Here is the partial content");
1807             goto end;
1808         }
1809
1810         for(i=0; i<n; i++)
1811             Dbprintf("(dbg) %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x",
1812                      tmpBlocks[i][0], tmpBlocks[i][1], tmpBlocks[i][2], tmpBlocks[i][3], tmpBlocks[i][4], tmpBlocks[i][5], tmpBlocks[i][6], tmpBlocks[i][7],
1813                     tmpBlocks[i][8], tmpBlocks[i][9], tmpBlocks[i][10], tmpBlocks[i][11], tmpBlocks[i][12], tmpBlocks[i][13], tmpBlocks[i][14], tmpBlocks[i][15]);
1814         if(!ident) {
1815             for(i=0; i<n; i++) {
1816                 if(IsBlock0PCF7931(tmpBlocks[i])) {
1817                     // Found block 0 ?
1818                     if(i < n-1 && IsBlock1PCF7931(tmpBlocks[i+1])) {
1819                         // Found block 1!
1820                         // \o/
1821                         ident = 1;
1822                         memcpy(Blocks[0], tmpBlocks[i], 16);
1823                         Blocks[0][ALLOC] = 1;
1824                         memcpy(Blocks[1], tmpBlocks[i+1], 16);
1825                         Blocks[1][ALLOC] = 1;
1826                         max_blocks = max((Blocks[1][14] & 0x7f), Blocks[1][15]) + 1;
1827                         // Debug print
1828                         Dbprintf("(dbg) Max blocks: %d", max_blocks);
1829                         num_blocks = 2;
1830                         // Handle following blocks
1831                         for(j=i+2, ind2=2; j!=i; j++, ind2++, num_blocks++) {
1832                             if(j==n) j=0;
1833                             if(j==i) break;
1834                             memcpy(Blocks[ind2], tmpBlocks[j], 16);
1835                             Blocks[ind2][ALLOC] = 1;
1836                         }
1837                         break;
1838                     }
1839                 }
1840             }
1841         }
1842         else {
1843             for(i=0; i<n; i++) { // Look for identical block in known blocks
1844                 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
1845                     for(j=0; j<max_blocks; j++) {
1846                         if(Blocks[j][ALLOC] == 1 && !memcmp(tmpBlocks[i], Blocks[j], 16)) {
1847                             // Found an identical block
1848                             for(ind=i-1,ind2=j-1; ind >= 0; ind--,ind2--) {
1849                                 if(ind2 < 0)
1850                                     ind2 = max_blocks;
1851                                 if(!Blocks[ind2][ALLOC]) { // Block ind2 not already found
1852                                     // Dbprintf("Tmp %d -> Block %d", ind, ind2);
1853                                     memcpy(Blocks[ind2], tmpBlocks[ind], 16);
1854                                     Blocks[ind2][ALLOC] = 1;
1855                                     num_blocks++;
1856                                     if(num_blocks == max_blocks) goto end;
1857                                 }
1858                             }
1859                             for(ind=i+1,ind2=j+1; ind < n; ind++,ind2++) {
1860                                 if(ind2 > max_blocks)
1861                                     ind2 = 0;
1862                                 if(!Blocks[ind2][ALLOC]) { // Block ind2 not already found
1863                                     // Dbprintf("Tmp %d -> Block %d", ind, ind2);
1864                                     memcpy(Blocks[ind2], tmpBlocks[ind], 16);
1865                                     Blocks[ind2][ALLOC] = 1;
1866                                     num_blocks++;
1867                                     if(num_blocks == max_blocks) goto end;
1868                                 }
1869                             }
1870                         }
1871                     }
1872                 }
1873             }
1874         }
1875         tries++;
1876         if (BUTTON_PRESS()) return;
1877     } while (num_blocks != max_blocks);
1878  end:
1879     Dbprintf("-----------------------------------------");
1880     Dbprintf("Memory content:");
1881     Dbprintf("-----------------------------------------");
1882     for(i=0; i<max_blocks; i++) {
1883         if(Blocks[i][ALLOC]==1)
1884             Dbprintf("%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x",
1885                      Blocks[i][0], Blocks[i][1], Blocks[i][2], Blocks[i][3], Blocks[i][4], Blocks[i][5], Blocks[i][6], Blocks[i][7],
1886                     Blocks[i][8], Blocks[i][9], Blocks[i][10], Blocks[i][11], Blocks[i][12], Blocks[i][13], Blocks[i][14], Blocks[i][15]);
1887         else
1888             Dbprintf("<missing block %d>", i);
1889     }
1890     Dbprintf("-----------------------------------------");
1891
1892     return ;
1893 }
1894
1895
1896 //-----------------------------------
1897 // EM4469 / EM4305 routines
1898 //-----------------------------------
1899 #define FWD_CMD_LOGIN 0xC //including the even parity, binary mirrored
1900 #define FWD_CMD_WRITE 0xA
1901 #define FWD_CMD_READ 0x9
1902 #define FWD_CMD_DISABLE 0x5
1903
1904
1905 uint8_t forwardLink_data[64]; //array of forwarded bits
1906 uint8_t * forward_ptr; //ptr for forward message preparation
1907 uint8_t fwd_bit_sz; //forwardlink bit counter
1908 uint8_t * fwd_write_ptr; //forwardlink bit pointer
1909
1910 //====================================================================
1911 // prepares command bits
1912 // see EM4469 spec
1913 //====================================================================
1914 //--------------------------------------------------------------------
1915 uint8_t Prepare_Cmd( uint8_t cmd ) {
1916     //--------------------------------------------------------------------
1917
1918     *forward_ptr++ = 0; //start bit
1919     *forward_ptr++ = 0; //second pause for 4050 code
1920
1921     *forward_ptr++ = cmd;
1922     cmd >>= 1;
1923     *forward_ptr++ = cmd;
1924     cmd >>= 1;
1925     *forward_ptr++ = cmd;
1926     cmd >>= 1;
1927     *forward_ptr++ = cmd;
1928
1929     return 6; //return number of emited bits
1930 }
1931
1932 //====================================================================
1933 // prepares address bits
1934 // see EM4469 spec
1935 //====================================================================
1936
1937 //--------------------------------------------------------------------
1938 uint8_t Prepare_Addr( uint8_t addr ) {
1939     //--------------------------------------------------------------------
1940
1941     register uint8_t line_parity;
1942
1943     uint8_t i;
1944     line_parity = 0;
1945     for(i=0;i<6;i++) {
1946         *forward_ptr++ = addr;
1947         line_parity ^= addr;
1948         addr >>= 1;
1949     }
1950
1951     *forward_ptr++ = (line_parity & 1);
1952
1953     return 7; //return number of emited bits
1954 }
1955
1956 //====================================================================
1957 // prepares data bits intreleaved with parity bits
1958 // see EM4469 spec
1959 //====================================================================
1960
1961 //--------------------------------------------------------------------
1962 uint8_t Prepare_Data( uint16_t data_low, uint16_t data_hi) {
1963     //--------------------------------------------------------------------
1964
1965     register uint8_t line_parity;
1966     register uint8_t column_parity;
1967     register uint8_t i, j;
1968     register uint16_t data;
1969
1970     data = data_low;
1971     column_parity = 0;
1972
1973     for(i=0; i<4; i++) {
1974         line_parity = 0;
1975         for(j=0; j<8; j++) {
1976             line_parity ^= data;
1977             column_parity ^= (data & 1) << j;
1978             *forward_ptr++ = data;
1979             data >>= 1;
1980         }
1981         *forward_ptr++ = line_parity;
1982         if(i == 1)
1983             data = data_hi;
1984     }
1985
1986     for(j=0; j<8; j++) {
1987         *forward_ptr++ = column_parity;
1988         column_parity >>= 1;
1989     }
1990     *forward_ptr = 0;
1991
1992     return 45; //return number of emited bits
1993 }
1994
1995 //====================================================================
1996 // Forward Link send function
1997 // Requires: forwarLink_data filled with valid bits (1 bit per byte)
1998 // fwd_bit_count set with number of bits to be sent
1999 //====================================================================
2000 void SendForward(uint8_t fwd_bit_count) {
2001
2002     fwd_write_ptr = forwardLink_data;
2003     fwd_bit_sz = fwd_bit_count;
2004
2005     LED_D_ON();
2006
2007     //Field on
2008     FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
2009     FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
2010     FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
2011
2012     // Give it a bit of time for the resonant antenna to settle.
2013     // And for the tag to fully power up
2014     SpinDelay(150);
2015
2016     // force 1st mod pulse (start gap must be longer for 4305)
2017     fwd_bit_sz--; //prepare next bit modulation
2018     fwd_write_ptr++;
2019     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
2020     SpinDelayUs(55*8); //55 cycles off (8us each)for 4305
2021     FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
2022     FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);//field on
2023     SpinDelayUs(16*8); //16 cycles on (8us each)
2024
2025     // now start writting
2026     while(fwd_bit_sz-- > 0) { //prepare next bit modulation
2027         if(((*fwd_write_ptr++) & 1) == 1)
2028             SpinDelayUs(32*8); //32 cycles at 125Khz (8us each)
2029         else {
2030             //These timings work for 4469/4269/4305 (with the 55*8 above)
2031             FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
2032             SpinDelayUs(23*8); //16-4 cycles off (8us each)
2033             FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
2034             FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);//field on
2035             SpinDelayUs(9*8); //16 cycles on (8us each)
2036         }
2037     }
2038 }
2039
2040 void EM4xLogin(uint32_t Password) {
2041
2042     uint8_t fwd_bit_count;
2043
2044     forward_ptr = forwardLink_data;
2045     fwd_bit_count = Prepare_Cmd( FWD_CMD_LOGIN );
2046     fwd_bit_count += Prepare_Data( Password&0xFFFF, Password>>16 );
2047
2048     SendForward(fwd_bit_count);
2049
2050     //Wait for command to complete
2051     SpinDelay(20);
2052
2053 }
2054
2055 void EM4xReadWord(uint8_t Address, uint32_t Pwd, uint8_t PwdMode) {
2056
2057     uint8_t fwd_bit_count;
2058     uint8_t *dest = BigBuf_get_addr();
2059     int m=0, i=0;
2060
2061     //If password mode do login
2062     if (PwdMode == 1) EM4xLogin(Pwd);
2063
2064     forward_ptr = forwardLink_data;
2065     fwd_bit_count = Prepare_Cmd( FWD_CMD_READ );
2066     fwd_bit_count += Prepare_Addr( Address );
2067
2068     m = BigBuf_max_traceLen();
2069     // Clear destination buffer before sending the command
2070     memset(dest, 128, m);
2071     // Connect the A/D to the peak-detected low-frequency path.
2072     SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
2073     // Now set up the SSC to get the ADC samples that are now streaming at us.
2074     FpgaSetupSsc();
2075
2076     SendForward(fwd_bit_count);
2077
2078     // Now do the acquisition
2079     i = 0;
2080     for(;;) {
2081         if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_TXRDY) {
2082             AT91C_BASE_SSC->SSC_THR = 0x43;
2083         }
2084         if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) {
2085             dest[i] = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
2086             i++;
2087             if (i >= m) break;
2088         }
2089     }
2090     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
2091     LED_D_OFF();
2092 }
2093
2094 void EM4xWriteWord(uint32_t Data, uint8_t Address, uint32_t Pwd, uint8_t PwdMode) {
2095
2096     uint8_t fwd_bit_count;
2097
2098     //If password mode do login
2099     if (PwdMode == 1) EM4xLogin(Pwd);
2100
2101     forward_ptr = forwardLink_data;
2102     fwd_bit_count = Prepare_Cmd( FWD_CMD_WRITE );
2103     fwd_bit_count += Prepare_Addr( Address );
2104     fwd_bit_count += Prepare_Data( Data&0xFFFF, Data>>16 );
2105
2106     SendForward(fwd_bit_count);
2107
2108     //Wait for write to complete
2109     SpinDelay(20);
2110     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
2111     LED_D_OFF();
2112 }