armsrc/hitag2.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 // Hitag2 emulation
   7 //
   8 // (c) 2009 Henryk Plötz <henryk@ploetzli.ch>
   9 //-----------------------------------------------------------------------------
  10 // Hitag2 complete rewrite of the code
  11 // - Fixed modulation/encoding issues
  12 // - Rewrote code for transponder emulation
  13 // - Added sniffing of transponder communication
  14 // - Added reader functionality
  15 //
  16 // (c) 2012 Roel Verdult
  17 //-----------------------------------------------------------------------------
  18 // Piwi, 2019
  19 // Iceman, 2019
  20 // Anon, 2019
  21 // Doegox, 2020
  22
  23 #define DBG  if (DBGLEVEL >= DBG_EXTENDED)
  24
  25 #include "hitag2.h"
  26 #include "hitag2_crypto.h"
  27 #include "string.h"
  28 #include "proxmark3_arm.h"
  29 #include "cmd.h"
  30 #include "BigBuf.h"
  31 #include "fpgaloader.h"
  32 #include "ticks.h"
  33 #include "dbprint.h"
  34 #include "util.h"
  35 #include "lfadc.h"
  36 #include "lfsampling.h"
  37 #include "lfdemod.h"
  38 #include "commonutil.h"
  39 #include "appmain.h"
  40
  41 #define test_bit(data, i)  (*(data + (i/8)) >> (7-(i % 8))) & 1
  42 #define set_bit(data, i)   *(data + (i/8)) |= (1 << (7-(i % 8)))
  43 #define clear_bit(data, i) *(data + (i/8)) &= ~(1 << (7-(i % 8)))
  44 #define flip_bit(data, i)  *(data + (i/8)) ^= (1 << (7-(i % 8)))
  45
  46 // Successful crypto auth
  47 static bool bCrypto;
  48 // Is in auth stage
  49 static bool bAuthenticating;
  50 // Successful password auth
  51 static bool bSelecting;
  52 static bool bCollision;
  53 static bool bPwd;
  54 static bool bSuccessful;
  55
  56 /*
  57 Password Mode : 0x06 - 0000 0110
  58 Crypto Mode   : 0x0E - 0000 1110
  59 Public Mode A : 0x02 - 0000 0010
  60 Public Mode B : 0x00 - 0000 0000
  61 Public Mode C : 0x04 - 0000 0100
  62 */
  63
  64 static struct hitag2_tag tag = {
  65     .state = TAG_STATE_RESET,
  66     .sectors = {                          // Password mode:               | Crypto mode:
  67         [0]  = { 0x02, 0x4e, 0x02, 0x20}, // UID                          | UID
  68         [1]  = { 0x4d, 0x49, 0x4b, 0x52}, // Password RWD                 | 32 bit LSB key
  69         [2]  = { 0x20, 0xf0, 0x4f, 0x4e}, // Reserved                     | 16 bit MSB key, 16 bit reserved
  70         [3]  = { 0x06, 0xaa, 0x48, 0x54}, // Configuration, password TAG  | Configuration, password TAG
  71         [4]  = { 0x46, 0x5f, 0x4f, 0x4b}, // Data: F_OK
  72         [5]  = { 0x55, 0x55, 0x55, 0x55}, // Data: UUUU
  73         [6]  = { 0xaa, 0xaa, 0xaa, 0xaa}, // Data: ....
  74         [7]  = { 0x55, 0x55, 0x55, 0x55}, // Data: UUUU
  75         [8]  = { 0x00, 0x00, 0x00, 0x00}, // RSK Low
  76         [9]  = { 0x00, 0x00, 0x00, 0x00}, // RSK High
  77         [10] = { 0x00, 0x00, 0x00, 0x00}, // RCF
  78         [11] = { 0x00, 0x00, 0x00, 0x00}, // SYNC
  79         // up to index 15 reserved for HITAG1/HITAGS public data
  80     },
  81 };
  82
  83 static enum {
  84     WRITE_STATE_START = 0x0,
  85     WRITE_STATE_PAGENUM_WRITTEN,
  86     WRITE_STATE_PROG
  87 } writestate;
  88
  89 // ToDo: define a meaningful maximum size for auth_table. The bigger this is, the lower will be the available memory for traces.
  90 // Historically it used to be FREE_BUFFER_SIZE, which was 2744.
  91 #define AUTH_TABLE_LENGTH 2744
  92 static uint8_t *auth_table;
  93 static size_t auth_table_pos = 0;
  94 static size_t auth_table_len = AUTH_TABLE_LENGTH;
  95
  96 static uint8_t password[4];
  97 static uint8_t NrAr[8];
  98 static uint8_t key[8];
  99 static uint8_t writedata[4];
 100 static uint8_t logdata_0[4], logdata_1[4];
 101 static uint8_t nonce[4];
 102 static uint8_t key_no;
 103 static uint64_t cipher_state;
 104
 105 static int16_t blocknr;
 106 static size_t flipped_bit = 0;
 107 static uint32_t byte_value = 0;
 108
 109 static void hitag2_reset(void) {
 110     tag.state = TAG_STATE_RESET;
 111     tag.crypto_active = 0;
 112 }
 113
 114 static void hitag2_init(void) {
 115     hitag2_reset();
 116 }
 117
 118 // Sam7s has several timers, we will use the source TIMER_CLOCK1 (aka AT91C_TC_CLKS_TIMER_DIV1_CLOCK)
 119 // TIMER_CLOCK1 = MCK/2, MCK is running at 48 MHz, Timer is running at 48/2 = 24 MHz
 120 // Hitag units (T0) have duration of 8 microseconds (us), which is 1/125000 per second (carrier)
 121 // T0 = TIMER_CLOCK1 / 125000 = 192
 122 #ifndef HITAG_T0
 123 #define HITAG_T0               192
 124 #endif
 125
 126 #define HITAG_FRAME_LEN  20
 127 #define HITAG_T_STOP     36 /* T_EOF should be > 36 */
 128 #define HITAG_T_LOW      8  /* T_LOW should be 4..10 */
 129 #define HITAG_T_0_MIN    15 /* T[0] should be 18..22 */
 130 #define HITAG_T_0        20 /* T[0] should be 18..22 */
 131 #define HITAG_T_1_MIN    25 /* T[1] should be 26..30 */
 132 #define HITAG_T_1        30 /* T[1] should be 26..30 */
 133 #define HITAG_T_EOF      80 /* T_EOF should be > 36    and must be larger than HITAG_T_TAG_CAPTURE_FOUR_HALF */
 134 #define HITAG_T_WAIT_1_MIN   199 /* T_wresp should be 199..206 */
 135 #define HITAG_T_WAIT_2_MIN   90 /* T_wait2 should be at least 90 */
 136 #define HITAG_T_WAIT_MAX 300 /* bit more than HITAG_T_WAIT_1 + HITAG_T_WAIT_2 */
 137 #define HITAG_T_PROG     614
 138 #define HITAG_T_WAIT_POWERUP   313 /* transponder internal powerup time is 312.5 */
 139 #define HITAG_T_WAIT_START_AUTH_MAX   232 /* transponder waiting time to receive the START_AUTH command is 232.5, then it enters public mode */
 140
 141 #define HITAG_T_TAG_ONE_HALF_PERIOD     10
 142 #define HITAG_T_TAG_TWO_HALF_PERIOD     25
 143 #define HITAG_T_TAG_THREE_HALF_PERIOD   41
 144 #define HITAG_T_TAG_FOUR_HALF_PERIOD    57
 145
 146 #define HITAG_T_TAG_HALF_PERIOD         16
 147 #define HITAG_T_TAG_FULL_PERIOD         32
 148
 149 #define HITAG_T_TAG_CAPTURE_ONE_HALF    13
 150 #define HITAG_T_TAG_CAPTURE_TWO_HALF    25
 151 #define HITAG_T_TAG_CAPTURE_THREE_HALF  41
 152 #define HITAG_T_TAG_CAPTURE_FOUR_HALF   57
 153
 154 /*
 155 // sim
 156 static void hitag_send_bit(int bit) {
 157     LED_A_ON();
 158
 159     // Reset clock for the next bit
 160     AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG;
 161
 162     // Fixed modulation, earlier proxmark version used inverted signal
 163     // check datasheet if reader uses BiPhase?
 164     if (bit == 0) {
 165         // Manchester: Unloaded, then loaded |__--|
 166         LOW(GPIO_SSC_DOUT);
 167         while (AT91C_BASE_TC0->TC_CV < HITAG_T0 * HITAG_T_TAG_HALF_PERIOD);
 168         HIGH(GPIO_SSC_DOUT);
 169         while (AT91C_BASE_TC0->TC_CV < HITAG_T0 * HITAG_T_TAG_FULL_PERIOD);
 170     } else {
 171         // Manchester: Loaded, then unloaded |--__|
 172         HIGH(GPIO_SSC_DOUT);
 173         while (AT91C_BASE_TC0->TC_CV < HITAG_T0 * HITAG_T_TAG_HALF_PERIOD);
 174         LOW(GPIO_SSC_DOUT);
 175         while (AT91C_BASE_TC0->TC_CV < HITAG_T0 * HITAG_T_TAG_FULL_PERIOD);
 176     }
 177     LED_A_OFF();
 178 }
 179
 180 // sim
 181 static void hitag_send_frame(const uint8_t *frame, size_t frame_len) {
 182     // SOF - send start of frame
 183     hitag_send_bit(1);
 184     hitag_send_bit(1);
 185     hitag_send_bit(1);
 186     hitag_send_bit(1);
 187     hitag_send_bit(1);
 188
 189     // Send the content of the frame
 190     for (size_t i = 0; i < frame_len; i++) {
 191         hitag_send_bit((frame[i / 8] >> (7 - (i % 8))) & 1);
 192     }
 193
 194     // Drop the modulation
 195     LOW(GPIO_SSC_DOUT);
 196 }
 197 */
 198
 199 // sim
 200 static void hitag2_handle_reader_command(uint8_t *rx, const size_t rxlen, uint8_t *tx, size_t *txlen) {
 201     uint8_t rx_air[HITAG_FRAME_LEN];
 202
 203     // Copy the (original) received frame how it is send over the air
 204     memcpy(rx_air, rx, nbytes(rxlen));
 205
 206     if (tag.crypto_active) {
 207         hitag2_cipher_transcrypt(&(tag.cs), rx, rxlen / 8, rxlen % 8);
 208     }
 209
 210     // Reset the transmission frame length
 211     *txlen = 0;
 212
 213     // Try to find out which command was send by selecting on length (in bits)
 214     switch (rxlen) {
 215         // Received 11000 from the reader, request for UID, send UID
 216         case 05: {
 217             // Always send over the air in the clear plaintext mode
 218             if (rx_air[0] != 0xC0) {
 219                 // Unknown frame ?
 220                 return;
 221             }
 222             *txlen = 32;
 223             memcpy(tx, tag.sectors[0], 4);
 224             tag.crypto_active = 0;
 225         }
 226         break;
 227
 228         // Read/Write command: ..xx x..y  yy with yyy == ~xxx, xxx is sector number
 229         case 10: {
 230             uint16_t sector = (~(((rx[0] << 2) & 0x04) | ((rx[1] >> 6) & 0x03)) & 0x07);
 231
 232             // Verify complement of sector index
 233             if (sector != ((rx[0] >> 3) & 0x07)) {
 234                 DbpString("Transmission error (read/write)");
 235                 return;
 236             }
 237
 238             switch (rx[0] & 0xC6) {
 239                 // Read command: 11xx x00y
 240                 case 0xC0: {
 241                     memcpy(tx, tag.sectors[sector], 4);
 242                     *txlen = 32;
 243                     break;
 244                 }
 245                 // Inverted Read command: 01xx x10y
 246                 case 0x44: {
 247                     for (size_t i = 0; i < 4; i++) {
 248                         tx[i] = tag.sectors[sector][i] ^ 0xff;
 249                     }
 250                     *txlen = 32;
 251                     break;
 252                 }
 253                 // Write command: 10xx x01y
 254                 case 0x82: {
 255                     // Prepare write, acknowledge by repeating command
 256                     memcpy(tx, rx, nbytes(rxlen));
 257                     *txlen = rxlen;
 258                     tag.active_sector = sector;
 259                     tag.state = TAG_STATE_WRITING;
 260                     break;
 261                 }
 262                 // Unknown command
 263                 default: {
 264                     Dbprintf("Unknown command: %02x %02x", rx[0], rx[1]);
 265                     return;
 266                 }
 267             }
 268         }
 269         break;
 270
 271         // Writing data or Reader password
 272         case 32: {
 273             if (tag.state == TAG_STATE_WRITING) {
 274                 // These are the sector contents to be written. We don't have to do anything else.
 275                 memcpy(tag.sectors[tag.active_sector], rx, nbytes(rxlen));
 276                 tag.state = TAG_STATE_RESET;
 277                 return;
 278             } else {
 279                 // Received RWD password, respond with configuration and our password
 280                 if (memcmp(rx, tag.sectors[1], 4) != 0) {
 281                     DbpString("Reader password is wrong");
 282                     return;
 283                 }
 284                 *txlen = 32;
 285                 memcpy(tx, tag.sectors[3], 4);
 286             }
 287         }
 288         break;
 289
 290         // Received RWD authentication challenge and respnse
 291         case 64: {
 292             // Store the authentication attempt
 293             if (auth_table_len < (AUTH_TABLE_LENGTH - 8)) {
 294                 memcpy(auth_table + auth_table_len, rx, 8);
 295                 auth_table_len += 8;
 296             }
 297
 298             // Reset the cipher state
 299             hitag2_cipher_reset(&tag, rx);
 300
 301             // Check if the authentication was correct
 302             if (!hitag2_cipher_authenticate(&(tag.cs), rx + 4)) {
 303                 // The reader failed to authenticate, do nothing
 304                 Dbprintf("auth: %02x%02x%02x%02x%02x%02x%02x%02x Failed!", rx[0], rx[1], rx[2], rx[3], rx[4], rx[5], rx[6], rx[7]);
 305                 return;
 306             }
 307             // Activate encryption algorithm for all further communication
 308             tag.crypto_active = 1;
 309
 310             // Use the tag password as response
 311             memcpy(tx, tag.sectors[3], 4);
 312             *txlen = 32;
 313         }
 314         break;
 315     }
 316
 317     // LogTrace(rx, nbytes(rxlen), 0, 0, NULL, false);
 318     // LogTrace(tx, nbytes(txlen), 0, 0, NULL, true);
 319
 320     if (tag.crypto_active) {
 321         hitag2_cipher_transcrypt(&(tag.cs), tx, *txlen / 8, *txlen % 8);
 322     }
 323 }
 324
 325 // reader/writer
 326 // returns how long it took
 327 static uint32_t hitag_reader_send_bit(int bit) {
 328     uint32_t wait = 0;
 329     LED_A_ON();
 330     // Binary pulse length modulation (BPLM) is used to encode the data stream
 331     // This means that a transmission of a one takes longer than that of a zero
 332
 333     // Enable modulation, which means, drop the field
 334     lf_modulation(true);
 335
 336     // Wait for 4-10 times the carrier period
 337     lf_wait_periods(8); // wait for 4-10 times the carrier period
 338     wait += 8;
 339
 340     // Disable modulation, just activates the field again
 341     lf_modulation(false);
 342
 343     if (bit == 0) {
 344         // Zero bit: |_-|
 345         lf_wait_periods(HITAG_T_0 - HITAG_T_LOW); // wait for 18-22 times the carrier period
 346         wait += HITAG_T_0 - HITAG_T_LOW;
 347     } else {
 348         // One bit: |_--|
 349         lf_wait_periods(HITAG_T_1 - HITAG_T_LOW); // wait for 26-32 times the carrier period
 350         wait += HITAG_T_1 - HITAG_T_LOW;
 351     }
 352
 353     LED_A_OFF();
 354     return wait;
 355 }
 356
 357 // reader / writer commands
 358 static uint32_t hitag_reader_send_frame(const uint8_t *frame, size_t frame_len) {
 359
 360     uint32_t wait = 0;
 361     // Send the content of the frame
 362     for (size_t i = 0; i < frame_len; i++) {
 363         wait += hitag_reader_send_bit((frame[i / 8] >> (7 - (i % 8))) & 1);
 364     }
 365
 366     // Enable modulation, which means, drop the field
 367     lf_modulation(true);
 368
 369     // Wait for 4-10 times the carrier period
 370     lf_wait_periods(HITAG_T_LOW);
 371     wait += HITAG_T_LOW;
 372
 373     // Disable modulation, just activates the field again
 374     lf_modulation(false);
 375
 376     // t_stop, high field for stop condition (> 36)
 377     lf_wait_periods(HITAG_T_STOP);
 378     wait += HITAG_T_STOP;
 379
 380     return wait;
 381 }
 382
 383 static uint8_t hitag_crc(uint8_t *data, size_t n) {
 384     uint8_t crc = 0xFF;
 385     for (size_t i = 0; i < ((n + 7) / 8); i++) {
 386         crc ^= *(data + i);
 387         uint8_t bit = n < (8 * (i + 1)) ? (n % 8) : 8;
 388         while (bit--) {
 389             if (crc & 0x80) {
 390                 crc <<= 1;
 391                 crc ^= 0x1D;
 392             } else {
 393                 crc <<= 1;
 394             }
 395         }
 396     }
 397     return crc;
 398 }
 399
 400 /*
 401 void fix_ac_decoding(uint8_t *input, size_t len) {
 402     // Reader routine tries to decode AC data after Manchester decoding
 403     // AC has double the bitrate, extract data from bit-pairs
 404     uint8_t temp[len / 16];
 405     memset(temp, 0, sizeof(temp));
 406
 407     for (size_t i = 1; i < len; i += 2) {
 408         if (test_bit(input, i) && test_bit(input, (i + 1))) {
 409             set_bit(temp, (i / 2));
 410         }
 411     }
 412     memcpy(input, temp, sizeof(temp));
 413 }
 414 */
 415
 416
 417 // looks at number of received bits.
 418 // 0 = collision?
 419 // 32 =  good response
 420 static bool hitag_plain(uint8_t *rx, const size_t rxlen, uint8_t *tx, size_t *txlen, bool hitag_s) {
 421     *txlen = 0;
 422     switch (rxlen) {
 423         case 0: {
 424             // retry waking up card
 425             /*tx[0] = 0xb0; // Rev 3.0*/
 426             tx[0] = 0x30; // Rev 2.0
 427             *txlen = 5;
 428             if (!bCollision) blocknr--;
 429             if (blocknr < 0) {
 430                 blocknr = 0;
 431             }
 432             if (!hitag_s) {
 433                 if (blocknr > 1 && blocknr < 31) {
 434                     blocknr = 31;
 435                 }
 436             }
 437             bCollision = true;
 438             return true;
 439         }
 440         case 32: {
 441             uint8_t crc;
 442             if (bCollision) {
 443                 // Select card by serial from response
 444                 tx[0] = 0x00 | rx[0] >> 5;
 445                 tx[1] = rx[0] << 3 | rx[1] >> 5;
 446                 tx[2] = rx[1] << 3 | rx[2] >> 5;
 447                 tx[3] = rx[2] << 3 | rx[3] >> 5;
 448                 tx[4] = rx[3] << 3;
 449                 crc = hitag_crc(tx, 37);
 450                 tx[4] |= crc >> 5;
 451                 tx[5] = crc << 3;
 452                 *txlen = 45;
 453                 bCollision = false;
 454             } else {
 455                 memcpy(tag.sectors[blocknr], rx, 4);
 456                 blocknr++;
 457                 if (!hitag_s) {
 458                     if (blocknr > 1 && blocknr < 31) {
 459                         blocknr = 31;
 460                     }
 461                 }
 462                 if (blocknr > 63) {
 463                     DbpString("Read succesful!");
 464                     *txlen = 0;
 465                     bSuccessful = true;
 466                     return false;
 467                 }
 468                 // read next page of card until done
 469                 Dbprintf("Reading page %02u", blocknr);
 470                 tx[0] = 0xc0 | blocknr >> 4; // RDPPAGE
 471                 tx[1] = blocknr << 4;
 472                 crc = hitag_crc(tx, 12);
 473                 tx[1] |= crc >> 4;
 474                 tx[2] = crc << 4;
 475                 *txlen = 20;
 476             }
 477         }
 478         break;
 479         default: {
 480             Dbprintf("Uknown frame length: %d", rxlen);
 481             return false;
 482         }
 483         break;
 484     }
 485     return true;
 486 }
 487
 488
 489 static bool hitag1_authenticate(uint8_t *rx, const size_t rxlen, uint8_t *tx, size_t *txlen) {
 490     uint8_t crc;
 491     *txlen = 0;
 492     switch (rxlen) {
 493         case 0: {
 494             // retry waking up card
 495             /*tx[0] = 0xb0; // Rev 3.0*/
 496             tx[0] = 0x30; // Rev 2.0
 497             *txlen = 5;
 498             if (bCrypto && byte_value <= 0xff) {
 499                 // to retry
 500                 bCrypto = false;
 501             }
 502             if (!bCollision) blocknr--;
 503             if (blocknr < 0) {
 504                 blocknr = 0;
 505             }
 506             bCollision = true;
 507             // will receive 32-bit UID
 508         }
 509         break;
 510         case 2: {
 511             if (bAuthenticating) {
 512                 // received Auth init ACK, send nonce
 513                 // TODO Roel, bit-manipulation goes here
 514                 /*nonce[0] = 0x2d;*/
 515                 /*nonce[1] = 0x74;*/
 516                 /*nonce[2] = 0x80;*/
 517                 /*nonce[3] = 0xa5;*/
 518                 nonce[0] = byte_value;
 519                 byte_value++;
 520                 /*set_bit(nonce,flipped_bit);*/
 521                 memcpy(tx, nonce, 4);
 522                 *txlen = 32;
 523                 // will receive 32 bit encrypted Logdata
 524             } else if (bCrypto) {
 525                 // authed, start reading
 526                 tx[0] = 0xe0 | blocknr >> 4; // RDCPAGE
 527                 tx[1] = blocknr << 4;
 528                 crc = hitag_crc(tx, 12);
 529                 tx[1] |= crc >> 4;
 530                 tx[2] = crc << 4;
 531                 *txlen = 20;
 532                 // will receive 32-bit encrypted page
 533             }
 534         }
 535         break;
 536         case 32: {
 537             if (bCollision) {
 538                 // Select card by serial from response
 539                 tx[0] = 0x00 | rx[0] >> 5;
 540                 tx[1] = rx[0] << 3 | rx[1] >> 5;
 541                 tx[2] = rx[1] << 3 | rx[2] >> 5;
 542                 tx[3] = rx[2] << 3 | rx[3] >> 5;
 543                 tx[4] = rx[3] << 3;
 544                 crc = hitag_crc(tx, 37);
 545                 tx[4] |= crc >> 5;
 546                 tx[5] = crc << 3;
 547                 *txlen = 45;
 548                 bCollision = false;
 549                 bSelecting = true;
 550                 // will receive 32-bit configuration page
 551             } else if (bSelecting) {
 552                 // Initiate auth
 553                 tx[0] = 0xa0 | (key_no); // WRCPAGE
 554                 tx[1] = blocknr << 4;
 555                 crc = hitag_crc(tx, 12);
 556                 tx[1] |= crc >> 4;
 557                 tx[2] = crc << 4;
 558                 *txlen = 20;
 559                 bSelecting = false;
 560                 bAuthenticating = true;
 561                 // will receive 2-bit ACK
 562             } else if (bAuthenticating) {
 563                 // received 32-bit logdata 0
 564                 // TODO decrypt logdata 0, verify against logdata_0
 565                 memcpy(tag.sectors[0], rx, 4);
 566                 memcpy(tag.sectors[1], tx, 4);
 567                 Dbprintf("%02x%02x%02x%02x %02x%02x%02x%02x", rx[0], rx[1], rx[2], rx[3], tx[0], tx[1], tx[2], tx[3]);
 568                 // TODO replace with secret data stream
 569                 // TODO encrypt logdata_1
 570                 memcpy(tx, logdata_1, 4);
 571                 *txlen = 32;
 572                 bAuthenticating = false;
 573                 bCrypto = true;
 574                 // will receive 2-bit ACK
 575             } else if (bCrypto) {
 576                 // received 32-bit encrypted page
 577                 // TODO decrypt rx
 578                 memcpy(tag.sectors[blocknr], rx, 4);
 579                 blocknr++;
 580                 if (blocknr > 63) {
 581                     DbpString("Read succesful!");
 582                     bSuccessful = true;
 583                     return false;
 584                 }
 585
 586                 // TEST
 587                 Dbprintf("Succesfully authenticated with logdata:");
 588                 Dbhexdump(4, logdata_1, false);
 589                 bSuccessful = true;
 590                 return false;
 591                 /*
 592                                 // read next page of card until done
 593                                 tx[0] = 0xe0 | blocknr >> 4; // RDCPAGE
 594                                 tx[1] = blocknr << 4;
 595                                 crc = hitag_crc(tx, 12);
 596                                 tx[1] |= crc >> 4;
 597                                 tx[2] = crc << 4;
 598                                 *txlen = 20;
 599                 */
 600             }
 601         }
 602         break;
 603         default: {
 604             Dbprintf("Uknown frame length: %d", rxlen);
 605             return false;
 606         }
 607         break;
 608     }
 609
 610     return true;
 611 }
 612
 613 //-----------------------------------------------------------------------------
 614 // Hitag2 operations
 615 //-----------------------------------------------------------------------------
 616
 617 static bool hitag2_write_page(uint8_t *rx, const size_t rxlen, uint8_t *tx, size_t *txlen) {
 618     switch (writestate) {
 619         case WRITE_STATE_START:
 620             *txlen = 10;
 621             tx[0] = 0x82 | (blocknr << 3) | ((blocknr ^ 7) >> 2);
 622             tx[1] = ((blocknr ^ 7) << 6);
 623             writestate = WRITE_STATE_PAGENUM_WRITTEN;
 624             break;
 625         case WRITE_STATE_PAGENUM_WRITTEN:
 626             // Check if page number was received correctly
 627             if ((rxlen == 10)
 628                     && (rx[0] == (0x82 | (blocknr << 3) | ((blocknr ^ 7) >> 2)))
 629                     && (rx[1] == (((blocknr & 0x3) ^ 0x3) << 6))) {
 630
 631                 *txlen = 32;
 632                 memset(tx, 0, HITAG_FRAME_LEN);
 633                 memcpy(tx, writedata, 4);
 634                 writestate = WRITE_STATE_PROG;
 635             } else {
 636                 Dbprintf("hitag2_write_page: Page number was not received correctly: rxlen=%d rx=%02x%02x%02x%02x",
 637                          rxlen, rx[0], rx[1], rx[2], rx[3]);
 638                 bSuccessful = false;
 639                 return false;
 640             }
 641             break;
 642         case WRITE_STATE_PROG:
 643             if (rxlen == 0) {
 644                 bSuccessful = true;
 645             } else {
 646                 bSuccessful = false;
 647                 Dbprintf("hitag2_write_page: unexpected rx data (%d) after page write", rxlen);
 648             }
 649             return false;
 650         default:
 651             DbpString("hitag2_write_page: Unknown state %d");
 652             bSuccessful = false;
 653             return false;
 654     }
 655
 656     return true;
 657 }
 658
 659 static bool hitag2_password(uint8_t *rx, const size_t rxlen, uint8_t *tx, size_t *txlen, bool write) {
 660     // Reset the transmission frame length
 661     *txlen = 0;
 662
 663     if (bPwd && !bAuthenticating && write) {
 664         if (!hitag2_write_page(rx, rxlen, tx, txlen)) {
 665             return false;
 666         }
 667     } else {
 668         // Try to find out which command was send by selecting on length (in bits)
 669         switch (rxlen) {
 670             // No answer, try to resurrect
 671             case 0: {
 672                 // Stop if there is no answer (after sending password)
 673                 if (bPwd) {
 674                     DbpString("Password failed!");
 675                     return false;
 676                 }
 677                 *txlen = 5;
 678                 memcpy(tx, "\xC0", nbytes(*txlen));
 679             }
 680             break;
 681
 682             // Received UID, tag password
 683             case 32: {
 684                 // stage 1, got UID
 685                 if (!bPwd) {
 686                     bPwd = true;
 687                     bAuthenticating = true;
 688                     memcpy(tx, password, 4);
 689                     *txlen = 32;
 690                 } else {
 691                     // stage 2, got config byte+password TAG, discard as will read later
 692                     if (bAuthenticating) {
 693                         bAuthenticating = false;
 694                         if (write) {
 695                             if (!hitag2_write_page(rx, rxlen, tx, txlen)) {
 696                                 return false;
 697                             }
 698                             break;
 699                         }
 700                     }
 701                     // stage 2+, got data block
 702                     else {
 703                         memcpy(tag.sectors[blocknr], rx, 4);
 704                         blocknr++;
 705                     }
 706
 707                     if (blocknr > 7) {
 708                         bSuccessful = true;
 709                         return false;
 710                     }
 711
 712                     *txlen = 10;
 713                     tx[0] = 0xC0 | (blocknr << 3) | ((blocknr ^ 7) >> 2);
 714                     tx[1] = ((blocknr ^ 7) << 6);
 715                 }
 716             }
 717             break;
 718
 719             // Unexpected response
 720             default: {
 721                 Dbprintf("Unknown frame length: %d", rxlen);
 722                 return false;
 723             }
 724             break;
 725         }
 726     }
 727
 728     return true;
 729 }
 730
 731 static bool hitag2_crypto(uint8_t *rx, const size_t rxlen, uint8_t *tx, size_t *txlen, bool write) {
 732     // Reset the transmission frame length
 733     *txlen = 0;
 734
 735     if (bCrypto) {
 736         hitag2_cipher_transcrypt(&cipher_state, rx, rxlen / 8, rxlen % 8);
 737     }
 738
 739     if (bCrypto && !bAuthenticating && write) {
 740         if (!hitag2_write_page(rx, rxlen, tx, txlen)) {
 741             return false;
 742         }
 743     } else {
 744
 745         // Try to find out which command was send by selecting on length (in bits)
 746         switch (rxlen) {
 747             // No answer, try to resurrect
 748             case 0: {
 749                 // Stop if there is no answer while we are in crypto mode (after sending NrAr)
 750                 if (bCrypto) {
 751                     // Failed during authentication
 752                     if (bAuthenticating) {
 753                         DbpString("Authentication failed!");
 754                         return false;
 755                     } else {
 756                         // Failed reading a block, could be (read/write) locked, skip block and re-authenticate
 757                         if (blocknr == 1) {
 758                             // Write the low part of the key in memory
 759                             memcpy(tag.sectors[1], key + 2, 4);
 760                         } else if (blocknr == 2) {
 761                             // Write the high part of the key in memory
 762                             tag.sectors[2][0] = 0x00;
 763                             tag.sectors[2][1] = 0x00;
 764                             tag.sectors[2][2] = key[0];
 765                             tag.sectors[2][3] = key[1];
 766                         } else {
 767                             // Just put zero's in the memory (of the unreadable block)
 768                             memset(tag.sectors[blocknr], 0x00, 4);
 769                         }
 770                         blocknr++;
 771                         bCrypto = false;
 772                     }
 773                 } else {
 774                     *txlen = 5;
 775                     memcpy(tx, "\xc0", nbytes(*txlen));
 776                 }
 777                 break;
 778             }
 779             // Received UID, crypto tag answer
 780             case 32: {
 781                 // stage 1, got UID
 782                 if (!bCrypto) {
 783                     uint64_t ui64key = key[0] | ((uint64_t)key[1]) << 8 | ((uint64_t)key[2]) << 16 | ((uint64_t)key[3]) << 24 | ((uint64_t)key[4]) << 32 | ((uint64_t)key[5]) << 40;
 784                     uint32_t ui32uid = rx[0] | ((uint32_t)rx[1]) << 8 | ((uint32_t)rx[2]) << 16 | ((uint32_t)rx[3]) << 24;
 785                     Dbprintf("hitag2_crypto: key=0x%x%x uid=0x%x", (uint32_t)((REV64(ui64key)) >> 32), (uint32_t)((REV64(ui64key)) & 0xffffffff), REV32(ui32uid));
 786                     cipher_state = _hitag2_init(REV64(ui64key), REV32(ui32uid), 0);
 787                     // PRN
 788                     memset(tx, 0x00, 4);
 789                     // Secret data
 790                     memset(tx + 4, 0xff, 4);
 791                     hitag2_cipher_transcrypt(&cipher_state, tx + 4, 4, 0);
 792                     *txlen = 64;
 793                     bCrypto = true;
 794                     bAuthenticating = true;
 795                 } else {
 796                     // stage 2, got config byte+password TAG, discard as will read later
 797                     if (bAuthenticating) {
 798                         bAuthenticating = false;
 799                         if (write) {
 800                             if (!hitag2_write_page(rx, rxlen, tx, txlen)) {
 801                                 return false;
 802                             }
 803                             break;
 804                         }
 805                     }
 806                     // stage 2+, got data block
 807                     else {
 808                         // Store the received block
 809                         memcpy(tag.sectors[blocknr], rx, 4);
 810                         blocknr++;
 811                     }
 812                     if (blocknr > 7) {
 813                         DbpString("Read successful!");
 814                         bSuccessful = true;
 815                         return false;
 816                     } else {
 817                         *txlen = 10;
 818                         tx[0] = 0xc0 | (blocknr << 3) | ((blocknr ^ 7) >> 2);
 819                         tx[1] = ((blocknr ^ 7) << 6);
 820                     }
 821                 }
 822             }
 823             break;
 824
 825             // Unexpected response
 826             default: {
 827                 Dbprintf("Unknown frame length: %d", rxlen);
 828                 return false;
 829             }
 830             break;
 831         }
 832     }
 833
 834     if (bCrypto) {
 835         // We have to return now to avoid double encryption
 836         if (!bAuthenticating) {
 837             hitag2_cipher_transcrypt(&cipher_state, tx, *txlen / 8, *txlen % 8);
 838         }
 839     }
 840
 841     return true;
 842 }
 843
 844 static bool hitag2_authenticate(uint8_t *rx, const size_t rxlen, uint8_t *tx, size_t *txlen) {
 845     // Reset the transmission frame length
 846     *txlen = 0;
 847
 848     // Try to find out which command was send by selecting on length (in bits)
 849     switch (rxlen) {
 850         // No answer, try to resurrect
 851         case 0: {
 852             // Stop if there is no answer while we are in crypto mode (after sending NrAr)
 853             if (bCrypto) {
 854                 DbpString("Authentication failed!");
 855                 return false;
 856             }
 857             *txlen = 5;
 858             memcpy(tx, "\xC0", nbytes(*txlen));
 859         }
 860         break;
 861
 862         // Received UID, crypto tag answer
 863         case 32: {
 864             if (!bCrypto) {
 865                 *txlen = 64;
 866                 memcpy(tx, NrAr, 8);
 867                 bCrypto = true;
 868             } else {
 869                 DbpString("Authentication successful!");
 870                 return true;
 871             }
 872         }
 873         break;
 874
 875         // Unexpected response
 876         default: {
 877             Dbprintf("Unknown frame length: %d", rxlen);
 878             return false;
 879         }
 880         break;
 881     }
 882
 883     return true;
 884 }
 885
 886 static bool hitag2_test_auth_attempts(uint8_t *rx, const size_t rxlen, uint8_t *tx, size_t *txlen) {
 887
 888     // Reset the transmission frame length
 889     *txlen = 0;
 890
 891     // Try to find out which command was send by selecting on length (in bits)
 892     switch (rxlen) {
 893         // No answer, try to resurrect
 894         case 0: {
 895             // Stop if there is no answer while we are in crypto mode (after sending NrAr)
 896             if (bCrypto) {
 897                 Dbprintf("auth: %02x%02x%02x%02x%02x%02x%02x%02x Failed, removed entry!", NrAr[0], NrAr[1], NrAr[2], NrAr[3], NrAr[4], NrAr[5], NrAr[6], NrAr[7]);
 898
 899                 // Removing failed entry from authentiations table
 900                 memcpy(auth_table + auth_table_pos, auth_table + auth_table_pos + 8, 8);
 901                 auth_table_len -= 8;
 902
 903                 // Return if we reached the end of the authentications table
 904                 bCrypto = false;
 905                 if (auth_table_pos == auth_table_len) {
 906                     return false;
 907                 }
 908
 909                 // Copy the next authentication attempt in row (at the same position, b/c we removed last failed entry)
 910                 memcpy(NrAr, auth_table + auth_table_pos, 8);
 911             }
 912             *txlen = 5;
 913             memcpy(tx, "\xc0", nbytes(*txlen));
 914         }
 915         break;
 916
 917         // Received UID, crypto tag answer, or read block response
 918         case 32: {
 919             if (!bCrypto) {
 920                 *txlen = 64;
 921                 memcpy(tx, NrAr, 8);
 922                 bCrypto = true;
 923             } else {
 924                 Dbprintf("auth: %02x%02x%02x%02x%02x%02x%02x%02x OK", NrAr[0], NrAr[1], NrAr[2], NrAr[3], NrAr[4], NrAr[5], NrAr[6], NrAr[7]);
 925                 bCrypto = false;
 926                 if ((auth_table_pos + 8) == auth_table_len) {
 927                     return false;
 928                 }
 929                 auth_table_pos += 8;
 930                 memcpy(NrAr, auth_table + auth_table_pos, 8);
 931             }
 932         }
 933         break;
 934
 935         default: {
 936             Dbprintf("Unknown frame length: %d", rxlen);
 937             return false;
 938         }
 939         break;
 940     }
 941
 942     return true;
 943 }
 944
 945 static bool hitag2_read_uid(uint8_t *rx, const size_t rxlen, uint8_t *tx, size_t *txlen) {
 946     // Reset the transmission frame length
 947     *txlen = 0;
 948
 949     // Try to find out which command was send by selecting on length (in bits)
 950     switch (rxlen) {
 951         // No answer, try to resurrect
 952         case 0: {
 953             // Just starting or if there is no answer
 954             *txlen = 5;
 955             memcpy(tx, "\xC0", nbytes(*txlen));
 956         }
 957         break;
 958         // Received UID
 959         case 32: {
 960             // Check if we received answer tag (at)
 961             if (bAuthenticating) {
 962                 bAuthenticating = false;
 963             } else {
 964                 // Store the received block
 965                 memcpy(tag.sectors[blocknr], rx, 4);
 966                 blocknr++;
 967
 968                 DBG Dbhexdump(4, rx, false);
 969             }
 970             if (blocknr > 0) {
 971                 DBG DbpString("Read successful!");
 972                 bSuccessful = true;
 973                 return true;
 974             }
 975         }
 976         break;
 977         // Unexpected response
 978         default: {
 979             DBG Dbprintf("Unknown frame length: %d", rxlen);
 980             return false;
 981         }
 982         break;
 983     }
 984     return true;
 985 }
 986
 987 void EloadHitag(uint8_t *data, uint16_t len) {
 988     memcpy(tag.sectors, data, sizeof(tag.sectors));
 989 }
 990
 991 // Hitag2 Sniffing
 992
 993 // T0     18-22 fc  (total time ZERO)
 994 // T1     26-32 fc  (total time ONE)
 995 // Tstop  36 >  fc  (high field stop limit)
 996 // Tlow   4-10  fc  (reader field low time)
 997 void SniffHitag2(void) {
 998     DbpString("Starting Hitag2 sniffing");
 999     LED_D_ON();
1000
1001     FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
1002
1003     BigBuf_free();
1004     BigBuf_Clear_ext(false);
1005     clear_trace();
1006     set_tracing(true);
1007
1008     /*
1009         lf_init(false, false);
1010
1011         // no logging of the raw signal
1012         g_logging = lf_get_reader_modulation();
1013         uint32_t total_count = 0;
1014
1015         uint8_t rx[20 * 8 * 2];
1016         while (BUTTON_PRESS() == false) {
1017
1018             lf_reset_counter();
1019
1020             WDT_HIT();
1021
1022             size_t periods = 0;
1023             uint16_t rxlen = 0;
1024             memset(rx, 0x00, sizeof(rx));
1025
1026             // Use the current modulation state as starting point
1027             uint8_t mod_state = lf_get_reader_modulation();
1028
1029             while (rxlen < sizeof(rx)) {
1030                 periods = lf_count_edge_periods(64);
1031                 // Evaluate the number of periods before the next edge
1032                 if (periods >= 24 && periods < 64) {
1033                     // Detected two sequential equal bits and a modulation switch
1034                     // NRZ modulation: (11 => --|) or (11 __|)
1035                     rx[rxlen++] = mod_state;
1036                     rx[rxlen++] = mod_state;
1037                     // toggle tag modulation state
1038                     mod_state ^= 1;
1039                 } else if (periods > 0 && periods < 24) {
1040                     // Detected one bit and a modulation switch
1041                     // NRZ modulation: (1 => -|) or (0 _|)
1042                     rx[rxlen++] = mod_state;
1043                     mod_state ^= 1;
1044                 } else {
1045                     mod_state ^= 1;
1046                     break;
1047                 }
1048             }
1049
1050             if (rxlen == 0)
1051                 continue;
1052
1053             // tag sends 11111 + uid,
1054             bool got_tag = ((memcmp(rx, "\x01\x00\x01\x00\x01\x00\x01\x00\x01\x00", 10) == 0));
1055
1056             if (got_tag) {
1057                 // mqnchester decode
1058                 bool bad_man = false;
1059                 uint16_t bitnum = 0;
1060                 for (uint16_t i = 0; i < rxlen; i += 2) {
1061                     if (rx[i] == 1 && (rx[i + 1] == 0)) {
1062                         rx[bitnum++] = 0;
1063                     } else if ((rx[i] == 0) && rx[i + 1] == 1) {
1064                         rx[bitnum++] = 1;
1065                     } else {
1066                         bad_man = true;
1067                     }
1068                 }
1069
1070                 if (bad_man) {
1071                     DBG DbpString("bad manchester");
1072                     continue;
1073                 }
1074
1075                 if (bitnum < 5) {
1076                     DBG DbpString("too few bits");
1077                     continue;
1078                 }
1079
1080                 // skip header 11111
1081                 uint16_t i = 0;
1082                 if (got_tag) {
1083                     i = 5;
1084                 }
1085
1086                 // Pack the response into a byte array
1087                 rxlen = 0;
1088                 for (; i < bitnum; i++) {
1089                     uint8_t b = rx[i];
1090                     rx[rxlen >> 3] |= b << (7 - (rxlen % 8));
1091                     rxlen++;
1092                 }
1093
1094                 // skip spurious bit
1095                 if (rxlen % 8 == 1) {
1096                     rxlen--;
1097                 }
1098
1099                 // nothing to log
1100                 if (rxlen == 0)
1101                     continue;
1102
1103                 LogTrace(rx, nbytes(rxlen), 0, 0, NULL, false);
1104                 total_count += nbytes(rxlen);
1105             } else {
1106                 // decode reader comms
1107                 LogTrace(rx, rxlen, 0, 0, NULL, true);
1108                 total_count += rxlen;
1109                 // Pack the response into a byte array
1110
1111                 // LogTrace(rx, nbytes(rdr), 0, 0, NULL, true);
1112                 // total_count += nbytes(rdr);
1113             }
1114             LED_A_INV();
1115         }
1116
1117         lf_finalize();
1118
1119         Dbprintf("Collected %u bytes", total_count);
1120
1121         */
1122
1123     // Set up eavesdropping mode, frequency divisor which will drive the FPGA
1124     // and analog mux selection.
1125     FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_EDGE_DETECT  | FPGA_LF_EDGE_DETECT_TOGGLE_MODE);
1126     FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); // 125Khz
1127     SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
1128     RELAY_OFF();
1129
1130     // Configure output pin that is connected to the FPGA (for modulating)
1131     AT91C_BASE_PIOA->PIO_OER = GPIO_SSC_DOUT;
1132     AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT;
1133
1134     // Disable modulation, we are going to eavesdrop, not modulate ;)
1135     LOW(GPIO_SSC_DOUT);
1136
1137     // Enable Peripheral Clock for TIMER_CLOCK1, used to capture edges of the reader frames
1138     AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_TC1);
1139     AT91C_BASE_PIOA->PIO_BSR = GPIO_SSC_FRAME;
1140
1141     // Disable timer during configuration
1142     AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
1143
1144     // Capture mode, defaul timer source = MCK/2 (TIMER_CLOCK1), TIOA is external trigger,
1145     // external trigger rising edge, load RA on rising edge of TIOA.
1146     AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK | AT91C_TC_ETRGEDG_BOTH | AT91C_TC_ABETRG | AT91C_TC_LDRA_BOTH;
1147
1148     // Enable and reset counter
1149     AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
1150
1151     int frame_count = 0, response = 0, overflow = 0, lastbit = 1, tag_sof = 4;
1152     bool rising_edge = false, reader_frame = false, bSkip = true;
1153     uint8_t rx[HITAG_FRAME_LEN];
1154     size_t rxlen = 0;
1155
1156     auth_table_len = 0;
1157     auth_table_pos = 0;
1158
1159     // Reset the received frame, frame count and timing info
1160     memset(rx, 0x00, sizeof(rx));
1161
1162     auth_table = (uint8_t *)BigBuf_malloc(AUTH_TABLE_LENGTH);
1163     memset(auth_table, 0x00, AUTH_TABLE_LENGTH);
1164
1165     while (BUTTON_PRESS() == false) {
1166
1167         WDT_HIT();
1168         memset(rx, 0x00, sizeof(rx));
1169
1170         // Receive frame, watch for at most T0 * EOF periods
1171         while (AT91C_BASE_TC1->TC_CV < (HITAG_T0 * HITAG_T_EOF)) {
1172             // Check if rising edge in modulation is detected
1173             if (AT91C_BASE_TC1->TC_SR & AT91C_TC_LDRAS) {
1174                 // Retrieve the new timing values
1175                 int ra = (AT91C_BASE_TC1->TC_RA / HITAG_T0);
1176
1177                 // Find out if we are dealing with a rising or falling edge
1178                 rising_edge = (AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_FRAME) > 0;
1179
1180                 // Shorter periods will only happen with reader frames
1181                 if (reader_frame == false && rising_edge && ra < HITAG_T_TAG_CAPTURE_ONE_HALF) {
1182                     // Switch from tag to reader capture
1183                     LED_C_OFF();
1184                     reader_frame = true;
1185                     rxlen = 0;
1186                 }
1187
1188                 // Only handle if reader frame and rising edge, or tag frame and falling edge
1189                 if (reader_frame == rising_edge) {
1190                     overflow += ra;
1191                     continue;
1192                 }
1193
1194                 // Add the buffered timing values of earlier captured edges which were skipped
1195                 ra += overflow;
1196                 overflow = 0;
1197
1198                 if (reader_frame) {
1199                     LED_B_ON();
1200                     // Capture reader frame
1201                     if (ra >= HITAG_T_STOP) {
1202 //                      if (rxlen != 0) {
1203                         //DbpString("wierd0?");
1204 //                      }
1205                         // Capture the T0 periods that have passed since last communication or field drop (reset)
1206                         response = (ra - HITAG_T_LOW);
1207                     } else if (ra >= HITAG_T_1_MIN) {
1208                         // '1' bit
1209                         rx[rxlen / 8] |= 1 << (7 - (rxlen % 8));
1210                         rxlen++;
1211                     } else if (ra >= HITAG_T_0_MIN) {
1212                         // '0' bit
1213                         rx[rxlen / 8] |= 0 << (7 - (rxlen % 8));
1214                         rxlen++;
1215                     }
1216
1217                 } else {
1218                     LED_C_ON();
1219                     // Capture tag frame (manchester decoding using only falling edges)
1220                     if (ra >= HITAG_T_EOF) {
1221 //                      if (rxlen != 0) {
1222                         //DbpString("wierd1?");
1223 //                      }
1224                         // Capture the T0 periods that have passed since last communication or field drop (reset)
1225                         // We always recieve a 'one' first, which has the falling edge after a half period |-_|
1226                         response = ra - HITAG_T_TAG_HALF_PERIOD;
1227
1228                     } else if (ra >= HITAG_T_TAG_CAPTURE_FOUR_HALF) {
1229                         // Manchester coding example |-_|_-|-_| (101)
1230                         rx[rxlen / 8] |= 0 << (7 - (rxlen % 8));
1231                         rxlen++;
1232                         rx[rxlen / 8] |= 1 << (7 - (rxlen % 8));
1233                         rxlen++;
1234
1235                     } else if (ra >= HITAG_T_TAG_CAPTURE_THREE_HALF) {
1236                         // Manchester coding example |_-|...|_-|-_| (0...01)
1237                         rx[rxlen / 8] |= 0 << (7 - (rxlen % 8));
1238                         rxlen++;
1239                         // We have to skip this half period at start and add the 'one' the second time
1240                         if (bSkip == false) {
1241                             rx[rxlen / 8] |= 1 << (7 - (rxlen % 8));
1242                             rxlen++;
1243                         }
1244                         lastbit = !lastbit;
1245                         bSkip = !bSkip;
1246
1247                     } else if (ra >= HITAG_T_TAG_CAPTURE_TWO_HALF) {
1248                         // Manchester coding example |_-|_-| (00) or |-_|-_| (11)
1249                         if (tag_sof) {
1250                             // Ignore bits that are transmitted during SOF
1251                             tag_sof--;
1252                         } else {
1253                             // bit is same as last bit
1254                             rx[rxlen / 8] |= lastbit << (7 - (rxlen % 8));
1255                             rxlen++;
1256                         }
1257                     }
1258                 }
1259             }
1260         }
1261
1262         // Check if frame was captured
1263         if (rxlen) {
1264             frame_count++;
1265             LogTrace(rx, nbytes(rxlen), response, 0, NULL, reader_frame);
1266
1267             // Check if we recognize a valid authentication attempt
1268             if (nbytes(rxlen) == 8) {
1269                 // Store the authentication attempt
1270                 if (auth_table_len < (AUTH_TABLE_LENGTH - 8)) {
1271                     memcpy(auth_table + auth_table_len, rx, 8);
1272                     auth_table_len += 8;
1273                 }
1274             }
1275
1276             // Reset the received frame and response timing info
1277             memset(rx, 0x00, sizeof(rx));
1278             response = 0;
1279             reader_frame = false;
1280             lastbit = 1;
1281             bSkip = true;
1282             tag_sof = 4;
1283             overflow = 0;
1284
1285             LED_B_OFF();
1286             LED_C_OFF();
1287         } else {
1288             // Save the timer overflow, will be 0 when frame was received
1289             overflow += (AT91C_BASE_TC1->TC_CV / HITAG_T0);
1290         }
1291         // Reset the frame length
1292         rxlen = 0;
1293         // Reset the timer to restart while-loop that receives frames
1294         AT91C_BASE_TC1->TC_CCR = AT91C_TC_SWTRG;
1295         AT91C_BASE_TC1->TC_CCR = AT91C_TC_SWTRG;
1296     }
1297
1298     LEDsoff();
1299     AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
1300     AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS;
1301     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1302     set_tracing(false);
1303
1304     Dbprintf("frame received: %d", frame_count);
1305     Dbprintf("Authentication Attempts: %d", (auth_table_len / 8));
1306
1307 }
1308
1309 // Hitag2 simulation
1310 void SimulateHitag2(void) {
1311
1312     BigBuf_free();
1313     BigBuf_Clear_ext(false);
1314     clear_trace();
1315     set_tracing(true);
1316
1317     // empties bigbuff etc
1318     lf_init(false, true);
1319
1320     int response = 0;
1321     uint8_t rx[HITAG_FRAME_LEN] = {0};
1322     uint8_t tx[HITAG_FRAME_LEN] = {0};
1323
1324     auth_table_len = 0;
1325     auth_table_pos = 0;
1326 //    auth_table = BigBuf_malloc(AUTH_TABLE_LENGTH);
1327 //    memset(auth_table, 0x00, AUTH_TABLE_LENGTH);
1328
1329     // Reset the received frame, frame count and timing info
1330 //    memset(rx, 0x00, sizeof(rx));
1331 //    memset(tx, 0x00, sizeof(tx));
1332
1333     DbpString("Starting Hitag2 simulation");
1334
1335     // hitag2 state machine?
1336     hitag2_init();
1337
1338     // printing
1339     uint32_t block = 0;
1340     for (size_t i = 0; i < 12; i++) {
1341
1342         // num2bytes?
1343         for (size_t j = 0; j < 4; j++) {
1344             block <<= 8;
1345             block |= tag.sectors[i][j];
1346         }
1347         Dbprintf("| %d | %08x |", i, block);
1348     }
1349
1350     size_t max_nrzs = 8 * HITAG_FRAME_LEN + 5;
1351     uint8_t nrz_samples[max_nrzs];
1352
1353 //    uint32_t command_start = 0, command_duration = 0;
1354     //  int16_t checked = 0;
1355
1356 // SIMULATE
1357     uint32_t signal_size = 10000;
1358     while (BUTTON_PRESS() == false) {
1359
1360         // use malloc
1361         initSampleBufferEx(&signal_size, true);
1362
1363         LED_D_ON();
1364
1365 //        lf_reset_counter();
1366         LED_A_OFF();
1367         WDT_HIT();
1368
1369         /*
1370                 // only every 1000th times, in order to save time when collecting samples.
1371                 if (checked == 100) {
1372                     if (data_available()) {
1373                         checked = -1;
1374                         break;
1375                     } else {
1376                         checked = 0;
1377                     }
1378                 }
1379                 ++checked;
1380         */
1381         size_t rxlen = 0, txlen = 0;
1382
1383         // Keep administration of the first edge detection
1384         bool waiting_for_first_edge = true;
1385
1386         // Did we detected any modulaiton at all
1387         bool detected_modulation = false;
1388
1389         // Use the current modulation state as starting point
1390         uint8_t reader_modulation = lf_get_reader_modulation();
1391
1392         // Receive frame, watch for at most max_nrzs periods
1393         // Reset the number of NRZ samples and use edge detection to detect them
1394         size_t nrzs = 0;
1395         while (nrzs < max_nrzs) {
1396             // Get the timing of the next edge in number of wave periods
1397             size_t periods = lf_count_edge_periods(128);
1398
1399             // Just break out of loop after an initial time-out (tag is probably not available)
1400             // The function lf_count_edge_periods() returns 0 when a time-out occurs
1401             if (periods == 0) {
1402                 break;
1403             }
1404
1405             LED_A_ON();
1406
1407             // Are we dealing with the first incoming edge
1408             if (waiting_for_first_edge) {
1409
1410                 // Register the number of periods that have passed
1411                 response = periods;
1412
1413                 // Indicate that we have dealt with the first edge
1414                 waiting_for_first_edge = false;
1415
1416                 // The first edge is always a single NRZ bit, force periods on 16
1417                 periods = 16;
1418
1419                 // We have received more than 0 periods, so we have detected a tag response
1420                 detected_modulation = true;
1421             }
1422
1423             // Evaluate the number of periods before the next edge
1424             if (periods > 24 && periods <= 64) {
1425                 // Detected two sequential equal bits and a modulation switch
1426                 // NRZ modulation: (11 => --|) or (11 __|)
1427                 nrz_samples[nrzs++] = reader_modulation;
1428                 nrz_samples[nrzs++] = reader_modulation;
1429                 // Invert tag modulation state
1430                 reader_modulation ^= 1;
1431             } else if (periods > 0 && periods <= 24) {
1432                 // Detected one bit and a modulation switch
1433                 // NRZ modulation: (1 => -|) or (0 _|)
1434                 nrz_samples[nrzs++] = reader_modulation;
1435                 reader_modulation ^= 1;
1436             } else {
1437                 reader_modulation ^= 1;
1438                 // The function lf_count_edge_periods() returns > 64 periods, this is not a valid number periods
1439                 Dbprintf("Detected unexpected period count: %d", periods);
1440                 break;
1441             }
1442         }
1443
1444         LED_D_OFF();
1445
1446         // If there is no response, just repeat the loop
1447         if (!detected_modulation) continue;
1448
1449         LED_A_OFF();
1450
1451         // Make sure we always have an even number of samples. This fixes the problem
1452         // of ending the manchester decoding with a zero. See the example below where
1453         // the '|' character is end of modulation
1454         //  One at the end: ..._-|_____...
1455         // Zero at the end: ...-_|_____...
1456         // The last modulation change of a zero is not detected, but we should take
1457         // the half period in account, otherwise the demodulator will fail.
1458         if ((nrzs % 2) != 0) {
1459             nrz_samples[nrzs++] = reader_modulation;
1460         }
1461
1462         LED_B_ON();
1463
1464         // decode bitstream
1465         manrawdecode((uint8_t *)nrz_samples, &nrzs, true, 0);
1466
1467         // Verify if the header consists of five consecutive ones
1468         if (nrzs < 5) {
1469             Dbprintf("Detected unexpected number of manchester decoded samples [%d]", nrzs);
1470             continue;
1471         } else {
1472             for (size_t i = 0; i < 5; i++) {
1473                 if (nrz_samples[i] != 1) {
1474                     Dbprintf("Detected incorrect header, the bit [%d] is zero instead of one", i);
1475                 }
1476             }
1477         }
1478
1479         // Pack the response into a byte array
1480         for (size_t i = 5; i < 37; i++) {
1481             uint8_t bit = nrz_samples[i];
1482             rx[rxlen / 8] |= bit << (7 - (rxlen % 8));
1483             rxlen++;
1484         }
1485
1486         // Check if frame was captured
1487         if (rxlen > 4) {
1488
1489             LogTrace(rx, nbytes(rxlen), response, response, NULL, true);
1490
1491             // Process the incoming frame (rx) and prepare the outgoing frame (tx)
1492             hitag2_handle_reader_command(rx, rxlen, tx, &txlen);
1493
1494             // Wait for HITAG_T_WAIT_1 carrier periods after the last reader bit,
1495             // not that since the clock counts since the rising edge, but T_Wait1 is
1496             // with respect to the falling edge, we need to wait actually (T_Wait1 - T_Low)
1497             // periods. The gap time T_Low varies (4..10). All timer values are in
1498             // terms of T0 units (HITAG_T_WAIT_1_MIN - HITAG_T_LOW )
1499             lf_wait_periods(HITAG_T_WAIT_1_MIN);
1500
1501             // Send and store the tag answer (if there is any)
1502             if (txlen) {
1503                 // Transmit the tag frame
1504                 //hitag_send_frame(tx, txlen);
1505                 lf_manchester_send_bytes(tx, txlen);
1506
1507                 // Store the frame in the trace
1508                 LogTrace(tx, nbytes(txlen), 0, 0, NULL, false);
1509             }
1510
1511             // Reset the received frame and response timing info
1512             memset(rx, 0x00, sizeof(rx));
1513             response = 0;
1514
1515             LED_B_OFF();
1516         }
1517     }
1518
1519     lf_finalize();
1520
1521     // release allocated memory from BigBuff.
1522     BigBuf_free();
1523
1524     DbpString("Sim stopped");
1525
1526 //    reply_ng(CMD_LF_HITAG_SIMULATE, (checked == -1) ? PM3_EOPABORTED : PM3_SUCCESS, (uint8_t *)tag.sectors, tag_size);
1527 }
1528
1529 void ReaderHitag(hitag_function htf, hitag_data *htd) {
1530
1531     uint32_t command_start = 0, command_duration = 0;
1532     uint32_t response_start = 0, response_duration = 0;
1533
1534     uint8_t rx[HITAG_FRAME_LEN] = {0};
1535     size_t rxlen = 0;
1536     uint8_t txbuf[HITAG_FRAME_LEN] = {0};
1537     uint8_t *tx = txbuf;
1538     size_t txlen = 0;
1539
1540     int t_wait_1 = 204;
1541     int t_wait_1_guard = 8;
1542     int t_wait_2 = 128;
1543     size_t tag_size = 48;
1544     bool bStop = false;
1545
1546     // Raw demodulation/decoding by sampling edge periods
1547     size_t periods = 0;
1548
1549     // Reset the return status
1550     bSuccessful = false;
1551     bCrypto = false;
1552
1553     // Clean up trace and prepare it for storing frames
1554     set_tracing(true);
1555     clear_trace();
1556
1557     // Check configuration
1558     switch (htf) {
1559         case RHT1F_PLAIN: {
1560             DBG Dbprintf("Read public blocks in plain mode");
1561             // this part will be unreadable
1562             memset(tag.sectors + 2, 0x0, 30);
1563             blocknr = 0;
1564             break;
1565         }
1566         case RHT1F_AUTHENTICATE: {
1567             DBG Dbprintf("Read all blocks in authed mode");
1568             memcpy(nonce, htd->ht1auth.nonce, 4);
1569             memcpy(key, htd->ht1auth.key, 4);
1570             memcpy(logdata_0, htd->ht1auth.logdata_0, 4);
1571             memcpy(logdata_1, htd->ht1auth.logdata_1, 4);
1572             // TEST
1573             memset(nonce, 0x0, 4);
1574             memset(logdata_1, 0x00, 4);
1575             byte_value = 0;
1576             key_no = htd->ht1auth.key_no;
1577             DBG Dbprintf("Authenticating using key #%u :", key_no);
1578             DBG Dbhexdump(4, key, false);
1579             DBG DbpString("Nonce:");
1580             DBG Dbhexdump(4, nonce, false);
1581             DBG DbpString("Logdata_0:");
1582             DBG Dbhexdump(4, logdata_0, false);
1583             DBG DbpString("Logdata_1:");
1584             DBG Dbhexdump(4, logdata_1, false);
1585             blocknr = 0;
1586             break;
1587         }
1588         case RHT2F_PASSWORD: {
1589             DBG Dbprintf("List identifier in password mode");
1590             if (memcmp(htd->pwd.password, "\x00\x00\x00\x00", 4) == 0)
1591                 memcpy(password, tag.sectors[1], sizeof(password));
1592             else
1593                 memcpy(password, htd->pwd.password, sizeof(password));
1594
1595             blocknr = 0;
1596             bPwd = false;
1597             bAuthenticating = false;
1598             break;
1599         }
1600         case RHT2F_AUTHENTICATE: {
1601             DBG DbpString("Authenticating using nr,ar pair:");
1602             memcpy(NrAr, htd->auth.NrAr, 8);
1603             DBG Dbhexdump(8, NrAr, false);
1604             bCrypto = false;
1605             bAuthenticating = false;
1606             break;
1607         }
1608         case RHT2F_CRYPTO: {
1609             DBG DbpString("Authenticating using key:");
1610             memcpy(key, htd->crypto.key, 6);  //HACK; 4 or 6??  I read both in the code.
1611             DBG Dbhexdump(6, key, false);
1612             DBG DbpString("Nonce:");
1613             DBG Dbhexdump(4, nonce, false);
1614             memcpy(nonce, htd->crypto.data, 4);
1615             blocknr = 0;
1616             bCrypto = false;
1617             bAuthenticating = false;
1618             break;
1619         }
1620         case RHT2F_TEST_AUTH_ATTEMPTS: {
1621             DBG Dbprintf("Testing %d authentication attempts", (auth_table_len / 8));
1622             auth_table_pos = 0;
1623             memcpy(NrAr, auth_table, 8);
1624             bCrypto = false;
1625             break;
1626         }
1627         case RHT2F_UID_ONLY: {
1628             blocknr = 0;
1629             bCrypto = false;
1630             bAuthenticating = false;
1631             break;
1632         }
1633         default: {
1634             DBG Dbprintf("Error, unknown function: %d", htf);
1635             set_tracing(false);
1636             return;
1637         }
1638     }
1639
1640     LED_D_ON();
1641
1642     // hitag2 state machine?
1643     hitag2_init();
1644
1645     uint8_t attempt_count = 0;
1646
1647     // Tag specific configuration settings (sof, timings, etc.)
1648 // TODO HTS
1649     /*  if (htf <= HTS_LAST_CMD) {
1650             // hitagS settings
1651             t_wait_1 = 204;
1652             t_wait_2 = 128;
1653             flipped_bit = 0;
1654             tag_size = 8;
1655             DBG DbpString("Configured for hitagS reader");
1656         } else */
1657     if (htf <= HT1_LAST_CMD) {
1658         // hitag1 settings
1659         t_wait_1 = 204;
1660         t_wait_2 = 128;
1661         tag_size = 256;
1662         flipped_bit = 0;
1663         DBG DbpString("Configured for hitag1 reader");
1664     } else if (htf <= HT2_LAST_CMD) {
1665         // hitag2 settings
1666         t_wait_1 = HITAG_T_WAIT_1_MIN;
1667         t_wait_2 = HITAG_T_WAIT_2_MIN;
1668         tag_size = 48;
1669         DBG DbpString("Configured for hitag2 reader");
1670     }
1671
1672     // init as reader
1673     lf_init(true, false);
1674     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1675
1676     uint8_t tag_modulation;
1677     size_t max_nrzs = (8 * HITAG_FRAME_LEN + 5) * 2; // up to 2 nrzs per bit
1678     uint8_t nrz_samples[max_nrzs];
1679     bool turn_on = true;
1680     size_t nrzs = 0;
1681     int16_t checked = 0;
1682     uint32_t signal_size = 10000;
1683
1684     while (bStop == false && BUTTON_PRESS() == false) {
1685
1686         // use malloc
1687         initSampleBufferEx(&signal_size, true);
1688
1689         WDT_HIT();
1690
1691         // only every 1000th times, in order to save time when collecting samples.
1692         if (checked == 4000) {
1693             if (data_available()) {
1694                 checked = -1;
1695                 break;
1696             } else {
1697                 checked = 0;
1698             }
1699         }
1700         ++checked;
1701
1702         // By default reset the transmission buffer
1703         tx = txbuf;
1704         switch (htf) {
1705             case RHT1F_PLAIN: {
1706                 bStop = !hitag_plain(rx, rxlen, tx, &txlen, false);
1707                 break;
1708             }
1709             case RHT1F_AUTHENTICATE: {
1710                 bStop = !hitag1_authenticate(rx, rxlen, tx, &txlen);
1711                 break;
1712             }
1713             case RHT2F_PASSWORD: {
1714                 bStop = !hitag2_password(rx, rxlen, tx, &txlen, false);
1715                 break;
1716             }
1717             case RHT2F_AUTHENTICATE: {
1718                 bStop = !hitag2_authenticate(rx, rxlen, tx, &txlen);
1719                 break;
1720             }
1721             case RHT2F_CRYPTO: {
1722                 bStop = !hitag2_crypto(rx, rxlen, tx, &txlen, false);
1723                 break;
1724             }
1725             case RHT2F_TEST_AUTH_ATTEMPTS: {
1726                 bStop = !hitag2_test_auth_attempts(rx, rxlen, tx, &txlen);
1727                 break;
1728             }
1729             case RHT2F_UID_ONLY: {
1730                 bStop = !hitag2_read_uid(rx, rxlen, tx, &txlen);
1731                 if (bSuccessful) bStop = true;
1732                 attempt_count++; //attempt 3 times to get uid then quit
1733                 if (!bStop && attempt_count == 3)
1734                     bStop = true;
1735
1736                 break;
1737             }
1738             default: {
1739                 DBG Dbprintf("Error, unknown function: %d", htf);
1740                 goto out;
1741             }
1742         }
1743         if (bStop) break;
1744         if (turn_on) {
1745             // Wait 50ms with field off to be sure the transponder gets reset
1746             SpinDelay(50);
1747             FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
1748             turn_on = false;
1749             // Wait with field on to be in "Wait for START_AUTH" timeframe
1750             lf_wait_periods(HITAG_T_WAIT_POWERUP + HITAG_T_WAIT_START_AUTH_MAX / 4);
1751             command_start += HITAG_T_WAIT_POWERUP + HITAG_T_WAIT_START_AUTH_MAX / 4;
1752         } else {
1753             // Wait for t_wait_2 carrier periods after the last tag bit before transmitting,
1754             lf_wait_periods(t_wait_2);
1755             command_start += t_wait_2;
1756         }
1757         // Transmit the reader frame
1758         command_duration = hitag_reader_send_frame(tx, txlen);
1759         response_start = command_start + command_duration;
1760
1761         // Let the antenna and ADC values settle
1762         // And find the position where edge sampling should start
1763         lf_wait_periods(t_wait_1 - t_wait_1_guard);
1764         response_start += t_wait_1 - t_wait_1_guard;
1765
1766         // Keep administration of the first edge detection
1767         bool waiting_for_first_edge = true;
1768
1769         // Did we detected any modulaiton at all
1770         bool detected_tag_modulation = false;
1771
1772         // Use the current modulation state as starting point
1773         tag_modulation = lf_get_tag_modulation();
1774
1775         // Reset the number of NRZ samples and use edge detection to detect them
1776         nrzs = 0;
1777         while (nrzs < max_nrzs) {
1778             // Get the timing of the next edge in number of wave periods
1779             periods = lf_count_edge_periods(128);
1780
1781             // Are we dealing with the first incoming edge
1782             if (waiting_for_first_edge) {
1783                 // Just break out of loop after an initial time-out (tag is probably not available)
1784                 if (periods == 0) break;
1785                 if (tag_modulation == 0) {
1786                     // hitag replies always start with 11111 == 1010101010, if we see 0
1787                     // it means we missed the first period, e.g. if the signal never crossed 0 since reader signal
1788                     // so let's add it:
1789                     nrz_samples[nrzs++] = tag_modulation ^ 1;
1790                     // Register the number of periods that have passed
1791                     // we missed the begin of response but we know it happened one period of 16 earlier
1792                     response_start += periods - 16;
1793                     response_duration = response_start;
1794                 } else {
1795                     // Register the number of periods that have passed
1796                     response_start += periods;
1797                     response_duration = response_start;
1798                 }
1799                 // Indicate that we have dealt with the first edge
1800                 waiting_for_first_edge = false;
1801                 // The first edge is always a single NRZ bit, force periods on 16
1802                 periods = 16;
1803                 // We have received more than 0 periods, so we have detected a tag response
1804                 detected_tag_modulation = true;
1805             } else {
1806                 // The function lf_count_edge_periods() returns 0 when a time-out occurs
1807                 if (periods == 0) {
1808                     DBG Dbprintf("Detected timeout after [%d] nrz samples", nrzs);
1809                     break;
1810                 }
1811             }
1812             // Evaluate the number of periods before the next edge
1813             if (periods > 24 && periods <= 64) {
1814                 // Detected two sequential equal bits and a modulation switch
1815                 // NRZ modulation: (11 => --|) or (11 __|)
1816                 nrz_samples[nrzs++] = tag_modulation;
1817                 nrz_samples[nrzs++] = tag_modulation;
1818                 response_duration += periods;
1819                 // Invert tag modulation state
1820                 tag_modulation ^= 1;
1821             } else if (periods > 0 && periods <= 24) {
1822                 // Detected one bit and a modulation switch
1823                 // NRZ modulation: (1 => -|) or (0 _|)
1824                 nrz_samples[nrzs++] = tag_modulation;
1825                 response_duration += periods;
1826                 tag_modulation ^= 1;
1827             } else {
1828                 // The function lf_count_edge_periods() returns > 64 periods, this is not a valid number periods
1829                 DBG Dbprintf("Detected unexpected period count: %d", periods);
1830                 break;
1831             }
1832         }
1833
1834         // Store the TX frame, we do this now at this point, to avoid delay in processing
1835         // and to be able to overwrite the first samples with the trace (since they currently
1836         // still use the same memory space)
1837         if (txlen > 0) {
1838             LogTrace(tx, nbytes(txlen), command_start, command_start + command_duration, NULL, true);
1839         }
1840
1841         // Reset values for receiving frames
1842         memset(rx, 0x00, sizeof(rx));
1843         rxlen = 0;
1844
1845         // If there is no response, just repeat the loop
1846         if (!detected_tag_modulation) continue;
1847
1848         // Make sure we always have an even number of samples. This fixes the problem
1849         // of ending the manchester decoding with a zero. See the example below where
1850         // the '|' character is end of modulation
1851         //  One at the end: ..._-|_____...
1852         // Zero at the end: ...-_|_____...
1853         // The last modulation change of a zero is not detected, but we should take
1854         // the half period in account, otherwise the demodulator will fail.
1855         if ((nrzs % 2) != 0) {
1856             nrz_samples[nrzs++] = tag_modulation;
1857         }
1858
1859         LED_B_ON();
1860
1861         // decode bitstream
1862         manrawdecode((uint8_t *)nrz_samples, &nrzs, true, 0);
1863
1864         // decode frame
1865
1866         // Verify if the header consists of five consecutive ones
1867         if (nrzs < 5) {
1868             DBG Dbprintf("Detected unexpected number of manchester decoded samples [%d]", nrzs);
1869             break;
1870         } else {
1871             size_t i;
1872             for (i = 0; i < 5; i++) {
1873                 if (nrz_samples[i] != 1) {
1874                     DBG Dbprintf("Detected incorrect header, the bit [%d] is zero instead of one, abort", i);
1875                     break;
1876                 }
1877             }
1878             if (i < 5) break;
1879         }
1880
1881         // Pack the response into a byte array
1882         for (size_t i = 5; i < nrzs && rxlen < (sizeof(rx) << 3); i++) {
1883             uint8_t bit = nrz_samples[i];
1884             if (bit > 1) { // When Manchester detects impossible symbol it writes "7"
1885                 DBG Dbprintf("Error in Manchester decoding, abort");
1886                 break;
1887             }
1888             rx[rxlen >> 3] |= bit << (7 - (rxlen % 8));
1889             rxlen++;
1890         }
1891
1892         if (rxlen % 8 == 1) // skip spurious bit
1893             rxlen--;
1894
1895         // Check if frame was captured and store it
1896         if (rxlen > 0) {
1897
1898             LogTrace(rx, nbytes(rxlen), response_start, response_start + response_duration, NULL, false);
1899
1900 // TODO when using cumulative time for command_start, pm3 doesn't reply anymore, e.g. on lf hitag reader --23 -k 4F4E4D494B52
1901 // Use delta time?
1902 //            command_start = response_start + response_duration;
1903             command_start = 0;
1904             nrzs = 0;
1905         }
1906     }
1907
1908 out:
1909     lf_finalize();
1910
1911     // release allocated memory from BigBuff.
1912     BigBuf_free();
1913
1914     if (bSuccessful)
1915         reply_mix(CMD_ACK, bSuccessful, 0, 0, (uint8_t *)tag.sectors, tag_size);
1916     else
1917         reply_mix(CMD_ACK, bSuccessful, 0, 0, 0, 0);
1918 }
1919
1920 void WriterHitag(hitag_function htf, hitag_data *htd, int page) {
1921
1922     uint32_t command_start = 0;
1923     uint32_t command_duration = 0;
1924     uint32_t response_start = 0;
1925     uint32_t response_duration = 0;
1926     uint8_t rx[HITAG_FRAME_LEN];
1927     size_t rxlen = 0;
1928     uint8_t txbuf[HITAG_FRAME_LEN];
1929     uint8_t *tx = txbuf;
1930     size_t txlen = 0;
1931
1932     int t_wait_1 = 204;
1933     int t_wait_1_guard = 8;
1934     int t_wait_2 = 128;
1935     size_t tag_size = 48;
1936
1937     bool bStop = false;
1938
1939     // Raw demodulation/decoding by sampling edge periods
1940     size_t periods = 0;
1941
1942     // Reset the return status
1943     bSuccessful = false;
1944     bCrypto = false;
1945
1946     // Clean up trace and prepare it for storing frames
1947     set_tracing(true);
1948     clear_trace();
1949
1950
1951     // Check configuration
1952     switch (htf) {
1953         case WHT2F_CRYPTO: {
1954             DbpString("Authenticating using key:");
1955             memcpy(key, htd->crypto.key, 6); //HACK; 4 or 6??  I read both in the code.
1956             memcpy(writedata, htd->crypto.data, 4);
1957             Dbhexdump(6, key, false);
1958             blocknr = page;
1959             bCrypto = false;
1960             bAuthenticating = false;
1961             writestate = WRITE_STATE_START;
1962         }
1963         break;
1964         case WHT2F_PASSWORD: {
1965             DbpString("Authenticating using password:");
1966             memcpy(password, htd->pwd.password, 4);
1967             memcpy(writedata, htd->crypto.data, 4);
1968             Dbhexdump(4, password, false);
1969             blocknr = page;
1970             bPwd = false;
1971             bAuthenticating = false;
1972             writestate = WRITE_STATE_START;
1973         }
1974         break;
1975         default: {
1976             Dbprintf("Error, unknown function: %d", htf);
1977             return;
1978         }
1979         break;
1980     }
1981
1982     LED_D_ON();
1983
1984     hitag2_init();
1985
1986     // init as reader
1987     lf_init(true, false);
1988     FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1989
1990     // Tag specific configuration settings (sof, timings, etc.)
1991 // TODO HTS
1992     /*    if (htf <= HTS_LAST_CMD) {
1993             // hitagS settings
1994             t_wait_1 = 204;
1995             t_wait_2 = 128;
1996             //tag_size = 256;
1997             flipped_bit = 0;
1998             tag_size = 8;
1999             DbpString("Configured for hitagS writer");
2000         } else */
2001 // TODO HT1
2002     /*    if (htf <= HT1_LAST_CMD) {
2003             // hitag1 settings
2004             t_wait_1 = 204;
2005             t_wait_2 = 128;
2006             tag_size = 256;
2007             flipped_bit = 0;
2008             DbpString("Configured for hitag1 writer");
2009         } else */
2010 //    if (htf <= HT2_LAST_CMD) {
2011     // hitag2 settings
2012     t_wait_1 = HITAG_T_WAIT_1_MIN;
2013     t_wait_2 = HITAG_T_WAIT_2_MIN;
2014     tag_size = 48;
2015     DbpString("Configured for hitag2 writer");
2016 //    }
2017
2018     uint8_t tag_modulation;
2019     size_t max_nrzs = (8 * HITAG_FRAME_LEN + 5) * 2; // up to 2 nrzs per bit
2020     uint8_t nrz_samples[max_nrzs];
2021     size_t nrzs = 0;
2022     int16_t checked = 0;
2023     uint32_t signal_size = 10000;
2024     bool turn_on = true;
2025
2026     while (bStop == false && BUTTON_PRESS() == false) {
2027
2028         // use malloc
2029         initSampleBufferEx(&signal_size, true);
2030
2031         // only every 4000th times, in order to save time when collecting samples.
2032         if (checked == 4000) {
2033             if (data_available()) {
2034                 checked = -1;
2035                 break;
2036             } else {
2037                 checked = 0;
2038             }
2039         }
2040         ++checked;
2041
2042         WDT_HIT();
2043
2044         // By default reset the transmission buffer
2045         tx = txbuf;
2046         switch (htf) {
2047             case WHT2F_CRYPTO: {
2048                 bStop = !hitag2_crypto(rx, rxlen, tx, &txlen, true);
2049                 break;
2050             }
2051             case WHT2F_PASSWORD: {
2052                 bStop = !hitag2_password(rx, rxlen, tx, &txlen, true);
2053                 break;
2054             }
2055             default: {
2056                 Dbprintf("Error, unknown function: %d", htf);
2057                 goto out;
2058             }
2059         }
2060
2061         if (bStop) break;
2062         if (turn_on) {
2063             // Wait 50ms with field off to be sure the transponder gets reset
2064             SpinDelay(50);
2065             FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_ADC | FPGA_LF_ADC_READER_FIELD);
2066             turn_on = false;
2067             // Wait with field on to be in "Wait for START_AUTH" timeframe
2068             lf_wait_periods(HITAG_T_WAIT_POWERUP + HITAG_T_WAIT_START_AUTH_MAX / 4);
2069             command_start += HITAG_T_WAIT_POWERUP + HITAG_T_WAIT_START_AUTH_MAX / 4;
2070         } else {
2071             // Wait for t_wait_2 carrier periods after the last tag bit before transmitting,
2072             lf_wait_periods(t_wait_2);
2073             command_start += t_wait_2;
2074         }
2075
2076         // Transmit the reader frame
2077         command_duration = hitag_reader_send_frame(tx, txlen);
2078
2079         response_start = command_start + command_duration;
2080
2081         // Let the antenna and ADC values settle
2082         // And find the position where edge sampling should start
2083         lf_wait_periods(t_wait_1 - t_wait_1_guard);
2084         response_start += t_wait_1 - t_wait_1_guard;
2085
2086         // Keep administration of the first edge detection
2087         bool waiting_for_first_edge = true;
2088
2089         // Did we detected any modulaiton at all
2090         bool detected_tag_modulation = false;
2091
2092         // Use the current modulation state as starting point
2093         tag_modulation = lf_get_tag_modulation();
2094
2095         // Reset the number of NRZ samples and use edge detection to detect them
2096         nrzs = 0;
2097         while (nrzs < max_nrzs) {
2098             // Get the timing of the next edge in number of wave periods
2099             periods = lf_count_edge_periods(128);
2100
2101             // Are we dealing with the first incoming edge
2102             if (waiting_for_first_edge) {
2103                 // Just break out of loop after an initial time-out (tag is probably not available)
2104                 if (periods == 0) break;
2105                 if (tag_modulation == 0) {
2106                     // hitag replies always start with 11111 == 1010101010, if we see 0
2107                     // it means we missed the first period, e.g. if the signal never crossed 0 since reader signal
2108                     // so let's add it:
2109                     nrz_samples[nrzs++] = tag_modulation ^ 1;
2110                     // Register the number of periods that have passed
2111                     // we missed the begin of response but we know it happened one period of 16 earlier
2112                     response_start += periods - 16;
2113                     response_duration = response_start;
2114                 } else {
2115                     // Register the number of periods that have passed
2116                     response_start += periods;
2117                     response_duration = response_start;
2118                 }
2119                 // Indicate that we have dealt with the first edge
2120                 waiting_for_first_edge = false;
2121                 // The first edge is always a single NRZ bit, force periods on 16
2122                 periods = 16;
2123                 // We have received more than 0 periods, so we have detected a tag response
2124                 detected_tag_modulation = true;
2125             } else {
2126                 // The function lf_count_edge_periods() returns 0 when a time-out occurs
2127                 if (periods == 0) {
2128                     //Dbprintf("Detected timeout after [%d] nrz samples", nrzs);
2129                     break;
2130                 }
2131             }
2132             // Evaluate the number of periods before the next edge
2133             if (periods > 24 && periods <= 64) {
2134                 // Detected two sequential equal bits and a modulation switch
2135                 // NRZ modulation: (11 => --|) or (11 __|)
2136                 nrz_samples[nrzs++] = tag_modulation;
2137                 nrz_samples[nrzs++] = tag_modulation;
2138                 response_duration += periods;
2139                 // Invert tag modulation state
2140                 tag_modulation ^= 1;
2141             } else if (periods > 0 && periods <= 24) {
2142                 // Detected one bit and a modulation switch
2143                 // NRZ modulation: (1 => -|) or (0 _|)
2144                 nrz_samples[nrzs++] = tag_modulation;
2145                 response_duration += periods;
2146                 tag_modulation ^= 1;
2147             } else {
2148                 // The function lf_count_edge_periods() returns > 64 periods, this is not a valid number periods
2149                 //Dbprintf("Detected unexpected period count: %d", periods);
2150                 break;
2151             }
2152         }
2153
2154         // Wait some extra time for flash to be programmed
2155         //
2156
2157         // Store the TX frame, we do this now at this point, to avoid delay in processing
2158         // and to be able to overwrite the first samples with the trace (since they currently
2159         // still use the same memory space)
2160         if (txlen > 0) {
2161             LogTrace(tx, nbytes(txlen), command_start, command_start + command_duration, NULL, true);
2162         }
2163
2164         // Reset values for receiving frames
2165         memset(rx, 0x00, sizeof(rx));
2166         rxlen = 0;
2167
2168         // If there is no response, just repeat the loop
2169         if (!detected_tag_modulation) continue;
2170
2171         // Make sure we always have an even number of samples. This fixes the problem
2172         // of ending the manchester decoding with a zero. See the example below where
2173         // the '|' character is end of modulation
2174         //  One at the end: ..._-|_____...
2175         // Zero at the end: ...-_|_____...
2176         // The last modulation change of a zero is not detected, but we should take
2177         // the half period in account, otherwise the demodulator will fail.
2178         if ((nrzs % 2) != 0) {
2179             nrz_samples[nrzs++] = tag_modulation;
2180         }
2181
2182         LED_B_ON();
2183
2184         // decode bitstream
2185         manrawdecode((uint8_t *)nrz_samples, &nrzs, true, 0);
2186
2187         // decode frame
2188
2189         // Verify if the header consists of five consecutive ones
2190         if (nrzs < 5) {
2191             break;
2192         } else {
2193             size_t i;
2194             for (i = 0; i < 5; i++) {
2195                 if (nrz_samples[i] != 1) {
2196                     Dbprintf("Detected incorrect header, the bit [%d] is zero instead of one, abort", i);
2197                     break;
2198                 }
2199             }
2200             if (i < 5) break;
2201         }
2202
2203         // Pack the response into a byte array
2204         for (size_t i = 5; i < nrzs && rxlen < (sizeof(rx) << 3); i++) {
2205             uint8_t bit = nrz_samples[i];
2206             if (bit > 1) { // When Manchester detects impossible symbol it writes "7"
2207                 break;
2208             }
2209             // >> 3 instead of div by 8
2210             rx[rxlen >> 3] |= bit << (7 - (rxlen % 8));
2211             rxlen++;
2212         }
2213
2214         if (rxlen % 8 == 1) // skip spurious bit
2215             rxlen--;
2216
2217         // Check if frame was captured and store it
2218         if (rxlen > 0) {
2219             LogTrace(rx, nbytes(rxlen), response_start, response_start + response_duration, NULL, false);
2220             command_start = 0;
2221         }
2222     }
2223
2224
2225 out:
2226     lf_finalize();
2227
2228     // release allocated memory from BigBuff.
2229     BigBuf_free();
2230
2231     reply_mix(CMD_ACK, bSuccessful, 0, 0, (uint8_t *)tag.sectors, tag_size);
2232 }