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

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