diff --git a/armsrc/BigBuf.c b/armsrc/BigBuf.c
index 4642d0079..8637443a0 100644
--- a/armsrc/BigBuf.c
+++ b/armsrc/BigBuf.c
@@ -22,29 +22,29 @@ extern uint8_t _stack_start, __bss_end__;
 static uint8_t *BigBuf = &__bss_end__;
 
 /* BigBuf memory layout:
-Pointer to highest available memory: BigBuf_hi
-    high BigBuf_size
-    reserved = BigBuf_malloc()  subtracts amount from BigBuf_hi,
+Pointer to highest available memory: s_bigbuf_hi
+    high s_bigbuf_size
+    reserved = BigBuf_malloc()  subtracts amount from s_bigbuf_hi,
     low  0x00
 */
 
-static uint32_t BigBuf_size = 0;
+static uint32_t  s_bigbuf_size = 0;
 
 // High memory mark
-static uint32_t BigBuf_hi = 0;
+static uint32_t s_bigbuf_hi = 0;
 
 // pointer to the emulator memory.
 static uint8_t *emulator_memory = NULL;
 
 // trace related variables
-static uint32_t traceLen = 0;
+static uint32_t trace_len = 0;
 static bool tracing = true;
 
 // compute the available size for BigBuf
 void BigBuf_initialize(void) {
-    BigBuf_size = (uint32_t)&_stack_start - (uint32_t)&__bss_end__;
-    BigBuf_hi = BigBuf_size;
-    traceLen = 0;
+    s_bigbuf_size = (uint32_t)&_stack_start - (uint32_t)&__bss_end__;
+    s_bigbuf_hi = s_bigbuf_size;
+    trace_len = 0;
 }
 
 // get the address of BigBuf
@@ -53,7 +53,7 @@ uint8_t *BigBuf_get_addr(void) {
 }
 
 uint32_t BigBuf_get_size(void) {
-    return BigBuf_size;
+    return s_bigbuf_size;
 }
 
 // get the address of the emulator memory. Allocate part of Bigbuf for it, if not yet done
@@ -64,6 +64,11 @@ uint8_t *BigBuf_get_EM_addr(void) {
 
     return emulator_memory;
 }
