- improved reader sensitivity for 14443a cards (FPGA change!)

[proxmark3-svn] / armsrc / iso14443a.c

diff --git a/armsrc/iso14443a.c b/armsrc/iso14443a.c
index 33a946050041835cfef644cb73c3807cbd19ebcc..00dc622fdf0e1fd1d164b3e0e51378635334e007 100644 (file)
--- a/armsrc/iso14443a.c
+++ b/armsrc/iso14443a.c
@@ -96,9 +96,9 @@ uint32_t GetParity(const uint8_t * pbtCmd, int iLen)
        int i;
        uint32_t dwPar = 0;
 
        int i;
        uint32_t dwPar = 0;
 

-       // Generate the encrypted data
+       // Generate the parity bits

        for (i = 0; i < iLen; i++) {
        for (i = 0; i < iLen; i++) {

-               // Save the encrypted parity bit
+               // and save them to a 32Bit word

                dwPar |= ((OddByteParity[pbtCmd[i]]) << i);
        }
        return dwPar;
                dwPar |= ((OddByteParity[pbtCmd[i]]) << i);
        }
        return dwPar;


@@ -375,196 +375,176 @@ static RAMFUNC int MillerDecoding(int bit)
 }
 
 //=============================================================================
 }
 
 //=============================================================================

-// ISO 14443 Type A - Manchester
+// ISO 14443 Type A - Manchester decoder

 //=============================================================================
 //=============================================================================

+// Basics:
+// The tag will modulate the reader field by asserting different loads to it. As a consequence, the voltage
+// at the reader antenna will be modulated as well. The FPGA detects the modulation for us and would deliver e.g. the following:
+// ........ 0 0 1 1 1 1 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 .......
+// The Manchester decoder needs to identify the following sequences:
+// 4 ticks modulated followed by 4 ticks unmodulated:  Sequence D = 1 (also used as "start of communication")
+// 4 ticks unmodulated followed by 4 ticks modulated:  Sequence E = 0
+// 8 ticks unmodulated:                                                                        Sequence F = end of communication
+// 8 ticks modulated:                                                                  A collision. Save the collision position and treat as Sequence D
+// Note 1: the bitstream may start at any time (either in first or second nibble within the parameter bit). We therefore need to sync.
+// Note 2: parameter offset is used to determine the position of the parity bits (required for the anticollision command only)

 static tDemod Demod;
 
 static tDemod Demod;
 

