Index: firmware/Makefile.local
===================================================================
--- firmware/Makefile.local	(revision 67700)
+++ firmware/Makefile.local	(revision 67701)
@@ -316,7 +316,6 @@
 LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
 LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
 LDFLAGS += -Wl,--gc-sections -Wl,--entry=_startup
-LDFLAGS += -lstdc++_nano
 
 # Set Linker-Script Depending On Selected Memory and Controller
 ifeq ($(RUN_MODE),RUN_FROM_RAM)
Index: firmware/src/os/dbgu.c
===================================================================
--- firmware/src/os/dbgu.c	(revision 67700)
+++ firmware/src/os/dbgu.c	(revision 67701)
@@ -153,10 +153,9 @@
 			  " compiled " COMPILE_DATE
 			  " by " COMPILE_BY "\n\r\n\r");
 	DEBUGP("\n\rDEBUG Interface:\n\r"
-			  "0) Set Pull-up 1) Clear Pull-up 2) Toggle LED1 3) "
-			  "Toggle LED2\r\n9) Reset\n\r");
+			  "0) Set Pull-up 1) Clear Pull-up 2) Toggle LED1 3) Toggle LED2\r\n9) Reset\n\r");
 
-	DEBUGP("RSTC_SR=0x%08x\n\r", rst_status); 
+	DEBUGP("RSTC_SR=0x%08x\n\r", rst_status);
 }
 
 /*
Index: firmware/src/os/req_ctx.c
===================================================================
--- firmware/src/os/req_ctx.c	(revision 67700)
+++ firmware/src/os/req_ctx.c	(revision 67701)
@@ -186,7 +186,7 @@
 		req_ctx[i].tot_len = 0;
 		req_ctx[i].data = rctx_data[i];
 		req_ctx[i].state = RCTX_STATE_FREE;
-		DEBUGPCR("SMALL req_ctx[%02i] initialized at %08X, Data: %08X => %08X", i, req_ctx + i, req_ctx[i].data, req_ctx[i].data + RCTX_SIZE_SMALL);
+		//		DEBUGPCR("SMALL req_ctx[%02i] initialized at %08X, Data: %08X => %08X", i, req_ctx + i, req_ctx[i].data, req_ctx[i].data + RCTX_SIZE_SMALL);
 	}
 
 	for (; i < NUM_REQ_CTX; i++) {
@@ -196,7 +196,7 @@
 		req_ctx[i].tot_len = 0;
 		req_ctx[i].data = rctx_data_large[i];
 		req_ctx[i].state = RCTX_STATE_FREE;
-		DEBUGPCR("LARGE req_ctx[%02i] initialized at %08X, Data: %08X => %08X", i, req_ctx + i, req_ctx[i].data, req_ctx[i].data + RCTX_SIZE_LARGE);
+		//		DEBUGPCR("LARGE req_ctx[%02i] initialized at %08X, Data: %08X => %08X", i, req_ctx + i, req_ctx[i].data, req_ctx[i].data + RCTX_SIZE_LARGE);
 	}
 	req_ctx[0].prev = NULL;
 	req_ctx[NUM_REQ_CTX - 1].next = NULL;
Index: firmware/src/simtrace/iso7816_uart.c
===================================================================
--- firmware/src/simtrace/iso7816_uart.c	(revision 67700)
+++ firmware/src/simtrace/iso7816_uart.c	(revision 67701)
@@ -33,6 +33,7 @@
 
 #include "../simtrace.h"
 #include "../openpcd.h"
+#include "sim_switch.h"
 #include "iso7816_uart.h"
 #include "tc_etu.h"
 
@@ -39,6 +40,7 @@
 #define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
 
 static const AT91PS_USART usart = AT91C_BASE_US0;
+static const AT91PS_USART mitm_usart = AT91C_BASE_US1;
 
 enum iso7816_3_state {
 	ISO7816_S_RESET,	/* in Reset */
@@ -94,10 +96,10 @@
 	u_int32_t waiting_time;
 
 	enum atr_state atr_state;
-	u_int8_t atr_idx;
 	u_int8_t atr_hist_len;
 	u_int8_t atr_last_td;
-	u_int8_t atr[64];
+        u_int8_t atr_tx_gen;  //  Counts TA/B/C/D Generation
+	struct iso_uart_atr_data atr;
 
 	u_int16_t prot_t_supported;
 
@@ -124,13 +126,20 @@
 
 /* Table 7 from ISO 7816-3 */
 static const u_int8_t di_table[] = {
-	0, 1, 2, 4, 8, 16, 32, 64,
-	12, 20, 2, 4, 8, 16, 32, 64,
+        /* DIVIDE values */ 0, 1, 2, 4, 8, 16, 32, 64, 12, 20, 
+	/* MULTIPLY values */ 2, 4, 8, 16, 32, 64,
 };
 
