X-Git-Url: http://cvs.zerfleddert.de/cgi-bin/gitweb.cgi/proxmark3-svn/blobdiff_plain/bcf61bd34ab2747580aabf648093e5854b7283ed..76471e5d1734786f6095c0b689fa49c5f6e92136:/armsrc/iclass.c?ds=inline

diff --git a/armsrc/iclass.c b/armsrc/iclass.c
index 51a9011f..42deea86 100644
--- a/armsrc/iclass.c
+++ b/armsrc/iclass.c
@@ -36,7 +36,7 @@
 //
 //-----------------------------------------------------------------------------
 
-#include "../include/proxmark3.h"
+#include "proxmark3.h"
 #include "apps.h"
 #include "util.h"
 #include "string.h"
@@ -45,9 +45,8 @@
 // Needed for CRC in emulation mode;
 // same construction as in ISO 14443;
 // different initial value (CRC_ICLASS)
-#include "../common/iso14443crc.h"
-#include "../common/iso15693tools.h"
-//#include "iso15693tools.h"
+#include "iso14443crc.h"
+#include "iso15693tools.h"
 #include "protocols.h"
 #include "optimized_cipher.h"
 
@@ -427,10 +426,11 @@ static RAMFUNC int ManchesterDecoding(int v)
 					Demod.state = DEMOD_UNSYNCD;
 					error = 0x88;
 				}
-
+				
+				// TODO: use this error value to print?  Ask Holiman.
+				// 2016-01-08 iceman
 			}
 			error = 0;