-static RAMFUNC int ManchesterDecoding(int v)
+inline RAMFUNC bool IsModulation(byte_t b)

 {
 {

-       int bit;
-       int modulation;
-       //int error = 0;
-
-       if(!Demod.buff) {
-               Demod.buff = 1;
-               Demod.buffer = v;
-               return FALSE;
-       }
-       else {
-               bit = Demod.buffer;
-               Demod.buffer = v;
-       }
+       if (b >= 5 || b == 3)           // majority decision: 2 or more bits are set
+               return true;
+       else
+               return false;
+       
+}

 
 

-       if(Demod.state==DEMOD_UNSYNCD) {
-               Demod.output[Demod.len] = 0xfa;
-               Demod.syncBit = 0;
-               //Demod.samples = 0;
-               Demod.posCount = 1;             // This is the first half bit period, so after syncing handle the second part
+inline RAMFUNC bool IsModulationNibble1(byte_t b)
+{
+       return IsModulation((b & 0xE0) >> 5);
+}

 
 

-               if(bit & 0x08) {
-                       Demod.syncBit = 0x08;
-               }
+inline RAMFUNC bool IsModulationNibble2(byte_t b)
+{
+       return IsModulation((b & 0x0E) >> 1);
+}

 
 

-               if(bit & 0x04) {
-                       if(Demod.syncBit) {
-                               bit <<= 4;
+static RAMFUNC int ManchesterDecoding(int bit, uint16_t offset)
+{
+       
+       switch (Demod.state) {
+
+               case DEMOD_UNSYNCD:                                             // not yet synced
+                       Demod.len = 0;                                          // initialize number of decoded data bytes
+                       Demod.bitCount = offset;                        // initialize number of decoded data bits
+                       Demod.shiftReg = 0;                                     // initialize shiftreg to hold decoded data bits
+                       Demod.parityBits = 0;                           // initialize parity bits
+                       Demod.collisionPos = 0;                         // Position of collision bit
+                       
+                       if (IsModulationNibble1(bit) 
+                               && !IsModulationNibble2(bit)) {                                                         // this is the start bit
+                               Demod.samples = 8;
+                               if(trigger) LED_A_OFF();
+                               Demod.state = DEMOD_MANCHESTER_DATA;
+                       } else if (!IsModulationNibble1(bit) && IsModulationNibble2(bit)) { // this may be the first half of the start bit
+                                       Demod.samples = 4;
+                                       Demod.state = DEMOD_HALF_SYNCD;

                        }
                        }

-                       Demod.syncBit = 0x04;
-               }
+                       break;

 
 

-               if(bit & 0x02) {
-                       if(Demod.syncBit) {
-                               bit <<= 2;
-                       }
-                       Demod.syncBit = 0x02;
-               }

 
 

-               if(bit & 0x01 && Demod.syncBit) {
-                       Demod.syncBit = 0x01;
-               }
-               
-               if(Demod.syncBit) {
-                       Demod.len = 0;
-                       Demod.state = DEMOD_START_OF_COMMUNICATION;
-                       Demod.sub = SUB_FIRST_HALF;
-                       Demod.bitCount = 0;
-                       Demod.shiftReg = 0;
-                       Demod.parityBits = 0;
-                       Demod.samples = 0;
-                       if(Demod.posCount) {
-                               if(trigger) LED_A_OFF();
-                               switch(Demod.syncBit) {
-                                       case 0x08: Demod.samples = 3; break;
-                                       case 0x04: Demod.samples = 2; break;
-                                       case 0x02: Demod.samples = 1; break;
-                                       case 0x01: Demod.samples = 0; break;
+               case DEMOD_HALF_SYNCD:
+                       Demod.samples += 8;
+                       if (IsModulationNibble1(bit)) {                                                         // error: this was not a start bit.
+                               Demod.state = DEMOD_UNSYNCD;
+                       } else {
+                               if (IsModulationNibble2(bit)) {                                                 // modulation in first half
+                                       Demod.state = DEMOD_MOD_FIRST_HALF;
+                               } else {                                                                                                // no modulation in first half
+                                       Demod.state = DEMOD_NOMOD_FIRST_HALF;

                                }
                        }
                                }
                        }

-                       //error = 0;
-               }
-       }
-       else {
-               //modulation = bit & Demod.syncBit;
-               modulation = ((bit << 1) ^ ((Demod.buffer & 0x08) >> 3)) & Demod.syncBit;
-
-               Demod.samples += 4;
-
-               if(Demod.posCount==0) {
-                       Demod.posCount = 1;
-                       if(modulation) {
-                               Demod.sub = SUB_FIRST_HALF;
+                       break;
+                       
+                       
+               case DEMOD_MOD_FIRST_HALF:
+                       Demod.samples += 8;
+                       Demod.bitCount++;
+                       if (IsModulationNibble1(bit)) {                                                         // modulation in both halfs - collision
+                               if (!Demod.collisionPos) {
+                                       Demod.collisionPos = (Demod.len << 3) + Demod.bitCount;
+                               }
+                       }                                                                                                                       // modulation in first half only - Sequence D = 1
+                       Demod.shiftReg = (Demod.shiftReg >> 1) | 0x100;                         // add a 1 to the shiftreg
+                       if(Demod.bitCount >= 9) {                                                                       // if we decoded a full byte (including parity)
+                               Demod.parityBits <<= 1;                                                                 // make room for the parity bit
+                               Demod.output[Demod.len++] = (Demod.shiftReg & 0xff);
+                               Demod.parityBits |= ((Demod.shiftReg >> 8) & 0x01);     // store parity bit
+                               Demod.bitCount = 0;
+                               Demod.shiftReg = 0;

                        }
                        }

-                       else {
-                               Demod.sub = SUB_NONE;
+                       if (IsModulationNibble2(bit)) {                                                         // modulation in first half
+                               Demod.state = DEMOD_MOD_FIRST_HALF;
+                       } else {                                                                                                        // no modulation in first half
+                               Demod.state = DEMOD_NOMOD_FIRST_HALF;

                        }
                        }

-               }
-               else {
-                       Demod.posCount = 0;
-                       if(modulation && (Demod.sub == SUB_FIRST_HALF)) {
-                               if(Demod.state!=DEMOD_ERROR_WAIT) {
-                                       Demod.state = DEMOD_ERROR_WAIT;
-                                       Demod.output[Demod.len] = 0xaa;
-                                       //error = 0x01;
+                       break;
+
+
+               case DEMOD_NOMOD_FIRST_HALF:
+                       if (IsModulationNibble1(bit)) {                                                         // modulation in second half only - Sequence E = 0
+                               Demod.bitCount++;
+                               Demod.samples += 8;
+                               Demod.shiftReg = (Demod.shiftReg >> 1);                                 // add a 0 to the shiftreg
+                               if(Demod.bitCount >= 9) {                                                               // if we decoded a full byte (including parity)
+                                       Demod.parityBits <<= 1;                                                         // make room for the new parity bit
+                                       Demod.output[Demod.len++] = (Demod.shiftReg & 0xff);
+                                       Demod.parityBits |= ((Demod.shiftReg >> 8) & 0x01); // store parity bit
+                                       Demod.bitCount = 0;
+                                       Demod.shiftReg = 0;
+                               }
+                       } else {                                                                                                        // no modulation in both halves - End of communication
+                               Demod.samples += 4;
+                               if(Demod.bitCount > 0) {                                                                // if we decoded bits
+                                       Demod.shiftReg >>= (9 - Demod.bitCount);                        // add the remaining decoded bits to the output
+                                       Demod.output[Demod.len++] = Demod.shiftReg & 0xff;
+                                       // No parity bit, so just shift a 0
+                                       Demod.parityBits <<= 1;

                                }
                                }

+                               Demod.state = DEMOD_UNSYNCD;                                                    // start from the beginning
+                               return TRUE;                                                                                    // we are finished with decoding the raw data sequence

                        }
                        }

-                       else if(modulation) {
-                               Demod.sub = SUB_SECOND_HALF;
+                       if (IsModulationNibble2(bit)) {                                                         // modulation in first half
+                               Demod.state = DEMOD_MOD_FIRST_HALF;
+                       } else {                                                                                                        // no modulation in first half
+                               Demod.state = DEMOD_NOMOD_FIRST_HALF;

                        }
                        }

+                       break;
+                       

 
 

-                       switch(Demod.state) {
-                               case DEMOD_START_OF_COMMUNICATION:
-                                       if(Demod.sub == SUB_FIRST_HALF) {
-                                               Demod.state = DEMOD_MANCHESTER_D;
-                                       }
-                                       else {
-                                               Demod.output[Demod.len] = 0xab;
-                                               Demod.state = DEMOD_ERROR_WAIT;
-                                               //error = 0x02;
-                                       }
-                                       break;
-
-                               case DEMOD_MANCHESTER_D:
-                               case DEMOD_MANCHESTER_E:
-                                       if(Demod.sub == SUB_FIRST_HALF) {
-                                               Demod.bitCount++;
-                                               Demod.shiftReg = (Demod.shiftReg >> 1) ^ 0x100;
-                                               Demod.state = DEMOD_MANCHESTER_D;
+               case DEMOD_MANCHESTER_DATA:
+                       Demod.samples += 8;
+                       if (IsModulationNibble1(bit)) {                                                                 // modulation in first half
+                               if (IsModulationNibble2(bit) & 0x0f) {                                          // ... and in second half = collision
+                                       if (!Demod.collisionPos) {
+                                               Demod.collisionPos = (Demod.len << 3) + Demod.bitCount;

                                        }
                                        }

-                                       else if(Demod.sub == SUB_SECOND_HALF) {
-                                               Demod.bitCount++;
-                                               Demod.shiftReg >>= 1;
-                                               Demod.state = DEMOD_MANCHESTER_E;
+                               }                                                                                                               // modulation in first half only - Sequence D = 1
+                               Demod.bitCount++;
+                               Demod.shiftReg = (Demod.shiftReg >> 1) | 0x100;                 // in both cases, add a 1 to the shiftreg
+                               if(Demod.bitCount >= 9) {                                                               // if we decoded a full byte (including parity)
+                                       Demod.parityBits <<= 1;                                                         // make room for the parity bit
+                                       Demod.output[Demod.len++] = (Demod.shiftReg & 0xff);
+                                       Demod.parityBits |= ((Demod.shiftReg >> 8) & 0x01); // store parity bit
+                                       Demod.bitCount = 0;
+                                       Demod.shiftReg = 0;
+                               }
+                       } else {                                                                                                        // no modulation in first half
+                               if (IsModulationNibble2(bit)) {                                                 // and modulation in second half = Sequence E = 0
+                                       Demod.bitCount++;
+                                       Demod.shiftReg = (Demod.shiftReg >> 1);                         // add a 0 to the shiftreg
+                                       if(Demod.bitCount >= 9) {                                                       // if we decoded a full byte (including parity)
+                                               Demod.parityBits <<= 1;                                                 // make room for the new parity bit
+                                               Demod.output[Demod.len++] = (Demod.shiftReg & 0xff);
+                                               Demod.parityBits |= ((Demod.shiftReg >> 8) & 0x01); // store parity bit
+                                               Demod.bitCount = 0;
+                                               Demod.shiftReg = 0;

                                        }
                                        }

-                                       else {
-                                               Demod.state = DEMOD_MANCHESTER_F;
+                               } else {                                                                                                // no modulation in both halves - End of communication
+                                       if(Demod.bitCount > 0) {                                                        // if we decoded bits
+                                               Demod.shiftReg >>= (9 - Demod.bitCount);                // add the remaining decoded bits to the output
+                                               Demod.output[Demod.len++] = Demod.shiftReg & 0xff;
+                                               // No parity bit, so just shift a 0
+                                               Demod.parityBits <<= 1;

                                        }
                                        }

-                                       break;
-
-                               case DEMOD_MANCHESTER_F:
-                                       // Tag response does not need to be a complete byte!
-                                       if(Demod.len > 0 || Demod.bitCount > 0) {
-                                               if(Demod.bitCount > 0) {
-                                                       Demod.shiftReg >>= (9 - Demod.bitCount);
-                                                       Demod.output[Demod.len] = Demod.shiftReg & 0xff;
-                                                       Demod.len++;
-                                                       // No parity bit, so just shift a 0
-                                                       Demod.parityBits <<= 1;
-                                               }
-
-                                               Demod.state = DEMOD_UNSYNCD;
-                                               return TRUE;
-                                       }
-                                       else {
-                                               Demod.output[Demod.len] = 0xad;
-                                               Demod.state = DEMOD_ERROR_WAIT;
-                                               //error = 0x03;
-                                       }
-                                       break;
-
-                               case DEMOD_ERROR_WAIT:
-                                       Demod.state = DEMOD_UNSYNCD;
-                                       break;
-
-                               default:
-                                       Demod.output[Demod.len] = 0xdd;
-                                       Demod.state = DEMOD_UNSYNCD;
-                                       break;
-                       }
-
-                       if(Demod.bitCount>=9) {
-                               Demod.output[Demod.len] = Demod.shiftReg & 0xff;
-                               Demod.len++;
-
-                               Demod.parityBits <<= 1;
-                               Demod.parityBits ^= ((Demod.shiftReg >> 8) & 0x01);
-
-                               Demod.bitCount = 0;
-                               Demod.shiftReg = 0;
+                                       Demod.state = DEMOD_UNSYNCD;                                            // start from the beginning
+                                       return TRUE;                                                                            // we are finished with decoding the raw data sequence
+                               }

                        }
                        }

+                       
+       } 

 
 

-                       /*if(error) {
-                               Demod.output[Demod.len] = 0xBB;
-                               Demod.len++;
-                               Demod.output[Demod.len] = error & 0xFF;
-                               Demod.len++;
-                               Demod.output[Demod.len] = 0xBB;
-                               Demod.len++;
-                               Demod.output[Demod.len] = bit & 0xFF;
-                               Demod.len++;
-                               Demod.output[Demod.len] = Demod.buffer & 0xFF;
-                               Demod.len++;
-                               Demod.output[Demod.len] = Demod.syncBit & 0xFF;
-                               Demod.len++;
-                               Demod.output[Demod.len] = 0xBB;
-                               Demod.len++;
-                               return TRUE;
-                       }*/
-
-               }
-
-       } // end (state != UNSYNCED)
-
-    return FALSE;
+    return FALSE;      // not finished yet, need more data

 }
 
 //=============================================================================
 }
 
 //=============================================================================


