Merge pull request #462 from pwpiwi/fix_hfmfsim
authorIceman <iceman@iuse.se>
Fri, 10 Nov 2017 17:07:25 +0000 (18:07 +0100)
committerGitHub <noreply@github.com>
Fri, 10 Nov 2017 17:07:25 +0000 (18:07 +0100)
Fix hf mf sim (issue #412)

1  2 
armsrc/iso14443a.c

diff --combined armsrc/iso14443a.c
index 45425e079db22ee3d4cb5a3275b1dc07f2a320a7,9fa08044281bd778adb1ce2f7045f1927ab5a92c..8e2c56b04638947c4ef50240be0d8904cc09e1f5
  
  #include "iso14443a.h"
  
 +#include <stdio.h>
 +#include <string.h>
  #include "proxmark3.h"
  #include "apps.h"
  #include "util.h"
 -#include "string.h"
  #include "cmd.h"
  #include "iso14443crc.h"
  #include "crapto1/crapto1.h"
@@@ -901,11 -900,11 +901,11 @@@ static int GetIso14443aCommandFromReade
  }
  
  
- static int EmSend4bitEx(uint8_t resp, bool correctionNeeded);
+ static int EmSend4bitEx(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) {
@@@ -1139,7 -1138,7 +1139,7 @@@ void SimulateIso14443aTag(int tagType, 
                } 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;
                cmdsRecvd++;
  
                if (p_response != NULL) {
-                       EmSendPrecompiledCmd(p_response, receivedCmd[0] == 0x52);
+                       EmSendPrecompiledCmd(p_response);
                }
                
                if (!tracing) {
@@@ -1414,12 -1413,6 +1414,6 @@@ int EmGetCmd(uint8_t *received, uint16_
        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 =
        // 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);
  
-       // 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();
  
  
                // 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();
  }
  
  
- 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;
-       
+       bool correctionNeeded;
        // Modulate Manchester
        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;
                }
        }
  
-       // 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;
  }
  
  
- static int EmSend4bitEx(uint8_t resp, bool correctionNeeded){
+ static int EmSend4bitEx(uint8_t 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;
  
  
  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);
-       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;
  }
  
  
- 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);
-       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);
-       return EmSendCmdExPar(resp, respLen, false, par);
+       return EmSendCmdExPar(resp, respLen, 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;
@@@ -1874,47 -1877,29 +1878,47 @@@ void iso14443a_setup(uint8_t fpga_minor
  
  int iso14_apdu(uint8_t *cmd, uint16_t cmd_len, void *data) {
        uint8_t parity[MAX_PARITY_SIZE];
 -      uint8_t real_cmd[cmd_len+4];
 -      real_cmd[0] = 0x0a; //I-Block
 +      uint8_t real_cmd[cmd_len + 4];
 +      
 +      // ISO 14443 APDU frame: PCB [CID] [NAD] APDU CRC PCB=0x02
 +      real_cmd[0] = 0x02; // bnr,nad,cid,chn=0; i-block(0x00) 
        // 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);
 +      memcpy(real_cmd + 1, cmd, cmd_len);
 +      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;
 -      if (!len)
 +
 +      if (!len) {
                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{
 +              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
 -      {
 -              iso14_pcb_blocknum ^= 1;
 -      }
 +              {
 +                      iso14_pcb_blocknum ^= 1;
 +              }
  
 +              // crc check
 +              if (len >=3 && !CheckCrc14443(CRC_14443_A, data_bytes, len)) {
 +                      return -1;
 +              }
 +              
 +      }
 +      
 +      // 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;
  }
  
@@@ -1933,34 -1918,23 +1937,34 @@@ void ReaderIso14443a(UsbCommand *c
        uint32_t arg0 = 0;
        byte_t buf[USB_CMD_DATA_SIZE] = {0};
        uint8_t par[MAX_PARITY_SIZE];
 +      bool cantSELECT = false;
    
 -      if(param & ISO14A_CONNECT) {
 +      set_tracing(true);
 +      
 +      if(param & ISO14A_CLEAR_TRACE) {
                clear_trace();
        }
  
 -      set_tracing(true);
 -
        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);
 +
 +                      // 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;
 +                      }
 +                      
 +                      LED_B_ON();
                        cmd_send(CMD_ACK,arg0,card->uidlen,0,buf,sizeof(iso14a_card_select_t));
 +                      LED_B_OFF();
                }
        }
  
                iso14a_set_timeout(timeout);
        }
  
 -      if(param & ISO14A_APDU) {
 +      if(param & ISO14A_APDU && !cantSELECT) {
                arg0 = iso14_apdu(cmd, len, buf);
 -              cmd_send(CMD_ACK,arg0,0,0,buf,sizeof(buf));
 +              LED_B_ON();
 +              cmd_send(CMD_ACK, arg0, 0, 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);
                        }
                }
                arg0 = ReaderReceive(buf, par);
 +
 +              LED_B_ON();
                cmd_send(CMD_ACK,arg0,0,0,buf,sizeof(buf));
 +              LED_B_OFF();
        }
  
        if(param & ISO14A_REQUEST_TRIGGER) {
Impressum, Datenschutz