diff --git a/CHANGELOG.md b/CHANGELOG.md
index 085778f36..aa81e6baa 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -434,6 +434,7 @@ This project uses the changelog in accordance with [keepchangelog](http://keepac
  - Added `hf mf mad` and `hf mfp mad` MAD decode, check and print commands (@merlokk)
  - Added `script run luxeodump` (@0xdrrb)
  - Fix `lf hitag reader 02` - print all bytes (@bosb)
+ - Fix hitag S simulation (still not working), write, add example HITAG S 256  (@bosb)
 
 
 ### Fixed
diff --git a/appveyor.yml b/appveyor.yml
index b2ab84fb4..230bb44bf 100644
--- a/appveyor.yml
+++ b/appveyor.yml
@@ -24,7 +24,7 @@ init:
     Add-AppveyorMessage -Message "[$env:APPVEYOR_REPO_COMMIT_SHORT]$env:appveyor_repo_name($env:APPVEYOR_REPO_BRANCH)" -Category Information -Details "repository: $env:appveyor_repo_name branch: $env:APPVEYOR_REPO_BRANCH release: $releasename"
 
 
-    iex ((new-object net.webclient).DownloadString('https://raw.githubusercontent.com/appveyor/ci/master/scripts/enable-rdp.ps1'))
+    # iex ((new-object net.webclient).DownloadString('https://raw.githubusercontent.com/appveyor/ci/master/scripts/enable-rdp.ps1'))
 clone_script:
 - ps: >-
     Write-Host "Removing ProxSpace..." -NoNewLine
@@ -409,4 +409,4 @@ on_success:
 on_failure:
 - ps: Write-Host "Build error." -ForegroundColor Red
 on_finish:
-- ps: $blockRdp = $false; iex ((new-object net.webclient).DownloadString('https://raw.githubusercontent.com/appveyor/ci/master/scripts/enable-rdp.ps1'))
+- ps: # $blockRdp = $false; iex ((new-object net.webclient).DownloadString('https://raw.githubusercontent.com/appveyor/ci/master/scripts/enable-rdp.ps1'))
diff --git a/armsrc/Standalone/lf_icehid.c b/armsrc/Standalone/lf_icehid.c
index d66186c9d..d68f8423d 100644
--- a/armsrc/Standalone/lf_icehid.c
+++ b/armsrc/Standalone/lf_icehid.c
@@ -105,7 +105,7 @@ uint32_t IceEM410xdemod() {
     memset(entry, 0, sizeof(entry));
 
     if (size == 128) {
-        sprintf((char *)entry, "EM XL TAG ID: %06lx%08lx%08lx - (%05ld_%03ld_%08ld)\n",
+        sprintf((char *)entry, "EM XL TAG ID: %06"PRIx32"%08"PRIx32"%08"PRIx32" - (%05"PRIu32"_%03"PRIu32"_%08"PRIu32")\n",
                 hi,
                 (uint32_t)(lo >> 32),
                 (uint32_t)lo,
@@ -113,7 +113,7 @@ uint32_t IceEM410xdemod() {
                 (uint32_t)((lo >> 16LL) & 0xFF),
                 (uint32_t)(lo & 0xFFFFFF));
     } else {
-        sprintf((char *)entry, "EM TAG ID: %02lx%08lx - (%05ld_%03ld_%08ld)\n",
+        sprintf((char *)entry, "EM TAG ID: %02"PRIx32"%08"PRIx32" - (%05"PRIu32"_%03"PRIu32"_%08"PRIu32")\n",
                 (uint32_t)(lo >> 32),
                 (uint32_t)lo,
                 (uint32_t)(lo & 0xFFFF),
@@ -160,16 +160,16 @@ uint32_t IceAWIDdemod() {
         uint8_t fac = bytebits_to_byte(dest + 9, 8);
         uint32_t cardnum = bytebits_to_byte(dest + 17, 16);
         uint32_t code1 = bytebits_to_byte(dest + 8, fmtLen);
-        sprintf((char *)entry, "AWID bit len: %d, FC: %d, Card: %ld - Wiegand: %lx, Raw: %08lx%08lx%08lx\n", fmtLen, fac, cardnum, code1, rawHi2, rawHi, rawLo);
+        sprintf((char *)entry, "AWID bit len: %d, FC: %d, Card: %"PRIu32" - Wiegand: %"PRIx32", Raw: %08"PRIx32"%08"PRIx32"%08"PRIx32"\n", fmtLen, fac, cardnum, code1, rawHi2, rawHi, rawLo);
     } else {
         uint32_t cardnum = bytebits_to_byte(dest + 8 + (fmtLen - 17), 16);
         if (fmtLen > 32) {
             uint32_t code1 = bytebits_to_byte(dest + 8, fmtLen - 32);
             uint32_t code2 = bytebits_to_byte(dest + 8 + (fmtLen - 32), 32);
-            sprintf((char *)entry, "AWID bit len: %d -unk bit len - Card: %ld - Wiegand: %lx%08lx, Raw: %08lx%08lx%08lx\n", fmtLen, cardnum, code1, code2, rawHi2, rawHi, rawLo);
+            sprintf((char *)entry, "AWID bit len: %d -unk bit len - Card: %"PRIu32" - Wiegand: %"PRIx32"%08"PRIx32", Raw: %08"PRIx32"%08"PRIx32"%08"PRIx32"\n", fmtLen, cardnum, code1, code2, rawHi2, rawHi, rawLo);
         } else {
             uint32_t code1 = bytebits_to_byte(dest + 8, fmtLen);
-            sprintf((char *)entry, "AWID bit len: %d -unk bit len - Card: %ld - Wiegand: %lx, Raw: %08lx%08lx%08lx\n", fmtLen, cardnum, code1, rawHi2, rawHi, rawLo);
+            sprintf((char *)entry, "AWID bit len: %d -unk bit len - Card: %"PRIu32" - Wiegand: %"PRIx32", Raw: %08"PRIx32"%08"PRIx32"%08"PRIx32"\n", fmtLen, cardnum, code1, rawHi2, rawHi, rawLo);
         }
     }
 
@@ -209,7 +209,7 @@ uint32_t IceIOdemod() {
     uint8_t entry[64];
     memset(entry, 0, sizeof(entry));
 
-    sprintf((char *)entry, "IO Prox XSF(%02d)%02x:%05d (%08lx%08lx)\n"
+    sprintf((char *)entry, "IO Prox XSF(%02u)%02x:%05u (%08"PRIx32"%08"PRIx32")\n"
             , version
             , facilitycode
             , number
@@ -249,7 +249,7 @@ uint32_t IceHIDDemod() {
         // go over previously decoded manchester data and decode into usable tag ID
         if (hi2 != 0) { //extra large HID tags  88/192 bits
 
-            sprintf((char *)entry, "HID large: %lx%08lx%08lx (%ld)\n",
+            sprintf((char *)entry, "HID large: %"PRIx32"%08"PRIx32"%08"PRIx32" (%"PRIu32")\n",
                     hi2,
                     hi,
                     lo,
@@ -296,7 +296,7 @@ uint32_t IceHIDDemod() {
                 fac = ((hi & 0xF) << 12) | (lo >> 20);
             }
 
-            sprintf((char *)entry, "HID: %lx%08lx (%ld) Format: %d bit FC: %ld Card: %ld\n",
+            sprintf((char *)entry, "HID: %"PRIx32"%08"PRIx32" (%"PRIu32") Format: %d bit FC: %"PRIu32" Card: %"PRIu32"\n",
                     hi,
                     lo,
                     (lo >> 1) & 0xFFFF,
diff --git a/armsrc/hitag2.c b/armsrc/hitag2.c
index 2df49ac3d..7e4c7ca14 100644
--- a/armsrc/hitag2.c
+++ b/armsrc/hitag2.c
@@ -544,7 +544,7 @@ bool hitag1_authenticate(uint8_t *rx, const size_t rxlen, uint8_t *tx, size_t *t
                 // will receive 32-bit configuration page
             } else if (bSelecting) {
                 // Initiate auth
-                tx[0] = 0xa0 | key_no >> 4; // WRCPAGE
+                tx[0] = 0xa0 | (key_no); // WRCPAGE
                 tx[1] = blocknr << 4;
                 crc = hitag_crc(tx, 12);
                 tx[1] |= crc >> 4;
diff --git a/armsrc/hitag2crack.c b/armsrc/hitag2crack.c
index 2d0d868b9..d66f740bc 100644
--- a/armsrc/hitag2crack.c
+++ b/armsrc/hitag2crack.c
@@ -300,7 +300,7 @@ bool hitag2crack_read_page(uint8_t *responsestr, uint8_t pagenum, uint8_t *nrar,
     uint8_t response[32];
     int i;
 
-    if ((pagenum < 0) || (pagenum > 7)) {
+    if (pagenum > 7) {
         UserMessage("hitag2crack_read_page:\r\n invalid pagenum\r\n");
         return false;
     }
@@ -351,7 +351,7 @@ bool hitag2crack_read_page(uint8_t *responsestr, uint8_t pagenum, uint8_t *nrar,
 // cmd is the binarray of the encrypted command to send;
 // len is the length of the encrypted command.
 bool hitag2crack_send_e_cmd(uint8_t *responsestr, uint8_t *nrar, uint8_t *cmd, int len) {
-    uint8_t tmp[37];
+//    uint8_t tmp[37];
     uint8_t uid[9];
     uint8_t e_page3str[9];
     int ret = 0;
@@ -551,14 +551,14 @@ bool hitag2_keystream(uint8_t *response, uint8_t *nrarhex) {
     uint8_t uid[32];
     uint8_t nrar[64];
     uint8_t e_firstcmd[10];
-    uint8_t e_page0cmd[10];
+//    uint8_t e_page0cmd[10];
 //    uint8_t keybits[2080];
     uint8_t *keybits = DataBuff;
     uint8_t keybitshex[67];
     int kslen;
     int ksoffset;
-    uint8_t pagehex[9];
-    uint8_t temp[20];
+//    uint8_t pagehex[9];
+//    uint8_t temp[20];
     int i;
     uint8_t *spaceptr = NULL;
 
diff --git a/armsrc/hitag2crack.h b/armsrc/hitag2crack.h
index 75fa54546..af3e41d14 100644
--- a/armsrc/hitag2crack.h
+++ b/armsrc/hitag2crack.h
@@ -19,9 +19,9 @@ bool hitag2crack_tx_rx(uint8_t *responsestr, uint8_t *msg, int len, int state, b
 
 bool hitag2crack_rng_init(uint8_t *response, uint8_t *input);
 bool hitag2crack_decrypt_hex(uint8_t *response, uint8_t *hex);
-bool hitag2crack_decrypt_bin(uint8_t *response, uint8_t *hex);
+bool hitag2crack_decrypt_bin(uint8_t *response, uint8_t *e_binstr);
 bool hitag2crack_encrypt_hex(uint8_t *response, uint8_t *hex);
-bool hitag2crack_encrypt_bin(uint8_t *response, uint8_t *hex);
+bool hitag2crack_encrypt_bin(uint8_t *response, uint8_t *e_binstr);
 
 bool hitag2_keystream(uint8_t *response, uint8_t *nrarhex);
 bool hitag2crack_send_auth(uint8_t *nrar);
diff --git a/armsrc/hitagS.c b/armsrc/hitagS.c
index 3b4f05eac..07b445434 100644
--- a/armsrc/hitagS.c
+++ b/armsrc/hitagS.c
@@ -10,6 +10,7 @@
 //-----------------------------------------------------------------------------
 // Some code was copied from Hitag2.c
 //-----------------------------------------------------------------------------
+// bosb 2020
 
 #include "hitagS.h"
 
@@ -23,6 +24,7 @@
 #include "string.h"
 #include "commonutil.h"
 #include "hitag2_crypto.h"
+#include "lfadc.h"
 
 #define CRC_PRESET 0xFF
 #define CRC_POLYNOM 0x1D
@@ -50,6 +52,22 @@ size_t blocknr;
 bool end = false;
 //#define SENDBIT_TEST
 
+/* array index 3 2 1 0 // bytes in sim.bin file are 0 1 2 3 
+// UID is 0 1 2 3 // tag.uid is 3210 
+// datasheet HitagS_V11.pdf bytes in tables printed 3 2 1 0
+
+#db# UID: 5F C2 11 84
+#db# conf0: C9 conf1: 00 conf2: 00
+                3  2  1  0
+#db# Page[ 0]: 84 11 C2 5F uid
+#db# Page[ 1]: AA 00 00 C9 conf, HITAG S 256
+#db# Page[ 2]: 4E 4F 54 48
+#db# Page[ 3]: 52 4B 49 4D
+#db# Page[ 4]: 00 00 00 00
+#db# Page[ 5]: 00 00 00 00
+#db# Page[ 6]: 00 00 00 00
+#db# Page[ 7]: 4B 4F 5F 57 */
+
 #define ht2bs_4a(a,b,c,d)   (~(((a|b)&c)^(a|d)^b))
 #define ht2bs_4b(a,b,c,d)   (~(((d|c)&(a^b))^(d|a|b)))
 #define ht2bs_5c(a,b,c,d,e) (~((((((c^e)|d)&a)^b)&(c^b))^(((d^e)|a)&((d^b)|c))))
@@ -86,7 +104,7 @@ bool end = false;
 #define HITAG_T_TAG_CAPTURE_THREE_HALF 41
 #define HITAG_T_TAG_CAPTURE_FOUR_HALF  57
 
-#define DEBUG 0
+#define DBGLEVEL 0
 
 /*
  * Implementation of the crc8 calculation from Hitag S
@@ -207,6 +225,22 @@ static void hitag_send_bit(int bit) {
 }
 
 static void hitag_send_frame(const uint8_t *frame, size_t frame_len) {
+    if (DBGLEVEL >= DBG_EXTENDED)
+        Dbprintf("hitag_send_frame: (%i) %02X %02X %02X %02X", frame_len, frame[0], frame[1], frame[2], frame[3]);
+    // The beginning of the frame is hidden in some high level; pause until our bits will have an effect
+    AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG;
+    HIGH(GPIO_SSC_DOUT);
+    switch (m) {
+        case AC4K:
+        case MC8K:
+            while (AT91C_BASE_TC0->TC_CV < T0 * 40) {}; //FADV
+            break;
+        case AC2K:
+        case MC4K:
+            while (AT91C_BASE_TC0->TC_CV < T0 * 20) {}; //STD + ADV
+            break;
+    }
+
     // SOF - send start of frame
     for (size_t i = 0; i < sof_bits; i++) {
         hitag_send_bit(1);
@@ -299,6 +333,25 @@ static int check_select(uint8_t *rx, uint32_t uid) {
     return 0;
 }
 
+void hitagS_set_frame_modulation() {
+    switch (tag.mode) {
+        case HT_STANDARD:
+            sof_bits = 1;
+            m = MC4K;
+            break;
+        case HT_ADVANCED:
+            sof_bits = 6;
+            m = MC4K;
+            break;
+        case HT_FAST_ADVANCED:
+            sof_bits = 6;
+            m = MC8K;
+            break;
+        default:
+            break;
+    }
+}
+
 /*
  * handles all commands from a reader
  */
@@ -318,20 +371,28 @@ static void hitagS_handle_reader_command(uint8_t *rx, const size_t rxlen,
     switch (rxlen) {
         case 5: {
             //UID request with a selected response protocol mode
+            if (DBGLEVEL >= DBG_EXTENDED)
+                Dbprintf("UID request: length: %i first byte: %02x", rxlen, rx[0]);
             tag.pstate = HT_READY;
             tag.tstate = HT_NO_OP;
             if ((rx[0] & 0xf0) == 0x30) {
+                if (DBGLEVEL >= DBG_EXTENDED)
+                    Dbprintf("HT_STANDARD");
                 tag.mode = HT_STANDARD;
                 sof_bits = 1;
                 m = AC2K;
             }
             if ((rx[0] & 0xf0) == 0xc0) {
                 tag.mode = HT_ADVANCED;
+                if (DBGLEVEL >= DBG_EXTENDED)
+                    Dbprintf("HT_ADVANCED");
                 sof_bits = 3;
                 m = AC2K;
             }
 
             if ((rx[0] & 0xf0) == 0xd0) {
+                if (DBGLEVEL >= DBG_EXTENDED)
+                    Dbprintf("HT_FAST_ADVANCED");
                 tag.mode = HT_FAST_ADVANCED;
                 sof_bits = 3;
                 m = AC4K;
@@ -344,29 +405,18 @@ static void hitagS_handle_reader_command(uint8_t *rx, const size_t rxlen,
         break;
         case 45: {
             //select command from reader received
+            if (DBGLEVEL >= DBG_EXTENDED)
+                DbpString("SELECT");
             if (check_select(rx, tag.uid) == 1) {
+                if (DBGLEVEL >= DBG_EXTENDED)
+                    DbpString("SELECT match");
                 //if the right tag was selected
                 *txlen = 32;
-                switch (tag.mode) {
-                    case HT_STANDARD:
-                        sof_bits = 1;
-                        m = MC4K;
-                        break;
-                    case HT_ADVANCED:
-                        sof_bits = 6;
-                        m = MC4K;
-                        break;
-                    case HT_FAST_ADVANCED:
-                        sof_bits = 6;
-                        m = MC8K;
-                        break;
-                    default:
-                        break;
-                }
+                hitagS_set_frame_modulation();
 
                 //send configuration
                 for (int i = 0; i < 4; i++)
-                    tx[i] = (tag.pages[0][1] >> (i * 8)) & 0xff;
+                    tx[i] = tag.pages[1][i];
                 tx[3] = 0xff;
                 if (tag.mode != HT_STANDARD) {
                     *txlen = 40;
@@ -390,22 +440,7 @@ static void hitagS_handle_reader_command(uint8_t *rx, const size_t rxlen,
             Dbprintf(",{0x%02X, 0x%02X, 0x%02X, 0x%02X, 0x%02X, 0x%02X, 0x%02X, 0x%02X}",
                      rx[0], rx[1], rx[2], rx[3], rx[4], rx[5], rx[6], rx[7]);
 
-            switch (tag.mode) {
-                case HT_STANDARD:
-                    sof_bits = 1;
-                    m = MC4K;
-                    break;
-                case HT_ADVANCED:
-                    sof_bits = 6;
-                    m = MC4K;
-                    break;
-                case HT_FAST_ADVANCED:
-                    sof_bits = 6;
-                    m = MC8K;
-                    break;
-                default:
-                    break;
-            }
+            hitagS_set_frame_modulation();
 
             for (int i = 0; i < 4; i++)
                 _hitag2_byte(&state);
@@ -438,7 +473,10 @@ static void hitagS_handle_reader_command(uint8_t *rx, const size_t rxlen,
              }
              */
         }
+        break;
         case 40:
+            if (DBGLEVEL >= DBG_EXTENDED)
+                Dbprintf("WRITE");
             //data received to be written
             if (tag.tstate == HT_WRITING_PAGE_DATA) {
                 tag.tstate = HT_NO_OP;
@@ -448,44 +486,14 @@ static void hitagS_handle_reader_command(uint8_t *rx, const size_t rxlen,
                 *txlen = 2;
                 tx[0] = 0x40;
                 page_to_be_written = 0;
-                switch (tag.mode) {
-                    case HT_STANDARD:
-                        sof_bits = 1;
-                        m = MC4K;
-                        break;
-                    case HT_ADVANCED:
-                        sof_bits = 6;
-                        m = MC4K;
-                        break;
-                    case HT_FAST_ADVANCED:
-                        sof_bits = 6;
-                        m = MC8K;
-                        break;
-                    default:
-                        break;
-                }
+                hitagS_set_frame_modulation();
             } else if (tag.tstate == HT_WRITING_BLOCK_DATA) {
                 tag.pages[page_to_be_written / 4][page_to_be_written % 4] = (rx[0]
                                                                             << 24) + (rx[1] << 16) + (rx[2] << 8) + rx[3];
                 //send ack
                 *txlen = 2;
                 tx[0] = 0x40;
-                switch (tag.mode) {
-                    case HT_STANDARD:
-                        sof_bits = 1;
-                        m = MC4K;
-                        break;
-                    case HT_ADVANCED:
-                        sof_bits = 6;
-                        m = MC4K;
-                        break;
-                    case HT_FAST_ADVANCED:
-                        sof_bits = 6;
-                        m = MC8K;
-                        break;
-                    default:
-                        break;
-                }
+                hitagS_set_frame_modulation();
                 page_to_be_written++;
                 block_data_left--;
                 if (block_data_left == 0) {
@@ -500,29 +508,14 @@ static void hitagS_handle_reader_command(uint8_t *rx, const size_t rxlen,
                 //send page data
                 uint8_t page = ((rx[0] & 0x0f) * 16) + ((rx[1] & 0xf0) / 16);
                 *txlen = 32;
-                tx[0] = (tag.pages[page / 4][page % 4]) & 0xff;
-                tx[1] = (tag.pages[page / 4][page % 4] >> 8) & 0xff;
-                tx[2] = (tag.pages[page / 4][page % 4] >> 16) & 0xff;
-                tx[3] = (tag.pages[page / 4][page % 4] >> 24) & 0xff;
+                tx[0] = tag.pages[page][0];
+                tx[1] = tag.pages[page][1];
+                tx[2] = tag.pages[page][2];
+                tx[3] = tag.pages[page][3];
                 if (tag.LKP && page == 1)
                     tx[3] = 0xff;
 
-                switch (tag.mode) {
-                    case HT_STANDARD:
-                        sof_bits = 1;
-                        m = MC4K;
-                        break;
-                    case HT_ADVANCED:
-                        sof_bits = 6;
-                        m = MC4K;
-                        break;
-                    case HT_FAST_ADVANCED:
-                        sof_bits = 6;
-                        m = MC8K;
-                        break;
-                    default:
-                        break;
-                }
+                hitagS_set_frame_modulation();
 
                 if (tag.mode != HT_STANDARD) {
                     //add crc8
@@ -543,29 +536,13 @@ static void hitagS_handle_reader_command(uint8_t *rx, const size_t rxlen,
                 *txlen = 32 * 4;
                 //send page,...,page+3 data
                 for (int i = 0; i < 4; i++) {
-                    tx[0 + i * 4] = (tag.pages[page / 4][page % 4]) & 0xff;
-                    tx[1 + i * 4] = (tag.pages[page / 4][page % 4] >> 8) & 0xff;
-                    tx[2 + i * 4] = (tag.pages[page / 4][page % 4] >> 16) & 0xff;
-                    tx[3 + i * 4] = (tag.pages[page / 4][page % 4] >> 24) & 0xff;
-                    page++;
+                    tx[0 + i * 4] = tag.pages[page + 0 + i * 4][0];
+                    tx[1 + i * 4] = tag.pages[page + 1 + i * 4][1];
+                    tx[2 + i * 4] = tag.pages[page + 2 + i * 4][2];
+                    tx[3 + i * 4] = tag.pages[page + 3 + i * 4][3];
                 }
 
-                switch (tag.mode) {
-                    case HT_STANDARD:
-                        sof_bits = 1;
-                        m = MC4K;
-                        break;
-                    case HT_ADVANCED:
-                        sof_bits = 6;
-                        m = MC4K;
-                        break;
-                    case HT_FAST_ADVANCED:
-                        sof_bits = 6;
-                        m = MC8K;
-                        break;
-                    default:
-                        break;
-                }
+                hitagS_set_frame_modulation();
 
                 if (tag.mode != HT_STANDARD) {
                     //add crc8
@@ -576,29 +553,12 @@ static void hitagS_handle_reader_command(uint8_t *rx, const size_t rxlen,
                     tx[16] = crc;
                 }
 
-                if ((page - 4) % 4 != 0 || (tag.LKP && (page - 4) == 0)) {
+                if ((page) % 4 != 0 || (tag.LKP && (page) == 0)) {
                     sof_bits = 0;
                     *txlen = 0;
                 }
             } else if ((rx[0] & 0xf0) == 0x80) { //write page
                 uint8_t page = ((rx[0] & 0x0f) * 16) + ((rx[1] & 0xf0) / 16);
-
-                switch (tag.mode) {
-                    case HT_STANDARD:
-                        sof_bits = 1;
-                        m = MC4K;
-                        break;
-                    case HT_ADVANCED:
-                        sof_bits = 6;
-                        m = MC4K;
-                        break;
-                    case HT_FAST_ADVANCED:
-                        sof_bits = 6;
-                        m = MC8K;
-                        break;
-                    default:
-                        break;
-                }
                 if ((tag.LCON && page == 1)
                         || (tag.LKP && (page == 2 || page == 3))) {
                     //deny
@@ -613,22 +573,7 @@ static void hitagS_handle_reader_command(uint8_t *rx, const size_t rxlen,
 
             } else if ((rx[0] & 0xf0) == 0x90) { //write block
                 uint8_t page = ((rx[0] & 0x0f) * 6) + ((rx[1] & 0xf0) / 16);
-                switch (tag.mode) {
-                    case HT_STANDARD:
-                        sof_bits = 1;
-                        m = MC4K;
-                        break;
-                    case HT_ADVANCED:
-                        sof_bits = 6;
-                        m = MC4K;
-                        break;
-                    case HT_FAST_ADVANCED:
-                        sof_bits = 6;
-                        m = MC8K;
-                        break;
-                    default:
-                        break;
-                }
+                hitagS_set_frame_modulation();
                 if (page % 4 != 0 || page == 0) {
                     //deny
                     *txlen = 0;
@@ -644,7 +589,8 @@ static void hitagS_handle_reader_command(uint8_t *rx, const size_t rxlen,
         }
         break;
         default:
-
+            if (DBGLEVEL >= DBG_EXTENDED)
+                Dbprintf("unknown rxlen: (%i) %02X %02X %02X %02X ...", rxlen, rx[0], rx[1], rx[2], rx[3]);
             break;
     }
 }
@@ -722,7 +668,7 @@ static int hitagS_handle_tag_auth(hitag_function htf, uint64_t key, uint64_t NrA
                        | (uid[30] << 1)
                        | uid[31];
 
-        if (DEBUG)
+        if (DBGLEVEL >= DBG_EXTENDED)
             Dbprintf("UID: %02X %02X %02X %02X", uid1, uid2, uid3, uid4);
 
         tag.uid = (uid4 << 24 | uid3 << 16 | uid2 << 8 | uid1);
@@ -816,7 +762,7 @@ static int hitagS_handle_tag_auth(hitag_function htf, uint64_t key, uint64_t NrA
         tag.LCK1 = response_bit[26];
         tag.LCK0 = response_bit[27];
 
-        if (DEBUG)
+        if (DBGLEVEL >= DBG_EXTENDED)
             Dbprintf("conf0: %02X conf1: %02X conf2: %02X", conf_pages[0], conf_pages[1], conf_pages[2]);
 
         if (tag.auth == 1) {
@@ -839,7 +785,7 @@ static int hitagS_handle_tag_auth(hitag_function htf, uint64_t key, uint64_t NrA
                     tx[5] = auth_ks[1];
                     tx[6] = auth_ks[2];
                     tx[7] = auth_ks[3];
-                    if (DEBUG)
+                    if (DBGLEVEL >= DBG_EXTENDED)
                         Dbprintf("%02X %02X %02X %02X %02X %02X %02X %02X", tx[0],
                                  tx[1], tx[2], tx[3], tx[4], tx[5], tx[6], tx[7]);
                 } else if (htf == 01 || htf == 03) { //RHTS_CHALLENGE //WHTS_CHALLENGE
@@ -864,7 +810,7 @@ static int hitagS_handle_tag_auth(hitag_function htf, uint64_t key, uint64_t NrA
         calc_crc(&crc, ((rx[1] & 0x0f) * 16 + ((rx[2] & 0xf0) / 16)), 8);
         calc_crc(&crc, ((rx[2] & 0x0f) * 16 + ((rx[3] & 0xf0) / 16)), 8);
         calc_crc(&crc, ((rx[3] & 0x0f) * 16 + ((rx[4] & 0xf0) / 16)), 8);
-        if (DEBUG) {
+        if (DBGLEVEL >= DBG_EXTENDED) {
             Dbprintf("UID:::%X", tag.uid);
             Dbprintf("RND:::%X", rnd);
         }
@@ -884,7 +830,7 @@ static int hitagS_handle_tag_auth(hitag_function htf, uint64_t key, uint64_t NrA
                 pwdl1 = ((rx[3] & 0x0f) * 16 + ((rx[4] & 0xf0) / 16)) ^ _hitag2_byte(&state);
             }
 
-            if (DEBUG)
+            if (DBGLEVEL >= DBG_EXTENDED)
                 Dbprintf("pwdh0 %02X pwdl0 %02X pwdl1 %02X", pwdh0, pwdl0, pwdl1);
 
             //Dbprintf("%X %02X", rnd, ((rx[4] & 0x0f) * 16) + ((rx[5] & 0xf0) / 16));
@@ -930,28 +876,30 @@ void SimulateHitagSTag(bool tag_mem_supplied, uint8_t *data) {
     tag.pstate = HT_READY;
     tag.tstate = HT_NO_OP;
 
-    for (i = 0; i < 16; i++)
-        for (j = 0; j < 4; j++)
-            tag.pages[i][j] = 0x0;
-
     // read tag data into memory
     if (tag_mem_supplied) {
+        for (i = 0; i < 16; i++)
+            for (j = 0; j < 4; j++)
+                tag.pages[i][j] = 0x0;
+
         DbpString("Loading hitagS memory...");
         memcpy((uint8_t *)tag.pages, data, 4 * 64);
+    } else {
+        // use the last read tag
     }
 
-    tag.uid = (uint32_t)tag.pages[0];
-    tag.key = (intptr_t)tag.pages[3];
+    tag.uid = (tag.pages[0][3] << 24 | tag.pages[0][2] << 16 | tag.pages[0][1] << 8 | tag.pages[0][0]);
+    tag.key = (tag.pages[3][3] << 24 | tag.pages[3][2] << 16 | tag.pages[3][1] << 8 | tag.pages[3][0]);
     tag.key <<= 16;
-    tag.key += ((tag.pages[2][0]) << 8) + tag.pages[2][1];
-    tag.pwdl0 = tag.pages[2][3];
-    tag.pwdl1 = tag.pages[2][2];
-    tag.pwdh0 = tag.pages[1][0];
+    tag.key += ((tag.pages[2][3]) << 8) + tag.pages[2][2];
+    tag.pwdl0 = tag.pages[2][0];
+    tag.pwdl1 = tag.pages[2][1];
+    tag.pwdh0 = tag.pages[1][3];
     //con0
     tag.max_page = 64;
-    if ((tag.pages[1][3] & 0x2) == 0 && (tag.pages[1][3] & 0x1) == 1)
+    if ((tag.pages[1][0] & 0x2) == 0 && (tag.pages[1][0] & 0x1) == 1)
         tag.max_page = 8;
-    if ((tag.pages[1][3] & 0x2) == 0 && (tag.pages[1][3] & 0x1) == 0)
+    if ((tag.pages[1][0] & 0x2) == 0 && (tag.pages[1][0] & 0x1) == 0)
         tag.max_page = 0;
     if (DBGLEVEL >= DBG_EXTENDED)
         for (i = 0; i < tag.max_page; i++)
@@ -962,41 +910,42 @@ void SimulateHitagSTag(bool tag_mem_supplied, uint8_t *data) {
                      tag.pages[i][0] & 0xff);
     //con1
     tag.auth = 0;
-    if ((tag.pages[1][2] & 0x80) == 0x80)
+    if ((tag.pages[1][1] & 0x80) == 0x80)
         tag.auth = 1;
     tag.LCON = 0;
-    if ((tag.pages[1][2] & 0x2) == 0x02)
+    if ((tag.pages[1][1] & 0x2) == 0x02)
         tag.LCON = 1;
     tag.LKP = 0;
-    if ((tag.pages[1][2] & 0x1) == 0x01)
+    if ((tag.pages[1][1] & 0x1) == 0x01)
         tag.LKP = 1;
     //con2
     //0=read write 1=read only
     tag.LCK7 = 0;
-    if ((tag.pages[1][1] & 0x80) == 0x80)
+    if ((tag.pages[1][2] & 0x80) == 0x80)
         tag.LCK7 = 1;
     tag.LCK6 = 0;
-    if ((tag.pages[1][1] & 0x40) == 0x040)
+    if ((tag.pages[1][2] & 0x40) == 0x040)
         tag.LCK6 = 1;
     tag.LCK5 = 0;
-    if ((tag.pages[1][1] & 0x20) == 0x20)
+    if ((tag.pages[1][2] & 0x20) == 0x20)
         tag.LCK5 = 1;
     tag.LCK4 = 0;
-    if ((tag.pages[1][1] & 0x10) == 0x10)
+    if ((tag.pages[1][2] & 0x10) == 0x10)
         tag.LCK4 = 1;
     tag.LCK3 = 0;
-    if ((tag.pages[1][1] & 0x8) == 0x08)
+    if ((tag.pages[1][2] & 0x8) == 0x08)
         tag.LCK3 = 1;
     tag.LCK2 = 0;
-    if ((tag.pages[1][1] & 0x4) == 0x04)
+    if ((tag.pages[1][2] & 0x4) == 0x04)
         tag.LCK2 = 1;
     tag.LCK1 = 0;
-    if ((tag.pages[1][1] & 0x2) == 0x02)
+    if ((tag.pages[1][2] & 0x2) == 0x02)
         tag.LCK1 = 1;
     tag.LCK0 = 0;
-    if ((tag.pages[1][1] & 0x1) == 0x01)
+    if ((tag.pages[1][2] & 0x1) == 0x01)
         tag.LCK0 = 1;
 
+
     // Set up simulator mode, frequency divisor which will drive the FPGA
     // and analog mux selection.
     FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
@@ -1028,7 +977,7 @@ void SimulateHitagSTag(bool tag_mem_supplied, uint8_t *data) {
     // TC1: Capture mode, default timer source = MCK/2 (TIMER_CLOCK1), TIOA is external trigger,
     // external trigger rising edge, load RA on rising edge of TIOA.
     AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK
-                             | AT91C_TC_ETRGEDG_RISING | AT91C_TC_ABETRG | AT91C_TC_LDRA_RISING;
+                              | AT91C_TC_ETRGEDG_RISING | AT91C_TC_ABETRG | AT91C_TC_LDRA_RISING;
 
     // Enable and reset counter
     AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
@@ -1100,12 +1049,13 @@ void SimulateHitagSTag(bool tag_mem_supplied, uint8_t *data) {
                 LogTrace(tx, nbytes(txlen), 0, 0, NULL, false);
             }
 
+            // Enable and reset external trigger in timer for capturing future frames
+            AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
+            
             // Reset the received frame and response timing info
             memset(rx, 0x00, sizeof(rx));
             response = 0;
 
-            // Enable and reset external trigger in timer for capturing future frames
-            AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
             LED_B_OFF();
         }
         // Reset the frame length
@@ -1114,22 +1064,92 @@ void SimulateHitagSTag(bool tag_mem_supplied, uint8_t *data) {
         overflow += (AT91C_BASE_TC1->TC_CV / T0);
         // Reset the timer to restart while-loop that receives frames
         AT91C_BASE_TC1->TC_CCR = AT91C_TC_SWTRG;
+
     }
 
-    LEDsoff();
-    FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
     set_tracing(false);
-    AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
-    AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS;
-
+    lf_finalize();
     // release allocated memory from BigBuff.
     BigBuf_free();
 
-    StartTicks();
-
     DbpString("Sim Stopped");
 }
 
+void hitagS_receive_frame(uint8_t *rx, size_t *rxlen, int *response) {
+
+    // Reset values for receiving frames
+    memset(rx, 0x00, HITAG_FRAME_LEN * sizeof(uint8_t));
+    *rxlen = 0;
+    int lastbit = 1;
+    bool bSkip = true;
+    int tag_sof = 1;
+    *response = 0;
+    uint32_t errorCount = 0;
+
+    // Receive frame, watch for at most T0*EOF periods
+    while (AT91C_BASE_TC1->TC_CV < T0 * HITAG_T_WAIT_MAX) {
+        // Check if falling edge in tag modulation is detected
+        if (AT91C_BASE_TC1->TC_SR & AT91C_TC_LDRAS) {
+            // Retrieve the new timing values
+            int ra = (AT91C_BASE_TC1->TC_RA / T0);
+
+            // Reset timer every frame, we have to capture the last edge for timing
+            AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG;
+
+            LED_B_ON();
+
+            // Capture tag frame (manchester decoding using only falling edges)
+            if (ra >= HITAG_T_EOF) {
+                if (*rxlen != 0) {
+                    //DbpString("wierd1?");
+                }
+                // Capture the T0 periods that have passed since last communication or field drop (reset)
+                // We always recieve a 'one' first, which has the falling edge after a half period |-_|
+                *response = ra - HITAG_T_TAG_HALF_PERIOD;
+            } else if (ra >= HITAG_T_TAG_CAPTURE_FOUR_HALF) {
+                // Manchester coding example |-_|_-|-_| (101)
+                rx[(*rxlen) / 8] |= 0 << (7 - ((*rxlen) % 8));
+                (*rxlen)++;
+                rx[(*rxlen) / 8] |= 1 << (7 - ((*rxlen) % 8));
+                (*rxlen)++;
+            } else if (ra >= HITAG_T_TAG_CAPTURE_THREE_HALF) {
+                // Manchester coding example |_-|...|_-|-_| (0...01)
+                rx[(*rxlen) / 8] |= 0 << (7 - ((*rxlen) % 8));
+                (*rxlen)++;
+                // We have to skip this half period at start and add the 'one' the second time
+                if (!bSkip) {
+                    rx[(*rxlen) / 8] |= 1 << (7 - ((*rxlen) % 8));
+                    (*rxlen)++;
+                }
+                lastbit = !lastbit;
+                bSkip = !bSkip;
+            } else if (ra >= HITAG_T_TAG_CAPTURE_TWO_HALF) {
+                // Manchester coding example |_-|_-| (00) or |-_|-_| (11)
+                if (tag_sof) {
+                    // Ignore bits that are transmitted during SOF
+                    tag_sof--;
+                } else {
+                    // bit is same as last bit
+                    rx[(*rxlen) / 8] |= lastbit << (7 - ((*rxlen) % 8));
+                    (*rxlen)++;
+                }
+            } else {
+                // Ignore wierd value, is to small to mean anything
+                errorCount++;
+            }
+        }
+
+        // if we saw over 100 wierd values break it probably isn't hitag...
+        if (errorCount > 100) break;
+
+        // We can break this loop if we received the last bit from a frame
+        if (AT91C_BASE_TC1->TC_CV > T0 * HITAG_T_EOF) {
+            if ((*rxlen) > 0)
+                break;
+        }
+    }
+}
+
 /*
  * Authenticates to the Tag with the given key or challenge.
  * If the key was given the password will be decrypted.
@@ -1149,7 +1169,6 @@ void ReadHitagS(hitag_function htf, hitag_data *htd) {
     uint8_t *tx = txbuf;
     size_t txlen = 0;
     int lastbit = 1;
-    int reset_sof = 1;
     int t_wait = HITAG_T_WAIT_MAX;
     bool bStop = false;
     int pageNum = 0;
@@ -1164,6 +1183,9 @@ void ReadHitagS(hitag_function htf, hitag_data *htd) {
     uint64_t NrAr = 0;
     uint8_t key_[6];
 
+    tag.pstate = HT_READY;
+    tag.tstate = HT_NO_OP;
+
     switch (htf) {
         case RHTSF_CHALLENGE: {
             DbpString("Authenticating using nr,ar pair:");
@@ -1238,10 +1260,8 @@ void ReadHitagS(hitag_function htf, hitag_data *htd) {
 
     // synchronized startup procedure
     while (AT91C_BASE_TC0->TC_CV > 0); // wait until TC0 returned to zero
-
     // Reset the received frame, frame count and timing info
     t_wait = 200;
-
     while (!bStop && !BUTTON_PRESS() && !data_available())  {
 
         WDT_HIT();
@@ -1381,85 +1401,12 @@ void ReadHitagS(hitag_function htf, hitag_data *htd) {
             LogTrace(tx, nbytes(txlen), HITAG_T_WAIT_2, 0, NULL, true);
         }
 
-        // Reset values for receiving frames
-        memset(rx, 0x00, sizeof(rx));
-        rxlen = 0;
-        lastbit = 1;
-        bool bSkip = true;
-        int tag_sof = reset_sof;
-        response = 0;
-
-        // Receive frame, watch for at most T0*EOF periods
-        while (AT91C_BASE_TC1->TC_CV < T0 * HITAG_T_WAIT_MAX) {
-            // Check if falling edge in tag modulation is detected
-            if (AT91C_BASE_TC1->TC_SR & AT91C_TC_LDRAS) {
-                // Retrieve the new timing values
-                int ra = (AT91C_BASE_TC1->TC_RA / T0);
-
-                // Reset timer every frame, we have to capture the last edge for timing
-                AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG;
-
-                LED_B_ON();
-
-                // Capture tag frame (manchester decoding using only falling edges)
-                if (ra >= HITAG_T_EOF) {
-                    if (rxlen != 0) {
-                        //DbpString("wierd1?");
-                    }
-                    // Capture the T0 periods that have passed since last communication or field drop (reset)
-                    // We always recieve a 'one' first, which has the falling edge after a half period |-_|
-                    response = ra - HITAG_T_TAG_HALF_PERIOD;
-                } else if (ra >= HITAG_T_TAG_CAPTURE_FOUR_HALF) {
-                    // Manchester coding example |-_|_-|-_| (101)
-                    rx[rxlen / 8] |= 0 << (7 - (rxlen % 8));
-                    rxlen++;
-                    rx[rxlen / 8] |= 1 << (7 - (rxlen % 8));
-                    rxlen++;
-                } else if (ra >= HITAG_T_TAG_CAPTURE_THREE_HALF) {
-                    // Manchester coding example |_-|...|_-|-_| (0...01)
-                    rx[rxlen / 8] |= 0 << (7 - (rxlen % 8));
-                    rxlen++;
-                    // We have to skip this half period at start and add the 'one' the second time
-                    if (!bSkip) {
-                        rx[rxlen / 8] |= 1 << (7 - (rxlen % 8));
-                        rxlen++;
-                    }
-                    lastbit = !lastbit;
-                    bSkip = !bSkip;
-                } else if (ra >= HITAG_T_TAG_CAPTURE_TWO_HALF) {
-                    // Manchester coding example |_-|_-| (00) or |-_|-_| (11)
-                    if (tag_sof) {
-                        // Ignore bits that are transmitted during SOF
-                        tag_sof--;
-                    } else {
-                        // bit is same as last bit
-                        rx[rxlen / 8] |= lastbit << (7 - (rxlen % 8));
-                        rxlen++;
-                    }
-                } else {
-                    // Ignore wierd value, is to small to mean anything
-                }
-            }
-
-            // We can break this loop if we received the last bit from a frame
-            if (AT91C_BASE_TC1->TC_CV > T0 * HITAG_T_EOF) {
-                if (rxlen > 0)
-                    break;
-            }
-        }
+        hitagS_receive_frame(rx, &rxlen, &response);
     }
     end = false;
-
-    LEDsoff();
-    FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
     set_tracing(false);
 
-    AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
-    AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS;
-    FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
-
-    StartTicks();
-
+    lf_finalize();
     reply_old(CMD_ACK, bSuccessful, 0, 0, 0, 0);
 }
 
@@ -1479,7 +1426,6 @@ void WritePageHitagS(hitag_function htf, hitag_data *htd, int page) {
     uint8_t *tx = txbuf;
     size_t txlen = 0;
     int lastbit;
-    int reset_sof;
     int t_wait = HITAG_T_WAIT_MAX;
     bool bStop;
     unsigned char crc;
@@ -1555,6 +1501,7 @@ void WritePageHitagS(hitag_function htf, hitag_data *htd, int page) {
     AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
 
     // Capture mode, defaul timer source = MCK/2 (TIMER_CLOCK1), TIOA is external trigger,
+    AT91C_BASE_TC0->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK;
     // external trigger rising edge, load RA on falling edge of TIOA.
     AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK
                              | AT91C_TC_ETRGEDG_FALLING
@@ -1570,7 +1517,6 @@ void WritePageHitagS(hitag_function htf, hitag_data *htd, int page) {
     // Reset the received frame, frame count and timing info
     lastbit = 1;
     bStop = false;
-    reset_sof = 1;
     t_wait = 200;
 
     while (!bStop && !BUTTON_PRESS() && !data_available()) {
@@ -1670,87 +1616,13 @@ void WritePageHitagS(hitag_function htf, hitag_data *htd, int page) {
             LogTrace(tx, nbytes(txlen), HITAG_T_WAIT_2, 0, NULL, true);
         }
 
-        // Reset values for receiving frames
-        memset(rx, 0x00, sizeof(rx));
-        rxlen = 0;
-        lastbit = 1;
-        bool bSkip = true;
-        int tag_sof = reset_sof;
-        response = 0;
-        uint32_t errorCount = 0;
+        hitagS_receive_frame(rx, &rxlen, &response);
 
-        // Receive frame, watch for at most T0*EOF periods
-        while (AT91C_BASE_TC1->TC_CV < T0 * HITAG_T_WAIT_MAX) {
-            // Check if falling edge in tag modulation is detected
-            if (AT91C_BASE_TC1->TC_SR & AT91C_TC_LDRAS) {
-                // Retrieve the new timing values
-                int ra = (AT91C_BASE_TC1->TC_RA / T0);
-
-                // Reset timer every frame, we have to capture the last edge for timing
-                AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG;
-
-                LED_B_ON();
-
-                // Capture tag frame (manchester decoding using only falling edges)
-                if (ra >= HITAG_T_EOF) {
-                    if (rxlen != 0) {
-                        //DbpString("wierd1?");
-                    }
-                    // Capture the T0 periods that have passed since last communication or field drop (reset)
-                    // We always recieve a 'one' first, which has the falling edge after a half period |-_|
-                    response = ra - HITAG_T_TAG_HALF_PERIOD;
-                } else if (ra >= HITAG_T_TAG_CAPTURE_FOUR_HALF) {
-                    // Manchester coding example |-_|_-|-_| (101)
-                    rx[rxlen / 8] |= 0 << (7 - (rxlen % 8));
-                    rxlen++;
-                    rx[rxlen / 8] |= 1 << (7 - (rxlen % 8));
-                    rxlen++;
-                } else if (ra >= HITAG_T_TAG_CAPTURE_THREE_HALF) {
-                    // Manchester coding example |_-|...|_-|-_| (0...01)
-                    rx[rxlen / 8] |= 0 << (7 - (rxlen % 8));
-                    rxlen++;
-                    // We have to skip this half period at start and add the 'one' the second time
-                    if (!bSkip) {
-                        rx[rxlen / 8] |= 1 << (7 - (rxlen % 8));
-                        rxlen++;
-                    }
-                    lastbit = !lastbit;
-                    bSkip = !bSkip;
-                } else if (ra >= HITAG_T_TAG_CAPTURE_TWO_HALF) {
-                    // Manchester coding example |_-|_-| (00) or |-_|-_| (11)
-                    if (tag_sof) {
-                        // Ignore bits that are transmitted during SOF
-                        tag_sof--;
-                    } else {
-                        // bit is same as last bit
-                        rx[rxlen / 8] |= lastbit << (7 - (rxlen % 8));
-                        rxlen++;
-                    }
-                } else {
-                    // Ignore wierd value, is to small to mean anything
-                    errorCount++;
-                }
-            }
-
-            // if we saw over 100 wierd values break it probably isn't hitag...
-            if (errorCount > 100) break;
-
-            // We can break this loop if we received the last bit from a frame
-            if (AT91C_BASE_TC1->TC_CV > T0 * HITAG_T_EOF) {
-                if (rxlen > 0)
-                    break;
-            }
-        }
     }
     end = false;
-    LEDsoff();
-    FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
     set_tracing(false);
 
-    AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
-    AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS;
-
-    StartTicks();
+    lf_finalize();
 
     reply_old(CMD_ACK, bSuccessful, 0, 0, 0, 0);
 }
@@ -1773,7 +1645,7 @@ void check_challenges(bool file_given, uint8_t *data) {
     size_t rxlen = 0;
     uint8_t txbuf[HITAG_FRAME_LEN];
     int t_wait = HITAG_T_WAIT_MAX;
-    int lastbit, reset_sof, STATE = 0;;
+    int lastbit, STATE = 0;;
     bool bStop;
     int response_bit[200];
     unsigned char mask = 1;
@@ -1834,7 +1706,6 @@ void check_challenges(bool file_given, uint8_t *data) {
     // Reset the received frame, frame count and timing info
     lastbit = 1;
     bStop = false;
-    reset_sof = 1;
     t_wait = 200;
 
     if (file_given) {
@@ -1984,85 +1855,10 @@ void check_challenges(bool file_given, uint8_t *data) {
             LogTrace(tx, nbytes(txlen), HITAG_T_WAIT_2, 0, NULL, true);
         }
 
-        // Reset values for receiving frames
-        memset(rx, 0x00, sizeof(rx));
-        rxlen = 0;
-        lastbit = 1;
-        bool bSkip = true;
-        int tag_sof = reset_sof;
-        response = 0;
-
-        // Receive frame, watch for at most T0*EOF periods
-        while (AT91C_BASE_TC1->TC_CV < T0 * HITAG_T_WAIT_MAX) {
-            // Check if falling edge in tag modulation is detected
-            if (AT91C_BASE_TC1->TC_SR & AT91C_TC_LDRAS) {
-                // Retrieve the new timing values
-                int ra = (AT91C_BASE_TC1->TC_RA / T0);
-
-                // Reset timer every frame, we have to capture the last edge for timing
-                AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG;
-
-                LED_B_ON();
-
-                // Capture tag frame (manchester decoding using only falling edges)
-                if (ra >= HITAG_T_EOF) {
-                    if (rxlen != 0) {
-                        //DbpString("wierd1?");
-                    }
-                    // Capture the T0 periods that have passed since last communication or field drop (reset)
-                    // We always recieve a 'one' first, which has the falling edge after a half period |-_|
-                    response = ra - HITAG_T_TAG_HALF_PERIOD;
-                } else if (ra >= HITAG_T_TAG_CAPTURE_FOUR_HALF) {
-                    // Manchester coding example |-_|_-|-_| (101)
-                    rx[rxlen / 8] |= 0 << (7 - (rxlen % 8));
-                    rxlen++;
-                    rx[rxlen / 8] |= 1 << (7 - (rxlen % 8));
-                    rxlen++;
-                } else if (ra >= HITAG_T_TAG_CAPTURE_THREE_HALF) {
-                    // Manchester coding example |_-|...|_-|-_| (0...01)
-                    rx[rxlen / 8] |= 0 << (7 - (rxlen % 8));
-                    rxlen++;
-                    // We have to skip this half period at start and add the 'one' the second time
-                    if (!bSkip) {
-                        rx[rxlen / 8] |= 1 << (7 - (rxlen % 8));
-                        rxlen++;
-                    }
-                    lastbit = !lastbit;
-                    bSkip = !bSkip;
-                } else if (ra >= HITAG_T_TAG_CAPTURE_TWO_HALF) {
-                    // Manchester coding example |_-|_-| (00) or |-_|-_| (11)
-                    if (tag_sof) {
-                        // Ignore bits that are transmitted during SOF
-                        tag_sof--;
-                    } else {
-                        // bit is same as last bit
-                        rx[rxlen / 8] |= lastbit << (7 - (rxlen % 8));
-                        rxlen++;
-                    }
-                } else {
-                    // Ignore wierd value, is to small to mean anything
-                }
-            }
-
-            // We can break this loop if we received the last bit from a frame
-            if (AT91C_BASE_TC1->TC_CV > T0 * HITAG_T_EOF) {
-                if (rxlen > 0)
-                    break;
-            }
-        }
+        hitagS_receive_frame(rx, &rxlen, &response);
     }
 
-    LEDsoff();
-    FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
     set_tracing(false);
-
-    AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
-    AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS;
-
-    StartTicks();
-
+    lf_finalize();
     reply_old(CMD_ACK, bSuccessful, 0, 0, 0, 0);
 }
-
-
-
diff --git a/client/cmdhficlass.c b/client/cmdhficlass.c
index 4e944c178..827cbcc38 100644
--- a/client/cmdhficlass.c
+++ b/client/cmdhficlass.c
@@ -785,7 +785,7 @@ static int CmdHFiClassDecrypt(const char *Cmd) {
                 return usage_hf_iclass_decrypt();
             case 'd':
                 if (param_gethex(Cmd, cmdp + 1, enc_data, 16)) {
-                    PrintAndLogEx(ERR, "data must be 16 HEX symbols");
+                    PrintAndLogEx(ERR, "Data must be 16 HEX symbols");
                     errors = true;
                     break;
                 }
@@ -794,7 +794,7 @@ static int CmdHFiClassDecrypt(const char *Cmd) {
                 break;
             case 'f':
                 if (param_getstr(Cmd, cmdp + 1, filename, sizeof(filename)) == 0) {
-                    PrintAndLogEx(WARNING, "no filename found after f");
+                    PrintAndLogEx(WARNING, "No filename found after f");
                     errors = true;
                     break;
                 }
@@ -864,18 +864,24 @@ static int CmdHFiClassDecrypt(const char *Cmd) {
 
         BLOCK79ENCRYPTION aa1_encryption = (decrypted[(6 * 8) + 7] & 0x03);
 
-        for (uint16_t blocknum = 0; blocknum < applimit; ++blocknum) {
+        uint32_t limit = MIN(applimit, decryptedlen / 8);
+
+        if (decryptedlen / 8 != applimit) {
+            PrintAndLogEx(WARNING, "Actual file len " _YELLOW_("%u") "vs HID app-limit len " _YELLOW_("%u"), decryptedlen, applimit * 8);
+            PrintAndLogEx(INFO, "Setting limit to " _GREEN_("%u"), limit * 8);
+        }
+        uint8_t numblocks4userid = GetNumberBlocksForUserId(decrypted + (6 * 8));
+
+        for (uint16_t blocknum = 0; blocknum < limit; ++blocknum) {
 
             uint8_t idx = blocknum * 8;
             memcpy(enc_data, decrypted + idx, 8);
 
-            // block 7 or higher,  and not empty 0xFF
-            // look inside block 6 to determine if aa1 is encrypted.
-            if (blocknum > 6 && memcmp(enc_data, empty, 8) != 0) {
-
-                if (aa1_encryption == RFU || aa1_encryption == None)
-                    continue;
-
+            if (aa1_encryption == RFU || aa1_encryption == None)
+                continue;
+                
+            // Decrypted block 7,8,9 if configured.
+            if (blocknum > 6 && blocknum <= 6 + numblocks4userid && memcmp(enc_data, empty, 8) != 0) {
                 if (use_sc) {
                     Decrypt(enc_data, decrypted + idx);
                 } else {
@@ -885,7 +891,12 @@ static int CmdHFiClassDecrypt(const char *Cmd) {
         }
 
         //Use the first block (CSN) for filename
-        char *fptr = calloc(42, sizeof(uint8_t));
+        char *fptr = calloc(50, sizeof(uint8_t));
+        if (!fptr) {
+            PrintAndLogEx(WARNING, "Failed to allocate memory");
+            free(decrypted);
+            return PM3_EMALLOC;
+        }
         strcat(fptr, "hf-iclass-");
         FillFileNameByUID(fptr, hdr->csn, "-data-decrypted", sizeof(hdr->csn));
 
@@ -893,9 +904,9 @@ static int CmdHFiClassDecrypt(const char *Cmd) {
         saveFileEML(fptr, decrypted, decryptedlen, 8);
         saveFileJSON(fptr, jsfIclass, decrypted, decryptedlen);
 
+        PrintAndLogEx(INFO, "Following output skips CSN / block0");
         printIclassDumpContents(decrypted, 1, (decryptedlen / 8), decryptedlen);
 
-
         // decode block 6
         if (memcmp(decrypted + (8 * 6), empty, 8) != 0) {
             if (use_sc) {
@@ -1251,7 +1262,7 @@ static int CmdHFiClassReader_Dump(const char *Cmd) {
         if (kbd_enter_pressed()) {
             PrintAndLogEx(WARNING, "\n[!] aborted via keyboard!\n");
             DropField();
-            return 0;
+            return PM3_EOPABORTED;
         }
 
         if (WaitForResponseTimeout(CMD_ACK, &resp, 2000))
@@ -1343,8 +1354,8 @@ static int CmdHFiClassReader_Dump(const char *Cmd) {
 
     // print the dump
     PrintAndLogEx(NORMAL, "");
-    PrintAndLogEx(NORMAL, "------+--+-------------------------+");
-    PrintAndLogEx(NORMAL, "CSN   |00| %s|", sprint_hex(tag_data, 8));
+    PrintAndLogEx(INFO, "------+--+-------------------------+");
+    PrintAndLogEx(INFO, "CSN   |00| %s|", sprint_hex(tag_data, 8));
     printIclassDumpContents(tag_data, 1, (gotBytes / 8), gotBytes);
 
     if (filename[0] == 0) {
diff --git a/client/cmdhflto.c b/client/cmdhflto.c
index dfbca68c1..c5b73c0c6 100644
--- a/client/cmdhflto.c
+++ b/client/cmdhflto.c
@@ -524,7 +524,7 @@ static int CmdHfLTODump(const char *Cmd) {
     return PM3_SUCCESS;
 }
 
-int restoreLTO(uint8_t *dump_data, bool verbose) {
+int restoreLTO(uint8_t *dump, bool verbose) {
 
     clearCommandBuffer();
     lto_switch_on_field();
@@ -545,13 +545,13 @@ int restoreLTO(uint8_t *dump_data, bool verbose) {
     for (uint8_t blk = 2; blk < 255; blk++) {
 
         for (int i = 0; i < 32; i++) {
-            blkData[i] = dump_data[i + blk * 32];
+            blkData[i] = dump[i + blk * 32];
         }
 
         ret_val = lto_wrbl(blk, blkData, verbose);
 
         if (ret_val == PM3_SUCCESS) {
-            PrintAndLogEx(SUCCESS, "BLK %03d: " _YELLOW_("write success"), blk);
+            PrintAndLogEx(SUCCESS, "Block %03d - " _YELLOW_("write success"), blk);
         } else {
             lto_switch_off_field();
             return ret_val;
diff --git a/client/cmdhfmfu.c b/client/cmdhfmfu.c
index 5387cc855..120db2a9c 100644
--- a/client/cmdhfmfu.c
+++ b/client/cmdhfmfu.c
@@ -62,19 +62,19 @@ static int usage_hf_mfu_info(void) {
 static int usage_hf_mfu_dump(void) {
     PrintAndLogEx(NORMAL, "Reads all pages from Ultralight, Ultralight-C, Ultralight EV1");
     PrintAndLogEx(NORMAL, "NTAG 203, NTAG 210, NTAG 212, NTAG 213, NTAG 215, NTAG 216");
-    PrintAndLogEx(NORMAL, "and saves binary dump into the file `filename.bin` or `cardUID.bin`");
+    PrintAndLogEx(NORMAL, "and saves binary dump into the file " _YELLOW_("`filename.bin`") "or " _YELLOW_("`cardUID.bin`") );
     PrintAndLogEx(NORMAL, "It autodetects card type.\n");
     PrintAndLogEx(NORMAL, "Usage:  hf mfu dump k <key> l f <filename w/o .bin> p <page#> q <#pages>");
     PrintAndLogEx(NORMAL, "  Options :");
     PrintAndLogEx(NORMAL, "  k <key> : (optional) key for authentication [UL-C 16bytes, EV1/NTAG 4bytes]");
     PrintAndLogEx(NORMAL, "  l       : (optional) swap entered key's endianness");
-    PrintAndLogEx(NORMAL, "  f <FN > : filename w/o .bin to save the dump as");
-    PrintAndLogEx(NORMAL, "  p <Pg > : starting Page number to manually set a page to start the dump at");
+    PrintAndLogEx(NORMAL, "  f <fn>  : " _YELLOW_("filename w/o .bin") "to save the dump as");
+    PrintAndLogEx(NORMAL, "  p <pg>  : starting Page number to manually set a page to start the dump at");
     PrintAndLogEx(NORMAL, "  q <qty> : number of Pages to manually set how many pages to dump");
     PrintAndLogEx(NORMAL, "");
     PrintAndLogEx(NORMAL, "Examples:");
     PrintAndLogEx(NORMAL, "       hf mfu dump");
-    PrintAndLogEx(NORMAL, "       hf mfu dump n myfile");
+    PrintAndLogEx(NORMAL, "       hf mfu dump f myfile");
     PrintAndLogEx(NORMAL, "       hf mfu dump k 00112233445566778899AABBCCDDEEFF");
     PrintAndLogEx(NORMAL, "       hf mfu dump k AABBCCDD\n");
     return PM3_SUCCESS;
@@ -82,14 +82,14 @@ static int usage_hf_mfu_dump(void) {
 
 static int usage_hf_mfu_restore(void) {
     PrintAndLogEx(NORMAL, "Restore dumpfile onto card.");
-    PrintAndLogEx(NORMAL, "Usage:  hf mfu restore [h] [l] [s] k <key> n <filename w/o .bin> ");
+    PrintAndLogEx(NORMAL, "Usage:  hf mfu restore [h] [l] [s] k <key> n <filename w .bin> ");
     PrintAndLogEx(NORMAL, "  Options :");
     PrintAndLogEx(NORMAL, "  k <key> : (optional) key for authentication [UL-C 16bytes, EV1/NTAG 4bytes]");
     PrintAndLogEx(NORMAL, "  l       : (optional) swap entered key's endianness");
-    PrintAndLogEx(NORMAL, "  s       : (optional) enable special write UID -MAGIC TAG ONLY-");
-    PrintAndLogEx(NORMAL, "  e       : (optional) enable special write version/signature -MAGIC NTAG 21* ONLY-");
-    PrintAndLogEx(NORMAL, "  r       : (optional) use the password found in dumpfile to configure tag. requires 'e' parameter to work");
-    PrintAndLogEx(NORMAL, "  f <FN>  : filename w/o .bin to restore");
+    PrintAndLogEx(NORMAL, "  s       : (optional) enable special write UID " _BLUE_("-MAGIC TAG ONLY-") );
+    PrintAndLogEx(NORMAL, "  e       : (optional) enable special write version/signature " _BLUE_("-MAGIC NTAG 21* ONLY-") );
+    PrintAndLogEx(NORMAL, "  r       : (optional) use the password found in dumpfile to configure tag. requires " _YELLOW_("'e'") "parameter to work");
+    PrintAndLogEx(NORMAL, "  f <fn>  : " _YELLOW_("filename w .bin") "to restore");
     PrintAndLogEx(NORMAL, "");
     PrintAndLogEx(NORMAL, "Examples:");
     PrintAndLogEx(NORMAL, "       hf mfu restore s f myfile");
@@ -129,8 +129,8 @@ static int usage_hf_mfu_wrbl(void) {
 }
 
 static int usage_hf_mfu_eload(void) {
-    PrintAndLogEx(NORMAL, "It loads emul dump from the file `filename.eml`");
-    PrintAndLogEx(NORMAL, "Hint: See script dumptoemul-mfu.lua to convert the .bin to the eml");
+    PrintAndLogEx(NORMAL, "It loads emul dump from the file " _YELLOW_("`filename.eml`") );
+    PrintAndLogEx(NORMAL, "Hint: See " _YELLOW_("`script run dumptoemul-mfu`") "to convert the .bin to the eml");
     PrintAndLogEx(NORMAL, "Usage:  hf mfu eload u <file name w/o `.eml`> [numblocks]");
     PrintAndLogEx(NORMAL, "  Options:");
     PrintAndLogEx(NORMAL, "    h          : this help");
@@ -174,6 +174,7 @@ static int usage_hf_mfu_ucauth(void) {
 }
 
 static int usage_hf_mfu_ucsetpwd(void) {
+    PrintAndLogEx(NORMAL, "Set 3DES password on Mifare Ultralight-C tag.");
     PrintAndLogEx(NORMAL, "Usage:  hf mfu setpwd <password (32 hex symbols)>");
     PrintAndLogEx(NORMAL, "       [password] - (32 hex symbols)");
     PrintAndLogEx(NORMAL, "");
@@ -186,7 +187,8 @@ static int usage_hf_mfu_ucsetpwd(void) {
 static int usage_hf_mfu_ucsetuid(void) {
     PrintAndLogEx(NORMAL, "Usage:  hf mfu setuid <uid (14 hex symbols)>");
     PrintAndLogEx(NORMAL, "       [uid] - (14 hex symbols)");
-    PrintAndLogEx(NORMAL, "\nThis only works for Magic Ultralight tags.");
+    PrintAndLogEx(NORMAL, "\n");
+    PrintAndLogEx(NORMAL, "This only works for " _BLUE_("Magic Ultralight") "tags.");
     PrintAndLogEx(NORMAL, "");
     PrintAndLogEx(NORMAL, "Examples:");
     PrintAndLogEx(NORMAL, "         hf mfu setuid 11223344556677");
@@ -223,7 +225,7 @@ static int usage_hf_mfu_pwdgen(void) {
 }
 
 static int usage_hf_mfu_otp_tearoff(void) {
-    PrintAndLogEx(NORMAL, "Tear-off test against OTP block on MFU tags - More help sooner or later\n");
+    PrintAndLogEx(NORMAL, "Tear-off test against OTP block on MFU tags.");
     PrintAndLogEx(NORMAL, "Usage:  hf mfu otptear [h]");
     PrintAndLogEx(NORMAL, "Options:");
     PrintAndLogEx(NORMAL, "    h       : this help");
@@ -527,7 +529,7 @@ static int ul_print_default(uint8_t *data) {
     uid[5] = data[6];
     uid[6] = data[7];
 
-    PrintAndLogEx(NORMAL, "       UID : %s ", sprint_hex(uid, 7));
+    PrintAndLogEx(NORMAL, "       UID : " _YELLOW_("%s"), sprint_hex(uid, 7));
     PrintAndLogEx(NORMAL, "    UID[0] : %02X, %s",  uid[0], getTagInfo(uid[0]));
     if (uid[0] == 0x05 && ((uid[1] & 0xf0) >> 4) == 2) {   // is infineon and 66RxxP
         uint8_t chip = (data[8] & 0xC7); // 11000111  mask, bit 3,4,5 RFU
@@ -546,17 +548,17 @@ static int ul_print_default(uint8_t *data) {
     // CT (cascade tag byte) 0x88 xor SN0 xor SN1 xor SN2
     int crc0 = 0x88 ^ uid[0] ^ uid[1] ^ uid[2];
     if (data[3] == crc0)
-        PrintAndLogEx(NORMAL, "      BCC0 : %02X, Ok", data[3]);
+        PrintAndLogEx(NORMAL, "      BCC0 : %02X ( " _GREEN_("ok") ")", data[3]);
     else
         PrintAndLogEx(NORMAL, "      BCC0 : %02X, crc should be %02X", data[3], crc0);
 
     int crc1 = uid[3] ^ uid[4] ^ uid[5] ^ uid[6];
     if (data[8] == crc1)
-        PrintAndLogEx(NORMAL, "      BCC1 : %02X, Ok", data[8]);
+        PrintAndLogEx(NORMAL, "      BCC1 : %02X ( " _GREEN_("ok") ")", data[8]);
     else
         PrintAndLogEx(NORMAL, "      BCC1 : %02X, crc should be %02X", data[8], crc1);
 
-    PrintAndLogEx(NORMAL, "  Internal : %02X, %sdefault", data[9], (data[9] == 0x48) ? "" : "not ");
+    PrintAndLogEx(NORMAL, "  Internal : %02X ( %s)", data[9], (data[9] == 0x48) ? _GREEN_("default") : _RED_("not default") );
 
     PrintAndLogEx(NORMAL, "      Lock : %s - %s",
                   sprint_hex(data + 10, 2),
@@ -648,10 +650,10 @@ static int ndef_print_CC(uint8_t *data) {
     PrintAndLogEx(NORMAL, "  Additional feature information");
     PrintAndLogEx(NORMAL, "  %02X", data[3]);
     PrintAndLogEx(NORMAL, "  00000000");
-    PrintAndLogEx(NORMAL, "  xxx      - %02X : RFU (%s)", msb3, (msb3 == 0) ? _GREEN_("OK") : _RED_("Fail"));
+    PrintAndLogEx(NORMAL, "  xxx      - %02X : RFU ( %s)", msb3, (msb3 == 0) ? _GREEN_("ok") : _RED_("fail"));
     PrintAndLogEx(NORMAL, "     x     - %02X : %s special frame", sf, (sf) ? "support" : "don\'t support");
     PrintAndLogEx(NORMAL, "      x    - %02X : %s lock block", lb, (lb) ? "support" : "don\'t support");
-    PrintAndLogEx(NORMAL, "       xx  - %02X : RFU (%s)", mlrule, (mlrule == 0) ? _GREEN_("OK") : _RED_("Fail"));
+    PrintAndLogEx(NORMAL, "       xx  - %02X : RFU ( %s)", mlrule, (mlrule == 0) ? _GREEN_("ok") : _RED_("fail"));
     PrintAndLogEx(NORMAL, "         x - %02X : IC %s multiple block reads", mbread, (mbread) ? "support" : "don\'t support");
     return PM3_SUCCESS;
 }
@@ -845,7 +847,7 @@ static int ulev1_print_configuration(uint32_t tagtype, uint8_t *data, uint8_t st
     if (authlim == 0)
         PrintAndLogEx(NORMAL, "                    - Unlimited password attempts");
     else
-        PrintAndLogEx(NORMAL, "                    - Max number of password attempts is %d", authlim);
+        PrintAndLogEx(NORMAL, "                    - Max number of password attempts is " _YELLOW_("%d"), authlim);
 
     PrintAndLogEx(NORMAL, "                    - NFC counter %s", (nfc_cnf_en) ? "enabled" : "disabled");
     PrintAndLogEx(NORMAL, "                    - NFC counter %s", (nfc_cnf_prot_pwd) ? "not protected" : "password protection enabled");
@@ -1276,7 +1278,7 @@ static int CmdHF14AMfUInfo(const char *Cmd) {
             num_to_bytes(ul_ev1_pwdgenA(card.uid), 4, key);
             len = ulev1_requestAuthentication(key, pack, sizeof(pack));
             if (len > -1) {
-                PrintAndLogEx(SUCCESS, "Found a default password:" _GREEN_("%s") " || Pack: %02X %02X", sprint_hex(key, 4), pack[0], pack[1]);
+                PrintAndLogEx(SUCCESS, "Found a default password: " _GREEN_("%s") "|| Pack: %02X %02X", sprint_hex(key, 4), pack[0], pack[1]);
                 goto out;
             }
 
@@ -1286,7 +1288,7 @@ static int CmdHF14AMfUInfo(const char *Cmd) {
             num_to_bytes(ul_ev1_pwdgenB(card.uid), 4, key);
             len = ulev1_requestAuthentication(key, pack, sizeof(pack));
             if (len > -1) {
-                PrintAndLogEx(SUCCESS, "Found a default password:" _GREEN_("%s") " || Pack: %02X %02X", sprint_hex(key, 4), pack[0], pack[1]);
+                PrintAndLogEx(SUCCESS, "Found a default password: " _GREEN_("%s") "|| Pack: %02X %02X", sprint_hex(key, 4), pack[0], pack[1]);
                 goto out;
             }
 
@@ -1296,7 +1298,7 @@ static int CmdHF14AMfUInfo(const char *Cmd) {
             num_to_bytes(ul_ev1_pwdgenC(card.uid), 4, key);
             len = ulev1_requestAuthentication(key, pack, sizeof(pack));
             if (len > -1) {
-                PrintAndLogEx(SUCCESS, "Found a default password:" _GREEN_("%s") " || Pack: %02X %02X", sprint_hex(key, 4), pack[0], pack[1]);
+                PrintAndLogEx(SUCCESS, "Found a default password: " _GREEN_("%s") "|| Pack: %02X %02X", sprint_hex(key, 4), pack[0], pack[1]);
                 goto out;
             }
 
@@ -1316,7 +1318,7 @@ static int CmdHF14AMfUInfo(const char *Cmd) {
                 key = default_pwd_pack[i];
                 len = ulev1_requestAuthentication(key, pack, sizeof(pack));
                 if (len > -1) {
-                    PrintAndLogEx(SUCCESS, "Found a default password:" _GREEN_("%s") " || Pack: %02X %02X", sprint_hex(key, 4), pack[0], pack[1]);
+                    PrintAndLogEx(SUCCESS, "Found a default password: " _GREEN_("%s") "|| Pack: %02X %02X", sprint_hex(key, 4), pack[0], pack[1]);
                     break;
                 } else {
                     if (ul_auth_select(&card, tagtype, hasAuthKey, authkeyptr, pack, sizeof(pack)) == PM3_ESOFT) return PM3_ESOFT;
diff --git a/client/cmdlfindala.h b/client/cmdlfindala.h
index 1d0762f5f..65ed29e61 100644
--- a/client/cmdlfindala.h
+++ b/client/cmdlfindala.h
@@ -19,6 +19,6 @@ int detectIndala26(uint8_t *bitStream, size_t *size, uint8_t *invert);
 int detectIndala64(uint8_t *bitStream, size_t *size, uint8_t *invert);
 int detectIndala224(uint8_t *bitStream, size_t *size, uint8_t *invert);
 int demodIndala(void);
-int getIndalaBits(uint8_t fc, uint16_t csn, uint8_t *bits);
+int getIndalaBits(uint8_t fc, uint16_t cn, uint8_t *bits);
 
 #endif
diff --git a/client/cmdlfkeri.c b/client/cmdlfkeri.c
index 3c7863361..c698842a6 100644
--- a/client/cmdlfkeri.c
+++ b/client/cmdlfkeri.c
@@ -144,7 +144,7 @@ static int CmdKeriMSScramble (KeriMSScramble_t Action, uint32_t *FC, uint32_t *I
         *CardID = *CardID | Parity;
 
         // Bit 31 was fixed but not in check/parity bits
-        *CardID |= (1 << 31); 
+        *CardID |= (uint32_t)(1 << 31);
 
         PrintAndLogEx(SUCCESS, "Scrambled FC : %d - Card ID : %d to RAW : E0000000%08X",*FC,*ID,*CardID);
     }
diff --git a/client/fileutils.c b/client/fileutils.c
index d9e6e4445..f47e32b73 100644
--- a/client/fileutils.c
+++ b/client/fileutils.c
@@ -70,7 +70,7 @@ struct wave_info_t {
         char tag[4];
         uint32_t size;
     } PACKED audio_data;
-} PACKED wave_info;
+} PACKED;
 
 /**
  * @brief checks if a file exists
diff --git a/common/cardhelper.c b/common/cardhelper.c
index 5cebf4322..df3aff5be 100644
--- a/common/cardhelper.c
+++ b/common/cardhelper.c
@@ -15,9 +15,10 @@
 #include "ui.h"
 #include "util.h"
 
-#define CARD_INS_DECRYPT 0x01
-#define CARD_INS_ENCRYPT 0x02
-#define CARD_INS_DECODE  0x06
+#define CARD_INS_DECRYPT    0x01
+#define CARD_INS_ENCRYPT    0x02
+#define CARD_INS_DECODE     0x06
+#define CARD_INS_NUMBLOCKS  0x07
 static uint8_t cmd[] = {0x96, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
 
 // look for CryptoHelper
@@ -64,6 +65,7 @@ bool Encrypt(uint8_t *src, uint8_t *dest) {
     return executeCrypto(CARD_INS_ENCRYPT, src, dest);
 }
 
+// Call with block6
 void DecodeBlock6(uint8_t *src) {
     int resp_len = 0;
     uint8_t resp[254] = {0};
@@ -81,3 +83,12 @@ void DecodeBlock6(uint8_t *src) {
     PrintAndLogEx(SUCCESS, "%.*s", resp_len - 11, resp + 9);
 }
 
+// Call with block6
+uint8_t GetNumberBlocksForUserId(uint8_t *src) {
+    int resp_len = 0;
+    uint8_t resp[254] = {0};
+    uint8_t c[] = {0x96, CARD_INS_NUMBLOCKS, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+    memcpy(c + 5, src, 8);
+    ExchangeAPDUSC(true, c, sizeof(c), false, true, resp, sizeof(resp), &resp_len);
+    return resp[8];
+}
\ No newline at end of file
diff --git a/common/cardhelper.h b/common/cardhelper.h
index 14ae23d1f..fcdd73cfe 100644
--- a/common/cardhelper.h
+++ b/common/cardhelper.h
@@ -18,4 +18,5 @@ bool IsCryptoHelperPresent(void);
 bool Encrypt(uint8_t *src, uint8_t *dest);
 bool Decrypt(uint8_t *src, uint8_t *dest);
 void DecodeBlock6(uint8_t *src);
+uint8_t GetNumberBlocksForUserId(uint8_t *src);
 #endif
diff --git a/common/generator.c b/common/generator.c
index 6dbac2feb..4be63a925 100644
--- a/common/generator.c
+++ b/common/generator.c
@@ -34,7 +34,7 @@
 // XYZ 3D printing
 // Vinglock
 //------------------------------------
-static void transform_D(uint8_t *ru) {
+void transform_D(uint8_t* ru) {
 
     const uint32_t c_D[] = {
         0x6D835AFC, 0x7D15CD97, 0x0942B409, 0x32F9C923, 0xA811FB02, 0x64F121E8,
@@ -44,32 +44,32 @@ static void transform_D(uint8_t *ru) {
         0x5728B869, 0x30726D5A
     };
 
-    //Transform
-    uint8_t i;
-    uint8_t p = 0;
-    uint32_t v1 = ((ru[3] << 24) | (ru[2] << 16) | (ru[1] << 8) | ru[0]) + c_D[p++];
-    uint32_t v2 = ((ru[7] << 24) | (ru[6] << 16) | (ru[5] << 8) | ru[4]) + c_D[p++];
+	//Transform
+	uint8_t i;
+	uint8_t p = 0;
+	uint32_t v1 = ((ru[3] << 24) | (ru[2] << 16) | (ru[1] << 8) | ru[0]) + c_D[p++];
+	uint32_t v2 = ((ru[7] << 24) | (ru[6] << 16) | (ru[5] << 8) | ru[4]) + c_D[p++];
+	for (i = 0; i < 12; i += 2)
+	{
+                uint32_t tempA = v1 ^ v2;
+		uint32_t t1 = PM3_ROTL(tempA, v2 & 0x1F) + c_D[p++];
+                uint32_t tempB = v2 ^ t1;
+		uint32_t t2 = PM3_ROTL(tempB, t1 & 0x1F) + c_D[p++];
+                tempA = t1 ^ t2;
+		v1 = PM3_ROTL(tempA, t2 & 0x1F) + c_D[p++];
+                tempB = t2 ^ v1;
+		v2 = PM3_ROTL(tempB, v1 & 0x1F) + c_D[p++];
+	}
 
-    for (i = 0; i < 12; i += 2) {
-        uint32_t xor1 = v1 ^ v2;
-        uint32_t t1 = ROTL(xor1, v2 & 0x1F) + c_D[p++];
-        uint32_t xor2 = v2 ^ t1;
-        uint32_t t2 = ROTL(xor2, t1 & 0x1F) + c_D[p++];
-        uint32_t xor3 = t1 ^ t2;
-        uint32_t xor4 = t2 ^ v1;
-        v1 = ROTL(xor3, t2 & 0x1F) + c_D[p++];
-        v2 = ROTL(xor4, v1 & 0x1F) + c_D[p++];
-    }
-
-    //Re-use ru
-    ru[0] = v1 & 0xFF;
-    ru[1] = (v1 >> 8) & 0xFF;
-    ru[2] = (v1 >> 16) & 0xFF;
-    ru[3] = (v1 >> 24) & 0xFF;
-    ru[4] = v2 & 0xFF;
-    ru[5] = (v2 >> 8) & 0xFF;
-    ru[6] = (v2 >> 16) & 0xFF;
-    ru[7] = (v2 >> 24) & 0xFF;
+	//Re-use ru
+	ru[0] = v1 & 0xFF;
+	ru[1] = (v1 >> 8) & 0xFF;
+	ru[2] = (v1 >> 16) & 0xFF;
+	ru[3] = (v1 >> 24) & 0xFF;
+	ru[4] = v2 & 0xFF;
+	ru[5] = (v2 >> 8) & 0xFF;
+	ru[6] = (v2 >> 16) & 0xFF;
+	ru[7] = (v2 >> 24) & 0xFF;
 }
 
 // Transport system (IT) pwd generation algo nickname A.
@@ -130,17 +130,23 @@ uint32_t ul_ev1_pwdgenC(uint8_t *uid) {
 
 // XYZ 3d printing pwd generation algo nickname D.
 uint32_t ul_ev1_pwdgenD(uint8_t *uid) {
+
     uint8_t i;
-    uint8_t r = (uid[1] + uid[3] + uid[5]) & 7; // rotation offset
-    uint8_t ru[8] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; // rotated UID
+    // rotation offset
+    uint8_t r = (uid[1] + uid[3] + uid[5]) & 7;
+
+    // rotated UID
+    uint8_t ru[8] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; 
     for (i = 0; i < 7; i++)
         ru[(i + r) & 7] = uid[i];
 
     transform_D(ru);
 
-    // calc key
+    // offset    
+    r = (ru[0] + ru[2] + ru[4] + ru[6]) & 3;
+
+    // calc key    
     uint32_t pwd = 0;
-    r = (ru[0] + ru[2] + ru[4] + ru[6]) & 3; // offset
     for (i = 0; i < 4; i++)
         pwd = ru[i + r] + (pwd << 8);
 
diff --git a/include/common.h b/include/common.h
index 653000409..e7d6bba90 100644
--- a/include/common.h
+++ b/include/common.h
@@ -64,8 +64,8 @@ extern int DBGLEVEL;
 # define ROTR(x,n) (((uintmax_t)(x) >> (n)) | ((uintmax_t)(x) << ((sizeof(x) * 8) - (n))))
 #endif
 
-#ifndef ROTL
-# define ROTL(x,n) (((uintmax_t)(x) << (n)) | ((uintmax_t)(x) >> ((sizeof(x) * 8) - (n))))
+#ifndef PM3_ROTL
+# define PM3_ROTL(x,n) (((uintmax_t)(x) << (n)) | ((uintmax_t)(x) >> ((sizeof(x) * 8) - (n))))
 #endif
 
 // endian change for 64bit
diff --git a/tools/hitag2crack/crack2/hitagcrypto.c b/tools/hitag2crack/crack2/hitagcrypto.c
index 5ebfc5ae9..3c4f84bc6 100644
--- a/tools/hitag2crack/crack2/hitagcrypto.c
+++ b/tools/hitag2crack/crack2/hitagcrypto.c
@@ -228,18 +228,17 @@ static uint32_t hitag2_crypt(uint64_t x);
 #define pickbits1_2_1(S, A, B, C)  ( ((S >> A) & 1) | ((S >> (B - 1)) & 6) | \
                                    ((S >> (C - 3)) & 8) )
 
-
-static uint32_t hitag2_crypt(uint64_t s) {
+static uint32_t hitag2_crypt(uint64_t x) {
     const uint32_t ht2_function4a = 0x2C79;	// 0010 1100 0111 1001
     const uint32_t ht2_function4b = 0x6671;	// 0110 0110 0111 0001
     const uint32_t ht2_function5c = 0x7907287B;	// 0111 1001 0000 0111 0010 1000 0111 1011
     uint32_t bitindex;
 
-    bitindex = (ht2_function4a >> pickbits2_2(s, 1, 4)) & 1;
-    bitindex |= ((ht2_function4b << 1) >> pickbits1_1_2(s, 7, 11, 13)) & 0x02;
-    bitindex |= ((ht2_function4b << 2) >> pickbits1x4(s, 16, 20, 22, 25)) & 0x04;
-    bitindex |= ((ht2_function4b << 3) >> pickbits2_1_1(s, 27, 30, 32)) & 0x08;
-    bitindex |= ((ht2_function4a << 4) >> pickbits1_2_1(s, 33, 42, 45)) & 0x10;
+    bitindex = (ht2_function4a >> pickbits2_2(x, 1, 4)) & 1;
+    bitindex |= ((ht2_function4b << 1) >> pickbits1_1_2(x, 7, 11, 13)) & 0x02;
+    bitindex |= ((ht2_function4b << 2) >> pickbits1x4(x, 16, 20, 22, 25)) & 0x04;
+    bitindex |= ((ht2_function4b << 3) >> pickbits2_1_1(x, 27, 30, 32)) & 0x08;
+    bitindex |= ((ht2_function4a << 4) >> pickbits1_2_1(x, 33, 42, 45)) & 0x10;
 
     DEBUG_PRINTF("hitag2_crypt bitindex = %02x\n", bitindex);
     return (ht2_function5c >> bitindex) & 1;
diff --git a/tools/hitag2crack/crack2/ht2crack2buildtable.c b/tools/hitag2crack/crack2/ht2crack2buildtable.c
index ffabd8f2e..0aca200ef 100644
--- a/tools/hitag2crack/crack2/ht2crack2buildtable.c
+++ b/tools/hitag2crack/crack2/ht2crack2buildtable.c
@@ -143,16 +143,16 @@ void writetable(struct table *t1) {
 
 // store value in table
 void store(unsigned char *data) {
-    unsigned char d1, d2;
+    unsigned char d_1, d_2;
     int offset;
     struct table *t1;
 
     // use the first two bytes as an index
-    d1 = data[0];
-    d2 = data[1];
-    offset = (d1 * 0x100) + d2;
+    d_1 = data[0];
+    d_2 = data[1];
+    offset = (d_1 * 0x100) + d_2;
 
-    if (debug) printf("store, d1=%02X, d2=%02X, offset = %d\n", d1, d2, offset);
+    if (debug) printf("store, d1=%02X, d2=%02X, offset = %d\n", d_1, d_2, offset);
 
     // get pointer to table entry
     t1 = t + offset;
@@ -362,10 +362,10 @@ void makedirs() {
 }
 
 static int datacmp(const void *p1, const void *p2, void *dummy) {
-    unsigned char *d1 = (unsigned char *)p1;
-    unsigned char *d2 = (unsigned char *)p2;
+    unsigned char *d_1 = (unsigned char *)p1;
+    unsigned char *d_2 = (unsigned char *)p2;
 
-    return memcmp(d1, d2, DATASIZE);
+    return memcmp(d_1, d_2, DATASIZE);
 }
 
 void *sorttable(void *d) {
diff --git a/tools/hitag2crack/crack3/hitagcrypto.c b/tools/hitag2crack/crack3/hitagcrypto.c
index 23b88228d..21662b43e 100644
--- a/tools/hitag2crack/crack3/hitagcrypto.c
+++ b/tools/hitag2crack/crack3/hitagcrypto.c
@@ -229,17 +229,17 @@ static uint32_t hitag2_crypt(uint64_t x);
                                    ((S >> (C - 3)) & 8) )
 
 
-static uint32_t hitag2_crypt(uint64_t s) {
+static uint32_t hitag2_crypt(uint64_t x) {
     const uint32_t ht2_function4a = 0x2C79;	// 0010 1100 0111 1001
     const uint32_t ht2_function4b = 0x6671;	// 0110 0110 0111 0001
     const uint32_t ht2_function5c = 0x7907287B;	// 0111 1001 0000 0111 0010 1000 0111 1011
     uint32_t bitindex;
 
-    bitindex = (ht2_function4a >> pickbits2_2(s, 1, 4)) & 1;
-    bitindex |= ((ht2_function4b << 1) >> pickbits1_1_2(s, 7, 11, 13)) & 0x02;
-    bitindex |= ((ht2_function4b << 2) >> pickbits1x4(s, 16, 20, 22, 25)) & 0x04;
-    bitindex |= ((ht2_function4b << 3) >> pickbits2_1_1(s, 27, 30, 32)) & 0x08;
-    bitindex |= ((ht2_function4a << 4) >> pickbits1_2_1(s, 33, 42, 45)) & 0x10;
+    bitindex = (ht2_function4a >> pickbits2_2(x, 1, 4)) & 1;
+    bitindex |= ((ht2_function4b << 1) >> pickbits1_1_2(x, 7, 11, 13)) & 0x02;
+    bitindex |= ((ht2_function4b << 2) >> pickbits1x4(x, 16, 20, 22, 25)) & 0x04;
+    bitindex |= ((ht2_function4b << 3) >> pickbits2_1_1(x, 27, 30, 32)) & 0x08;
+    bitindex |= ((ht2_function4a << 4) >> pickbits1_2_1(x, 33, 42, 45)) & 0x10;
 
     DEBUG_PRINTF("hitag2_crypt bitindex = %02x\n", bitindex);
     return (ht2_function5c >> bitindex) & 1;
diff --git a/tools/hitag2crack/crack3/ht2crack3.c b/tools/hitag2crack/crack3/ht2crack3.c
index 747f7785c..da35bbd14 100644
--- a/tools/hitag2crack/crack3/ht2crack3.c
+++ b/tools/hitag2crack/crack3/ht2crack3.c
@@ -1,6 +1,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <pthread.h>
+#include <inttypes.h>
 
 #include "HardwareProfile.h"
 #include "rfidler.h"
@@ -243,7 +244,7 @@ void *crack(void *d) {
 
     // find keys
     for (klower = data->klowerstart; klower < (data->klowerstart + data->klowerrange); klower++) {
-        printf("trying klower = 0x%05lx\n", klower);
+        printf("trying klower = 0x%05"PRIx64"\n", klower);
         // build table
         count = 0;
         for (y = 0; y < 0x40000; y++) {
@@ -304,7 +305,7 @@ void *crack(void *d) {
 
             if ((found) && (!badguess)) {
                 // brute
-                printf("possible partial key found: 0x%012lx\n", ((uint64_t)kmiddle << 16) | klower);
+                printf("possible partial key found: 0x%012"PRIx64"\n", ((uint64_t)kmiddle << 16) | klower);
 
                 if (testkey(&foundkey, uid, (kmiddle << 16 | klower), TnRaR[0].nR, TnRaR[0].aR) &&
                         testkey(&foundkey, uid, (kmiddle << 16 | klower), TnRaR[1].nR, TnRaR[1].aR)) {
@@ -381,14 +382,14 @@ int main(int argc, char *argv[]) {
     while (getline(&buf, &lenbuf, fp) > 0) {
         buft1 = strchr(buf, ' ');
         if (!buft1) {
-            printf("invalid file input on line %d\n", numnrar + 1);
+            printf("invalid file input on line %u\n", numnrar + 1);
             exit(1);
         }
         *buft1 = 0x00;
         buft1++;
         buft2 = strchr(buft1, '\n');
         if (!buft2) {
-            printf("no CR on line %d\n", numnrar + 1);
+            printf("no CR on line %u\n", numnrar + 1);
             exit(1);
         }
         *buft2 = 0x00;
@@ -406,7 +407,7 @@ int main(int argc, char *argv[]) {
     fclose(fp);
     fp = NULL;
 
-    printf("Loaded %d NrAr pairs\n", numnrar);
+    printf("Loaded %u NrAr pairs\n", numnrar);
 
     // create table of thread data
     tdata = (struct threaddata *)malloc(sizeof(struct threaddata) * NUM_THREADS);
diff --git a/tools/hitag2crack/crack4/ht2crack4.c b/tools/hitag2crack/crack4/ht2crack4.c
index 44952f4b7..5c2d11c2e 100644
--- a/tools/hitag2crack/crack4/ht2crack4.c
+++ b/tools/hitag2crack/crack4/ht2crack4.c
@@ -270,14 +270,14 @@ void init_guess_table(char *filename, char *uidstr) {
     while ((getline(&buf, &lenbuf, fp) > 0) && (num_nRaR < MAX_NONCES)) {
         buft1 = strchr(buf, ' ');
         if (!buft1) {
-            printf("invalid file input on line %d\n", num_nRaR + 1);
+            printf("invalid file input on line %u\n", num_nRaR + 1);
             exit(1);
         }
         *buft1 = 0x00;
         buft1++;
         buft2 = strchr(buft1, '\n');
         if (!buft2) {
-            printf("no CR on line %d\n", num_nRaR + 1);
+            printf("no CR on line %u\n", num_nRaR + 1);
             exit(1);
         }
         *buft2 = 0x00;
@@ -294,7 +294,7 @@ void init_guess_table(char *filename, char *uidstr) {
     fclose(fp);
     fp = NULL;
 
-    fprintf(stderr, "Loaded %d nRaR pairs\n", num_nRaR);
+    fprintf(stderr, "Loaded %u nRaR pairs\n", num_nRaR);
 
     // set key and copy in enc_nR and ks values
     // set score to -1.0 to distinguish them from 0 scores
@@ -516,7 +516,7 @@ void score_all_traces(unsigned int size) {
     // start the threads
     for (i = 0; i < NUM_THREADS; i++) {
         if (pthread_create(&(threads[i]), NULL, score_some_traces, (void *)(tdata + i))) {
-            printf("cannot start thread %d\n", i);
+            printf("cannot start thread %u\n", i);
             exit(1);
         }
     }
@@ -524,7 +524,7 @@ void score_all_traces(unsigned int size) {
     // wait for threads to end
     for (i = 0; i < NUM_THREADS; i++) {
         if (pthread_join(threads[i], &status)) {
-            printf("cannot join thread %d\n", i);
+            printf("cannot join thread %u\n", i);
             exit(1);
         }
     }
@@ -620,13 +620,13 @@ void crack() {
     uint64_t foundkey;
 
     for (i = 16; i <= 48; i++) {
-        fprintf(stderr, "round %2d, size=%2d\n", i - 16, i);
+        fprintf(stderr, "round %2u, size=%2u\n", i - 16, i);
         execute_round(i);
 
         // print some metrics
         revkey = rev64(guesses[0].key);
         foundkey = ((revkey >> 40) & 0xff) | ((revkey >> 24) & 0xff00) | ((revkey >> 8) & 0xff0000) | ((revkey << 8) & 0xff000000) | ((revkey << 24) & 0xff00000000) | ((revkey << 40) & 0xff0000000000);
-        fprintf(stderr, " guess=%012" PRIx64 ", num_guesses = %d, top score=%1.10f, min score=%1.10f\n", foundkey, num_guesses, guesses[0].score, guesses[num_guesses - 1].score);
+        fprintf(stderr, " guess=%012" PRIx64 ", num_guesses = %u, top score=%1.10f, min score=%1.10f\n", foundkey, num_guesses, guesses[0].score, guesses[num_guesses - 1].score);
     }
 }
 
@@ -745,7 +745,7 @@ void test() {
         packed = packstate(lfsr);
 
         if (hitag2_crypt(lfsr) != f20(packed)) {
-            printf(" * * * FAIL: %3" PRIu64 ": 0x%012" PRIx64 " = %d, 0x%012" PRIx64 " = 0x%05" PRIx64 "\n", i, lfsr, hitag2_crypt(lfsr), packed, f20(packed));
+            printf(" * * * FAIL: %3" PRIu64 ": 0x%012" PRIx64 " = %u, 0x%012" PRIx64 " = 0x%05" PRIx64 "\n", i, lfsr, hitag2_crypt(lfsr), packed, f20(packed));
         }
     }
 
@@ -821,7 +821,7 @@ int main(int argc, char *argv[]) {
     if ((tot_nRaR > 0) && (tot_nRaR <= num_nRaR)) {
         num_nRaR = tot_nRaR;
     }
-    fprintf(stderr, "Using %d nRaR pairs\n", num_nRaR);
+    fprintf(stderr, "Using %u nRaR pairs\n", num_nRaR);
 
     crack();