File content as of revision 5:63ef3a21e932:

#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     (512)

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)
{
#ifdef __FORCE_FLUSH__
    for (int i = 0; i < size; i++)
    {
        g_dbg_serial->putc(msg[i]);
    }
#else
    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();
    }
#endif
}

// Asserts and trap processor.
void dbg_assert(bool test, const char* format, ...)
{
    if (test) return;
        
    char* assert_msg = "ASSERT ";
    int assert_size;
    
    assert_size = sprintf(g_dbg_msg, assert_msg);
    
    // expand assert string
    va_list args;
    va_start(args, format);
    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();
}