diff --git a/armsrc/appmain.c b/armsrc/appmain.c
index 2061f6b3..f3e61f8b 100644
--- a/armsrc/appmain.c
+++ b/armsrc/appmain.c
@@ -862,11 +862,17 @@ void UsbPacketReceived(uint8_t *packet, int len)
 			SnoopIClass();
 			break;
 		case CMD_SIMULATE_TAG_ICLASS:
-			SimulateIClass(c->arg[0], c->arg[1], c->arg[2], c->d.asBytes);
+			SimulateIClass(c->arg[0], c->d.asBytes);
 			break;
 		case CMD_READER_ICLASS:
 			ReaderIClass(c->arg[0]);
 			break;
+		case CMD_READER_ICLASS_REPLAY:
+		    ReaderIClass_Replay(c->arg[0], c->d.asBytes);
+			break;
+		case CMD_ICLASS_ISO14443A_GETPUBLIC:
+		    IClass_iso14443A_GetPublic(c->arg[0]);
+		    break;
 #endif
 
 		case CMD_SIMULATE_TAG_HF_LISTEN:
diff --git a/armsrc/apps.h b/armsrc/apps.h
index 1ef0e472..083d5b43 100644
--- a/armsrc/apps.h
+++ b/armsrc/apps.h
@@ -199,9 +199,12 @@ void SetDebugIso15693(uint32_t flag);
 
 /// iclass.h
 void RAMFUNC SnoopIClass(void);
-void SimulateIClass(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain);
+void SimulateIClass(uint8_t arg0, uint8_t *datain);
 void ReaderIClass(uint8_t arg0);
+void ReaderIClass_Replay(uint8_t arg0,uint8_t *MAC);
+void IClass_iso14443A_GetPublic(uint8_t arg0);
 //int doIClassSimulation(uint8_t csn[], int breakAfterMacReceived);
+
 // hitag2.h
 void SnoopHitag(uint32_t type);
 void SimulateHitagTag(bool tag_mem_supplied, byte_t* data);
diff --git a/armsrc/iclass.c b/armsrc/iclass.c
index d5cd366d..4d005dbc 100644
--- a/armsrc/iclass.c
+++ b/armsrc/iclass.c
@@ -41,13 +41,26 @@
 #include "util.h"
 #include "string.h"
 #include "common.h"
+#include "cmd.h"
 // Needed for CRC in emulation mode;
 // same construction as in ISO 14443;
 // different initial value (CRC_ICLASS)
 #include "iso14443crc.h"
+#include "iso15693tools.h"
 
 static int timeout = 4096;
 
+// CARD TO READER
+// Sequence D: 11110000 modulation with subcarrier during first half
+// Sequence E: 00001111 modulation with subcarrier during second half
+// Sequence F: 00000000 no modulation with subcarrier
+// READER TO CARD
+// Sequence X: 00001100 drop after half a period
+// Sequence Y: 00000000 no drop
+// Sequence Z: 11000000 drop at start
+#define	SEC_X 0x0c
+#define	SEC_Y 0x00
+#define	SEC_Z 0xc0
 
 static int SendIClassAnswer(uint8_t *resp, int respLen, int delay);
 
@@ -655,7 +668,12 @@ static RAMFUNC int ManchesterDecoding(int v)
 //-----------------------------------------------------------------------------
 void RAMFUNC SnoopIClass(void)
 {
-
+// DEFINED ABOVE
+// #define RECV_CMD_OFFSET   3032
+// #define RECV_RES_OFFSET   3096
+// #define DMA_BUFFER_OFFSET 3160
+// #define DMA_BUFFER_SIZE   4096
+// #define TRACE_SIZE        3000
 
     // We won't start recording the frames that we acquire until we trigger;
     // a good trigger condition to get started is probably when we see a
@@ -665,12 +683,14 @@ void RAMFUNC SnoopIClass(void)
     // The command (reader -> tag) that we're receiving.
 	// The length of a received command will in most cases be no more than 18 bytes.
 	// So 32 should be enough!
-	uint8_t *readerToTagCmd = (((uint8_t *)BigBuf) + RECV_CMD_OFFSET);
+    uint8_t *receivedCmd = (((uint8_t *)BigBuf) + RECV_CMD_OFFSET);
     // The response (tag -> reader) that we're receiving.
-	uint8_t *tagToReaderResponse = (((uint8_t *)BigBuf) + RECV_RES_OFFSET);
+    uint8_t *receivedResponse = (((uint8_t *)BigBuf) + RECV_RES_OFFSET);
 
-    FpgaDownloadAndGo(FPGA_BITSTREAM_HF);
- 
+    // As we receive stuff, we copy it from receivedCmd or receivedResponse
+    // into trace, along with its length and other annotations.
+    //uint8_t *trace = (uint8_t *)BigBuf;
+    
     // reset traceLen to 0
     iso14a_set_tracing(TRUE);
     iso14a_clear_trace();
@@ -688,8 +708,10 @@ void RAMFUNC SnoopIClass(void)
     int samples = 0;
     rsamples = 0;
 
+    memset(trace, 0x44, RECV_CMD_OFFSET);
+
     // Set up the demodulator for tag -> reader responses.
-	Demod.output = tagToReaderResponse;
+    Demod.output = receivedResponse;
     Demod.len = 0;
     Demod.state = DEMOD_UNSYNCD;
 
@@ -701,7 +723,7 @@ void RAMFUNC SnoopIClass(void)
 
     // And the reader -> tag commands
     memset(&Uart, 0, sizeof(Uart));
-	Uart.output = readerToTagCmd;
+    Uart.output = receivedCmd;
     Uart.byteCntMax = 32; // was 100 (greg)////////////////////////////////////////////////////////////////////////
     Uart.state = STATE_UNSYNCD;
 
@@ -711,9 +733,6 @@ void RAMFUNC SnoopIClass(void)
     FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_SNIFFER);
     SetAdcMuxFor(GPIO_MUXSEL_HIPKD);
 
-	uint32_t time_0 = GetCountSspClk();
-
-
     int div = 0;
     //int div2 = 0;
     int decbyte = 0;
@@ -747,13 +766,20 @@ void RAMFUNC SnoopIClass(void)
 
         //samples += 4;
 	samples += 1;
+	//div2++;	
 
+	//if(div2 > 3) {
+		//div2 = 0;
+	//decbyte ^= ((smpl & 0x01) << (3 - div));
+	//decbyte ^= (((smpl & 0x01) | ((smpl & 0x02) >> 1)) << (3 - div)); // better already...
+	//decbyte ^= (((smpl & 0x01) | ((smpl & 0x02) >> 1) | ((smpl & 0x04) >> 2)) << (3 - div)); // even better...
 	if(smpl & 0xF) {
 		decbyte ^= (1 << (3 - div));
 	}
+	//decbyte ^= (MajorityNibble[(smpl & 0x0F)] << (3 - div));
 	
 	// FOR READER SIDE COMMUMICATION...
-
+	//decbyte ^=  ((smpl & 0x10) << (3 - div));
 	decbyter <<= 2;
 	decbyter ^= (smpl & 0x30);
 
@@ -764,17 +790,21 @@ void RAMFUNC SnoopIClass(void)
 		if(OutOfNDecoding((smpl & 0xF0) >> 4)) {
 		    rsamples = samples - Uart.samples;
 		    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)
-			{
-				LogTrace(Uart.output,Uart.byteCnt, (GetCountSspClk()-time_0) << 4, Uart.parityBits,TRUE);
-				LogTrace(NULL, 0, (GetCountSspClk()-time_0) << 4, 0, TRUE);
-			}
-
-
-			/* And ready to receive another command. */
+		    //if(triggered) {
+			trace[traceLen++] = ((rsamples >>  0) & 0xff);
+			trace[traceLen++] = ((rsamples >>  8) & 0xff);
+			trace[traceLen++] = ((rsamples >> 16) & 0xff);
+			trace[traceLen++] = ((rsamples >> 24) & 0xff);
+			trace[traceLen++] = ((Uart.parityBits >>  0) & 0xff);
+			trace[traceLen++] = ((Uart.parityBits >>  8) & 0xff);
+			trace[traceLen++] = ((Uart.parityBits >> 16) & 0xff);
+			trace[traceLen++] = ((Uart.parityBits >> 24) & 0xff);
+			trace[traceLen++] = Uart.byteCnt;
+			memcpy(trace+traceLen, receivedCmd, Uart.byteCnt);
+			traceLen += Uart.byteCnt;
+			if(traceLen > TRACE_SIZE) break;
+		    //}
+		    /* 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. */
@@ -791,16 +821,26 @@ void RAMFUNC SnoopIClass(void)
 		    rsamples = samples - Demod.samples;
 		    LED_B_ON();
 
-			if(tracing)
-			{
-				LogTrace(Demod.output,Demod.len, (GetCountSspClk()-time_0) << 4 , Demod.parityBits,FALSE);
-				LogTrace(NULL, 0, (GetCountSspClk()-time_0) << 4, 0, FALSE);
-			}
+		    // timestamp, as a count of samples
+		    trace[traceLen++] = ((rsamples >>  0) & 0xff);
+		    trace[traceLen++] = ((rsamples >>  8) & 0xff);
+		    trace[traceLen++] = ((rsamples >> 16) & 0xff);
+		    trace[traceLen++] = 0x80 | ((rsamples >> 24) & 0xff);
+		    trace[traceLen++] = ((Demod.parityBits >>  0) & 0xff);
+		    trace[traceLen++] = ((Demod.parityBits >>  8) & 0xff);
+		    trace[traceLen++] = ((Demod.parityBits >> 16) & 0xff);
+		    trace[traceLen++] = ((Demod.parityBits >> 24) & 0xff);
+		    // length
+		    trace[traceLen++] = Demod.len;
+		    memcpy(trace+traceLen, receivedResponse, Demod.len);
+		    traceLen += Demod.len;
+		    if(traceLen > TRACE_SIZE) break;
 
+		    //triggered = TRUE;
 
 		    // And ready to receive another response.
 		    memset(&Demod, 0, sizeof(Demod));
-			Demod.output = tagToReaderResponse;
+		    Demod.output = receivedResponse;
 		    Demod.state = DEMOD_UNSYNCD;
 		    LED_C_OFF();
 		}
@@ -884,8 +924,6 @@ static int GetIClassCommandFromReader(uint8_t *received, int *len, int maxLen)
 //-----------------------------------------------------------------------------
 static void CodeIClassTagAnswer(const uint8_t *cmd, int len)
 {
-	//So far a dummy implementation, not used
-	//int lastProxToAirDuration =0;
 	int i;
 
 	ToSendReset();
@@ -894,7 +932,7 @@ static void CodeIClassTagAnswer(const uint8_t *cmd, int len)
 	ToSend[++ToSendMax] = 0x00;
 	ToSend[++ToSendMax] = 0x00;
 	ToSend[++ToSendMax] = 0x00;
-	ToSend[++ToSendMax] = 0xff;//Proxtoair duration starts here
+	ToSend[++ToSendMax] = 0xff;
 	ToSend[++ToSendMax] = 0xff;
 	ToSend[++ToSendMax] = 0xff;
 	ToSend[++ToSendMax] = 0x00;
@@ -922,13 +960,11 @@ static void CodeIClassTagAnswer(const uint8_t *cmd, int len)
 	ToSend[++ToSendMax] = 0x00;
 	ToSend[++ToSendMax] = 0xff;
 	ToSend[++ToSendMax] = 0xff;
-	ToSend[++ToSendMax] = 0xff;	
+	ToSend[++ToSendMax] = 0xff;
 	ToSend[++ToSendMax] = 0x00;
 	ToSend[++ToSendMax] = 0x00;
 	ToSend[++ToSendMax] = 0x00;
 
-	//lastProxToAirDuration  = 8*ToSendMax - 3*8 - 3*8;//Not counting zeroes in the beginning or end
-
 	// Convert from last byte pos to length
 	ToSendMax++;
 }
@@ -936,10 +972,8 @@ static void CodeIClassTagAnswer(const uint8_t *cmd, int len)
 // Only SOF 
 static void CodeIClassTagSOF()
 {
-	//So far a dummy implementation, not used
-	//int lastProxToAirDuration =0;
-
 	ToSendReset();
+
 	// Send SOF
 	ToSend[++ToSendMax] = 0x00;
 	ToSend[++ToSendMax] = 0x00;
@@ -949,92 +983,37 @@ static void CodeIClassTagSOF()
 	ToSend[++ToSendMax] = 0xff;
 	ToSend[++ToSendMax] = 0x00;
 	ToSend[++ToSendMax] = 0xff;
-
-//	lastProxToAirDuration  = 8*ToSendMax - 3*8;//Not counting zeroes in the beginning
-
 	
 	// Convert from last byte pos to length
 	ToSendMax++;
 }
