diff options
| -rw-r--r-- | openpcd/firmware/src/dbgu.c | 104 |
1 files changed, 101 insertions, 3 deletions
diff --git a/openpcd/firmware/src/dbgu.c b/openpcd/firmware/src/dbgu.c index 45b1a99..5c4390a 100644 --- a/openpcd/firmware/src/dbgu.c +++ b/openpcd/firmware/src/dbgu.c @@ -20,6 +20,7 @@ #include "openpcd.h" #include "led.h" #include "main.h" +#include <asm/system.h> #define USART_SYS_LEVEL 4 /*---------------------------- Global Variable ------------------------------*/ @@ -107,12 +108,15 @@ static void DBGU_irq_handler(void) } // end switch } +void dbgu_rb_init(void); //*---------------------------------------------------------------------------- //* \fn AT91F_DBGU_Init //* \brief This function is used to send a string through the DBGU channel (Very low level debugging) //*---------------------------------------------------------------------------- void AT91F_DBGU_Init(void) { + dbgu_rb_init(); + //* Open PIO for DBGU AT91F_DBGU_CfgPIO(); //* Enable Transmitter & receivier @@ -165,6 +169,7 @@ void AT91F_DBGU_Frame(char *buffer) for (len = 0; buffer[len] != '\0'; len++) { } + AT91F_US_SendFrame((AT91PS_USART) AT91C_BASE_DBGU, (unsigned char *)buffer, len, 0, 0); @@ -208,6 +213,99 @@ hexdump(const void *data, unsigned int len) return string; } +struct dbgu { + char buf[4096]; + char *next_inbyte; + char *next_outbyte; +}; +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); + } +} + +/* 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); +} + +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); +} + static char dbg_buf[256]; void debugp(const char *format, ...) { @@ -219,8 +317,8 @@ void debugp(const char *format, ...) dbg_buf[sizeof(dbg_buf)-1] = '\0'; //AT91F_DBGU_Frame(dbg_buf); - AT91F_DBGU_Printk(dbg_buf); + //AT91F_DBGU_Printk(dbg_buf); + dbgu_rb_append(dbg_buf, strlen(dbg_buf)); } -#endif - +#endif |