diff --git a/firmware/include/simtrace_usb.h b/firmware/include/simtrace_usb.h
index 08e4523..3190d53 100644
--- a/firmware/include/simtrace_usb.h
+++ b/firmware/include/simtrace_usb.h
@@ -3,11 +3,16 @@
 
 //#include <stdint.h>
 
-/* this is kept compatible with OpenPCD protocol */
+/* this WAS compatible with OpenPCD protocol
+ * NOW modified to add apdu_offset and total_length
+ */
 struct simtrace_hdr {
 	u_int8_t cmd;
 	u_int8_t flags;
 	u_int8_t res[2];
+	u_int16_t seq_num;
+	u_int16_t offset;
+	u_int16_t tot_len;
 	u_int8_t data[0];
 } __attribute__ ((packed));
 
diff --git a/firmware/src/os/dbgu.c b/firmware/src/os/dbgu.c
index 113208e..a24ea5c 100644
--- a/firmware/src/os/dbgu.c
+++ b/firmware/src/os/dbgu.c
@@ -45,18 +45,16 @@
 #include <asm/system.h>
 #include <compile.h>
 
-//#define DEBUG_UNBUFFERED
+// IN case we find that while (); is non interruptible, we may need to uncommend this line
+// #define ALLOW_INTERRUPT_LOOP asm("nop;nop;nop;nop;nop;nop;nop;nop;")
+
+#define ALLOW_INTERRUPT_LOOP
+
+// DEBUG_BUFFER_SIZE MUST BE A POWER OF 2
+#define DEBUG_BUFFER_SIZE     (1 << 10)
+#define DEBUG_BUFFER_MASK     (DEBUG_BUFFER_SIZE - 1)
 
-#define USART_SYS_LEVEL 4
 /*---------------------------- Global Variable ------------------------------*/
-//*--------------------------1--------------------------------------------------
-//* \fn    AT91F_DBGU_Printk
-//* \brief This function is used to send a string through the DBGU channel
-//*----------------------------------------------------------------------------
-void AT91F_DBGU_Ready(void)
-{
-	while (!(AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXEMPTY)) ;
-}
 
 //*----------------------------------------------------------------------------
 //* Function Name       : Send_reset
@@ -69,7 +67,7 @@ static void Send_reset(void)
 	// Acknoledge the interrupt
 	// Mark the End of Interrupt on the AIC
 	AT91C_BASE_AIC->AIC_EOICR = 0;
-	AT91F_DBGU_Ready();
+	while (!(AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXEMPTY)) ;
 	// Jump in reset
 	pfct();
 }
@@ -92,26 +90,26 @@ static void DBGU_irq_handler(u_int32_t sr)
 		break;
 	case '1':		//* info
 		udp_pullup_off();
-		AT91F_DBGU_Printk("Set Pull up\n\r");
+		AT91F_DBGU_Frame("Set Pull up\n\r");
 		// Reset Application
 		Send_reset();
 		break;
 	case '2':
-		AT91F_DBGU_Printk("Toggling LED 1\n\r");
+		AT91F_DBGU_Frame("Toggling LED 1\n\r");
 		led_toggle(1);
 		break;
 	case '3':
-		AT91F_DBGU_Printk("Toggling LED 2\n\r");
+		AT91F_DBGU_Frame("Toggling LED 2\n\r");
 		led_toggle(2);
 		break;
 	case '9':
-		AT91F_DBGU_Printk("Resetting SAM7\n\r");
+		AT91F_DBGU_Frame("Resetting SAM7\n\r");
 		AT91F_RSTSoftReset(AT91C_BASE_RSTC, AT91C_RSTC_PROCRST|
 				   AT91C_RSTC_PERRST|AT91C_RSTC_EXTRST);
 		break;
 	default:
 		if (_main_dbgu(value) < 0)
-			AT91F_DBGU_Printk("\n\r");
+			AT91F_DBGU_Frame("\n\r");
 		break;
 	}			// end switch
 }
@@ -148,17 +146,17 @@ void AT91F_DBGU_Init(void)
 	//* open interrupt
 	sysirq_register(AT91SAM7_SYSIRQ_DBGU, &DBGU_irq_handler);
 
-	AT91F_DBGU_Printk("\n\r");
-	AT91F_DBGU_Printk("(C) 2006-2011 by Harald Welte <hwelte@hmw-consulting.de>\n\r"
+	debugp("\n\r");
+	debugp("(C) 2006-2011 by Harald Welte <hwelte@hmw-consulting.de>\n\r"
 			  "This software is FREE SOFTWARE licensed under GNU GPL\n\r");
-	AT91F_DBGU_Printk("Version " COMPILE_SVNREV
+	debugp("Version " COMPILE_SVNREV
 			  " compiled " COMPILE_DATE
 			  " by " COMPILE_BY "\n\r\n\r");
-	AT91F_DBGU_Printk("\n\rDEBUG Interface:\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");
 
-	debugp("RSTC_SR=0x%08x\n", rst_status);
+	debugp("RSTC_SR=0x%08x\n\r", rst_status); 
 }
 
 /*
@@ -173,31 +171,36 @@ void AT91F_DBGU_Fini(void)
 }
 
 //*----------------------------------------------------------------------------
-//* \fn    AT91F_DBGU_Printk
-//* \brief This function is used to send a string through the DBGU channel (Very low level debugging)
-//*----------------------------------------------------------------------------
-void AT91F_DBGU_Printk(char *buffer)
-{
-	while (*buffer != '\0') {
-		while (!AT91F_US_TxReady((AT91PS_USART) AT91C_BASE_DBGU)) ;
-		AT91F_US_PutChar((AT91PS_USART) AT91C_BASE_DBGU, *buffer++);
-	}
-}
-
-//*----------------------------------------------------------------------------
 //* \fn    AT91F_DBGU_Frame
-//* \brief This function is used to send a string through the DBGU channel
+//* \brief This function is used to send a string through the DBGU channel (Very low level debugging)
 //*----------------------------------------------------------------------------
 void AT91F_DBGU_Frame(char *buffer)
 {
-	unsigned char len;
-
-	for (len = 0; buffer[len] != '\0'; len++) {
+	unsigned long intcFlags;
+	unsigned int len = 0;
+	while (buffer[len++]) ALLOW_INTERRUPT_LOOP;
+	len--;
+	local_irq_save(intcFlags);
+	AT91C_BASE_DBGU->DBGU_PTCR = 1 << 9; // Disable transfer
+	if (AT91C_BASE_DBGU->DBGU_TNCR) {
+		AT91C_BASE_DBGU->DBGU_PTCR = 1 << 8;  // Resume transfer
+		local_irq_restore(intcFlags);
+		while ((AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXBUFE) == 0) ALLOW_INTERRUPT_LOOP;
+		local_irq_save(intcFlags);
+		AT91C_BASE_DBGU->DBGU_PTCR = 1 << 9; // Disable transfer    
+		AT91C_BASE_DBGU->DBGU_TPR = (unsigned)buffer;
+		AT91C_BASE_DBGU->DBGU_TCR = len;
+	} else if (AT91C_BASE_DBGU->DBGU_TCR) {
+		AT91C_BASE_DBGU->DBGU_TNPR = (unsigned)buffer;
+		AT91C_BASE_DBGU->DBGU_TNCR = len;
+	} else {
+		AT91C_BASE_DBGU->DBGU_TPR = (unsigned)buffer;
+		AT91C_BASE_DBGU->DBGU_TCR = len;
 	}
-
-	AT91F_US_SendFrame((AT91PS_USART) AT91C_BASE_DBGU, 
-			   (unsigned char *)buffer, len, 0, 0);
-
+	AT91C_BASE_DBGU->DBGU_PTCR = 1 << 8;  // Resume transfer
+	local_irq_restore(intcFlags);
+	// Return ONLY after we complete Transfer
+	while ((AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXBUFE) == 0) ALLOW_INTERRUPT_LOOP;
 }
 
 //*----------------------------------------------------------------------------
@@ -239,105 +242,175 @@ hexdump(const void *data, unsigned int len)
 }
 
 struct dbgu {
-	char buf[4096];
-	char *next_inbyte;
-	char *next_outbyte;
+	char buf[DEBUG_BUFFER_SIZE];
+  // Since debugp appears to require to be re-entrant, we need a bit more state variables
+  
+  /* States: 
+     INUSE: when a position is filled and should be written out by dbgu_rb_flush
+     AVAIL: when a position is available to receive from dbgu_rb_append
+     in_head: Position where incoming *append* characters have finished copy to buffer
+     in_tail: Position where NEW incoming *append* characters should COPY to
+     out_head: Position where the LAST *possibly incomplete* dbgu_rb_flush started from
+     out_tail: Position where the NEXT dbug_rb_flush should start from
+
+     The position in the RING should be in this order:
+     --> out_tail --> in_head --> in_tail --> out_head -->     
+  */
+	volatile unsigned int in_head, in_tail, out_head, out_tail;
+
+  // flush_stack is to indicate rb_flush is being executed, NOT to execute again
+  // append_stack is to indicate the re-entrack stack order of the current dbgu_append call
+	volatile unsigned int flush_stack, append_stack;
 };
