Fast SIM cards loosing bytes

Min Xu mxu at sanjole.com
Fri Jan 24 21:24:49 UTC 2014


It's incompatible because due to the way the USB peripheral transfer the
data out to the host, there is NO mechanism to indicate exactly where the
next USB transaction should start.  Therefore, when the data is flowing
very fast, every 64 byte (the bus width) write to the USB peripheral will
be concatenated together, resulting in the host read to combine multiple
req_ctx buffers.

The way I solved it is by adding fields to the simtrace_hdr structure:
(the + indicates the new fields).  This allows me on the host to keep track
if / where a new req_ctx begins in the received usb data.

 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));


Since the laptop that I was using to create multiple commits is not
available at the moment, I am attaching the   git  diff   from the
repository (instead of from my last commits in previous email).  Meaning
this is a single diff file.

For the host software, the basic change in the process_usb_msg function.
My implementation is a naive implementation using a much larger buffer to
copy data to, where if a req_ctx is split across multiple usb transfers, it
will join those fragments then process:

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)) */;

static char usb_buffer[66000];
static unsigned usb_remainder = 0;

int process_usb_msg(uint8_t *buf, int len) {
    struct simtrace_hdr *sh;
    uint8_t *payload;
    int payload_len, sh_offset, i;

    sendto(s, buf, len, 0, (struct sockaddr*)&destUSB, sizeof(struct
sockaddr_in));

    memcpy(usb_buffer + usb_remainder, buf, len);
    len += usb_remainder;
    usb_remainder = len;
    sh_offset = 0;
    sh = usb_buffer;
    payload = (char*)sh + sizeof(struct simtrace_hdr);
    payload_len = sh->tot_len - sizeof(struct simtrace_hdr);
    if (usb_remainder < sh->tot_len)
        sh = NULL;

    while (sh) {
        switch (sh->cmd) {
            case SIMTRACE_MSGT_DATA:
                /* special treatment for ATR */
                if (sh->flags & SIMTRACE_FLAG_ATR) {
                    apdu_split_reset(as);
                    break;
                }
                if (sh->flags & SIMTRACE_FLAG_PPS_FIDI) {
                    module_out("PPS(Fi=%u/Di=%u)\n",
                        sh->res[0], sh->res[1]);
                }
                /* everything else goes into APDU splitter */
                apdu_split_in(as, payload, payload_len);
        #if 0
                /* If waiting time has expired, signal explicit boundary */
                if (sh->flags & SIMTRACE_FLAG_WTIME_EXP)
                    apdu_split_boundary(as);
        #endif
                break;
            case SIMTRACE_MSGT_RESET:
            default:
                module_out("unknown simtrace msg type 0x%02x\n", sh->cmd);
                break;
        }
        sh_offset += sh->tot_len;
        sh = usb_buffer + sh_offset;
        if (sh_offset < len) {
            if (sh_offset + sh->tot_len > len) {
                memcpy(usb_buffer, sh, len - sh_offset);
                usb_remainder = len - sh_offset;
                sh = NULL;
            } else {
                payload = (char*)sh + sizeof(struct simtrace_hdr);
                payload_len = sh->tot_len - sizeof(struct simtrace_hdr);
            }
        } else {
            sh = NULL;
            usb_remainder = 0;
        }
    }
    return 0;
}





On Fri, Jan 24, 2014 at 11:06 AM, Dean Chester <dean.g.chester at gmail.com>wrote:

> Hi min,
>
> How do you mean that they would be incompatible with early firmware based
> tracers?
>
> I'd be interested in using your firmware if you were to make it available,
> I'm also working on a more user friendly host software.
>
> Kind regards
>
> Dean Chester.
>
> Sent from my iPad
>
> On 24 Jan 2014, at 19:41, Min Xu <mxu at sanjole.com> wrote:
>
> Hi Holger
>
> To give you an update.  As far as I can tell, the changes I have
> submitted, with the addition of a change (on my local system) where the USB
> protocol has been modified to remove USB overrun, is stable so I can
> monitor iPhone 5S as well as Galaxy S4 continuously.  If anyone asks, I
> would be happy to send them the USB protocol changes.  However, it will be
> INCOMPATIBLE with earlier firmware based SIMTrace boards.
>
> I am making changes to the host program (on Windows) to monitor multiple
> SIMTrace devices, and am interested to know if anyone else is working on
> something similar.
>
> Best Regards
>
>
> Date: Wed, 22 Jan 2014 21:27:31 +0100
>> From: Holger Hans Peter Freyther <holger at freyther.de>
>> To: Dean Chester <dean.g.chester at gmail.com>
>> Cc: "simtrace at lists.osmocom.org" <simtrace at lists.osmocom.org>
>> Subject: Re: Fast SIM cards loosing bytes
>> Message-ID: <20140122202731.GL15138 at xiaoyu.lan>
>> Content-Type: text/plain; charset=us-ascii
>>
>> On Mon, Jan 13, 2014 at 08:35:05AM +0000, Dean Chester wrote:
>> > Hi,
>> >
>> > I see earlier in the archives that reducing the buffer size in the
>> firmware will resolve this. Hasn't this already been done though in new
>> releases of the firmware? As I have pulled the latest version from the
>> openpcd git repo and built it. If not what have people reduced the buffer
>> size to? And has it had any fixes?
>>
>> The closest are the patches that Min Xu sent. They require some
>> clean-ups and review (specially the code changing the interrupt
>> handling).
>>
>> holger
>>
>>
>>
>> ------------------------------
>>
>> _______________________________________________
>> simtrace mailing list
>> simtrace at lists.osmocom.org
>> https://lists.osmocom.org/mailman/listinfo/simtrace
>>
>>
>> End of simtrace Digest, Vol 33, Issue 4
>> ***************************************
>>
>
>
-------------- next part --------------
An HTML attachment was scrubbed...
URL: <http://lists.osmocom.org/pipermail/simtrace/attachments/20140124/d5b95645/attachment.html>
-------------- next part --------------
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 at hmw-consulting.de>\n\r"
+	debugp("\n\r");
+	debugp("(C) 2006-2011 by Harald Welte <hwelte at 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)


More information about the simtrace mailing list