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

   1 //-----------------------------------------------------------------------------
   2 // This code is licensed to you under the terms of the GNU GPL, version 2 or,
   3 // at your option, any later version. See the LICENSE.txt file for the text of
   4 // the license.
   5 //-----------------------------------------------------------------------------
   6 // Miscellaneous routines for low frequency tag operations.
   7 // Tags supported here so far are Texas Instruments (TI), HID, EM4x05, EM410x
   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 "protocols.h"
  20 #include "usb_cdc.h" // for usb_poll_validate_length
  21
  22 /**
  23  * Function to do a modulation and then get samples.
  24  * @param delay_off
  25  * @param period_0
  26  * @param period_1
  27  * @param command
  28  */
  29 void ModThenAcquireRawAdcSamples125k(uint32_t delay_off, uint32_t period_0, uint32_t period_1, uint8_t *command)
  30 {
  31         // start timer
  32         StartTicks();
  33
  34         // use lf config settings
  35         sample_config *sc = getSamplingConfig();
  36
  37         // Make sure the tag is reset
  38         FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
  39         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
  40         WaitMS(2500);
  41
  42         // clear read buffer (after fpga bitstream loaded...)
  43         BigBuf_Clear_keep_EM();
  44
  45         // power on
  46         LFSetupFPGAForADC(sc->divisor, 1);
  47
  48         // And a little more time for the tag to fully power up
  49         WaitMS(2000);
  50         // if delay_off = 0 then just bitbang 1 = antenna on 0 = off for respective periods.
  51         bool bitbang = delay_off == 0;
  52         // now modulate the reader field
  53
  54         if (bitbang) {
  55                 // HACK it appears the loop and if statements take up about 7us so adjust waits accordingly...
  56                 uint8_t hack_cnt = 7;
  57                 if (period_0 < hack_cnt || period_1 < hack_cnt) {
  58                         DbpString("Warning periods cannot be less than 7us in bit bang mode");
  59                         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
  60                         LED_D_OFF();
  61                         return;
  62                 }
  63
  64                 // hack2 needed---  it appears to take about 8-16us to turn the antenna back on
  65                 // leading to ~ 1 to 2 125khz samples extra in every off period
  66                 // so we should test for last 0 before next 1 and reduce period_0 by this extra amount...
  67                 // but is this time different for every antenna or other hw builds???  more testing needed
  68
  69                 // prime cmd_len to save time comparing strings while modulating
  70                 int cmd_len = 0;
  71                 while(command[cmd_len] != '\0' && command[cmd_len] != ' ')
  72                         cmd_len++;
  73
  74                 int counter = 0;
  75                 bool off = false;
  76                 for (counter = 0; counter < cmd_len; counter++) {
  77                         // if cmd = 0 then turn field off
  78                         if (command[counter] == '0') {
  79                                 // if field already off leave alone (affects timing otherwise)
  80                                 if (off == false) {
  81                                         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
  82                                         LED_D_OFF();
  83                                         off = true;
  84                                 }
  85                                 // note we appear to take about 7us to switch over (or run the if statements/loop...)
  86                                 WaitUS(period_0-hack_cnt);
  87                         // else if cmd = 1 then turn field on
  88                         } else {
  89                                 // if field already on leave alone (affects timing otherwise)
  90                                 if (off) {
  91                                         FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
  92                                         LED_D_ON();
  93                                         off = false;
  94                                 }
  95                                 // note we appear to take about 7us to switch over (or run the if statements/loop...)
  96                                 WaitUS(period_1-hack_cnt);
  97                         }
  98                 }
  99         } else { // old mode of cmd read using delay as off period
 100                 while(*command != '\0' && *command != ' ') {
 101                         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 102                         LED_D_OFF();
 103                         WaitUS(delay_off);
 104                         FpgaSendCommand(FPGA_CMD_SET_DIVISOR, sc->divisor);
 105                         FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
 106                         LED_D_ON();
 107                         if(*(command++) == '0') {
 108                                 WaitUS(period_0);
 109                         } else {
 110                                 WaitUS(period_1);
 111                         }
 112                 }
 113                 FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 114                 LED_D_OFF();
 115                 WaitUS(delay_off);
 116                 FpgaSendCommand(FPGA_CMD_SET_DIVISOR, sc->divisor);
 117         }
 118
 119         FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
 120
 121         // now do the read
 122         DoAcquisition_config(false, 0);
 123
 124         // Turn off antenna
 125         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 126         // tell client we are done
 127         cmd_send(CMD_ACK,0,0,0,0,0);
 128 }
 129
 130 /* blank r/w tag data stream
 131 ...0000000000000000 01111111
 132 1010101010101010101010101010101010101010101010101010101010101010
 133 0011010010100001
 134 01111111
 135 101010101010101[0]000...
 136
 137 [5555fe852c5555555555555555fe0000]
 138 */
 139 void ReadTItag(void)
 140 {
 141         // some hardcoded initial params
 142         // when we read a TI tag we sample the zerocross line at 2Mhz
 143         // TI tags modulate a 1 as 16 cycles of 123.2Khz
 144         // TI tags modulate a 0 as 16 cycles of 134.2Khz
 145  #define FSAMPLE 2000000
 146  #define FREQLO 123200
 147  #define FREQHI 134200
 148
 149         signed char *dest = (signed char *)BigBuf_get_addr();
 150         uint16_t n = BigBuf_max_traceLen();
 151         // 128 bit shift register [shift3:shift2:shift1:shift0]
 152         uint32_t shift3 = 0, shift2 = 0, shift1 = 0, shift0 = 0;
 153
 154         int i, cycles=0, samples=0;
 155         // how many sample points fit in 16 cycles of each frequency
 156         uint32_t sampleslo = (FSAMPLE<<4)/FREQLO, sampleshi = (FSAMPLE<<4)/FREQHI;
 157         // when to tell if we're close enough to one freq or another
 158         uint32_t threshold = (sampleslo - sampleshi + 1)>>1;
 159
 160         // TI tags charge at 134.2Khz
 161         FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
 162         FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 88); //134.8Khz
 163
 164         // Place FPGA in passthrough mode, in this mode the CROSS_LO line
 165         // connects to SSP_DIN and the SSP_DOUT logic level controls
 166         // whether we're modulating the antenna (high)
 167         // or listening to the antenna (low)
 168         FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_PASSTHRU);
 169
 170         // get TI tag data into the buffer
 171         AcquireTiType();
 172
 173         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 174
 175         for (i=0; i<n-1; i++) {
 176                 // count cycles by looking for lo to hi zero crossings
 177                 if ( (dest[i]<0) && (dest[i+1]>0) ) {
 178                         cycles++;
 179                         // after 16 cycles, measure the frequency
 180                         if (cycles>15) {
 181                                 cycles=0;
 182                                 samples=i-samples; // number of samples in these 16 cycles
 183
 184                                 // TI bits are coming to us lsb first so shift them
 185                                 // right through our 128 bit right shift register
 186                                 shift0 = (shift0>>1) | (shift1 << 31);
 187                                 shift1 = (shift1>>1) | (shift2 << 31);
 188                                 shift2 = (shift2>>1) | (shift3 << 31);
 189                                 shift3 >>= 1;
 190
 191                                 // check if the cycles fall close to the number
 192                                 // expected for either the low or high frequency
 193                                 if ( (samples>(sampleslo-threshold)) && (samples<(sampleslo+threshold)) ) {
 194                                         // low frequency represents a 1
 195                                         shift3 |= (1<<31);
 196                                 } else if ( (samples>(sampleshi-threshold)) && (samples<(sampleshi+threshold)) ) {
 197                                         // high frequency represents a 0
 198                                 } else {
 199                                         // probably detected a gay waveform or noise
 200                                         // use this as gaydar or discard shift register and start again
 201                                         shift3 = shift2 = shift1 = shift0 = 0;
 202                                 }
 203                                 samples = i;
 204
 205                                 // for each bit we receive, test if we've detected a valid tag
 206
 207                                 // if we see 17 zeroes followed by 6 ones, we might have a tag
 208                                 // remember the bits are backwards
 209                                 if ( ((shift0 & 0x7fffff) == 0x7e0000) ) {
 210                                         // if start and end bytes match, we have a tag so break out of the loop
 211                                         if ( ((shift0>>16)&0xff) == ((shift3>>8)&0xff) ) {
 212                                                 cycles = 0xF0B; //use this as a flag (ugly but whatever)
 213                                                 break;
 214                                         }
 215                                 }
 216                         }
 217                 }
 218         }
 219
 220         // if flag is set we have a tag
 221         if (cycles!=0xF0B) {
 222                 DbpString("Info: No valid tag detected.");
 223         } else {
 224                 // put 64 bit data into shift1 and shift0
 225                 shift0 = (shift0>>24) | (shift1 << 8);
 226                 shift1 = (shift1>>24) | (shift2 << 8);
 227
 228                 // align 16 bit crc into lower half of shift2
 229                 shift2 = ((shift2>>24) | (shift3 << 8)) & 0x0ffff;
 230
 231                 // if r/w tag, check ident match
 232                 if (shift3 & (1<<15) ) {
 233                         DbpString("Info: TI tag is rewriteable");
 234                         // only 15 bits compare, last bit of ident is not valid
 235                         if (((shift3 >> 16) ^ shift0) & 0x7fff ) {
 236                                 DbpString("Error: Ident mismatch!");
 237                         } else {
 238                                 DbpString("Info: TI tag ident is valid");
 239                         }
 240                 } else {
 241                         DbpString("Info: TI tag is readonly");
 242                 }
 243
 244                 // WARNING the order of the bytes in which we calc crc below needs checking
 245                 // i'm 99% sure the crc algorithm is correct, but it may need to eat the
 246                 // bytes in reverse or something
 247                 // calculate CRC
 248                 uint32_t crc=0;
 249
 250                 crc = update_crc16(crc, (shift0)&0xff);
 251                 crc = update_crc16(crc, (shift0>>8)&0xff);
 252                 crc = update_crc16(crc, (shift0>>16)&0xff);
 253                 crc = update_crc16(crc, (shift0>>24)&0xff);
 254                 crc = update_crc16(crc, (shift1)&0xff);
 255                 crc = update_crc16(crc, (shift1>>8)&0xff);
 256                 crc = update_crc16(crc, (shift1>>16)&0xff);
 257                 crc = update_crc16(crc, (shift1>>24)&0xff);
 258
 259                 Dbprintf("Info: Tag data: %x%08x, crc=%x",
 260                                  (unsigned int)shift1, (unsigned int)shift0, (unsigned int)shift2 & 0xFFFF);
 261                 if (crc != (shift2&0xffff)) {
 262                         Dbprintf("Error: CRC mismatch, expected %x", (unsigned int)crc);
 263                 } else {
 264                         DbpString("Info: CRC is good");
 265                 }
 266         }
 267 }
 268
 269 void WriteTIbyte(uint8_t b)
 270 {
 271         int i = 0;
 272
 273         // modulate 8 bits out to the antenna
 274         for (i=0; i<8; i++)
 275         {
 276                 if (b&(1<<i)) {
 277                         // stop modulating antenna
 278                         LOW(GPIO_SSC_DOUT);
 279                         SpinDelayUs(1000);
 280                         // modulate antenna
 281                         HIGH(GPIO_SSC_DOUT);
 282                         SpinDelayUs(1000);
 283                 } else {
 284                         // stop modulating antenna
 285                         LOW(GPIO_SSC_DOUT);
 286                         SpinDelayUs(300);
 287                         // modulate antenna
 288                         HIGH(GPIO_SSC_DOUT);
 289                         SpinDelayUs(1700);
 290                 }
 291         }
 292 }
 293
 294 void AcquireTiType(void)
 295 {
 296         int i, j, n;
 297         // tag transmission is <20ms, sampling at 2M gives us 40K samples max
 298         // each sample is 1 bit stuffed into a uint32_t so we need 1250 uint32_t
 299  #define TIBUFLEN 1250
 300
 301         // clear buffer
 302         uint32_t *BigBuf = (uint32_t *)BigBuf_get_addr();
 303         BigBuf_Clear_ext(false);
 304
 305         // Set up the synchronous serial port
 306         AT91C_BASE_PIOA->PIO_PDR = GPIO_SSC_DIN;
 307         AT91C_BASE_PIOA->PIO_ASR = GPIO_SSC_DIN;
 308
 309         // steal this pin from the SSP and use it to control the modulation
 310         AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT;
 311         AT91C_BASE_PIOA->PIO_OER = GPIO_SSC_DOUT;
 312
 313         AT91C_BASE_SSC->SSC_CR = AT91C_SSC_SWRST;
 314         AT91C_BASE_SSC->SSC_CR = AT91C_SSC_RXEN | AT91C_SSC_TXEN;
 315
 316         // Sample at 2 Mbit/s, so TI tags are 16.2 vs. 14.9 clocks long
 317         // 48/2 = 24 MHz clock must be divided by 12
 318         AT91C_BASE_SSC->SSC_CMR = 12;
 319
 320         AT91C_BASE_SSC->SSC_RCMR = SSC_CLOCK_MODE_SELECT(0);
 321         AT91C_BASE_SSC->SSC_RFMR = SSC_FRAME_MODE_BITS_IN_WORD(32) | AT91C_SSC_MSBF;
 322         AT91C_BASE_SSC->SSC_TCMR = 0;
 323         AT91C_BASE_SSC->SSC_TFMR = 0;
 324
 325         LED_D_ON();
 326
 327         // modulate antenna
 328         HIGH(GPIO_SSC_DOUT);
 329
 330         // Charge TI tag for 50ms.
 331         SpinDelay(50);
 332
 333         // stop modulating antenna and listen
 334         LOW(GPIO_SSC_DOUT);
 335
 336         LED_D_OFF();
 337
 338         i = 0;
 339         for(;;) {
 340                 if(AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY) {
 341                         BigBuf[i] = AT91C_BASE_SSC->SSC_RHR;    // store 32 bit values in buffer
 342                         i++; if(i >= TIBUFLEN) break;
 343                 }
 344                 WDT_HIT();
 345         }
 346
 347         // return stolen pin to SSP
 348         AT91C_BASE_PIOA->PIO_PDR = GPIO_SSC_DOUT;
 349         AT91C_BASE_PIOA->PIO_ASR = GPIO_SSC_DIN | GPIO_SSC_DOUT;
 350
 351         char *dest = (char *)BigBuf_get_addr();
 352         n = TIBUFLEN*32;
 353         // unpack buffer
 354         for (i=TIBUFLEN-1; i>=0; i--) {
 355                 for (j=0; j<32; j++) {
 356                         if(BigBuf[i] & (1 << j)) {
 357                                 dest[--n] = 1;
 358                         } else {
 359                                 dest[--n] = -1;
 360                         }
 361                 }
 362         }
 363 }
 364
 365 // arguments: 64bit data split into 32bit idhi:idlo and optional 16bit crc
 366 // if crc provided, it will be written with the data verbatim (even if bogus)
 367 // if not provided a valid crc will be computed from the data and written.
 368 void WriteTItag(uint32_t idhi, uint32_t idlo, uint16_t crc)
 369 {
 370         FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
 371         if(crc == 0) {
 372                 crc = update_crc16(crc, (idlo)&0xff);
 373                 crc = update_crc16(crc, (idlo>>8)&0xff);
 374                 crc = update_crc16(crc, (idlo>>16)&0xff);
 375                 crc = update_crc16(crc, (idlo>>24)&0xff);
 376                 crc = update_crc16(crc, (idhi)&0xff);
 377                 crc = update_crc16(crc, (idhi>>8)&0xff);
 378                 crc = update_crc16(crc, (idhi>>16)&0xff);
 379                 crc = update_crc16(crc, (idhi>>24)&0xff);
 380         }
 381         Dbprintf("Writing to tag: %x%08x, crc=%x",
 382                         (unsigned int) idhi, (unsigned int) idlo, crc);
 383
 384         // TI tags charge at 134.2Khz
 385         FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 88); //134.8Khz
 386         // Place FPGA in passthrough mode, in this mode the CROSS_LO line
 387         // connects to SSP_DIN and the SSP_DOUT logic level controls
 388         // whether we're modulating the antenna (high)
 389         // or listening to the antenna (low)
 390         FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_PASSTHRU);
 391         LED_A_ON();
 392
 393         // steal this pin from the SSP and use it to control the modulation
 394         AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT;
 395         AT91C_BASE_PIOA->PIO_OER = GPIO_SSC_DOUT;
 396
 397         // writing algorithm:
 398         // a high bit consists of a field off for 1ms and field on for 1ms
 399         // a low bit consists of a field off for 0.3ms and field on for 1.7ms
 400         // initiate a charge time of 50ms (field on) then immediately start writing bits
 401         // start by writing 0xBB (keyword) and 0xEB (password)
 402         // then write 80 bits of data (or 64 bit data + 16 bit crc if you prefer)
 403         // finally end with 0x0300 (write frame)
 404         // all data is sent lsb firts
 405         // finish with 15ms programming time
 406
 407         // modulate antenna
 408         HIGH(GPIO_SSC_DOUT);
 409         SpinDelay(50);  // charge time
 410
 411         WriteTIbyte(0xbb); // keyword
 412         WriteTIbyte(0xeb); // password
 413         WriteTIbyte( (idlo    )&0xff );
 414         WriteTIbyte( (idlo>>8 )&0xff );
 415         WriteTIbyte( (idlo>>16)&0xff );
 416         WriteTIbyte( (idlo>>24)&0xff );
 417         WriteTIbyte( (idhi    )&0xff );
 418         WriteTIbyte( (idhi>>8 )&0xff );
 419         WriteTIbyte( (idhi>>16)&0xff );
 420         WriteTIbyte( (idhi>>24)&0xff ); // data hi to lo
 421         WriteTIbyte( (crc     )&0xff ); // crc lo
 422         WriteTIbyte( (crc>>8  )&0xff ); // crc hi
 423         WriteTIbyte(0x00); // write frame lo
 424         WriteTIbyte(0x03); // write frame hi
 425         HIGH(GPIO_SSC_DOUT);
 426         SpinDelay(50);  // programming time
 427
 428         LED_A_OFF();
 429
 430         // get TI tag data into the buffer
 431         AcquireTiType();
 432
 433         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 434         DbpString("Now use `lf ti read` to check");
 435 }
 436
 437 void SimulateTagLowFrequency(int period, int gap, int ledcontrol)
 438 {
 439         int i;
 440         uint8_t *tab = BigBuf_get_addr();
 441
 442         //note FpgaDownloadAndGo destroys the bigbuf so be sure this is called before now...
 443         //FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
 444         FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_EDGE_DETECT);
 445
 446         AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT | GPIO_SSC_CLK;
 447
 448         AT91C_BASE_PIOA->PIO_OER = GPIO_SSC_DOUT;
 449         AT91C_BASE_PIOA->PIO_ODR = GPIO_SSC_CLK;
 450
 451  #define SHORT_COIL()   LOW(GPIO_SSC_DOUT)
 452  #define OPEN_COIL()    HIGH(GPIO_SSC_DOUT)
 453
 454         i = 0;
 455         for(;;) {
 456                 //wait until SSC_CLK goes HIGH
 457                 int ii = 0;
 458                 while(!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK)) {
 459                         //only check every 1000th time (usb_poll_validate_length on some systems was too slow)
 460                         if ( ii == 1000 ) {
 461                                 if (BUTTON_PRESS() || usb_poll_validate_length() ) {
 462                                         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 463                                         DbpString("Stopped");
 464                                         return;
 465                                 }
 466                                 ii=0;
 467                         }
 468                         WDT_HIT();
 469                         ii++;
 470                 }
 471                 if (ledcontrol)
 472                         LED_D_ON();
 473
 474                 if(tab[i])
 475                         OPEN_COIL();
 476                 else
 477                         SHORT_COIL();
 478
 479                 if (ledcontrol)
 480                         LED_D_OFF();
 481                 ii=0;
 482                 //wait until SSC_CLK goes LOW
 483                 while(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK) {
 484                         //only check every 1000th time (usb_poll_validate_length on some systems was too slow)
 485                         if ( ii == 1000 ) {
 486                                 if (BUTTON_PRESS() || usb_poll_validate_length() ) {
 487                                         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 488                                         DbpString("Stopped");
 489                                         return;
 490                                 }
 491                                 ii=0;
 492                         }
 493                         WDT_HIT();
 494                         ii++;
 495                 }
 496
 497                 i++;
 498                 if(i == period) {
 499
 500                         i = 0;
 501                         if (gap) {
 502                                 SHORT_COIL();
 503                                 SpinDelayUs(gap);
 504                         }
 505                 }
 506
 507         }
 508 }
 509
 510 #define DEBUG_FRAME_CONTENTS 1
 511 void SimulateTagLowFrequencyBidir(int divisor, int t0)
 512 {
 513 }
 514
 515 // compose fc/8 fc/10 waveform (FSK2)
 516 static void fc(int c, int *n)
 517 {
 518         uint8_t *dest = BigBuf_get_addr();
 519         int idx;
 520
 521         // for when we want an fc8 pattern every 4 logical bits
 522         if(c==0) {
 523                 dest[((*n)++)]=1;
 524                 dest[((*n)++)]=1;
 525                 dest[((*n)++)]=1;
 526                 dest[((*n)++)]=1;
 527                 dest[((*n)++)]=0;
 528                 dest[((*n)++)]=0;
 529                 dest[((*n)++)]=0;
 530                 dest[((*n)++)]=0;
 531         }
 532
 533         //      an fc/8  encoded bit is a bit pattern of  11110000  x6 = 48 samples
 534         if(c==8) {
 535                 for (idx=0; idx<6; idx++) {
 536                         dest[((*n)++)]=1;
 537                         dest[((*n)++)]=1;
 538                         dest[((*n)++)]=1;
 539                         dest[((*n)++)]=1;
 540                         dest[((*n)++)]=0;
 541                         dest[((*n)++)]=0;
 542                         dest[((*n)++)]=0;
 543                         dest[((*n)++)]=0;
 544                 }
 545         }
 546
 547         //      an fc/10 encoded bit is a bit pattern of 1111100000 x5 = 50 samples
 548         if(c==10) {
 549                 for (idx=0; idx<5; idx++) {
 550                         dest[((*n)++)]=1;
 551                         dest[((*n)++)]=1;
 552                         dest[((*n)++)]=1;
 553                         dest[((*n)++)]=1;
 554                         dest[((*n)++)]=1;
 555                         dest[((*n)++)]=0;
 556                         dest[((*n)++)]=0;
 557                         dest[((*n)++)]=0;
 558                         dest[((*n)++)]=0;
 559                         dest[((*n)++)]=0;
 560                 }
 561         }
 562 }
 563 // compose fc/X fc/Y waveform (FSKx)
 564 static void fcAll(uint8_t fc, int *n, uint8_t clock, uint16_t *modCnt)
 565 {
 566         uint8_t *dest = BigBuf_get_addr();
 567         uint8_t halfFC = fc/2;
 568         uint8_t wavesPerClock = clock/fc;
 569         uint8_t mod = clock % fc;    //modifier
 570         uint8_t modAdj = fc/mod;     //how often to apply modifier
 571         bool modAdjOk = !(fc % mod); //if (fc % mod==0) modAdjOk=true;
 572         // loop through clock - step field clock
 573         for (uint8_t idx=0; idx < wavesPerClock; idx++){
 574                 // put 1/2 FC length 1's and 1/2 0's per field clock wave (to create the wave)
 575                 memset(dest+(*n), 0, fc-halfFC);  //in case of odd number use extra here
 576                 memset(dest+(*n)+(fc-halfFC), 1, halfFC);
 577                 *n += fc;
 578         }
 579         if (mod>0) (*modCnt)++;
 580         if ((mod>0) && modAdjOk){  //fsk2
 581                 if ((*modCnt % modAdj) == 0){ //if 4th 8 length wave in a rf/50 add extra 8 length wave
 582                         memset(dest+(*n), 0, fc-halfFC);
 583                         memset(dest+(*n)+(fc-halfFC), 1, halfFC);
 584                         *n += fc;
 585                 }
 586         }
 587         if (mod>0 && !modAdjOk){  //fsk1
 588                 memset(dest+(*n), 0, mod-(mod/2));
 589                 memset(dest+(*n)+(mod-(mod/2)), 1, mod/2);
 590                 *n += mod;
 591         }
 592 }
 593
 594 // prepare a waveform pattern in the buffer based on the ID given then
 595 // simulate a HID tag until the button is pressed
 596 void CmdHIDsimTAG(int hi2, int hi, int lo, int ledcontrol)
 597 {
 598         int n=0, i=0;
 599         /*
 600          HID tag bitstream format
 601          The tag contains a 44bit unique code. This is sent out MSB first in sets of 4 bits
 602          A 1 bit is represented as 6 fc8 and 5 fc10 patterns
 603          A 0 bit is represented as 5 fc10 and 6 fc8 patterns
 604          A fc8 is inserted before every 4 bits
 605          A special start of frame pattern is used consisting a0b0 where a and b are neither 0
 606          nor 1 bits, they are special patterns (a = set of 12 fc8 and b = set of 10 fc10)
 607         */
 608
 609         if (hi2>0x0FFFFFFF) {
 610                 DbpString("Tags can only have 44 or 84 bits. - USE lf simfsk for larger tags");
 611                 return;
 612         }
 613         // set LF so we don't kill the bigbuf we are setting with simulation data.
 614         FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
 615
 616         fc(0,&n);
 617         // special start of frame marker containing invalid bit sequences
 618         fc(8,  &n);     fc(8,  &n); // invalid
 619         fc(8,  &n);     fc(10, &n); // logical 0
 620         fc(10, &n);     fc(10, &n); // invalid
 621         fc(8,  &n);     fc(10, &n); // logical 0
 622
 623         WDT_HIT();
 624         if (hi2 > 0 || hi > 0xFFF){
 625                 // manchester encode bits 91 to 64 (91-84 are part of the header)
 626                 for (i=27; i>=0; i--) {
 627                         if ((i%4)==3) fc(0,&n);
 628                         if ((hi2>>i)&1) {
 629                                 fc(10, &n); fc(8,  &n);         // low-high transition
 630                         } else {
 631                                 fc(8,  &n); fc(10, &n);         // high-low transition
 632                         }
 633                 }
 634                 WDT_HIT();
 635                 // manchester encode bits 63 to 32
 636                 for (i=31; i>=0; i--) {
 637                         if ((i%4)==3) fc(0,&n);
 638                         if ((hi>>i)&1) {
 639                                 fc(10, &n); fc(8,  &n);         // low-high transition
 640                         } else {
 641                                 fc(8,  &n); fc(10, &n);         // high-low transition
 642                         }
 643                 }
 644         } else {
 645                 // manchester encode bits 43 to 32
 646                 for (i=11; i>=0; i--) {
 647                         if ((i%4)==3) fc(0,&n);
 648                         if ((hi>>i)&1) {
 649                                 fc(10, &n); fc(8,  &n);         // low-high transition
 650                         } else {
 651                                 fc(8,  &n); fc(10, &n);         // high-low transition
 652                         }
 653                 }
 654         }
 655
 656         WDT_HIT();
 657         // manchester encode bits 31 to 0
 658         for (i=31; i>=0; i--) {
 659                 if ((i%4)==3) fc(0,&n);
 660                 if ((lo>>i)&1) {
 661                         fc(10, &n); fc(8,  &n);         // low-high transition
 662                 } else {
 663                         fc(8,  &n); fc(10, &n);         // high-low transition
 664                 }
 665         }
 666
 667         if (ledcontrol)
 668                 LED_A_ON();
 669         SimulateTagLowFrequency(n, 0, ledcontrol);
 670
 671         if (ledcontrol)
 672                 LED_A_OFF();
 673 }
 674
 675 // prepare a waveform pattern in the buffer based on the ID given then
 676 // simulate a FSK tag until the button is pressed
 677 // arg1 contains fcHigh and fcLow, arg2 contains invert and clock
 678 void CmdFSKsimTAG(uint16_t arg1, uint16_t arg2, size_t size, uint8_t *BitStream)
 679 {
 680         int ledcontrol=1;
 681         int n=0, i=0;
 682         uint8_t fcHigh = arg1 >> 8;
 683         uint8_t fcLow = arg1 & 0xFF;
 684         uint16_t modCnt = 0;
 685         uint8_t clk = arg2 & 0xFF;
 686         uint8_t invert = (arg2 >> 8) & 1;
 687
 688         // set LF so we don't kill the bigbuf we are setting with simulation data.
 689         FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
 690
 691         for (i=0; i<size; i++){
 692                 if (BitStream[i] == invert){
 693                         fcAll(fcLow, &n, clk, &modCnt);
 694                 } else {
 695                         fcAll(fcHigh, &n, clk, &modCnt);
 696                 }
 697         }
 698         Dbprintf("Simulating with fcHigh: %d, fcLow: %d, clk: %d, invert: %d, n: %d",fcHigh, fcLow, clk, invert, n);
 699         /*Dbprintf("DEBUG: First 32:");
 700         uint8_t *dest = BigBuf_get_addr();
 701         i=0;
 702         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]);
 703         i+=16;
 704         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]);
 705         */
 706         if (ledcontrol)
 707                 LED_A_ON();
 708
 709         SimulateTagLowFrequency(n, 0, ledcontrol);
 710
 711         if (ledcontrol)
 712                 LED_A_OFF();
 713 }
 714
 715 // compose ask waveform for one bit(ASK)
 716 static void askSimBit(uint8_t c, int *n, uint8_t clock, uint8_t manchester)
 717 {
 718         uint8_t *dest = BigBuf_get_addr();
 719         uint8_t halfClk = clock/2;
 720         // c = current bit 1 or 0
 721         if (manchester==1){
 722                 memset(dest+(*n), c, halfClk);
 723                 memset(dest+(*n) + halfClk, c^1, halfClk);
 724         } else {
 725                 memset(dest+(*n), c, clock);
 726         }
 727         *n += clock;
 728 }
 729
 730 static void biphaseSimBit(uint8_t c, int *n, uint8_t clock, uint8_t *phase)
 731 {
 732         uint8_t *dest = BigBuf_get_addr();
 733         uint8_t halfClk = clock/2;
 734         if (c){
 735                 memset(dest+(*n), c ^ 1 ^ *phase, halfClk);
 736                 memset(dest+(*n) + halfClk, c ^ *phase, halfClk);
 737         } else {
 738                 memset(dest+(*n), c ^ *phase, clock);
 739                 *phase ^= 1;
 740         }
 741         *n += clock;
 742 }
 743
 744 static void stAskSimBit(int *n, uint8_t clock) {
 745         uint8_t *dest = BigBuf_get_addr();
 746         uint8_t halfClk = clock/2;
 747         //ST = .5 high .5 low 1.5 high .5 low 1 high
 748         memset(dest+(*n), 1, halfClk);
 749         memset(dest+(*n) + halfClk, 0, halfClk);
 750         memset(dest+(*n) + clock, 1, clock + halfClk);
 751         memset(dest+(*n) + clock*2 + halfClk, 0, halfClk);
 752         memset(dest+(*n) + clock*3, 1, clock);
 753         *n += clock*4;
 754 }
 755
 756 // args clock, ask/man or askraw, invert, transmission separator
 757 void CmdASKsimTag(uint16_t arg1, uint16_t arg2, size_t size, uint8_t *BitStream)
 758 {
 759         int ledcontrol = 1;
 760         int n=0, i=0;
 761         uint8_t clk = (arg1 >> 8) & 0xFF;
 762         uint8_t encoding = arg1 & 0xFF;
 763         uint8_t separator = arg2 & 1;
 764         uint8_t invert = (arg2 >> 8) & 1;
 765
 766         // set LF so we don't kill the bigbuf we are setting with simulation data.
 767         FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
 768
 769         if (encoding==2){  //biphase
 770                 uint8_t phase=0;
 771                 for (i=0; i<size; i++){
 772                         biphaseSimBit(BitStream[i]^invert, &n, clk, &phase);
 773                 }
 774                 if (phase==1) { //run a second set inverted to keep phase in check
 775                         for (i=0; i<size; i++){
 776                                 biphaseSimBit(BitStream[i]^invert, &n, clk, &phase);
 777                         }
 778                 }
 779         } else {  // ask/manchester || ask/raw
 780                 for (i=0; i<size; i++){
 781                         askSimBit(BitStream[i]^invert, &n, clk, encoding);
 782                 }
 783                 if (encoding==0 && BitStream[0]==BitStream[size-1]){ //run a second set inverted (for ask/raw || biphase phase)
 784                         for (i=0; i<size; i++){
 785                                 askSimBit(BitStream[i]^invert^1, &n, clk, encoding);
 786                         }
 787                 }
 788         }
 789         if (separator==1 && encoding == 1)
 790                 stAskSimBit(&n, clk);
 791         else if (separator==1)
 792                 Dbprintf("sorry but separator option not yet available");
 793
 794         Dbprintf("Simulating with clk: %d, invert: %d, encoding: %d, separator: %d, n: %d",clk, invert, encoding, separator, n);
 795         //DEBUG
 796         //Dbprintf("First 32:");
 797         //uint8_t *dest = BigBuf_get_addr();
 798         //i=0;
 799         //Dbprintf("%d%d%d%d%d%d%d%d%d%d%d%d%d%d%d%d", dest[i],dest[i+1],dest[i+2],dest[i+3],dest[i+4],dest[i+5],dest[i+6],dest[i+7],dest[i+8],dest[i+9],dest[i+10],dest[i+11],dest[i+12],dest[i+13],dest[i+14],dest[i+15]);
 800         //i+=16;
 801         //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]);
 802
 803         if (ledcontrol) LED_A_ON();
 804         SimulateTagLowFrequency(n, 0, ledcontrol);
 805         if (ledcontrol) LED_A_OFF();
 806 }
 807
 808 //carrier can be 2,4 or 8
 809 static void pskSimBit(uint8_t waveLen, int *n, uint8_t clk, uint8_t *curPhase, bool phaseChg)
 810 {
 811         uint8_t *dest = BigBuf_get_addr();
 812         uint8_t halfWave = waveLen/2;
 813         //uint8_t idx;
 814         int i = 0;
 815         if (phaseChg){
 816                 // write phase change
 817                 memset(dest+(*n), *curPhase^1, halfWave);
 818                 memset(dest+(*n) + halfWave, *curPhase, halfWave);
 819                 *n += waveLen;
 820                 *curPhase ^= 1;
 821                 i += waveLen;
 822         }
 823         //write each normal clock wave for the clock duration
 824         for (; i < clk; i+=waveLen){
 825                 memset(dest+(*n), *curPhase, halfWave);
 826                 memset(dest+(*n) + halfWave, *curPhase^1, halfWave);
 827                 *n += waveLen;
 828         }
 829 }
 830
 831 // args clock, carrier, invert,
 832 void CmdPSKsimTag(uint16_t arg1, uint16_t arg2, size_t size, uint8_t *BitStream)
 833 {
 834         int ledcontrol=1;
 835         int n=0, i=0;
 836         uint8_t clk = arg1 >> 8;
 837         uint8_t carrier = arg1 & 0xFF;
 838         uint8_t invert = arg2 & 0xFF;
 839         uint8_t curPhase = 0;
 840         // set LF so we don't kill the bigbuf we are setting with simulation data.
 841         FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
 842
 843         for (i=0; i<size; i++){
 844                 if (BitStream[i] == curPhase){
 845                         pskSimBit(carrier, &n, clk, &curPhase, false);
 846                 } else {
 847                         pskSimBit(carrier, &n, clk, &curPhase, true);
 848                 }
 849         }
 850         Dbprintf("Simulating with Carrier: %d, clk: %d, invert: %d, n: %d",carrier, clk, invert, n);
 851         //Dbprintf("DEBUG: First 32:");
 852         //uint8_t *dest = BigBuf_get_addr();
 853         //i=0;
 854         //Dbprintf("%d%d%d%d%d%d%d%d%d%d%d%d%d%d%d%d", dest[i],dest[i+1],dest[i+2],dest[i+3],dest[i+4],dest[i+5],dest[i+6],dest[i+7],dest[i+8],dest[i+9],dest[i+10],dest[i+11],dest[i+12],dest[i+13],dest[i+14],dest[i+15]);
 855         //i+=16;
 856         //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]);
 857
 858         if (ledcontrol) LED_A_ON();
 859         SimulateTagLowFrequency(n, 0, ledcontrol);
 860         if (ledcontrol) LED_A_OFF();
 861 }
 862
 863 // loop to get raw HID waveform then FSK demodulate the TAG ID from it
 864 void CmdHIDdemodFSK(int findone, int *high2, int *high, int *low, int ledcontrol)
 865 {
 866         uint8_t *dest = BigBuf_get_addr();
 867         //const size_t sizeOfBigBuff = BigBuf_max_traceLen();
 868         size_t size;
 869         uint32_t hi2=0, hi=0, lo=0;
 870         int idx=0;
 871         int dummyIdx = 0;
 872         // Configure to go in 125Khz listen mode
 873         LFSetupFPGAForADC(95, true);
 874
 875         //clear read buffer
 876         BigBuf_Clear_keep_EM();
 877
 878         while(!BUTTON_PRESS() && !usb_poll_validate_length()) {
 879                 WDT_HIT();
 880                 if (ledcontrol) LED_A_ON();
 881
 882                 DoAcquisition_default(-1,true);
 883                 // FSK demodulator
 884                 //size = sizeOfBigBuff;  //variable size will change after demod so re initialize it before use
 885                 size = 50*128*2; //big enough to catch 2 sequences of largest format
 886                 idx = HIDdemodFSK(dest, &size, &hi2, &hi, &lo, &dummyIdx);
 887
 888                 if (idx>0 && lo>0 && (size==96 || size==192)){
 889                         uint8_t bitlen = 0;
 890                         uint32_t fc = 0;
 891                         uint32_t cardnum = 0;
 892                         bool decoded = false;
 893
 894                         // go over previously decoded manchester data and decode into usable tag ID
 895                         if ((hi2 & 0x000FFFF) != 0){ //extra large HID tags  88/192 bits
 896                                 uint32_t bp = hi2 & 0x000FFFFF;
 897                                 bitlen = 63;
 898                                 while (bp > 0) {
 899                                         bp = bp >> 1;
 900                                         bitlen++;
 901                                 }
 902                         } else if ((hi >> 6) > 0) {
 903                                 uint32_t bp = hi;
 904                                 bitlen = 31;
 905                                 while (bp > 0) {
 906                                         bp = bp >> 1;
 907                                         bitlen++;
 908                                 }
 909                         } else if (((hi >> 5) & 1) == 0) {
 910                                 bitlen = 37;
 911                         } else if ((hi & 0x0000001F) > 0 ) {
 912                                 uint32_t bp = (hi & 0x0000001F);
 913                                 bitlen = 31;
 914                                 while (bp > 0) {
 915                                         bp = bp >> 1;
 916                                         bitlen++;
 917                                 }
 918                         } else {
 919                                 uint32_t bp = lo;
 920                                 bitlen = 0;
 921                                 while (bp > 0) {
 922                                         bp = bp >> 1;
 923                                         bitlen++;
 924                                 }
 925                         }
 926                         switch (bitlen){
 927                                 case 26:
 928                                         cardnum = (lo>>1)&0xFFFF;
 929                                         fc = (lo>>17)&0xFF;
 930                                         decoded = true;
 931                                         break;
 932                                 case 35:
 933                                         cardnum = (lo>>1)&0xFFFFF;
 934                                         fc = ((hi&1)<<11)|(lo>>21);
 935                                         decoded = true;
 936                                         break;
 937                         }
 938
 939                         if (hi2 != 0) //extra large HID tags  88/192 bits
 940                                 Dbprintf("TAG ID: %x%08x%08x (%d)",
 941                                         (unsigned int) hi2, (unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF);
 942                         else
 943                                 Dbprintf("TAG ID: %x%08x (%d)",
 944                                         (unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF);
 945
 946                         if (decoded)
 947                                 Dbprintf("Format Len: %dbits - FC: %d - Card: %d",
 948                                         (unsigned int) bitlen, (unsigned int) fc, (unsigned int) cardnum);
 949
 950                         if (findone){
 951                                 if (ledcontrol) LED_A_OFF();
 952                                 *high2 = hi2;
 953                                 *high = hi;
 954                                 *low = lo;
 955                                 break;
 956                         }
 957                         // reset
 958                 }
 959                 hi2 = hi = lo = idx = 0;
 960                 WDT_HIT();
 961         }
 962
 963         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 964         DbpString("Stopped");
 965         if (ledcontrol) LED_A_OFF();
 966 }
 967
 968 // loop to get raw HID waveform then FSK demodulate the TAG ID from it
 969 void CmdAWIDdemodFSK(int findone, int *high, int *low, int ledcontrol)
 970 {
 971         uint8_t *dest = BigBuf_get_addr();
 972         size_t size;
 973         int idx=0, dummyIdx=0;
 974         //clear read buffer
 975         BigBuf_Clear_keep_EM();
 976         // Configure to go in 125Khz listen mode
 977         LFSetupFPGAForADC(95, true);
 978
 979         while(!BUTTON_PRESS() && !usb_poll_validate_length()) {
 980
 981                 WDT_HIT();
 982                 if (ledcontrol) LED_A_ON();
 983
 984                 DoAcquisition_default(-1,true);
 985                 // FSK demodulator
 986                 size = 50*128*2; //big enough to catch 2 sequences of largest format
 987                 idx = AWIDdemodFSK(dest, &size, &dummyIdx);
 988
 989                 if (idx<=0 || size!=96) continue;
 990                 // Index map
 991                 // 0            10            20            30              40            50              60
 992                 // |            |             |             |               |             |               |
 993                 // 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
 994                 // -----------------------------------------------------------------------------
 995                 // 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
 996                 // 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
 997                 //          |---26 bit---|    |-----117----||-------------142-------------|
 998                 // b = format bit len, o = odd parity of last 3 bits
 999                 // f = facility code, c = card number
1000                 // w = wiegand parity
1001                 // (26 bit format shown)
1002
1003                 //get raw ID before removing parities
1004                 uint32_t rawLo = bytebits_to_byte(dest+idx+64,32);
1005                 uint32_t rawHi = bytebits_to_byte(dest+idx+32,32);
1006                 uint32_t rawHi2 = bytebits_to_byte(dest+idx,32);
1007
1008                 size = removeParity(dest, idx+8, 4, 1, 88);
1009                 if (size != 66) continue;
1010                 // ok valid card found!
1011
1012                 // Index map
1013                 // 0           10         20        30          40        50        60
1014                 // |           |          |         |           |         |         |
1015                 // 01234567 8 90123456 7890123456789012 3 456789012345678901234567890123456
1016                 // -----------------------------------------------------------------------------
1017                 // 00011010 1 01110101 0000000010001110 1 000000000000000000000000000000000
1018                 // bbbbbbbb w ffffffff cccccccccccccccc w xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
1019                 // |26 bit|   |-117--| |-----142------|
1020                 // b = format bit len, o = odd parity of last 3 bits
1021                 // f = facility code, c = card number
1022                 // w = wiegand parity
1023                 // (26 bit format shown)
1024
1025                 uint32_t fc = 0;
1026                 uint32_t cardnum = 0;
1027                 uint32_t code1 = 0;
1028                 uint32_t code2 = 0;
1029                 uint8_t fmtLen = bytebits_to_byte(dest,8);
1030                 if (fmtLen==26){
1031                         fc = bytebits_to_byte(dest+9, 8);
1032                         cardnum = bytebits_to_byte(dest+17, 16);
1033                         code1 = bytebits_to_byte(dest+8,fmtLen);
1034                         Dbprintf("AWID Found - BitLength: %d, FC: %d, Card: %d - Wiegand: %x, Raw: %08x%08x%08x", fmtLen, fc, cardnum, code1, rawHi2, rawHi, rawLo);
1035                 } else {
1036                         cardnum = bytebits_to_byte(dest+8+(fmtLen-17), 16);
1037                         if (fmtLen>32){
1038                                 code1 = bytebits_to_byte(dest+8,fmtLen-32);
1039                                 code2 = bytebits_to_byte(dest+8+(fmtLen-32),32);
1040                                 Dbprintf("AWID Found - BitLength: %d -unknown BitLength- (%d) - Wiegand: %x%08x, Raw: %08x%08x%08x", fmtLen, cardnum, code1, code2, rawHi2, rawHi, rawLo);
1041                         } else{
1042                                 code1 = bytebits_to_byte(dest+8,fmtLen);
1043                                 Dbprintf("AWID Found - BitLength: %d -unknown BitLength- (%d) - Wiegand: %x, Raw: %08x%08x%08x", fmtLen, cardnum, code1, rawHi2, rawHi, rawLo);
1044                         }
1045                 }
1046                 if (findone){
1047                         if (ledcontrol) LED_A_OFF();
1048                         break;
1049                 }
1050                 // reset
1051                 idx = 0;
1052                 WDT_HIT();
1053         }
1054         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1055         DbpString("Stopped");
1056         if (ledcontrol) LED_A_OFF();
1057 }
1058
1059 void CmdEM410xdemod(int findone, int *high, int *low, int ledcontrol)
1060 {
1061         uint8_t *dest = BigBuf_get_addr();
1062
1063         size_t size=0, idx=0;
1064         int clk=0, invert=0, errCnt=0, maxErr=20;
1065         uint32_t hi=0;
1066         uint64_t lo=0;
1067         //clear read buffer
1068         BigBuf_Clear_keep_EM();
1069         // Configure to go in 125Khz listen mode
1070         LFSetupFPGAForADC(95, true);
1071
1072         while(!BUTTON_PRESS() && !usb_poll_validate_length()) {
1073
1074                 WDT_HIT();
1075                 if (ledcontrol) LED_A_ON();
1076
1077                 DoAcquisition_default(-1,true);
1078                 size  = BigBuf_max_traceLen();
1079                 //askdemod and manchester decode
1080                 if (size > 16385) size = 16385; //big enough to catch 2 sequences of largest format
1081                 errCnt = askdemod(dest, &size, &clk, &invert, maxErr, 0, 1);
1082                 WDT_HIT();
1083
1084                 if (errCnt<0) continue;
1085
1086                 errCnt = Em410xDecode(dest, &size, &idx, &hi, &lo);
1087                 if (errCnt){
1088                         if (size>64){
1089                                 Dbprintf("EM XL TAG ID: %06x%08x%08x - (%05d_%03d_%08d)",
1090                                   hi,
1091                                   (uint32_t)(lo>>32),
1092                                   (uint32_t)lo,
1093                                   (uint32_t)(lo&0xFFFF),
1094                                   (uint32_t)((lo>>16LL) & 0xFF),
1095                                   (uint32_t)(lo & 0xFFFFFF));
1096                         } else {
1097                                 Dbprintf("EM TAG ID: %02x%08x - (%05d_%03d_%08d)",
1098                                   (uint32_t)(lo>>32),
1099                                   (uint32_t)lo,
1100                                   (uint32_t)(lo&0xFFFF),
1101                                   (uint32_t)((lo>>16LL) & 0xFF),
1102                                   (uint32_t)(lo & 0xFFFFFF));
1103                         }
1104
1105                         if (findone){
1106                                 if (ledcontrol) LED_A_OFF();
1107                                 *high=lo>>32;
1108                                 *low=lo & 0xFFFFFFFF;
1109                                 break;
1110                         }
1111                 }
1112                 WDT_HIT();
1113                 hi = lo = size = idx = 0;
1114                 clk = invert = errCnt = 0;
1115         }
1116         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1117         DbpString("Stopped");
1118         if (ledcontrol) LED_A_OFF();
1119 }
1120
1121 void CmdIOdemodFSK(int findone, int *high, int *low, int ledcontrol)
1122 {
1123         uint8_t *dest = BigBuf_get_addr();
1124         int idx=0;
1125         uint32_t code=0, code2=0;
1126         uint8_t version=0;
1127         uint8_t facilitycode=0;
1128         uint16_t number=0;
1129         int dummyIdx=0;
1130         //clear read buffer
1131         BigBuf_Clear_keep_EM();
1132         // Configure to go in 125Khz listen mode
1133         LFSetupFPGAForADC(95, true);
1134
1135         while(!BUTTON_PRESS() && !usb_poll_validate_length()) {
1136                 WDT_HIT();
1137                 if (ledcontrol) LED_A_ON();
1138                 DoAcquisition_default(-1,true);
1139                 //fskdemod and get start index
1140                 WDT_HIT();
1141                 idx = IOdemodFSK(dest, BigBuf_max_traceLen(), &dummyIdx);
1142                 if (idx<0) continue;
1143                 //valid tag found
1144
1145                 //Index map
1146                 //0           10          20          30          40          50          60
1147                 //|           |           |           |           |           |           |
1148                 //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23
1149                 //-----------------------------------------------------------------------------
1150                 //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11
1151                 //
1152                 //XSF(version)facility:codeone+codetwo
1153                 //Handle the data
1154                 if(findone){ //only print binary if we are doing one
1155                         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]);
1156                         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]);
1157                         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]);
1158                         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]);
1159                         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]);
1160                         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]);
1161                         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]);
1162                 }
1163                 code = bytebits_to_byte(dest+idx,32);
1164                 code2 = bytebits_to_byte(dest+idx+32,32);
1165                 version = bytebits_to_byte(dest+idx+27,8); //14,4
1166                 facilitycode = bytebits_to_byte(dest+idx+18,8);
1167                 number = (bytebits_to_byte(dest+idx+36,8)<<8)|(bytebits_to_byte(dest+idx+45,8)); //36,9
1168
1169                 Dbprintf("XSF(%02d)%02x:%05d (%08x%08x)",version,facilitycode,number,code,code2);
1170                 // if we're only looking for one tag
1171                 if (findone){
1172                         if (ledcontrol) LED_A_OFF();
1173                         //LED_A_OFF();
1174                         *high=code;
1175                         *low=code2;
1176                         break;
1177                 }
1178                 code=code2=0;
1179                 version=facilitycode=0;
1180                 number=0;
1181                 idx=0;
1182
1183                 WDT_HIT();
1184         }
1185         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1186         DbpString("Stopped");
1187         if (ledcontrol) LED_A_OFF();
1188 }
1189
1190 /*------------------------------
1191  * T5555/T5557/T5567/T5577 routines
1192  *------------------------------
1193  * NOTE: T55x7/T5555 configuration register definitions moved to protocols.h
1194  *
1195  * Relevant communication times in microsecond
1196  * To compensate antenna falling times shorten the write times
1197  * and enlarge the gap ones.
1198  * Q5 tags seems to have issues when these values changes.
1199  */
1200 #define START_GAP 31*8 // was 250 // SPEC:  1*8 to 50*8 - typ 15*8 (or 15fc)
1201 #define WRITE_GAP 20*8 // was 160 // SPEC:  1*8 to 20*8 - typ 10*8 (or 10fc)
1202 #define WRITE_0   18*8 // was 144 // SPEC: 16*8 to 32*8 - typ 24*8 (or 24fc)
1203 #define WRITE_1   50*8 // was 400 // SPEC: 48*8 to 64*8 - typ 56*8 (or 56fc)  432 for T55x7; 448 for E5550
1204 #define READ_GAP  15*8
1205
1206 void TurnReadLFOn(int delay) {
1207         FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
1208         // Give it a bit of time for the resonant antenna to settle.
1209         WaitUS(delay); //155*8 //50*8
1210 }
1211
1212 // Write one bit to card
1213 void T55xxWriteBit(int bit) {
1214         if (!bit)
1215                 TurnReadLFOn(WRITE_0);
1216         else
1217                 TurnReadLFOn(WRITE_1);
1218         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1219         WaitUS(WRITE_GAP);
1220 }
1221
1222 // Send T5577 reset command then read stream (see if we can identify the start of the stream)
1223 void T55xxResetRead(void) {
1224         LED_A_ON();
1225         //clear buffer now so it does not interfere with timing later
1226         BigBuf_Clear_keep_EM();
1227
1228         // Set up FPGA, 125kHz
1229         LFSetupFPGAForADC(95, true);
1230         StartTicks();
1231         // make sure tag is fully powered up...
1232         WaitMS(5);
1233
1234         // Trigger T55x7 in mode.
1235         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1236         WaitUS(START_GAP);
1237
1238         // reset tag - op code 00
1239         T55xxWriteBit(0);
1240         T55xxWriteBit(0);
1241
1242         TurnReadLFOn(READ_GAP);
1243
1244         // Acquisition
1245         DoPartialAcquisition(0, true, BigBuf_max_traceLen(), 0);
1246
1247         // Turn the field off
1248         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1249         cmd_send(CMD_ACK,0,0,0,0,0);
1250         LED_A_OFF();
1251 }
1252
1253 // Write one card block in page 0, no lock
1254 void T55xxWriteBlockExt(uint32_t Data, uint32_t Block, uint32_t Pwd, uint8_t arg) {
1255         LED_A_ON();
1256         bool PwdMode = arg & 0x1;
1257         uint8_t Page = (arg & 0x2)>>1;
1258         bool testMode = arg & 0x4;
1259         uint32_t i = 0;
1260
1261         // Set up FPGA, 125kHz
1262         LFSetupFPGAForADC(95, true);
1263         StartTicks();
1264         // make sure tag is fully powered up...
1265         WaitMS(5);
1266         // Trigger T55x7 in mode.
1267         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1268         WaitUS(START_GAP);
1269
1270         if (testMode) Dbprintf("TestMODE");
1271         // Std Opcode 10
1272         T55xxWriteBit(testMode ? 0 : 1);
1273         T55xxWriteBit(testMode ? 1 : Page); //Page 0
1274
1275         if (PwdMode) {
1276                 // Send Pwd
1277                 for (i = 0x80000000; i != 0; i >>= 1)
1278                         T55xxWriteBit(Pwd & i);
1279         }
1280         // Send Lock bit
1281         T55xxWriteBit(0);
1282
1283         // Send Data
1284         for (i = 0x80000000; i != 0; i >>= 1)
1285                 T55xxWriteBit(Data & i);
1286
1287         // Send Block number
1288         for (i = 0x04; i != 0; i >>= 1)
1289                 T55xxWriteBit(Block & i);
1290
1291         // Perform write (nominal is 5.6 ms for T55x7 and 18ms for E5550,
1292         // so wait a little more)
1293
1294         // "there is a clock delay before programming"
1295         //  - programming takes ~5.6ms for t5577 ~18ms for E5550 or t5567
1296         //  so we should wait 1 clock + 5.6ms then read response?
1297         //  but we need to know we are dealing with t5577 vs t5567 vs e5550 (or q5) marshmellow...
1298         if (testMode) {
1299                 //TESTMODE TIMING TESTS:
1300                 // <566us does nothing
1301                 // 566-568 switches between wiping to 0s and doing nothing
1302                 // 5184 wipes and allows 1 block to be programmed.
1303                 // indefinite power on wipes and then programs all blocks with bitshifted data sent.
1304                 TurnReadLFOn(5184);
1305
1306         } else {
1307                 TurnReadLFOn(20 * 1000);
1308                 //could attempt to do a read to confirm write took
1309                 // as the tag should repeat back the new block
1310                 // until it is reset, but to confirm it we would
1311                 // need to know the current block 0 config mode for
1312                 // modulation clock an other details to demod the response...
1313                 // response should be (for t55x7) a 0 bit then (ST if on)
1314                 // block data written in on repeat until reset.
1315
1316                 //DoPartialAcquisition(20, true, 12000);
1317         }
1318
1319         // turn field off
1320         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1321         LED_A_OFF();
1322 }
1323
1324 // Write one card block in page 0, no lock
1325 void T55xxWriteBlock(uint32_t Data, uint32_t Block, uint32_t Pwd, uint8_t arg) {
1326         T55xxWriteBlockExt(Data, Block, Pwd, arg);
1327         cmd_send(CMD_ACK,0,0,0,0,0);
1328 }
1329
1330 // Read one card block in page [page]
1331 void T55xxReadBlock(uint16_t arg0, uint8_t Block, uint32_t Pwd) {
1332         LED_A_ON();
1333         bool PwdMode = arg0 & 0x1;
1334         uint8_t Page = (arg0 & 0x2) >> 1;
1335         uint32_t i = 0;
1336         bool RegReadMode = (Block == 0xFF);//regular read mode
1337
1338         //clear buffer now so it does not interfere with timing later
1339         BigBuf_Clear_ext(false);
1340
1341         //make sure block is at max 7
1342         Block &= 0x7;
1343
1344         // Set up FPGA, 125kHz to power up the tag
1345         LFSetupFPGAForADC(95, true);
1346         StartTicks();
1347         // make sure tag is fully powered up...
1348         WaitMS(5);
1349         // Trigger T55x7 Direct Access Mode with start gap
1350         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1351         WaitUS(START_GAP);
1352
1353         // Opcode 1[page]
1354         T55xxWriteBit(1);
1355         T55xxWriteBit(Page); //Page 0
1356
1357         if (PwdMode){
1358                 // Send Pwd
1359                 for (i = 0x80000000; i != 0; i >>= 1)
1360                         T55xxWriteBit(Pwd & i);
1361         }
1362         // Send a zero bit separation
1363         T55xxWriteBit(0);
1364
1365         // Send Block number (if direct access mode)
1366         if (!RegReadMode)
1367                 for (i = 0x04; i != 0; i >>= 1)
1368                         T55xxWriteBit(Block & i);
1369
1370         // Turn field on to read the response
1371         // 137*8 seems to get to the start of data pretty well...
1372         //  but we want to go past the start and let the repeating data settle in...
1373         TurnReadLFOn(210*8);
1374
1375         // Acquisition
1376         // Now do the acquisition
1377         DoPartialAcquisition(0, true, 12000, 0);
1378
1379         // Turn the field off
1380         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1381         cmd_send(CMD_ACK,0,0,0,0,0);
1382         LED_A_OFF();
1383 }
1384
1385 void T55xxWakeUp(uint32_t Pwd){
1386         LED_B_ON();
1387         uint32_t i = 0;
1388
1389         // Set up FPGA, 125kHz
1390         LFSetupFPGAForADC(95, true);
1391         StartTicks();
1392         // make sure tag is fully powered up...
1393         WaitMS(5);
1394
1395         // Trigger T55x7 Direct Access Mode
1396         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1397         WaitUS(START_GAP);
1398
1399         // Opcode 10
1400         T55xxWriteBit(1);
1401         T55xxWriteBit(0); //Page 0
1402
1403         // Send Pwd
1404         for (i = 0x80000000; i != 0; i >>= 1)
1405                 T55xxWriteBit(Pwd & i);
1406
1407         // Turn and leave field on to let the begin repeating transmission
1408         TurnReadLFOn(20*1000);
1409 }
1410
1411 /*-------------- Cloning routines -----------*/
1412
1413 void WriteT55xx(uint32_t *blockdata, uint8_t startblock, uint8_t numblocks) {
1414         // write last block first and config block last (if included)
1415         for (uint8_t i = numblocks+startblock; i > startblock; i--) {
1416                 T55xxWriteBlockExt(blockdata[i-1],i-1,0,0);
1417         }
1418 }
1419
1420 // Copy HID id to card and setup block 0 config
1421 void CopyHIDtoT55x7(uint32_t hi2, uint32_t hi, uint32_t lo, uint8_t longFMT) {
1422         uint32_t data[] = {0,0,0,0,0,0,0};
1423         uint8_t last_block = 0;
1424
1425         if (longFMT) {
1426                 // Ensure no more than 84 bits supplied
1427                 if (hi2>0xFFFFF) {
1428                         DbpString("Tags can only have 84 bits.");
1429                         return;
1430                 }
1431                 // Build the 6 data blocks for supplied 84bit ID
1432                 last_block = 6;
1433                 // load preamble (1D) & long format identifier (9E manchester encoded)
1434                 data[1] = 0x1D96A900 | (manchesterEncode2Bytes((hi2 >> 16) & 0xF) & 0xFF);
1435                 // load raw id from hi2, hi, lo to data blocks (manchester encoded)
1436                 data[2] = manchesterEncode2Bytes(hi2 & 0xFFFF);
1437                 data[3] = manchesterEncode2Bytes(hi >> 16);
1438                 data[4] = manchesterEncode2Bytes(hi & 0xFFFF);
1439                 data[5] = manchesterEncode2Bytes(lo >> 16);
1440                 data[6] = manchesterEncode2Bytes(lo & 0xFFFF);
1441         }       else {
1442                 // Ensure no more than 44 bits supplied
1443                 if (hi>0xFFF) {
1444                         DbpString("Tags can only have 44 bits.");
1445                         return;
1446                 }
1447                 // Build the 3 data blocks for supplied 44bit ID
1448                 last_block = 3;
1449                 // load preamble
1450                 data[1] = 0x1D000000 | (manchesterEncode2Bytes(hi) & 0xFFFFFF);
1451                 data[2] = manchesterEncode2Bytes(lo >> 16);
1452                 data[3] = manchesterEncode2Bytes(lo & 0xFFFF);
1453         }
1454         // load chip config block
1455         data[0] = T55x7_BITRATE_RF_50 | T55x7_MODULATION_FSK2a | last_block << T55x7_MAXBLOCK_SHIFT;
1456
1457         //TODO add selection of chip for Q5 or T55x7
1458         // data[0] = (((50-2)/2)<<T5555_BITRATE_SHIFT) | T5555_MODULATION_FSK2 | T5555_INVERT_OUTPUT | last_block << T5555_MAXBLOCK_SHIFT;
1459
1460         LED_D_ON();
1461         // Program the data blocks for supplied ID
1462         // and the block 0 for HID format
1463         WriteT55xx(data, 0, last_block+1);
1464
1465         LED_D_OFF();
1466
1467         DbpString("DONE!");
1468 }
1469
1470 void CopyIOtoT55x7(uint32_t hi, uint32_t lo) {
1471         uint32_t data[] = {T55x7_BITRATE_RF_64 | T55x7_MODULATION_FSK2a | (2 << T55x7_MAXBLOCK_SHIFT), hi, lo};
1472         //TODO add selection of chip for Q5 or T55x7
1473         // data[0] = (((64-2)/2)<<T5555_BITRATE_SHIFT) | T5555_MODULATION_FSK2 | T5555_INVERT_OUTPUT | 2 << T5555_MAXBLOCK_SHIFT;
1474
1475         LED_D_ON();
1476         // Program the data blocks for supplied ID
1477         // and the block 0 config
1478         WriteT55xx(data, 0, 3);
1479
1480         LED_D_OFF();
1481
1482         DbpString("DONE!");
1483 }
1484
1485 // Clone Indala 64-bit tag by UID to T55x7
1486 void CopyIndala64toT55x7(uint32_t hi, uint32_t lo) {
1487         //Program the 2 data blocks for supplied 64bit UID
1488         // and the Config for Indala 64 format (RF/32;PSK1 with RF/2;Maxblock=2)
1489         uint32_t data[] = { T55x7_BITRATE_RF_32 | T55x7_MODULATION_PSK1 | (2 << T55x7_MAXBLOCK_SHIFT), hi, lo};
1490         //TODO add selection of chip for Q5 or T55x7
1491         // data[0] = (((32-2)/2)<<T5555_BITRATE_SHIFT) | T5555_MODULATION_PSK1 | 2 << T5555_MAXBLOCK_SHIFT;
1492
1493         WriteT55xx(data, 0, 3);
1494         //Alternative config for Indala (Extended mode;RF/32;PSK1 with RF/2;Maxblock=2;Inverse data)
1495         //      T5567WriteBlock(0x603E1042,0);
1496         DbpString("DONE!");
1497 }
1498 // Clone Indala 224-bit tag by UID to T55x7
1499 void CopyIndala224toT55x7(uint32_t uid1, uint32_t uid2, uint32_t uid3, uint32_t uid4, uint32_t uid5, uint32_t uid6, uint32_t uid7) {
1500         //Program the 7 data blocks for supplied 224bit UID
1501         uint32_t data[] = {0, uid1, uid2, uid3, uid4, uid5, uid6, uid7};
1502         // and the block 0 for Indala224 format
1503         //Config for Indala (RF/32;PSK2 with RF/2;Maxblock=7)
1504         data[0] = T55x7_BITRATE_RF_32 | T55x7_MODULATION_PSK2 | (7 << T55x7_MAXBLOCK_SHIFT);
1505         //TODO add selection of chip for Q5 or T55x7
1506         // data[0] = (((32-2)>>1)<<T5555_BITRATE_SHIFT) | T5555_MODULATION_PSK2 | 7 << T5555_MAXBLOCK_SHIFT;
1507         WriteT55xx(data, 0, 8);
1508         //Alternative config for Indala (Extended mode;RF/32;PSK1 with RF/2;Maxblock=7;Inverse data)
1509         //      T5567WriteBlock(0x603E10E2,0);
1510         DbpString("DONE!");
1511 }
1512 // clone viking tag to T55xx
1513 void CopyVikingtoT55xx(uint32_t block1, uint32_t block2, uint8_t Q5) {
1514         uint32_t data[] = {T55x7_BITRATE_RF_32 | T55x7_MODULATION_MANCHESTER | (2 << T55x7_MAXBLOCK_SHIFT), block1, block2};
1515         if (Q5) data[0] = T5555_SET_BITRATE(32) | T5555_MODULATION_MANCHESTER | 2 << T5555_MAXBLOCK_SHIFT;
1516         // Program the data blocks for supplied ID and the block 0 config
1517         WriteT55xx(data, 0, 3);
1518         LED_D_OFF();
1519         cmd_send(CMD_ACK,0,0,0,0,0);
1520 }
1521
1522 // Define 9bit header for EM410x tags
1523 #define EM410X_HEADER             0x1FF
1524 #define EM410X_ID_LENGTH        40
1525
1526 void WriteEM410x(uint32_t card, uint32_t id_hi, uint32_t id_lo) {
1527         int i, id_bit;
1528         uint64_t id = EM410X_HEADER;
1529         uint64_t rev_id = 0;    // reversed ID
1530         int c_parity[4];        // column parity
1531         int r_parity = 0;       // row parity
1532         uint32_t clock = 0;
1533
1534         // Reverse ID bits given as parameter (for simpler operations)
1535         for (i = 0; i < EM410X_ID_LENGTH; ++i) {
1536                 if (i < 32) {
1537                         rev_id = (rev_id << 1) | (id_lo & 1);
1538                         id_lo >>= 1;
1539                 } else {
1540                         rev_id = (rev_id << 1) | (id_hi & 1);
1541                         id_hi >>= 1;
1542                 }
1543         }
1544
1545         for (i = 0; i < EM410X_ID_LENGTH; ++i) {
1546                 id_bit = rev_id & 1;
1547
1548                 if (i % 4 == 0) {
1549                         // Don't write row parity bit at start of parsing
1550                         if (i)
1551                                 id = (id << 1) | r_parity;
1552                         // Start counting parity for new row
1553                         r_parity = id_bit;
1554                 } else {
1555                         // Count row parity
1556                         r_parity ^= id_bit;
1557                 }
1558
1559                 // First elements in column?
1560                 if (i < 4)
1561                         // Fill out first elements
1562                         c_parity[i] = id_bit;
1563                 else
1564                         // Count column parity
1565                         c_parity[i % 4] ^= id_bit;
1566
1567                 // Insert ID bit
1568                 id = (id << 1) | id_bit;
1569                 rev_id >>= 1;
1570         }
1571
1572         // Insert parity bit of last row
1573         id = (id << 1) | r_parity;
1574
1575         // Fill out column parity at the end of tag
1576         for (i = 0; i < 4; ++i)
1577                 id = (id << 1) | c_parity[i];
1578
1579         // Add stop bit
1580         id <<= 1;
1581
1582         Dbprintf("Started writing %s tag ...", card ? "T55x7":"T5555");
1583         LED_D_ON();
1584
1585         // Write EM410x ID
1586         uint32_t data[] = {0, (uint32_t)(id>>32), (uint32_t)(id & 0xFFFFFFFF)};
1587
1588         clock = (card & 0xFF00) >> 8;
1589         clock = (clock == 0) ? 64 : clock;
1590         Dbprintf("Clock rate: %d", clock);
1591         if (card & 0xFF) { //t55x7
1592                 clock = GetT55xxClockBit(clock);
1593                 if (clock == 0) {
1594                         Dbprintf("Invalid clock rate: %d", clock);
1595                         return;
1596                 }
1597                 data[0] = clock | T55x7_MODULATION_MANCHESTER | (2 << T55x7_MAXBLOCK_SHIFT);
1598         } else { //t5555 (Q5)
1599                 data[0] = T5555_SET_BITRATE(clock) | T5555_MODULATION_MANCHESTER | (2 << T5555_MAXBLOCK_SHIFT);
1600         }
1601
1602         WriteT55xx(data, 0, 3);
1603
1604         LED_D_OFF();
1605         Dbprintf("Tag %s written with 0x%08x%08x\n", card ? "T55x7":"T5555",
1606                          (uint32_t)(id >> 32), (uint32_t)id);
1607 }
1608
1609 //-----------------------------------
1610 // EM4469 / EM4305 routines
1611 //-----------------------------------
1612 #define FWD_CMD_LOGIN 0xC //including the even parity, binary mirrored
1613 #define FWD_CMD_WRITE 0xA
1614 #define FWD_CMD_READ 0x9
1615 #define FWD_CMD_DISABLE 0x5
1616
1617 uint8_t forwardLink_data[64]; //array of forwarded bits
1618 uint8_t * forward_ptr; //ptr for forward message preparation
1619 uint8_t fwd_bit_sz; //forwardlink bit counter
1620 uint8_t * fwd_write_ptr; //forwardlink bit pointer
1621
1622 //====================================================================
1623 // prepares command bits
1624 // see EM4469 spec
1625 //====================================================================
1626 //--------------------------------------------------------------------
1627 //  VALUES TAKEN FROM EM4x function: SendForward
1628 //  START_GAP = 440;       (55*8) cycles at 125Khz (8us = 1cycle)
1629 //  WRITE_GAP = 128;       (16*8)
1630 //  WRITE_1   = 256 32*8;  (32*8)
1631
1632 //  These timings work for 4469/4269/4305 (with the 55*8 above)
1633 //  WRITE_0 = 23*8 , 9*8  SpinDelayUs(23*8);
1634
1635 uint8_t Prepare_Cmd( uint8_t cmd ) {
1636
1637         *forward_ptr++ = 0; //start bit
1638         *forward_ptr++ = 0; //second pause for 4050 code
1639
1640         *forward_ptr++ = cmd;
1641         cmd >>= 1;
1642         *forward_ptr++ = cmd;
1643         cmd >>= 1;
1644         *forward_ptr++ = cmd;
1645         cmd >>= 1;
1646         *forward_ptr++ = cmd;
1647
1648         return 6; //return number of emited bits
1649 }
1650
1651 //====================================================================
1652 // prepares address bits
1653 // see EM4469 spec
1654 //====================================================================
1655 uint8_t Prepare_Addr( uint8_t addr ) {
1656
1657         register uint8_t line_parity;
1658
1659         uint8_t i;
1660         line_parity = 0;
1661         for(i=0;i<6;i++) {
1662                 *forward_ptr++ = addr;
1663                 line_parity ^= addr;
1664                 addr >>= 1;
1665         }
1666
1667         *forward_ptr++ = (line_parity & 1);
1668
1669         return 7; //return number of emited bits
1670 }
1671
1672 //====================================================================
1673 // prepares data bits intreleaved with parity bits
1674 // see EM4469 spec
1675 //====================================================================
1676 uint8_t Prepare_Data( uint16_t data_low, uint16_t data_hi) {
1677
1678         register uint8_t line_parity;
1679         register uint8_t column_parity;
1680         register uint8_t i, j;
1681         register uint16_t data;
1682
1683         data = data_low;
1684         column_parity = 0;
1685
1686         for(i=0; i<4; i++) {
1687                 line_parity = 0;
1688                 for(j=0; j<8; j++) {
1689                         line_parity ^= data;
1690                         column_parity ^= (data & 1) << j;
1691                         *forward_ptr++ = data;
1692                         data >>= 1;
1693                 }
1694                 *forward_ptr++ = line_parity;
1695                 if(i == 1)
1696                         data = data_hi;
1697         }
1698
1699         for(j=0; j<8; j++) {
1700                 *forward_ptr++ = column_parity;
1701                 column_parity >>= 1;
1702         }
1703         *forward_ptr = 0;
1704
1705         return 45; //return number of emited bits
1706 }
1707
1708 //====================================================================
1709 // Forward Link send function
1710 // Requires: forwarLink_data filled with valid bits (1 bit per byte)
1711 // fwd_bit_count set with number of bits to be sent
1712 //====================================================================
1713 void SendForward(uint8_t fwd_bit_count) {
1714
1715         fwd_write_ptr = forwardLink_data;
1716         fwd_bit_sz = fwd_bit_count;
1717
1718         // Set up FPGA, 125kHz or 95 divisor
1719         LFSetupFPGAForADC(95, true);
1720
1721         // force 1st mod pulse (start gap must be longer for 4305)
1722         fwd_bit_sz--; //prepare next bit modulation
1723         fwd_write_ptr++;
1724         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1725         WaitUS(55*8); //55 cycles off (8us each)for 4305  //another reader has 37 here...
1726         FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);//field on
1727         WaitUS(18*8); //18 cycles on (8us each)
1728
1729         // now start writting
1730         while(fwd_bit_sz-- > 0) { //prepare next bit modulation
1731                 if(((*fwd_write_ptr++) & 1) == 1)
1732                         WaitUS(32*8); //32 cycles at 125Khz (8us each)
1733                 else {
1734                         //These timings work for 4469/4269/4305 (with the 55*8 above)
1735                         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1736                         WaitUS(23*8); //23 cycles off (8us each)
1737                         FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);//field on
1738                         WaitUS(18*8); //18 cycles on (8us each)
1739                 }
1740         }
1741 }
1742
1743 void EM4xLogin(uint32_t Password) {
1744
1745         uint8_t fwd_bit_count;
1746
1747         forward_ptr = forwardLink_data;
1748         fwd_bit_count = Prepare_Cmd( FWD_CMD_LOGIN );
1749         fwd_bit_count += Prepare_Data( Password&0xFFFF, Password>>16 );
1750
1751         SendForward(fwd_bit_count);
1752
1753         //Wait for command to complete
1754         SpinDelay(20);
1755 }
1756
1757 void EM4xReadWord(uint8_t Address, uint32_t Pwd, uint8_t PwdMode) {
1758
1759         uint8_t fwd_bit_count;
1760
1761         // Clear destination buffer before sending the command
1762         BigBuf_Clear_ext(false);
1763
1764         LED_A_ON();
1765         StartTicks();
1766         //If password mode do login
1767         if (PwdMode == 1) EM4xLogin(Pwd);
1768
1769         forward_ptr = forwardLink_data;
1770         fwd_bit_count = Prepare_Cmd( FWD_CMD_READ );
1771         fwd_bit_count += Prepare_Addr( Address );
1772
1773         SendForward(fwd_bit_count);
1774         WaitUS(400);
1775         // Now do the acquisition
1776         DoPartialAcquisition(20, true, 6000, 1000);
1777
1778         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1779         LED_A_OFF();
1780         cmd_send(CMD_ACK,0,0,0,0,0);
1781 }
1782
1783 void EM4xWriteWord(uint32_t flag, uint32_t Data, uint32_t Pwd) {
1784
1785         bool PwdMode = (flag & 0xF);
1786         uint8_t Address = (flag >> 8) & 0xFF;
1787         uint8_t fwd_bit_count;
1788
1789         //clear buffer now so it does not interfere with timing later
1790         BigBuf_Clear_ext(false);
1791
1792         LED_A_ON();
1793         StartTicks();
1794         //If password mode do login
1795         if (PwdMode) EM4xLogin(Pwd);
1796
1797         forward_ptr = forwardLink_data;
1798         fwd_bit_count = Prepare_Cmd( FWD_CMD_WRITE );
1799         fwd_bit_count += Prepare_Addr( Address );
1800         fwd_bit_count += Prepare_Data( Data&0xFFFF, Data>>16 );
1801
1802         SendForward(fwd_bit_count);
1803
1804         //Wait for write to complete
1805         //SpinDelay(10);
1806
1807         WaitUS(6500);
1808         //Capture response if one exists
1809         DoPartialAcquisition(20, true, 6000, 1000);
1810
1811         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1812         LED_A_OFF();
1813         cmd_send(CMD_ACK,0,0,0,0,0);
1814 }
1815 /*
1816 Reading a COTAG.
1817
1818 COTAG needs the reader to send a startsequence and the card has an extreme slow datarate.
1819 because of this, we can "sample" the data signal but we interpreate it to Manchester direct.
1820
1821 READER START SEQUENCE:
1822 burst 800 us,    gap   2.2 msecs
1823 burst 3.6 msecs  gap   2.2 msecs
1824 burst 800 us     gap   2.2 msecs
1825 pulse 3.6 msecs
1826
1827 This triggers a COTAG tag to response
1828 */
1829 void Cotag(uint32_t arg0) {
1830
1831 #define OFF     { FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); WaitUS(2035); }
1832 #define ON(x)   { FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD); WaitUS((x)); }
1833
1834         uint8_t rawsignal = arg0 & 0xF;
1835
1836         LED_A_ON();
1837
1838         // Switching to LF image on FPGA. This might empty BigBuff
1839         FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
1840
1841         //clear buffer now so it does not interfere with timing later
1842         BigBuf_Clear_ext(false);
1843
1844         // Set up FPGA, 132kHz to power up the tag
1845         FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 89);
1846         FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
1847
1848         // Connect the A/D to the peak-detected low-frequency path.
1849         SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
1850
1851         // Now set up the SSC to get the ADC samples that are now streaming at us.
1852         FpgaSetupSsc(FPGA_MAJOR_MODE_LF_ADC);
1853
1854         // start clock - 1.5ticks is 1us
1855         StartTicks();
1856
1857         //send COTAG start pulse
1858         ON(740)  OFF
1859         ON(3330) OFF
1860         ON(740)  OFF
1861         ON(1000)
1862
1863         switch(rawsignal) {
1864                 case 0: doCotagAcquisition(50000); break;
1865                 case 1: doCotagAcquisitionManchester(); break;
1866                 case 2: DoAcquisition_config(true, 0); break;
1867         }
1868
1869         // Turn the field off
1870         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1871         cmd_send(CMD_ACK,0,0,0,0,0);
1872         LED_A_OFF();
1873 }