diff --git a/firmware/Makefile b/firmware/Makefile
index 3f7a2e9..55d8c2c 100644
--- a/firmware/Makefile
+++ b/firmware/Makefile
@@ -6,7 +6,7 @@
 # by Martin Thomas, Kaiserslautern, Germany 
 # <eversmith@heizung-thomas.de>
 #
-# based on the WinAVR makefile written by Eric B. Weddington, J�rg Wunsch, et al.
+# based on the WinAVR makefile written by Eric B. Weddington, Jörg Wunsch, et al.
 # Released to the Public Domain
 # Please read the make user manual!
 #
@@ -239,7 +239,7 @@ ADEFS += -D__$(SUBMDL)__
 #  -Wa,...:      tell GCC to pass this to the assembler.
 #    -adhlns...: create assembler listing
 #
-# Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
+# Flags for C and C++ (arm-none-eabi-gcc/arm-none-eabi-g++)
 CFLAGS = -g$(DEBUGF)
 CFLAGS += $(CDEFS) $(CINCS)
 CFLAGS += -O$(OPT)
@@ -263,7 +263,7 @@ CONLYFLAGS += -Wstrict-prototypes
 CONLYFLAGS += -Wmissing-declarations
 endif
 
-# flags only for C++ (arm-elf-g++)
+# flags only for C++ (arm-none-eabi-g++)
 # CPPFLAGS = -fno-rtti -fno-exceptions
 CPPFLAGS = 
 
@@ -351,12 +351,12 @@ LPC21ISP_CONTROL = -control
 
 # Define programs and commands.
 SHELL = sh
-CC = arm-elf-gcc
-CPP = arm-elf-g++
-OBJCOPY = arm-elf-objcopy
-OBJDUMP = arm-elf-objdump
-SIZE = arm-elf-size
-NM = arm-elf-nm
+CC = arm-none-eabi-gcc
+CPP = arm-none-eabi-g++
+OBJCOPY = arm-none-eabi-objcopy
+OBJDUMP = arm-none-eabi-objdump
+SIZE = arm-none-eabi-size
+NM = arm-none-eabi-nm
 REMOVE = rm -f
 COPY = cp
 
@@ -488,7 +488,7 @@ endif
 	@echo
 	@echo $(MSG_FLASH) $@
 	$(OBJCOPY) -O $(FORMAT) $< $@
-	
+
 # Create final output file (.bin) from ELF output file.
 %.bin: %.elf
 	@echo
diff --git a/firmware/Makefile.dfu b/firmware/Makefile.dfu
index 3c797ef..96a3e25 100644
--- a/firmware/Makefile.dfu
+++ b/firmware/Makefile.dfu
@@ -6,7 +6,7 @@
 # by Martin Thomas, Kaiserslautern, Germany 
 # <eversmith@heizung-thomas.de>
 #
-# based on the WinAVR makefile written by Eric B. Weddington, J�rg Wunsch, et al.
+# based on the WinAVR makefile written by Eric B. Weddington, Jörg Wunsch, et al.
 # Released to the Public Domain
 # Please read the make user manual!
 #
@@ -205,7 +205,7 @@ ADEFS += -D__$(SUBMDL)__
 #  -Wa,...:      tell GCC to pass this to the assembler.
 #    -adhlns...: create assembler listing
 #
-# Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
+# Flags for C and C++ (arm-none-eabi-gcc/arm-none-eabi-g++)
 CFLAGS += -g$(DEBUGF) -DBOARD=$(BOARD) 
 CFLAGS += $(CDEFS) $(CINCS)
 CFLAGS += -O$(OPT)
@@ -229,7 +229,7 @@ CONLYFLAGS += -Wstrict-prototypes
 CONLYFLAGS += -Wmissing-declarations
 endif
 
-# flags only for C++ (arm-elf-g++)
+# flags only for C++ (arm-none-eabi-g++)
 # CPPFLAGS = -fno-rtti -fno-exceptions
 CPPFLAGS = 
 
