command to change fidi send by host

5 files changed, 79 insertions, 252 deletions

diff --git a/firmware/src_simtrace/host_communication.c b/firmware/src_simtrace/host_communication.c
index fcec1c1..990119f 100644
--- a/firmware/src_simtrace/host_communication.c
+++ b/firmware/src_simtrace/host_communication.c
@@ -1,14 +1,14 @@
 #include "board.h"
 
 static volatile bool write_to_host_in_progress = false;
-static struct iso7816_3_handle ih = {0};
 static bool check_for_pts = false;
-static enum pts_state state;
+
+static struct Usart_info usart_info = {.base = USART_PHONE, .id = ID_USART_PHONE, .state = USART_RCV};
 
 void USB_write_callback(uint8_t *pArg, uint8_t status, uint32_t transferred, uint32_t remaining)
 {
     if (status != USBD_STATUS_SUCCESS) {
-        TRACE_ERROR("USB err status: %d (%s)\n", __FUNCTION__, status);
+        TRACE_ERROR("USB err status: %d(%s)\n", __FUNCTION__, status);
     }
     write_to_host_in_progress = false;
     TRACE_DEBUG("WR_CB\n");
@@ -40,26 +40,6 @@ int check_data_from_phone()
     if((rbuf_is_empty(&sim_rcv_buf) || write_to_host_in_progress == true)) {
         return ret;
     }
-    if ((check_for_pts == false) && (rbuf_peek(&sim_rcv_buf) == 0xff)) {
-// FIXME: set var to false
-        check_for_pts = true;
-        ih = (struct iso7816_3_handle){0};
-    }
-    if (check_for_pts == true) {
-        while (!rbuf_is_empty(&sim_rcv_buf) && (ih.pts_state != PTS_END)) {
-            state = process_byte_pts(&ih, rbuf_read(&sim_rcv_buf));
-        }
-        if (ih.pts_bytes_processed > 6 && ih.pts_state != PTS_END) {
-            int i;
-            for (i = 0; i < ih.pts_bytes_processed; i++)
-                printf("s: %x", ih.pts_req[i]);
-                check_for_pts = false;
-                rbuf_write(&sim_rcv_buf, ih.pts_req[i]);
-        } else {
-            printf("fin pts\n", ih.pts_state);
-            check_for_pts = false;
-        }
-    }
     ret = send_to_host();
     return ret;
 }