-int doIClassSimulation(uint8_t csn[], int breakAfterMacReceived, uint8_t *reader_mac_buf);
-/**
- * @brief SimulateIClass simulates an iClass card.
- * @param arg0 type of simulation
- *			- 0 uses the first 8 bytes in usb data as CSN
- *			- 2 "dismantling iclass"-attack. This mode iterates through all CSN's specified
- *			in the usb data. This mode collects MAC from the reader, in order to do an offline
- *			attack on the keys. For more info, see "dismantling iclass" and proxclone.com.
- *			- Other : Uses the default CSN (031fec8af7ff12e0)
- * @param arg1 - number of CSN's contained in datain (applicable for mode 2 only)
- * @param arg2
- * @param datain
- */
-void SimulateIClass(uint32_t arg0, uint32_t arg1, uint32_t arg2, uint8_t *datain)
-{
-	uint32_t simType = arg0;
-	uint32_t numberOfCSNS = arg1;
-	FpgaDownloadAndGo(FPGA_BITSTREAM_HF);
-
-	// Enable and clear the trace
-	iso14a_set_tracing(TRUE);
-	iso14a_clear_trace();
-
-	uint8_t csn_crc[] = { 0x03, 0x1f, 0xec, 0x8a, 0xf7, 0xff, 0x12, 0xe0, 0x00, 0x00 };
-	if(simType == 0) {
-		// Use the CSN from commandline
-		memcpy(csn_crc, datain, 8);
-		doIClassSimulation(csn_crc,0,NULL);
-	}else if(simType == 1)
-	{
-		doIClassSimulation(csn_crc,0,NULL);
-	}
-	else if(simType == 2)
-	{
-
-		uint8_t mac_responses[64] = { 0 };
-		Dbprintf("Going into attack mode");
-		// In this mode, a number of csns are within datain. We'll simulate each one, one at a time
-		// in order to collect MAC's from the reader. This can later be used in an offlne-attack
-		// in order to obtain the keys, as in the "dismantling iclass"-paper.
-		int i = 0;
-		for( ; i < numberOfCSNS && i*8+8 < USB_CMD_DATA_SIZE; i++)
-		{
-			// The usb data is 512 bytes, fitting 65 8-byte CSNs in there.
-
-			memcpy(csn_crc, datain+(i*8), 8);
-			if(doIClassSimulation(csn_crc,1,mac_responses))
-			{
-				return; // Button pressed
-			}
-		}
-		cmd_send(CMD_ACK,CMD_SIMULATE_TAG_ICLASS,i,0,mac_responses,i*8);
-
-	}
-	else{
-		// We may want a mode here where we hardcode the csns to use (from proxclone).
-		// That will speed things up a little, but not required just yet.
-		Dbprintf("The mode is not implemented, reserved for future use");
-	}
-	Dbprintf("Done...");
-
-}
-/**
- * @brief Does the actual simulation
- * @param csn - csn to use
- * @param breakAfterMacReceived if true, returns after reader MAC has been received.
- */
-int doIClassSimulation(uint8_t csn[], int breakAfterMacReceived, uint8_t *reader_mac_buf)
+
+//-----------------------------------------------------------------------------
+// Simulate iClass Card
+// Only CSN (Card Serial Number)
+// 
+//-----------------------------------------------------------------------------
+void SimulateIClass(uint8_t arg0, uint8_t *datain)
 {
+	uint8_t simType = arg0;
 
+  // Enable and clear the trace
+	tracing = TRUE;
+	traceLen = 0;
+  memset(trace, 0x44, TRACE_SIZE);
 
 	// CSN followed by two CRC bytes
 	uint8_t response2[] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
-	uint8_t response3[] = { 0,0,0,0,0,0,0,0,0,0};
-	memcpy(response3,csn,sizeof(response3));
-	Dbprintf("Simulating 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]);
+	uint8_t response3[] = { 0x03, 0x1f, 0xec, 0x8a, 0xf7, 0xff, 0x12, 0xe0, 0x00, 0x00 };
+
 	// e-Purse
 	uint8_t response4[] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
 
+	if(simType == 0) {
+		// Use the CSN from commandline
+		memcpy(response3, datain, 8);
+	}
+
 	// Construct anticollision-CSN
 	rotateCSN(response3,response2);
 
@@ -1042,7 +1021,6 @@ int doIClassSimulation(uint8_t csn[], int breakAfterMacReceived, uint8_t *reader
 	ComputeCrc14443(CRC_ICLASS, response2, 8, &response2[8], &response2[9]);
 	ComputeCrc14443(CRC_ICLASS, response3, 8, &response3[8], &response3[9]);
 
-	int exitLoop = 0;
 	// Reader 0a
 	// Tag    0f
 	// Reader 0c
@@ -1076,7 +1054,7 @@ int doIClassSimulation(uint8_t csn[], int breakAfterMacReceived, uint8_t *reader
 	int resp4Len;
 
 	// + 1720..
-	uint8_t *receivedCmd = (((uint8_t *)BigBuf) + RECV_CMD_OFFSET);
+  uint8_t *receivedCmd = (((uint8_t *)BigBuf) + RECV_CMD_OFFSET);
 	memset(receivedCmd, 0x44, RECV_CMD_SIZE);
 	int len;
 
@@ -1099,52 +1077,29 @@ int doIClassSimulation(uint8_t csn[], int breakAfterMacReceived, uint8_t *reader
 	CodeIClassTagAnswer(response4, sizeof(response4));
 	memcpy(resp4, ToSend, ToSendMax); resp4Len = ToSendMax;
 
-
-	// Start from off (no field generated)
-	//FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
-	//SpinDelay(200);
-	FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_TAGSIM_LISTEN);
-	SpinDelay(100);
-	StartCountSspClk();
 	// We need to listen to the high-frequency, peak-detected path.
 	SetAdcMuxFor(GPIO_MUXSEL_HIPKD);
 	FpgaSetupSsc();
 
 	// To control where we are in the protocol
 	int cmdsRecvd = 0;
-	uint32_t time_0 = GetCountSspClk();
-	uint32_t t2r_time =0;
-	uint32_t r2t_time =0;
 
 	LED_A_ON();
-	bool buttonPressed = false;
-
-	/** Hack  for testing
-	memcpy(reader_mac_buf,csn,8);
-	exitLoop = true;
-	end hack **/
-
-	while(!exitLoop) {
-
+	for(;;) {
 		LED_B_OFF();
-		//Signal tracer
-		// Can be used to get a trigger for an oscilloscope..
-		LED_C_OFF();
-
 		if(!GetIClassCommandFromReader(receivedCmd, &len, 100)) {
-			buttonPressed = true;
+			DbpString("button press");
 			break;
 		}
-		r2t_time = GetCountSspClk();
-		//Signal tracer
-		LED_C_ON();
 
 		// Okay, look at the command now.
-		if(receivedCmd[0] == 0x0a ) {
+		if(receivedCmd[0] == 0x0a) {
 			// Reader in anticollission phase
 			resp = resp1; respLen = resp1Len; //order = 1;
 			respdata = &sof;
 			respsize = sizeof(sof);
+			//resp = resp2; respLen = resp2Len; order = 2;
+			//DbpString("Hello request from reader:");
 		} else if(receivedCmd[0] == 0x0c) {
 			// Reader asks for anticollission CSN
 			resp = resp2; respLen = resp2Len; //order = 2;
@@ -1166,32 +1121,30 @@ int doIClassSimulation(uint8_t csn[], int breakAfterMacReceived, uint8_t *reader
 			LED_B_ON();
 		} else if(receivedCmd[0] == 0x05) {
 			// Reader random and reader MAC!!!
+			// Lets store this ;-)
+/*
+			Dbprintf("                CSN: %02x %02x %02x %02x %02x %02x %02x %02x",
+			response3[0], response3[1], response3[2],
+			response3[3], response3[4], response3[5],
+			response3[6], response3[7]);
+*/			
+			Dbprintf("READER AUTH (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]);
+
 			// Do not respond
 			// We do not know what to answer, so lets keep quit
 			resp = resp1; respLen = 0; //order = 5;
 			respdata = NULL;
 			respsize = 0;
-			if (breakAfterMacReceived){
-				// TODO, actually return this to the caller instead of just
-				// 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] == 0x00 && len == 1) {
 			// Reader ends the session
 			resp = resp1; respLen = 0; //order = 0;
 			respdata = NULL;
 			respsize = 0;
 		} else {
-			//#db# Unknown command received from reader (len=5): 26 1 0 f6 a 44 44 44 44
 			// Never seen this command before
 			Dbprintf("Unknown command received from reader (len=%d): %x %x %x %x %x %x %x %x %x",
 			len,
@@ -1204,9 +1157,9 @@ int doIClassSimulation(uint8_t csn[], int breakAfterMacReceived, uint8_t *reader
 			respsize = 0;
 		}
 
-		if(cmdsRecvd >  100) {
-			//DbpString("100 commands later...");
-			//break;
+		if(cmdsRecvd > 999) {
+			DbpString("1000 commands later...");
+			break;
 		}
 		else {
 			cmdsRecvd++;
@@ -1214,68 +1167,64 @@ int doIClassSimulation(uint8_t csn[], int breakAfterMacReceived, uint8_t *reader
 
 		if(respLen > 0) {
 			SendIClassAnswer(resp, respLen, 21);
-			t2r_time = GetCountSspClk();
 		}
-
+		
 		if (tracing) {
-			LogTrace(receivedCmd,len, (r2t_time-time_0)<< 4, Uart.parityBits,TRUE);
-			LogTrace(NULL,0, (r2t_time-time_0) << 4, 0,TRUE);
-
+			LogTrace(receivedCmd,len, rsamples, Uart.parityBits, TRUE);
 			if (respdata != NULL) {
-				LogTrace(respdata,respsize, (t2r_time-time_0) << 4,SwapBits(GetParity(respdata,respsize),respsize),FALSE);
-				LogTrace(NULL,0, (t2r_time-time_0) << 4,0,FALSE);
-
-
+				LogTrace(respdata,respsize, rsamples, SwapBits(GetParity(respdata,respsize),respsize), FALSE);
 			}
-			if(!tracing) {
+			if(traceLen > TRACE_SIZE) {
 				DbpString("Trace full");
-				//break;
+				break;
 			}
-
 		}
+
 		memset(receivedCmd, 0x44, RECV_CMD_SIZE);
 	}
 
-	//Dbprintf("%x", cmdsRecvd);
+	Dbprintf("%x", cmdsRecvd);
 	LED_A_OFF();
 	LED_B_OFF();
-	if(buttonPressed)
-	{
-		DbpString("Button pressed");
-	}
-	return buttonPressed;
 }
 
 static int SendIClassAnswer(uint8_t *resp, int respLen, int delay)
 {
-	int i = 0, d=0;//, u = 0, d = 0;
+	int i = 0, u = 0, d = 0;
 	uint8_t b = 0;
-
-	FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_SIMULATOR|FPGA_HF_SIMULATOR_MODULATE_424K);
-
+	// return 0;
+	// Modulate Manchester
+	// FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_TAGSIM_MOD424);
+	FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_TAGSIM_MOD);
 	AT91C_BASE_SSC->SSC_THR = 0x00;
 	FpgaSetupSsc();
-	while(!BUTTON_PRESS()) {
-		if((AT91C_BASE_SSC->SSC_SR & AT91C_SSC_RXRDY)){
-			b = AT91C_BASE_SSC->SSC_RHR; (void) b;
+	
+	// send cycle
+	for(;;) {
+		if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) {
+			volatile uint8_t b = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
+			(void)b;
 		}
-		if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)){
-			b = 0x00;
+		if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) {
 			if(d < delay) {
+				b = 0x00;
 				d++;
 			}
-			else {
-				if( i < respLen){
-					b = resp[i];
-					//Hack
-					//b = 0xAC;
-				}
-				i++;
+			else if(i >= respLen) {
+				b = 0x00;
+				u++;
+			} else {
+				b = resp[i];
+				u++;
+				if(u > 1) { i++; u = 0; }
 			}
 			AT91C_BASE_SSC->SSC_THR = b;
-		}
 
-		if (i > respLen +4) break;
+			if(u > 4) break;
+		}
+		if(BUTTON_PRESS()) {
+			break;
+		}
 	}
 
 	return 0;
@@ -1289,6 +1238,7 @@ 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();
@@ -1364,12 +1314,12 @@ void CodeIClassCommand(const uint8_t * cmd, int len)
     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;
-			}
+	if(k == (b & 3)) {
+	    ToSend[++ToSendMax] = 0x0f;
+	}
+	else {
+	    ToSend[++ToSendMax] = 0x00;
+	}
       }
       b >>= 2;
     }
@@ -1473,10 +1423,8 @@ void ReaderIClass(uint8_t arg0) {
 
 	uint8_t* resp = (((uint8_t *)BigBuf) + 3560);	// was 3560 - tied to other size changes
 
-    FpgaDownloadAndGo(FPGA_BITSTREAM_HF);
-
 	// Reset trace buffer
-	memset(trace, 0x44, RECV_CMD_OFFSET);
+    	memset(trace, 0x44, RECV_CMD_OFFSET);
 	traceLen = 0;
 
 	// Setup SSC
@@ -1530,4 +1478,245 @@ void ReaderIClass(uint8_t arg0) {
 	LED_A_OFF();
 }
 
+void ReaderIClass_Replay(uint8_t arg0, 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 };
+	
+    uint16_t crc = 0;
+	uint8_t cardsize=0;
+	bool read_success=false;
+	uint8_t mem=0;
+	
+	static struct memory_t{
+	  int k16;
+	  int book;
+	  int k2;
+	  int lockauth;
+	  int keyaccess;
+	} memory;
+	
+	uint8_t* resp = (((uint8_t *)BigBuf) + 3560);	// was 3560 - tied to other size changes
+
+	// 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);
+
+	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();
+
+	for(int i=0;i<1;i++) {
+	
+		if(traceLen > TRACE_SIZE) {
+			DbpString("Trace full");
+			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]);
+				}else {
+				  Dbprintf("Error: Authentication Fail!");
+				  return;
+				}
+				Dbprintf("Dump Contents");
+				//first get configuration block
+				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;
+				//then loop around remaining blocks
+				for(uint8_t j=0; j<cardsize; j++){
+				    read_success=false;
+				    uint8_t *blockno=&j;
+				    //crc_data[0]=j;
+				    read[1]=j;
+				    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;
+				         Dbprintf("     %02x: %02x %02x %02x %02x %02x %02x %02x %02x",
+					  j, resp[0], resp[1], resp[2],
+					  resp[3], resp[4], resp[5],
+					  resp[6], resp[7]);
+				      }
+				    }
+				}
+			}
+		}
+		WDT_HIT();
+	}
+	
+	LED_A_OFF();
+}
+
+//1. Create Method to Read sectors/blocks 0,1,2 and Send to client
+void IClass_iso14443A_GetPublic(uint8_t arg0) {
+	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 read[]        = { 0x0c, 0x00, 0x00, 0x00 };	
+	uint8_t card_data[24]={0};
+	
+	//bool read_success=false;
+	uint8_t* resp = (((uint8_t *)BigBuf) + 3560);	// was 3560 - tied to other size changes
+
+	// 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);
+
+	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();
+
+	for(int i=0;i<1;i++) {
+	
+		if(traceLen > TRACE_SIZE) {
+			DbpString("Trace full");
+			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]);
+				}
+				memcpy(card_data,resp,8);
+				// 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]);
+				}
+				memcpy(card_data+8,resp,8);
+				//prep to read config block
+				/*  read card configuration block
+				  while(!read_success){
+				  uint8_t sector_config=0x01;
+				  memcpy(read+1,&sector_config,1);
+				  ReaderTransmitIClass(read, sizeof(read));
+				  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]);
+					read_success=true;
+                    memcpy(card_data+16,resp,8);
+				  }
+				}*/
+			}
+		}
+		WDT_HIT();
+	}
+	//Dbprintf("DEBUG: %02x%02x%02x%02x%02x%02x%02x%02x",card_data[0],card_data[1],card_data[2],card_data[3],card_data[4],card_data[5],card_data[6],card_data[7]);
+	//Dbprintf("DEBUG: %02x%02x%02x%02x%02x%02x%02x%02x",card_data[8],card_data[9],card_data[10],card_data[11],card_data[12],card_data[13],card_data[14],card_data[15]);
+	LED_A_OFF();
+	LED_B_ON();
+	//send data back to the client
+    cmd_send(CMD_ACK,0,0,0,card_data,16);
+	LED_B_OFF();
+}
+
+//TODO: Create Write method
 
diff --git a/client/Makefile b/client/Makefile
index e4a3580b..eede2268 100644
--- a/client/Makefile
+++ b/client/Makefile
@@ -58,6 +58,10 @@ CORESRCS = 	uart.c \
 CMDSRCS = 	nonce2key/crapto1.c\
 		nonce2key/crypto1.c\
 		nonce2key/nonce2key.c\
+		loclass/cipher.c \
+		loclass/cipherutils.c \
+		loclass/des.c \
+		loclass/ikeys.c \
 			mifarehost.c\
 			crc16.c \
 			iso14443crc.c \
diff --git a/client/cmdhficlass.c b/client/cmdhficlass.c
index b8e1e098..21297b5e 100644
--- a/client/cmdhficlass.c
+++ b/client/cmdhficlass.c
@@ -1,6 +1,7 @@
 //-----------------------------------------------------------------------------
 // Copyright (C) 2010 iZsh <izsh at fail0verflow.com>, Hagen Fritsch
 // Copyright (C) 2011 Gerhard de Koning Gans
+// Copyright (C) 2014 Midnitesnake & Andy Davies
 //
 // This code is licensed to you under the terms of the GNU GPL, version 2 or,
 // at your option, any later version. See the LICENSE.txt file for the text of
@@ -12,7 +13,6 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include <sys/stat.h>
 #include "iso14443crc.h" // Can also be used for iClass, using 0xE012 as CRC-type
 #include "data.h"
 //#include "proxusb.h"
@@ -22,137 +22,14 @@
 #include "cmdhficlass.h"
 #include "common.h"
 #include "util.h"
-#include "cmdmain.h"
+#include "loclass/des.h"
+#include "loclass/cipherutils.h"
+#include "loclass/cipher.h"
+#include "loclass/ikeys.h"
 
 static int CmdHelp(const char *Cmd);
 
-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)
-{
-
-	bool ShowWaitCycles = false;
-	char param = param_getchar(Cmd, 0);
-
-	if (param != 0) {
-		PrintAndLog("List data in trace buffer.");
-		PrintAndLog("Usage:  hf iclass list");
-		PrintAndLog("h - help");
-		PrintAndLog("sample: hf iclass list");
-		return 0;
-	}
-
-	uint8_t got[1920];
-	GetFromBigBuf(got,sizeof(got),0);
-	WaitForResponse(CMD_ACK,NULL);
-
-	PrintAndLog("Recorded Activity");
-	PrintAndLog("");
-	PrintAndLog("Start = Start of Start Bit, End = End of last modulation. Src = Source of Transfer");
-	PrintAndLog("All times are in carrier periods (1/13.56Mhz)");
-	PrintAndLog("");
-	PrintAndLog("     Start |       End | Src | Data");
-	PrintAndLog("-----------|-----------|-----|--------");
-
-	int i;
-	uint32_t first_timestamp = 0;
-	uint32_t timestamp;
-	bool tagToReader;
-	uint32_t parityBits;
-	uint8_t len;
-	uint8_t *frame;
-	uint32_t EndOfTransmissionTimestamp = 0;
-
-
-	for( i=0; i < 1900;)
-	{
-		//First 32 bits contain
-		// isResponse (1 bit)
-		// timestamp (remaining)
-		//Then paritybits
-		//Then length
-		timestamp = *((uint32_t *)(got+i));
-		parityBits = *((uint32_t *)(got+i+4));
-		len = got[i+8];
-		frame = (got+i+9);
-		uint32_t next_timestamp = (*((uint32_t *)(got+i+9))) & 0x7fffffff;
-
-		tagToReader = timestamp & 0x80000000;
-		timestamp &= 0x7fffffff;
-
-		if(i==0) {
-			first_timestamp = timestamp;
-		}
-
-		// Break and stick with current result if buffer was not completely full
-		if (frame[0] == 0x44 && frame[1] == 0x44 && frame[2] == 0x44 && frame[3] == 0x44) break;
-
-		char line[1000] = "";
-
-		if(len)//We have some data to display
-		{
-			int j,oddparity;
-
-			for(j = 0; j < len ; j++)
-			{
-				oddparity = 0x01 ^ xorbits_8(frame[j] & 0xFF);
-
-				if (tagToReader && (oddparity != ((parityBits >> (len - j - 1)) & 0x01))) {
-					sprintf(line+(j*4), "%02x!  ", frame[j]);
-				} else {
-					sprintf(line+(j*4), "%02x   ", frame[j]);
-				}
-			}
-		}else
-		{
-			if (ShowWaitCycles) {
-				sprintf(line, "fdt (Frame Delay Time): %d", (next_timestamp - timestamp));
-			}
-		}
-
-		char *crc = "";
-
-		if(len > 2)
-		{
-			uint8_t b1, b2;
-			if(!tagToReader && len == 4) {
-				// Rough guess that this is a command from the reader
-				// For iClass the command byte is not part of the CRC
-					ComputeCrc14443(CRC_ICLASS, &frame[1], len-3, &b1, &b2);
-			}
-			else {
-				  // For other data.. CRC might not be applicable (UPDATE commands etc.)
-				ComputeCrc14443(CRC_ICLASS, frame, len-2, &b1, &b2);
-			}
-
-			if (b1 != frame[len-2] || b2 != frame[len-1]) {
-				crc = (tagToReader & (len < 8)) ? "" : " !crc";
-			}
-		}
-
-		i += (len + 9);
-		EndOfTransmissionTimestamp = (*((uint32_t *)(got+i))) & 0x7fffffff;
-
-		// Not implemented for iclass on the ARM-side
-		//if (!ShowWaitCycles) i += 9;
-
-		PrintAndLog(" %9d | %9d | %s | %s %s",
-			(timestamp - first_timestamp),
-			(EndOfTransmissionTimestamp - first_timestamp),
-			(len?(tagToReader ? "Tag" : "Rdr"):"   "),
-			line, crc);
-	}
-	return 0;
-}
-
-int CmdHFiClassListOld(const char *Cmd)
 {
   uint8_t got[1920];
   GetFromBigBuf(got,sizeof(got),0);
@@ -178,9 +55,7 @@ int CmdHFiClassListOld(const char *Cmd)
       isResponse = 0;
     }
 