@@ -303,12 +303,12 @@ LPC21ISP_CONTROL = -control
 
 # Define programs and commands.
 SHELL = sh
-CC = arm-elf-gcc
-CPP = arm-elf-g++
-OBJCOPY = arm-elf-objcopy
-OBJDUMP = arm-elf-objdump
-SIZE = arm-elf-size
-NM = arm-elf-nm
+CC = arm-none-eabi-gcc
+CPP = arm-none-eabi-g++
+OBJCOPY = arm-none-eabi-objcopy
+OBJDUMP = arm-none-eabi-objdump
+SIZE = arm-none-eabi-size
+NM = arm-none-eabi-nm
 REMOVE = rm -f
 COPY = cp
 
@@ -440,7 +440,7 @@ endif
 	@echo
 	@echo $(MSG_FLASH) $@
 	$(OBJCOPY) -O $(FORMAT) $< $@
-	
+
 # Create final output file (.bin) from ELF output file.
 %.bin: %.elf
 	@echo
diff --git a/firmware/src/os/dbgu.c b/firmware/src/os/dbgu.c
index 113208e..22941f8 100644
--- a/firmware/src/os/dbgu.c
+++ b/firmware/src/os/dbgu.c
@@ -47,16 +47,18 @@
 
 //#define DEBUG_UNBUFFERED
 
+// #define ALLOW_INTERRUPT_LOOP asm("nop;nop;nop;nop;nop;nop;nop;nop;")
+
+#define ALLOW_INTERRUPT_LOOP
+
+// #define DEBUG_BUFFER_SIZE     (1 << 11)
+
+// 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 +71,8 @@ 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();
+	//	AT91F_DBGU_Ready();
+ while (!(AT91C_BASE_DBGU->DBGU_CSR & AT91C_US_TXEMPTY)) ;
 	// Jump in reset
 	pfct();
 }
@@ -86,7 +89,7 @@ static void DBGU_irq_handler(u_int32_t sr)
 	AT91F_DBGU_Get(&value);
 	switch (value) {
 	case '0':		//* info
-		AT91F_DBGU_Frame("Clear Pull up\n\r");
+		AT91F_DBGU_Printk("Clear Pull up\n\r");
 		// Set
 		udp_pullup_on();
 		break;
@@ -106,8 +109,8 @@ static void DBGU_irq_handler(u_int32_t sr)
 		break;
 	case '9':
 		AT91F_DBGU_Printk("Resetting SAM7\n\r");
-		AT91F_RSTSoftReset(AT91C_BASE_RSTC, AT91C_RSTC_PROCRST|
-				   AT91C_RSTC_PERRST|AT91C_RSTC_EXTRST);
+		//		AT91F_RSTSoftReset(AT91C_BASE_RSTC, AT91C_RSTC_PROCRST|
+		//		   AT91C_RSTC_PERRST|AT91C_RSTC_EXTRST);
 		break;
 	default:
 		if (_main_dbgu(value) < 0)
@@ -148,17 +151,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 +176,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++) {
-	}
-
-	AT91F_US_SendFrame((AT91PS_USART) AT91C_BASE_DBGU, 
-			   (unsigned char *)buffer, len, 0, 0);
-
+  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;
+  }
+  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 +247,172 @@ 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)
-{
-	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);
-	}
-
-	memcpy(pos, data, len);
+  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;
+  }
+  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;
-	
-	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;
-	}
-	__dbgu_rb_append(data_cur, len);
-
-	local_irq_restore(flags);
+  /* 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(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) {
+    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;
+    }
+  }
+  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..28d5638 100644
--- a/firmware/src/os/dbgu.h
+++ b/firmware/src/os/dbgu.h
@@ -24,8 +24,8 @@
 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..6f9f87d 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,
@@ -137,99 +139,105 @@ void udp_unthrottle(void)
 	pUDP->UDP_IER = AT91C_UDP_EPINT1;
 }
 
-static int __udp_refill_ep(int ep)
+extern struct req_ctx req_extra;
+
+int udp_refill_ep(int ep)
 {
 	u_int16_t i;
 	AT91PS_UDP pUDP = upcd.pUdp;
 	struct req_ctx *rctx;
 	unsigned int start, end;
+	unsigned long flags;
 
 	/* If we're not configured by the host yet, there is no point
 	 * in trying to send data to it... */
 	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) {
+	if (atomic_read(&upcd.ep[ep].pkts_in_transit) == 2 ||
+	    (pUDP->UDP_CSR[ep] & AT91C_UDP_TXPKTRDY)) {
 		return -EBUSY;
 	}
 
+	pUDP->UDP_IDR |= 1 << ep;
+
+	//	upcd.ep[ep].incomplete.rctx = &req_extra;
+	//	upcd.ep[ep].incomplete.bytes_sent = 0;
+
 	/* 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) {
 		rctx = upcd.ep[ep].incomplete.rctx;
 		start = upcd.ep[ep].incomplete.bytes_sent;
+		/*		DEBUGPCR("Resume USBTx %08X, Len = %04u, Head 4/Tail 4 bytes: %02X %02X %02X %02X / %02X %02X %02X %02X",
+			 rctx->data, rctx->tot_len,
+			 rctx->data[4], rctx->data[5], rctx->data[6], rctx->data[7],
+			 rctx->data[rctx->tot_len - 4], rctx->data[rctx->tot_len - 3], 
+			 rctx->data[rctx->tot_len - 2], rctx->data[rctx->tot_len - 1]); */
 	} else {
 		/* get pending rctx and start transmitting from zero */
 		rctx = req_ctx_find_get(0, epstate[ep].state_pending, 
 					epstate[ep].state_busy);
-		if (!rctx)
-			return 0;
+		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("USBTx %08X, Len = %04u, Head 4/Tail 4 bytes: %02X %02X %02X %02X / %02X %02X %02X %02X",
+			 rctx->data, rctx->tot_len,
+			 rctx->data[4], rctx->data[5], rctx->data[6], rctx->data[7],
+			 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)
+	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 */
-		pUDP->UDP_CSR[ep] |= AT91C_UDP_TXPKTRDY;
-	}
+	  pUDP->UDP_FDR[ep] = rctx->data[i];
 