-void iso_uart_report_errors(void) 
+static unsigned snif_count = 0, tx_count = 0, int_count = 0, empty_count = 0, last_csr = 0, manual_csr = 0;
+
+#ifdef SIMTRACE_MITM_ATR
+struct iso_uart_atr_data static_atr;
+#endif
+
+void iso_uart_report_changes(void) 
 {
-	static unsigned lastOverrun = 0, lastParity = 0, lastFrame = 0;
+  static unsigned lastOverrun = 0, lastParity = 0, lastFrame = 0, lastRst = 0, last_snif_count = 0;
+  // static unsigned last_tx_count = 0, last_int_count = 0, last_empty_count = 0;
 	if (isoh.stats.overrun != lastOverrun) {
 		DEBUGPCR("UART overrun: %u", lastOverrun = isoh.stats.overrun);
 	}
@@ -140,6 +149,64 @@
 	if (isoh.stats.parity_err != lastParity) {
 		DEBUGPCR("UART parity error: %u", lastParity = isoh.stats.parity_err);
 	}
+	if (isoh.stats.rst != lastRst) {
+	        DEBUGPCR("UART rst: %u", lastRst = isoh.stats.rst);
+	}
+	/*	if (last_tx_count != tx_count) {
+	  DEBUGPCR("UART tx: %u  MANUAL_CSR = %08x  int_csr = %08X", 
+		   last_tx_count = tx_count, manual_csr, last_csr);
+	}
+	if (last_int_count != int_count) {
+	  DEBUGPCR("UART int: %u  csr %08X  static_atr.len %u  isoh.atr.len %u",
+		   last_int_count = int_count, last_csr, 
+#ifdef SIMTRACE_MITM_ATR
+		   static_atr.len,
+#else
+		   0,
+#endif
+		   isoh.atr.len);
+	}
+	if (last_empty_count != empty_count) {
+	  DEBUGPCR("UART empty: %u", last_empty_count = empty_count);
+	  } */
+
+	if (last_snif_count != snif_count) {
+	  char atr_buf[203], x;
+	  unsigned pos, i;
+	  atr_buf[0] = 'A';
+	  atr_buf[1] = 'T';
+	  atr_buf[2] = 'R';
+	  atr_buf[3] = ' ';
+	  atr_buf[4] = ' ';
+	  pos = 5;
+	  for (i = 0; i < isoh.atr.len; i++) {
+	    x = isoh.atr.data[i];
+	    atr_buf[pos++] = nibble2hex(x >> 4);
+	    atr_buf[pos++] = nibble2hex(x & 15);
+	    atr_buf[pos++] = ' ';
+	  }
+	  atr_buf[pos - 1] = '\r';
+	  atr_buf[pos] = '\n';
+	  atr_buf[pos + 1] = 0;
+	  DEBUGP(atr_buf);
+#ifdef SIMTRACE_MITM_ATR
+	  atr_buf[0] = 'F';
+	  atr_buf[1] = 'W';
+	  atr_buf[2] = 'D';
+	  pos = 5;
+	  for (i = 0; i < static_atr.len; i++) {
+	    x = static_atr.data[i];
+	    atr_buf[pos++] = nibble2hex(x >> 4);
+	    atr_buf[pos++] = nibble2hex(x & 15);
+	    atr_buf[pos++] = ' ';
+	  }
+	  atr_buf[pos - 1] = '\r';
+	  atr_buf[pos] = '\n';
+	  atr_buf[pos + 1] = 0;
+	  DEBUGP(atr_buf);
+#endif
+	  last_snif_count = snif_count;
+	}
 }
 
 void iso_uart_stats_dump(void)
@@ -176,7 +243,7 @@
 
 	/* See table 7 of ISO 7816-3: From 1000 on we divide by 1/d,
 	 * which equals a multiplication by d */
-	if (di < 8) 
+	if (di < 10) 
 		ret = f / d;
 	else
 		ret = f * d;
@@ -231,26 +298,44 @@
 	ih->stats.rctx_sent++;
 }
 
-struct iso_uart_atr_data static_atr;
+static void iso_mitm_tx_mode(char tx) {
+  if (tx) {
+    mitm_usart->US_CR |= AT91C_US_TXEN;
+    mitm_usart->US_IER = AT91C_US_TXRDY | AT91C_US_TXEMPTY;
+  } else {
+    mitm_usart->US_CR |= AT91C_US_TXDIS;
+  }
+}
 
+static unsigned usart1_fwd_atr(unsigned *txcount);
+
 /* Update the ATR sub-state */
 static void set_atr_state(struct iso7816_3_handle *ih, enum atr_state new_atrs)
 {
+#ifdef SIMTRACE_MITM_ATR
+  unsigned flags, txcount;
+#endif
 	if (new_atrs == ATR_S_WAIT_TS) {
-		ih->atr_idx = 0;
 		ih->atr_hist_len = 0;
 		ih->atr_last_td = 0;
+		ih->atr_tx_gen = 0;
 		ih->prot_t_supported = (1 << 0);
-		memset(ih->atr, 0, sizeof(ih->atr));
-	} else if (ih->atr_state == new_atrs)
-		return;
-	else if (new_atrs == ATR_S_DONE) {
-	  memcpy(static_atr.data, ih->atr, 64);
-	  static_atr.len = ih->atr_idx;
+		memset(&ih->atr, 0, sizeof(ih->atr));
+#ifndef SIMTRACE_MITM_ATR
+	} else if (new_atrs == ATR_S_DONE) {
+	        snif_count++;
 	}
-
-	//DEBUGPCR("ATR state %u -> %u", ih->atr_state, new_atrs);
 	ih->atr_state = new_atrs;
+#else
+		static_atr.len = 0;
+	}
+	ih->atr_state = new_atrs;
+	simtrace_set_mode(SIMTRACE_MD_MITM, MITM_PHONE_TX);
+        local_irq_save(flags);
+	iso_mitm_tx_mode(1);
+	usart1_fwd_atr(&txcount);
+        local_irq_restore(flags);
+#endif
 }
 
 #define ISO7816_3_INIT_WTIME		9600
@@ -269,8 +354,9 @@
 		usart->US_CR |= AT91C_US_RXEN | AT91C_US_STTTO;
 		/* notify ETU timer about this */
 		tc_etu_set_etu(rc);
-	} else
+	} else {
 		DEBUGPCRF("computed FiDi ratio %d unsupported", rc);
+	}
 }
 
 /* Update the ISO 7816-3 APDU receiver state */