-
     int metric = 0;
-
     int parityBits = *((uint32_t *)(got+i+4));
     // 4 bytes of additional information...
     // maximum of 32 additional parity bit information
@@ -290,11 +165,6 @@ int CmdHFiClassListOld(const char *Cmd)
   return 0;
 }
 
-/*void iso14a_set_timeout(uint32_t timeout) {
-	UsbCommand c = {CMD_READER_ISO_14443a, {ISO14A_SET_TIMEOUT, 0, timeout}};
-	SendCommand(&c);
-}*/
-
 int CmdHFiClassSnoop(const char *Cmd)
 {
   UsbCommand c = {CMD_SNOOP_ICLASS};
@@ -307,92 +177,23 @@ 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) {
-	PrintAndLog("Usage:  hf iclass sim [0 <CSN>] | x");
-	PrintAndLog("        options");
-	PrintAndLog("                0 <CSN> simulate the given CSN");
-	PrintAndLog("                1       simulate default CSN");
-	PrintAndLog("                2       iterate CSNs, gather MACs");
+  if (strlen(Cmd)<2) {
+	PrintAndLog("Usage:  hf iclass sim    <sim type> <CSN (16 hex symbols)>");
 	PrintAndLog("        sample: hf iclass sim 0 031FEC8AF7FF12E0");
-	PrintAndLog("        sample: hf iclass sim 2");
 	return 0;
   }	
 
   simType = param_get8(Cmd, 0);
-
-  if(simType == 0)
-  {
-	  if (param_gethex(Cmd, 1, CSN, 16)) {
-		  PrintAndLog("A CSN should consist of 16 HEX symbols");
-		  return 1;
-	  }
-	  PrintAndLog("--simtype:%02x csn:%s", simType, sprint_hex(CSN, 8));
-
+  if (param_gethex(Cmd, 1, CSN, 16)) {
+	PrintAndLog("A CSN should consist of 16 HEX symbols");
+	return 1;
   }
-  if(simType > 2)
-  {
-	  PrintAndLog("Undefined simptype %d", simType);
-	  return 1;
-  }
-  uint8_t numberOfCSNs=0;
+  PrintAndLog("--simtype:%02x csn:%s", simType, sprint_hex(CSN, 8));
 
-	if(simType == 2)
-	{
-		UsbCommand c = {CMD_SIMULATE_TAG_ICLASS, {simType,63}};
-		UsbCommand resp = {0};
+  UsbCommand c = {CMD_SIMULATE_TAG_ICLASS, {simType}};
+  memcpy(c.d.asBytes, CSN, 8);
+  SendCommand(&c);
 
-		uint8_t csns[64] = {
-			 0x00,0x0B,0x0F,0xFF,0xF7,0xFF,0x12,0xE0 ,
-			 0x00,0x13,0x94,0x7e,0x76,0xff,0x12,0xe0 ,
-			 0x2a,0x99,0xac,0x79,0xec,0xff,0x12,0xe0 ,
-			 0x17,0x12,0x01,0xfd,0xf7,0xff,0x12,0xe0 ,
-			 0xcd,0x56,0x01,0x7c,0x6f,0xff,0x12,0xe0 ,
-			 0x4b,0x5e,0x0b,0x72,0xef,0xff,0x12,0xe0 ,
-			 0x00,0x73,0xd8,0x75,0x58,0xff,0x12,0xe0 ,
-			 0x0c,0x90,0x32,0xf3,0x5d,0xff,0x12,0xe0 };
-
-		memcpy(c.d.asBytes, csns, 64);
-
-		SendCommand(&c);
-		if (!WaitForResponseTimeout(CMD_ACK, &resp, -1)) {
-			PrintAndLog("Command timed out");
-			return 0;
-		}
-
-		uint8_t num_mac_responses  = resp.arg[1];
-		PrintAndLog("Mac responses: %d MACs obtained (should be 8)", num_mac_responses);
-
-		size_t datalen = 8*24;
-		/*
-		 * Now, time to dump to file. We'll use this format:
-		 * <8-byte CSN><8-byte CC><4 byte NR><4 byte MAC>....
-		 * So, it should wind up as
-		 * 8 * 24 bytes.
-		 *
-		 * The returndata from the pm3 is on the following format
-		 * <4 byte NR><4 byte MAC>
-		 * CC are all zeroes, CSN is the same as was sent in
-		 **/
-		void* dump = malloc(datalen);
-		memset(dump,0,datalen);//<-- Need zeroes for the CC-field
-		uint8_t i = 0;
-		for(i = 0 ; i < 8 ; i++)
-		{
-			memcpy(dump+i*24, csns+i*8,8); //CSN
-			//8 zero bytes here...
-			//Then comes NR_MAC (eight bytes from the response)
-			memcpy(dump+i*24+16,resp.d.asBytes+i*8,8);
-
-		}
-		/** Now, save to dumpfile **/
-		saveFile("iclass_mac_attack", "bin", dump,datalen);
-		free(dump);
-	}else
-	{
-		UsbCommand c = {CMD_SIMULATE_TAG_ICLASS, {simType,numberOfCSNs}};
-		memcpy(c.d.asBytes, CSN, 8);
-		SendCommand(&c);
-	}
   return 0;
 }
 
@@ -410,27 +211,130 @@ int CmdHFiClassReader(const char *Cmd)
   PrintAndLog("--readertype:%02x", readerType);
 
   UsbCommand c = {CMD_READER_ICLASS, {readerType}};
-  //memcpy(c.d.asBytes, CSN, 8);
   SendCommand(&c);
 
-  /*UsbCommand * resp = WaitForResponseTimeout(CMD_ACK, 1500);
-  if (resp != NULL) {
-	uint8_t                isOK  = resp->arg[0] & 0xff;
+  return 0;
+}
+
+int CmdHFiClassReader_Replay(const char *Cmd)
+{
+  uint8_t readerType = 0;
+  uint8_t MAC[4]={0x00, 0x00, 0x00, 0x00};
+
+  if (strlen(Cmd)<1) {
+    PrintAndLog("Usage:  hf iclass replay <MAC>");
+    PrintAndLog("        sample: hf iclass replay 00112233");
+    return 0;
+  }
+
+  if (param_gethex(Cmd, 0, MAC, 8)) {
+    PrintAndLog("MAC must include 8 HEX symbols");
+    return 1;
+  }
+
+  UsbCommand c = {CMD_READER_ICLASS_REPLAY, {readerType}};
+  memcpy(c.d.asBytes, MAC, 4);
+  SendCommand(&c);
+
+  return 0;
+}
+
+int CmdHFiClassReader_Dump(const char *Cmd)
+{
+  uint8_t readerType = 0;
+  uint8_t MAC[4]={0x00,0x00,0x00,0x00};
+  uint8_t 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 CC_temp[8]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
+  uint8_t result[8]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
+  uint8_t div_key[8]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
+  des_context ctx_enc;
+  uint64_t crypted_id=0;
+
+  if (strlen(Cmd)<1) 
+  {
+    //PrintAndLog("Usage:  hf iclass dump <Key> <CSN> <CC>");
+    //PrintAndLog("        sample: hf iclass dump 0011223344556677 aabbccddeeffgghh FFFFFFFFFFFFFFFF");
+    PrintAndLog("Usage:  hf iclass dump <Key>");
+    PrintAndLog("        sample: hf iclass dump 0011223344556677");
+    return 0;
+  }
+
+  if (param_gethex(Cmd, 0, KEY, 16)) 
+  {
+    PrintAndLog("KEY must include 16 HEX symbols");
+    return 1;
+  }
+  
+  /*if (param_gethex(Cmd, 1, CSN, 16)) 
+  {
+    PrintAndLog("CSN must include 16 HEX symbols");
+    return 1;
+  }
+  if (param_gethex(Cmd, 2, CC_temp, 16)) 
+  {
+    PrintAndLog("CC must include 16 HEX symbols");
+    return 1;
+  }*/
+  
+  UsbCommand c = {CMD_ICLASS_ISO14443A_GETPUBLIC, {0}};
+  //memcpy(c.d.asBytes, MAC, 4);
+  SendCommand(&c);
+  
+  UsbCommand resp;
+  if (WaitForResponseTimeout(CMD_ACK,&resp,4500)) {
+    uint8_t isOK    = resp.arg[0] & 0xff;
+    uint8_t * data  = resp.d.asBytes;
+    
+    memcpy(CSN,data,8);
+    memcpy(CCNR,data+8,8);
+    PrintAndLog("DEBUG: CSN %s",sprint_hex(CSN,8));
+    PrintAndLog("DEBUG: CC  %s",sprint_hex(CCNR,8));
 	PrintAndLog("isOk:%02x", isOK);
   } else {
 	PrintAndLog("Command execute timeout");
-  }*/
+  }
+  
+  //memcpy(CCNR,CC_temp,8);
+  des_setkey_enc( &ctx_enc, KEY);
+  des_crypt_ecb(&ctx_enc,CSN,result);
+  PrintAndLog("DES Key: %s",sprint_hex(result,8));
+  uint64_t newz=0;
+  crypted_id = bytes_to_num(result,8);
+  uint64_t x = (crypted_id & 0xFFFF000000000000 );
+  pushbackSixBitByte(&newz, getSixBitByte(crypted_id,0),7);
+  pushbackSixBitByte(&newz, getSixBitByte(crypted_id,1),6);
+  pushbackSixBitByte(&newz, getSixBitByte(crypted_id,2),5);
+  pushbackSixBitByte(&newz, getSixBitByte(crypted_id,3),4);
+  pushbackSixBitByte(&newz, getSixBitByte(crypted_id,4),3);
+  pushbackSixBitByte(&newz, getSixBitByte(crypted_id,5),2);
+  pushbackSixBitByte(&newz, getSixBitByte(crypted_id,6),1);
+  pushbackSixBitByte(&newz, getSixBitByte(crypted_id,7),0);
+  newz|= x;
+  crypted_id=newz;
+  num_to_bytes(crypted_id,8,result);
+  PrintAndLog("DESr Key: %s",sprint_hex(result,8));	
+  hash0(crypted_id,div_key);
+  PrintAndLog("Div Key: %s",sprint_hex(div_key,8));
+  calc_iclass_mac(CCNR,12,div_key,MAC);
+
+  UsbCommand d = {CMD_READER_ICLASS_REPLAY, {readerType}};
+  memcpy(d.d.asBytes, MAC, 4);
+  SendCommand(&d);
 
   return 0;
 }
 
 static command_t CommandTable[] = 
 {
-  {"help",    CmdHelp,        1, "This help"},
-  {"list",    CmdHFiClassList,   0, "List iClass history"},
-  {"snoop",   CmdHFiClassSnoop,  0, "Eavesdrop iClass communication"},
-  {"sim",     CmdHFiClassSim,    0, "Simulate iClass tag"},
-  {"reader",  CmdHFiClassReader, 0, "Read an iClass tag"},
+  {"help",	CmdHelp,		1,	"This help"},
+  {"list",	CmdHFiClassList,	0,	"List iClass history"},
+  {"snoop",	CmdHFiClassSnoop,	0,	"Eavesdrop iClass communication"},
+  {"sim",	CmdHFiClassSim,		0,	"Simulate iClass tag"},
+  {"reader",	CmdHFiClassReader,	0,	"Read an iClass tag"},
+  {"replay",	CmdHFiClassReader_Replay,0,	"Read an iClass tag via Reply Attack"},
+  {"dump",	CmdHFiClassReader_Dump,	0,	"Authenticate and Dump iClass tag"},
   {NULL, NULL, 0, NULL}
 };
 
@@ -446,52 +350,3 @@ int CmdHelp(const char *Cmd)
   return 0;
 }
 
