#include <stdio.h>
#include <libopencm3/cm3/scb.h>
#include <libopencm3/cm3/nvic.h>
#ifndef STM32F1
#include <libopencm3/stm32/syscfg.h>
#endif

struct hardfault_args {
	unsigned long r0;
	unsigned long r1;
	unsigned long r2;
	unsigned long r3;
	unsigned long r12;
	unsigned long lr;
	unsigned long pc;
	unsigned long psr;
};

extern void hard_fault_handler_c(struct hardfault_args *args);

__attribute__((naked))
void hard_fault_handler(void)
{
	__asm(	".syntax unified\n"
		"movs r0, #4	\n"
		"mov r1, lr	\n"
		"tst r0, r1	\n"
		"beq _MSP	\n"
		"mrs r0, psp	\n"
		"b _CHAND	\n"
	"_MSP:			\n"
		"mrs r0, msp	\n"
	"_CHAND:		\n"
		"b hard_fault_handler_c\n"
		".syntax divided\n");
}

void hard_fault_handler_c(struct hardfault_args *args)
{
	//usart_stop(1, 0);
	fprintf(stderr, "HARDFAULT\r\n");
	fprintf(stderr, "R0=%08lx, R1=%08lx, R2=%08lx, R3=%08lx, R12=%08lx\r\n",
		args->r0, args->r1, args->r2, args->r3, args->r12);
	fprintf(stderr, "LR[R14]=%08lx, PC[R15]=%08lx, PSR=%08lx\r\n",
		args->lr, args->pc, args->psr);

	/* we better reset the CPU to avoid getting stuck in some state */
	scb_reset_system();
}

void nmi_handler(void)
{
	/* We better do a reset in case we receive a NMI */
	fprintf(stderr, "NMI\r\n");
#ifdef SYSCFG_CFGR2
	if (SYSCFG_CFGR2 & 0x10) {
		fprintf(stderr, "PARITY\r\n");
		SYSCFG_CFGR2 = 0x10;
	}
#endif
	scb_reset_system();
}