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