-	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));
-		upcd.ep[ep].incomplete.rctx = NULL;
-		req_ctx_put(rctx);
+	if (atomic_inc_return(&upcd.ep[ep].pkts_in_transit) == 1)
+	  pUDP->UDP_CSR[ep] |= AT91C_UDP_TXPKTRDY;
+	
+	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);
+	  upcd.ep[ep].incomplete.rctx = rctx;
+	  upcd.ep[ep].incomplete.bytes_sent += end - start;
 	}
-
+	pUDP->UDP_IER |= 1 << ep;
 	return 1;
 }
 
-int udp_refill_ep(int ep)
+static void udp_irq(void)
 {
-	unsigned long flags;
-	int ret;
+	u_int32_t csr;
+	AT91PS_UDP pUDP;
+	AT91_REG isr;
 
-	local_irq_save(flags);
-	ret = __udp_refill_ep(ep);
-	local_irq_restore(flags);
+	led_switch(1, 1);
 
-	return ret;
-}
+	// Enable Interrupt Nesting
+	asm("MRS R0, CPSR; AND R0, R0, #0xFFFFFF3F; MSR CPSR_c, R0; "); // Clear IRQ disable bit and FIQ disable bit
 
-static void udp_irq(void)
-{
-	u_int32_t csr;
-	AT91PS_UDP pUDP = upcd.pUdp;
-	AT91_REG isr = pUDP->UDP_ISR;
+	pUDP = upcd.pUdp;
+	isr = pUDP->UDP_ISR;
 
 	DEBUGI("udp_irq(imr=0x%04x, isr=0x%04x, state=%d): ", 
 		pUDP->UDP_IMR, isr, upcd.state);
@@ -359,7 +367,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 +382,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) {
@@ -413,6 +421,8 @@ cont_ep2:
 out:
 	DEBUGI("END\r\n");
 	AT91F_AIC_ClearIt(AT91C_BASE_AIC, AT91C_ID_UDP);
+
+	led_switch(1, 0);
 }
 
 void udp_pullup_on(void)