@@ -335,11 +421,13 @@
 		ih->prot_t_supported |= (1 << (ch & 0xf));
 	case ATR_S_WAIT_T0:
 		ih->atr_last_td = ch;
+		if (ch & 0xF0) // Any TA,TB,TC or TD exist
+		    ih->atr_tx_gen++;
 		goto from_td;
 	case ATR_S_WAIT_TC:
-		if ((ih->atr_last_td & 0x0f) == 0x02) {
-			/* TC2 contains WI */
-			ih->wi = ch;
+		if (ih->atr_tx_gen == 2) {
+		    /* TC2 contains WI */
+		    ih->wi = ch;
 		}
 		goto from_tc;
 	case ATR_S_WAIT_TB:
@@ -375,17 +463,21 @@
 static enum iso7816_3_state
 process_byte_atr(struct iso7816_3_handle *ih, u_int8_t byte)
 {
+        unsigned new_len = ih->atr.len;
 	/* add byte to ATR buffer */
-	ih->atr[ih->atr_idx] = byte;
-	ih->atr_idx++;
+	ih->atr.data[new_len] = byte;
+#ifdef SIMTRACE_MITM_ATR
+	static_atr.data[new_len] = byte;
+#endif
+	new_len++;
 
 	switch (ih->atr_state) {
 	case ATR_S_WAIT_TS:
 		/* FIXME: if we don't have the RST line we might get this */
 		if (byte == 0) {
-			ih->atr_idx--;
-			break;
+			return ISO7816_S_IN_ATR;
 		}
+		ih->atr.len = new_len;
 		/* FIXME: check inverted logic */
 		set_atr_state(ih, ATR_S_WAIT_T0);
 		break;
@@ -392,16 +484,25 @@
 	case ATR_S_WAIT_T0:
 		/* obtain the number of historical bytes */
 		ih->atr_hist_len = byte & 0xf;
+		ih->atr.len = new_len;
 		/* Mask out the hist-byte-length to indiicate T=0 */
 		set_atr_state(ih, next_intb_state(ih, byte & 0xf0));
 		break;
 	case ATR_S_WAIT_TA:
+#ifdef SIMTRACE_MITM_ATR
+	        if (ih->atr_tx_gen == 3) {
+		    /* Need to suppress Class C indicator */
+		    static_atr.data[ih->atr.len] &= 0xF3;
+		}
+#endif
 	case ATR_S_WAIT_TB:
 	case ATR_S_WAIT_TC:
 	case ATR_S_WAIT_TD:
+		ih->atr.len = new_len;
 		set_atr_state(ih, next_intb_state(ih, byte));
 		break;
 	case ATR_S_WAIT_HIST:
+		ih->atr.len = new_len;
 		ih->atr_hist_len--;
 		/* after all historical bytes are recieved, go to TCK */
 		if (ih->atr_hist_len == 0)
@@ -409,6 +510,7 @@
 		break;
 	case ATR_S_WAIT_TCK:
 		/* FIXME: process and verify the TCK */
+		ih->atr.len = new_len;
 		atr_done_wait_apdu(ih);
 		return ISO7816_S_WAIT_APDU;
 	}
@@ -488,6 +590,18 @@
 	case PTS_S_WAIT_REQ_PCK:
 		/* FIXME: check PCK */
 		ih->pts_req[_PCK] = byte;
+		{		  
+		  char msg_buf[20], x;
+		  int pos = 0, idx;
+		  for (idx = 0; idx < _PCK + 1; idx++) {
+		    x = ih->pts_req[idx];
+		    msg_buf[pos++] = nibble2hex(x >> 4);
+		    msg_buf[pos++] = nibble2hex(x & 15);
+		    msg_buf[pos++] = ' ';
+		  }
+		  msg_buf[pos - 1] = 0;
+		  DEBUGPCR("PTS REQ %s", msg_buf);
+		}
 		break;
 	case PTS_S_WAIT_RESP_PTSS:
 		ih->pts_resp[_PTSS] = byte;
@@ -499,7 +613,6 @@
 		/* This must be TA1 */
 		ih->fi = byte >> 4;
 		ih->di = byte & 0xf;
-		DEBUGPCR("found Fi=%u Di=%u", ih->fi, ih->di);
 		ih->sh.flags |= SIMTRACE_FLAG_PPS_FIDI;
 		ih->pts_resp[_PTS1] = byte;
 		break;
@@ -514,11 +627,50 @@
 		/* FIXME: check PCK */
 		set_pts_state(ih, PTS_S_WAIT_REQ_PTSS);
 		/* update baud rate generator with Fi/Di */
+		{
+		  char msg_buf[20], x, pts0;
+		  int pos = 0;
+		  x = ih->pts_resp[_PTSS];
+		  msg_buf[pos++] = nibble2hex(x >> 4);
+		  msg_buf[pos++] = nibble2hex(x & 15);
+		  msg_buf[pos++] = ' ';
+		  pts0 = x = ih->pts_resp[_PTS0];
+		  msg_buf[pos++] = nibble2hex(x >> 4);
+		  msg_buf[pos++] = nibble2hex(x & 15);
+		  msg_buf[pos++] = ' ';
+		  if (pts0 & (1 << 4)) {
+		    x = ih->pts_resp[_PTS1];
+		    msg_buf[pos++] = nibble2hex(x >> 4);
+		    msg_buf[pos++] = nibble2hex(x & 15);
+		    msg_buf[pos++] = ' ';
+		  }
+		  if (pts0 & (1 << 5)) {
+		    x = ih->pts_resp[_PTS2];
+		    msg_buf[pos++] = nibble2hex(x >> 4);
+		    msg_buf[pos++] = nibble2hex(x & 15);
+		    msg_buf[pos++] = ' ';
+		  }
+		  if (pts0 & (1 << 6)) {
+		    x = ih->pts_resp[_PTS3];
+		    msg_buf[pos++] = nibble2hex(x >> 4);
+		    msg_buf[pos++] = nibble2hex(x & 15);
+		    msg_buf[pos++] = ' ';
+		  }
+		  x = ih->pts_resp[_PCK];
+		  msg_buf[pos++] = nibble2hex(x >> 4);
+		  msg_buf[pos++] = nibble2hex(x & 15);
+		  msg_buf[pos] = 0;
+		  DEBUGPCR("PTS RSP (Fi=%u Di=%u) %s", ih->fi, ih->di, msg_buf);
+		}
 		update_fidi(ih);
 		/* Wait for the next APDU */
 		return ISO7816_S_WAIT_APDU;
 	}
 	/* calculate the next state and set it */
+	if ((ih == PTS_S_WAIT_REQ_PTSS ||
+	     ih == PTS_S_WAIT_RESP_PTSS) && byte == 0)
+	  return ISO7816_S_IN_PTS;  // INVALID in char
+
 	set_pts_state(ih, next_pts_state(ih));
 
 	return ISO7816_S_IN_PTS;
@@ -612,17 +764,57 @@
 	last_len = isoh.rctx->tot_len;
 }
 