+
 static struct dbgu dbgu;
 
 void dbgu_rb_init(void)
 {
-	memset(dbgu.buf, 0, sizeof(dbgu.buf));
-	dbgu.next_inbyte = &dbgu.buf[0];
-	dbgu.next_outbyte = &dbgu.buf[0];
-}
-
-/* pull one char out of debug ring buffer */
-static int dbgu_rb_pull(char *ret)
-{
-	unsigned long flags;
-
-	local_irq_save(flags);
-
-	if (dbgu.next_outbyte == dbgu.next_inbyte) {
-		local_irq_restore(flags);
-		return -1;
-	}
-
-	*ret = *dbgu.next_outbyte;
-
-	dbgu.next_outbyte++;
-	if (dbgu.next_outbyte == &dbgu.buf[0]+sizeof(dbgu.buf)) {
-		//AT91F_DBGU_Printk("WRAP DURING PULL\r\n");
-		dbgu.next_outbyte = &dbgu.buf[0];
-	} else if (dbgu.next_outbyte > &dbgu.buf[0]+sizeof(dbgu.buf)) {
-		//AT91F_DBGU_Printk("OUTBYTE > END_OF_BUF!!\r\n");
-		dbgu.next_outbyte -= sizeof(dbgu.buf);
-	}
-
-	local_irq_restore(flags);
-
-	return 0;
-}
-
-static void __rb_flush(void)
-{
-	char ch;
-	while (dbgu_rb_pull(&ch) >= 0) {
-		while (!AT91F_US_TxReady((AT91PS_USART) AT91C_BASE_DBGU)) ;
-		AT91F_US_PutChar((AT91PS_USART) AT91C_BASE_DBGU, ch);
-	}
+	dbgu.in_head = dbgu.in_tail = dbgu.out_head = dbgu.out_tail = 0;
+	dbgu.flush_stack = dbgu.append_stack = 0;
 }
 
 /* flush pending data from debug ring buffer to serial port */
-void dbgu_rb_flush(void)
-{
-	__rb_flush();
-}
 
