X-Git-Url: http://cvs.zerfleddert.de/cgi-bin/gitweb.cgi/proxmark3-svn/blobdiff_plain/86724c17c9914f495e2a7df729d4fe65a9746d82..a826cb0df1dd2cd11facd5ab8fe0618422fa0054:/armsrc/iclass.c

diff --git a/armsrc/iclass.c b/armsrc/iclass.c
index 9139d3bd..38ccf5a4 100644
--- a/armsrc/iclass.c
+++ b/armsrc/iclass.c
@@ -353,7 +353,7 @@ static struct {
 		SUB_SECOND_HALF,
 		SUB_BOTH
 	}		sub;
-    uint8_t *output;
+    uint8_t   *output;
 } Demod;
 
 static RAMFUNC int ManchesterDecoding(int v)
@@ -658,7 +658,7 @@ void RAMFUNC SnoopIClass(void)
 	clear_trace();
     iso14a_set_trigger(FALSE);
 
-	int lastRxCounter;
+    int lastRxCounter;
     uint8_t *upTo;
     int smpl;
     int maxBehindBy = 0;
@@ -774,7 +774,7 @@ void RAMFUNC SnoopIClass(void)
 		if(ManchesterDecoding(smpl & 0x0F)) {
 			time_stop = (GetCountSspClk()-time_0) << 4;
 
-			rsamples = samples - Demod.samples;
+		    rsamples = samples - Demod.samples;
 		    LED_B_ON();
 
 			if(tracing)	{
@@ -816,6 +816,7 @@ done:
     LED_B_OFF();
     LED_C_OFF();
     LED_D_OFF();
+	set_tracing(FALSE);	
 }
 
 void rotateCSN(uint8_t* originalCSN, uint8_t* rotatedCSN) {
@@ -944,7 +945,7 @@ static void CodeIClassTagAnswer(const uint8_t *cmd, int len)
 		uint8_t b = cmd[i];
 		ToSend[++ToSendMax] = encode4Bits(b & 0xF); //Least significant half
 		ToSend[++ToSendMax] = encode4Bits((b >>4) & 0xF);//Most significant half
-	}
+			}
 
 	// Send EOF
 	ToSend[++ToSendMax] = 0xB8;
@@ -1040,7 +1041,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)
 {
@@ -1125,7 +1126,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
@@ -1230,24 +1230,24 @@ int doIClassSimulation( int simulationMode, uint8_t *reader_mac_buf)
 				//exitLoop = true;
 			}else
 			{	//Not fullsim, we don't respond
-				// We do not know what to answer, so lets keep quiet
+            // We do not know what to answer, so lets keep quiet
 				modulated_response = resp_sof; modulated_response_size = 0;
-				trace_data = NULL;
-				trace_data_size = 0;
+			trace_data = NULL;
+			trace_data_size = 0;
 				if (simulationMode == MODE_EXIT_AFTER_MAC){
-					// dbprintf:ing ...
-					Dbprintf("CSN: %02x %02x %02x %02x %02x %02x %02x %02x"
-							   ,csn[0],csn[1],csn[2],csn[3],csn[4],csn[5],csn[6],csn[7]);
-					Dbprintf("RDR:  (len=%02d): %02x %02x %02x %02x %02x %02x %02x %02x %02x",len,
-							receivedCmd[0], receivedCmd[1], receivedCmd[2],
-							receivedCmd[3], receivedCmd[4], receivedCmd[5],
-							receivedCmd[6], receivedCmd[7], receivedCmd[8]);
-					if (reader_mac_buf != NULL)
-					{
-						memcpy(reader_mac_buf,receivedCmd+1,8);
-					}
-					exitLoop = true;
+				// dbprintf:ing ...
+				Dbprintf("CSN: %02x %02x %02x %02x %02x %02x %02x %02x"
+						   ,csn[0],csn[1],csn[2],csn[3],csn[4],csn[5],csn[6],csn[7]);
+				Dbprintf("RDR:  (len=%02d): %02x %02x %02x %02x %02x %02x %02x %02x %02x",len,
+						receivedCmd[0], receivedCmd[1], receivedCmd[2],
+						receivedCmd[3], receivedCmd[4], receivedCmd[5],
+						receivedCmd[6], receivedCmd[7], receivedCmd[8]);
+				if (reader_mac_buf != NULL)
+				{
+					memcpy(reader_mac_buf,receivedCmd+1,8);
 				}
+				exitLoop = true;
+			}
 			}
 
 		} else if(receivedCmd[0] == ICLASS_CMD_HALT && len == 1) {
@@ -1336,7 +1336,6 @@ int doIClassSimulation( int simulationMode, uint8_t *reader_mac_buf)
 			}
 
 		}
-		memset(receivedCmd, 0x44, MAX_FRAME_SIZE);
 	}
 
 	//Dbprintf("%x", cmdsRecvd);