+#ifdef SIMTRACE_MITM_ATR
+static __ramfunc unsigned usart1_fwd_atr(unsigned *txcount)
+{
+  unsigned csr = mitm_usart->US_CSR, count = 0;
+  char last_tx_char;
+  if (csr & AT91C_US_TXRDY) {
+    while ((static_atr.len < isoh.atr.len) &&
+	  ((csr = mitm_usart->US_CSR) & AT91C_US_TXRDY)) { 
+      last_tx_char = static_atr.data[static_atr.len++];
+      mitm_usart->US_THR = last_tx_char;
+      count++;
+    }
+    csr = mitm_usart->US_CSR;
+  }
+  *txcount = count;
+  tx_count += count;
+  return csr;
+}
+#endif
+
+static __ramfunc void usart1_irq(void)
+{
+#ifdef SIMTRACE_MITM_ATR
+  u_int32_t csr, flags, txcount;
+  local_irq_save(flags);
+  csr = usart1_fwd_atr(&txcount);
+  if (csr & AT91C_US_TXEMPTY) {
+    iso_mitm_tx_mode(0);
+    if (isoh.atr_state == ATR_S_DONE) {
+      simtrace_set_mode(SIMTRACE_MD_SNIFFER, 0); // Back to SNIFFER mode
+      snif_count++;
+    }
+    empty_count++;
+  } else if (txcount == 0) {
+    // Nothing transmitted... we are using EDGE interrupt event
+    mitm_usart->US_IDR = AT91C_US_TXRDY;
+  }
+  int_count++;
+  last_csr = csr;
+  local_irq_restore(flags);
+#endif
+}
+
 static __ramfunc void usart_irq(void)
 {
 	u_int32_t csr = usart->US_CSR;
 	u_int8_t octet;
 
-	//DEBUGP("USART IRQ, CSR=0x%08x\n", csr);
-
 	if (csr & AT91C_US_RXRDY) {
 		/* at least one character received */
 		octet = usart->US_RHR & 0xff;
-		//DEBUGP("%02x ", octet);
 		process_byte(&isoh, octet);
 	}
 
@@ -656,6 +848,9 @@
 		iso_uart_flush();
 		set_state(&isoh, ISO7816_S_RESET);
 	} else {
+#ifdef SIMTRACE_MITM_ATR
+	        simtrace_set_mode(SIMTRACE_MD_MITM, MITM_PHONE_RX);
+#endif
 		iso_uart_flush();
 		set_state(&isoh, ISO7816_S_WAIT_ATR);
 		isoh.stats.rst++;
@@ -701,6 +896,7 @@
 void iso_uart_clk_master(unsigned int master)
 {
 	DEBUGPCR("USART Clock Master %u", master);
+#ifdef DISABLED
 	if (master) {
 		usart->US_MR = AT91C_US_USMODE_ISO7816_0 | AT91C_US_CLKS_CLOCK |
 				AT91C_US_CHRL_8_BITS | AT91C_US_NBSTOP_1_BIT |
@@ -712,14 +908,47 @@
 				AT91C_US_CKLO | AT91C_US_INACK;
 				usart->US_BRGR = (0x0000 << 16) | 0x0001; */
 	}
+#endif
 }
 
+void iso_mitm_uart_init(void) {
+        DEBUGPCR("USART1 Initializing");
+	if ((AT91C_BASE_PMC->PMC_PCSR & (1u << AT91C_ID_US1)) == 0) {
+	  DEBUGPCR("Enabling UART1 Peripheral Clock");
+	  AT91C_BASE_PMC->PMC_PCER = 1u << AT91C_ID_US1;
+	}
+
+	/* Configure Interrupt */
+	AT91F_AIC_ConfigureIt(AT91C_BASE_AIC, AT91C_ID_US1,
+			      OPENPCD_IRQ_PRIO_RC632,
+			      AT91C_AIC_SRCTYPE_INT_POSITIVE_EDGE, &usart1_irq);
+
+	AT91F_AIC_EnableIt(AT91C_BASE_AIC, AT91C_ID_US1);
+
+	mitm_usart->US_CR = AT91C_US_RXDIS | AT91C_US_TXDIS | AT91C_US_RSTRX | AT91C_US_RSTTX;
+
+	/* FIXME: wait for some time */
+	mitm_usart->US_CR = AT91C_US_RXDIS | AT91C_US_TXDIS;
+
+	mitm_usart->US_MR = AT91C_US_USMODE_ISO7816_0 | AT91C_US_CLKS_EXT | AT91C_US_INACK;
+	mitm_usart->US_FIDI = 372 & 0x3FF; // Standard ATR rate
+
+	/* Disable all interrupts */
+	mitm_usart->US_IDR = 0xff;
+	/* Clock Divider = 1, i.e. no division of SCLK */
+	mitm_usart->US_BRGR = (0x0000 << 16) | 0x0001;
+	/* Disable Receiver Time-out */
+	mitm_usart->US_RTOR = 0;
+	/* Enable Transmitter Timeguard */
+	mitm_usart->US_TTGR = 1;
+}
+
 void iso_uart_init(void)
 {
 	DEBUGPCR("USART Initializing");
 
 	memset(&isoh, 0, sizeof(isoh));
-
+	
 	refill_rctx(&isoh);
 
 	/* make sure we get clock from the power management controller */
@@ -733,25 +962,16 @@
 	AT91F_AIC_ConfigureIt(AT91C_BASE_AIC, AT91C_ID_US0,
 			      OPENPCD_IRQ_PRIO_USART,
 			      AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL, &usart_irq);
-	AT91F_AIC_EnableIt(AT91C_BASE_AIC, AT91C_ID_US0);
 
-	usart->US_CR = AT91C_US_RXDIS | AT91C_US_TXDIS |
-					(AT91C_US_RSTRX | AT91C_US_RSTTX);
+	AT91F_AIC_EnableIt(AT91C_BASE_AIC, AT91C_ID_US0);	
+
+	usart->US_CR = AT91C_US_RXDIS | AT91C_US_TXDIS | AT91C_US_RSTRX | AT91C_US_RSTTX;
 	/* FIXME: wait for some time */
 	usart->US_CR = AT91C_US_RXDIS | AT91C_US_TXDIS;
 
-	/* ISO7816 T=0 mode with external clock input */
-	/*	       	usart->US_MR = AT91C_US_USMODE_ISO7816_0 | AT91C_US_CLKS_EXT | 
-	  AT91C_US_CHRL_8_BITS | AT91C_US_NBSTOP_15_BIT | AT91C_US_PAR_EVEN |
-	AT91C_US_CKLO | AT91C_US_INACK |
-	AT91C_US_OVER; */
-	
-
-	// OK
 	usart->US_MR = AT91C_US_USMODE_ISO7816_0 | AT91C_US_CLKS_EXT | 
-	  AT91C_US_CHRL_8_BITS | AT91C_US_NBSTOP_15_BIT | AT91C_US_PAR_EVEN |
-	  AT91C_US_CKLO | AT91C_US_INACK;// |
-	  //	AT91C_US_OVER; 
+	  //	  AT91C_US_CHRL_8_BITS | AT91C_US_NBSTOP_15_BIT | AT91C_US_PAR_EVEN |  -- ISO7816 Predefines MODE
+	  AT91C_US_CKLO | AT91C_US_INACK;
 
 	/* Disable all interrupts */
 	usart->US_IDR = 0xff;
Index: firmware/src/simtrace/iso7816_uart.h
===================================================================
--- firmware/src/simtrace/iso7816_uart.h	(revision 67700)
+++ firmware/src/simtrace/iso7816_uart.h	(revision 67701)
@@ -2,22 +2,25 @@
 #define SIMTRACE_ISO7816_UART_H
 
 struct simtrace_stats *iso_uart_stats_get(void);
-void iso_uart_report_errors(void);
+void iso_uart_report_changes(void);
 void iso_uart_stats_dump(void);
 void iso_uart_dump(void);
 void iso_uart_rst(unsigned int state);
 void iso_uart_rx_mode(void);
 void iso_uart_clk_master(unsigned int master);
+void iso_mitm_uart_init(void);
 void iso_uart_init(void);
 void iso_uart_flush(void);
 void iso_uart_idleflush(void);
 void iso7816_wtime_expired(void);
 
+static inline char nibble2hex(char x) {
+  return x < 10 ? '0' + x : 'A' + x - 10;
+}
+
 struct iso_uart_atr_data {
   unsigned char len;
   char data[64];
 };
 
-extern struct iso_uart_atr_data static_atr;
-
 #endif
Index: firmware/src/simtrace/main_simtrace.c
===================================================================
--- firmware/src/simtrace/main_simtrace.c	(revision 67700)
+++ firmware/src/simtrace/main_simtrace.c	(revision 67701)
@@ -21,6 +21,7 @@
 #include <errno.h>
 #include <include/lib_AT91SAM7.h>
 #include <include/openpcd.h>
+#include <include/compile.h>
 #include <os/dbgu.h>
 #include <os/led.h>
 #include <os/pcd_enumerate.h>
@@ -35,50 +36,17 @@
 #include <simtrace/sim_switch.h>
 #include <simtrace_usb.h>
 
-enum simtrace_md {
-	SIMTRACE_MD_OFF,
-	SIMTRACE_MD_SNIFFER,
-	SIMTRACE_MD_MITM,
-};
+static void simtrace_init_pins() {
+        AT91F_PIO_CfgInput(AT91C_BASE_PIOA, UART0_PINS);
+	AT91F_PIO_CfgPullupDis(AT91C_BASE_PIOA, UART0_PINS);
 
-#define UART1_PINS (SIMTRACE_PIO_nRST_PH |		\
-		    SIMTRACE_PIO_CLK_PH |		\
-		    SIMTRACE_PIO_CLK_PH_T |		\
-		    SIMTRACE_PIO_IO_PH_RX |		\
-		    SIMTRACE_PIO_IO_PH_TX |             \
-		    SIMTRACE_PIO_VCC_PHONE)
+#ifdef SIMTRACE_MITM_ATR
+	AT91F_PIO_CfgInput(AT91C_BASE_PIOA, UART1_PINS);
+	AT91F_PIO_CfgPullupDis(AT91C_BASE_PIOA, UART1_PINS); // UART1_RX_PINS);  // Leave TX alone
+	//	AT91F_PIO_CfgPullupEn(AT91C_BASE_PIOA, UART1_TX_PINS);  // Leave TX alone
+#endif
 
-#define UART0_PINS (SIMTRACE_PIO_nRST |			\
-		    SIMTRACE_PIO_CLK |			\
-		    SIMTRACE_PIO_CLK_T |		\
-		    SIMTRACE_PIO_IO |			\
-		    SIMTRACE_PIO_IO_T)
-
-static void simtrace_set_mode(enum simtrace_md mode)
-{
-	switch (mode) {
-	case SIMTRACE_MD_SNIFFER:
-		DEBUGPCR("MODE: SNIFFER\n");
-
-		/* UART1 PINS -- PHONE SIDE SHOULD ALWAYS BE ON INPUT, NO CHANGE --
-		   DO NOT UPDATE IT
-		   switch UART1 pins to input, no pull-up */
-		//		AT91F_PIO_CfgInput(AT91C_BASE_PIOA, UART1_PINS);
-		//		AT91F_PIO_CfgPullupDis(AT91C_BASE_PIOA, UART1_PINS);
-
-		/* switch UART0 pins to 'ISO7816 card mode' */
-		AT91F_PIO_CfgInput(AT91C_BASE_PIOA, UART0_PINS);
-		AT91F_PIO_CfgPullupDis(AT91C_BASE_PIOA, UART0_PINS);
-		AT91F_PIO_CfgPeriph(AT91C_BASE_PIOA, SIMTRACE_PIO_IO, SIMTRACE_PIO_CLK);
-		sim_switch_mode(1, 1);
-		break;
-	case SIMTRACE_MD_MITM:
-		DEBUGPCR("MODE: MITM\n");
-		/* switch UART1 pins to 'ISO7816 card mode' */
-		/* switch UART0 pins to 'ISO7816 reader mode' */
-		sim_switch_mode(0, 0);
-		break;
-	}
+	AT91F_PIO_CfgPeriph(AT91C_BASE_PIOA, SIMTRACE_PIO_IO, SIMTRACE_PIO_CLK);
 }
 
 static int simtrace_usb_in(struct req_ctx *rctx)
@@ -132,9 +100,22 @@
         int loop = 0, loopX;
 	AT91C_BASE_AIC->AIC_SPU = (int)custom_spurious_entry;
 
-	/* low-level hardware initialization */
+#ifdef SIMTRACE_1P3
+	DEBUGP("Compatible with SIMtrace 1.3 [FET Pin-nEN=0]\n\r");
+#else
+	DEBUGP("Compatible with Custom [FET Pin-EN=1]\n\r");
+#endif
+
+	simtrace_init_pins();
 	pio_irq_init();
 	iso_uart_init();
+
+#ifdef SIMTRACE_MITM_ATR
+	DEBUGP("ATR Man-in-the-Middle Enabled\n\r");
+	iso_mitm_uart_init();
+#else
+	DEBUGP("Strict SNIFFER\n\r");
+#endif
 	tc_etu_init();
 	sim_switch_init();
 
@@ -146,24 +127,31 @@
 	led_switch(1, 0);
 	led_switch(2, 0);
 
+	iso_uart_rx_mode();
+
+#ifdef SIMTRACE_1P3
 	sim_switch_set_power(SIM_SWITCH_PWR_PASS);
-	// sim_switch_set_power(SIM_SWITCH_PWR_OFF);
+	simtrace_set_mode(SIMTRACE_MD_SNIFFER, MITM_PHONE_RX);
+#else
+	sim_switch_set_power(SIM_SWITCH_PWR_OFF);
 
-	//	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_VCC_SIM);
 	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_nPWRFWD);
 	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_LDOEN);
 	