-static void __dbgu_rb_append(char *data, int len)
+void dbgu_rb_flush(void)
 {
-	char *pos = dbgu.next_inbyte;
-
-	dbgu.next_inbyte += len;
-	if (dbgu.next_inbyte >= &dbgu.buf[0]+sizeof(dbgu.buf)) {
-		AT91F_DBGU_Printk("WRAP DURING APPEND\r\n");
-		dbgu.next_inbyte -= sizeof(dbgu.buf);
+	unsigned long intcFlags;
+	unsigned int flush_stack, start, len;
+	if (dbgu.in_head == dbgu.out_tail)
+		return;
+
+	// Transmit can ONLY be disabled when Interrupt is disabled.  We don't want to be interrupted while  Transmit is disabled.
+	local_irq_save(intcFlags);
+	flush_stack = dbgu.flush_stack;
+	dbgu.flush_stack = 1;
+	if (flush_stack) {
+		local_irq_restore(intcFlags);
+		return;
 	}
-
-	memcpy(pos, data, len);
+	AT91C_BASE_DBGU->DBGU_PTCR = 1 << 9; // Disable transfer
+	start = (unsigned)dbgu.buf + dbgu.out_tail;
+	len = (dbgu.in_head - dbgu.out_tail) & DEBUG_BUFFER_MASK;
+	if (dbgu.in_head > dbgu.out_tail || dbgu.in_head == 0) { // Just 1 fragmentf
+		if (AT91C_BASE_DBGU->DBGU_TNCR) {
+			if (AT91C_BASE_DBGU->DBGU_TNPR + AT91C_BASE_DBGU->DBGU_TNCR == start) {
+				AT91C_BASE_DBGU->DBGU_TNCR += len;
+			} else {
+				AT91C_BASE_DBGU->DBGU_PTCR = 1 << 8;  // Resume transfer
+				local_irq_restore(intcFlags);
+				while ((AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXBUFE) == 0) ALLOW_INTERRUPT_LOOP;
+				dbgu.out_head = dbgu.out_tail; // in case we are interrupted, they may need more space
+				/* Since the ONLY place where Transmit is started is dbgu_rb_flush and AT91F_DBGU_Frame and:
+				 * 1) AT91F_DBGU_Frame always leave the dbgu in a TX completed state
+				 * 2) dbgu_rb is non-reentrant by safeguard at the beginning of this routing
+				 * We can assume that after this INTERRUPTIBLE while loop, there are NO data to be transmitted
+				 */
+				local_irq_save(intcFlags);
+				AT91C_BASE_DBGU->DBGU_PTCR = 1 << 9; // Disable transfer
+				AT91C_BASE_DBGU->DBGU_TPR = start;
+				AT91C_BASE_DBGU->DBGU_TCR = len;
+			}
+		} else if (AT91C_BASE_DBGU->DBGU_TCR) {
+		  if (AT91C_BASE_DBGU->DBGU_TPR + AT91C_BASE_DBGU->DBGU_TCR == start) {
+		    dbgu.out_head = AT91C_BASE_DBGU->DBGU_TPR - (unsigned)dbgu.buf;
+		    AT91C_BASE_DBGU->DBGU_TCR += len;
+		  } else {
+		    AT91C_BASE_DBGU->DBGU_TNPR = start;
+		    AT91C_BASE_DBGU->DBGU_TNCR = len;
+		  }
+		} else {
+		  AT91C_BASE_DBGU->DBGU_TPR = start;
+		  AT91C_BASE_DBGU->DBGU_TCR = len;
+		  dbgu.out_head = dbgu.out_tail;
+		}
+	} else { // 2 fragments
+		if ((AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXBUFE) == 0) {
+			AT91C_BASE_DBGU->DBGU_PTCR = 1 << 8;  // Resume transfer
+			local_irq_restore(intcFlags);
+			while ((AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXBUFE) == 0) ALLOW_INTERRUPT_LOOP;
+			dbgu.out_head = dbgu.out_tail; // in case we are interrupted, they may need more space
+			local_irq_save(intcFlags);
+			AT91C_BASE_DBGU->DBGU_PTCR = 1 << 9; // Disable transfer
+		}
+		AT91C_BASE_DBGU->DBGU_TPR = start;
+		AT91C_BASE_DBGU->DBGU_TCR = DEBUG_BUFFER_SIZE - dbgu.out_tail;
+		AT91C_BASE_DBGU->DBGU_TNPR = (unsigned)dbgu.buf;
+		AT91C_BASE_DBGU->DBGU_TNCR = dbgu.in_head;
+		dbgu.out_head = dbgu.out_tail;
+	}
+	AT91C_BASE_DBGU->DBGU_PTCR = 1 << 8;  // Resume transfer
+	dbgu.out_tail = dbgu.in_head;
+	dbgu.flush_stack = 0;
+	local_irq_restore(intcFlags);
 }
 
 void dbgu_rb_append(char *data, int len)
 {
-	unsigned long flags;
-	int bytes_left;
-	char *data_cur;
+	/* Rules:
+	 * 1) ONLY the LOWEST order of dbgu_rb_append CAN update in_head;
+	 * 2) WHEN updateing in_head, always set it to the current in_tail (since all higher order dbgu_rb_append have completed
+	 * 3) ONLY the LOWEST order of dbgu_rb_append MAY call dbgu_rb_flush
+	 */
+	unsigned long intcFlags;
+	unsigned int append_stack, avail, local_head;
+	if (len <= 0)
+		return;
 	
-	local_irq_save(flags);
-
-	bytes_left = &dbgu.buf[0]+sizeof(dbgu.buf)-dbgu.next_inbyte;
-	data_cur = data;
-
-	if (len > bytes_left) {
-		AT91F_DBGU_Printk("LEN > BYTES_LEFT\r\n");
-		__rb_flush();
-		__dbgu_rb_append(data_cur, bytes_left);
-		len -= bytes_left;
-		data_cur += bytes_left;
+	local_irq_save(intcFlags);
+	append_stack = dbgu.append_stack++;
+	if (AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXBUFE)
+		dbgu.out_head = dbgu.out_tail;
+	avail = (dbgu.out_head - 1 - dbgu.in_tail) & DEBUG_BUFFER_MASK;
+	local_head = (unsigned)len;
+	if (local_head > avail) {
+		local_irq_restore(intcFlags);
+		while ((AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXBUFE) == 0);
+		local_irq_save(intcFlags);
+		while ((AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXBUFE) == 0);
+		dbgu.out_head = dbgu.out_tail;
+		avail = (dbgu.out_head - 1 - dbgu.in_tail) & DEBUG_BUFFER_MASK;
+		if (local_head > avail) {
+			local_head -= avail;
+			AT91C_BASE_DBGU->DBGU_TPR = (unsigned)data;
+			AT91C_BASE_DBGU->DBGU_TCR = local_head;
+			data += local_head;
+			len = avail;
+		}
 	}
-	__dbgu_rb_append(data_cur, len);
-
-	local_irq_restore(flags);
+	local_head = dbgu.in_tail;
+	dbgu.in_tail += len;
+	dbgu.in_tail &= DEBUG_BUFFER_MASK;
+	local_irq_restore(intcFlags);
+	if (dbgu.out_head <= local_head) {
+		// We may have to wrap around: out_head won't change because NO call to flush will be made YET
+		avail = DEBUG_BUFFER_SIZE - local_head;
+		if (avail >= (unsigned)len) {
+			memcpy(dbgu.buf + local_head, data, (size_t)len);
+		} else {
+			memcpy(dbgu.buf + local_head, data, (size_t)avail);
+			memcpy(dbgu.buf, data + avail, (size_t)(len - avail));
+		}
+	} else {
+		memcpy(dbgu.buf + local_head, data, len);
+	}
+	local_irq_save(intcFlags);
+	dbgu.append_stack--;
+	if (!append_stack)
+		dbgu.in_head = dbgu.in_tail;
+	local_irq_restore(intcFlags);
+	if (!append_stack)
+		dbgu_rb_flush();
 }
 
 static char dbg_buf[256];
+static int line_num = 0;
 void debugp(const char *format, ...)
 {
 	va_list ap;
 
 	va_start(ap, format);
-	vsnprintf(dbg_buf, sizeof(dbg_buf)-1, format, ap);
+	sprintf(dbg_buf, "[%06X] ", line_num++);
+	vsnprintf(dbg_buf + 9, sizeof(dbg_buf)-10, format, ap);
 	va_end(ap);
 
 	dbg_buf[sizeof(dbg_buf)-1] = '\0';			
diff --git a/firmware/src/os/dbgu.h b/firmware/src/os/dbgu.h
index c36ac8c..ed2d3e8 100644
--- a/firmware/src/os/dbgu.h
+++ b/firmware/src/os/dbgu.h
@@ -17,15 +17,13 @@
 
 #define AT91C_DBGU_BAUD 115200
 
-//#define DEBUGP(x)	AT91F_DBGU_Printk(x)
-
 //* ----------------------- External Function Prototype -----------------------
 
 extern const char *hexdump(const void *data, unsigned int len);
 void AT91F_DBGU_Init(void);
 void AT91F_DBGU_Fini(void);
-void AT91F_DBGU_Printk(	char *buffer);
-void AT91F_DBGU_Frame(	char *buffer);
+void AT91F_DBGU_Frame(char *buffer);
+inline void AT91F_DBGU_Printk(char *buffer) { AT91F_DBGU_Frame(buffer); }
 int AT91F_DBGU_Get( char *val);
 void dbgu_rb_flush(void);
 #ifndef __WinARM__
diff --git a/firmware/src/os/main.c b/firmware/src/os/main.c
index 968f6ad..363befc 100644
--- a/firmware/src/os/main.c
+++ b/firmware/src/os/main.c
@@ -77,7 +77,6 @@ int main(void)
 	while (1) {
 		/* Call application specific main idle function */
 		_main_func();
-		dbgu_rb_flush();
 		
 		/* restart watchdog timer */
 		wdt_restart();
diff --git a/firmware/src/os/pcd_enumerate.c b/firmware/src/os/pcd_enumerate.c
index 3a7397f..0e11822 100644
--- a/firmware/src/os/pcd_enumerate.c
+++ b/firmware/src/os/pcd_enumerate.c
@@ -99,8 +99,10 @@ struct epstate {
 };
 
 static const struct epstate epstate[] = {
-	[0] =	{ .state_busy = RCTX_STATE_INVALID },
-	[1] =	{ .state_busy = RCTX_STATE_INVALID },
+ 	[0] =	{ .state_busy = RCTX_STATE_UDP_EP0_BUSY,
+		  .state_pending = RCTX_STATE_UDP_EP0_PENDING },
+	[1] =	{ .state_busy = RCTX_STATE_UDP_EP1_BUSY,
+		  .state_pending = RCTX_STATE_UDP_EP1_PENDING },
 	[2] =	{ .state_busy = RCTX_STATE_UDP_EP2_BUSY,
 		  .state_pending = RCTX_STATE_UDP_EP2_PENDING },
 	[3] =	{ .state_busy = RCTX_STATE_UDP_EP3_BUSY,
@@ -112,8 +114,6 @@ static void reset_ep(unsigned int ep)
 	AT91PS_UDP pUDP = upcd.pUdp;
 	struct req_ctx *rctx;
 
-	//pUDP->UDP_CSR[ep] = AT91C_UDP_EPEDS;
-
 	atomic_set(&upcd.ep[ep].pkts_in_transit, 0);
 
 	/* free all currently transmitting contexts */
@@ -125,6 +125,7 @@ static void reset_ep(unsigned int ep)
 
 	pUDP->UDP_RSTEP |= (1 << ep);
 	pUDP->UDP_RSTEP &= ~(1 << ep);
+	pUDP->UDP_CSR[ep] = AT91C_UDP_EPEDS;
 
 	upcd.ep[ep].incomplete.rctx = NULL;
 }
@@ -137,7 +138,7 @@ void udp_unthrottle(void)
 	pUDP->UDP_IER = AT91C_UDP_EPINT1;
 }
 
-static int __udp_refill_ep(int ep)
+int udp_refill_ep(int ep)
 {
 	u_int16_t i;
 	AT91PS_UDP pUDP = upcd.pUdp;
@@ -149,13 +150,17 @@ static int __udp_refill_ep(int ep)
 	if (!upcd.cur_config) {
 		return -ENXIO;
 	}
-	
+
 	/* If there are already two packets in transit, the DPR of
 	 * the SAM7 UDC doesn't have space for more data */
+	// if (atomic_read(&upcd.ep[ep].pkts_in_transit) == 2 || (pUDP->UDP_CSR[ep] & AT91C_UDP_TXPKTRDY)) {
+	// if (pUDP->UDP_CSR[ep] & AT91C_UDP_TXPKTRDY) {
 	if (atomic_read(&upcd.ep[ep].pkts_in_transit) == 2) {
 		return -EBUSY;
 	}
 
+	pUDP->UDP_IDR |= 1 << ep;
+
 	/* If we have an incompletely-transmitted req_ctx (>EP size),
 	 * we need to transmit the rest and finish the transaction */
 	if (upcd.ep[ep].incomplete.rctx) {
@@ -165,66 +170,55 @@ static int __udp_refill_ep(int ep)
 		/* get pending rctx and start transmitting from zero */
 		rctx = req_ctx_find_get(0, epstate[ep].state_pending, 
 					epstate[ep].state_busy);
-		if (!rctx)
+		if (!rctx) {
+			pUDP->UDP_IER |= 1 << ep;
+			return 0;
+		}
+		if (rctx->tot_len == 0) {
+			pUDP->UDP_IER |= 1 << ep;
+			req_ctx_put(rctx);
 			return 0;
+		}
+
+		DEBUGPCR("USBT(D=%08X, S=%02X%02X, L=%04u, P=%02u) H4/T4: %02X %02X %02X %02X / %02X %02X %02X %02X",
+			 rctx->data, rctx->data[5], rctx->data[4], rctx->tot_len, req_ctx_count(epstate[ep].state_pending),
+			 rctx->data[10], rctx->data[11], rctx->data[12], rctx->data[13],
+			 rctx->data[rctx->tot_len - 4], rctx->data[rctx->tot_len - 3], 
+			 rctx->data[rctx->tot_len - 2], rctx->data[rctx->tot_len - 1]);
+
 		start = 0;
 
 		upcd.ep[ep].incomplete.bytes_sent = 0;
 	}
 
-	if (rctx->tot_len - start <= AT91C_EP_IN_SIZE)
+/*	pUDP->UDP_IER |= 1 << ep;
+	req_ctx_put(rctx);
+	return 0; */
+
+	if (rctx->tot_len - start <= AT91C_EP_IN_SIZE) {
 		end = rctx->tot_len;
-	else
+	} else {
 		end = start + AT91C_EP_IN_SIZE;
+	}
 
 	/* fill FIFO/DPR */
-	DEBUGII("RCTX_tx(ep=%u,ctx=%u):%u ", ep, req_ctx_num(rctx),
-		end - start);
 	for (i = start; i < end; i++) 
 		pUDP->UDP_FDR[ep] = rctx->data[i];
 
-	if (atomic_inc_return(&upcd.ep[ep].pkts_in_transit) == 1) {
-		/* not been transmitting before, start transmit */
+	if (atomic_inc_return(&upcd.ep[ep].pkts_in_transit) == 1)
 		pUDP->UDP_CSR[ep] |= AT91C_UDP_TXPKTRDY;
-	}
-
-	if ((end - start < AT91C_EP_OUT_SIZE) ||
-	    (((end - start) == 0) && end && (rctx->tot_len % AT91C_EP_OUT_SIZE) == 0)) {
-		/* CASE 1: return context to pool, if
-		 * - packet transfer < AT91C_EP_OUT_SIZE
-		 * - after ZLP of transfer == AT91C_EP_OUT_SIZE
-		 * - after ZLP of transfer % AT91C_EP_OUT_SIZE == 0
-		 * - after last packet of transfer % AT91C_EP_OUT_SIZE != 0
-		 */
-		DEBUGII("RCTX(ep=%u,ctx=%u)_tx_done ", ep, req_ctx_num(rctx));
+	
+	if (end == rctx->tot_len) {
 		upcd.ep[ep].incomplete.rctx = NULL;
 		req_ctx_put(rctx);
 	} else {
-		/* CASE 2: mark transfer as incomplete, if
-		 * - after data of transfer == AT91C_EP_OUT_SIZE
-		 * - after data of transfer > AT91C_EP_OUT_SIZE
-		 * - after last packet of transfer % AT91C_EP_OUT_SIZE == 0
-		 */
 		upcd.ep[ep].incomplete.rctx = rctx;
 		upcd.ep[ep].incomplete.bytes_sent += end - start;
-		DEBUGII("RCTX(ep=%u)_tx_cont ", ep);
 	}
-
+	pUDP->UDP_IER |= 1 << ep;
 	return 1;
 }
 
-int udp_refill_ep(int ep)
-{
-	unsigned long flags;
-	int ret;
-
-	local_irq_save(flags);
-	ret = __udp_refill_ep(ep);
-	local_irq_restore(flags);
-
-	return ret;
-}
-
 static void udp_irq(void)
 {
 	u_int32_t csr;
@@ -238,19 +232,18 @@ static void udp_irq(void)
 		DEBUGI("ENDBUSRES ");
 		pUDP->UDP_ICR = AT91C_UDP_ENDBUSRES;
 		pUDP->UDP_IER = AT91C_UDP_EPINT0;
-		/* reset all endpoints */
-		pUDP->UDP_RSTEP = (unsigned int)-1;
-		pUDP->UDP_RSTEP = 0;
+		reset_ep(0);
+		reset_ep(1);
+		reset_ep(2);
+		reset_ep(3);
+
 		/* Enable the function */
 		pUDP->UDP_FADDR = AT91C_UDP_FEN;
+
 		/* Configure endpoint 0 */
 		pUDP->UDP_CSR[0] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_CTRL);
 		upcd.cur_config = 0;
 		upcd.state = USB_STATE_DEFAULT;
-
-		reset_ep(1);
-		reset_ep(2);
-		reset_ep(3);
 		
 #ifdef CONFIG_DFU
 		if (*dfu->dfu_state == DFU_STATE_appDETACH) {
@@ -359,7 +352,7 @@ cont_ep2:
 			if (atomic_dec_return(&upcd.ep[2].pkts_in_transit) == 1)
 				pUDP->UDP_CSR[2] |= AT91C_UDP_TXPKTRDY;
 
-			__udp_refill_ep(2);
+			udp_refill_ep(2);
 		}
 	}
 	if (isr & AT91C_UDP_EPINT3) {
@@ -374,7 +367,7 @@ cont_ep2:
 			if (atomic_dec_return(&upcd.ep[3].pkts_in_transit) == 1)
 				pUDP->UDP_CSR[3] |= AT91C_UDP_TXPKTRDY;
 
-			__udp_refill_ep(3);
+			udp_refill_ep(3);
 		}
 	}
 	if (isr & AT91C_UDP_RXSUSP) {
diff --git a/firmware/src/os/req_ctx.c b/firmware/src/os/req_ctx.c
index cc8d57b..ab7775c 100644
--- a/firmware/src/os/req_ctx.c
+++ b/firmware/src/os/req_ctx.c
@@ -29,77 +29,184 @@
 /* FIXME: locking, FIFO order processing */
 
 #if defined(__AT91SAM7S64__) || defined(RUN_FROM_RAM)
-#define NUM_RCTX_SMALL 16
-#define NUM_RCTX_LARGE 1
+#define NUM_RCTX_SMALL 0
+#define NUM_RCTX_LARGE 8
 #else
-#define NUM_RCTX_SMALL 8
-#define NUM_RCTX_LARGE 4
+#define NUM_RCTX_SMALL 0
+#define NUM_RCTX_LARGE 20
 #endif
 
 #define NUM_REQ_CTX	(NUM_RCTX_SMALL+NUM_RCTX_LARGE)
 
 static u_int8_t rctx_data[NUM_RCTX_SMALL][RCTX_SIZE_SMALL];
-static u_int8_t rctx_data_large[NUM_RCTX_LARGE][RCTX_SIZE_LARGE];
+static u_int8_t rctx_data_large[NUM_RCTX_LARGE][RCTX_SIZE_LARGE], rctx_data_TEST[RCTX_SIZE_LARGE];
 
 static struct req_ctx req_ctx[NUM_REQ_CTX];
 
+static struct req_ctx *req_ctx_queues[RCTX_STATE_COUNT], *req_ctx_tails[RCTX_STATE_COUNT];
+static unsigned req_counts[RCTX_STATE_COUNT];
+
 struct req_ctx __ramfunc *req_ctx_find_get(int large,
 				 unsigned long old_state, 
 				 unsigned long new_state)
 {
+        struct req_ctx *toReturn;
 	unsigned long flags;
-	u_int8_t i;
-	
-	if (large)
-		i = NUM_RCTX_SMALL;
-	else
-		i = 0;
 
-	for (; i < NUM_REQ_CTX; i++) {
-		local_irq_save(flags);
-		if (req_ctx[i].state == old_state) {
-			req_ctx[i].state = new_state;
-			local_irq_restore(flags);
-			return &req_ctx[i];
-		}
-		local_irq_restore(flags);
+	if (old_state >= RCTX_STATE_COUNT || new_state >= RCTX_STATE_COUNT) {
+		DEBUGPCR("Invalid parameters for req_ctx_find_get");
+		return NULL;
 	}
-
-	return NULL;
+	local_irq_save(flags);
+	toReturn = req_ctx_queues[old_state];
+	if (toReturn) {
+		if ((req_ctx_queues[old_state] = toReturn->next))
+			toReturn->next->prev = NULL;
+		else
+			req_ctx_tails[old_state] = NULL;
+		req_counts[old_state]--;
+		if ((toReturn->prev = req_ctx_tails[new_state]))
+	  		toReturn->prev->next = toReturn;
+		else
+			req_ctx_queues[new_state] = toReturn;
+		req_ctx_tails[new_state] = toReturn;
+		toReturn->state = new_state;
+		toReturn->next = NULL;
+		req_counts[new_state]++;
+	}
+	local_irq_restore(flags);
+	return toReturn;
 }
 
 u_int8_t req_ctx_num(struct req_ctx *ctx)
 {
-	return ((char *)ctx - (char *)&req_ctx[0])/sizeof(*ctx);
+	return ctx - req_ctx;
 }
 
 void req_ctx_set_state(struct req_ctx *ctx, unsigned long new_state)
 {
 	unsigned long flags;
+	unsigned old_state;
 
-	/* FIXME: do we need this kind of locking, we're UP! */
+	if (new_state >= RCTX_STATE_COUNT) {
+		DEBUGPCR("Invalid new_state for req_ctx_set_state");
+		return;
+	}
 	local_irq_save(flags);
+	old_state = ctx->state;
+	if (ctx->prev)
+		ctx->prev->next = ctx->next;
+	else 
+		req_ctx_queues[old_state] = ctx->next;
+	if (ctx->next)
+		ctx->next->prev = ctx->prev;
+	else
+		req_ctx_tails[old_state] = ctx->prev;
+	req_counts[old_state]--;
+	if ((ctx->prev = req_ctx_tails[new_state]))
+		ctx->prev->next = ctx;
+	else
+		req_ctx_queues[new_state] = ctx;
+	req_ctx_tails[new_state] = ctx;
 	ctx->state = new_state;
+	ctx->next = NULL;
+	req_counts[new_state]++;
 	local_irq_restore(flags);
 }
 
+#ifdef DEBUG_REQCTX
+void req_print(int state) {
+	int count = 0;
+	struct req_ctx *ctx, *last = NULL;
+	DEBUGP("State [%02i] start <==> ", state);
+	ctx = req_ctx_queues[state];
+	while (ctx) {
+		if (last != ctx->prev)
+			DEBUGP("*INV_PREV* ");
+		DEBUGP("%08X => ", ctx);
+		last = ctx;
+		ctx = ctx->next;
+		count++;
+		if (count > NUM_REQ_CTX) {
+		  DEBUGP("*WILD POINTER* => ");
+		  break;
+		}
+	}
+	DEBUGPCR("NULL");
+	if (!req_ctx_queues[state] && req_ctx_tails[state]) {
+		DEBUGPCR("NULL head, NON-NULL tail");
+	}
+	if (last != req_ctx_tails[state]) {
+		DEBUGPCR("Tail does not match last element");
+	}
+}
+#endif
+
 void req_ctx_put(struct req_ctx *ctx)
 {
-	req_ctx_set_state(ctx, RCTX_STATE_FREE);
+        unsigned long intcFlags;
+	unsigned old_state;
+	
+	local_irq_save(intcFlags);
+	old_state = ctx->state;
+	if (ctx->prev)
+		ctx->prev->next = ctx->next;
+	else 
+		req_ctx_queues[old_state] = ctx->next;
+	if (ctx->next)
+		ctx->next->prev = ctx->prev;
+	else
+		req_ctx_tails[old_state] = ctx->prev;
+	req_counts[old_state]--;
+	if ((ctx->prev = req_ctx_tails[RCTX_STATE_FREE]))
+		ctx->prev->next = ctx;
+	else
+		req_ctx_queues[RCTX_STATE_FREE] = ctx;
+	req_ctx_tails[RCTX_STATE_FREE] = ctx;
+	ctx->state = RCTX_STATE_FREE;
+	ctx->next = NULL;
+	req_counts[RCTX_STATE_FREE]++;
+	local_irq_restore(intcFlags);
+}
+
+unsigned req_ctx_count(unsigned long state)
+{
+	if (state >= RCTX_STATE_COUNT)
+		return 0;
+	return req_counts[state];
 }
 
 void req_ctx_init(void)
 {
 	int i;
-
 	for (i = 0; i < NUM_RCTX_SMALL; i++) {
+	        req_ctx[i].prev = req_ctx + i - 1;
+	        req_ctx[i].next = req_ctx + i + 1;
 		req_ctx[i].size = RCTX_SIZE_SMALL;
+		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);
 	}
 
-	for (i = 0; i < NUM_RCTX_LARGE; i++) {
-		req_ctx[NUM_RCTX_SMALL+i].size = RCTX_SIZE_LARGE;
-		req_ctx[NUM_RCTX_SMALL+i].data = rctx_data_large[i];
+	for (; i < NUM_REQ_CTX; i++) {
+	        req_ctx[i].prev = req_ctx + i - 1;
+  	        req_ctx[i].next = req_ctx + i + 1;
+		req_ctx[i].size = RCTX_SIZE_LARGE;
+		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);
+	}
+	req_ctx[0].prev = NULL;
+	req_ctx[NUM_REQ_CTX - 1].next = NULL;
+
+	req_ctx_queues[RCTX_STATE_FREE] = req_ctx;
+	req_ctx_tails[RCTX_STATE_FREE] = req_ctx + NUM_REQ_CTX - 1;
+	req_counts[RCTX_STATE_FREE] = NUM_REQ_CTX;
+
+	for (i = RCTX_STATE_FREE + 1; i < RCTX_STATE_COUNT; i++) {
+		req_ctx_queues[i] = req_ctx_tails[i] = NULL;
+		req_counts[i] = 0;
 	}
 }
diff --git a/firmware/src/os/req_ctx.h b/firmware/src/os/req_ctx.h
index 94b5c5a..7d09124 100644
--- a/firmware/src/os/req_ctx.h
+++ b/firmware/src/os/req_ctx.h
@@ -1,11 +1,10 @@
 #ifndef _REQ_CTX_H
 #define _REQ_CTX_H
 
-#define RCTX_SIZE_LARGE	2048
-#define RCTX_SIZE_SMALL	128
+#define RCTX_SIZE_LARGE       960
+#define RCTX_SIZE_SMALL       320
 
 #define MAX_HDRSIZE	sizeof(struct openpcd_hdr)
-#define MAX_REQSIZE	(64-MAX_HDRSIZE)
 
 #define req_buf_payload(x)	(x->data[x->hdr_len])
 #define req_buf_hdr(x)		(x->data[0])
@@ -13,37 +12,42 @@
 #include <sys/types.h>
 #include <lib_AT91SAM7.h>
 
+typedef struct req_ctx *req_ctx_ptr;
+
 struct req_ctx {
-	volatile u_int32_t state;
-	u_int16_t size;
-	u_int16_t tot_len;
-	u_int8_t *data;
+  volatile u_int32_t state;
+  volatile req_ctx_ptr prev, next;
+  u_int16_t size;
+  u_int16_t tot_len;
+  u_int8_t *data;
 };
 
-#define RCTX_STATE_FREE			0xfe
-#define RCTX_STATE_UDP_RCV_BUSY		0x01
-#define RCTX_STATE_UDP_RCV_DONE		0x02
-#define RCTX_STATE_MAIN_PROCESSING	0x03
-#define RCTX_STATE_RC632IRQ_BUSY	0x04
-
-#define RCTX_STATE_UDP_EP2_PENDING	0x10
-#define RCTX_STATE_UDP_EP2_BUSY		0x11
-
-#define RCTX_STATE_UDP_EP3_PENDING	0x12
-#define RCTX_STATE_UDP_EP3_BUSY		0x13
-
-#define RCTX_STATE_SSC_RX_BUSY		0x20
-
-#define RCTX_STATE_LIBRFID_BUSY		0x30
-
-#define RCTX_STATE_PIOIRQ_BUSY		0x80
-
-#define RCTX_STATE_INVALID		0xff
+#define RCTX_STATE_FREE                 0
+#define RCTX_STATE_UDP_RCV_BUSY         1
+#define RCTX_STATE_UDP_RCV_DONE         2
+#define RCTX_STATE_MAIN_PROCESSING      3
+#define RCTX_STATE_RC632IRQ_BUSY        4
+#define RCTX_STATE_UDP_EP2_PENDING      5
+#define RCTX_STATE_UDP_EP2_BUSY         6
+#define RCTX_STATE_UDP_EP3_PENDING      7
+#define RCTX_STATE_UDP_EP3_BUSY         8
+#define RCTX_STATE_SSC_RX_BUSY          9
+#define RCTX_STATE_LIBRFID_BUSY        10
+#define RCTX_STATE_PIOIRQ_BUSY         11
+#define RCTX_STATE_INVALID             12
+// Nominally UNUSED states
+#define RCTX_STATE_UDP_EP0_PENDING     13
+#define RCTX_STATE_UDP_EP0_BUSY        14
+#define RCTX_STATE_UDP_EP1_PENDING     15
+#define RCTX_STATE_UDP_EP1_BUSY        16
+// Count of the number of STATES
+#define RCTX_STATE_COUNT               17
 
 extern struct req_ctx __ramfunc *req_ctx_find_get(int large, unsigned long old_state, unsigned long new_state);
 extern struct req_ctx *req_ctx_find_busy(void);
 extern void req_ctx_set_state(struct req_ctx *ctx, unsigned long new_state);
 extern void req_ctx_put(struct req_ctx *ctx);
 extern u_int8_t req_ctx_num(struct req_ctx *ctx);
+unsigned req_ctx_count(unsigned long state);
 
 #endif /* _REQ_CTX_H */
diff --git a/firmware/src/os/system_irq.c b/firmware/src/os/system_irq.c
index dc787eb..cc1fa08 100644
--- a/firmware/src/os/system_irq.c
+++ b/firmware/src/os/system_irq.c
@@ -25,15 +25,15 @@
 
 #include <os/system_irq.h>
 #include <os/dbgu.h>
+#include <string.h>
 
 #include "../openpcd.h"
 
 static sysirq_hdlr *sysirq_hdlrs[AT91SAM7_SYSIRQ_COUNT];
 
-static void sys_irq(void)
+void sys_irq(u_int32_t previous_pc)
 {
 	u_int32_t sr;
-	DEBUGP("sys_irq ");
 
 	/* Somehow Atmel decided to do really stupid interrupt sharing
 	 * for commonly-used interrupts such as the timer irq */
@@ -139,6 +139,10 @@ static void sys_irq(void)
 	if (*AT91C_WDTC_WDMR & AT91C_WDTC_WDFIEN) {
 		sr = *AT91C_WDTC_WDSR;
 		if (sr) {
+			char dbg_buf[100];
+			sprintf(dbg_buf, "sys_irq [Old PC = %08X]\n\r", previous_pc);
+			AT91F_DBGU_Frame(dbg_buf);
+
 			DEBUGP("WDT(");
 			if (sysirq_hdlrs[AT91SAM7_SYSIRQ_WDT]) {
 				DEBUGP("handler ");
@@ -154,6 +158,13 @@ static void sys_irq(void)
 	DEBUGPCR("END");
 }
 
+static void sysirq_entry() {
+  	// DO NOT MODIFY THIS SECTION AND Cstartup.S/IRQ_Handler_Entry.
+	register unsigned *previous_pc asm("r0");
+	asm("ADD R1, SP, #16; LDR R0, [R1]"); 
+	sys_irq(previous_pc);
+}
+
 void sysirq_register(enum sysirqs irq, sysirq_hdlr *hdlr)
 {
 	if (irq >= AT91SAM7_SYSIRQ_COUNT)
@@ -167,6 +178,6 @@ void sysirq_init(void)
 	AT91F_AIC_ConfigureIt(AT91C_BASE_AIC, AT91C_ID_SYS,
 			      OPENPCD_IRQ_PRIO_SYS,
 			      AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL,
-			      &sys_irq);
+			      &sysirq_entry);
 	AT91F_AIC_EnableIt(AT91C_BASE_AIC, AT91C_ID_SYS);
 }
diff --git a/firmware/src/os/usbcmd_generic.c b/firmware/src/os/usbcmd_generic.c
index 77b992f..aff13eb 100644
--- a/firmware/src/os/usbcmd_generic.c
+++ b/firmware/src/os/usbcmd_generic.c
@@ -132,7 +132,7 @@ static int gen_usb_rx(struct req_ctx *rctx)
 
 void usbcmd_gen_init(void)
 {
-	DEBUGP("Inititalizing usbcmd_gen_init\n");
+	DEBUGP("Inititalizing usbcmd_gen_init\n\r");
 	/* setup FLASH write support for environment storage */
 	flash_init();
 	
diff --git a/firmware/src/os/wdt.c b/firmware/src/os/wdt.c
index 99d86a1..73c6d59 100644
--- a/firmware/src/os/wdt.c
+++ b/firmware/src/os/wdt.c
@@ -30,12 +30,14 @@
 #undef  WDT_DEBUG
 #endif/*WDT_DEBUG*/
 
+#define WDT_DEBUG
+
 static void wdt_irq(u_int32_t sr)
 {
 	if (sr & 1)
-		DEBUGPCRF("================> WATCHDOG EXPIRED !!!!!");
+		AT91F_DBGU_Frame("================> WATCHDOG EXPIRED !!!!!\n\r");
 	if (sr & 2)
-		DEBUGPCRF("================> WATCHDOG ERROR !!!!!");
+		AT91F_DBGU_Frame("================> WATCHDOG ERROR !!!!!\n\r");
 }
 
 void wdt_restart(void)
diff --git a/firmware/src/simtrace/iso7816_uart.c b/firmware/src/simtrace/iso7816_uart.c
index 52522c0..1334d03 100644
--- a/firmware/src/simtrace/iso7816_uart.c
+++ b/firmware/src/simtrace/iso7816_uart.c
@@ -23,6 +23,7 @@
 #include <AT91SAM7.h>
 #include <lib_AT91SAM7.h>
 #include <openpcd.h>
+#include <asm/system.h>
 
 #include <simtrace_usb.h>
 
@@ -125,6 +126,20 @@ static const u_int8_t di_table[] = {
 	12, 20, 2, 4, 8, 16, 32, 64,
 };
 
+void iso_uart_report_errors(void) 
+{
+	static unsigned lastOverrun = 0, lastParity = 0, lastFrame = 0;
+	if (isoh.stats.overrun != lastOverrun) {
+		DEBUGPCR("UART overrun: %u", lastOverrun = isoh.stats.overrun);
+	}
+	if (isoh.stats.frame_err != lastFrame) {
+		DEBUGPCR("UART frame error: %u", lastFrame = isoh.stats.frame_err);
+	}
+	if (isoh.stats.parity_err != lastParity) {
+		DEBUGPCR("UART parity error: %u", lastParity = isoh.stats.parity_err);
+	}
+}
+
 void iso_uart_stats_dump(void)
 {
 	DEBUGPCRF("no_rctx: %u, rctx_sent: %u, rst: %u, pps: %u, bytes: %u, "
@@ -188,23 +203,29 @@ static void refill_rctx(struct iso7816_3_handle *ih)
 
 static void send_rctx(struct iso7816_3_handle *ih)
 {
+	static u_int16_t seq_num = 0;
 	struct req_ctx *rctx = ih->rctx;
+	unsigned flags;
 
 	if (!rctx)
 		return;
 
+	local_irq_save(flags);
 	/* Put Fi and Di into res[2] array */
 	ih->sh.res[0] = ih->fi;
 	ih->sh.res[1] = ih->di;
 
+	ih->sh.seq_num = seq_num++;
+	ih->sh.offset = 0;
+	ih->sh.tot_len = rctx->tot_len;
+
 	/* copy the simtrace header */
 	memcpy(rctx->data, &ih->sh, sizeof(ih->sh));
-
-	req_ctx_set_state(rctx, RCTX_STATE_UDP_EP2_PENDING);
-
-	memset(&ih->sh, 0, sizeof(ih->sh));
+	ih->sh.flags = 0;
 	ih->rctx = NULL;
+	local_irq_restore(flags);
 
+	req_ctx_set_state(rctx, RCTX_STATE_UDP_EP2_PENDING);
 	ih->stats.rctx_sent++;
 }
 
@@ -567,6 +588,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 &&
+		req_ctx_count(RCTX_STATE_UDP_EP2_PENDING) == 0 &&
+		(isoh.sh.flags & SIMTRACE_FLAG_ATR) == 0) {
+		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;
@@ -600,7 +638,7 @@ static __ramfunc void usart_irq(void)
 			isoh.stats.overrun++;
 	}
 
-	if (csr & AT91C_US_INACK) {
+	if (csr & AT91C_US_NACK) {
 		/* we would have sent a NACK if INACK was not set */
 		usart->US_CR |= AT91C_US_RSTNACK;
 	}
@@ -609,10 +647,13 @@ static __ramfunc void usart_irq(void)
 /* handler for the RST input pin state change */
 static void reset_pin_irq(u_int32_t pio)
 {
+	// When ever reset occured, we make sure to flush out any populated req_ctx
 	if (!AT91F_PIO_IsInputSet(AT91C_BASE_PIOA, pio)) {
+		iso_uart_flush();
 		DEBUGPCR("nRST");
 		set_state(&isoh, ISO7816_S_RESET);
 	} else {
+		iso_uart_flush();
 		DEBUGPCR("RST");
 		set_state(&isoh, ISO7816_S_WAIT_ATR);
 		isoh.stats.rst++;
@@ -675,6 +716,8 @@ 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 */
@@ -698,7 +741,8 @@ void iso_uart_init(void)
 	/* 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_1_BIT |
-			AT91C_US_CKLO | AT91C_US_INACK;
+			AT91C_US_CKLO | AT91C_US_INACK |
+			AT91C_US_OVER;
 
 	/* Disable all interrupts */
 	usart->US_IDR = 0xff;
diff --git a/firmware/src/simtrace/iso7816_uart.h b/firmware/src/simtrace/iso7816_uart.h
index beb0b23..969e6f9 100644
--- a/firmware/src/simtrace/iso7816_uart.h
+++ b/firmware/src/simtrace/iso7816_uart.h
@@ -1,8 +1,15 @@
+#ifndef SIMTRACE_ISO7816_UART_H
+#define SIMTRACE_ISO7816_UART_H
 
 struct simtrace_stats *iso_uart_stats_get(void);
+void iso_uart_report_errors(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_uart_init(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 6f82d96..2bdf8d5 100644
--- a/firmware/src/simtrace/main_simtrace.c
+++ b/firmware/src/simtrace/main_simtrace.c
@@ -134,10 +134,37 @@ static int simtrace_usb_in(struct req_ctx *rctx)
 		req_ctx_set_state(rctx, RCTX_STATE_FREE);
 		break;
 	}
+	return 0;
+}
+
+static volatile unsigned SPUIRQ_PC, SPUIRQ_COUNT = 0;
+
+void check_spurious_irq() {
+	static unsigned last_count = 0;
+	if (last_count != SPUIRQ_COUNT) {
+		DEBUGPCR("SPURRIOUS IRQ %i [Old PC = %08X]", SPUIRQ_COUNT, SPUIRQ_PC);
+		last_count = SPUIRQ_COUNT;
+	}
+}
+
+void custom_spurious_handler(unsigned previous_pc) {
+	unsigned flags;
+	local_irq_save(flags);
+	SPUIRQ_PC = previous_pc;
+	SPUIRQ_COUNT++;
+	local_irq_restore(flags);
+}
+
+void custom_spurious_entry(void) {
+	register unsigned *previous_pc asm("r0");
+	asm("ADD R1, SP, #16; LDR R0, [R1]");
+	custom_spurious_handler(previous_pc);
 }
 
 void _init_func(void)
 {
+	AT91C_BASE_AIC->AIC_SPU = (int)custom_spurious_entry;
+
 	/* low-level hardware initialization */
 	pio_irq_init();
 	iso_uart_init();
@@ -218,6 +245,8 @@ int _main_dbgu(char key)
 
 void _main_func(void)
 {
+  	static unsigned loopLow = 0, loopHigh = 0;
+
 	/* first we try to get rid of pending to-be-sent stuff */
 	usb_out_process();
 
@@ -225,4 +254,15 @@ void _main_func(void)
 	usb_in_process();
 
 	udp_unthrottle();
+
+       	if ((loopLow & 0xFFFF) == 0) {
+		DEBUGPCR("Heart beat %08X", loopHigh++);
+	}
+	if ((loopLow & 0x3F) == 0) {
+		iso_uart_idleflush();
+	}
+	loopLow++;
+
+	iso_uart_report_errors();
+	check_spurious_irq();
 }
diff --git a/firmware/src/simtrace/sim_switch.c b/firmware/src/simtrace/sim_switch.c
index faf48f0..aca8704 100644
--- a/firmware/src/simtrace/sim_switch.c
+++ b/firmware/src/simtrace/sim_switch.c
@@ -59,10 +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
+		iso_uart_flush();
+	} else {
 		DEBUGPCR("VCC_PHONE on");
+	}
 }
 
 void sim_switch_init(void)