armsrc/hitag2.c

   1 //-----------------------------------------------------------------------------
   2 // This code is licensed to you under the terms of the GNU GPL, version 2 or,
   3 // at your option, any later version. See the LICENSE.txt file for the text of
   4 // the license.
   5 //-----------------------------------------------------------------------------
   6 // Hitag2 emulation (preliminary test version)
   7 //
   8 // (c) 2009 Henryk Plötz <henryk@ploetzli.ch>
   9 //-----------------------------------------------------------------------------
  10 // Hitag2 complete rewrite of the code
  11 // - Fixed modulation/encoding issues
  12 // - Rewrote code for transponder emulation
  13 // - Added snooping of transponder communication
  14 // - Added reader functionality
  15 //
  16 // (c) 2012 Roel Verdult
  17 //-----------------------------------------------------------------------------
  18
  19 #include "proxmark3.h"
  20 #include "apps.h"
  21 #include "util.h"
  22 #include "hitag2.h"
  23 #include "string.h"
  24 #include "BigBuf.h"
  25
  26 static bool bQuiet;
  27
  28 static bool bCrypto;
  29 static bool bAuthenticating;
  30 static bool bPwd;
  31 static bool bSuccessful;
  32
  33
  34
  35 struct hitag2_tag {
  36         uint32_t uid;
  37         enum {
  38                 TAG_STATE_RESET      = 0x01,       // Just powered up, awaiting GetSnr
  39                 TAG_STATE_ACTIVATING = 0x02 ,      // In activation phase (password mode), sent UID, awaiting reader password
  40                 TAG_STATE_ACTIVATED  = 0x03,       // Activation complete, awaiting read/write commands
  41                 TAG_STATE_WRITING    = 0x04,       // In write command, awaiting sector contents to be written
  42         } state;
  43         unsigned int active_sector;
  44         byte_t crypto_active;
  45         uint64_t cs;
  46         byte_t sectors[12][4];
  47 };
  48
  49 static struct hitag2_tag tag = {
  50     .state = TAG_STATE_RESET,
  51     .sectors = {                         // Password mode:               | Crypto mode:
  52         [0]  = { 0x02, 0x4e, 0x02, 0x20}, // UID                          | UID
  53         [1]  = { 0x4d, 0x49, 0x4b, 0x52}, // Password RWD                 | 32 bit LSB key
  54         [2]  = { 0x20, 0xf0, 0x4f, 0x4e}, // Reserved                     | 16 bit MSB key, 16 bit reserved
  55         [3]  = { 0x0e, 0xaa, 0x48, 0x54}, // Configuration, password TAG  | Configuration, password TAG
  56         [4]  = { 0x46, 0x5f, 0x4f, 0x4b}, // Data: F_OK
  57         [5]  = { 0x55, 0x55, 0x55, 0x55}, // Data: UUUU
  58         [6]  = { 0xaa, 0xaa, 0xaa, 0xaa}, // Data: ....
  59         [7]  = { 0x55, 0x55, 0x55, 0x55}, // Data: UUUU
  60         [8]  = { 0x00, 0x00, 0x00, 0x00}, // RSK Low
  61         [9]  = { 0x00, 0x00, 0x00, 0x00}, // RSK High
  62         [10] = { 0x00, 0x00, 0x00, 0x00}, // RCF
  63         [11] = { 0x00, 0x00, 0x00, 0x00}, // SYNC
  64     },
  65 };
  66
  67 // ToDo: define a meaningful maximum size for auth_table. The bigger this is, the lower will be the available memory for traces.
  68 // Historically it used to be FREE_BUFFER_SIZE, which was 2744.
  69 #define AUTH_TABLE_LENGTH 2744
  70 static byte_t* auth_table;
  71 static size_t auth_table_pos = 0;
  72 static size_t auth_table_len = AUTH_TABLE_LENGTH;
  73
  74 static byte_t password[4];
  75 static byte_t NrAr[8];
  76 static byte_t key[8];
  77 static uint64_t cipher_state;
  78
  79 /* Following is a modified version of cryptolib.com/ciphers/hitag2/ */
  80 // Software optimized 48-bit Philips/NXP Mifare Hitag2 PCF7936/46/47/52 stream cipher algorithm by I.C. Wiener 2006-2007.
  81 // For educational purposes only.
  82 // No warranties or guarantees of any kind.
  83 // This code is released into the public domain by its author.
  84
  85 // Basic macros:
  86
  87 #define u8                              uint8_t
  88 #define u32                             uint32_t
  89 #define u64                             uint64_t
  90 #define rev8(x)                 ((((x)>>7)&1)+((((x)>>6)&1)<<1)+((((x)>>5)&1)<<2)+((((x)>>4)&1)<<3)+((((x)>>3)&1)<<4)+((((x)>>2)&1)<<5)+((((x)>>1)&1)<<6)+(((x)&1)<<7))
  91 #define rev16(x)                (rev8 (x)+(rev8 (x>> 8)<< 8))
  92 #define rev32(x)                (rev16(x)+(rev16(x>>16)<<16))
  93 #define rev64(x)                (rev32(x)+(rev32(x>>32)<<32))
  94 #define bit(x,n)                (((x)>>(n))&1)
  95 #define bit32(x,n)              ((((x)[(n)>>5])>>((n)))&1)
  96 #define inv32(x,i,n)    ((x)[(i)>>5]^=((u32)(n))<<((i)&31))
  97 #define rotl64(x, n)    ((((u64)(x))<<((n)&63))+(((u64)(x))>>((0-(n))&63)))
  98
  99 // Single bit Hitag2 functions:
 100
 101 #define i4(x,a,b,c,d)   ((u32)((((x)>>(a))&1)+(((x)>>(b))&1)*2+(((x)>>(c))&1)*4+(((x)>>(d))&1)*8))
 102
 103 static const u32 ht2_f4a = 0x2C79;              // 0010 1100 0111 1001
 104 static const u32 ht2_f4b = 0x6671;              // 0110 0110 0111 0001
 105 static const u32 ht2_f5c = 0x7907287B;  // 0111 1001 0000 0111 0010 1000 0111 1011
 106
 107 static u32 _f20 (const u64 x)
 108 {
 109         u32                                     i5;
 110
 111         i5 = ((ht2_f4a >> i4 (x, 1, 2, 4, 5)) & 1)* 1
 112            + ((ht2_f4b >> i4 (x, 7,11,13,14)) & 1)* 2
 113            + ((ht2_f4b >> i4 (x,16,20,22,25)) & 1)* 4
 114            + ((ht2_f4b >> i4 (x,27,28,30,32)) & 1)* 8
 115            + ((ht2_f4a >> i4 (x,33,42,43,45)) & 1)*16;
 116
 117         return (ht2_f5c >> i5) & 1;
 118 }
 119
 120 static u64 _hitag2_init (const u64 key, const u32 serial, const u32 IV)
 121 {
 122         u32                                     i;
 123         u64                                     x = ((key & 0xFFFF) << 32) + serial;
 124
 125         for (i = 0; i < 32; i++)
 126         {
 127                 x >>= 1;
 128                 x += (u64) (_f20 (x) ^ (((IV >> i) ^ (key >> (i+16))) & 1)) << 47;
 129         }
 130         return x;
 131 }
 132
 133 static u64 _hitag2_round (u64 *state)
 134 {
 135         u64                                     x = *state;
 136
 137         x = (x >>  1) +
 138          ((((x >>  0) ^ (x >>  2) ^ (x >>  3) ^ (x >>  6)
 139           ^ (x >>  7) ^ (x >>  8) ^ (x >> 16) ^ (x >> 22)
 140           ^ (x >> 23) ^ (x >> 26) ^ (x >> 30) ^ (x >> 41)
 141           ^ (x >> 42) ^ (x >> 43) ^ (x >> 46) ^ (x >> 47)) & 1) << 47);
 142
 143         *state = x;
 144         return _f20 (x);
 145 }
 146
 147 static u32 _hitag2_byte (u64 * x)
 148 {
 149         u32                                     i, c;
 150
 151         for (i = 0, c = 0; i < 8; i++) c += (u32) _hitag2_round (x) << (i^7);
 152         return c;
 153 }
 154
 155 static int hitag2_reset(void)
 156 {
 157         tag.state = TAG_STATE_RESET;
 158         tag.crypto_active = 0;
 159         return 0;
 160 }
 161
 162 static int hitag2_init(void)
 163 {
 164 //      memcpy(&tag, &resetdata, sizeof(tag));
 165         hitag2_reset();
 166         return 0;
 167 }
 168
 169 static void hitag2_cipher_reset(struct hitag2_tag *tag, const byte_t *iv)
 170 {
 171         uint64_t key =  ((uint64_t)tag->sectors[2][2]) |
 172                   ((uint64_t)tag->sectors[2][3] << 8) |
 173                   ((uint64_t)tag->sectors[1][0] << 16) |
 174                   ((uint64_t)tag->sectors[1][1] << 24) |
 175                   ((uint64_t)tag->sectors[1][2] << 32) |
 176                   ((uint64_t)tag->sectors[1][3] << 40);
 177         uint32_t uid =  ((uint32_t)tag->sectors[0][0]) |
 178                   ((uint32_t)tag->sectors[0][1] << 8) |
 179                   ((uint32_t)tag->sectors[0][2] << 16) |
 180                   ((uint32_t)tag->sectors[0][3] << 24);
 181         uint32_t iv_ = (((uint32_t)(iv[0]))) |
 182                         (((uint32_t)(iv[1])) << 8) |
 183                         (((uint32_t)(iv[2])) << 16) |
 184                         (((uint32_t)(iv[3])) << 24);
 185         tag->cs = _hitag2_init(rev64(key), rev32(uid), rev32(iv_));
 186 }
 187
 188 static int hitag2_cipher_authenticate(uint64_t* cs, const byte_t *authenticator_is)
 189 {
 190         byte_t authenticator_should[4];
 191         authenticator_should[0] = ~_hitag2_byte(cs);
 192         authenticator_should[1] = ~_hitag2_byte(cs);
 193         authenticator_should[2] = ~_hitag2_byte(cs);
 194         authenticator_should[3] = ~_hitag2_byte(cs);
 195         return (memcmp(authenticator_should, authenticator_is, 4) == 0);
 196 }
 197
 198 static int hitag2_cipher_transcrypt(uint64_t* cs, byte_t *data, unsigned int bytes, unsigned int bits)
 199 {
 200         int i;
 201         for(i=0; i<bytes; i++) data[i] ^= _hitag2_byte(cs);
 202         for(i=0; i<bits; i++) data[bytes] ^= _hitag2_round(cs) << (7-i);
 203         return 0;
 204 }
 205
 206 // Sam7s has several timers, we will use the source TIMER_CLOCK1 (aka AT91C_TC_CLKS_TIMER_DIV1_CLOCK)
 207 // TIMER_CLOCK1 = MCK/2, MCK is running at 48 MHz, Timer is running at 48/2 = 24 MHz
 208 // Hitag units (T0) have duration of 8 microseconds (us), which is 1/125000 per second (carrier)
 209 // T0 = TIMER_CLOCK1 / 125000 = 192
 210 #define T0 192
 211
 212 #define SHORT_COIL()    LOW(GPIO_SSC_DOUT)
 213 #define OPEN_COIL()             HIGH(GPIO_SSC_DOUT)
 214
 215 #define HITAG_FRAME_LEN 20
 216 #define HITAG_T_STOP  36 /* T_EOF should be > 36 */
 217 #define HITAG_T_LOW             8  /* T_LOW should be 4..10 */
 218 #define HITAG_T_0_MIN 15 /* T[0] should be 18..22 */
 219 #define HITAG_T_1_MIN 25 /* T[1] should be 26..30 */
 220 //#define HITAG_T_EOF   40 /* T_EOF should be > 36 */
 221 #define HITAG_T_EOF   80         /* T_EOF should be > 36 */
 222 #define HITAG_T_WAIT_1 200 /* T_wresp should be 199..206 */
 223 #define HITAG_T_WAIT_2 90 /* T_wresp should be 199..206 */
 224 #define HITAG_T_WAIT_MAX 300 /* bit more than HITAG_T_WAIT_1 + HITAG_T_WAIT_2 */
 225
 226 #define HITAG_T_TAG_ONE_HALF_PERIOD                     10
 227 #define HITAG_T_TAG_TWO_HALF_PERIOD                     25
 228 #define HITAG_T_TAG_THREE_HALF_PERIOD           41
 229 #define HITAG_T_TAG_FOUR_HALF_PERIOD    57
 230
 231 #define HITAG_T_TAG_HALF_PERIOD                                 16
 232 #define HITAG_T_TAG_FULL_PERIOD                                 32
 233
 234 #define HITAG_T_TAG_CAPTURE_ONE_HALF            13
 235 #define HITAG_T_TAG_CAPTURE_TWO_HALF            25
 236 #define HITAG_T_TAG_CAPTURE_THREE_HALF  41
 237 #define HITAG_T_TAG_CAPTURE_FOUR_HALF   57
 238
 239
 240 static void hitag_send_bit(int bit) {
 241         LED_A_ON();
 242         // Reset clock for the next bit
 243         AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG;
 244
 245         // Fixed modulation, earlier proxmark version used inverted signal
 246         if(bit == 0) {
 247                 // Manchester: Unloaded, then loaded |__--|
 248                 LOW(GPIO_SSC_DOUT);
 249                 while(AT91C_BASE_TC0->TC_CV < T0*HITAG_T_TAG_HALF_PERIOD);
 250                 HIGH(GPIO_SSC_DOUT);
 251                 while(AT91C_BASE_TC0->TC_CV < T0*HITAG_T_TAG_FULL_PERIOD);
 252         } else {
 253                 // Manchester: Loaded, then unloaded |--__|
 254                 HIGH(GPIO_SSC_DOUT);
 255                 while(AT91C_BASE_TC0->TC_CV < T0*HITAG_T_TAG_HALF_PERIOD);
 256                 LOW(GPIO_SSC_DOUT);
 257                 while(AT91C_BASE_TC0->TC_CV < T0*HITAG_T_TAG_FULL_PERIOD);
 258         }
 259         LED_A_OFF();
 260 }
 261
 262 static void hitag_send_frame(const byte_t* frame, size_t frame_len)
 263 {
 264         // Send start of frame
 265         for(size_t i=0; i<5; i++) {
 266                 hitag_send_bit(1);
 267         }
 268
 269         // Send the content of the frame
 270         for(size_t i=0; i<frame_len; i++) {
 271                 hitag_send_bit((frame[i/8] >> (7-(i%8)))&1);
 272         }
 273
 274         // Drop the modulation
 275         LOW(GPIO_SSC_DOUT);
 276 }
 277
 278
 279 static void hitag2_handle_reader_command(byte_t* rx, const size_t rxlen, byte_t* tx, size_t* txlen)
 280 {
 281         byte_t rx_air[HITAG_FRAME_LEN];
 282
 283         // Copy the (original) received frame how it is send over the air
 284         memcpy(rx_air,rx,nbytes(rxlen));
 285
 286         if(tag.crypto_active) {
 287                 hitag2_cipher_transcrypt(&(tag.cs),rx,rxlen/8,rxlen%8);
 288         }
 289
 290         // Reset the transmission frame length
 291         *txlen = 0;
 292
 293         // Try to find out which command was send by selecting on length (in bits)
 294         switch (rxlen) {
 295                 // Received 11000 from the reader, request for UID, send UID
 296                 case 05: {
 297                         // Always send over the air in the clear plaintext mode
 298                         if(rx_air[0] != 0xC0) {
 299                                 // Unknown frame ?
 300                                 return;
 301                         }
 302                         *txlen = 32;
 303                         memcpy(tx,tag.sectors[0],4);
 304                         tag.crypto_active = 0;
 305                 }
 306                 break;
 307
 308                 // Read/Write command: ..xx x..y  yy with yyy == ~xxx, xxx is sector number
 309                 case 10: {
 310                         unsigned int sector = (~( ((rx[0]<<2)&0x04) | ((rx[1]>>6)&0x03) ) & 0x07);
 311                         // Verify complement of sector index
 312                         if(sector != ((rx[0]>>3)&0x07)) {
 313                                 //DbpString("Transmission error (read/write)");
 314                                 return;
 315                         }
 316
 317                         switch (rx[0] & 0xC6) {
 318                                 // Read command: 11xx x00y
 319                                 case 0xC0:
 320                                         memcpy(tx,tag.sectors[sector],4);
 321                                         *txlen = 32;
 322                                 break;
 323
 324                                  // Inverted Read command: 01xx x10y
 325                                 case 0x44:
 326                                         for (size_t i=0; i<4; i++) {
 327                                                 tx[i] = tag.sectors[sector][i] ^ 0xff;
 328                                         }
 329                                         *txlen = 32;
 330                                 break;
 331
 332                                 // Write command: 10xx x01y
 333                                 case 0x82:
 334                                         // Prepare write, acknowledge by repeating command
 335                                         memcpy(tx,rx,nbytes(rxlen));
 336                                         *txlen = rxlen;
 337                                         tag.active_sector = sector;
 338                                         tag.state=TAG_STATE_WRITING;
 339                                 break;
 340
 341                                 // Unknown command
 342                                 default:
 343                                         Dbprintf("Uknown command: %02x %02x",rx[0],rx[1]);
 344                                         return;
 345                                 break;
 346                         }
 347                 }
 348                 break;
 349
 350                 // Writing data or Reader password
 351                 case 32: {
 352                         if(tag.state == TAG_STATE_WRITING) {
 353                                 // These are the sector contents to be written. We don't have to do anything else.
 354                                 memcpy(tag.sectors[tag.active_sector],rx,nbytes(rxlen));
 355                                 tag.state=TAG_STATE_RESET;
 356                                 return;
 357                         } else {
 358                                 // Received RWD password, respond with configuration and our password
 359                                 if(memcmp(rx,tag.sectors[1],4) != 0) {
 360                                         DbpString("Reader password is wrong");
 361                                         return;
 362                                 }
 363                                 *txlen = 32;
 364                                 memcpy(tx,tag.sectors[3],4);
 365                         }
 366                 }
 367                 break;
 368
 369                 // Received RWD authentication challenge and respnse
 370                 case 64: {
 371                         // Store the authentication attempt
 372                         if (auth_table_len < (AUTH_TABLE_LENGTH-8)) {
 373                                 memcpy(auth_table+auth_table_len,rx,8);
 374                                 auth_table_len += 8;
 375                         }
 376
 377                         // Reset the cipher state
 378                         hitag2_cipher_reset(&tag,rx);
 379                         // Check if the authentication was correct
 380                         if(!hitag2_cipher_authenticate(&(tag.cs),rx+4)) {
 381                                 // The reader failed to authenticate, do nothing
 382                                 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]);
 383                                 return;
 384                         }
 385                         // Succesful, but commented out reporting back to the Host, this may delay to much.
 386                         // Dbprintf("auth: %02x%02x%02x%02x%02x%02x%02x%02x OK!",rx[0],rx[1],rx[2],rx[3],rx[4],rx[5],rx[6],rx[7]);
 387
 388                         // Activate encryption algorithm for all further communication
 389                         tag.crypto_active = 1;
 390
 391                         // Use the tag password as response
 392                         memcpy(tx,tag.sectors[3],4);
 393                         *txlen = 32;
 394                 }
 395                 break;
 396         }
 397
 398 //      LogTraceHitag(rx,rxlen,0,0,false);
 399 //      LogTraceHitag(tx,*txlen,0,0,true);
 400
 401         if(tag.crypto_active) {
 402                 hitag2_cipher_transcrypt(&(tag.cs), tx, *txlen/8, *txlen%8);
 403         }
 404 }
 405
 406 static void hitag_reader_send_bit(int bit) {
 407         LED_A_ON();
 408         // Reset clock for the next bit
 409         AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG;
 410
 411         // Binary puls length modulation (BPLM) is used to encode the data stream
 412         // This means that a transmission of a one takes longer than that of a zero
 413
 414         // Enable modulation, which means, drop the the field
 415         HIGH(GPIO_SSC_DOUT);
 416
 417         // Wait for 4-10 times the carrier period
 418         while(AT91C_BASE_TC0->TC_CV < T0*6);
 419         //      SpinDelayUs(8*8);
 420
 421         // Disable modulation, just activates the field again
 422         LOW(GPIO_SSC_DOUT);
 423
 424         if(bit == 0) {
 425                 // Zero bit: |_-|
 426                 while(AT91C_BASE_TC0->TC_CV < T0*22);
 427                 //              SpinDelayUs(16*8);
 428         } else {
 429                 // One bit: |_--|
 430                 while(AT91C_BASE_TC0->TC_CV < T0*28);
 431                 //              SpinDelayUs(22*8);
 432         }
 433         LED_A_OFF();
 434 }
 435
 436
 437 static void hitag_reader_send_frame(const byte_t* frame, size_t frame_len)
 438 {
 439         // Send the content of the frame
 440         for(size_t i=0; i<frame_len; i++) {
 441                 hitag_reader_send_bit((frame[i/8] >> (7-(i%8)))&1);
 442         }
 443         // Send EOF
 444         AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG;
 445         // Enable modulation, which means, drop the the field
 446         HIGH(GPIO_SSC_DOUT);
 447         // Wait for 4-10 times the carrier period
 448         while(AT91C_BASE_TC0->TC_CV < T0*6);
 449         // Disable modulation, just activates the field again
 450         LOW(GPIO_SSC_DOUT);
 451 }
 452
 453 size_t blocknr;
 454
 455 static bool hitag2_password(byte_t* rx, const size_t rxlen, byte_t* tx, size_t* txlen) {
 456         // Reset the transmission frame length
 457         *txlen = 0;
 458
 459         // Try to find out which command was send by selecting on length (in bits)
 460         switch (rxlen) {
 461                 // No answer, try to resurrect
 462                 case 0: {
 463                         // Stop if there is no answer (after sending password)
 464                         if (bPwd) {
 465                                 DbpString("Password failed!");
 466                                 return false;
 467                         }
 468                         *txlen = 5;
 469                         memcpy(tx,"\xc0",nbytes(*txlen));
 470                 } break;
 471
 472                 // Received UID, tag password
 473                 case 32: {
 474                         if (!bPwd) {
 475                                 *txlen = 32;
 476                                 memcpy(tx,password,4);
 477                                 bPwd = true;
 478         memcpy(tag.sectors[blocknr],rx,4);
 479         blocknr++;
 480                         } else {
 481
 482                         if(blocknr == 1){
 483                                 //store password in block1, the TAG answers with Block3, but we need the password in memory
 484                                 memcpy(tag.sectors[blocknr],tx,4);
 485                         }else{
 486                                 memcpy(tag.sectors[blocknr],rx,4);
 487                         }
 488
 489                         blocknr++;
 490                         if (blocknr > 7) {
 491                           DbpString("Read succesful!");
 492         bSuccessful = true;
 493                           return false;
 494                         }
 495                         *txlen = 10;
 496                         tx[0] = 0xc0 | (blocknr << 3) | ((blocknr^7) >> 2);
 497                         tx[1] = ((blocknr^7) << 6);
 498                         }
 499                 } break;
 500
 501                 // Unexpected response
 502     default: {
 503                         Dbprintf("Uknown frame length: %d",rxlen);
 504                         return false;
 505                 } break;
 506         }
 507         return true;
 508 }
 509
 510 static bool hitag2_crypto(byte_t* rx, const size_t rxlen, byte_t* tx, size_t* txlen) {
 511         // Reset the transmission frame length
 512         *txlen = 0;
 513
 514   if(bCrypto) {
 515                 hitag2_cipher_transcrypt(&cipher_state,rx,rxlen/8,rxlen%8);
 516         }
 517
 518         // Try to find out which command was send by selecting on length (in bits)
 519         switch (rxlen) {
 520       // No answer, try to resurrect
 521                 case 0: {
 522                         // Stop if there is no answer while we are in crypto mode (after sending NrAr)
 523                         if (bCrypto) {
 524         // Failed during authentication
 525         if (bAuthenticating) {
 526           DbpString("Authentication failed!");
 527           return false;
 528         } else {
 529           // Failed reading a block, could be (read/write) locked, skip block and re-authenticate
 530           if (blocknr == 1) {
 531             // Write the low part of the key in memory
 532             memcpy(tag.sectors[1],key+2,4);
 533           } else if (blocknr == 2) {
 534             // Write the high part of the key in memory
 535             tag.sectors[2][0] = 0x00;
 536             tag.sectors[2][1] = 0x00;
 537             tag.sectors[2][2] = key[0];
 538             tag.sectors[2][3] = key[1];
 539           } else {
 540             // Just put zero's in the memory (of the unreadable block)
 541             memset(tag.sectors[blocknr],0x00,4);
 542           }
 543           blocknr++;
 544           bCrypto = false;
 545         }
 546                         } else {
 547         *txlen = 5;
 548         memcpy(tx,"\xc0",nbytes(*txlen));
 549       }
 550                 } break;
 551
 552       // Received UID, crypto tag answer
 553                 case 32: {
 554                         if (!bCrypto) {
 555         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;
 556         uint32_t ui32uid = rx[0] | ((uint32_t)rx[1]) << 8 | ((uint32_t)rx[2]) << 16 | ((uint32_t)rx[3]) << 24;
 557         cipher_state = _hitag2_init(rev64(ui64key), rev32(ui32uid), 0);
 558         memset(tx,0x00,4);
 559         memset(tx+4,0xff,4);
 560         hitag2_cipher_transcrypt(&cipher_state,tx+4,4,0);
 561                                 *txlen = 64;
 562                                 bCrypto = true;
 563         bAuthenticating = true;
 564                         } else {
 565         // Check if we received answer tag (at)
 566         if (bAuthenticating) {
 567           bAuthenticating = false;
 568         } else {
 569           // Store the received block
 570           memcpy(tag.sectors[blocknr],rx,4);
 571           blocknr++;
 572         }
 573         if (blocknr > 7) {
 574           DbpString("Read succesful!");
 575           bSuccessful = true;
 576           return false;
 577         }
 578         *txlen = 10;
 579         tx[0] = 0xc0 | (blocknr << 3) | ((blocknr^7) >> 2);
 580         tx[1] = ((blocknr^7) << 6);
 581                         }
 582                 } break;
 583
 584       // Unexpected response
 585                 default: {
 586                         Dbprintf("Uknown frame length: %d",rxlen);
 587                         return false;
 588                 } break;
 589         }
 590
 591
 592   if(bCrypto) {
 593     // We have to return now to avoid double encryption
 594     if (!bAuthenticating) {
 595       hitag2_cipher_transcrypt(&cipher_state,tx,*txlen/8,*txlen%8);
 596     }
 597         }
 598
 599         return true;
 600 }
 601
 602
 603 static bool hitag2_authenticate(byte_t* rx, const size_t rxlen, byte_t* tx, size_t* txlen) {
 604         // Reset the transmission frame length
 605         *txlen = 0;
 606
 607         // Try to find out which command was send by selecting on length (in bits)
 608         switch (rxlen) {
 609                 // No answer, try to resurrect
 610                 case 0: {
 611                         // Stop if there is no answer while we are in crypto mode (after sending NrAr)
 612                         if (bCrypto) {
 613                                 DbpString("Authentication failed!");
 614                                 return false;
 615                         }
 616                         *txlen = 5;
 617                         memcpy(tx,"\xc0",nbytes(*txlen));
 618                 } break;
 619
 620                 // Received UID, crypto tag answer
 621                 case 32: {
 622                         if (!bCrypto) {
 623                                 *txlen = 64;
 624                                 memcpy(tx,NrAr,8);
 625                                 bCrypto = true;
 626                         } else {
 627                                 DbpString("Authentication succesful!");
 628                                 // We are done... for now
 629                                 return false;
 630                         }
 631                 } break;
 632
 633                 // Unexpected response
 634                 default: {
 635                         Dbprintf("Uknown frame length: %d",rxlen);
 636                         return false;
 637                 } break;
 638         }
 639
 640         return true;
 641 }
 642
 643
 644 static bool hitag2_test_auth_attempts(byte_t* rx, const size_t rxlen, byte_t* tx, size_t* txlen) {
 645
 646         // Reset the transmission frame length
 647         *txlen = 0;
 648
 649         // Try to find out which command was send by selecting on length (in bits)
 650         switch (rxlen) {
 651                         // No answer, try to resurrect
 652                 case 0: {
 653                         // Stop if there is no answer while we are in crypto mode (after sending NrAr)
 654                         if (bCrypto) {
 655                                 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]);
 656
 657                                 // Removing failed entry from authentiations table
 658                                 memcpy(auth_table+auth_table_pos,auth_table+auth_table_pos+8,8);
 659                                 auth_table_len -= 8;
 660
 661                                 // Return if we reached the end of the authentications table
 662                                 bCrypto = false;
 663                                 if (auth_table_pos == auth_table_len) {
 664                                         return false;
 665                                 }
 666
 667                                 // Copy the next authentication attempt in row (at the same position, b/c we removed last failed entry)
 668                                 memcpy(NrAr,auth_table+auth_table_pos,8);
 669                         }
 670                         *txlen = 5;
 671                         memcpy(tx,"\xc0",nbytes(*txlen));
 672                 }       break;
 673
 674                         // Received UID, crypto tag answer, or read block response
 675                 case 32: {
 676                         if (!bCrypto) {
 677                                 *txlen = 64;
 678                                 memcpy(tx,NrAr,8);
 679                                 bCrypto = true;
 680                         } else {
 681                                 Dbprintf("auth: %02x%02x%02x%02x%02x%02x%02x%02x OK",NrAr[0],NrAr[1],NrAr[2],NrAr[3],NrAr[4],NrAr[5],NrAr[6],NrAr[7]);
 682                                 bCrypto = false;
 683                                 if ((auth_table_pos+8) == auth_table_len) {
 684                                         return false;
 685                                 }
 686                                 auth_table_pos += 8;
 687                                 memcpy(NrAr,auth_table+auth_table_pos,8);
 688                         }
 689                 } break;
 690
 691                 default: {
 692                         Dbprintf("Uknown frame length: %d",rxlen);
 693                         return false;
 694                 } break;
 695         }
 696
 697         return true;
 698 }
 699
 700
 701 void SnoopHitag(uint32_t type) {
 702         int frame_count;
 703         int response;
 704         int overflow;
 705         bool rising_edge;
 706         bool reader_frame;
 707         int lastbit;
 708         bool bSkip;
 709         int tag_sof;
 710         byte_t rx[HITAG_FRAME_LEN];
 711         size_t rxlen=0;
 712
 713         auth_table_len = 0;
 714         auth_table_pos = 0;
 715         BigBuf_free();
 716     auth_table = (byte_t *)BigBuf_malloc(AUTH_TABLE_LENGTH);
 717         memset(auth_table, 0x00, AUTH_TABLE_LENGTH);
 718
 719         // Clean up trace and prepare it for storing frames
 720         set_tracing(TRUE);
 721         clear_trace();
 722
 723         DbpString("Starting Hitag2 snoop");
 724         LED_D_ON();
 725
 726         // Set up eavesdropping mode, frequency divisor which will drive the FPGA
 727         // and analog mux selection.
 728         FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
 729         FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_EDGE_DETECT  | FPGA_LF_EDGE_DETECT_TOGGLE_MODE);
 730         FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
 731         SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
 732         RELAY_OFF();
 733
 734         // Configure output pin that is connected to the FPGA (for modulating)
 735         AT91C_BASE_PIOA->PIO_OER = GPIO_SSC_DOUT;
 736         AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT;
 737
 738         // Disable modulation, we are going to eavesdrop, not modulate ;)
 739         LOW(GPIO_SSC_DOUT);
 740
 741         // Enable Peripheral Clock for TIMER_CLOCK1, used to capture edges of the reader frames
 742         AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_TC1);
 743         AT91C_BASE_PIOA->PIO_BSR = GPIO_SSC_FRAME;
 744
 745         // Disable timer during configuration
 746         AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
 747
 748         // Capture mode, defaul timer source = MCK/2 (TIMER_CLOCK1), TIOA is external trigger,
 749         // external trigger rising edge, load RA on rising edge of TIOA.
 750         uint32_t t1_channel_mode = AT91C_TC_CLKS_TIMER_DIV1_CLOCK | AT91C_TC_ETRGEDG_BOTH | AT91C_TC_ABETRG | AT91C_TC_LDRA_BOTH;
 751         AT91C_BASE_TC1->TC_CMR = t1_channel_mode;
 752
 753         // Enable and reset counter
 754         AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
 755
 756         // Reset the received frame, frame count and timing info
 757         memset(rx,0x00,sizeof(rx));
 758         frame_count = 0;
 759         response = 0;
 760         overflow = 0;
 761         reader_frame = false;
 762         lastbit = 1;
 763         bSkip = true;
 764         tag_sof = 4;
 765
 766         while(!BUTTON_PRESS()) {
 767                 // Watchdog hit
 768                 WDT_HIT();
 769
 770                 // Receive frame, watch for at most T0*EOF periods
 771                 while (AT91C_BASE_TC1->TC_CV < T0*HITAG_T_EOF) {
 772                         // Check if rising edge in modulation is detected
 773                         if(AT91C_BASE_TC1->TC_SR & AT91C_TC_LDRAS) {
 774                                 // Retrieve the new timing values
 775                                 int ra = (AT91C_BASE_TC1->TC_RA/T0);
 776
 777                                 // Find out if we are dealing with a rising or falling edge
 778                                 rising_edge = (AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_FRAME) > 0;
 779
 780                                 // Shorter periods will only happen with reader frames
 781                                 if (!reader_frame && rising_edge && ra < HITAG_T_TAG_CAPTURE_ONE_HALF) {
 782                                         // Switch from tag to reader capture
 783                                         LED_C_OFF();
 784                                         reader_frame = true;
 785                                         memset(rx,0x00,sizeof(rx));
 786                                         rxlen = 0;
 787                                 }
 788
 789                                 // Only handle if reader frame and rising edge, or tag frame and falling edge
 790                                 if (reader_frame != rising_edge) {
 791                                   overflow += ra;
 792                                         continue;
 793                                 }
 794
 795                                 // Add the buffered timing values of earlier captured edges which were skipped
 796                                 ra += overflow;
 797                                 overflow = 0;
 798
 799                                 if (reader_frame) {
 800                                         LED_B_ON();
 801                                         // Capture reader frame
 802                                         if(ra >= HITAG_T_STOP) {
 803                                                 if (rxlen != 0) {
 804                                                         //DbpString("wierd0?");
 805                                                 }
 806                                                 // Capture the T0 periods that have passed since last communication or field drop (reset)
 807                                                 response = (ra - HITAG_T_LOW);
 808                                         } else if(ra >= HITAG_T_1_MIN ) {
 809                                                 // '1' bit
 810                                                 rx[rxlen / 8] |= 1 << (7-(rxlen%8));
 811                                                 rxlen++;
 812                                         } else if(ra >= HITAG_T_0_MIN) {
 813                                                 // '0' bit
 814                                                 rx[rxlen / 8] |= 0 << (7-(rxlen%8));
 815                                                 rxlen++;
 816                                         } else {
 817                                                 // Ignore wierd value, is to small to mean anything
 818                                         }
 819                                 } else {
 820                                         LED_C_ON();
 821                                         // Capture tag frame (manchester decoding using only falling edges)
 822                                         if(ra >= HITAG_T_EOF) {
 823                                                 if (rxlen != 0) {
 824                                                         //DbpString("wierd1?");
 825                                                 }
 826                                                 // Capture the T0 periods that have passed since last communication or field drop (reset)
 827                                                 // We always recieve a 'one' first, which has the falling edge after a half period |-_|
 828                                                 response = ra-HITAG_T_TAG_HALF_PERIOD;
 829                                         } else if(ra >= HITAG_T_TAG_CAPTURE_FOUR_HALF) {
 830                                                 // Manchester coding example |-_|_-|-_| (101)
 831                                                 rx[rxlen / 8] |= 0 << (7-(rxlen%8));
 832                                                 rxlen++;
 833                                                 rx[rxlen / 8] |= 1 << (7-(rxlen%8));
 834                                                 rxlen++;
 835                                         } else if(ra >= HITAG_T_TAG_CAPTURE_THREE_HALF) {
 836                                                 // Manchester coding example |_-|...|_-|-_| (0...01)
 837                                                 rx[rxlen / 8] |= 0 << (7-(rxlen%8));
 838                                                 rxlen++;
 839                                                 // We have to skip this half period at start and add the 'one' the second time
 840                                                 if (!bSkip) {
 841                                                         rx[rxlen / 8] |= 1 << (7-(rxlen%8));
 842                                                         rxlen++;
 843                                                 }
 844                                                 lastbit = !lastbit;
 845                                                 bSkip = !bSkip;
 846                                         } else if(ra >= HITAG_T_TAG_CAPTURE_TWO_HALF) {
 847                                                 // Manchester coding example |_-|_-| (00) or |-_|-_| (11)
 848                                                 if (tag_sof) {
 849                                                         // Ignore bits that are transmitted during SOF
 850                                                         tag_sof--;
 851                                                 } else {
 852                                                         // bit is same as last bit
 853                                                         rx[rxlen / 8] |= lastbit << (7-(rxlen%8));
 854                                                         rxlen++;
 855                                                 }
 856                                         } else {
 857                                                 // Ignore wierd value, is to small to mean anything
 858                                         }
 859                                 }
 860                         }
 861                 }
 862
 863                 // Check if frame was captured
 864                 if(rxlen > 0) {
 865                         frame_count++;
 866                         if (!LogTraceHitag(rx,rxlen,response,0,reader_frame)) {
 867                                 DbpString("Trace full");
 868                                 break;
 869                         }
 870
 871                         // Check if we recognize a valid authentication attempt
 872                         if (nbytes(rxlen) == 8) {
 873                                 // Store the authentication attempt
 874                                 if (auth_table_len < (AUTH_TABLE_LENGTH-8)) {
 875                                         memcpy(auth_table+auth_table_len,rx,8);
 876                                         auth_table_len += 8;
 877                                 }
 878                         }
 879
 880                         // Reset the received frame and response timing info
 881                         memset(rx,0x00,sizeof(rx));
 882                         response = 0;
 883                         reader_frame = false;
 884                         lastbit = 1;
 885                         bSkip = true;
 886                         tag_sof = 4;
 887                         overflow = 0;
 888
 889                         LED_B_OFF();
 890                         LED_C_OFF();
 891                 } else {
 892                         // Save the timer overflow, will be 0 when frame was received
 893                         overflow += (AT91C_BASE_TC1->TC_CV/T0);
 894                 }
 895                 // Reset the frame length
 896                 rxlen = 0;
 897                 // Reset the timer to restart while-loop that receives frames
 898                 AT91C_BASE_TC1->TC_CCR = AT91C_TC_SWTRG;
 899         }
 900     LED_A_ON();
 901         LED_B_OFF();
 902         LED_C_OFF();
 903         LED_D_OFF();
 904         AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
 905     AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS;
 906         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 907     LED_A_OFF();
 908
 909 //      Dbprintf("frame received: %d",frame_count);
 910 //      Dbprintf("Authentication Attempts: %d",(auth_table_len/8));
 911 //      DbpString("All done");
 912 }
 913
 914 void SimulateHitagTag(bool tag_mem_supplied, byte_t* data) {
 915         int frame_count;
 916         int response;
 917         int overflow;
 918         byte_t rx[HITAG_FRAME_LEN];
 919         size_t rxlen=0;
 920         byte_t tx[HITAG_FRAME_LEN];
 921         size_t txlen=0;
 922         bool bQuitTraceFull = false;
 923         bQuiet = false;
 924
 925         auth_table_len = 0;
 926         auth_table_pos = 0;
 927     byte_t* auth_table;
 928         BigBuf_free();
 929     auth_table = (byte_t *)BigBuf_malloc(AUTH_TABLE_LENGTH);
 930         memset(auth_table, 0x00, AUTH_TABLE_LENGTH);
 931
 932         // Clean up trace and prepare it for storing frames
 933         set_tracing(TRUE);
 934         clear_trace();
 935
 936         DbpString("Starting Hitag2 simulation");
 937         LED_D_ON();
 938         hitag2_init();
 939
 940         if (tag_mem_supplied) {
 941                 DbpString("Loading hitag2 memory...");
 942                 memcpy((byte_t*)tag.sectors,data,48);
 943         }
 944
 945         uint32_t block = 0;
 946         for (size_t i=0; i<12; i++) {
 947                 for (size_t j=0; j<4; j++) {
 948                         block <<= 8;
 949                         block |= tag.sectors[i][j];
 950                 }
 951                 Dbprintf("| %d | %08x |",i,block);
 952         }
 953
 954         // Set up simulator mode, frequency divisor which will drive the FPGA
 955         // and analog mux selection.
 956         FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
 957         FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_EDGE_DETECT | FPGA_LF_EDGE_DETECT_READER_FIELD);
 958         FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
 959         SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
 960         RELAY_OFF();
 961
 962         // Configure output pin that is connected to the FPGA (for modulating)
 963         AT91C_BASE_PIOA->PIO_OER = GPIO_SSC_DOUT;
 964         AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT;
 965
 966         // Disable modulation at default, which means release resistance
 967         LOW(GPIO_SSC_DOUT);
 968
 969         // Enable Peripheral Clock for TIMER_CLOCK0, used to measure exact timing before answering
 970         AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_TC0);
 971
 972         // Enable Peripheral Clock for TIMER_CLOCK1, used to capture edges of the reader frames
 973         AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_TC1);
 974         AT91C_BASE_PIOA->PIO_BSR = GPIO_SSC_FRAME;
 975
 976   // Disable timer during configuration
 977         AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
 978
 979         // Capture mode, default timer source = MCK/2 (TIMER_CLOCK1), TIOA is external trigger,
 980         // external trigger rising edge, load RA on rising edge of TIOA.
 981         AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK | AT91C_TC_ETRGEDG_RISING | AT91C_TC_ABETRG | AT91C_TC_LDRA_RISING;
 982
 983         // Reset the received frame, frame count and timing info
 984         memset(rx,0x00,sizeof(rx));
 985         frame_count = 0;
 986         response = 0;
 987         overflow = 0;
 988
 989         // Enable and reset counter
 990         AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
 991
 992         while(!BUTTON_PRESS()) {
 993                 // Watchdog hit
 994                 WDT_HIT();
 995
 996                 // Receive frame, watch for at most T0*EOF periods
 997                 while (AT91C_BASE_TC1->TC_CV < T0*HITAG_T_EOF) {
 998                         // Check if rising edge in modulation is detected
 999                         if(AT91C_BASE_TC1->TC_SR & AT91C_TC_LDRAS) {
1000                                 // Retrieve the new timing values
1001                                 int ra = (AT91C_BASE_TC1->TC_RA/T0) + overflow;
1002                                 overflow = 0;
1003
1004                                 // Reset timer every frame, we have to capture the last edge for timing
1005                                 AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
1006
1007                                 LED_B_ON();
1008
1009                                 // Capture reader frame
1010                                 if(ra >= HITAG_T_STOP) {
1011                                         if (rxlen != 0) {
1012                                                 //DbpString("wierd0?");
1013                                         }
1014                                         // Capture the T0 periods that have passed since last communication or field drop (reset)
1015                                         response = (ra - HITAG_T_LOW);
1016                                 } else if(ra >= HITAG_T_1_MIN ) {
1017                                         // '1' bit
1018                                         rx[rxlen / 8] |= 1 << (7-(rxlen%8));
1019                                         rxlen++;
1020                                 } else if(ra >= HITAG_T_0_MIN) {
1021                                         // '0' bit
1022                                         rx[rxlen / 8] |= 0 << (7-(rxlen%8));
1023                                         rxlen++;
1024                                 } else {
1025                                         // Ignore wierd value, is to small to mean anything
1026                                 }
1027                         }
1028                 }
1029
1030                 // Check if frame was captured
1031                 if(rxlen > 4) {
1032                         frame_count++;
1033                         if (!bQuiet) {
1034                                 if (!LogTraceHitag(rx,rxlen,response,0,true)) {
1035                                         DbpString("Trace full");
1036                                         if (bQuitTraceFull) {
1037                                                 break;
1038                                         } else {
1039                                                 bQuiet = true;
1040                                         }
1041                                 }
1042                         }
1043
1044                         // Disable timer 1 with external trigger to avoid triggers during our own modulation
1045                         AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
1046
1047                         // Process the incoming frame (rx) and prepare the outgoing frame (tx)
1048                         hitag2_handle_reader_command(rx,rxlen,tx,&txlen);
1049
1050                         // Wait for HITAG_T_WAIT_1 carrier periods after the last reader bit,
1051                         // not that since the clock counts since the rising edge, but T_Wait1 is
1052                         // with respect to the falling edge, we need to wait actually (T_Wait1 - T_Low)
1053                         // periods. The gap time T_Low varies (4..10). All timer values are in
1054                         // terms of T0 units
1055                         while(AT91C_BASE_TC0->TC_CV < T0*(HITAG_T_WAIT_1-HITAG_T_LOW));
1056
1057                         // Send and store the tag answer (if there is any)
1058                         if (txlen) {
1059                                 // Transmit the tag frame
1060                                 hitag_send_frame(tx,txlen);
1061                                 // Store the frame in the trace
1062                                 if (!bQuiet) {
1063                                         if (!LogTraceHitag(tx,txlen,0,0,false)) {
1064                                                 DbpString("Trace full");
1065                                                 if (bQuitTraceFull) {
1066                                                         break;
1067                                                 } else {
1068                                                         bQuiet = true;
1069                                                 }
1070                                         }
1071                                 }
1072                         }
1073
1074                         // Reset the received frame and response timing info
1075                         memset(rx,0x00,sizeof(rx));
1076                         response = 0;
1077
1078                         // Enable and reset external trigger in timer for capturing future frames
1079                         AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
1080                         LED_B_OFF();
1081                 }
1082                 // Reset the frame length
1083                 rxlen = 0;
1084                 // Save the timer overflow, will be 0 when frame was received
1085                 overflow += (AT91C_BASE_TC1->TC_CV/T0);
1086                 // Reset the timer to restart while-loop that receives frames
1087                 AT91C_BASE_TC1->TC_CCR = AT91C_TC_SWTRG;
1088         }
1089         LED_B_OFF();
1090         LED_D_OFF();
1091         AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
1092         AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS;
1093         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1094
1095         DbpString("Sim Stopped");
1096
1097 }
1098
1099 void ReaderHitag(hitag_function htf, hitag_data* htd) {
1100         int frame_count;
1101         int response;
1102         byte_t rx[HITAG_FRAME_LEN];
1103         size_t rxlen=0;
1104         byte_t txbuf[HITAG_FRAME_LEN];
1105         byte_t* tx = txbuf;
1106         size_t txlen=0;
1107         int lastbit;
1108         bool bSkip;
1109         int reset_sof;
1110         int tag_sof;
1111         int t_wait = HITAG_T_WAIT_MAX;
1112         bool bStop;
1113         bool bQuitTraceFull = false;
1114
1115         FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
1116         // Reset the return status
1117         bSuccessful = false;
1118
1119         // Clean up trace and prepare it for storing frames
1120         set_tracing(TRUE);
1121         clear_trace();
1122
1123         DbpString("Starting Hitag reader family");
1124
1125         // Check configuration
1126         switch(htf) {
1127                 case RHT2F_PASSWORD: {
1128                         Dbprintf("List identifier in password mode");
1129                         memcpy(password,htd->pwd.password,4);
1130                 blocknr = 0;
1131                         bQuitTraceFull = false;
1132                         bQuiet = false;
1133                         bPwd = false;
1134                 } break;
1135
1136                 case RHT2F_AUTHENTICATE: {
1137                         DbpString("Authenticating using nr,ar pair:");
1138                         memcpy(NrAr,htd->auth.NrAr,8);
1139                         Dbhexdump(8,NrAr,false);
1140                         bQuiet = false;
1141                         bCrypto = false;
1142                         bAuthenticating = false;
1143                         bQuitTraceFull = true;
1144                 } break;
1145
1146                 case RHT2F_CRYPTO: {
1147                         DbpString("Authenticating using key:");
1148                         memcpy(key,htd->crypto.key,4);    //HACK; 4 or 6??  I read both in the code.
1149                         Dbhexdump(6,key,false);
1150                         blocknr = 0;
1151                         bQuiet = false;
1152                         bCrypto = false;
1153                         bAuthenticating = false;
1154                         bQuitTraceFull = true;
1155                 } break;
1156
1157                 case RHT2F_TEST_AUTH_ATTEMPTS: {
1158                         Dbprintf("Testing %d authentication attempts",(auth_table_len/8));
1159                         auth_table_pos = 0;
1160                         memcpy(NrAr, auth_table, 8);
1161                         bQuitTraceFull = false;
1162                         bQuiet = false;
1163                         bCrypto = false;
1164                 } break;
1165
1166                 default: {
1167                         Dbprintf("Error, unknown function: %d",htf);
1168                         return;
1169                 } break;
1170         }
1171
1172         LED_D_ON();
1173         hitag2_init();
1174
1175         // Configure output and enable pin that is connected to the FPGA (for modulating)
1176         AT91C_BASE_PIOA->PIO_OER = GPIO_SSC_DOUT;
1177         AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT;
1178
1179         // Set fpga in edge detect with reader field, we can modulate as reader now
1180         FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_EDGE_DETECT | FPGA_LF_EDGE_DETECT_READER_FIELD);
1181
1182         // Set Frequency divisor which will drive the FPGA and analog mux selection
1183         FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
1184         SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
1185         RELAY_OFF();
1186
1187         // Disable modulation at default, which means enable the field
1188         LOW(GPIO_SSC_DOUT);
1189
1190         // Give it a bit of time for the resonant antenna to settle.
1191         SpinDelay(30);
1192
1193         // Enable Peripheral Clock for TIMER_CLOCK0, used to measure exact timing before answering
1194         AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_TC0);
1195
1196         // Enable Peripheral Clock for TIMER_CLOCK1, used to capture edges of the tag frames
1197         AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_TC1);
1198         AT91C_BASE_PIOA->PIO_BSR = GPIO_SSC_FRAME;
1199
1200     // Disable timer during configuration
1201         AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
1202
1203         // Capture mode, defaul timer source = MCK/2 (TIMER_CLOCK1), TIOA is external trigger,
1204         // external trigger rising edge, load RA on falling edge of TIOA.
1205         AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK | AT91C_TC_ETRGEDG_FALLING | AT91C_TC_ABETRG | AT91C_TC_LDRA_FALLING;
1206
1207         // Enable and reset counters
1208         AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
1209         AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
1210
1211         // Reset the received frame, frame count and timing info
1212         frame_count = 0;
1213         response = 0;
1214         lastbit = 1;
1215         bStop = false;
1216
1217   // Tag specific configuration settings (sof, timings, etc.)
1218   if (htf < 10){
1219     // hitagS settings
1220     reset_sof = 1;
1221     t_wait = 200;
1222     DbpString("Configured for hitagS reader");
1223   } else if (htf < 20) {
1224     // hitag1 settings
1225     reset_sof = 1;
1226     t_wait = 200;
1227     DbpString("Configured for hitag1 reader");
1228   } else if (htf < 30) {
1229     // hitag2 settings
1230     reset_sof = 4;
1231     t_wait = HITAG_T_WAIT_2;
1232     DbpString("Configured for hitag2 reader");
1233         } else {
1234     Dbprintf("Error, unknown hitag reader type: %d",htf);
1235     return;
1236   }
1237
1238         while(!bStop && !BUTTON_PRESS()) {
1239                 // Watchdog hit
1240                 WDT_HIT();
1241
1242                 // Check if frame was captured and store it
1243                 if(rxlen > 0) {
1244                         frame_count++;
1245                         if (!bQuiet) {
1246                                 if (!LogTraceHitag(rx,rxlen,response,0,false)) {
1247                                         DbpString("Trace full");
1248                                         if (bQuitTraceFull) {
1249                                                 break;
1250                                         } else {
1251                                                 bQuiet = true;
1252                                         }
1253                                 }
1254                         }
1255                 }
1256
1257                 // By default reset the transmission buffer
1258                 tx = txbuf;
1259                 switch(htf) {
1260                         case RHT2F_PASSWORD: {
1261                                 bStop = !hitag2_password(rx,rxlen,tx,&txlen);
1262                         } break;
1263                         case RHT2F_AUTHENTICATE: {
1264                                 bStop = !hitag2_authenticate(rx,rxlen,tx,&txlen);
1265                         } break;
1266                         case RHT2F_CRYPTO: {
1267                                 bStop = !hitag2_crypto(rx,rxlen,tx,&txlen);
1268                         } break;
1269                         case RHT2F_TEST_AUTH_ATTEMPTS: {
1270                                 bStop = !hitag2_test_auth_attempts(rx,rxlen,tx,&txlen);
1271                         } break;
1272                         default: {
1273                                 Dbprintf("Error, unknown function: %d",htf);
1274                                 return;
1275                         } break;
1276                 }
1277
1278                 // Send and store the reader command
1279                 // Disable timer 1 with external trigger to avoid triggers during our own modulation
1280                 AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
1281
1282                 // Wait for HITAG_T_WAIT_2 carrier periods after the last tag bit before transmitting,
1283                 // Since the clock counts since the last falling edge, a 'one' means that the
1284                 // falling edge occured halfway the period. with respect to this falling edge,
1285                 // we need to wait (T_Wait2 + half_tag_period) when the last was a 'one'.
1286                 // All timer values are in terms of T0 units
1287                 while(AT91C_BASE_TC0->TC_CV < T0*(t_wait+(HITAG_T_TAG_HALF_PERIOD*lastbit)));
1288
1289                 // Transmit the reader frame
1290                 hitag_reader_send_frame(tx,txlen);
1291
1292                 // Enable and reset external trigger in timer for capturing future frames
1293                 AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
1294
1295                 // Add transmitted frame to total count
1296                 if(txlen > 0) {
1297                         frame_count++;
1298                         if (!bQuiet) {
1299                                 // Store the frame in the trace
1300                                 if (!LogTraceHitag(tx,txlen,HITAG_T_WAIT_2,0,true)) {
1301                                         if (bQuitTraceFull) {
1302                                                 break;
1303                                         } else {
1304                                                 bQuiet = true;
1305                                         }
1306                                 }
1307                         }
1308                 }
1309
1310                 // Reset values for receiving frames
1311                 memset(rx,0x00,sizeof(rx));
1312                 rxlen = 0;
1313                 lastbit = 1;
1314                 bSkip = true;
1315                 tag_sof = reset_sof;
1316                 response = 0;
1317
1318                 // Receive frame, watch for at most T0*EOF periods
1319                 while (AT91C_BASE_TC1->TC_CV < T0*HITAG_T_WAIT_MAX) {
1320                         // Check if falling edge in tag modulation is detected
1321                         if(AT91C_BASE_TC1->TC_SR & AT91C_TC_LDRAS) {
1322                                 // Retrieve the new timing values
1323                                 int ra = (AT91C_BASE_TC1->TC_RA/T0);
1324
1325                                 // Reset timer every frame, we have to capture the last edge for timing
1326                                 AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG;
1327
1328                                 LED_B_ON();
1329
1330                                 // Capture tag frame (manchester decoding using only falling edges)
1331                                 if(ra >= HITAG_T_EOF) {
1332                                         if (rxlen != 0) {
1333                                                 //DbpString("wierd1?");
1334                                         }
1335                                         // Capture the T0 periods that have passed since last communication or field drop (reset)
1336                                         // We always recieve a 'one' first, which has the falling edge after a half period |-_|
1337                                         response = ra-HITAG_T_TAG_HALF_PERIOD;
1338                                 } else if(ra >= HITAG_T_TAG_CAPTURE_FOUR_HALF) {
1339                                         // Manchester coding example |-_|_-|-_| (101)
1340                                         rx[rxlen / 8] |= 0 << (7-(rxlen%8));
1341                                         rxlen++;
1342                                         rx[rxlen / 8] |= 1 << (7-(rxlen%8));
1343                                         rxlen++;
1344                                 } else if(ra >= HITAG_T_TAG_CAPTURE_THREE_HALF) {
1345                                         // Manchester coding example |_-|...|_-|-_| (0...01)
1346                                         rx[rxlen / 8] |= 0 << (7-(rxlen%8));
1347                                         rxlen++;
1348                                         // We have to skip this half period at start and add the 'one' the second time
1349                                         if (!bSkip) {
1350                                                 rx[rxlen / 8] |= 1 << (7-(rxlen%8));
1351                                                 rxlen++;
1352                                         }
1353                                         lastbit = !lastbit;
1354                                         bSkip = !bSkip;
1355                                 } else if(ra >= HITAG_T_TAG_CAPTURE_TWO_HALF) {
1356                                         // Manchester coding example |_-|_-| (00) or |-_|-_| (11)
1357                                         if (tag_sof) {
1358                                                 // Ignore bits that are transmitted during SOF
1359                                                 tag_sof--;
1360                                         } else {
1361                                                 // bit is same as last bit
1362                                                 rx[rxlen / 8] |= lastbit << (7-(rxlen%8));
1363                                                 rxlen++;
1364                                         }
1365                                 } else {
1366                                         // Ignore wierd value, is to small to mean anything
1367                                 }
1368                         }
1369
1370                         // We can break this loop if we received the last bit from a frame
1371                         if (AT91C_BASE_TC1->TC_CV > T0*HITAG_T_EOF) {
1372                                 if (rxlen>0) break;
1373                         }
1374                 }
1375         }
1376         LED_B_OFF();
1377         LED_D_OFF();
1378         AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
1379         AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS;
1380         FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
1381         Dbprintf("frame received: %d",frame_count);
1382   DbpString("All done");
1383   cmd_send(CMD_ACK,bSuccessful,0,0,(byte_t*)tag.sectors,48);
1384 }