-
 		}
 	}
 	else {
@@ -653,8 +653,9 @@ void RAMFUNC SnoopIClass(void)
     // The DMA buffer, used to stream samples from the FPGA
     uint8_t *dmaBuf = BigBuf_malloc(DMA_BUFFER_SIZE);
  
-	set_tracing(TRUE);
 	clear_trace();
+	set_tracing(TRUE);
+	
     iso14a_set_trigger(FALSE);
 
     int lastRxCounter;
@@ -676,7 +677,11 @@ void RAMFUNC SnoopIClass(void)
     FpgaSetupSsc();
     upTo = dmaBuf;
     lastRxCounter = DMA_BUFFER_SIZE;
-    FpgaSetupSscDma((uint8_t *)dmaBuf, DMA_BUFFER_SIZE);
+	// Setup and start DMA.
+	if ( !FpgaSetupSscDma((uint8_t*) dmaBuf, DMA_BUFFER_SIZE) ){
+		if (MF_DBGLEVEL > 1) Dbprintf("FpgaSetupSscDma failed. Exiting"); 
+		return;
+	}
 
     // And the reader -> tag commands
     memset(&Uart, 0, sizeof(Uart));
@@ -703,22 +708,22 @@ void RAMFUNC SnoopIClass(void)
     for(;;) {
         LED_A_ON();
         WDT_HIT();
-        int behindBy = (lastRxCounter - AT91C_BASE_PDC_SSC->PDC_RCR) &
-                                (DMA_BUFFER_SIZE-1);
-        if(behindBy > maxBehindBy) {
+        int behindBy = (lastRxCounter - AT91C_BASE_PDC_SSC->PDC_RCR) & (DMA_BUFFER_SIZE-1);
+
+        if ( behindBy > maxBehindBy) {
             maxBehindBy = behindBy;
-            if(behindBy > (9 * DMA_BUFFER_SIZE / 10)) {
+            if ( behindBy > (9 * DMA_BUFFER_SIZE / 10)) {
                 Dbprintf("blew circular buffer! behindBy=0x%x", behindBy);
                 goto done;
             }
         }
-        if(behindBy < 1) continue;
+        if( behindBy < 1) continue;
 
-	LED_A_OFF();
+		LED_A_OFF();
         smpl = upTo[0];
         upTo++;
         lastRxCounter -= 1;
-        if(upTo - dmaBuf > DMA_BUFFER_SIZE) {
+        if (upTo - dmaBuf > DMA_BUFFER_SIZE) {
             upTo -= DMA_BUFFER_SIZE;
             lastRxCounter += DMA_BUFFER_SIZE;
             AT91C_BASE_PDC_SSC->PDC_RNPR = (uint32_t) upTo;
@@ -726,77 +731,75 @@ void RAMFUNC SnoopIClass(void)
         }
 
         //samples += 4;
-	samples += 1;
+		samples += 1;
 
-	if(smpl & 0xF) {
-		decbyte ^= (1 << (3 - div));
-	}
+		if(smpl & 0xF)
+			decbyte ^= (1 << (3 - div));
 	
-	// FOR READER SIDE COMMUMICATION...
+	
+		// FOR READER SIDE COMMUMICATION...
 
-	decbyter <<= 2;
-	decbyter ^= (smpl & 0x30);
+		decbyter <<= 2;
+		decbyter ^= (smpl & 0x30);
 
-	div++;
+		++div;
 	
-	if((div + 1) % 2 == 0) {
-		smpl = decbyter;	
-		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;
-			if(tracing)	{
-				uint8_t parity[MAX_PARITY_SIZE];
-				GetParity(Uart.output, Uart.byteCnt, parity);
-				LogTrace(Uart.output,Uart.byteCnt, time_start, time_stop, parity, TRUE);
-			}
-
+		if (( div + 1) % 2 == 0) {
+			smpl = decbyter;	
+			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;
+				if(tracing)	{
+					uint8_t parity[MAX_PARITY_SIZE];
+					GetParity(Uart.output, Uart.byteCnt, parity);
+					LogTrace(Uart.output,Uart.byteCnt, time_start, time_stop, 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;
+				/* 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;
+			}
+			decbyter = 0;
 		}
-		decbyter = 0;
-	}
 
-	if(div > 3) {
-		smpl = decbyte;
-		if(ManchesterDecoding(smpl & 0x0F)) {
-			time_stop = (GetCountSspClk()-time_0) << 4;
+		if(div > 3) {
+			smpl = decbyte;
+			if(ManchesterDecoding(smpl & 0x0F)) {
+				time_stop = (GetCountSspClk()-time_0) << 4;
 
-		    rsamples = samples - Demod.samples;
-		    LED_B_ON();
+				rsamples = samples - Demod.samples;
+				LED_B_ON();
 
-			if(tracing)	{
-				uint8_t parity[MAX_PARITY_SIZE];
-				GetParity(Demod.output, Demod.len, parity);
-				LogTrace(Demod.output, Demod.len, time_start, time_stop, parity, FALSE);
-			}
+				if(tracing)	{
+					uint8_t parity[MAX_PARITY_SIZE];
+					GetParity(Demod.output, Demod.len, parity);
+					LogTrace(Demod.output, Demod.len, time_start, time_stop, 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;
+				// 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;
+			}
+			
+			div = 0;
+			decbyte = 0x00;
 		}
-		
-		div = 0;
-		decbyte = 0x00;
-	}
-	//}
 
-        if(BUTTON_PRESS()) {
+        if (BUTTON_PRESS()) {
             DbpString("cancelled_a");
             goto done;
         }
@@ -808,20 +811,17 @@ void RAMFUNC SnoopIClass(void)
 	Dbprintf("%x %x %x", Uart.byteCntMax, BigBuf_get_traceLen(), (int)Uart.output[0]);
 
 done:
-    AT91C_BASE_PDC_SSC->PDC_PTCR = AT91C_PDC_RXTDIS;
+    FpgaDisableSscDma();
     Dbprintf("%x %x %x", maxBehindBy, Uart.state, Uart.byteCnt);
 	Dbprintf("%x %x %x", Uart.byteCntMax, BigBuf_get_traceLen(), (int)Uart.output[0]);
-    LED_A_OFF();
-    LED_B_OFF();
-    LED_C_OFF();
-    LED_D_OFF();
+	LEDsoff();
+	set_tracing(FALSE);	
 }
 
 void rotateCSN(uint8_t* originalCSN, uint8_t* rotatedCSN) {
 	int i; 
-	for(i = 0; i < 8; i++) {
+	for(i = 0; i < 8; i++)
 		rotatedCSN[i] = (originalCSN[i] >> 3) | (originalCSN[(i+1)%8] << 5);
-	}
 }
 
 //-----------------------------------------------------------------------------
@@ -923,7 +923,7 @@ static void CodeIClassTagAnswer(const uint8_t *cmd, int len)
 	 * The mode FPGA_HF_SIMULATOR_MODULATE_424K_8BIT which we use to simulate tag,
 	 * works like this.
 	 * - A 1-bit input to the FPGA becomes 8 pulses on 423.5kHz (fc/32) (18.88us).
-	 * - A 0-bit inptu to the FPGA becomes an unmodulated time of 18.88us
+	 * - A 0-bit input to the FPGA becomes an unmodulated time of 18.88us
 	 *
 	 * In this mode the SOF can be written as 00011101 = 0x1D
 	 * The EOF can be written as 10111000 = 0xb8
@@ -990,8 +990,9 @@ void SimulateIClass(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain
 	FpgaDownloadAndGo(FPGA_BITSTREAM_HF);
 
 	// Enable and clear the trace
-	set_tracing(TRUE);
 	clear_trace();
+	set_tracing(TRUE);
+
 	//Use the emulator memory for SIM
 	uint8_t *emulator = BigBuf_get_EM_addr();
 
@@ -1039,7 +1040,7 @@ void SimulateIClass(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain
 		Dbprintf("The mode is not implemented, reserved for future use");
 	}
 	Dbprintf("Done...");
-
+	set_tracing(FALSE);	
 }
 void AppendCrc(uint8_t* data, int len)
 {
@@ -1124,7 +1125,6 @@ int doIClassSimulation( int simulationMode, uint8_t *reader_mac_buf)
 	int resp_cc_len;
 
 	uint8_t *receivedCmd = BigBuf_malloc(MAX_FRAME_SIZE);
-	memset(receivedCmd, 0x44, MAX_FRAME_SIZE);
 	int len;
 
 	// Prepare card messages
@@ -1300,7 +1300,8 @@ int doIClassSimulation( int simulationMode, uint8_t *reader_mac_buf)
 			receivedCmd[3], receivedCmd[4], receivedCmd[5],
 			receivedCmd[6], receivedCmd[7], receivedCmd[8]);
 			// Do not respond
-			modulated_response = resp_sof; modulated_response_size = 0; //order = 0;
+			modulated_response = resp_sof;
+			modulated_response_size = 0; //order = 0;
 			trace_data = NULL;
 			trace_data_size = 0;
 		}
@@ -1329,24 +1330,17 @@ int doIClassSimulation( int simulationMode, uint8_t *reader_mac_buf)
 				GetParity(trace_data, trace_data_size, parity);
 				LogTrace(trace_data, trace_data_size, (t2r_time-time_0) << 4, (t2r_time-time_0) << 4, parity, FALSE);
 			}
-			if(!tracing) {
+			if(!tracing)
 				DbpString("Trace full");
-				//break;
-			}
 
 		}
-		memset(receivedCmd, 0x44, MAX_FRAME_SIZE);
 	}
 
-	//Dbprintf("%x", cmdsRecvd);
-	LED_A_OFF();
-	LED_B_OFF();
-	LED_C_OFF();
-
+	LEDsoff();
+	
 	if(buttonPressed)
-	{
 		DbpString("Button pressed");
-	}
+	
 	return buttonPressed;
 }
 
@@ -1394,63 +1388,62 @@ static int SendIClassAnswer(uint8_t *resp, int respLen, int delay)
 //-----------------------------------------------------------------------------
 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();
-
-   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();
-  }
+	int c;
+	volatile uint32_t r;
+	FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_READER_MOD);
+	AT91C_BASE_SSC->SSC_THR = 0x00;
+	FpgaSetupSsc();
 
-   }
+	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)) {
+				r = AT91C_BASE_SSC->SSC_RHR;
+				(void)r;
+			}
+			WDT_HIT();
+		}
+	}
 
-  uint8_t sendbyte;
-  bool firstpart = TRUE;
-  c = 0;
-  for(;;) {
-    if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) {
 
-      // 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;
+	uint8_t sendbyte;
+	bool firstpart = TRUE;
+	c = 0;
+	for(;;) {
+		if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) {
 
-      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) *samples = (c + *wait) << 3;
-}
+			// 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;
 
+			if(c >= len) break;
+
+		}
+		if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) {
+			r = AT91C_BASE_SSC->SSC_RHR;
+			(void)r;
+		}
+		
+		WDT_HIT();
+	}
+	if (samples && wait) *samples = (c + *wait) << 3;
+}
 
 //-----------------------------------------------------------------------------
 // Prepare iClass reader command to send to FPGA
@@ -1550,9 +1543,15 @@ static int GetIClassAnswer(uint8_t *receivedResponse, int maxLen, int *samples,
 			if (elapsed) (*elapsed)++;
 		}
 		if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) {
-			if(c < timeout) { c++; } else { return FALSE; }
+			if(c < timeout)
+				c++;
+			else 
+				return FALSE;
+			
 			b = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
+			
 			skip = !skip;
+			
 			if(skip) continue;
 		
 			if(ManchesterDecoding(b & 0x0f)) {
@@ -1581,9 +1580,9 @@ void setupIclassReader()
 {
     FpgaDownloadAndGo(FPGA_BITSTREAM_HF);
     // Reset trace buffer
-	set_tracing(TRUE);
 	clear_trace();
-
+	set_tracing(TRUE);
+	
     // Setup SSC
     FpgaSetupSsc();
     // Start from off (no field generated)
@@ -1602,16 +1601,16 @@ void setupIclassReader()
 
 }
 
-size_t sendCmdGetResponseWithRetries(uint8_t* command, size_t cmdsize, uint8_t* resp, uint8_t expected_size, uint8_t retries)
+bool sendCmdGetResponseWithRetries(uint8_t* command, size_t cmdsize, uint8_t* resp, uint8_t expected_size, uint8_t retries)
 {
 	while(retries-- > 0)
 	{
 		ReaderTransmitIClass(command, cmdsize);
 		if(expected_size == ReaderReceiveIClass(resp)){
-			return 0;
+			return true;
 		}
 	}
-	return 1;//Error
+	return false;//Error
 }
 
 /**
@@ -1621,14 +1620,17 @@ size_t sendCmdGetResponseWithRetries(uint8_t* command, size_t cmdsize, uint8_t*
  *         1 = Got CSN
  *         2 = Got CSN and CC
  */
-uint8_t handshakeIclassTag(uint8_t *card_data)
+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 };
+	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,};
+	static uint8_t readcheck_cc[]= { 0x88, 0x02 };
+	if (use_credit_key)
+		readcheck_cc[0] = 0x18;
+	else
+		readcheck_cc[0] = 0x88;
 
 	uint8_t resp[ICLASS_BUFFER_SIZE];
 
@@ -1669,6 +1671,9 @@ uint8_t handshakeIclassTag(uint8_t *card_data)
 
 	return read_status;
 }
+uint8_t handshakeIclassTag(uint8_t *card_data){
+	return handshakeIclassTag_ext(card_data, false);
+}
 
 
 // Reader iClass Anticollission
@@ -1688,6 +1693,9 @@ void ReaderIClass(uint8_t arg0) {
 	uint8_t result_status = 0;
     bool abort_after_read = arg0 & FLAG_ICLASS_READER_ONLY_ONCE;
 	bool try_once = arg0 & FLAG_ICLASS_READER_ONE_TRY;
+	bool use_credit_key = false;
+	if (arg0 & FLAG_ICLASS_READER_CEDITKEY)
+		use_credit_key = true;
 	set_tracing(TRUE);
     setupIclassReader();
 
@@ -1695,14 +1703,16 @@ void ReaderIClass(uint8_t arg0) {
     while(!BUTTON_PRESS())
     {
 		if (try_once && tryCnt > 5) break; 
+		
 		tryCnt++;
+		
 		if(!tracing) {
 			DbpString("Trace full");
 			break;
 		}
 		WDT_HIT();
 
-		read_status = handshakeIclassTag(card_data);
+		read_status = handshakeIclassTag_ext(card_data, use_credit_key);
 
 		if(read_status == 0) continue;
 		if(read_status == 1) result_status = FLAG_ICLASS_READER_CSN;
@@ -1716,11 +1726,10 @@ void ReaderIClass(uint8_t arg0) {
 		if(arg0 & FLAG_ICLASS_READER_CONF)
 		{
 			if(sendCmdGetResponseWithRetries(readConf, sizeof(readConf),card_data+8, 10, 10))
-			{
-				Dbprintf("Failed to dump config block");
-			}else
 			{
 				result_status |= FLAG_ICLASS_READER_CONF;
+			} else {
+				Dbprintf("Failed to dump config block");
 			}
 		}
 
@@ -1728,10 +1737,9 @@ void ReaderIClass(uint8_t arg0) {
 		if(arg0 & FLAG_ICLASS_READER_AA){
 			if(sendCmdGetResponseWithRetries(readAA, sizeof(readAA),card_data+(8*4), 10, 10))
 			{
-//				Dbprintf("Failed to dump AA block");
-			}else
-			{
 				result_status |= FLAG_ICLASS_READER_AA;
+			} else {
+				//Dbprintf("Failed to dump AA block");
 			}
 		}
 
@@ -1754,17 +1762,18 @@ void ReaderIClass(uint8_t arg0) {
 				cmd_send(CMD_ACK,result_status,0,0,card_data,sizeof(card_data));
 				if(abort_after_read) {
 					LED_A_OFF();
+					set_tracing(FALSE);	
 					return;
 				}
-                    //Save that we already sent this....
-                        memcpy(last_csn, card_data, 8);
+				//Save that we already sent this....
+				memcpy(last_csn, card_data, 8);
 			}
-
 		}
 		LED_B_OFF();
     }
     cmd_send(CMD_ACK,0,0,0,card_data, 0);
     LED_A_OFF();
+	set_tracing(FALSE);		
 }
 
 void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC) {
@@ -1812,33 +1821,33 @@ void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC) {
 		uint8_t read_status = handshakeIclassTag(card_data);
 		if(read_status < 2) continue;
 
-				//for now replay captured auth (as cc not updated)
-				memcpy(check+5,MAC,4);
+		//for now replay captured auth (as cc not updated)
+		memcpy(check+5,MAC,4);
 
-		if(sendCmdGetResponseWithRetries(check, sizeof(check),resp, 4, 5))
+		if(!sendCmdGetResponseWithRetries(check, sizeof(check),resp, 4, 5))
 		{
-				  Dbprintf("Error: Authentication Fail!");
+			Dbprintf("Error: Authentication Fail!");
 			continue;
-				}
+		}
 
 		//first get configuration block (block 1)
 		crc = block_crc_LUT[1];
-				read[1]=1;
-				read[2] = crc >> 8;
-				read[3] = crc & 0xff;
+		read[1]=1;
+		read[2] = crc >> 8;
+		read[3] = crc & 0xff;
 
-		if(sendCmdGetResponseWithRetries(read, sizeof(read),resp, 10, 10))
+		if(!sendCmdGetResponseWithRetries(read, sizeof(read),resp, 10, 10))
 		{
 			Dbprintf("Dump config (block 1) failed");
 			continue;
 		}
 
-					 mem=resp[5];
-					 memory.k16= (mem & 0x80);
-					 memory.book= (mem & 0x20);
-					 memory.k2= (mem & 0x8);
-					 memory.lockauth= (mem & 0x2);
-					 memory.keyaccess= (mem & 0x1);
+		 mem=resp[5];
+		 memory.k16= (mem & 0x80);
+		 memory.book= (mem & 0x20);
+		 memory.k2= (mem & 0x8);
+		 memory.lockauth= (mem & 0x2);
+		 memory.keyaccess= (mem & 0x1);
 
 		cardsize = memory.k16 ? 255 : 32;
 		WDT_HIT();
@@ -1851,15 +1860,15 @@ void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC) {
 
 			read[1]= block;
 			crc = block_crc_LUT[block];
-				    read[2] = crc >> 8;
-				    read[3] = crc & 0xff;
+			read[2] = crc >> 8;
+			read[3] = crc & 0xff;
 
-			if(!sendCmdGetResponseWithRetries(read, sizeof(read), resp, 10, 10))
+			if(sendCmdGetResponseWithRetries(read, sizeof(read), resp, 10, 10))
 			{
-				         Dbprintf("     %02x: %02x %02x %02x %02x %02x %02x %02x %02x",
-						 block, resp[0], resp[1], resp[2],
-					  resp[3], resp[4], resp[5],
-					  resp[6], resp[7]);
+				Dbprintf("     %02x: %02x %02x %02x %02x %02x %02x %02x %02x",
+					block, resp[0], resp[1], resp[2],
+					resp[3], resp[4], resp[5],
+					resp[6], resp[7]);
 
 				//Fill up the buffer
 				memcpy(card_data+stored_data_length,resp,8);
@@ -1875,8 +1884,7 @@ void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC) {
 					stored_data_length = 0;
 					failedRead = 0;
 				}
-
-			}else{
+			} else {
 				failedRead = 1;
 				stored_data_length +=8;//Otherwise, data becomes misaligned
 				Dbprintf("Failed to dump block %d", block);
@@ -1903,132 +1911,132 @@ void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC) {
 			 card_data, 0);
 
 	LED_A_OFF();
+	set_tracing(FALSE);		
 }
 
-//2. Create Read method (cut-down from above) based off responses from 1. 
-//   Since we have the MAC could continue to use replay function.
-//3. Create Write method
-/*
-void IClass_iso14443A_write(uint8_t arg0, uint8_t blockNo, uint8_t *data, uint8_t *MAC) {
-	uint8_t act_all[]     = { 0x0a };
-	uint8_t identify[]    = { 0x0c };
-	uint8_t select[]      = { 0x81, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
-	uint8_t readcheck_cc[]= { 0x88, 0x02 };
-	uint8_t check[]       = { 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
-	uint8_t read[]        = { 0x0c, 0x00, 0x00, 0x00 };
-	uint8_t write[]       = { 0x87, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
-	
-    uint16_t crc = 0;
-	
-	uint8_t* resp = (((uint8_t *)BigBuf) + 3560);
-
-	// Reset trace buffer
-	memset(trace, 0x44, RECV_CMD_OFFSET);
-	traceLen = 0;
+void iClass_ReadCheck(uint8_t	blockNo, uint8_t keyType) {
+	uint8_t readcheck[] = { keyType, blockNo };
+	uint8_t resp[] = {0,0,0,0,0,0,0,0};
+	size_t isOK = 0;
+	isOK = sendCmdGetResponseWithRetries(readcheck, sizeof(readcheck), resp, sizeof(resp), 6);
+	cmd_send(CMD_ACK,isOK,0,0,0,0);
+}
 
-	// Setup SSC
-	FpgaSetupSsc();
-	// Start from off (no field generated)
-	// Signal field is off with the appropriate LED
-	LED_D_OFF();
-	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
-	SpinDelay(200);
+void iClass_Authentication(uint8_t *MAC) {
+	uint8_t check[] = { ICLASS_CMD_CHECK, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
+	uint8_t resp[ICLASS_BUFFER_SIZE];
+	memcpy(check+5,MAC,4);
+	bool isOK;
+	isOK = sendCmdGetResponseWithRetries(check, sizeof(check), resp, 4, 6);
+	cmd_send(CMD_ACK,isOK,0,0,0,0);
+}
+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};
+	bool isOK = false;
+
+	//readcmd[1] = blockNo;
+	isOK = sendCmdGetResponseWithRetries(readcmd, sizeof(readcmd), resp, 10, 10);
+	memcpy(readdata, resp, sizeof(resp));
+
+	return isOK;
+}
 
-	SetAdcMuxFor(GPIO_MUXSEL_HIPKD);
+void iClass_ReadBlk(uint8_t blockno) {
+	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);
+}
 
-	// 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);
+void iClass_Dump(uint8_t blockno, uint8_t numblks) {
+	uint8_t readblockdata[] = {0,0,0,0,0,0,0,0,0,0};
+	bool isOK = false;
+	uint8_t blkCnt = 0;
 
-	LED_A_ON();
-
-	for(int i=0;i<1;i++) {
-	
-		if(traceLen > TRACE_SIZE) {
-			DbpString("Trace full");
+	BigBuf_free();
+	uint8_t *dataout = BigBuf_malloc(255*8);
+	if (dataout == NULL){
+		Dbprintf("out of memory");
+		OnError(1);
+		return;
+	}
+	memset(dataout,0xFF,255*8);
+
+	for (;blkCnt < numblks; blkCnt++) {
+		isOK = iClass_ReadBlock(blockno+blkCnt, readblockdata);
+		if (!isOK || (readblockdata[0] == 0xBB || readblockdata[7] == 0xBB || readblockdata[2] == 0xBB)) { //try again
+			isOK = iClass_ReadBlock(blockno+blkCnt, readblockdata);
+			if (!isOK) {
+				Dbprintf("Block %02X failed to read", blkCnt+blockno);
 			break;
 		}
-		
-		if (BUTTON_PRESS()) break;
-
-		// Send act_all
-		ReaderTransmitIClass(act_all, 1);
-		// Card present?
-		if(ReaderReceiveIClass(resp)) {
-			ReaderTransmitIClass(identify, 1);
-			if(ReaderReceiveIClass(resp) == 10) {
-				// Select card          
-				memcpy(&select[1],resp,8);
-				ReaderTransmitIClass(select, sizeof(select));
-
-				if(ReaderReceiveIClass(resp) == 10) {
-					Dbprintf("     Selected CSN: %02x %02x %02x %02x %02x %02x %02x %02x",
-					resp[0], resp[1], resp[2],
-					resp[3], resp[4], resp[5],
-					resp[6], resp[7]);
-				}
-				// Card selected
-				Dbprintf("Readcheck on Sector 2");
-				ReaderTransmitIClass(readcheck_cc, sizeof(readcheck_cc));
-				if(ReaderReceiveIClass(resp) == 8) {
-				   Dbprintf("     CC: %02x %02x %02x %02x %02x %02x %02x %02x",
-					resp[0], resp[1], resp[2],
-					resp[3], resp[4], resp[5],
-					resp[6], resp[7]);
-				}else return;
-				Dbprintf("Authenticate");
-				//for now replay captured auth (as cc not updated)
-				memcpy(check+5,MAC,4);
-				Dbprintf("     AA: %02x %02x %02x %02x",
-					check[5], check[6], check[7],check[8]);
-				ReaderTransmitIClass(check, sizeof(check));
-				if(ReaderReceiveIClass(resp) == 4) {
-				   Dbprintf("     AR: %02x %02x %02x %02x",
-					resp[0], resp[1], resp[2],resp[3]);
+		}
+		memcpy(dataout+(blkCnt*8),readblockdata,8);
+	}
+	//return pointer to dump memory in arg3
+	cmd_send(CMD_ACK,isOK,blkCnt,BigBuf_max_traceLen(),0,0);
+	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
+	LEDsoff();
+	BigBuf_free();
+}
+
+bool iClass_WriteBlock_ext(uint8_t blockNo, uint8_t *data) {
+	uint8_t write[] = { ICLASS_CMD_UPDATE, blockNo, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
+	//uint8_t readblockdata[10];
+	//write[1] = blockNo;
+	memcpy(write+2, data, 12); // data + mac
+	uint8_t resp[] = {0,0,0,0,0,0,0,0,0,0};
+	bool isOK;
+	isOK = sendCmdGetResponseWithRetries(write,sizeof(write),resp,sizeof(resp),10);
+	if (isOK) {
+		//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)) {
+			//error try again
+			isOK = sendCmdGetResponseWithRetries(write,sizeof(write),resp,sizeof(resp),10);
+		}
+	}
+	return isOK;
+}
+
+void iClass_WriteBlock(uint8_t blockNo, uint8_t *data) {
+	bool isOK = iClass_WriteBlock_ext(blockNo, data);
+	if (isOK){
+		Dbprintf("Write block [%02x] successful",blockNo);
 				}else {
-				  Dbprintf("Error: Authentication Fail!");
-				  return;
-				}
-				Dbprintf("Write Block");
-				
-				//read configuration for max block number
-				read_success=false;
-				read[1]=1;
-				uint8_t *blockno=&read[1];
-				crc = iclass_crc16((char *)blockno,1);
-				read[2] = crc >> 8;
-				read[3] = crc & 0xff;
-				while(!read_success){
-				      ReaderTransmitIClass(read, sizeof(read));
-				      if(ReaderReceiveIClass(resp) == 10) {
-					 read_success=true;
-					 mem=resp[5];
-					 memory.k16= (mem & 0x80);
-					 memory.book= (mem & 0x20);
-					 memory.k2= (mem & 0x8);
-					 memory.lockauth= (mem & 0x2);
-					 memory.keyaccess= (mem & 0x1);
-
-				      }
-				}
-				if (memory.k16){
-				  cardsize=255;
-				}else cardsize=32;
-				//check card_size
-				
-				memcpy(write+1,blockNo,1);
-				memcpy(write+2,data,8);
-				memcpy(write+10,mac,4);
-				while(!send_success){
-				  ReaderTransmitIClass(write, sizeof(write));
-				  if(ReaderReceiveIClass(resp) == 10) {
-				    write_success=true;
-				}
-			}//
+		Dbprintf("Write block [%02x] failed",blockNo);		
+	}
+	cmd_send(CMD_ACK,isOK,0,0,0,0);	
+}
+
+void iClass_Clone(uint8_t startblock, uint8_t endblock, uint8_t *data) {
+	int i;
+	int written = 0;
+	int total_block = (endblock - startblock) + 1;
+	for (i = 0; i < total_block;i++){
+		// block number
+		if (iClass_WriteBlock_ext(i+startblock, data+(i*12))){
+			Dbprintf("Write block [%02x] successful",i + startblock);
+			written++;
+		} else {
+			if (iClass_WriteBlock_ext(i+startblock, data+(i*12))){
+				Dbprintf("Write block [%02x] successful",i + startblock);
+				written++;
+			} else {
+				Dbprintf("Write block [%02x] failed",i + startblock);
+			}
 		}
-		WDT_HIT();
 	}
-	
-	LED_A_OFF();
-}*/
+	if (written == total_block)
+		Dbprintf("Clone complete");
+	else
+		Dbprintf("Clone incomplete");   
+
+	cmd_send(CMD_ACK,1,0,0,0,0);
+	FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
+	LEDsoff();
+}