-	if (!AT91F_PIO_IsInputSet(AT91C_BASE_PIOA, SIMTRACE_PIO_VCC_PHONE)) {
-	  //	  sim_switch_set_power(SIM_SWITCH_PWR_OFF);
-	  led_switch(1, 0);
-	} else {  // USE LDO, otherwise, phone tries multiple Reset
-	  //	  sim_switch_set_power(SIM_SWITCH_PWR_LDO);
-	  led_switch(1, 1);
+	if (AT91F_PIO_IsInputSet(AT91C_BASE_PIOA, SIMTRACE_PIO_VCC_PHONE)) {
+	  led_switch(1,  1);
+	  sim_switch_set_power(SIM_SWITCH_PWR_PASS);
+	  simtrace_set_mode(SIMTRACE_MD_SNIFFER, MITM_PHONE_RX);
+	} else {
+	  led_switch(1,  0);
+#ifdef SIMTRACE_MITM_ATR
+	  simtrace_set_mode(SIMTRACE_MD_MITM, MITM_PHONE_TX);
+#else
+	  simtrace_set_mode(SIMTRACE_MD_SNIFFER, MITM_PHONE_RX);
+#endif
 	}
+#endif
 
-	iso_uart_rx_mode();
-	simtrace_set_mode(SIMTRACE_MD_SNIFFER);
-
 	while (loop++ < 3) {
 	  led_switch(2, 1);
 	  loopX = 0;
@@ -190,8 +178,6 @@
   DEBUGPCR("t: ISO UART statistics");
   DEBUGPCR("s: disconnect SIM bus switch");
   DEBUGPCR("S: connect SIM bus switch");
-  DEBUGPCR("v: turn off vcc_sim");
-  DEBUGPCR("V: turn on vcc_sim");
   DEBUGPCR("p: read PA25/VCC_PHONE");
   DEBUGPCR("b: toggle heartbeat");
   DEBUGPCR("<: Previous PIN");
@@ -317,10 +303,10 @@
 
 	switch (key) {
 	case 's':
-		simtrace_set_mode(SIMTRACE_MD_MITM);
+	        simtrace_set_mode(SIMTRACE_MD_MITM, MITM_PHONE_RX);
 		break;
 	case 'S':
-		simtrace_set_mode(SIMTRACE_MD_SNIFFER);
+	        simtrace_set_mode(SIMTRACE_MD_SNIFFER, MITM_PHONE_RX);
 		break;
 	case 't':
 		iso_uart_stats_dump();
@@ -343,12 +329,25 @@
 	case 'd':
 		iso_uart_dump();
 		break;
-	case 'v':
-	  sim_switch_set_power(SIM_SWITCH_PWR_OFF);
+
+	  /*
+	case 'z':
+	  AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_LDOEN);
+	  DEBUGPCR("LDOEN = 0");
 	  break;
-	case 'V':
-	  sim_switch_set_power(SIM_SWITCH_PWR_LDO);
+	case 'Z':
+	  AT91F_PIO_SetOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_LDOEN);
+	  DEBUGPCR("LDOEN = 1");
 	  break;
+	case 'x':
+	  AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_nPWRFWD);
+	  DEBUGPCR("nPWRFWD = 0");
+	  break;
+	case 'X':
+	  AT91F_PIO_SetOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_nPWRFWD);
+	  DEBUGPCR("nPWRFWD = 1");
+	  break;
+	  */
 	case 'p':
 	  DEBUGPCR("PA25/VCC_PHONE = %i", (AT91F_PIO_GetInput(AT91C_BASE_PIOA) >> 25) & 1);
 	  break;
@@ -400,6 +399,7 @@
 
 	  break;
 	case '?':
+	default:
 		help();
 		break;
 	}
@@ -410,8 +410,6 @@
 void _main_func(void)
 {
   	static unsigned loopLow = 0, loopHigh = 0;
-	char atr_buf[200], x;
-	unsigned char i, pos;
 
 	/* first we try to get rid of pending to-be-sent stuff */
 	usb_out_process();
@@ -421,27 +419,9 @@
 
 	udp_unthrottle();
 
-	if (static_atr.len) {
-	  atr_buf[0] = 'A';
-	  atr_buf[1] = 'T';
-	  atr_buf[2] = 'R';
-	  atr_buf[3] = ' ';
-	  atr_buf[4] = ' ';
-	  pos = 5;
-	  for (i = 0; i < static_atr.len; i++) {
-	    x = static_atr.data[i] >> 4;
-	    atr_buf[pos++] = (x < 10) ? '0' + x : 'A' + x - 10;
-	    x = static_atr.data[i] & 15;
-	    atr_buf[pos++] = (x < 10) ? '0' + x : 'A' + x - 10;
-	    atr_buf[pos++] = ' ';
-	  }
-	  atr_buf[pos - 1] = '\r';
-	  atr_buf[pos] = '\n';
-	  atr_buf[pos + 1] = 0;
-	  static_atr.len = 0;
-	  DEBUGP(atr_buf);
-	}
-       	if (do_heart_beat && (loopLow & 0xFFFF) == 0) {
+	iso_uart_report_changes();
+       	if ((loopLow & 0xFFFF) == 0) {
+	  if (do_heart_beat)
 		DEBUGPCR("Heart beat %08X", loopHigh++);
 	}
 	if ((loopLow & 0x3F) == 0) {
@@ -464,6 +444,5 @@
 	  }
 	  } */
 	
-	iso_uart_report_errors();
 	check_spurious_irq();
 }
Index: firmware/src/simtrace/sim_switch.c
===================================================================
--- firmware/src/simtrace/sim_switch.c	(revision 67700)
+++ firmware/src/simtrace/sim_switch.c	(revision 67701)
@@ -47,8 +47,13 @@
 		 * to the SIM.  Pin unused in v1.0(p) and v1.1p */
 	  //		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_VCC_SIM);
 		/* v1.0/1.1: pass-throguh power from the reader/phone (FPC) */
+#ifdef SIMTRACE_1P3
 		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_LDOEN);
+	        AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_nPWRFWD);
+#else
+		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_LDOEN);
 		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_nPWRFWD);