@@ -691,7 +671,7 @@ void RAMFUNC SnoopIso14443a(uint8_t param) {
                        LED_B_OFF();
                }
 
                        LED_B_OFF();
                }
 

-               if(ManchesterDecoding(data[0] & 0x0F)) {
+               if(ManchesterDecoding(data[0], 0)) {

                        LED_B_ON();
 
                        if (!LogTrace(receivedResponse, Demod.len, 0 - Demod.samples, Demod.parityBits, FALSE)) break;
                        LED_B_ON();
 
                        if (!LogTrace(receivedResponse, Demod.len, 0 - Demod.samples, Demod.parityBits, FALSE)) break;


@@ -775,48 +755,48 @@ static void CodeIso14443aAsTag(const uint8_t *cmd, int len){
        CodeIso14443aAsTagPar(cmd, len, GetParity(cmd, len));
 }
 
        CodeIso14443aAsTagPar(cmd, len, GetParity(cmd, len));
 }
 

-//-----------------------------------------------------------------------------
-// This is to send a NACK kind of answer, its only 3 bits, I know it should be 4
-//-----------------------------------------------------------------------------
-static void CodeStrangeAnswerAsTag()
-{
-       int i;
-
-       ToSendReset();
-
-       // Correction bit, might be removed when not needed
-       ToSendStuffBit(0);
-       ToSendStuffBit(0);
-       ToSendStuffBit(0);
-       ToSendStuffBit(0);
-       ToSendStuffBit(1);  // 1
-       ToSendStuffBit(0);
-       ToSendStuffBit(0);
-       ToSendStuffBit(0);
-
-       // Send startbit
-       ToSend[++ToSendMax] = SEC_D;
-
-       // 0
-       ToSend[++ToSendMax] = SEC_E;
-
-       // 0
-       ToSend[++ToSendMax] = SEC_E;
-
-       // 1
-       ToSend[++ToSendMax] = SEC_D;
-
-       // Send stopbit
-       ToSend[++ToSendMax] = SEC_F;
-
-       // Flush the buffer in FPGA!!
-       for(i = 0; i < 5; i++) {
-               ToSend[++ToSendMax] = SEC_F;
-       }
-
-       // Convert from last byte pos to length
-       ToSendMax++;
-}
+////-----------------------------------------------------------------------------
+//// This is to send a NACK kind of answer, its only 3 bits, I know it should be 4
+////-----------------------------------------------------------------------------
+//static void CodeStrangeAnswerAsTag()
+//{
+//     int i;
+//
+//     ToSendReset();
+//
+//     // Correction bit, might be removed when not needed
+//     ToSendStuffBit(0);
+//     ToSendStuffBit(0);
+//     ToSendStuffBit(0);
+//     ToSendStuffBit(0);
+//     ToSendStuffBit(1);  // 1
+//     ToSendStuffBit(0);
+//     ToSendStuffBit(0);
+//     ToSendStuffBit(0);
+//
+//     // Send startbit
+//     ToSend[++ToSendMax] = SEC_D;
+//
+//     // 0
+//     ToSend[++ToSendMax] = SEC_E;
+//
+//     // 0
+//     ToSend[++ToSendMax] = SEC_E;
+//
+//     // 1
+//     ToSend[++ToSendMax] = SEC_D;
+//
+//     // Send stopbit
+//     ToSend[++ToSendMax] = SEC_F;
+//
+//     // Flush the buffer in FPGA!!
+//     for(i = 0; i < 5; i++) {
+//             ToSend[++ToSendMax] = SEC_F;
+//     }
+//
+//     // Convert from last byte pos to length
+//     ToSendMax++;
+//}

 
 static void Code4bitAnswerAsTag(uint8_t cmd)
 {
 
 static void Code4bitAnswerAsTag(uint8_t cmd)
 {


@@ -908,6 +888,67 @@ int EmSendCmdEx(uint8_t *resp, int respLen, int correctionNeeded);
 int EmSendCmd(uint8_t *resp, int respLen);
 int EmSendCmdPar(uint8_t *resp, int respLen, uint32_t par);
 
 int EmSendCmd(uint8_t *resp, int respLen);
 int EmSendCmdPar(uint8_t *resp, int respLen, uint32_t par);
 

+static uint8_t* free_buffer_pointer = (((uint8_t *)BigBuf) + FREE_BUFFER_OFFSET);
+
+typedef struct {
+  uint8_t* response;
+  size_t   response_n;
+  uint8_t* modulation;
+  size_t   modulation_n;
+} tag_response_info_t;
+
+void reset_free_buffer() {
+  free_buffer_pointer = (((uint8_t *)BigBuf) + FREE_BUFFER_OFFSET);
+}
+
+bool prepare_tag_modulation(tag_response_info_t* response_info, size_t max_buffer_size) {
+       // Exmaple response, answer to MIFARE Classic read block will be 16 bytes + 2 CRC = 18 bytes
+       // This will need the following byte array for a modulation sequence
+       //    144        data bits (18 * 8)
+       //     18        parity bits
+       //      2        Start and stop
+       //      1        Correction bit (Answer in 1172 or 1236 periods, see FPGA)
+       //      1        just for the case
+       // ----------- +
+       //    166 bytes, since every bit that needs to be send costs us a byte
+       //
+  
+  // Prepare the tag modulation bits from the message
+  CodeIso14443aAsTag(response_info->response,response_info->response_n);
+  
+  // Make sure we do not exceed the free buffer space
+  if (ToSendMax > max_buffer_size) {
+    Dbprintf("Out of memory, when modulating bits for tag answer:");
+    Dbhexdump(response_info->response_n,response_info->response,false);
+    return false;
+  }
+  
+  // Copy the byte array, used for this modulation to the buffer position
+  memcpy(response_info->modulation,ToSend,ToSendMax);
+  
+  // Store the number of bytes that were used for encoding/modulation
+  response_info->modulation_n = ToSendMax;
+  
+  return true;
+}
+
+bool prepare_allocated_tag_modulation(tag_response_info_t* response_info) {
+  // Retrieve and store the current buffer index
+  response_info->modulation = free_buffer_pointer;
+  
+  // Determine the maximum size we can use from our buffer
+  size_t max_buffer_size = (((uint8_t *)BigBuf)+FREE_BUFFER_OFFSET+FREE_BUFFER_SIZE)-free_buffer_pointer;
+  
+  // Forward the prepare tag modulation function to the inner function
+  if (prepare_tag_modulation(response_info,max_buffer_size)) {
+    // Update the free buffer offset
+    free_buffer_pointer += ToSendMax;
+    return true;
+  } else {
+    return false;
+  }
+}
+

 //-----------------------------------------------------------------------------
 // Main loop of simulated tag: receive commands from reader, decide what
 // response to send, and send it.
 //-----------------------------------------------------------------------------
 // Main loop of simulated tag: receive commands from reader, decide what
 // response to send, and send it.


@@ -990,57 +1031,41 @@ void SimulateIso14443aTag(int tagType, int uid_1st, int uid_2nd, byte_t* data)
        ComputeCrc14443(CRC_14443_A, response3a, 1, &response3a[1], &response3a[2]);
 
        uint8_t response5[] = { 0x00, 0x00, 0x00, 0x00 }; // Very random tag nonce
        ComputeCrc14443(CRC_14443_A, response3a, 1, &response3a[1], &response3a[2]);
 
        uint8_t response5[] = { 0x00, 0x00, 0x00, 0x00 }; // Very random tag nonce

-       uint8_t response6[] = { 0x03, 0x3B, 0x00, 0x00, 0x00 }; // dummy ATS (pseudo-ATR), answer to RATS
-       ComputeCrc14443(CRC_14443_A, response6, 3, &response6[3], &response6[4]);
-
-       uint8_t *resp = NULL;
-       int respLen;
-
-       // Longest possible response will be 16 bytes + 2 CRC = 18 bytes
-       // This will need
-       //    144        data bits (18 * 8)
-       //     18        parity bits
-       //      2        Start and stop
-       //      1        Correction bit (Answer in 1172 or 1236 periods, see FPGA)
-       //      1        just for the case
-       // ----------- +
-       //    166
-       //
-       // 166 bytes, since every bit that needs to be send costs us a byte
-       //
-
-       // Respond with card type
-       uint8_t *resp1 = (((uint8_t *)BigBuf) + FREE_BUFFER_OFFSET);
-       int resp1Len;
-
-       // Anticollision cascade1 - respond with uid
-       uint8_t *resp2 = (((uint8_t *)BigBuf) + FREE_BUFFER_OFFSET + 166);
-       int resp2Len;
-
-       // Anticollision cascade2 - respond with 2nd half of uid if asked
-       // we're only going to be asked if we set the 1st byte of the UID (during cascade1) to 0x88
-       uint8_t *resp2a = (((uint8_t *)BigBuf) + 1140);
-       int resp2aLen;
-
-       // Acknowledge select - cascade 1
-       uint8_t *resp3 = (((uint8_t *)BigBuf) + FREE_BUFFER_OFFSET + (166*2));
-       int resp3Len;
-
-       // Acknowledge select - cascade 2
-       uint8_t *resp3a = (((uint8_t *)BigBuf) + FREE_BUFFER_OFFSET + (166*3));
-       int resp3aLen;
-
-       // Response to a read request - not implemented atm
-       uint8_t *resp4 = (((uint8_t *)BigBuf) + FREE_BUFFER_OFFSET + (166*4));
-//     int resp4Len;
-
-       // Authenticate response - nonce
-       uint8_t *resp5 = (((uint8_t *)BigBuf) + FREE_BUFFER_OFFSET + (166*5));
-       int resp5Len;
-
-       // Authenticate response - nonce
-       uint8_t *resp6 = (((uint8_t *)BigBuf) + FREE_BUFFER_OFFSET + (166*6));
-       int resp6Len;
+       uint8_t response6[] = { 0x04, 0x58, 0x00, 0x02, 0x00, 0x00 }; // dummy ATS (pseudo-ATR), answer to RATS
+       ComputeCrc14443(CRC_14443_A, response6, 4, &response6[4], &response6[5]);
+
+  #define TAG_RESPONSE_COUNT 7
+  tag_response_info_t responses[TAG_RESPONSE_COUNT] = {
+    { .response = response1,  .response_n = sizeof(response1)  },  // Answer to request - respond with card type
+    { .response = response2,  .response_n = sizeof(response2)  },  // Anticollision cascade1 - respond with uid
+    { .response = response2a, .response_n = sizeof(response2a) },  // Anticollision cascade2 - respond with 2nd half of uid if asked
+    { .response = response3,  .response_n = sizeof(response3)  },  // Acknowledge select - cascade 1
+    { .response = response3a, .response_n = sizeof(response3a) },  // Acknowledge select - cascade 2
+    { .response = response5,  .response_n = sizeof(response5)  },  // Authentication answer (random nonce)
+    { .response = response6,  .response_n = sizeof(response6)  },  // dummy ATS (pseudo-ATR), answer to RATS
+  };
+
+  // Allocate 512 bytes for the dynamic modulation, created when the reader querries for it
+  // Such a response is less time critical, so we can prepare them on the fly
+  #define DYNAMIC_RESPONSE_BUFFER_SIZE 64
+  #define DYNAMIC_MODULATION_BUFFER_SIZE 512
+  uint8_t dynamic_response_buffer[DYNAMIC_RESPONSE_BUFFER_SIZE];
+  uint8_t dynamic_modulation_buffer[DYNAMIC_MODULATION_BUFFER_SIZE];
+  tag_response_info_t dynamic_response_info = {
+    .response = dynamic_response_buffer,
+    .response_n = 0,
+    .modulation = dynamic_modulation_buffer,
+    .modulation_n = 0
+  };
+  
+  // Reset the offset pointer of the free buffer
+  reset_free_buffer();
+  
+  // Prepare the responses of the anticollision phase
+       // there will be not enough time to do this at the moment the reader sends it REQA
+  for (size_t i=0; i<TAG_RESPONSE_COUNT; i++) {
+    prepare_allocated_tag_modulation(&responses[i]);
+  }

 
        uint8_t *receivedCmd = (((uint8_t *)BigBuf) + RECV_CMD_OFFSET);
        int len;
 
        uint8_t *receivedCmd = (((uint8_t *)BigBuf) + RECV_CMD_OFFSET);
        int len;


@@ -1052,60 +1077,22 @@ void SimulateIso14443aTag(int tagType, int uid_1st, int uid_2nd, byte_t* data)
        // Just to allow some checks
        int happened = 0;
        int happened2 = 0;
        // Just to allow some checks
        int happened = 0;
        int happened2 = 0;

-

        int cmdsRecvd = 0;
        int cmdsRecvd = 0;

-       uint8_t* respdata = NULL;
-       int respsize = 0;
-//     uint8_t nack = 0x04;
-
-       memset(receivedCmd, 0x44, RECV_CMD_SIZE);
-
-       // Prepare the responses of the anticollision phase
-       // there will be not enough time to do this at the moment the reader sends it REQA
-
-       // Answer to request
-       CodeIso14443aAsTag(response1, sizeof(response1));
-       memcpy(resp1, ToSend, ToSendMax); resp1Len = ToSendMax;
-
-       // Send our UID (cascade 1)
-       CodeIso14443aAsTag(response2, sizeof(response2));
-       memcpy(resp2, ToSend, ToSendMax); resp2Len = ToSendMax;
-
-       // Answer to select (cascade1)
-       CodeIso14443aAsTag(response3, sizeof(response3));
-       memcpy(resp3, ToSend, ToSendMax); resp3Len = ToSendMax;
-
-       // Send the cascade 2 2nd part of the uid
-       CodeIso14443aAsTag(response2a, sizeof(response2a));
-       memcpy(resp2a, ToSend, ToSendMax); resp2aLen = ToSendMax;
-
-       // Answer to select (cascade 2)
-       CodeIso14443aAsTag(response3a, sizeof(response3a));
-       memcpy(resp3a, ToSend, ToSendMax); resp3aLen = ToSendMax;
-
-       // Strange answer is an example of rare message size (3 bits)
-       CodeStrangeAnswerAsTag();
-       memcpy(resp4, ToSend, ToSendMax);// resp4Len = ToSendMax;
-
-       // Authentication answer (random nonce)
-       CodeIso14443aAsTag(response5, sizeof(response5));
-       memcpy(resp5, ToSend, ToSendMax); resp5Len = ToSendMax;
-
-       // dummy ATS (pseudo-ATR), answer to RATS
-       CodeIso14443aAsTag(response6, sizeof(response6));
-       memcpy(resp6, ToSend, ToSendMax); resp6Len = ToSendMax;

 
        // We need to listen to the high-frequency, peak-detected path.
        SetAdcMuxFor(GPIO_MUXSEL_HIPKD);
        FpgaSetupSsc();
 
        cmdsRecvd = 0;
 
        // We need to listen to the high-frequency, peak-detected path.
        SetAdcMuxFor(GPIO_MUXSEL_HIPKD);
        FpgaSetupSsc();
 
        cmdsRecvd = 0;

+  tag_response_info_t* p_response;

 
        LED_A_ON();
        for(;;) {
 
        LED_A_ON();
        for(;;) {

+    // Clean receive command buffer
+    memset(receivedCmd, 0x44, RECV_CMD_SIZE);

        
                if(!GetIso14443aCommandFromReader(receivedCmd, &len, RECV_CMD_SIZE)) {
        
                if(!GetIso14443aCommandFromReader(receivedCmd, &len, RECV_CMD_SIZE)) {

-                       DbpString("button press");
+                       DbpString("Button press");

                        break;
                }
     
                        break;
                }
     


@@ -1113,69 +1100,98 @@ void SimulateIso14443aTag(int tagType, int uid_1st, int uid_2nd, byte_t* data)
                        LogTrace(receivedCmd,len, 0, Uart.parityBits, TRUE);
                }
     
                        LogTrace(receivedCmd,len, 0, Uart.parityBits, TRUE);
                }
     

+    p_response = NULL;
+    

                // doob - added loads of debug strings so we can see what the reader is saying to us during the sim as hi14alist is not populated
                // Okay, look at the command now.
                lastorder = order;
                if(receivedCmd[0] == 0x26) { // Received a REQUEST
                // doob - added loads of debug strings so we can see what the reader is saying to us during the sim as hi14alist is not populated
                // Okay, look at the command now.
                lastorder = order;
                if(receivedCmd[0] == 0x26) { // Received a REQUEST

-                       resp = resp1; respLen = resp1Len; order = 1;
-                       respdata = response1;
-                       respsize = sizeof(response1);
+                       p_response = &responses[0]; order = 1;

                } else if(receivedCmd[0] == 0x52) { // Received a WAKEUP
                } else if(receivedCmd[0] == 0x52) { // Received a WAKEUP

-                       resp = resp1; respLen = resp1Len; order = 6;
-                       respdata = response1;
-                       respsize = sizeof(response1);
+                       p_response = &responses[0]; order = 6;

                } else if(receivedCmd[1] == 0x20 && receivedCmd[0] == 0x93) {   // Received request for UID (cascade 1)
                } else if(receivedCmd[1] == 0x20 && receivedCmd[0] == 0x93) {   // Received request for UID (cascade 1)

-                       resp = resp2; respLen = resp2Len; order = 2;
-                       respdata = response2;
-                       respsize = sizeof(response2);
+                       p_response = &responses[1]; order = 2;

                } else if(receivedCmd[1] == 0x20 && receivedCmd[0] == 0x95) { // Received request for UID (cascade 2)
                } else if(receivedCmd[1] == 0x20 && receivedCmd[0] == 0x95) { // Received request for UID (cascade 2)

-                       resp = resp2a; respLen = resp2aLen; order = 20;
-                       respdata = response2a;
-                       respsize = sizeof(response2a);
+                       p_response = &responses[2]; order = 20;

                } else if(receivedCmd[1] == 0x70 && receivedCmd[0] == 0x93) {   // Received a SELECT (cascade 1)
                } else if(receivedCmd[1] == 0x70 && receivedCmd[0] == 0x93) {   // Received a SELECT (cascade 1)

-                       resp = resp3; respLen = resp3Len; order = 3;
-                       respdata = response3;
-                       respsize = sizeof(response3);
+                       p_response = &responses[3]; order = 3;

                } else if(receivedCmd[1] == 0x70 && receivedCmd[0] == 0x95) {   // Received a SELECT (cascade 2)
                } else if(receivedCmd[1] == 0x70 && receivedCmd[0] == 0x95) {   // Received a SELECT (cascade 2)

-                       resp = resp3a; respLen = resp3aLen; order = 30;
-                       respdata = response3a;
-                       respsize = sizeof(response3a);
+                       p_response = &responses[4]; order = 30;

                } else if(receivedCmd[0] == 0x30) {     // Received a (plain) READ
                } else if(receivedCmd[0] == 0x30) {     // Received a (plain) READ

-//                     resp = resp4; respLen = resp4Len; order = 4; // Do nothing
-//                     respdata = &nack;
-//                     respsize = sizeof(nack); // 4-bit answer

                        EmSendCmdEx(data+(4*receivedCmd[0]),16,false);
                        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
                        EmSendCmdEx(data+(4*receivedCmd[0]),16,false);
                        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

-                       respLen = 0;
+      p_response = NULL;

                } else if(receivedCmd[0] == 0x50) {     // Received a HALT
 //                     DbpString("Reader requested we HALT!:");
                } else if(receivedCmd[0] == 0x50) {     // Received a HALT
 //                     DbpString("Reader requested we HALT!:");

-                       // Do not respond
-                       resp = resp1; respLen = 0; order = 0;
-                       respdata = NULL;
-                       respsize = 0;
+      p_response = NULL;

                } else if(receivedCmd[0] == 0x60 || receivedCmd[0] == 0x61) {   // Received an authentication request
                } else if(receivedCmd[0] == 0x60 || receivedCmd[0] == 0x61) {   // Received an authentication request

-                       resp = resp5; respLen = resp5Len; order = 7;
-                       respdata = response5;
-                       respsize = sizeof(response5);
+                       p_response = &responses[5]; order = 7;

                } else if(receivedCmd[0] == 0xE0) {     // Received a RATS request
                } else if(receivedCmd[0] == 0xE0) {     // Received a RATS request

-                       resp = resp6; respLen = resp6Len; order = 70;
-                       respdata = response6;
-                       respsize = sizeof(response6);
-               } else {
-      if (order == 7 && len ==8) {
-        uint32_t nr = bytes_to_num(receivedCmd,4);
-        uint32_t ar = bytes_to_num(receivedCmd+4,4);
-        Dbprintf("Auth attempt {nr}{ar}: %08x %08x",nr,ar);
-      } else {
-        // Never seen this command before
-        Dbprintf("Received unknown command (len=%d):",len);
-        Dbhexdump(len,receivedCmd,false);
+                       p_response = &responses[6]; order = 70;
+               } else if (order == 7 && len ==8) { // Received authentication request
+      uint32_t nr = bytes_to_num(receivedCmd,4);
+      uint32_t ar = bytes_to_num(receivedCmd+4,4);
+      Dbprintf("Auth attempt {nr}{ar}: %08x %08x",nr,ar);
+    } else {
+      // Check for ISO 14443A-4 compliant commands, look at left nibble
+      switch (receivedCmd[0]) {
+
+        case 0x0B:
+        case 0x0A: { // IBlock (command)
+          dynamic_response_info.response[0] = receivedCmd[0];
+          dynamic_response_info.response[1] = 0x00;
+          dynamic_response_info.response[2] = 0x90;
+          dynamic_response_info.response[3] = 0x00;
+          dynamic_response_info.response_n = 4;
+        } break;
+
+        case 0x1A:
+        case 0x1B: { // Chaining command
+          dynamic_response_info.response[0] = 0xaa | ((receivedCmd[0]) & 1);
+          dynamic_response_info.response_n = 2;
+        } break;
+
+        case 0xaa:
+        case 0xbb: {
+          dynamic_response_info.response[0] = receivedCmd[0] ^ 0x11;
+          dynamic_response_info.response_n = 2;
+        } break;
+          
+        case 0xBA: { //
+          memcpy(dynamic_response_info.response,"\xAB\x00",2);
+          dynamic_response_info.response_n = 2;
+        } break;
+
+        case 0xCA:
+        case 0xC2: { // Readers sends deselect command
+          memcpy(dynamic_response_info.response,"\xCA\x00",2);
+          dynamic_response_info.response_n = 2;
+        } break;
+
+        default: {
+          // Never seen this command before
+          Dbprintf("Received unknown command (len=%d):",len);
+          Dbhexdump(len,receivedCmd,false);
+          // Do not respond
+          dynamic_response_info.response_n = 0;
+        } break;
+      }
+      
+      if (dynamic_response_info.response_n > 0) {
+        // Copy the CID from the reader query
+        dynamic_response_info.response[1] = receivedCmd[1];
+
+        // Add CRC bytes, always used in ISO 14443A-4 compliant cards
+        AppendCrc14443a(dynamic_response_info.response,dynamic_response_info.response_n);
+        dynamic_response_info.response_n += 2;
+        
+        if (prepare_tag_modulation(&dynamic_response_info,DYNAMIC_MODULATION_BUFFER_SIZE) == false) {
+          Dbprintf("Error preparing tag response");
+          break;
+        }
+        p_response = &dynamic_response_info;

       }
       }

-      // Do not respond
-      resp = resp1; respLen = 0; order = 0;
-      respdata = NULL;
-      respsize = 0;

                }
 
                // Count number of wakeups received after a halt
                }
 
                // Count number of wakeups received after a halt


@@ -1193,25 +1209,19 @@ void SimulateIso14443aTag(int tagType, int uid_1st, int uid_2nd, byte_t* data)
                if(cmdsRecvd > 999) {
                        DbpString("1000 commands later...");
                        break;
                if(cmdsRecvd > 999) {
                        DbpString("1000 commands later...");
                        break;

-               } else {
-                       cmdsRecvd++;
-               }
-
-               if(respLen > 0) {
-                       EmSendCmd14443aRaw(resp, respLen, receivedCmd[0] == 0x52);
-               }
-               
-               if (tracing) {
-                       if (respdata != NULL) {
-                               LogTrace(respdata,respsize, 0, SwapBits(GetParity(respdata,respsize),respsize), FALSE);
-                       }
-                       if(traceLen > TRACE_SIZE) {
-                               DbpString("Trace full");
-                               break;
-                       }

                }
                }

+               cmdsRecvd++;

 
 

-               memset(receivedCmd, 0x44, RECV_CMD_SIZE);
+               if (p_response != NULL) {
+      EmSendCmd14443aRaw(p_response->modulation, p_response->modulation_n, receivedCmd[0] == 0x52);
+      if (tracing) {
+        LogTrace(p_response->response,p_response->response_n,0,SwapBits(GetParity(p_response->response,p_response->response_n),p_response->response_n),FALSE);
+        if(traceLen > TRACE_SIZE) {
+          DbpString("Trace full");
+//          break;
+        }
+      }
+    }

   }
 
        Dbprintf("%x %x %x", happened, happened2, cmdsRecvd);
   }
 
        Dbprintf("%x %x %x", happened, happened2, cmdsRecvd);


@@ -1242,9 +1252,6 @@ void PrepareDelayedTransfer(uint16_t delay)
        }
 }
 
        }
 }
 

