]> git.zerfleddert.de Git - proxmark3-svn/commitdiff
HitagS Improvements (#721)
authorflorianrock <45316656+florianrock@users.noreply.github.com>
Fri, 30 Nov 2018 16:42:22 +0000 (17:42 +0100)
committerpwpiwi <pwpiwi@users.noreply.github.com>
Fri, 30 Nov 2018 16:42:22 +0000 (17:42 +0100)
* support of HITAG S standard communication mode
* fixed wrong AC (Anti Collision) decoding
* support of block read mode
* fixed wrong uid send when using simulation
* support of communication mode parameter  (client is backward compatible)
* support of start-page parameter (important for some weird tags) (client is backward compatible)
* also expect pages if tag memory size in con0 is 11 (we got some tags)
* corrected hitagS reader cmd help

CHANGELOG.md
armsrc/BigBuf.c
armsrc/appmain.c
armsrc/apps.h
armsrc/hitagS.c
client/cmdlfhitag.c
include/hitagS.h
include/usb_cmd.h

index b0073f34050983629da5fa0352f317e25b4a98b8..7d8993dc0800d0ddc57a629557ee1d3436cd5c1a 100644 (file)
@@ -7,13 +7,20 @@ This project uses the changelog in accordance with [keepchangelog](http://keepac
 ### Changed
 - Changed hf mfp security. Now it works in all the modes. (drHatson)
 - `hf fido` - show/check DER certificate and signatures (Merlok)
+- Changed `lf hitag reader 0x ... <firstPage> <tagmode>` - to select first page to read and tagmode (0=STANDARD, 1=ADVANCED, 2=FAST_ADVANCED)
+- Accept hitagS con0 tags with memory bits set to 11 and handle like 2048 tag
 
 ### Fixed
+- AC-Mode decoding for HitagS
+- Wrong UID at HitagS simulation 
 
 ### Added
+- Support Standard Communication Mode in HITAG S
 - Added `hf emv scan` - commands for scan EMV card and dump data to json file (Merlok)
 - `hf mfp` group of commands (Merlok)
 - Added `hf fido` - FIDO U2F authenticator commands https://fidoalliance.org/ (Merlok)
+- Added `lf hitag reader 03` - read block (instead of pages) 
+- Added `lf hitag reader 04` - read block (instead of pages) 
 
 ## [v3.1.0][2018-10-10]
 
index 4fe97b462d2fdd63e458a10da1251c435651ca65..4b1264b6684abc375af7c0ca51967a82fc6730ab 100644 (file)
@@ -229,8 +229,11 @@ int LogTraceHitag(const uint8_t * btBytes, int iBits, int iSamples, uint32_t dwP
 
        uint8_t *trace = BigBuf_get_addr();
        uint16_t iLen = nbytes(iBits);
+
        // Return when trace is full
-       if (traceLen + sizeof(rsamples) + sizeof(dwParity) + sizeof(iBits) + iLen > BigBuf_max_traceLen()) return false;
+       if (traceLen + sizeof(rsamples) + sizeof(dwParity) + sizeof(iBits) + iLen > BigBuf_max_traceLen()) {
+               return false;
+       }       
 
        //Hitag traces appear to use this traceformat:
        // 32 bits timestamp (little endian,Highest Bit used as readerToTag flag)
@@ -238,6 +241,8 @@ int LogTraceHitag(const uint8_t * btBytes, int iBits, int iSamples, uint32_t dwP
        // 8 bits size (number of bits in the trace entry, not number of bytes)
        // y Bytes data
 
+
+
        rsamples += iSamples;
        trace[traceLen++] = ((rsamples >> 0) & 0xff);
        trace[traceLen++] = ((rsamples >> 8) & 0xff);
index 573a3a71251cadb61bb9a983b142cd1ac32ee8aa..35c9e5bfcc3fa6db04ab6c3d37b89b3f0698ace6 100644 (file)
@@ -1086,10 +1086,13 @@ void UsbPacketReceived(uint8_t *packet, int len)
                        SimulateHitagSTag((bool)c->arg[0],(byte_t*)c->d.asBytes);
                        break;
                case CMD_TEST_HITAGS_TRACES:// Tests every challenge within the given file
-                       check_challenges((bool)c->arg[0],(byte_t*)c->d.asBytes);
+                       check_challenges_cmd((bool)c->arg[0], (byte_t*)c->d.asBytes, (uint8_t)c->arg[1]);
                        break;
                case CMD_READ_HITAG_S://Reader for only Hitag S tags, args = key or challenge
-                       ReadHitagS((hitag_function)c->arg[0],(hitag_data*)c->d.asBytes);
+                       ReadHitagSCmd((hitag_function)c->arg[0], (hitag_data*)c->d.asBytes, (uint8_t)c->arg[1], (uint8_t)c->arg[2], false);
+                       break;
+               case CMD_READ_HITAG_S_BLK:
+                       ReadHitagSCmd((hitag_function)c->arg[0], (hitag_data*)c->d.asBytes, (uint8_t)c->arg[1], (uint8_t)c->arg[2], true);
                        break;
                case CMD_WR_HITAG_S://writer for Hitag tags args=data to write,page and key or challenge
                        if ((hitag_function)c->arg[0] < 10) {
index 6f728c6103de6a50eb2545968c525667bfc614a1..6af22b57d379f255a14f607b9c6cde1f06d6ce5b 100644 (file)
@@ -180,10 +180,11 @@ void ReaderHitag(hitag_function htf, hitag_data* htd);
 void WriterHitag(hitag_function htf, hitag_data* htd, int page);
 
 //hitagS.h
+void ReadHitagSCmd(hitag_function htf, hitag_data* htd, uint64_t startPage, uint64_t tagMode, bool readBlock);
 void SimulateHitagSTag(bool tag_mem_supplied, byte_t* data);
-void ReadHitagS(hitag_function htf, hitag_data* htd);
 void WritePageHitagS(hitag_function htf, hitag_data* htd,int page);
-void check_challenges(bool file_given, byte_t* data);
+void check_challenges_cmd(bool file_given, byte_t* data, uint64_t tagMode);
+
 
 
 // cmd.h
index f6ba0c6bf8186060e7c724d503b01f497fe9feef..5fa6a9a143035013f5307f750c756dd3b27780d1 100644 (file)
@@ -156,112 +156,112 @@ void calc_crc(unsigned char * crc, unsigned char data, unsigned char Bitcount) {
        } while (--Bitcount);
 }
 
+
 static void hitag_send_bit(int bit) {
        LED_A_ON();
        // Reset clock for the next bit
        AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG;
 
        switch (m) {
-       case AC2K:
-               if (bit == 0) {
-                       // AC Coding --__
-                       HIGH(GPIO_SSC_DOUT);
-                       while (AT91C_BASE_TC0->TC_CV < T0 * 32)
-                               ;
-                       LOW(GPIO_SSC_DOUT);
-                       while (AT91C_BASE_TC0->TC_CV < T0 * 64)
-                               ;
-               } else {
-                       // AC coding -_-_
-                       HIGH(GPIO_SSC_DOUT);
-                       while (AT91C_BASE_TC0->TC_CV < T0 * 16)
-                               ;
-                       LOW(GPIO_SSC_DOUT);
-                       while (AT91C_BASE_TC0->TC_CV < T0 * 32)
-                               ;
-                       HIGH(GPIO_SSC_DOUT);
-                       while (AT91C_BASE_TC0->TC_CV < T0 * 48)
-                               ;
-                       LOW(GPIO_SSC_DOUT);
-                       while (AT91C_BASE_TC0->TC_CV < T0 * 64)
-                               ;;
-               }
-               LED_A_OFF();
-               break;
-       case AC4K:
-               if (bit == 0) {
-                       // AC Coding --__
-                       HIGH(GPIO_SSC_DOUT);
-                       while (AT91C_BASE_TC0->TC_CV < T0 * HITAG_T_TAG_HALF_PERIOD)
-                               ;
-                       LOW(GPIO_SSC_DOUT);
-                       while (AT91C_BASE_TC0->TC_CV < T0 * HITAG_T_TAG_FULL_PERIOD)
-                               ;
-               } else {
-                       // AC coding -_-_
-                       HIGH(GPIO_SSC_DOUT);
-                       while (AT91C_BASE_TC0->TC_CV < T0 * 8)
-                               ;
-                       LOW(GPIO_SSC_DOUT);
-                       while (AT91C_BASE_TC0->TC_CV < T0 * 16)
-                               ;
-                       HIGH(GPIO_SSC_DOUT);
-                       while (AT91C_BASE_TC0->TC_CV < T0 * 24)
-                               ;
-                       LOW(GPIO_SSC_DOUT);
-                       while (AT91C_BASE_TC0->TC_CV < T0 * 32)
-                               ;
-               }
-               LED_A_OFF();
-               break;
-       case MC4K:
-               if (bit == 0) {
-                       // Manchester: Unloaded, then loaded |__--|
-                       LOW(GPIO_SSC_DOUT);
-                       while (AT91C_BASE_TC0->TC_CV < T0 * 16)
-                               ;
-                       HIGH(GPIO_SSC_DOUT);
-                       while (AT91C_BASE_TC0->TC_CV < T0 * 32)
-                               ;
-               } else {
-                       // Manchester: Loaded, then unloaded |--__|
-                       HIGH(GPIO_SSC_DOUT);
-                       while (AT91C_BASE_TC0->TC_CV < T0 * 16)
-                               ;
-                       LOW(GPIO_SSC_DOUT);
-                       while (AT91C_BASE_TC0->TC_CV < T0 * 32)
-                               ;
-               }
-               LED_A_OFF();
-               break;
-       case MC8K:
-               if (bit == 0) {
-                       // Manchester: Unloaded, then loaded |__--|
-                       LOW(GPIO_SSC_DOUT);
-                       while (AT91C_BASE_TC0->TC_CV < T0 * 8)
-                               ;
-                       HIGH(GPIO_SSC_DOUT);
-                       while (AT91C_BASE_TC0->TC_CV < T0 * 16)
-                               ;
-               } else {
-                       // Manchester: Loaded, then unloaded |--__|
-                       HIGH(GPIO_SSC_DOUT);
-                       while (AT91C_BASE_TC0->TC_CV < T0 * 8)
-                               ;
-                       LOW(GPIO_SSC_DOUT);
-                       while (AT91C_BASE_TC0->TC_CV < T0 * 16)
-                               ;
-               }
-               LED_A_OFF();
-               break;
-       default:
-               break;
+               case AC2K:
+                       if (bit == 0) {
+                               // AC Coding --__
+                               HIGH(GPIO_SSC_DOUT);
+                               while (AT91C_BASE_TC0->TC_CV < T0 * 32)
+                                       ;
+                               LOW(GPIO_SSC_DOUT);
+                               while (AT91C_BASE_TC0->TC_CV < T0 * 64)
+                                       ;
+                       } else {
+                               // AC coding -_-_
+                               HIGH(GPIO_SSC_DOUT);
+                               while (AT91C_BASE_TC0->TC_CV < T0 * 16)
+                                       ;
+                               LOW(GPIO_SSC_DOUT);
+                               while (AT91C_BASE_TC0->TC_CV < T0 * 32)
+                                       ;
+                               HIGH(GPIO_SSC_DOUT);
+                               while (AT91C_BASE_TC0->TC_CV < T0 * 48)
+                                       ;
+                               LOW(GPIO_SSC_DOUT);
+                               while (AT91C_BASE_TC0->TC_CV < T0 * 64)
+                                       ;;
+                       }
+                       LED_A_OFF();
+                       break;
+               case AC4K:
+                       if (bit == 0) {
+                               // AC Coding --__
+                               HIGH(GPIO_SSC_DOUT);
+                               while (AT91C_BASE_TC0->TC_CV < T0 * HITAG_T_TAG_HALF_PERIOD)
+                                       ;
+                               LOW(GPIO_SSC_DOUT);
+                               while (AT91C_BASE_TC0->TC_CV < T0 * HITAG_T_TAG_FULL_PERIOD)
+                                       ;
+                       } else {
+                               // AC coding -_-_
+                               HIGH(GPIO_SSC_DOUT);
+                               while (AT91C_BASE_TC0->TC_CV < T0 * 8)
+                                       ;
+                               LOW(GPIO_SSC_DOUT);
+                               while (AT91C_BASE_TC0->TC_CV < T0 * 16)
+                                       ;
+                               HIGH(GPIO_SSC_DOUT);
+                               while (AT91C_BASE_TC0->TC_CV < T0 * 24)
+                                       ;
+                               LOW(GPIO_SSC_DOUT);
+                               while (AT91C_BASE_TC0->TC_CV < T0 * 32)
+                                       ;
+                       }
+                       LED_A_OFF();
+                       break;
+               case MC4K:
+                       if (bit == 0) {
+                               // Manchester: Unloaded, then loaded |__--|
+                               LOW(GPIO_SSC_DOUT);
+                               while (AT91C_BASE_TC0->TC_CV < T0 * 16)
+                                       ;
+                               HIGH(GPIO_SSC_DOUT);
+                               while (AT91C_BASE_TC0->TC_CV < T0 * 32)
+                                       ;
+                       } else {
+                               // Manchester: Loaded, then unloaded |--__|
+                               HIGH(GPIO_SSC_DOUT);
+                               while (AT91C_BASE_TC0->TC_CV < T0 * 16)
+                                       ;
+                               LOW(GPIO_SSC_DOUT);
+                               while (AT91C_BASE_TC0->TC_CV < T0 * 32)
+                                       ;
+                       }
+                       LED_A_OFF();
+                       break;
+               case MC8K:
+                       if (bit == 0) {
+                               // Manchester: Unloaded, then loaded |__--|
+                               LOW(GPIO_SSC_DOUT);
+                               while (AT91C_BASE_TC0->TC_CV < T0 * 8)
+                                       ;
+                               HIGH(GPIO_SSC_DOUT);
+                               while (AT91C_BASE_TC0->TC_CV < T0 * 16)
+                                       ;
+                       } else {
+                               // Manchester: Loaded, then unloaded |--__|
+                               HIGH(GPIO_SSC_DOUT);
+                               while (AT91C_BASE_TC0->TC_CV < T0 * 8)
+                                       ;
+                               LOW(GPIO_SSC_DOUT);
+                               while (AT91C_BASE_TC0->TC_CV < T0 * 16)
+                                       ;
+                       }
+                       LED_A_OFF();
+                       break;
+               default:
+                       break;
        }
 }
 
-static void hitag_send_frame(const byte_t* frame, size_t frame_len) {
+static void hitag_tag_send_frame(const byte_t* frame, size_t frame_len) {
 // Send start of frame
-
        for (size_t i = 0; i < sof_bits; i++) {
                hitag_send_bit(1);
        }
@@ -331,12 +331,12 @@ static void hitag_reader_send_bit(int bit) {
 }
 
 static void hitag_reader_send_frame(const byte_t* frame, size_t frame_len) {
-// Send the content of the frame
+       // Send the content of the frame
        for (size_t i = 0; i < frame_len; i++) {
                if (frame[0] == 0xf8) {
                        //Dbprintf("BIT: %d",(frame[i / 8] >> (7 - (i % 8))) & 1);
                }
-               hitag_reader_send_bit((frame[i / 8] >> (7 - (i % 8))) & 1);
+               hitag_reader_send_bit(((frame[i / 8] >> (7 - (i % 8))) & 1));
        }
 // Send EOF
        AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG;
@@ -349,6 +349,485 @@ static void hitag_reader_send_frame(const byte_t* frame, size_t frame_len) {
        LOW(GPIO_SSC_DOUT);
 }
 
+static void hitag_decode_frame_MC(int bitRate, int sofBits, byte_t* rx, size_t* rxlenOrg, int* response, int rawMod[], int rawLen) {
+       size_t rxlen = 0;
+       bool bSkip = true;
+       int lastbit = 1;
+       int tag_sof = 0;
+       int timing = 1;
+       if (bitRate == 8) {
+               timing = 2;
+       }
+
+       for (int i=0; i < rawLen; i++) {
+               int ra = rawMod[i];
+               if (ra >= HITAG_T_EOF) {
+                       if (rxlen != 0) {
+                               //DbpString("wierd1?");
+                       }
+                       tag_sof = sofBits;
+
+                       // 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 / timing) {
+                       tag_sof=0;
+                       // 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 / timing) {
+                       tag_sof=0;
+                       // 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 / timing) {
+                       // 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
+               }
+       }       
+       *rxlenOrg = rxlen;
+}
+
+/* 
+static void hitag_decode_frame_AC2K_rising(byte_t* rx, size_t* rxlenOrg, int* response, int rawMod[], int rawLen) {
+       int tag_sof = 1; //skip start of frame
+       size_t rxlen = 0;
+
+       for (int i=0; i < rawLen; i++) {
+               int ra = rawMod[i];
+               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 |-_|
+                       tag_sof = 1;
+                       *response = ra - HITAG_T_TAG_HALF_PERIOD;
+               } else if (ra >= HITAG_T_TAG_CAPTURE_FOUR_HALF) {
+                       // AC coding example |--__|--__| means 0
+                       rx[rxlen / 8] |= 0 << (7 - (rxlen % 8));
+                       rxlen++;
+                       if (rawMod[i+1] == 0) { //TODO: this is weird - may we miss one capture with current configuration
+                               rx[rxlen / 8] |= 0 << (7 - (rxlen % 8));
+                               rxlen++;
+                               i++; //drop next capture
+                       }
+               } else if (ra >= HITAG_T_TAG_CAPTURE_TWO_HALF) {
+                       if (tag_sof) {
+                               // Ignore bits that are transmitted during SOF
+                               tag_sof--;
+                       } else {
+                               // AC coding example |-_-_|-_-_| which means 1
+                               //check if another high is coming (only -_-_ = 1) except end of the frame (support 0)
+                               if (rawMod[i+1] == 0 || rawMod[i+1] >= HITAG_T_TAG_CAPTURE_TWO_HALF) {
+                                       rx[rxlen / 8] |= 1 << (7 - (rxlen % 8));
+                                       rxlen++;
+                                       i++; //drop next capture
+                               } else {
+                                       Dbprintf("got weird high - %d,%d", ra, rawMod[i+1]);
+                               }
+                       }
+               } else {
+                       // Ignore wierd value, is to small to mean anything
+               }
+       }
+       *rxlenOrg = rxlen;
+}
+*/
+
+static void hitag_decode_frame_AC(int bitRate, int sofBits, byte_t* rx, size_t* rxlenOrg, int* response, int rawMod[], int rawLen) {
+       int tag_sof = 1;
+       size_t rxlen = 0;
+       int timing = 1;
+       if (bitRate == 4) {
+               timing = 2;
+       }
+       
+
+       for (int i=0; i < rawLen; i++) {
+               int ra = rawMod[i];
+               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 |-_|
+                       tag_sof = sofBits;
+                       *response = ra - HITAG_T_TAG_HALF_PERIOD;
+               } else if (ra >= HITAG_T_TAG_CAPTURE_FOUR_HALF / timing) {
+                       tag_sof=0;
+
+                       // AC coding example |--__|--__| means 0
+                       rx[rxlen / 8] |= 0 << (7 - (rxlen % 8));
+                       rxlen++;
+               } else if (ra >= HITAG_T_TAG_CAPTURE_THREE_HALF / timing) {
+                       tag_sof=0;
+
+                       if (rawMod[i-1] >= HITAG_T_TAG_CAPTURE_THREE_HALF / timing) {
+                               //treat like HITAG_T_TAG_CAPTURE_TWO_HALF
+                               if (rawMod[i+1] >= HITAG_T_TAG_CAPTURE_TWO_HALF / timing) {
+                                       rx[rxlen / 8] |= 1 << (7 - (rxlen % 8));
+                                       rxlen++;
+                                       i++; //drop next capture
+                               } else {
+                                       Dbprintf("got weird value - %d,%d", ra, rawMod[i+1]);
+                               }
+                       } else {
+                               //treat like HITAG_T_TAG_CAPTURE_FOUR_HALF
+                               rx[rxlen / 8] |= 0 << (7 - (rxlen % 8));
+                               rxlen++;
+                       }
+               } else if (ra >= HITAG_T_TAG_CAPTURE_TWO_HALF / timing) {
+                       if (tag_sof) {
+                               // Ignore bits that are transmitted during SOF
+                               tag_sof--;
+                       } else {
+                               // AC coding example |-_-_|-_-_| which means 1
+                               //check if another high is coming (only -_-_ = 1) except end of the frame (support 0)
+                               if (rawMod[i+1] == 0 || rawMod[i+1] >= HITAG_T_TAG_CAPTURE_TWO_HALF / timing) {
+                                       rx[rxlen / 8] |= 1 << (7 - (rxlen % 8));
+                                       rxlen++;
+                                       i++; //drop next capture
+                               } else {
+                                       Dbprintf("got weird value - %d,%d", ra, rawMod[i+1]);
+                               }
+                       }
+               } else {
+                       // Ignore wierd value, is to small to mean anything
+               }
+       }
+       *rxlenOrg = rxlen;
+}
+
+static void hitag_receive_frame(byte_t* rx, size_t* rxlen, int* response) {
+       int rawMod[200] = {0};
+       int rawLen = 0;
+       int i = 0;
+       int sofBits = 0;
+
+       m = MC4K;
+       if (tag.pstate == READY) {
+               switch (tag.mode) {
+                       case STANDARD:
+                               m = AC2K;
+                               sofBits = 1;
+                               break;
+                       case ADVANCED:
+                               m = AC2K;
+                               sofBits = 5; //3 sof bits but 5 captures 
+                               break;
+                       case FAST_ADVANCED:
+                               m = AC4K;
+                               sofBits = 5; //3 sof bits but 5 captures 
+                               break;
+                       default:
+                               break;
+               }
+       } else {
+               switch (tag.mode) {
+                       case STANDARD:
+                               m = MC4K;
+                               sofBits = 0; //in theory 1
+                               break;
+                       case ADVANCED:
+                               m = MC4K;
+                               sofBits = 5; //in theory 6
+                               break;
+                       case FAST_ADVANCED:
+                               m = MC8K;
+                               sofBits = 5; //in theory 6
+                               break;
+                       default:
+                               break;
+               }
+       }
+       
+       //rising AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK | AT91C_TC_ETRGEDG_RISING | AT91C_TC_ABETRG | AT91C_TC_LDRA_RISING;
+       AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK | AT91C_TC_ETRGEDG_FALLING | AT91C_TC_ABETRG | AT91C_TC_LDRA_FALLING;
+
+
+       //first capture timing values
+       while (AT91C_BASE_TC1->TC_CV < T0 * HITAG_T_WAIT_MAX ) {
+               // Check if rising edge in 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);
+
+                       LED_B_ON();
+                       // Reset timer every frame, we have to capture the last edge for timing
+                       AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
+                       //AT91C_BASE_TC0->TC_CCR = AT91C_TC_SWTRG;
+
+                       if (rawLen >= 200) { //avoid exception
+                               break;
+                       }
+                       rawMod[rawLen] = ra;
+                       rawLen++;
+
+                       // 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 (rawLen > 2) {
+                                       if (DEBUG >= 2) { Dbprintf("AT91C_BASE_TC1->TC_CV > T0 * HITAG_T_EOF breaking (%d)", rawLen); }
+                                       break;
+                               }
+                       }
+               }
+       }
+
+       if (DEBUG >= 2) {
+               for (i=0; i < rawLen; i+=20) {
+                       Dbprintf("raw modulation: - %d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d", 
+                               rawMod[i],rawMod[i+1],rawMod[i+2],rawMod[i+3], rawMod[i+4],rawMod[i+5],rawMod[i+6],rawMod[i+7],
+                               rawMod[i+8],rawMod[i+9],rawMod[i+10],rawMod[i+11], rawMod[i+12],rawMod[i+13],rawMod[i+14],rawMod[i+15],
+                               rawMod[i+16],rawMod[i+17],rawMod[i+18],rawMod[i+19]
+                       );
+               }
+       }
+
+       switch (m) {
+               // DATA           |  1 |  0 |  1 |  1 |  0 |
+               // Manchester     |--__|__--|--__|--__|__--|
+               // Anti Collision |-_-_|--__|-_-_|-_-_|--__|
+        //                |<-->|
+        //                | T  |
+               case AC2K:
+                       if (DEBUG >= 2) { Dbprintf("decoding frame with modulation AC2K"); }
+                       hitag_decode_frame_AC(2, sofBits, rx, rxlen, response, rawMod, rawLen);
+                       break;
+               case AC4K:
+                       if (DEBUG >= 2) { Dbprintf("decoding frame with modulation AC4K"); }
+                       hitag_decode_frame_AC(4, sofBits, rx, rxlen, response, rawMod, rawLen);
+                       break;
+               case MC4K:
+                       if (DEBUG >= 2) { Dbprintf("decoding frame with modulation MC4K"); }
+                       hitag_decode_frame_MC(4, sofBits, rx, rxlen, response, rawMod, rawLen);
+                       break;
+               case MC8K:
+                       if (DEBUG >= 2) { Dbprintf("decoding frame with modulation MC8K"); }
+                       hitag_decode_frame_MC(8, sofBits, rx, rxlen, response, rawMod, rawLen);
+                       break;
+       }
+
+       LED_B_OFF();
+       if (DEBUG >= 2) {
+               int rb[200] = {0}; int z = 0;
+               for (i = 0; i < 16; i++) { for (int j = 0; j < 8; j++) {
+                       rb[z] = 0;
+                       if ((rx[i] & ((1 << 7) >> j)) != 0) { rb[z] = 1; }
+                       z++;
+               } }
+               for (i=0; i < z; i+=8) {
+                       Dbprintf("raw bit: - %d%d%d%d%d%d%d%d", rb[i],rb[i+1],rb[i+2],rb[i+3],rb[i+4],rb[i+5],rb[i+6],rb[i+7] );
+               }
+       }
+}
+
+static void hitag_start_auth(byte_t* tx, size_t* txlen) {
+       *txlen = 5;
+       switch (tag.mode) {
+               case STANDARD:
+                       //00110 - 0x30 - STANDARD MODE
+                       memcpy(tx, "\x30", nbytes(*txlen));
+                       break;
+               case ADVANCED:
+                       //11000 - 0xc0 - Advance Mode
+                       memcpy(tx, "\xc0", nbytes(*txlen));
+                       break;
+               case FAST_ADVANCED:
+                       //TODO!
+                       break;
+               default: //STANDARD MODE
+                       memcpy(tx, "\x30", nbytes(*txlen));
+                       break;
+       }
+       tag.pstate = READY;
+       tag.tstate = NO_OP;
+}
+
+static int hitag_read_page(hitag_function htf, uint64_t key, byte_t* rx, size_t* rxlen, byte_t* tx, size_t* txlen, int pageNum) {
+       int i, j, z;
+       int response_bit[200];
+       unsigned char mask = 1;
+       unsigned char crc;
+       unsigned char pageData[32];
+
+       if (pageNum >= tag.max_page) {
+               return -1;
+       }
+       if (tag.pstate == SELECTED && tag.tstate == NO_OP && *rxlen > 0) {
+               //send read request
+               tag.tstate = READING_PAGE;
+               *txlen = 20;
+               crc = CRC_PRESET;
+               tx[0] = 0xc0 + (pageNum / 16);
+               calc_crc(&crc, tx[0], 8);
+               calc_crc(&crc, 0x00 + ((pageNum % 16) * 16), 4);
+               tx[1] = 0x00 + ((pageNum % 16) * 16) + (crc / 16);
+               tx[2] = 0x00 + (crc % 16) * 16;  
+       } else if (tag.pstate == SELECTED && tag.tstate == READING_PAGE && *rxlen > 0) {
+               //save received data
+               z = 0;
+               for (i = 0; i < 4; i++) {
+                       for (j = 0; j < 8; j++) {
+                               response_bit[z] = 0;
+                               if ((rx[i] & ((mask << 7) >> j)) != 0) {
+                                       response_bit[z] = 1;
+                               }
+                               if (z < 32) {
+                                       pageData[z] = response_bit[z];
+                               }
+
+                               z++;
+                       }
+               }
+               for (i = 0; i < 4; i++) {
+                       tag.pages[pageNum][i] = 0x0;
+               }
+               for (i = 0; i < 4; i++) {
+                       tag.pages[pageNum][i] += ((pageData[i * 8] << 7) | (pageData[1 + (i * 8)] << 6) | 
+                                       (pageData[2 + (i * 8)] << 5) | (pageData[3 + (i * 8)] << 4) | 
+                                       (pageData[4 + (i * 8)] << 3) | (pageData[5 + (i * 8)] << 2) | 
+                                       (pageData[6 + (i * 8)]
+                                        << 1) | pageData[7 + (i * 8)]);
+               }
+               if (tag.auth && tag.LKP && pageNum == 1) {
+                       Dbprintf("Page[%2d]: %02X %02X %02X %02X", pageNum, pwdh0,
+                                       tag.pages[pageNum][2], tag.pages[pageNum][1], tag.pages[pageNum][0]);
+               } else {
+                       Dbprintf("Page[%2d]: %02X %02X %02X %02X", pageNum,
+                                       tag.pages[pageNum][3], tag.pages[pageNum][2], 
+                                       tag.pages[pageNum][1], tag.pages[pageNum][0]);
+               }
+
+               
+               //display key and password if possible
+               if (pageNum == 1 && tag.auth == 1 && tag.LKP) {
+                       if (htf == 02) { //RHTS_KEY
+                               Dbprintf("Page[ 2]: %02X %02X %02X %02X",
+                                               (byte_t)(key >> 8) & 0xff,
+                                               (byte_t) key & 0xff, pwdl1, pwdl0);
+                               Dbprintf("Page[ 3]: %02X %02X %02X %02X",
+                                               (byte_t)(key >> 40) & 0xff,
+                                               (byte_t)(key >> 32) & 0xff,
+                                               (byte_t)(key >> 24) & 0xff,
+                                               (byte_t)(key >> 16) & 0xff);
+                       } else {
+                               //if the authentication is done with a challenge the key and password are unknown
+                               Dbprintf("Page[ 2]: __ __ __ __");
+                               Dbprintf("Page[ 3]: __ __ __ __");
+                       }
+               }
+
+               *txlen = 20;
+               crc = CRC_PRESET;
+               tx[0] = 0xc0 + ((pageNum+1) / 16);
+               calc_crc(&crc, tx[0], 8);
+               calc_crc(&crc, 0x00 + (((pageNum+1) % 16) * 16), 4);
+               tx[1] = 0x00 + (((pageNum+1) % 16) * 16) + (crc / 16);
+               tx[2] = 0x00 + (crc % 16) * 16;
+
+               return 1;
+       }
+       return 0;
+}
+
+static int hitag_read_block(hitag_function htf, uint64_t key, byte_t* rx, size_t* rxlen, byte_t* tx, size_t* txlen, int blockNum) {
+       int i, j, z;
+       int response_bit[200];
+       unsigned char mask = 1;
+       unsigned char crc;
+       unsigned char blockData[128];
+
+       if (blockNum+4 >= tag.max_page) { //block always = 4 pages
+               return -1;
+       }
+
+       if (tag.pstate == SELECTED && tag.tstate == NO_OP && *rxlen > 0) {
+               //send read request
+               tag.tstate = READING_BLOCK;
+               *txlen = 20;
+               crc = CRC_PRESET;
+               tx[0] = 0xd0 + (blockNum / 16);
+               calc_crc(&crc, tx[0], 8);
+               calc_crc(&crc, 0x00 + ((blockNum % 16) * 16), 4);
+               tx[1] = 0x00 + ((blockNum % 16) * 16) + (crc / 16);
+               tx[2] = 0x00 + (crc % 16) * 16;  
+       } else if (tag.pstate == SELECTED && tag.tstate == READING_BLOCK && *rxlen > 0) {
+               //save received data
+               z = 0;
+               for (i = 0; i < 16; i++) {
+                       for (j = 0; j < 8; j++) {
+                               response_bit[z] = 0;
+                               if ((rx[i] & ((mask << 7) >> j)) != 0) {
+                                       response_bit[z] = 1;
+                               }
+                               if (z < 128) {
+                                       blockData[z] = response_bit[z];
+                               }
+                               z++;
+                       }
+               }
+
+               for (z = 0; z < 4; z++) { //4 pages
+                       for (i = 0; i < 4; i++) {
+                               tag.pages[blockNum+z][i] = 0x0;
+                       }
+               }
+               for (z = 0; z < 4; z++) { //4 pages
+                       for (i = 0; i < 4; i++) {
+                               j = (i * 8) + (z*32); //bit in page + pageStart
+                               tag.pages[blockNum+z][i] = ((blockData[j] << 7) | (blockData[1 + j] << 6) | 
+                                       (blockData[2 + j] << 5) | (blockData[3 + j] << 4) | 
+                                       (blockData[4 + j] << 3) | (blockData[5 + j] << 2) | 
+                                       (blockData[6 + j] << 1) | blockData[7 + j]);
+                       }
+               }
+               if (DEBUG) {
+                       for (z = 0; z < 4; z++) { 
+                               Dbprintf("Page[%2d]: %02X %02X %02X %02X", blockNum+z,
+                                               tag.pages[blockNum+z][3], tag.pages[blockNum+z][2], 
+                                               tag.pages[blockNum+z][1], tag.pages[blockNum+z][0]);
+                       }
+               }
+               Dbprintf("Block[%2d]: %02X %02X %02X %02X - %02X %02X %02X %02X - %02X %02X %02X %02X - %02X %02X %02X %02X", blockNum,
+                                       tag.pages[blockNum][3],   tag.pages[blockNum][2],    tag.pages[blockNum][1],   tag.pages[blockNum][0],
+                                       tag.pages[blockNum+1][3], tag.pages[blockNum+1][2],  tag.pages[blockNum+1][1], tag.pages[blockNum+1][0],
+                                       tag.pages[blockNum+2][3], tag.pages[blockNum+2][2],  tag.pages[blockNum+2][1], tag.pages[blockNum+2][0],
+                                       tag.pages[blockNum+3][3], tag.pages[blockNum+3][2],  tag.pages[blockNum+3][1], tag.pages[blockNum+3][0]);
+
+               *txlen = 20;
+               crc = CRC_PRESET;
+               tx[0] = 0xd0 + ((blockNum+4) / 16);
+               calc_crc(&crc, tx[0], 8);
+               calc_crc(&crc, 0x00 + (((blockNum+4) % 16) * 16), 4);
+               tx[1] = 0x00 + (((blockNum+4) % 16) * 16) + (crc / 16);
+               tx[2] = 0x00 + (crc % 16) * 16;
+
+               return 1;
+       }
+       return 0;
+}
+
+
 /*
  * to check if the right uid was selected
  */
@@ -380,44 +859,83 @@ static void hitagS_handle_reader_command(byte_t* rx, const size_t rxlen,
        u64 state;
        unsigned char crc;
 
-// Copy the (original) received frame how it is send over the air
+       // Copy the (original) received frame how it is send over the air
        memcpy(rx_air, rx, nbytes(rxlen));
-// Reset the transmission frame length
+       // Reset the transmission frame length
        *txlen = 0;
-// Try to find out which command was send by selecting on length (in bits)
+       // Try to find out which command was send by selecting on length (in bits)
        switch (rxlen) {
-       case 5: {
-               //UID request with a selected response protocol mode
-               tag.pstate = READY;
-               tag.tstate = NO_OP;
-               if ((rx[0] & 0xf0) == 0x30) {
-                       tag.mode = STANDARD;
-                       sof_bits = 1;
-                       m = AC2K;
-               }
-               if ((rx[0] & 0xf0) == 0xc0) {
-                       tag.mode = ADVANCED;
-                       sof_bits = 3;
-                       m = AC2K;
+               case 5: {
+                       //UID request with a selected response protocol mode
+                       tag.pstate = READY;
+                       tag.tstate = NO_OP;
+                       if ((rx[0] & 0xf0) == 0x30) {
+                               Dbprintf("recieved uid request in Standard Mode");
+                               tag.mode = STANDARD;
+                               sof_bits = 1;
+                               m = AC2K;
+                       }
+                       if ((rx[0] & 0xf0) == 0xc0) {
+                               Dbprintf("recieved uid request in ADVANCE Mode");
+                               tag.mode = ADVANCED;
+                               sof_bits = 3;
+                               m = AC2K;
+                       }
+                       if ((rx[0] & 0xf0) == 0xd0) {
+                               Dbprintf("recieved uid request in FAST_ADVANCE Mode");
+                               tag.mode = FAST_ADVANCED;
+                               sof_bits = 3;
+                               m = AC4K;
+                       }
+                       //send uid as a response
+                       *txlen = 32;
+                       for (i = 0; i < 4; i++) {
+                               tx[i] = (tag.uid >> (24 - (i * 8))) & 0xff;
+                       }
                }
+               break;
+               case 45: {
+                       //select command from reader received
+                       if (check_select(rx, tag.uid) == 1) {
+                               //if the right tag was selected
+                               *txlen = 32;
+                               switch (tag.mode) {
+                                       case STANDARD:
+                                               Dbprintf("uid selected in Standard Mode");
+                                               sof_bits = 1;
+                                               m = MC4K;
+                                               break;
+                                       case ADVANCED:
+                                               Dbprintf("uid selected in ADVANCE Mode");
+                                               sof_bits = 6;
+                                               m = MC4K;
+                                               break;
+                                       case FAST_ADVANCED:
+                                               Dbprintf("uid selected in FAST_ADVANCE Mode");
+                                               sof_bits = 6;
+                                               m = MC8K;
+                                               break;
+                                       default:
+                                               break;
+                               }
 
-               if ((rx[0] & 0xf0) == 0xd0) {
-                       tag.mode = FAST_ADVANCED;
-                       sof_bits = 3;
-                       m = AC4K;
+                               //send configuration
+                               tx[0] = tag.pages[1][3];
+                               tx[1] = tag.pages[1][2];
+                               tx[2] = tag.pages[1][1];
+                               tx[3] = 0xff;
+                               if (tag.mode != STANDARD) {
+                                       *txlen = 40;
+                                       crc = CRC_PRESET;
+                                       for (i = 0; i < 4; i++)
+                                               calc_crc(&crc, tx[i], 8);
+                                       tx[4] = crc;
+                               }
+                       }
                }
-               //send uid as a response
-               *txlen = 32;
-               for (i = 0; i < 4; i++)
-                       tx[i] = (tag.uid >> (24 - (i * 8))) & 0xff;
-       }
                break;
-       case 45: {
-               //select command from reader received
-               if (check_select(rx, tag.uid) == 1) {
-                       //if the right tag was selected
-                       *txlen = 32;
-                       switch (tag.mode) {
+       case 64: {
+               switch (tag.mode) {
                        case STANDARD:
                                sof_bits = 1;
                                m = MC4K;
@@ -432,53 +950,23 @@ static void hitagS_handle_reader_command(byte_t* rx, const size_t rxlen,
                                break;
                        default:
                                break;
-                       }
-
-                       //send configuration
-                       for (i = 0; i < 4; i++)
-                               tx[i] = (tag.pages[0][1] >> (i * 8)) & 0xff;
-                       tx[3] = 0xff;
-                       if (tag.mode != STANDARD) {
-                               *txlen = 40;
-                               crc = CRC_PRESET;
-                               for (i = 0; i < 4; i++)
-                                       calc_crc(&crc, tx[i], 8);
-                               tx[4] = crc;
-                       }
                }
-       }
-               break;
-       case 64: {
                //challenge message received
                Dbprintf("Challenge for UID: %X", temp_uid);
                temp2++;
-               *txlen = 32;
                state = hitag2_init(rev64(tag.key), rev32(tag.pages[0][0]),
                                rev32(((rx[3] << 24) + (rx[2] << 16) + (rx[1] << 8) + rx[0])));
                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 STANDARD:
-                       sof_bits = 1;
-                       m = MC4K;
-                       break;
-               case ADVANCED:
-                       sof_bits = 6;
-                       m = MC4K;
-                       break;
-               case FAST_ADVANCED:
-                       sof_bits = 6;
-                       m = MC8K;
-                       break;
-               default:
-                       break;
-               }
 
-               for (i = 0; i < 4; i++)
+               for (i = 0; i < 4; i++) {
                        hitag2_byte(&state);
+               }
+
+               *txlen = 32;
                //send con2,pwdh0,pwdl0,pwdl1 encrypted as a response
-               tx[0] = hitag2_byte(&state) ^ ((tag.pages[0][1] >> 16) & 0xff);
+               tx[0] = hitag2_byte(&state) ^ tag.pages[1][1];
                tx[1] = hitag2_byte(&state) ^ tag.pwdh0;
                tx[2] = hitag2_byte(&state) ^ tag.pwdl0;
                tx[3] = hitag2_byte(&state) ^ tag.pwdl1;
@@ -486,33 +974,22 @@ static void hitagS_handle_reader_command(byte_t* rx, const size_t rxlen,
                        //add crc8
                        *txlen = 40;
                        crc = CRC_PRESET;
-                       calc_crc(&crc, ((tag.pages[0][1] >> 16) & 0xff), 8);
+                       calc_crc(&crc, tag.pages[1][1], 8);
                        calc_crc(&crc, tag.pwdh0, 8);
                        calc_crc(&crc, tag.pwdl0, 8);
                        calc_crc(&crc, tag.pwdl1, 8);
                        tx[4] = (crc ^ hitag2_byte(&state));
                }
-               /*
-                * some readers do not allow to authenticate multiple times in a row with the same tag.
-                * use this to change the uid between authentications.
-                */
-
-               /*
-                if (temp2 % 2 == 0) {
-                tag.uid = 0x11223344;
-                tag.pages[0][0] = 0x44332211;
-                } else {
-                tag.uid = 0x55667788;
-                tag.pages[0][0] = 0x88776655;
-                }
-                */
        }
        case 40:
                //data received to be written
                if (tag.tstate == WRITING_PAGE_DATA) {
                        tag.tstate = NO_OP;
-                       tag.pages[page_to_be_written / 4][page_to_be_written % 4] = (rx[0]
-                                       << 0) + (rx[1] << 8) + (rx[2] << 16) + (rx[3] << 24);
+                       tag.pages[page_to_be_written][0] = rx[3];
+                       tag.pages[page_to_be_written][1] = rx[2];
+                       tag.pages[page_to_be_written][2] = rx[1];
+                       tag.pages[page_to_be_written][3] = rx[0];
+
                        //send ack
                        *txlen = 2;
                        tx[0] = 0x40;
@@ -534,8 +1011,11 @@ static void hitagS_handle_reader_command(byte_t* rx, const size_t rxlen,
                                break;
                        }
                } else if (tag.tstate == 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];
+                       tag.pages[page_to_be_written][0] = rx[0];
+                       tag.pages[page_to_be_written][1] = rx[1];
+                       tag.pages[page_to_be_written][2] = rx[2];
+                       tag.pages[page_to_be_written][3] = rx[3];
+
                        //send ack
                        *txlen = 2;
                        tx[0] = 0x40;
@@ -565,15 +1045,16 @@ static void hitagS_handle_reader_command(byte_t* rx, const size_t rxlen,
                break;
        case 20: {
                //write page, write block, read page or read block command received
-               if ((rx[0] & 0xf0) == 0xc0) //read page
-                               {
+               if ((rx[0] & 0xf0) == 0xc0) { //read page
                        //send page data
                        page = ((rx[0] & 0x0f) * 16) + ((rx[1] & 0xf0) / 16);
+                       Dbprintf("reading page %d", page);
                        *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;
 
@@ -608,34 +1089,34 @@ static void hitagS_handle_reader_command(byte_t* rx, const size_t rxlen,
                                sof_bits = 0;
                                *txlen = 0;
                        }
-               } else if ((rx[0] & 0xf0) == 0xd0) //read block
-                               {
+               } else if ((rx[0] & 0xf0) == 0xd0) { //read block
                        page = ((rx[0] & 0x0f) * 16) + ((rx[1] & 0xf0) / 16);
+                       Dbprintf("reading block %d", page);
                        *txlen = 32 * 4;
                        //send page,...,page+3 data
                        for (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;
+                               tx[0 + (i * 4)] = tag.pages[page][0];
+                               tx[1 + (i * 4)] = tag.pages[page][1];
+                               tx[2 + (i * 4)] = tag.pages[page][2];
+                               tx[3 + (i * 4)] = tag.pages[page][3];
                                page++;
                        }
 
                        switch (tag.mode) {
-                       case STANDARD:
-                               sof_bits = 1;
-                               m = MC4K;
-                               break;
-                       case ADVANCED:
-                               sof_bits = 6;
-                               m = MC4K;
-                               break;
-                       case FAST_ADVANCED:
-                               sof_bits = 6;
-                               m = MC8K;
-                               break;
-                       default:
-                               break;
+                               case STANDARD:
+                                       sof_bits = 1;
+                                       m = MC4K;
+                                       break;
+                               case ADVANCED:
+                                       sof_bits = 6;
+                                       m = MC4K;
+                                       break;
+                               case FAST_ADVANCED:
+                                       sof_bits = 6;
+                                       m = MC8K;
+                                       break;
+                               default:
+                                       break;
                        }
 
                        if (tag.mode != STANDARD) {
@@ -651,8 +1132,7 @@ static void hitagS_handle_reader_command(byte_t* rx, const size_t rxlen,
                                sof_bits = 0;
                                *txlen = 0;
                        }
-               } else if ((rx[0] & 0xf0) == 0x80) //write page
-                               {
+               } else if ((rx[0] & 0xf0) == 0x80) { //write page
                        page = ((rx[0] & 0x0f) * 16) + ((rx[1] & 0xf0) / 16);
 
                        switch (tag.mode) {
@@ -683,8 +1163,7 @@ static void hitagS_handle_reader_command(byte_t* rx, const size_t rxlen,
                                tag.tstate = WRITING_PAGE_DATA;
                        }
 
-               } else if ((rx[0] & 0xf0) == 0x90) //write block
-                               {
+               } else if ((rx[0] & 0xf0) == 0x90) { //write block
                        page = ((rx[0] & 0x0f) * 6) + ((rx[1] & 0xf0) / 16);
                        switch (tag.mode) {
                        case STANDARD:
@@ -722,13 +1201,14 @@ static void hitagS_handle_reader_command(byte_t* rx, const size_t rxlen,
        }
 }
 
+
 /*
  * to autenticate to a tag with the given key or challenge
  */
-static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr, byte_t* rx, const size_t rxlen, byte_t* tx,
-               size_t* txlen) {
+static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr, byte_t* rx, 
+               const size_t rxlen, byte_t* tx, size_t* txlen) {
        byte_t rx_air[HITAG_FRAME_LEN];
-       int response_bit[200];
+       int response_bit[200] = {0};
        int i, j, z, k;
        unsigned char mask = 1;
        unsigned char uid[32];
@@ -740,7 +1220,11 @@ static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr
        memcpy(rx_air, rx, nbytes(rxlen));
        *txlen = 0;
 
-       if (tag.pstate == READY && rxlen >= 67) {
+       if (DEBUG) {
+        Dbprintf("START hitagS_handle_tag_auth - rxlen: %d, tagstate=%d", rxlen, (int)tag.pstate);
+    }
+
+       if (tag.pstate == READY && rxlen >= 32) {
                //received uid
                if(end==true) {
                        Dbprintf("authentication failed!");
@@ -755,13 +1239,10 @@ static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr
                                z++;
                        }
                }
-               k = 0;
-               for (i = 5; i < z; i += 2) {
-                       uid[k] = response_bit[i];
-                       k++;
-                       if (k > 31)
-                               break;
+               for (i = 0; i < 32; i++) {
+                       uid[i] = response_bit[i];
                }
+               
                uid1 = (uid[0] << 7) | (uid[1] << 6) | (uid[2] << 5) | (uid[3] << 4)
                                | (uid[4] << 3) | (uid[5] << 2) | (uid[6] << 1) | uid[7];
                uid2 = (uid[8] << 7) | (uid[9] << 6) | (uid[10] << 5) | (uid[11] << 4)
@@ -769,34 +1250,36 @@ static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr
                uid3 = (uid[16] << 7) | (uid[17] << 6) | (uid[18] << 5) | (uid[19] << 4)
                                | (uid[20] << 3) | (uid[21] << 2) | (uid[22] << 1) | uid[23];
                uid4 = (uid[24] << 7) | (uid[25] << 6) | (uid[26] << 5) | (uid[27] << 4)
-                               | (uid[28] << 3) | (uid[29] << 2) | (uid[30] << 1) | uid[31];
-               if (DEBUG)
-                       Dbprintf("UID: %02X %02X %02X %02X", uid1, uid2, uid3, uid4);
+                               | (uid[28] << 3) | (uid[29] << 2) | (uid[30] << 1) | uid[31];   
+               Dbprintf("UID: %02X %02X %02X %02X", uid1, uid2, uid3, uid4);
                tag.uid = (uid4 << 24 | uid3 << 16 | uid2 << 8 | uid1);
 
                //select uid
-               *txlen = 45;
                crc = CRC_PRESET;
                calc_crc(&crc, 0x00, 5);
                calc_crc(&crc, uid1, 8);
                calc_crc(&crc, uid2, 8);
                calc_crc(&crc, uid3, 8);
                calc_crc(&crc, uid4, 8);
+               Dbprintf("crc: %02X", crc);
+
+               //resetting response bit
                for (i = 0; i < 100; i++) {
                        response_bit[i] = 0;
                }
-               for (i = 0; i < 5; i++) {
-                       response_bit[i] = 0;
-               }
+
+               //skip the first 5 
                for (i = 5; i < 37; i++) {
                        response_bit[i] = uid[i - 5];
                }
+               //add crc value
                for (j = 0; j < 8; j++) {
                        response_bit[i] = 0;
                        if ((crc & ((mask << 7) >> j)) != 0)
                                response_bit[i] = 1;
                        i++;
                }
+
                k = 0;
                for (i = 0; i < 6; i++) {
                        tx[i] = (response_bit[k] << 7) | (response_bit[k + 1] << 6)
@@ -805,63 +1288,78 @@ static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr
                                        | (response_bit[k + 6] << 1) | response_bit[k + 7];
                        k += 8;
                }
+               *txlen = 45;
                tag.pstate = INIT;
-       } else if (tag.pstate == INIT && rxlen == 44) {
+       } else if (tag.pstate == INIT && rxlen > 24) {
                // received configuration after select command
                z = 0;
-               for (i = 0; i < 6; i++) {
+               for (i = 0; i < 4; i++) {
                        for (j = 0; j < 8; j++) {
                                response_bit[z] = 0;
-                               if ((rx[i] & ((mask << 7) >> j)) != 0)
+                               if ((rx[i] & ((mask << 7) >> j)) != 0) {
                                        response_bit[z] = 1;
+                               }
                                z++;
                        }
                }
-               conf_pages[0] = ((response_bit[4] << 7) | (response_bit[5] << 6)
-                               | (response_bit[6] << 5) | (response_bit[7] << 4)
-                               | (response_bit[8] << 3) | (response_bit[9] << 2)
-                               | (response_bit[10] << 1) | response_bit[11]);
+
                //check wich memorysize this tag has
-               if (response_bit[10] == 0 && response_bit[11] == 0)
+               //CON0
+               if (response_bit[6] == 0 && response_bit[7] == 0)
                        tag.max_page = 32 / 32;
-               if (response_bit[10] == 0 && response_bit[11] == 1)
+               if (response_bit[6] == 0 && response_bit[7] == 1)
                        tag.max_page = 256 / 32;
-               if (response_bit[10] == 1 && response_bit[11] == 0)
+               if (response_bit[6] == 1 && response_bit[7] == 0)
+                       tag.max_page = 2048 / 32;
+               if (response_bit[6] == 1 && response_bit[7] == 1) //reserved but some tags got this setting
                        tag.max_page = 2048 / 32;
-               conf_pages[1] = ((response_bit[12] << 7) | (response_bit[13] << 6)
-                               | (response_bit[14] << 5) | (response_bit[15] << 4)
-                               | (response_bit[16] << 3) | (response_bit[17] << 2)
-                               | (response_bit[18] << 1) | response_bit[19]);
-               tag.auth = response_bit[12];
-               tag.TTFC = response_bit[13];
-               //tag.TTFDR in response_bit[14] and response_bit[15]
-               //tag.TTFM in response_bit[16] and response_bit[17]
-               tag.LCON = response_bit[18];
-               tag.LKP = response_bit[19];
-               conf_pages[2] = ((response_bit[20] << 7) | (response_bit[21] << 6)
-                               | (response_bit[22] << 5) | (response_bit[23] << 4)
-                               | (response_bit[24] << 3) | (response_bit[25] << 2)
-                               | (response_bit[26] << 1) | response_bit[27]);
-               tag.LCK7 = response_bit[20];
-               tag.LCK6 = response_bit[21];
-               tag.LCK5 = response_bit[22];
-               tag.LCK4 = response_bit[23];
-               tag.LCK3 = response_bit[24];
-               tag.LCK2 = response_bit[25];
-               tag.LCK1 = response_bit[26];
-               tag.LCK0 = response_bit[27];
-
-               if (DEBUG)
-                       Dbprintf("conf0: %02X conf1: %02X conf2: %02X", conf_pages[0],
-                                       conf_pages[1], conf_pages[2]);
+
+               //CON1
+               tag.auth = response_bit[8];
+               tag.TTFC = response_bit[9];
+               //tag.TTFDR in response_bit[10] and response_bit[11]
+               //tag.TTFM in response_bit[12] and response_bit[13]
+               tag.LCON = response_bit[14];
+               tag.LKP = response_bit[15];
+               
+               //CON2
+               tag.LCK7 = response_bit[16];
+               tag.LCK6 = response_bit[17];
+               tag.LCK5 = response_bit[18];
+               tag.LCK4 = response_bit[19];
+               tag.LCK3 = response_bit[20];
+               tag.LCK2 = response_bit[21];
+               tag.LCK1 = response_bit[22];
+               tag.LCK0 = response_bit[23];
+
+               if (DEBUG) {
+                       conf_pages[0] = ((response_bit[0] << 7) | (response_bit[1] << 6)
+                               | (response_bit[2] << 5) | (response_bit[3] << 4)
+                               | (response_bit[4] << 3) | (response_bit[5] << 2)
+                               | (response_bit[6] << 1) | response_bit[7]);
+                       conf_pages[1] = ((response_bit[8] << 7) | (response_bit[9] << 6)
+                               | (response_bit[10] << 5) | (response_bit[11] << 4)
+                               | (response_bit[12] << 3) | (response_bit[13] << 2)
+                               | (response_bit[14] << 1) | response_bit[15]);
+                       conf_pages[2] = ((response_bit[16] << 7) | (response_bit[17] << 6)
+                               | (response_bit[18] << 5) | (response_bit[19] << 4)
+                               | (response_bit[20] << 3) | (response_bit[21] << 2)
+                               | (response_bit[22] << 1) | response_bit[23]);
+                       Dbprintf("conf0: %02X conf1: %02X conf2: %02X", conf_pages[0], conf_pages[1], conf_pages[2]);
+                       Dbprintf("tag.max_page: %d, tag.auth: %d", tag.max_page, tag.auth);
+               }
+
                if (tag.auth == 1) {
                        //if the tag is in authentication mode try the key or challenge
                        *txlen = 64;
                        if(end!=true){
                                if(htf==02||htf==04){ //RHTS_KEY //WHTS_KEY
-                                       state = hitag2_init(rev64(key), rev32(tag.uid),
-                                                       rev32(rnd));
-
+                                       state = hitag2_init(rev64(key), rev32(tag.uid), rev32(rnd));
+                                       /*
+                                       Dbprintf("key: %02X %02X\n\n", key, rev64(key)); 
+                                       Dbprintf("tag.uid: %02X %02X\n\n", tag.uid, rev32(tag.uid)); 
+                                       Dbprintf("rnd: %02X %02X\n\n", rnd, rev32(rnd)); 
+                                       */
                                        for (i = 0; i < 4; i++) {
                                                auth_ks[i] = hitag2_byte(&state) ^ 0xff;
                                        }
@@ -892,14 +1390,8 @@ static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr
                        tag.pstate = SELECTED;
                }
                
-       } else if (tag.pstate == AUTHENTICATE && rxlen == 44) {
+       } else if (tag.pstate == AUTHENTICATE && rxlen >= 32) {
                //encrypted con2,password received.
-               crc = CRC_PRESET;
-               calc_crc(&crc, 0x80, 1);
-               calc_crc(&crc, ((rx[0] & 0x0f) * 16 + ((rx[1] & 0xf0) / 16)), 8);
-               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) {
                        Dbprintf("UID:::%X", tag.uid);
                        Dbprintf("RND:::%X", rnd);
@@ -909,30 +1401,26 @@ static int hitagS_handle_tag_auth(hitag_function htf,uint64_t key, uint64_t NrAr
                pwdh0=0;
                pwdl0=0;
                pwdl1=0;
-               if(htf==02 || htf==04){ //RHTS_KEY //WHTS_KEY
-               {
+               if(htf==02 || htf==04) { //RHTS_KEY //WHTS_KEY
                        state = hitag2_init(rev64(key), rev32(tag.uid), rev32(rnd));
-                       for (i = 0; i < 5; i++)
+                       for (i = 0; i < 5; i++)  {
                                hitag2_byte(&state);
-                       pwdh0 = ((rx[1] & 0x0f) * 16 + ((rx[2] & 0xf0) / 16))
-                                       ^ hitag2_byte(&state);
-                       pwdl0 = ((rx[2] & 0x0f) * 16 + ((rx[3] & 0xf0) / 16))
-                                       ^ hitag2_byte(&state);
-                       pwdl1 = ((rx[3] & 0x0f) * 16 + ((rx[4] & 0xf0) / 16))
-                                       ^ hitag2_byte(&state);
+                       }
+                       pwdh0 = ((rx[1] & 0x0f) * 16 + ((rx[2] & 0xf0) / 16)) ^ hitag2_byte(&state);
+                       pwdl0 = ((rx[2] & 0x0f) * 16 + ((rx[3] & 0xf0) / 16)) ^ hitag2_byte(&state);
+                       pwdl1 = ((rx[3] & 0x0f) * 16 + ((rx[4] & 0xf0) / 16)) ^ hitag2_byte(&state);
+                       if (DEBUG) {
+                               Dbprintf("pwdh0 %02X pwdl0 %02X pwdl1 %02X", pwdh0, pwdl0, pwdl1);
+                       }
                }
-
-               if (DEBUG)
-                       Dbprintf("pwdh0 %02X pwdl0 %02X pwdl1 %02X", pwdh0, pwdl0, pwdl1);
-               
-
-               //Dbprintf("%X %02X", rnd, ((rx[4] & 0x0f) * 16) + ((rx[5] & 0xf0) / 16));
-               //rnd += 1;
-       }
                tag.pstate = SELECTED; //tag is now ready for read/write commands
        }
+
+       if (DEBUG) {
+        Dbprintf("END hitagS_handle_tag_auth - tagstate=%d", (int)tag.pstate);
+    }
+
        return 0;
-       
 }
 
 /*
@@ -949,6 +1437,7 @@ void SimulateHitagSTag(bool tag_mem_supplied, byte_t* data) {
        byte_t txbuf[HITAG_FRAME_LEN];
        byte_t* tx = txbuf;
        size_t txlen = 0;
+       uint8_t con0, con1, con2;
        BigBuf_free();
 
 // Clean up trace and prepare it for storing frames
@@ -960,102 +1449,104 @@ void SimulateHitagSTag(bool tag_mem_supplied, byte_t* data) {
 
        tag.pstate = READY;
        tag.tstate = 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) {
                DbpString("Loading hitagS memory...");
-               memcpy((byte_t*)tag.pages,data,4*64);
+               for (i = 0; i < 64; i++) {
+                       for (j = 0; j < 4; j++) {
+                               tag.pages[i][j] = 0x0;
+                       }
+               }
+
+               for (i = 0; i < 64; i++) { 
+                       for (j = 0; j < 4; j++) {
+                               tag.pages[i][j] = data[(i*4)+j];
+                       }
+               }
        }
-       tag.uid=(uint32_t)tag.pages[0];
+       tag.uid = (tag.pages[0][3] << 24 | tag.pages[0][2] << 16 | tag.pages[0][1] << 8 | tag.pages[0][0]);
+       con0 = tag.pages[1][3];
+       con1 = tag.pages[1][2];
+       con2 = tag.pages[1][1];
+       Dbprintf("UID: %X", tag.uid);
        Dbprintf("Hitag S simulation started");
-       tag.key=(intptr_t)tag.pages[3];
-       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];
+
+       //0x01 plain mode - Reserved, CON2, CON1, CON0
+       //0x01 auth  mode - PWDH 0,   CON2, CON1, CON0
+       //0x02 auth  mode - KEYH 1, KEYH 0, PWDL 1, PWDL 0
+       //0x03 auth  mode - KEYL 3, KEYL 2, KEYL 1, KEYL 0
+
        //con0
-       tag.max_page=64;
-       if((tag.pages[1][3]&0x2)==0 && (tag.pages[1][3]&0x1)==1)
-               tag.max_page=8;
-       if((tag.pages[1][3]&0x2)==0 && (tag.pages[1][3]&0x1)==0)
-               tag.max_page=0;
-       //con1
-       tag.auth=0;
-       if (tag.pages[1][2]&0x80)
-               tag.auth=1;
-       tag.LCON=0;
-       if (tag.pages[1][2]&0x2)
-               tag.LCON=1;
-       tag.LKP=0;
-       if (tag.pages[1][2]&0x1)
-               tag.LKP=1;
-       //con2
-       //0=read write 1=read only
-       tag.LCK7=0;
-       if (tag.pages[1][1]&0x80)
-               tag.LCK7=1;
-       tag.LCK6=0;
-       if (tag.pages[1][1]&0x40)
-               tag.LCK6=1;
-       tag.LCK5=0;
-       if (tag.pages[1][1]&0x20)
-               tag.LCK5=1;
-       tag.LCK4=0;
-       if (tag.pages[1][1]&0x10)
-               tag.LCK4=1;
-       tag.LCK3=0;
-       if (tag.pages[1][1]&0x8)
-               tag.LCK3=1;
-       tag.LCK2=0;
-       if (tag.pages[1][1]&0x4)
-               tag.LCK2=1;
-       tag.LCK1=0;
-       if (tag.pages[1][1]&0x2)
-               tag.LCK1=1;
-       tag.LCK0=0;
-       if (tag.pages[1][1]&0x1)
-               tag.LCK0=1;
-
-// Set up simulator mode, frequency divisor which will drive the FPGA
-// and analog mux selection.
+       tag.max_page = 2048 / 32;
+       if ((con0 & 0x2) == 0 && (con0 & 0x1) == 1)
+               tag.max_page = 256 / 32;
+       if ((con0 & 0x2) == 0 && (con0 & 0x1) == 0)
+               tag.max_page = 32 / 32;
+
+       //CON1
+       tag.auth = ((con1 & 0x80) == 0x80) ? 1 : 0;
+       tag.TTFC = ((con1 & 0x40) == 0x40) ? 1 : 0;
+       //tag.TTFDR in response_bit[10] and response_bit[11]
+       //tag.TTFM in response_bit[12] and response_bit[13]
+       tag.LCON = ((con1 & 0x2) == 0x2) ? 1 : 0;
+       tag.LKP  = ((con1 & 0x2) == 0x1) ? 1 : 0;
+               
+       //CON2
+       tag.LCK7 = ((con2 & 0x80) == 0x80) ? 1 : 0;
+       tag.LCK6 = ((con2 & 0x40) == 0x40) ? 1 : 0;
+       tag.LCK5 = ((con2 & 0x20) == 0x20) ? 1 : 0;
+       tag.LCK4 = ((con2 & 0x10) == 0x10) ? 1 : 0;
+       tag.LCK3 = ((con2 & 0x8) == 0x8) ? 1 : 0;
+       tag.LCK2 = ((con2 & 0x4) == 0x4) ? 1 : 0;
+       tag.LCK1 = ((con2 & 0x2) == 0x2) ? 1 : 0;
+       tag.LCK0 = ((con2 & 0x1) == 0x1) ? 1 : 0;
+
+       if (tag.auth == 1) {
+               //TODO check if this working :D
+               tag.key=(intptr_t)tag.pages[3];
+               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];
+       }
+
+       // Set up simulator mode, frequency divisor which will drive the FPGA
+       // and analog mux selection.
        FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
        FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_EDGE_DETECT);
        FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
        SetAdcMuxFor(GPIO_MUXSEL_LOPKD);
        RELAY_OFF();
 
-// Configure output pin that is connected to the FPGA (for modulating)
+       // Configure output pin that is connected to the FPGA (for modulating)
        AT91C_BASE_PIOA->PIO_OER = GPIO_SSC_DOUT;
        AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT;
 
-// Disable modulation at default, which means release resistance
+       // Disable modulation at default, which means release resistance
        LOW(GPIO_SSC_DOUT);
 
-// Enable Peripheral Clock for TIMER_CLOCK0, used to measure exact timing before answering
+       // Enable Peripheral Clock for TIMER_CLOCK0, used to measure exact timing before answering
        AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_TC0);
 
-// Enable Peripheral Clock for TIMER_CLOCK1, used to capture edges of the reader frames
+       // Enable Peripheral Clock for TIMER_CLOCK1, used to capture edges of the reader frames
        AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_TC1);
        AT91C_BASE_PIOA->PIO_BSR = GPIO_SSC_FRAME;
 
-// Disable timer during configuration
+       // Disable timer during configuration
        AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
 
-// 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;
+       // 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;
 
-// Reset the received frame, frame count and timing info
+       // Reset the received frame, frame count and timing info
        memset(rx, 0x00, sizeof(rx));
        frame_count = 0;
        response = 0;
        overflow = 0;
 
-// Enable and reset counter
+       // Enable and reset counter
        AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
 
        while (!BUTTON_PRESS()) {
@@ -1063,6 +1554,7 @@ void SimulateHitagSTag(bool tag_mem_supplied, byte_t* data) {
                WDT_HIT();
 
                // Receive frame, watch for at most T0*EOF periods
+               //HITAG_T_WAIT_MAX
                while (AT91C_BASE_TC1->TC_CV < T0 * HITAG_T_EOF) {
                        // Check if rising edge in modulation is detected
                        if (AT91C_BASE_TC1->TC_SR & AT91C_TC_LDRAS) {
@@ -1117,13 +1609,13 @@ void SimulateHitagSTag(bool tag_mem_supplied, byte_t* data) {
                        // with respect to the falling edge, we need to wait actually (T_Wait1 - T_Low)
                        // periods. The gap time T_Low varies (4..10). All timer values are in
                        // terms of T0 units
-                       while (AT91C_BASE_TC0->TC_CV < T0 * (HITAG_T_WAIT_1 - HITAG_T_LOW))
-                               ;
+                       while (AT91C_BASE_TC0->TC_CV < T0 * (HITAG_T_WAIT_1 - HITAG_T_LOW)) { }
 
                        // Send and store the tag answer (if there is any)
                        if (txlen > 0) {
                                // Transmit the tag frame
-                               hitag_send_frame(tx, txlen);
+                               hitag_tag_send_frame(tx, txlen);
+
                                // Store the frame in the trace
                                if (!bQuiet) {
                                        if (!LogTraceHitag(tx, txlen, 0, 0, false)) {
@@ -1133,12 +1625,13 @@ void SimulateHitagSTag(bool tag_mem_supplied, byte_t* data) {
                                }
                        }
 
+                       // 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
@@ -1148,6 +1641,7 @@ void SimulateHitagSTag(bool tag_mem_supplied, byte_t* data) {
                // Reset the timer to restart while-loop that receives frames
                AT91C_BASE_TC1->TC_CCR = AT91C_TC_SWTRG;
        }
+       Dbprintf("Hitag S simulation stopped");
        LED_B_OFF();
        LED_D_OFF();
        AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS;
@@ -1160,10 +1654,16 @@ void SimulateHitagSTag(bool tag_mem_supplied, byte_t* data) {
  * If the key was given the password will be decrypted.
  * Reads every page of a hitag S transpoder.
  */
-void ReadHitagS(hitag_function htf, hitag_data* htd) {
-       int i, j, z, k;
+void ReadHitagSintern(hitag_function htf, hitag_data* htd, stype tagMode, int startPage, bool readBlock) {
        int frame_count;
-       int response_bit[200];
+       int i;
+       int sendNum = startPage;
+       tag.mode = tagMode;
+
+       //int i, j, z;
+       //int response_bit[200];
+       //unsigned char mask = 1;
+
        int response;
        byte_t rx[HITAG_FRAME_LEN];
        size_t rxlen = 0;
@@ -1171,16 +1671,10 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) {
        byte_t* tx = txbuf;
        size_t txlen = 0;
        int lastbit;
-       bool bSkip;
-       int reset_sof;
-       int tag_sof;
        int t_wait = HITAG_T_WAIT_MAX;
        bool bStop;
        bool bQuitTraceFull = false;
-       int sendNum = 0;
-       unsigned char mask = 1;
-       unsigned char crc;
-       unsigned char pageData[32];
+
        page_to_be_written = 0;
        
        //read given key/challenge
@@ -1189,14 +1683,16 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) {
        uint64_t NrAr=0;
        byte_t key_[6];
        switch(htf) {
-               case 01: { //RHTS_CHALLENGE
+               case 01:
+               case 03: { //RHTS_CHALLENGE
                        DbpString("Authenticating using nr,ar pair:");
                        memcpy(NrAr_,htd->auth.NrAr,8);
                        Dbhexdump(8,NrAr_,false);
                        NrAr=NrAr_[7] | ((uint64_t)NrAr_[6]) << 8 | ((uint64_t)NrAr_[5]) << 16 | ((uint64_t)NrAr_[4]) << 24 | ((uint64_t)NrAr_[3]) << 32 |
                                        ((uint64_t)NrAr_[2]) << 40| ((uint64_t)NrAr_[1]) << 48 | ((uint64_t)NrAr_[0]) << 56;
                } break;
-               case 02: { //RHTS_KEY
+               case 02:
+               case 04: { //RHTS_KEY
                        DbpString("Authenticating using key:");
                        memcpy(key_,htd->crypto.key,6);  
                        Dbhexdump(6,key_,false);
@@ -1219,7 +1715,6 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) {
        clear_trace();
 
        bQuiet = false;
-       bQuitTraceFull = true;
 
        LED_D_ON();
 
@@ -1228,8 +1723,7 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) {
        AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT;
 
 // Set fpga in edge detect with reader field, we can modulate as reader now
-       FpgaWriteConfWord(
-       FPGA_MAJOR_MODE_LF_EDGE_DETECT | FPGA_LF_EDGE_DETECT_READER_FIELD);
+       FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_EDGE_DETECT | FPGA_LF_EDGE_DETECT_READER_FIELD);
 
 // Set Frequency divisor which will drive the FPGA and analog mux selection
        FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95);                                      //125Khz
@@ -1254,9 +1748,7 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) {
 
 // Capture mode, defaul timer source = MCK/2 (TIMER_CLOCK1), TIOA is external trigger,
 // 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 | AT91C_TC_ABETRG | AT91C_TC_LDRA_FALLING;
+       AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK | AT91C_TC_ETRGEDG_FALLING | AT91C_TC_ABETRG | AT91C_TC_LDRA_FALLING;
 
 // Enable and reset counters
        AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
@@ -1267,14 +1759,34 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) {
        response = 0;
        lastbit = 1;
        bStop = false;
-
-       reset_sof = 1;
        t_wait = 200;
 
        while (!bStop && !BUTTON_PRESS()) {
                // Watchdog hit
                WDT_HIT();
 
+
+               // Add transmitted frame to total count
+               if (txlen > 0) {
+                       frame_count++;
+
+                       if (tag.pstate == READY && rxlen < 1) {
+                               //skip logging starting auths if no response
+                       } else {
+                               if (!bQuiet) {
+                                       // Store the frame in the trace
+                                       if (!LogTraceHitag(tx, txlen, HITAG_T_WAIT_2, 0, true)) {
+                                               if (bQuitTraceFull) {
+                                                       DbpString("Trace full");
+                                                       break;
+                                               } else {
+                                                       bQuiet = true;
+                                               }
+                                       }
+                               }
+                       }
+               }
+
                // Check if frame was captured and store it
                if (rxlen > 0) {
                        frame_count++;
@@ -1294,94 +1806,31 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) {
                tx = txbuf;
                txlen = 0;
 
+               if (DEBUG >= 2) {
+               Dbprintf("FRO %d rxlen: %d, pstate=%d, tstate=%d", frame_count, rxlen, (int)tag.pstate,  (int)tag.tstate);
+       }
+
                if (rxlen == 0) {
                        //start authentication
-                       txlen = 5;
-                       memcpy(tx, "\xc0", nbytes(txlen));
-                       tag.pstate = READY;
-                       tag.tstate = NO_OP;
+                       hitag_start_auth(tx, &txlen);
                } else if (tag.pstate != SELECTED) {
-                       if (hitagS_handle_tag_auth(htf, key,NrAr,rx, rxlen, tx, &txlen) == -1)
+                       if (hitagS_handle_tag_auth(htf, key,NrAr,rx, rxlen, tx, &txlen) == -1) {
+                               Dbprintf("hitagS_handle_tag_auth - bStop = !false");
                                bStop = !false;
-               }
-               if (tag.pstate == SELECTED && tag.tstate == NO_OP && rxlen > 0) {
-                       //send read request
-                       tag.tstate = READING_PAGE;
-                       txlen = 20;
-                       crc = CRC_PRESET;
-                       tx[0] = 0xc0 + (sendNum / 16);
-                       calc_crc(&crc, tx[0], 8);
-                       calc_crc(&crc, 0x00 + ((sendNum % 16) * 16), 4);
-                       tx[1] = 0x00 + ((sendNum % 16) * 16) + (crc / 16);
-                       tx[2] = 0x00 + (crc % 16) * 16;
-               } else if (tag.pstate == SELECTED && tag.tstate == READING_PAGE
-                               && rxlen > 0) {
-                       //save received data
-                       z = 0;
-                       for (i = 0; i < 5; i++) {
-                               for (j = 0; j < 8; j++) {
-                                       response_bit[z] = 0;
-                                       if ((rx[i] & ((mask << 7) >> j)) != 0)
-                                               response_bit[z] = 1;
-                                       z++;
-                               }
-                       }
-                       k = 0;
-                       for (i = 4; i < 36; i++) {
-                               pageData[k] = response_bit[i];
-                               k++;
-                       }
-                       for (i = 0; i < 4; i++)
-                               tag.pages[sendNum / 4][sendNum % 4] = 0x0;
-                       for (i = 0; i < 4; i++) {
-                               tag.pages[sendNum / 4][sendNum % 4] += ((pageData[i * 8] << 7)
-                                               | (pageData[1 + (i * 8)] << 6)
-                                               | (pageData[2 + (i * 8)] << 5)
-                                               | (pageData[3 + (i * 8)] << 4)
-                                               | (pageData[4 + (i * 8)] << 3)
-                                               | (pageData[5 + (i * 8)] << 2)
-                                               | (pageData[6 + (i * 8)] << 1) | pageData[7 + (i * 8)])
-                                               << (i * 8);
-                       }
-                       if (tag.auth && tag.LKP && sendNum == 1) {
-                               Dbprintf("Page[%2d]: %02X %02X %02X %02X", sendNum, pwdh0,
-                                               (tag.pages[sendNum / 4][sendNum % 4] >> 16) & 0xff,
-                                               (tag.pages[sendNum / 4][sendNum % 4] >> 8) & 0xff,
-                                               tag.pages[sendNum / 4][sendNum % 4] & 0xff);
-                       } else {
-                               Dbprintf("Page[%2d]: %02X %02X %02X %02X", sendNum,
-                                               (tag.pages[sendNum / 4][sendNum % 4] >> 24) & 0xff,
-                                               (tag.pages[sendNum / 4][sendNum % 4] >> 16) & 0xff,
-                                               (tag.pages[sendNum / 4][sendNum % 4] >> 8) & 0xff,
-                                               tag.pages[sendNum / 4][sendNum % 4] & 0xff);
-                       }
-
-                       sendNum++;
-                       //display key and password if possible
-                       if (sendNum == 2 && tag.auth == 1 && tag.LKP) {
-                               if (htf == 02) { //RHTS_KEY
-                                       Dbprintf("Page[ 2]: %02X %02X %02X %02X",
-                                                       (byte_t)(key >> 8) & 0xff,
-                                                       (byte_t) key & 0xff, pwdl1, pwdl0);
-                                       Dbprintf("Page[ 3]: %02X %02X %02X %02X",
-                                                       (byte_t)(key >> 40) & 0xff,
-                                                       (byte_t)(key >> 32) & 0xff,
-                                                       (byte_t)(key >> 24) & 0xff,
-                                                       (byte_t)(key >> 16) & 0xff);
-                               } else {
-                                       //if the authentication is done with a challenge the key and password are unknown
-                                       Dbprintf("Page[ 2]: __ __ __ __");
-                                       Dbprintf("Page[ 3]: __ __ __ __");
-                               }
                        }
+               }
 
-                       txlen = 20;
-                       crc = CRC_PRESET;
-                       tx[0] = 0xc0 + (sendNum / 16);
-                       calc_crc(&crc, tx[0], 8);
-                       calc_crc(&crc, 0x00 + ((sendNum % 16) * 16), 4);
-                       tx[1] = 0x00 + ((sendNum % 16) * 16) + (crc / 16);
-                       tx[2] = 0x00 + (crc % 16) * 16;
+
+               
+               if (readBlock && tag.pstate == SELECTED && (tag.tstate == READING_BLOCK || tag.tstate == NO_OP) && rxlen > 0) {
+                       i = hitag_read_block(htf, key, rx, &rxlen, tx, &txlen, sendNum);
+                       if (i > 0) { sendNum+=4; }
+                       if (sendNum+4 >= tag.max_page) {
+                               bStop = !false;
+                       }
+               } else if (!readBlock && tag.pstate == SELECTED && (tag.tstate == READING_PAGE || tag.tstate == NO_OP) && rxlen > 0) {
+                       i = hitag_read_page(htf, key, rx, &rxlen, tx, &txlen, sendNum);
+                       if (i > 0) { sendNum++; }
                        if (sendNum >= tag.max_page) {
                                bStop = !false;
                        }
@@ -1397,98 +1846,24 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) {
                // we need to wait (T_Wait2 + half_tag_period) when the last was a 'one'.
                // All timer values are in terms of T0 units
 
-               while (AT91C_BASE_TC0->TC_CV
-                               < T0 * (t_wait + (HITAG_T_TAG_HALF_PERIOD * lastbit)))
-                       ;
+               while (AT91C_BASE_TC0->TC_CV < T0 * (t_wait + (HITAG_T_TAG_HALF_PERIOD * lastbit))) { }
 
                // Transmit the reader frame
                hitag_reader_send_frame(tx, txlen);
 
+
                // Enable and reset external trigger in timer for capturing future frames
                AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
 
-               // Add transmitted frame to total count
-               if (txlen > 0) {
-                       frame_count++;
-                       if (!bQuiet) {
-                               // Store the frame in the trace
-                               if (!LogTraceHitag(tx, txlen, HITAG_T_WAIT_2, 0, true)) {
-                                       if (bQuitTraceFull) {
-                                               DbpString("Trace full");
-                                               break;
-                                       } else {
-                                               bQuiet = true;
-                                       }
-                               }
-                       }
-               }
 
                // Reset values for receiving frames
                memset(rx, 0x00, sizeof(rx));
                rxlen = 0;
                lastbit = 1;
-               bSkip = true;
-               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;
-                       }
-               }
+               // get tag id in anti-collision mode (proprietary data format, so switch off manchester and read at double the data rate, for 4 x the data bits)                
+               hitag_receive_frame(rx, &rxlen, &response);
        }
        end=false;
        LED_B_OFF();
@@ -1499,6 +1874,21 @@ void ReadHitagS(hitag_function htf, hitag_data* htd) {
        cmd_send(CMD_ACK, bSuccessful, 0, 0, 0, 0);
 }
 
+void ReadHitagSCmd(hitag_function htf, hitag_data* htd, uint64_t startPage, uint64_t tagMode, bool readBlock) {
+       if (tagMode == 1) {
+               Dbprintf("ReadHitagS in mode=ADVANCED, blockRead=%d, startPage=%d", readBlock, startPage);
+               ReadHitagSintern(htf, htd, ADVANCED, (int)startPage, readBlock);
+       } else if (tagMode == 2) {
+               Dbprintf("ReadHitagS in mode=FAST_ADVANCED, blockRead=%d, startPage=%d", readBlock, startPage);
+               ReadHitagSintern(htf, htd, FAST_ADVANCED, (int)startPage, readBlock);
+       } else {
+               Dbprintf("ReadHitagS in mode=STANDARD, blockRead=%d, startPage=%d", readBlock, startPage);
+               ReadHitagSintern(htf, htd, STANDARD, (int)startPage, readBlock);
+       }       
+}
+
+
+
 /*
  * Authenticates to the Tag with the given Key or Challenge.
  * Writes the given 32Bit data into page_
@@ -1512,9 +1902,6 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) {
        byte_t* tx = txbuf;
        size_t txlen = 0;
        int lastbit;
-       bool bSkip;
-       int reset_sof;
-       int tag_sof;
        int t_wait = HITAG_T_WAIT_MAX;
        bool bStop;
        bool bQuitTraceFull = false;
@@ -1563,7 +1950,6 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) {
        clear_trace();
 
        bQuiet = false;
-       bQuitTraceFull = true;
 
        LED_D_ON();
 
@@ -1572,8 +1958,7 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) {
        AT91C_BASE_PIOA->PIO_PER = GPIO_SSC_DOUT;
 
 // Set fpga in edge detect with reader field, we can modulate as reader now
-       FpgaWriteConfWord(
-       FPGA_MAJOR_MODE_LF_EDGE_DETECT | FPGA_LF_EDGE_DETECT_READER_FIELD);
+       FpgaWriteConfWord(FPGA_MAJOR_MODE_LF_EDGE_DETECT | FPGA_LF_EDGE_DETECT_READER_FIELD);
 
 // Set Frequency divisor which will drive the FPGA and analog mux selection
        FpgaSendCommand(FPGA_CMD_SET_DIVISOR, 95); //125Khz
@@ -1611,8 +1996,6 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) {
        response = 0;
        lastbit = 1;
        bStop = false;
-
-       reset_sof = 1;
        t_wait = 200;
 
        while (!bStop && !BUTTON_PRESS()) {
@@ -1635,11 +2018,12 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) {
                }
 
                //check for valid input
+               /*
                if (page == 0) {
-                       Dbprintf(
-                                       "usage: lf hitag writer [03 | 04] [CHALLENGE | KEY] [page] [byte0] [byte1] [byte2] [byte3]");           
+                       Dbprintf("usage: lf hitag writer [03 | 04] [CHALLENGE | KEY] [page] [byte0] [byte1] [byte2] [byte3]");
                        bStop = !false;
-               }
+               }*/
+
 
                // By default reset the transmission buffer
                tx = txbuf;
@@ -1651,15 +2035,18 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) {
                        bStop = !false;
                } else if (rxlen == 0 && tag.tstate != WRITING_PAGE_DATA) {
                        //start the authetication
-                       txlen = 5;
-                       memcpy(tx, "\xc0", nbytes(txlen));
-                       tag.pstate = READY;
-                       tag.tstate = NO_OP;
+                       //tag.mode = ADVANCED;
+                       tag.mode = STANDARD;
+                       hitag_start_auth(tx, &txlen);
+                       m = AC2K;
                } else if (tag.pstate != SELECTED) {
                        //try to authenticate with the given key or challenge
-                       if (hitagS_handle_tag_auth(htf,key,NrAr,rx, rxlen, tx, &txlen) == -1)
+                       if (hitagS_handle_tag_auth(htf,key,NrAr,rx, rxlen, tx, &txlen) == -1) {
                                bStop = !false;
+                       }
+                       m = MC4K;
                }
+
                if (tag.pstate == SELECTED && tag.tstate == NO_OP && rxlen > 0) {
                        //check if the given page exists
                        if (page > tag.max_page) {
@@ -1676,7 +2063,7 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) {
                        tx[1] = 0x00 + ((page % 16) * 16) + (crc / 16);
                        tx[2] = 0x00 + (crc % 16) * 16;
                } else if (tag.pstate == SELECTED && tag.tstate == WRITING_PAGE_ACK
-                               && rxlen == 6 && rx[0] == 0xf4) {
+                               && rxlen == 2 && rx[0] == 0x40) {
                        //ACK recieved to write the page. send data
                        tag.tstate = WRITING_PAGE_DATA;
                        txlen = 40;
@@ -1691,7 +2078,7 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) {
                        tx[3] = data[0];
                        tx[4] = crc;
                } else if (tag.pstate == SELECTED && tag.tstate == WRITING_PAGE_DATA
-                               && rxlen == 6 && rx[0] == 0xf4) {
+                               && rxlen == 2 && rx[0] == 0x40) {
                        //received ACK
                        Dbprintf("Successful!");
                        bStop = !false;
@@ -1737,68 +2124,9 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) {
                memset(rx, 0x00, sizeof(rx));
                rxlen = 0;
                lastbit = 1;
-               bSkip = true;
-               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;
-                       }
-               }
+               hitag_receive_frame(rx, &rxlen, &response);
        }
        end=false;
        LED_B_OFF();
@@ -1809,6 +2137,7 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) {
        cmd_send(CMD_ACK, bSuccessful, 0, 0, 0, 0);
 }
 
+
 /*
  * Tries to authenticate to a Hitag S Transponder with the given challenges from a .cc file.
  * Displays all Challenges that failed.
@@ -1816,7 +2145,7 @@ void WritePageHitagS(hitag_function htf, hitag_data* htd,int page_) {
  * is not received correctly due to Antenna problems. This function
  * detects these challenges.
  */
-void check_challenges(bool file_given, byte_t* data) {
+void check_challenges_cmd(bool file_given, byte_t* data, uint64_t tagMode) {
        int i, j, z, k;
        byte_t uid_byte[4];
        int frame_count;
@@ -1829,9 +2158,6 @@ void check_challenges(bool file_given, byte_t* data) {
        byte_t* tx = txbuf;
        size_t txlen = 0;
        int lastbit;
-       bool bSkip;
-       int reset_sof;
-       int tag_sof;
        int t_wait = HITAG_T_WAIT_MAX;
        int STATE = 0;
        bool bStop;
@@ -1841,6 +2167,18 @@ void check_challenges(bool file_given, byte_t* data) {
        unsigned char uid[32];
        unsigned char crc;
 
+       if (tagMode == 1) {
+               Dbprintf("check_challenges in mode=ADVANCED");
+               tag.mode = ADVANCED;
+       } else if (tagMode == 2) {
+               Dbprintf("check_challenges in mode=FAST_ADVANCED");
+               tag.mode = FAST_ADVANCED;
+       } else {
+               Dbprintf("check_challenges in mode=STANDARD");
+               tag.mode = STANDARD;
+       }
+
+
        FpgaDownloadAndGo(FPGA_BITSTREAM_LF);
 // Reset the return status
        bSuccessful = false;
@@ -1850,7 +2188,6 @@ void check_challenges(bool file_given, byte_t* data) {
        clear_trace();
 
        bQuiet = false;
-       bQuitTraceFull = true;
 
        LED_D_ON();
 
@@ -1885,9 +2222,7 @@ void check_challenges(bool file_given, byte_t* data) {
 
 // Capture mode, defaul timer source = MCK/2 (TIMER_CLOCK1), TIOA is external trigger,
 // 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 | AT91C_TC_ABETRG | AT91C_TC_LDRA_FALLING;
+       AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK | AT91C_TC_ETRGEDG_FALLING | AT91C_TC_ABETRG | AT91C_TC_LDRA_FALLING;
 
 // Enable and reset counters
        AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
@@ -1899,7 +2234,6 @@ void check_challenges(bool file_given, byte_t* data) {
        lastbit = 1;
        bStop = false;
 
-       reset_sof = 1;
        t_wait = 200;
 
        if (file_given) {
@@ -1929,18 +2263,17 @@ void check_challenges(bool file_given, byte_t* data) {
                tx = txbuf;
                txlen = 0;
                if (rxlen == 0) {
-                       if (STATE == 2)
+                       if (STATE == 2) {
                                // challenge failed
                                Dbprintf("Challenge failed: %02X %02X %02X %02X %02X %02X %02X %02X",
                                                unlocker[u1 - 1][0], unlocker[u1 - 1][1],
                                                unlocker[u1 - 1][2], unlocker[u1 - 1][3],
                                                unlocker[u1 - 1][4], unlocker[u1 - 1][5],
                                                unlocker[u1 - 1][6], unlocker[u1 - 1][7]);
+                       }
                        STATE = 0;
-                       txlen = 5;
-                       //start new authentication
-                       memcpy(tx, "\xc0", nbytes(txlen));
-               } else if (rxlen >= 67 && STATE == 0) {
+                       hitag_start_auth(tx, &txlen);
+               } else if (rxlen >= 32 && STATE == 0) {
                        //received uid
                        z = 0;
                        for (i = 0; i < 10; i++) {
@@ -1951,13 +2284,10 @@ void check_challenges(bool file_given, byte_t* data) {
                                        z++;
                                }
                        }
-                       k = 0;
-                       for (i = 5; i < z; i += 2) {
-                               uid[k] = response_bit[i];
-                               k++;
-                               if (k > 31)
-                                       break;
+                       for (i = 0; i < 32; i++) {
+                               uid[i] = response_bit[i];
                        }
+
                        uid_byte[0] = (uid[0] << 7) | (uid[1] << 6) | (uid[2] << 5)
                                        | (uid[3] << 4) | (uid[4] << 3) | (uid[5] << 2)
                                        | (uid[6] << 1) | uid[7];
@@ -2005,7 +2335,9 @@ void check_challenges(bool file_given, byte_t* data) {
                                k += 8;
                        }
 
-               } else if (STATE == 1 && rxlen == 44) {
+
+                       tag.pstate = INIT;
+               } else if (STATE == 1 && rxlen > 24) {
                        //received configuration
                        STATE = 2;
                        z = 0;
@@ -2025,7 +2357,8 @@ void check_challenges(bool file_given, byte_t* data) {
                                tx[i] = unlocker[u1][i];
                        u1++;
 
-               } else if (STATE == 2 && rxlen >= 44) {
+                       tag.pstate = SELECTED;
+               } else if (STATE == 2 && rxlen >= 32) {
                        STATE = 0;
                }
 
@@ -2038,10 +2371,7 @@ void check_challenges(bool file_given, byte_t* data) {
                // falling edge occured halfway the period. with respect to this falling edge,
                // we need to wait (T_Wait2 + half_tag_period) when the last was a 'one'.
                // All timer values are in terms of T0 units
-
-               while (AT91C_BASE_TC0->TC_CV
-                               < T0 * (t_wait + (HITAG_T_TAG_HALF_PERIOD * lastbit)))
-                       ;
+               while (AT91C_BASE_TC0->TC_CV < T0 * (t_wait + (HITAG_T_TAG_HALF_PERIOD * lastbit))) { }
 
                // Transmit the reader frame
                hitag_reader_send_frame(tx, txlen);
@@ -2069,68 +2399,9 @@ void check_challenges(bool file_given, byte_t* data) {
                memset(rx, 0x00, sizeof(rx));
                rxlen = 0;
                lastbit = 1;
-               bSkip = true;
-               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;
-                       }
-               }
+               hitag_receive_frame(rx, &rxlen, &response);
        }
        LED_B_OFF();
        LED_D_OFF();
@@ -2140,5 +2411,13 @@ void check_challenges(bool file_given, byte_t* data) {
        cmd_send(CMD_ACK, bSuccessful, 0, 0, 0, 0);
 }
 
+/**
+Backward compatibility
+*/
+void check_challenges(bool file_given, byte_t* data) {
+       check_challenges_cmd(file_given, data, 1);
+}
 
-
+void ReadHitagS(hitag_function htf, hitag_data* htd) {
+       ReadHitagSintern(htf, htd, ADVANCED, 0, false);
+}
index 8eef23595bd383159291a4a76d32b12b77f98939..84fb5458f6bfd7b46d06061e8aa9fb9f471870ce 100644 (file)
@@ -32,7 +32,6 @@ size_t nbytes(size_t nbits) {
 int CmdLFHitagList(const char *Cmd)
 {
        uint8_t *got = malloc(USB_CMD_DATA_SIZE);
-
        // Query for the actual size of the trace
        UsbCommand response;
        GetFromBigBuf(got, USB_CMD_DATA_SIZE, 0, &response, -1, false);
@@ -52,6 +51,7 @@ int CmdLFHitagList(const char *Cmd)
        PrintAndLog(" ETU     :nbits: who bytes");
        PrintAndLog("---------+-----+----+-----------");
 
+       int j;
        int i = 0;
        int prev = -1;
        int len = strlen(Cmd);
@@ -93,7 +93,7 @@ int CmdLFHitagList(const char *Cmd)
                // or each half bit period in 256 levels.
 
                int bits = got[i+8];
-               int len = nbytes(got[i+8]);
+               int len = nbytes(bits);
 
                if (len > 100) {
                  break;
@@ -101,19 +101,43 @@ int CmdLFHitagList(const char *Cmd)
                if (i + len > traceLen) { break;}
 
                uint8_t *frame = (got+i+9);
+/*
+               int fillupBits = 8 - (bits % 8);
+               byte_t framefilled[bits+fillupBits];
+               byte_t* ff = framefilled;
+               
+               int response_bit[200] = {0};
+               int z = 0;
+               for (int y = 0; y < len; y++) {
+                       for (j = 0; j < 8; j++) {
+                               response_bit[z] = 0;
+                               if ((frame[y] & ((mask << 7) >> j)) != 0)
+                                       response_bit[z] = 1;
+                               z++;
+                       }
+               }
+               z = 0;
+               for (int y = 0; y < len; y++) {
+                       ff[y] = (response_bit[z] << 7) | (response_bit[z + 1] << 6)
+                                       | (response_bit[z + 2] << 5) | (response_bit[z + 3] << 4)
+                                       | (response_bit[z + 4] << 3) | (response_bit[z + 5] << 2)
+                                       | (response_bit[z + 6] << 1) | response_bit[z + 7];
+                       z += 8;
+               }
+*/
+
+
+
 
                // Break and stick with current result if buffer was not completely full
                if (frame[0] == 0x44 && frame[1] == 0x44 && frame[3] == 0x44) { break; }
 
                char line[1000] = "";
-               int j;
                for (j = 0; j < len; j++) {
-
                  //if((parityBits >> (len - j - 1)) & 0x01) {
                  if (isResponse && (oddparity8(frame[j]) != ((parityBits >> (len - j - 1)) & 0x01))) {
                        sprintf(line+(j*4), "%02x!  ", frame[j]);
-                 }
-                 else {
+                 } else {
                        sprintf(line+(j*4), "%02x   ", frame[j]);
                  }
                }
@@ -194,10 +218,27 @@ int CmdLFHitagReader(const char *Cmd) {
                        c = (UsbCommand){ CMD_READ_HITAG_S };
                        num_to_bytes(param_get32ex(Cmd,1,0,16),4,htd->auth.NrAr);
                        num_to_bytes(param_get32ex(Cmd,2,0,16),4,htd->auth.NrAr+4);
+                       c.arg[1] = param_get64ex(Cmd,3,0,0); //firstpage
+                       c.arg[2] = param_get64ex(Cmd,4,0,0); //tag mode
                } break;
                case 02: { //RHTSF_KEY
                        c = (UsbCommand){ CMD_READ_HITAG_S };
                        num_to_bytes(param_get64ex(Cmd,1,0,16),6,htd->crypto.key);
+                       c.arg[1] = param_get64ex(Cmd,2,0,0); //firstpage
+                       c.arg[2] = param_get64ex(Cmd,3,0,0); //tag mode
+               } break;
+               case 03: { //RHTSF_CHALLENGE BLOCK
+                       c = (UsbCommand){ CMD_READ_HITAG_S_BLK };
+                       num_to_bytes(param_get32ex(Cmd,1,0,16),4,htd->auth.NrAr);
+                       num_to_bytes(param_get32ex(Cmd,2,0,16),4,htd->auth.NrAr+4);
+                       c.arg[1] = param_get64ex(Cmd,3,0,0); //firstpage
+                       c.arg[2] = param_get64ex(Cmd,4,0,0); //tag mode
+               } break;
+               case 04: { //RHTSF_KEY BLOCK
+                       c = (UsbCommand){ CMD_READ_HITAG_S_BLK };
+                       num_to_bytes(param_get64ex(Cmd,1,0,16),6,htd->crypto.key);
+                       c.arg[1] = param_get64ex(Cmd,2,0,0); //firstpage
+                       c.arg[2] = param_get64ex(Cmd,3,0,0); //tag mode
                } break;
                case RHT2F_PASSWORD: {
                        num_to_bytes(param_get32ex(Cmd,1,0,16),4,htd->pwd.password);
@@ -222,8 +263,11 @@ int CmdLFHitagReader(const char *Cmd) {
                        PrintAndLog("Usage: hitag reader <Reader Function #>");
                        PrintAndLog("Reader Functions:");
                        PrintAndLog(" HitagS (0*)");
-                       PrintAndLog("  01 <nr> <ar> (Challenge) read all pages from a Hitag S tag");
-                       PrintAndLog("  02 <key> (set to 0 if no authentication is needed) read all pages from a Hitag S tag");
+                       PrintAndLog("  01 <nr> <ar> (Challenge) <firstPage> <tagmode> read all pages from a Hitag S tag");
+                       PrintAndLog("  02 <key> (set to 0 if no authentication is needed) <firstPage> <tagmode> read all pages from a Hitag S tag");
+                       PrintAndLog("  03 <nr> <ar> (Challenge) <firstPage> <tagmode> read all blocks from a Hitag S tag");
+                       PrintAndLog("  04 <key> (set to 0 if no authentication is needed) <firstPage> <tagmode> read all blocks from a Hitag S tag");
+                       PrintAndLog("  Valid tagmodes are 0=STANDARD, 1=ADVANCED, 2=FAST_ADVANCED (default is ADVANCED)");
                        PrintAndLog(" Hitag1 (1*)");
                        PrintAndLog(" Hitag2 (2*)");
                        PrintAndLog("  21 <password> (password mode)");
@@ -334,6 +378,7 @@ int CmdLFHitagCheckChallenges(const char *Cmd) {
        
        //file with all the challenges to try
        c.arg[0] = (uint32_t)file_given;
+       c.arg[1] = param_get64ex(Cmd,2,0,0); //get mode
 
   SendCommand(&c);
   return 0;
@@ -394,7 +439,7 @@ static command_t CommandTable[] =
   {"snoop",            CmdLFHitagSnoop,   1, "Eavesdrop Hitag communication"},
   {"writer",                   CmdLFHitagWP,      1, "Act like a Hitag Writer" },
   {"simS",             CmdLFHitagSimS,    1, "<hitagS.hts> Simulate HitagS transponder" }, 
-  {"checkChallenges",  CmdLFHitagCheckChallenges,   1, "<challenges.cc> test all challenges" }, {
+  {"checkChallenges",  CmdLFHitagCheckChallenges,   1, "<challenges.cc> <tagmode> test all challenges" }, {
                                NULL,NULL, 0, NULL }
 };
 
index e447714bc7cf87111294d25d6d32f95248052318..6fd31841fe7eab59d010394237cf32cc8f17f2e6 100644 (file)
 #include "hitag2.h"
 
 typedef enum PROTO_STATE {READY=0,INIT,AUTHENTICATE,SELECTED,QUIET,TTF,FAIL} PSTATE;                           //protocol-state
-typedef enum TAG_STATE   {NO_OP=0,READING_PAGE,WRITING_PAGE_ACK,WRITING_PAGE_DATA,WRITING_BLOCK_DATA} TSATE;   //tag-state
+typedef enum TAG_STATE   {NO_OP=0,READING_PAGE,READING_BLOCK,WRITING_PAGE_ACK,WRITING_PAGE_DATA,WRITING_BLOCK_DATA} TSATE;     //tag-state
 typedef enum SOF_TYPE    {STANDARD=0,ADVANCED,FAST_ADVANCED,ONE,NO_BITS} stype;                                        //number of start-of-frame bits
 
 struct hitagS_tag {
        PSTATE pstate; //protocol-state
        TSATE tstate;  //tag-state
        uint32_t uid;
-       uint32_t pages[16][4];
+       uint8_t  pages[64][4];
        uint64_t key;
        byte_t pwdl0,pwdl1,pwdh0;
        //con0
index bdff726142d635ed089603de9809986660217513..fa66634f2ee688ca6df737900ad16e5c7c3a5858 100644 (file)
@@ -137,6 +137,7 @@ typedef struct{
 #define CMD_SIMULATE_HITAG_S                                              0x0368
 #define CMD_TEST_HITAGS_TRACES                                            0x0367
 #define CMD_READ_HITAG_S                                                  0x0373
+#define CMD_READ_HITAG_S_BLK                                              0x0374
 #define CMD_WR_HITAG_S                                                    0x0375
 #define CMD_EMU_HITAG_S                                                   0x0376
 
Impressum, Datenschutz