Merge pull request #862 from pwpiwi/fix_iclass_sim

[proxmark3-svn] / armsrc / iso15693.c

diff --git a/armsrc/iso15693.c b/armsrc/iso15693.c
index f33e0156cda0614154f45bc56694b1828dc39c86..85af0859ec8721a89d2a0d83d8784a0e12552d74 100644 (file)
--- a/armsrc/iso15693.c
+++ b/armsrc/iso15693.c
@@ -246,6 +246,8 @@ static void CodeIso15693AsReader256(uint8_t *cmd, int n)
        // }
 // }
 
+static const uint8_t encode_4bits[16] = { 0xaa, 0x6a, 0x9a, 0x5a, 0xa6, 0x66, 0x96, 0x56, 0xa9, 0x69, 0x99, 0x59, 0xa5, 0x65, 0x95, 0x55 };
+
 void CodeIso15693AsTag(uint8_t *cmd, size_t len) {
        /*
         * SOF comprises 3 parts;
@@ -280,16 +282,9 @@ void CodeIso15693AsTag(uint8_t *cmd, size_t len) {
        ToSend[++ToSendMax] = 0x1D;  // 00011101
 
        // data
-       for(int i = 0; i < len; i++) {
-               for(int j = 0; j < 8; j++) {
-                       if ((cmd[i] >> j) & 0x01) {
-                                       ToSendStuffBit(0);
-                                       ToSendStuffBit(1);
-                       } else {
-                                       ToSendStuffBit(1);
-                                       ToSendStuffBit(0);
-                       }
-               }
+       for (int i = 0; i < len; i++) {
+               ToSend[++ToSendMax] = encode_4bits[cmd[i] & 0xF];
+               ToSend[++ToSendMax] = encode_4bits[cmd[i] >> 4];
        }
 
        // EOF
@@ -327,19 +322,32 @@ static void TransmitTo15693Tag(const uint8_t *cmd, int len, uint32_t start_time)
 //-----------------------------------------------------------------------------
 // Transmit the tag response (to the reader) that was placed in cmd[].
 //-----------------------------------------------------------------------------
-void TransmitTo15693Reader(const uint8_t *cmd, size_t len, uint32_t start_time, bool slow) {
+void TransmitTo15693Reader(const uint8_t *cmd, size_t len, uint32_t *start_time, uint32_t slot_time, bool slow) {
        // don't use the FPGA_HF_SIMULATOR_MODULATE_424K_8BIT minor mode. It would spoil GetCountSspClk()
        FpgaWriteConfWord(FPGA_MAJOR_MODE_HF_SIMULATOR | FPGA_HF_SIMULATOR_MODULATE_424K);
 
-       uint8_t shift_delay = start_time & 0x00000007;
+       uint32_t modulation_start_time = *start_time + 3 * 8;  // no need to transfer the unmodulated start of SOF
+       
+       while (GetCountSspClk() > (modulation_start_time & 0xfffffff8) + 3) { // we will miss the intended time
+               if (slot_time) {
+                       modulation_start_time += slot_time; // use next available slot
+               } else {
+                       modulation_start_time = (modulation_start_time & 0xfffffff8) + 8; // next possible time
+               }
+       }
+
+       while (GetCountSspClk() < (modulation_start_time & 0xfffffff8)) 
+               /* wait */ ;
 
-       while (GetCountSspClk() < (start_time & 0xfffffff8)) ;
+       uint8_t shift_delay = modulation_start_time & 0x00000007;
+
+       *start_time = modulation_start_time - 3 * 8;
 
        LED_C_ON();
        uint8_t bits_to_shift = 0x00;
        uint8_t bits_to_send = 0x00;
-       for(size_t c = 0; c < len; c++) {
-               for (int i = 7; i >= 0; i--) {
+       for (size_t c = 0; c < len; c++) {
+               for (int i = (c==0?4:7); i >= 0; i--) {
                        uint8_t cmd_bits = ((cmd[c] >> i) & 0x01) ? 0xff : 0x00;
                        for (int j = 0; j < (slow?4:1); ) {
                                if (AT91C_BASE_SSC->SSC_SR & AT91C_SSC_TXRDY) {
@@ -361,7 +369,6 @@ void TransmitTo15693Reader(const uint8_t *cmd, size_t len, uint32_t start_time,
                }
        }
        LED_C_OFF();
-
 }
 
 
@@ -827,6 +834,7 @@ static int inline __attribute__((always_inline)) Handle15693SampleFromReader(uin
                        break;
 
                case STATE_READER_RECEIVE_DATA_1_OUT_OF_4:
+                       bit = !!bit;
                        DecodeReader->posCount++;
                        if (DecodeReader->posCount == 1) {
                                DecodeReader->sum1 = bit;
@@ -839,17 +847,14 @@ static int inline __attribute__((always_inline)) Handle15693SampleFromReader(uin
                        }
                        if (DecodeReader->posCount == 8) {
                                DecodeReader->posCount = 0;
-                               int corr10 = DecodeReader->sum1 - DecodeReader->sum2;
-                               int corr01 = DecodeReader->sum2 - DecodeReader->sum1;
-                               int corr11 = (DecodeReader->sum1 + DecodeReader->sum2) / 2;
-                               if (corr01 > corr11 && corr01 > corr10) { // EOF
+                               if (DecodeReader->sum1 <= 1 && DecodeReader->sum2 >= 3) { // EOF
                                        LED_B_OFF(); // Finished receiving
                                        DecodeReaderReset(DecodeReader);
                                        if (DecodeReader->byteCount != 0) {
                                                return true;
                                        }
                                }
-                               if (corr10 > corr11) { // detected a 2bit position
+                               if (DecodeReader->sum1 >= 3 && DecodeReader->sum2 <= 1) { // detected a 2bit position
                                        DecodeReader->shiftReg >>= 2;
                                        DecodeReader->shiftReg |= (DecodeReader->bitCount << 6);
                                }
@@ -869,6 +874,7 @@ static int inline __attribute__((always_inline)) Handle15693SampleFromReader(uin
                        break;
 
                case STATE_READER_RECEIVE_DATA_1_OUT_OF_256:
+                       bit = !!bit;
                        DecodeReader->posCount++;
                        if (DecodeReader->posCount == 1) {
                                DecodeReader->sum1 = bit;
@@ -881,17 +887,14 @@ static int inline __attribute__((always_inline)) Handle15693SampleFromReader(uin
                        }
                        if (DecodeReader->posCount == 8) {
                                DecodeReader->posCount = 0;
-                               int corr10 = DecodeReader->sum1 - DecodeReader->sum2;
-                               int corr01 = DecodeReader->sum2 - DecodeReader->sum1;
-                               int corr11 = (DecodeReader->sum1 + DecodeReader->sum2) / 2;
-                               if (corr01 > corr11 && corr01 > corr10) { // EOF
+                               if (DecodeReader->sum1 <= 1 && DecodeReader->sum2 >= 3) { // EOF
                                        LED_B_OFF(); // Finished receiving
                                        DecodeReaderReset(DecodeReader);
                                        if (DecodeReader->byteCount != 0) {
                                                return true;
                                        }
                                }
-                               if (corr10 > corr11) { // detected the bit position
+                               if (DecodeReader->sum1 >= 3 && DecodeReader->sum2 <= 1) { // detected the bit position
                                        DecodeReader->shiftReg = DecodeReader->bitCount;
                                }
                                if (DecodeReader->bitCount == 255) { // we have a full byte
@@ -1533,7 +1536,7 @@ void SimTagIso15693(uint32_t parameter, uint8_t *uid)
                if ((cmd_len >= 5) && (cmd[0] & ISO15693_REQ_INVENTORY) && (cmd[1] == ISO15693_INVENTORY)) { // TODO: check more flags
                        bool slow = !(cmd[0] & ISO15693_REQ_DATARATE_HIGH);
                        start_time = eof_time + DELAY_ISO15693_VCD_TO_VICC_SIM - DELAY_ARM_TO_READER_SIM;
-                       TransmitTo15693Reader(ToSend, ToSendMax, start_time, slow);
+                       TransmitTo15693Reader(ToSend, ToSendMax, &start_time, 0, slow);
                }
 
                Dbprintf("%d bytes read from reader:", cmd_len);