diff --git a/firmware/src_simtrace/iso7816_uart.c b/firmware/src_simtrace/iso7816_uart.c
deleted file mode 100644
index 7d01395..0000000
--- a/firmware/src_simtrace/iso7816_uart.c
+++ /dev/null
@@ -1,199 +0,0 @@
-/* Driver for AT91SAM7 USART0 in ISO7816-3 mode for passive sniffing
- * (C) 2010 by Harald Welte <hwelte@hmw-consulting.de>
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; either version 2 of the License, or
- *  (at your option) any later version.
- *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
- *
- */
-
-#include "board.h"
-#include "errno.h"
-
-#define _PTSS	0
-#define _PTS0	1
-#define _PTS1	2
-#define _PTS2	3
-#define _PTS3	4
-#define _PCK	5
-
-#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
-
-static struct Usart_info usart_info = {.base = USART_PHONE, .id = ID_USART_PHONE, .state = USART_RCV};
-
-/* Table 6 from ISO 7816-3 */
-static const uint16_t fi_table[] = {
-	0, 372, 558, 744, 1116, 1488, 1860, 0,
-	0, 512, 768, 1024, 1536, 2048, 0, 0
-};
-
-/* Table 7 from ISO 7816-3 */
-static const uint8_t di_table[] = {
-	0, 1, 2, 4, 8, 16, 0, 0,
-	0, 0, 2, 4, 8, 16, 32, 64,
-};
-
-/* compute the F/D ratio based on Fi and Di values */
-static int compute_fidi_ratio(uint8_t fi, uint8_t di)
-{
-	uint16_t f, d;
-	int ret;
-
-	if (fi >= ARRAY_SIZE(fi_table) ||
-	    di >= ARRAY_SIZE(di_table))
-		return -EINVAL;
-
-	f = fi_table[fi];
-	if (f == 0)
-		return -EINVAL;
-
-	d = di_table[di];
-	if (d == 0)
-		return -EINVAL;
-
-	if (di < 8) 
-		ret = f / d;
-	else
-		ret = f * d;
-
-	return ret;
-}
-
-static void update_fidi(struct iso7816_3_handle *ih)
-{
-	int rc;
-
-	rc = compute_fidi_ratio(ih->fi, ih->di);
-	if (rc > 0 && rc < 0x400) {
-		TRACE_INFO("computed Fi(%u) Di(%u) ratio: %d", ih->fi, ih->di, rc);
-/*              make sure UART uses new F/D ratio */
-		USART_PHONE->US_CR |= US_CR_RXDIS | US_CR_RSTRX;
-		USART_PHONE->US_FIDI = rc & 0x3ff;
-		USART_PHONE->US_CR |= US_CR_RXEN | US_CR_STTTO;
-	} else
-		TRACE_INFO("computed FiDi ratio %d unsupported", rc);
-}
-
-/* Update the ATR sub-state */
-static void set_pts_state(struct iso7816_3_handle *ih, enum pts_state new_ptss)
-{
-	//DEBUGPCR("PTS state %u -> %u", ih->pts_state, new_ptss);
-	ih->pts_state = new_ptss;
-}
-
-/* Determine the next PTS state */
-static enum pts_state next_pts_state(struct iso7816_3_handle *ih)
-{
-	uint8_t is_resp = ih->pts_state & 0x10;
-	uint8_t sstate = ih->pts_state & 0x0f;
-	uint8_t *pts_ptr;
-
-        if (ih->pts_state == PTS_END) {
-            return PTS_END;
-        }
-	if (!is_resp)
-		pts_ptr = ih->pts_req;
-	else
-		pts_ptr = ih->pts_resp;
-
-	switch (sstate) {
-	case PTS_S_WAIT_REQ_PTSS:
-		goto from_ptss;
-	case PTS_S_WAIT_REQ_PTS0:
-		goto from_pts0;
-	case PTS_S_WAIT_REQ_PTS1:
-		goto from_pts1;
-	case PTS_S_WAIT_REQ_PTS2:
-		goto from_pts2;
-	case PTS_S_WAIT_REQ_PTS3:
-		goto from_pts3;
-	}
-
-	if (ih->pts_state == PTS_S_WAIT_REQ_PCK)
-		return PTS_S_WAIT_RESP_PTSS;
-
-from_ptss:
-	return PTS_S_WAIT_REQ_PTS0 | is_resp;
-from_pts0:
-	if (pts_ptr[_PTS0] & (1 << 4))
-		return PTS_S_WAIT_REQ_PTS1 | is_resp;
-from_pts1:
-	if (pts_ptr[_PTS0] & (1 << 5))
-		return PTS_S_WAIT_REQ_PTS2 | is_resp;
-from_pts2:
-	if (pts_ptr[_PTS0] & (1 << 6))
-		return PTS_S_WAIT_REQ_PTS3 | is_resp;
-from_pts3:
-	return PTS_S_WAIT_REQ_PCK | is_resp;
-}
-
-enum pts_state process_byte_pts(struct iso7816_3_handle *ih, uint8_t byte)
-{
-        printf("PTS: %x, stat: %x\n", byte, ih->pts_state);
-	switch (ih->pts_state) {
-	case PTS_S_WAIT_REQ_PTSS:
-		ih->pts_req[_PTSS] = byte;
-		break;
-	case PTS_S_WAIT_REQ_PTS0:
-		ih->pts_req[_PTS0] = byte;
-		break;
-	case PTS_S_WAIT_REQ_PTS1:
-		ih->pts_req[_PTS1] = byte;
-		break;
-	case PTS_S_WAIT_REQ_PTS2:
-		ih->pts_req[_PTS2] = byte;
-		break;
-	case PTS_S_WAIT_REQ_PTS3:
-		ih->pts_req[_PTS3] = byte;
-		break;
-	case PTS_S_WAIT_REQ_PCK:
-		/* FIXME: check PCK */
-		ih->pts_req[_PCK] = byte;
-		break;
-	case PTS_S_WAIT_RESP_PTSS:
-		ih->pts_resp[_PTSS] = byte;
-		break;
-	case PTS_S_WAIT_RESP_PTS0:
-		ih->pts_resp[_PTS0] = byte;
-		break;
-	case PTS_S_WAIT_RESP_PTS1:
-		/* This must be TA1 */
-		ih->fi = byte >> 4;
-		ih->di = byte & 0xf;
-		TRACE_DEBUG("found Fi=%u Di=%u", ih->fi, ih->di);
-		ih->pts_resp[_PTS1] = byte;
-		break;
-	case PTS_S_WAIT_RESP_PTS2:
-		ih->pts_resp[_PTS2] = byte;
-		break;
-	case PTS_S_WAIT_RESP_PTS3:
-		ih->pts_resp[_PTS3] = byte;
-		break;
-	case PTS_S_WAIT_RESP_PCK:
-		ih->pts_resp[_PCK] = byte;
-		/* FIXME: check PCK */
-                for (int i = 0; ih->pts_resp != 0; i++)
-                    ISO7816_SendChar(ih->pts_req[i], &usart_info);
-		/* update baud rate generator with Fi/Di */
-		update_fidi(ih);
-		//set_pts_state(ih, PTS_S_WAIT_REQ_PTSS);
-		/* Wait for the next APDU */
-		ih->pts_state = PTS_END;
-        case PTS_END:
-                TRACE_INFO("PTS state PTS_END reached");
-	}
-	/* calculate the next state and set it */
-	set_pts_state(ih, next_pts_state(ih));
-        printf("stat: %x\n", ih->pts_state);
-	return ih->pts_state;
-}
diff --git a/firmware/src_simtrace/phone.c b/firmware/src_simtrace/phone.c
index 1b78ecb..6138dab 100644
--- a/firmware/src_simtrace/phone.c
+++ b/firmware/src_simtrace/phone.c
@@ -122,6 +122,7 @@ static struct Usart_info usart_info = {.base = USART_PHONE, .id = ID_USART_PHONE
  *          Internal variables
  *-----------------------------------------------------------------------------*/
 static uint8_t host_to_sim_buf[BUFLEN];
+static bool change_fidi = false;
 
 void receive_from_host( void );
 void sendResponse_to_phone( uint8_t *pArg, uint8_t status, uint32_t transferred, uint32_t remaining)
@@ -135,12 +136,25 @@ void sendResponse_to_phone( uint8_t *pArg, uint8_t status, uint32_t transferred,
 
     USART_SetReceiverEnabled(USART_PHONE, 0);
     USART_SetTransmitterEnabled(USART_PHONE, 1);
-    for (uint32_t i = 0; i < transferred; i++ ) {
+    uint32_t i = 0;
+    if (host_to_sim_buf[0] == 0xff) {
+        printf("Change FIDI detected\n");
+        // PTS command, change FIDI after command
+        i = 2;
+        change_fidi = true;
+    }
+    for (; i < transferred; i++ ) {
         ISO7816_SendChar(host_to_sim_buf[i], &usart_info);
     }
     USART_SetTransmitterEnabled(USART_PHONE, 0);
     USART_SetReceiverEnabled(USART_PHONE, 1);
 
+    if (change_fidi == true) {
+        printf("Change FIDI: %x\n", host_to_sim_buf[2]);
+        update_fidi(host_to_sim_buf[2]);
+        change_fidi = false;
+    }
+
     receive_from_host();
 }
 
diff --git a/firmware/src_simtrace/simtrace.h b/firmware/src_simtrace/simtrace.h
index ad9b7b5..2530ec7 100644
--- a/firmware/src_simtrace/simtrace.h
+++ b/firmware/src_simtrace/simtrace.h
@@ -58,36 +58,8 @@ typedef struct {
 
 extern const USBConfigurationDescriptor *configurationDescriptorsArr[];
 
-/***    PTS parsing     ***/
-/* detailed sub-states of ISO7816_S_IN_PTS */
-enum pts_state {
-	PTS_S_WAIT_REQ_PTSS,
-	PTS_S_WAIT_REQ_PTS0,
-	PTS_S_WAIT_REQ_PTS1,
-	PTS_S_WAIT_REQ_PTS2,
-	PTS_S_WAIT_REQ_PTS3,
-	PTS_S_WAIT_REQ_PCK,
-	PTS_S_WAIT_RESP_PTSS = PTS_S_WAIT_REQ_PTSS | 0x10,
-	PTS_S_WAIT_RESP_PTS0 = PTS_S_WAIT_REQ_PTS0 | 0x10,
-	PTS_S_WAIT_RESP_PTS1 = PTS_S_WAIT_REQ_PTS1 | 0x10,
-	PTS_S_WAIT_RESP_PTS2 = PTS_S_WAIT_REQ_PTS2 | 0x10,
-	PTS_S_WAIT_RESP_PTS3 = PTS_S_WAIT_REQ_PTS3 | 0x10,
-	PTS_S_WAIT_RESP_PCK = PTS_S_WAIT_REQ_PCK | 0x10,
-	PTS_END
-};
-
-struct iso7816_3_handle {
-	uint8_t fi;
-	uint8_t di;
-
-	enum pts_state pts_state;
-	uint8_t pts_req[6];
-	uint8_t pts_resp[6];
-        uint8_t pts_bytes_processed;
-};
-
 int check_data_from_phone();
-enum pts_state process_byte_pts(struct iso7816_3_handle *ih, uint8_t byte);
+void update_fidi(uint8_t fidi);
 
 void ISR_PhoneRST( const Pin *pPin);
 
diff --git a/firmware/src_simtrace/simtrace_iso7816.c b/firmware/src_simtrace/simtrace_iso7816.c
index 1376ee7..452bcc8 100644
--- a/firmware/src_simtrace/simtrace_iso7816.c
+++ b/firmware/src_simtrace/simtrace_iso7816.c
@@ -34,6 +34,7 @@
 #include "board.h"
 
 #include <string.h>
+#include <errno.h>
 
 volatile uint32_t char_stat;
 
@@ -110,3 +111,62 @@ void USART1_IrqHandler( void )
         char_stat = stat;
     }
 }
+
+/*  FIDI update functions   */
+#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
+
+/* Table 6 from ISO 7816-3 */
+static const uint16_t fi_table[] = {
+	0, 372, 558, 744, 1116, 1488, 1860, 0,
+	0, 512, 768, 1024, 1536, 2048, 0, 0
+};
+
+/* Table 7 from ISO 7816-3 */
+static const uint8_t di_table[] = {
+	0, 1, 2, 4, 8, 16, 0, 0,
+	0, 0, 2, 4, 8, 16, 32, 64,
+};
+
+/* compute the F/D ratio based on Fi and Di values */
+static int compute_fidi_ratio(uint8_t fi, uint8_t di)
+{
+	uint16_t f, d;
+	int ret;
+
+	if (fi >= ARRAY_SIZE(fi_table) ||
+	    di >= ARRAY_SIZE(di_table))
+		return -EINVAL;
+
+	f = fi_table[fi];
+	if (f == 0)
+		return -EINVAL;
+
+	d = di_table[di];
+	if (d == 0)
+		return -EINVAL;
+
+	if (di < 8) 
+		ret = f / d;
+	else
+		ret = f * d;
+
+	return ret;
+}
+
+void update_fidi(uint8_t fidi)
+{
+	int rc;
+
+        uint8_t fi = fidi >> 4;
+        uint8_t di = fidi & 0xf;
+
+	rc = compute_fidi_ratio(fi, di);
+	if (rc > 0 && rc < 0x400) {
+		TRACE_INFO("computed Fi(%u) Di(%u) ratio: %d", fi, di, rc);
+/*              make sure UART uses new F/D ratio */
+		USART_PHONE->US_CR |= US_CR_RXDIS | US_CR_RSTRX;
+		USART_PHONE->US_FIDI = rc & 0x3ff;
+		USART_PHONE->US_CR |= US_CR_RXEN | US_CR_STTTO;
+	} else
+		TRACE_INFO("computed FiDi ratio %d unsupported", rc);
+}