X-Git-Url: http://git.zerfleddert.de/cgi-bin/gitweb.cgi/proxmark3-svn/blobdiff_plain/6fcb5dda51b7bedb8a8bb1bb216a2f4a60a6095c..383f4e2479f45df77d44078618454d1ddbf52290:/armsrc/hitagS.c

diff --git a/armsrc/hitagS.c b/armsrc/hitagS.c
index 58e09a45..3c247d55 100644
--- a/armsrc/hitagS.c
+++ b/armsrc/hitagS.c
@@ -156,112 +156,112 @@ void calc_crc(unsigned char * crc, unsigned char data, unsigned char Bitcount) {
 	} while (--Bitcount);
 }
 
+
 static void hitag_send_bit(int bit) {
 	LED_A_ON();
 	// Reset clock for the next bit
 	AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG;
 
 	switch (m) {
-	case AC2K:
-		if (bit == 0) {
-			// AC Coding --__
-			HIGH(GPIO_SSC_DOUT);
-			while (AT91C_BASE_TC0->TC_CV < T0 * 32)
-				;
-			LOW(GPIO_SSC_DOUT);
-			while (AT91C_BASE_TC0->TC_CV < T0 * 64)
-				;
-		} else {
-			// AC coding -_-_
-			HIGH(GPIO_SSC_DOUT);
-			while (AT91C_BASE_TC0->TC_CV < T0 * 16)
-				;
-			LOW(GPIO_SSC_DOUT);
-			while (AT91C_BASE_TC0->TC_CV < T0 * 32)
-				;
-			HIGH(GPIO_SSC_DOUT);
-			while (AT91C_BASE_TC0->TC_CV < T0 * 48)
-				;
-			LOW(GPIO_SSC_DOUT);
-			while (AT91C_BASE_TC0->TC_CV < T0 * 64)
-				;;
-		}
-		LED_A_OFF();
-		break;
-	case AC4K:
-		if (bit == 0) {
-			// AC Coding --__
-			HIGH(GPIO_SSC_DOUT);
-			while (AT91C_BASE_TC0->TC_CV < T0 * HITAG_T_TAG_HALF_PERIOD)
-				;
-			LOW(GPIO_SSC_DOUT);
-			while (AT91C_BASE_TC0->TC_CV < T0 * HITAG_T_TAG_FULL_PERIOD)
-				;
-		} else {
-			// AC coding -_-_
-			HIGH(GPIO_SSC_DOUT);
-			while (AT91C_BASE_TC0->TC_CV < T0 * 8)
-				;
-			LOW(GPIO_SSC_DOUT);
-			while (AT91C_BASE_TC0->TC_CV < T0 * 16)
-				;
-			HIGH(GPIO_SSC_DOUT);
-			while (AT91C_BASE_TC0->TC_CV < T0 * 24)
-				;
-			LOW(GPIO_SSC_DOUT);
-			while (AT91C_BASE_TC0->TC_CV < T0 * 32)
-				;;
-		}
-		LED_A_OFF();
-		break;
-	case MC4K:
-		if (bit == 0) {
-			// Manchester: Unloaded, then loaded |__--|
-			LOW(GPIO_SSC_DOUT);
-			while (AT91C_BASE_TC0->TC_CV < T0 * 16)
-				;
-			HIGH(GPIO_SSC_DOUT);
-			while (AT91C_BASE_TC0->TC_CV < T0 * 32)
-				;
-		} else {
-			// Manchester: Loaded, then unloaded |--__|
-			HIGH(GPIO_SSC_DOUT);
-			while (AT91C_BASE_TC0->TC_CV < T0 * 16)
-				;
-			LOW(GPIO_SSC_DOUT);
-			while (AT91C_BASE_TC0->TC_CV < T0 * 32)
-				;
-		}
-		LED_A_OFF();
-		break;
-	case MC8K:
-		if (bit == 0) {
-			// Manchester: Unloaded, then loaded |__--|
-			LOW(GPIO_SSC_DOUT);
-			while (AT91C_BASE_TC0->TC_CV < T0 * 8)
-				;
-			HIGH(GPIO_SSC_DOUT);
-			while (AT91C_BASE_TC0->TC_CV < T0 * 16)
-				;
-		} else {
-			// Manchester: Loaded, then unloaded |--__|
-			HIGH(GPIO_SSC_DOUT);
-			while (AT91C_BASE_TC0->TC_CV < T0 * 8)
-				;
-			LOW(GPIO_SSC_DOUT);
-			while (AT91C_BASE_TC0->TC_CV < T0 * 16)
-				;
-		}
-		LED_A_OFF();
-		break;
-	default:
-		break;
+		case AC2K:
+			if (bit == 0) {
+				// AC Coding --__
+				HIGH(GPIO_SSC_DOUT);
+				while (AT91C_BASE_TC0->TC_CV < T0 * 32)
+					;
+				LOW(GPIO_SSC_DOUT);
+				while (AT91C_BASE_TC0->TC_CV < T0 * 64)
+					;
+			} else {
+				// AC coding -_-_
+				HIGH(GPIO_SSC_DOUT);
+				while (AT91C_BASE_TC0->TC_CV < T0 * 16)
+					;
+				LOW(GPIO_SSC_DOUT);
+				while (AT91C_BASE_TC0->TC_CV < T0 * 32)
+					;
+				HIGH(GPIO_SSC_DOUT);
+				while (AT91C_BASE_TC0->TC_CV < T0 * 48)
+					;
+				LOW(GPIO_SSC_DOUT);
+				while (AT91C_BASE_TC0->TC_CV < T0 * 64)
+					;;
+			}
+			LED_A_OFF();
+			break;
+		case AC4K:
+			if (bit == 0) {
+				// AC Coding --__
+				HIGH(GPIO_SSC_DOUT);
+				while (AT91C_BASE_TC0->TC_CV < T0 * HITAG_T_TAG_HALF_PERIOD)
+					;
+				LOW(GPIO_SSC_DOUT);
+				while (AT91C_BASE_TC0->TC_CV < T0 * HITAG_T_TAG_FULL_PERIOD)
+					;
+			} else {
+				// AC coding -_-_
+				HIGH(GPIO_SSC_DOUT);
+				while (AT91C_BASE_TC0->TC_CV < T0 * 8)
+					;
+				LOW(GPIO_SSC_DOUT);
+				while (AT91C_BASE_TC0->TC_CV < T0 * 16)
+					;
+				HIGH(GPIO_SSC_DOUT);
+				while (AT91C_BASE_TC0->TC_CV < T0 * 24)
+					;
+				LOW(GPIO_SSC_DOUT);
+				while (AT91C_BASE_TC0->TC_CV < T0 * 32)
+					;
+			}
+			LED_A_OFF();
+			break;
+		case MC4K:
+			if (bit == 0) {
+				// Manchester: Unloaded, then loaded |__--|
+				LOW(GPIO_SSC_DOUT);
+				while (AT91C_BASE_TC0->TC_CV < T0 * 16)
+					;
+				HIGH(GPIO_SSC_DOUT);
+				while (AT91C_BASE_TC0->TC_CV < T0 * 32)
+					;
+			} else {
+				// Manchester: Loaded, then unloaded |--__|
+				HIGH(GPIO_SSC_DOUT);
+				while (AT91C_BASE_TC0->TC_CV < T0 * 16)
+					;
+				LOW(GPIO_SSC_DOUT);
+				while (AT91C_BASE_TC0->TC_CV < T0 * 32)
+					;
+			}
+			LED_A_OFF();
+			break;
+		case MC8K:
+			if (bit == 0) {
+				// Manchester: Unloaded, then loaded |__--|
+				LOW(GPIO_SSC_DOUT);
+				while (AT91C_BASE_TC0->TC_CV < T0 * 8)
+					;
+				HIGH(GPIO_SSC_DOUT);
+				while (AT91C_BASE_TC0->TC_CV < T0 * 16)
+					;
+			} else {
+				// Manchester: Loaded, then unloaded |--__|
+				HIGH(GPIO_SSC_DOUT);
+				while (AT91C_BASE_TC0->TC_CV < T0 * 8)
+					;
+				LOW(GPIO_SSC_DOUT);
+				while (AT91C_BASE_TC0->TC_CV < T0 * 16)
+					;
+			}
+			LED_A_OFF();
+			break;
+		default:
+			break;
 	}
 }
 