diff --git a/firmware/src/os/req_ctx.c b/firmware/src/os/req_ctx.c
index cc8d57b..dedae69 100644
--- a/firmware/src/os/req_ctx.c
+++ b/firmware/src/os/req_ctx.c
@@ -27,7 +27,11 @@
 #include "../openpcd.h"
 
 /* FIXME: locking, FIFO order processing */
+#define NUM_RCTX_SMALL 0
+#define NUM_RCTX_LARGE 19
 
+// 8 * 128 + 4 * 2048 =>   18 * 270 + 2 * 2048
+/*
 #if defined(__AT91SAM7S64__) || defined(RUN_FROM_RAM)
 #define NUM_RCTX_SMALL 16
 #define NUM_RCTX_LARGE 1
@@ -35,71 +39,172 @@
 #define NUM_RCTX_SMALL 8
 #define NUM_RCTX_LARGE 4
 #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];
+struct req_ctx req_extra;
+
+static struct req_ctx *req_ctx_queues[RCTX_STATE_COUNT], *req_ctx_tails[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;
+	  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;
+	}
+	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;
+
+	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;
 	local_irq_restore(flags);
 }
-
+/*
+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");
+  }
+}
+*/
 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;
+
+	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;
+	local_irq_restore(intcFlags);
 }
 
 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 < 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;
+
+	for (i = RCTX_STATE_FREE + 1; i < RCTX_STATE_COUNT; i++) {
+	  req_ctx_queues[i] = req_ctx_tails[i] = NULL;
 	}
 
-	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];
+	req_extra.prev = req_extra.next = NULL;
+	req_extra.size = RCTX_SIZE_LARGE;
+	req_extra.tot_len = 256;
+	req_extra.data = rctx_data_TEST;
+	req_extra.state = 0;
+	for (i = 0; i < 256; i++) {
+	  req_extra.data[i] = i;
 	}
 }
diff --git a/firmware/src/os/req_ctx.h b/firmware/src/os/req_ctx.h
index 94b5c5a..b655622 100644
--- a/firmware/src/os/req_ctx.h
+++ b/firmware/src/os/req_ctx.h
@@ -1,8 +1,11 @@
 #ifndef _REQ_CTX_H
 #define _REQ_CTX_H
 
-#define RCTX_SIZE_LARGE	2048
-#define RCTX_SIZE_SMALL	128
+//#define RCTX_SIZE_LARGE	2048
+//#define RCTX_SIZE_SMALL	128
+
+#define RCTX_SIZE_LARGE       1000
+#define RCTX_SIZE_SMALL       270
 
 #define MAX_HDRSIZE	sizeof(struct openpcd_hdr)
 #define MAX_REQSIZE	(64-MAX_HDRSIZE)
@@ -13,13 +16,17 @@
 #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