+/*
+uint32_t BigBuf_get_EM_size(void) {
+    return CARD_MEMORY_SIZE;
+}
+*/
 
 // clear ALL of BigBuf
 void BigBuf_Clear(void) {
@@ -72,9 +77,9 @@ void BigBuf_Clear(void) {
 
 // clear ALL of BigBuf
 void BigBuf_Clear_ext(bool verbose) {
-    memset(BigBuf, 0, BigBuf_size);
+    memset(BigBuf, 0, s_bigbuf_size);
     if (verbose)
-        Dbprintf("Buffer cleared (%i bytes)", BigBuf_size);
+        Dbprintf("Buffer cleared (%i bytes)", s_bigbuf_size);
 }
 
 void BigBuf_Clear_EM(void) {
@@ -82,23 +87,23 @@ void BigBuf_Clear_EM(void) {
 }
 
 void BigBuf_Clear_keep_EM(void) {
-    memset(BigBuf, 0, BigBuf_hi);
+    memset(BigBuf, 0, s_bigbuf_hi);
 }
 
 // allocate a chunk of memory from BigBuf. We allocate high memory first. The unallocated memory
 // at the beginning of BigBuf is always for traces/samples
 uint8_t *BigBuf_malloc(uint16_t chunksize) {
-    if (BigBuf_hi < chunksize)
+    if (s_bigbuf_hi < chunksize)
         return NULL; // no memory left
 
     chunksize = (chunksize + 3) & 0xfffc; // round to next multiple of 4
-    BigBuf_hi -= chunksize;  // aligned to 4 Byte boundary
-    return (uint8_t *)BigBuf + BigBuf_hi;
+    s_bigbuf_hi -= chunksize;  // aligned to 4 Byte boundary
+    return (uint8_t *)BigBuf + s_bigbuf_hi;
 }
 
 // free ALL allocated chunks. The whole BigBuf is available for traces or samples again.
 void BigBuf_free(void) {
-    BigBuf_hi = BigBuf_size;
+    s_bigbuf_hi = s_bigbuf_size;
     emulator_memory = NULL;
     // shouldn't this empty BigBuf also?
 }
@@ -106,33 +111,33 @@ void BigBuf_free(void) {
 // free allocated chunks EXCEPT the emulator memory
 void BigBuf_free_keep_EM(void) {
     if (emulator_memory != NULL)
-        BigBuf_hi = emulator_memory - (uint8_t *)BigBuf;
+        s_bigbuf_hi = emulator_memory - (uint8_t *)BigBuf;
     else
-        BigBuf_hi = BigBuf_size;
+        s_bigbuf_hi = s_bigbuf_size;
 
     // shouldn't this empty BigBuf also?
 }
 
 void BigBuf_print_status(void) {
     DbpString(_CYAN_("Memory"));
-    Dbprintf("  BigBuf_size.............%d", BigBuf_size);
-    Dbprintf("  Available memory........%d", BigBuf_hi);
+    Dbprintf("  BigBuf_size.............%d", s_bigbuf_size);
+    Dbprintf("  Available memory........%d", s_bigbuf_hi);
     DbpString(_CYAN_("Tracing"));
     Dbprintf("  tracing ................%d", tracing);
-    Dbprintf("  traceLen ...............%d", traceLen);
+    Dbprintf("  traceLen ...............%d", trace_len);
 }
 
 // return the maximum trace length (i.e. the unallocated size of BigBuf)
 uint16_t BigBuf_max_traceLen(void) {
-    return BigBuf_hi;
+    return s_bigbuf_hi;
 }
 
 void clear_trace(void) {
-    traceLen = 0;
+    trace_len = 0;
 }
 
 void set_tracelen(uint32_t value) {
-    traceLen = value;
+    trace_len = value;
 }
 
 void set_tracing(bool enable) {
@@ -148,7 +153,7 @@ bool get_tracing(void) {
  * @return
  */
 uint32_t BigBuf_get_traceLen(void) {
-    return traceLen;
+    return trace_len;
 }
 
 /**
@@ -164,12 +169,12 @@ bool RAMFUNC LogTrace(const uint8_t *btBytes, uint16_t iLen, uint32_t timestamp_
     }
 
     uint8_t *trace = BigBuf_get_addr();
-    tracelog_hdr_t *hdr = (tracelog_hdr_t *)(trace + traceLen);
+    tracelog_hdr_t *hdr = (tracelog_hdr_t *)(trace + trace_len);
 
     uint32_t num_paritybytes = (iLen - 1) / 8 + 1; // number of valid paritybytes in *parity
 
     // Return when trace is full
-    if (TRACELOG_HDR_LEN + iLen + num_paritybytes >= BigBuf_max_traceLen() - traceLen) {
+    if (TRACELOG_HDR_LEN + iLen + num_paritybytes >= BigBuf_max_traceLen() - trace_len) {
         tracing = false; // don't trace any more
         if (DBGLEVEL >= DBG_DEBUG) { Dbprintf("trace is full"); }
         return false;
@@ -185,33 +190,35 @@ bool RAMFUNC LogTrace(const uint8_t *btBytes, uint16_t iLen, uint32_t timestamp_
     if (duration > 0x7FFF) {
         if (DBGLEVEL >= DBG_DEBUG) {
             Dbprintf("Error in LogTrace: duration too long for 15 bits encoding: 0x%08x start:0x%08x end:0x%08x", duration, timestamp_start, timestamp_end);
-            Dbprintf("Forcing duration = 0");
+//            Dbprintf("Forcing duration = 0");
         }
-        duration = 0;
+        
+        duration /= 32;
+        // duration >>= 5;
+//        duration = 0;
     }
 
     hdr->timestamp = timestamp_start;
     hdr->duration = duration;
     hdr->data_len = iLen;
     hdr->isResponse = !readerToTag;
-    traceLen += TRACELOG_HDR_LEN;
+    trace_len += TRACELOG_HDR_LEN;
 
     // data bytes
     if (btBytes != NULL && iLen != 0) {
-        memcpy(trace + traceLen, btBytes, iLen);
+        memcpy(trace + trace_len, btBytes, iLen);
     }
-    traceLen += iLen;
+    trace_len += iLen;
 
     // parity bytes
     if (num_paritybytes != 0) {
         if (parity != NULL) {
-            memcpy(trace + traceLen, parity, num_paritybytes);
+            memcpy(trace + trace_len, parity, num_paritybytes);
         } else {
-            memset(trace + traceLen, 0x00, num_paritybytes);
+            memset(trace + trace_len, 0x00, num_paritybytes);
         }
     }
-    traceLen += num_paritybytes;
-
+    trace_len += num_paritybytes;
     return true;
 }
 
diff --git a/armsrc/Makefile b/armsrc/Makefile
index 44ca3ab6b..2768ab3a4 100644
--- a/armsrc/Makefile
+++ b/armsrc/Makefile
@@ -30,7 +30,7 @@ SRC_ISO14443b = iso14443b.c
 SRC_FELICA = felica.c
 SRC_CRAPTO1 = crypto1.c des.c desfire_crypto.c mifaredesfire.c aes.c platform_util.c
 SRC_CRC = crc.c crc16.c crc32.c
-SRC_ICLASS = iclass.c optimized_cipher.c
+SRC_ICLASS = iclass.c optimized_cipherutils.c optimized_ikeys.c optimized_elite.c optimized_cipher.c 
 SRC_LEGIC = legicrf.c legicrfsim.c legic_prng.c
 SRC_NFCBARCODE = thinfilm.c
 
diff --git a/armsrc/iclass.c b/armsrc/iclass.c
index 478bafa43..a68565830 100644
--- a/armsrc/iclass.c
+++ b/armsrc/iclass.c
@@ -781,9 +781,10 @@ static void iclass_send_as_reader(uint8_t *frame, int len, uint32_t *start_time)
 
 	CodeIso15693AsReader(frame, len);
 	TransmitTo15693Tag(ToSend, ToSendMax, start_time);
-	uint32_t end_time = *start_time + 32 * (8 * ToSendMax - 4); // substract the 4 padding bits after EOF
+	uint32_t end_time = *start_time + (32 * ((8 * ToSendMax) - 4)); // substract the 4 padding bits after EOF
 
-    LogTrace(frame, len,  *start_time * 4, end_time * 4, NULL, true);
+    if (LogTrace(frame, len, (*start_time * 4), (end_time * 4), NULL, true) == false) 
+        DbpString("send_as_reader: failed logtrace");
 }
 
 static bool iclass_send_cmd_with_retries(uint8_t* cmd, size_t cmdsize, uint8_t* resp, size_t max_resp_size,
@@ -1112,35 +1113,57 @@ void iClass_ReadCheck(uint8_t blockno, uint8_t keytype) {
 
 // used with function select_and_auth (cmdhficlass.c)
 // which needs to authenticate before doing more things like read/write
-void iClass_Authentication(uint8_t *mac) {
+// selects and authenticate to a card,  sends back div_key and mac to client.
+void iClass_Authentication(uint8_t *bytes) {
+
+    struct p {
+        uint8_t key[8];
+        bool use_raw;
+        bool use_elite;
+        bool use_credit_key;
+    } PACKED;
+    struct p *payload = (struct p *)bytes;
+
+    // device response message
+    struct {
+        bool isOK;
+        uint8_t div_key[8];
+        uint8_t mac[4];
+    } PACKED packet;
 
 	Iso15693InitReader();
     StartCountSspClk();
         
     uint8_t card_data[3 * 8] = {0xFF};
-    bool use_credit_key = false;
     uint32_t eof_time = 0;
 
-    bool isOK = select_iclass_tag(card_data, use_credit_key, &eof_time);
-    if (isOK == false) {
-        reply_ng(CMD_HF_ICLASS_AUTH, PM3_SUCCESS, (uint8_t *)&isOK, sizeof(uint8_t));
+    packet.isOK = select_iclass_tag(card_data, payload->use_credit_key, &eof_time);
+    if (packet.isOK == false) {
+        reply_ng(CMD_HF_ICLASS_AUTH, PM3_SUCCESS, (uint8_t *)&packet, sizeof(packet));
         return;
     }
     uint32_t start_time = eof_time + DELAY_ICLASS_VICC_TO_VCD_READER;
     
     uint8_t check[9] = { ICLASS_CMD_CHECK };
-//    uint8_t mac[4];
-//    opt_doReaderMAC(uint8_t *cc_nr_p, uint8_t *div_key_p, mac )
+    uint8_t ccnr[12] = {0};
+    memcpy(ccnr, card_data + 16, 8);
+
+    if (payload->use_raw)
+        memcpy(packet.div_key, payload->key, 8);
+    else
+        iclass_calc_div_key(card_data, payload->key, packet.div_key, payload->use_elite);
+
+    opt_doReaderMAC(ccnr, packet.div_key, packet.mac);
 
     // copy MAC to check command (readersignature)
-    check[5] = mac[0];
-    check[6] = mac[1];
-    check[7] = mac[2];
-    check[8] = mac[3];
+    check[5] = packet.mac[0];
+    check[6] = packet.mac[1];
+    check[7] = packet.mac[2];
+    check[8] = packet.mac[3];
 
     uint8_t resp[ICLASS_BUFFER_SIZE];
-    isOK = iclass_send_cmd_with_retries(check, sizeof(check), resp, sizeof(resp), 4, 3, start_time, ICLASS_READER_TIMEOUT_OTHERS, &eof_time);
-    reply_ng(CMD_HF_ICLASS_AUTH, PM3_SUCCESS, (uint8_t *)&isOK, sizeof(uint8_t));
+    packet.isOK = iclass_send_cmd_with_retries(check, sizeof(check), resp, sizeof(resp), 4, 3, start_time, ICLASS_READER_TIMEOUT_OTHERS, &eof_time);
+    reply_ng(CMD_HF_ICLASS_AUTH, PM3_SUCCESS, (uint8_t *)&packet, sizeof(packet));
 }
 
 typedef struct iclass_premac {
@@ -1300,6 +1323,8 @@ void iClass_Dump(uint8_t start_blockno, uint8_t numblks) {
 
 	// return pointer to dump memory in arg3
 	// iceman:  why not return | dataout - getbigbuf ?  Should give exact location.
+    Dbprintf("ICE::  dataout, %u  max trace %u,  bb start %u,   data-bb %u ",  dataout, BigBuf_max_traceLen(),  BigBuf_get_addr(),  dataout - BigBuf_get_addr()  );
+    Dbprintf("ICE:: bb size %u,  malloced %u  (255*8)",  BigBuf_get_size(),  BigBuf_get_size() - (dataout - BigBuf_get_addr()) );
     reply_mix(CMD_ACK, isOK, blkcnt, BigBuf_max_traceLen(), 0, 0);
     BigBuf_free();
 }
diff --git a/armsrc/iso15693.c b/armsrc/iso15693.c
index f8214b5e4..39a64a8c4 100644
--- a/armsrc/iso15693.c
+++ b/armsrc/iso15693.c
@@ -688,9 +688,9 @@ int GetIso15693AnswerFromTag(uint8_t* response, uint16_t max_len, uint16_t timeo
     FpgaDisableSscDma();
 
     uint32_t sof_time = *eof_time
-                        - DecodeTag.len * 8 * 8 * 16 // time for byte transfers
-                        - 32 * 16  // time for SOF transfer
-                        - (DecodeTag.lastBit != SOF_PART2?32*16:0); // time for EOF transfer
+                        - (DecodeTag.len * 8 * 8 * 16) // time for byte transfers
+                        - (32 * 16)  // time for SOF transfer
+                        - (DecodeTag.lastBit != SOF_PART2 ? (32 * 16) : 0); // time for EOF transfer
 
     if (DBGLEVEL >= DBG_EXTENDED) {
         Dbprintf("samples = %d, ret = %d, Decoder: state = %d, lastBit = %d, len = %d, bitCount = %d, posCount = %d",
@@ -705,7 +705,8 @@ int GetIso15693AnswerFromTag(uint8_t* response, uint16_t max_len, uint16_t timeo
         Dbprintf("timing: sof_time = %d, eof_time = %d", (sof_time * 4), (*eof_time * 4));
     }
 
-    LogTrace(DecodeTag.output, DecodeTag.len, (sof_time * 4), (*eof_time * 4), NULL, false);
+    if (LogTrace(DecodeTag.output, DecodeTag.len, (sof_time * 4), (*eof_time * 4), NULL, false) == false)
+        DbpString("GetIso15693AnswerFromTag: failed logtrace");
 
     if (ret < 0) {
         return ret;
@@ -1090,7 +1091,8 @@ int GetIso15693CommandFromReader(uint8_t *received, size_t max_len, uint32_t *eo
                         - DecodeReader.byteCount * (DecodeReader.Coding==CODING_1_OUT_OF_4?128:2048) // time for byte transfers
                         - 32  // time for SOF transfer
                         - 16; // time for EOF transfer
-        LogTrace(DecodeReader.output, DecodeReader.byteCount, sof_time*32, *eof_time*32, NULL, true);
+        if (LogTrace(DecodeReader.output, DecodeReader.byteCount, sof_time*32, *eof_time*32, NULL, true) == false)
+            DbpString("GetIso15693CommandFromReader: failed logtrace");
     }
 
     return DecodeReader.byteCount;
@@ -1223,16 +1225,18 @@ void SniffIso15693(uint8_t jam_search_len, uint8_t *jam_search_string) {
             }
         }
 
-        if (!TagIsActive) {                                                // no need to try decoding reader data if the tag is sending
+        // no need to try decoding reader data if the tag is sending
+        if (TagIsActive == false) {
+
             if (Handle15693SampleFromReader(sniffdata & 0x02, &DecodeReader)) {
 
-                uint32_t eof_time = dma_start_time + samples*16 + 8 - DELAY_READER_TO_ARM_SNIFF; // end of EOF
+                uint32_t eof_time = dma_start_time + (samples * 16) + 8 - DELAY_READER_TO_ARM_SNIFF; // end of EOF
                 if (DecodeReader.byteCount > 0) {
                     uint32_t sof_time = eof_time
-                                    - DecodeReader.byteCount * (DecodeReader.Coding==CODING_1_OUT_OF_4?128*16:2048*16) // time for byte transfers
-                                    - 32*16  // time for SOF transfer
-                                    - 16*16; // time for EOF transfer
-                    LogTrace(DecodeReader.output, DecodeReader.byteCount, sof_time*4, eof_time*4, NULL, true);
+                                    - DecodeReader.byteCount * (DecodeReader.Coding == CODING_1_OUT_OF_4 ? 128 * 16 : 2048 * 16) // time for byte transfers
+                                    - 32 * 16  // time for SOF transfer
+                                    - 16 * 16; // time for EOF transfer
+                    LogTrace(DecodeReader.output, DecodeReader.byteCount, (sof_time * 4), (eof_time * 4), NULL, true);
                 }
                 // And ready to receive another command.
                 DecodeReaderReset(&DecodeReader);
@@ -1244,13 +1248,13 @@ void SniffIso15693(uint8_t jam_search_len, uint8_t *jam_search_string) {
 
             } else if (Handle15693SampleFromReader(sniffdata & 0x01, &DecodeReader)) {
 
-                uint32_t eof_time = dma_start_time + samples*16 + 16 - DELAY_READER_TO_ARM_SNIFF; // end of EOF
+                uint32_t eof_time = dma_start_time + (samples * 16) + 16 - DELAY_READER_TO_ARM_SNIFF; // end of EOF
                 if (DecodeReader.byteCount > 0) {
                     uint32_t sof_time = eof_time
-                                    - DecodeReader.byteCount * (DecodeReader.Coding==CODING_1_OUT_OF_4?128*16:2048*16) // time for byte transfers
-                                    - 32*16  // time for SOF transfer
-                                    - 16*16; // time for EOF transfer
-                    LogTrace(DecodeReader.output, DecodeReader.byteCount, sof_time*4, eof_time*4, NULL, true);
+                                    - DecodeReader.byteCount * (DecodeReader.Coding == CODING_1_OUT_OF_4 ? 128 * 16 : 2048 * 16) // time for byte transfers
+                                    - 32 * 16  // time for SOF transfer
+                                    - 16 * 16; // time for EOF transfer
+                    LogTrace(DecodeReader.output, DecodeReader.byteCount, (sof_time * 4), (eof_time * 4), NULL, true);
                 }
                 // And ready to receive another command
                 DecodeReaderReset(&DecodeReader);
@@ -1269,15 +1273,16 @@ void SniffIso15693(uint8_t jam_search_len, uint8_t *jam_search_string) {
         if (!ReaderIsActive && ExpectTagAnswer) {                       // no need to try decoding tag data if the reader is currently sending or no answer expected yet
             if (Handle15693SamplesFromTag(sniffdata >> 2, &DecodeTag)) {
 
-                uint32_t eof_time = dma_start_time + samples*16 - DELAY_TAG_TO_ARM_SNIFF; // end of EOF
+                uint32_t eof_time = dma_start_time + (samples * 16) - DELAY_TAG_TO_ARM_SNIFF; // end of EOF
                 if (DecodeTag.lastBit == SOF_PART2) {
-                    eof_time -= 8*16; // needed 8 additional samples to confirm single SOF (iCLASS)
+                    eof_time -= (8 * 16); // needed 8 additional samples to confirm single SOF (iCLASS)
                 }
                 uint32_t sof_time = eof_time
                                     - DecodeTag.len * 8 * 8 * 16 // time for byte transfers
-                                    - 32 * 16  // time for SOF transfer
-                                    - (DecodeTag.lastBit != SOF_PART2?32*16:0); // time for EOF transfer
-                LogTrace(DecodeTag.output, DecodeTag.len, sof_time*4, eof_time*4, NULL, false);
+                                    - (32 * 16)  // time for SOF transfer
+                                    - (DecodeTag.lastBit != SOF_PART2 ? (32 * 16) : 0); // time for EOF transfer
+
+                LogTrace(DecodeTag.output, DecodeTag.len, (sof_time * 4), (eof_time * 4), NULL, false);
                 // And ready to receive another response.
                 DecodeTagReset(&DecodeTag);
                 DecodeReaderReset(&DecodeReader);
diff --git a/armsrc/optimized_cipher.c b/armsrc/optimized_cipher.c
index 24bb8d7ee..021b8ae22 100644
--- a/armsrc/optimized_cipher.c
+++ b/armsrc/optimized_cipher.c
@@ -77,7 +77,15 @@
   -- piwi 2019
 **/
 
+/**
+  add the possibility to do iCLASS on device only
+  -- iceman 2020
+**/
+
 #include "optimized_cipher.h"
+#include "optimized_elite.h"
+#include "optimized_ikeys.h"
+#include "optimized_cipherutils.h"
 
 static const uint8_t opt_select_LUT[256] = {
     00, 03, 02, 01, 02, 03, 00, 01, 04, 07, 07, 04, 06, 07, 05, 04,
@@ -290,3 +298,23 @@ void opt_doTagMAC_2(State _init,  uint8_t *nr, uint8_t mac[4], const uint8_t *di
     opt_suc(div_key_p, &_init, nr, 4, true);
     opt_output(div_key_p, &_init, mac);
 }
+
+
+void iclass_calc_div_key(uint8_t *csn, uint8_t *key, uint8_t *div_key, bool elite) {
+    if (elite) {
+        uint8_t keytable[128] = {0};
+        uint8_t key_index[8] = {0};
+        uint8_t key_sel[8] = { 0 };
+        uint8_t key_sel_p[8] = { 0 };
+        hash2(key, keytable);
+        hash1(csn, key_index);
+        for (uint8_t i = 0; i < 8 ; i++)
+            key_sel[i] = keytable[key_index[i]];
+
+        //Permute from iclass format to standard format
+        permutekey_rev(key_sel, key_sel_p);
+        diversifyKey(csn, key_sel_p, div_key);
+    } else {
+        diversifyKey(csn, key, div_key);
+    }
+}
diff --git a/armsrc/optimized_cipher.h b/armsrc/optimized_cipher.h
index c6df25ab8..e65b6c4cb 100644
--- a/armsrc/optimized_cipher.h
+++ b/armsrc/optimized_cipher.h
@@ -46,4 +46,6 @@ State opt_doTagMAC_1(uint8_t *cc_p, const uint8_t *div_key_p);
  */
 void opt_doTagMAC_2(State _init, uint8_t *nr, uint8_t mac[4], const uint8_t *div_key_p);
 
+
+void iclass_calc_div_key(uint8_t *csn, uint8_t *key, uint8_t *div_key, bool elite);
 #endif // OPTIMIZED_CIPHER_H
diff --git a/armsrc/optimized_cipherutils.c b/armsrc/optimized_cipherutils.c
new file mode 100644
index 000000000..c51f83f9b
--- /dev/null
+++ b/armsrc/optimized_cipherutils.c
@@ -0,0 +1,140 @@
+/*****************************************************************************
+ * WARNING
+ *
+ * THIS CODE IS CREATED FOR EXPERIMENTATION AND EDUCATIONAL USE ONLY.
+ *
+ * USAGE OF THIS CODE IN OTHER WAYS MAY INFRINGE UPON THE INTELLECTUAL
+ * PROPERTY OF OTHER PARTIES, SUCH AS INSIDE SECURE AND HID GLOBAL,
+ * AND MAY EXPOSE YOU TO AN INFRINGEMENT ACTION FROM THOSE PARTIES.
+ *
+ * THIS CODE SHOULD NEVER BE USED TO INFRINGE PATENTS OR INTELLECTUAL PROPERTY RIGHTS.
+ *
+ *****************************************************************************
+ *
+ * This file is part of loclass. 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, or, at your option, any later version.
+ *
+ * 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 loclass.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *
+ ****************************************************************************/
+#include "optimized_cipherutils.h"
+#include <stdint.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) << (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
+ */
+void x_num_to_bytes(uint64_t n, size_t len, uint8_t *dest) {
+    while (len--) {
+        dest[len] = (uint8_t) n;
+        n >>= 8;
+    }
+}
+
+uint64_t x_bytes_to_num(uint8_t *src, size_t len) {
+    uint64_t num = 0;
+    while (len--) {
+        num = (num << 8) | (*src);
+        src++;
+    }
+    return num;
+}
+
+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]);
+    }
+}
+
+void reverse_arraycopy(uint8_t *arr, uint8_t *dest, size_t len) {
+    uint8_t i;
+    for (i = 0; i < len ; i++) {
+        dest[i] = reversebytes(arr[i]);
+    }
+}
+
diff --git a/armsrc/optimized_cipherutils.h b/armsrc/optimized_cipherutils.h
new file mode 100644
index 000000000..63ba8b8aa
--- /dev/null
+++ b/armsrc/optimized_cipherutils.h
@@ -0,0 +1,66 @@
+/*****************************************************************************
+ * WARNING
+ *
+ * THIS CODE IS CREATED FOR EXPERIMENTATION AND EDUCATIONAL USE ONLY.
+ *
+ * USAGE OF THIS CODE IN OTHER WAYS MAY INFRINGE UPON THE INTELLECTUAL
+ * PROPERTY OF OTHER PARTIES, SUCH AS INSIDE SECURE AND HID GLOBAL,
+ * AND MAY EXPOSE YOU TO AN INFRINGEMENT ACTION FROM THOSE PARTIES.
+ *
+ * THIS CODE SHOULD NEVER BE USED TO INFRINGE PATENTS OR INTELLECTUAL PROPERTY RIGHTS.
+ *
+ *****************************************************************************
+ *
+ * This file is part of loclass. 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, or, at your option, any later version.
+ *
+ * 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 loclass.  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);
+
+void push6bits(BitstreamOut *stream, uint8_t bits);
+void x_num_to_bytes(uint64_t n, size_t len, uint8_t *dest);
+uint64_t x_bytes_to_num(uint8_t *src, size_t len);
+uint8_t reversebytes(uint8_t b);
+void reverse_arraybytes(uint8_t *arr, size_t len);
+void reverse_arraycopy(uint8_t *arr, uint8_t *dest, size_t len);
+#endif // CIPHERUTILS_H
diff --git a/armsrc/optimized_elite.c b/armsrc/optimized_elite.c
new file mode 100644
index 000000000..d2c57ac68
--- /dev/null
+++ b/armsrc/optimized_elite.c
@@ -0,0 +1,238 @@
+/*****************************************************************************
+ * WARNING
+ *
+ * THIS CODE IS CREATED FOR EXPERIMENTATION AND EDUCATIONAL USE ONLY.
+ *
+ * USAGE OF THIS CODE IN OTHER WAYS MAY INFRINGE UPON THE INTELLECTUAL
+ * PROPERTY OF OTHER PARTIES, SUCH AS INSIDE SECURE AND HID GLOBAL,
+ * AND MAY EXPOSE YOU TO AN INFRINGEMENT ACTION FROM THOSE PARTIES.
+ *
+ * THIS CODE SHOULD NEVER BE USED TO INFRINGE PATENTS OR INTELLECTUAL PROPERTY RIGHTS.
+ *
+ *****************************************************************************
+ *
+ * This file is part of loclass. 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, or, at your option, any later version.
+ *
+ * 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 loclass.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *
+ *
+ ****************************************************************************/
+#include "optimized_elite.h"
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include "mbedtls/des.h"
+#include "optimized_ikeys.h"
+
+/**
+ * @brief Permutes a key from standard NIST format to Iclass specific format
+ *  from http://www.proxmark.org/forum/viewtopic.php?pid=11220#p11220
+ *
+ *  If you permute [6c 8d 44 f9 2a 2d 01 bf]  you get  [8a 0d b9 88 bb a7 90 ea]  as shown below.
+ *
+ *  1 0 1 1 1 1 1 1  bf
+ *  0 0 0 0 0 0 0 1  01
+ *  0 0 1 0 1 1 0 1  2d
+ *  0 0 1 0 1 0 1 0  2a
+ *  1 1 1 1 1 0 0 1  f9
+ *  0 1 0 0 0 1 0 0  44
+ *  1 0 0 0 1 1 0 1  8d
+ *  0 1 1 0 1 1 0 0  6c
+ *
+ *  8 0 b 8 b a 9 e
+ *  a d 9 8 b 7 0 a
+ *
+ * @param key
+ * @param dest
+ */
+void permutekey(uint8_t key[8], uint8_t dest[8]) {
+    int i;
+    for (i = 0 ; i < 8 ; i++) {
+        dest[i] = (((key[7] & (0x80 >> i)) >> (7 - i)) << 7) |
+                  (((key[6] & (0x80 >> i)) >> (7 - i)) << 6) |
+                  (((key[5] & (0x80 >> i)) >> (7 - i)) << 5) |
+                  (((key[4] & (0x80 >> i)) >> (7 - i)) << 4) |
+                  (((key[3] & (0x80 >> i)) >> (7 - i)) << 3) |
+                  (((key[2] & (0x80 >> i)) >> (7 - i)) << 2) |
+                  (((key[1] & (0x80 >> i)) >> (7 - i)) << 1) |
+                  (((key[0] & (0x80 >> i)) >> (7 - i)) << 0);
+    }
+}
+/**
+ * Permutes  a key from iclass specific format to NIST format
+ * @brief permutekey_rev
+ * @param key
+ * @param dest
+ */
+void permutekey_rev(uint8_t key[8], uint8_t dest[8]) {
+    int i;
+    for (i = 0 ; i < 8 ; i++) {
+        dest[7 - i] = (((key[0] & (0x80 >> i)) >> (7 - i)) << 7) |
+                      (((key[1] & (0x80 >> i)) >> (7 - i)) << 6) |
+                      (((key[2] & (0x80 >> i)) >> (7 - i)) << 5) |
+                      (((key[3] & (0x80 >> i)) >> (7 - i)) << 4) |
+                      (((key[4] & (0x80 >> i)) >> (7 - i)) << 3) |
+                      (((key[5] & (0x80 >> i)) >> (7 - i)) << 2) |
+                      (((key[6] & (0x80 >> i)) >> (7 - i)) << 1) |
+                      (((key[7] & (0x80 >> i)) >> (7 - i)) << 0);
+    }
+}
+
+/**
+ * Helper function for hash1
+ * @brief rr
+ * @param val
+ * @return
+ */
+static inline uint8_t rr(uint8_t val) {
+    return val >> 1 | ((val & 1) << 7);
+}
+
+/**
+ * Helper function for hash1
+ * @brief rl
+ * @param val
+ * @return
+ */
+static inline uint8_t rl(uint8_t val) {
+    return val << 1 | ((val & 0x80) >> 7);
+}
+
+/**
+ * Helper function for hash1
+ * @brief swap
+ * @param val
+ * @return
+ */
+static inline uint8_t swap(uint8_t val) {
+    return ((val >> 4) & 0xFF) | ((val & 0xFF) << 4);
+}
+
+/**
+ * Hash1 takes CSN as input, and determines what bytes in the keytable will be used
+ * when constructing the K_sel.
+ * @param csn the CSN used
+ * @param k output
+ */
+void hash1(uint8_t csn[], uint8_t k[]) {
+    k[0] = csn[0] ^ csn[1] ^ csn[2] ^ csn[3] ^ csn[4] ^ csn[5] ^ csn[6] ^ csn[7];
+    k[1] = csn[0] + csn[1] + csn[2] + csn[3] + csn[4] + csn[5] + csn[6] + csn[7];
+    k[2] = rr(swap(csn[2] + k[1]));
+    k[3] = rl(swap(csn[3] + k[0]));
+    k[4] = ~rr(csn[4] + k[2]) + 1;
+    k[5] = ~rl(csn[5] + k[3]) + 1;
+    k[6] = rr(csn[6] + (k[4] ^ 0x3c));
+    k[7] = rl(csn[7] + (k[5] ^ 0xc3));
+
+    k[7] &= 0x7F;
+    k[6] &= 0x7F;
+    k[5] &= 0x7F;
+    k[4] &= 0x7F;
+    k[3] &= 0x7F;
+    k[2] &= 0x7F;
+    k[1] &= 0x7F;
+    k[0] &= 0x7F;
+}
+/**
+Definition 14. Define the rotate key function rk : (F 82 ) 8 × N → (F 82 ) 8 as
+rk(x [0] . . . x [7] , 0) = x [0] . . . x [7]
+rk(x [0] . . . x [7] , n + 1) = rk(rl(x [0] ) . . . rl(x [7] ), n)
+**/
+static void rk(uint8_t *key, uint8_t n, uint8_t *outp_key) {
+    memcpy(outp_key, key, 8);
+    uint8_t j;
+    while (n-- > 0) {
+        for (j = 0; j < 8 ; j++)
+            outp_key[j] = rl(outp_key[j]);
+    }
+    return;
+}
+
+static mbedtls_des_context ctx_enc;
+static mbedtls_des_context ctx_dec;
+
+static void desdecrypt_iclass(uint8_t *iclass_key, uint8_t *input, uint8_t *output) {
+    uint8_t key_std_format[8] = {0};
+    permutekey_rev(iclass_key, key_std_format);
+    mbedtls_des_setkey_dec(&ctx_dec, key_std_format);
+    mbedtls_des_crypt_ecb(&ctx_dec, input, output);
+}
+
+static void desencrypt_iclass(uint8_t *iclass_key, uint8_t *input, uint8_t *output) {
+    uint8_t key_std_format[8] = {0};
+    permutekey_rev(iclass_key, key_std_format);
+    mbedtls_des_setkey_enc(&ctx_enc, key_std_format);
+    mbedtls_des_crypt_ecb(&ctx_enc, input, output);
+}
+
+/**
+ * @brief Insert uint8_t[8] custom master key to calculate hash2 and return key_select.
+ * @param key unpermuted custom key
+ * @param hash1 hash1
+ * @param key_sel output key_sel=h[hash1[i]]
+ */
+void hash2(uint8_t *key64, uint8_t *outp_keytable) {
+    /**
+     *Expected:
+     * High Security Key Table
+
+    00  F1 35 59 A1 0D 5A 26 7F 18 60 0B 96 8A C0 25 C1
+    10  BF A1 3B B0 FF 85 28 75 F2 1F C6 8F 0E 74 8F 21
+    20  14 7A 55 16 C8 A9 7D B3 13 0C 5D C9 31 8D A9 B2
+    30  A3 56 83 0F 55 7E DE 45 71 21 D2 6D C1 57 1C 9C
+    40  78 2F 64 51 42 7B 64 30 FA 26 51 76 D3 E0 FB B6
+    50  31 9F BF 2F 7E 4F 94 B4 BD 4F 75 91 E3 1B EB 42
+    60  3F 88 6F B8 6C 2C 93 0D 69 2C D5 20 3C C1 61 95
+    70  43 08 A0 2F FE B3 26 D7 98 0B 34 7B 47 70 A0 AB
+
+    **** The 64-bit HS Custom Key Value = 5B7C62C491C11B39 ******/
+    uint8_t key64_negated[8] = {0};
+    uint8_t z[8][8] = {{0}, {0}};
+    uint8_t temp_output[8] = {0};
+
+    //calculate complement of key
+    int i;
+    for (i = 0; i < 8; i++)
+        key64_negated[i] = ~key64[i];
+
+    // Once again, key is on iclass-format
+    desencrypt_iclass(key64, key64_negated, z[0]);
+
+    uint8_t y[8][8] = {{0}, {0}};
+
+    // y[0]=DES_dec(z[0],~key)
+    // Once again, key is on iclass-format
+    desdecrypt_iclass(z[0], key64_negated, y[0]);
+
+    for (i = 1; i < 8; i++) {
+        rk(key64, i, temp_output);
+        desdecrypt_iclass(temp_output, z[i - 1], z[i]);
+        desencrypt_iclass(temp_output, y[i - 1], y[i]);
+    }
+
+    if (outp_keytable != NULL) {
+        for (i = 0 ; i < 8 ; i++) {
+            memcpy(outp_keytable + i * 16, y[i], 8);
+            memcpy(outp_keytable + 8 + i * 16, z[i], 8);
+        }
+    }
+}
diff --git a/armsrc/optimized_elite.h b/armsrc/optimized_elite.h
new file mode 100644
index 000000000..281ecf0bb
--- /dev/null
+++ b/armsrc/optimized_elite.h
@@ -0,0 +1,62 @@
+/*****************************************************************************
+ * WARNING
+ *
+ * THIS CODE IS CREATED FOR EXPERIMENTATION AND EDUCATIONAL USE ONLY.
+ *
+ * USAGE OF THIS CODE IN OTHER WAYS MAY INFRINGE UPON THE INTELLECTUAL
+ * PROPERTY OF OTHER PARTIES, SUCH AS INSIDE SECURE AND HID GLOBAL,
+ * AND MAY EXPOSE YOU TO AN INFRINGEMENT ACTION FROM THOSE PARTIES.
+ *
+ * THIS CODE SHOULD NEVER BE USED TO INFRINGE PATENTS OR INTELLECTUAL PROPERTY RIGHTS.
+ *
+ *****************************************************************************
+ *
+ * This file is part of loclass. 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, or, at your option, any later version.
+ *
+ * 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 loclass.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *
+ ****************************************************************************/
+
+
+#ifndef ELITE_CRACK_H
+#define ELITE_CRACK_H
+
+#include <stdint.h>
+#include <stdlib.h>
+
+void permutekey(uint8_t key[8], uint8_t dest[8]);
+/**
+ * Permutes  a key from iclass specific format to NIST format
+ * @brief permutekey_rev
+ * @param key
+ * @param dest
+ */
+void permutekey_rev(uint8_t key[8], uint8_t dest[8]);
+/**
+ * Hash1 takes CSN as input, and determines what bytes in the keytable will be used
+ * when constructing the K_sel.
+ * @param csn the CSN used
+ * @param k output
+ */
+void hash1(uint8_t *csn, uint8_t *k);
+void hash2(uint8_t *key64, uint8_t *outp_keytable);
+
+#endif
diff --git a/armsrc/optimized_ikeys.c b/armsrc/optimized_ikeys.c
new file mode 100644
index 000000000..eeb00e562
--- /dev/null
+++ b/armsrc/optimized_ikeys.c
@@ -0,0 +1,324 @@
+/*****************************************************************************
+ * WARNING
+ *
+ * THIS CODE IS CREATED FOR EXPERIMENTATION AND EDUCATIONAL USE ONLY.
+ *
+ * USAGE OF THIS CODE IN OTHER WAYS MAY INFRINGE UPON THE INTELLECTUAL
+ * PROPERTY OF OTHER PARTIES, SUCH AS INSIDE SECURE AND HID GLOBAL,
+ * AND MAY EXPOSE YOU TO AN INFRINGEMENT ACTION FROM THOSE PARTIES.
+ *
+ * THIS CODE SHOULD NEVER BE USED TO INFRINGE PATENTS OR INTELLECTUAL PROPERTY RIGHTS.
+ *
+ *****************************************************************************
+ *
+ * This file is part of loclass. 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, or, at your option, any later version.
+ *
+ * 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 loclass.  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 "optimized_ikeys.h"
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <inttypes.h>
+#include "mbedtls/des.h"
+#include "optimized_cipherutils.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 mbedtls_des_context ctx_enc;
+
+/**
+ * @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 last 48 bits.
+ *
+ * This function picks out one from such a collection
+ * @param all
+ * @param n bitnumber
+ * @return
+ */
+static uint8_t getSixBitByte(uint64_t c, int n) {
+    return (c >> (42 - 6 * n)) & 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.
+ */
+static 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;
+
+}
+/**
+ * @brief Swaps the z-values.
+ * If the input value has format XYZ0Z1...Z7, the output will have the format
+ * XYZ7Z6...Z0 instead
+ * @param c
+ * @return
+ */
+static 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
+*/
+static uint64_t ck(int i, int j, uint64_t z) {
+    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)) {
+        //ck(i, j − 1, z [0] . . . z [i] ← j . . . z [3] )
+        uint64_t newz = 0;
+        int c;
+        for (c = 0; c < 4; c++) {
+            uint8_t val = getSixBitByte(z, c);
+            if (c == i)
+                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.
+**/
+
+static 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);
+
+    //The ck function will place the values
+    // in the middle of z.
+    ck1 &= 0x00000000FFFFFF000000;
+    ck2 &= 0x00000000FFFFFF000000;
+
+    return ck1 | ck2 >> 24;
+}
+
+static 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);
+
+        push6bits(out, zl + 1);
+        permute(p_in, z, l + 1, r, out);
+    } else { // otherwise
+        uint8_t zr = getSixBitByte(z, r);
+
+        push6bits(out, zr);
+        permute(p_in, z, l, r + 1, out);
+    }
+}
+
+/**
+ * @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[8]) {
+    c = swapZvalues(c);
+
+    //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;
+    uint64_t zP = 0;
+
+    for (int n = 0;  n < 4 ; n++) {
+        uint8_t zn = getSixBitByte(c, n);
+        uint8_t zn4 = getSixBitByte(c, n + 4);
+        uint8_t _zn = (zn % (63 - n)) + n;
+        uint8_t _zn4 = (zn4 % (64 - n)) + n;
+        pushbackSixBitByte(&zP, _zn, n);
+        pushbackSixBitByte(&zP, _zn4, n + 4);
+    }
+
+    uint64_t zCaret = check(zP);
+    uint8_t p = pi[x % 35];
+
+    if (x & 1) //Check if x7 is 1
+        p = ~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
+    //Shift z-values down onto the lower segment
+
+    uint64_t zTilde = x_bytes_to_num(outbuffer, sizeof(outbuffer));
+
+    zTilde >>= 16;
+
+    for (int 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 ;
+
+        uint8_t zTilde_i = getSixBitByte(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
+            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;
+        }
+    }
+}
+/**
+ * @brief Performs Elite-class key diversification
+ * @param csn
+ * @param key
+ * @param div_key
+ */
+void diversifyKey(uint8_t *csn, uint8_t *key, uint8_t *div_key) {
+    // Prepare the DES key
+    mbedtls_des_setkey_enc(&ctx_enc, key);
+
+    uint8_t crypted_csn[8] = {0};
+
+    // Calculate DES(CSN, KEY)
+    mbedtls_des_crypt_ecb(&ctx_enc, csn, crypted_csn);
+
+    //Calculate HASH0(DES))
+    uint64_t c_csn = x_bytes_to_num(crypted_csn, sizeof(crypted_csn));
+
+    hash0(c_csn, div_key);
+}
+
diff --git a/armsrc/optimized_ikeys.h b/armsrc/optimized_ikeys.h
new file mode 100644
index 000000000..91fa406ad
--- /dev/null
+++ b/armsrc/optimized_ikeys.h
@@ -0,0 +1,69 @@
+/*****************************************************************************
+ * WARNING
+ *
+ * THIS CODE IS CREATED FOR EXPERIMENTATION AND EDUCATIONAL USE ONLY.
+ *
+ * USAGE OF THIS CODE IN OTHER WAYS MAY INFRINGE UPON THE INTELLECTUAL
+ * PROPERTY OF OTHER PARTIES, SUCH AS INSIDE SECURE AND HID GLOBAL,
+ * AND MAY EXPOSE YOU TO AN INFRINGEMENT ACTION FROM THOSE PARTIES.
+ *
+ * THIS CODE SHOULD NEVER BE USED TO INFRINGE PATENTS OR INTELLECTUAL PROPERTY RIGHTS.
+ *
+ *****************************************************************************
+ *
+ * This file is part of loclass. 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, or, at your option, any later version.
+ *
+ * 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 loclass.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *
+ ****************************************************************************/
+
+#ifndef IKEYS_H
+#define IKEYS_H
+
+#include <inttypes.h>
+
+/**
+ * @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[8]);
+/**
+ * @brief Performs Elite-class key diversification
+ * @param csn
+ * @param key
+ * @param div_key
+ */
+
+void diversifyKey(uint8_t csn[8], uint8_t key[8], uint8_t div_key[8]);
+/**
+ * @brief Permutes a key from standard NIST format to Iclass specific format
+ * @param key
+ * @param dest
+ */
+
+#endif // IKEYS_H
diff --git a/client/src/cmdhficlass.c b/client/src/cmdhficlass.c
index 1eda5678b..62ba0f70a 100644
--- a/client/src/cmdhficlass.c
+++ b/client/src/cmdhficlass.c
@@ -1158,28 +1158,22 @@ static bool select_only(uint8_t *CSN, uint8_t *CCNR, bool use_credit_key, bool v
 }
 
 static bool select_and_auth(uint8_t *KEY, uint8_t *MAC, uint8_t *div_key, bool use_credit_key, bool elite, bool rawkey, bool verbose) {
-    uint8_t CSN[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
-    uint8_t CCNR[12] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
-
-    if (select_only(CSN, CCNR, use_credit_key, verbose) == false) {
-        if (verbose) PrintAndLogEx(FAILED, "selecting tag failed");
-//        DropField();
-        return false;
-    }
-
-    //get div_key
-    if (rawkey)
-        memcpy(div_key, KEY, 8);
-    else
-        HFiClassCalcDivKey(CSN, KEY, div_key, elite);
-
-    if (verbose) PrintAndLogEx(SUCCESS, "authing with %s: %s", rawkey ? "raw key" : "diversified key", sprint_hex(div_key, 8));
-
-    doMAC(CCNR, div_key, MAC);
-
+  
+    struct {
+        uint8_t key[8];
+        bool use_raw;
+        bool use_elite;
+        bool use_credit_key;
+    } PACKED payload;
+    
+    memcpy(payload.key, KEY, 8);
+    payload.use_raw = rawkey;
+    payload.use_elite = elite;
+    payload.use_credit_key = use_credit_key;
+    
+    SendCommandNG(CMD_HF_ICLASS_AUTH, (uint8_t*)&payload, sizeof(payload));
     PacketResponseNG resp;
-    clearCommandBuffer();
-    SendCommandNG(CMD_HF_ICLASS_AUTH, MAC, 4);
+    clearCommandBuffer();   
     if (WaitForResponseTimeout(CMD_HF_ICLASS_AUTH, &resp, 2000) == 0) {
         if (verbose) PrintAndLogEx(WARNING, "Command execute timeout");
         return false;
@@ -1190,12 +1184,25 @@ static bool select_and_auth(uint8_t *KEY, uint8_t *MAC, uint8_t *div_key, bool u
         return false;
     }
 
-    uint8_t isOK = resp.data.asBytes[0];
-    if (isOK == 0) {
+    struct p {
+        bool isOK;
+        uint8_t div_key[8];
+        uint8_t mac[4];
+    } PACKED;
+    struct p *packet = (struct p *)resp.data.asBytes;
+    
+    if (packet->isOK == 0) {
         if (verbose) PrintAndLogEx(FAILED, "authentication error");
         return false;
     }
-
+    
+    if (div_key)
+        memcpy(div_key, packet->div_key, sizeof(packet->div_key));
+    
+    if (MAC)
+        memcpy(MAC, packet->mac, sizeof(packet->mac));
+    
+    if (verbose) PrintAndLogEx(SUCCESS, "authing with %s: %s", rawkey ? "raw key" : "diversified key", sprint_hex(div_key, 8));
     return true;
 }
 
@@ -2972,7 +2979,7 @@ int readIclass(bool loop, bool verbose) {
 
             uint8_t readStatus = resp.oldarg[0] & 0xff;
 
-            PrintAndLogEx(NORMAL, "ICE: %x", readStatus);
+//            PrintAndLogEx(NORMAL, "ICE: %x", readStatus);
 
             // no tag found or button pressed
             if ((readStatus == 0 && !loop) || readStatus == 0xFF) {