From: pwpiwi <pwpiwi@users.noreply.github.com>
Date: Wed, 30 Oct 2019 17:55:13 +0000 (+0100)
Subject: Merge pull request #876 from pwpiwi/fix_iclass_reader
X-Git-Url: http://git.zerfleddert.de/cgi-bin/gitweb.cgi/proxmark3-svn/commitdiff_plain/e55b441992629938fecd75693d204dd98831d3ba?hp=b41be3cb11103739d2fa92ced09d1e29f8151b7b

Merge pull request #876 from pwpiwi/fix_iclass_reader

fix 'hf iclass reader'
* code deduplication. Use functions from iso15693.c
* speedup CodeIso15693AsReader()
* invert reader command coding. 0 now means 'unmodulated' ( = field on)
* decode SOF only as a valid tag response in Handle15693SamplesFromTag()
* complete decoding of EOF in Handle15693SamplesFromTag()
* determine and write correct times to trace
* FPGA-change: generate shorter frame signal to allow proper sync in StartCountSspClk()
* modify StartCountSspClk() for 16bit SSC transfers
* whitespace in util.c
* add specific LogTrace_ISO15693() with scaled down duration. Modify cmdhflist.c accordingly.
* allow 'hf 15 raw' with single byte commands
* check for buffer overflow, card timeout and single SOF in 'hf 15 raw'
* decode and handle SOF only responses in Handle14443bSamplesDemod()
* allow 1 byte commands with 'hf 14b raw'
* don't do READCHECK when not trying to authenticate
* standard LED handling
* remove unused FLAG_ICLASS_READER_ONLY_ONCE and FLAG_ICLASS_READER_ONE_TRY
* sanity check for negative times in TransmitTo15693Tag()
* increase reader timeout for 'hf 15' functions to be enough for slot 7 answers to ACTALL
* add 'hf iclass permute' inspired by RRG repository
* whitespace in cmdhficlass.c
---

diff --git a/armsrc/appmain.c b/armsrc/appmain.c
index e3bd1fe0..56da5434 100644
--- a/armsrc/appmain.c
+++ b/armsrc/appmain.c
@@ -1327,8 +1327,11 @@ void UsbPacketReceived(uint8_t *packet, int len)
 		case CMD_ICLASS_READBLOCK:
 			iClass_ReadBlk(c->arg[0]);
 			break;
-		case CMD_ICLASS_AUTHENTICATION: //check
-			iClass_Authentication(c->d.asBytes);
+		case CMD_ICLASS_CHECK:
+			iClass_Check(c->d.asBytes);
+			break;
+		case CMD_ICLASS_READCHECK:
+			iClass_Readcheck(c->arg[0], c->arg[1]);
 			break;
 		case CMD_ICLASS_DUMP:
 			iClass_Dump(c->arg[0], c->arg[1]);
diff --git a/armsrc/iclass.c b/armsrc/iclass.c
index 2533d1f9..1a729f3f 100644
--- a/armsrc/iclass.c
+++ b/armsrc/iclass.c
@@ -57,14 +57,17 @@
 #include "usb_cdc.h" // for usb_poll_validate_length
 #include "fpgaloader.h"
 
-static int timeout = 4096;
-
 // iCLASS has a slightly different timing compared to ISO15693. According to the picopass data sheet the tag response is expected 330us after
 // the reader command. This is measured from end of reader EOF to first modulation of the tag's SOF which starts with a 56,64us unmodulated period.
 // 330us = 140 ssp_clk cycles @ 423,75kHz when simulating.
 // 56,64us = 24 ssp_clk_cycles
-#define DELAY_ICLASS_VCD_TO_VICC_SIM 140
-#define TAG_SOF_UNMODULATED          24
+#define DELAY_ICLASS_VCD_TO_VICC_SIM     (140 - 24)
+// times in ssp_clk_cycles @ 3,3625MHz when acting as reader
+#define DELAY_ICLASS_VICC_TO_VCD_READER  DELAY_ISO15693_VICC_TO_VCD_READER
+// times in samples @ 212kHz when acting as reader
+#define ICLASS_READER_TIMEOUT_ACTALL     330 // 1558us, nominal 330us + 7slots*160us = 1450us
+#define ICLASS_READER_TIMEOUT_OTHERS      80 // 380us, nominal 330us
+
 
 //-----------------------------------------------------------------------------
 // The software UART that receives commands from the reader, and its state
@@ -692,20 +695,18 @@ void RAMFUNC SnoopIClass(void) {
 			if (OutOfNDecoding((smpl & 0xF0) >> 4)) {
 				rsamples = samples - Uart.samples;
 				time_stop = (GetCountSspClk()-time_0) << 4;
-				LED_C_ON();
 
 				//if (!LogTrace(Uart.output, Uart.byteCnt, rsamples, Uart.parityBits,true)) break;
 				//if (!LogTrace(NULL, 0, Uart.endTime*16 - DELAY_READER_AIR2ARM_AS_SNIFFER, 0, true)) break;
 				uint8_t parity[MAX_PARITY_SIZE];
 				GetParity(Uart.output, Uart.byteCnt, parity);
-				LogTrace(Uart.output, Uart.byteCnt, time_start, time_stop, parity, true);
+				LogTrace_ISO15693(Uart.output, Uart.byteCnt, time_start*32, time_stop*32, parity, true);
 
 				/* And ready to receive another command. */
 				Uart.state = STATE_UNSYNCD;
 				/* And also reset the demod code, which might have been */
 				/* false-triggered by the commands from the reader. */
 				Demod.state = DEMOD_UNSYNCD;
-				LED_B_OFF();
 				Uart.byteCnt = 0;
 			} else {
 				time_start = (GetCountSspClk()-time_0) << 4;
@@ -719,17 +720,15 @@ void RAMFUNC SnoopIClass(void) {
 				time_stop = (GetCountSspClk()-time_0) << 4;
 
 				rsamples = samples - Demod.samples;
-				LED_B_ON();
 
 				uint8_t parity[MAX_PARITY_SIZE];
 				GetParity(Demod.output, Demod.len, parity);
-				LogTrace(Demod.output, Demod.len, time_start, time_stop, parity, false);
+				LogTrace_ISO15693(Demod.output, Demod.len, time_start*32, time_stop*32, parity, false);
 
 				// And ready to receive another response.
 				memset(&Demod, 0, sizeof(Demod));
 				Demod.output = tagToReaderResponse;
 				Demod.state = DEMOD_UNSYNCD;
-				LED_C_OFF();
 			} else {
 				time_start = (GetCountSspClk()-time_0) << 4;
 			}
@@ -1230,9 +1229,9 @@ int doIClassSimulation(int simulationMode, uint8_t *reader_mac_buf) {
 		A legit tag has about 273,4us delay between reader EOT and tag SOF.
 		**/
 		if (modulated_response_size > 0) {
-			uint32_t response_time = reader_eof_time + DELAY_ICLASS_VCD_TO_VICC_SIM - TAG_SOF_UNMODULATED - DELAY_ARM_TO_READER_SIM;
+			uint32_t response_time = reader_eof_time + DELAY_ICLASS_VCD_TO_VICC_SIM;
 			TransmitTo15693Reader(modulated_response, modulated_response_size, &response_time, 0, false);
-			LogTrace(trace_data, trace_data_size, response_time + DELAY_ARM_TO_READER_SIM, response_time + (modulated_response_size << 6) + DELAY_ARM_TO_READER_SIM, NULL, false);
+			LogTrace_ISO15693(trace_data, trace_data_size, response_time*32, response_time*32 + modulated_response_size/2, NULL, false);
 		}
 
 	}
@@ -1327,214 +1326,22 @@ void SimulateIClass(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain
 
 /// THE READER CODE
 
-//-----------------------------------------------------------------------------
-// Transmit the command (to the tag) that was placed in ToSend[].
-//-----------------------------------------------------------------------------
-static void TransmitIClassCommand(const uint8_t *cmd, int len, int *samples, int *wait) {
-	int c;
-	FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_READER_MOD);
-	AT91C_BASE_SSC->SSC_THR = 0x00;
-	FpgaSetupSsc(FPGA_MAJOR_MODE_HF_ISO14443A);
-
-	if (wait) {
-		if (*wait < 10) *wait = 10;
-
-		for (c = 0; c < *wait;) {
-			if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) {
-				AT91C_BASE_SSC->SSC_THR = 0x00;     // For exact timing!
-				c++;
-			}
-			if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) {
-				volatile uint32_t r = AT91C_BASE_SSC->SSC_RHR;
-				(void)r;
-			}
-			WDT_HIT();
-		}
-	}
+static void ReaderTransmitIClass(uint8_t *frame, int len, uint32_t *start_time) {
 
-	uint8_t sendbyte;
-	bool firstpart = true;
-	c = 0;
-	for (;;) {
-		if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) {
+	CodeIso15693AsReader(frame, len);
 
-			// DOUBLE THE SAMPLES!
-			if (firstpart) {
-				sendbyte = (cmd[c] & 0xf0) | (cmd[c] >> 4);
-			} else {
-				sendbyte = (cmd[c] & 0x0f) | (cmd[c] << 4);
-				c++;
-			}
-			if (sendbyte == 0xff) {
-				sendbyte = 0xfe;
-			}
-			AT91C_BASE_SSC->SSC_THR = sendbyte;
-			firstpart = !firstpart;
+	TransmitTo15693Tag(ToSend, ToSendMax, start_time);
 
-			if (c >= len) {
-				break;
-			}
-		}
-		if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) {
-			volatile uint32_t r = AT91C_BASE_SSC->SSC_RHR;
-			(void)r;
-		}
-		WDT_HIT();
-	}
-	if (samples && wait) *samples = (c + *wait) << 3;
+	uint32_t end_time = *start_time + 32*(8*ToSendMax-4); // substract the 4 padding bits after EOF
+	LogTrace_ISO15693(frame, len, *start_time*4, end_time*4, NULL, true);
 }
 
 
