armsrc/hitag2.c

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