+#endif
 		break;
 	case SIM_SWITCH_PWR_LDO:
 		/* switch VCC_SIM pin into output mode, as in the first
@@ -56,9 +61,13 @@
 		 * to the SIM.  Pin unused in v1.0(p) and v1.1p */
 	  //		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_VCC_SIM);
 		/* v1.0/1.1: power from the internal 3.3V LDO */
+#ifdef SIMTRACE_1P3
+	        AT91F_PIO_SetOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_nPWRFWD);
+		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_LDOEN);
+#else
 	        AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_nPWRFWD);
-	  //		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_nPWRFWD);
 		AT91F_PIO_SetOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_LDOEN);
+#endif
 		break;
 	default:
 	case SIM_SWITCH_PWR_OFF:
@@ -67,8 +76,13 @@
 		 * to the SIM.  Pin unused in v1.0(p) and v1.1p */
 	  //		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_VCC_SIM);
 		/* switch all power paths off */
+#ifdef SIMTRACE_1P3
 		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_LDOEN);
-		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_nPWRFWD);
+	        AT91F_PIO_SetOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_nPWRFWD);
+#else
+		AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_LDOEN);
+	        AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SIM_nPWRFWD);
+#endif
 		break;
 	}
 }
@@ -97,16 +111,42 @@
 
 static void vcc_phone_irq(u_int32_t pio)
 {
+#ifdef SIMTRACE_1P3
+        led_switch(1, AT91F_PIO_IsInputSet(AT91C_BASE_PIOA, SIMTRACE_PIO_VCC_PHONE) ? 1 : 0);
+        iso_uart_flush();
+#else
 	if (!AT91F_PIO_IsInputSet(AT91C_BASE_PIOA, SIMTRACE_PIO_VCC_PHONE)) {
-	  //	  sim_switch_set_power(SIM_SWITCH_PWR_OFF);
+	        sim_switch_set_power(SIM_SWITCH_PWR_OFF);
+	        sim_switch_mode(0, 0);
 		led_switch(1, 0);
 		iso_uart_flush();
-	} else { // USE LDO, otherwise, phone tries multiple Reset
-	  //	  sim_switch_set_power(SIM_SWITCH_PWR_LDO);
-		led_switch(1, 1);
+	} else {
+	        sim_switch_set_power(SIM_SWITCH_PWR_PASS);
+	        sim_switch_mode(1, 1);
+	        led_switch(1, 1);
 	}
+#endif
 }
 
