]> cvs.zerfleddert.de Git - proxmark3-svn/blobdiff - armsrc/iso14443a.c
fixed bug in ASKbiphaseDemod()
[proxmark3-svn] / armsrc / iso14443a.c
index a336703606e95dcf99169b2ac7f1695c93f03806..2f4baf175d5a413c16252a3f58a0115b7461f853 100644 (file)
 
 #include "iso14443a.h"
 
 
 #include "iso14443a.h"
 
+#include <stdio.h>
+#include <string.h>
 #include "proxmark3.h"
 #include "apps.h"
 #include "util.h"
 #include "proxmark3.h"
 #include "apps.h"
 #include "util.h"
-#include "string.h"
 #include "cmd.h"
 #include "iso14443crc.h"
 #include "crapto1/crapto1.h"
 #include "cmd.h"
 #include "iso14443crc.h"
 #include "crapto1/crapto1.h"
@@ -24,6 +25,7 @@
 #include "BigBuf.h"
 #include "protocols.h"
 #include "parity.h"
 #include "BigBuf.h"
 #include "protocols.h"
 #include "parity.h"
+#include "fpgaloader.h"
 
 typedef struct {
        enum {
 
 typedef struct {
        enum {
@@ -80,6 +82,8 @@ typedef struct {
 } tUart;
 
 static uint32_t iso14a_timeout;
 } tUart;
 
 static uint32_t iso14a_timeout;
+#define MAX_ISO14A_TIMEOUT 524288
+
 int rsamples = 0;
 uint8_t trigger = 0;
 // the block number for the ISO14443-4 PCB
 int rsamples = 0;
 uint8_t trigger = 0;
 // the block number for the ISO14443-4 PCB
@@ -185,34 +189,17 @@ void iso14a_set_trigger(bool enable) {
 }
 
 
 }
 
 
-static void iso14a_set_timeout(uint32_t timeout) {
-       iso14a_timeout = timeout;
-       if(MF_DBGLEVEL >= 3) Dbprintf("ISO14443A Timeout set to %ld (%dms)", iso14a_timeout, iso14a_timeout / 106);
+void iso14a_set_timeout(uint32_t timeout) {
+       // adjust timeout by FPGA delays and 2 additional ssp_frames to detect SOF
+       iso14a_timeout = timeout + (DELAY_AIR2ARM_AS_READER + DELAY_ARM2AIR_AS_READER)/(16*8) + 2;
+       if(MF_DBGLEVEL >= 3) Dbprintf("ISO14443A Timeout set to %ld (%dms)", timeout, timeout / 106);
 }
 
 
 }
 
 
-static void iso14a_set_ATS_timeout(uint8_t *ats) {
-
-       uint8_t tb1;
-       uint8_t fwi; 
-       uint32_t fwt;
-       
-       if (ats[0] > 1) {                                                       // there is a format byte T0
-               if ((ats[1] & 0x20) == 0x20) {                  // there is an interface byte TB(1)
-                       if ((ats[1] & 0x10) == 0x10) {          // there is an interface byte TA(1) preceding TB(1)
-                               tb1 = ats[3];
-                       } else {
-                               tb1 = ats[2];
-                       }
-                       fwi = (tb1 & 0xf0) >> 4;                        // frame waiting indicator (FWI)
-                       fwt = 256 * 16 * (1 << fwi);            // frame waiting time (FWT) in 1/fc
-                       
-                       iso14a_set_timeout(fwt/(8*16));
-               }
-       }
+uint32_t iso14a_get_timeout(void) {
+       return iso14a_timeout - (DELAY_AIR2ARM_AS_READER + DELAY_ARM2AIR_AS_READER)/(16*8) - 2;
 }
 
 }
 
-
 //-----------------------------------------------------------------------------
 // Generate the parity value for a byte sequence
 //
 //-----------------------------------------------------------------------------
 // Generate the parity value for a byte sequence
 //
@@ -332,15 +319,18 @@ static RAMFUNC bool MillerDecoding(uint8_t bit, uint32_t non_real_time)
                        Uart.startTime -= Uart.syncBit;
                        Uart.endTime = Uart.startTime;
                        Uart.state = STATE_START_OF_COMMUNICATION;
                        Uart.startTime -= Uart.syncBit;
                        Uart.endTime = Uart.startTime;
                        Uart.state = STATE_START_OF_COMMUNICATION;
+                       LED_B_ON();
                }
 
        } else {
 
                if (IsMillerModulationNibble1(Uart.fourBits >> Uart.syncBit)) {                 
                        if (IsMillerModulationNibble2(Uart.fourBits >> Uart.syncBit)) {         // Modulation in both halves - error
                }
 
        } else {
 
                if (IsMillerModulationNibble1(Uart.fourBits >> Uart.syncBit)) {                 
                        if (IsMillerModulationNibble2(Uart.fourBits >> Uart.syncBit)) {         // Modulation in both halves - error
+                               LED_B_OFF();
                                UartReset();
                        } else {                                                                                                                        // Modulation in first half = Sequence Z = logic "0"
                                if (Uart.state == STATE_MILLER_X) {                                                             // error - must not follow after X
                                UartReset();
                        } else {                                                                                                                        // Modulation in first half = Sequence Z = logic "0"
                                if (Uart.state == STATE_MILLER_X) {                                                             // error - must not follow after X
+                                       LED_B_OFF();
                                        UartReset();
                                } else {
                                        Uart.bitCount++;
                                        UartReset();
                                } else {
                                        Uart.bitCount++;
@@ -379,6 +369,7 @@ static RAMFUNC bool MillerDecoding(uint8_t bit, uint32_t non_real_time)
                                }
                        } else {                                                                                                                        // no modulation in both halves - Sequence Y
                                if (Uart.state == STATE_MILLER_Z || Uart.state == STATE_MILLER_Y) {     // Y after logic "0" - End of Communication
                                }
                        } else {                                                                                                                        // no modulation in both halves - Sequence Y
                                if (Uart.state == STATE_MILLER_Z || Uart.state == STATE_MILLER_Y) {     // Y after logic "0" - End of Communication
+                                       LED_B_OFF();
                                        Uart.state = STATE_UNSYNCD;
                                        Uart.bitCount--;                                                                                        // last "0" was part of EOC sequence
                                        Uart.shiftReg <<= 1;                                                                            // drop it
                                        Uart.state = STATE_UNSYNCD;
                                        Uart.bitCount--;                                                                                        // last "0" was part of EOC sequence
                                        Uart.shiftReg <<= 1;                                                                            // drop it
@@ -400,6 +391,7 @@ static RAMFUNC bool MillerDecoding(uint8_t bit, uint32_t non_real_time)
                                        }
                                }
                                if (Uart.state == STATE_START_OF_COMMUNICATION) {                               // error - must not follow directly after SOC
                                        }
                                }
                                if (Uart.state == STATE_START_OF_COMMUNICATION) {                               // error - must not follow directly after SOC
+                                       LED_B_OFF();
                                        UartReset();
                                } else {                                                                                                                // a logic "0"
                                        Uart.bitCount++;
                                        UartReset();
                                } else {                                                                                                                // a logic "0"
                                        Uart.bitCount++;
@@ -505,6 +497,7 @@ static RAMFUNC int ManchesterDecoding(uint8_t bit, uint16_t offset, uint32_t non
                                Demod.startTime -= Demod.syncBit;
                                Demod.bitCount = offset;                        // number of decoded data bits
                                Demod.state = DEMOD_MANCHESTER_DATA;
                                Demod.startTime -= Demod.syncBit;
                                Demod.bitCount = offset;                        // number of decoded data bits
                                Demod.state = DEMOD_MANCHESTER_DATA;
+                               LED_C_ON();
                        }
                }
 
                        }
                }
 
@@ -547,6 +540,7 @@ static RAMFUNC int ManchesterDecoding(uint8_t bit, uint16_t offset, uint32_t non
                                }
                                Demod.endTime = Demod.startTime + 8*(9*Demod.len + Demod.bitCount + 1);
                        } else {                                                                                                        // no modulation in both halves - End of communication
                                }
                                Demod.endTime = Demod.startTime + 8*(9*Demod.len + Demod.bitCount + 1);
                        } else {                                                                                                        // no modulation in both halves - End of communication
+                               LED_C_OFF();
                                if(Demod.bitCount > 0) {                                                                // there are some remaining data bits
                                        Demod.shiftReg >>= (9 - Demod.bitCount);                        // right align the decoded bits
                                        Demod.output[Demod.len++] = Demod.shiftReg & 0xff;      // and add them to the output
                                if(Demod.bitCount > 0) {                                                                // there are some remaining data bits
                                        Demod.shiftReg >>= (9 - Demod.bitCount);                        // right align the decoded bits
                                        Demod.output[Demod.len++] = Demod.shiftReg & 0xff;      // and add them to the output
@@ -587,6 +581,7 @@ void RAMFUNC SnoopIso14443a(uint8_t param) {
        // bit 1 - trigger from first reader 7-bit request
        
        LEDsoff();
        // bit 1 - trigger from first reader 7-bit request
        
        LEDsoff();
+       LED_A_ON();
 
        iso14443a_setup(FPGA_HF_ISO14443A_SNIFFER);
 
 
        iso14443a_setup(FPGA_HF_ISO14443A_SNIFFER);
 
@@ -639,7 +634,6 @@ void RAMFUNC SnoopIso14443a(uint8_t param) {
                        break;
                }
 
                        break;
                }
 
-               LED_A_ON();
                WDT_HIT();
 
                int register readBufDataP = data - dmaBuf;
                WDT_HIT();
 
                int register readBufDataP = data - dmaBuf;
@@ -671,18 +665,15 @@ void RAMFUNC SnoopIso14443a(uint8_t param) {
                        AT91C_BASE_PDC_SSC->PDC_RNCR = DMA_BUFFER_SIZE;
                }
 
                        AT91C_BASE_PDC_SSC->PDC_RNCR = DMA_BUFFER_SIZE;
                }
 
-               LED_A_OFF();
-               
                if (rsamples & 0x01) {                          // Need two samples to feed Miller and Manchester-Decoder
 
                        if(!TagIsActive) {              // no need to try decoding reader data if the tag is sending
                                uint8_t readerdata = (previous_data & 0xF0) | (*data >> 4);
                                if (MillerDecoding(readerdata, (rsamples-1)*4)) {
                if (rsamples & 0x01) {                          // Need two samples to feed Miller and Manchester-Decoder
 
                        if(!TagIsActive) {              // no need to try decoding reader data if the tag is sending
                                uint8_t readerdata = (previous_data & 0xF0) | (*data >> 4);
                                if (MillerDecoding(readerdata, (rsamples-1)*4)) {
-                                       LED_C_ON();
-
                                        // check - if there is a short 7bit request from reader
                                        // check - if there is a short 7bit request from reader
-                                       if ((!triggered) && (param & 0x02) && (Uart.len == 1) && (Uart.bitCount == 7)) triggered = true;
-
+                                       if ((!triggered) && (param & 0x02) && (Uart.len == 1) && (Uart.bitCount == 7)) {
+                                               triggered = true;
+                                       }
                                        if(triggered) {
                                                if (!LogTrace(receivedCmd, 
                                                                                Uart.len, 
                                        if(triggered) {
                                                if (!LogTrace(receivedCmd, 
                                                                                Uart.len, 
@@ -696,31 +687,24 @@ void RAMFUNC SnoopIso14443a(uint8_t param) {
                                        /* And also reset the demod code, which might have been */
                                        /* false-triggered by the commands from the reader. */
                                        DemodReset();
                                        /* And also reset the demod code, which might have been */
                                        /* false-triggered by the commands from the reader. */
                                        DemodReset();
-                                       LED_B_OFF();
                                }
                                ReaderIsActive = (Uart.state != STATE_UNSYNCD);
                        }
 
                                }
                                ReaderIsActive = (Uart.state != STATE_UNSYNCD);
                        }
 
-                       if(!ReaderIsActive) {           // no need to try decoding tag data if the reader is sending - and we cannot afford the time
+                       if (!ReaderIsActive) {          // no need to try decoding tag data if the reader is sending - and we cannot afford the time
                                uint8_t tagdata = (previous_data << 4) | (*data & 0x0F);
                                uint8_t tagdata = (previous_data << 4) | (*data & 0x0F);
-                               if(ManchesterDecoding(tagdata, 0, (rsamples-1)*4)) {
-                                       LED_B_ON();
-
+                               if (ManchesterDecoding(tagdata, 0, (rsamples-1)*4)) {
                                        if (!LogTrace(receivedResponse, 
                                                                        Demod.len, 
                                                                        Demod.startTime*16 - DELAY_TAG_AIR2ARM_AS_SNIFFER, 
                                                                        Demod.endTime*16 - DELAY_TAG_AIR2ARM_AS_SNIFFER,
                                                                        Demod.parity,
                                                                        false)) break;
                                        if (!LogTrace(receivedResponse, 
                                                                        Demod.len, 
                                                                        Demod.startTime*16 - DELAY_TAG_AIR2ARM_AS_SNIFFER, 
                                                                        Demod.endTime*16 - DELAY_TAG_AIR2ARM_AS_SNIFFER,
                                                                        Demod.parity,
                                                                        false)) break;
-
                                        if ((!triggered) && (param & 0x01)) triggered = true;
                                        if ((!triggered) && (param & 0x01)) triggered = true;
-
                                        // And ready to receive another response.
                                        DemodReset();
                                        // And reset the Miller decoder including itS (now outdated) input buffer
                                        UartInit(receivedCmd, receivedCmdPar);
                                        // And ready to receive another response.
                                        DemodReset();
                                        // And reset the Miller decoder including itS (now outdated) input buffer
                                        UartInit(receivedCmd, receivedCmdPar);
-
-                                       LED_C_OFF();
                                } 
                                TagIsActive = (Demod.state != DEMOD_UNSYNCD);
                        }
                                } 
                                TagIsActive = (Demod.state != DEMOD_UNSYNCD);
                        }
@@ -734,12 +718,12 @@ void RAMFUNC SnoopIso14443a(uint8_t param) {
                }
        } // main cycle
 
                }
        } // main cycle
 
-       DbpString("COMMAND FINISHED");
-
        FpgaDisableSscDma();
        FpgaDisableSscDma();
+       LEDsoff();
+
+       DbpString("COMMAND FINISHED");
        Dbprintf("maxDataLen=%d, Uart.state=%x, Uart.len=%d", maxDataLen, Uart.state, Uart.len);
        Dbprintf("traceLen=%d, Uart.output[0]=%08x", BigBuf_get_traceLen(), (uint32_t)Uart.output[0]);
        Dbprintf("maxDataLen=%d, Uart.state=%x, Uart.len=%d", maxDataLen, Uart.state, Uart.len);
        Dbprintf("traceLen=%d, Uart.output[0]=%08x", BigBuf_get_traceLen(), (uint32_t)Uart.output[0]);
-       LEDsoff();
 }
 
 //-----------------------------------------------------------------------------
 }
 
 //-----------------------------------------------------------------------------
@@ -900,11 +884,11 @@ static int GetIso14443aCommandFromReader(uint8_t *received, uint8_t *parity, int
 }
 
 
 }
 
 
-static int EmSend4bitEx(uint8_t resp, bool correctionNeeded);
+static int EmSend4bitEx(uint8_t resp);
 int EmSend4bit(uint8_t resp);
 int EmSend4bit(uint8_t resp);
-static int EmSendCmdExPar(uint8_t *resp, uint16_t respLen, bool correctionNeeded, uint8_t *par);
-int EmSendCmdEx(uint8_t *resp, uint16_t respLen, bool correctionNeeded);
-int EmSendPrecompiledCmd(tag_response_info_t *response_info, bool correctionNeeded);
+static int EmSendCmdExPar(uint8_t *resp, uint16_t respLen, uint8_t *par);
+int EmSendCmdEx(uint8_t *resp, uint16_t respLen);
+int EmSendPrecompiledCmd(tag_response_info_t *response_info);
 
 
 static bool prepare_tag_modulation(tag_response_info_t* response_info, size_t max_buffer_size) {
 
 
 static bool prepare_tag_modulation(tag_response_info_t* response_info, size_t max_buffer_size) {
@@ -1138,7 +1122,7 @@ void SimulateIso14443aTag(int tagType, int uid_1st, int uid_2nd, byte_t* data)
                } else if(receivedCmd[1] == 0x70 && receivedCmd[0] == 0x95) {   // Received a SELECT (cascade 2)
                        p_response = &responses[4]; order = 30;
                } else if(receivedCmd[0] == 0x30) {     // Received a (plain) READ
                } else if(receivedCmd[1] == 0x70 && receivedCmd[0] == 0x95) {   // Received a SELECT (cascade 2)
                        p_response = &responses[4]; order = 30;
                } else if(receivedCmd[0] == 0x30) {     // Received a (plain) READ
-                       EmSendCmdEx(data+(4*receivedCmd[1]),16,false);
+                       EmSendCmdEx(data+(4*receivedCmd[1]),16);
                        // Dbprintf("Read request from reader: %x %x",receivedCmd[0],receivedCmd[1]);
                        // We already responded, do not send anything with the EmSendCmd14443aRaw() that is called below
                        p_response = NULL;
                        // Dbprintf("Read request from reader: %x %x",receivedCmd[0],receivedCmd[1]);
                        // We already responded, do not send anything with the EmSendCmd14443aRaw() that is called below
                        p_response = NULL;
@@ -1231,10 +1215,10 @@ void SimulateIso14443aTag(int tagType, int uid_1st, int uid_2nd, byte_t* data)
                cmdsRecvd++;
 
                if (p_response != NULL) {
                cmdsRecvd++;
 
                if (p_response != NULL) {
-                       EmSendPrecompiledCmd(p_response, receivedCmd[0] == 0x52);
+                       EmSendPrecompiledCmd(p_response);
                }
                
                }
                
-               if (!tracing) {
+               if (!get_tracing()) {
                        Dbprintf("Trace Full. Simulation stopped.");
                        break;
                }
                        Dbprintf("Trace Full. Simulation stopped.");
                        break;
                }
@@ -1274,13 +1258,13 @@ static void PrepareDelayedTransfer(uint16_t delay)
 // Transmit the command (to the tag) that was placed in ToSend[].
 // Parameter timing:
 // if NULL: transfer at next possible time, taking into account
 // Transmit the command (to the tag) that was placed in ToSend[].
 // Parameter timing:
 // if NULL: transfer at next possible time, taking into account
-//                     request guard time and frame delay time
+//                     request guard time, startup frame guard time and frame delay time
 // if == 0:    transfer immediately and return time of transfer
 // if != 0: delay transfer until time specified
 //-------------------------------------------------------------------------------------
 static void TransmitFor14443a(const uint8_t *cmd, uint16_t len, uint32_t *timing)
 {
 // if == 0:    transfer immediately and return time of transfer
 // if != 0: delay transfer until time specified
 //-------------------------------------------------------------------------------------
 static void TransmitFor14443a(const uint8_t *cmd, uint16_t len, uint32_t *timing)
 {
-       
+       LED_D_ON();
        FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_READER_MOD);
 
        uint32_t ThisTransferTime = 0;
        FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_READER_MOD);
 
        uint32_t ThisTransferTime = 0;
@@ -1413,40 +1397,45 @@ int EmGetCmd(uint8_t *received, uint16_t *len, uint8_t *parity)
        int analogCnt = 0;
        int analogAVG = 0;
 
        int analogCnt = 0;
        int analogAVG = 0;
 
-       // Set FPGA mode to "simulated ISO 14443 tag", no modulation (listen
-       // only, since we are receiving, not transmitting).
-       // Signal field is off with the appropriate LED
-       LED_D_OFF();
-       FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_TAGSIM_LISTEN);
-
        // Set ADC to read field strength
        AT91C_BASE_ADC->ADC_CR = AT91C_ADC_SWRST;
        AT91C_BASE_ADC->ADC_MR =
                                ADC_MODE_PRESCALE(63) |
                                ADC_MODE_STARTUP_TIME(1) |
                                ADC_MODE_SAMPLE_HOLD_TIME(15);
        // Set ADC to read field strength
        AT91C_BASE_ADC->ADC_CR = AT91C_ADC_SWRST;
        AT91C_BASE_ADC->ADC_MR =
                                ADC_MODE_PRESCALE(63) |
                                ADC_MODE_STARTUP_TIME(1) |
                                ADC_MODE_SAMPLE_HOLD_TIME(15);
-       AT91C_BASE_ADC->ADC_CHER = ADC_CHANNEL(ADC_CHAN_HF);
+       AT91C_BASE_ADC->ADC_CHER = ADC_CHANNEL(ADC_CHAN_HF_LOW);
        // start ADC
        AT91C_BASE_ADC->ADC_CR = AT91C_ADC_START;
        
        // start ADC
        AT91C_BASE_ADC->ADC_CR = AT91C_ADC_START;
        
-       // Now run a 'software UART' on the stream of incoming samples.
+       // Run a 'software UART' on the stream of incoming samples.
        UartInit(received, parity);
 
        UartInit(received, parity);
 
-       // Clear RXRDY:
-    uint8_t b = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
-       
+       // Ensure that the FPGA Delay Queue is empty before we switch to TAGSIM_LISTEN
+       do {
+               if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) {
+                       AT91C_BASE_SSC->SSC_THR = SEC_F;
+                       uint8_t b = (uint8_t)AT91C_BASE_SSC->SSC_RHR; (void) b;
+               }
+       } while (GetCountSspClk() < LastTimeProxToAirStart + LastProxToAirDuration + (FpgaSendQueueDelay>>3));
+
+       // Set FPGA mode to "simulated ISO 14443 tag", no modulation (listen
+       // only, since we are receiving, not transmitting).
+       // Signal field is off with the appropriate LED
+       LED_D_OFF();
+       FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_TAGSIM_LISTEN);
+
        for(;;) {
                WDT_HIT();
 
                if (BUTTON_PRESS()) return 1;
 
                // test if the field exists
        for(;;) {
                WDT_HIT();
 
                if (BUTTON_PRESS()) return 1;
 
                // test if the field exists
-               if (AT91C_BASE_ADC->ADC_SR & ADC_END_OF_CONVERSION(ADC_CHAN_HF)) {
+               if (AT91C_BASE_ADC->ADC_SR & ADC_END_OF_CONVERSION(ADC_CHAN_HF_LOW)) {
                        analogCnt++;
                        analogCnt++;
-                       analogAVG += AT91C_BASE_ADC->ADC_CDR[ADC_CHAN_HF];
+                       analogAVG += AT91C_BASE_ADC->ADC_CDR[ADC_CHAN_HF_LOW];
                        AT91C_BASE_ADC->ADC_CR = AT91C_ADC_START;
                        if (analogCnt >= 32) {
                        AT91C_BASE_ADC->ADC_CR = AT91C_ADC_START;
                        if (analogCnt >= 32) {
-                               if ((MAX_ADC_HF_VOLTAGE * (analogAVG / analogCnt) >> 10) < MF_MINFIELDV) {
+                               if ((MAX_ADC_HF_VOLTAGE_LOW * (analogAVG / analogCnt) >> 10) < MF_MINFIELDV) {
                                        vtime = GetTickCount();
                                        if (!timer) timer = vtime;
                                        // 50ms no field --> card to idle state
                                        vtime = GetTickCount();
                                        if (!timer) timer = vtime;
                                        // 50ms no field --> card to idle state
@@ -1460,7 +1449,7 @@ int EmGetCmd(uint8_t *received, uint16_t *len, uint8_t *parity)
 
                // receive and test the miller decoding
         if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) {
 
                // receive and test the miller decoding
         if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) {
-            b = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
+            uint8_t b = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
                        if(MillerDecoding(b, 0)) {
                                *len = Uart.len;
                                EmLogTraceReader();
                        if(MillerDecoding(b, 0)) {
                                *len = Uart.len;
                                EmLogTraceReader();
@@ -1472,18 +1461,28 @@ int EmGetCmd(uint8_t *received, uint16_t *len, uint8_t *parity)
 }
 
 
 }
 
 
-static int EmSendCmd14443aRaw(uint8_t *resp, uint16_t respLen, bool correctionNeeded)
+static int EmSendCmd14443aRaw(uint8_t *resp, uint16_t respLen)
 {
        uint8_t b;
        uint16_t i = 0;
 {
        uint8_t b;
        uint16_t i = 0;
-       
+       bool correctionNeeded;
+
        // Modulate Manchester
        // Modulate Manchester
+       LED_D_OFF();
        FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_TAGSIM_MOD);
 
        // include correction bit if necessary
        FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_TAGSIM_MOD);
 
        // include correction bit if necessary
-       if (Uart.parityBits & 0x01) {
-               correctionNeeded = true;
+       if (Uart.bitCount == 7)
+       {
+               // Short tags (7 bits) don't have parity, determine the correct value from MSB
+               correctionNeeded = Uart.output[0] & 0x40;
+       }
+       else
+       {
+               // Look at the last parity bit
+               correctionNeeded = Uart.parity[(Uart.len-1)/8] & (0x80 >> ((Uart.len-1) & 7));
        }
        }
+
        if(correctionNeeded) {
                // 1236, so correction bit needed
                i = 0;
        if(correctionNeeded) {
                // 1236, so correction bit needed
                i = 0;
@@ -1517,23 +1516,13 @@ static int EmSendCmd14443aRaw(uint8_t *resp, uint16_t respLen, bool correctionNe
                }
        }
 
                }
        }
 
-       // Ensure that the FPGA Delay Queue is empty before we switch to TAGSIM_LISTEN again:
-       uint8_t fpga_queued_bits = FpgaSendQueueDelay >> 3;
-       for (i = 0; i < fpga_queued_bits/8; ) {
-               if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) {
-                       AT91C_BASE_SSC->SSC_THR = SEC_F;
-                       FpgaSendQueueDelay = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
-                       i++;
-               }
-       }
-
        return 0;
 }
 
 
        return 0;
 }
 
 
-static int EmSend4bitEx(uint8_t resp, bool correctionNeeded){
+static int EmSend4bitEx(uint8_t resp){
        Code4bitAnswerAsTag(resp);
        Code4bitAnswerAsTag(resp);
-       int res = EmSendCmd14443aRaw(ToSend, ToSendMax, correctionNeeded);
+       int res = EmSendCmd14443aRaw(ToSend, ToSendMax);
        // do the tracing for the previous reader request and this tag answer:
        EmLogTraceTag(&resp, 1, NULL, LastProxToAirDuration);
        return res;
        // do the tracing for the previous reader request and this tag answer:
        EmLogTraceTag(&resp, 1, NULL, LastProxToAirDuration);
        return res;
@@ -1541,40 +1530,40 @@ static int EmSend4bitEx(uint8_t resp, bool correctionNeeded){
 
 
 int EmSend4bit(uint8_t resp){
 
 
 int EmSend4bit(uint8_t resp){
-       return EmSend4bitEx(resp, false);
+       return EmSend4bitEx(resp);
 }
 
 
 }
 
 
-static int EmSendCmdExPar(uint8_t *resp, uint16_t respLen, bool correctionNeeded, uint8_t *par){
+static int EmSendCmdExPar(uint8_t *resp, uint16_t respLen, uint8_t *par){
        CodeIso14443aAsTagPar(resp, respLen, par);
        CodeIso14443aAsTagPar(resp, respLen, par);
-       int res = EmSendCmd14443aRaw(ToSend, ToSendMax, correctionNeeded);
+       int res = EmSendCmd14443aRaw(ToSend, ToSendMax);
        // do the tracing for the previous reader request and this tag answer:
        EmLogTraceTag(resp, respLen, par, LastProxToAirDuration);
        return res;
 }
 
 
        // do the tracing for the previous reader request and this tag answer:
        EmLogTraceTag(resp, respLen, par, LastProxToAirDuration);
        return res;
 }
 
 
-int EmSendCmdEx(uint8_t *resp, uint16_t respLen, bool correctionNeeded){
+int EmSendCmdEx(uint8_t *resp, uint16_t respLen){
        uint8_t par[MAX_PARITY_SIZE];
        GetParity(resp, respLen, par);
        uint8_t par[MAX_PARITY_SIZE];
        GetParity(resp, respLen, par);
-       return EmSendCmdExPar(resp, respLen, correctionNeeded, par);
+       return EmSendCmdExPar(resp, respLen, par);
 }
 
 
 int EmSendCmd(uint8_t *resp, uint16_t respLen){
        uint8_t par[MAX_PARITY_SIZE];
        GetParity(resp, respLen, par);
 }
 
 
 int EmSendCmd(uint8_t *resp, uint16_t respLen){
        uint8_t par[MAX_PARITY_SIZE];
        GetParity(resp, respLen, par);
-       return EmSendCmdExPar(resp, respLen, false, par);
+       return EmSendCmdExPar(resp, respLen, par);
 }
 
 
 int EmSendCmdPar(uint8_t *resp, uint16_t respLen, uint8_t *par){
 }
 
 
 int EmSendCmdPar(uint8_t *resp, uint16_t respLen, uint8_t *par){
-       return EmSendCmdExPar(resp, respLen, false, par);
+       return EmSendCmdExPar(resp, respLen, par);
 }
 
 
 }
 
 
-int EmSendPrecompiledCmd(tag_response_info_t *response_info, bool correctionNeeded) {
-       int ret = EmSendCmd14443aRaw(response_info->modulation, response_info->modulation_n, correctionNeeded);
+int EmSendPrecompiledCmd(tag_response_info_t *response_info) {
+       int ret = EmSendCmd14443aRaw(response_info->modulation, response_info->modulation_n);
        // do the tracing for the previous reader request and this tag answer:
        EmLogTraceTag(response_info->response, response_info->response_n, &(response_info->par), response_info->ProxToAirDuration);
        return ret;
        // do the tracing for the previous reader request and this tag answer:
        EmLogTraceTag(response_info->response, response_info->response_n, &(response_info->par), response_info->ProxToAirDuration);
        return ret;
@@ -1629,9 +1618,7 @@ void ReaderTransmitBitsPar(uint8_t* frame, uint16_t bits, uint8_t *par, uint32_t
                LED_A_ON();
   
        // Log reader command in trace buffer
                LED_A_ON();
   
        // Log reader command in trace buffer
-       if (tracing) {
-               LogTrace(frame, nbytes(bits), LastTimeProxToAirStart*16 + DELAY_ARM2AIR_AS_READER, (LastTimeProxToAirStart + LastProxToAirDuration)*16 + DELAY_ARM2AIR_AS_READER, par, true);
-       }
+       LogTrace(frame, nbytes(bits), LastTimeProxToAirStart*16 + DELAY_ARM2AIR_AS_READER, (LastTimeProxToAirStart + LastProxToAirDuration)*16 + DELAY_ARM2AIR_AS_READER, par, true);
 }
 
 
 }
 
 
@@ -1662,9 +1649,7 @@ void ReaderTransmit(uint8_t* frame, uint16_t len, uint32_t *timing)
 static int ReaderReceiveOffset(uint8_t* receivedAnswer, uint16_t offset, uint8_t *parity)
 {
        if (!GetIso14443aAnswerFromTag(receivedAnswer, parity, offset)) return false;
 static int ReaderReceiveOffset(uint8_t* receivedAnswer, uint16_t offset, uint8_t *parity)
 {
        if (!GetIso14443aAnswerFromTag(receivedAnswer, parity, offset)) return false;
-       if (tracing) {
-               LogTrace(receivedAnswer, Demod.len, Demod.startTime*16 - DELAY_AIR2ARM_AS_READER, Demod.endTime*16 - DELAY_AIR2ARM_AS_READER, parity, false);
-       }
+       LogTrace(receivedAnswer, Demod.len, Demod.startTime*16 - DELAY_AIR2ARM_AS_READER, Demod.endTime*16 - DELAY_AIR2ARM_AS_READER, parity, false);
        return Demod.len;
 }
 
        return Demod.len;
 }
 
@@ -1672,12 +1657,63 @@ static int ReaderReceiveOffset(uint8_t* receivedAnswer, uint16_t offset, uint8_t
 int ReaderReceive(uint8_t *receivedAnswer, uint8_t *parity)
 {
        if (!GetIso14443aAnswerFromTag(receivedAnswer, parity, 0)) return false;
 int ReaderReceive(uint8_t *receivedAnswer, uint8_t *parity)
 {
        if (!GetIso14443aAnswerFromTag(receivedAnswer, parity, 0)) return false;
-       if (tracing) {
-               LogTrace(receivedAnswer, Demod.len, Demod.startTime*16 - DELAY_AIR2ARM_AS_READER, Demod.endTime*16 - DELAY_AIR2ARM_AS_READER, parity, false);
-       }
+       LogTrace(receivedAnswer, Demod.len, Demod.startTime*16 - DELAY_AIR2ARM_AS_READER, Demod.endTime*16 - DELAY_AIR2ARM_AS_READER, parity, false);
        return Demod.len;
 }
 
        return Demod.len;
 }
 
+
+static void iso14a_set_ATS_times(uint8_t *ats) {
+
+       uint8_t tb1;
+       uint8_t fwi, sfgi; 
+       uint32_t fwt, sfgt;
+       
+       if (ats[0] > 1) {                                                       // there is a format byte T0
+               if ((ats[1] & 0x20) == 0x20) {                  // there is an interface byte TB(1)
+                       if ((ats[1] & 0x10) == 0x10) {          // there is an interface byte TA(1) preceding TB(1)
+                               tb1 = ats[3];
+                       } else {
+                               tb1 = ats[2];
+                       }
+                       fwi = (tb1 & 0xf0) >> 4;                        // frame waiting time integer (FWI)
+                       if (fwi != 15) {
+                               fwt = 256 * 16 * (1 << fwi);    // frame waiting time (FWT) in 1/fc
+                               iso14a_set_timeout(fwt/(8*16));
+                       }
+                       sfgi = tb1 & 0x0f;                                      // startup frame guard time integer (SFGI)
+                       if (sfgi != 0 && sfgi != 15) {
+                               sfgt = 256 * 16 * (1 << sfgi);  // startup frame guard time (SFGT) in 1/fc
+                               NextTransferTime = MAX(NextTransferTime, Demod.endTime + (sfgt - DELAY_AIR2ARM_AS_READER - DELAY_ARM2AIR_AS_READER)/16);
+                       }
+               }
+       }
+}
+
+
+static int GetATQA(uint8_t *resp, uint8_t *resp_par) {
+
+#define WUPA_RETRY_TIMEOUT     10      // 10ms
+       uint8_t wupa[]       = { 0x52 };  // 0x26 - REQA  0x52 - WAKE-UP
+
+       uint32_t save_iso14a_timeout = iso14a_get_timeout();
+       iso14a_set_timeout(1236/(16*8)+1);              // response to WUPA is expected at exactly 1236/fc. No need to wait longer.
+       
+       uint32_t start_time = GetTickCount();
+       int len;
+       
+       // we may need several tries if we did send an unknown command or a wrong authentication before...
+       do {
+               // Broadcast for a card, WUPA (0x52) will force response from all cards in the field
+               ReaderTransmitBitsPar(wupa, 7, NULL, NULL);
+               // Receive the ATQA
+               len = ReaderReceive(resp, resp_par);
+       } while (len == 0 && GetTickCount() <= start_time + WUPA_RETRY_TIMEOUT);
+                       
+       iso14a_set_timeout(save_iso14a_timeout);
+       return len;
+}
+
+
 // performs iso14443a anticollision (optional) and card select procedure
 // fills the uid and cuid pointer unless NULL
 // fills the card info record unless NULL
 // performs iso14443a anticollision (optional) and card select procedure
 // fills the uid and cuid pointer unless NULL
 // fills the card info record unless NULL
@@ -1685,7 +1721,6 @@ int ReaderReceive(uint8_t *receivedAnswer, uint8_t *parity)
 // and num_cascades must be set (1: 4 Byte UID, 2: 7 Byte UID, 3: 10 Byte UID)
 // requests ATS unless no_rats is true
 int iso14443a_select_card(byte_t *uid_ptr, iso14a_card_select_t *p_hi14a_card, uint32_t *cuid_ptr, bool anticollision, uint8_t num_cascades, bool no_rats) {
 // and num_cascades must be set (1: 4 Byte UID, 2: 7 Byte UID, 3: 10 Byte UID)
 // requests ATS unless no_rats is true
 int iso14443a_select_card(byte_t *uid_ptr, iso14a_card_select_t *p_hi14a_card, uint32_t *cuid_ptr, bool anticollision, uint8_t num_cascades, bool no_rats) {
-       uint8_t wupa[]       = { 0x52 };  // 0x26 - REQA  0x52 - WAKE-UP
        uint8_t sel_all[]    = { 0x93,0x20 };
        uint8_t sel_uid[]    = { 0x93,0x70,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
        uint8_t rats[]       = { 0xE0,0x80,0x00,0x00 }; // FSD=256, FSDI=8, CID=0
        uint8_t sel_all[]    = { 0x93,0x20 };
        uint8_t sel_uid[]    = { 0x93,0x70,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
        uint8_t rats[]       = { 0xE0,0x80,0x00,0x00 }; // FSD=256, FSDI=8, CID=0
@@ -1698,16 +1733,19 @@ int iso14443a_select_card(byte_t *uid_ptr, iso14a_card_select_t *p_hi14a_card, u
        int cascade_level = 0;
        int len;
 
        int cascade_level = 0;
        int len;
 
-       // Broadcast for a card, WUPA (0x52) will force response from all cards in the field
-    ReaderTransmitBitsPar(wupa, 7, NULL, NULL);
-       
-       // Receive the ATQA
-       if(!ReaderReceive(resp, resp_par)) return 0;
+       // init card struct
+       if(p_hi14a_card) {
+               p_hi14a_card->uidlen = 0;
+               memset(p_hi14a_card->uid, 0, 10);
+               p_hi14a_card->ats_len = 0;
+       }
+
+       if (!GetATQA(resp, resp_par)) {
+               return 0;
+       }
 
        if(p_hi14a_card) {
                memcpy(p_hi14a_card->atqa, resp, 2);
 
        if(p_hi14a_card) {
                memcpy(p_hi14a_card->atqa, resp, 2);
-               p_hi14a_card->uidlen = 0;
-               memset(p_hi14a_card->uid,0,10);
        }
 
        if (anticollision) {
        }
 
        if (anticollision) {
@@ -1732,7 +1770,9 @@ int iso14443a_select_card(byte_t *uid_ptr, iso14a_card_select_t *p_hi14a_card, u
                if (anticollision) {
                        // SELECT_ALL
                        ReaderTransmit(sel_all, sizeof(sel_all), NULL);
                if (anticollision) {
                        // SELECT_ALL
                        ReaderTransmit(sel_all, sizeof(sel_all), NULL);
-                       if (!ReaderReceive(resp, resp_par)) return 0;
+                       if (!ReaderReceive(resp, resp_par)) {
+                               return 0;
+                       }
 
                        if (Demod.collisionPos) {                       // we had a collision and need to construct the UID bit by bit
                                memset(uid_resp, 0, 4);
 
                        if (Demod.collisionPos) {                       // we had a collision and need to construct the UID bit by bit
                                memset(uid_resp, 0, 4);
@@ -1754,7 +1794,9 @@ int iso14443a_select_card(byte_t *uid_ptr, iso14a_card_select_t *p_hi14a_card, u
                                        }
                                        collision_answer_offset = uid_resp_bits%8;
                                        ReaderTransmitBits(sel_uid, 16 + uid_resp_bits, NULL);
                                        }
                                        collision_answer_offset = uid_resp_bits%8;
                                        ReaderTransmitBits(sel_uid, 16 + uid_resp_bits, NULL);
-                                       if (!ReaderReceiveOffset(resp, collision_answer_offset, resp_par)) return 0;
+                                       if (!ReaderReceiveOffset(resp, collision_answer_offset, resp_par)) {
+                                               return 0;
+                                       }
                                }
                                // finally, add the last bits and BCC of the UID
                                for (uint16_t i = collision_answer_offset; i < (Demod.len-1)*8; i++, uid_resp_bits++) {
                                }
                                // finally, add the last bits and BCC of the UID
                                for (uint16_t i = collision_answer_offset; i < (Demod.len-1)*8; i++, uid_resp_bits++) {
@@ -1788,7 +1830,9 @@ int iso14443a_select_card(byte_t *uid_ptr, iso14a_card_select_t *p_hi14a_card, u
                ReaderTransmit(sel_uid, sizeof(sel_uid), NULL);
 
                // Receive the SAK
                ReaderTransmit(sel_uid, sizeof(sel_uid), NULL);
 
                // Receive the SAK
-               if (!ReaderReceive(resp, resp_par)) return 0;
+               if (!ReaderReceive(resp, resp_par)) {
+                       return 0;
+               }
                sak = resp[0];
        
                // Test if more parts of the uid are coming
                sak = resp[0];
        
                // Test if more parts of the uid are coming
@@ -1813,10 +1857,9 @@ int iso14443a_select_card(byte_t *uid_ptr, iso14a_card_select_t *p_hi14a_card, u
 
        if(p_hi14a_card) {
                p_hi14a_card->sak = sak;
 
        if(p_hi14a_card) {
                p_hi14a_card->sak = sak;
-               p_hi14a_card->ats_len = 0;
        }
 
        }
 
-       // non iso14443a compliant tag
+       // PICC compilant with iso14443a-4 ---> (SAK & 0x20 != 0)
        if( (sak & 0x20) == 0) return 2; 
 
        if (!no_rats) {
        if( (sak & 0x20) == 0) return 2; 
 
        if (!no_rats) {
@@ -1824,7 +1867,9 @@ int iso14443a_select_card(byte_t *uid_ptr, iso14a_card_select_t *p_hi14a_card, u
                AppendCrc14443a(rats, 2);
                ReaderTransmit(rats, sizeof(rats), NULL);
 
                AppendCrc14443a(rats, 2);
                ReaderTransmit(rats, sizeof(rats), NULL);
 
-               if (!(len = ReaderReceive(resp, resp_par))) return 0;
+               if (!(len = ReaderReceive(resp, resp_par))) {
+                       return 0;
+               }
 
                if(p_hi14a_card) {
                        memcpy(p_hi14a_card->ats, resp, len);
 
                if(p_hi14a_card) {
                        memcpy(p_hi14a_card->ats, resp, len);
@@ -1834,8 +1879,9 @@ int iso14443a_select_card(byte_t *uid_ptr, iso14a_card_select_t *p_hi14a_card, u
                // reset the PCB block number
                iso14_pcb_blocknum = 0;
 
                // reset the PCB block number
                iso14_pcb_blocknum = 0;
 
-               // set default timeout based on ATS
-               iso14a_set_ATS_timeout(resp);
+               // set default timeout and delay next transfer based on ATS
+               iso14a_set_ATS_times(resp);
+               
        }
        return 1;       
 }
        }
        return 1;       
 }
@@ -1844,7 +1890,7 @@ int iso14443a_select_card(byte_t *uid_ptr, iso14a_card_select_t *p_hi14a_card, u
 void iso14443a_setup(uint8_t fpga_minor_mode) {
        FpgaDownloadAndGo(FPGA_BITSTREAM_HF);
        // Set up the synchronous serial port
 void iso14443a_setup(uint8_t fpga_minor_mode) {
        FpgaDownloadAndGo(FPGA_BITSTREAM_HF);
        // Set up the synchronous serial port
-       FpgaSetupSsc();
+       FpgaSetupSsc(FPGA_MAJOR_MODE_HF_ISO14443A);
        // connect Demodulated Signal to ADC:
        SetAdcMuxFor(GPIO_MUXSEL_HIPKD);
 
        // connect Demodulated Signal to ADC:
        SetAdcMuxFor(GPIO_MUXSEL_HIPKD);
 
@@ -1866,32 +1912,106 @@ void iso14443a_setup(uint8_t fpga_minor_mode) {
        iso14a_set_timeout(1060); // 10ms default
 }
 
        iso14a_set_timeout(1060); // 10ms default
 }
 
-
-int iso14_apdu(uint8_t *cmd, uint16_t cmd_len, void *data) {
+/* Peter Fillmore 2015
+Added card id field to the function
+ info from ISO14443A standard
+b1 = Block Number
+b2 = RFU (always 1)
+b3 = depends on block
+b4 = Card ID following if set to 1
+b5 = depends on block type
+b6 = depends on block type
+b7,b8 = block type.
+Coding of I-BLOCK:
+b8 b7 b6 b5 b4 b3 b2 b1
+0  0  0  x  x  x  1  x
+b5 = chaining bit
+Coding of R-block:
+b8 b7 b6 b5 b4 b3 b2 b1
+1  0  1  x  x  0  1  x
+b5 = ACK/NACK
+Coding of S-block:
+b8 b7 b6 b5 b4 b3 b2 b1
+1  1  x  x  x  0  1  0 
+b5,b6 = 00 - DESELECT
+        11 - WTX 
+*/    
+int iso14_apdu(uint8_t *cmd, uint16_t cmd_len, bool send_chaining, void *data, uint8_t *res) {
        uint8_t parity[MAX_PARITY_SIZE];
        uint8_t parity[MAX_PARITY_SIZE];
-       uint8_t real_cmd[cmd_len+4];
-       real_cmd[0] = 0x0a; //I-Block
-       // put block number into the PCB
-       real_cmd[0] |= iso14_pcb_blocknum;
-       real_cmd[1] = 0x00; //CID: 0 //FIXME: allow multiple selected cards
-       memcpy(real_cmd+2, cmd, cmd_len);
-       AppendCrc14443a(real_cmd,cmd_len+2);
+       uint8_t real_cmd[cmd_len + 4];
+       
+       if (cmd_len) {
+               // ISO 14443 APDU frame: PCB [CID] [NAD] APDU CRC PCB=0x02
+               real_cmd[0] = 0x02; // bnr,nad,cid,chn=0; i-block(0x00) 
+               if (send_chaining) {
+                       real_cmd[0] |= 0x10;
+               }
+               // put block number into the PCB
+               real_cmd[0] |= iso14_pcb_blocknum;
+               memcpy(real_cmd + 1, cmd, cmd_len);
+       } else {
+               // R-block. ACK
+               real_cmd[0] = 0xA2; // r-block + ACK    
+               real_cmd[0] |= iso14_pcb_blocknum;
+       }
+       AppendCrc14443a(real_cmd, cmd_len + 1);
  
  
-       ReaderTransmit(real_cmd, cmd_len+4, NULL);
+       ReaderTransmit(real_cmd, cmd_len + 3, NULL);
+
        size_t len = ReaderReceive(data, parity);
        uint8_t *data_bytes = (uint8_t *) data;
        size_t len = ReaderReceive(data, parity);
        uint8_t *data_bytes = (uint8_t *) data;
-       if (!len)
+
+       if (!len) {
                return 0; //DATA LINK ERROR
                return 0; //DATA LINK ERROR
-       // if we received an I- or R(ACK)-Block with a block number equal to the
-       // current block number, toggle the current block number
-       else if (len >= 4 // PCB+CID+CRC = 4 bytes
+       } else{
+               // S-Block WTX 
+               while(len && ((data_bytes[0] & 0xF2) == 0xF2)) {
+                       uint32_t save_iso14a_timeout = iso14a_get_timeout();
+                       // temporarily increase timeout
+                       iso14a_set_timeout(MAX((data_bytes[1] & 0x3f) * save_iso14a_timeout, MAX_ISO14A_TIMEOUT));
+                       // Transmit WTX back 
+                       // byte1 - WTXM [1..59]. command FWT=FWT*WTXM
+                       data_bytes[1] = data_bytes[1] & 0x3f; // 2 high bits mandatory set to 0b
+                       // now need to fix CRC.
+                       AppendCrc14443a(data_bytes, len - 2);
+                       // transmit S-Block
+                       ReaderTransmit(data_bytes, len, NULL);
+                       // retrieve the result again (with increased timeout) 
+                       len = ReaderReceive(data, parity);
+                       data_bytes = data;
+                       // restore timeout
+                       iso14a_set_timeout(save_iso14a_timeout);
+               }
+
+               // if we received an I- or R(ACK)-Block with a block number equal to the
+               // current block number, toggle the current block number
+               if (len >= 3 // PCB+CRC = 3 bytes
                 && ((data_bytes[0] & 0xC0) == 0 // I-Block
                     || (data_bytes[0] & 0xD0) == 0x80) // R-Block with ACK bit set to 0
                 && (data_bytes[0] & 0x01) == iso14_pcb_blocknum) // equal block numbers
                 && ((data_bytes[0] & 0xC0) == 0 // I-Block
                     || (data_bytes[0] & 0xD0) == 0x80) // R-Block with ACK bit set to 0
                 && (data_bytes[0] & 0x01) == iso14_pcb_blocknum) // equal block numbers
-       {
-               iso14_pcb_blocknum ^= 1;
-       }
+               {
+                       iso14_pcb_blocknum ^= 1;
+               }
+               
+               // if we received I-block with chaining we need to send ACK and receive another block of data
+               if (res)
+                       *res = data_bytes[0];
 
 
+               // crc check
+               if (len >= 3 && !CheckCrc14443(CRC_14443_A, data_bytes, len)) {
+                       return -1;
+               }
+               
+       }
+       
+       if (len) {
+               // cut frame byte
+               len -= 1;
+               // memmove(data_bytes, data_bytes + 1, len);
+               for (int i = 0; i < len; i++)
+                       data_bytes[i] = data_bytes[i + 1];
+       }
+               
        return len;
 }
 
        return len;
 }
 
@@ -1908,25 +2028,36 @@ void ReaderIso14443a(UsbCommand *c)
        size_t lenbits = c->arg[1] >> 16;
        uint32_t timeout = c->arg[2];
        uint32_t arg0 = 0;
        size_t lenbits = c->arg[1] >> 16;
        uint32_t timeout = c->arg[2];
        uint32_t arg0 = 0;
-       byte_t buf[USB_CMD_DATA_SIZE];
+       byte_t buf[USB_CMD_DATA_SIZE] = {0};
        uint8_t par[MAX_PARITY_SIZE];
        uint8_t par[MAX_PARITY_SIZE];
+       bool cantSELECT = false;
   
   
-       if(param & ISO14A_CONNECT) {
+       set_tracing(true);
+       
+       if(param & ISO14A_CLEAR_TRACE) {
                clear_trace();
        }
 
                clear_trace();
        }
 
-       set_tracing(true);
-
        if(param & ISO14A_REQUEST_TRIGGER) {
                iso14a_set_trigger(true);
        }
 
        if(param & ISO14A_CONNECT) {
        if(param & ISO14A_REQUEST_TRIGGER) {
                iso14a_set_trigger(true);
        }
 
        if(param & ISO14A_CONNECT) {
+               LED_A_ON();
                iso14443a_setup(FPGA_HF_ISO14443A_READER_LISTEN);
                if(!(param & ISO14A_NO_SELECT)) {
                        iso14a_card_select_t *card = (iso14a_card_select_t*)buf;
                        arg0 = iso14443a_select_card(NULL, card, NULL, true, 0, param & ISO14A_NO_RATS);
                iso14443a_setup(FPGA_HF_ISO14443A_READER_LISTEN);
                if(!(param & ISO14A_NO_SELECT)) {
                        iso14a_card_select_t *card = (iso14a_card_select_t*)buf;
                        arg0 = iso14443a_select_card(NULL, card, NULL, true, 0, param & ISO14A_NO_RATS);
+
+                       // if we cant select then we cant send data
+                       if (arg0 != 1 && arg0 != 2) {
+                               // 1 - all is OK with ATS, 2 - without ATS
+                               cantSELECT = true;
+                       }
+                       FpgaDisableTracing();
+                       LED_B_ON();
                        cmd_send(CMD_ACK,arg0,card->uidlen,0,buf,sizeof(iso14a_card_select_t));
                        cmd_send(CMD_ACK,arg0,card->uidlen,0,buf,sizeof(iso14a_card_select_t));
+                       LED_B_OFF();
                }
        }
 
                }
        }
 
@@ -1934,12 +2065,16 @@ void ReaderIso14443a(UsbCommand *c)
                iso14a_set_timeout(timeout);
        }
 
                iso14a_set_timeout(timeout);
        }
 
-       if(param & ISO14A_APDU) {
-               arg0 = iso14_apdu(cmd, len, buf);
-               cmd_send(CMD_ACK,arg0,0,0,buf,sizeof(buf));
+       if(param & ISO14A_APDU && !cantSELECT) {
+               uint8_t res;
+               arg0 = iso14_apdu(cmd, len, (param & ISO14A_SEND_CHAINING), buf, &res);
+               FpgaDisableTracing();
+               LED_B_ON();
+               cmd_send(CMD_ACK, arg0, res, 0, buf, sizeof(buf));
+               LED_B_OFF();
        }
 
        }
 
-       if(param & ISO14A_RAW) {
+       if(param & ISO14A_RAW && !cantSELECT) {
                if(param & ISO14A_APPEND_CRC) {
                        if(param & ISO14A_TOPAZMODE) {
                                AppendCrc14443b(cmd,len);
                if(param & ISO14A_APPEND_CRC) {
                        if(param & ISO14A_TOPAZMODE) {
                                AppendCrc14443b(cmd,len);
@@ -1975,7 +2110,11 @@ void ReaderIso14443a(UsbCommand *c)
                        }
                }
                arg0 = ReaderReceive(buf, par);
                        }
                }
                arg0 = ReaderReceive(buf, par);
+               FpgaDisableTracing();
+
+               LED_B_ON();
                cmd_send(CMD_ACK,arg0,0,0,buf,sizeof(buf));
                cmd_send(CMD_ACK,arg0,0,0,buf,sizeof(buf));
+               LED_B_OFF();
        }
 
        if(param & ISO14A_REQUEST_TRIGGER) {
        }
 
        if(param & ISO14A_REQUEST_TRIGGER) {
@@ -2031,9 +2170,7 @@ void ReaderMifare(bool first_try)
        uint8_t receivedAnswer[MAX_MIFARE_FRAME_SIZE];
        uint8_t receivedAnswerPar[MAX_MIFARE_PARITY_SIZE];
 
        uint8_t receivedAnswer[MAX_MIFARE_FRAME_SIZE];
        uint8_t receivedAnswerPar[MAX_MIFARE_PARITY_SIZE];
 
-       if (first_try) { 
-               iso14443a_setup(FPGA_HF_ISO14443A_READER_MOD);
-       }
+       iso14443a_setup(FPGA_HF_ISO14443A_READER_MOD);
        
        // free eventually allocated BigBuf memory. We want all for tracing.
        BigBuf_free();
        
        // free eventually allocated BigBuf memory. We want all for tracing.
        BigBuf_free();
@@ -2041,9 +2178,9 @@ void ReaderMifare(bool first_try)
        clear_trace();
        set_tracing(true);
 
        clear_trace();
        set_tracing(true);
 
-       byte_t nt_diff = 0;
+       uint8_t nt_diff = 0;
        uint8_t par[1] = {0};   // maximum 8 Bytes to be sent here, 1 byte parity is therefore enough
        uint8_t par[1] = {0};   // maximum 8 Bytes to be sent here, 1 byte parity is therefore enough
-       static byte_t par_low = 0;
+       static uint8_t par_low = 0;
        bool led_on = true;
        uint8_t uid[10]  ={0};
        uint32_t cuid;
        bool led_on = true;
        uint8_t uid[10]  ={0};
        uint32_t cuid;
@@ -2051,11 +2188,11 @@ void ReaderMifare(bool first_try)
        uint32_t nt = 0;
        uint32_t previous_nt = 0;
        static uint32_t nt_attacked = 0;
        uint32_t nt = 0;
        uint32_t previous_nt = 0;
        static uint32_t nt_attacked = 0;
-       byte_t par_list[8] = {0x00};
-       byte_t ks_list[8] = {0x00};
+       uint8_t par_list[8] = {0x00};
+       uint8_t ks_list[8] = {0x00};
 
        #define PRNG_SEQUENCE_LENGTH  (1 << 16);
 
        #define PRNG_SEQUENCE_LENGTH  (1 << 16);
-       static uint32_t sync_time;
+       uint32_t sync_time = GetCountSspClk() & 0xfffffff8;
        static int32_t sync_cycles;
        int catch_up_cycles = 0;
        int last_catch_up = 0;
        static int32_t sync_cycles;
        int catch_up_cycles = 0;
        int last_catch_up = 0;
@@ -2065,10 +2202,9 @@ void ReaderMifare(bool first_try)
 
        if (first_try) { 
                mf_nr_ar3 = 0;
 
        if (first_try) { 
                mf_nr_ar3 = 0;
-               sync_time = GetCountSspClk() & 0xfffffff8;
+               par[0] = par_low = 0;
                sync_cycles = PRNG_SEQUENCE_LENGTH;                                                     // theory: Mifare Classic's random generator repeats every 2^16 cycles (and so do the tag nonces).
                nt_attacked = 0;
                sync_cycles = PRNG_SEQUENCE_LENGTH;                                                     // theory: Mifare Classic's random generator repeats every 2^16 cycles (and so do the tag nonces).
                nt_attacked = 0;
-               par[0] = 0;
        }
        else {
                // we were unsuccessful on a previous call. Try another READER nonce (first 3 parity bits remain the same)
        }
        else {
                // we were unsuccessful on a previous call. Try another READER nonce (first 3 parity bits remain the same)
@@ -2084,6 +2220,7 @@ void ReaderMifare(bool first_try)
 
        #define MAX_UNEXPECTED_RANDOM   4               // maximum number of unexpected (i.e. real) random numbers when trying to sync. Then give up.
        #define MAX_SYNC_TRIES                  32
 
        #define MAX_UNEXPECTED_RANDOM   4               // maximum number of unexpected (i.e. real) random numbers when trying to sync. Then give up.
        #define MAX_SYNC_TRIES                  32
+       #define SYNC_TIME_BUFFER                16              // if there is only SYNC_TIME_BUFFER left before next planned sync, wait for next PRNG cycle
        #define NUM_DEBUG_INFOS                 8               // per strategy
        #define MAX_STRATEGY                    3
        uint16_t unexpected_random = 0;
        #define NUM_DEBUG_INFOS                 8               // per strategy
        #define MAX_STRATEGY                    3
        uint16_t unexpected_random = 0;
@@ -2133,8 +2270,8 @@ void ReaderMifare(bool first_try)
                        sync_time = (sync_time & 0xfffffff8) + sync_cycles + catch_up_cycles;
                        catch_up_cycles = 0;
 
                        sync_time = (sync_time & 0xfffffff8) + sync_cycles + catch_up_cycles;
                        catch_up_cycles = 0;
 
-                       // if we missed the sync time already, advance to the next nonce repeat
-                       while(GetCountSspClk() > sync_time) {
+                       // if we missed the sync time already or are about to miss it, advance to the next nonce repeat
+                       while(sync_time < GetCountSspClk() + SYNC_TIME_BUFFER) {
                                elapsed_prng_sequences++;
                                sync_time = (sync_time & 0xfffffff8) + sync_cycles;
                        }
                                elapsed_prng_sequences++;
                                sync_time = (sync_time & 0xfffffff8) + sync_cycles;
                        }
@@ -2290,14 +2427,16 @@ void ReaderMifare(bool first_try)
                }
        }
        
                }
        }
        
-       byte_t buf[28];
+       FpgaDisableTracing();
+
+       uint8_t buf[32];
        memcpy(buf + 0,  uid, 4);
        num_to_bytes(nt, 4, buf + 4);
        memcpy(buf + 8,  par_list, 8);
        memcpy(buf + 16, ks_list, 8);
        memcpy(buf + 0,  uid, 4);
        num_to_bytes(nt, 4, buf + 4);
        memcpy(buf + 8,  par_list, 8);
        memcpy(buf + 16, ks_list, 8);
-       memcpy(buf + 24, mf_nr_ar, 4);
+       memcpy(buf + 24, mf_nr_ar, 8);
                
                
-       cmd_send(CMD_ACK, isOK, 0, 0, buf, 28);
+       cmd_send(CMD_ACK, isOK, 0, 0, buf, 32);
 
        // Thats it...
        FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
 
        // Thats it...
        FpgaWriteConfWord(FPGA_MAJOR_MODE_OFF);
@@ -2318,6 +2457,8 @@ void RAMFUNC SniffMifare(uint8_t param) {
 
        // C(red) A(yellow) B(green)
        LEDsoff();
 
        // C(red) A(yellow) B(green)
        LEDsoff();
+       LED_A_ON();
+       
        // init trace buffer
        clear_trace();
        set_tracing(true);
        // init trace buffer
        clear_trace();
        set_tracing(true);
@@ -2353,8 +2494,6 @@ void RAMFUNC SniffMifare(uint8_t param) {
        // Setup for the DMA.
        FpgaSetupSscDma((uint8_t *)dmaBuf, DMA_BUFFER_SIZE); // set transfer address and number of bytes. Start transfer.
 
        // Setup for the DMA.
        FpgaSetupSscDma((uint8_t *)dmaBuf, DMA_BUFFER_SIZE); // set transfer address and number of bytes. Start transfer.
 
-       LED_D_OFF();
-       
        // init sniffer
        MfSniffInit();
 
        // init sniffer
        MfSniffInit();
 
@@ -2362,11 +2501,10 @@ void RAMFUNC SniffMifare(uint8_t param) {
        for(uint32_t sniffCounter = 0; true; ) {
        
                if(BUTTON_PRESS()) {
        for(uint32_t sniffCounter = 0; true; ) {
        
                if(BUTTON_PRESS()) {
-                       DbpString("cancelled by button");
+                       DbpString("Canceled by button.");
                        break;
                }
 
                        break;
                }
 
-               LED_A_ON();
                WDT_HIT();
                
                if ((sniffCounter & 0x0000FFFF) == 0) { // from time to time
                WDT_HIT();
                
                if ((sniffCounter & 0x0000FFFF) == 0) { // from time to time
@@ -2412,14 +2550,12 @@ void RAMFUNC SniffMifare(uint8_t param) {
                        AT91C_BASE_PDC_SSC->PDC_RNCR = DMA_BUFFER_SIZE;
                }
 
                        AT91C_BASE_PDC_SSC->PDC_RNCR = DMA_BUFFER_SIZE;
                }
 
-               LED_A_OFF();
-               
                if (sniffCounter & 0x01) {
 
                        if(!TagIsActive) {              // no need to try decoding tag data if the reader is sending
                                uint8_t readerdata = (previous_data & 0xF0) | (*data >> 4);
                                if(MillerDecoding(readerdata, (sniffCounter-1)*4)) {
                if (sniffCounter & 0x01) {
 
                        if(!TagIsActive) {              // no need to try decoding tag data if the reader is sending
                                uint8_t readerdata = (previous_data & 0xF0) | (*data >> 4);
                                if(MillerDecoding(readerdata, (sniffCounter-1)*4)) {
-                                       LED_C_INV();
+
                                        if (MfSniffLogic(receivedCmd, Uart.len, Uart.parity, Uart.bitCount, true)) break;
 
                                        /* And ready to receive another command. */
                                        if (MfSniffLogic(receivedCmd, Uart.len, Uart.parity, Uart.bitCount, true)) break;
 
                                        /* And ready to receive another command. */
@@ -2434,7 +2570,6 @@ void RAMFUNC SniffMifare(uint8_t param) {
                        if(!ReaderIsActive) {           // no need to try decoding tag data if the reader is sending
                                uint8_t tagdata = (previous_data << 4) | (*data & 0x0F);
                                if(ManchesterDecoding(tagdata, 0, (sniffCounter-1)*4)) {
                        if(!ReaderIsActive) {           // no need to try decoding tag data if the reader is sending
                                uint8_t tagdata = (previous_data << 4) | (*data & 0x0F);
                                if(ManchesterDecoding(tagdata, 0, (sniffCounter-1)*4)) {
-                                       LED_C_INV();
 
                                        if (MfSniffLogic(receivedResponse, Demod.len, Demod.parity, Demod.bitCount, false)) break;
 
 
                                        if (MfSniffLogic(receivedResponse, Demod.len, Demod.parity, Demod.bitCount, false)) break;
 
@@ -2456,11 +2591,13 @@ void RAMFUNC SniffMifare(uint8_t param) {
 
        } // main cycle
 
 
        } // main cycle
 
-       DbpString("COMMAND FINISHED");
-
+       FpgaDisableTracing();
        FpgaDisableSscDma();
        FpgaDisableSscDma();
+       LEDsoff();
+
+       DbpString("COMMAND FINISHED.");
+
        MfSniffEnd();
        
        Dbprintf("maxDataLen=%x, Uart.state=%x, Uart.len=%x", maxDataLen, Uart.state, Uart.len);
        MfSniffEnd();
        
        Dbprintf("maxDataLen=%x, Uart.state=%x, Uart.len=%x", maxDataLen, Uart.state, Uart.len);
-       LEDsoff();
 }
 }
Impressum, Datenschutz