-/**
- * @brief checks if a file exists
- * @param filename
- * @return
- */
-int fileExists(const char *filename) {
-	struct stat st;
-	int result = stat(filename, &st);
-	return result == 0;
-}
-/**
- * @brief Utility function to save data to a file. This method takes a preferred name, but if that
- * file already exists, it tries with another name until it finds something suitable.
- * E.g. dumpdata-15.txt
- * @param preferredName
- * @param suffix the file suffix. Leave out the ".".
- * @param data The binary data to write to the file
- * @param datalen the length of the data
- * @return 0 for ok, 1 for failz
- */
-int saveFile(const char *preferredName, const char *suffix, const void* data, size_t datalen)
-{
-	FILE *f = fopen(preferredName, "wb");
-	int size = sizeof(char) * (strlen(preferredName)+strlen(suffix)+5);
-	char * fileName = malloc(size);
-
-	memset(fileName,0,size);
-	int num = 1;
-	sprintf(fileName,"%s.%s", preferredName, suffix);
-	while(fileExists(fileName))
-	{
-		sprintf(fileName,"%s-%d.%s", preferredName, num, suffix);
-		num++;
-	}
-	/* We should have a valid filename now, e.g. dumpdata-3.bin */
-
-	/*Opening file for writing in binary mode*/
-	FILE *fileHandle=fopen(fileName,"wb");
-	if(!f) {
-		PrintAndLog("Failed to write to file '%s'", fileName);
-		return 0;
-	}
-	fwrite(data, 1,	datalen, fileHandle);
-	fclose(fileHandle);
-	PrintAndLog("Saved data to '%s'", fileName);
-
-	free(fileName);
-	return 0;
-}
diff --git a/client/loclass/cipher.c b/client/loclass/cipher.c
new file mode 100644
index 00000000..aad77a2e
--- /dev/null
+++ b/client/loclass/cipher.c
@@ -0,0 +1,261 @@
+/*****************************************************************************
+ * This file is part of iClassCipher. It is a reconstructon of the cipher engine
+ * used in iClass, and RFID techology.
+ *
+ * The implementation is based on the work performed by
+ * Flavio D. Garcia, Gerhard de Koning Gans, Roel Verdult and
+ * Milosch Meriac in the paper "Dismantling IClass".
+ *
+ * Copyright (C) 2014 Martin Holst Swende
+ *
+ * This is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as published
+ * by the Free Software Foundation.
+ *
+ * This file is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with IClassCipher.  If not, see <http://www.gnu.org/licenses/>.
+ ****************************************************************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include "loclass/cipher.h"
+#include "loclass/cipherutils.h"
+#include "loclass/ikeys.h"
+
+uint8_t keytable[] = { 0,0,0,0,0,0,0,0};
+
+/**
+*	Definition 2. The feedback function for the top register T : F 16/2 → F 2
+*	is defined as
+*	T (x 0 x 1 . . . . . . x 15 ) = x 0 ⊕ x 1 ⊕ x 5 ⊕ x 7 ⊕ x 10 ⊕ x 11 ⊕ x 14 ⊕ x 15 .
+**/
+bool T(State state)
+{
+	bool x0 = state.t & 0x8000;
+	bool x1 = state.t & 0x4000;
+	bool x5 = state.t & 0x0400;
+	bool x7 = state.t & 0x0100;
+	bool x10 = state.t & 0x0020;
+	bool x11 = state.t & 0x0010;
+	bool x14 = state.t & 0x0002;
+	bool x15 = state.t & 0x0001;
+	return x0 ^ x1 ^ x5 ^ x7 ^ x10 ^ x11 ^ x14 ^ x15;
+}
+/**
+*	Similarly, the feedback function for the bottom register B : F 8/2 → F 2 is defined as
+*	B(x 0 x 1 . . . x 7 ) = x 1 ⊕ x 2 ⊕ x 3 ⊕ x 7 .
+**/
+bool B(State state)
+{
+	bool x1 = state.b & 0x40;
+	bool x2 = state.b & 0x20;
+	bool x3 = state.b & 0x10;
+	bool x7 = state.b & 0x01;
+
+	return x1 ^ x2 ^ x3 ^ x7;
+
+}
+
+
+/**
+*	Definition 3 (Selection function). The selection function select : F 2 × F 2 ×
+*	F 8/2 → F 3/2 is defined as select(x, y, r) = z 0 z 1 z 2 where
+*	z 0 = (r 0 ∧ r 2 ) ⊕ (r 1 ∧ r 3 ) ⊕ (r 2 ∨ r 4 )
+*	z 1 = (r 0 ∨ r 2 ) ⊕ (r 5 ∨ r 7 ) ⊕ r 1 ⊕ r 6 ⊕ x ⊕ y
+*	z 2 = (r 3 ∧ r 5 ) ⊕ (r 4 ∧ r 6 ) ⊕ r 7 ⊕ x
+**/
+uint8_t _select(bool x, bool y, uint8_t r)
+{
+	bool r0 = r >> 7 & 0x1;
+	bool r1 = r >> 6 & 0x1;
+	bool r2 = r >> 5 & 0x1;
+	bool r3 = r >> 4 & 0x1;
+	bool r4 = r >> 3 & 0x1;
+	bool r5 = r >> 2 & 0x1;
+	bool r6 = r >> 1 & 0x1;
+	bool r7 = r & 0x1;
+
+	bool z0 = (r0 & r2) ^ (r1 & ~r3) ^ (r2 | r4);
+	bool z1 = (r0 | r2) ^ ( r5 | r7) ^ r1 ^ r6 ^ x ^ y;
+	bool z2 = (r3 & ~r5) ^ (r4 & r6 ) ^ r7 ^ x;
+
+	// The three bitz z0.. z1 are packed into a uint8_t:
+	// 00000ZZZ
+	//Return value is a uint8_t
+	uint8_t retval = 0;
+	retval |= (z0 << 2) & 4;
+	retval |= (z1 << 1) & 2;
+	retval |= z2 & 1;
+
+	// Return value 0 <= retval <= 7
+	return retval;
+}
+
+/**
+*	Definition 4 (Successor state). Let s = l, r, t, b be a cipher state, k ∈ (F 82 ) 8
+*	be a key and y ∈ F 2 be the input bit. Then, the successor cipher state s ′ =
+*	l ′ , r ′ , t ′ , b ′ is defined as
+*	t ′ := (T (t) ⊕ r 0 ⊕ r 4 )t 0 . . . t 14 l ′ := (k [select(T (t),y,r)] ⊕ b ′ ) ⊞ l ⊞ r
+*	b ′ := (B(b) ⊕ r 7 )b 0 . . . b 6 r ′ := (k [select(T (t),y,r)] ⊕ b ′ ) ⊞ l
+*
+* @param s - state
+* @param k - array containing 8 bytes
+**/
+State successor(uint8_t* k, State s, bool y)
+{
+	bool r0 = s.r >> 7 & 0x1;
+	bool r4 = s.r >> 3 & 0x1;
+	bool r7 = s.r & 0x1;
+
+	State successor = {0,0,0,0};
+
+	successor.t = s.t >> 1;
+	successor.t |= (T(s) ^ r0 ^ r4) << 15;
+
+	successor.b = s.b >> 1;
+	successor.b |= (B(s) ^ r7) << 7;
+
+	bool Tt = T(s);
+
+	successor.l = ((k[_select(Tt,y,s.r)] ^ successor.b) + s.l+s.r ) & 0xFF;
+	successor.r = ((k[_select(Tt,y,s.r)] ^ successor.b) + s.l ) & 0xFF;
+
+	return successor;
+}
+/**
+*	We define the successor function suc which takes a key k ∈ (F 82 ) 8 , a state s and
+*	an input y ∈ F 2 and outputs the successor state s ′ . We overload the function suc
+*	to multiple bit input x ∈ F n 2 which we define as
+* @param k - array containing 8 bytes
+**/
+State suc(uint8_t* k,State s, BitstreamIn *bitstream)
+{
+	if(bitsLeft(bitstream) == 0)
+	{
+		return s;
+	}
+	bool lastbit = tailBit(bitstream);
+	return successor(k,suc(k,s,bitstream), lastbit);
+}
+
+/**
+*	Definition 5 (Output). Define the function output which takes an internal
+*	state s =< l, r, t, b > and returns the bit r 5 . We also define the function output
+*	on multiple bits input which takes a key k, a state s and an input x ∈ F n 2 as
+*	output(k, s, ǫ) = ǫ
+*	output(k, s, x 0 . . . x n ) = output(s) · output(k, s ′ , x 1 . . . x n )
+*	where s ′ = suc(k, s, x 0 ).
+**/
+void output(uint8_t* k,State s, BitstreamIn* in,  BitstreamOut* out)
+{
+	if(bitsLeft(in) == 0)
+	{
+		return;
+	}
+	//printf("bitsleft %d" , bitsLeft(in));
+	//printf(" %0d", s.r >> 2 & 1);
+	pushBit(out,(s.r >> 2) & 1);
+	//Remove first bit
+	uint8_t x0 = headBit(in);
+	State ss = successor(k,s,x0);
+	output(k,ss,in, out);
+}
+
+/**
+* Definition 6 (Initial state). Define the function init which takes as input a
+* key k ∈ (F 82 ) 8 and outputs the initial cipher state s =< l, r, t, b >
+**/
+
+State init(uint8_t* k)
+{
+	State s = {
+	((k[0] ^ 0x4c) + 0xEC) & 0xFF,// l
+	((k[0] ^ 0x4c) + 0x21) & 0xFF,// r
+	0x4c, // b
+	0xE012 // t
+	};
+	return s;
+}
+void MAC(uint8_t* k, BitstreamIn input, BitstreamOut out)
+{
+	uint8_t zeroes_32[] = {0,0,0,0};
+	BitstreamIn input_32_zeroes = {zeroes_32,sizeof(zeroes_32)*8,0};
+	State initState = suc(k,init(k),&input);
+	output(k,initState,&input_32_zeroes,&out);
+
+}
+
+
+void printarr(char * name, uint8_t* arr, int len)
+{
+	int i ;
+	printf("uint8_t %s[] = {", name);
+	for(i =0 ;  i< len ; i++)
+	{
+		printf("0x%02x,",*(arr+i));
+	}
+	printf("};\n");
+}
+
+int testMAC()
+{
+
+	//From the "dismantling.IClass" paper:
+	uint8_t cc_nr[] = {0xFE,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0,0,0,0};
+	// But actually, that must be reversed, it's "on-the-wire" data
+	reverse_arraybytes(cc_nr,sizeof(cc_nr));
+
+	//From the paper
+	uint8_t div_key[] = {0xE0,0x33,0xCA,0x41,0x9A,0xEE,0x43,0xF9};
+	uint8_t correct_MAC[] = {0x1d,0x49,0xC9,0xDA};
+
+	BitstreamIn bitstream = {cc_nr,sizeof(cc_nr) * 8,0};
+	uint8_t dest []= {0,0,0,0,0,0,0,0};
+	BitstreamOut out = { dest, sizeof(dest)*8, 0 };
+	MAC(div_key,bitstream, out);
+	//The output MAC must also be reversed
+	reverse_arraybytes(dest, sizeof(dest));
+
+	if(false && memcmp(dest, correct_MAC,4) == 0)
+	{
+		printf("MAC calculation OK!\n");
+
+	}else
+	{
+		printf("MAC calculation failed\n");
+		printarr("Calculated_MAC", dest, 4);
+		printarr("Correct_MAC   ", correct_MAC, 4);
+		return 1;
+	}
+	return 0;
+}
+
+int calc_iclass_mac(uint8_t *cc_nr_p, int length, uint8_t *div_key_p, uint8_t *mac)
+{
+    uint8_t *cc_nr;
+    uint8_t div_key[8];
+    cc_nr=(uint8_t*)malloc(length+1);
+    memcpy(cc_nr,cc_nr_p,length);
+    memcpy(div_key,div_key_p,8);
+    
+	reverse_arraybytes(cc_nr,length);
+	BitstreamIn bitstream = {cc_nr,length * 8,0};
+	uint8_t dest []= {0,0,0,0,0,0,0,0};
+	BitstreamOut out = { dest, sizeof(dest)*8, 0 };
+	MAC(div_key,bitstream, out);
+	//The output MAC must also be reversed
+	reverse_arraybytes(dest, sizeof(dest));
+	
+	printf("Calculated_MAC\t%02x%02x%02x%02x\n", dest[0],dest[1],dest[2],dest[3]);
+	memcpy(mac,dest,4);
+	free(cc_nr);
+	return 1;
+}
\ No newline at end of file
diff --git a/client/loclass/cipher.h b/client/loclass/cipher.h
new file mode 100644
index 00000000..4af92b16
--- /dev/null
+++ b/client/loclass/cipher.h
@@ -0,0 +1,46 @@
+/*****************************************************************************
+ * This file is part of iClassCipher. It is a reconstructon of the cipher engine
+ * used in iClass, and RFID techology.
+ *
+ * The implementation is based on the work performed by
+ * Flavio D. Garcia, Gerhard de Koning Gans, Roel Verdult and
+ * Milosch Meriac in the paper "Dismantling IClass".
+ *
+ * Copyright (C) 2014 Martin Holst Swende
+ *
+ * This is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as published
+ * by the Free Software Foundation.
+ *
+ * This file is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with IClassCipher.  If not, see <http://www.gnu.org/licenses/>.
+ ****************************************************************************/
+
+#ifndef CIPHER_H
+#define CIPHER_H
+#include <stdint.h>
+
+/**
+* Definition 1 (Cipher state). A cipher state of iClass s is an element of F 40/2
+* consisting of the following four components:
+* 	1. the left register l = (l 0 . . . l 7 ) ∈ F 8/2 ;
+* 	2. the right register r = (r 0 . . . r 7 ) ∈ F 8/2 ;
+* 	3. the top register t = (t 0 . . . t 15 ) ∈ F 16/2 .
+* 	4. the bottom register b = (b 0 . . . b 7 ) ∈ F 8/2 .
+**/
+typedef struct {
+	uint8_t l;
+	uint8_t r;
+	uint8_t b;
+	uint16_t t;
+} State;
+
+void printarr(char * name, uint8_t* arr, int len);
+int calc_iclass_mac(uint8_t *cc_nr_p, int length, uint8_t *div_key_p, uint8_t *mac);
+
+#endif // CIPHER_H
diff --git a/client/loclass/cipherutils.c b/client/loclass/cipherutils.c
new file mode 100644
index 00000000..685a3815
--- /dev/null
+++ b/client/loclass/cipherutils.c
@@ -0,0 +1,195 @@
+/*****************************************************************************
+ * This file is part of iClassCipher. It is a reconstructon of the cipher engine
+ * used in iClass, and RFID techology.
+ *
+ * The implementation is based on the work performed by
+ * Flavio D. Garcia, Gerhard de Koning Gans, Roel Verdult and
+ * Milosch Meriac in the paper "Dismantling IClass".
+ *
+ * Copyright (C) 2014 Martin Holst Swende
+ *
+ * This is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as published
+ * by the Free Software Foundation.
+ *
+ * This file is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with IClassCipher.  If not, see <http://www.gnu.org/licenses/>.
+ ****************************************************************************/
+
+#include "cipherutils.h"
+#include "../util.h"
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+
+/**
+ *
+ * @brief Return and remove the first bit (x0) in the stream : <x0 x1 x2 x3 ... xn >
+ * @param stream
+ * @return
+ */
+bool headBit( BitstreamIn *stream)
+{
+	int bytepos = stream->position >> 3; // divide by 8
+	int bitpos = (stream->position++) & 7; // mask out 00000111
+	return (*(stream->buffer + bytepos) >> (7-bitpos)) & 1;
+}
+/**
+ * @brief Return and remove the last bit (xn) in the stream: <x0 x1 x2 ... xn>
+ * @param stream
+ * @return
+ */
+bool tailBit( BitstreamIn *stream)
+{
+	int bitpos = stream->numbits -1 - (stream->position++);
+
+	int bytepos= bitpos >> 3;
+	bitpos &= 7;
+	return (*(stream->buffer + bytepos) >> (7-bitpos)) & 1;
+}
+/**
+ * @brief Pushes bit onto the stream
+ * @param stream
+ * @param bit
+ */
+void pushBit( BitstreamOut* stream, bool bit)
+{
+	int bytepos = stream->position >> 3; // divide by 8
+	int bitpos = stream->position & 7;
+	*(stream->buffer+bytepos) |= (bit & 1) <<  (7 - bitpos);
+	stream->position++;
+	stream->numbits++;
+}
+
+/**
+ * @brief Pushes the lower six bits onto the stream
+ * as b0 b1 b2 b3 b4 b5 b6
+ * @param stream
+ * @param bits
+ */
+void push6bits( BitstreamOut* stream, uint8_t bits)
+{
+	pushBit(stream, bits & 0x20);
+	pushBit(stream, bits & 0x10);
+	pushBit(stream, bits & 0x08);
+	pushBit(stream, bits & 0x04);
+	pushBit(stream, bits & 0x02);
+	pushBit(stream, bits & 0x01);
+}
+
+/**
+ * @brief bitsLeft
+ * @param stream
+ * @return number of bits left in stream
+ */
+int bitsLeft( BitstreamIn *stream)
+{
+	return stream->numbits - stream->position;
+}
+/**
+ * @brief numBits
+ * @param stream
+ * @return Number of bits stored in stream
+ */
+int numBits(BitstreamOut *stream)
+{
+	return stream->numbits;
+}
+
+uint8_t reversebytes(uint8_t b) {
+	b = (b & 0xF0) >> 4 | (b & 0x0F) << 4;
+	b = (b & 0xCC) >> 2 | (b & 0x33) << 2;
+	b = (b & 0xAA) >> 1 | (b & 0x55) << 1;
+   return b;
+}
+void reverse_arraybytes(uint8_t* arr, size_t len)
+{
+	uint8_t i;
+	for( i =0; i< len ; i++)
+	{
+		arr[i] = reversebytes(arr[i]);
+	}
+}
+
+
+//-----------------------------
+// Code for testing below
+//-----------------------------
+
+
+int testBitStream()
+{
+	uint8_t input [] = {0xDE,0xAD,0xBE,0xEF,0xDE,0xAD,0xBE,0xEF};
+	uint8_t output [] = {0,0,0,0,0,0,0,0};
+	BitstreamIn in = { input, sizeof(input) * 8,0};
+	BitstreamOut out ={ output, 0,0}
+					  ;
+	while(bitsLeft(&in) > 0)
+	{
+		pushBit(&out, headBit(&in));
+		//printf("Bits left: %d\n", bitsLeft(&in));
+		//printf("Bits out: %d\n", numBits(&out));
+	}
+	if(memcmp(input, output, sizeof(input)) == 0)
+	{
+		printf("Bitstream test 1 ok\n");
+	}else
+	{
+		printf("Bitstream test 1 failed\n");
+		uint8_t i;
+		for(i = 0 ; i < sizeof(input) ; i++)
+		{
+			printf("IN %02x, OUT %02x\n", input[i], output[i]);
+		}
+		return 1;
+	}
+	return 0;
+}
+
+int testReversedBitstream()
+{
+	uint8_t input [] = {0xDE,0xAD,0xBE,0xEF,0xDE,0xAD,0xBE,0xEF};
+	uint8_t reverse [] = {0,0,0,0,0,0,0,0};
+	uint8_t output [] = {0,0,0,0,0,0,0,0};
+	BitstreamIn in = { input, sizeof(input) * 8,0};
+	BitstreamOut out ={ output, 0,0};
+	BitstreamIn reversed_in ={ reverse, sizeof(input)*8,0};
+	BitstreamOut reversed_out ={ reverse,0 ,0};
+
+	while(bitsLeft(&in) > 0)
+	{
+		pushBit(&reversed_out, tailBit(&in));
+	}
+	while(bitsLeft(&reversed_in) > 0)
+	{
+		pushBit(&out, tailBit(&reversed_in));
+	}
+	if(memcmp(input, output, sizeof(input)) == 0)
+	{
+		printf("Bitstream test 2 ok\n");
+	}else
+	{
+		printf("Bitstream test 2 failed\n");
+		uint8_t i;
+		for(i = 0 ; i < sizeof(input) ; i++)
+		{
+			printf("IN %02x, MIDDLE: %02x, OUT %02x\n", input[i],reverse[i], output[i]);
+		}
+		return 1;
+	}
+	return 0;
+}
+
+
+int testCipherUtils(void)
+{
+	int retval = 0;
+	retval |= testBitStream();
+	retval |= testReversedBitstream();
+	return retval;
+}
diff --git a/client/loclass/cipherutils.h b/client/loclass/cipherutils.h
new file mode 100644
index 00000000..84435da9
--- /dev/null
+++ b/client/loclass/cipherutils.h
@@ -0,0 +1,55 @@
+/*****************************************************************************
+ * This file is part of iClassCipher. It is a reconstructon of the cipher engine
+ * used in iClass, and RFID techology.
+ *
+ * The implementation is based on the work performed by
+ * Flavio D. Garcia, Gerhard de Koning Gans, Roel Verdult and
+ * Milosch Meriac in the paper "Dismantling IClass".
+ *
+ * Copyright (C) 2014 Martin Holst Swende
+ *
+ * This is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as published
+ * by the Free Software Foundation.
+ *
+ * This file is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with IClassCipher.  If not, see <http://www.gnu.org/licenses/>.
+ ****************************************************************************/
+
+#ifndef CIPHERUTILS_H
+#define CIPHERUTILS_H
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+
+typedef struct {
+	uint8_t * buffer;
+	uint8_t numbits;
+	uint8_t position;
+} BitstreamIn;
+
+typedef struct {
+	uint8_t * buffer;
+	uint8_t numbits;
+	uint8_t position;
+}BitstreamOut;
+
+bool headBit( BitstreamIn *stream);
+bool tailBit( BitstreamIn *stream);
+void pushBit( BitstreamOut *stream, bool bit);
+int bitsLeft( BitstreamIn *stream);
+bool xorbits_8(uint8_t val);
+bool xorbits_16(uint16_t val);
+int testCipherUtils(void);
+int testMAC();
+void push6bits( BitstreamOut* stream, uint8_t bits);
+void EncryptDES(bool key[56], bool outBlk[64], bool inBlk[64], int verbose) ;
+uint8_t reversebytes(uint8_t b);
+void reverse_arraybytes(uint8_t* arr, size_t len);
+
+#endif // CIPHERUTILS_H
diff --git a/client/loclass/des.c b/client/loclass/des.c
new file mode 100644
index 00000000..746752d7
--- /dev/null
+++ b/client/loclass/des.c
@@ -0,0 +1,1014 @@
+/*
+ *  FIPS-46-3 compliant Triple-DES implementation
+ *
+ *  Copyright (C) 2006-2014, Brainspark B.V.
+ *
+ *  This file is part of PolarSSL (http://www.polarssl.org)
+ *  Lead Maintainer: Paul Bakker <polarssl_maintainer at polarssl.org>
+ *
+ *  All rights reserved.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with this program; if not, write to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+/*
+ *  DES, on which TDES is based, was originally designed by Horst Feistel
+ *  at IBM in 1974, and was adopted as a standard by NIST (formerly NBS).
+ *
+ *  http://csrc.nist.gov/publications/fips/fips46-3/fips46-3.pdf
+ */
+
+//#include "polarssl/config.h"
+#define POLARSSL_DES_C
+
+#if defined(POLARSSL_DES_C)
+
+#include "des.h"
+
+#if defined(POLARSSL_PLATFORM_C)
+#include "polarssl/platform.h"
+#else
+#define polarssl_printf printf
+#endif
+
+#if !defined(POLARSSL_DES_ALT)
+
+/*
+ * 32-bit integer manipulation macros (big endian)
+ */
+#ifndef GET_UINT32_BE
+#define GET_UINT32_BE(n,b,i)                            \
+{                                                       \
+	(n) = ( (uint32_t) (b)[(i)    ] << 24 )             \
+		| ( (uint32_t) (b)[(i) + 1] << 16 )             \
+		| ( (uint32_t) (b)[(i) + 2] <<  8 )             \
+		| ( (uint32_t) (b)[(i) + 3]       );            \
+}
+#endif
+
+#ifndef PUT_UINT32_BE
+#define PUT_UINT32_BE(n,b,i)                            \
+{                                                       \
+	(b)[(i)    ] = (unsigned char) ( (n) >> 24 );       \
+	(b)[(i) + 1] = (unsigned char) ( (n) >> 16 );       \
+	(b)[(i) + 2] = (unsigned char) ( (n) >>  8 );       \
+	(b)[(i) + 3] = (unsigned char) ( (n)       );       \
+}
+#endif
+
+/*
+ * Expanded DES S-boxes
+ */
+static const uint32_t SB1[64] =
+{
+	0x01010400, 0x00000000, 0x00010000, 0x01010404,
+	0x01010004, 0x00010404, 0x00000004, 0x00010000,
+	0x00000400, 0x01010400, 0x01010404, 0x00000400,
+	0x01000404, 0x01010004, 0x01000000, 0x00000004,
+	0x00000404, 0x01000400, 0x01000400, 0x00010400,
+	0x00010400, 0x01010000, 0x01010000, 0x01000404,
+	0x00010004, 0x01000004, 0x01000004, 0x00010004,
+	0x00000000, 0x00000404, 0x00010404, 0x01000000,
+	0x00010000, 0x01010404, 0x00000004, 0x01010000,
+	0x01010400, 0x01000000, 0x01000000, 0x00000400,
+	0x01010004, 0x00010000, 0x00010400, 0x01000004,
+	0x00000400, 0x00000004, 0x01000404, 0x00010404,
+	0x01010404, 0x00010004, 0x01010000, 0x01000404,
+	0x01000004, 0x00000404, 0x00010404, 0x01010400,
+	0x00000404, 0x01000400, 0x01000400, 0x00000000,
+	0x00010004, 0x00010400, 0x00000000, 0x01010004
+};
+
+static const uint32_t SB2[64] =
+{
+	0x80108020, 0x80008000, 0x00008000, 0x00108020,
+	0x00100000, 0x00000020, 0x80100020, 0x80008020,
+	0x80000020, 0x80108020, 0x80108000, 0x80000000,
+	0x80008000, 0x00100000, 0x00000020, 0x80100020,
+	0x00108000, 0x00100020, 0x80008020, 0x00000000,
+	0x80000000, 0x00008000, 0x00108020, 0x80100000,
+	0x00100020, 0x80000020, 0x00000000, 0x00108000,
+	0x00008020, 0x80108000, 0x80100000, 0x00008020,
+	0x00000000, 0x00108020, 0x80100020, 0x00100000,
+	0x80008020, 0x80100000, 0x80108000, 0x00008000,
+	0x80100000, 0x80008000, 0x00000020, 0x80108020,
+	0x00108020, 0x00000020, 0x00008000, 0x80000000,
+	0x00008020, 0x80108000, 0x00100000, 0x80000020,
+	0x00100020, 0x80008020, 0x80000020, 0x00100020,
+	0x00108000, 0x00000000, 0x80008000, 0x00008020,
+	0x80000000, 0x80100020, 0x80108020, 0x00108000
+};
+
+static const uint32_t SB3[64] =
+{
+	0x00000208, 0x08020200, 0x00000000, 0x08020008,
+	0x08000200, 0x00000000, 0x00020208, 0x08000200,
+	0x00020008, 0x08000008, 0x08000008, 0x00020000,
+	0x08020208, 0x00020008, 0x08020000, 0x00000208,
+	0x08000000, 0x00000008, 0x08020200, 0x00000200,
+	0x00020200, 0x08020000, 0x08020008, 0x00020208,
+	0x08000208, 0x00020200, 0x00020000, 0x08000208,
+	0x00000008, 0x08020208, 0x00000200, 0x08000000,
+	0x08020200, 0x08000000, 0x00020008, 0x00000208,
+	0x00020000, 0x08020200, 0x08000200, 0x00000000,
+	0x00000200, 0x00020008, 0x08020208, 0x08000200,
+	0x08000008, 0x00000200, 0x00000000, 0x08020008,
+	0x08000208, 0x00020000, 0x08000000, 0x08020208,
+	0x00000008, 0x00020208, 0x00020200, 0x08000008,
+	0x08020000, 0x08000208, 0x00000208, 0x08020000,
+	0x00020208, 0x00000008, 0x08020008, 0x00020200
+};
+
+static const uint32_t SB4[64] =
+{
+	0x00802001, 0x00002081, 0x00002081, 0x00000080,
+	0x00802080, 0x00800081, 0x00800001, 0x00002001,
+	0x00000000, 0x00802000, 0x00802000, 0x00802081,
+	0x00000081, 0x00000000, 0x00800080, 0x00800001,
+	0x00000001, 0x00002000, 0x00800000, 0x00802001,
+	0x00000080, 0x00800000, 0x00002001, 0x00002080,
+	0x00800081, 0x00000001, 0x00002080, 0x00800080,
+	0x00002000, 0x00802080, 0x00802081, 0x00000081,
+	0x00800080, 0x00800001, 0x00802000, 0x00802081,
+	0x00000081, 0x00000000, 0x00000000, 0x00802000,
+	0x00002080, 0x00800080, 0x00800081, 0x00000001,
+	0x00802001, 0x00002081, 0x00002081, 0x00000080,
+	0x00802081, 0x00000081, 0x00000001, 0x00002000,
+	0x00800001, 0x00002001, 0x00802080, 0x00800081,
+	0x00002001, 0x00002080, 0x00800000, 0x00802001,
+	0x00000080, 0x00800000, 0x00002000, 0x00802080
+};
+
+static const uint32_t SB5[64] =
+{
+	0x00000100, 0x02080100, 0x02080000, 0x42000100,
+	0x00080000, 0x00000100, 0x40000000, 0x02080000,
+	0x40080100, 0x00080000, 0x02000100, 0x40080100,
+	0x42000100, 0x42080000, 0x00080100, 0x40000000,
+	0x02000000, 0x40080000, 0x40080000, 0x00000000,
+	0x40000100, 0x42080100, 0x42080100, 0x02000100,
+	0x42080000, 0x40000100, 0x00000000, 0x42000000,
+	0x02080100, 0x02000000, 0x42000000, 0x00080100,
+	0x00080000, 0x42000100, 0x00000100, 0x02000000,
+	0x40000000, 0x02080000, 0x42000100, 0x40080100,
+	0x02000100, 0x40000000, 0x42080000, 0x02080100,
+	0x40080100, 0x00000100, 0x02000000, 0x42080000,
+	0x42080100, 0x00080100, 0x42000000, 0x42080100,
+	0x02080000, 0x00000000, 0x40080000, 0x42000000,
+	0x00080100, 0x02000100, 0x40000100, 0x00080000,
+	0x00000000, 0x40080000, 0x02080100, 0x40000100
+};
+
+static const uint32_t SB6[64] =
+{
+	0x20000010, 0x20400000, 0x00004000, 0x20404010,
+	0x20400000, 0x00000010, 0x20404010, 0x00400000,
+	0x20004000, 0x00404010, 0x00400000, 0x20000010,
+	0x00400010, 0x20004000, 0x20000000, 0x00004010,
+	0x00000000, 0x00400010, 0x20004010, 0x00004000,
+	0x00404000, 0x20004010, 0x00000010, 0x20400010,
+	0x20400010, 0x00000000, 0x00404010, 0x20404000,
+	0x00004010, 0x00404000, 0x20404000, 0x20000000,
+	0x20004000, 0x00000010, 0x20400010, 0x00404000,
+	0x20404010, 0x00400000, 0x00004010, 0x20000010,
+	0x00400000, 0x20004000, 0x20000000, 0x00004010,
+	0x20000010, 0x20404010, 0x00404000, 0x20400000,
+	0x00404010, 0x20404000, 0x00000000, 0x20400010,
+	0x00000010, 0x00004000, 0x20400000, 0x00404010,
+	0x00004000, 0x00400010, 0x20004010, 0x00000000,
+	0x20404000, 0x20000000, 0x00400010, 0x20004010
+};
+
+static const uint32_t SB7[64] =
+{
+	0x00200000, 0x04200002, 0x04000802, 0x00000000,
+	0x00000800, 0x04000802, 0x00200802, 0x04200800,
+	0x04200802, 0x00200000, 0x00000000, 0x04000002,
+	0x00000002, 0x04000000, 0x04200002, 0x00000802,
+	0x04000800, 0x00200802, 0x00200002, 0x04000800,
+	0x04000002, 0x04200000, 0x04200800, 0x00200002,
+	0x04200000, 0x00000800, 0x00000802, 0x04200802,
+	0x00200800, 0x00000002, 0x04000000, 0x00200800,
+	0x04000000, 0x00200800, 0x00200000, 0x04000802,
+	0x04000802, 0x04200002, 0x04200002, 0x00000002,
+	0x00200002, 0x04000000, 0x04000800, 0x00200000,
+	0x04200800, 0x00000802, 0x00200802, 0x04200800,
+	0x00000802, 0x04000002, 0x04200802, 0x04200000,
+	0x00200800, 0x00000000, 0x00000002, 0x04200802,
+	0x00000000, 0x00200802, 0x04200000, 0x00000800,
+	0x04000002, 0x04000800, 0x00000800, 0x00200002
+};
+
+static const uint32_t SB8[64] =
+{
+	0x10001040, 0x00001000, 0x00040000, 0x10041040,
+	0x10000000, 0x10001040, 0x00000040, 0x10000000,
+	0x00040040, 0x10040000, 0x10041040, 0x00041000,
+	0x10041000, 0x00041040, 0x00001000, 0x00000040,
+	0x10040000, 0x10000040, 0x10001000, 0x00001040,
+	0x00041000, 0x00040040, 0x10040040, 0x10041000,
+	0x00001040, 0x00000000, 0x00000000, 0x10040040,
+	0x10000040, 0x10001000, 0x00041040, 0x00040000,
+	0x00041040, 0x00040000, 0x10041000, 0x00001000,
+	0x00000040, 0x10040040, 0x00001000, 0x00041040,
+	0x10001000, 0x00000040, 0x10000040, 0x10040000,
+	0x10040040, 0x10000000, 0x00040000, 0x10001040,
+	0x00000000, 0x10041040, 0x00040040, 0x10000040,
+	0x10040000, 0x10001000, 0x10001040, 0x00000000,
+	0x10041040, 0x00041000, 0x00041000, 0x00001040,
+	0x00001040, 0x00040040, 0x10000000, 0x10041000
+};
+
+/*
+ * PC1: left and right halves bit-swap
+ */
+static const uint32_t LHs[16] =
+{
+	0x00000000, 0x00000001, 0x00000100, 0x00000101,
+	0x00010000, 0x00010001, 0x00010100, 0x00010101,
+	0x01000000, 0x01000001, 0x01000100, 0x01000101,
+	0x01010000, 0x01010001, 0x01010100, 0x01010101
+};
+
+static const uint32_t RHs[16] =
+{
+	0x00000000, 0x01000000, 0x00010000, 0x01010000,
+	0x00000100, 0x01000100, 0x00010100, 0x01010100,
+	0x00000001, 0x01000001, 0x00010001, 0x01010001,
+	0x00000101, 0x01000101, 0x00010101, 0x01010101,
+};
+
+/*
+ * Initial Permutation macro
+ */
+#define DES_IP(X,Y)                                             \
+{                                                               \
+	T = ((X >>  4) ^ Y) & 0x0F0F0F0F; Y ^= T; X ^= (T <<  4);   \
+	T = ((X >> 16) ^ Y) & 0x0000FFFF; Y ^= T; X ^= (T << 16);   \
+	T = ((Y >>  2) ^ X) & 0x33333333; X ^= T; Y ^= (T <<  2);   \
+	T = ((Y >>  8) ^ X) & 0x00FF00FF; X ^= T; Y ^= (T <<  8);   \
+	Y = ((Y << 1) | (Y >> 31)) & 0xFFFFFFFF;                    \
+	T = (X ^ Y) & 0xAAAAAAAA; Y ^= T; X ^= T;                   \
+	X = ((X << 1) | (X >> 31)) & 0xFFFFFFFF;                    \
+}
+
+/*
+ * Final Permutation macro
+ */
+#define DES_FP(X,Y)                                             \
+{                                                               \
+	X = ((X << 31) | (X >> 1)) & 0xFFFFFFFF;                    \
+	T = (X ^ Y) & 0xAAAAAAAA; X ^= T; Y ^= T;                   \
+	Y = ((Y << 31) | (Y >> 1)) & 0xFFFFFFFF;                    \
+	T = ((Y >>  8) ^ X) & 0x00FF00FF; X ^= T; Y ^= (T <<  8);   \
+	T = ((Y >>  2) ^ X) & 0x33333333; X ^= T; Y ^= (T <<  2);   \
+	T = ((X >> 16) ^ Y) & 0x0000FFFF; Y ^= T; X ^= (T << 16);   \
+	T = ((X >>  4) ^ Y) & 0x0F0F0F0F; Y ^= T; X ^= (T <<  4);   \
+}
+
+/*
+ * DES round macro
+ */
+#define DES_ROUND(X,Y)                          \
+{                                               \
+	T = *SK++ ^ X;                              \
+	Y ^= SB8[ (T      ) & 0x3F ] ^              \
+		 SB6[ (T >>  8) & 0x3F ] ^              \
+		 SB4[ (T >> 16) & 0x3F ] ^              \
+		 SB2[ (T >> 24) & 0x3F ];               \
+												\
+	T = *SK++ ^ ((X << 28) | (X >> 4));         \
+	Y ^= SB7[ (T      ) & 0x3F ] ^              \
+		 SB5[ (T >>  8) & 0x3F ] ^              \
+		 SB3[ (T >> 16) & 0x3F ] ^              \
+		 SB1[ (T >> 24) & 0x3F ];               \
+}
+
+#define SWAP(a,b) { uint32_t t = a; a = b; b = t; t = 0; }
+
+static const unsigned char odd_parity_table[128] = { 1,  2,  4,  7,  8,
+		11, 13, 14, 16, 19, 21, 22, 25, 26, 28, 31, 32, 35, 37, 38, 41, 42, 44,
+		47, 49, 50, 52, 55, 56, 59, 61, 62, 64, 67, 69, 70, 73, 74, 76, 79, 81,
+		82, 84, 87, 88, 91, 93, 94, 97, 98, 100, 103, 104, 107, 109, 110, 112,
+		115, 117, 118, 121, 122, 124, 127, 128, 131, 133, 134, 137, 138, 140,
+		143, 145, 146, 148, 151, 152, 155, 157, 158, 161, 162, 164, 167, 168,
+		171, 173, 174, 176, 179, 181, 182, 185, 186, 188, 191, 193, 194, 196,
+		199, 200, 203, 205, 206, 208, 211, 213, 214, 217, 218, 220, 223, 224,
+		227, 229, 230, 233, 234, 236, 239, 241, 242, 244, 247, 248, 251, 253,
+		254 };
+
+void des_key_set_parity( unsigned char key[DES_KEY_SIZE] )
+{
+	int i;
+
+	for( i = 0; i < DES_KEY_SIZE; i++ )
+		key[i] = odd_parity_table[key[i] / 2];
+}
+
+/*
+ * Check the given key's parity, returns 1 on failure, 0 on SUCCESS
+ */
+int des_key_check_key_parity( const unsigned char key[DES_KEY_SIZE] )
+{
+	int i;
+
+	for( i = 0; i < DES_KEY_SIZE; i++ )
+		if ( key[i] != odd_parity_table[key[i] / 2] )
+			return( 1 );
+
+	return( 0 );
+}
+
+/*
+ * Table of weak and semi-weak keys
+ *
+ * Source: http://en.wikipedia.org/wiki/Weak_key
+ *
+ * Weak:
+ * Alternating ones + zeros (0x0101010101010101)
+ * Alternating 'F' + 'E' (0xFEFEFEFEFEFEFEFE)
+ * '0xE0E0E0E0F1F1F1F1'
+ * '0x1F1F1F1F0E0E0E0E'
+ *
+ * Semi-weak:
+ * 0x011F011F010E010E and 0x1F011F010E010E01
+ * 0x01E001E001F101F1 and 0xE001E001F101F101
+ * 0x01FE01FE01FE01FE and 0xFE01FE01FE01FE01
+ * 0x1FE01FE00EF10EF1 and 0xE01FE01FF10EF10E
+ * 0x1FFE1FFE0EFE0EFE and 0xFE1FFE1FFE0EFE0E
+ * 0xE0FEE0FEF1FEF1FE and 0xFEE0FEE0FEF1FEF1
+ *
+ */
+
+#define WEAK_KEY_COUNT 16
+
+static const unsigned char weak_key_table[WEAK_KEY_COUNT][DES_KEY_SIZE] =
+{
+	{ 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+	{ 0xFE, 0xFE, 0xFE, 0xFE, 0xFE, 0xFE, 0xFE, 0xFE },
+	{ 0x1F, 0x1F, 0x1F, 0x1F, 0x0E, 0x0E, 0x0E, 0x0E },
+	{ 0xE0, 0xE0, 0xE0, 0xE0, 0xF1, 0xF1, 0xF1, 0xF1 },
+
+	{ 0x01, 0x1F, 0x01, 0x1F, 0x01, 0x0E, 0x01, 0x0E },
+	{ 0x1F, 0x01, 0x1F, 0x01, 0x0E, 0x01, 0x0E, 0x01 },
+	{ 0x01, 0xE0, 0x01, 0xE0, 0x01, 0xF1, 0x01, 0xF1 },
+	{ 0xE0, 0x01, 0xE0, 0x01, 0xF1, 0x01, 0xF1, 0x01 },
+	{ 0x01, 0xFE, 0x01, 0xFE, 0x01, 0xFE, 0x01, 0xFE },
+	{ 0xFE, 0x01, 0xFE, 0x01, 0xFE, 0x01, 0xFE, 0x01 },
+	{ 0x1F, 0xE0, 0x1F, 0xE0, 0x0E, 0xF1, 0x0E, 0xF1 },
+	{ 0xE0, 0x1F, 0xE0, 0x1F, 0xF1, 0x0E, 0xF1, 0x0E },
+	{ 0x1F, 0xFE, 0x1F, 0xFE, 0x0E, 0xFE, 0x0E, 0xFE },
+	{ 0xFE, 0x1F, 0xFE, 0x1F, 0xFE, 0x0E, 0xFE, 0x0E },
+	{ 0xE0, 0xFE, 0xE0, 0xFE, 0xF1, 0xFE, 0xF1, 0xFE },
+	{ 0xFE, 0xE0, 0xFE, 0xE0, 0xFE, 0xF1, 0xFE, 0xF1 }
+};
+
+int des_key_check_weak( const unsigned char key[DES_KEY_SIZE] )
+{
+	int i;
+
+	for( i = 0; i < WEAK_KEY_COUNT; i++ )
+		if( memcmp( weak_key_table[i], key, DES_KEY_SIZE) == 0)
+			return( 1 );
+
+	return( 0 );
+}
+
+static void des_setkey( uint32_t SK[32], const unsigned char key[DES_KEY_SIZE] )
+{
+	int i;
+	uint32_t X, Y, T;
+
+	GET_UINT32_BE( X, key, 0 );
+	GET_UINT32_BE( Y, key, 4 );
+
+	/*
+	 * Permuted Choice 1
+	 */
+	T =  ((Y >>  4) ^ X) & 0x0F0F0F0F;  X ^= T; Y ^= (T <<  4);
+	T =  ((Y      ) ^ X) & 0x10101010;  X ^= T; Y ^= (T      );
+
+	X =   (LHs[ (X      ) & 0xF] << 3) | (LHs[ (X >>  8) & 0xF ] << 2)
+		| (LHs[ (X >> 16) & 0xF] << 1) | (LHs[ (X >> 24) & 0xF ]     )
+		| (LHs[ (X >>  5) & 0xF] << 7) | (LHs[ (X >> 13) & 0xF ] << 6)
+		| (LHs[ (X >> 21) & 0xF] << 5) | (LHs[ (X >> 29) & 0xF ] << 4);
+
+	Y =   (RHs[ (Y >>  1) & 0xF] << 3) | (RHs[ (Y >>  9) & 0xF ] << 2)
+		| (RHs[ (Y >> 17) & 0xF] << 1) | (RHs[ (Y >> 25) & 0xF ]     )
+		| (RHs[ (Y >>  4) & 0xF] << 7) | (RHs[ (Y >> 12) & 0xF ] << 6)
+		| (RHs[ (Y >> 20) & 0xF] << 5) | (RHs[ (Y >> 28) & 0xF ] << 4);
+
+	X &= 0x0FFFFFFF;
+	Y &= 0x0FFFFFFF;
+
+	/*
+	 * calculate subkeys
+	 */
+	for( i = 0; i < 16; i++ )
+	{
+		if( i < 2 || i == 8 || i == 15 )
+		{
+			X = ((X <<  1) | (X >> 27)) & 0x0FFFFFFF;
+			Y = ((Y <<  1) | (Y >> 27)) & 0x0FFFFFFF;
+		}
+		else
+		{
+			X = ((X <<  2) | (X >> 26)) & 0x0FFFFFFF;
+			Y = ((Y <<  2) | (Y >> 26)) & 0x0FFFFFFF;
+		}
+
+		*SK++ =   ((X <<  4) & 0x24000000) | ((X << 28) & 0x10000000)
+				| ((X << 14) & 0x08000000) | ((X << 18) & 0x02080000)
+				| ((X <<  6) & 0x01000000) | ((X <<  9) & 0x00200000)
+				| ((X >>  1) & 0x00100000) | ((X << 10) & 0x00040000)
+				| ((X <<  2) & 0x00020000) | ((X >> 10) & 0x00010000)
+				| ((Y >> 13) & 0x00002000) | ((Y >>  4) & 0x00001000)
+				| ((Y <<  6) & 0x00000800) | ((Y >>  1) & 0x00000400)
+				| ((Y >> 14) & 0x00000200) | ((Y      ) & 0x00000100)
+				| ((Y >>  5) & 0x00000020) | ((Y >> 10) & 0x00000010)
+				| ((Y >>  3) & 0x00000008) | ((Y >> 18) & 0x00000004)
+				| ((Y >> 26) & 0x00000002) | ((Y >> 24) & 0x00000001);
+
+		*SK++ =   ((X << 15) & 0x20000000) | ((X << 17) & 0x10000000)
+				| ((X << 10) & 0x08000000) | ((X << 22) & 0x04000000)
+				| ((X >>  2) & 0x02000000) | ((X <<  1) & 0x01000000)
+				| ((X << 16) & 0x00200000) | ((X << 11) & 0x00100000)
+				| ((X <<  3) & 0x00080000) | ((X >>  6) & 0x00040000)
+				| ((X << 15) & 0x00020000) | ((X >>  4) & 0x00010000)
+				| ((Y >>  2) & 0x00002000) | ((Y <<  8) & 0x00001000)
+				| ((Y >> 14) & 0x00000808) | ((Y >>  9) & 0x00000400)
+				| ((Y      ) & 0x00000200) | ((Y <<  7) & 0x00000100)
+				| ((Y >>  7) & 0x00000020) | ((Y >>  3) & 0x00000011)
+				| ((Y <<  2) & 0x00000004) | ((Y >> 21) & 0x00000002);
+	}
+}
+
+/*
+ * DES key schedule (56-bit, encryption)
+ */
+int des_setkey_enc( des_context *ctx, const unsigned char key[DES_KEY_SIZE] )
+{
+	des_setkey( ctx->sk, key );
+
+	return( 0 );
+}
+
+/*
+ * DES key schedule (56-bit, decryption)
+ */
+int des_setkey_dec( des_context *ctx, const unsigned char key[DES_KEY_SIZE] )
+{
+	int i;
+
+	des_setkey( ctx->sk, key );
+
+	for( i = 0; i < 16; i += 2 )
+	{
+		SWAP( ctx->sk[i    ], ctx->sk[30 - i] );
+		SWAP( ctx->sk[i + 1], ctx->sk[31 - i] );
+	}
+
+	return( 0 );
+}
+
+static void des3_set2key( uint32_t esk[96],
+						  uint32_t dsk[96],
+						  const unsigned char key[DES_KEY_SIZE*2] )
+{
+	int i;
+
+	des_setkey( esk, key );
+	des_setkey( dsk + 32, key + 8 );
+
+	for( i = 0; i < 32; i += 2 )
+	{
+		dsk[i     ] = esk[30 - i];
+		dsk[i +  1] = esk[31 - i];
+
+		esk[i + 32] = dsk[62 - i];
+		esk[i + 33] = dsk[63 - i];
+
+		esk[i + 64] = esk[i    ];
+		esk[i + 65] = esk[i + 1];
+
+		dsk[i + 64] = dsk[i    ];
+		dsk[i + 65] = dsk[i + 1];
+	}
+}
+
+/*
+ * Triple-DES key schedule (112-bit, encryption)
+ */
+int des3_set2key_enc( des3_context *ctx, const unsigned char key[DES_KEY_SIZE * 2] )
+{
+	uint32_t sk[96];
+
+	des3_set2key( ctx->sk, sk, key );
+	memset( sk,  0, sizeof( sk ) );
+
+	return( 0 );
+}
+
+/*
+ * Triple-DES key schedule (112-bit, decryption)
+ */
+int des3_set2key_dec( des3_context *ctx, const unsigned char key[DES_KEY_SIZE * 2] )
+{
+	uint32_t sk[96];
+
+	des3_set2key( sk, ctx->sk, key );
+	memset( sk,  0, sizeof( sk ) );
+
+	return( 0 );
+}
+
+static void des3_set3key( uint32_t esk[96],
+						  uint32_t dsk[96],
+						  const unsigned char key[24] )
+{
+	int i;
+
+	des_setkey( esk, key );
+	des_setkey( dsk + 32, key +  8 );
+	des_setkey( esk + 64, key + 16 );
+
+	for( i = 0; i < 32; i += 2 )
+	{
+		dsk[i     ] = esk[94 - i];
+		dsk[i +  1] = esk[95 - i];
+
+		esk[i + 32] = dsk[62 - i];
+		esk[i + 33] = dsk[63 - i];
+
+		dsk[i + 64] = esk[30 - i];
+		dsk[i + 65] = esk[31 - i];
+	}
+}
+
+/*
+ * Triple-DES key schedule (168-bit, encryption)
+ */
+int des3_set3key_enc( des3_context *ctx, const unsigned char key[DES_KEY_SIZE * 3] )
+{
+	uint32_t sk[96];
+
+	des3_set3key( ctx->sk, sk, key );
+	memset( sk, 0, sizeof( sk ) );
+
+	return( 0 );
+}
+
+/*
+ * Triple-DES key schedule (168-bit, decryption)
+ */
+int des3_set3key_dec( des3_context *ctx, const unsigned char key[DES_KEY_SIZE * 3] )
+{
+	uint32_t sk[96];
+
+	des3_set3key( sk, ctx->sk, key );
+	memset( sk, 0, sizeof( sk ) );
+
+	return( 0 );
+}
+
+/*
+ * DES-ECB block encryption/decryption
+ */
+int des_crypt_ecb( des_context *ctx,
+					const unsigned char input[8],
+					unsigned char output[8] )
+{
+	int i;
+	uint32_t X, Y, T, *SK;
+
+	SK = ctx->sk;
+
+	GET_UINT32_BE( X, input, 0 );
+	GET_UINT32_BE( Y, input, 4 );
+
+	DES_IP( X, Y );
+
+	for( i = 0; i < 8; i++ )
+	{
+		DES_ROUND( Y, X );
+		DES_ROUND( X, Y );
+	}
+
+	DES_FP( Y, X );
+
+	PUT_UINT32_BE( Y, output, 0 );
+	PUT_UINT32_BE( X, output, 4 );
+
+	return( 0 );
+}
+
+#if defined(POLARSSL_CIPHER_MODE_CBC)
+/*
+ * DES-CBC buffer encryption/decryption
+ */
+int des_crypt_cbc( des_context *ctx,
+					int mode,
+					size_t length,
+					unsigned char iv[8],
+					const unsigned char *input,
+					unsigned char *output )
+{
+	int i;
+	unsigned char temp[8];
+
+	if( length % 8 )
+		return( POLARSSL_ERR_DES_INVALID_INPUT_LENGTH );
+
+	if( mode == DES_ENCRYPT )
+	{
+		while( length > 0 )
+		{
+			for( i = 0; i < 8; i++ )
+				output[i] = (unsigned char)( input[i] ^ iv[i] );
+
+			des_crypt_ecb( ctx, output, output );
+			memcpy( iv, output, 8 );
+
+			input  += 8;
+			output += 8;
+			length -= 8;
+		}
+	}
+	else /* DES_DECRYPT */
+	{
+		while( length > 0 )
+		{
+			memcpy( temp, input, 8 );
+			des_crypt_ecb( ctx, input, output );
+
+			for( i = 0; i < 8; i++ )
+				output[i] = (unsigned char)( output[i] ^ iv[i] );
+
+			memcpy( iv, temp, 8 );
+
+			input  += 8;
+			output += 8;
+			length -= 8;
+		}
+	}
+
+	return( 0 );
+}
+#endif /* POLARSSL_CIPHER_MODE_CBC */
+
+/*
+ * 3DES-ECB block encryption/decryption
+ */
+int des3_crypt_ecb( des3_context *ctx,
+					 const unsigned char input[8],
+					 unsigned char output[8] )
+{
+	int i;
+	uint32_t X, Y, T, *SK;
+
+	SK = ctx->sk;
+
+	GET_UINT32_BE( X, input, 0 );
+	GET_UINT32_BE( Y, input, 4 );
+
+	DES_IP( X, Y );
+
+	for( i = 0; i < 8; i++ )
+	{
+		DES_ROUND( Y, X );
+		DES_ROUND( X, Y );
+	}
+
+	for( i = 0; i < 8; i++ )
+	{
+		DES_ROUND( X, Y );
+		DES_ROUND( Y, X );
+	}
+
+	for( i = 0; i < 8; i++ )
+	{
+		DES_ROUND( Y, X );
+		DES_ROUND( X, Y );
+	}
+
+	DES_FP( Y, X );
+
+	PUT_UINT32_BE( Y, output, 0 );
+	PUT_UINT32_BE( X, output, 4 );
+
+	return( 0 );
+}
+
+#if defined(POLARSSL_CIPHER_MODE_CBC)
+/*
+ * 3DES-CBC buffer encryption/decryption
+ */
+int des3_crypt_cbc( des3_context *ctx,
+					 int mode,
+					 size_t length,
+					 unsigned char iv[8],
+					 const unsigned char *input,
+					 unsigned char *output )
+{
+	int i;
+	unsigned char temp[8];
+
+	if( length % 8 )
+		return( POLARSSL_ERR_DES_INVALID_INPUT_LENGTH );
+
+	if( mode == DES_ENCRYPT )
+	{
+		while( length > 0 )
+		{
+			for( i = 0; i < 8; i++ )
+				output[i] = (unsigned char)( input[i] ^ iv[i] );
+
+			des3_crypt_ecb( ctx, output, output );
+			memcpy( iv, output, 8 );
+
+			input  += 8;
+			output += 8;
+			length -= 8;
+		}
+	}
+	else /* DES_DECRYPT */
+	{
+		while( length > 0 )
+		{
+			memcpy( temp, input, 8 );
+			des3_crypt_ecb( ctx, input, output );
+
+			for( i = 0; i < 8; i++ )
+				output[i] = (unsigned char)( output[i] ^ iv[i] );
+
+			memcpy( iv, temp, 8 );
+
+			input  += 8;
+			output += 8;
+			length -= 8;
+		}
+	}
+
+	return( 0 );
+}
+#endif /* POLARSSL_CIPHER_MODE_CBC */
+
+#endif /* !POLARSSL_DES_ALT */
+
+#if defined(POLARSSL_SELF_TEST)
+
+#include <stdio.h>
+
+/*
+ * DES and 3DES test vectors from:
+ *
+ * http://csrc.nist.gov/groups/STM/cavp/documents/des/tripledes-vectors.zip
+ */
+static const unsigned char des3_test_keys[24] =
+{
+	0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF,
+	0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF, 0x01,
+	0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF, 0x01, 0x23
+};
+
+static const unsigned char des3_test_buf[8] =
+{
+	0x4E, 0x6F, 0x77, 0x20, 0x69, 0x73, 0x20, 0x74
+};
+
+static const unsigned char des3_test_ecb_dec[3][8] =
+{
+	{ 0xCD, 0xD6, 0x4F, 0x2F, 0x94, 0x27, 0xC1, 0x5D },
+	{ 0x69, 0x96, 0xC8, 0xFA, 0x47, 0xA2, 0xAB, 0xEB },
+	{ 0x83, 0x25, 0x39, 0x76, 0x44, 0x09, 0x1A, 0x0A }
+};
+
+static const unsigned char des3_test_ecb_enc[3][8] =
+{
+	{ 0x6A, 0x2A, 0x19, 0xF4, 0x1E, 0xCA, 0x85, 0x4B },
+	{ 0x03, 0xE6, 0x9F, 0x5B, 0xFA, 0x58, 0xEB, 0x42 },
+	{ 0xDD, 0x17, 0xE8, 0xB8, 0xB4, 0x37, 0xD2, 0x32 }
+};
+
+#if defined(POLARSSL_CIPHER_MODE_CBC)
+static const unsigned char des3_test_iv[8] =
+{
+	0x12, 0x34, 0x56, 0x78, 0x90, 0xAB, 0xCD, 0xEF,
+};
+
+static const unsigned char des3_test_cbc_dec[3][8] =
+{
+	{ 0x12, 0x9F, 0x40, 0xB9, 0xD2, 0x00, 0x56, 0xB3 },
+	{ 0x47, 0x0E, 0xFC, 0x9A, 0x6B, 0x8E, 0xE3, 0x93 },
+	{ 0xC5, 0xCE, 0xCF, 0x63, 0xEC, 0xEC, 0x51, 0x4C }
+};
+
+static const unsigned char des3_test_cbc_enc[3][8] =
+{
+	{ 0x54, 0xF1, 0x5A, 0xF6, 0xEB, 0xE3, 0xA4, 0xB4 },
+	{ 0x35, 0x76, 0x11, 0x56, 0x5F, 0xA1, 0x8E, 0x4D },
+	{ 0xCB, 0x19, 0x1F, 0x85, 0xD1, 0xED, 0x84, 0x39 }
+};
+#endif /* POLARSSL_CIPHER_MODE_CBC */
+
+/*
+ * Checkup routine
+ */
+int des_self_test( int verbose )
+{
+	int i, j, u, v;
+	des_context ctx;
+	des3_context ctx3;
+	unsigned char key[24];
+	unsigned char buf[8];
+#if defined(POLARSSL_CIPHER_MODE_CBC)
+	unsigned char prv[8];
+	unsigned char iv[8];
+#endif
+
+	memset( key, 0, 24 );
+
+	/*
+	 * ECB mode
+	 */
+	for( i = 0; i < 6; i++ )
+	{
+		u = i >> 1;
+		v = i  & 1;
+
+		if( verbose != 0 )
+			polarssl_printf( "  DES%c-ECB-%3d (%s): ",
+							 ( u == 0 ) ? ' ' : '3', 56 + u * 56,
+							 ( v == DES_DECRYPT ) ? "dec" : "enc" );
+
+		memcpy( buf, des3_test_buf, 8 );
+
+		switch( i )
+		{
+		case 0:
+			des_setkey_dec( &ctx, des3_test_keys );
+			break;
+
+		case 1:
+			des_setkey_enc( &ctx, des3_test_keys );
+			break;
+
+		case 2:
+			des3_set2key_dec( &ctx3, des3_test_keys );
+			break;
+
+		case 3:
+			des3_set2key_enc( &ctx3, des3_test_keys );
+			break;
+
+		case 4:
+			des3_set3key_dec( &ctx3, des3_test_keys );
+			break;
+
+		case 5:
+			des3_set3key_enc( &ctx3, des3_test_keys );
+			break;
+
+		default:
+			return( 1 );
+		}
+
+		for( j = 0; j < 10000; j++ )
+		{
+			if( u == 0 )
+				des_crypt_ecb( &ctx, buf, buf );
+			else
+				des3_crypt_ecb( &ctx3, buf, buf );
+		}
+
+		if( ( v == DES_DECRYPT &&
+				memcmp( buf, des3_test_ecb_dec[u], 8 ) != 0 ) ||
+			( v != DES_DECRYPT &&
+				memcmp( buf, des3_test_ecb_enc[u], 8 ) != 0 ) )
+		{
+			if( verbose != 0 )
+				polarssl_printf( "failed\n" );
+
+			return( 1 );
+		}
+
+		if( verbose != 0 )
+			polarssl_printf( "passed\n" );
+	}
+
+	if( verbose != 0 )
+		polarssl_printf( "\n" );
+
+#if defined(POLARSSL_CIPHER_MODE_CBC)
+	/*
+	 * CBC mode
+	 */
+	for( i = 0; i < 6; i++ )
+	{
+		u = i >> 1;
+		v = i  & 1;
+
+		if( verbose != 0 )
+			polarssl_printf( "  DES%c-CBC-%3d (%s): ",
+							 ( u == 0 ) ? ' ' : '3', 56 + u * 56,
+							 ( v == DES_DECRYPT ) ? "dec" : "enc" );
+
+		memcpy( iv,  des3_test_iv,  8 );
+		memcpy( prv, des3_test_iv,  8 );
+		memcpy( buf, des3_test_buf, 8 );
+
+		switch( i )
+		{
+		case 0:
+			des_setkey_dec( &ctx, des3_test_keys );
+			break;
+
+		case 1:
+			des_setkey_enc( &ctx, des3_test_keys );
+			break;
+
+		case 2:
+			des3_set2key_dec( &ctx3, des3_test_keys );
+			break;
+
+		case 3:
+			des3_set2key_enc( &ctx3, des3_test_keys );
+			break;
+
+		case 4:
+			des3_set3key_dec( &ctx3, des3_test_keys );
+			break;
+
+		case 5:
+			des3_set3key_enc( &ctx3, des3_test_keys );
+			break;
+
+		default:
+			return( 1 );
+		}
+
+		if( v == DES_DECRYPT )
+		{
+			for( j = 0; j < 10000; j++ )
+			{
+				if( u == 0 )
+					des_crypt_cbc( &ctx, v, 8, iv, buf, buf );
+				else
+					des3_crypt_cbc( &ctx3, v, 8, iv, buf, buf );
+			}
+		}
+		else
+		{
+			for( j = 0; j < 10000; j++ )
+			{
+				unsigned char tmp[8];
+
+				if( u == 0 )
+					des_crypt_cbc( &ctx, v, 8, iv, buf, buf );
+				else
+					des3_crypt_cbc( &ctx3, v, 8, iv, buf, buf );
+
+				memcpy( tmp, prv, 8 );
+				memcpy( prv, buf, 8 );
+				memcpy( buf, tmp, 8 );
+			}
+
+			memcpy( buf, prv, 8 );
+		}
+
+		if( ( v == DES_DECRYPT &&
+				memcmp( buf, des3_test_cbc_dec[u], 8 ) != 0 ) ||
+			( v != DES_DECRYPT &&
+				memcmp( buf, des3_test_cbc_enc[u], 8 ) != 0 ) )
+		{
+			if( verbose != 0 )
+				polarssl_printf( "failed\n" );
+
+			return( 1 );
+		}
+
+		if( verbose != 0 )
+			polarssl_printf( "passed\n" );
+	}
+#endif /* POLARSSL_CIPHER_MODE_CBC */
+
+	if( verbose != 0 )
+		polarssl_printf( "\n" );
+
+	return( 0 );
+}
+
+#endif
+
+#endif
diff --git a/client/loclass/des.h b/client/loclass/des.h
new file mode 100644
index 00000000..907d56b1
--- /dev/null
+++ b/client/loclass/des.h
@@ -0,0 +1,256 @@
+/**
+ * \file des.h
+ *
+ * \brief DES block cipher
+ *
+ *  Copyright (C) 2006-2013, Brainspark B.V.
+ *
+ *  This file is part of PolarSSL (http://www.polarssl.org)
+ *  Lead Maintainer: Paul Bakker <polarssl_maintainer at polarssl.org>
+ *
+ *  All rights reserved.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with this program; if not, write to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+#ifndef POLARSSL_DES_H
+#define POLARSSL_DES_H
+
+//#include "config.h"
+
+#include <string.h>
+
+#if defined(_MSC_VER) && !defined(EFIX64) && !defined(EFI32)
+#include <basetsd.h>
+typedef UINT32 uint32_t;
+#else
+#include <inttypes.h>
+#endif
+
+#define DES_ENCRYPT     1
+#define DES_DECRYPT     0
+
+#define POLARSSL_ERR_DES_INVALID_INPUT_LENGTH              -0x0032  /**< The data input has an invalid length. */
+
+#define DES_KEY_SIZE    8
+
+#if !defined(POLARSSL_DES_ALT)
+// Regular implementation
+//
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * \brief          DES context structure
+ */
+typedef struct
+{
+	int mode;                   /*!<  encrypt/decrypt   */
+	uint32_t sk[32];            /*!<  DES subkeys       */
+}
+des_context;
+
+/**
+ * \brief          Triple-DES context structure
+ */
+typedef struct
+{
+	int mode;                   /*!<  encrypt/decrypt   */
+	uint32_t sk[96];            /*!<  3DES subkeys      */
+}
+des3_context;
+
+/**
+ * \brief          Set key parity on the given key to odd.
+ *
+ *                 DES keys are 56 bits long, but each byte is padded with
+ *                 a parity bit to allow verification.
+ *
+ * \param key      8-byte secret key
+ */
+void des_key_set_parity( unsigned char key[DES_KEY_SIZE] );
+
+/**
+ * \brief          Check that key parity on the given key is odd.
+ *
+ *                 DES keys are 56 bits long, but each byte is padded with
+ *                 a parity bit to allow verification.
+ *
+ * \param key      8-byte secret key
+ *
+ * \return         0 is parity was ok, 1 if parity was not correct.
+ */
+int des_key_check_key_parity( const unsigned char key[DES_KEY_SIZE] );
+
+/**
+ * \brief          Check that key is not a weak or semi-weak DES key
+ *
+ * \param key      8-byte secret key
+ *
+ * \return         0 if no weak key was found, 1 if a weak key was identified.
+ */
+int des_key_check_weak( const unsigned char key[DES_KEY_SIZE] );
+
+/**
+ * \brief          DES key schedule (56-bit, encryption)
+ *
+ * \param ctx      DES context to be initialized
+ * \param key      8-byte secret key
+ *
+ * \return         0
+ */
+int des_setkey_enc( des_context *ctx, const unsigned char key[DES_KEY_SIZE] );
+
+/**
+ * \brief          DES key schedule (56-bit, decryption)
+ *
+ * \param ctx      DES context to be initialized
+ * \param key      8-byte secret key
+ *
+ * \return         0
+ */
+int des_setkey_dec( des_context *ctx, const unsigned char key[DES_KEY_SIZE] );
+
+/**
+ * \brief          Triple-DES key schedule (112-bit, encryption)
+ *
+ * \param ctx      3DES context to be initialized
+ * \param key      16-byte secret key
+ *
+ * \return         0
+ */
+int des3_set2key_enc( des3_context *ctx, const unsigned char key[DES_KEY_SIZE * 2] );
+
+/**
+ * \brief          Triple-DES key schedule (112-bit, decryption)
+ *
+ * \param ctx      3DES context to be initialized
+ * \param key      16-byte secret key
+ *
+ * \return         0
+ */
+int des3_set2key_dec( des3_context *ctx, const unsigned char key[DES_KEY_SIZE * 2] );
+
+/**
+ * \brief          Triple-DES key schedule (168-bit, encryption)
+ *
+ * \param ctx      3DES context to be initialized
+ * \param key      24-byte secret key
+ *
+ * \return         0
+ */
+int des3_set3key_enc( des3_context *ctx, const unsigned char key[DES_KEY_SIZE * 3] );
+
+/**
+ * \brief          Triple-DES key schedule (168-bit, decryption)
+ *
+ * \param ctx      3DES context to be initialized
+ * \param key      24-byte secret key
+ *
+ * \return         0
+ */
+int des3_set3key_dec( des3_context *ctx, const unsigned char key[DES_KEY_SIZE * 3] );
+
+/**
+ * \brief          DES-ECB block encryption/decryption
+ *
+ * \param ctx      DES context
+ * \param input    64-bit input block
+ * \param output   64-bit output block
+ *
+ * \return         0 if successful
+ */
+int des_crypt_ecb( des_context *ctx,
+					const unsigned char input[8],
+					unsigned char output[8] );
+
+#if defined(POLARSSL_CIPHER_MODE_CBC)
+/**
+ * \brief          DES-CBC buffer encryption/decryption
+ *
+ * \param ctx      DES context
+ * \param mode     DES_ENCRYPT or DES_DECRYPT
+ * \param length   length of the input data
+ * \param iv       initialization vector (updated after use)
+ * \param input    buffer holding the input data
+ * \param output   buffer holding the output data
+ */
+int des_crypt_cbc( des_context *ctx,
+					int mode,
+					size_t length,
+					unsigned char iv[8],
+					const unsigned char *input,
+					unsigned char *output );
+#endif /* POLARSSL_CIPHER_MODE_CBC */
+
+/**
+ * \brief          3DES-ECB block encryption/decryption
+ *
+ * \param ctx      3DES context
+ * \param input    64-bit input block
+ * \param output   64-bit output block
+ *
+ * \return         0 if successful
+ */
+int des3_crypt_ecb( des3_context *ctx,
+					 const unsigned char input[8],
+					 unsigned char output[8] );
+
+#if defined(POLARSSL_CIPHER_MODE_CBC)
+/**
+ * \brief          3DES-CBC buffer encryption/decryption
+ *
+ * \param ctx      3DES context
+ * \param mode     DES_ENCRYPT or DES_DECRYPT
+ * \param length   length of the input data
+ * \param iv       initialization vector (updated after use)
+ * \param input    buffer holding the input data
+ * \param output   buffer holding the output data
+ *
+ * \return         0 if successful, or POLARSSL_ERR_DES_INVALID_INPUT_LENGTH
+ */
+int des3_crypt_cbc( des3_context *ctx,
+					 int mode,
+					 size_t length,
+					 unsigned char iv[8],
+					 const unsigned char *input,
+					 unsigned char *output );
+#endif /* POLARSSL_CIPHER_MODE_CBC */
+
+#ifdef __cplusplus
+}
+#endif
+
+#else  /* POLARSSL_DES_ALT */
+#include "des_alt.h"
+#endif /* POLARSSL_DES_ALT */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * \brief          Checkup routine
+ *
+ * \return         0 if successful, or 1 if the test failed
+ */
+int des_self_test( int verbose );
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* des.h */
diff --git a/client/loclass/ikeys.c b/client/loclass/ikeys.c
new file mode 100644
index 00000000..18571b0d
--- /dev/null
+++ b/client/loclass/ikeys.c
@@ -0,0 +1,469 @@
+/*****************************************************************************
+ * This file is part of iClassCipher. It is a reconstructon of the cipher engine
+ * used in iClass, and RFID techology.
+ *
+ * The implementation is based on the work performed by
+ * Flavio D. Garcia, Gerhard de Koning Gans, Roel Verdult and
+ * Milosch Meriac in the paper "Dismantling IClass".
+ *
+ * Copyright (C) 2014 Martin Holst Swende
+ *
+ * This is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as published
+ * by the Free Software Foundation.
+ *
+ * This file is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with IClassCipher.  If not, see <http://www.gnu.org/licenses/>.
+ ****************************************************************************/
+/**
+From "Dismantling iclass":
+	This section describes in detail the built-in key diversification algorithm of iClass.
+	Besides the obvious purpose of deriving a card key from a master key, this
+	algorithm intends to circumvent weaknesses in the cipher by preventing the
+	usage of certain ‘weak’ keys. In order to compute a diversified key, the iClass
+	reader first encrypts the card identity id with the master key K, using single
+	DES. The resulting ciphertext is then input to a function called hash0 which
+	outputs the diversified key k.
+
+	k = hash0(DES enc (id, K))
+
+	Here the DES encryption of id with master key K outputs a cryptogram c
+	of 64 bits. These 64 bits are divided as c = x, y, z [0] , . . . , z [7] ∈ F 82 × F 82 × (F 62 ) 8
+	which is used as input to the hash0 function. This function introduces some
+	obfuscation by performing a number of permutations, complement and modulo
+	operations, see Figure 2.5. Besides that, it checks for and removes patterns like
+	similar key bytes, which could produce a strong bias in the cipher. Finally, the
+	output of hash0 is the diversified card key k = k [0] , . . . , k [7] ∈ (F 82 ) 8 .
+
+
+**/
+
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include "cipherutils.h"
+#include "cipher.h"
+#include "../util.h"
+#include <stdio.h>
+#include "des.h"
+#include <inttypes.h>
+
+uint8_t pi[35] = {0x0F,0x17,0x1B,0x1D,0x1E,0x27,0x2B,0x2D,0x2E,0x33,0x35,0x39,0x36,0x3A,0x3C,0x47,0x4B,0x4D,0x4E,0x53,0x55,0x56,0x59,0x5A,0x5C,0x63,0x65,0x66,0x69,0x6A,0x6C,0x71,0x72,0x74,0x78};
+
+static des_context ctx_enc = {DES_ENCRYPT,{0}};
+static des_context ctx_dec = {DES_DECRYPT,{0}};
+
+static bool debug_print = false;
+
+/**
+ * @brief The key diversification algorithm uses 6-bit bytes.
+ * This implementation uses 64 bit uint to pack seven of them into one
+ * variable. When they are there, they are placed as follows:
+ * XXXX XXXX N0 .... N7, occupying the lsat 48 bits.
+ *
+ * This function picks out one from such a collection
+ * @param all
+ * @param n bitnumber
+ * @return
+ */
+uint8_t getSixBitByte(uint64_t c, int n)
+{
+	return (c >> (42-6*n)) & 0x3F;
+	//return (c >> n*6) & 0x3f;
+}
+
+/**
+ * @brief Puts back a six-bit 'byte' into a uint64_t.
+ * @param c buffer
+ * @param z the value to place there
+ * @param n bitnumber.
+ */
+void pushbackSixBitByte(uint64_t *c, uint8_t z, int n)
+{
+	//0x XXXX YYYY ZZZZ ZZZZ ZZZZ
+	//             ^z0         ^z7
+	//z0:  1111 1100 0000 0000
+
+	uint64_t masked = z & 0x3F;
+	uint64_t eraser = 0x3F;
+	masked <<= 42-6*n;
+	eraser <<= 42-6*n;
+
+	//masked <<= 6*n;
+	//eraser <<= 6*n;
+
+	eraser = ~eraser;
+	(*c) &= eraser;
+	(*c) |= masked;
+
+}
+
+uint64_t swapZvalues(uint64_t c)
+{
+	uint64_t newz = 0;
+	pushbackSixBitByte(&newz, getSixBitByte(c,0),7);
+	pushbackSixBitByte(&newz, getSixBitByte(c,1),6);
+	pushbackSixBitByte(&newz, getSixBitByte(c,2),5);
+	pushbackSixBitByte(&newz, getSixBitByte(c,3),4);
+	pushbackSixBitByte(&newz, getSixBitByte(c,4),3);
+	pushbackSixBitByte(&newz, getSixBitByte(c,5),2);
+	pushbackSixBitByte(&newz, getSixBitByte(c,6),1);
+	pushbackSixBitByte(&newz, getSixBitByte(c,7),0);
+	newz |= (c & 0xFFFF000000000000);
+	return newz;
+}
+
+/**
+* @return 4 six-bit bytes chunked into a uint64_t,as 00..00a0a1a2a3
+*/
+uint64_t ck(int i, int j, uint64_t z)
+{
+
+//	printf("ck( i=%d, j=%d), zi=[%d],zj=[%d] \n",i,j,getSixBitByte(z,i),getSixBitByte(z,j) );
+
+	if(i == 1 && j == -1)
+	{
+		// ck(1, −1, z [0] . . . z [3] ) = z [0] . . . z [3]
+		return z;
+
+	}else if( j == -1)
+	{
+		// ck(i, −1, z [0] . . . z [3] ) = ck(i − 1, i − 2, z [0] . . . z [3] )
+		return ck(i-1,i-2, z);
+	}
+
+	if(getSixBitByte(z,i) == getSixBitByte(z,j))
+	{
+		// TODO, I dont know what they mean here in the paper
+		//ck(i, j − 1, z [0] . . . z [i] ← j . . . z [3] )
+		uint64_t newz = 0;
+		int c;
+		//printf("z[i]=z[i] (0x%02x), i=%d, j=%d\n",getSixBitByte(z,i),i,j );
+		for(c = 0; c < 4 ;c++)
+		{
+			uint8_t val = getSixBitByte(z,c);
+			if(c == i)
+			{
+				//printf("oops\n");
+				pushbackSixBitByte(&newz, j, c);
+			}else
+			{
+				pushbackSixBitByte(&newz, val, c);
+			}
+		}
+		return ck(i,j-1,newz);
+	}else
+	{
+		return ck(i,j-1,z);
+	}
+
+}
+/**
+
+	Definition 8.
+	Let the function check : (F 62 ) 8 → (F 62 ) 8 be defined as
+	check(z [0] . . . z [7] ) = ck(3, 2, z [0] . . . z [3] ) · ck(3, 2, z [4] . . . z [7] )
+
+	where ck : N × N × (F 62 ) 4 → (F 62 ) 4 is defined as
+
+		ck(1, −1, z [0] . . . z [3] ) = z [0] . . . z [3]
+		ck(i, −1, z [0] . . . z [3] ) = ck(i − 1, i − 2, z [0] . . . z [3] )
+		ck(i, j, z [0] . . . z [3] ) =
+		ck(i, j − 1, z [0] . . . z [i] ← j . . . z [3] ),  if z [i] = z [j] ;
+		ck(i, j − 1, z [0] . . . z [3] ), otherwise
+
+	otherwise.
+**/
+
+uint64_t check(uint64_t z)
+{
+	//These 64 bits are divided as c = x, y, z [0] , . . . , z [7]
+
+	// ck(3, 2, z [0] . . . z [3] )
+	uint64_t ck1 = ck(3,2, z );
+
+	// ck(3, 2, z [4] . . . z [7] )
+	uint64_t ck2 = ck(3,2, z << 24);
+	ck1 &= 0x00000000FFFFFF000000;
+	ck2 &= 0x00000000FFFFFF000000;
+
+	return ck1 | ck2 >> 24;
+
+}
+
+void permute(BitstreamIn *p_in, uint64_t z,int l,int r, BitstreamOut* out)
+{
+	if(bitsLeft(p_in) == 0)
+	{
+		return;
+	}
+	bool pn = tailBit(p_in);
+	if( pn ) // pn = 1
+	{
+		uint8_t zl = getSixBitByte(z,l);
+		//printf("permute pushing, zl=0x%02x, zl+1=0x%02x\n", zl, zl+1);
+		push6bits(out, zl+1);
+		permute(p_in, z, l+1,r, out);
+	}else // otherwise
+	{
+		uint8_t zr = getSixBitByte(z,r);
+		//printf("permute pushing, zr=0x%02x\n", zr);
+		push6bits(out, zr);
+		permute(p_in,z,l,r+1,out);
+	}
+}
+void testPermute()
+{
+
+	uint64_t x = 0;
+	pushbackSixBitByte(&x,0x00,0);
+	pushbackSixBitByte(&x,0x01,1);
+	pushbackSixBitByte(&x,0x02,2);
+	pushbackSixBitByte(&x,0x03,3);
+	pushbackSixBitByte(&x,0x04,4);
+	pushbackSixBitByte(&x,0x05,5);
+	pushbackSixBitByte(&x,0x06,6);
+	pushbackSixBitByte(&x,0x07,7);
+
+	uint8_t mres[8] = { getSixBitByte(x, 0),
+						getSixBitByte(x, 1),
+						getSixBitByte(x, 2),
+						getSixBitByte(x, 3),
+						getSixBitByte(x, 4),
+						getSixBitByte(x, 5),
+						getSixBitByte(x, 6),
+						getSixBitByte(x, 7)};
+	printarr("input_perm", mres,8);
+
+	uint8_t p = ~pi[0];
+	BitstreamIn p_in = { &p, 8,0 };
+	uint8_t outbuffer[] = {0,0,0,0,0,0,0,0};
+	BitstreamOut out = {outbuffer,0,0};
+
+	permute(&p_in, x,0,4, &out);
+
+	uint64_t permuted = bytes_to_num(outbuffer,8);
+	//printf("zTilde 0x%"PRIX64"\n", zTilde);
+	permuted >>= 16;
+
+	uint8_t res[8] = { getSixBitByte(permuted, 0),
+						getSixBitByte(permuted, 1),
+						getSixBitByte(permuted, 2),
+						getSixBitByte(permuted, 3),
+						getSixBitByte(permuted, 4),
+						getSixBitByte(permuted, 5),
+						getSixBitByte(permuted, 6),
+						getSixBitByte(permuted, 7)};
+	printarr("permuted", res, 8);
+}
+void printbegin()
+{
+	if(! debug_print)
+		return;
+
+	printf("          | x| y|z0|z1|z2|z3|z4|z5|z6|z7|\n");
+}
+
+void printState(char* desc, int x,int y, uint64_t c)
+{
+	if(! debug_print)
+		return;
+
+	printf("%s : ", desc);
+	//uint8_t x = 	(c & 0xFF00000000000000 ) >> 56;
+	//uint8_t y = 	(c & 0x00FF000000000000 ) >> 48;
+	printf("  %02x %02x", x,y);
+	int i ;
+	for(i =0 ; i < 8 ; i++)
+	{
+		printf(" %02x", getSixBitByte(c,i));
+	}
+	printf("\n");
+}
+
+/**
+ * @brief
+ *Definition 11. Let the function hash0 : F 82 × F 82 × (F 62 ) 8 → (F 82 ) 8 be defined as
+ *	hash0(x, y, z [0] . . . z [7] ) = k [0] . . . k [7] where
+ * z'[i] = (z[i] mod (63-i)) + i	i =  0...3
+ * z'[i+4] = (z[i+4] mod (64-i)) + i	i =  0...3
+ * ẑ = check(z');
+ * @param c
+ * @param k this is where the diversified key is put (should be 8 bytes)
+ * @return
+ */
+void hash0(uint64_t c, uint8_t *k)
+{
+	printbegin();
+	//These 64 bits are divided as c = x, y, z [0] , . . . , z [7]
+	// x = 8 bits
+	// y = 8 bits
+	// z0-z7 6 bits each : 48 bits
+	uint8_t x = 	(c & 0xFF00000000000000 ) >> 56;
+	uint8_t y = 	(c & 0x00FF000000000000 ) >> 48;
+	printState("origin",x,y,c);
+	int n;
+	uint8_t zn, zn4, _zn, _zn4;
+	uint64_t zP = 0;
+
+	for(n = 0;  n < 4 ; n++)
+	{
+		zn = getSixBitByte(c,n);
+		zn4 = getSixBitByte(c,n+4);
+
+		_zn = (zn % (63-n)) + n;
+		_zn4 = (zn4 % (64-n)) + n;
+
+		pushbackSixBitByte(&zP, _zn,n);
+		pushbackSixBitByte(&zP, _zn4,n+4);
+
+	}
+	printState("x|y|z'",x,y,zP);
+
+	uint64_t zCaret = check(zP);
+	printState("x|y|z^",x,y,zP);
+
+
+	uint8_t p = pi[x % 35];
+
+	if(x & 1) //Check if x7 is 1
+	{
+		p = ~p;
+	}
+    printState("p|y|z^",p,y,zP);
+	//if(debug_print) printf("p:%02x\n", p);
+
+	BitstreamIn p_in = { &p, 8,0 };
+	uint8_t outbuffer[] = {0,0,0,0,0,0,0,0};
+	BitstreamOut out = {outbuffer,0,0};
+	permute(&p_in,zCaret,0,4,&out);//returns 48 bits? or 6 8-bytes
+
+	//Out is now a buffer containing six-bit bytes, should be 48 bits
+	// if all went well
+	//printf("Permute output is %d num bits (48?)\n", out.numbits);
+	//Shift z-values down onto the lower segment
+
+	uint64_t zTilde = bytes_to_num(outbuffer,8);
+
+	//printf("zTilde 0x%"PRIX64"\n", zTilde);
+	zTilde >>= 16;
+	//printf("z~ 0x%"PRIX64"\n", zTilde);
+	printState("p|y|z~", p,y,zTilde);
+
+	int i;
+	int zerocounter =0 ;
+	for(i =0 ; i < 8  ; i++)
+	{
+
+		// the key on index i is first a bit from y
+		// then six bits from z,
+		// then a bit from p
+
+		// Init with zeroes
+		k[i] = 0;
+		// First, place yi leftmost in k
+		//k[i] |= (y  << i) & 0x80 ;
+
+		// First, place y(7-i) leftmost in k
+		k[i] |= (y  << (7-i)) & 0x80 ;
+
+		//printf("y%d = %d\n",i,(y  << i) & 0x80);
+
+		uint8_t zTilde_i = getSixBitByte(zTilde, i);
+		//printf("zTilde_%d 0x%02x (should be <= 0x3F)\n",i, zTilde_i);
+		// zTildeI is now on the form 00XXXXXX
+		// with one leftshift, it'll be
+		// 0XXXXXX0
+		// So after leftshift, we can OR it into k
+		// However, when doing complement, we need to
+		// again MASK 0XXXXXX0 (0x7E)
+		zTilde_i <<= 1;
+
+		//Finally, add bit from p or p-mod
+		//Shift bit i into rightmost location (mask only after complement)
+		uint8_t p_i = p >> i & 0x1;
+
+		if( k[i] )// yi = 1
+		{
+			//printf("k[%d] +1\n", i);
+			k[i] |= ~zTilde_i & 0x7E;
+			k[i] |= p_i & 1;
+			k[i] += 1;
+
+		}else // otherwise
+		{
+			k[i] |= zTilde_i & 0x7E;
+			k[i] |= (~p_i) & 1;
+		}
+		if((k[i]  & 1 )== 0)
+		{
+			zerocounter ++;
+		}
+	}
+	//printf("zerocounter=%d (should be 4)\n",zerocounter);
+	//printf("permute fin, y:0x%02x, x: 0x%02x\n", y, x);
+
+	//return k;
+}
+
+void reorder(uint8_t arr[8])
+{
+	uint8_t tmp[4] = {arr[3],arr[2],arr[1], arr[0]};
+	arr[0] = arr[7];
+	arr[1] = arr[6];
+	arr[2] = arr[5];
+	arr[3] = arr[4];
+	arr[4] = tmp[0];//arr[3];
+	arr[5] = tmp[1];//arr[2];
+	arr[6] = tmp[2];//arr[3];
+	arr[7] = tmp[3];//arr[1]
+}
+
+//extern void printarr(char * name, uint8_t* arr, int len);
+
+bool des_getParityBitFromKey(uint8_t key)
+{//The top 7 bits is used
+	bool parity = ((key & 0x80) >> 7)
+			^ ((key & 0x40) >> 6) ^ ((key & 0x20) >> 5)
+			^ ((key & 0x10) >> 4) ^ ((key & 0x08) >> 3)
+			^ ((key & 0x04) >> 2) ^ ((key & 0x02) >> 1);
+	return !parity;
+}
+void des_checkParity(uint8_t* key)
+{
+	int i;
+	int fails =0;
+	for(i =0 ; i < 8 ; i++)
+	{
+		bool parity = des_getParityBitFromKey(key[i]);
+		if(parity != (key[i] & 0x1))
+		{
+			fails++;
+			printf("parity1 fail, byte %d [%02x] was %d, should be %d\n",i,key[i],(key[i] & 0x1),parity);
+		}
+	}
+	if(fails)
+	{
+		printf("parity fails: %d\n", fails);
+	}else
+	{
+		printf("Key syntax is with parity bits inside each byte\n");
+	}
+}
+
+void printarr2(char * name, uint8_t* arr, int len)
+{
+	int i ;
+	printf("%s :", name);
+	for(i =0 ;  i< len ; i++)
+	{
+		printf("%02x",*(arr+i));
+	}
+	printf("\n");
+}
diff --git a/client/loclass/ikeys.h b/client/loclass/ikeys.h
new file mode 100644
index 00000000..0b943d6b
--- /dev/null
+++ b/client/loclass/ikeys.h
@@ -0,0 +1,8 @@
+#ifndef IKEYS_H
+#define IKEYS_H
+int testKeyDiversification();
+int doKeyTests();
+void hash0(uint64_t c, uint8_t *k);
+void pushbackSixBitByte(uint64_t *c, uint8_t z, int n);
+uint8_t getSixBitByte(uint64_t c, int n);
+#endif // IKEYS_H
diff --git a/common/iso15693tools.c b/common/iso15693tools.c
index add0ba69..eb4acf3e 100644
--- a/common/iso15693tools.c
+++ b/common/iso15693tools.c
@@ -12,6 +12,8 @@
 #include <stdlib.h>
 //#include "iso15693tools.h"
 