-static void hitag_send_frame(const byte_t* frame, size_t frame_len) {
+static void hitag_tag_send_frame(const byte_t* frame, size_t frame_len) {
 // Send start of frame
-
 	for (size_t i = 0; i < sof_bits; i++) {
 		hitag_send_bit(1);
 	}
@@ -331,12 +331,12 @@ static void hitag_reader_send_bit(int bit) {
 }
 
 static void hitag_reader_send_frame(const byte_t* frame, size_t frame_len) {
-// Send the content of the frame
+	// Send the content of the frame
 	for (size_t i = 0; i < frame_len; i++) {
 		if (frame[0] == 0xf8) {
 			//Dbprintf("BIT: %d",(frame[i / 8] >> (7 - (i % 8))) & 1);
 		}
-		hitag_reader_send_bit((frame[i / 8] >> (7 - (i % 8))) & 1);
+		hitag_reader_send_bit(((frame[i / 8] >> (7 - (i % 8))) & 1));
 	}
 // Send EOF
 	AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG;
@@ -349,6 +349,485 @@ static void hitag_reader_send_frame(const byte_t* frame, size_t frame_len) {
 	LOW(GPIO_SSC_DOUT);
 }
 
+static void hitag_decode_frame_MC(int bitRate, int sofBits, byte_t* rx, size_t* rxlenOrg, int* response, int rawMod[], int rawLen) {
+	size_t rxlen = 0;
+	bool bSkip = true;
+	int lastbit = 1;
+	int tag_sof = 0;
+	int timing = 1;
+	if (bitRate == 8) {
+		timing = 2;
+	}
+
+	for (int i=0; i < rawLen; i++) {
+		int ra = rawMod[i];
+		if (ra >= HITAG_T_EOF) {
+			if (rxlen != 0) {
+				//DbpString("wierd1?");
+			}
+			tag_sof = sofBits;
+
+			// 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 / timing) {
+			tag_sof=0;
+			// Manchester coding example |-_|_-|-_| (101)
+			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 / timing) {
+			tag_sof=0;
+			// Manchester coding example |_-|...|_-|-_| (0...01)
+			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 / timing) {
+			// Manchester coding example |_-|_-| (00) or |-_|-_| (11)
+			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 {
+			// Ignore wierd value, is to small to mean anything
+		}
+	}	
+	*rxlenOrg = rxlen;
+}
+
+/* 
+static void hitag_decode_frame_AC2K_rising(byte_t* rx, size_t* rxlenOrg, int* response, int rawMod[], int rawLen) {
+	int tag_sof = 1; //skip start of frame
+	size_t rxlen = 0;
+
+	for (int i=0; i < rawLen; i++) {
+		int ra = rawMod[i];
+		if (ra >= HITAG_T_EOF) {
+			if (rxlen != 0) {
+				//DbpString("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 |-_|
+			tag_sof = 1;
+			*response = ra - HITAG_T_TAG_HALF_PERIOD;
+		} else if (ra >= HITAG_T_TAG_CAPTURE_FOUR_HALF) {
+			// AC coding example |--__|--__| means 0
+			rx[rxlen / 8] |= 0 << (7 - (rxlen % 8));
+			rxlen++;
+			if (rawMod[i+1] == 0) { //TODO: this is weird - may we miss one capture with current configuration
+				rx[rxlen / 8] |= 0 << (7 - (rxlen % 8));
+				rxlen++;
+				i++; //drop next capture
+			}
+		} else if (ra >= HITAG_T_TAG_CAPTURE_TWO_HALF) {
+			if (tag_sof) {
+				// Ignore bits that are transmitted during SOF
+				tag_sof--;
+			} else {
+				// AC coding example |-_-_|-_-_| which means 1
+				//check if another high is coming (only -_-_ = 1) except end of the frame (support 0)
+				if (rawMod[i+1] == 0 || rawMod[i+1] >= HITAG_T_TAG_CAPTURE_TWO_HALF) {
+					rx[rxlen / 8] |= 1 << (7 - (rxlen % 8));
+					rxlen++;
+					i++; //drop next capture
+				} else {
+					Dbprintf("got weird high - %d,%d", ra, rawMod[i+1]);
+				}
+			}
+		} else {
+			// Ignore wierd value, is to small to mean anything
+		}
+	}
+	*rxlenOrg = rxlen;
+}
+*/
+
+static void hitag_decode_frame_AC(int bitRate, int sofBits, byte_t* rx, size_t* rxlenOrg, int* response, int rawMod[], int rawLen) {
+	int tag_sof = 1;
+	size_t rxlen = 0;
+	int timing = 1;
+	if (bitRate == 4) {
+		timing = 2;
+	}
+	
+
+	for (int i=0; i < rawLen; i++) {
+		int ra = rawMod[i];
+		if (ra >= HITAG_T_EOF) {
+			if (rxlen != 0) {
+				//DbpString("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 |-_|
+			tag_sof = sofBits;
+			*response = ra - HITAG_T_TAG_HALF_PERIOD;
+		} else if (ra >= HITAG_T_TAG_CAPTURE_FOUR_HALF / timing) {
+			tag_sof=0;
+
+			// AC coding example |--__|--__| means 0
+			rx[rxlen / 8] |= 0 << (7 - (rxlen % 8));
+			rxlen++;
+		} else if (ra >= HITAG_T_TAG_CAPTURE_THREE_HALF / timing) {
+			tag_sof=0;
+
+			if (rawMod[i-1] >= HITAG_T_TAG_CAPTURE_THREE_HALF / timing) {
+				//treat like HITAG_T_TAG_CAPTURE_TWO_HALF
+				if (rawMod[i+1] >= HITAG_T_TAG_CAPTURE_TWO_HALF / timing) {
+					rx[rxlen / 8] |= 1 << (7 - (rxlen % 8));
+					rxlen++;
+					i++; //drop next capture
+				} else {
+					Dbprintf("got weird value - %d,%d", ra, rawMod[i+1]);
+				}
+			} else {
+				//treat like HITAG_T_TAG_CAPTURE_FOUR_HALF
+				rx[rxlen / 8] |= 0 << (7 - (rxlen % 8));
+				rxlen++;
+			}
+		} else if (ra >= HITAG_T_TAG_CAPTURE_TWO_HALF / timing) {
+			if (tag_sof) {
+				// Ignore bits that are transmitted during SOF
+				tag_sof--;
+			} else {
+				// AC coding example |-_-_|-_-_| which means 1
+				//check if another high is coming (only -_-_ = 1) except end of the frame (support 0)
+				if (rawMod[i+1] == 0 || rawMod[i+1] >= HITAG_T_TAG_CAPTURE_TWO_HALF / timing) {
+					rx[rxlen / 8] |= 1 << (7 - (rxlen % 8));
+					rxlen++;
+					i++; //drop next capture
+				} else {
+					Dbprintf("got weird value - %d,%d", ra, rawMod[i+1]);
+				}
+			}
+		} else {
+			// Ignore wierd value, is to small to mean anything
+		}
+	}
+	*rxlenOrg = rxlen;
+}
+
+static void hitag_receive_frame(byte_t* rx, size_t* rxlen, int* response) {
+	int rawMod[200] = {0};
+	int rawLen = 0;
+	int i = 0;
+	int sofBits = 0;
+
+	m = MC4K;
+	if (tag.pstate == READY) {
+		switch (tag.mode) {
+			case STANDARD:
+				m = AC2K;
+				sofBits = 1;
+				break;
+			case ADVANCED:
+				m = AC2K;
+				sofBits = 5; //3 sof bits but 5 captures 
+				break;
+			case FAST_ADVANCED:
+				m = AC4K;
+				sofBits = 5; //3 sof bits but 5 captures 
+				break;
+			default:
+				break;
+		}
+	} else {
+		switch (tag.mode) {
+			case STANDARD:
+				m = MC4K;
+				sofBits = 0; //in theory 1
+				break;
+			case ADVANCED:
+				m = MC4K;
+				sofBits = 5; //in theory 6
+				break;
+			case FAST_ADVANCED:
+				m = MC8K;
+				sofBits = 5; //in theory 6
+				break;
+			default:
+				break;
+		}
+	}
+	
+	//rising AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK | AT91C_TC_ETRGEDG_RISING | AT91C_TC_ABETRG | AT91C_TC_LDRA_RISING;
+	AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK | AT91C_TC_ETRGEDG_FALLING | AT91C_TC_ABETRG | AT91C_TC_LDRA_FALLING;
+
+
+	//first capture timing values
+	while (AT91C_BASE_TC1->TC_CV < T0 * HITAG_T_WAIT_MAX ) {
+		// Check if rising edge in 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);
+
+			LED_B_ON();
+			// Reset timer every frame, we have to capture the last edge for timing
+			AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
+			//AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG;
+
+			if (rawLen >= 200) { //avoid exception
+				break;
+			}
+			rawMod[rawLen] = ra;
+			rawLen++;
+
+			// 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 (rawLen > 2) {
+					if (DEBUG >= 2) { Dbprintf("AT91C_BASE_TC1->TC_CV > T0 * HITAG_T_EOF breaking (%d)", rawLen); }
+					break;
+				}
+			}
+		}
+	}
+
+	if (DEBUG >= 2) {
+		for (i=0; i < rawLen; i+=20) {
+			Dbprintf("raw modulation: - %d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d", 
+				rawMod[i],rawMod[i+1],rawMod[i+2],rawMod[i+3], rawMod[i+4],rawMod[i+5],rawMod[i+6],rawMod[i+7],
+				rawMod[i+8],rawMod[i+9],rawMod[i+10],rawMod[i+11], rawMod[i+12],rawMod[i+13],rawMod[i+14],rawMod[i+15],
+				rawMod[i+16],rawMod[i+17],rawMod[i+18],rawMod[i+19]
+			);
+		}
+	}
+
+	switch (m) {
+		// DATA           |  1 |  0 |  1 |  1 |  0 |
+		// Manchester     |--__|__--|--__|--__|__--|
+		// Anti Collision |-_-_|--__|-_-_|-_-_|--__|
+        //                |<-->|
+        //                | T  |
+		case AC2K:
+			if (DEBUG >= 2) { Dbprintf("decoding frame with modulation AC2K"); }
+			hitag_decode_frame_AC(2, sofBits, rx, rxlen, response, rawMod, rawLen);
+			break;
+		case AC4K:
+			if (DEBUG >= 2) { Dbprintf("decoding frame with modulation AC4K"); }
+			hitag_decode_frame_AC(4, sofBits, rx, rxlen, response, rawMod, rawLen);
+			break;
+		case MC4K:
+			if (DEBUG >= 2) { Dbprintf("decoding frame with modulation MC4K"); }
+			hitag_decode_frame_MC(4, sofBits, rx, rxlen, response, rawMod, rawLen);
+			break;
+		case MC8K:
+			if (DEBUG >= 2) { Dbprintf("decoding frame with modulation MC8K"); }
+			hitag_decode_frame_MC(8, sofBits, rx, rxlen, response, rawMod, rawLen);
+			break;
+	}
+
+	LED_B_OFF();
+	if (DEBUG >= 2) {
+		int rb[200] = {0}; int z = 0;
+		for (i = 0; i < 16; i++) { for (int j = 0; j < 8; j++) {
+			rb[z] = 0;
+			if ((rx[i] & ((1 << 7) >> j)) != 0) { rb[z] = 1; }
+			z++;
+		} }
+		for (i=0; i < z; i+=8) {
+			Dbprintf("raw bit: - %d%d%d%d%d%d%d%d", rb[i],rb[i+1],rb[i+2],rb[i+3],rb[i+4],rb[i+5],rb[i+6],rb[i+7] );
+		}
+	}
+}
+
+static void hitag_start_auth(byte_t* tx, size_t* txlen) {
+	*txlen = 5;
+	switch (tag.mode) {
+		case STANDARD:
+			//00110 - 0x30 - STANDARD MODE
+			memcpy(tx, "\x30", nbytes(*txlen));
+			break;
+		case ADVANCED:
+			//11000 - 0xc0 - Advance Mode
+			memcpy(tx, "\xc0", nbytes(*txlen));
+			break;
+		case FAST_ADVANCED:
+			//TODO!
+			break;
+		default: //STANDARD MODE
+			memcpy(tx, "\x30", nbytes(*txlen));
+			break;
+	}
+	tag.pstate = READY;
+	tag.tstate = NO_OP;
+}
+
+static int hitag_read_page(hitag_function htf, uint64_t key, byte_t* rx, size_t* rxlen, byte_t* tx, size_t* txlen, int pageNum) {
+	int i, j, z;
+	int response_bit[200];
+	unsigned char mask = 1;
+	unsigned char crc;
+	unsigned char pageData[32];
+
+	if (pageNum >= tag.max_page) {
+		return -1;
+	}
+	if (tag.pstate == SELECTED && tag.tstate == NO_OP && *rxlen > 0) {
+		//send read request
+		tag.tstate = READING_PAGE;
+		*txlen = 20;
+		crc = CRC_PRESET;
+		tx[0] = 0xc0 + (pageNum / 16);
+		calc_crc(&crc, tx[0], 8);
+		calc_crc(&crc, 0x00 + ((pageNum % 16) * 16), 4);
+		tx[1] = 0x00 + ((pageNum % 16) * 16) + (crc / 16);
+		tx[2] = 0x00 + (crc % 16) * 16;	 
+	} else if (tag.pstate == SELECTED && tag.tstate == READING_PAGE && *rxlen > 0) {
+		//save received data
+		z = 0;
+		for (i = 0; i < 4; i++) {
+			for (j = 0; j < 8; j++) {
+				response_bit[z] = 0;
+				if ((rx[i] & ((mask << 7) >> j)) != 0) {
+					response_bit[z] = 1;
+				}
+				if (z < 32) {
+					pageData[z] = response_bit[z];
+				}
+
+				z++;
+			}
+		}
+		for (i = 0; i < 4; i++) {
+			tag.pages[pageNum][i] = 0x0;
+		}
+		for (i = 0; i < 4; i++) {
+			tag.pages[pageNum][i] += ((pageData[i * 8] << 7) | (pageData[1 + (i * 8)] << 6) | 
+					(pageData[2 + (i * 8)] << 5) | (pageData[3 + (i * 8)] << 4) | 
+					(pageData[4 + (i * 8)] << 3) | (pageData[5 + (i * 8)] << 2) | 
+					(pageData[6 + (i * 8)]
+					 << 1) | pageData[7 + (i * 8)]);
+		}
+		if (tag.auth && tag.LKP && pageNum == 1) {
+			Dbprintf("Page[%2d]: %02X %02X %02X %02X", pageNum, pwdh0,
+					tag.pages[pageNum][2], tag.pages[pageNum][1], tag.pages[pageNum][0]);
+		} else {
+			Dbprintf("Page[%2d]: %02X %02X %02X %02X", pageNum,
+					tag.pages[pageNum][3], tag.pages[pageNum][2], 
+					tag.pages[pageNum][1], tag.pages[pageNum][0]);
+		}
+
+		
+		//display key and password if possible
+		if (pageNum == 1 && tag.auth == 1 && tag.LKP) {
+			if (htf == 02) { //RHTS_KEY
+				Dbprintf("Page[ 2]: %02X %02X %02X %02X",
+						(byte_t)(key >> 8) & 0xff,
+						(byte_t) key & 0xff, pwdl1, pwdl0);
+				Dbprintf("Page[ 3]: %02X %02X %02X %02X",
+						(byte_t)(key >> 40) & 0xff,
+						(byte_t)(key >> 32) & 0xff,
+						(byte_t)(key >> 24) & 0xff,
+						(byte_t)(key >> 16) & 0xff);
+			} else {
+				//if the authentication is done with a challenge the key and password are unknown
+				Dbprintf("Page[ 2]: __ __ __ __");
+				Dbprintf("Page[ 3]: __ __ __ __");
+			}
+		}
+
+		*txlen = 20;
+		crc = CRC_PRESET;
+		tx[0] = 0xc0 + ((pageNum+1) / 16);
+		calc_crc(&crc, tx[0], 8);
+		calc_crc(&crc, 0x00 + (((pageNum+1) % 16) * 16), 4);
+		tx[1] = 0x00 + (((pageNum+1) % 16) * 16) + (crc / 16);
+		tx[2] = 0x00 + (crc % 16) * 16;
+
+		return 1;
+	}
+	return 0;
+}
+
+static int hitag_read_block(hitag_function htf, uint64_t key, byte_t* rx, size_t* rxlen, byte_t* tx, size_t* txlen, int blockNum) {
+	int i, j, z;
+	int response_bit[200];
+	unsigned char mask = 1;
+	unsigned char crc;
+	unsigned char blockData[128];
+
+	if (blockNum+4 >= tag.max_page) { //block always = 4 pages
+		return -1;
+	}
+
+	if (tag.pstate == SELECTED && tag.tstate == NO_OP && *rxlen > 0) {
+		//send read request
+		tag.tstate = READING_BLOCK;
+		*txlen = 20;
+		crc = CRC_PRESET;
+		tx[0] = 0xd0 + (blockNum / 16);
+		calc_crc(&crc, tx[0], 8);
+		calc_crc(&crc, 0x00 + ((blockNum % 16) * 16), 4);
+		tx[1] = 0x00 + ((blockNum % 16) * 16) + (crc / 16);
+		tx[2] = 0x00 + (crc % 16) * 16;	 
+	} else if (tag.pstate == SELECTED && tag.tstate == READING_BLOCK && *rxlen > 0) {
+		//save received data
+		z = 0;
+		for (i = 0; i < 16; i++) {
+			for (j = 0; j < 8; j++) {
+				response_bit[z] = 0;
+				if ((rx[i] & ((mask << 7) >> j)) != 0) {
+					response_bit[z] = 1;
+				}
+				if (z < 128) {
+					blockData[z] = response_bit[z];
+				}
+				z++;
+			}
+		}
+
+		for (z = 0; z < 4; z++) { //4 pages
+			for (i = 0; i < 4; i++) {
+				tag.pages[blockNum+z][i] = 0x0;
+			}
+		}
+		for (z = 0; z < 4; z++) { //4 pages
+			for (i = 0; i < 4; i++) {
+				j = (i * 8) + (z*32); //bit in page + pageStart
+				tag.pages[blockNum+z][i] = ((blockData[j] << 7) | (blockData[1 + j] << 6) | 
+					(blockData[2 + j] << 5) | (blockData[3 + j] << 4) | 
+					(blockData[4 + j] << 3) | (blockData[5 + j] << 2) | 
+					(blockData[6 + j] << 1) | blockData[7 + j]);
+			}
+		}
+		if (DEBUG) {
+			for (z = 0; z < 4; z++) { 
+				Dbprintf("Page[%2d]: %02X %02X %02X %02X", blockNum+z,
+						tag.pages[blockNum+z][3], tag.pages[blockNum+z][2], 
+						tag.pages[blockNum+z][1], tag.pages[blockNum+z][0]);
+			}
+		}
+		Dbprintf("Block[%2d]: %02X %02X %02X %02X - %02X %02X %02X %02X - %02X %02X %02X %02X - %02X %02X %02X %02X", blockNum,
+					tag.pages[blockNum][3],   tag.pages[blockNum][2],    tag.pages[blockNum][1],   tag.pages[blockNum][0],
+					tag.pages[blockNum+1][3], tag.pages[blockNum+1][2],  tag.pages[blockNum+1][1], tag.pages[blockNum+1][0],
+					tag.pages[blockNum+2][3], tag.pages[blockNum+2][2],  tag.pages[blockNum+2][1], tag.pages[blockNum+2][0],
+					tag.pages[blockNum+3][3], tag.pages[blockNum+3][2],  tag.pages[blockNum+3][1], tag.pages[blockNum+3][0]);
+
+		*txlen = 20;
+		crc = CRC_PRESET;
+		tx[0] = 0xd0 + ((blockNum+4) / 16);
+		calc_crc(&crc, tx[0], 8);
+		calc_crc(&crc, 0x00 + (((blockNum+4) % 16) * 16), 4);
+		tx[1] = 0x00 + (((blockNum+4) % 16) * 16) + (crc / 16);
+		tx[2] = 0x00 + (crc % 16) * 16;
+
+		return 1;
+	}
+	return 0;
+}
+
+
 /*
  * to check if the right uid was selected
  */
@@ -380,44 +859,83 @@ static void hitagS_handle_reader_command(byte_t* rx, const size_t rxlen,
 	u64 state;
 	unsigned char crc;
 
-// Copy the (original) received frame how it is send over the air
+	// Copy the (original) received frame how it is send over the air
 	memcpy(rx_air, rx, nbytes(rxlen));
-// Reset the transmission frame length
+	// Reset the transmission frame length
 	*txlen = 0;
-// Try to find out which command was send by selecting on length (in bits)
+	// Try to find out which command was send by selecting on length (in bits)
 	switch (rxlen) {
-	case 5: {
-		//UID request with a selected response protocol mode
-		tag.pstate = READY;
-		tag.tstate = NO_OP;
-		if ((rx[0] & 0xf0) == 0x30) {
-			tag.mode = STANDARD;
-			sof_bits = 1;
-			m = AC2K;
-		}
-		if ((rx[0] & 0xf0) == 0xc0) {
-			tag.mode = ADVANCED;
-			sof_bits = 3;
-			m = AC2K;
+		case 5: {
+			//UID request with a selected response protocol mode
+			tag.pstate = READY;
+			tag.tstate = NO_OP;
+			if ((rx[0] & 0xf0) == 0x30) {
+				Dbprintf("recieved uid request in Standard Mode");
+				tag.mode = STANDARD;
+				sof_bits = 1;
+				m = AC2K;
+			}
+			if ((rx[0] & 0xf0) == 0xc0) {
+				Dbprintf("recieved uid request in ADVANCE Mode");
+				tag.mode = ADVANCED;
+				sof_bits = 3;
+				m = AC2K;
+			}
+			if ((rx[0] & 0xf0) == 0xd0) {
+				Dbprintf("recieved uid request in FAST_ADVANCE Mode");
+				tag.mode = FAST_ADVANCED;
+				sof_bits = 3;
+				m = AC4K;
+			}
+			//send uid as a response
+			*txlen = 32;
+			for (i = 0; i < 4; i++) {
+				tx[i] = (tag.uid >> (24 - (i * 8))) & 0xff;
+			}
 		}
+		break;
+		case 45: {
+			//select command from reader received
+			if (check_select(rx, tag.uid) == 1) {
+				//if the right tag was selected
+				*txlen = 32;
+				switch (tag.mode) {
+					case STANDARD:
+						Dbprintf("uid selected in Standard Mode");
+						sof_bits = 1;
+						m = MC4K;
+						break;
+					case ADVANCED:
+						Dbprintf("uid selected in ADVANCE Mode");
+						sof_bits = 6;
+						m = MC4K;
+						break;
+					case FAST_ADVANCED:
+						Dbprintf("uid selected in FAST_ADVANCE Mode");
+						sof_bits = 6;
+						m = MC8K;
+						break;
+					default:
+						break;
+				}
 
-		if ((rx[0] & 0xf0) == 0xd0) {
-			tag.mode = FAST_ADVANCED;
-			sof_bits = 3;
-			m = AC4K;
+				//send configuration
+				tx[0] = tag.pages[1][3];
+				tx[1] = tag.pages[1][2];
+				tx[2] = tag.pages[1][1];
+				tx[3] = 0xff;
+				if (tag.mode != STANDARD) {
+					*txlen = 40;
+					crc = CRC_PRESET;
+					for (i = 0; i < 4; i++)
+						calc_crc(&crc, tx[i], 8);
+					tx[4] = crc;
+				}
+			}
 		}
-		//send uid as a response
-		*txlen = 32;
-		for (i = 0; i < 4; i++)
-			tx[i] = (tag.uid >> (24 - (i * 8))) & 0xff;
-	}
 		break;
-	case 45: {
-		//select command from reader received
-		if (check_select(rx, tag.uid) == 1) {
-			//if the right tag was selected
-			*txlen = 32;
-			switch (tag.mode) {
+	case 64: {
+		switch (tag.mode) {
 			case STANDARD:
 				sof_bits = 1;
 				m = MC4K;
@@ -432,53 +950,23 @@ static void hitagS_handle_reader_command(byte_t* rx, const size_t rxlen,
 				break;
 			default:
 				break;
-			}
-
-			//send configuration
-			for (i = 0; i < 4; i++)
-				tx[i] = (tag.pages[0][1] >> (i * 8)) & 0xff;
-			tx[3] = 0xff;
-			if (tag.mode != STANDARD) {
-				*txlen = 40;
-				crc = CRC_PRESET;
-				for (i = 0; i < 4; i++)
-					calc_crc(&crc, tx[i], 8);
-				tx[4] = crc;
-			}
 		}
-	}
-		break;
-	case 64: {
 		//challenge message received
 		Dbprintf("Challenge for UID: %X", temp_uid);
 		temp2++;
-		*txlen = 32;
 		state = hitag2_init(rev64(tag.key), rev32(tag.pages[0][0]),
 				rev32(((rx[3] << 24) + (rx[2] << 16) + (rx[1] << 8) + rx[0])));
 		Dbprintf(
 				",{0x%02X, 0x%02X, 0x%02X, 0x%02X, 0x%02X, 0x%02X, 0x%02X, 0x%02X}",
 				rx[0], rx[1], rx[2], rx[3], rx[4], rx[5], rx[6], rx[7]);
-		switch (tag.mode) {
-		case STANDARD:
-			sof_bits = 1;
-			m = MC4K;
-			break;
-		case ADVANCED:
-			sof_bits = 6;
-			m = MC4K;
-			break;
-		case FAST_ADVANCED:
-			sof_bits = 6;
-			m = MC8K;
-			break;
-		default:
-			break;
-		}
 
-		for (i = 0; i < 4; i++)
+		for (i = 0; i < 4; i++) {
 			hitag2_byte(&state);
+		}
+
+		*txlen = 32;
 		//send con2,pwdh0,pwdl0,pwdl1 encrypted as a response
-		tx[0] = hitag2_byte(&state) ^ ((tag.pages[0][1] >> 16) & 0xff);
+		tx[0] = hitag2_byte(&state) ^ tag.pages[1][1];
 		tx[1] = hitag2_byte(&state) ^ tag.pwdh0;
 		tx[2] = hitag2_byte(&state) ^ tag.pwdl0;
 		tx[3] = hitag2_byte(&state) ^ tag.pwdl1;
@@ -486,33 +974,22 @@ static void hitagS_handle_reader_command(byte_t* rx, const size_t rxlen,
 			//add crc8
 			*txlen = 40;
 			crc = CRC_PRESET;
-			calc_crc(&crc, ((tag.pages[0][1] >> 16) & 0xff), 8);
+			calc_crc(&crc, tag.pages[1][1], 8);
 			calc_crc(&crc, tag.pwdh0, 8);
 			calc_crc(&crc, tag.pwdl0, 8);
 			calc_crc(&crc, tag.pwdl1, 8);
 			tx[4] = (crc ^ hitag2_byte(&state));
 		}
-		/*
-		 * some readers do not allow to authenticate multiple times in a row with the same tag.
-		 * use this to change the uid between authentications.
-		 */
-
-		/*
-		 if (temp2 % 2 == 0) {
-		 tag.uid = 0x11223344;
-		 tag.pages[0][0] = 0x44332211;
-		 } else {
-		 tag.uid = 0x55667788;
-		 tag.pages[0][0] = 0x88776655;
-		 }
-		 */
 	}
 	case 40:
 		//data received to be written
 		if (tag.tstate == WRITING_PAGE_DATA) {
 			tag.tstate = NO_OP;
-			tag.pages[page_to_be_written / 4][page_to_be_written % 4] = (rx[0]
-					<< 0) + (rx[1] << 8) + (rx[2] << 16) + (rx[3] << 24);
+			tag.pages[page_to_be_written][0] = rx[3];
+			tag.pages[page_to_be_written][1] = rx[2];
+			tag.pages[page_to_be_written][2] = rx[1];
+			tag.pages[page_to_be_written][3] = rx[0];
+
 			//send ack
 			*txlen = 2;
 			tx[0] = 0x40;
@@ -534,8 +1011,11 @@ static void hitagS_handle_reader_command(byte_t* rx, const size_t rxlen,
 				break;
 			}
 		} else if (tag.tstate == WRITING_BLOCK_DATA) {
-			tag.pages[page_to_be_written / 4][page_to_be_written % 4] = (rx[0]
-					<< 24) + (rx[1] << 16) + (rx[2] << 8) + rx[3];
+			tag.pages[page_to_be_written][0] = rx[0];
+			tag.pages[page_to_be_written][1] = rx[1];
+			tag.pages[page_to_be_written][2] = rx[2];
+			tag.pages[page_to_be_written][3] = rx[3];
+
 			//send ack
 			*txlen = 2;
 			tx[0] = 0x40;
@@ -565,15 +1045,16 @@ static void hitagS_handle_reader_command(byte_t* rx, const size_t rxlen,
 		break;
 	case 20: {
 		//write page, write block, read page or read block command received
-		if ((rx[0] & 0xf0) == 0xc0) //read page
-				{
+		if ((rx[0] & 0xf0) == 0xc0) { //read page
 			//send page data
 			page = ((rx[0] & 0x0f) * 16) + ((rx[1] & 0xf0) / 16);
+			Dbprintf("reading page %d", page);
 			*txlen = 32;
-			tx[0] = (tag.pages[page / 4][page % 4]) & 0xff;
-			tx[1] = (tag.pages[page / 4][page % 4] >> 8) & 0xff;
-			tx[2] = (tag.pages[page / 4][page % 4] >> 16) & 0xff;
-			tx[3] = (tag.pages[page / 4][page % 4] >> 24) & 0xff;
+			tx[0] = tag.pages[page][0];
+			tx[1] = tag.pages[page][1];
+			tx[2] = tag.pages[page][2];
+			tx[3] = tag.pages[page][3];
+			
 			if (tag.LKP && page == 1)
 				tx[3] = 0xff;
 
@@ -608,34 +1089,34 @@ static void hitagS_handle_reader_command(byte_t* rx, const size_t rxlen,
 				sof_bits = 0;
 				*txlen = 0;
 			}
-		} else if ((rx[0] & 0xf0) == 0xd0) //read block
-				{
+		} else if ((rx[0] & 0xf0) == 0xd0) { //read block
 			page = ((rx[0] & 0x0f) * 16) + ((rx[1] & 0xf0) / 16);
+			Dbprintf("reading block %d", page);
 			*txlen = 32 * 4;
 			//send page,...,page+3 data
 			for (i = 0; i < 4; i++) {
-				tx[0 + i * 4] = (tag.pages[page / 4][page % 4]) & 0xff;
-				tx[1 + i * 4] = (tag.pages[page / 4][page % 4] >> 8) & 0xff;
-				tx[2 + i * 4] = (tag.pages[page / 4][page % 4] >> 16) & 0xff;
-				tx[3 + i * 4] = (tag.pages[page / 4][page % 4] >> 24) & 0xff;
+				tx[0 + (i * 4)] = tag.pages[page][0];
+				tx[1 + (i * 4)] = tag.pages[page][1];
+				tx[2 + (i * 4)] = tag.pages[page][2];
+				tx[3 + (i * 4)] = tag.pages[page][3];
 				page++;
 			}
 
 			switch (tag.mode) {
-			case STANDARD:
-				sof_bits = 1;
-				m = MC4K;
-				break;
-			case ADVANCED:
-				sof_bits = 6;
-				m = MC4K;
-				break;
-			case FAST_ADVANCED:
-				sof_bits = 6;
-				m = MC8K;
-				break;
-			default:
-				break;
+				case STANDARD:
+					sof_bits = 1;
+					m = MC4K;
+					break;
+				case ADVANCED:
+					sof_bits = 6;
+					m = MC4K;
+					break;
+				case FAST_ADVANCED:
+					sof_bits = 6;
+					m = MC8K;
+					break;
+				default:
+					break;
 			}
 
 			if (tag.mode != STANDARD) {
@@ -651,8 +1132,7 @@ static void hitagS_handle_reader_command(byte_t* rx, const size_t rxlen,
 				sof_bits = 0;
 				*txlen = 0;
 			}
-		} else if ((rx[0] & 0xf0) == 0x80) //write page
-				{
+		} else if ((rx[0] & 0xf0) == 0x80) { //write page
 			page = ((rx[0] & 0x0f) * 16) + ((rx[1] & 0xf0) / 16);
 
 			switch (tag.mode) {
@@ -683,8 +1163,7 @@ static void hitagS_handle_reader_command(byte_t* rx, const size_t rxlen,
 				tag.tstate = WRITING_PAGE_DATA;
 			}
 
-		} else if ((rx[0] & 0xf0) == 0x90) //write block
-				{
+		} else if ((rx[0] & 0xf0) == 0x90) { //write block
 			page = ((rx[0] & 0x0f) * 6) + ((rx[1] & 0xf0) / 16);
 			switch (tag.mode) {
 			case STANDARD:
@@ -722,13 +1201,14 @@ static void hitagS_handle_reader_command(byte_t* rx, const size_t rxlen,
 	}
 }
 
+
 /*
  * to autenticate to a tag with the given key or challenge
  */
-static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr, byte_t* rx, const size_t rxlen, byte_t* tx,
-		size_t* txlen) {
+static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr, byte_t* rx, 
+		const size_t rxlen, byte_t* tx, size_t* txlen) {
 	byte_t rx_air[HITAG_FRAME_LEN];
-	int response_bit[200];
+	int response_bit[200] = {0};
 	int i, j, z, k;
 	unsigned char mask = 1;
 	unsigned char uid[32];
@@ -740,7 +1220,11 @@ static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr
 	memcpy(rx_air, rx, nbytes(rxlen));
 	*txlen = 0;
 
-	if (tag.pstate == READY && rxlen >= 67) {
+	if (DEBUG) {
+        Dbprintf("START hitagS_handle_tag_auth - rxlen: %d, tagstate=%d", rxlen, (int)tag.pstate);
+    }
+
+	if (tag.pstate == READY && rxlen >= 32) {
 		//received uid
 		if(end==true) {
 			Dbprintf("authentication failed!");
@@ -755,13 +1239,10 @@ static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr
 				z++;
 			}
 		}
-		k = 0;
-		for (i = 5; i < z; i += 2) {
-			uid[k] = response_bit[i];
-			k++;
-			if (k > 31)
-				break;
+		for (i = 0; i < 32; i++) {
+			uid[i] = response_bit[i];
 		}
+		
 		uid1 = (uid[0] << 7) | (uid[1] << 6) | (uid[2] << 5) | (uid[3] << 4)
 				| (uid[4] << 3) | (uid[5] << 2) | (uid[6] << 1) | uid[7];
 		uid2 = (uid[8] << 7) | (uid[9] << 6) | (uid[10] << 5) | (uid[11] << 4)
@@ -769,34 +1250,36 @@ static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr
 		uid3 = (uid[16] << 7) | (uid[17] << 6) | (uid[18] << 5) | (uid[19] << 4)
 				| (uid[20] << 3) | (uid[21] << 2) | (uid[22] << 1) | uid[23];
 		uid4 = (uid[24] << 7) | (uid[25] << 6) | (uid[26] << 5) | (uid[27] << 4)
-				| (uid[28] << 3) | (uid[29] << 2) | (uid[30] << 1) | uid[31];
-		if (DEBUG)
-			Dbprintf("UID: %02X %02X %02X %02X", uid1, uid2, uid3, uid4);
+				| (uid[28] << 3) | (uid[29] << 2) | (uid[30] << 1) | uid[31];	
+		Dbprintf("UID: %02X %02X %02X %02X", uid1, uid2, uid3, uid4);
 		tag.uid = (uid4 << 24 | uid3 << 16 | uid2 << 8 | uid1);
 
 		//select uid
-		*txlen = 45;
 		crc = CRC_PRESET;
 		calc_crc(&crc, 0x00, 5);
 		calc_crc(&crc, uid1, 8);
 		calc_crc(&crc, uid2, 8);
 		calc_crc(&crc, uid3, 8);
 		calc_crc(&crc, uid4, 8);
+		Dbprintf("crc: %02X", crc);
+
+		//resetting response bit
 		for (i = 0; i < 100; i++) {
 			response_bit[i] = 0;
 		}
-		for (i = 0; i < 5; i++) {
-			response_bit[i] = 0;
-		}
+
+		//skip the first 5 
 		for (i = 5; i < 37; i++) {
 			response_bit[i] = uid[i - 5];
 		}
+		//add crc value
 		for (j = 0; j < 8; j++) {
 			response_bit[i] = 0;
 			if ((crc & ((mask << 7) >> j)) != 0)
 				response_bit[i] = 1;
 			i++;
 		}
+
 		k = 0;
 		for (i = 0; i < 6; i++) {
 			tx[i] = (response_bit[k] << 7) | (response_bit[k + 1] << 6)
@@ -805,63 +1288,78 @@ static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr
 					| (response_bit[k + 6] << 1) | response_bit[k + 7];
 			k += 8;
 		}
+		*txlen = 45;
 		tag.pstate = INIT;
-	} else if (tag.pstate == INIT && rxlen == 44) {
+	} else if (tag.pstate == INIT && rxlen > 24) {
 		// received configuration after select command
 		z = 0;
-		for (i = 0; i < 6; i++) {
+		for (i = 0; i < 4; i++) {
 			for (j = 0; j < 8; j++) {
 				response_bit[z] = 0;
-				if ((rx[i] & ((mask << 7) >> j)) != 0)
+				if ((rx[i] & ((mask << 7) >> j)) != 0) {
 					response_bit[z] = 1;
+				}
 				z++;
 			}
 		}
-		conf_pages[0] = ((response_bit[4] << 7) | (response_bit[5] << 6)
-				| (response_bit[6] << 5) | (response_bit[7] << 4)
-				| (response_bit[8] << 3) | (response_bit[9] << 2)
-				| (response_bit[10] << 1) | response_bit[11]);
+
 		//check wich memorysize this tag has
-		if (response_bit[10] == 0 && response_bit[11] == 0)
+		//CON0
+		if (response_bit[6] == 0 && response_bit[7] == 0)
 			tag.max_page = 32 / 32;
-		if (response_bit[10] == 0 && response_bit[11] == 1)
+		if (response_bit[6] == 0 && response_bit[7] == 1)
 			tag.max_page = 256 / 32;
-		if (response_bit[10] == 1 && response_bit[11] == 0)
+		if (response_bit[6] == 1 && response_bit[7] == 0)
+			tag.max_page = 2048 / 32;
+		if (response_bit[6] == 1 && response_bit[7] == 1) //reserved but some tags got this setting
 			tag.max_page = 2048 / 32;
-		conf_pages[1] = ((response_bit[12] << 7) | (response_bit[13] << 6)
-				| (response_bit[14] << 5) | (response_bit[15] << 4)
-				| (response_bit[16] << 3) | (response_bit[17] << 2)
-				| (response_bit[18] << 1) | response_bit[19]);
-		tag.auth = response_bit[12];
-		tag.TTFC = response_bit[13];
-		//tag.TTFDR in response_bit[14] and response_bit[15]
-		//tag.TTFM in response_bit[16] and response_bit[17]
-		tag.LCON = response_bit[18];
-		tag.LKP = response_bit[19];
-		conf_pages[2] = ((response_bit[20] << 7) | (response_bit[21] << 6)
-				| (response_bit[22] << 5) | (response_bit[23] << 4)
-				| (response_bit[24] << 3) | (response_bit[25] << 2)
-				| (response_bit[26] << 1) | response_bit[27]);
-		tag.LCK7 = response_bit[20];
-		tag.LCK6 = response_bit[21];
-		tag.LCK5 = response_bit[22];
-		tag.LCK4 = response_bit[23];
-		tag.LCK3 = response_bit[24];
-		tag.LCK2 = response_bit[25];
-		tag.LCK1 = response_bit[26];
-		tag.LCK0 = response_bit[27];
-
-		if (DEBUG)
-			Dbprintf("conf0: %02X conf1: %02X conf2: %02X", conf_pages[0],
-					conf_pages[1], conf_pages[2]);
+
+		//CON1
+		tag.auth = response_bit[8];
+		tag.TTFC = response_bit[9];
+		//tag.TTFDR in response_bit[10] and response_bit[11]
+		//tag.TTFM in response_bit[12] and response_bit[13]
+		tag.LCON = response_bit[14];
+		tag.LKP = response_bit[15];
+		
+		//CON2
+		tag.LCK7 = response_bit[16];
+		tag.LCK6 = response_bit[17];
+		tag.LCK5 = response_bit[18];
+		tag.LCK4 = response_bit[19];
+		tag.LCK3 = response_bit[20];
+		tag.LCK2 = response_bit[21];
+		tag.LCK1 = response_bit[22];
+		tag.LCK0 = response_bit[23];
+
+		if (DEBUG) {
+			conf_pages[0] = ((response_bit[0] << 7) | (response_bit[1] << 6)
+				| (response_bit[2] << 5) | (response_bit[3] << 4)
+				| (response_bit[4] << 3) | (response_bit[5] << 2)
+				| (response_bit[6] << 1) | response_bit[7]);
+			conf_pages[1] = ((response_bit[8] << 7) | (response_bit[9] << 6)
+				| (response_bit[10] << 5) | (response_bit[11] << 4)
+				| (response_bit[12] << 3) | (response_bit[13] << 2)
+				| (response_bit[14] << 1) | response_bit[15]);
+			conf_pages[2] = ((response_bit[16] << 7) | (response_bit[17] << 6)
+				| (response_bit[18] << 5) | (response_bit[19] << 4)
+				| (response_bit[20] << 3) | (response_bit[21] << 2)
+				| (response_bit[22] << 1) | response_bit[23]);
+			Dbprintf("conf0: %02X conf1: %02X conf2: %02X", conf_pages[0], conf_pages[1], conf_pages[2]);
+			Dbprintf("tag.max_page: %d, tag.auth: %d", tag.max_page, tag.auth);
+		}
+
 		if (tag.auth == 1) {
 			//if the tag is in authentication mode try the key or challenge
 			*txlen = 64;
 			if(end!=true){
 				if(htf==02||htf==04){ //RHTS_KEY //WHTS_KEY
-					state = hitag2_init(rev64(key), rev32(tag.uid),
-							rev32(rnd));
-
+					state = hitag2_init(rev64(key), rev32(tag.uid), rev32(rnd));
+					/*
+					Dbprintf("key: %02X %02X\n\n", key, rev64(key)); 
+					Dbprintf("tag.uid: %02X %02X\n\n", tag.uid, rev32(tag.uid)); 
+					Dbprintf("rnd: %02X %02X\n\n", rnd, rev32(rnd)); 
+					*/
 					for (i = 0; i < 4; i++) {
 						auth_ks[i] = hitag2_byte(&state) ^ 0xff;
 					}
@@ -892,14 +1390,8 @@ static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr
 			tag.pstate = SELECTED;
 		}
 		
-	} else if (tag.pstate == AUTHENTICATE && rxlen == 44) {
+	} else if (tag.pstate == AUTHENTICATE && rxlen >= 32) {
 		//encrypted con2,password received.
-		crc = CRC_PRESET;
-		calc_crc(&crc, 0x80, 1);
-		calc_crc(&crc, ((rx[0] & 0x0f) * 16 + ((rx[1] & 0xf0) / 16)), 8);
-		calc_crc(&crc, ((rx[1] & 0x0f) * 16 + ((rx[2] & 0xf0) / 16)), 8);
-		calc_crc(&crc, ((rx[2] & 0x0f) * 16 + ((rx[3] & 0xf0) / 16)), 8);
-		calc_crc(&crc, ((rx[3] & 0x0f) * 16 + ((rx[4] & 0xf0) / 16)), 8);
 		if (DEBUG) {
 			Dbprintf("UID:::%X", tag.uid);
 			Dbprintf("RND:::%X", rnd);
@@ -909,30 +1401,26 @@ static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr
 		pwdh0=0;
 		pwdl0=0;
 		pwdl1=0;
-		if(htf==02 || htf==04){ //RHTS_KEY //WHTS_KEY
-		{
+		if(htf==02 || htf==04) { //RHTS_KEY //WHTS_KEY
 			state = hitag2_init(rev64(key), rev32(tag.uid), rev32(rnd));
-			for (i = 0; i < 5; i++)
+			for (i = 0; i < 5; i++)  {
 				hitag2_byte(&state);
-			pwdh0 = ((rx[1] & 0x0f) * 16 + ((rx[2] & 0xf0) / 16))
-					^ hitag2_byte(&state);
-			pwdl0 = ((rx[2] & 0x0f) * 16 + ((rx[3] & 0xf0) / 16))
-					^ hitag2_byte(&state);
-			pwdl1 = ((rx[3] & 0x0f) * 16 + ((rx[4] & 0xf0) / 16))
-					^ hitag2_byte(&state);
+			}
+			pwdh0 = ((rx[1] & 0x0f) * 16 + ((rx[2] & 0xf0) / 16)) ^ hitag2_byte(&state);
+			pwdl0 = ((rx[2] & 0x0f) * 16 + ((rx[3] & 0xf0) / 16)) ^ hitag2_byte(&state);
+			pwdl1 = ((rx[3] & 0x0f) * 16 + ((rx[4] & 0xf0) / 16)) ^ hitag2_byte(&state);
+			if (DEBUG) {
+				Dbprintf("pwdh0 %02X pwdl0 %02X pwdl1 %02X", pwdh0, pwdl0, pwdl1);
+			}
 		}
-
-		if (DEBUG)
-			Dbprintf("pwdh0 %02X pwdl0 %02X pwdl1 %02X", pwdh0, pwdl0, pwdl1);
-		
-
-		//Dbprintf("%X %02X", rnd, ((rx[4] & 0x0f) * 16) + ((rx[5] & 0xf0) / 16));
-		//rnd += 1;
-	}
 		tag.pstate = SELECTED; //tag is now ready for read/write commands
 	}
+
+	if (DEBUG) {
+        Dbprintf("END hitagS_handle_tag_auth - tagstate=%d", (int)tag.pstate);
+    }
+
 	return 0;
-	
 }
 
 /*
@@ -945,15 +1433,15 @@ void SimulateHitagSTag(bool tag_mem_supplied, byte_t* data) {
 	int i, j;
 	byte_t rx[HITAG_FRAME_LEN];
 	size_t rxlen = 0;
-//bool bQuitTraceFull = false;
 	bQuiet = false;
 	byte_t txbuf[HITAG_FRAME_LEN];
 	byte_t* tx = txbuf;
 	size_t txlen = 0;
+	uint8_t con0, con1, con2;
 	BigBuf_free();
 
 // Clean up trace and prepare it for storing frames
-	set_tracing(TRUE);
+	set_tracing(true);
 	clear_trace();
 
 	DbpString("Starting HitagS simulation");
@@ -961,103 +1449,104 @@ void SimulateHitagSTag(bool tag_mem_supplied, byte_t* data) {
 
 	tag.pstate = READY;
 	tag.tstate = NO_OP;
-	for (i = 0; i < 16; i++)
-		for (j = 0; j < 4; j++)
-			tag.pages[i][j] = 0x0;
 	//read tag data into memory
 	if (tag_mem_supplied) {
 		DbpString("Loading hitagS memory...");
-		memcpy((byte_t*)tag.pages,data,4*64);
+		for (i = 0; i < 64; i++) {
+			for (j = 0; j < 4; j++) {
+				tag.pages[i][j] = 0x0;
+			}
+		}
+
+		for (i = 0; i < 64; i++) { 
+			for (j = 0; j < 4; j++) {
+				tag.pages[i][j] = data[(i*4)+j];
+			}
+		}
 	}
-	tag.uid=(uint32_t)tag.pages[0];
+	tag.uid = (tag.pages[0][3] << 24 | tag.pages[0][2] << 16 | tag.pages[0][1] << 8 | tag.pages[0][0]);
+	con0 = tag.pages[1][3];
+	con1 = tag.pages[1][2];
+	con2 = tag.pages[1][1];
+	Dbprintf("UID: %X", tag.uid);
 	Dbprintf("Hitag S simulation started");
-	tag.key=(intptr_t)tag.pages[3];
-	tag.key<<=16;
-	tag.key+=((tag.pages[2][0])<<8)+tag.pages[2][1];
-	tag.pwdl0=tag.pages[2][3];
-	tag.pwdl1=tag.pages[2][2];
-	tag.pwdh0=tag.pages[1][0];
+
+	//0x01 plain mode - Reserved, CON2, CON1, CON0
+	//0x01 auth  mode - PWDH 0,   CON2, CON1, CON0
+	//0x02 auth  mode - KEYH 1, KEYH 0, PWDL 1, PWDL 0
+	//0x03 auth  mode - KEYL 3, KEYL 2, KEYL 1, KEYL 0
+
 	//con0
-	tag.max_page=64;
-	if((tag.pages[1][3]&0x2)==0 && (tag.pages[1][3]&0x1)==1)
-		tag.max_page=8;
-	if((tag.pages[1][3]&0x2)==0 && (tag.pages[1][3]&0x1)==0)
-		tag.max_page=0;
-	//con1
-	tag.auth=0;
-	if((tag.pages[1][2]&0x80)==1)
-		tag.auth=1;
-	tag.LCON=0;
-	if((tag.pages[1][2]&0x2)==1)
-		tag.LCON=1;
-	tag.LKP=0;
-	if((tag.pages[1][2]&0x1)==1)
-		tag.LKP=1;
-	//con2
-	//0=read write 1=read only
-	tag.LCK7=0;
-	if((tag.pages[1][1]&0x80)==1)
-		tag.LCK7=1;
-	tag.LCK6=0;
-	if((tag.pages[1][1]&0x40)==1)
-		tag.LCK6=1;
-	tag.LCK5=0;
-	if((tag.pages[1][1]&0x20)==1)
-		tag.LCK5=1;
-	tag.LCK4=0;
-	if((tag.pages[1][1]&0x10)==1)
-		tag.LCK4=1;
-	tag.LCK3=0;
-	if((tag.pages[1][1]&0x8)==1)
-		tag.LCK3=1;
-	tag.LCK2=0;
-	if((tag.pages[1][1]&0x4)==1)
-		tag.LCK2=1;
-	tag.LCK1=0;
-	if((tag.pages[1][1]&0x2)==1)
-		tag.LCK1=1;
-	tag.LCK0=0;
-	if((tag.pages[1][1]&0x1)==1)
-		tag.LCK0=1;
-
-// Set up simulator mode, frequency divisor which will drive the FPGA
-// and analog mux selection.
+	tag.max_page = 2048 / 32;
+	if ((con0 & 0x2) == 0 && (con0 & 0x1) == 1)
+		tag.max_page = 256 / 32;
+	if ((con0 & 0x2) == 0 && (con0 & 0x1) == 0)
+		tag.max_page = 32 / 32;
+
+	//CON1
+	tag.auth = ((con1 & 0x80) == 0x80) ? 1 : 0;
+	tag.TTFC = ((con1 & 0x40) == 0x40) ? 1 : 0;
+	//tag.TTFDR in response_bit[10] and response_bit[11]
+	//tag.TTFM in response_bit[12] and response_bit[13]
+	tag.LCON = ((con1 & 0x2) == 0x2) ? 1 : 0;
+	tag.LKP  = ((con1 & 0x1) == 0x1) ? 1 : 0;
+		
+	//CON2
+	tag.LCK7 = ((con2 & 0x80) == 0x80) ? 1 : 0;
+	tag.LCK6 = ((con2 & 0x40) == 0x40) ? 1 : 0;
+	tag.LCK5 = ((con2 & 0x20) == 0x20) ? 1 : 0;
+	tag.LCK4 = ((con2 & 0x10) == 0x10) ? 1 : 0;
+	tag.LCK3 = ((con2 & 0x8) == 0x8) ? 1 : 0;
+	tag.LCK2 = ((con2 & 0x4) == 0x4) ? 1 : 0;
+	tag.LCK1 = ((con2 & 0x2) == 0x2) ? 1 : 0;
+	tag.LCK0 = ((con2 & 0x1) == 0x1) ? 1 : 0;
+
+	if (tag.auth == 1) {
+		//TODO check if this working :D
+		tag.key=(intptr_t)tag.pages[3];
+		tag.key<<=16;
+		tag.key+=((tag.pages[2][0])<<8)+tag.pages[2][1];
+		tag.pwdl0=tag.pages[2][3];
+		tag.pwdl1=tag.pages[2][2];
+		tag.pwdh0=tag.pages[1][0];
+	}
+
+	// Set up simulator mode, frequency divisor which will drive the FPGA
+	// and analog mux selection.
 	FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
-	FpgaWriteConfWord(
-	FPGA_MAJOR_MODE_LF_EDGE_DETECT | FPGA_LF_EDGE_DETECT_READER_FIELD);
+	FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_EDGE_DETECT);
 	FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
 	SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
 	RELAY_OFF();
 
-// Configure output pin that is connected to the FPGA (for modulating)
+	// Configure output 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;
 
-// Disable modulation at default, which means release resistance
+	// Disable modulation at default, which means release resistance
 	LOW(GPIO_SSC_DOUT);
 
-// Enable Peripheral Clock for TIMER_CLOCK0, used to measure exact timing before answering
+	// 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 reader frames
+	// Enable Peripheral Clock for TIMER_CLOCK1, used to capture edges of the reader frames
 	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, 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;
+	// 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;
 
-// Reset the received frame, frame count and timing info
+	// 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
+	// Enable and reset counter
 	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
 
 	while (!BUTTON_PRESS()) {
@@ -1065,6 +1554,7 @@ void SimulateHitagSTag(bool tag_mem_supplied, byte_t* data) {
 		WDT_HIT();
 
 		// Receive frame, watch for at most T0*EOF periods
+		//HITAG_T_WAIT_MAX
 		while (AT91C_BASE_TC1->TC_CV < T0 * HITAG_T_EOF) {
 			// Check if rising edge in modulation is detected
 			if (AT91C_BASE_TC1->TC_SR & AT91C_TC_LDRAS) {
@@ -1119,13 +1609,13 @@ void SimulateHitagSTag(bool tag_mem_supplied, byte_t* data) {
 			// with respect to the falling edge, we need to wait actually (T_Wait1 - T_Low)
 			// periods. The gap time T_Low varies (4..10). All timer values are in
 			// terms of T0 units
-			while (AT91C_BASE_TC0->TC_CV < T0 * (HITAG_T_WAIT_1 - HITAG_T_LOW))
-				;
+			while (AT91C_BASE_TC0->TC_CV < T0 * (HITAG_T_WAIT_1 - HITAG_T_LOW)) { }
 
 			// Send and store the tag answer (if there is any)
 			if (txlen > 0) {
 				// Transmit the tag frame
-				hitag_send_frame(tx, txlen);
+				hitag_tag_send_frame(tx, txlen);
+
 				// Store the frame in the trace
 				if (!bQuiet) {
 					if (!LogTraceHitag(tx, txlen, 0, 0, false)) {
@@ -1135,12 +1625,13 @@ void SimulateHitagSTag(bool tag_mem_supplied, byte_t* data) {
 				}
 			}
 
+			// Enable and reset external trigger in timer for capturing future frames
+			AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
+
 			// Reset the received frame and response timing info
 			memset(rx, 0x00, sizeof(rx));
 			response = 0;
 
-			// Enable and reset external trigger in timer for capturing future frames
-			AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
 			LED_B_OFF();
 		}
 		// Reset the frame length
@@ -1150,6 +1641,7 @@ void SimulateHitagSTag(bool tag_mem_supplied, byte_t* data) {
 		// Reset the timer to restart while-loop that receives frames
 		AT91C_BASE_TC1->TC_CCR = AT91C_TC_SWTRG;
 	}
+	Dbprintf("Hitag S simulation stopped");
 	LED_B_OFF();
 	LED_D_OFF();
 	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
@@ -1162,10 +1654,16 @@ void SimulateHitagSTag(bool tag_mem_supplied, byte_t* data) {
  * If the key was given the password will be decrypted.
  * Reads every page of a hitag S transpoder.
  */
-void ReadHitagS(hitag_function htf, hitag_data* htd) {
-	int i, j, z, k;
+void ReadHitagSintern(hitag_function htf, hitag_data* htd, stype tagMode, int startPage, bool readBlock) {
 	int frame_count;
-	int response_bit[200];
+	int i;
+	int sendNum = startPage;
+	tag.mode = tagMode;
+
+	//int i, j, z;
+	//int response_bit[200];
+	//unsigned char mask = 1;
+
 	int response;
 	byte_t rx[HITAG_FRAME_LEN];
 	size_t rxlen = 0;
@@ -1173,16 +1671,10 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) {
 	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;
-	int sendNum = 0;
-	unsigned char mask = 1;
-	unsigned char crc;
-	unsigned char pageData[32];
+
 	page_to_be_written = 0;
 	
 	//read given key/challenge
@@ -1191,14 +1683,16 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) {
 	uint64_t NrAr=0;
 	byte_t key_[6];
 	switch(htf) {
-		case 01: { //RHTS_CHALLENGE
+		case 01:
+		case 03: { //RHTS_CHALLENGE
 			DbpString("Authenticating using nr,ar pair:");
 			memcpy(NrAr_,htd->auth.NrAr,8);
 			Dbhexdump(8,NrAr_,false);
 			NrAr=NrAr_[7] | ((uint64_t)NrAr_[6]) << 8 | ((uint64_t)NrAr_[5]) << 16 | ((uint64_t)NrAr_[4]) << 24 | ((uint64_t)NrAr_[3]) << 32 |
 				        ((uint64_t)NrAr_[2]) << 40| ((uint64_t)NrAr_[1]) << 48 | ((uint64_t)NrAr_[0]) << 56;
 		} break;
-		case 02: { //RHTS_KEY
+		case 02:
+		case 04: { //RHTS_KEY
 			DbpString("Authenticating using key:");
 			memcpy(key_,htd->crypto.key,6);	 
 			Dbhexdump(6,key_,false);
@@ -1217,11 +1711,10 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) {
 	bSuccessful = false;
 
 // Clean up trace and prepare it for storing frames
-	set_tracing(TRUE);
+	set_tracing(true);
 	clear_trace();
 
 	bQuiet = false;
-	bQuitTraceFull = true;
 
 	LED_D_ON();
 
@@ -1230,8 +1723,7 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) {
 	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);
+	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
@@ -1256,9 +1748,7 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) {
 
 // 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;
+	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;
@@ -1269,14 +1759,34 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) {
 	response = 0;
 	lastbit = 1;
 	bStop = false;
-
-	reset_sof = 1;
 	t_wait = 200;
 
 	while (!bStop && !BUTTON_PRESS()) {
 		// Watchdog hit
 		WDT_HIT();
 
+
+		// Add transmitted frame to total count
+		if (txlen > 0) {
+			frame_count++;
+
+			if (tag.pstate == READY && rxlen < 1) {
+				//skip logging starting auths if no response
+			} else {
+				if (!bQuiet) {
+					// Store the frame in the trace
+					if (!LogTraceHitag(tx, txlen, HITAG_T_WAIT_2, 0, true)) {
+						if (bQuitTraceFull) {
+							DbpString("Trace full");
+							break;
+						} else {
+							bQuiet = true;
+						}
+					}
+				}
+			}
+		}
+
 		// Check if frame was captured and store it
 		if (rxlen > 0) {
 			frame_count++;
@@ -1296,94 +1806,31 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) {
 		tx = txbuf;
 		txlen = 0;
 
+		if (DEBUG >= 2) {
+        	Dbprintf("FRO %d rxlen: %d, pstate=%d, tstate=%d", frame_count, rxlen, (int)tag.pstate,  (int)tag.tstate);
+    	}
+
 		if (rxlen == 0) {
 			//start authentication
-			txlen = 5;
-			memcpy(tx, "\xc0", nbytes(txlen));
-			tag.pstate = READY;
-			tag.tstate = NO_OP;
+			hitag_start_auth(tx, &txlen);
 		} else if (tag.pstate != SELECTED) {
-			if (hitagS_handle_tag_auth(htf, key,NrAr,rx, rxlen, tx, &txlen) == -1)
+			if (hitagS_handle_tag_auth(htf, key,NrAr,rx, rxlen, tx, &txlen) == -1) {
+				Dbprintf("hitagS_handle_tag_auth - bStop = !false");
 				bStop = !false;
-		}
-		if (tag.pstate == SELECTED && tag.tstate == NO_OP && rxlen > 0) {
-			//send read request
-			tag.tstate = READING_PAGE;
-			txlen = 20;
-			crc = CRC_PRESET;
-			tx[0] = 0xc0 + (sendNum / 16);
-			calc_crc(&crc, tx[0], 8);
-			calc_crc(&crc, 0x00 + ((sendNum % 16) * 16), 4);
-			tx[1] = 0x00 + ((sendNum % 16) * 16) + (crc / 16);
-			tx[2] = 0x00 + (crc % 16) * 16;
-		} else if (tag.pstate == SELECTED && tag.tstate == READING_PAGE
-				&& rxlen > 0) {
-			//save received data
-			z = 0;
-			for (i = 0; i < 5; i++) {
-				for (j = 0; j < 8; j++) {
-					response_bit[z] = 0;
-					if ((rx[i] & ((mask << 7) >> j)) != 0)
-						response_bit[z] = 1;
-					z++;
-				}
-			}
-			k = 0;
-			for (i = 4; i < 36; i++) {
-				pageData[k] = response_bit[i];
-				k++;
-			}
-			for (i = 0; i < 4; i++)
-				tag.pages[sendNum / 4][sendNum % 4] = 0x0;
-			for (i = 0; i < 4; i++) {
-				tag.pages[sendNum / 4][sendNum % 4] += ((pageData[i * 8] << 7)
-						| (pageData[1 + (i * 8)] << 6)
-						| (pageData[2 + (i * 8)] << 5)
-						| (pageData[3 + (i * 8)] << 4)
-						| (pageData[4 + (i * 8)] << 3)
-						| (pageData[5 + (i * 8)] << 2)
-						| (pageData[6 + (i * 8)] << 1) | pageData[7 + (i * 8)])
-						<< (i * 8);
-			}
-			if (tag.auth && tag.LKP && sendNum == 1) {
-				Dbprintf("Page[%2d]: %02X %02X %02X %02X", sendNum, pwdh0,
-						(tag.pages[sendNum / 4][sendNum % 4] >> 16) & 0xff,
-						(tag.pages[sendNum / 4][sendNum % 4] >> 8) & 0xff,
-						tag.pages[sendNum / 4][sendNum % 4] & 0xff);
-			} else {
-				Dbprintf("Page[%2d]: %02X %02X %02X %02X", sendNum,
-						(tag.pages[sendNum / 4][sendNum % 4] >> 24) & 0xff,
-						(tag.pages[sendNum / 4][sendNum % 4] >> 16) & 0xff,
-						(tag.pages[sendNum / 4][sendNum % 4] >> 8) & 0xff,
-						tag.pages[sendNum / 4][sendNum % 4] & 0xff);
-			}
-
-			sendNum++;
-			//display key and password if possible
-			if (sendNum == 2 && tag.auth == 1 && tag.LKP) {
-				if (htf == 02) { //RHTS_KEY
-					Dbprintf("Page[ 2]: %02X %02X %02X %02X",
-							(byte_t)(key >> 8) & 0xff,
-							(byte_t) key & 0xff, pwdl1, pwdl0);
-					Dbprintf("Page[ 3]: %02X %02X %02X %02X",
-							(byte_t)(key >> 40) & 0xff,
-							(byte_t)(key >> 32) & 0xff,
-							(byte_t)(key >> 24) & 0xff,
-							(byte_t)(key >> 16) & 0xff);
-				} else {
-					//if the authentication is done with a challenge the key and password are unknown
-					Dbprintf("Page[ 2]: __ __ __ __");
-					Dbprintf("Page[ 3]: __ __ __ __");
-				}
 			}
+		}
 
-			txlen = 20;
-			crc = CRC_PRESET;
-			tx[0] = 0xc0 + (sendNum / 16);
-			calc_crc(&crc, tx[0], 8);
-			calc_crc(&crc, 0x00 + ((sendNum % 16) * 16), 4);
-			tx[1] = 0x00 + ((sendNum % 16) * 16) + (crc / 16);
-			tx[2] = 0x00 + (crc % 16) * 16;
+
+		
+		if (readBlock && tag.pstate == SELECTED && (tag.tstate == READING_BLOCK || tag.tstate == NO_OP) && rxlen > 0) {
+			i = hitag_read_block(htf, key, rx, &rxlen, tx, &txlen, sendNum);
+			if (i > 0) { sendNum+=4; }
+			if (sendNum+4 >= tag.max_page) {
+				bStop = !false;
+			}
+		} else if (!readBlock && tag.pstate == SELECTED && (tag.tstate == READING_PAGE || tag.tstate == NO_OP) && rxlen > 0) {
+			i = hitag_read_page(htf, key, rx, &rxlen, tx, &txlen, sendNum);
+			if (i > 0) { sendNum++; }
 			if (sendNum >= tag.max_page) {
 				bStop = !false;
 			}
@@ -1399,98 +1846,24 @@ void ReadHitagS(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)))
-			;
+		while (AT91C_BASE_TC0->TC_CV < T0 * (t_wait + (HITAG_T_TAG_HALF_PERIOD * lastbit))) { }
 
 		// 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) {
-						DbpString("Trace full");
-						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;
 
-		// 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) {
-						//DbpString("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)
-					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)
-					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)
-					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 {
-					// Ignore wierd value, is to small to mean anything
-				}
-			}
-
-			// 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;
-			}
-		}
+		// get tag id in anti-collision mode (proprietary data format, so switch off manchester and read at double the data rate, for 4 x the data bits)		
+ 		hitag_receive_frame(rx, &rxlen, &response);
 	}
 	end=false;
 	LED_B_OFF();
@@ -1501,6 +1874,21 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) {
 	cmd_send(CMD_ACK, bSuccessful, 0, 0, 0, 0);
 }
 
+void ReadHitagSCmd(hitag_function htf, hitag_data* htd, uint64_t startPage, uint64_t tagMode, bool readBlock) {
+	if (tagMode == 1) {
+		Dbprintf("ReadHitagS in mode=ADVANCED, blockRead=%d, startPage=%d", readBlock, startPage);
+		ReadHitagSintern(htf, htd, ADVANCED, (int)startPage, readBlock);
+	} else if (tagMode == 2) {
+		Dbprintf("ReadHitagS in mode=FAST_ADVANCED, blockRead=%d, startPage=%d", readBlock, startPage);
+		ReadHitagSintern(htf, htd, FAST_ADVANCED, (int)startPage, readBlock);
+	} else {
+		Dbprintf("ReadHitagS in mode=STANDARD, blockRead=%d, startPage=%d", readBlock, startPage);
+		ReadHitagSintern(htf, htd, STANDARD, (int)startPage, readBlock);
+	}	
+}
+
+
+
 /*
  * Authenticates to the Tag with the given Key or Challenge.
  * Writes the given 32Bit data into page_
@@ -1514,9 +1902,6 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) {
 	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;
@@ -1561,11 +1946,10 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) {
 	tag.tstate = NO_OP;
 
 // Clean up trace and prepare it for storing frames
-	set_tracing(TRUE);
+	set_tracing(true);
 	clear_trace();
 
 	bQuiet = false;
-	bQuitTraceFull = true;
 
 	LED_D_ON();
 
@@ -1574,8 +1958,7 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) {
 	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);
+	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
@@ -1613,8 +1996,6 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) {
 	response = 0;
 	lastbit = 1;
 	bStop = false;
-
-	reset_sof = 1;
 	t_wait = 200;
 
 	while (!bStop && !BUTTON_PRESS()) {
@@ -1637,11 +2018,12 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) {
 		}
 
 		//check for valid input
+		/*
 		if (page == 0) {
-			Dbprintf(
-					"usage: lf hitag writer [03 | 04] [CHALLENGE | KEY] [page] [byte0] [byte1] [byte2] [byte3]");		
+			Dbprintf("usage: lf hitag writer [03 | 04] [CHALLENGE | KEY] [page] [byte0] [byte1] [byte2] [byte3]");
 			bStop = !false;
-		}
+		}*/
+
 
 		// By default reset the transmission buffer
 		tx = txbuf;
@@ -1653,15 +2035,18 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) {
 			bStop = !false;
 		} else if (rxlen == 0 && tag.tstate != WRITING_PAGE_DATA) {
 			//start the authetication
-			txlen = 5;
-			memcpy(tx, "\xc0", nbytes(txlen));
-			tag.pstate = READY;
-			tag.tstate = NO_OP;
+			//tag.mode = ADVANCED;
+			tag.mode = STANDARD;
+			hitag_start_auth(tx, &txlen);
+			m = AC2K;
 		} else if (tag.pstate != SELECTED) {
 			//try to authenticate with the given key or challenge
-			if (hitagS_handle_tag_auth(htf,key,NrAr,rx, rxlen, tx, &txlen) == -1)
+			if (hitagS_handle_tag_auth(htf,key,NrAr,rx, rxlen, tx, &txlen) == -1) {
 				bStop = !false;
+			}
+			m = MC4K;
 		}
+
 		if (tag.pstate == SELECTED && tag.tstate == NO_OP && rxlen > 0) {
 			//check if the given page exists
 			if (page > tag.max_page) {
@@ -1678,7 +2063,7 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) {
 			tx[1] = 0x00 + ((page % 16) * 16) + (crc / 16);
 			tx[2] = 0x00 + (crc % 16) * 16;
 		} else if (tag.pstate == SELECTED && tag.tstate == WRITING_PAGE_ACK
-				&& rxlen == 6 && rx[0] == 0xf4) {
+				&& rxlen == 2 && rx[0] == 0x40) {
 			//ACK recieved to write the page. send data
 			tag.tstate = WRITING_PAGE_DATA;
 			txlen = 40;
@@ -1693,7 +2078,7 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) {
 			tx[3] = data[0];
 			tx[4] = crc;
 		} else if (tag.pstate == SELECTED && tag.tstate == WRITING_PAGE_DATA
-				&& rxlen == 6 && rx[0] == 0xf4) {
+				&& rxlen == 2 && rx[0] == 0x40) {
 			//received ACK
 			Dbprintf("Successful!");
 			bStop = !false;
@@ -1739,68 +2124,9 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) {
 		memset(rx, 0x00, sizeof(rx));
 		rxlen = 0;
 		lastbit = 1;
-		bSkip = true;
-		tag_sof = reset_sof;
 		response = 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) {
-						//DbpString("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)
-					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)
-					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)
-					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 {
-					// Ignore wierd value, is to small to mean anything
-				}
-			}
-
-			// 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;
-			}
-		}
+		hitag_receive_frame(rx, &rxlen, &response);
 	}
 	end=false;
 	LED_B_OFF();
@@ -1811,6 +2137,7 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) {
 	cmd_send(CMD_ACK, bSuccessful, 0, 0, 0, 0);
 }
 
+
 /*
  * Tries to authenticate to a Hitag S Transponder with the given challenges from a .cc file.
  * Displays all Challenges that failed.
@@ -1818,7 +2145,7 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) {
  * is not received correctly due to Antenna problems. This function
  * detects these challenges.
  */
-void check_challenges(bool file_given, byte_t* data) {
+void check_challenges_cmd(bool file_given, byte_t* data, uint64_t tagMode) {
 	int i, j, z, k;
 	byte_t uid_byte[4];
 	int frame_count;
@@ -1831,9 +2158,6 @@ void check_challenges(bool file_given, byte_t* data) {
 	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;
 	int STATE = 0;
 	bool bStop;
@@ -1843,16 +2167,27 @@ void check_challenges(bool file_given, byte_t* data) {
 	unsigned char uid[32];
 	unsigned char crc;
 
+	if (tagMode == 1) {
+		Dbprintf("check_challenges in mode=ADVANCED");
+		tag.mode = ADVANCED;
+	} else if (tagMode == 2) {
+		Dbprintf("check_challenges in mode=FAST_ADVANCED");
+		tag.mode = FAST_ADVANCED;
+	} else {
+		Dbprintf("check_challenges in mode=STANDARD");
+		tag.mode = STANDARD;
+	}
+
+
 	FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
 // Reset the return status
 	bSuccessful = false;
 
 // Clean up trace and prepare it for storing frames
-	set_tracing(TRUE);
+	set_tracing(true);
 	clear_trace();
 
 	bQuiet = false;
-	bQuitTraceFull = true;
 
 	LED_D_ON();
 
@@ -1887,9 +2222,7 @@ void check_challenges(bool file_given, byte_t* data) {
 
 // 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;
+	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;
@@ -1901,7 +2234,6 @@ void check_challenges(bool file_given, byte_t* data) {
 	lastbit = 1;
 	bStop = false;
 
-	reset_sof = 1;
 	t_wait = 200;
 
 	if (file_given) {
@@ -1931,18 +2263,17 @@ void check_challenges(bool file_given, byte_t* data) {
 		tx = txbuf;
 		txlen = 0;
 		if (rxlen == 0) {
-			if (STATE == 2)
+			if (STATE == 2) {
 				// challenge failed
 				Dbprintf("Challenge failed: %02X %02X %02X %02X %02X %02X %02X %02X",
 						unlocker[u1 - 1][0], unlocker[u1 - 1][1],
 						unlocker[u1 - 1][2], unlocker[u1 - 1][3],
 						unlocker[u1 - 1][4], unlocker[u1 - 1][5],
 						unlocker[u1 - 1][6], unlocker[u1 - 1][7]);
+			}
 			STATE = 0;
-			txlen = 5;
-			//start new authentication
-			memcpy(tx, "\xc0", nbytes(txlen));
-		} else if (rxlen >= 67 && STATE == 0) {
+			hitag_start_auth(tx, &txlen);
+		} else if (rxlen >= 32 && STATE == 0) {
 			//received uid
 			z = 0;
 			for (i = 0; i < 10; i++) {
@@ -1953,13 +2284,10 @@ void check_challenges(bool file_given, byte_t* data) {
 					z++;
 				}
 			}
-			k = 0;
-			for (i = 5; i < z; i += 2) {
-				uid[k] = response_bit[i];
-				k++;
-				if (k > 31)
-					break;
+			for (i = 0; i < 32; i++) {
+				uid[i] = response_bit[i];
 			}
+
 			uid_byte[0] = (uid[0] << 7) | (uid[1] << 6) | (uid[2] << 5)
 					| (uid[3] << 4) | (uid[4] << 3) | (uid[5] << 2)
 					| (uid[6] << 1) | uid[7];
@@ -2007,7 +2335,9 @@ void check_challenges(bool file_given, byte_t* data) {
 				k += 8;
 			}
 
-		} else if (STATE == 1 && rxlen == 44) {
+
+			tag.pstate = INIT;
+		} else if (STATE == 1 && rxlen > 24) {
 			//received configuration
 			STATE = 2;
 			z = 0;
@@ -2027,7 +2357,8 @@ void check_challenges(bool file_given, byte_t* data) {
 				tx[i] = unlocker[u1][i];
 			u1++;
 
-		} else if (STATE == 2 && rxlen >= 44) {
+			tag.pstate = SELECTED;
+		} else if (STATE == 2 && rxlen >= 32) {
 			STATE = 0;
 		}
 
@@ -2040,10 +2371,7 @@ void check_challenges(bool file_given, byte_t* data) {
 		// 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)))
-			;
+		while (AT91C_BASE_TC0->TC_CV < T0 * (t_wait + (HITAG_T_TAG_HALF_PERIOD * lastbit))) { }
 
 		// Transmit the reader frame
 		hitag_reader_send_frame(tx, txlen);
@@ -2071,68 +2399,9 @@ void check_challenges(bool file_given, byte_t* data) {
 		memset(rx, 0x00, sizeof(rx));
 		rxlen = 0;
 		lastbit = 1;
-		bSkip = true;
-		tag_sof = reset_sof;
 		response = 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) {
-						//DbpString("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)
-					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)
-					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)
-					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 {
-					// Ignore wierd value, is to small to mean anything
-				}
-			}
-
-			// 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;
-			}
-		}
+		hitag_receive_frame(rx, &rxlen, &response);
 	}
 	LED_B_OFF();
 	LED_D_OFF();
@@ -2142,5 +2411,13 @@ void check_challenges(bool file_given, byte_t* data) {
 	cmd_send(CMD_ACK, bSuccessful, 0, 0, 0, 0);
 }
 
+/**
+Backward compatibility
+*/
+void check_challenges(bool file_given, byte_t* data) {
+	check_challenges_cmd(file_given, data, 1);
+}
 
-
+void ReadHitagS(hitag_function htf, hitag_data* htd) {
+	ReadHitagSintern(htf, htd, ADVANCED, 0, false);
+}