4 files changed, 31 insertions, 2 deletions
diff --git a/firmware/src/simtrace/iso7816_uart.c b/firmware/src/simtrace/iso7816_uart.c
index 611ccfb..780340f 100644
--- a/firmware/src/simtrace/iso7816_uart.c
+++ b/firmware/src/simtrace/iso7816_uart.c
@@ -575,6 +575,23 @@ void iso7816_wtime_expired(void)
 	set_state(&isoh, ISO7816_S_WAIT_APDU);
 }
 
+void iso_uart_flush(void)
+{
+	send_rctx(&isoh);
+}
+
+void iso_uart_idleflush(void)
+{
+	static struct req_ctx *last_req = NULL;
+	static u_int16_t last_len = 0;
+
+	if (last_req == isoh.rctx || last_len == isoh.rctx->tot_len) {
+		send_rctx(&isoh);
+	}
+	last_req = isoh.rctx;
+	last_len = isoh.rctx->tot_len;
+}
+
 static __ramfunc void usart_irq(void)
 {
 	u_int32_t csr = usart->US_CSR;
@@ -618,9 +635,13 @@ static __ramfunc void usart_irq(void)
 static void reset_pin_irq(u_int32_t pio)
 {
 	if (!AT91F_PIO_IsInputSet(AT91C_BASE_PIOA, pio)) {
+		/* make sure to flush pending req_ctx */
+		iso_uart_flush();
 		DEBUGPCR("nRST");
 		set_state(&isoh, ISO7816_S_RESET);
 	} else {
+		/* make sure to flush pending req_ctx */
+		iso_uart_flush();
 		DEBUGPCR("RST");
 		set_state(&isoh, ISO7816_S_WAIT_ATR);
 		isoh.stats.rst++;
diff --git a/firmware/src/simtrace/iso7816_uart.h b/firmware/src/simtrace/iso7816_uart.h
index 1a2b202..c5790c8 100644
--- a/firmware/src/simtrace/iso7816_uart.h
+++ b/firmware/src/simtrace/iso7816_uart.h
@@ -9,5 +9,7 @@ void iso_uart_rx_mode(void);
 void iso_uart_clk_master(unsigned int master);
 void iso_uart_init(void);
 void iso_uart_report_overrun(void);
+void iso_uart_flush(void);
+void iso_uart_idleflush(void);
 
 #endif
diff --git a/firmware/src/simtrace/main_simtrace.c b/firmware/src/simtrace/main_simtrace.c
index ca4e646..6bad290 100644
--- a/firmware/src/simtrace/main_simtrace.c
+++ b/firmware/src/simtrace/main_simtrace.c
@@ -231,6 +231,9 @@ void _main_func(void)
 	if ((loopLow & 0xFFFF) == 0) {
 		DEBUGPCR("Heart beat %08X", loopHigh++);
 	}
+	if ((loopLow & 0x3FF) == 0) {
+		iso_uart_idleflush();
+	}
 	loopLow++;
 
 	iso_uart_report_overrun();
diff --git a/firmware/src/simtrace/sim_switch.c b/firmware/src/simtrace/sim_switch.c
index faf48f0..90067ac 100644
--- a/firmware/src/simtrace/sim_switch.c
+++ b/firmware/src/simtrace/sim_switch.c
@@ -59,9 +59,12 @@ static void sw_sim_irq(u_int32_t pio)
 
 static void vcc_phone_irq(u_int32_t pio)
 {
-	if (!AT91F_PIO_IsInputSet(AT91C_BASE_PIOA, SIMTRACE_PIO_VCC_PHONE))
+	if (!AT91F_PIO_IsInputSet(AT91C_BASE_PIOA, SIMTRACE_PIO_VCC_PHONE)) {
 		DEBUGPCR("VCC_PHONE off");
-	else
+		/* flush any pending req_ctx to make sure the next ATR
+		 * will be aligned to position 0 */
+		iso_uart_flush();
+	} else
 		DEBUGPCR("VCC_PHONE on");
 }