@@ -1404,17 +1403,17 @@ static void TransmitIClassCommand(const uint8_t *cmd, int len, int *samples, int
    {
      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();
-     }
+  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();
+  }
 
    }
 
@@ -1497,18 +1496,18 @@ void CodeIClassCommand(const uint8_t * cmd, int len)
 
 void ReaderTransmitIClass(uint8_t* frame, int len)
 {
-	int wait = 0;
-	int samples = 0;
+  int wait = 0;
+  int samples = 0;
 
-	// This is tied to other size changes
-	CodeIClassCommand(frame,len);
+  // This is tied to other size changes
+  CodeIClassCommand(frame,len);
 
-	// Select the card
-	TransmitIClassCommand(ToSend, ToSendMax, &samples, &wait);
-	if(trigger)
-		LED_A_ON();
+  // Select the card
+  TransmitIClassCommand(ToSend, ToSendMax, &samples, &wait);
+  if(trigger)
+  	LED_A_ON();
 
-	// Store reader command in buffer
+  // Store reader command in buffer
 	if (tracing) {
 		uint8_t par[MAX_PARITY_SIZE];
 		GetParity(frame, len, par);
@@ -1544,7 +1543,7 @@ static int GetIClassAnswer(uint8_t *receivedResponse, int maxLen, int *samples,
 	for(;;) {
 		WDT_HIT();
 
-	        if(BUTTON_PRESS()) return FALSE;
+	    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!!
@@ -1603,16 +1602,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
 }
 
 /**
@@ -1622,14 +1621,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];
 
@@ -1670,6 +1672,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
@@ -1677,7 +1682,7 @@ void ReaderIClass(uint8_t arg0) {
 
 	uint8_t card_data[6 * 8]={0};
 	memset(card_data, 0xFF, sizeof(card_data));
-	uint8_t last_csn[8]={0};
+    uint8_t last_csn[8]={0};
 	
 	//Read conf block CRC(0x01) => 0xfa 0x22
 	uint8_t readConf[] = { ICLASS_CMD_READ_OR_IDENTIFY,0x01, 0xfa, 0x22};
@@ -1685,16 +1690,19 @@ void ReaderIClass(uint8_t arg0) {
 	uint8_t readAA[] = { ICLASS_CMD_READ_OR_IDENTIFY,0x05, 0xde, 0x64};
 
 
-	int read_status= 0;
+    int read_status= 0;
 	uint8_t result_status = 0;
-	bool abort_after_read = arg0 & FLAG_ICLASS_READER_ONLY_ONCE;
+    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();
+    setupIclassReader();
 
 	uint16_t tryCnt=0;
-	while(!BUTTON_PRESS())
-	{
+    while(!BUTTON_PRESS())
+    {
 		if (try_once && tryCnt > 5) break; 
 		tryCnt++;
 		if(!tracing) {
@@ -1703,7 +1711,7 @@ void ReaderIClass(uint8_t arg0) {
 		}
 		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;
@@ -1717,11 +1725,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");
 			}
 		}
 
@@ -1729,10 +1736,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");
 			}
 		}
 
@@ -1745,9 +1751,9 @@ void ReaderIClass(uint8_t arg0) {
 		//Then we can 'ship' back the 8 * 5 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
-		if(memcmp(last_csn, card_data, 8) != 0)
+                    LED_B_ON();
+                    //Send back to client, but don't bother if we already sent this
+                    if(memcmp(last_csn, card_data, 8) != 0)
 		{
 			// If caller requires that we get CC, continue until we got it
 			if( (arg0 & read_status & FLAG_ICLASS_READER_CC) || !(arg0 & FLAG_ICLASS_READER_CC))
@@ -1755,17 +1761,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);
 			}
-
 		}
 		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) {
@@ -1816,7 +1823,7 @@ void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC) {
 		//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!");
 			continue;
@@ -1828,18 +1835,18 @@ 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))
+		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();
@@ -1847,7 +1854,7 @@ void ReaderIClass_Replay(uint8_t arg0, uint8_t *MAC) {
 		memset(card_data,0x0,USB_CMD_DATA_SIZE);
 		uint8_t failedRead =0;
 		uint32_t stored_data_length =0;
-		//then loop around remaining blocks
+				//then loop around remaining blocks
 		for(int block=0; block < cardsize; block++){
 
 			read[1]= block;
@@ -1855,12 +1862,12 @@ 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))
+			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]);
+					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);
@@ -1876,8 +1883,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);
@@ -1904,132 +1910,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;
-
-	// 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_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);
+}
 
-	SetAdcMuxFor(GPIO_MUXSEL_HIPKD);
+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;
+}
 
-	// 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_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);
+}
 
-	LED_A_ON();
+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;
 
-	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();
+}