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

#define bkpt() __asm volatile("BKPT #0\n")
OSAL_IRQ_HANDLER(HardFault_Handler) {
    // Copy to local variables (not pointers) to allow GDB "i loc" to directly show the info
    struct port_extctx     ctx;
    volatile unsigned long _CFSR;
    volatile unsigned long _HFSR;
    volatile unsigned long _DFSR;
    volatile unsigned long _AFSR;
    volatile unsigned long _BFAR;
    volatile unsigned long _MMAR;
    // Get thread context. Contains main registers including PC and LR
    memcpy(&ctx, (void *)__get_PSP(), sizeof(struct port_extctx));
    (void)ctx;

    // Configurable Fault Status Register
    // Consists of MMSR, BFSR and UFSR
    _CFSR = (*((volatile unsigned long *)(0xE000ED28)));
    (void)(_CFSR);

    // Hard Fault Status Register
    _HFSR = (*((volatile unsigned long *)(0xE000ED2C)));
    (void)(_HFSR);

    // Debug Fault Status Register
    _DFSR = (*((volatile unsigned long *)(0xE000ED30)));
    (void)(_DFSR);

    // Auxiliary Fault Status Register
    _AFSR = (*((volatile unsigned long *)(0xE000ED3C)));
    (void)(_AFSR);

    // Read the Fault Address Registers. These may not contain valid values.
    // Check BFARVALID/MMARVALID to see if they are valid values
    // MemManage Fault Address Register
    _MMAR = (*((volatile unsigned long *)(0xE000ED34)));
    (void)(_MMAR);
    // Bus Fault Address Register
    _BFAR = (*((volatile unsigned long *)(0xE000ED38)));
    (void)(_BFAR);
    // Cause debugger to stop. Ignored if no debugger is attached
    bkpt();
    NVIC_SystemReset();  // If no debugger connected, just reset the board
}