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, 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
 880                 WDT_HIT();
 881                 if (ledcontrol) LED_A_ON();
 882
 883                 DoAcquisition_default(-1,true);
 884                 // FSK demodulator
 885                 //size = sizeOfBigBuff;  //variable size will change after demod so re initialize it before use
 886                 size = 50*128*2; //big enough to catch 2 sequences of largest format
 887                 idx = HIDdemodFSK(dest, &size, &hi2, &hi, &lo, &dummyIdx);
 888
 889                 if (idx>0 && lo>0 && (size==96 || size==192)){
 890                         // go over previously decoded manchester data and decode into usable tag ID
 891                         if (hi2 != 0){ //extra large HID tags  88/192 bits
 892                                 Dbprintf("TAG ID: %x%08x%08x (%d)",
 893                                   (unsigned int) hi2, (unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF);
 894                         } else {  //standard HID tags 44/96 bits
 895                                 Dbprintf("TAG ID: %x%08x (%d)",(unsigned int) hi, (unsigned int) lo, (unsigned int) (lo>>1) & 0xFFFF); //old print cmd
 896                         }
 897                         if (findone){
 898                                 if (ledcontrol) LED_A_OFF();
 899                                 *high2 = hi2;
 900                                 *high = hi;
 901                                 *low = lo;
 902                                 break;
 903                         }
 904                         // reset
 905                 }
 906                 hi2 = hi = lo = idx = 0;
 907                 WDT_HIT();
 908         }
 909
 910         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 911         DbpString("Stopped");
 912         if (ledcontrol) LED_A_OFF();
 913 }
 914
 915 // loop to get raw HID waveform then FSK demodulate the TAG ID from it
 916 void CmdAWIDdemodFSK(int findone, int *high, int *low, int ledcontrol)
 917 {
 918         uint8_t *dest = BigBuf_get_addr();
 919         size_t size;
 920         int idx=0, dummyIdx=0;
 921         //clear read buffer
 922         BigBuf_Clear_keep_EM();
 923         // Configure to go in 125Khz listen mode
 924         LFSetupFPGAForADC(95, true);
 925
 926         while(!BUTTON_PRESS() && !usb_poll_validate_length()) {
 927
 928                 WDT_HIT();
 929                 if (ledcontrol) LED_A_ON();
 930
 931                 DoAcquisition_default(-1,true);
 932                 // FSK demodulator
 933                 size = 50*128*2; //big enough to catch 2 sequences of largest format
 934                 idx = AWIDdemodFSK(dest, &size, &dummyIdx);
 935
 936                 if (idx<=0 || size!=96) continue;
 937                 // Index map
 938                 // 0            10            20            30              40            50              60
 939                 // |            |             |             |               |             |               |
 940                 // 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
 941                 // -----------------------------------------------------------------------------
 942                 // 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
 943                 // 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
 944                 //          |---26 bit---|    |-----117----||-------------142-------------|
 945                 // b = format bit len, o = odd parity of last 3 bits
 946                 // f = facility code, c = card number
 947                 // w = wiegand parity
 948                 // (26 bit format shown)
 949
 950                 //get raw ID before removing parities
 951                 uint32_t rawLo = bytebits_to_byte(dest+idx+64,32);
 952                 uint32_t rawHi = bytebits_to_byte(dest+idx+32,32);
 953                 uint32_t rawHi2 = bytebits_to_byte(dest+idx,32);
 954
 955                 size = removeParity(dest, idx+8, 4, 1, 88);
 956                 if (size != 66) continue;
 957                 // ok valid card found!
 958
 959                 // Index map
 960                 // 0           10         20        30          40        50        60
 961                 // |           |          |         |           |         |         |
 962                 // 01234567 8 90123456 7890123456789012 3 456789012345678901234567890123456
 963                 // -----------------------------------------------------------------------------
 964                 // 00011010 1 01110101 0000000010001110 1 000000000000000000000000000000000
 965                 // bbbbbbbb w ffffffff cccccccccccccccc w xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
 966                 // |26 bit|   |-117--| |-----142------|
 967                 // b = format bit len, o = odd parity of last 3 bits
 968                 // f = facility code, c = card number
 969                 // w = wiegand parity
 970                 // (26 bit format shown)
 971
 972                 uint32_t fc = 0;
 973                 uint32_t cardnum = 0;
 974                 uint32_t code1 = 0;
 975                 uint32_t code2 = 0;
 976                 uint8_t fmtLen = bytebits_to_byte(dest,8);
 977                 if (fmtLen==26){
 978                         fc = bytebits_to_byte(dest+9, 8);
 979                         cardnum = bytebits_to_byte(dest+17, 16);
 980                         code1 = bytebits_to_byte(dest+8,fmtLen);
 981                         Dbprintf("AWID Found - BitLength: %d, FC: %d, Card: %d - Wiegand: %x, Raw: %08x%08x%08x", fmtLen, fc, cardnum, code1, rawHi2, rawHi, rawLo);
 982                 } else {
 983                         cardnum = bytebits_to_byte(dest+8+(fmtLen-17), 16);
 984                         if (fmtLen>32){
 985                                 code1 = bytebits_to_byte(dest+8,fmtLen-32);
 986                                 code2 = bytebits_to_byte(dest+8+(fmtLen-32),32);
 987                                 Dbprintf("AWID Found - BitLength: %d -unknown BitLength- (%d) - Wiegand: %x%08x, Raw: %08x%08x%08x", fmtLen, cardnum, code1, code2, rawHi2, rawHi, rawLo);
 988                         } else{
 989                                 code1 = bytebits_to_byte(dest+8,fmtLen);
 990                                 Dbprintf("AWID Found - BitLength: %d -unknown BitLength- (%d) - Wiegand: %x, Raw: %08x%08x%08x", fmtLen, cardnum, code1, rawHi2, rawHi, rawLo);
 991                         }
 992                 }
 993                 if (findone){
 994                         if (ledcontrol) LED_A_OFF();
 995                         break;
 996                 }
 997                 // reset
 998                 idx = 0;
 999                 WDT_HIT();
1000         }
1001         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1002         DbpString("Stopped");
1003         if (ledcontrol) LED_A_OFF();
1004 }
1005
1006 void CmdEM410xdemod(int findone, int *high, int *low, int ledcontrol)
1007 {
1008         uint8_t *dest = BigBuf_get_addr();
1009
1010         size_t size=0, idx=0;
1011         int clk=0, invert=0, errCnt=0, maxErr=20;
1012         uint32_t hi=0;
1013         uint64_t lo=0;
1014         //clear read buffer
1015         BigBuf_Clear_keep_EM();
1016         // Configure to go in 125Khz listen mode
1017         LFSetupFPGAForADC(95, true);
1018
1019         while(!BUTTON_PRESS() && !usb_poll_validate_length()) {
1020
1021                 WDT_HIT();
1022                 if (ledcontrol) LED_A_ON();
1023
1024                 DoAcquisition_default(-1,true);
1025                 size  = BigBuf_max_traceLen();
1026                 //askdemod and manchester decode
1027                 if (size > 16385) size = 16385; //big enough to catch 2 sequences of largest format
1028                 errCnt = askdemod(dest, &size, &clk, &invert, maxErr, 0, 1);
1029                 WDT_HIT();
1030
1031                 if (errCnt<0) continue;
1032
1033                 errCnt = Em410xDecode(dest, &size, &idx, &hi, &lo);
1034                 if (errCnt){
1035                         if (size>64){
1036                                 Dbprintf("EM XL TAG ID: %06x%08x%08x - (%05d_%03d_%08d)",
1037                                   hi,
1038                                   (uint32_t)(lo>>32),
1039                                   (uint32_t)lo,
1040                                   (uint32_t)(lo&0xFFFF),
1041                                   (uint32_t)((lo>>16LL) & 0xFF),
1042                                   (uint32_t)(lo & 0xFFFFFF));
1043                         } else {
1044                                 Dbprintf("EM TAG ID: %02x%08x - (%05d_%03d_%08d)",
1045                                   (uint32_t)(lo>>32),
1046                                   (uint32_t)lo,
1047                                   (uint32_t)(lo&0xFFFF),
1048                                   (uint32_t)((lo>>16LL) & 0xFF),
1049                                   (uint32_t)(lo & 0xFFFFFF));
1050                         }
1051
1052                         if (findone){
1053                                 if (ledcontrol) LED_A_OFF();
1054                                 *high=lo>>32;
1055                                 *low=lo & 0xFFFFFFFF;
1056                                 break;
1057                         }
1058                 }
1059                 WDT_HIT();
1060                 hi = lo = size = idx = 0;
1061                 clk = invert = errCnt = 0;
1062         }
1063         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1064         DbpString("Stopped");
1065         if (ledcontrol) LED_A_OFF();
1066 }
1067
1068 void CmdIOdemodFSK(int findone, int *high, int *low, int ledcontrol)
1069 {
1070         uint8_t *dest = BigBuf_get_addr();
1071         int idx=0;
1072         uint32_t code=0, code2=0;
1073         uint8_t version=0;
1074         uint8_t facilitycode=0;
1075         uint16_t number=0;
1076         int dummyIdx=0;
1077         //clear read buffer
1078         BigBuf_Clear_keep_EM();
1079         // Configure to go in 125Khz listen mode
1080         LFSetupFPGAForADC(95, true);
1081
1082         while(!BUTTON_PRESS() && !usb_poll_validate_length()) {
1083                 WDT_HIT();
1084                 if (ledcontrol) LED_A_ON();
1085                 DoAcquisition_default(-1,true);
1086                 //fskdemod and get start index
1087                 WDT_HIT();
1088                 idx = IOdemodFSK(dest, BigBuf_max_traceLen(), &dummyIdx);
1089                 if (idx<0) continue;
1090                 //valid tag found
1091
1092                 //Index map
1093                 //0           10          20          30          40          50          60
1094                 //|           |           |           |           |           |           |
1095                 //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23
1096                 //-----------------------------------------------------------------------------
1097                 //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11
1098                 //
1099                 //XSF(version)facility:codeone+codetwo
1100                 //Handle the data
1101                 if(findone){ //only print binary if we are doing one
1102                         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]);
1103                         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]);
1104                         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]);
1105                         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]);
1106                         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]);
1107                         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]);
1108                         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]);
1109                 }
1110                 code = bytebits_to_byte(dest+idx,32);
1111                 code2 = bytebits_to_byte(dest+idx+32,32);
1112                 version = bytebits_to_byte(dest+idx+27,8); //14,4
1113                 facilitycode = bytebits_to_byte(dest+idx+18,8);
1114                 number = (bytebits_to_byte(dest+idx+36,8)<<8)|(bytebits_to_byte(dest+idx+45,8)); //36,9
1115
1116                 Dbprintf("XSF(%02d)%02x:%05d (%08x%08x)",version,facilitycode,number,code,code2);
1117                 // if we're only looking for one tag
1118                 if (findone){
1119                         if (ledcontrol) LED_A_OFF();
1120                         //LED_A_OFF();
1121                         *high=code;
1122                         *low=code2;
1123                         break;
1124                 }
1125                 code=code2=0;
1126                 version=facilitycode=0;
1127                 number=0;
1128                 idx=0;
1129
1130                 WDT_HIT();
1131         }
1132         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1133         DbpString("Stopped");
1134         if (ledcontrol) LED_A_OFF();
1135 }
1136
1137 /*------------------------------
1138  * T5555/T5557/T5567/T5577 routines
1139  *------------------------------
1140  * NOTE: T55x7/T5555 configuration register definitions moved to protocols.h
1141  *
1142  * Relevant communication times in microsecond
1143  * To compensate antenna falling times shorten the write times
1144  * and enlarge the gap ones.
1145  * Q5 tags seems to have issues when these values changes.
1146  */
1147 #define START_GAP 31*8 // was 250 // SPEC:  1*8 to 50*8 - typ 15*8 (or 15fc)
1148 #define WRITE_GAP 20*8 // was 160 // SPEC:  1*8 to 20*8 - typ 10*8 (or 10fc)
1149 #define WRITE_0   18*8 // was 144 // SPEC: 16*8 to 32*8 - typ 24*8 (or 24fc)
1150 #define WRITE_1   50*8 // was 400 // SPEC: 48*8 to 64*8 - typ 56*8 (or 56fc)  432 for T55x7; 448 for E5550
1151 #define READ_GAP  15*8
1152
1153 void TurnReadLFOn(int delay) {
1154         FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
1155         // Give it a bit of time for the resonant antenna to settle.
1156         WaitUS(delay); //155*8 //50*8
1157 }
1158
1159 // Write one bit to card
1160 void T55xxWriteBit(int bit) {
1161         if (!bit)
1162                 TurnReadLFOn(WRITE_0);
1163         else
1164                 TurnReadLFOn(WRITE_1);
1165         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1166         WaitUS(WRITE_GAP);
1167 }
1168
1169 // Send T5577 reset command then read stream (see if we can identify the start of the stream)
1170 void T55xxResetRead(void) {
1171         LED_A_ON();
1172         //clear buffer now so it does not interfere with timing later
1173         BigBuf_Clear_keep_EM();
1174
1175         // Set up FPGA, 125kHz
1176         LFSetupFPGAForADC(95, true);
1177         StartTicks();
1178         // make sure tag is fully powered up...
1179         WaitMS(5);
1180
1181         // Trigger T55x7 in mode.
1182         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1183         WaitUS(START_GAP);
1184
1185         // reset tag - op code 00
1186         T55xxWriteBit(0);
1187         T55xxWriteBit(0);
1188
1189         TurnReadLFOn(READ_GAP);
1190
1191         // Acquisition
1192         DoPartialAcquisition(0, true, BigBuf_max_traceLen(), 0);
1193
1194         // Turn the field off
1195         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1196         cmd_send(CMD_ACK,0,0,0,0,0);
1197         LED_A_OFF();
1198 }
1199
1200 // Write one card block in page 0, no lock
1201 void T55xxWriteBlockExt(uint32_t Data, uint32_t Block, uint32_t Pwd, uint8_t arg) {
1202         LED_A_ON();
1203         bool PwdMode = arg & 0x1;
1204         uint8_t Page = (arg & 0x2)>>1;
1205         bool testMode = arg & 0x4;
1206         uint32_t i = 0;
1207
1208         // Set up FPGA, 125kHz
1209         LFSetupFPGAForADC(95, true);
1210         StartTicks();
1211         // make sure tag is fully powered up...
1212         WaitMS(5);
1213         // Trigger T55x7 in mode.
1214         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1215         WaitUS(START_GAP);
1216
1217         if (testMode) Dbprintf("TestMODE");
1218         // Std Opcode 10
1219         T55xxWriteBit(testMode ? 0 : 1);
1220         T55xxWriteBit(testMode ? 1 : Page); //Page 0
1221
1222         if (PwdMode) {
1223                 // Send Pwd
1224                 for (i = 0x80000000; i != 0; i >>= 1)
1225                         T55xxWriteBit(Pwd & i);
1226         }
1227         // Send Lock bit
1228         T55xxWriteBit(0);
1229
1230         // Send Data
1231         for (i = 0x80000000; i != 0; i >>= 1)
1232                 T55xxWriteBit(Data & i);
1233
1234         // Send Block number
1235         for (i = 0x04; i != 0; i >>= 1)
1236                 T55xxWriteBit(Block & i);
1237
1238         // Perform write (nominal is 5.6 ms for T55x7 and 18ms for E5550,
1239         // so wait a little more)
1240
1241         // "there is a clock delay before programming"
1242         //  - programming takes ~5.6ms for t5577 ~18ms for E5550 or t5567
1243         //  so we should wait 1 clock + 5.6ms then read response?
1244         //  but we need to know we are dealing with t5577 vs t5567 vs e5550 (or q5) marshmellow...
1245         if (testMode) {
1246                 //TESTMODE TIMING TESTS:
1247                 // <566us does nothing
1248                 // 566-568 switches between wiping to 0s and doing nothing
1249                 // 5184 wipes and allows 1 block to be programmed.
1250                 // indefinite power on wipes and then programs all blocks with bitshifted data sent.
1251                 TurnReadLFOn(5184);
1252
1253         } else {
1254                 TurnReadLFOn(20 * 1000);
1255                 //could attempt to do a read to confirm write took
1256                 // as the tag should repeat back the new block
1257                 // until it is reset, but to confirm it we would
1258                 // need to know the current block 0 config mode for
1259                 // modulation clock an other details to demod the response...
1260                 // response should be (for t55x7) a 0 bit then (ST if on)
1261                 // block data written in on repeat until reset.
1262
1263                 //DoPartialAcquisition(20, true, 12000);
1264         }
1265
1266         // turn field off
1267         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1268         LED_A_OFF();
1269 }
1270
1271 // Write one card block in page 0, no lock
1272 void T55xxWriteBlock(uint32_t Data, uint32_t Block, uint32_t Pwd, uint8_t arg) {
1273         T55xxWriteBlockExt(Data, Block, Pwd, arg);
1274         cmd_send(CMD_ACK,0,0,0,0,0);
1275 }
1276
1277 // Read one card block in page [page]
1278 void T55xxReadBlock(uint16_t arg0, uint8_t Block, uint32_t Pwd) {
1279         LED_A_ON();
1280         bool PwdMode = arg0 & 0x1;
1281         uint8_t Page = (arg0 & 0x2) >> 1;
1282         uint32_t i = 0;
1283         bool RegReadMode = (Block == 0xFF);//regular read mode
1284
1285         //clear buffer now so it does not interfere with timing later
1286         BigBuf_Clear_ext(false);
1287
1288         //make sure block is at max 7
1289         Block &= 0x7;
1290
1291         // Set up FPGA, 125kHz to power up the tag
1292         LFSetupFPGAForADC(95, true);
1293         StartTicks();
1294         // make sure tag is fully powered up...
1295         WaitMS(5);
1296         // Trigger T55x7 Direct Access Mode with start gap
1297         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1298         WaitUS(START_GAP);
1299
1300         // Opcode 1[page]
1301         T55xxWriteBit(1);
1302         T55xxWriteBit(Page); //Page 0
1303
1304         if (PwdMode){
1305                 // Send Pwd
1306                 for (i = 0x80000000; i != 0; i >>= 1)
1307                         T55xxWriteBit(Pwd & i);
1308         }
1309         // Send a zero bit separation
1310         T55xxWriteBit(0);
1311
1312         // Send Block number (if direct access mode)
1313         if (!RegReadMode)
1314                 for (i = 0x04; i != 0; i >>= 1)
1315                         T55xxWriteBit(Block & i);
1316
1317         // Turn field on to read the response
1318         // 137*8 seems to get to the start of data pretty well...
1319         //  but we want to go past the start and let the repeating data settle in...
1320         TurnReadLFOn(210*8);
1321
1322         // Acquisition
1323         // Now do the acquisition
1324         DoPartialAcquisition(0, true, 12000, 0);
1325
1326         // Turn the field off
1327         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1328         cmd_send(CMD_ACK,0,0,0,0,0);
1329         LED_A_OFF();
1330 }
1331
1332 void T55xxWakeUp(uint32_t Pwd){
1333         LED_B_ON();
1334         uint32_t i = 0;
1335
1336         // Set up FPGA, 125kHz
1337         LFSetupFPGAForADC(95, true);
1338         StartTicks();
1339         // make sure tag is fully powered up...
1340         WaitMS(5);
1341
1342         // Trigger T55x7 Direct Access Mode
1343         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1344         WaitUS(START_GAP);
1345
1346         // Opcode 10
1347         T55xxWriteBit(1);
1348         T55xxWriteBit(0); //Page 0
1349
1350         // Send Pwd
1351         for (i = 0x80000000; i != 0; i >>= 1)
1352                 T55xxWriteBit(Pwd & i);
1353
1354         // Turn and leave field on to let the begin repeating transmission
1355         TurnReadLFOn(20*1000);
1356 }
1357
1358 /*-------------- Cloning routines -----------*/
1359
1360 void WriteT55xx(uint32_t *blockdata, uint8_t startblock, uint8_t numblocks) {
1361         // write last block first and config block last (if included)
1362         for (uint8_t i = numblocks+startblock; i > startblock; i--) {
1363                 T55xxWriteBlockExt(blockdata[i-1],i-1,0,0);
1364         }
1365 }
1366
1367 // Copy HID id to card and setup block 0 config
1368 void CopyHIDtoT55x7(uint32_t hi2, uint32_t hi, uint32_t lo, uint8_t longFMT) {
1369         uint32_t data[] = {0,0,0,0,0,0,0};
1370         uint8_t last_block = 0;
1371
1372         if (longFMT) {
1373                 // Ensure no more than 84 bits supplied
1374                 if (hi2>0xFFFFF) {
1375                         DbpString("Tags can only have 84 bits.");
1376                         return;
1377                 }
1378                 // Build the 6 data blocks for supplied 84bit ID
1379                 last_block = 6;
1380                 // load preamble (1D) & long format identifier (9E manchester encoded)
1381                 data[1] = 0x1D96A900 | (manchesterEncode2Bytes((hi2 >> 16) & 0xF) & 0xFF);
1382                 // load raw id from hi2, hi, lo to data blocks (manchester encoded)
1383                 data[2] = manchesterEncode2Bytes(hi2 & 0xFFFF);
1384                 data[3] = manchesterEncode2Bytes(hi >> 16);
1385                 data[4] = manchesterEncode2Bytes(hi & 0xFFFF);
1386                 data[5] = manchesterEncode2Bytes(lo >> 16);
1387                 data[6] = manchesterEncode2Bytes(lo & 0xFFFF);
1388         }       else {
1389                 // Ensure no more than 44 bits supplied
1390                 if (hi>0xFFF) {
1391                         DbpString("Tags can only have 44 bits.");
1392                         return;
1393                 }
1394                 // Build the 3 data blocks for supplied 44bit ID
1395                 last_block = 3;
1396                 // load preamble
1397                 data[1] = 0x1D000000 | (manchesterEncode2Bytes(hi) & 0xFFFFFF);
1398                 data[2] = manchesterEncode2Bytes(lo >> 16);
1399                 data[3] = manchesterEncode2Bytes(lo & 0xFFFF);
1400         }
1401         // load chip config block
1402         data[0] = T55x7_BITRATE_RF_50 | T55x7_MODULATION_FSK2a | last_block << T55x7_MAXBLOCK_SHIFT;
1403
1404         //TODO add selection of chip for Q5 or T55x7
1405         // data[0] = (((50-2)/2)<<T5555_BITRATE_SHIFT) | T5555_MODULATION_FSK2 | T5555_INVERT_OUTPUT | last_block << T5555_MAXBLOCK_SHIFT;
1406
1407         LED_D_ON();
1408         // Program the data blocks for supplied ID
1409         // and the block 0 for HID format
1410         WriteT55xx(data, 0, last_block+1);
1411
1412         LED_D_OFF();
1413
1414         DbpString("DONE!");
1415 }
1416
1417 void CopyIOtoT55x7(uint32_t hi, uint32_t lo) {
1418         uint32_t data[] = {T55x7_BITRATE_RF_64 | T55x7_MODULATION_FSK2a | (2 << T55x7_MAXBLOCK_SHIFT), hi, lo};
1419         //TODO add selection of chip for Q5 or T55x7
1420         // data[0] = (((64-2)/2)<<T5555_BITRATE_SHIFT) | T5555_MODULATION_FSK2 | T5555_INVERT_OUTPUT | 2 << T5555_MAXBLOCK_SHIFT;
1421
1422         LED_D_ON();
1423         // Program the data blocks for supplied ID
1424         // and the block 0 config
1425         WriteT55xx(data, 0, 3);
1426
1427         LED_D_OFF();
1428
1429         DbpString("DONE!");
1430 }
1431
1432 // Clone Indala 64-bit tag by UID to T55x7
1433 void CopyIndala64toT55x7(uint32_t hi, uint32_t lo) {
1434         //Program the 2 data blocks for supplied 64bit UID
1435         // and the Config for Indala 64 format (RF/32;PSK1 with RF/2;Maxblock=2)
1436         uint32_t data[] = { T55x7_BITRATE_RF_32 | T55x7_MODULATION_PSK1 | (2 << T55x7_MAXBLOCK_SHIFT), hi, lo};
1437         //TODO add selection of chip for Q5 or T55x7
1438         // data[0] = (((32-2)/2)<<T5555_BITRATE_SHIFT) | T5555_MODULATION_PSK1 | 2 << T5555_MAXBLOCK_SHIFT;
1439
1440         WriteT55xx(data, 0, 3);
1441         //Alternative config for Indala (Extended mode;RF/32;PSK1 with RF/2;Maxblock=2;Inverse data)
1442         //      T5567WriteBlock(0x603E1042,0);
1443         DbpString("DONE!");
1444 }
1445 // Clone Indala 224-bit tag by UID to T55x7
1446 void CopyIndala224toT55x7(uint32_t uid1, uint32_t uid2, uint32_t uid3, uint32_t uid4, uint32_t uid5, uint32_t uid6, uint32_t uid7) {
1447         //Program the 7 data blocks for supplied 224bit UID
1448         uint32_t data[] = {0, uid1, uid2, uid3, uid4, uid5, uid6, uid7};
1449         // and the block 0 for Indala224 format
1450         //Config for Indala (RF/32;PSK2 with RF/2;Maxblock=7)
1451         data[0] = T55x7_BITRATE_RF_32 | T55x7_MODULATION_PSK2 | (7 << T55x7_MAXBLOCK_SHIFT);
1452         //TODO add selection of chip for Q5 or T55x7
1453         // data[0] = (((32-2)>>1)<<T5555_BITRATE_SHIFT) | T5555_MODULATION_PSK2 | 7 << T5555_MAXBLOCK_SHIFT;
1454         WriteT55xx(data, 0, 8);
1455         //Alternative config for Indala (Extended mode;RF/32;PSK1 with RF/2;Maxblock=7;Inverse data)
1456         //      T5567WriteBlock(0x603E10E2,0);
1457         DbpString("DONE!");
1458 }
1459 // clone viking tag to T55xx
1460 void CopyVikingtoT55xx(uint32_t block1, uint32_t block2, uint8_t Q5) {
1461         uint32_t data[] = {T55x7_BITRATE_RF_32 | T55x7_MODULATION_MANCHESTER | (2 << T55x7_MAXBLOCK_SHIFT), block1, block2};
1462         if (Q5) data[0] = T5555_SET_BITRATE(32) | T5555_MODULATION_MANCHESTER | 2 << T5555_MAXBLOCK_SHIFT;
1463         // Program the data blocks for supplied ID and the block 0 config
1464         WriteT55xx(data, 0, 3);
1465         LED_D_OFF();
1466         cmd_send(CMD_ACK,0,0,0,0,0);
1467 }
1468
1469 // Define 9bit header for EM410x tags
1470 #define EM410X_HEADER             0x1FF
1471 #define EM410X_ID_LENGTH        40
1472
1473 void WriteEM410x(uint32_t card, uint32_t id_hi, uint32_t id_lo) {
1474         int i, id_bit;
1475         uint64_t id = EM410X_HEADER;
1476         uint64_t rev_id = 0;    // reversed ID
1477         int c_parity[4];        // column parity
1478         int r_parity = 0;       // row parity
1479         uint32_t clock = 0;
1480
1481         // Reverse ID bits given as parameter (for simpler operations)
1482         for (i = 0; i < EM410X_ID_LENGTH; ++i) {
1483                 if (i < 32) {
1484                         rev_id = (rev_id << 1) | (id_lo & 1);
1485                         id_lo >>= 1;
1486                 } else {
1487                         rev_id = (rev_id << 1) | (id_hi & 1);
1488                         id_hi >>= 1;
1489                 }
1490         }
1491
1492         for (i = 0; i < EM410X_ID_LENGTH; ++i) {
1493                 id_bit = rev_id & 1;
1494
1495                 if (i % 4 == 0) {
1496                         // Don't write row parity bit at start of parsing
1497                         if (i)
1498                                 id = (id << 1) | r_parity;
1499                         // Start counting parity for new row
1500                         r_parity = id_bit;
1501                 } else {
1502                         // Count row parity
1503                         r_parity ^= id_bit;
1504                 }
1505
1506                 // First elements in column?
1507                 if (i < 4)
1508                         // Fill out first elements
1509                         c_parity[i] = id_bit;
1510                 else
1511                         // Count column parity
1512                         c_parity[i % 4] ^= id_bit;
1513
1514                 // Insert ID bit
1515                 id = (id << 1) | id_bit;
1516                 rev_id >>= 1;
1517         }
1518
1519         // Insert parity bit of last row
1520         id = (id << 1) | r_parity;
1521
1522         // Fill out column parity at the end of tag
1523         for (i = 0; i < 4; ++i)
1524                 id = (id << 1) | c_parity[i];
1525
1526         // Add stop bit
1527         id <<= 1;
1528
1529         Dbprintf("Started writing %s tag ...", card ? "T55x7":"T5555");
1530         LED_D_ON();
1531
1532         // Write EM410x ID
1533         uint32_t data[] = {0, (uint32_t)(id>>32), (uint32_t)(id & 0xFFFFFFFF)};
1534
1535         clock = (card & 0xFF00) >> 8;
1536         clock = (clock == 0) ? 64 : clock;
1537         Dbprintf("Clock rate: %d", clock);
1538         if (card & 0xFF) { //t55x7
1539                 clock = GetT55xxClockBit(clock);
1540                 if (clock == 0) {
1541                         Dbprintf("Invalid clock rate: %d", clock);
1542                         return;
1543                 }
1544                 data[0] = clock | T55x7_MODULATION_MANCHESTER | (2 << T55x7_MAXBLOCK_SHIFT);
1545         } else { //t5555 (Q5)
1546                 data[0] = T5555_SET_BITRATE(clock) | T5555_MODULATION_MANCHESTER | (2 << T5555_MAXBLOCK_SHIFT);
1547         }
1548
1549         WriteT55xx(data, 0, 3);
1550
1551         LED_D_OFF();
1552         Dbprintf("Tag %s written with 0x%08x%08x\n", card ? "T55x7":"T5555",
1553                          (uint32_t)(id >> 32), (uint32_t)id);
1554 }
1555
1556 //-----------------------------------
1557 // EM4469 / EM4305 routines
1558 //-----------------------------------
1559 #define FWD_CMD_LOGIN 0xC //including the even parity, binary mirrored
1560 #define FWD_CMD_WRITE 0xA
1561 #define FWD_CMD_READ 0x9
1562 #define FWD_CMD_DISABLE 0x5
1563
1564 uint8_t forwardLink_data[64]; //array of forwarded bits
1565 uint8_t * forward_ptr; //ptr for forward message preparation
1566 uint8_t fwd_bit_sz; //forwardlink bit counter
1567 uint8_t * fwd_write_ptr; //forwardlink bit pointer
1568
1569 //====================================================================
1570 // prepares command bits
1571 // see EM4469 spec
1572 //====================================================================
1573 //--------------------------------------------------------------------
1574 //  VALUES TAKEN FROM EM4x function: SendForward
1575 //  START_GAP = 440;       (55*8) cycles at 125Khz (8us = 1cycle)
1576 //  WRITE_GAP = 128;       (16*8)
1577 //  WRITE_1   = 256 32*8;  (32*8)
1578
1579 //  These timings work for 4469/4269/4305 (with the 55*8 above)
1580 //  WRITE_0 = 23*8 , 9*8  SpinDelayUs(23*8);
1581
1582 uint8_t Prepare_Cmd( uint8_t cmd ) {
1583
1584         *forward_ptr++ = 0; //start bit
1585         *forward_ptr++ = 0; //second pause for 4050 code
1586
1587         *forward_ptr++ = cmd;
1588         cmd >>= 1;
1589         *forward_ptr++ = cmd;
1590         cmd >>= 1;
1591         *forward_ptr++ = cmd;
1592         cmd >>= 1;
1593         *forward_ptr++ = cmd;
1594
1595         return 6; //return number of emited bits
1596 }
1597
1598 //====================================================================
1599 // prepares address bits
1600 // see EM4469 spec
1601 //====================================================================
1602 uint8_t Prepare_Addr( uint8_t addr ) {
1603
1604         register uint8_t line_parity;
1605
1606         uint8_t i;
1607         line_parity = 0;
1608         for(i=0;i<6;i++) {
1609                 *forward_ptr++ = addr;
1610                 line_parity ^= addr;
1611                 addr >>= 1;
1612         }
1613
1614         *forward_ptr++ = (line_parity & 1);
1615
1616         return 7; //return number of emited bits
1617 }
1618
1619 //====================================================================
1620 // prepares data bits intreleaved with parity bits
1621 // see EM4469 spec
1622 //====================================================================
1623 uint8_t Prepare_Data( uint16_t data_low, uint16_t data_hi) {
1624
1625         register uint8_t line_parity;
1626         register uint8_t column_parity;
1627         register uint8_t i, j;
1628         register uint16_t data;
1629
1630         data = data_low;
1631         column_parity = 0;
1632
1633         for(i=0; i<4; i++) {
1634                 line_parity = 0;
1635                 for(j=0; j<8; j++) {
1636                         line_parity ^= data;
1637                         column_parity ^= (data & 1) << j;
1638                         *forward_ptr++ = data;
1639                         data >>= 1;
1640                 }
1641                 *forward_ptr++ = line_parity;
1642                 if(i == 1)
1643                         data = data_hi;
1644         }
1645
1646         for(j=0; j<8; j++) {
1647                 *forward_ptr++ = column_parity;
1648                 column_parity >>= 1;
1649         }
1650         *forward_ptr = 0;
1651
1652         return 45; //return number of emited bits
1653 }
1654
1655 //====================================================================
1656 // Forward Link send function
1657 // Requires: forwarLink_data filled with valid bits (1 bit per byte)
1658 // fwd_bit_count set with number of bits to be sent
1659 //====================================================================
1660 void SendForward(uint8_t fwd_bit_count) {
1661
1662         fwd_write_ptr = forwardLink_data;
1663         fwd_bit_sz = fwd_bit_count;
1664
1665         // Set up FPGA, 125kHz or 95 divisor
1666         LFSetupFPGAForADC(95, true);
1667
1668         // force 1st mod pulse (start gap must be longer for 4305)
1669         fwd_bit_sz--; //prepare next bit modulation
1670         fwd_write_ptr++;
1671         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1672         WaitUS(55*8); //55 cycles off (8us each)for 4305  //another reader has 37 here...
1673         FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);//field on
1674         WaitUS(18*8); //18 cycles on (8us each)
1675
1676         // now start writting
1677         while(fwd_bit_sz-- > 0) { //prepare next bit modulation
1678                 if(((*fwd_write_ptr++) & 1) == 1)
1679                         WaitUS(32*8); //32 cycles at 125Khz (8us each)
1680                 else {
1681                         //These timings work for 4469/4269/4305 (with the 55*8 above)
1682                         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1683                         WaitUS(23*8); //23 cycles off (8us each)
1684                         FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);//field on
1685                         WaitUS(18*8); //18 cycles on (8us each)
1686                 }
1687         }
1688 }
1689
1690 void EM4xLogin(uint32_t Password) {
1691
1692         uint8_t fwd_bit_count;
1693
1694         forward_ptr = forwardLink_data;
1695         fwd_bit_count = Prepare_Cmd( FWD_CMD_LOGIN );
1696         fwd_bit_count += Prepare_Data( Password&0xFFFF, Password>>16 );
1697
1698         SendForward(fwd_bit_count);
1699
1700         //Wait for command to complete
1701         SpinDelay(20);
1702 }
1703
1704 void EM4xReadWord(uint8_t Address, uint32_t Pwd, uint8_t PwdMode) {
1705
1706         uint8_t fwd_bit_count;
1707
1708         // Clear destination buffer before sending the command
1709         BigBuf_Clear_ext(false);
1710
1711         LED_A_ON();
1712         StartTicks();
1713         //If password mode do login
1714         if (PwdMode == 1) EM4xLogin(Pwd);
1715
1716         forward_ptr = forwardLink_data;
1717         fwd_bit_count = Prepare_Cmd( FWD_CMD_READ );
1718         fwd_bit_count += Prepare_Addr( Address );
1719
1720         SendForward(fwd_bit_count);
1721         WaitUS(400);
1722         // Now do the acquisition
1723         DoPartialAcquisition(20, true, 6000, 1000);
1724
1725         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1726         LED_A_OFF();
1727         cmd_send(CMD_ACK,0,0,0,0,0);
1728 }
1729
1730 void EM4xWriteWord(uint32_t flag, uint32_t Data, uint32_t Pwd) {
1731
1732         bool PwdMode = (flag & 0xF);
1733         uint8_t Address = (flag >> 8) & 0xFF;
1734         uint8_t fwd_bit_count;
1735
1736         //clear buffer now so it does not interfere with timing later
1737         BigBuf_Clear_ext(false);
1738
1739         LED_A_ON();
1740         StartTicks();
1741         //If password mode do login
1742         if (PwdMode) EM4xLogin(Pwd);
1743
1744         forward_ptr = forwardLink_data;
1745         fwd_bit_count = Prepare_Cmd( FWD_CMD_WRITE );
1746         fwd_bit_count += Prepare_Addr( Address );
1747         fwd_bit_count += Prepare_Data( Data&0xFFFF, Data>>16 );
1748
1749         SendForward(fwd_bit_count);
1750
1751         //Wait for write to complete
1752         //SpinDelay(10);
1753
1754         WaitUS(6500);
1755         //Capture response if one exists
1756         DoPartialAcquisition(20, true, 6000, 1000);
1757
1758         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1759         LED_A_OFF();
1760         cmd_send(CMD_ACK,0,0,0,0,0);
1761 }
1762 /*
1763 Reading a COTAG.
1764
1765 COTAG needs the reader to send a startsequence and the card has an extreme slow datarate.
1766 because of this, we can "sample" the data signal but we interpreate it to Manchester direct.
1767
1768 READER START SEQUENCE:
1769 burst 800 us,    gap   2.2 msecs
1770 burst 3.6 msecs  gap   2.2 msecs
1771 burst 800 us     gap   2.2 msecs
1772 pulse 3.6 msecs
1773
1774 This triggers a COTAG tag to response
1775 */
1776 void Cotag(uint32_t arg0) {
1777
1778 #define OFF     { FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); WaitUS(2035); }
1779 #define ON(x)   { FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD); WaitUS((x)); }
1780
1781         uint8_t rawsignal = arg0 & 0xF;
1782
1783         LED_A_ON();
1784
1785         // Switching to LF image on FPGA. This might empty BigBuff
1786         FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
1787
1788         //clear buffer now so it does not interfere with timing later
1789         BigBuf_Clear_ext(false);
1790
1791         // Set up FPGA, 132kHz to power up the tag
1792         FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 89);
1793         FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
1794
1795         // Connect the A/D to the peak-detected low-frequency path.
1796         SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
1797
1798         // Now set up the SSC to get the ADC samples that are now streaming at us.
1799         FpgaSetupSsc();
1800
1801         // start clock - 1.5ticks is 1us
1802         StartTicks();
1803
1804         //send COTAG start pulse
1805         ON(740)  OFF
1806         ON(3330) OFF
1807         ON(740)  OFF
1808         ON(1000)
1809
1810         switch(rawsignal) {
1811                 case 0: doCotagAcquisition(50000); break;
1812                 case 1: doCotagAcquisitionManchester(); break;
1813                 case 2: DoAcquisition_config(true, 0); break;
1814         }
1815
1816         // Turn the field off
1817         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF); // field off
1818         cmd_send(CMD_ACK,0,0,0,0,0);
1819         LED_A_OFF();
1820 }