+void simtrace_set_mode(enum simtrace_md mode, char mitm_usart_tx)
+{
+	switch (mode) {
+	case SIMTRACE_MD_OFF:
+	case SIMTRACE_MD_SNIFFER:
+		sim_switch_mode(1, 1);
+	        AT91F_PIO_CfgInput(AT91C_BASE_PIOA, UART1_TX_PINS);
+		break;
+	case SIMTRACE_MD_MITM:
+	        sim_switch_mode(0, 1);
+	        if (mitm_usart_tx) {
+		  AT91F_PIO_CfgPeriph(AT91C_BASE_PIOA, SIMTRACE_PIO_IO_PH_TX, 0);
+		} else {
+		  AT91F_PIO_CfgInput(AT91C_BASE_PIOA, UART1_TX_PINS);
+		}
+		break;
+	}
+}
+
 void sim_switch_init(void)
 {
 	DEBUGPCR("ISO_SW Initializing");
@@ -113,6 +153,9 @@
 
 	/* make sure we get clock from the power management controller */
 	AT91F_US0_CfgPMC();
+#ifdef SIMTRACE_MITM_ATR
+	AT91F_US1_CfgPMC();
+#endif
 
 	/* configure both signals as output */
 	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, SIMTRACE_PIO_SC_SW |
Index: firmware/src/simtrace/sim_switch.h
===================================================================
--- firmware/src/simtrace/sim_switch.h	(revision 67700)
+++ firmware/src/simtrace/sim_switch.h	(revision 67701)
@@ -7,8 +7,18 @@
 	SIM_SWITCH_PWR_LDO
 };
 
