#include <ch.h>
#include <hal.h>
#include "stm32l1xx.h"

void usart1_putstring(const char *p)
{
    while (*p)
    {
        while(!(USART1->SR & USART_SR_TXE));
        USART1->DR=*p;
        p++;
    } 
}

void reboot()
{
    SCB->AIRCR = 0x05FA0000 | SCB_AIRCR_SYSRESETREQ;
}

void **HARDFAULT_PSP;
register void *stack_pointer asm("sp");

void __attribute__((naked)) HardFaultVector()
{
    // Hijack the process stack pointer to make backtrace work
    asm("mrs %0, psp" : "=r"(HARDFAULT_PSP) : :);
    usart1_putstring("\r\nHARDFAULT\r\n");
    (void)SCB->HFSR;
    
    stack_pointer = HARDFAULT_PSP;
    reboot();
}

void NMIVector(void) __attribute__((alias("HardFaultVector")));
void MemManageVector(void) __attribute__((alias("HardFaultVector")));
void BusFaultVector(void) __attribute__((alias("HardFaultVector")));
void UsageFaultVector(void) __attribute__((alias("HardFaultVector")));

int check_debug()
{
    return ENABLE_DEBUG || palReadPad(GPIOA, GPIOA_RX);
}

#if ENABLE_DEBUG

struct fastlog_t
{
    uint32_t time;
    const char *msg;
    uint32_t arg;
};

struct fastlog_t g_fastlog[256];
uint32_t g_fastlog_p = 0;

void fastlog(const char *msg, uint32_t arg)
{
    uint32_t p = __sync_fetch_and_add(&g_fastlog_p, 1);
    p = p & 0xFF;
    g_fastlog[p].time = halGetCounterValue();
    g_fastlog[p].msg = msg;
    g_fastlog[p].arg = arg;
}
#endif