WizziDebug.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "WizziDebug.h"
00004 #include <stdio.h>
00005 #include <string.h>
00006 #include <stdarg.h>
00007 
00008 #define DBG_MAX_STRING_SIZE     (256)
00009 #define TX_BUF_SIZE             (512)
00010 
00011 static kal_buf_circ_handle_t           g_cbuf;
00012 static kal_buf_circ_static_handle_t    g_cbuf_h;
00013 static kal_buf_circ_static_buffer_t(g_cbuf_b, TX_BUF_SIZE, sizeof(uint8_t));
00014 static char                     g_dbg_msg[DBG_MAX_STRING_SIZE];
00015 static Thread                   g_dbg_thread(osPriorityLow, 512, NULL, "wizzidbg");
00016 static RawSerial*               g_dbg_serial;
00017 static Mutex                    g_dbg_ressource;
00018 static Semaphore                g_dbg_print(0);
00019 static uint32_t                 g_dbg_nb_mallocs;
00020 static PinName                  __attribute__((unused)) g_dbg_led;
00021 static uint32_t                 g_dbg_missing;
00022 static void                     (*g_dbg_rx_callback)(char c) = NULL;
00023 
00024 void dbg_print_thread();
00025 
00026 static void dbg_serial_rx_isr(void)
00027 {
00028     if (!g_dbg_rx_callback)
00029     {
00030         PRINT("%c", g_dbg_serial->getc());
00031         return;
00032     }
00033     
00034     // Loop just in case more than one character is in UART's receive FIFO buffer
00035     while (g_dbg_serial->readable())
00036     {
00037         g_dbg_rx_callback(g_dbg_serial->getc());
00038     }
00039 }
00040 
00041 // Redefine mbed error function
00042 void error(const char* format, ...)
00043 {
00044     char buf[DBG_MAX_STRING_SIZE];
00045     
00046     va_list args;
00047     va_start(args, format);
00048     vsprintf(buf, format, args);
00049     va_end(args);
00050     
00051     ASSERT(false, buf);
00052 }
00053 
00054 void dbg_open(PinName led, PinName tx, PinName rx)
00055 {    
00056     //FPRINT("\r\n");
00057         
00058     g_dbg_nb_mallocs = 0;
00059     g_dbg_led = led;
00060     g_dbg_missing = 0;
00061     
00062     g_cbuf = &g_cbuf_h;
00063     kal_buf_circ_create_static(&g_cbuf_h, g_cbuf_b, TX_BUF_SIZE, sizeof(char));
00064     
00065     g_dbg_serial = new RawSerial(tx, rx, 115200);
00066     g_dbg_serial->format(8, SerialBase::None, 1);
00067     g_dbg_serial->attach(callback(&dbg_serial_rx_isr), Serial::RxIrq);
00068     
00069     g_dbg_thread.start(dbg_print_thread);
00070 }
00071 
00072 // Destructor
00073 void dbg_close( void )
00074 {
00075     FPRINT("\r\n");
00076     g_dbg_thread.terminate();
00077     delete g_dbg_serial;
00078 }
00079 
00080 void dbg_set_led(PinName led, PinName dummy1, PinName dummy2)
00081 {
00082     FPRINT("\r\n");
00083     g_dbg_led = led;
00084 }
00085 
00086 static void dbg_add_to_buf(char* msg, int size)
00087 {
00088     uint8_t c;
00089     
00090 #ifdef __FORCE_FLUSH__
00091     for (int i = 0; i < size; i++)
00092     {
00093         g_dbg_serial->putc(msg[i]);
00094     }
00095 #else
00096     if(size > 0)
00097     {
00098         if (kal_buf_circ_space(g_cbuf) < size)
00099         {
00100     #ifdef __FLUSH_IF_FULL__
00101             // Flush just what is needed
00102             do {
00103                 kal_buf_circ_pop(g_cbuf, &c);
00104                 g_dbg_serial->putc(c);
00105             } while (kal_buf_circ_space(g_cbuf) < size);
00106             
00107             kal_buf_circ_put(g_cbuf, (uint8_t*)msg, size);
00108     #else
00109             // Discard
00110             g_dbg_missing++;
00111     #endif
00112         }
00113         else
00114         {
00115             // add
00116             kal_buf_circ_put(g_cbuf, (uint8_t*)msg, size);
00117         }
00118         
00119         // Allow printing
00120         g_dbg_print.release();
00121     }
00122 #endif
00123 }
00124 
00125 // Asserts and trap processor.
00126 void dbg_assert(bool test, const char* format, ...)
00127 {
00128     if (test) return;
00129         
00130     int assert_size;
00131     
00132     assert_size = sprintf(g_dbg_msg, "ASSERT ");
00133     
00134     // expand assert string
00135     va_list args;
00136     va_start(args, format);
00137     vsprintf(g_dbg_msg+assert_size, format, args);
00138     va_end(args);
00139         
00140     dbg_flush();
00141     
00142     g_dbg_serial->printf(g_dbg_msg);
00143         
00144 #ifdef __REBOOT_ON_ASSERT__
00145     ThisThread::sleep_for(1000);
00146     NVIC_SystemReset();
00147 #else
00148     DigitalOut* dbg_led = NULL;
00149     if (g_dbg_led != NC)
00150     {
00151         dbg_led = new DigitalOut(g_dbg_led);
00152     }
00153     
00154     // Die...
00155     while(1)
00156     {
00157         if (dbg_led != NULL)
00158         {
00159             *(dbg_led) = !(*(dbg_led));
00160         }
00161         ThisThread::sleep_for(30);
00162         //wait_ms(30);
00163     }
00164 #endif
00165 }
00166 
00167 // Prints a message to the debug port.
00168 void dbg_print(const char* format, ...)
00169 {
00170     int size;
00171 
00172     va_list args;
00173     va_start(args, format);
00174     size = vsprintf(g_dbg_msg, format, args);
00175     va_end(args);
00176     
00177     dbg_add_to_buf(g_dbg_msg, size);
00178 }
00179 
00180 // Flush the pending debug messages.
00181 void dbg_flush( void )
00182 {
00183     uint8_t c;
00184     
00185     g_dbg_ressource.lock();
00186     while(!kal_buf_circ_empty(g_cbuf))
00187     {
00188         kal_buf_circ_pop(g_cbuf, &c);
00189         g_dbg_serial->putc(c);
00190     }
00191     g_dbg_ressource.unlock();
00192 }
00193 
00194 // Prints malloc parameters.
00195 void* dbg_malloc(size_t size, const char* f, uint32_t l)
00196 {
00197     void* a;
00198     a = malloc(size);
00199     g_dbg_nb_mallocs++;
00200     dbg_print("M @%08x %dB %s:%d (%d)\r\n", (uint32_t)a, size, f, l, g_dbg_nb_mallocs);
00201     return a;
00202 }
00203 
00204 // Prints realloc parameters.
00205 void* dbg_realloc(void* p, size_t size, const char* f, uint32_t l)
00206 {
00207     void* a;
00208     dbg_assert(((uint32_t)p >= RAM_START_ADDRESS) && ((uint32_t)p < RAM_END_ADDRESS), "Trying to realloc illegal address @08x\r\n", (uint32_t)p);
00209     a = realloc(p, size);
00210     dbg_print("R @%08x->@%08x %dB %s:%d\r\n", (uint32_t)p, (uint32_t)a, size, f, l);
00211     return a;
00212 }
00213 
00214 // Prints free parameters.
00215 void dbg_free(void* p, const char* f, uint32_t l)
00216 {
00217     g_dbg_nb_mallocs--;
00218     dbg_print("F @%08x %s:%d (%d)\r\n", (uint32_t)p, f, l, g_dbg_nb_mallocs);
00219     dbg_assert(((uint32_t)p >= RAM_START_ADDRESS) && ((uint32_t)p < RAM_END_ADDRESS), "Trying to free illegal address @08x\r\n", (uint32_t)p);
00220     free(p);
00221 }
00222 
00223 void dbg_print_data(const char* before, const char* format, uint8_t* data, uint8_t length, const char* after)
00224 {
00225     uint32_t total_len = 0;
00226     
00227     total_len += sprintf(g_dbg_msg, before);
00228     
00229     for (uint8_t i=0 ; i<length ; i++)
00230     {
00231         total_len += sprintf(&g_dbg_msg[total_len], format, data[i]);
00232         dbg_assert(total_len < DBG_MAX_STRING_SIZE, "Print data too long.\r\n");
00233     }
00234     
00235     total_len += sprintf(&g_dbg_msg[total_len], after);
00236     
00237     dbg_add_to_buf(g_dbg_msg, total_len);
00238 }
00239 
00240 
00241 // Thread for printing debug messages.
00242 void dbg_print_thread()
00243 {
00244     uint8_t c;
00245     
00246     FPRINT("(id:0x%08x)\r\n", osThreadGetId());
00247     
00248     while (true)
00249     {
00250         g_dbg_print.acquire();
00251         
00252         if (g_dbg_missing)
00253         {
00254             g_dbg_serial->printf("Missing %d traces!!!\r\n", g_dbg_missing);
00255             g_dbg_missing = 0;
00256         }
00257         
00258         while(!kal_buf_circ_empty(g_cbuf))
00259         {
00260             g_dbg_ressource.lock();
00261 
00262 #ifdef __FORCE_LINE_INTEGRITY__
00263             // Flush until EOL
00264             uint8_t byte;
00265             do {
00266                 kal_buf_circ_pop(g_cbuf, &c);
00267                 g_dbg_serial->putc(c);
00268             } while (byte != '\n' && !kal_buf_circ_empty(g_cbuf));
00269 #else
00270             // Flush characters one by one
00271             kal_buf_circ_pop(g_cbuf, &c);
00272             g_dbg_serial->putc(c);
00273 #endif
00274 
00275             g_dbg_ressource.unlock();
00276             ThisThread::yield();
00277         }
00278     }
00279 }
00280 
00281 void dbg_set_rx_callback(void (*rx_isr)(char c))
00282 {
00283     g_dbg_rx_callback = rx_isr;
00284 }