-//-----------------------------------------------------------------------------
-// Prepare iClass reader command to send to FPGA
-//-----------------------------------------------------------------------------
-void CodeIClassCommand(const uint8_t *cmd, int len) {
-	int i, j, k;
-
-	ToSendReset();
-
-	// Start of Communication: 1 out of 4
-	ToSend[++ToSendMax] = 0xf0;
-	ToSend[++ToSendMax] = 0x00;
-	ToSend[++ToSendMax] = 0x0f;
-	ToSend[++ToSendMax] = 0x00;
-
-	// Modulate the bytes
-	for (i = 0; i < len; i++) {
-		uint8_t b = cmd[i];
-		for (j = 0; j < 4; j++) {
-			for (k = 0; k < 4; k++) {
-				if (k == (b & 3)) {
-					ToSend[++ToSendMax] = 0x0f;
-				} else {
-					ToSend[++ToSendMax] = 0x00;
-				}
-			}
-			b >>= 2;
-		}
-	}
-
-	// End of Communication
-	ToSend[++ToSendMax] = 0x00;
-	ToSend[++ToSendMax] = 0x00;
-	ToSend[++ToSendMax] = 0xf0;
-	ToSend[++ToSendMax] = 0x00;
-
-	// Convert from last character reference to length
-	ToSendMax++;
-}
-
-static void ReaderTransmitIClass(uint8_t *frame, int len) {
-	int wait = 0;
-	int samples = 0;
-
-	// This is tied to other size changes
-	CodeIClassCommand(frame, len);
-
-	// Select the card
-	TransmitIClassCommand(ToSend, ToSendMax, &samples, &wait);
-	if (trigger)
-		LED_A_ON();
-
-	// Store reader command in buffer
-	uint8_t par[MAX_PARITY_SIZE];
-	GetParity(frame, len, par);
-	LogTrace(frame, len, rsamples, rsamples, par, true);
-}
-
-//-----------------------------------------------------------------------------
-// Wait a certain time for tag response
-//  If a response is captured return true
-//  If it takes too long return false
-//-----------------------------------------------------------------------------
-static int GetIClassAnswer(uint8_t *receivedResponse, int maxLen, int *samples, int *elapsed) {
-	//uint8_t *buffer
-	// buffer needs to be 512 bytes
-	int c;
-
-	// Set FPGA mode to "reader listen mode", no modulation (listen
-	// only, since we are receiving, not transmitting).
-	FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_READER_LISTEN);
-
-	// Now get the answer from the card
-	Demod.output = receivedResponse;
-	Demod.len = 0;
-	Demod.state = DEMOD_UNSYNCD;
-
-	uint8_t b;
-	if (elapsed) *elapsed = 0;
-
-	bool skip = false;
-
-	c = 0;
-	for (;;) {
-		WDT_HIT();
-
-		if (BUTTON_PRESS()) return false;
-
-		if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) {
-			AT91C_BASE_SSC->SSC_THR = 0x00;  // To make use of exact timing of next command from reader!!
-			if (elapsed) (*elapsed)++;
-		}
-		if (AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) {
-			if (c < timeout) {
-				c++;
-			} else {
-				return false;
-			}
-			b = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
-			skip = !skip;
-			if (skip) continue;
-
-			if (ManchesterDecoding(b & 0x0f)) {
-				*samples = c << 3;
-				return  true;
-			}
-		}
-	}
-}
-
-static int ReaderReceiveIClass(uint8_t *receivedAnswer) {
-	int samples = 0;
-	if (!GetIClassAnswer(receivedAnswer, 160, &samples, 0)) {
-		return false;
-	}
-	rsamples += samples;
-	uint8_t parity[MAX_PARITY_SIZE];
-	GetParity(receivedAnswer, Demod.len, parity);
-	LogTrace(receivedAnswer, Demod.len, rsamples, rsamples, parity, false);
-	if (samples == 0) return false;
-	return Demod.len;
-}
-
-static void setupIclassReader() {
-	FpgaDownloadAndGo(FPGA_BITSTREAM_HF);
-	// Reset trace buffer
-	set_tracing(true);
-	clear_trace();
-
-	// Setup SSC
-	FpgaSetupSsc(FPGA_MAJOR_MODE_HF_ISO14443A);
-	// Start from off (no field generated)
-	// Signal field is off with the appropriate LED
-	LED_D_OFF();
-	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
-	SpinDelay(200);
-
-	SetAdcMuxFor(GPIO_MUXSEL_HIPKD);
-
-	// Now give it time to spin up.
-	// Signal field is on with the appropriate LED
-	FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_READER_MOD);
-	SpinDelay(200);
-	LED_A_ON();
-
-}
-
-static bool sendCmdGetResponseWithRetries(uint8_t* command, size_t cmdsize, uint8_t* resp, uint8_t expected_size, uint8_t retries) {
+static bool sendCmdGetResponseWithRetries(uint8_t* command, size_t cmdsize, uint8_t* resp, size_t max_resp_size,
+										  uint8_t expected_size, uint8_t retries, uint32_t start_time, uint32_t *eof_time) {
 	while (retries-- > 0) {
-		ReaderTransmitIClass(command, cmdsize);
-		if (expected_size == ReaderReceiveIClass(resp)) {
+		ReaderTransmitIClass(command, cmdsize, &start_time);
+		if (expected_size == GetIso15693AnswerFromTag(resp, max_resp_size, ICLASS_READER_TIMEOUT_OTHERS, eof_time)) {
 			return true;
 		}
 	}
@@ -1542,183 +1349,129 @@ static bool sendCmdGetResponseWithRetries(uint8_t* command, size_t cmdsize, uint
 }
 
 /**
- * @brief Talks to an iclass tag, sends the commands to get CSN and CC.
- * @param card_data where the CSN and CC are stored for return
- * @return 0 = fail
- *         1 = Got CSN
- *         2 = Got CSN and CC
+ * @brief Selects an iclass tag
+ * @param card_data where the CSN is stored for return
+ * @return false = fail
+ *         true = success
  */
-static uint8_t handshakeIclassTag_ext(uint8_t *card_data, bool use_credit_key) {
-	static uint8_t act_all[]     = { 0x0a };
-	//static uint8_t identify[]    = { 0x0c };
-	static uint8_t identify[]    = { 0x0c, 0x00, 0x73, 0x33 };
-	static uint8_t select[]      = { 0x81, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
-	static uint8_t readcheck_cc[]= { 0x88, 0x02 };
-	if (use_credit_key)
-		readcheck_cc[0] = 0x18;
-	else
-		readcheck_cc[0] = 0x88;
+static bool selectIclassTag(uint8_t *card_data, uint32_t *eof_time) {
+	uint8_t act_all[]      = { 0x0a };
+	uint8_t identify[]     = { 0x0c };
+	uint8_t select[]       = { 0x81, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
 
 	uint8_t resp[ICLASS_BUFFER_SIZE];
 
-	uint8_t read_status = 0;
+	uint32_t start_time = GetCountSspClk();
 
 	// Send act_all
-	ReaderTransmitIClass(act_all, 1);
+	ReaderTransmitIClass(act_all, 1, &start_time);
 	// Card present?
-	if (!ReaderReceiveIClass(resp)) return read_status;//Fail
+	if (GetIso15693AnswerFromTag(resp, sizeof(resp), ICLASS_READER_TIMEOUT_ACTALL, eof_time) < 0) return false;//Fail
 
 	//Send Identify
-	ReaderTransmitIClass(identify, 1);
+	start_time = *eof_time + DELAY_ICLASS_VICC_TO_VCD_READER;
+	ReaderTransmitIClass(identify, 1, &start_time);
 	//We expect a 10-byte response here, 8 byte anticollision-CSN and 2 byte CRC
-	uint8_t len = ReaderReceiveIClass(resp);
-	if (len != 10) return read_status;//Fail
+	uint8_t len = GetIso15693AnswerFromTag(resp, sizeof(resp), ICLASS_READER_TIMEOUT_OTHERS, eof_time);
+	if (len != 10) return false;//Fail
 
 	//Copy the Anti-collision CSN to our select-packet
 	memcpy(&select[1], resp, 8);
 	//Select the card
-	ReaderTransmitIClass(select, sizeof(select));
+	start_time = *eof_time + DELAY_ICLASS_VICC_TO_VCD_READER;
+	ReaderTransmitIClass(select, sizeof(select), &start_time);
 	//We expect a 10-byte response here, 8 byte CSN and 2 byte CRC
-	len = ReaderReceiveIClass(resp);
-	if (len != 10) return read_status;//Fail
+	len = GetIso15693AnswerFromTag(resp, sizeof(resp), ICLASS_READER_TIMEOUT_OTHERS, eof_time);
+	if (len != 10) return false;//Fail
 
-	//Success - level 1, we got CSN
+	//Success - we got CSN
 	//Save CSN in response data
 	memcpy(card_data, resp, 8);
 
-	//Flag that we got to at least stage 1, read CSN
-	read_status = 1;
-
-	// Card selected, now read e-purse (cc) (only 8 bytes no CRC)
-	ReaderTransmitIClass(readcheck_cc, sizeof(readcheck_cc));
-	if (ReaderReceiveIClass(resp) == 8) {
-		//Save CC (e-purse) in response data
-		memcpy(card_data+8, resp, 8);
-		read_status++;
-	}
-
-	return read_status;
+	return true;
 }
 
-static uint8_t handshakeIclassTag(uint8_t *card_data) {
-	return handshakeIclassTag_ext(card_data, false);
-}
 
-
-// Reader iClass Anticollission
+// Select an iClass tag and read all blocks which are always readable without authentication
 void ReaderIClass(uint8_t arg0) {
 
+	LED_A_ON();
+
 	uint8_t card_data[6 * 8] = {0};
 	memset(card_data, 0xFF, sizeof(card_data));
-	uint8_t last_csn[8] = {0,0,0,0,0,0,0,0};
 	uint8_t resp[ICLASS_BUFFER_SIZE];
-	memset(resp, 0xFF, sizeof(resp));
 	//Read conf block CRC(0x01) => 0xfa 0x22
-	uint8_t readConf[] = { ICLASS_CMD_READ_OR_IDENTIFY, 0x01, 0xfa, 0x22};
+	uint8_t readConf[] = {ICLASS_CMD_READ_OR_IDENTIFY, 0x01, 0xfa, 0x22};
+	//Read e-purse block CRC(0x02) => 0x61 0x10
+	uint8_t readEpurse[] = {ICLASS_CMD_READ_OR_IDENTIFY, 0x02, 0x61, 0x10};
 	//Read App Issuer Area block CRC(0x05) => 0xde  0x64
-	uint8_t readAA[] = { ICLASS_CMD_READ_OR_IDENTIFY, 0x05, 0xde, 0x64};
+	uint8_t readAA[] = {ICLASS_CMD_READ_OR_IDENTIFY, 0x05, 0xde, 0x64};
 
-	int read_status= 0;
 	uint8_t result_status = 0;
-	// flag to read until one tag is found successfully
-	bool abort_after_read = arg0 & FLAG_ICLASS_READER_ONLY_ONCE;
-	// flag to only try 5 times to find one tag then return
-	bool try_once = arg0 & FLAG_ICLASS_READER_ONE_TRY;
-	// if neither abort_after_read nor try_once then continue reading until button pressed.
 
-	bool use_credit_key = arg0 & FLAG_ICLASS_READER_CEDITKEY;
 	// test flags for what blocks to be sure to read
 	uint8_t flagReadConfig = arg0 & FLAG_ICLASS_READER_CONF;
 	uint8_t flagReadCC = arg0 & FLAG_ICLASS_READER_CC;
 	uint8_t flagReadAA = arg0 & FLAG_ICLASS_READER_AA;
 
 	set_tracing(true);
-	setupIclassReader();
+	clear_trace();
+	Iso15693InitReader();
 
-	uint16_t tryCnt = 0;
-	bool userCancelled = BUTTON_PRESS() || usb_poll_validate_length();
-	while (!userCancelled) {
-		// if only looking for one card try 2 times if we missed it the first time
-		if (try_once && tryCnt > 2) {
-			break;
-		}
-		tryCnt++;
-		if (!get_tracing()) {
-			DbpString("Trace full");
-			break;
-		}
-		WDT_HIT();
+	StartCountSspClk();
+	uint32_t start_time = 0;
+	uint32_t eof_time = 0;
 
-		read_status = handshakeIclassTag_ext(card_data, use_credit_key);
-
-		if (read_status == 0) continue;
-		if (read_status == 1) result_status = FLAG_ICLASS_READER_CSN;
-		if (read_status == 2) result_status = FLAG_ICLASS_READER_CSN | FLAG_ICLASS_READER_CC;
-
-		// handshakeIclass returns CSN|CC, but the actual block
-		// layout is CSN|CONFIG|CC, so here we reorder the data,
-		// moving CC forward 8 bytes
-		memcpy(card_data+16, card_data+8, 8);
-		//Read block 1, config
-		if (flagReadConfig) {
-			if (sendCmdGetResponseWithRetries(readConf, sizeof(readConf), resp, 10, 10)) {
-				result_status |= FLAG_ICLASS_READER_CONF;
-				memcpy(card_data+8, resp, 8);
-			} else {
-				Dbprintf("Failed to dump config block");
-			}
+	if (selectIclassTag(resp, &eof_time)) {
+		result_status = FLAG_ICLASS_READER_CSN;
+		memcpy(card_data, resp, 8);
+	}
+	
+	start_time = eof_time + DELAY_ICLASS_VICC_TO_VCD_READER;
+	
+	//Read block 1, config
+	if (flagReadConfig) {
+		if (sendCmdGetResponseWithRetries(readConf, sizeof(readConf), resp, sizeof(resp), 10, 10, start_time, &eof_time)) {
+			result_status |= FLAG_ICLASS_READER_CONF;
+			memcpy(card_data+8, resp, 8);
+		} else {
+			Dbprintf("Failed to read config block");
 		}
+		start_time = eof_time + DELAY_ICLASS_VICC_TO_VCD_READER;
+	}
 
-		//Read block 5, AA
-		if (flagReadAA) {
-			if (sendCmdGetResponseWithRetries(readAA, sizeof(readAA), resp, 10, 10)) {
-				result_status |= FLAG_ICLASS_READER_AA;
-				memcpy(card_data + (8*5), resp, 8);
-			} else {
-				//Dbprintf("Failed to dump AA block");
-			}
+	//Read block 2, e-purse
+	if (flagReadCC) {
+		if (sendCmdGetResponseWithRetries(readEpurse, sizeof(readEpurse), resp, sizeof(resp), 10, 10, start_time, &eof_time)) {
+			result_status |= FLAG_ICLASS_READER_CC;
+			memcpy(card_data + (8*2), resp, 8);
+		} else {
+			Dbprintf("Failed to read e-purse");
 		}
+		start_time = eof_time + DELAY_ICLASS_VICC_TO_VCD_READER;
+	}
 
-		// 0 : CSN
-		// 1 : Configuration
-		// 2 : e-purse
-		// 3 : kd / debit / aa2 (write-only)
-		// 4 : kc / credit / aa1 (write-only)
-		// 5 : AIA, Application issuer area
-		//Then we can 'ship' back the 6 * 8 bytes of data,
-		// with 0xFF:s in block 3 and 4.
-
-		LED_B_ON();
-		//Send back to client, but don't bother if we already sent this -
-		//  only useful if looping in arm (not try_once && not abort_after_read)
-		if (memcmp(last_csn, card_data, 8) != 0) {
-			// If caller requires that we get Conf, CC, AA, continue until we got it
-			if ( (result_status ^ FLAG_ICLASS_READER_CSN ^ flagReadConfig ^ flagReadCC ^ flagReadAA) == 0) {
-				cmd_send(CMD_ACK, result_status, 0, 0, card_data, sizeof(card_data));
-				if (abort_after_read) {
-					FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
-					LED_A_OFF();
-					LED_B_OFF();
-					return;
-				}
-				//Save that we already sent this....
-				memcpy(last_csn, card_data, 8);
-			}
-
+	//Read block 5, AA
+	if (flagReadAA) {
+		if (sendCmdGetResponseWithRetries(readAA, sizeof(readAA), resp, sizeof(resp), 10, 10, start_time, &eof_time)) {
+			result_status |= FLAG_ICLASS_READER_AA;
+			memcpy(card_data + (8*5), resp, 8);
+		} else {
+			Dbprintf("Failed to read AA block");
 		}
-		LED_B_OFF();
-		userCancelled = BUTTON_PRESS() || usb_poll_validate_length();
-	}
-	if (userCancelled) {
-		cmd_send(CMD_ACK, 0xFF, 0, 0, card_data, 0);
-	} else {
-		cmd_send(CMD_ACK, 0, 0, 0, card_data, 0);
 	}
+
+	cmd_send(CMD_ACK, result_status, 0, 0, card_data, sizeof(card_data));
+
 	LED_A_OFF();
 }
 
+
 void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC) {
 
+	LED_A_ON();
+
+	bool use_credit_key = false;
 	uint8_t card_data[USB_CMD_DATA_SIZE]={0};
 	uint16_t block_crc_LUT[255] = {0};
 
@@ -1729,6 +1482,9 @@ void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC) {
 	}
 	//Dbprintf("Lookup table: %02x %02x %02x" ,block_crc_LUT[0],block_crc_LUT[1],block_crc_LUT[2]);
 
+	uint8_t readcheck_cc[] = { ICLASS_CMD_READCHECK_KD, 0x02 };
+	if (use_credit_key)
+		readcheck_cc[0] = ICLASS_CMD_READCHECK_KC;
 	uint8_t check[]       = { 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
 	uint8_t read[]        = { 0x0c, 0x00, 0x00, 0x00 };
 
@@ -1746,8 +1502,13 @@ void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC) {
 
 	uint8_t resp[ICLASS_BUFFER_SIZE];
 
-	setupIclassReader();
 	set_tracing(true);
+	clear_trace();
+	Iso15693InitReader();
+
+	StartCountSspClk();
+	uint32_t start_time = 0;
+	uint32_t eof_time = 0;
 
 	while (!BUTTON_PRESS()) {
 
@@ -1758,13 +1519,16 @@ void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC) {
 			break;
 		}
 
-		uint8_t read_status = handshakeIclassTag(card_data);
-		if (read_status < 2) continue;
+		if (!selectIclassTag(card_data, &eof_time)) continue;
 
-		//for now replay captured auth (as cc not updated)
+		start_time = eof_time + DELAY_ICLASS_VICC_TO_VCD_READER;
+		if (!sendCmdGetResponseWithRetries(readcheck_cc, sizeof(readcheck_cc), resp, sizeof(resp), 8, 3, start_time, &eof_time)) continue;
+
+		// replay captured auth (cc must not have been updated)
 		memcpy(check+5, MAC, 4);
 
-		if (!sendCmdGetResponseWithRetries(check, sizeof(check), resp, 4, 5)) {
+		start_time = eof_time + DELAY_ICLASS_VICC_TO_VCD_READER;
+		if (!sendCmdGetResponseWithRetries(check, sizeof(check), resp, sizeof(resp), 4, 5, start_time, &eof_time)) {
 			Dbprintf("Error: Authentication Fail!");
 			continue;
 		}
@@ -1775,7 +1539,9 @@ void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC) {
 		read[2] = crc >> 8;
 		read[3] = crc & 0xff;
 
-		if (!sendCmdGetResponseWithRetries(read, sizeof(read),resp, 10, 10)) {
+		start_time = eof_time + DELAY_ICLASS_VICC_TO_VCD_READER;
+		if (!sendCmdGetResponseWithRetries(read, sizeof(read), resp, sizeof(resp), 10, 10, start_time, &eof_time)) {
+			start_time = eof_time + DELAY_ICLASS_VICC_TO_VCD_READER;
 			Dbprintf("Dump config (block 1) failed");
 			continue;
 		}
@@ -1800,7 +1566,8 @@ void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC) {
 			read[2] = crc >> 8;
 			read[3] = crc & 0xff;
 
-			if (sendCmdGetResponseWithRetries(read, sizeof(read), resp, 10, 10)) {
+			start_time = eof_time + DELAY_ICLASS_VICC_TO_VCD_READER;
+			if (sendCmdGetResponseWithRetries(read, sizeof(read), resp, sizeof(resp), 10, 10, start_time, &eof_time)) {
 				Dbprintf("     %02x: %02x %02x %02x %02x %02x %02x %02x %02x",
 						block, resp[0], resp[1], resp[2],
 						resp[3], resp[4], resp[5],
@@ -1849,43 +1616,68 @@ void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC) {
 			 0);
 
 	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
+	LED_D_OFF();
 	LED_A_OFF();
 }
 
-void iClass_Authentication(uint8_t *MAC) {
-	uint8_t check[] = { ICLASS_CMD_CHECK_KD, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
-	uint8_t resp[ICLASS_BUFFER_SIZE];
+
+void iClass_Check(uint8_t *MAC) {
+	uint8_t check[9] = {ICLASS_CMD_CHECK_KD, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+	uint8_t resp[4];
 	memcpy(check+5, MAC, 4);
-	bool isOK;
-	isOK = sendCmdGetResponseWithRetries(check, sizeof(check), resp, 4, 6);
-	cmd_send(CMD_ACK,isOK, 0, 0, 0, 0);
+	uint32_t eof_time;
+	bool isOK = sendCmdGetResponseWithRetries(check, sizeof(check), resp, sizeof(resp), 4, 6, 0, &eof_time);
+	cmd_send(CMD_ACK, isOK, 0, 0, resp, sizeof(resp));
 }
 
+
+void iClass_Readcheck(uint8_t block, bool use_credit_key) {
+	uint8_t readcheck[2] = {ICLASS_CMD_READCHECK_KD, block};
+	if (use_credit_key) {
+		readcheck[0] = ICLASS_CMD_READCHECK_KC;
+	}
+	uint8_t resp[8];
+	uint32_t eof_time;
+	bool isOK = sendCmdGetResponseWithRetries(readcheck, sizeof(readcheck), resp, sizeof(resp), 8, 6, 0, &eof_time);
+	cmd_send(CMD_ACK, isOK, 0, 0, resp, sizeof(resp));
+}
+
+
 static bool iClass_ReadBlock(uint8_t blockNo, uint8_t *readdata) {
 	uint8_t readcmd[] = {ICLASS_CMD_READ_OR_IDENTIFY, blockNo, 0x00, 0x00}; //0x88, 0x00 // can i use 0C?
 	char bl = blockNo;
 	uint16_t rdCrc = iclass_crc16(&bl, 1);
 	readcmd[2] = rdCrc >> 8;
 	readcmd[3] = rdCrc & 0xff;
-	uint8_t resp[] = {0,0,0,0,0,0,0,0,0,0};
+	uint8_t resp[10];
 	bool isOK = false;
+	uint32_t eof_time;
 
-	//readcmd[1] = blockNo;
-	isOK = sendCmdGetResponseWithRetries(readcmd, sizeof(readcmd), resp, 10, 10);
+	isOK = sendCmdGetResponseWithRetries(readcmd, sizeof(readcmd), resp, sizeof(resp), 10, 10, 0, &eof_time);
 	memcpy(readdata, resp, sizeof(resp));
 
 	return isOK;
 }
 
+
 void iClass_ReadBlk(uint8_t blockno) {
+
+	LED_A_ON();
+
 	uint8_t readblockdata[] = {0,0,0,0,0,0,0,0,0,0};
 	bool isOK = false;
 	isOK = iClass_ReadBlock(blockno, readblockdata);
 	cmd_send(CMD_ACK, isOK, 0, 0, readblockdata, 8);
 	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
+	LED_D_OFF();
+
+	LED_A_OFF();
 }
 
 void iClass_Dump(uint8_t blockno, uint8_t numblks) {
+
+	LED_A_ON();
+
 	uint8_t readblockdata[] = {0,0,0,0,0,0,0,0,0,0};
 	bool isOK = false;
 	uint8_t blkCnt = 0;
@@ -1915,12 +1707,19 @@ void iClass_Dump(uint8_t blockno, uint8_t numblks) {
 	}
 	//return pointer to dump memory in arg3
 	cmd_send(CMD_ACK, isOK, blkCnt, BigBuf_max_traceLen(), 0, 0);
+
 	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
-	LEDsoff();
+	LED_D_OFF();
 	BigBuf_free();
+
+	LED_A_OFF();
 }
 
+
 static bool iClass_WriteBlock_ext(uint8_t blockNo, uint8_t *data) {
+
+	LED_A_ON();
+
 	uint8_t write[] = { ICLASS_CMD_UPDATE, blockNo, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
 	//uint8_t readblockdata[10];
 	//write[1] = blockNo;
@@ -1929,23 +1728,32 @@ static bool iClass_WriteBlock_ext(uint8_t blockNo, uint8_t *data) {
 	uint16_t wrCrc = iclass_crc16(wrCmd, 13);
 	write[14] = wrCrc >> 8;
 	write[15] = wrCrc & 0xff;
-	uint8_t resp[] = {0,0,0,0,0,0,0,0,0,0};
+	uint8_t resp[10];
 	bool isOK = false;
+	uint32_t eof_time = 0;
 
-	isOK = sendCmdGetResponseWithRetries(write, sizeof(write), resp, sizeof(resp), 10);
+	isOK = sendCmdGetResponseWithRetries(write, sizeof(write), resp, sizeof(resp), 10, 10, 0, &eof_time);
+	uint32_t start_time = eof_time + DELAY_ICLASS_VICC_TO_VCD_READER;
 	if (isOK) { //if reader responded correctly
 		//Dbprintf("WriteResp: %02X%02X%02X%02X%02X%02X%02X%02X%02X%02X",resp[0],resp[1],resp[2],resp[3],resp[4],resp[5],resp[6],resp[7],resp[8],resp[9]);
 		if (memcmp(write+2, resp, 8)) {  //if response is not equal to write values
 			if (blockNo != 3 && blockNo != 4) { //if not programming key areas (note key blocks don't get programmed with actual key data it is xor data)
 				//error try again
-				isOK = sendCmdGetResponseWithRetries(write, sizeof(write), resp, sizeof(resp), 10);
+				isOK = sendCmdGetResponseWithRetries(write, sizeof(write), resp, sizeof(resp), 10, 10, start_time, &eof_time);
 			}
 		}
 	}
+
+	LED_A_OFF();
+
 	return isOK;
 }
 
+
 void iClass_WriteBlock(uint8_t blockNo, uint8_t *data) {
+
+	LED_A_ON();
+
 	bool isOK = iClass_WriteBlock_ext(blockNo, data);
 	if (isOK){
 		Dbprintf("Write block [%02x] successful", blockNo);
@@ -1953,7 +1761,11 @@ void iClass_WriteBlock(uint8_t blockNo, uint8_t *data) {
 		Dbprintf("Write block [%02x] failed", blockNo);
 	}
 	cmd_send(CMD_ACK, isOK, 0, 0, 0, 0);
+
 	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
+	LED_D_OFF();
+
+	LED_A_OFF();
 }
 
 void iClass_Clone(uint8_t startblock, uint8_t endblock, uint8_t *data) {
@@ -1981,5 +1793,6 @@ void iClass_Clone(uint8_t startblock, uint8_t endblock, uint8_t *data) {
 
 	cmd_send(CMD_ACK, 1, 0, 0, 0, 0);
 	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
-	LEDsoff();
+	LED_D_OFF();
+	LED_A_OFF();
 }
diff --git a/armsrc/iclass.h b/armsrc/iclass.h
index 3cbe79fb..9666e888 100644
--- a/armsrc/iclass.h
+++ b/armsrc/iclass.h
@@ -15,6 +15,7 @@
 #define ICLASS_H__
 
 #include <stdint.h>
+#include <stdbool.h>
 #include "common.h" // for RAMFUNC
 
 extern void RAMFUNC SnoopIClass(void);
@@ -22,7 +23,8 @@ extern void SimulateIClass(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t
 extern void ReaderIClass(uint8_t arg0);
 extern void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC);
 extern void IClass_iso14443A_GetPublic(uint8_t arg0);
-extern void iClass_Authentication(uint8_t *MAC);
+extern void iClass_Readcheck(uint8_t block, bool use_credit_key);
+extern void iClass_Check(uint8_t *MAC);
 extern void iClass_WriteBlock(uint8_t blockNo, uint8_t *data);
 extern void iClass_ReadBlk(uint8_t blockNo);
 extern void iClass_Dump(uint8_t blockno, uint8_t numblks);
diff --git a/armsrc/iso14443b.c b/armsrc/iso14443b.c
index f276158f..6434409d 100644
--- a/armsrc/iso14443b.c
+++ b/armsrc/iso14443b.c
@@ -586,9 +586,10 @@ static RAMFUNC int Handle14443bSamplesDemod(int ci, int cq)
 					Demod.state = DEMOD_UNSYNCD;
 				} else {
 					LED_C_ON(); // Got SOF
-					Demod.state = DEMOD_AWAITING_START_BIT;
 					Demod.posCount = 0;
+					Demod.bitCount = 0;
 					Demod.len = 0;
+					Demod.state = DEMOD_AWAITING_START_BIT;
 /* this had been used to add RSSI (Received Signal Strength Indication) to traces. Currently not implemented.
 					Demod.metricN = 0;
 					Demod.metric = 0;
@@ -605,13 +606,16 @@ static RAMFUNC int Handle14443bSamplesDemod(int ci, int cq)
 		case DEMOD_AWAITING_START_BIT:
 			Demod.posCount++;
 			MAKE_SOFT_DECISION();
-			if(v > 0) {
-				if(Demod.posCount > 3*2) { 		// max 19us between characters = 16 1/fs, max 3 etu after low phase of SOF = 24 1/fs
-					Demod.state = DEMOD_UNSYNCD;
+			if (v > 0) {
+				if (Demod.posCount > 3*2) { 		// max 19us between characters = 16 1/fs, max 3 etu after low phase of SOF = 24 1/fs
 					LED_C_OFF();
+					if (Demod.bitCount == 0 && Demod.len == 0) { // received SOF only, this is valid for iClass/Picopass
+						return true;
+					} else {
+						Demod.state = DEMOD_UNSYNCD;
+					}
 				}
 			} else {							// start bit detected
-				Demod.bitCount = 0;
 				Demod.posCount = 1;				// this was the first half
 				Demod.thisBit = v;
 				Demod.shiftReg = 0;
@@ -621,7 +625,7 @@ static RAMFUNC int Handle14443bSamplesDemod(int ci, int cq)
 
 		case DEMOD_RECEIVING_DATA:
 			MAKE_SOFT_DECISION();
-			if(Demod.posCount == 0) { 			// first half of bit
+			if (Demod.posCount == 0) { 			// first half of bit
 				Demod.thisBit = v;
 				Demod.posCount = 1;
 			} else {							// second half of bit
@@ -637,22 +641,23 @@ static RAMFUNC int Handle14443bSamplesDemod(int ci, int cq)
 */
 
 				Demod.shiftReg >>= 1;
-				if(Demod.thisBit > 0) {	// logic '1'
+				if (Demod.thisBit > 0) {	// logic '1'
 					Demod.shiftReg |= 0x200;
 				}
 
 				Demod.bitCount++;
-				if(Demod.bitCount == 10) {
+				if (Demod.bitCount == 10) {
 					uint16_t s = Demod.shiftReg;
-					if((s & 0x200) && !(s & 0x001)) { // stop bit == '1', start bit == '0'
+					if ((s & 0x200) && !(s & 0x001)) { // stop bit == '1', start bit == '0'
 						uint8_t b = (s >> 1);
 						Demod.output[Demod.len] = b;
 						Demod.len++;
+						Demod.bitCount = 0;
 						Demod.state = DEMOD_AWAITING_START_BIT;
 					} else {
 						Demod.state = DEMOD_UNSYNCD;
 						LED_C_OFF();
-						if(s == 0x000) {
+						if (s == 0x000) {
 							// This is EOF (start, stop and all data bits == '0'
 							return true;
 						}
@@ -693,8 +698,8 @@ static void DemodInit(uint8_t *data)
  *  Demodulate the samples we received from the tag, also log to tracebuffer
  *  quiet: set to 'true' to disable debug output
  */
-static void GetSamplesFor14443bDemod(int timeout, bool quiet)
-{
+static int GetSamplesFor14443bDemod(int timeout, bool quiet) {
+	int ret = 0;
 	int maxBehindBy = 0;
 	bool gotFrame = false;
 	int lastRxCounter, samples = 0;
@@ -750,12 +755,14 @@ static void GetSamplesFor14443bDemod(int timeout, bool quiet)
 		}
 		samples++;
 
-		if(Handle14443bSamplesDemod(ci, cq)) {
+		if (Handle14443bSamplesDemod(ci, cq)) {
+			ret = Demod.len;
 			gotFrame = true;
 			break;
 		}
 
 		if(samples > timeout && Demod.state < DEMOD_PHASE_REF_TRAINING) {
+			ret = -1;
 			LED_C_OFF();
 			break;
 		}
@@ -764,10 +771,14 @@ static void GetSamplesFor14443bDemod(int timeout, bool quiet)
 	FpgaDisableSscDma();
 
 	if (!quiet) Dbprintf("max behindby = %d, samples = %d, gotFrame = %d, Demod.len = %d, Demod.sumI = %d, Demod.sumQ = %d", maxBehindBy, samples, gotFrame, Demod.len, Demod.sumI, Demod.sumQ);
-	//Tracing
-	if (Demod.len > 0) {
-		LogTrace(Demod.output, Demod.len, 0, 0, NULL, false);
+
+	if (ret < 0) {
+		return ret;
 	}
+ 	//Tracing
+	LogTrace(Demod.output, Demod.len, 0, 0, NULL, false);
+
+	return ret;
 }
 
 
@@ -858,8 +869,7 @@ static void CodeAndTransmit14443bAsReader(const uint8_t *cmd, int len)
 /* Sends an APDU to the tag
  * TODO: check CRC and preamble
  */
-int iso14443b_apdu(uint8_t const *message, size_t message_length, uint8_t *response)
-{
+int iso14443b_apdu(uint8_t const *message, size_t message_length, uint8_t *response) {
 	LED_A_ON();
 	uint8_t message_frame[message_length + 4];
 	// PCB
@@ -874,21 +884,19 @@ int iso14443b_apdu(uint8_t const *message, size_t message_length, uint8_t *respo
 	// send
 	CodeAndTransmit14443bAsReader(message_frame, message_length + 4);
 	// get response
-	GetSamplesFor14443bDemod(RECEIVE_SAMPLES_TIMEOUT, true);
+	int ret = GetSamplesFor14443bDemod(RECEIVE_SAMPLES_TIMEOUT, true);
 	FpgaDisableTracing();
-	if(Demod.len < 3)
-	{
+	if (ret < 3) {
 		LED_A_OFF();
 		return 0;
 	}
 	// TODO: Check CRC
 	// copy response contents
-	if(response != NULL)
-	{
+	if (response != NULL) {
 		memcpy(response, Demod.output, Demod.len);
 	}
 	LED_A_OFF();
-	return Demod.len;
+	return ret;
 }
 
 /* Perform the ISO 14443 B Card Selection procedure
@@ -907,10 +915,9 @@ int iso14443b_select_card()
 
 	// first, wake up the tag
 	CodeAndTransmit14443bAsReader(wupb, sizeof(wupb));
-	GetSamplesFor14443bDemod(RECEIVE_SAMPLES_TIMEOUT, true);
+	int ret = GetSamplesFor14443bDemod(RECEIVE_SAMPLES_TIMEOUT, true);
 	// ATQB too short?
-	if (Demod.len < 14)
-	{
+	if (ret < 14) {
 		return 2;
 	}
 
@@ -922,10 +929,9 @@ int iso14443b_select_card()
     attrib[7] = Demod.output[10] & 0x0F;
     ComputeCrc14443(CRC_14443_B, attrib, 9, attrib + 9, attrib + 10);
     CodeAndTransmit14443bAsReader(attrib, sizeof(attrib));
-    GetSamplesFor14443bDemod(RECEIVE_SAMPLES_TIMEOUT, true);
+    ret = GetSamplesFor14443bDemod(RECEIVE_SAMPLES_TIMEOUT, true);
     // Answer to ATTRIB too short?
-    if(Demod.len < 3)
-	{
+    if (ret < 3) {
 		return 2;
 	}
 	// reset PCB block number
@@ -985,9 +991,9 @@ void ReadSTMemoryIso14443b(uint32_t dwLast)
 	// First command: wake up the tag using the INITIATE command
 	uint8_t cmd1[] = {0x06, 0x00, 0x97, 0x5b};
 	CodeAndTransmit14443bAsReader(cmd1, sizeof(cmd1));
-	GetSamplesFor14443bDemod(RECEIVE_SAMPLES_TIMEOUT, true);
+	int ret = GetSamplesFor14443bDemod(RECEIVE_SAMPLES_TIMEOUT, true);
 
-	if (Demod.len == 0) {
+	if (ret < 0) {
 		FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 		DbpString("No response from tag");
 		LEDsoff();
@@ -1003,7 +1009,7 @@ void ReadSTMemoryIso14443b(uint32_t dwLast)
 	cmd1[1] = Demod.output[0];
 	ComputeCrc14443(CRC_14443_B, cmd1, 2, &cmd1[2], &cmd1[3]);
 	CodeAndTransmit14443bAsReader(cmd1, sizeof(cmd1));
-	GetSamplesFor14443bDemod(RECEIVE_SAMPLES_TIMEOUT, true);
+	ret = GetSamplesFor14443bDemod(RECEIVE_SAMPLES_TIMEOUT, true);
 	if (Demod.len != 3) {
 		FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 		Dbprintf("Expected 3 bytes from tag, got %d", Demod.len);
@@ -1031,8 +1037,8 @@ void ReadSTMemoryIso14443b(uint32_t dwLast)
 	cmd1[0] = 0x0B;
 	ComputeCrc14443(CRC_14443_B, cmd1, 1 , &cmd1[1], &cmd1[2]);
 	CodeAndTransmit14443bAsReader(cmd1, 3); // Only first three bytes for this one
-	GetSamplesFor14443bDemod(RECEIVE_SAMPLES_TIMEOUT, true);
-	if (Demod.len != 10) {
+	ret = GetSamplesFor14443bDemod(RECEIVE_SAMPLES_TIMEOUT, true);
+	if (ret != 10) {
 		FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 		Dbprintf("Expected 10 bytes from tag, got %d", Demod.len);
 		LEDsoff();
@@ -1062,8 +1068,8 @@ void ReadSTMemoryIso14443b(uint32_t dwLast)
 		cmd1[1] = i;
 		ComputeCrc14443(CRC_14443_B, cmd1, 2, &cmd1[2], &cmd1[3]);
 		CodeAndTransmit14443bAsReader(cmd1, sizeof(cmd1));
-		GetSamplesFor14443bDemod(RECEIVE_SAMPLES_TIMEOUT, true);
-		if (Demod.len != 6) { // Check if we got an answer from the tag
+		ret = GetSamplesFor14443bDemod(RECEIVE_SAMPLES_TIMEOUT, true);
+		if (ret != 6) { // Check if we got an answer from the tag
 			FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 			DbpString("Expected 6 bytes from tag, got less...");
 			LEDsoff();
@@ -1071,7 +1077,7 @@ void ReadSTMemoryIso14443b(uint32_t dwLast)
 		}
 		// The check the CRC of the answer (use cmd1 as temporary variable):
 		ComputeCrc14443(CRC_14443_B, Demod.output, 4, &cmd1[2], &cmd1[3]);
-		if(cmd1[2] != Demod.output[4] || cmd1[3] != Demod.output[5]) {
+		if (cmd1[2] != Demod.output[4] || cmd1[3] != Demod.output[5]) {
 			Dbprintf("CRC Error reading block! Expected: %04x got: %04x",
 					(cmd1[2]<<8)+cmd1[3], (Demod.output[4]<<8)+Demod.output[5]);
 			// Do not return;, let's go on... (we should retry, maybe ?)
@@ -1213,8 +1219,8 @@ void RAMFUNC SnoopIso14443b(void)
 			ReaderIsActive = (Uart.state > STATE_GOT_FALLING_EDGE_OF_SOF);
 		}
 
-		if(!ReaderIsActive && triggered) {						// no need to try decoding tag data if the reader is sending or not yet triggered
-			if(Handle14443bSamplesDemod(ci/2, cq/2)) {
+		if (!ReaderIsActive && triggered) {						// no need to try decoding tag data if the reader is sending or not yet triggered
+			if (Handle14443bSamplesDemod(ci/2, cq/2) >= 0) {
 				//Use samples as a time measurement
 				LogTrace(Demod.output, Demod.len, samples, samples, NULL, false);
 				// And ready to receive another response.
@@ -1265,17 +1271,17 @@ void SendRawCommand14443B(uint32_t datalen, uint32_t recv, uint8_t powerfield, u
 		
 		CodeAndTransmit14443bAsReader(data, datalen);
 
-		if(recv) {
-			GetSamplesFor14443bDemod(RECEIVE_SAMPLES_TIMEOUT, true);
+		if (recv) {
+			int ret = GetSamplesFor14443bDemod(5*RECEIVE_SAMPLES_TIMEOUT, true);
 			FpgaDisableTracing();
 			uint16_t iLen = MIN(Demod.len, USB_CMD_DATA_SIZE);
-			cmd_send(CMD_ACK, iLen, 0, 0, Demod.output, iLen);
+			cmd_send(CMD_ACK, ret, 0, 0, Demod.output, iLen);
 		}
 
 		FpgaDisableTracing();
 	}
 
-	if(!powerfield) {
+	if (!powerfield) {
 		FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 		LED_D_OFF();
 	}
diff --git a/armsrc/iso15693.c b/armsrc/iso15693.c
index 85af0859..3b39d576 100644
--- a/armsrc/iso15693.c
+++ b/armsrc/iso15693.c
@@ -64,8 +64,26 @@
 
 #define arraylen(x) (sizeof(x)/sizeof((x)[0]))
 
+// Delays in SSP_CLK ticks.
+// SSP_CLK runs at 13,56MHz / 32 = 423.75kHz when simulating a tag
+#define DELAY_READER_TO_ARM               8
+#define DELAY_ARM_TO_READER               0
+//SSP_CLK runs at 13.56MHz / 4 = 3,39MHz when acting as reader. All values should be multiples of 16
+#define DELAY_TAG_TO_ARM                 32
+#define DELAY_ARM_TO_TAG                 16
+
 static int DEBUG = 0;
 
+
+// specific LogTrace function for ISO15693: the duration needs to be scaled because otherwise it won't fit into a uint16_t
+bool LogTrace_ISO15693(const uint8_t *btBytes, uint16_t iLen, uint32_t timestamp_start, uint32_t timestamp_end, uint8_t *parity, bool readerToTag) {
+	uint32_t duration = timestamp_end - timestamp_start;
+	duration /= 32;
+	timestamp_end = timestamp_start + duration;
+	return LogTrace(btBytes, iLen, timestamp_start, timestamp_end, parity, readerToTag);
+}
+
+
 ///////////////////////////////////////////////////////////////////////
 // ISO 15693 Part 2 - Air Interface
 // This section basically contains transmission and receiving of bits
@@ -84,83 +102,36 @@ static int DEBUG = 0;
 // resulting data rate is 26.48 kbit/s (fc/512)
 // cmd ... data
 // n ... length of data
-static void CodeIso15693AsReader(uint8_t *cmd, int n)
-{
-	int i, j;
+void CodeIso15693AsReader(uint8_t *cmd, int n) {
 
 	ToSendReset();
 
-	// Give it a bit of slack at the beginning
-	for(i = 0; i < 24; i++) {
-		ToSendStuffBit(1);
-	}
-
 	// SOF for 1of4
-	ToSendStuffBit(0);
-	ToSendStuffBit(1);
-	ToSendStuffBit(1);
-	ToSendStuffBit(1);
-	ToSendStuffBit(1);
-	ToSendStuffBit(0);
-	ToSendStuffBit(1);
-	ToSendStuffBit(1);
-	for(i = 0; i < n; i++) {
-		for(j = 0; j < 8; j += 2) {
-			int these = (cmd[i] >> j) & 3;
+	ToSend[++ToSendMax] = 0x84; //10000100
+
+	// data
+	for (int i = 0; i < n; i++) {
+		for (int j = 0; j < 8; j += 2) {
+			int these = (cmd[i] >> j) & 0x03;
 			switch(these) {
 				case 0:
-					ToSendStuffBit(1);
-					ToSendStuffBit(0);
-					ToSendStuffBit(1);
-					ToSendStuffBit(1);
-					ToSendStuffBit(1);
-					ToSendStuffBit(1);
-					ToSendStuffBit(1);
-					ToSendStuffBit(1);
+					ToSend[++ToSendMax] = 0x40; //01000000
 					break;
 				case 1:
-					ToSendStuffBit(1);
-					ToSendStuffBit(1);
-					ToSendStuffBit(1);
-					ToSendStuffBit(0);
-					ToSendStuffBit(1);
-					ToSendStuffBit(1);
-					ToSendStuffBit(1);
-					ToSendStuffBit(1);
+					ToSend[++ToSendMax] = 0x10; //00010000
 					break;
 				case 2:
-					ToSendStuffBit(1);
-					ToSendStuffBit(1);
-					ToSendStuffBit(1);
-					ToSendStuffBit(1);
-					ToSendStuffBit(1);
-					ToSendStuffBit(0);
-					ToSendStuffBit(1);
-					ToSendStuffBit(1);
+					ToSend[++ToSendMax] = 0x04; //00000100
 					break;
 				case 3:
-					ToSendStuffBit(1);
-					ToSendStuffBit(1);
-					ToSendStuffBit(1);
-					ToSendStuffBit(1);
-					ToSendStuffBit(1);
-					ToSendStuffBit(1);
-					ToSendStuffBit(1);
-					ToSendStuffBit(0);
+					ToSend[++ToSendMax] = 0x01; //00000001
 					break;
 			}
 		}
 	}
+
 	// EOF
-	ToSendStuffBit(1);
-	ToSendStuffBit(1);
-	ToSendStuffBit(0);
-	ToSendStuffBit(1);
-
-	// Fill remainder of last byte with 1
-	for(i = 0; i < 4; i++) {
-		ToSendStuffBit(1);
-	}
+	ToSend[++ToSendMax] = 0x20; //0010 + 0000 padding
 
 	ToSendMax++;
 }
@@ -170,46 +141,26 @@ static void CodeIso15693AsReader(uint8_t *cmd, int n)
 // is designed for more robust communication over longer distances
 static void CodeIso15693AsReader256(uint8_t *cmd, int n)
 {
-	int i, j;
-
 	ToSendReset();
 
-	// Give it a bit of slack at the beginning
-	for(i = 0; i < 24; i++) {
-		ToSendStuffBit(1);
-	}
-
 	// SOF for 1of256
-	ToSendStuffBit(0);
-	ToSendStuffBit(1);
-	ToSendStuffBit(1);
-	ToSendStuffBit(1);
-	ToSendStuffBit(1);
-	ToSendStuffBit(1);
-	ToSendStuffBit(1);
-	ToSendStuffBit(0);
-
-	for(i = 0; i < n; i++) {
-		for (j = 0; j<=255; j++) {
-			if (cmd[i]==j) {
-				ToSendStuffBit(1);
+	ToSend[++ToSendMax] = 0x81; //10000001
+
+	// data
+	for(int i = 0; i < n; i++) {
+		for (int j = 0; j <= 255; j++) {
+			if (cmd[i] == j) {
 				ToSendStuffBit(0);
-			} else {
-				ToSendStuffBit(1);
 				ToSendStuffBit(1);
+			} else {
+				ToSendStuffBit(0);
+				ToSendStuffBit(0);
 			}
 		}
 	}
+
 	// EOF
-	ToSendStuffBit(1);
-	ToSendStuffBit(1);
-	ToSendStuffBit(0);
-	ToSendStuffBit(1);
-
-	// Fill remainder of last byte with 1
-	for(i = 0; i < 4; i++) {
-		ToSendStuffBit(1);
-	}
+	ToSend[++ToSendMax] = 0x20; //0010 + 0000 padding
 
 	ToSendMax++;
 }
@@ -295,27 +246,42 @@ void CodeIso15693AsTag(uint8_t *cmd, size_t len) {
 
 
 // Transmit the command (to the tag) that was placed in cmd[].
-static void TransmitTo15693Tag(const uint8_t *cmd, int len, uint32_t start_time)
-{
+void TransmitTo15693Tag(const uint8_t *cmd, int len, uint32_t *start_time) {
+
 	FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_READER | FPGA_HF_READER_MODE_SEND_FULL_MOD);
-	FpgaSetupSsc(FPGA_MAJOR_MODE_HF_READER);
 
-	while (GetCountSspClk() < start_time) ;
+	if (*start_time < DELAY_ARM_TO_TAG) {
+		*start_time = DELAY_ARM_TO_TAG;
+	}
+
+	*start_time = (*start_time - DELAY_ARM_TO_TAG) & 0xfffffff0;
+
+	while (GetCountSspClk() > *start_time) { // we may miss the intended time
+		*start_time += 16; // next possible time
+	}
+
+
+	while (GetCountSspClk() < *start_time)
+		/* wait */ ;
 
 	LED_B_ON();
-	for(int c = 0; c < len; c++) {
+	for (int c = 0; c < len; c++) {
 		uint8_t data = cmd[c];
 		for (int i = 0; i < 8; i++) {
-			uint16_t send_word = (data & 0x80) ? 0x0000 : 0xffff;
+			uint16_t send_word = (data & 0x80) ? 0xffff : 0x0000;
 			while (!(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY))) ;
 			AT91C_BASE_SSC->SSC_THR = send_word;
 			while (!(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY))) ;
 			AT91C_BASE_SSC->SSC_THR = send_word;
+
 			data <<= 1;
 		}
 		WDT_HIT();
 	}
 	LED_B_OFF();
+
+	*start_time = *start_time + DELAY_ARM_TO_TAG;
+
 }
 
 
@@ -326,8 +292,8 @@ void TransmitTo15693Reader(const uint8_t *cmd, size_t len, uint32_t *start_time,
 	// don't use the FPGA_HF_SIMULATOR_MODULATE_424K_8BIT minor mode. It would spoil GetCountSspClk()
 	FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_SIMULATOR | FPGA_HF_SIMULATOR_MODULATE_424K);
 
-	uint32_t modulation_start_time = *start_time + 3 * 8;  // no need to transfer the unmodulated start of SOF
-	
+	uint32_t modulation_start_time = *start_time - DELAY_ARM_TO_READER + 3 * 8;  // no need to transfer the unmodulated start of SOF
+
 	while (GetCountSspClk() > (modulation_start_time & 0xfffffff8) + 3) { // we will miss the intended time
 		if (slot_time) {
 			modulation_start_time += slot_time; // use next available slot
@@ -336,12 +302,12 @@ void TransmitTo15693Reader(const uint8_t *cmd, size_t len, uint32_t *start_time,
 		}
 	}
 
-	while (GetCountSspClk() < (modulation_start_time & 0xfffffff8)) 
+	while (GetCountSspClk() < (modulation_start_time & 0xfffffff8))
 		/* wait */ ;
 
 	uint8_t shift_delay = modulation_start_time & 0x00000007;
 
-	*start_time = modulation_start_time - 3 * 8;
+	*start_time = modulation_start_time + DELAY_ARM_TO_READER - 3 * 8;
 
 	LED_C_ON();
 	uint8_t bits_to_shift = 0x00;
@@ -386,15 +352,18 @@ void TransmitTo15693Reader(const uint8_t *cmd, size_t len, uint32_t *start_time,
 //          false if we are still waiting for some more
 //=============================================================================
 
-#define NOISE_THRESHOLD    160      // don't try to correlate noise
+#define NOISE_THRESHOLD          160                   // don't try to correlate noise
+#define MAX_PREVIOUS_AMPLITUDE   (-1 - NOISE_THRESHOLD)
 
 typedef struct DecodeTag {
 	enum {
 		STATE_TAG_SOF_LOW,
+		STATE_TAG_SOF_RISING_EDGE,
 		STATE_TAG_SOF_HIGH,
 		STATE_TAG_SOF_HIGH_END,
 		STATE_TAG_RECEIVING_DATA,
-		STATE_TAG_EOF
+		STATE_TAG_EOF,
+		STATE_TAG_EOF_TAIL
 	}         state;
 	int       bitCount;
 	int       posCount;
@@ -409,6 +378,9 @@ typedef struct DecodeTag {
 	uint8_t   *output;
 	int       len;
 	int       sum1, sum2;
+	int       threshold_sof;
+	int       threshold_half;
+	uint16_t  previous_amplitude;
 } DecodeTag_t;
 
 
@@ -416,40 +388,58 @@ static int inline __attribute__((always_inline)) Handle15693SamplesFromTag(uint1
 {
 	switch(DecodeTag->state) {
 		case STATE_TAG_SOF_LOW:
-			// waiting for 12 times low (11 times low is accepted as well)
-			if (amplitude < NOISE_THRESHOLD) {
-				DecodeTag->posCount++;
-			} else {
+			// waiting for a rising edge
+			if (amplitude > NOISE_THRESHOLD + DecodeTag->previous_amplitude) {
 				if (DecodeTag->posCount > 10) {
-					DecodeTag->posCount = 1;
-					DecodeTag->sum1 = 0;
-					DecodeTag->state = STATE_TAG_SOF_HIGH;
+					DecodeTag->threshold_sof = amplitude - DecodeTag->previous_amplitude;
+					DecodeTag->threshold_half = 0;
+					DecodeTag->state = STATE_TAG_SOF_RISING_EDGE;
 				} else {
 					DecodeTag->posCount = 0;
 				}
+			} else {
+				DecodeTag->posCount++;
+				DecodeTag->previous_amplitude = amplitude;
+			}
+			break;
+
+		case STATE_TAG_SOF_RISING_EDGE:
+			if (amplitude - DecodeTag->previous_amplitude > DecodeTag->threshold_sof) { // edge still rising
+				if (amplitude - DecodeTag->threshold_sof > DecodeTag->threshold_sof) { // steeper edge, take this as time reference
+					DecodeTag->posCount = 1;
+				} else {
+					DecodeTag->posCount = 2;
+				}
+				DecodeTag->threshold_sof = (amplitude - DecodeTag->previous_amplitude) / 2;
+			} else {
+				DecodeTag->posCount = 2;
+				DecodeTag->threshold_sof = DecodeTag->threshold_sof/2;
 			}
+			// DecodeTag->posCount = 2;
+			DecodeTag->state = STATE_TAG_SOF_HIGH;
 			break;
 
 		case STATE_TAG_SOF_HIGH:
 			// waiting for 10 times high. Take average over the last 8
-			if (amplitude > NOISE_THRESHOLD) {
+			if (amplitude > DecodeTag->threshold_sof) {
 				DecodeTag->posCount++;
 				if (DecodeTag->posCount > 2) {
-					DecodeTag->sum1 += amplitude; // keep track of average high value
+					DecodeTag->threshold_half += amplitude; // keep track of average high value
 				}
 				if (DecodeTag->posCount == 10) {
-					DecodeTag->sum1 >>= 4;        // calculate half of average high value (8 samples)
+					DecodeTag->threshold_half >>= 2; // (4 times 1/2 average)
 					DecodeTag->state = STATE_TAG_SOF_HIGH_END;
 				}
 			} else { // high phase was too short
 				DecodeTag->posCount = 1;
+				DecodeTag->previous_amplitude = amplitude;
 				DecodeTag->state = STATE_TAG_SOF_LOW;
 			}
 			break;
 
 		case STATE_TAG_SOF_HIGH_END:
-			// waiting for a falling edge
-			if (amplitude < DecodeTag->sum1) {   // signal drops below 50% average high: a falling edge
+			// check for falling edge
+			if (DecodeTag->posCount == 13 && amplitude < DecodeTag->threshold_sof) {
 				DecodeTag->lastBit = SOF_PART1;  // detected 1st part of SOF (12 samples low and 12 samples high)
 				DecodeTag->shiftReg = 0;
 				DecodeTag->bitCount = 0;
@@ -458,11 +448,18 @@ static int inline __attribute__((always_inline)) Handle15693SamplesFromTag(uint1
 				DecodeTag->sum2 = 0;
 				DecodeTag->posCount = 2;
 				DecodeTag->state = STATE_TAG_RECEIVING_DATA;
+				FpgaDisableTracing(); // DEBUGGING
+				Dbprintf("amplitude = %d, threshold_sof = %d, threshold_half/4 = %d, previous_amplitude = %d",
+					amplitude,
+					DecodeTag->threshold_sof,
+					DecodeTag->threshold_half/4,
+					DecodeTag->previous_amplitude); // DEBUGGING
 				LED_C_ON();
 			} else {
 				DecodeTag->posCount++;
 				if (DecodeTag->posCount > 13) { // high phase too long
 					DecodeTag->posCount = 0;
+					DecodeTag->previous_amplitude = amplitude;
 					DecodeTag->state = STATE_TAG_SOF_LOW;
 					LED_C_OFF();
 				}
@@ -480,18 +477,16 @@ static int inline __attribute__((always_inline)) Handle15693SamplesFromTag(uint1
 				DecodeTag->sum2 += amplitude;
 			}
 			if (DecodeTag->posCount == 8) {
-				int32_t corr_1 = DecodeTag->sum2 - DecodeTag->sum1;
-				int32_t corr_0 = -corr_1;
-				int32_t corr_EOF = (DecodeTag->sum1 + DecodeTag->sum2) / 2;
-				if (corr_EOF > corr_0 && corr_EOF > corr_1) {
+				if (DecodeTag->sum1 > DecodeTag->threshold_half && DecodeTag->sum2 > DecodeTag->threshold_half) { // modulation in both halves
 					if (DecodeTag->lastBit == LOGIC0) {  // this was already part of EOF
 						DecodeTag->state = STATE_TAG_EOF;
 					} else {
 						DecodeTag->posCount = 0;
+						DecodeTag->previous_amplitude = amplitude;
 						DecodeTag->state = STATE_TAG_SOF_LOW;
 						LED_C_OFF();
 					}
-				} else if (corr_1 > corr_0) {
+				} else if (DecodeTag->sum1 < DecodeTag->threshold_half && DecodeTag->sum2 > DecodeTag->threshold_half) { // modulation in second half
 					// logic 1
 					if (DecodeTag->lastBit == SOF_PART1) { // still part of SOF
 						DecodeTag->lastBit = SOF_PART2;    // SOF completed
@@ -503,20 +498,21 @@ static int inline __attribute__((always_inline)) Handle15693SamplesFromTag(uint1
 						if (DecodeTag->bitCount == 8) {
 							DecodeTag->output[DecodeTag->len] = DecodeTag->shiftReg;
 							DecodeTag->len++;
+							// if (DecodeTag->shiftReg == 0x12 && DecodeTag->len == 1) FpgaDisableTracing(); // DEBUGGING
 							if (DecodeTag->len > DecodeTag->max_len) {
 								// buffer overflow, give up
-								DecodeTag->posCount = 0;
-								DecodeTag->state = STATE_TAG_SOF_LOW;
 								LED_C_OFF();
+								return true;
 							}
 							DecodeTag->bitCount = 0;
 							DecodeTag->shiftReg = 0;
 						}
 					}
-				} else {
+				} else if (DecodeTag->sum1 > DecodeTag->threshold_half && DecodeTag->sum2 < DecodeTag->threshold_half) { // modulation in first half
 					// logic 0
 					if (DecodeTag->lastBit == SOF_PART1) { // incomplete SOF
 						DecodeTag->posCount = 0;
+						DecodeTag->previous_amplitude = amplitude;
 						DecodeTag->state = STATE_TAG_SOF_LOW;
 						LED_C_OFF();
 					} else {
@@ -526,9 +522,11 @@ static int inline __attribute__((always_inline)) Handle15693SamplesFromTag(uint1
 						if (DecodeTag->bitCount == 8) {
 							DecodeTag->output[DecodeTag->len] = DecodeTag->shiftReg;
 							DecodeTag->len++;
+							// if (DecodeTag->shiftReg == 0x12 && DecodeTag->len == 1) FpgaDisableTracing(); // DEBUGGING
 							if (DecodeTag->len > DecodeTag->max_len) {
 								// buffer overflow, give up
 								DecodeTag->posCount = 0;
+								DecodeTag->previous_amplitude = amplitude;
 								DecodeTag->state = STATE_TAG_SOF_LOW;
 								LED_C_OFF();
 							}
@@ -536,6 +534,15 @@ static int inline __attribute__((always_inline)) Handle15693SamplesFromTag(uint1
 							DecodeTag->shiftReg = 0;
 						}
 					}
+				} else { // no modulation
+					if (DecodeTag->lastBit == SOF_PART2) { // only SOF (this is OK for iClass)
+						LED_C_OFF();
+						return true;
+					} else {
+						DecodeTag->posCount = 0;
+						DecodeTag->state = STATE_TAG_SOF_LOW;
+						LED_C_OFF();
+					}
 				}
 				DecodeTag->posCount = 0;
 			}
@@ -553,29 +560,50 @@ static int inline __attribute__((always_inline)) Handle15693SamplesFromTag(uint1
 				DecodeTag->sum2 += amplitude;
 			}
 			if (DecodeTag->posCount == 8) {
-				int32_t corr_1 = DecodeTag->sum2 - DecodeTag->sum1;
-				int32_t corr_0 = -corr_1;
-				int32_t corr_EOF = (DecodeTag->sum1 + DecodeTag->sum2) / 2;
-				if (corr_EOF > corr_0 || corr_1 > corr_0) {
+				if (DecodeTag->sum1 > DecodeTag->threshold_half && DecodeTag->sum2 < DecodeTag->threshold_half) { // modulation in first half
 					DecodeTag->posCount = 0;
+					DecodeTag->state = STATE_TAG_EOF_TAIL;
+				} else {
+					DecodeTag->posCount = 0;
+					DecodeTag->previous_amplitude = amplitude;
 					DecodeTag->state = STATE_TAG_SOF_LOW;
 					LED_C_OFF();
-				} else {
+				}
+			}
+			DecodeTag->posCount++;
+			break;
+
+		case STATE_TAG_EOF_TAIL:
+			if (DecodeTag->posCount == 1) {
+				DecodeTag->sum1 = 0;
+				DecodeTag->sum2 = 0;
+			}
+			if (DecodeTag->posCount <= 4) {
+				DecodeTag->sum1 += amplitude;
+			} else {
+				DecodeTag->sum2 += amplitude;
+			}
+			if (DecodeTag->posCount == 8) {
+				if (DecodeTag->sum1 < DecodeTag->threshold_half && DecodeTag->sum2 < DecodeTag->threshold_half) { // no modulation in both halves
 					LED_C_OFF();
 					return true;
+				} else {
+					DecodeTag->posCount = 0;
+					DecodeTag->previous_amplitude = amplitude;
+					DecodeTag->state = STATE_TAG_SOF_LOW;
+					LED_C_OFF();
 				}
 			}
 			DecodeTag->posCount++;
 			break;
-
 	}
 
 	return false;
 }
 
 
-static void DecodeTagInit(DecodeTag_t *DecodeTag, uint8_t *data, uint16_t max_len)
-{
+static void DecodeTagInit(DecodeTag_t *DecodeTag, uint8_t *data, uint16_t max_len) {
+	DecodeTag->previous_amplitude = MAX_PREVIOUS_AMPLITUDE;
 	DecodeTag->posCount = 0;
 	DecodeTag->state = STATE_TAG_SOF_LOW;
 	DecodeTag->output = data;
@@ -583,22 +611,22 @@ static void DecodeTagInit(DecodeTag_t *DecodeTag, uint8_t *data, uint16_t max_le
 }
 
 
-static void DecodeTagReset(DecodeTag_t *DecodeTag)
-{
+static void DecodeTagReset(DecodeTag_t *DecodeTag) {
 	DecodeTag->posCount = 0;
 	DecodeTag->state = STATE_TAG_SOF_LOW;
+	DecodeTag->previous_amplitude = MAX_PREVIOUS_AMPLITUDE;
 }
 
 
 /*
  *  Receive and decode the tag response, also log to tracebuffer
  */
-static int GetIso15693AnswerFromTag(uint8_t* response, uint16_t max_len, int timeout)
-{
+int GetIso15693AnswerFromTag(uint8_t* response, uint16_t max_len, uint16_t timeout, uint32_t *eof_time) {
+
 	int samples = 0;
-	bool gotFrame = false;
+	int ret = 0;
 
-	uint16_t *dmaBuf = (uint16_t*)BigBuf_malloc(ISO15693_DMA_BUFFER_SIZE*sizeof(uint16_t));
+	uint16_t dmaBuf[ISO15693_DMA_BUFFER_SIZE];
 
 	// the Decoder data structure
 	DecodeTag_t DecodeTag = { 0 };
@@ -613,6 +641,7 @@ static int GetIso15693AnswerFromTag(uint8_t* response, uint16_t max_len, int tim
 	// Setup and start DMA.
 	FpgaSetupSsc(FPGA_MAJOR_MODE_HF_READER);
 	FpgaSetupSscDma((uint8_t*) dmaBuf, ISO15693_DMA_BUFFER_SIZE);
+	uint32_t dma_start_time = 0;
 	uint16_t *upTo = dmaBuf;
 
 	for(;;) {
@@ -620,12 +649,19 @@ static int GetIso15693AnswerFromTag(uint8_t* response, uint16_t max_len, int tim
 
 		if (behindBy == 0) continue;
 
+		samples++;
+		if (samples == 1) {
+			// DMA has transferred the very first data
+			dma_start_time = GetCountSspClk() & 0xfffffff0;
+		}
+
 		uint16_t tagdata = *upTo++;
 
 		if(upTo >= dmaBuf + ISO15693_DMA_BUFFER_SIZE) {                // we have read all of the DMA buffer content.
 			upTo = dmaBuf;                                             // start reading the circular buffer from the beginning
 			if(behindBy > (9*ISO15693_DMA_BUFFER_SIZE/10)) {
 				Dbprintf("About to blow circular buffer - aborted! behindBy=%d", behindBy);
+				ret = -1;
 				break;
 			}
 		}
@@ -634,30 +670,42 @@ static int GetIso15693AnswerFromTag(uint8_t* response, uint16_t max_len, int tim
 			AT91C_BASE_PDC_SSC->PDC_RNCR = ISO15693_DMA_BUFFER_SIZE;   // DMA Next Counter registers
 		}
 
-		samples++;
-
 		if (Handle15693SamplesFromTag(tagdata, &DecodeTag)) {
-			gotFrame = true;
+			*eof_time = dma_start_time + samples*16 - DELAY_TAG_TO_ARM; // end of EOF
+			if (DecodeTag.lastBit == SOF_PART2) {
+				*eof_time -= 8*16; // needed 8 additional samples to confirm single SOF (iCLASS)
+			}
+			if (DecodeTag.len > DecodeTag.max_len) {
+				ret = -2; // buffer overflow
+			}
 			break;
 		}
 
 		if (samples > timeout && DecodeTag.state < STATE_TAG_RECEIVING_DATA) {
-			DecodeTag.len = 0;
+			ret = -1;   // timeout
 			break;
 		}
 
 	}
 
 	FpgaDisableSscDma();
-	BigBuf_free();
 
-	if (DEBUG) Dbprintf("samples = %d, gotFrame = %d, Decoder: state = %d, len = %d, bitCount = %d, posCount = %d",
-						samples, gotFrame, DecodeTag.state, DecodeTag.len, DecodeTag.bitCount, DecodeTag.posCount);
+	if (DEBUG) Dbprintf("samples = %d, ret = %d, Decoder: state = %d, lastBit = %d, len = %d, bitCount = %d, posCount = %d",
+						samples, ret, DecodeTag.state, DecodeTag.lastBit, DecodeTag.len, DecodeTag.bitCount, DecodeTag.posCount);
 
-	if (DecodeTag.len > 0) {
-		LogTrace(DecodeTag.output, DecodeTag.len, 0, 0, NULL, false);
+	if (ret < 0) {
+		return ret;
 	}
 
+	uint32_t sof_time = *eof_time
+						- DecodeTag.len * 8 * 8 * 16 // time for byte transfers
+						- 32 * 16  // time for SOF transfer
+						- (DecodeTag.lastBit != SOF_PART2?32*16:0); // time for EOF transfer
+
+	if (DEBUG) Dbprintf("timing: sof_time = %d, eof_time = %d", sof_time, *eof_time);
+
+	LogTrace_ISO15693(DecodeTag.output, DecodeTag.len, sof_time*4, *eof_time*4, NULL, false);
+
 	return DecodeTag.len;
 }
 
@@ -977,7 +1025,7 @@ int GetIso15693CommandFromReader(uint8_t *received, size_t max_len, uint32_t *eo
 
 		for (int i = 7; i >= 0; i--) {
 			if (Handle15693SampleFromReader((b >> i) & 0x01, &DecodeReader)) {
-				*eof_time = dma_start_time + samples - DELAY_READER_TO_ARM_SIM; // end of EOF
+				*eof_time = dma_start_time + samples - DELAY_READER_TO_ARM; // end of EOF
 				gotFrame = true;
 				break;
 			}
@@ -1006,7 +1054,7 @@ int GetIso15693CommandFromReader(uint8_t *received, size_t max_len, uint32_t *eo
 						- DecodeReader.byteCount * (DecodeReader.Coding==CODING_1_OUT_OF_4?128:2048) // time for byte transfers
 						- 32  // time for SOF transfer
 						- 16; // time for EOF transfer
-		LogTrace(DecodeReader.output, DecodeReader.byteCount, sof_time, *eof_time, NULL, true);
+		LogTrace_ISO15693(DecodeReader.output, DecodeReader.byteCount, sof_time*32, *eof_time*32, NULL, true);
 	}
 
 	return DecodeReader.byteCount;
@@ -1043,24 +1091,24 @@ static void BuildIdentifyRequest(void)
 //-----------------------------------------------------------------------------
 void AcquireRawAdcSamplesIso15693(void)
 {
-	LEDsoff();
 	LED_A_ON();
 
 	uint8_t *dest = BigBuf_get_addr();
 
 	FpgaDownloadAndGo(FPGA_BITSTREAM_HF);
 	FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_READER);
+	LED_D_ON();
 	FpgaSetupSsc(FPGA_MAJOR_MODE_HF_READER);
 	SetAdcMuxFor(GPIO_MUXSEL_HIPKD);
 
 	BuildIdentifyRequest();
 
 	// Give the tags time to energize
-	LED_D_ON();
 	SpinDelay(100);
 
 	// Now send the command
-	TransmitTo15693Tag(ToSend, ToSendMax, 0);
+	uint32_t start_time = 0;
+	TransmitTo15693Tag(ToSend, ToSendMax, &start_time);
 
 	// wait for last transfer to complete
 	while (!(AT91C_BASE_SSC->SSC_SR & AT91C_SSC_TXEMPTY)) ;
@@ -1082,6 +1130,7 @@ void AcquireRawAdcSamplesIso15693(void)
 void SnoopIso15693(void)
 {
 	LED_A_ON();
+	
 	FpgaDownloadAndGo(FPGA_BITSTREAM_HF);
 	BigBuf_free();
 
@@ -1156,7 +1205,7 @@ void SnoopIso15693(void)
 			if (Handle15693SampleFromReader(snoopdata & 0x02, &DecodeReader)) {
 				FpgaDisableSscDma();
 				ExpectTagAnswer = true;
-				LogTrace(DecodeReader.output, DecodeReader.byteCount, samples, samples, NULL, true);
+				LogTrace_ISO15693(DecodeReader.output, DecodeReader.byteCount, samples*64, samples*64, NULL, true);
 				/* And ready to receive another command. */
 				DecodeReaderReset(&DecodeReader);
 				/* And also reset the demod code, which might have been */
@@ -1168,7 +1217,7 @@ void SnoopIso15693(void)
 			if (Handle15693SampleFromReader(snoopdata & 0x01, &DecodeReader)) {
 				FpgaDisableSscDma();
 				ExpectTagAnswer = true;
-				LogTrace(DecodeReader.output, DecodeReader.byteCount, samples, samples, NULL, true);
+				LogTrace_ISO15693(DecodeReader.output, DecodeReader.byteCount, samples*64, samples*64, NULL, true);
 				/* And ready to receive another command. */
 				DecodeReaderReset(&DecodeReader);
 				/* And also reset the demod code, which might have been */
@@ -1184,7 +1233,7 @@ void SnoopIso15693(void)
 			if (Handle15693SamplesFromTag(snoopdata >> 2, &DecodeTag)) {
 				FpgaDisableSscDma();
 				//Use samples as a time measurement
-				LogTrace(DecodeTag.output, DecodeTag.len, samples, samples, NULL, false);
+				LogTrace_ISO15693(DecodeTag.output, DecodeTag.len, samples*64, samples*64, NULL, false);
 				// And ready to receive another response.
 				DecodeTagReset(&DecodeTag);
 				DecodeReaderReset(&DecodeReader);
@@ -1213,10 +1262,8 @@ void SnoopIso15693(void)
 
 
 // Initialize the proxmark as iso15k reader
-static void Iso15693InitReader() {
+void Iso15693InitReader() {
 	FpgaDownloadAndGo(FPGA_BITSTREAM_HF);
-	// Setup SSC
-	// FpgaSetupSsc();
 
 	// Start from off (no field generated)
 	LED_D_OFF();
@@ -1300,16 +1347,15 @@ static void BuildInventoryResponse(uint8_t *uid)
 //  init ... should we initialize the reader?
 //  speed ... 0 low speed, 1 hi speed
 //  *recv will contain the tag's answer
-//  return: lenght of received data
-int SendDataTag(uint8_t *send, int sendlen, bool init, int speed, uint8_t *recv, uint16_t max_recv_len, uint32_t start_time) {
-
-	LED_A_ON();
-	LED_B_OFF();
-	LED_C_OFF();
+//  return: length of received data, or -1 for timeout
+int SendDataTag(uint8_t *send, int sendlen, bool init, int speed, uint8_t *recv, uint16_t max_recv_len, uint32_t start_time, uint32_t *eof_time) {
 
-	if (init) Iso15693InitReader();
+	if (init) {
+		Iso15693InitReader();
+		StartCountSspClk();
+	}
 
-	int answerLen=0;
+	int answerLen = 0;
 
 	if (!speed) {
 		// low speed (1 out of 256)
@@ -1319,15 +1365,13 @@ int SendDataTag(uint8_t *send, int sendlen, bool init, int speed, uint8_t *recv,
 		CodeIso15693AsReader(send, sendlen);
 	}
 
-	TransmitTo15693Tag(ToSend, ToSendMax, start_time);
+	TransmitTo15693Tag(ToSend, ToSendMax, &start_time);
 
 	// Now wait for a response
 	if (recv != NULL) {
-		answerLen = GetIso15693AnswerFromTag(recv, max_recv_len, DELAY_ISO15693_VCD_TO_VICC_READER * 2);
+		answerLen = GetIso15693AnswerFromTag(recv, max_recv_len, ISO15693_READER_TIMEOUT, eof_time);
 	}
 
-	LED_A_OFF();
-
 	return answerLen;
 }
 
@@ -1411,9 +1455,8 @@ void SetDebugIso15693(uint32_t debug) {
 // Simulate an ISO15693 reader, perform anti-collision and then attempt to read a sector.
 // all demodulation performed in arm rather than host. - greg
 //---------------------------------------------------------------------------------------
-void ReaderIso15693(uint32_t parameter)
-{
-	LEDsoff();
+void ReaderIso15693(uint32_t parameter) {
+
 	LED_A_ON();
 
 	set_tracing(true);
@@ -1445,11 +1488,13 @@ void ReaderIso15693(uint32_t parameter)
 
 	// Now send the IDENTIFY command
 	BuildIdentifyRequest();
-	TransmitTo15693Tag(ToSend, ToSendMax, 0);
+	uint32_t start_time = 0;
+	TransmitTo15693Tag(ToSend, ToSendMax, &start_time);
 
 	// Now wait for a response
-	answerLen = GetIso15693AnswerFromTag(answer, sizeof(answer), DELAY_ISO15693_VCD_TO_VICC_READER * 2) ;
-	uint32_t start_time = GetCountSspClk() + DELAY_ISO15693_VICC_TO_VCD_READER;
+	uint32_t eof_time;
+	answerLen = GetIso15693AnswerFromTag(answer, sizeof(answer), DELAY_ISO15693_VCD_TO_VICC_READER * 2, &eof_time) ;
+	start_time = eof_time + DELAY_ISO15693_VICC_TO_VCD_READER;
 
 	if (answerLen >=12) // we should do a better check than this
 	{
@@ -1487,9 +1532,9 @@ void ReaderIso15693(uint32_t parameter)
 	if (answerLen >= 12 && DEBUG) {
 		for (int i = 0; i < 32; i++) {  // sanity check, assume max 32 pages
 			BuildReadBlockRequest(TagUID, i);
-			TransmitTo15693Tag(ToSend, ToSendMax, start_time);
-			int answerLen = GetIso15693AnswerFromTag(answer, sizeof(answer), DELAY_ISO15693_VCD_TO_VICC_READER * 2);
-			start_time = GetCountSspClk() + DELAY_ISO15693_VICC_TO_VCD_READER;
+			TransmitTo15693Tag(ToSend, ToSendMax, &start_time);
+			int answerLen = GetIso15693AnswerFromTag(answer, sizeof(answer), DELAY_ISO15693_VCD_TO_VICC_READER * 2, &eof_time);
+			start_time = eof_time + DELAY_ISO15693_VICC_TO_VCD_READER;
 			if (answerLen > 0) {
 				Dbprintf("READ SINGLE BLOCK %d returned %d octets:", i, answerLen);
 				DbdecodeIso15693Answer(answerLen, answer);
@@ -1511,9 +1556,8 @@ void ReaderIso15693(uint32_t parameter)
 // Simulate an ISO15693 TAG.
 // For Inventory command: print command and send Inventory Response with given UID
 // TODO: interpret other reader commands and send appropriate response
-void SimTagIso15693(uint32_t parameter, uint8_t *uid)
-{
-	LEDsoff();
+void SimTagIso15693(uint32_t parameter, uint8_t *uid) {
+
 	LED_A_ON();
 
 	FpgaDownloadAndGo(FPGA_BITSTREAM_HF);
@@ -1535,7 +1579,7 @@ void SimTagIso15693(uint32_t parameter, uint8_t *uid)
 
 		if ((cmd_len >= 5) && (cmd[0] & ISO15693_REQ_INVENTORY) && (cmd[1] == ISO15693_INVENTORY)) { // TODO: check more flags
 			bool slow = !(cmd[0] & ISO15693_REQ_DATARATE_HIGH);
-			start_time = eof_time + DELAY_ISO15693_VCD_TO_VICC_SIM - DELAY_ARM_TO_READER_SIM;
+			start_time = eof_time + DELAY_ISO15693_VCD_TO_VICC_SIM;
 			TransmitTo15693Reader(ToSend, ToSendMax, &start_time, 0, slow);
 		}
 
@@ -1544,7 +1588,8 @@ void SimTagIso15693(uint32_t parameter, uint8_t *uid)
 	}
 
 	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
-	LEDsoff();
+	LED_D_OFF();
+	LED_A_OFF();
 }
 
 
@@ -1552,16 +1597,12 @@ void SimTagIso15693(uint32_t parameter, uint8_t *uid)
 // (some manufactures offer a way to read the AFI, though)
 void BruteforceIso15693Afi(uint32_t speed)
 {
-	LEDsoff();
 	LED_A_ON();
 
 	uint8_t data[6];
 	uint8_t recv[ISO15693_MAX_RESPONSE_LENGTH];
-
-	int datalen=0, recvlen=0;
-
-	Iso15693InitReader();
-	StartCountSspClk();
+	int datalen = 0, recvlen = 0;
+	uint32_t eof_time;
 
 	// first without AFI
 	// Tags should respond without AFI and with AFI=0 even when AFI is active
@@ -1570,8 +1611,9 @@ void BruteforceIso15693Afi(uint32_t speed)
 	data[1] = ISO15693_INVENTORY;
 	data[2] = 0; // mask length
 	datalen = Iso15693AddCrc(data,3);
-	recvlen = SendDataTag(data, datalen, false, speed, recv, sizeof(recv), 0);
-	uint32_t start_time = GetCountSspClk() + DELAY_ISO15693_VICC_TO_VCD_READER;
+	uint32_t start_time = GetCountSspClk();
+	recvlen = SendDataTag(data, datalen, true, speed, recv, sizeof(recv), 0, &eof_time);
+	start_time = eof_time + DELAY_ISO15693_VICC_TO_VCD_READER;
 	WDT_HIT();
 	if (recvlen>=12) {
 		Dbprintf("NoAFI UID=%s", Iso15693sprintUID(NULL, &recv[2]));
@@ -1587,8 +1629,8 @@ void BruteforceIso15693Afi(uint32_t speed)
 	for (int i = 0; i < 256; i++) {
 		data[2] = i & 0xFF;
 		datalen = Iso15693AddCrc(data,4);
-		recvlen = SendDataTag(data, datalen, false, speed, recv, sizeof(recv), start_time);
-		start_time = GetCountSspClk() + DELAY_ISO15693_VICC_TO_VCD_READER;
+		recvlen = SendDataTag(data, datalen, false, speed, recv, sizeof(recv), start_time, &eof_time);
+		start_time = eof_time + DELAY_ISO15693_VICC_TO_VCD_READER;
 		WDT_HIT();
 		if (recvlen >= 12) {
 			Dbprintf("AFI=%i UID=%s", i, Iso15693sprintUID(NULL, &recv[2]));
@@ -1597,40 +1639,46 @@ void BruteforceIso15693Afi(uint32_t speed)
 	Dbprintf("AFI Bruteforcing done.");
 
 	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
-	LEDsoff();
+	LED_D_OFF();
+	LED_A_OFF();
+
 }
 
 // Allows to directly send commands to the tag via the client
 void DirectTag15693Command(uint32_t datalen, uint32_t speed, uint32_t recv, uint8_t data[]) {
 
+	LED_A_ON();
+
 	int recvlen = 0;
 	uint8_t recvbuf[ISO15693_MAX_RESPONSE_LENGTH];
-
-	LED_A_ON();
+	uint32_t eof_time;
 
 	if (DEBUG) {
 		Dbprintf("SEND:");
 		Dbhexdump(datalen, data, false);
 	}
 
-	recvlen = SendDataTag(data, datalen, true, speed, (recv?recvbuf:NULL), sizeof(recvbuf), 0);
+	recvlen = SendDataTag(data, datalen, true, speed, (recv?recvbuf:NULL), sizeof(recvbuf), 0, &eof_time);
+
+	// for the time being, switch field off to protect rdv4.0
+	// note: this prevents using hf 15 cmd with s option - which isn't implemented yet anyway
+	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
+	LED_D_OFF();
 
 	if (recv) {
 		if (DEBUG) {
 			Dbprintf("RECV:");
-			Dbhexdump(recvlen, recvbuf, false);
-			DbdecodeIso15693Answer(recvlen, recvbuf);
+			if (recvlen > 0) {
+				Dbhexdump(recvlen, recvbuf, false);
+				DbdecodeIso15693Answer(recvlen, recvbuf);
+			}
 		}
-
-		cmd_send(CMD_ACK, recvlen>ISO15693_MAX_RESPONSE_LENGTH?ISO15693_MAX_RESPONSE_LENGTH:recvlen, 0, 0, recvbuf, ISO15693_MAX_RESPONSE_LENGTH);
-
+		if (recvlen > ISO15693_MAX_RESPONSE_LENGTH) {
+			recvlen = ISO15693_MAX_RESPONSE_LENGTH;
+		}
+		cmd_send(CMD_ACK, recvlen, 0, 0, recvbuf, ISO15693_MAX_RESPONSE_LENGTH);
 	}
 
-	// for the time being, switch field off to protect rdv4.0
-	// note: this prevents using hf 15 cmd with s option - which isn't implemented yet anyway
-	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
-	LED_D_OFF();
-
 	LED_A_OFF();
 }
 
@@ -1640,16 +1688,16 @@ void DirectTag15693Command(uint32_t datalen, uint32_t speed, uint32_t recv, uint
 //-----------------------------------------------------------------------------
 
 // Set the UID to the tag (based on Iceman work).
-void SetTag15693Uid(uint8_t *uid)
-{
-	uint8_t cmd[4][9] = {0x00};
+void SetTag15693Uid(uint8_t *uid) {
 
+	LED_A_ON();
+
+	uint8_t cmd[4][9] = {0x00};
 	uint16_t crc;
 
 	int recvlen = 0;
 	uint8_t recvbuf[ISO15693_MAX_RESPONSE_LENGTH];
-
-	LED_A_ON();
+	uint32_t eof_time;
 
 	// Command 1 : 02213E00000000
 	cmd[0][0] = 0x02;
@@ -1687,7 +1735,7 @@ void SetTag15693Uid(uint8_t *uid)
 	cmd[3][5] = uid[1];
 	cmd[3][6] = uid[0];
 
-	for (int i=0; i<4; i++) {
+	for (int i = 0; i < 4; i++) {
 		// Add the CRC
 		crc = Iso15693Crc(cmd[i], 7);
 		cmd[i][7] = crc & 0xff;
@@ -1698,19 +1746,19 @@ void SetTag15693Uid(uint8_t *uid)
 			Dbhexdump(sizeof(cmd[i]), cmd[i], false);
 		}
 
-		recvlen = SendDataTag(cmd[i], sizeof(cmd[i]), true, 1, recvbuf, sizeof(recvbuf), 0);
+		recvlen = SendDataTag(cmd[i], sizeof(cmd[i]), true, 1, recvbuf, sizeof(recvbuf), 0, &eof_time);
 
 		if (DEBUG) {
 			Dbprintf("RECV:");
-			Dbhexdump(recvlen, recvbuf, false);
-			DbdecodeIso15693Answer(recvlen, recvbuf);
+			if (recvlen > 0) {
+				Dbhexdump(recvlen, recvbuf, false);
+				DbdecodeIso15693Answer(recvlen, recvbuf);
+			}
 		}
 
 		cmd_send(CMD_ACK, recvlen>ISO15693_MAX_RESPONSE_LENGTH?ISO15693_MAX_RESPONSE_LENGTH:recvlen, 0, 0, recvbuf, ISO15693_MAX_RESPONSE_LENGTH);
 	}
 
-	LED_D_OFF();
-
 	LED_A_OFF();
 }
 
diff --git a/armsrc/iso15693.h b/armsrc/iso15693.h
index 7d2e7598..5cbe5ecc 100644
--- a/armsrc/iso15693.h
+++ b/armsrc/iso15693.h
@@ -17,16 +17,20 @@
 
 // Delays in SSP_CLK ticks.
 // SSP_CLK runs at 13,56MHz / 32 = 423.75kHz when simulating a tag
-#define DELAY_READER_TO_ARM_SIM           8
-#define DELAY_ARM_TO_READER_SIM           0
-#define DELAY_ISO15693_VCD_TO_VICC_SIM    132  // 132/423.75kHz = 311.5us from end of command EOF to start of tag response
-//SSP_CLK runs at 13.56MHz / 4 = 3,39MHz when acting as reader
+#define DELAY_ISO15693_VCD_TO_VICC_SIM     132  // 132/423.75kHz = 311.5us from end of command EOF to start of tag response
+//SSP_CLK runs at 13.56MHz / 4 = 3,39MHz when acting as reader. All values should be multiples of 16
 #define DELAY_ISO15693_VCD_TO_VICC_READER 1056 // 1056/3,39MHz = 311.5us from end of command EOF to start of tag response
-#define DELAY_ISO15693_VICC_TO_VCD_READER 1017 // 1017/3.39MHz = 300us between end of tag response and next reader command
+#define DELAY_ISO15693_VICC_TO_VCD_READER 1024 // 1024/3.39MHz = 302.1us between end of tag response and next reader command
+// times in samples @ 212kHz when acting as reader
+#define ISO15693_READER_TIMEOUT            330 // 330/212kHz = 1558us, should be even enough for iClass tags responding to ACTALL
 
+void Iso15693InitReader();
+void CodeIso15693AsReader(uint8_t *cmd, int n);
 void CodeIso15693AsTag(uint8_t *cmd, size_t len);
-int GetIso15693CommandFromReader(uint8_t *received, size_t max_len, uint32_t *eof_time);
 void TransmitTo15693Reader(const uint8_t *cmd, size_t len, uint32_t *start_time, uint32_t slot_time, bool slow);
+int GetIso15693CommandFromReader(uint8_t *received, size_t max_len, uint32_t *eof_time);
+void TransmitTo15693Tag(const uint8_t *cmd, int len, uint32_t *start_time);
+int GetIso15693AnswerFromTag(uint8_t* response, uint16_t max_len, uint16_t timeout, uint32_t *eof_time);
 void SnoopIso15693(void);
 void AcquireRawAdcSamplesIso15693(void);
 void ReaderIso15693(uint32_t parameter);
@@ -35,5 +39,6 @@ void BruteforceIso15693Afi(uint32_t speed);
 void DirectTag15693Command(uint32_t datalen, uint32_t speed, uint32_t recv, uint8_t data[]);
 void SetTag15693Uid(uint8_t *uid);
 void SetDebugIso15693(uint32_t flag);
+bool LogTrace_ISO15693(const uint8_t *btBytes, uint16_t iLen, uint32_t timestamp_start, uint32_t timestamp_end, uint8_t *parity, bool readerToTag);
 
 #endif
diff --git a/armsrc/util.c b/armsrc/util.c
index aac68a34..4bff3a26 100644
--- a/armsrc/util.c
+++ b/armsrc/util.c
@@ -21,7 +21,7 @@ void print_result(char *name, uint8_t *buf, size_t len) {
 
    if ( len % 16 == 0 ) {
 	   for(; p-buf < len; p += 16)
-       Dbprintf("[%s:%d/%d] %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x",
+	   Dbprintf("[%s:%d/%d] %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x",
 				name,
 				p-buf,
 				len,
@@ -30,7 +30,7 @@ void print_result(char *name, uint8_t *buf, size_t len) {
    }
    else {
    for(; p-buf < len; p += 8)
-       Dbprintf("[%s:%d/%d] %02x %02x %02x %02x %02x %02x %02x %02x", name, p-buf, len, p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7]);
+	   Dbprintf("[%s:%d/%d] %02x %02x %02x %02x %02x %02x %02x %02x", name, p-buf, len, p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7]);
    }
 }
 
@@ -68,17 +68,17 @@ uint64_t bytes_to_num(uint8_t* src, size_t len)
 
 // RotateLeft - Ultralight, Desfire
 void rol(uint8_t *data, const size_t len){
-    uint8_t first = data[0];
-    for (size_t i = 0; i < len-1; i++) {
-        data[i] = data[i+1];
-    }
-    data[len-1] = first;
+	uint8_t first = data[0];
+	for (size_t i = 0; i < len-1; i++) {
+		data[i] = data[i+1];
+	}
+	data[len-1] = first;
 }
 void lsl (uint8_t *data, size_t len) {
-    for (size_t n = 0; n < len - 1; n++) {
-        data[n] = (data[n] << 1) | (data[n+1] >> 7);
-    }
-    data[len - 1] <<= 1;
+	for (size_t n = 0; n < len - 1; n++) {
+		data[n] = (data[n] << 1) | (data[n+1] >> 7);
+	}
+	data[len - 1] <<= 1;
 }
 
 void LEDsoff()
@@ -309,16 +309,16 @@ void FormatVersionInformation(char *dst, int len, const char *prefix, void *vers
 //  -------------------------------------------------------------------------
 //  test procedure:
 //
-//	ti = GetTickCount();
-//	SpinDelay(1000);
-//	ti = GetTickCount() - ti;
-//	Dbprintf("timer(1s): %d t=%d", ti, GetTickCount());
+//  ti = GetTickCount();
+//  SpinDelay(1000);
+//  ti = GetTickCount() - ti;
+//  Dbprintf("timer(1s): %d t=%d", ti, GetTickCount());
 
 void StartTickCount()
 {
 	// This timer is based on the slow clock. The slow clock frequency is between 22kHz and 40kHz.
 	// We can determine the actual slow clock frequency by looking at the Main Clock Frequency Register.
-    uint16_t mainf = AT91C_BASE_PMC->PMC_MCFR & 0xffff;		// = 16 * main clock frequency (16MHz) / slow clock frequency
+	uint16_t mainf = AT91C_BASE_PMC->PMC_MCFR & 0xffff;     // = 16 * main clock frequency (16MHz) / slow clock frequency
 	// set RealTimeCounter divider to count at 1kHz:
 	AT91C_BASE_RTTC->RTTC_RTMR = AT91C_RTTC_RTTRST | ((256000 + (mainf/2)) / mainf);
 	// note: worst case precision is approx 2.5%
@@ -334,12 +334,12 @@ uint32_t RAMFUNC GetTickCount(){
 
 
 //  -------------------------------------------------------------------------
-//  microseconds timer 
+//  microseconds timer
 //  -------------------------------------------------------------------------
 void StartCountUS()
 {
 	AT91C_BASE_PMC->PMC_PCER |= (0x1 << 12) | (0x1 << 13) | (0x1 << 14);
-//	AT91C_BASE_TCB->TCB_BMR = AT91C_TCB_TC1XC1S_TIOA0;
+//  AT91C_BASE_TCB->TCB_BMR = AT91C_TCB_TC1XC1S_TIOA0;
 	AT91C_BASE_TCB->TCB_BMR = AT91C_TCB_TC0XC0S_NONE | AT91C_TCB_TC1XC1S_TIOA0 | AT91C_TCB_TC2XC2S_NONE;
 
 	// fast clock
@@ -349,10 +349,10 @@ void StartCountUS()
 														AT91C_TC_ACPC_SET | AT91C_TC_ASWTRG_SET;
 	AT91C_BASE_TC0->TC_RA = 1;
 	AT91C_BASE_TC0->TC_RC = 0xBFFF + 1; // 0xC000
-	
-	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS; // timer disable  
+
+	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS; // timer disable
 	AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_XC1; // from timer 0
-	
+
 	AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN;
 	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN;
 	AT91C_BASE_TCB->TCB_BCR = 1;
@@ -375,61 +375,72 @@ uint32_t RAMFUNC GetDeltaCountUS(){
 
 
 //  -------------------------------------------------------------------------
-//  Timer for iso14443 commands. Uses ssp_clk from FPGA 
+//  Timer for iso14443 commands. Uses ssp_clk from FPGA
 //  -------------------------------------------------------------------------
 void StartCountSspClk()
 {
 	AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_TC0) | (1 << AT91C_ID_TC1) | (1 << AT91C_ID_TC2);  // Enable Clock to all timers
-	AT91C_BASE_TCB->TCB_BMR = AT91C_TCB_TC0XC0S_TIOA1 		// XC0 Clock = TIOA1
-							| AT91C_TCB_TC1XC1S_NONE 		// XC1 Clock = none
-							| AT91C_TCB_TC2XC2S_TIOA0;		// XC2 Clock = TIOA0
+	AT91C_BASE_TCB->TCB_BMR = AT91C_TCB_TC0XC0S_TIOA1       // XC0 Clock = TIOA1
+							| AT91C_TCB_TC1XC1S_NONE        // XC1 Clock = none
+							| AT91C_TCB_TC2XC2S_TIOA0;      // XC2 Clock = TIOA0
 
 	// configure TC1 to create a short pulse on TIOA1 when a rising edge on TIOB1 (= ssp_clk from FPGA) occurs:
-	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS; 				// disable TC1
+	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;               // disable TC1
 	AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK // TC1 Clock = MCK(48MHz)/2 = 24MHz
-							| AT91C_TC_CPCSTOP				// Stop clock on RC compare
-							| AT91C_TC_EEVTEDG_RISING		// Trigger on rising edge of Event
-							| AT91C_TC_EEVT_TIOB			// Event-Source: TIOB1 (= ssp_clk from FPGA = 13,56MHz/16 ... 13,56MHz/4)
-							| AT91C_TC_ENETRG				// Enable external trigger event
-							| AT91C_TC_WAVESEL_UP	 		// Upmode without automatic trigger on RC compare
-							| AT91C_TC_WAVE 				// Waveform Mode
-							| AT91C_TC_AEEVT_SET 			// Set TIOA1 on external event
-							| AT91C_TC_ACPC_CLEAR; 			// Clear TIOA1 on RC Compare
-	AT91C_BASE_TC1->TC_RC = 0x02; 							// RC Compare value = 0x02
+							| AT91C_TC_CPCSTOP              // Stop clock on RC compare
+							| AT91C_TC_EEVTEDG_RISING       // Trigger on rising edge of Event
+							| AT91C_TC_EEVT_TIOB            // Event-Source: TIOB1 (= ssp_clk from FPGA = 13,56MHz/16 ... 13,56MHz/4)
+							| AT91C_TC_ENETRG               // Enable external trigger event
+							| AT91C_TC_WAVESEL_UP           // Upmode without automatic trigger on RC compare
+							| AT91C_TC_WAVE                 // Waveform Mode
+							| AT91C_TC_AEEVT_SET            // Set TIOA1 on external event
+							| AT91C_TC_ACPC_CLEAR;          // Clear TIOA1 on RC Compare
+	AT91C_BASE_TC1->TC_RC = 0x02;                           // RC Compare value = 0x02
 
 	// use TC0 to count TIOA1 pulses
-	AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS;				// disable TC0
-	AT91C_BASE_TC0->TC_CMR = AT91C_TC_CLKS_XC0	 			// TC0 clock = XC0 clock = TIOA1
-							| AT91C_TC_WAVE 				// Waveform Mode
-							| AT91C_TC_WAVESEL_UP	 		// just count
-							| AT91C_TC_ACPA_CLEAR 			// Clear TIOA0 on RA Compare
-							| AT91C_TC_ACPC_SET; 			// Set TIOA0 on RC Compare
-	AT91C_BASE_TC0->TC_RA = 1;								// RA Compare value = 1; pulse width to TC2
-	AT91C_BASE_TC0->TC_RC = 0; 								// RC Compare value = 0; increment TC2 on overflow
+	AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS;               // disable TC0
+	AT91C_BASE_TC0->TC_CMR = AT91C_TC_CLKS_XC0              // TC0 clock = XC0 clock = TIOA1
+							| AT91C_TC_WAVE                 // Waveform Mode
+							| AT91C_TC_WAVESEL_UP           // just count
+							| AT91C_TC_ACPA_CLEAR           // Clear TIOA0 on RA Compare
+							| AT91C_TC_ACPC_SET;            // Set TIOA0 on RC Compare
+	AT91C_BASE_TC0->TC_RA = 1;                              // RA Compare value = 1; pulse width to TC2
+	AT91C_BASE_TC0->TC_RC = 0;                              // RC Compare value = 0; increment TC2 on overflow
 
 	// use TC2 to count TIOA0 pulses (giving us a 32bit counter (TC0/TC2) clocked by ssp_clk)
-	AT91C_BASE_TC2->TC_CCR = AT91C_TC_CLKDIS; 				// disable TC2  
-	AT91C_BASE_TC2->TC_CMR = AT91C_TC_CLKS_XC2	 			// TC2 clock = XC2 clock = TIOA0
-							| AT91C_TC_WAVE 				// Waveform Mode
-							| AT91C_TC_WAVESEL_UP;	 		// just count
-	
-	AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN;				// enable TC0
-	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN;				// enable TC1
-	AT91C_BASE_TC2->TC_CCR = AT91C_TC_CLKEN;				// enable TC2
+	AT91C_BASE_TC2->TC_CCR = AT91C_TC_CLKDIS;               // disable TC2
+	AT91C_BASE_TC2->TC_CMR = AT91C_TC_CLKS_XC2              // TC2 clock = XC2 clock = TIOA0
+							| AT91C_TC_WAVE                 // Waveform Mode
+							| AT91C_TC_WAVESEL_UP;          // just count
+
+	AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN;                // enable TC0
+	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN;                // enable TC1
+	AT91C_BASE_TC2->TC_CCR = AT91C_TC_CLKEN;                // enable TC2
 
 	//
-	// synchronize the counter with the ssp_frame signal. Note: FPGA must be in a FPGA mode with SSC transfer, otherwise SSC_FRAME and SSC_CLK signals would not be present 
+	// synchronize the counter with the ssp_frame signal. Note: FPGA must be in a FPGA mode with SSC transfer, otherwise SSC_FRAME and SSC_CLK signals would not be present
 	//
-	while(!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_FRAME)); 	// wait for ssp_frame to go high (start of frame)
-	while(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_FRAME); 		// wait for ssp_frame to be low
-	while(!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK)); 	// wait for ssp_clk to go high
-	// note: up to now two ssp_clk rising edges have passed since the rising edge of ssp_frame
+	while(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_FRAME);      // wait for ssp_frame to be low
+	while(!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_FRAME));   // wait for ssp_frame to go high (start of frame)
+	while(!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK));     // wait for ssp_clk to go high; 1st ssp_clk after start of frame
+	while(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK);        // wait for ssp_clk to go low;
+	while(!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK));     // wait for ssp_clk to go high; 2nd ssp_clk after start of frame
+	if ((AT91C_BASE_SSC->SSC_RFMR & SSC_FRAME_MODE_BITS_IN_WORD(32)) == SSC_FRAME_MODE_BITS_IN_WORD(16)) {
+		while(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK);    // wait for ssp_clk to go low;
+		while(!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK)); // wait for ssp_clk to go high; 3rd ssp_clk after start of frame
+		while(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK);    // wait for ssp_clk to go low;
+		while(!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK)); // wait for ssp_clk to go high; 4th ssp_clk after start of frame
+		while(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK);    // wait for ssp_clk to go low;
+		while(!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK)); // wait for ssp_clk to go high; 5th ssp_clk after start of frame
+		while(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK);    // wait for ssp_clk to go low;
+		while(!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK)); // wait for ssp_clk to go high; 6th ssp_clk after start of frame
+	}
 	// it is now safe to assert a sync signal. This sets all timers to 0 on next active clock edge
-	AT91C_BASE_TCB->TCB_BCR = 1;							// assert Sync (set all timers to 0 on next active clock edge)
-	// at the next (3rd) ssp_clk rising edge, TC1 will be reset (and not generate a clock signal to TC0)
-	// at the next (4th) ssp_clk rising edge, TC0 (the low word of our counter) will be reset. From now on,
+	AT91C_BASE_TCB->TCB_BCR = 1;                            // assert Sync (set all timers to 0 on next active clock edge)
+	// at the next (3rd/7th) ssp_clk rising edge, TC1 will be reset (and not generate a clock signal to TC0)
+	// at the next (4th/8th) ssp_clk rising edge, TC0 (the low word of our counter) will be reset. From now on,
 	// whenever the last three bits of our counter go 0, we can be sure to be in the middle of a frame transfer.
-	// (just started with the transfer of the 4th Bit).
+	// (just started with the transfer of the 3rd Bit).
 	// The high word of the counter (TC2) will not reset until the low word (TC0) overflows. Therefore need to wait quite some time before
 	// we can use the counter.
 	while (AT91C_BASE_TC0->TC_CV < 0xFFFF);
@@ -445,19 +456,17 @@ void ResetSspClk(void) {
 	while (AT91C_BASE_TC2->TC_CV > 0);
 }
 
-
 uint32_t GetCountSspClk(){
 	uint32_t hi, lo;
 
-	do {
+	do { 
 		hi = AT91C_BASE_TC2->TC_CV;
 		lo = AT91C_BASE_TC0->TC_CV;
-	} while(hi != AT91C_BASE_TC2->TC_CV);
-
+	} while (hi != AT91C_BASE_TC2->TC_CV);
+	
 	return (hi << 16) | lo;
 }
 
-
 //  -------------------------------------------------------------------------
 //  Timer for bitbanging, or LF stuff when you need a very precis timer
 //  1us = 1.5ticks
@@ -476,11 +485,11 @@ void StartTicks(void){
 	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG; // re-enable timer and wait for TC0
 
 	// second configure TC0 (lower, 0x0000FFFF) 16 bit counter
-	AT91C_BASE_TC0->TC_CMR = AT91C_TC_CLKS_TIMER_DIV3_CLOCK | // MCK(48MHz) / 32 
-	                         AT91C_TC_WAVE | AT91C_TC_WAVESEL_UP_AUTO |
-	                         AT91C_TC_ACPA_CLEAR | // RA comperator clears TIOA (carry bit)
-	                         AT91C_TC_ACPC_SET |   // RC comperator sets TIOA (carry bit)
-	                         AT91C_TC_ASWTRG_SET;  // SWTriger sets TIOA (carry bit)
+	AT91C_BASE_TC0->TC_CMR = AT91C_TC_CLKS_TIMER_DIV3_CLOCK | // MCK(48MHz) / 32
+							 AT91C_TC_WAVE | AT91C_TC_WAVESEL_UP_AUTO |
+							 AT91C_TC_ACPA_CLEAR | // RA comperator clears TIOA (carry bit)
+							 AT91C_TC_ACPC_SET |   // RC comperator sets TIOA (carry bit)
+							 AT91C_TC_ASWTRG_SET;  // SWTriger sets TIOA (carry bit)
 	AT91C_BASE_TC0->TC_RC  = 0; // set TIOA (carry bit) on overflow, return to zero
 	AT91C_BASE_TC0->TC_RA  = 1; // clear carry bit on next clock cycle
 	AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG; // reset and re-enable timer
@@ -517,7 +526,7 @@ void WaitTicks(uint32_t ticks){
 }
 
 
-// Wait / Spindelay in us (microseconds) 
+// Wait / Spindelay in us (microseconds)
 // 1us = 1.5ticks.
 void WaitUS(uint16_t us){
 	WaitTicks( (uint32_t)us * 3 / 2 ) ;
@@ -546,7 +555,7 @@ void ResetTimer(AT91PS_TC timer){
 // stop clock
 void StopTicks(void){
 	AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS;
-	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;	
+	AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
 }
 
 
diff --git a/armsrc/util.h b/armsrc/util.h
index 7b3d0849..14ac5e10 100644
--- a/armsrc/util.h
+++ b/armsrc/util.h
@@ -66,7 +66,7 @@ uint32_t RAMFUNC GetDeltaCountUS();
 
 void StartCountSspClk();
 void ResetSspClk(void);
-uint32_t RAMFUNC GetCountSspClk();
+uint32_t GetCountSspClk();
 
 extern void StartTicks(void);
 extern uint32_t GetTicks(void);
diff --git a/client/cmdhf.c b/client/cmdhf.c
index 6d25cac0..83170cae 100644
--- a/client/cmdhf.c
+++ b/client/cmdhf.c
@@ -61,7 +61,7 @@ int CmdHFSearch(const char *Cmd){
 		return ans;
 	}
 	//14b is longest test currently (and rarest chip type) ... put last
-	ans = HF14BInfo(false);
+	ans = infoHF14B(false);
 	if (ans) {
 		PrintAndLog("\nValid ISO14443B Tag Found - Quiting Search\n");
 		return ans;
diff --git a/client/cmdhf14b.c b/client/cmdhf14b.c
index 7cd55476..4685f3f5 100644
--- a/client/cmdhf14b.c
+++ b/client/cmdhf14b.c
@@ -15,6 +15,7 @@
 #include <stdbool.h>
 #include <string.h>
 #include <stdint.h>
+#include <ctype.h>
 #include "iso14443crc.h"
 #include "comms.h"
 #include "graph.h"
@@ -25,101 +26,110 @@
 #include "taginfo.h"
 
 
-static int CmdHelp(const char *Cmd);
-
-int CmdHF14BList(const char *Cmd)
-{
+int CmdHF14BList(const char *Cmd) {
 	PrintAndLog("Deprecated command, use 'hf list 14b' instead");
-
 	return 0;
 }
 
-int CmdHF14BSim(const char *Cmd)
-{
+
+int CmdHF14BSim(const char *Cmd) {
   UsbCommand c={CMD_SIMULATE_TAG_ISO_14443B};
   clearCommandBuffer();
   SendCommand(&c);
   return 0;
 }
 
-int CmdHF14BSnoop(const char *Cmd)
-{
+
+int CmdHF14BSnoop(const char *Cmd) {
   UsbCommand c = {CMD_SNOOP_ISO_14443B};
   clearCommandBuffer();
   SendCommand(&c);
   return 0;
 }
 
+
 /* New command to read the contents of a SRI512 tag
  * SRI512 tags are ISO14443-B modulated memory tags,
  * this command just dumps the contents of the memory
  */
-int CmdSri512Read(const char *Cmd)
-{
+int CmdSri512Read(const char *Cmd) {
   UsbCommand c = {CMD_READ_SRI512_TAG, {strtol(Cmd, NULL, 0), 0, 0}};
   clearCommandBuffer();
   SendCommand(&c);
   return 0;
 }
 
+
 /* New command to read the contents of a SRIX4K tag
  * SRIX4K tags are ISO14443-B modulated memory tags,
  * this command just dumps the contents of the memory/
  */
-int CmdSrix4kRead(const char *Cmd)
-{
+int CmdSrix4kRead(const char *Cmd) {
   UsbCommand c = {CMD_READ_SRIX4K_TAG, {strtol(Cmd, NULL, 0), 0, 0}};
   clearCommandBuffer();
   SendCommand(&c);
   return 0;
 }
 
-int rawClose(void){
+
+static bool switch_off_field_14b(void) {
 	UsbCommand resp;
 	UsbCommand c = {CMD_ISO_14443B_COMMAND, {0, 0, 0}};
 	clearCommandBuffer();
 	SendCommand(&c);
-	if (!WaitForResponseTimeout(CMD_ACK,&resp,1000)) {
-		return 0;
+	if (!WaitForResponseTimeout(CMD_ACK, &resp, 1000)) {
+		return false;
 	}
-	return 0;
+	return false;
 }
 
-int HF14BCmdRaw(bool reply, bool *crc, bool power, uint8_t *data, uint8_t *datalen, bool verbose){
+
+int HF14BCmdRaw(bool reply, bool *crc, bool power, uint8_t *data, uint8_t *datalen, bool verbose) {
 	UsbCommand resp;
 	UsbCommand c = {CMD_ISO_14443B_COMMAND, {0, 0, 0}}; // len,recv,power
-	if(*crc)
-	{
+	if (*crc) {
 		uint8_t first, second;
 		ComputeCrc14443(CRC_14443_B, data, *datalen, &first, &second);
 		data[*datalen] = first;
 		data[*datalen + 1] = second;
 		*datalen += 2;
 	}
-	
+
 	c.arg[0] = *datalen;
 	c.arg[1] = reply;
 	c.arg[2] = power;
-	memcpy(c.d.asBytes,data,*datalen);
+	memcpy(c.d.asBytes,data, *datalen);
 	clearCommandBuffer();
 	SendCommand(&c);
-	
-	if (!reply) return 1; 
 
-	if (!WaitForResponseTimeout(CMD_ACK,&resp,1000)) {
+	if (!reply) return 1;
+
+	if (!WaitForResponseTimeout(CMD_ACK, &resp, 1000)) {
 		if (verbose) PrintAndLog("timeout while waiting for reply.");
 		return 0;
 	}
-	*datalen = resp.arg[0];
-	if (verbose) PrintAndLog("received %u octets", *datalen);
-	if(*datalen<2) return 0;
+
+	int ret = resp.arg[0];
+	if (verbose) {
+		if (ret < 0) {
+			PrintAndLog("tag didn't respond");
+		} else if (ret == 0) {
+			PrintAndLog("received SOF only (maybe iCLASS/Picopass)");
+		} else {
+			PrintAndLog("received %u octets", ret);
+		}
+	}
+
+	*datalen = ret;
+	
+	if (ret < 2) return 0;
 
 	memcpy(data, resp.d.asBytes, *datalen);
 	if (verbose) PrintAndLog("%s", sprint_hex(data, *datalen));
 
 	uint8_t first, second;
 	ComputeCrc14443(CRC_14443_B, data, *datalen-2, &first, &second);
-	if(data[*datalen-2] == first && data[*datalen-1] == second) {
+	if (data[*datalen-2] == first && data[*datalen-1] == second) {
 		if (verbose) PrintAndLog("CRC OK");
 		*crc = true;
 	} else {
@@ -129,7 +139,8 @@ int HF14BCmdRaw(bool reply, bool *crc, bool power, uint8_t *data, uint8_t *datal
 	return 1;
 }
 
-int CmdHF14BCmdRaw (const char *Cmd) {
+
+static int CmdHF14BCmdRaw (const char *Cmd) {
 	bool reply = true;
 	bool crc = false;
 	bool power = false;
@@ -140,7 +151,7 @@ int CmdHF14BCmdRaw (const char *Cmd) {
 	uint8_t datalen = 0;
 	unsigned int temp;
 	int i = 0;
-	if (strlen(Cmd)<3) {
+	if (strlen(Cmd) < 2) {
 			PrintAndLog("Usage: hf 14b raw [-r] [-c] [-p] [-s || -ss] <0A 0B 0C ... hex>");
 			PrintAndLog("       -r    do not read response");
 			PrintAndLog("       -c    calculate and append CRC");
@@ -151,28 +162,28 @@ int CmdHF14BCmdRaw (const char *Cmd) {
 	}
 
 	// strip
-	while (*Cmd==' ' || *Cmd=='\t') Cmd++;
-	
-	while (Cmd[i]!='\0') {
-		if (Cmd[i]==' ' || Cmd[i]=='\t') { i++; continue; }
-		if (Cmd[i]=='-') {
+	while (*Cmd == ' ' || *Cmd == '\t') Cmd++;
+
+	while (Cmd[i] != '\0') {
+		if (Cmd[i] == ' ' || Cmd[i] == '\t') { i++; continue; }
+		if (Cmd[i] == '-') {
 			switch (Cmd[i+1]) {
-				case 'r': 
-				case 'R': 
+				case 'r':
+				case 'R':
 					reply = false;
 					break;
 				case 'c':
-				case 'C':                
+				case 'C':
 					crc = true;
 					break;
-				case 'p': 
-				case 'P': 
+				case 'p':
+				case 'P':
 					power = true;
 					break;
 				case 's':
 				case 'S':
 					select = true;
-					if (Cmd[i+2]=='s' || Cmd[i+2]=='S') {
+					if (Cmd[i+2] == 's' || Cmd[i+2] == 'S') {
 						SRx = true;
 						i++;
 					}
@@ -181,34 +192,33 @@ int CmdHF14BCmdRaw (const char *Cmd) {
 					PrintAndLog("Invalid option");
 					return 0;
 			}
-			i+=2;
+			i += 2;
 			continue;
 		}
-		if ((Cmd[i]>='0' && Cmd[i]<='9') ||
-		    (Cmd[i]>='a' && Cmd[i]<='f') ||
-		    (Cmd[i]>='A' && Cmd[i]<='F') ) {
-			buf[strlen(buf)+1]=0;
-			buf[strlen(buf)]=Cmd[i];
+		if ((Cmd[i] >= '0' && Cmd[i] <= '9') ||
+			(Cmd[i] >= 'a' && Cmd[i] <= 'f') ||
+			(Cmd[i] >= 'A' && Cmd[i] <= 'F') ) {
+			buf[strlen(buf)+1] = 0;
+			buf[strlen(buf)] = Cmd[i];
 			i++;
-			
-			if (strlen(buf)>=2) {
-				sscanf(buf,"%x",&temp);
-				data[datalen++]=(uint8_t)(temp & 0xff);
-				*buf=0;
+
+			if (strlen(buf) >= 2) {
+				sscanf(buf, "%x", &temp);
+				data[datalen++] = (uint8_t)(temp & 0xff);
+				*buf = 0;
 			}
 			continue;
 		}
 		PrintAndLog("Invalid char on input");
 		return 0;
 	}
-	if (datalen == 0)
-	{
+	if (datalen == 0) {
 		PrintAndLog("Missing data input");
 		return 0;
 	}
 
-	if (select){ //auto select 14b tag
-		uint8_t	cmd2[16];
+	if (select) { //auto select 14b tag
+		uint8_t cmd2[16];
 		bool crc2 = true;
 		uint8_t cmdLen;
 
@@ -225,11 +235,11 @@ int CmdHF14BCmdRaw (const char *Cmd) {
 			cmd2[2] = 0x08;
 		}
 
-		if (HF14BCmdRaw(true, &crc2, true, cmd2, &cmdLen, false)==0) return rawClose();
+		if (HF14BCmdRaw(true, &crc2, true, cmd2, &cmdLen, false) == 0) return switch_off_field_14b();
+
+		if (SRx && (cmdLen != 3 || !crc2) ) return switch_off_field_14b();
+		else if (cmd2[0] != 0x50 || cmdLen != 14 || !crc2) return switch_off_field_14b();
 
-		if ( SRx && (cmdLen != 3 || !crc2) ) return rawClose();
-		else if (cmd2[0] != 0x50 || cmdLen != 14 || !crc2) return rawClose();
-		
 		uint8_t chipID = 0;
 		if (SRx) {
 			// select
@@ -239,7 +249,7 @@ int CmdHF14BCmdRaw (const char *Cmd) {
 			cmdLen = 2;
 		} else {
 			// attrib
-			cmd2[0] = 0x1D; 
+			cmd2[0] = 0x1D;
 			// UID from cmd2[1 - 4]
 			cmd2[5] = 0x00;
 			cmd2[6] = 0x08;
@@ -248,39 +258,40 @@ int CmdHF14BCmdRaw (const char *Cmd) {
 			cmdLen = 9;
 		}
 
-		if (HF14BCmdRaw(true, &crc2, true, cmd2, &cmdLen, false)==0) return rawClose();
+		if (HF14BCmdRaw(true, &crc2, true, cmd2, &cmdLen, false) == 0) return switch_off_field_14b();
 
-		if (cmdLen != 3 || !crc2) return rawClose();
-		if (SRx && cmd2[0] != chipID) return rawClose();
+		if (cmdLen != 3 || !crc2) return switch_off_field_14b();
+		if (SRx && cmd2[0] != chipID) return switch_off_field_14b();
 	}
 	return HF14BCmdRaw(reply, &crc, power, data, &datalen, true);
 }
 
+
 // print full atqb info
-static void print_atqb_resp(uint8_t *data){
+static void print_atqb_resp(uint8_t *data) {
 	//PrintAndLog ("           UID: %s", sprint_hex(data+1,4));
-	PrintAndLog ("      App Data: %s", sprint_hex(data+5,4));
-	PrintAndLog ("      Protocol: %s", sprint_hex(data+9,3));
+	PrintAndLog("      App Data: %s", sprint_hex(data+5,4));
+	PrintAndLog("      Protocol: %s", sprint_hex(data+9,3));
 	uint8_t BitRate = data[9];
-	if (!BitRate) 
+	if (!BitRate)
 		PrintAndLog ("      Bit Rate: 106 kbit/s only PICC <-> PCD");
 	if (BitRate & 0x10)
 		PrintAndLog ("      Bit Rate: 212 kbit/s PICC -> PCD supported");
 	if (BitRate & 0x20)
-		PrintAndLog ("      Bit Rate: 424 kbit/s PICC -> PCD supported"); 
+		PrintAndLog ("      Bit Rate: 424 kbit/s PICC -> PCD supported");
 	if (BitRate & 0x40)
-		PrintAndLog ("      Bit Rate: 847 kbit/s PICC -> PCD supported"); 
+		PrintAndLog ("      Bit Rate: 847 kbit/s PICC -> PCD supported");
 	if (BitRate & 0x01)
 		PrintAndLog ("      Bit Rate: 212 kbit/s PICC <- PCD supported");
 	if (BitRate & 0x02)
-		PrintAndLog ("      Bit Rate: 424 kbit/s PICC <- PCD supported"); 
+		PrintAndLog ("      Bit Rate: 424 kbit/s PICC <- PCD supported");
 	if (BitRate & 0x04)
-		PrintAndLog ("      Bit Rate: 847 kbit/s PICC <- PCD supported"); 
-	if (BitRate & 0x80) 
+		PrintAndLog ("      Bit Rate: 847 kbit/s PICC <- PCD supported");
+	if (BitRate & 0x80)
 		PrintAndLog ("                Same bit rate <-> required");
 
-	uint16_t maxFrame = data[10]>>4;
-	if (maxFrame < 5) 
+	uint16_t maxFrame = data[10] >> 4;
+	if (maxFrame < 5)
 		maxFrame = 8*maxFrame + 16;
 	else if (maxFrame == 5)
 		maxFrame = 64;
@@ -293,7 +304,7 @@ static void print_atqb_resp(uint8_t *data){
 	else
 		maxFrame = 257;
 
-	PrintAndLog ("Max Frame Size: %u%s",maxFrame, (maxFrame == 257) ? "+ RFU" : "");
+	PrintAndLog ("Max Frame Size: %u%s", maxFrame, (maxFrame == 257) ? "+ RFU" : "");
 
 	uint8_t protocolT = data[10] & 0xF;
 	PrintAndLog (" Protocol Type: Protocol is %scompliant with ISO/IEC 14443-4",(protocolT) ? "" : "not " );
@@ -302,32 +313,32 @@ static void print_atqb_resp(uint8_t *data){
 	PrintAndLog (" Frame Options: NAD is %ssupported",(data[11]&2) ? "" : "not ");
 	PrintAndLog (" Frame Options: CID is %ssupported",(data[11]&1) ? "" : "not ");
 	PrintAndLog ("Max Buf Length: %u (MBLI) %s",data[14]>>4, (data[14] & 0xF0) ? "" : "not supported");
-	
+
 	return;
 }
 
 
-int print_ST_Lock_info(uint8_t model){
+int print_ST_Lock_info(uint8_t model) {
 	//assume connection open and tag selected...
 	uint8_t data[16] = {0x00};
 	uint8_t datalen = 2;
 	bool crc = true;
 	uint8_t resplen;
-	uint8_t	blk1;
+	uint8_t blk1;
 	data[0] = 0x08;
 
-	if (model == 0x2) { //SR176 has special command:
-		data[1] = 0xf;
-		resplen = 4;			
+	if (model == 0x02) { //SR176 has special command:
+		data[1] = 0x0f;
+		resplen = 4;
 	} else {
 		data[1] = 0xff;
 		resplen = 6;
 	}
 
 	//std read cmd
-	if (HF14BCmdRaw(true, &crc, true, data, &datalen, false)==0) return rawClose();
+	if (HF14BCmdRaw(true, &crc, true, data, &datalen, false) == 0) return switch_off_field_14b();
 
-	if (datalen != resplen || !crc) return rawClose();
+	if (datalen != resplen || !crc) return switch_off_field_14b();
 
 	PrintAndLog("Chip Write Protection Bits:");
 	// now interpret the data
@@ -339,7 +350,7 @@ int print_ST_Lock_info(uint8_t model){
 			blk1 = 9;
 			PrintAndLog("   raw: %s",printBits(1,data+3));
 			PrintAndLog(" 07/08:%slocked", (data[3] & 1) ? " not " : " " );
-			for (uint8_t i = 1; i<8; i++){
+			for (uint8_t i = 1; i < 8; i++){
 				PrintAndLog("    %02u:%slocked", blk1, (data[3] & (1 << i)) ? " not " : " " );
 				blk1++;
 			}
@@ -349,9 +360,9 @@ int print_ST_Lock_info(uint8_t model){
 		case 0xC: //             (SRT512)
 			//need data[2] and data[3]
 			blk1 = 0;
-			PrintAndLog("   raw: %s",printBits(2,data+2));
-			for (uint8_t b=2; b<4; b++){
-				for (uint8_t i=0; i<8; i++){
+			PrintAndLog("   raw: %s", printBits(2,data+2));
+			for (uint8_t b = 2; b < 4; b++) {
+				for (uint8_t i = 0; i < 8; i++) {
 					PrintAndLog("    %02u:%slocked", blk1, (data[b] & (1 << i)) ? " not " : " " );
 					blk1++;
 				}
@@ -360,29 +371,31 @@ int print_ST_Lock_info(uint8_t model){
 		case 0x2: //             (SR176)
 			//need data[2]
 			blk1 = 0;
-			PrintAndLog("   raw: %s",printBits(1,data+2));
-			for (uint8_t i = 0; i<8; i++){
+			PrintAndLog("   raw: %s",printBits(1, data+2));
+			for (uint8_t i = 0; i < 8; i++){
 				PrintAndLog(" %02u/%02u:%slocked", blk1, blk1+1, (data[2] & (1 << i)) ? " " : " not " );
-				blk1+=2;
+				blk1 += 2;
 			}
 			break;
 		default:
-			return rawClose();
+			return switch_off_field_14b();
 	}
 	return 1;
 }
 
+
 // print UID info from SRx chips (ST Microelectronics)
-static void print_st_general_info(uint8_t *data){
+static void print_st_general_info(uint8_t *data) {
 	//uid = first 8 bytes in data
-	PrintAndLog("   UID: %s", sprint_hex(SwapEndian64(data,8,8),8));
+	PrintAndLog("   UID: %s", sprint_hex(SwapEndian64(data, 8, 8), 8));
 	PrintAndLog("   MFG: %02X, %s", data[6], getManufacturerName(data[6]));
 	PrintAndLog("  Chip: %02X, %s", data[5], getChipInfo(data[6], data[5]));
 	return;
 }
 
+
 // 14b get and print UID only (general info)
-int HF14BStdReader(uint8_t *data, uint8_t *datalen){
+int HF14BStdReader(uint8_t *data, uint8_t *datalen) {
 	//05 00 00 = find one tag in field
 	//1d xx xx xx xx 00 08 01 00 = attrib xx=UID (resp 10 [f9 e0])
 	//a3 = ?  (resp 03 [e2 c2])
@@ -407,18 +420,18 @@ int HF14BStdReader(uint8_t *data, uint8_t *datalen){
 	data[1] = 0x00;
 	data[2] = 0x08;
 
-	if (HF14BCmdRaw(true, &crc, true, data, datalen, false)==0) return rawClose();
+	if (HF14BCmdRaw(true, &crc, true, data, datalen, false) == 0) return switch_off_field_14b();
 
-	if (data[0] != 0x50 || *datalen != 14 || !crc) return rawClose();
+	if (data[0] != 0x50 || *datalen != 14 || !crc) return switch_off_field_14b();
 
 	PrintAndLog ("\n14443-3b tag found:");
-	PrintAndLog ("           UID: %s", sprint_hex(data+1,4));
+	PrintAndLog ("           UID: %s", sprint_hex(data+1, 4));
 
-	uint8_t	cmd2[16];
+	uint8_t cmd2[16];
 	uint8_t cmdLen = 3;
 	bool crc2 = true;
 
-	cmd2[0] = 0x1D; 
+	cmd2[0] = 0x1D;
 	// UID from data[1 - 4]
 	cmd2[1] = data[1];
 	cmd2[2] = data[2];
@@ -431,28 +444,29 @@ int HF14BStdReader(uint8_t *data, uint8_t *datalen){
 	cmdLen = 9;
 
 	// attrib
-	if (HF14BCmdRaw(true, &crc2, true, cmd2, &cmdLen, false)==0) return rawClose();
+	if (HF14BCmdRaw(true, &crc2, true, cmd2, &cmdLen, false) == 0) return switch_off_field_14b();
 
-	if (cmdLen != 3 || !crc2) return rawClose();
+	if (cmdLen != 3 || !crc2) return switch_off_field_14b();
 	// add attrib responce to data
 	data[14] = cmd2[0];
-	rawClose();
+	switch_off_field_14b();
 	return 1;
 }
 
+
 // 14b get and print Full Info (as much as we know)
-int HF14BStdInfo(uint8_t *data, uint8_t *datalen){
-	if (!HF14BStdReader(data,datalen)) return 0;
+static bool HF14B_Std_Info(uint8_t *data, uint8_t *datalen) {
+	if (!HF14BStdReader(data, datalen)) return false;
 
 	//add more info here
 	print_atqb_resp(data);
 
-
-	return 1;
+	return true;
 }
 
+
 // SRx get and print general info about SRx chip from UID
-int HF14B_ST_Reader(uint8_t *data, uint8_t *datalen, bool closeCon){
+static bool HF14B_ST_Reader(uint8_t *data, uint8_t *datalen, bool closeCon){
 	bool crc = true;
 	*datalen = 2;
 	//wake cmd
@@ -461,9 +475,9 @@ int HF14B_ST_Reader(uint8_t *data, uint8_t *datalen, bool closeCon){
 
 	//leave power on
 	// verbose on for now for testing - turn off when functional
-	if (HF14BCmdRaw(true, &crc, true, data, datalen, false)==0) return rawClose();
+	if (HF14BCmdRaw(true, &crc, true, data, datalen, false) == 0) return switch_off_field_14b();
 
-	if (*datalen != 3 || !crc) return rawClose();
+	if (*datalen != 3 || !crc) return switch_off_field_14b();
 
 	uint8_t chipID = data[0];
 	// select
@@ -472,118 +486,143 @@ int HF14B_ST_Reader(uint8_t *data, uint8_t *datalen, bool closeCon){
 	*datalen = 2;
 
 	//leave power on
-	if (HF14BCmdRaw(true, &crc, true, data, datalen, false)==0) return rawClose();
+	if (HF14BCmdRaw(true, &crc, true, data, datalen, false) == 0) return switch_off_field_14b();
 
-	if (*datalen != 3 || !crc || data[0] != chipID) return rawClose();
+	if (*datalen != 3 || !crc || data[0] != chipID) return switch_off_field_14b();
 
 	// get uid
 	data[0] = 0x0B;
 	*datalen = 1;
 
 	//leave power on
-	if (HF14BCmdRaw(true, &crc, true, data, datalen, false)==0) return rawClose();
+	if (HF14BCmdRaw(true, &crc, true, data, datalen, false) == 0) return switch_off_field_14b();
 
-	if (*datalen != 10 || !crc) return rawClose();
+	if (*datalen != 10 || !crc) return switch_off_field_14b();
 
 	//power off ?
-	if (closeCon) rawClose();
+	if (closeCon) switch_off_field_14b();
 
 	PrintAndLog("\n14443-3b ST tag found:");
 	print_st_general_info(data);
 	return 1;
 }
 
+
 // SRx get and print full info (needs more info...)
-int HF14B_ST_Info(uint8_t *data, uint8_t *datalen){
-	if (!HF14B_ST_Reader(data, datalen, false)) return 0;
-	
+static bool HF14B_ST_Info(bool verbose) {
+	uint8_t data[100];
+	uint8_t datalen;
+
+	if (!HF14B_ST_Reader(data, &datalen, false)) return false;
+
 	//add locking bit information here.
-	if (print_ST_Lock_info(data[5]>>2)) 
-		rawClose();
+	if (print_ST_Lock_info(data[5] >> 2))
+		switch_off_field_14b();
 
-	return 1;
+	return true;
 }
 
+
 // test for other 14b type tags (mimic another reader - don't have tags to identify)
-int HF14B_Other_Reader(uint8_t *data, uint8_t *datalen){
+static bool HF14B_Other_Reader(bool verbose) {
+	uint8_t data[4];
+	uint8_t datalen;
+
 	bool crc = true;
-	*datalen = 4;
+	datalen = 4;
 	//std read cmd
 	data[0] = 0x00;
 	data[1] = 0x0b;
 	data[2] = 0x3f;
 	data[3] = 0x80;
 
-	if (HF14BCmdRaw(true, &crc, true, data, datalen, false)!=0) {
-		if (*datalen > 2 || !crc) {
+	if (HF14BCmdRaw(true, &crc, true, data, &datalen, false) != 0) {
+		if (datalen > 2 || !crc) {
 			PrintAndLog ("\n14443-3b tag found:");
 			PrintAndLog ("Unknown tag type answered to a 0x000b3f80 command ans:");
-			PrintAndLog ("%s",sprint_hex(data,*datalen));
-			rawClose();
-			return 1;
+			PrintAndLog ("%s", sprint_hex(data, datalen));
+			switch_off_field_14b();
+			return true;
 		}
 	}
 
 	crc = false;
-	*datalen = 1;
+	datalen = 1;
 	data[0] = 0x0a;
 
-	if (HF14BCmdRaw(true, &crc, true, data, datalen, false)!=0) {
-		if (*datalen > 0) {
+	if (HF14BCmdRaw(true, &crc, true, data, &datalen, false) != 0) {
+		if (datalen > 0) {
 			PrintAndLog ("\n14443-3b tag found:");
 			PrintAndLog ("Unknown tag type answered to a 0x0A command ans:");
-			PrintAndLog ("%s",sprint_hex(data,*datalen));
-			rawClose();
-			return 1;
+			PrintAndLog ("%s", sprint_hex(data, datalen));
+			switch_off_field_14b();
+			return true;
 		}
 	}
-	
+
 	crc = false;
-	*datalen = 1;
+	datalen = 1;
 	data[0] = 0x0c;
 
-	if (HF14BCmdRaw(true, &crc, true, data, datalen, false)!=0) {
-		if (*datalen > 0) {
+	if (HF14BCmdRaw(true, &crc, true, data, &datalen, false) != 0) {
+		if (datalen > 0) {
 			PrintAndLog ("\n14443-3b tag found:");
 			PrintAndLog ("Unknown tag type answered to a 0x0C command ans:");
-			PrintAndLog ("%s",sprint_hex(data,*datalen));
-			rawClose();
-			return 1;
+			PrintAndLog ("%s", sprint_hex(data, datalen));
+			switch_off_field_14b();
+			return true;
 		}
 	}
-	rawClose();
-	return 0;
+	switch_off_field_14b();
+	return false;
 }
 
+
 // get and print all info known about any known 14b tag
-int HF14BInfo(bool verbose){
+static int usage_hf_14b_info(void) {
+	PrintAndLogEx(NORMAL, "Usage: hf 14b info [h] [s]");
+	PrintAndLogEx(NORMAL, "Options:");
+	PrintAndLogEx(NORMAL, "       h    this help");
+	PrintAndLogEx(NORMAL, "       s    silently");
+	PrintAndLogEx(NORMAL, "Example:");
+	PrintAndLogEx(NORMAL, "       hf 14b info");
+	return 0;
+}
+
+int infoHF14B(bool verbose) {
 	uint8_t data[100];
-	uint8_t datalen = 5;
-	
+	uint8_t datalen;
+
 	// try std 14b (atqb)
-	if (HF14BStdInfo(data, &datalen)) return 1;
+	if (HF14B_Std_Info(data, &datalen)) return 1;
 
 	// try st 14b
-	if (HF14B_ST_Info(data, &datalen)) return 1;
+	if (HF14B_ST_Info(verbose)) return 1;
 
 	// try unknown 14b read commands (to be identified later)
 	//   could be read of calypso, CEPAS, moneo, or pico pass.
-	if (HF14B_Other_Reader(data, &datalen)) return 1;
+	if (HF14B_Other_Reader(verbose)) return 1;
 
 	if (verbose) PrintAndLog("no 14443B tag found");
 	return 0;
 }
 
+
 // menu command to get and print all info known about any known 14b tag
-int CmdHF14Binfo(const char *Cmd){
-	return HF14BInfo(true);
+static int CmdHF14Binfo(const char *Cmd){
+	char cmdp = tolower(param_getchar(Cmd, 0));
+	if (cmdp == 'h') return usage_hf_14b_info();
+
+	bool verbose = !(cmdp == 's');
+	return infoHF14B(verbose);
 }
 
+
 // get and print general info about all known 14b chips
-int HF14BReader(bool verbose){
+int readHF14B(bool verbose){
 	uint8_t data[100];
 	uint8_t datalen = 5;
-	
+
 	// try std 14b (atqb)
 	if (HF14BStdReader(data, &datalen)) return 1;
 
@@ -592,33 +631,50 @@ int HF14BReader(bool verbose){
 
 	// try unknown 14b read commands (to be identified later)
 	//   could be read of calypso, CEPAS, moneo, or pico pass.
-	if (HF14B_Other_Reader(data, &datalen)) return 1;
+	if (HF14B_Other_Reader(verbose)) return 1;
 
 	if (verbose) PrintAndLog("no 14443B tag found");
 	return 0;
 }
 
+
 // menu command to get and print general info about all known 14b chips
-int CmdHF14BReader(const char *Cmd){
-	return HF14BReader(true);
+static int usage_hf_14b_reader(void) {
+	PrintAndLogEx(NORMAL, "Usage: hf 14b reader [h] [s]");
+	PrintAndLogEx(NORMAL, "Options:");
+	PrintAndLogEx(NORMAL, "       h    this help");
+	PrintAndLogEx(NORMAL, "       s    silently");
+	PrintAndLogEx(NORMAL, "Example:");
+	PrintAndLogEx(NORMAL, "       hf 14b reader");
+	return 0;
+}
+
+
+static int CmdHF14BReader(const char *Cmd) {
+	char cmdp = tolower(param_getchar(Cmd, 0));
+	if (cmdp == 'h') return usage_hf_14b_reader();
+
+	bool verbose = !(cmdp == 's');
+	return readHF14B(verbose);
 }
 
-int CmdSriWrite( const char *Cmd){
+
+int CmdSriWrite(const char *Cmd) {
 /*
  * For SRIX4K  blocks 00 - 7F
  * hf 14b raw -c -p 09 $srix4kwblock $srix4kwdata
  *
  * For SR512  blocks 00 - 0F
  * hf 14b raw -c -p 09 $sr512wblock $sr512wdata
- * 
+ *
  * Special block FF =  otp_lock_reg block.
  * Data len 4 bytes-
  */
- 	char cmdp = param_getchar(Cmd, 0);
+	char cmdp = param_getchar(Cmd, 0);
 	uint8_t blockno = -1;
 	uint8_t data[4] = {0x00};
 	bool isSrix4k = true;
-	char str[20];	
+	char str[20];
 
 	if (strlen(Cmd) < 1 || cmdp == 'h' || cmdp == 'H') {
 		PrintAndLog("Usage:  hf 14b write <1|2> <BLOCK> <DATA>");
@@ -634,43 +690,46 @@ int CmdSriWrite( const char *Cmd){
 
 	if ( cmdp == '2' )
 		isSrix4k = false;
-	
+
 	//blockno = param_get8(Cmd, 1);
-	
-	if ( param_gethex(Cmd,1, &blockno, 2) ) {
+
+	if (param_gethex(Cmd,1, &blockno, 2) ) {
 		PrintAndLog("Block number must include 2 HEX symbols");
 		return 0;
 	}
-	
-	if ( isSrix4k ){
-		if ( blockno > 0x7f && blockno != 0xff ){
+
+	if (isSrix4k) {
+		if (blockno > 0x7f && blockno != 0xff){
 			PrintAndLog("Block number out of range");
 			return 0;
-		}		
+		}
 	} else {
-		if ( blockno > 0x0f && blockno != 0xff ){
+		if (blockno > 0x0f && blockno != 0xff){
 			PrintAndLog("Block number out of range");
 			return 0;
-		}		
+		}
 	}
-	
+
 	if (param_gethex(Cmd, 2, data, 8)) {
 		PrintAndLog("Data must include 8 HEX symbols");
 		return 0;
 	}
- 
-	if ( blockno == 0xff)
-		PrintAndLog("[%s] Write special block %02X [ %s ]", (isSrix4k)?"SRIX4K":"SRI512" , blockno,  sprint_hex(data,4) );
+
+	if (blockno == 0xff)
+		PrintAndLog("[%s] Write special block %02X [ %s ]", (isSrix4k)?"SRIX4K":"SRI512", blockno, sprint_hex(data, 4));
 	else
-		PrintAndLog("[%s] Write block %02X [ %s ]", (isSrix4k)?"SRIX4K":"SRI512", blockno,  sprint_hex(data,4) );
- 
+		PrintAndLog("[%s] Write block %02X [ %s ]", (isSrix4k)?"SRIX4K":"SRI512", blockno, sprint_hex(data, 4));
+
 	sprintf(str, "-c 09 %02x %02x%02x%02x%02x", blockno, data[0], data[1], data[2], data[3]);
 
 	CmdHF14BCmdRaw(str);
 	return 0;
 }
 
-static command_t CommandTable[] = 
+
+static int CmdHelp(const char *Cmd);
+
+static command_t CommandTable[] =
 {
   {"help",        CmdHelp,        1, "This help"},
   {"info",        CmdHF14Binfo,   0, "Find and print details about a 14443B tag"},
diff --git a/client/cmdhf14b.h b/client/cmdhf14b.h
index 4fcae927..4897d879 100644
--- a/client/cmdhf14b.h
+++ b/client/cmdhf14b.h
@@ -21,6 +21,6 @@ int CmdHF14BSnoop(const char *Cmd);
 int CmdSri512Read(const char *Cmd);
 int CmdSrix4kRead(const char *Cmd);
 int CmdHF14BWrite( const char *cmd);
-int HF14BInfo(bool verbose);
+int infoHF14B(bool verbose);
 
 #endif
diff --git a/client/cmdhf15.c b/client/cmdhf15.c
index c2a13354..6bcad201 100644
--- a/client/cmdhf15.c
+++ b/client/cmdhf15.c
@@ -464,7 +464,7 @@ int CmdHF15CmdRaw (const char *cmd) {
 	char *hexout;
 
 	
-	if (strlen(cmd)<3) {
+	if (strlen(cmd)<2) {
 		PrintAndLog("Usage: hf 15 cmd raw  [-r] [-2] [-c] <0A 0B 0C ... hex>");
 		PrintAndLog("       -r    do not read response");
 		PrintAndLog("       -2    use slower '1 out of 256' mode");
@@ -526,22 +526,31 @@ int CmdHF15CmdRaw (const char *cmd) {
 	SendCommand(&c);
 	
 	if (reply) {
-		if (WaitForResponseTimeout(CMD_ACK,&resp,1000)) {
+		if (WaitForResponseTimeout(CMD_ACK, &resp, 1000)) {
 			recv = resp.d.asBytes;
-			PrintAndLog("received %i octets",resp.arg[0]);
-			hexout = (char *)malloc(resp.arg[0] * 3 + 1);
-			if (hexout != NULL) {
-				for (int i = 0; i < resp.arg[0]; i++) { // data in hex
-					sprintf(&hexout[i * 3], "%02X ", recv[i]);
+			int recv_len = resp.arg[0];
+			if (recv_len == 0) {
+				PrintAndLog("received SOF only. Maybe Picopass/iCLASS?");
+			} else if (recv_len > 0) {
+				PrintAndLog("received %i octets", recv_len);
+				hexout = (char *)malloc(resp.arg[0] * 3 + 1);
+				if (hexout != NULL) {
+					for (int i = 0; i < resp.arg[0]; i++) { // data in hex
+						sprintf(&hexout[i * 3], "%02X ", recv[i]);
+					}
+					PrintAndLog("%s", hexout);
+					free(hexout);
 				}
-				PrintAndLog("%s", hexout);
-				free(hexout);
+			} else if (recv_len == -1) {
+				PrintAndLog("card didn't respond");
+			} else if (recv_len == -2) {
+				PrintAndLog("receive buffer overflow");
 			}
 		} else {
 			PrintAndLog("timeout while waiting for reply.");
 		}
-		
-	} // if reply
+	}
+
 	return 0;
 }
 
diff --git a/client/cmdhficlass.c b/client/cmdhficlass.c
index 81738686..195a282d 100644
--- a/client/cmdhficlass.c
+++ b/client/cmdhficlass.c
@@ -35,7 +35,6 @@
 #include "util_posix.h"
 #include "cmdhf14a.h" // DropField()
 
-static int CmdHelp(const char *Cmd);
 
 #define ICLASS_KEYS_MAX 8
 static uint8_t iClass_Key_Table[ICLASS_KEYS_MAX][8] = {
@@ -49,12 +48,14 @@ static uint8_t iClass_Key_Table[ICLASS_KEYS_MAX][8] = {
 		{ 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 }
 };
 
+
 typedef struct iclass_block {
-    uint8_t d[8];
+	uint8_t d[8];
 } iclass_block_t;
 
-int usage_hf_iclass_chk(void) {
-	PrintAndLog("Checkkeys loads a dictionary text file with 8byte hex keys to test authenticating against a iClass tag");	
+
+static void usage_hf_iclass_chk(void) {
+	PrintAndLog("Checkkeys loads a dictionary text file with 8byte hex keys to test authenticating against a iClass tag");
 	PrintAndLog("Usage: hf iclass chk [h|e|r] <f  (*.dic)>");
 	PrintAndLog("Options:");
 	PrintAndLog("h             Show this help");
@@ -62,31 +63,25 @@ int usage_hf_iclass_chk(void) {
 	PrintAndLog("      e             target Elite / High security key scheme");
 	PrintAndLog("      r             interpret dictionary file as raw (diversified keys)");
 	PrintAndLog("Samples:");
-	PrintAndLog("		 hf iclass chk f default_iclass_keys.dic");	
-	PrintAndLog("		 hf iclass chk f default_iclass_keys.dic e");	
-	return 0;
+	PrintAndLog("        hf iclass chk f default_iclass_keys.dic");
+	PrintAndLog("        hf iclass chk f default_iclass_keys.dic e");
 }
 
-int xorbits_8(uint8_t val) {
-	uint8_t res = val ^ (val >> 1); //1st pass
-	res = res ^ (res >> 1); 		// 2nd pass
-	res = res ^ (res >> 2); 		// 3rd pass
-	res = res ^ (res >> 4); 			// 4th pass
-	return res & 1;
-}
 
-int CmdHFiClassList(const char *Cmd) {
+static int CmdHFiClassList(const char *Cmd) {
 	PrintAndLog("Deprecated command, use 'hf list iclass' instead");
 	return 0;
 }
 
-int CmdHFiClassSnoop(const char *Cmd) {
+
+static int CmdHFiClassSnoop(const char *Cmd) {
 	UsbCommand c = {CMD_SNOOP_ICLASS};
 	SendCommand(&c);
 	return 0;
 }
 
-int usage_hf_iclass_sim(void) {
+
+static void usage_hf_iclass_sim(void) {
 	PrintAndLog("Usage:  hf iclass sim <option> [CSN]");
 	PrintAndLog("        options");
 	PrintAndLog("                0 <CSN> simulate the given CSN");
@@ -97,9 +92,9 @@ int usage_hf_iclass_sim(void) {
 	PrintAndLog("        example: hf iclass sim 2");
 	PrintAndLog("        example: hf iclass eload 'tagdump.bin'");
 	PrintAndLog("                 hf iclass sim 3");
-	return 0;
 }
 
+
 // the original malicious IDs from Flavio D. Garcia, Gerhard de Koning Gans, Roel Verdult,
 // and Milosch Meriac. Dismantling iClass and iClass Elite.
 #define NUM_CSNS 15
@@ -127,32 +122,34 @@ static uint8_t csns[8 * NUM_CSNS] = {
 
 // #define NUM_CSNS 9
 // static uint8_t csns[8 * NUM_CSNS] = {
-    // 0x01, 0x0A, 0x0F, 0xFF, 0xF7, 0xFF, 0x12, 0xE0,
-    // 0x0C, 0x06, 0x0C, 0xFE, 0xF7, 0xFF, 0x12, 0xE0,
-    // 0x10, 0x97, 0x83, 0x7B, 0xF7, 0xFF, 0x12, 0xE0,
-    // 0x13, 0x97, 0x82, 0x7A, 0xF7, 0xFF, 0x12, 0xE0,
-    // 0x07, 0x0E, 0x0D, 0xF9, 0xF7, 0xFF, 0x12, 0xE0,
-    // 0x14, 0x96, 0x84, 0x76, 0xF7, 0xFF, 0x12, 0xE0,
-    // 0x17, 0x96, 0x85, 0x71, 0xF7, 0xFF, 0x12, 0xE0,
-    // 0xCE, 0xC5, 0x0F, 0x77, 0xF7, 0xFF, 0x12, 0xE0,
-    // 0xD2, 0x5A, 0x82, 0xF8, 0xF7, 0xFF, 0x12, 0xE0
-    // //0x04, 0x08, 0x9F, 0x78, 0x6E, 0xFF, 0x12, 0xE0
+	// 0x01, 0x0A, 0x0F, 0xFF, 0xF7, 0xFF, 0x12, 0xE0,
+	// 0x0C, 0x06, 0x0C, 0xFE, 0xF7, 0xFF, 0x12, 0xE0,
+	// 0x10, 0x97, 0x83, 0x7B, 0xF7, 0xFF, 0x12, 0xE0,
+	// 0x13, 0x97, 0x82, 0x7A, 0xF7, 0xFF, 0x12, 0xE0,
+	// 0x07, 0x0E, 0x0D, 0xF9, 0xF7, 0xFF, 0x12, 0xE0,
+	// 0x14, 0x96, 0x84, 0x76, 0xF7, 0xFF, 0x12, 0xE0,
+	// 0x17, 0x96, 0x85, 0x71, 0xF7, 0xFF, 0x12, 0xE0,
+	// 0xCE, 0xC5, 0x0F, 0x77, 0xF7, 0xFF, 0x12, 0xE0,
+	// 0xD2, 0x5A, 0x82, 0xF8, 0xF7, 0xFF, 0x12, 0xE0
+	// //0x04, 0x08, 0x9F, 0x78, 0x6E, 0xFF, 0x12, 0xE0
 // };
 
 
-int CmdHFiClassSim(const char *Cmd) {
+static int CmdHFiClassSim(const char *Cmd) {
 	uint8_t simType = 0;
 	uint8_t CSN[8] = {0, 0, 0, 0, 0, 0, 0, 0};
 
 	if (strlen(Cmd) < 1) {
-		return usage_hf_iclass_sim();
+		usage_hf_iclass_sim();
+		return 0;
 	}
 	simType = param_get8ex(Cmd, 0, 0, 10);
 
 	if (simType == ICLASS_SIM_MODE_CSN) {
 		if (param_gethex(Cmd, 1, CSN, 16)) {
 			PrintAndLog("A CSN should consist of 16 HEX symbols");
-			return usage_hf_iclass_sim();
+			usage_hf_iclass_sim();
+			return 0;
 		}
 		PrintAndLog("--simtype:%02x csn:%s", simType, sprint_hex(CSN, 8));
 	}
@@ -186,8 +183,8 @@ int CmdHFiClassSim(const char *Cmd) {
 		void* dump = malloc(datalen);
 		for(int i = 0; i < NUM_CSNS; i++) {
 			memcpy(dump + i*24, csns+i*8, 8); //CSN
-            //copy CC from response
-            memcpy(dump + i*24 + 8, resp.d.asBytes + i*16, 8);
+			//copy CC from response
+			memcpy(dump + i*24 + 8, resp.d.asBytes + i*16, 8);
 			//Then comes NR_MAC (eight bytes from the response)
 			memcpy(dump + i*24 + 16, resp.d.asBytes + i*16 + 8, 8);
 		}
@@ -202,17 +199,18 @@ int CmdHFiClassSim(const char *Cmd) {
 
 	} else {
 		PrintAndLog("Undefined simtype %d", simType);
-		return usage_hf_iclass_sim();
+		usage_hf_iclass_sim();
+		return 0;
 	}
 
 	return 0;
 }
 
+
 int HFiClassReader(const char *Cmd, bool loop, bool verbose) {
 	bool tagFound = false;
 	UsbCommand c = {CMD_READER_ICLASS, {FLAG_ICLASS_READER_CSN |
-		    FLAG_ICLASS_READER_CC | FLAG_ICLASS_READER_CONF | FLAG_ICLASS_READER_AA |
-		    FLAG_ICLASS_READER_ONLY_ONCE | FLAG_ICLASS_READER_ONE_TRY } };
+			FLAG_ICLASS_READER_CC | FLAG_ICLASS_READER_CONF | FLAG_ICLASS_READER_AA} };
 	// loop in client not device - else on windows have a communication error
 	UsbCommand resp;
 	while(!ukbhit()){
@@ -232,7 +230,7 @@ int HFiClassReader(const char *Cmd, bool loop, bool verbose) {
 				PrintAndLog("   CSN: %s",sprint_hex(data,8));
 				tagFound = true;
 			}
-			if( readStatus & FLAG_ICLASS_READER_CC) { 
+			if( readStatus & FLAG_ICLASS_READER_CC) {
 				PrintAndLog("    CC: %s",sprint_hex(data+16,8));
 			}
 			if( readStatus & FLAG_ICLASS_READER_CONF) {
@@ -244,7 +242,7 @@ int HFiClassReader(const char *Cmd, bool loop, bool verbose) {
 				for (int i = 0; i<8; i++) {
 					if (data[8*5+i] != 0xFF) {
 						legacy = false;
-					} 
+					}
 				}
 				PrintAndLog("      : Possible iClass %s",(legacy) ? "(legacy tag)" : "(NOT legacy tag)");
 			}
@@ -258,11 +256,13 @@ int HFiClassReader(const char *Cmd, bool loop, bool verbose) {
 	return 0;
 }
 
-int CmdHFiClassReader(const char *Cmd) {
+
+static int CmdHFiClassReader(const char *Cmd) {
 	return HFiClassReader(Cmd, true, true);
 }
 
-int CmdHFiClassReader_Replay(const char *Cmd) {
+
+static int CmdHFiClassReader_Replay(const char *Cmd) {
 	uint8_t readerType = 0;
 	uint8_t MAC[4]={0x00, 0x00, 0x00, 0x00};
 
@@ -284,26 +284,22 @@ int CmdHFiClassReader_Replay(const char *Cmd) {
 	return 0;
 }
 
-int iclassEmlSetMem(uint8_t *data, int blockNum, int blocksCount) {
-	UsbCommand c = {CMD_MIFARE_EML_MEMSET, {blockNum, blocksCount, 0}};
-	memcpy(c.d.asBytes, data, blocksCount * 16);
-	SendCommand(&c);
-	return 0;
-}
 
-int hf_iclass_eload_usage(void) {
+static void usage_hf_iclass_eload(void) {
 	PrintAndLog("Loads iclass tag-dump into emulator memory on device");
 	PrintAndLog("Usage:  hf iclass eload f <filename>");
 	PrintAndLog("");
 	PrintAndLog("Example: hf iclass eload f iclass_tagdump-aa162d30f8ff12f1.bin");
-	return 0;
 }
 
-int CmdHFiClassELoad(const char *Cmd) {
+
+static int CmdHFiClassELoad(const char *Cmd) {
 
 	char opt = param_getchar(Cmd, 0);
-	if (strlen(Cmd)<1 || opt == 'h')
-		return hf_iclass_eload_usage();
+	if (strlen(Cmd)<1 || opt == 'h') {
+		usage_hf_iclass_eload();
+		return 0;
+	}
 
 	//File handling and reading
 	FILE *f;
@@ -311,7 +307,8 @@ int CmdHFiClassELoad(const char *Cmd) {
 	if (opt == 'f' && param_getstr(Cmd, 1, filename, sizeof(filename)) > 0) {
 		f = fopen(filename, "rb");
 	} else {
-		return hf_iclass_eload_usage();
+		usage_hf_iclass_eload();
+		return 0;
 	}
 
 	if (!f) {
@@ -337,7 +334,7 @@ int CmdHFiClassELoad(const char *Cmd) {
 	printIclassDumpInfo(dump);
 	//Validate
 
-	if (bytes_read < fsize)	{
+	if (bytes_read < fsize) {
 		prnlog("Error, could only read %d bytes (should be %d)",bytes_read, fsize );
 		free(dump);
 		return 1;
@@ -359,6 +356,7 @@ int CmdHFiClassELoad(const char *Cmd) {
 	return 0;
 }
 
+
 static int readKeyfile(const char *filename, size_t len, uint8_t* buffer) {
 	FILE *f = fopen(filename, "rb");
 	if(!f) {
@@ -383,7 +381,8 @@ static int readKeyfile(const char *filename, size_t len, uint8_t* buffer) {
 	return 0;
 }
 
-int usage_hf_iclass_decrypt(void) {
+
+static void usage_hf_iclass_decrypt(void) {
 	PrintAndLog("Usage: hf iclass decrypt f <tagdump>");
 	PrintAndLog("");
 	PrintAndLog("OBS! In order to use this function, the file 'iclass_decryptionkey.bin' must reside");
@@ -394,10 +393,10 @@ int usage_hf_iclass_decrypt(void) {
 	PrintAndLog("OBS! This is pretty stupid implementation, it tries to decrypt every block after block 6. ");
 	PrintAndLog("Correct behaviour would be to decrypt only the application areas where the key is valid,");
 	PrintAndLog("which is defined by the configuration block.");
-	return 1;
 }
 
-int CmdHFiClassDecrypt(const char *Cmd) {
+
+static int CmdHFiClassDecrypt(const char *Cmd) {
 	uint8_t key[16] = { 0 };
 	if(readKeyfile("iclass_decryptionkey.bin", 16, key))
 	{
@@ -406,8 +405,10 @@ int CmdHFiClassDecrypt(const char *Cmd) {
 	}
 	PrintAndLog("Decryption file found... ");
 	char opt = param_getchar(Cmd, 0);
-	if (strlen(Cmd)<1 || opt == 'h')
-		return usage_hf_iclass_decrypt();
+	if (strlen(Cmd)<1 || opt == 'h') {
+		usage_hf_iclass_decrypt();
+		return 0;
+	}
 
 	//Open the tagdump-file
 	FILE *f;
@@ -419,7 +420,8 @@ int CmdHFiClassDecrypt(const char *Cmd) {
 			return 1;
 		}
 	} else {
-		return usage_hf_iclass_decrypt();
+		usage_hf_iclass_decrypt();
+		return 0;
 	}
 
 	fseek(f, 0, SEEK_END);
@@ -457,7 +459,8 @@ int CmdHFiClassDecrypt(const char *Cmd) {
 	return 0;
 }
 
-int usage_hf_iclass_encrypt(void) {
+
+static void usage_hf_iclass_encrypt(void) {
 	PrintAndLog("Usage: hf iclass encrypt <BlockData>");
 	PrintAndLog("");
 	PrintAndLog("OBS! In order to use this function, the file 'iclass_decryptionkey.bin' must reside");
@@ -465,9 +468,9 @@ int usage_hf_iclass_encrypt(void) {
 	PrintAndLog("");
 	PrintAndLog("example: hf iclass encrypt 0102030405060708");
 	PrintAndLog("");
-	return 0;
 }
 
+
 static int iClassEncryptBlkData(uint8_t *blkData) {
 	uint8_t key[16] = { 0 };
 	if(readKeyfile("iclass_decryptionkey.bin", 16, key))
@@ -481,7 +484,7 @@ static int iClassEncryptBlkData(uint8_t *blkData) {
 	uint8_t *encrypted = encryptedData;
 	mbedtls_des3_context ctx = { {0} };
 	mbedtls_des3_set2key_enc( &ctx, key);
-	
+
 	mbedtls_des3_crypt_ecb(&ctx, blkData,encrypted);
 	//printvar("decrypted block", decrypted, 8);
 	memcpy(blkData,encrypted,8);
@@ -489,25 +492,26 @@ static int iClassEncryptBlkData(uint8_t *blkData) {
 	return 1;
 }
 
-int CmdHFiClassEncryptBlk(const char *Cmd) {
+
+static int CmdHFiClassEncryptBlk(const char *Cmd) {
 	uint8_t blkData[8] = {0};
 	char opt = param_getchar(Cmd, 0);
-	if (strlen(Cmd)<1 || opt == 'h')
-		return usage_hf_iclass_encrypt();
-
+	if (strlen(Cmd)<1 || opt == 'h') {
+		usage_hf_iclass_encrypt();
+		return 0;
+	}
 	//get the bytes to encrypt
-	if (param_gethex(Cmd, 0, blkData, 16))
-	{
+	if (param_gethex(Cmd, 0, blkData, 16)) {
 		PrintAndLog("BlockData must include 16 HEX symbols");
 		return 0;
 	}
 	if (!iClassEncryptBlkData(blkData)) return 0;
-
 	printvar("encrypted block", blkData, 8);
 	return 1;
 }
 
-void Calc_wb_mac(uint8_t blockno, uint8_t *data, uint8_t *div_key, uint8_t MAC[4]) {
+
+static void Calc_wb_mac(uint8_t blockno, uint8_t *data, uint8_t *div_key, uint8_t MAC[4]) {
 	uint8_t WB[9];
 	WB[0] = blockno;
 	memcpy(WB + 1,data,8);
@@ -515,18 +519,15 @@ void Calc_wb_mac(uint8_t blockno, uint8_t *data, uint8_t *div_key, uint8_t MAC[4
 	//printf("Cal wb mac block [%02x][%02x%02x%02x%02x%02x%02x%02x%02x] : MAC [%02x%02x%02x%02x]",WB[0],WB[1],WB[2],WB[3],WB[4],WB[5],WB[6],WB[7],WB[8],MAC[0],MAC[1],MAC[2],MAC[3]);
 }
 
-static bool select_only(uint8_t *CSN, uint8_t *CCNR, bool use_credit_key, bool verbose) {
+
+static bool select_only(uint8_t *CSN, bool verbose) {
 	UsbCommand resp;
 
 	UsbCommand c = {CMD_READER_ICLASS, {0}};
-	c.arg[0] = FLAG_ICLASS_READER_ONLY_ONCE | FLAG_ICLASS_READER_CC | FLAG_ICLASS_READER_ONE_TRY;
-	if (use_credit_key)
-		c.arg[0] |= FLAG_ICLASS_READER_CEDITKEY;
 
 	clearCommandBuffer();
 	SendCommand(&c);
-	if (!WaitForResponseTimeout(CMD_ACK,&resp,4500))
-	{
+	if (!WaitForResponseTimeout(CMD_ACK, &resp, 4500)) {
 		PrintAndLog("Command execute timeout");
 		return false;
 	}
@@ -534,45 +535,81 @@ static bool select_only(uint8_t *CSN, uint8_t *CCNR, bool use_credit_key, bool v
 	uint8_t isOK = resp.arg[0] & 0xff;
 	uint8_t *data = resp.d.asBytes;
 
-	memcpy(CSN,data,8);
-	if (CCNR!=NULL)memcpy(CCNR,data+16,8);
-	if(isOK > 0)
-	{
-		if (verbose) PrintAndLog("CSN: %s",sprint_hex(CSN,8));
-	}
-	if(isOK <= 1){
-		PrintAndLog("Failed to obtain CC! Aborting");
+	memcpy(CSN, data, 8);
+
+	if (isOK > 0) {
+		if (verbose) PrintAndLog("CSN: %s", sprint_hex(CSN, 8));
+	} else {
+		PrintAndLog("Failed to select card! Aborting");
 		return false;
 	}
-	return true;	
+	return true;
 }
 
+
+static void HFiClassCalcDivKey(uint8_t *CSN, uint8_t *KEY, uint8_t *div_key, bool elite){
+	uint8_t keytable[128] = {0};
+	uint8_t key_index[8] = {0};
+	if (elite) {
+		uint8_t key_sel[8] = { 0 };
+		uint8_t key_sel_p[8] = { 0 };
+		hash2(KEY, keytable);
+		hash1(CSN, key_index);
+		for(uint8_t i = 0; i < 8 ; i++)
+			key_sel[i] = keytable[key_index[i]] & 0xFF;
+
+		//Permute from iclass format to standard format
+		permutekey_rev(key_sel, key_sel_p);
+		diversifyKey(CSN, key_sel_p, div_key);
+	} else {
+		diversifyKey(CSN, KEY, div_key);
+	}
+}
+
+
 static bool select_and_auth(uint8_t *KEY, uint8_t *MAC, uint8_t *div_key, bool use_credit_key, bool elite, bool rawkey, bool verbose) {
-	uint8_t CSN[8]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
-	uint8_t CCNR[12]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
+	uint8_t CSN[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
+	uint8_t CCNR[12] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
 
-	if (!select_only(CSN, CCNR, use_credit_key, verbose))
+	if (!select_only(CSN, verbose))
 		return false;
 
 	//get div_key
-	if(rawkey)
+	if (rawkey)
 		memcpy(div_key, KEY, 8);
 	else
 		HFiClassCalcDivKey(CSN, KEY, div_key, elite);
-	 if (verbose) PrintAndLog("Authing with %s: %02x%02x%02x%02x%02x%02x%02x%02x", rawkey ? "raw key" : "diversified key", div_key[0],div_key[1],div_key[2],div_key[3],div_key[4],div_key[5],div_key[6],div_key[7]);
 
-	doMAC(CCNR, div_key, MAC);
+	if (verbose) PrintAndLog("Authenticating with %s: %s", rawkey ? "raw key" : "diversified key", sprint_hex(div_key, 8));
+
 	UsbCommand resp;
-	UsbCommand d = {CMD_ICLASS_AUTHENTICATION, {0}};
+	UsbCommand d = {CMD_ICLASS_READCHECK, {2, use_credit_key, 0}};
+
+	clearCommandBuffer();
+	SendCommand(&d);
+
+	if (!WaitForResponseTimeout(CMD_ACK, &resp, 4500)) {
+		if (verbose) PrintAndLog("Auth Command (READCHECK[2]) execute timeout");
+		return false;
+	}
+	bool isOK = resp.arg[0];
+	if (!isOK) {
+		if (verbose) PrintAndLog("Couldn't get Card Challenge");
+		return false;
+	}
+	memcpy(CCNR, resp.d.asBytes, 8);
+
+	doMAC(CCNR, div_key, MAC);
+
+	d.cmd = CMD_ICLASS_CHECK;
 	memcpy(d.d.asBytes, MAC, 4);
 	clearCommandBuffer();
 	SendCommand(&d);
-	if (!WaitForResponseTimeout(CMD_ACK,&resp,4500))
-	{
-		if (verbose) PrintAndLog("Auth Command execute timeout");
+	if (!WaitForResponseTimeout(CMD_ACK, &resp, 4500)) {
+		if (verbose) PrintAndLog("Auth Command (CHECK) execute timeout");
 		return false;
 	}
-	uint8_t isOK = resp.arg[0] & 0xff;
+	isOK = resp.arg[0];
 	if (!isOK) {
 		if (verbose) PrintAndLog("Authentication error");
 		return false;
@@ -580,7 +617,8 @@ static bool select_and_auth(uint8_t *KEY, uint8_t *MAC, uint8_t *div_key, bool u
 	return true;
 }
 
-int usage_hf_iclass_dump(void) {
+
+static void usage_hf_iclass_dump(void) {
 	PrintAndLog("Usage:  hf iclass dump f <fileName> k <Key> c <CreditKey> e|r\n");
 	PrintAndLog("Options:");
 	PrintAndLog("  f <filename> : specify a filename to save dump to");
@@ -591,14 +629,45 @@ int usage_hf_iclass_dump(void) {
 	PrintAndLog("                 See 'hf iclass sim 2'. This key should be on iclass-format");
 	PrintAndLog("  r            : If 'r' is specified, the key is interpreted as raw block 3/4");
 	PrintAndLog("  NOTE: * = required");
-  PrintAndLog("Samples:");
+	PrintAndLog("Samples:");
 	PrintAndLog("  hf iclass dump k 001122334455667B");
 	PrintAndLog("  hf iclass dump k AAAAAAAAAAAAAAAA c 001122334455667B");
 	PrintAndLog("  hf iclass dump k AAAAAAAAAAAAAAAA e");
-	return 0;
 }
 
-int CmdHFiClassReader_Dump(const char *Cmd) {
+
+static void printIclassDumpContents(uint8_t *iclass_dump, uint8_t startblock, uint8_t endblock, size_t filesize) {
+	uint8_t mem_config;
+	memcpy(&mem_config, iclass_dump + 13,1);
+	uint8_t maxmemcount;
+	uint8_t filemaxblock = filesize / 8;
+	if (mem_config & 0x80)
+		maxmemcount = 255;
+	else
+		maxmemcount = 31;
+	//PrintAndLog   ("endblock: %d, filesize: %d, maxmemcount: %d, filemaxblock: %d", endblock,filesize, maxmemcount, filemaxblock);
+
+	if (startblock == 0)
+		startblock = 6;
+	if ((endblock > maxmemcount) || (endblock == 0))
+		endblock = maxmemcount;
+
+	// remember endblock need to relate to zero-index arrays.
+	if (endblock > filemaxblock-1)
+		endblock = filemaxblock;
+
+	int i = startblock;
+	printf("------+--+-------------------------+\n");
+	while (i <= endblock) {
+		uint8_t *blk = iclass_dump + (i * 8);
+		printf("Block |%02X| %s|\n", i, sprint_hex(blk, 8) );
+		i++;
+	}
+	printf("------+--+-------------------------+\n");
+}
+
+
+static int CmdHFiClassReader_Dump(const char *Cmd) {
 
 	uint8_t MAC[4] = {0x00,0x00,0x00,0x00};
 	uint8_t div_key[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
@@ -629,7 +698,8 @@ int CmdHFiClassReader_Dump(const char *Cmd) {
 		{
 		case 'h':
 		case 'H':
-			return usage_hf_iclass_dump();
+			usage_hf_iclass_dump();
+			return 0;
 		case 'c':
 		case 'C':
 			have_credit_key = true;
@@ -657,7 +727,7 @@ int CmdHFiClassReader_Dump(const char *Cmd) {
 			break;
 		case 'f':
 		case 'F':
-			fileNameLen = param_getstr(Cmd, cmdp+1, filename, sizeof(filename)); 
+			fileNameLen = param_getstr(Cmd, cmdp+1, filename, sizeof(filename));
 			if (fileNameLen < 1) {
 				PrintAndLog("No filename found after f");
 				errors = true;
@@ -668,7 +738,7 @@ int CmdHFiClassReader_Dump(const char *Cmd) {
 		case 'K':
 			have_debit_key = true;
 			dataLen = param_getstr(Cmd, cmdp+1, tempStr, sizeof(tempStr));
-			if (dataLen == 16) { 
+			if (dataLen == 16) {
 				errors = param_gethex(tempStr, 0, KEY, dataLen);
 			} else if (dataLen == 1) {
 				keyNbr = param_get8(Cmd, cmdp+1);
@@ -694,16 +764,21 @@ int CmdHFiClassReader_Dump(const char *Cmd) {
 			errors = true;
 			break;
 		}
-		if(errors) return usage_hf_iclass_dump();
+		if (errors) {
+			usage_hf_iclass_dump();
+			return 0;
+		}
 	}
 
-	if (cmdp < 2) return usage_hf_iclass_dump();
+	if (cmdp < 2) {
+		usage_hf_iclass_dump();
+		return 0;
+	}
 	// if no debit key given try credit key on AA1 (not for iclass but for some picopass this will work)
 	if (!have_debit_key && have_credit_key) use_credit_key = true;
 
 	//get config and first 3 blocks
-	UsbCommand c = {CMD_READER_ICLASS, {FLAG_ICLASS_READER_CSN |
-					FLAG_ICLASS_READER_CONF | FLAG_ICLASS_READER_ONLY_ONCE | FLAG_ICLASS_READER_ONE_TRY}};
+	UsbCommand c = {CMD_READER_ICLASS, {FLAG_ICLASS_READER_CSN | FLAG_ICLASS_READER_CONF}};
 	UsbCommand resp;
 	uint8_t tag_data[255*8];
 
@@ -739,7 +814,7 @@ int CmdHFiClassReader_Dump(const char *Cmd) {
 			return 0;
 		}
 	}
-	
+
 	// begin dump
 	UsbCommand w = {CMD_ICLASS_DUMP, {blockno, numblks-blockno+1}};
 	clearCommandBuffer();
@@ -796,7 +871,7 @@ int CmdHFiClassReader_Dump(const char *Cmd) {
 				PrintAndLog("Read Block Failed 2");
 				DropField();
 				return 0;
-			}		
+			}
 
 			startindex = resp.arg[2];
 			if (blocksRead*8 > sizeof(tag_data)-gotBytes) {
@@ -806,7 +881,7 @@ int CmdHFiClassReader_Dump(const char *Cmd) {
 			// get dumped data from bigbuf
 			GetFromBigBuf(tag_data+gotBytes, blocksRead*8, startindex, NULL, -1, false);
 
-			gotBytes += blocksRead*8;			
+			gotBytes += blocksRead*8;
 		} else { //field is still on - turn it off...
 			DropField();
 		}
@@ -819,11 +894,11 @@ int CmdHFiClassReader_Dump(const char *Cmd) {
 	printf("------+--+-------------------------+\n");
 	printf("CSN   |00| %s|\n",sprint_hex(tag_data, 8));
 	printIclassDumpContents(tag_data, 1, (gotBytes/8), gotBytes);
-	
+
 	if (filename[0] == 0){
 		snprintf(filename, FILE_PATH_SIZE,"iclass_tagdump-%02x%02x%02x%02x%02x%02x%02x%02x",
-		    tag_data[0],tag_data[1],tag_data[2],tag_data[3],
-		    tag_data[4],tag_data[5],tag_data[6],tag_data[7]);
+			tag_data[0],tag_data[1],tag_data[2],tag_data[3],
+			tag_data[4],tag_data[5],tag_data[6],tag_data[7]);
 	}
 
 	// save the dump to .bin file
@@ -832,6 +907,7 @@ int CmdHFiClassReader_Dump(const char *Cmd) {
 	return 1;
 }
 
+
 static int WriteBlock(uint8_t blockno, uint8_t *bldata, uint8_t *KEY, bool use_credit_key, bool elite, bool rawkey, bool verbose) {
 	uint8_t MAC[4]={0x00,0x00,0x00,0x00};
 	uint8_t div_key[8]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
@@ -844,7 +920,7 @@ static int WriteBlock(uint8_t blockno, uint8_t *bldata, uint8_t *KEY, bool use_c
 	UsbCommand w = {CMD_ICLASS_WRITEBLOCK, {blockno}};
 	memcpy(w.d.asBytes, bldata, 8);
 	memcpy(w.d.asBytes + 8, MAC, 4);
-	
+
 	clearCommandBuffer();
 	SendCommand(&w);
 	if (!WaitForResponseTimeout(CMD_ACK,&resp,4500))
@@ -861,22 +937,23 @@ static int WriteBlock(uint8_t blockno, uint8_t *bldata, uint8_t *KEY, bool use_c
 	return 1;
 }
 
-int usage_hf_iclass_writeblock(void) {
+
+static void usage_hf_iclass_writeblock(void) {
 	PrintAndLog("Options:");
-  PrintAndLog("  b <Block> : The block number as 2 hex symbols");
-  PrintAndLog("  d <data>  : Set the Data to write as 16 hex symbols");
+	PrintAndLog("  b <Block> : The block number as 2 hex symbols");
+	PrintAndLog("  d <data>  : Set the Data to write as 16 hex symbols");
 	PrintAndLog("  k <Key>   : Access Key as 16 hex symbols or 1 hex to select key from memory");
-  PrintAndLog("  c         : If 'c' is specified, the key set is assumed to be the credit key\n");
-  PrintAndLog("  e         : If 'e' is specified, elite computations applied to key");
-  PrintAndLog("  r         : If 'r' is specified, no computations applied to key");
-  PrintAndLog("Samples:");
+	PrintAndLog("  c         : If 'c' is specified, the key set is assumed to be the credit key\n");
+	PrintAndLog("  e         : If 'e' is specified, elite computations applied to key");
+	PrintAndLog("  r         : If 'r' is specified, no computations applied to key");
+	PrintAndLog("Samples:");
 	PrintAndLog("  hf iclass writeblk b 0A d AAAAAAAAAAAAAAAA k 001122334455667B");
 	PrintAndLog("  hf iclass writeblk b 1B d AAAAAAAAAAAAAAAA k 001122334455667B c");
 	PrintAndLog("  hf iclass writeblk b 0A d AAAAAAAAAAAAAAAA n 0");
-	return 0;
 }
 
-int CmdHFiClass_WriteBlock(const char *Cmd) {
+
+static int CmdHFiClass_WriteBlock(const char *Cmd) {
 	uint8_t blockno=0;
 	uint8_t bldata[8]={0,0,0,0,0,0,0,0};
 	uint8_t KEY[8]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
@@ -894,7 +971,8 @@ int CmdHFiClass_WriteBlock(const char *Cmd) {
 		{
 		case 'h':
 		case 'H':
-			return usage_hf_iclass_writeblock();
+			usage_hf_iclass_writeblock();
+			return 0;
 		case 'b':
 		case 'B':
 			if (param_gethex(Cmd, cmdp+1, &blockno, 2)) {
@@ -925,7 +1003,7 @@ int CmdHFiClass_WriteBlock(const char *Cmd) {
 		case 'k':
 		case 'K':
 			dataLen = param_getstr(Cmd, cmdp+1, tempStr, sizeof(tempStr));
-			if (dataLen == 16) { 
+			if (dataLen == 16) {
 				errors = param_gethex(tempStr, 0, KEY, dataLen);
 			} else if (dataLen == 1) {
 				keyNbr = param_get8(Cmd, cmdp+1);
@@ -951,16 +1029,23 @@ int CmdHFiClass_WriteBlock(const char *Cmd) {
 			errors = true;
 			break;
 		}
-		if(errors) return usage_hf_iclass_writeblock();
+		if(errors) {
+			usage_hf_iclass_writeblock();
+			return 0;
+		}
 	}
 
-	if (cmdp < 6) return usage_hf_iclass_writeblock();
+	if (cmdp < 6) {
+		usage_hf_iclass_writeblock();
+		return 0;
+	}
 	int ans = WriteBlock(blockno, bldata, KEY, use_credit_key, elite, rawkey, true);
 	DropField();
 	return ans;
 }
 
-int usage_hf_iclass_clone(void) {
+
+static void usage_hf_iclass_clone(void) {
 	PrintAndLog("Usage:  hf iclass clone f <tagfile.bin> b <first block> l <last block> k <KEY> c e|r");
 	PrintAndLog("Options:");
 	PrintAndLog("  f <filename>: specify a filename to clone from");
@@ -974,10 +1059,10 @@ int usage_hf_iclass_clone(void) {
 	PrintAndLog("  hf iclass clone f iclass_tagdump-121345.bin b 06 l 1A k 1122334455667788 e");
 	PrintAndLog("  hf iclass clone f iclass_tagdump-121345.bin b 05 l 19 k 0");
 	PrintAndLog("  hf iclass clone f iclass_tagdump-121345.bin b 06 l 19 k 0 e");
-	return -1;
 }
 
-int CmdHFiClassCloneTag(const char *Cmd) {
+
+static int CmdHFiClassCloneTag(const char *Cmd) {
 	char filename[FILE_PATH_SIZE] = {0};
 	char tempStr[50]={0};
 	uint8_t KEY[8]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
@@ -997,7 +1082,8 @@ int CmdHFiClassCloneTag(const char *Cmd) {
 		{
 		case 'h':
 		case 'H':
-			return usage_hf_iclass_clone();
+			usage_hf_iclass_clone();
+			return 0;
 		case 'b':
 		case 'B':
 			if (param_gethex(Cmd, cmdp+1, &startblock, 2)) {
@@ -1018,7 +1104,7 @@ int CmdHFiClassCloneTag(const char *Cmd) {
 			break;
 		case 'f':
 		case 'F':
-			fileNameLen = param_getstr(Cmd, cmdp+1, filename, sizeof(filename)); 
+			fileNameLen = param_getstr(Cmd, cmdp+1, filename, sizeof(filename));
 			if (fileNameLen < 1) {
 				PrintAndLog("No filename found after f");
 				errors = true;
@@ -1028,7 +1114,7 @@ int CmdHFiClassCloneTag(const char *Cmd) {
 		case 'k':
 		case 'K':
 			dataLen = param_getstr(Cmd, cmdp+1, tempStr, sizeof(tempStr));
-			if (dataLen == 16) { 
+			if (dataLen == 16) {
 				errors = param_gethex(tempStr, 0, KEY, dataLen);
 			} else if (dataLen == 1) {
 				keyNbr = param_get8(Cmd, cmdp+1);
@@ -1062,10 +1148,16 @@ int CmdHFiClassCloneTag(const char *Cmd) {
 			errors = true;
 			break;
 		}
-		if(errors) return usage_hf_iclass_clone();
+		if (errors) {
+			usage_hf_iclass_clone();
+			return 0;
+		}
 	}
 
-	if (cmdp < 8) return usage_hf_iclass_clone();
+	if (cmdp < 8) {
+		usage_hf_iclass_clone();
+		return 0;
+	}
 
 	FILE *f;
 
@@ -1108,21 +1200,21 @@ int CmdHFiClassCloneTag(const char *Cmd) {
 	uint8_t *ptr;
 	// calculate all mac for every the block we will write
 	for (i = startblock; i <= endblock; i++){
-	    Calc_wb_mac(i,tag_data[i - startblock].d,div_key,MAC);
-	    // usb command d start pointer = d + (i - 6) * 12
-	    // memcpy(pointer,tag_data[i - 6],8) 8 bytes
-	    // memcpy(pointer + 8,mac,sizoof(mac) 4 bytes;
-	    // next one
-	    ptr = w.d.asBytes + (i - startblock) * 12;
-	    memcpy(ptr, &(tag_data[i - startblock].d[0]), 8);
-	    memcpy(ptr + 8,MAC, 4);
+		Calc_wb_mac(i,tag_data[i - startblock].d,div_key,MAC);
+		// usb command d start pointer = d + (i - 6) * 12
+		// memcpy(pointer,tag_data[i - 6],8) 8 bytes
+		// memcpy(pointer + 8,mac,sizoof(mac) 4 bytes;
+		// next one
+		ptr = w.d.asBytes + (i - startblock) * 12;
+		memcpy(ptr, &(tag_data[i - startblock].d[0]), 8);
+		memcpy(ptr + 8,MAC, 4);
 	}
 	uint8_t p[12];
 	for (i = 0; i <= endblock - startblock;i++){
-	    memcpy(p,w.d.asBytes + (i * 12),12);
-	    printf("Block |%02x|",i + startblock);
-	    printf(" %02x%02x%02x%02x%02x%02x%02x%02x |",p[0],p[1],p[2],p[3],p[4],p[5],p[6],p[7]);
-	    printf(" MAC |%02x%02x%02x%02x|\n",p[8],p[9],p[10],p[11]);
+		memcpy(p,w.d.asBytes + (i * 12),12);
+		printf("Block |%02x|",i + startblock);
+		printf(" %02x%02x%02x%02x%02x%02x%02x%02x |",p[0],p[1],p[2],p[3],p[4],p[5],p[6],p[7]);
+		printf(" MAC |%02x%02x%02x%02x|\n",p[8],p[9],p[10],p[11]);
 	}
 	UsbCommand resp;
 	SendCommand(&w);
@@ -1134,7 +1226,9 @@ int CmdHFiClassCloneTag(const char *Cmd) {
 	return 1;
 }
 
+
 static int ReadBlock(uint8_t *KEY, uint8_t blockno, uint8_t keyType, bool elite, bool rawkey, bool verbose, bool auth) {
+
 	uint8_t MAC[4]={0x00,0x00,0x00,0x00};
 	uint8_t div_key[8]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
 
@@ -1142,9 +1236,8 @@ static int ReadBlock(uint8_t *KEY, uint8_t blockno, uint8_t keyType, bool elite,
 		if (!select_and_auth(KEY, MAC, div_key, (keyType==0x18), elite, rawkey, verbose))
 			return 0;
 	} else {
-		uint8_t CSN[8]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
-		uint8_t CCNR[12]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
-		if (!select_only(CSN, CCNR, (keyType==0x18), verbose))
+		uint8_t CSN[8];
+		if (!select_only(CSN, verbose))
 			return 0;
 	}
 
@@ -1152,8 +1245,7 @@ static int ReadBlock(uint8_t *KEY, uint8_t blockno, uint8_t keyType, bool elite,
 	UsbCommand w = {CMD_ICLASS_READBLOCK, {blockno}};
 	clearCommandBuffer();
 	SendCommand(&w);
-	if (!WaitForResponseTimeout(CMD_ACK,&resp,4500))
-	{
+	if (!WaitForResponseTimeout(CMD_ACK, &resp, 4500)) {
 		PrintAndLog("Command execute timeout");
 		return 0;
 	}
@@ -1167,22 +1259,23 @@ static int ReadBlock(uint8_t *KEY, uint8_t blockno, uint8_t keyType, bool elite,
 	return 1;
 }
 
-int usage_hf_iclass_readblock(void) {
-	PrintAndLog("Usage:  hf iclass readblk b <Block> k <Key> c e|r\n");
+
+static void usage_hf_iclass_readblock(void) {
+	PrintAndLog("Usage:  hf iclass readblk b <Block> k <Key> [c] [e|r]\n");
 	PrintAndLog("Options:");
-  PrintAndLog("  b <Block> : The block number as 2 hex symbols");
+	PrintAndLog("  b <Block> : The block number as 2 hex symbols");
 	PrintAndLog("  k <Key>   : Access Key as 16 hex symbols or 1 hex to select key from memory");
-  PrintAndLog("  c         : If 'c' is specified, the key set is assumed to be the credit key\n");
-  PrintAndLog("  e         : If 'e' is specified, elite computations applied to key");
-  PrintAndLog("  r         : If 'r' is specified, no computations applied to key");
-  PrintAndLog("Samples:");
+	PrintAndLog("  c         : If 'c' is specified, the key set is assumed to be the credit key\n");
+	PrintAndLog("  e         : If 'e' is specified, elite computations applied to key");
+	PrintAndLog("  r         : If 'r' is specified, no computations applied to key");
+	PrintAndLog("Samples:");
 	PrintAndLog("  hf iclass readblk b 06 k 0011223344556677");
 	PrintAndLog("  hf iclass readblk b 1B k 0011223344556677 c");
 	PrintAndLog("  hf iclass readblk b 0A k 0");
-	return 0;
 }
 
-int CmdHFiClass_ReadBlock(const char *Cmd) {
+
+static int CmdHFiClass_ReadBlock(const char *Cmd) {
 	uint8_t blockno=0;
 	uint8_t keyType = 0x88; //debit key
 	uint8_t KEY[8]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
@@ -1194,71 +1287,78 @@ int CmdHFiClass_ReadBlock(const char *Cmd) {
 	bool errors = false;
 	bool auth = false;
 	uint8_t cmdp = 0;
-	while(param_getchar(Cmd, cmdp) != 0x00)
-	{
-		switch(param_getchar(Cmd, cmdp))
-		{
-		case 'h':
-		case 'H':
-			return usage_hf_iclass_readblock();
-		case 'b':
-		case 'B':
-			if (param_gethex(Cmd, cmdp+1, &blockno, 2)) {
-				PrintAndLog("Block No must include 2 HEX symbols\n");
-				errors = true;
-			}
-			cmdp += 2;
-			break;
-		case 'c':
-		case 'C':
-			keyType = 0x18;
-			cmdp++;
-			break;
-		case 'e':
-		case 'E':
-			elite = true;
-			cmdp++;
-			break;
-		case 'k':
-		case 'K':
-			auth = true;
-			dataLen = param_getstr(Cmd, cmdp+1, tempStr, sizeof(tempStr));
-			if (dataLen == 16) { 
-				errors = param_gethex(tempStr, 0, KEY, dataLen);
-			} else if (dataLen == 1) {
-				keyNbr = param_get8(Cmd, cmdp+1);
-				if (keyNbr < ICLASS_KEYS_MAX) {
-					memcpy(KEY, iClass_Key_Table[keyNbr], 8);
+	while (param_getchar(Cmd, cmdp) != 0x00) {
+		switch (param_getchar(Cmd, cmdp)) {
+			case 'h':
+			case 'H':
+				usage_hf_iclass_readblock();
+				return 0;
+			case 'b':
+			case 'B':
+				if (param_gethex(Cmd, cmdp+1, &blockno, 2)) {
+					PrintAndLog("Block No must include 2 HEX symbols\n");
+					errors = true;
+				}
+				cmdp += 2;
+				break;
+			case 'c':
+			case 'C':
+				keyType = 0x18;
+				cmdp++;
+				break;
+			case 'e':
+			case 'E':
+				elite = true;
+				cmdp++;
+				break;
+			case 'k':
+			case 'K':
+				auth = true;
+				dataLen = param_getstr(Cmd, cmdp+1, tempStr, sizeof(tempStr));
+				if (dataLen == 16) {
+					errors = param_gethex(tempStr, 0, KEY, dataLen);
+				} else if (dataLen == 1) {
+					keyNbr = param_get8(Cmd, cmdp+1);
+					if (keyNbr < ICLASS_KEYS_MAX) {
+						memcpy(KEY, iClass_Key_Table[keyNbr], 8);
+					} else {
+						PrintAndLog("\nERROR: KeyNbr is invalid\n");
+						errors = true;
+					}
 				} else {
-					PrintAndLog("\nERROR: Credit KeyNbr is invalid\n");
+					PrintAndLog("\nERROR: Key is incorrect length\n");
 					errors = true;
 				}
-			} else {
-				PrintAndLog("\nERROR: Credit Key is incorrect length\n");
+				cmdp += 2;
+				break;
+			case 'r':
+			case 'R':
+				rawkey = true;
+				cmdp++;
+				break;
+			default:
+				PrintAndLog("Unknown parameter '%c'\n", param_getchar(Cmd, cmdp));
 				errors = true;
-			}
-			cmdp += 2;
-			break;
-		case 'r':
-		case 'R':
-			rawkey = true;
-			cmdp++;
-			break;
-		default:
-			PrintAndLog("Unknown parameter '%c'\n", param_getchar(Cmd, cmdp));
-			errors = true;
-			break;
+				break;
+		}
+		if (errors) {
+			usage_hf_iclass_readblock();
+			return 0;
 		}
-		if(errors) return usage_hf_iclass_readblock();
 	}
 
-	if (cmdp < 2) return usage_hf_iclass_readblock();
+	if (cmdp < 2) {
+		usage_hf_iclass_readblock();
+		return 0;
+	}
 	if (!auth)
 		PrintAndLog("warning: no authentication used with read, only a few specific blocks can be read accurately without authentication.");
+
 	return ReadBlock(KEY, blockno, keyType, elite, rawkey, true, auth);
 }
 
-int CmdHFiClass_loclass(const char *Cmd) {
+
+static int CmdHFiClass_loclass(const char *Cmd) {
 	char opt = param_getchar(Cmd, 0);
 
 	if (strlen(Cmd)<1 || opt == 'h') {
@@ -1297,49 +1397,22 @@ int CmdHFiClass_loclass(const char *Cmd) {
 	return 0;
 }
 
-void printIclassDumpContents(uint8_t *iclass_dump, uint8_t startblock, uint8_t endblock, size_t filesize) {
-	uint8_t mem_config;
-	memcpy(&mem_config, iclass_dump + 13,1);
-	uint8_t maxmemcount;
-	uint8_t filemaxblock = filesize / 8;
-	if (mem_config & 0x80)
-		maxmemcount = 255;
-	else
-		maxmemcount = 31;
-	//PrintAndLog	("endblock: %d, filesize: %d, maxmemcount: %d, filemaxblock: %d", endblock,filesize, maxmemcount, filemaxblock);
 
-	if (startblock == 0)
-		startblock = 6;
-	if ((endblock > maxmemcount) || (endblock == 0))
-		endblock = maxmemcount;
-
-	// remember endblock need to relate to zero-index arrays.
-	if (endblock > filemaxblock-1)
-		endblock = filemaxblock;
-	
-	int i = startblock;
-	printf("------+--+-------------------------+\n");
-	while (i <= endblock) {
-		uint8_t *blk = iclass_dump + (i * 8);
-		printf("Block |%02X| %s|\n", i, sprint_hex(blk, 8) );	
-		i++;
-	}
-	printf("------+--+-------------------------+\n");
-}
-
-int usage_hf_iclass_readtagfile() {
+static void usage_hf_iclass_readtagfile() {
 	PrintAndLog("Usage: hf iclass readtagfile <filename> [startblock] [endblock]");
-	return 1;
 }
 
-int CmdHFiClassReadTagFile(const char *Cmd) {
+
+static int CmdHFiClassReadTagFile(const char *Cmd) {
 	int startblock = 0;
 	int endblock = 0;
 	char tempnum[5];
 	FILE *f;
 	char filename[FILE_PATH_SIZE];
-	if (param_getstr(Cmd, 0, filename, sizeof(filename)) < 1)
-		return usage_hf_iclass_readtagfile();
+	if (param_getstr(Cmd, 0, filename, sizeof(filename)) < 1) {
+		usage_hf_iclass_readtagfile();
+		return 0;
+	}
 	if (param_getstr(Cmd, 1, tempnum, sizeof(tempnum)) < 1)
 		startblock = 0;
 	else
@@ -1397,24 +1470,7 @@ uint64_t hexarray_to_uint64(uint8_t *key) {
 	return uint_key;
 }
 */
-void HFiClassCalcDivKey(uint8_t	*CSN, uint8_t	*KEY, uint8_t *div_key, bool elite){
-	uint8_t keytable[128] = {0};
-	uint8_t key_index[8] = {0};
-	if (elite) {
-		uint8_t key_sel[8] = { 0 };
-		uint8_t key_sel_p[8] = { 0 };
-		hash2(KEY, keytable);
-		hash1(CSN, key_index);
-		for(uint8_t i = 0; i < 8 ; i++)
-			key_sel[i] = keytable[key_index[i]] & 0xFF;
 
-		//Permute from iclass format to standard format
-		permutekey_rev(key_sel, key_sel_p);
-		diversifyKey(CSN, key_sel_p, div_key);	
-	} else {
-		diversifyKey(CSN, KEY, div_key);
-	}		
-}
 
 //when told CSN, oldkey, newkey, if new key is elite (elite), and if old key was elite (oldElite)
 //calculate and return xor_div_key (ready for a key write command)
@@ -1426,18 +1482,19 @@ static void HFiClassCalcNewKey(uint8_t *CSN, uint8_t *OLDKEY, uint8_t *NEWKEY, u
 	HFiClassCalcDivKey(CSN, OLDKEY, old_div_key, oldElite);
 	//get new div key
 	HFiClassCalcDivKey(CSN, NEWKEY, new_div_key, elite);
-	
+
 	for (uint8_t i = 0; i < sizeof(old_div_key); i++){
 		xor_div_key[i] = old_div_key[i] ^ new_div_key[i];
 	}
 	if (verbose) {
 		printf("Old Div Key : %s\n",sprint_hex(old_div_key,8));
 		printf("New Div Key : %s\n",sprint_hex(new_div_key,8));
-		printf("Xor Div Key : %s\n",sprint_hex(xor_div_key,8));		
+		printf("Xor Div Key : %s\n",sprint_hex(xor_div_key,8));
 	}
 }
 
-int usage_hf_iclass_calc_newkey(void) {
+
+static void usage_hf_iclass_calc_newkey(void) {
 	PrintAndLog("HELP :  Manage iClass Keys in client memory:\n");
 	PrintAndLog("Usage:  hf iclass calc_newkey o <Old key> n <New key> s [csn] e");
 	PrintAndLog("  Options:");
@@ -1451,16 +1508,14 @@ int usage_hf_iclass_calc_newkey(void) {
 	PrintAndLog(" std key to e key read csn: hf iclass calcnewkey o 1122334455667788 n 2233445566778899 e");
 	PrintAndLog(" std to std read csn      : hf iclass calcnewkey o 1122334455667788 n 2233445566778899");
 	PrintAndLog("NOTE: * = required\n");
-
-	return 1;
 }
 
-int CmdHFiClassCalcNewKey(const char *Cmd) {
+
+static int CmdHFiClassCalcNewKey(const char *Cmd) {
 	uint8_t OLDKEY[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
 	uint8_t NEWKEY[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
 	uint8_t xor_div_key[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
 	uint8_t CSN[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
-	uint8_t CCNR[12] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
 	uint8_t keyNbr = 0;
 	uint8_t dataLen = 0;
 	char tempStr[50] = {0};
@@ -1475,7 +1530,8 @@ int CmdHFiClassCalcNewKey(const char *Cmd) {
 		{
 		case 'h':
 		case 'H':
-			return usage_hf_iclass_calc_newkey();
+			usage_hf_iclass_calc_newkey();
+			return 0;
 		case 'e':
 		case 'E':
 			dataLen = param_getstr(Cmd, cmdp, tempStr, sizeof(tempStr));
@@ -1487,7 +1543,7 @@ int CmdHFiClassCalcNewKey(const char *Cmd) {
 		case 'n':
 		case 'N':
 			dataLen = param_getstr(Cmd, cmdp+1, tempStr, sizeof(tempStr));
-			if (dataLen == 16) { 
+			if (dataLen == 16) {
 				errors = param_gethex(tempStr, 0, NEWKEY, dataLen);
 			} else if (dataLen == 1) {
 				keyNbr = param_get8(Cmd, cmdp+1);
@@ -1506,7 +1562,7 @@ int CmdHFiClassCalcNewKey(const char *Cmd) {
 		case 'o':
 		case 'O':
 			dataLen = param_getstr(Cmd, cmdp+1, tempStr, sizeof(tempStr));
-			if (dataLen == 16) { 
+			if (dataLen == 16) {
 				errors = param_gethex(tempStr, 0, OLDKEY, dataLen);
 			} else if (dataLen == 1) {
 				keyNbr = param_get8(Cmd, cmdp+1);
@@ -1525,8 +1581,10 @@ int CmdHFiClassCalcNewKey(const char *Cmd) {
 		case 's':
 		case 'S':
 			givenCSN = true;
-			if (param_gethex(Cmd, cmdp+1, CSN, 16))
-				return usage_hf_iclass_calc_newkey();
+			if (param_gethex(Cmd, cmdp+1, CSN, 16)) {
+				usage_hf_iclass_calc_newkey();
+				return 0;
+			}
 			cmdp += 2;
 			break;
 		default:
@@ -1534,19 +1592,26 @@ int CmdHFiClassCalcNewKey(const char *Cmd) {
 			errors = true;
 			break;
 		}
-		if(errors) return usage_hf_iclass_calc_newkey();
+		if (errors) {
+			usage_hf_iclass_calc_newkey();
+			return 0;
+		}
 	}
 
-	if (cmdp < 4) return usage_hf_iclass_calc_newkey();
+	if (cmdp < 4) {
+		usage_hf_iclass_calc_newkey();
+		return 0;
+	}
 
 	if (!givenCSN)
-		if (!select_only(CSN, CCNR, false, true))
+		if (!select_only(CSN, true))
 			return 0;
-	
+
 	HFiClassCalcNewKey(CSN, OLDKEY, NEWKEY, xor_div_key, elite, oldElite, true);
 	return 0;
 }
 
+
 static int loadKeys(char *filename) {
 	FILE *f;
 	f = fopen(filename,"rb");
@@ -1582,6 +1647,7 @@ static int loadKeys(char *filename) {
 	return 1;
 }
 
+
 static int saveKeys(char *filename) {
 	FILE *f;
 	f = fopen(filename,"wb");
@@ -1599,16 +1665,18 @@ static int saveKeys(char *filename) {
 	return 0;
 }
 
+
 static int printKeys(void) {
 	PrintAndLog("");
 	for (uint8_t i = 0; i < ICLASS_KEYS_MAX; i++){
 		PrintAndLog("%u: %s",i,sprint_hex(iClass_Key_Table[i],8));
 	}
-	PrintAndLog("");	
+	PrintAndLog("");
 	return 0;
 }
 
-int usage_hf_iclass_managekeys(void) {
+
+static void usage_hf_iclass_managekeys(void) {
 	PrintAndLog("HELP :  Manage iClass Keys in client memory:\n");
 	PrintAndLog("Usage:  hf iclass managekeys n [keynbr] k [key] f [filename] s l p\n");
 	PrintAndLog("  Options:");
@@ -1623,10 +1691,10 @@ int usage_hf_iclass_managekeys(void) {
 	PrintAndLog(" save key file: hf iclass managekeys f mykeys.bin s");
 	PrintAndLog(" load key file: hf iclass managekeys f mykeys.bin l");
 	PrintAndLog(" print keys   : hf iclass managekeys p\n");
-	return 0;
 }
 
-int CmdHFiClassManageKeys(const char *Cmd) {
+
+static int CmdHFiClassManageKeys(const char *Cmd) {
 	uint8_t keyNbr = 0;
 	uint8_t dataLen = 0;
 	uint8_t KEY[8] = {0};
@@ -1643,10 +1711,11 @@ int CmdHFiClassManageKeys(const char *Cmd) {
 		{
 		case 'h':
 		case 'H':
-			return usage_hf_iclass_managekeys();
+			usage_hf_iclass_managekeys();
+			return 0;
 		case 'f':
 		case 'F':
-			fileNameLen = param_getstr(Cmd, cmdp+1, filename, sizeof(filename)); 
+			fileNameLen = param_getstr(Cmd, cmdp+1, filename, sizeof(filename));
 			if (fileNameLen < 1) {
 				PrintAndLog("No filename found after f");
 				errors = true;
@@ -1664,7 +1733,7 @@ int CmdHFiClassManageKeys(const char *Cmd) {
 			break;
 		case 'k':
 		case 'K':
-			operation += 3; //set key 
+			operation += 3; //set key
 			dataLen = param_getstr(Cmd, cmdp+1, tempStr, sizeof(tempStr));
 			if (dataLen == 16) { //ul-c or ev1/ntag key length
 				errors = param_gethex(tempStr, 0, KEY, dataLen);
@@ -1694,19 +1763,25 @@ int CmdHFiClassManageKeys(const char *Cmd) {
 			errors = true;
 			break;
 		}
-		if(errors) return usage_hf_iclass_managekeys();
+		if (errors) {
+			usage_hf_iclass_managekeys();
+			return 0;
+		}
 	}
 	if (operation == 0){
 		PrintAndLog("no operation specified (load, save, or print)\n");
-		return usage_hf_iclass_managekeys();
+		usage_hf_iclass_managekeys();
+		return 0;
 	}
 	if (operation > 6){
 		PrintAndLog("Too many operations specified\n");
-		return usage_hf_iclass_managekeys();
+		usage_hf_iclass_managekeys();
+		return 0;
 	}
 	if (operation > 4 && fileNameLen == 0){
 		PrintAndLog("You must enter a filename when loading or saving\n");
-		return usage_hf_iclass_managekeys();
+		usage_hf_iclass_managekeys();
+		return 0;
 	}
 
 	switch (operation){
@@ -1719,7 +1794,8 @@ int CmdHFiClassManageKeys(const char *Cmd) {
 	return 0;
 }
 
-int CmdHFiClassCheckKeys(const char *Cmd) {
+
+static int CmdHFiClassCheckKeys(const char *Cmd) {
 
 	uint8_t mac[4] = {0x00,0x00,0x00,0x00};
 	uint8_t key[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
@@ -1727,9 +1803,9 @@ int CmdHFiClassCheckKeys(const char *Cmd) {
 
 	// elite key,  raw key, standard key
 	bool use_elite = false;
-	bool use_raw = false;	
+	bool use_raw = false;
 	bool found_debit = false;
-	bool found_credit = false;	
+	bool found_credit = false;
 	bool errors = false;
 	uint8_t cmdp = 0x00;
 	FILE * f;
@@ -1743,10 +1819,11 @@ int CmdHFiClassCheckKeys(const char *Cmd) {
 		switch (param_getchar(Cmd, cmdp)) {
 		case 'h':
 		case 'H':
-			return usage_hf_iclass_chk();
+			usage_hf_iclass_chk();
+			return 0;
 		case 'f':
 		case 'F':
-			fileNameLen = param_getstr(Cmd, cmdp+1, filename, sizeof(filename)); 
+			fileNameLen = param_getstr(Cmd, cmdp+1, filename, sizeof(filename));
 			if (fileNameLen < 1) {
 				PrintAndLog("No filename found after f");
 				errors = true;
@@ -1769,8 +1846,11 @@ int CmdHFiClassCheckKeys(const char *Cmd) {
 			break;
 		}
 	}
-	if (errors) return usage_hf_iclass_chk();	
-			
+	if (errors) {
+		usage_hf_iclass_chk();
+		return 0;
+	}
+
 	if ( !(f = fopen( filename , "r")) ) {
 		PrintAndLog("File: %s: not found or locked.", filename);
 		return 1;
@@ -1779,16 +1859,16 @@ int CmdHFiClassCheckKeys(const char *Cmd) {
 	while( fgets(buf, sizeof(buf), f) ){
 		if (strlen(buf) < 16 || buf[15] == '\n')
 			continue;
-	
+
 		while (fgetc(f) != '\n' && !feof(f)) ;  //goto next line
-		
-		if( buf[0]=='#' ) continue;	//The line start with # is comment, skip
+
+		if( buf[0]=='#' ) continue; //The line start with # is comment, skip
 
 		if (!isxdigit(buf[0])){
 			PrintAndLog("File content error. '%s' must include 16 HEX symbols",buf);
 			continue;
 		}
-		
+
 		buf[16] = 0;
 
 		p = realloc(keyBlock, 8 * (keyitems += 64));
@@ -1809,19 +1889,19 @@ int CmdHFiClassCheckKeys(const char *Cmd) {
 	}
 	fclose(f);
 	PrintAndLog("Loaded %2d keys from %s", keycnt, filename);
-	
+
 	// time
 	uint64_t t1 = msclock();
-				
+
 	for (uint32_t c = 0; c < keycnt; c += 1) {
-			printf("."); fflush(stdout);			
+			printf("."); fflush(stdout);
 			if (ukbhit()) {
 				int gc = getchar(); (void)gc;
 				printf("\naborted via keyboard!\n");
 				break;
 			}
-			
-			memcpy(key, keyBlock + 8 * c , 8); 
+
+			memcpy(key, keyBlock + 8 * c , 8);
 
 			// debit key. try twice
 			for (int foo = 0; foo < 2 && !found_debit; foo++) {
@@ -1833,18 +1913,18 @@ int CmdHFiClassCheckKeys(const char *Cmd) {
 			PrintAndLog("   Found AA1 debit key\t\t[%s]", sprint_hex(key, 8));
 				found_debit = true;
 			}
-			
+
 			// credit key. try twice
 			for (int foo = 0; foo < 2 && !found_credit; foo++) {
 				if (!select_and_auth(key, mac, div_key, true, use_elite, use_raw, false))
 					continue;
-				
+
 				// key found
 				PrintAndLog("\n--------------------------------------------------------");
 				PrintAndLog("   Found AA2 credit key\t\t[%s]", sprint_hex(key, 8));
 				found_credit = true;
 			}
-			
+
 			// both keys found.
 			if ( found_debit && found_credit )
 				break;
@@ -1853,45 +1933,108 @@ int CmdHFiClassCheckKeys(const char *Cmd) {
 	t1 = msclock() - t1;
 
 	PrintAndLog("\nTime in iclass checkkeys: %.0f seconds\n", (float)t1/1000.0);
-	
+
 	DropField();
 	free(keyBlock);
 	PrintAndLog("");
 	return 0;
 }
 
-static command_t CommandTable[] = 
-{
-	{"help",        CmdHelp,                    	1,	"This help"},
-	{"calcnewkey",  CmdHFiClassCalcNewKey,      	1,	"[options..] Calc Diversified keys (blocks 3 & 4) to write new keys"},
-	{"chk",         CmdHFiClassCheckKeys,        	0,	"            Check keys"},	
-	{"clone",       CmdHFiClassCloneTag,        	0,	"[options..] Authenticate and Clone from iClass bin file"},
-	{"decrypt",     CmdHFiClassDecrypt,         	1,	"[f <fname>] Decrypt tagdump" },
-	{"dump",        CmdHFiClassReader_Dump,     	0,	"[options..] Authenticate and Dump iClass tag's AA1"},
-	{"eload",       CmdHFiClassELoad,           	0,	"[f <fname>] (experimental) Load data into iClass emulator memory"},
-	{"encryptblk",  CmdHFiClassEncryptBlk,      	1,	"<BlockData> Encrypt given block data"},
-	{"list",        CmdHFiClassList,            	0,	"            (Deprecated) List iClass history"},
-	{"loclass",     CmdHFiClass_loclass,        	1,	"[options..] Use loclass to perform bruteforce of reader attack dump"},
-	{"managekeys",  CmdHFiClassManageKeys,      	1,	"[options..] Manage the keys to use with iClass"},
-	{"readblk",     CmdHFiClass_ReadBlock,      	0,	"[options..] Authenticate and Read iClass block"},
-	{"reader",      CmdHFiClassReader,          	0,	"            Look for iClass tags until a key or the pm3 button is pressed"},
-	{"readtagfile", CmdHFiClassReadTagFile,     	1,	"[options..] Display Content from tagfile"},
-	{"replay",      CmdHFiClassReader_Replay,   	0,	"<mac>       Read an iClass tag via Reply Attack"},
-	{"sim",         CmdHFiClassSim,             	0,	"[options..] Simulate iClass tag"},
-	{"snoop",       CmdHFiClassSnoop,           	0,	"            Eavesdrop iClass communication"},
-	{"writeblk",    CmdHFiClass_WriteBlock,     	0,	"[options..] Authenticate and Write iClass block"},
+
+static void usage_hf_iclass_permutekey(void) {
+	PrintAndLogEx(NORMAL, "Convert keys from standard NIST to iClass format (and vice versa)");
+	PrintAndLogEx(NORMAL, "");
+	PrintAndLogEx(NORMAL, "Usage:  hf iclass permute [h] [r] <key>");
+	PrintAndLogEx(NORMAL, "Options:");
+	PrintAndLogEx(NORMAL, "           h          This help");
+	PrintAndLogEx(NORMAL, "           r          reverse convert key from iClass to NIST format");
+	PrintAndLogEx(NORMAL, "");
+	PrintAndLogEx(NORMAL, "Examples:");
+	PrintAndLogEx(NORMAL, "      hf iclass permute r 0123456789abcdef");
+}
+
+
+static int CmdHFiClassPermuteKey(const char *Cmd) {
+
+	uint8_t key[8] = {0};
+	uint8_t data[16] = {0};
+	bool isReverse = false;
+	int len = sizeof(data);
+	char cmdp = tolower(param_getchar(Cmd, 0));
+	if (strlen(Cmd) == 0 || cmdp == 'h') {
+		usage_hf_iclass_permutekey();
+		return 0;
+	}
+
+	if (cmdp == 'r') {
+		isReverse = true;
+		param_gethex_ex(Cmd, 1, data, &len);
+	} else if (cmdp == 'f') {
+		param_gethex_ex(Cmd, 1, data, &len);
+	} else {
+		param_gethex_ex(Cmd, 0, data, &len);
+	}
+
+
+	if (len % 2) {
+		usage_hf_iclass_permutekey();
+		return 0;
+	}
+
+	len >>= 1;
+
+	memcpy(key, data, 8);
+
+	if (isReverse) {
+		// generate_rev(data, len);
+		uint8_t key_std_format[8] = {0};
+		permutekey_rev(key, key_std_format);
+		PrintAndLogEx(SUCCESS, "key in standard NIST format:     %s \n", sprint_hex(key_std_format, 8));
+		// if (mbedtls_des_key_check_key_parity(key_std_format
+	} else {
+		// generate(data, len);
+		uint8_t key_iclass_format[8] = {0};
+		permutekey(key, key_iclass_format);
+		PrintAndLogEx(SUCCESS, "key in iClass (permuted) format: %s \n", sprint_hex(key_iclass_format, 8));
+	}
+	return 0;
+}
+
+
+static int CmdHelp(const char *Cmd);
+
+static command_t CommandTable[] = {
+	{"help",        CmdHelp,                        1,  "This help"},
+	{"calcnewkey",  CmdHFiClassCalcNewKey,          1,  "[options..] Calc Diversified keys (blocks 3 & 4) to write new keys"},
+	{"chk",         CmdHFiClassCheckKeys,           0,  "            Check keys"},
+	{"clone",       CmdHFiClassCloneTag,            0,  "[options..] Authenticate and Clone from iClass bin file"},
+	{"decrypt",     CmdHFiClassDecrypt,             1,  "[f <fname>] Decrypt tagdump" },
+	{"dump",        CmdHFiClassReader_Dump,         0,  "[options..] Authenticate and Dump iClass tag's AA1"},
+	{"eload",       CmdHFiClassELoad,               0,  "[f <fname>] (experimental) Load data into iClass emulator memory"},
+	{"encryptblk",  CmdHFiClassEncryptBlk,          1,  "<BlockData> Encrypt given block data"},
+	{"list",        CmdHFiClassList,                0,  "            (Deprecated) List iClass history"},
+	{"loclass",     CmdHFiClass_loclass,            1,  "[options..] Use loclass to perform bruteforce of reader attack dump"},
+	{"managekeys",  CmdHFiClassManageKeys,          1,  "[options..] Manage the keys to use with iClass"},
+	{"permutekey",  CmdHFiClassPermuteKey,          1,  "            iClass key permutation"},
+	{"readblk",     CmdHFiClass_ReadBlock,          0,  "[options..] Authenticate and Read iClass block"},
+	{"reader",      CmdHFiClassReader,              0,  "            Look for iClass tags until a key or the pm3 button is pressed"},
+	{"readtagfile", CmdHFiClassReadTagFile,         1,  "[options..] Display Content from tagfile"},
+	{"replay",      CmdHFiClassReader_Replay,       0,  "<mac>       Read an iClass tag via Reply Attack"},
+	{"sim",         CmdHFiClassSim,                 0,  "[options..] Simulate iClass tag"},
+	{"snoop",       CmdHFiClassSnoop,               0,  "            Eavesdrop iClass communication"},
+	{"writeblk",    CmdHFiClass_WriteBlock,         0,  "[options..] Authenticate and Write iClass block"},
 	{NULL, NULL, 0, NULL}
 };
 
-int CmdHFiClass(const char *Cmd)
-{
-	clearCommandBuffer();	
+
+int CmdHFiClass(const char *Cmd) {
+	clearCommandBuffer();
 	CmdsParse(CommandTable, Cmd);
 	return 0;
 }
 
-int CmdHelp(const char *Cmd)
-{
+
+int CmdHelp(const char *Cmd) {
 	CmdsHelp(CommandTable);
 	return 0;
 }
diff --git a/client/cmdhficlass.h b/client/cmdhficlass.h
index 456bb1c3..09103b33 100644
--- a/client/cmdhficlass.h
+++ b/client/cmdhficlass.h
@@ -13,28 +13,6 @@
 #define CMDHFICLASS_H__
 
 int CmdHFiClass(const char *Cmd);
-
-int CmdHFiClassCalcNewKey(const char *Cmd);
-int CmdHFiClassCloneTag(const char *Cmd);
-int CmdHFiClassDecrypt(const char *Cmd);
-int CmdHFiClassEncryptBlk(const char *Cmd);
-int CmdHFiClassELoad(const char *Cmd);
-int CmdHFiClassList(const char *Cmd);
 int HFiClassReader(const char *Cmd, bool loop, bool verbose);
-int CmdHFiClassReader(const char *Cmd);
-int CmdHFiClassReader_Dump(const char *Cmd);
-int CmdHFiClassReader_Replay(const char *Cmd);
-int CmdHFiClassReadKeyFile(const char *filename);
-int CmdHFiClassReadTagFile(const char *Cmd);
-int CmdHFiClass_ReadBlock(const char *Cmd);
-int CmdHFiClass_TestMac(const char *Cmd);
-int CmdHFiClassManageKeys(const char *Cmd);
-int CmdHFiClass_loclass(const char *Cmd);
-int CmdHFiClassSnoop(const char *Cmd);
-int CmdHFiClassSim(const char *Cmd);
-int CmdHFiClassWriteKeyFile(const char *Cmd);
-int CmdHFiClass_WriteBlock(const char *Cmd);
-int CmdHFiClassCheckKeys(const char *Cmd);
-void printIclassDumpContents(uint8_t *iclass_dump, uint8_t startblock, uint8_t endblock, size_t filesize);
-void HFiClassCalcDivKey(uint8_t	*CSN, uint8_t	*KEY, uint8_t *div_key, bool elite);
+
 #endif
diff --git a/client/cmdhflist.c b/client/cmdhflist.c
index 07a286cc..29d10d92 100644
--- a/client/cmdhflist.c
+++ b/client/cmdhflist.c
@@ -903,8 +903,6 @@ uint16_t printTraceLine(uint16_t tracepos, uint16_t traceLen, uint8_t *trace, ui
 
 	// adjust for different time scales
 	if (protocol == ICLASS || protocol == ISO_15693) {
-		first_timestamp *= 32;
-		timestamp *= 32;
 		duration *= 32;
 	}
 
@@ -1037,10 +1035,6 @@ uint16_t printTraceLine(uint16_t tracepos, uint16_t traceLen, uint8_t *trace, ui
 
 	if (showWaitCycles && !isResponse && next_record_is_response(tracepos, trace)) {
 		uint32_t next_timestamp = *((uint32_t *)(trace + tracepos));
-		// adjust for different time scales
-		if (protocol == ICLASS || protocol == ISO_15693) {
-			next_timestamp *= 32;
-		}
 
 		PrintAndLog(" %10d | %10d | %s | fdt (Frame Delay Time): %d",
 			(EndOfTransmissionTimestamp - first_timestamp),
diff --git a/client/loclass/elite_crack.h b/client/loclass/elite_crack.h
index 46791fb5..c44f5105 100644
--- a/client/loclass/elite_crack.h
+++ b/client/loclass/elite_crack.h
@@ -85,7 +85,7 @@ typedef struct {
 	uint8_t cc_nr[12];
 	uint8_t mac[4];
 
-}dumpdata;
+} dumpdata;
 
 /**
  * @brief Performs brute force attack against a dump-data item, containing csn, cc_nr and mac.
diff --git a/fpga/fpga_hf.bit b/fpga/fpga_hf.bit
index 44b24280..38990597 100644
Binary files a/fpga/fpga_hf.bit and b/fpga/fpga_hf.bit differ
diff --git a/fpga/hi_reader.v b/fpga/hi_reader.v
index b0b21e30..aca9132f 100644
--- a/fpga/hi_reader.v
+++ b/fpga/hi_reader.v
@@ -247,9 +247,9 @@ begin
 
 	// set ssp_frame signal for corr_i_cnt = 1..3
 	// (send one frame with 16 Bits)
-    if (corr_i_cnt == 6'd2)
+    if (corr_i_cnt == 6'd1)
         ssp_frame <= 1'b1;
-    if (corr_i_cnt == 6'd14)
+    if (corr_i_cnt == 6'd5)
         ssp_frame <= 1'b0;
 end
 
diff --git a/include/usb_cmd.h b/include/usb_cmd.h
index 0802d2f1..75b7c210 100644
--- a/include/usb_cmd.h
+++ b/include/usb_cmd.h
@@ -165,7 +165,8 @@ typedef struct{
 #define CMD_ICLASS_READBLOCK                                              0x0396
 #define CMD_ICLASS_WRITEBLOCK                                             0x0397
 #define CMD_ICLASS_EML_MEMSET                                             0x0398
-#define CMD_ICLASS_AUTHENTICATION                                         0x0399
+#define CMD_ICLASS_CHECK                                                  0x0399
+#define CMD_ICLASS_READCHECK                                              0x039A                                         
 
 // For measurements of the antenna tuning
 #define CMD_MEASURE_ANTENNA_TUNING                                        0x0400
@@ -236,13 +237,11 @@ typedef struct{
 
 
 // iCLASS reader flags
-#define FLAG_ICLASS_READER_ONLY_ONCE    0x01
 #define FLAG_ICLASS_READER_CC           0x02
 #define FLAG_ICLASS_READER_CSN          0x04
 #define FLAG_ICLASS_READER_CONF         0x08
 #define FLAG_ICLASS_READER_AA           0x10
-#define FLAG_ICLASS_READER_ONE_TRY      0x20
-#define FLAG_ICLASS_READER_CEDITKEY     0x40
+#define FLAG_ICLASS_READER_CREDITKEY    0x40
 
 // iCLASS simulation modes
 #define ICLASS_SIM_MODE_CSN                   0