Diff: WizziDebug.cpp

Revision:

0:1d3ed1850649

Child:

2:9e2ab0547cd0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/WizziDebug.cpp	Wed Apr 26 12:31:00 2017 +0000
@@ -0,0 +1,283 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "WizziDebug.h"
+#include "CBuffer.h"
+#include <stdio.h>
+#include <string.h>
+#include <stdarg.h>
+
+#define DBG_MAX_STRING_SIZE     (150)
+
+static CBuffer<uint8_t, 2048>   g_dbg_buf;
+static char                     g_dbg_msg[DBG_MAX_STRING_SIZE];
+static Thread                   g_dbg_thread(osPriorityIdle, 512, NULL);
+static RawSerial*               g_dbg_serial;
+static Mutex                    g_dbg_ressource;
+static Semaphore                g_dbg_print(0);
+static uint32_t                 g_dbg_nb_mallocs;
+static PinName                  g_dbg_led;
+static uint32_t                 g_dbg_missing;
+
+void dbg_print_thread();
+
+// Redefine mbed error function
+void error(const char* format, ...)
+{
+    char buf[DBG_MAX_STRING_SIZE];
+    
+    va_list args;
+    va_start(args, format);
+    vsprintf(buf, format, args);
+    va_end(args);
+    
+    ASSERT(false, buf);
+}
+
+void dbg_open(PinName led, PinName tx, PinName rx)
+{    
+    //FPRINT("\r\n");
+        
+    g_dbg_nb_mallocs = 0;
+    g_dbg_led = led;
+    g_dbg_missing = 0;
+    
+    g_dbg_serial = new RawSerial(tx, rx, 115200);
+    g_dbg_serial->format(8, SerialBase::None, 1);
+    
+    g_dbg_thread.start(dbg_print_thread);
+}
+
+// Destructor
+void dbg_close( void )
+{
+    FPRINT("\r\n");
+    g_dbg_thread.terminate();
+    delete g_dbg_serial;
+}
+
+void dbg_set_led(PinName led, PinName dummy1, PinName dummy2)
+{
+    FPRINT("\r\n");
+    g_dbg_led = led;
+}
+
+static void dbg_add_to_buf(char* msg, int size)
+{
+    if(size > 0)
+    {
+        if (g_dbg_buf.remaining_space() < size)
+        {
+#ifdef __FLUSH_IF_FULL__
+            // Flush just what is needed
+            do {
+                g_dbg_serial->putc(g_dbg_buf.pop());
+            } while (g_dbg_buf.remaining_space() < size);
+            
+            g_dbg_buf.add((uint8_t*)msg, size);
+#else
+            // Discard
+            g_dbg_missing++;
+#endif
+        }
+        else
+        {
+            // add
+            g_dbg_buf.add((uint8_t*)msg, size);
+        }
+        
+        // Allow printing
+        g_dbg_print.release();
+    }
+}
+
+// Asserts and trap processor.
+void dbg_assert(bool test, const char* format, ...)
+{
+    if (test) return;
+        
+    char* assert_msg = "ASSERT ";
+    int assert_size;
+    int size;
+    
+    assert_size = sprintf(g_dbg_msg, assert_msg);
+    
+    // expand assert string
+    va_list args;
+    va_start(args, format);
+    size = vsprintf(g_dbg_msg+assert_size, format, args);
+    va_end(args);
+        
+    dbg_flush();
+    
+    g_dbg_serial->printf(g_dbg_msg);
+        
+#ifdef __REBOOT_ON_ASSERT__
+    Thread::wait(1000);
+    NVIC_SystemReset();
+#else
+    DigitalOut* dbg_led = NULL;
+    if (g_dbg_led != NC)
+    {
+        dbg_led = new DigitalOut(g_dbg_led);
+    }
+    
+    // Die...
+    while(1)
+    {
+        if (dbg_led != NULL)
+        {
+            *(dbg_led) = !(*(dbg_led));
+        }
+        Thread::wait(30);
+        //wait_ms(30);
+    }
+#endif
+}
+
+// Prints a message to the debug port.
+void dbg_print(const char* format, ...)
+{
+    // Critical section
+    g_dbg_ressource.lock();
+
+    int size;
+
+    va_list args;
+    va_start(args, format);
+    size = vsprintf(g_dbg_msg, format, args);
+    va_end(args);
+    
+    dbg_add_to_buf(g_dbg_msg, size);
+    
+    // End of critical section
+    g_dbg_ressource.unlock();
+}
+
+// Flush the pending debug messages.
+void dbg_flush( void )
+{
+    g_dbg_ressource.lock();
+    while(!g_dbg_buf.empty())
+    {
+        g_dbg_serial->putc(g_dbg_buf.pop());
+    }
+    g_dbg_ressource.unlock();
+}
+
+// Prints malloc parameters.
+void* dbg_malloc(size_t size, const char* f, uint32_t l)
+{
+    void* a;
+    a = malloc(size);
+    g_dbg_nb_mallocs++;
+    dbg_print("M @%08x %dB %s:%d (%d)\r\n", (uint32_t)a, size, f, l, g_dbg_nb_mallocs);
+    return a;
+}
+
+// Prints realloc parameters.
+void* dbg_realloc(void* p, size_t size, const char* f, uint32_t l)
+{
+    void* a;
+    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);
+    a = realloc(p, size);
+    dbg_print("R @%08x->@%08x %dB %s:%d\r\n", (uint32_t)p, (uint32_t)a, size, f, l);
+    return a;
+}
+
+// Prints free parameters.
+void dbg_free(void* p, const char* f, uint32_t l)
+{
+    g_dbg_nb_mallocs--;
+    dbg_print("F @%08x %s:%d (%d)\r\n", (uint32_t)p, f, l, g_dbg_nb_mallocs);
+    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);
+    free(p);
+}
+
+void dbg_print_data(const char* before, const char* format, uint8_t* data, uint8_t length, const char* after)
+{
+    // Critical section
+    g_dbg_ressource.lock();
+    
+    uint32_t total_len = 0;
+    
+    total_len += sprintf(g_dbg_msg, before);
+    
+    for (uint8_t i=0 ; i<length ; i++)
+    {
+        total_len += sprintf(&g_dbg_msg[total_len], format, data[i]);
+        dbg_assert(total_len < DBG_MAX_STRING_SIZE, "Print data too long.\r\n");
+    }
+    
+    total_len += sprintf(&g_dbg_msg[total_len], after);
+    
+    dbg_add_to_buf(g_dbg_msg, total_len);
+    
+    // End of critical section
+    g_dbg_ressource.unlock();
+}
+
+
+// Thread for printing debug messages.
+void dbg_print_thread()
+{
+    FPRINT("(id:0x%08x)\r\n", osThreadGetId());
+    
+    while (true)
+    {
+        g_dbg_print.wait();
+        
+        if (g_dbg_missing)
+        {
+            g_dbg_serial->printf("Missing %d traces!!!\r\n", g_dbg_missing);
+            g_dbg_missing = 0;
+        }
+        
+        while(!g_dbg_buf.empty())
+        {
+            g_dbg_ressource.lock();
+
+#ifdef __FORCE_LINE_INTEGRITY__
+            // Flush until EOL
+            uint8_t byte;
+            do {
+                byte = g_dbg_buf.pop();
+                g_dbg_serial->putc(byte);
+            } while (byte != '\n');
+#else
+            // Flush characters one by one
+            g_dbg_serial->putc(g_dbg_buf.pop());
+#endif
+
+            g_dbg_ressource.unlock();
+            Thread::yield();
+        }
+
+    }
+}
+
+// Reset_Handler
+// NMI_Handler
+// HardFault_Handler
+// MemManage_Handler
+// BusFault_Handler
+// UsageFault_Handler
+extern "C" void HardFault_Handler() {
+    ASSERT(false, "Hard Fault!\r\n");
+    //NVIC_SystemReset();
+}
+extern "C" void NMI_Handler() {
+    ASSERT(false, "NMI Fault!\r\n");
+    //NVIC_SystemReset();
+}
+extern "C" void MemManage_Handler() {
+    ASSERT(false, "MemManage Fault!\r\n");
+    //NVIC_SystemReset();
+}
+extern "C" void BusFault_Handler() {
+    ASSERT(false, "BusFault Fault!\r\n");
+    //NVIC_SystemReset();
+}
+extern "C" void UsageFault_Handler() {
+    ASSERT(false, "UsageFault Fault!\r\n");
+    //NVIC_SystemReset();
+}