+enum simtrace_md {
+	SIMTRACE_MD_OFF,
+	SIMTRACE_MD_SNIFFER,
+	SIMTRACE_MD_MITM,
+};
+
+#define MITM_PHONE_RX    0
+#define MITM_PHONE_TX    1    // TX enabled
+
 void sim_switch_set_power(enum sim_switch_power_mode mode);
 void sim_switch_mode(int connect_io, int connect_misc);
 void sim_switch_init(void);
+void simtrace_set_mode(enum simtrace_md mode, char mitm_usart_tx);
 
 #endif
Index: firmware/src/simtrace.h
===================================================================
--- firmware/src/simtrace.h	(revision 67700)
+++ firmware/src/simtrace.h	(revision 67701)
@@ -1,6 +1,10 @@
 
 #define USB_VENDOR_ID	OPENPCD_VENDOR_ID
 
+#define SIMTRACE_MITM_ATR
+// Original SIMTrace 1.2p boards has a negated input for SIM_nPWRFWD (Active LOW), where as Custom uses Active HIGN input
+// #define SIMTRACE_1P3
+
 #ifdef SIMTRACE
 #define OPENPCD_PIO_LED2	AT91C_PIO_PA17
 #define OPENPCD_PIO_LED1	AT91C_PIO_PA18
@@ -48,3 +52,16 @@
 #define PIO_SPIF_MOSI	AT91C_PA13_MOSI
 #define PIO_SPIF_MISO	AT91C_PA12_MISO
 #define PIO_SPIF_nCS	AT91C_PA11_NPCS0
+
+#define UART1_RX_PINS (SIMTRACE_PIO_nRST_PH |		\
+		       SIMTRACE_PIO_CLK_PH |		\
+		       SIMTRACE_PIO_CLK_PH_T |		\
+		       SIMTRACE_PIO_IO_PH_RX)
+#define UART1_TX_PINS SIMTRACE_PIO_IO_PH_TX
+#define UART1_PINS (UART1_RX_PINS | UART1_TX_PINS)
+#define UART0_PINS (SIMTRACE_PIO_nRST |			\
+		    SIMTRACE_PIO_CLK |			\
+		    SIMTRACE_PIO_CLK_T |		\
+		    SIMTRACE_PIO_IO |			\
+		    SIMTRACE_PIO_IO_T)
+