-
-
-

 //-----------------------------------------------------------------------------
 // Transmit the command (to the tag) that was placed in ToSend[].
 // Parameter timing:
 //-----------------------------------------------------------------------------
 // Transmit the command (to the tag) that was placed in ToSend[].
 // Parameter timing:


@@ -1269,7 +1276,7 @@ static void TransmitFor14443a(const uint8_t *cmd, int len, uint32_t *timing)
                while(GetCountMifare() < (*timing & 0xfffffff8));               // Delay transfer (multiple of 8 MF clock ticks)
        }
 
                while(GetCountMifare() < (*timing & 0xfffffff8));               // Delay transfer (multiple of 8 MF clock ticks)
        }
 

-       for(c = 0; c < 10;) {   // standard delay for each transfer (allow tag to be ready after last transmission)
+       for(c = 0; c < 10;) {   // standard delay for each transfer (allow tag to be ready after last transmission?)

                if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) {
                        AT91C_BASE_SSC->SSC_THR = 0x00; 
                        c++;
                if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_TXRDY)) {
                        AT91C_BASE_SSC->SSC_THR = 0x00; 
                        c++;


@@ -1531,13 +1538,12 @@ int EmSendCmdPar(uint8_t *resp, int respLen, uint32_t par){
 //-----------------------------------------------------------------------------
 // Wait a certain time for tag response
 //  If a response is captured return TRUE
 //-----------------------------------------------------------------------------
 // Wait a certain time for tag response
 //  If a response is captured return TRUE

-//  If it takes to long return FALSE
+//  If it takes too long return FALSE

 //-----------------------------------------------------------------------------
 //-----------------------------------------------------------------------------

-static int GetIso14443aAnswerFromTag(uint8_t *receivedResponse, int maxLen, int *samples, int *elapsed) //uint8_t *buffer
+static int GetIso14443aAnswerFromTag(uint8_t *receivedResponse, uint16_t offset, int maxLen, int *samples)

 {
 {

-       // buffer needs to be 512 bytes

        int c;
        int c;

-
+       

        // Set FPGA mode to "reader listen mode", no modulation (listen
        // only, since we are receiving, not transmitting).
        // Signal field is on with the appropriate LED
        // Set FPGA mode to "reader listen mode", no modulation (listen
        // only, since we are receiving, not transmitting).
        // Signal field is on with the appropriate LED


@@ -1550,7 +1556,6 @@ static int GetIso14443aAnswerFromTag(uint8_t *receivedResponse, int maxLen, int
        Demod.state = DEMOD_UNSYNCD;
 
        uint8_t b;
        Demod.state = DEMOD_UNSYNCD;
 
        uint8_t b;

-       if (elapsed) *elapsed = 0;

 
        c = 0;
        for(;;) {
 
        c = 0;
        for(;;) {


@@ -1563,12 +1568,8 @@ static int GetIso14443aAnswerFromTag(uint8_t *receivedResponse, int maxLen, int
                if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) {
                        if(c < iso14a_timeout) { c++; } else { return FALSE; }
                        b = (uint8_t)AT91C_BASE_SSC->SSC_RHR;
                if(AT91C_BASE_SSC->SSC_SR & (AT91C_SSC_RXRDY)) {
                        if(c < iso14a_timeout) { c++; } else { return FALSE; }
                        b = (uint8_t)AT91C_BASE_SSC->SSC_RHR;

-                       if(ManchesterDecoding((b>>4) & 0xf)) {
-                               *samples = ((c - 1) << 3) + 4;
-                               return TRUE;
-                       }
-                       if(ManchesterDecoding(b & 0x0f)) {
-                               *samples = c << 3;
+                       if(ManchesterDecoding(b, offset)) {
+                               *samples = Demod.samples;

                                return TRUE;
                        }
                }
                                return TRUE;
                        }
                }


@@ -1577,15 +1578,15 @@ static int GetIso14443aAnswerFromTag(uint8_t *receivedResponse, int maxLen, int
 
 void ReaderTransmitBitsPar(uint8_t* frame, int bits, uint32_t par, uint32_t *timing)
 {
 
 void ReaderTransmitBitsPar(uint8_t* frame, int bits, uint32_t par, uint32_t *timing)
 {

- 
+

   CodeIso14443aBitsAsReaderPar(frame,bits,par);
   
   CodeIso14443aBitsAsReaderPar(frame,bits,par);
   

-  // Select the card
+  // Send command to tag

   TransmitFor14443a(ToSend, ToSendMax, timing);
   if(trigger)
        LED_A_ON();
   
   TransmitFor14443a(ToSend, ToSendMax, timing);
   if(trigger)
        LED_A_ON();
   

-  // Store reader command in buffer
+  // Log reader command in trace buffer

   if (tracing) LogTrace(frame,nbytes(bits),0,par,TRUE);
 }
 
   if (tracing) LogTrace(frame,nbytes(bits),0,par,TRUE);
 }
 


@@ -1594,38 +1595,49 @@ void ReaderTransmitPar(uint8_t* frame, int len, uint32_t par, uint32_t *timing)
   ReaderTransmitBitsPar(frame,len*8,par, timing);
 }
 
   ReaderTransmitBitsPar(frame,len*8,par, timing);
 }
 

+void ReaderTransmitBits(uint8_t* frame, int len, uint32_t *timing)
+{
+  // Generate parity and redirect
+  ReaderTransmitBitsPar(frame,len,GetParity(frame,len/8), timing);
+}
+

 void ReaderTransmit(uint8_t* frame, int len, uint32_t *timing)
 {
   // Generate parity and redirect
   ReaderTransmitBitsPar(frame,len*8,GetParity(frame,len), timing);
 }
 
 void ReaderTransmit(uint8_t* frame, int len, uint32_t *timing)
 {
   // Generate parity and redirect
   ReaderTransmitBitsPar(frame,len*8,GetParity(frame,len), timing);
 }
 

+int ReaderReceiveOffset(uint8_t* receivedAnswer, uint16_t offset)
+{
+       int samples = 0;
+       if (!GetIso14443aAnswerFromTag(receivedAnswer,offset,160,&samples)) return FALSE;
+       if (tracing) LogTrace(receivedAnswer,Demod.len,samples,Demod.parityBits,FALSE);
+       if(samples == 0) return FALSE;
+       return Demod.len;
+}
+

 int ReaderReceive(uint8_t* receivedAnswer)
 {
 int ReaderReceive(uint8_t* receivedAnswer)
 {

-  int samples = 0;
-  if (!GetIso14443aAnswerFromTag(receivedAnswer,160,&samples,0)) return FALSE;
-  if (tracing) LogTrace(receivedAnswer,Demod.len,samples,Demod.parityBits,FALSE);
-  if(samples == 0) return FALSE;
-  return Demod.len;
+       return ReaderReceiveOffset(receivedAnswer, 0);

 }
 
 }
 

-int ReaderReceivePar(uint8_t* receivedAnswer, uint32_t * parptr)
+int ReaderReceivePar(uint8_t *receivedAnswer, uint32_t *parptr)

 {
 {

-  int samples = 0;
-  if (!GetIso14443aAnswerFromTag(receivedAnswer,160,&samples,0)) return FALSE;
-  if (tracing) LogTrace(receivedAnswer,Demod.len,samples,Demod.parityBits,FALSE);
+       int samples = 0;
+       if (!GetIso14443aAnswerFromTag(receivedAnswer,0,160,&samples)) return FALSE;
+       if (tracing) LogTrace(receivedAnswer,Demod.len,samples,Demod.parityBits,FALSE);

        *parptr = Demod.parityBits;
        *parptr = Demod.parityBits;

-  if(samples == 0) return FALSE;
-  return Demod.len;
+       if(samples == 0) return FALSE;
+       return Demod.len;

 }
 
 }
 

-/* performs iso14443a anticolision procedure
+/* performs iso14443a anticollision procedure

  * fills the uid pointer unless NULL
  * fills resp_data unless NULL */
 int iso14443a_select_card(byte_t* uid_ptr, iso14a_card_select_t* p_hi14a_card, uint32_t* cuid_ptr) {
   uint8_t wupa[]       = { 0x52 };  // 0x26 - REQA  0x52 - WAKE-UP
   uint8_t sel_all[]    = { 0x93,0x20 };
  * fills the uid pointer unless NULL
  * fills resp_data unless NULL */
 int iso14443a_select_card(byte_t* uid_ptr, iso14a_card_select_t* p_hi14a_card, uint32_t* cuid_ptr) {
   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 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* resp = (((uint8_t *)BigBuf) + FREE_BUFFER_OFFSET);  // was 3560 - tied to other size changes
   byte_t uid_resp[4];
   uint8_t rats[]       = { 0xE0,0x80,0x00,0x00 }; // FSD=256, FSDI=8, CID=0
   uint8_t* resp = (((uint8_t *)BigBuf) + FREE_BUFFER_OFFSET);  // was 3560 - tied to other size changes
   byte_t uid_resp[4];


@@ -1639,7 +1651,7 @@ int iso14443a_select_card(byte_t* uid_ptr, iso14a_card_select_t* p_hi14a_card, u
     ReaderTransmitBitsPar(wupa,7,0, NULL);
   // Receive the ATQA
   if(!ReaderReceive(resp)) return 0;
     ReaderTransmitBitsPar(wupa,7,0, NULL);
   // Receive the ATQA
   if(!ReaderReceive(resp)) return 0;

-//  Dbprintf("atqa: %02x %02x",resp[0],resp[1]);
+  // Dbprintf("atqa: %02x %02x",resp[0],resp[1]);

 
   if(p_hi14a_card) {
     memcpy(p_hi14a_card->atqa, resp, 2);
 
   if(p_hi14a_card) {
     memcpy(p_hi14a_card->atqa, resp, 2);


@@ -1663,19 +1675,50 @@ int iso14443a_select_card(byte_t* uid_ptr, iso14a_card_select_t* p_hi14a_card, u
     ReaderTransmit(sel_all,sizeof(sel_all), NULL);
     if (!ReaderReceive(resp)) return 0;
 
     ReaderTransmit(sel_all,sizeof(sel_all), NULL);
     if (!ReaderReceive(resp)) return 0;
 

-    // First backup the current uid
-    memcpy(uid_resp,resp,4);
-    uid_resp_len = 4;
+       if (Demod.collisionPos) {                       // we had a collision and need to construct the UID bit by bit
+               memset(uid_resp, 0, 4);
+               uint16_t uid_resp_bits = 0;
+               uint16_t collision_answer_offset = 0;
+               // anti-collision-loop:
+               while (Demod.collisionPos) {
+                       Dbprintf("Multiple tags detected. Collision after Bit %d", Demod.collisionPos);
+                       for (uint16_t i = collision_answer_offset; i < Demod.collisionPos; i++, uid_resp_bits++) {      // add valid UID bits before collision point
+                               uint16_t UIDbit = (resp[i/8] >> (i % 8)) & 0x01;
+                               uid_resp[uid_resp_bits & 0xf8] |= UIDbit << (uid_resp_bits % 8);
+                       }
+                       uid_resp[uid_resp_bits/8] |= 1 << (uid_resp_bits % 8);                                  // next time select the card(s) with a 1 in the collision position
+                       uid_resp_bits++;
+                       // construct anticollosion command:
+                       sel_uid[1] = ((2 + uid_resp_bits/8) << 4) | (uid_resp_bits & 0x07);     // length of data in bytes and bits
+                       for (uint16_t i = 0; i <= uid_resp_bits/8; i++) {
+                               sel_uid[2+i] = uid_resp[i];
+                       }
+                       collision_answer_offset = uid_resp_bits%8;
+                       ReaderTransmitBits(sel_uid, 16 + uid_resp_bits, NULL);
+                       if (!ReaderReceiveOffset(resp, collision_answer_offset)) 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++) {
+                       uint16_t UIDbit = (resp[i/8] >> (i%8)) & 0x01;
+                       uid_resp[uid_resp_bits/8] |= UIDbit << (uid_resp_bits % 8);
+               }
+
+       } else {                // no collision, use the response to SELECT_ALL as current uid
+               memcpy(uid_resp,resp,4);
+       }
+       uid_resp_len = 4;

     //    Dbprintf("uid: %02x %02x %02x %02x",uid_resp[0],uid_resp[1],uid_resp[2],uid_resp[3]);
 
     //    Dbprintf("uid: %02x %02x %02x %02x",uid_resp[0],uid_resp[1],uid_resp[2],uid_resp[3]);
 

-        // calculate crypto UID. Always use last 4 Bytes.
+    // calculate crypto UID. Always use last 4 Bytes.

     if(cuid_ptr) {
         *cuid_ptr = bytes_to_num(uid_resp, 4);
     }
 
     // Construct SELECT UID command
     if(cuid_ptr) {
         *cuid_ptr = bytes_to_num(uid_resp, 4);
     }
 
     // Construct SELECT UID command

-    memcpy(sel_uid+2,resp,5);
-    AppendCrc14443a(sel_uid,7);
+       sel_uid[1] = 0x70;                                                                                                      // transmitting a full UID (1 Byte cmd, 1 Byte NVB, 4 Byte UID, 1 Byte BCC, 2 Bytes CRC)
+    memcpy(sel_uid+2,uid_resp,4);                                                                              // the UID
+       sel_uid[6] = sel_uid[2] ^ sel_uid[3] ^ sel_uid[4] ^ sel_uid[5];         // calculate and add BCC
+    AppendCrc14443a(sel_uid,7);                                                                                        // calculate and add CRC

     ReaderTransmit(sel_uid,sizeof(sel_uid), NULL);
 
     // Receive the SAK
     ReaderTransmit(sel_uid,sizeof(sel_uid), NULL);
 
     // Receive the SAK


@@ -1683,7 +1726,7 @@ int iso14443a_select_card(byte_t* uid_ptr, iso14a_card_select_t* p_hi14a_card, u
     sak = resp[0];
 
     // Test if more parts of the uid are comming
     sak = resp[0];
 
     // Test if more parts of the uid are comming

-    if ((sak & 0x04) && uid_resp[0] == 0x88) {
+    if ((sak & 0x04) /* && uid_resp[0] == 0x88 */) {

       // Remove first byte, 0x88 is not an UID byte, it CT, see page 3 of:
       // http://www.nxp.com/documents/application_note/AN10927.pdf
       memcpy(uid_resp, uid_resp + 1, 3);
       // Remove first byte, 0x88 is not an UID byte, it CT, see page 3 of:
       // http://www.nxp.com/documents/application_note/AN10927.pdf
       memcpy(uid_resp, uid_resp + 1, 3);


@@ -1742,6 +1785,7 @@ void iso14443a_setup() {
        FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_READER_MOD);
        SpinDelay(7); // iso14443-3 specifies 5ms max.
 
        FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_READER_MOD);
        SpinDelay(7); // iso14443-3 specifies 5ms max.
 

+       Demod.state = DEMOD_UNSYNCD;

        iso14a_timeout = 2048; //default
 }
 
        iso14a_timeout = 2048; //default
 }
 


@@ -1788,6 +1832,7 @@ void ReaderIso14443a(UsbCommand * c)
        if(param & ISO14A_CONNECT) {
                iso14a_clear_trace();
        }
        if(param & ISO14A_CONNECT) {
                iso14a_clear_trace();
        }

+

        iso14a_set_tracing(true);
 
        if(param & ISO14A_REQUEST_TRIGGER) {
        iso14a_set_tracing(true);
 
        if(param & ISO14A_REQUEST_TRIGGER) {


@@ -1949,8 +1994,6 @@ void ReaderMifare(bool first_try)
                //keep the card active
                FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_READER_MOD);
 
                //keep the card active
                FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_ISO14443A | FPGA_HF_ISO14443A_READER_MOD);
 

-               // CodeIso14443aBitsAsReaderPar(mf_auth, sizeof(mf_auth)*8, GetParity(mf_auth, sizeof(mf_auth)*8));
-

                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;
 


@@ -2618,7 +2661,7 @@ void RAMFUNC SniffMifare(uint8_t param) {
                        Demod.state = DEMOD_UNSYNCD;
                }
 
                        Demod.state = DEMOD_UNSYNCD;
                }
 

-               if(ManchesterDecoding(data[0] & 0x0F)) {
+               if(ManchesterDecoding(data[0], 0)) {

                        LED_C_INV();
 
                        if (MfSniffLogic(receivedResponse, Demod.len, Demod.parityBits, Demod.bitCount, FALSE)) break;
                        LED_C_INV();
 
                        if (MfSniffLogic(receivedResponse, Demod.len, Demod.parityBits, Demod.bitCount, FALSE)) break;