+#define POLY 0x8408
+
 // The CRC as described in ISO 15693-Part 3-Annex C
 // 	v	buffer with data
 //		n	length
@@ -63,5 +65,31 @@ char* Iso15693sprintUID(char *target,uint8_t *uid) {
   return target;
 }
 
+//CRC-16 Routine used within iclass
+unsigned short iclass_crc16(char *data_p, unsigned short length)
+{
+      unsigned char i;
+      unsigned int data;
+      unsigned int crc = 0xffff;
 
+      if (length == 0)
+            return (~crc);
 
+      do
+      {
+            for (i=0, data=(unsigned int)0xff & *data_p++;
+                 i < 8; 
+                 i++, data >>= 1)
+            {
+                  if ((crc & 0x0001) ^ (data & 0x0001))
+                        crc = (crc >> 1) ^ POLY;
+                  else  crc >>= 1;
+            }
+      } while (--length);
+
+      crc = ~crc;
+      data = crc;
+      crc = (crc << 8) | (data >> 8 & 0xff);
+      crc = crc ^ 0xBC3;
+      return (crc);
+}
diff --git a/common/iso15693tools.h b/common/iso15693tools.h
index c831fec4..9e9fe5d3 100644
--- a/common/iso15693tools.h
+++ b/common/iso15693tools.h
@@ -70,7 +70,7 @@
 uint16_t Iso15693Crc(uint8_t *v, int n);
 int Iso15693AddCrc(uint8_t *req, int n);
 char* Iso15693sprintUID(char *target,uint8_t *uid);
-
+unsigned short iclass_crc16(char *data_p, unsigned short length);
 //-----------------------------------------------------------------------------
 // Map a sequence of octets (~layer 2 command) into the set of bits to feed
 // to the FPGA, to transmit that command to the tag.
diff --git a/include/usb_cmd.h b/include/usb_cmd.h
index 4b1fc2b6..d99e48d8 100644
--- a/include/usb_cmd.h
+++ b/include/usb_cmd.h
@@ -114,9 +114,13 @@ typedef struct {
 #define CMD_WRITER_LEGIC_RF                                               0x0389
 #define CMD_EPA_PACE_COLLECT_NONCE                                        0x038A
 
+//iclass
 #define CMD_SNOOP_ICLASS                                                  0x0392
 #define CMD_SIMULATE_TAG_ICLASS                                           0x0393
 #define CMD_READER_ICLASS                                                 0x0394
+#define CMD_READER_ICLASS_REPLAY                                          0x0395
+#define CMD_ICLASS_ISO14443A_GETPUBLIC                                    0x0396
+#define CMD_ICLASS_ISO14443A_WRITE                                        0x0397
 
 // For measurements of the antenna tuning
 #define CMD_MEASURE_ANTENNA_TUNING                                        0x0400