@@ -39,6 +46,28 @@ struct req_ctx {
 #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);
diff --git a/firmware/src/os/system_irq.c b/firmware/src/os/system_irq.c
index dc787eb..ca04207 100644
--- a/firmware/src/os/system_irq.c
+++ b/firmware/src/os/system_irq.c
@@ -33,6 +33,8 @@ static sysirq_hdlr *sysirq_hdlrs[AT91SAM7_SYSIRQ_COUNT];
 static void sys_irq(void)
 {
 	u_int32_t sr;
+	led_toggle(2);
+	led_toggle(1);
 	DEBUGP("sys_irq ");
 
 	/* Somehow Atmel decided to do really stupid interrupt sharing
diff --git a/firmware/src/os/usbcmd_generic.c b/firmware/src/os/usbcmd_generic.c
index 77b992f..defdcc6 100644
--- a/firmware/src/os/usbcmd_generic.c
+++ b/firmware/src/os/usbcmd_generic.c
@@ -85,9 +85,9 @@ static int gen_usb_rx(struct req_ctx *rctx)
 		break;
 		
 	case OPENPCD_CMD_RESET:
-		DEBUGP("CMD_RESET\n");
-		AT91F_RSTSoftReset(AT91C_BASE_RSTC, AT91C_RSTC_PROCRST|
-				   AT91C_RSTC_PERRST|AT91C_RSTC_EXTRST);
+		AT91F_DBGU_Frame("CMD_RESET\n");
+		//		AT91F_RSTSoftReset(AT91C_BASE_RSTC, AT91C_RSTC_PROCRST|
+		//		   AT91C_RSTC_PERRST|AT91C_RSTC_EXTRST);
 		break;
 
 	case OPENPCD_CMD_GET_VERSION:
@@ -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..52d3292 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..abf90c8 100644
--- a/firmware/src/simtrace/iso7816_uart.c
+++ b/firmware/src/simtrace/iso7816_uart.c
@@ -124,7 +124,13 @@ static const u_int8_t di_table[] = {
 	0, 1, 2, 4, 8, 16, 32, 64,
 	12, 20, 2, 4, 8, 16, 32, 64,
 };
+void iso_uart_report_overrun(void) {
+  static unsigned lastOverrun = 0;
+  if (isoh.stats.overrun != lastOverrun) {
+    DEBUGPCR("UART overrun: %u", lastOverrun = isoh.stats.overrun);
+  }
 
+}
 void iso_uart_stats_dump(void)
 {
 	DEBUGPCRF("no_rctx: %u, rctx_sent: %u, rst: %u, pps: %u, bytes: %u, "
@@ -569,10 +575,17 @@ void iso7816_wtime_expired(void)
 
 static __ramfunc void usart_irq(void)
 {
-	u_int32_t csr = usart->US_CSR;
+	u_int32_t csr;
 	u_int8_t octet;
 
 	//DEBUGP("USART IRQ, CSR=0x%08x\n", csr);
+	
+	// led_switch(1, 1);
+
+	// Enable Interrupt Nesting
+	asm("MRS R0, CPSR; AND R0, R0, #0xFFFFFF3F; MSR CPSR_c, R0; "); // Clear IRQ disable bit and FIQ disable bit
+	
+	csr =  usart->US_CSR;
 
 	if (csr & AT91C_US_RXRDY) {
 		/* at least one character received */
@@ -588,7 +601,7 @@ static __ramfunc void usart_irq(void)
 	if (csr & (AT91C_US_PARE|AT91C_US_FRAME|AT91C_US_OVRE)) {
 		u_int8_t nb_err = usart->US_NER;
 		/* FIXME: some error has occurrerd */
-		//DEBUGP("NER=%02x ", nb_err);
+		DEBUGP("NER=%02x ", nb_err);
 		/* clear the status */
 		usart->US_CR |= AT91C_US_RSTSTA;
 
@@ -604,6 +617,8 @@ static __ramfunc void usart_irq(void)
 		/* we would have sent a NACK if INACK was not set */
 		usart->US_CR |= AT91C_US_RSTNACK;
 	}
+
+	// led_switch(1, 0);
 }
 
 /* handler for the RST input pin state change */
@@ -675,6 +690,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 */
diff --git a/firmware/src/simtrace/main_simtrace.c b/firmware/src/simtrace/main_simtrace.c
index 6f82d96..031f06d 100644
--- a/firmware/src/simtrace/main_simtrace.c
+++ b/firmware/src/simtrace/main_simtrace.c
@@ -216,8 +216,11 @@ int _main_dbgu(char key)
 	return -EINVAL;
 }
 
+static unsigned loopCount = 0;
+
 void _main_func(void)
 {
+  struct req_ctx *rec1, *rec2;
 	/* first we try to get rid of pending to-be-sent stuff */
 	usb_out_process();
 
@@ -225,4 +228,10 @@ void _main_func(void)
 	usb_in_process();
 
 	udp_unthrottle();
+
+       	if ((loopCount & 0xFFFF) == 0) 
+	  DEBUGPCR("Main loop %08X", loopCount);
+	loopCount++;
+
+	iso_uart_report_overrun();
 }
