LibrePilot/flight/targets/boards/revoproto/firmware/cm3_fault_handlers.c

/*
 * cm3_fault_handlers.c
 *
 *  Created on: Apr 24, 2011
 *      Author: msmith
 */

#include <stdint.h>
#include "inc/dcc_stdio.h"
#ifdef STM32F4XX
# include <stm32f4xx.h>
#endif
#ifdef STM32F2XX
# include <stm32f2xx.h>
#endif

#define FAULT_TRAMPOLINE(_vec)										\
__attribute__((naked, no_instrument_function))						\
void																\
_vec##_Handler(void)												\
{																	\
     __asm(  ".syntax unified\n"									\
                     "MOVS   R0, #4  \n"							\
                     "MOV    R1, LR  \n"							\
                     "TST    R0, R1  \n"							\
                     "BEQ    1f    \n"								\
                     "MRS    R0, PSP \n"							\
                     "B      " #_vec "_Handler2      \n"			\
             "1:  \n"												\
                     "MRS    R0, MSP \n"							\
                     "B      " #_vec "_Handler2      \n"			\
             ".syntax divided\n");									\
}																	\
struct hack

struct cm3_frame {
	uint32_t	r0;
	uint32_t	r1;
	uint32_t	r2;
	uint32_t	r3;
	uint32_t	r12;
	uint32_t	lr;
	uint32_t	pc;
	uint32_t	psr;
};

FAULT_TRAMPOLINE(HardFault);
FAULT_TRAMPOLINE(BusFault);
FAULT_TRAMPOLINE(UsageFault);

/* this is a hackaround to avoid an issue where dereferencing SCB seems to result in bad codegen and a link error */
#define SCB_REG(_reg)	(*(uint32_t *)&(SCB->_reg))

void
HardFault_Handler2(struct cm3_frame *frame)
{
	dbg_write_str("\nHARD FAULT");
	dbg_write_hex32(frame->pc);
	dbg_write_char('\n');
	dbg_write_hex32(SCB_REG(HFSR));
	dbg_write_char('\n');
	for (;;);
}

void
BusFault_Handler2(struct cm3_frame *frame)
{
	dbg_write_str("\nBUS FAULT");
	dbg_write_hex32(frame->pc);
	dbg_write_char('\n');
	dbg_write_hex32(SCB_REG(CFSR));
	dbg_write_char('\n');
	dbg_write_hex32(SCB_REG(BFAR));
	dbg_write_char('\n');
	for (;;);
}

void
UsageFault_Handler2(struct cm3_frame *frame)
{
	dbg_write_str("\nUSAGE FAULT");
	dbg_write_hex32(frame->pc);
	dbg_write_char('\n');
	dbg_write_hex32(SCB_REG(CFSR));
	dbg_write_char('\n');
	for (;;);
}