X-Git-Url: https://git.zerfleddert.de/cgi-bin/gitweb.cgi/proxmark3-svn/blobdiff_plain/ed7bd3a3802f6398e5a8ae38cb67516efde0e327..6fc6cd0f57c9f036e3d1e976607df87dbb0214b2:/armsrc/hitag2.c

diff --git a/armsrc/hitag2.c b/armsrc/hitag2.c
index 90a95b5a..7702025b 100644
--- a/armsrc/hitag2.c
+++ b/armsrc/hitag2.c
@@ -21,11 +21,16 @@
 #include "util.h"
 #include "hitag2.h"
 #include "string.h"
+#include "BigBuf.h"
 
 static bool bQuiet;
 
-bool bCrypto;
-bool bPwd;
+static bool bCrypto;
+static bool bAuthenticating;
+static bool bPwd;
+static bool bSuccessful;
+
+
 
 struct hitag2_tag {
 	uint32_t uid;
@@ -41,8 +46,7 @@ struct hitag2_tag {
 	byte_t sectors[12][4];
 };
 
-static struct hitag2_tag tag;
-static const struct hitag2_tag resetdata = {
+static struct hitag2_tag tag = {
     .state = TAG_STATE_RESET,
     .sectors = {                         // Password mode:               | Crypto mode:
         [0]  = { 0x02, 0x4e, 0x02, 0x20}, // UID                          | UID
@@ -60,19 +64,25 @@ static const struct hitag2_tag resetdata = {
     },
 };
 
-//#define TRACE_LENGTH 3000
-//uint8_t *trace = (uint8_t *) BigBuf;
-//int traceLen = 0;
-//int rsamples = 0;
+static enum {
+	WRITE_STATE_START = 0x0,
+	WRITE_STATE_PAGENUM_WRITTEN,
+	WRITE_STATE_PROG
+} writestate;
+	
 
-#define AUTH_TABLE_OFFSET FREE_BUFFER_OFFSET
-#define AUTH_TABLE_LENGTH FREE_BUFFER_SIZE
-byte_t* auth_table = (byte_t *)BigBuf+AUTH_TABLE_OFFSET;
-size_t auth_table_pos = 0;
-size_t auth_table_len = AUTH_TABLE_LENGTH;
+// ToDo: define a meaningful maximum size for auth_table. The bigger this is, the lower will be the available memory for traces. 
+// Historically it used to be FREE_BUFFER_SIZE, which was 2744.
+#define AUTH_TABLE_LENGTH 2744
+static byte_t* auth_table;
+static size_t auth_table_pos = 0;
+static size_t auth_table_len = AUTH_TABLE_LENGTH;
 
-byte_t password[4];
-byte_t NrAr[8];
+static byte_t password[4];
+static byte_t NrAr[8];
+static byte_t key[8];
+static byte_t writedata[4];
+static uint64_t cipher_state;
 
 /* Following is a modified version of cryptolib.com/ciphers/hitag2/ */
 // Software optimized 48-bit Philips/NXP Mifare Hitag2 PCF7936/46/47/52 stream cipher algorithm by I.C. Wiener 2006-2007.
@@ -104,13 +114,13 @@ static const u32 ht2_f5c = 0x7907287B;	// 0111 1001 0000 0111 0010 1000 0111 101
 
 static u32 _f20 (const u64 x)
 {
-	u32					i5;
-
+ 	u32					i5;
+	
 	i5 = ((ht2_f4a >> i4 (x, 1, 2, 4, 5)) & 1)* 1
-	   + ((ht2_f4b >> i4 (x, 7,11,13,14)) & 1)* 2
-	   + ((ht2_f4b >> i4 (x,16,20,22,25)) & 1)* 4
-	   + ((ht2_f4b >> i4 (x,27,28,30,32)) & 1)* 8
-	   + ((ht2_f4a >> i4 (x,33,42,43,45)) & 1)*16;
+		+ ((ht2_f4b >> i4 (x, 7,11,13,14)) & 1)* 2
+		+ ((ht2_f4b >> i4 (x,16,20,22,25)) & 1)* 4
+		+ ((ht2_f4b >> i4 (x,27,28,30,32)) & 1)* 8
+		+ ((ht2_f4a >> i4 (x,33,42,43,45)) & 1)*16;
 
 	return (ht2_f5c >> i5) & 1;
 }
@@ -133,10 +143,10 @@ static u64 _hitag2_round (u64 *state)
 	u64					x = *state;
 
 	x = (x >>  1) +
-	 ((((x >>  0) ^ (x >>  2) ^ (x >>  3) ^ (x >>  6)
-	  ^ (x >>  7) ^ (x >>  8) ^ (x >> 16) ^ (x >> 22)
-	  ^ (x >> 23) ^ (x >> 26) ^ (x >> 30) ^ (x >> 41)
-	  ^ (x >> 42) ^ (x >> 43) ^ (x >> 46) ^ (x >> 47)) & 1) << 47);
+		((((x >>  0) ^ (x >>  2) ^ (x >>  3) ^ (x >>  6)
+		   ^ (x >>  7) ^ (x >>  8) ^ (x >> 16) ^ (x >> 22)
+		   ^ (x >> 23) ^ (x >> 26) ^ (x >> 30) ^ (x >> 41)
+		   ^ (x >> 42) ^ (x >> 43) ^ (x >> 46) ^ (x >> 47)) & 1) << 47);
 
 	*state = x;
 	return _f20 (x);
@@ -150,40 +160,36 @@ static u32 _hitag2_byte (u64 * x)
 	return c;
 }
 
-size_t nbytes(size_t nbits) {
-	return (nbits/8)+((nbits%8)>0);
-}
-
-int hitag2_reset(void)
+static int hitag2_reset(void)
 {
 	tag.state = TAG_STATE_RESET;
 	tag.crypto_active = 0;
 	return 0;
 }
 
-int hitag2_init(void)
+static int hitag2_init(void)
 {
-	memcpy(&tag, &resetdata, sizeof(tag));
+//	memcpy(&tag, &resetdata, sizeof(tag));
 	hitag2_reset();
 	return 0;
 }
 
 static void hitag2_cipher_reset(struct hitag2_tag *tag, const byte_t *iv)
 {
-	uint64_t key = ((uint64_t)tag->sectors[2][2]) |
-			((uint64_t)tag->sectors[2][3] << 8) |
-			((uint64_t)tag->sectors[1][0] << 16) |
-			((uint64_t)tag->sectors[1][1] << 24) |
-			((uint64_t)tag->sectors[1][2] << 32) |
-			((uint64_t)tag->sectors[1][3] << 40);
-	uint32_t uid = ((uint32_t)tag->sectors[0][0]) |
-			((uint32_t)tag->sectors[0][1] << 8) |
-			((uint32_t)tag->sectors[0][2] << 16) |
-			((uint32_t)tag->sectors[0][3] << 24);
+	uint64_t key =  ((uint64_t)tag->sectors[2][2]) |
+		((uint64_t)tag->sectors[2][3] << 8) |
+		((uint64_t)tag->sectors[1][0] << 16) |
+		((uint64_t)tag->sectors[1][1] << 24) |
+		((uint64_t)tag->sectors[1][2] << 32) |
+		((uint64_t)tag->sectors[1][3] << 40);
+	uint32_t uid =  ((uint32_t)tag->sectors[0][0]) |
+		((uint32_t)tag->sectors[0][1] << 8) |
+		((uint32_t)tag->sectors[0][2] << 16) |
+		((uint32_t)tag->sectors[0][3] << 24);
 	uint32_t iv_ = (((uint32_t)(iv[0]))) |
-			(((uint32_t)(iv[1])) << 8) |
-			(((uint32_t)(iv[2])) << 16) |
-			(((uint32_t)(iv[3])) << 24);
+		(((uint32_t)(iv[1])) << 8) |
+		(((uint32_t)(iv[2])) << 16) |
+		(((uint32_t)(iv[3])) << 24);
 	tag->cs = _hitag2_init(rev64(key), rev32(uid), rev32(iv_));
 }
 
@@ -224,6 +230,7 @@ static int hitag2_cipher_transcrypt(uint64_t* cs, byte_t *data, unsigned int byt
 #define HITAG_T_WAIT_1 200 /* T_wresp should be 199..206 */
 #define HITAG_T_WAIT_2 90 /* T_wresp should be 199..206 */
 #define HITAG_T_WAIT_MAX 300 /* bit more than HITAG_T_WAIT_1 + HITAG_T_WAIT_2 */
+#define HITAG_T_PROG 614
 
 #define HITAG_T_TAG_ONE_HALF_PERIOD			10
 #define HITAG_T_TAG_TWO_HALF_PERIOD			25
@@ -277,7 +284,8 @@ static void hitag_send_frame(const byte_t* frame, size_t frame_len)
 	LOW(GPIO_SSC_DOUT);
 }
 
-void hitag2_handle_reader_command(byte_t* rx, const size_t rxlen, byte_t* tx, size_t* txlen)
+
+static void hitag2_handle_reader_command(byte_t* rx, const size_t rxlen, byte_t* tx, size_t* txlen)
 {
 	byte_t rx_air[HITAG_FRAME_LEN];
 	
@@ -294,110 +302,110 @@ void hitag2_handle_reader_command(byte_t* rx, const size_t rxlen, byte_t* tx, si
 	// Try to find out which command was send by selecting on length (in bits)
 	switch (rxlen) {
 		// Received 11000 from the reader, request for UID, send UID 
-		case 05: {
-			// Always send over the air in the clear plaintext mode
-			if(rx_air[0] != 0xC0) {
-				// Unknown frame ?
-				return;
-			}
-			*txlen = 32;
-			memcpy(tx,tag.sectors[0],4);
-			tag.crypto_active = 0;
+	case 05: {
+		// Always send over the air in the clear plaintext mode
+		if(rx_air[0] != 0xC0) {
+			// Unknown frame ?
+			return;
 		}
+		*txlen = 32;
+		memcpy(tx,tag.sectors[0],4);
+		tag.crypto_active = 0;
+	}
 		break;
 
 		// Read/Write command: ..xx x..y  yy with yyy == ~xxx, xxx is sector number 
-		case 10: {
-			unsigned int sector = (~( ((rx[0]<<2)&0x04) | ((rx[1]>>6)&0x03) ) & 0x07);
-			// Verify complement of sector index
-			if(sector != ((rx[0]>>3)&0x07)) {
-				//DbpString("Transmission error (read/write)");
-				return;
-			}
+	case 10: {
+		unsigned int sector = (~( ((rx[0]<<2)&0x04) | ((rx[1]>>6)&0x03) ) & 0x07);
+		// Verify complement of sector index
+		if(sector != ((rx[0]>>3)&0x07)) {
+			//DbpString("Transmission error (read/write)");
+			return;
+		}
 
-			switch (rx[0] & 0xC6) {
-				// Read command: 11xx x00y
-				case 0xC0:
-					memcpy(tx,tag.sectors[sector],4);
-					*txlen = 32;
-				break;
+		switch (rx[0] & 0xC6) {
+			// Read command: 11xx x00y
+		case 0xC0:
+			memcpy(tx,tag.sectors[sector],4);
+			*txlen = 32;
+			break;
 					
-				 // Inverted Read command: 01xx x10y
-				case 0x44:
-					for (size_t i=0; i<4; i++) {
-						tx[i] = tag.sectors[sector][i] ^ 0xff;
-					}
-					*txlen = 32;
-				break;
-
-				// Write command: 10xx x01y
-				case 0x82:
-					// Prepare write, acknowledge by repeating command
-					memcpy(tx,rx,nbytes(rxlen));
-					*txlen = rxlen;
-					tag.active_sector = sector;
-					tag.state=TAG_STATE_WRITING;
-				break;
-				
-				// Unknown command
-				default:
-					Dbprintf("Uknown command: %02x %02x",rx[0],rx[1]);
-					return;
-				break;
+			// Inverted Read command: 01xx x10y
+		case 0x44:
+			for (size_t i=0; i<4; i++) {
+				tx[i] = tag.sectors[sector][i] ^ 0xff;
 			}
+			*txlen = 32;
+			break;
+
+			// Write command: 10xx x01y
+		case 0x82:
+			// Prepare write, acknowledge by repeating command
+			memcpy(tx,rx,nbytes(rxlen));
+			*txlen = rxlen;
+			tag.active_sector = sector;
+			tag.state=TAG_STATE_WRITING;
+			break;
+				
+			// Unknown command
+		default:
+			Dbprintf("Unknown command: %02x %02x",rx[0],rx[1]);
+			return;
+			break;
 		}
+	}
 		break;
 
 		// Writing data or Reader password
-		case 32: {
-			if(tag.state == TAG_STATE_WRITING) {
-				// These are the sector contents to be written. We don't have to do anything else.
-				memcpy(tag.sectors[tag.active_sector],rx,nbytes(rxlen));
-				tag.state=TAG_STATE_RESET;
+	case 32: {
+		if(tag.state == TAG_STATE_WRITING) {
+			// These are the sector contents to be written. We don't have to do anything else.
+			memcpy(tag.sectors[tag.active_sector],rx,nbytes(rxlen));
+			tag.state=TAG_STATE_RESET;
+			return;
+		} else {
+			// Received RWD password, respond with configuration and our password
+			if(memcmp(rx,tag.sectors[1],4) != 0) {
+				DbpString("Reader password is wrong");
 				return;
-			} else {
-				// Received RWD password, respond with configuration and our password
-				if(memcmp(rx,tag.sectors[1],4) != 0) {
-					DbpString("Reader password is wrong");
-					return;
-				}
-				*txlen = 32;
-				memcpy(tx,tag.sectors[3],4);
 			}
+			*txlen = 32;
+			memcpy(tx,tag.sectors[3],4);
 		}
+	}
 		break;
 
 		// Received RWD authentication challenge and respnse
-		case 64: {
-			// Store the authentication attempt
-			if (auth_table_len < (AUTH_TABLE_LENGTH-8)) {
-				memcpy(auth_table+auth_table_len,rx,8);
-				auth_table_len += 8;
-			}
+	case 64: {
+		// Store the authentication attempt
+		if (auth_table_len < (AUTH_TABLE_LENGTH-8)) {
+			memcpy(auth_table+auth_table_len,rx,8);
+			auth_table_len += 8;
+		}
 
-			// Reset the cipher state
-			hitag2_cipher_reset(&tag,rx);
-			// Check if the authentication was correct
-			if(!hitag2_cipher_authenticate(&(tag.cs),rx+4)) {
-				// The reader failed to authenticate, do nothing
-				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]);
-				return;
-			}
-			// Succesful, but commented out reporting back to the Host, this may delay to much.
-			// 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]);
+		// Reset the cipher state
+		hitag2_cipher_reset(&tag,rx);
+		// Check if the authentication was correct
+		if(!hitag2_cipher_authenticate(&(tag.cs),rx+4)) {
+			// The reader failed to authenticate, do nothing
+			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]);
+			return;
+		}
+		// Succesful, but commented out reporting back to the Host, this may delay to much.
+		// 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]);
 
-			// Activate encryption algorithm for all further communication
-			tag.crypto_active = 1;
+		// Activate encryption algorithm for all further communication
+		tag.crypto_active = 1;
 
-			// Use the tag password as response
-			memcpy(tx,tag.sectors[3],4);
-			*txlen = 32;
-		}
+		// Use the tag password as response
+		memcpy(tx,tag.sectors[3],4);
+		*txlen = 32;
+	}
 		break;
 	}
 
-//	LogTrace(rx,nbytes(rxlen),0,0,false);
-//	LogTrace(tx,nbytes(*txlen),0,0,true);
+//	LogTraceHitag(rx,rxlen,0,0,false);
+//	LogTraceHitag(tx,*txlen,0,0,true);
 	
 	if(tag.crypto_active) {
 		hitag2_cipher_transcrypt(&(tag.cs), tx, *txlen/8, *txlen%8);
@@ -412,7 +420,7 @@ static void hitag_reader_send_bit(int bit) {
 	// Binary puls length modulation (BPLM) is used to encode the data stream
 	// This means that a transmission of a one takes longer than that of a zero
 	
-	// Enable modulation, which means, drop the the field
+	// Enable modulation, which means, drop the field
 	HIGH(GPIO_SSC_DOUT);
 	
 	// Wait for 4-10 times the carrier period
@@ -434,6 +442,7 @@ static void hitag_reader_send_bit(int bit) {
 	LED_A_OFF();
 }
 
+
 static void hitag_reader_send_frame(const byte_t* frame, size_t frame_len)
 {
 	// Send the content of the frame
@@ -442,7 +451,7 @@ static void hitag_reader_send_frame(const byte_t* frame, size_t frame_len)
 	}
 	// Send EOF 
 	AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG;
-	// Enable modulation, which means, drop the the field
+	// Enable modulation, which means, drop the field
 	HIGH(GPIO_SSC_DOUT);
 	// Wait for 4-10 times the carrier period
 	while(AT91C_BASE_TC0->TC_CV < T0*6);
@@ -452,139 +461,349 @@ static void hitag_reader_send_frame(const byte_t* frame, size_t frame_len)
 
 size_t blocknr;
 
-bool hitag2_password(byte_t* rx, const size_t rxlen, byte_t* tx, size_t* txlen) {
+static bool hitag2_password(byte_t* rx, const size_t rxlen, byte_t* tx, size_t* txlen) {
 	// Reset the transmission frame length
 	*txlen = 0;
 	
 	// Try to find out which command was send by selecting on length (in bits)
 	switch (rxlen) {
 		// No answer, try to resurrect
-		case 0: {
-			// Stop if there is no answer (after sending password)
-			if (bPwd) {
-				DbpString("Password failed!");
-				return false;
-			}
-			*txlen = 5;
-			memcpy(tx,"\xc0",nbytes(*txlen));
-		} break;
+	case 0: {
+		// Stop if there is no answer (after sending password)
+		if (bPwd) {
+			DbpString("Password failed!");
+			return false;
+		}
+		*txlen = 5;
+		memcpy(tx,"\xc0",nbytes(*txlen));
+	} break;
 			
 		// Received UID, tag password
-		case 32: {
-			if (!bPwd) {
-				*txlen = 32;
-				memcpy(tx,password,4);
-				bPwd = true;
-			} else {
-        if (blocknr > 7) {
-          DbpString("Read succesful!");
-          // We are done... for now
-          return false;
-        }
-        *txlen = 10;
-        tx[0] = 0xc0 | (blocknr << 3) | ((blocknr^7) >> 2);
-        tx[1] = ((blocknr^7) << 6);
-        blocknr++;
+	case 32: {
+		if (!bPwd) {
+			*txlen = 32;
+			memcpy(tx,password,4);
+			bPwd = true;
+			memcpy(tag.sectors[blocknr],rx,4);
+			blocknr++;
+		} else {
+				
+			if(blocknr == 1){
+				//store password in block1, the TAG answers with Block3, but we need the password in memory
+				memcpy(tag.sectors[blocknr],tx,4);
+			}else{
+				memcpy(tag.sectors[blocknr],rx,4);
 			}
-		} break;
+			
+			blocknr++;
+			if (blocknr > 7) {
+				DbpString("Read succesful!");
+				bSuccessful = true;
+				return false;
+			}
+			*txlen = 10;
+			tx[0] = 0xc0 | (blocknr << 3) | ((blocknr^7) >> 2);
+			tx[1] = ((blocknr^7) << 6);
+		}
+	} break;
 			
 		// Unexpected response
-        default: {
-			Dbprintf("Uknown frame length: %d",rxlen);
+	default: {
+		Dbprintf("Uknown frame length: %d",rxlen);
+		return false;
+	} break;
+	}
+	return true;
+}
+
+static bool hitag2_write_page(byte_t* rx, const size_t rxlen, byte_t* tx, size_t* txlen)
+{
+	switch (writestate) {
+	case WRITE_STATE_START:
+		*txlen = 10;
+		tx[0] = 0x82 | (blocknr << 3) | ((blocknr^7) >> 2);
+		tx[1] = ((blocknr^7) << 6);
+		writestate = WRITE_STATE_PAGENUM_WRITTEN;
+		break;
+	case WRITE_STATE_PAGENUM_WRITTEN:
+		// Check if page number was received correctly
+		if ((rxlen == 10) &&
+		    (rx[0] == (0x82 | (blocknr << 3) | ((blocknr^7) >> 2))) &&
+		    (rx[1] == (((blocknr & 0x3) ^ 0x3) << 6))) {
+			*txlen = 32;
+			memset(tx, 0, HITAG_FRAME_LEN);
+			memcpy(tx, writedata, 4);
+			writestate = WRITE_STATE_PROG;
+		} else {
+			Dbprintf("hitag2_write_page: Page number was not received correctly: rxlen=%d rx=%02x%02x%02x%02x",
+				 rxlen, rx[0], rx[1], rx[2], rx[3]);
+			bSuccessful = false;
 			return false;
-		} break;
+		}
+		break;
+	case WRITE_STATE_PROG:
+		if (rxlen == 0) {
+			bSuccessful = true;
+		} else {
+			bSuccessful = false;
+			Dbprintf("hitag2_write_page: unexpected rx data (%d) after page write", rxlen);
+		}
+		return false;
+	default:
+		DbpString("hitag2_write_page: Unknown state %d");
+		bSuccessful = false;
+		return false;
 	}
+
 	return true;
 }
 
-bool hitag2_authenticate(byte_t* rx, const size_t rxlen, byte_t* tx, size_t* txlen) {
-	// Reset the transmission frame length 
+static bool hitag2_crypto(byte_t* rx, const size_t rxlen, byte_t* tx, size_t* txlen, bool write) {
+	// Reset the transmission frame length
 	*txlen = 0;
 	
+	if(bCrypto) {
+		hitag2_cipher_transcrypt(&cipher_state,rx,rxlen/8,rxlen%8);
+
+	}
+
+	if (bCrypto && !bAuthenticating && write) {
+		if (!hitag2_write_page(rx, rxlen, tx, txlen)) {
+			return false;
+		}
+	}
+	else
+	{
+
 	// Try to find out which command was send by selecting on length (in bits)
 	switch (rxlen) {
 		// No answer, try to resurrect
-		case 0: {
-			// Stop if there is no answer while we are in crypto mode (after sending NrAr)
-			if (bCrypto) {
+	case 0:
+	{
+		// Stop if there is no answer while we are in crypto mode (after sending NrAr)
+		if (bCrypto) {
+			// Failed during authentication
+			if (bAuthenticating) {
 				DbpString("Authentication failed!");
 				return false;
+			} else {
+				// Failed reading a block, could be (read/write) locked, skip block and re-authenticate
+				if (blocknr == 1) {
+					// Write the low part of the key in memory
+					memcpy(tag.sectors[1],key+2,4);
+				} else if (blocknr == 2) {
+					// Write the high part of the key in memory
+					tag.sectors[2][0] = 0x00;
+					tag.sectors[2][1] = 0x00;
+					tag.sectors[2][2] = key[0];
+					tag.sectors[2][3] = key[1];
+				} else {
+					// Just put zero's in the memory (of the unreadable block)
+					memset(tag.sectors[blocknr],0x00,4);
+				}
+				blocknr++;
+				bCrypto = false;
 			}
+		} else {
 			*txlen = 5;
 			memcpy(tx,"\xc0",nbytes(*txlen));
-		} break;
-			
-		// Received UID, crypto tag answer
-		case 32: {
-			if (!bCrypto) {
-				*txlen = 64;
-				memcpy(tx,NrAr,8);
-				bCrypto = true;
+		}
+	break;
+	}
+	// Received UID, crypto tag answer
+	case 32: {
+		if (!bCrypto) {
+			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;
+			uint32_t ui32uid = rx[0] | ((uint32_t)rx[1]) << 8 | ((uint32_t)rx[2]) << 16 | ((uint32_t)rx[3]) << 24;
+			Dbprintf("hitag2_crypto: key=0x%x%x uid=0x%x", (uint32_t) ((rev64(ui64key)) >> 32), (uint32_t) ((rev64(ui64key)) & 0xffffffff), rev32(ui32uid));
+			cipher_state = _hitag2_init(rev64(ui64key), rev32(ui32uid), 0);
+			memset(tx,0x00,4);
+			memset(tx+4,0xff,4);
+			hitag2_cipher_transcrypt(&cipher_state, tx+4, 4, 0);
+			*txlen = 64;
+			bCrypto = true;
+			bAuthenticating = true;
+		} else {
+			// Check if we received answer tag (at)
+			if (bAuthenticating) {
+				bAuthenticating = false;
+				if (write) {
+					if (!hitag2_write_page(rx, rxlen, tx, txlen)) {
+						return false;
+					}
+					break;
+				}
 			} else {
+				// Store the received block
+				memcpy(tag.sectors[blocknr],rx,4);
+				blocknr++;
+			}
+
+			if (blocknr > 7) {
 				DbpString("Read succesful!");
-				// We are done... for now
+				bSuccessful = true;
 				return false;
+			} else {
+				*txlen = 10;
+				tx[0] = 0xc0 | (blocknr << 3) | ((blocknr^7) >> 2);
+				tx[1] = ((blocknr^7) << 6);
 			}
-		} break;
+		}
+	} break;
 			
 		// Unexpected response
-		default: {
-			Dbprintf("Uknown frame length: %d",rxlen);
+	default: {
+		Dbprintf("Uknown frame length: %d",rxlen);
+		return false;
+	} break;
+	}
+	}
+  
+	if(bCrypto) {
+		// We have to return now to avoid double encryption
+		if (!bAuthenticating) {
+			hitag2_cipher_transcrypt(&cipher_state,tx,*txlen/8,*txlen%8);
+		}
+	}
+
+	return true;
+}
+
+
+static bool hitag2_authenticate(byte_t* rx, const size_t rxlen, byte_t* tx, size_t* txlen) {
+	// Reset the transmission frame length 
+	*txlen = 0;
+	
+	// Try to find out which command was send by selecting on length (in bits)
+	switch (rxlen) {
+		// No answer, try to resurrect
+	case 0: {
+		// Stop if there is no answer while we are in crypto mode (after sending NrAr)
+		if (bCrypto) {
+			DbpString("Authentication failed!");
 			return false;
-		} break;
+		}
+		*txlen = 5;
+		memcpy(tx,"\xc0",nbytes(*txlen));
+	} break;
+			
+		// Received UID, crypto tag answer
+	case 32: {
+		if (!bCrypto) {
+			*txlen = 64;
+			memcpy(tx,NrAr,8);
+			bCrypto = true;
+		} else {
+			DbpString("Authentication succesful!");
+			// We are done... for now
+			return false;
+		}
+	} break;
+			
+		// Unexpected response
+	default: {
+		Dbprintf("Uknown frame length: %d",rxlen);
+		return false;
+	} break;
 	}
 	
 	return true;
 }
 
-bool hitag2_test_auth_attempts(byte_t* rx, const size_t rxlen, byte_t* tx, size_t* txlen) {
+
+static bool hitag2_test_auth_attempts(byte_t* rx, const size_t rxlen, byte_t* tx, size_t* txlen) {
+
 	// Reset the transmission frame length 
 	*txlen = 0;
 	
 	// Try to find out which command was send by selecting on length (in bits)
 	switch (rxlen) {
-			// No answer, try to resurrect
-		case 0: {
-			// Stop if there is no answer while we are in crypto mode (after sending NrAr)
-			if (bCrypto) {
-				Dbprintf("auth: %02x%02x%02x%02x%02x%02x%02x%02x Failed!",NrAr[0],NrAr[1],NrAr[2],NrAr[3],NrAr[4],NrAr[5],NrAr[6],NrAr[7]);
-				bCrypto = false;
-				if ((auth_table_pos+8) == auth_table_len) {
-					return false;
-				}
-				auth_table_pos += 8;
-				memcpy(NrAr,auth_table+auth_table_pos,8);
+		// No answer, try to resurrect
+	case 0: {
+		// Stop if there is no answer while we are in crypto mode (after sending NrAr)
+		if (bCrypto) {
+			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]);
+
+			// Removing failed entry from authentiations table
+			memcpy(auth_table+auth_table_pos,auth_table+auth_table_pos+8,8);
+			auth_table_len -= 8;
+
+			// Return if we reached the end of the authentications table
+			bCrypto = false;
+			if (auth_table_pos == auth_table_len) {
+				return false;
 			}
-			*txlen = 5;
-			memcpy(tx,"\xc0",nbytes(*txlen));
-		}	break;
+
+			// Copy the next authentication attempt in row (at the same position, b/c we removed last failed entry)
+			memcpy(NrAr,auth_table+auth_table_pos,8);
+		}
+		*txlen = 5;
+		memcpy(tx,"\xc0",nbytes(*txlen));
+	}	break;
 			
-			// Received UID, crypto tag answer, or read block response
-		case 32: {
-			if (!bCrypto) {
-				*txlen = 64;
-				memcpy(tx,NrAr,8);
-				bCrypto = true;
-			} else {
-				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]);
-				bCrypto = false;
-				if ((auth_table_pos+8) == auth_table_len) {
-					return false;
-				}
-				auth_table_pos += 8;
-				memcpy(NrAr,auth_table+auth_table_pos,8);
+		// Received UID, crypto tag answer, or read block response
+	case 32: {
+		if (!bCrypto) {
+			*txlen = 64;
+			memcpy(tx,NrAr,8);
+			bCrypto = true;
+		} else {
+			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]);
+			bCrypto = false;
+			if ((auth_table_pos+8) == auth_table_len) {
+				return false;
 			}
-		} break;
+			auth_table_pos += 8;
+			memcpy(NrAr,auth_table+auth_table_pos,8);
+		}
+	} break;
 			
-		default: {
-			Dbprintf("Uknown frame length: %d",rxlen);
-			return false;
-		} break;
+	default: {
+		Dbprintf("Uknown frame length: %d",rxlen);
+		return false;
+	} break;
 	}
 	
 	return true;
 }
 
+static bool hitag2_read_uid(byte_t* rx, const size_t rxlen, byte_t* tx, size_t* txlen) {
+	// Reset the transmission frame length
+	*txlen = 0;
+
+	// Try to find out which command was send by selecting on length (in bits)
+	switch (rxlen) {
+		// No answer, try to resurrect
+	case 0: {
+		// Just starting or if there is no answer
+		*txlen = 5;
+		memcpy(tx,"\xc0",nbytes(*txlen));
+	} break;
+		// Received UID
+	case 32: {
+		// Check if we received answer tag (at)
+		if (bAuthenticating) {
+			bAuthenticating = false;
+		} else {
+			// Store the received block
+			memcpy(tag.sectors[blocknr],rx,4);
+			blocknr++;
+		}
+		if (blocknr > 0) {
+			//DbpString("Read successful!");
+			bSuccessful = true;
+			return false;
+		}
+	} break;
+		// Unexpected response
+	default: {
+		Dbprintf("Uknown frame length: %d",rxlen);
+		return false;
+	} break;
+	}
+	return true;
+}
+
 void SnoopHitag(uint32_t type) {
 	int frame_count;
 	int response;
@@ -597,20 +816,25 @@ void SnoopHitag(uint32_t type) {
 	byte_t rx[HITAG_FRAME_LEN];
 	size_t rxlen=0;
 	
-	// Clean up trace and prepare it for storing frames
-	iso14a_set_tracing(TRUE);
-	iso14a_clear_trace();
+	FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
 
+	// Clean up trace and prepare it for storing frames
+	set_tracing(TRUE);
+	clear_trace();
+	
 	auth_table_len = 0;
 	auth_table_pos = 0;
+
+	BigBuf_free();
+	auth_table = (byte_t *)BigBuf_malloc(AUTH_TABLE_LENGTH);
 	memset(auth_table, 0x00, AUTH_TABLE_LENGTH);
-	
+
 	DbpString("Starting Hitag2 snoop");
 	LED_D_ON();
 	
 	// Set up eavesdropping mode, frequency divisor which will drive the FPGA
 	// and analog mux selection.
-	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_EDGE_DETECT);
+	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_EDGE_DETECT  | FPGA_LF_EDGE_DETECT_TOGGLE_MODE);
 	FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
 	SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
 	RELAY_OFF();
@@ -626,7 +850,7 @@ void SnoopHitag(uint32_t type) {
 	AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_TC1);
 	AT91C_BASE_PIOA->PIO_BSR = GPIO_SSC_FRAME;
 	
-  // Disable timer during configuration	
+	// Disable timer during configuration	
 	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
 	
 	// Capture mode, defaul timer source = MCK/2 (TIMER_CLOCK1), TIOA is external trigger,
@@ -638,7 +862,6 @@ void SnoopHitag(uint32_t type) {
 	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
 	
 	// Reset the received frame, frame count and timing info
-	memset(rx,0x00,sizeof(rx));
 	frame_count = 0;
 	response = 0;
 	overflow = 0;
@@ -672,7 +895,7 @@ void SnoopHitag(uint32_t type) {
 				
 				// Only handle if reader frame and rising edge, or tag frame and falling edge
 				if (reader_frame != rising_edge) {
-				  overflow += ra;
+					overflow += ra;
 					continue;
 				}
 				
@@ -747,7 +970,7 @@ void SnoopHitag(uint32_t type) {
 		// Check if frame was captured
 		if(rxlen > 0) {
 			frame_count++;
-			if (!LogTrace(rx,nbytes(rxlen),response,0,reader_frame)) {
+			if (!LogTraceHitag(rx,rxlen,response,0,reader_frame)) {
 				DbpString("Trace full");
 				break;
 			}
@@ -781,14 +1004,14 @@ void SnoopHitag(uint32_t type) {
 		// Reset the timer to restart while-loop that receives frames
 		AT91C_BASE_TC1->TC_CCR = AT91C_TC_SWTRG;
 	}
-    LED_A_ON();
+	LED_A_ON();
 	LED_B_OFF();
 	LED_C_OFF();
 	LED_D_OFF();
 	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
-    AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS;
+	AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS;
 	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
-    LED_A_OFF();
+	LED_A_OFF();
 	
 //	Dbprintf("frame received: %d",frame_count);
 //	Dbprintf("Authentication Attempts: %d",(auth_table_len/8));
@@ -806,11 +1029,17 @@ void SimulateHitagTag(bool tag_mem_supplied, byte_t* data) {
 	bool bQuitTraceFull = false;
 	bQuiet = false;
 	
+	FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
+
 	// Clean up trace and prepare it for storing frames
-    iso14a_set_tracing(TRUE);
-    iso14a_clear_trace();
+	set_tracing(TRUE);
+	clear_trace();
+
 	auth_table_len = 0;
 	auth_table_pos = 0;
+	byte_t* auth_table;
+	BigBuf_free();
+	auth_table = (byte_t *)BigBuf_malloc(AUTH_TABLE_LENGTH);
 	memset(auth_table, 0x00, AUTH_TABLE_LENGTH);
 
 	DbpString("Starting Hitag2 simulation");
@@ -852,21 +1081,21 @@ void SimulateHitagTag(bool tag_mem_supplied, byte_t* data) {
 	AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_TC1);
 	AT91C_BASE_PIOA->PIO_BSR = GPIO_SSC_FRAME;
 	
-  // Disable timer during configuration	
+	// Disable timer during configuration	
 	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
 
-	// Capture mode, defaul timer source = MCK/2 (TIMER_CLOCK1), TIOA is external trigger,
+	// Capture mode, default timer source = MCK/2 (TIMER_CLOCK1), TIOA is external trigger,
 	// external trigger rising edge, load RA on rising edge of TIOA.
 	AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK | AT91C_TC_ETRGEDG_RISING | AT91C_TC_ABETRG | AT91C_TC_LDRA_RISING;
 	
-	// Enable and reset counter
-	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
-
 	// Reset the received frame, frame count and timing info
 	memset(rx,0x00,sizeof(rx));
 	frame_count = 0;
 	response = 0;
 	overflow = 0;
+
+	// Enable and reset counter
+	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
 	
 	while(!BUTTON_PRESS()) {
 		// Watchdog hit
@@ -910,7 +1139,7 @@ void SimulateHitagTag(bool tag_mem_supplied, byte_t* data) {
 		if(rxlen > 4) {
 			frame_count++;
 			if (!bQuiet) {
-				if (!LogTrace(rx,nbytes(rxlen),response,0,true)) {
+				if (!LogTraceHitag(rx,rxlen,response,0,true)) {
 					DbpString("Trace full");
 					if (bQuitTraceFull) {
 						break;
@@ -939,7 +1168,7 @@ void SimulateHitagTag(bool tag_mem_supplied, byte_t* data) {
 				hitag_send_frame(tx,txlen);
 				// Store the frame in the trace
 				if (!bQuiet) {
-					if (!LogTrace(tx,nbytes(txlen),0,0,false)) {
+					if (!LogTraceHitag(tx,txlen,0,0,false)) {
 						DbpString("Trace full");
 						if (bQuitTraceFull) {
 							break;
@@ -970,9 +1199,9 @@ void SimulateHitagTag(bool tag_mem_supplied, byte_t* data) {
 	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
 	AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS;
 	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
-//	Dbprintf("frame received: %d",frame_count);
-//	Dbprintf("Authentication Attempts: %d",(auth_table_len/8));
-//	DbpString("All done");
+	
+	DbpString("Sim Stopped");
+	
 }
 
 void ReaderHitag(hitag_function htf, hitag_data* htd) {
@@ -990,45 +1219,356 @@ void ReaderHitag(hitag_function htf, hitag_data* htd) {
 	int t_wait = HITAG_T_WAIT_MAX;
 	bool bStop;
 	bool bQuitTraceFull = false;
-	
+  
+	FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
+	// Reset the return status
+	bSuccessful = false;
+  
 	// Clean up trace and prepare it for storing frames
-    iso14a_set_tracing(TRUE);
-    iso14a_clear_trace();
-	DbpString("Starting Hitag reader family");
+	set_tracing(TRUE);
+	clear_trace();
+
+	//DbpString("Starting Hitag reader family");
 
 	// Check configuration
 	switch(htf) {
+	case RHT2F_PASSWORD: {
+		Dbprintf("List identifier in password mode");
+		memcpy(password,htd->pwd.password,4);
+		blocknr = 0;
+		bQuitTraceFull = false;
+		bQuiet = false;
+		bPwd = false;
+	} break;
+	case RHT2F_AUTHENTICATE: {
+		DbpString("Authenticating using nr,ar pair:");
+		memcpy(NrAr,htd->auth.NrAr,8);
+		Dbhexdump(8,NrAr,false);
+		bQuiet = false;
+		bCrypto = false;
+		bAuthenticating = false;
+		bQuitTraceFull = true;
+	} break;
+	case RHT2F_CRYPTO: 
+	{
+		DbpString("Authenticating using key:");
+		memcpy(key,htd->crypto.key,6);	  //HACK; 4 or 6??  I read both in the code.
+		Dbhexdump(6,key,false);
+		blocknr = 0;
+		bQuiet = false;
+		bCrypto = false;
+		bAuthenticating = false;
+		bQuitTraceFull = true;
+	} break;
+	case RHT2F_TEST_AUTH_ATTEMPTS: {
+		Dbprintf("Testing %d authentication attempts",(auth_table_len/8));
+		auth_table_pos = 0;
+		memcpy(NrAr, auth_table, 8);
+		bQuitTraceFull = false;
+		bQuiet = false;
+		bCrypto = false;
+	} break;
+	case RHT2F_UID_ONLY: {
+		blocknr = 0;
+		bQuiet = false;
+		bCrypto = false;
+		bAuthenticating = false;
+		bQuitTraceFull = true;
+	} break;
+	default: {
+		Dbprintf("Error, unknown function: %d",htf);
+		return;
+	} break;
+	}
+	
+	LED_D_ON();
+	hitag2_init();
+	
+	// Configure output and enable pin that is connected to the FPGA (for modulating)
+	AT91C_BASE_PIOA->PIO_OER = GPIO_SSC_DOUT;
+	AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT;
+	
+	// Set fpga in edge detect with reader field, we can modulate as reader now
+	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_EDGE_DETECT | FPGA_LF_EDGE_DETECT_READER_FIELD);
+
+	// Set Frequency divisor which will drive the FPGA and analog mux selection
+	FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
+	SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
+	RELAY_OFF();
+
+	// Disable modulation at default, which means enable the field
+	LOW(GPIO_SSC_DOUT);
+
+	// Give it a bit of time for the resonant antenna to settle.
+	SpinDelay(30);
+	
+	// Enable Peripheral Clock for TIMER_CLOCK0, used to measure exact timing before answering
+	AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_TC0);
+
+	// Enable Peripheral Clock for TIMER_CLOCK1, used to capture edges of the tag frames
+	AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_TC1);
+	AT91C_BASE_PIOA->PIO_BSR = GPIO_SSC_FRAME;
+	
+	// Disable timer during configuration	
+	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
+	
+	// Capture mode, defaul timer source = MCK/2 (TIMER_CLOCK1), TIOA is external trigger,
+	// external trigger rising edge, load RA on falling edge of TIOA.
+	AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK | AT91C_TC_ETRGEDG_FALLING | AT91C_TC_ABETRG | AT91C_TC_LDRA_FALLING;
+	
+	// Enable and reset counters
+	AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
+	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
+
+	// Reset the received frame, frame count and timing info
+	frame_count = 0;
+	response = 0;
+	lastbit = 1;
+	bStop = false;
+
+	// Tag specific configuration settings (sof, timings, etc.)
+	if (htf < 10){
+		// hitagS settings
+		reset_sof = 1;
+		t_wait = 200;
+		//DbpString("Configured for hitagS reader");
+	} else if (htf < 20) {
+		// hitag1 settings
+		reset_sof = 1;
+		t_wait = 200;
+		//DbpString("Configured for hitag1 reader");
+	} else if (htf < 30) {
+		// hitag2 settings
+		reset_sof = 4;
+		t_wait = HITAG_T_WAIT_2;
+		//DbpString("Configured for hitag2 reader");
+	} else {
+		Dbprintf("Error, unknown hitag reader type: %d",htf);
+		return;
+	}
+	uint8_t attempt_count=0;
+	while(!bStop && !BUTTON_PRESS()) {
+		// Watchdog hit
+		WDT_HIT();
+		
+		// Check if frame was captured and store it
+		if(rxlen > 0) {
+			frame_count++;
+			if (!bQuiet) {
+				if (!LogTraceHitag(rx,rxlen,response,0,false)) {
+					DbpString("Trace full");
+					if (bQuitTraceFull) {
+						break;
+					} else {
+						bQuiet = true;
+					}
+				}
+			}
+		}
+		
+		// By default reset the transmission buffer
+		tx = txbuf;
+		switch(htf) {
 		case RHT2F_PASSWORD: {
-            Dbprintf("List identifier in password mode");
-			memcpy(password,htd->pwd.password,4);
-      blocknr = 0;
-			bQuitTraceFull = false;
-			bQuiet = false;
-			bPwd = false;
+			bStop = !hitag2_password(rx,rxlen,tx,&txlen);
 		} break;
 		case RHT2F_AUTHENTICATE: {
-			DbpString("Authenticating in crypto mode");
-			memcpy(NrAr,htd->auth.NrAr,8);
-			Dbprintf("Reader-challenge:");
-			Dbhexdump(8,NrAr,false);
-			bQuiet = false;
-			bCrypto = false;
-			bQuitTraceFull = true;
+			bStop = !hitag2_authenticate(rx,rxlen,tx,&txlen);
+		} break;
+		case RHT2F_CRYPTO: {
+			bStop = !hitag2_crypto(rx,rxlen,tx,&txlen, false);
 		} break;
-
 		case RHT2F_TEST_AUTH_ATTEMPTS: {
-			Dbprintf("Testing %d authentication attempts",(auth_table_len/8));
-			auth_table_pos = 0;
-			memcpy(NrAr,auth_table,8);
-			bQuitTraceFull = false;
-			bQuiet = false;
-			bCrypto = false;
+			bStop = !hitag2_test_auth_attempts(rx,rxlen,tx,&txlen);
+		} break;
+		case RHT2F_UID_ONLY: {
+			bStop = !hitag2_read_uid(rx, rxlen, tx, &txlen);
+			attempt_count++; //attempt 3 times to get uid then quit
+			if (!bStop && attempt_count == 3) bStop = true;
 		} break;
-			
 		default: {
 			Dbprintf("Error, unknown function: %d",htf);
 			return;
 		} break;
+		}
+		
+		// Send and store the reader command
+		// Disable timer 1 with external trigger to avoid triggers during our own modulation
+		AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
+			
+		// Wait for HITAG_T_WAIT_2 carrier periods after the last tag bit before transmitting,
+		// Since the clock counts since the last falling edge, a 'one' means that the
+		// falling edge occured halfway the period. with respect to this falling edge,
+		// we need to wait (T_Wait2 + half_tag_period) when the last was a 'one'.
+		// All timer values are in terms of T0 units
+		while(AT91C_BASE_TC0->TC_CV < T0*(t_wait+(HITAG_T_TAG_HALF_PERIOD*lastbit)));
+			
+		//Dbprintf("DEBUG: Sending reader frame");
+		
+		// Transmit the reader frame
+		hitag_reader_send_frame(tx,txlen);
+
+                // Enable and reset external trigger in timer for capturing future frames
+		AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
+
+		// Add transmitted frame to total count
+		if(txlen > 0) {
+			frame_count++;
+			if (!bQuiet) {
+				// Store the frame in the trace
+				if (!LogTraceHitag(tx,txlen,HITAG_T_WAIT_2,0,true)) {
+					if (bQuitTraceFull) {
+						break;
+					} else {
+						bQuiet = true;
+					}
+				}
+			}
+		}
+
+		// Reset values for receiving frames
+		memset(rx,0x00,sizeof(rx));
+		rxlen = 0;
+		lastbit = 1;
+		bSkip = true;
+		tag_sof = reset_sof;
+		response = 0;
+		//Dbprintf("DEBUG: Waiting to receive frame");
+		uint32_t errorCount = 0;
+
+		// Receive frame, watch for at most T0*EOF periods
+		while (AT91C_BASE_TC1->TC_CV < T0*HITAG_T_WAIT_MAX) {
+			// Check if falling edge in tag modulation is detected
+			if(AT91C_BASE_TC1->TC_SR & AT91C_TC_LDRAS) {
+				// Retrieve the new timing values 
+				int ra = (AT91C_BASE_TC1->TC_RA/T0);
+				
+				// Reset timer every frame, we have to capture the last edge for timing
+				AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG;
+				
+				LED_B_ON();
+				 
+				// Capture tag frame (manchester decoding using only falling edges)
+				if(ra >= HITAG_T_EOF) {
+					if (rxlen != 0) {
+						//Dbprintf("DEBUG: Wierd1");
+					}
+					// Capture the T0 periods that have passed since last communication or field drop (reset)
+					// We always recieve a 'one' first, which has the falling edge after a half period |-_|
+					response = ra-HITAG_T_TAG_HALF_PERIOD;
+				} else if(ra >= HITAG_T_TAG_CAPTURE_FOUR_HALF) {
+					// Manchester coding example |-_|_-|-_| (101)
+
+					//need to test to verify we don't exceed memory...
+					//if ( ((rxlen+2) / 8) > HITAG_FRAME_LEN) {
+					//	break;
+					//}
+					rx[rxlen / 8] |= 0 << (7-(rxlen%8));
+					rxlen++;
+					rx[rxlen / 8] |= 1 << (7-(rxlen%8));
+					rxlen++;
+				} else if(ra >= HITAG_T_TAG_CAPTURE_THREE_HALF) {
+					// Manchester coding example |_-|...|_-|-_| (0...01)
+					
+					//need to test to verify we don't exceed memory...
+					//if ( ((rxlen+2) / 8) > HITAG_FRAME_LEN) {
+					//	break;
+					//}
+					rx[rxlen / 8] |= 0 << (7-(rxlen%8));
+					rxlen++;
+					// We have to skip this half period at start and add the 'one' the second time 
+					if (!bSkip) {
+						rx[rxlen / 8] |= 1 << (7-(rxlen%8));
+						rxlen++;
+					}
+					lastbit = !lastbit;
+					bSkip = !bSkip;
+				} else if(ra >= HITAG_T_TAG_CAPTURE_TWO_HALF) {
+					// Manchester coding example |_-|_-| (00) or |-_|-_| (11)
+
+					//need to test to verify we don't exceed memory...
+					//if ( ((rxlen+2) / 8) > HITAG_FRAME_LEN) {
+					//	break;
+					//}
+					if (tag_sof) {
+						// Ignore bits that are transmitted during SOF
+						tag_sof--;
+					} else {
+						// bit is same as last bit
+						rx[rxlen / 8] |= lastbit << (7-(rxlen%8));
+						rxlen++;
+					}
+				} else {
+					//Dbprintf("DEBUG: Wierd2");
+					errorCount++;
+					// Ignore wierd value, is to small to mean anything
+				}
+			}
+			//if we saw over 100 wierd values break it probably isn't hitag...
+			if (errorCount >100) break;
+			// We can break this loop if we received the last bit from a frame
+			if (AT91C_BASE_TC1->TC_CV > T0*HITAG_T_EOF) {
+				if (rxlen>0) break;
+			}
+		}
+	}
+	//Dbprintf("DEBUG: Done waiting for frame");
+	
+	LED_B_OFF();
+	LED_D_OFF();
+	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
+	AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS;
+	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
+	//Dbprintf("frame received: %d",frame_count);
+	//DbpString("All done");
+	cmd_send(CMD_ACK,bSuccessful,0,0,(byte_t*)tag.sectors,48);
+}
+
+void WriterHitag(hitag_function htf, hitag_data* htd, int page) {
+	int frame_count;
+	int response;
+	byte_t rx[HITAG_FRAME_LEN];
+	size_t rxlen=0;
+	byte_t txbuf[HITAG_FRAME_LEN];
+	byte_t* tx = txbuf;
+	size_t txlen=0;
+	int lastbit;
+	bool bSkip;
+	int reset_sof; 
+	int tag_sof;
+	int t_wait = HITAG_T_WAIT_MAX;
+	bool bStop;
+	bool bQuitTraceFull = false;
+  
+	FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
+	// Reset the return status
+	bSuccessful = false;
+  
+	// Clean up trace and prepare it for storing frames
+	set_tracing(TRUE);
+	clear_trace();
+
+	//DbpString("Starting Hitag reader family");
+
+	// Check configuration
+	switch(htf) {
+	case WHT2F_CRYPTO:
+	{
+		DbpString("Authenticating using key:");
+		memcpy(key,htd->crypto.key,6);	  //HACK; 4 or 6??  I read both in the code.
+		memcpy(writedata, htd->crypto.data, 4);
+		Dbhexdump(6,key,false);
+		blocknr = page;
+		bQuiet = false;
+		bCrypto = false;
+		bAuthenticating = false;
+		bQuitTraceFull = true;
+		writestate = WRITE_STATE_START;
+	} break;
+	default: {
+		Dbprintf("Error, unknown function: %d",htf);
+		return;
+	} break;
 	}
 	
 	LED_D_ON();
@@ -1059,7 +1599,7 @@ void ReaderHitag(hitag_function htf, hitag_data* htd) {
 	AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_TC1);
 	AT91C_BASE_PIOA->PIO_BSR = GPIO_SSC_FRAME;
 	
-    // Disable timer during configuration	
+	// Disable timer during configuration	
 	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
 	
 	// Capture mode, defaul timer source = MCK/2 (TIMER_CLOCK1), TIOA is external trigger,
@@ -1081,22 +1621,21 @@ void ReaderHitag(hitag_function htf, hitag_data* htd) {
 		// hitagS settings
 		reset_sof = 1;
 		t_wait = 200;
-		DbpString("Configured for hitagS reader");
+		//DbpString("Configured for hitagS reader");
 	} else if (htf < 20) {
 		// hitag1 settings
 		reset_sof = 1;
 		t_wait = 200;
-		DbpString("Configured for hitag1 reader");
+		//DbpString("Configured for hitag1 reader");
 	} else if (htf < 30) {
 		// hitag2 settings
 		reset_sof = 4;
 		t_wait = HITAG_T_WAIT_2;
-		DbpString("Configured for hitag2 reader");
+		//DbpString("Configured for hitag2 reader");
 	} else {
-        Dbprintf("Error, unknown hitag reader type: %d",htf);
-        return;
-    }
-		
+		Dbprintf("Error, unknown hitag reader type: %d",htf);
+		return;
+	}
 	while(!bStop && !BUTTON_PRESS()) {
 		// Watchdog hit
 		WDT_HIT();
@@ -1105,7 +1644,7 @@ void ReaderHitag(hitag_function htf, hitag_data* htd) {
 		if(rxlen > 0) {
 			frame_count++;
 			if (!bQuiet) {
-				if (!LogTrace(rx,nbytes(rxlen),response,0,false)) {
+				if (!LogTraceHitag(rx,rxlen,response,0,false)) {
 					DbpString("Trace full");
 					if (bQuitTraceFull) {
 						break;
@@ -1119,19 +1658,13 @@ void ReaderHitag(hitag_function htf, hitag_data* htd) {
 		// By default reset the transmission buffer
 		tx = txbuf;
 		switch(htf) {
-			case RHT2F_PASSWORD: {
-				bStop = !hitag2_password(rx,rxlen,tx,&txlen);
-			} break;
-			case RHT2F_AUTHENTICATE: {
-				bStop = !hitag2_authenticate(rx,rxlen,tx,&txlen);
-			} break;
-			case RHT2F_TEST_AUTH_ATTEMPTS: {
-				bStop = !hitag2_test_auth_attempts(rx,rxlen,tx,&txlen);
-			} break;
-			default: {
-				Dbprintf("Error, unknown function: %d",htf);
-				return;
-			} break;
+		case WHT2F_CRYPTO: {
+			bStop = !hitag2_crypto(rx,rxlen,tx,&txlen, true);
+		} break;
+		default: {
+			Dbprintf("Error, unknown function: %d",htf);
+			return;
+		} break;
 		}
 		
 		// Send and store the reader command
@@ -1144,11 +1677,13 @@ void ReaderHitag(hitag_function htf, hitag_data* htd) {
 		// we need to wait (T_Wait2 + half_tag_period) when the last was a 'one'.
 		// All timer values are in terms of T0 units
 		while(AT91C_BASE_TC0->TC_CV < T0*(t_wait+(HITAG_T_TAG_HALF_PERIOD*lastbit)));
+			
+		//Dbprintf("DEBUG: Sending reader frame");
 		
 		// Transmit the reader frame
 		hitag_reader_send_frame(tx,txlen);
 
-		// Enable and reset external trigger in timer for capturing future frames
+                // Enable and reset external trigger in timer for capturing future frames
 		AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
 
 		// Add transmitted frame to total count
@@ -1156,7 +1691,7 @@ void ReaderHitag(hitag_function htf, hitag_data* htd) {
 			frame_count++;
 			if (!bQuiet) {
 				// Store the frame in the trace
-				if (!LogTrace(tx,nbytes(txlen),HITAG_T_WAIT_2,0,true)) {
+				if (!LogTraceHitag(tx,txlen,HITAG_T_WAIT_2,0,true)) {
 					if (bQuitTraceFull) {
 						break;
 					} else {
@@ -1165,7 +1700,7 @@ void ReaderHitag(hitag_function htf, hitag_data* htd) {
 				}
 			}
 		}
-				
+
 		// Reset values for receiving frames
 		memset(rx,0x00,sizeof(rx));
 		rxlen = 0;
@@ -1173,7 +1708,9 @@ void ReaderHitag(hitag_function htf, hitag_data* htd) {
 		bSkip = true;
 		tag_sof = reset_sof;
 		response = 0;
-		
+		//Dbprintf("DEBUG: Waiting to receive frame");
+		uint32_t errorCount = 0;
+
 		// Receive frame, watch for at most T0*EOF periods
 		while (AT91C_BASE_TC1->TC_CV < T0*HITAG_T_WAIT_MAX) {
 			// Check if falling edge in tag modulation is detected
@@ -1185,23 +1722,33 @@ void ReaderHitag(hitag_function htf, hitag_data* htd) {
 				AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG;
 				
 				LED_B_ON();
-				
+				 
 				// Capture tag frame (manchester decoding using only falling edges)
 				if(ra >= HITAG_T_EOF) {
 					if (rxlen != 0) {
-						//DbpString("wierd1?");
+						//Dbprintf("DEBUG: Wierd1");
 					}
 					// Capture the T0 periods that have passed since last communication or field drop (reset)
 					// We always recieve a 'one' first, which has the falling edge after a half period |-_|
 					response = ra-HITAG_T_TAG_HALF_PERIOD;
 				} else if(ra >= HITAG_T_TAG_CAPTURE_FOUR_HALF) {
 					// Manchester coding example |-_|_-|-_| (101)
+
+					//need to test to verify we don't exceed memory...
+					//if ( ((rxlen+2) / 8) > HITAG_FRAME_LEN) {
+					//	break;
+					//}
 					rx[rxlen / 8] |= 0 << (7-(rxlen%8));
 					rxlen++;
 					rx[rxlen / 8] |= 1 << (7-(rxlen%8));
 					rxlen++;
 				} else if(ra >= HITAG_T_TAG_CAPTURE_THREE_HALF) {
 					// Manchester coding example |_-|...|_-|-_| (0...01)
+					
+					//need to test to verify we don't exceed memory...
+					//if ( ((rxlen+2) / 8) > HITAG_FRAME_LEN) {
+					//	break;
+					//}
 					rx[rxlen / 8] |= 0 << (7-(rxlen%8));
 					rxlen++;
 					// We have to skip this half period at start and add the 'one' the second time 
@@ -1213,6 +1760,11 @@ void ReaderHitag(hitag_function htf, hitag_data* htd) {
 					bSkip = !bSkip;
 				} else if(ra >= HITAG_T_TAG_CAPTURE_TWO_HALF) {
 					// Manchester coding example |_-|_-| (00) or |-_|-_| (11)
+
+					//need to test to verify we don't exceed memory...
+					//if ( ((rxlen+2) / 8) > HITAG_FRAME_LEN) {
+					//	break;
+					//}
 					if (tag_sof) {
 						// Ignore bits that are transmitted during SOF
 						tag_sof--;
@@ -1222,22 +1774,34 @@ void ReaderHitag(hitag_function htf, hitag_data* htd) {
 						rxlen++;
 					}
 				} else {
+					//Dbprintf("DEBUG: Wierd2");
+					errorCount++;
 					// Ignore wierd value, is to small to mean anything
 				}
 			}
-
+			//if we saw over 100 wierd values break it probably isn't hitag...
+			if (errorCount >100) break;
 			// We can break this loop if we received the last bit from a frame
 			if (AT91C_BASE_TC1->TC_CV > T0*HITAG_T_EOF) {
 				if (rxlen>0) break;
 			}
 		}
+		
+		// Wait some extra time for flash to be programmed
+		if ((rxlen == 0) && (writestate == WRITE_STATE_PROG))
+		{
+			AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG;
+			while(AT91C_BASE_TC0->TC_CV < T0*(HITAG_T_PROG - HITAG_T_WAIT_MAX));
+		}
 	}
+	//Dbprintf("DEBUG: Done waiting for frame");
+	
 	LED_B_OFF();
 	LED_D_OFF();
 	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
 	AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS;
 	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
-	
-//	Dbprintf("frame received: %d",frame_count);
-//	DbpString("All done");
+	//Dbprintf("frame received: %d",frame_count);
+	//DbpString("All done");
+	cmd_send(CMD_ACK,bSuccessful,0,0,(byte_t*)tag.sectors,48);
 }