/*
 * fault.c
 *
 *  Created on: Oct 1, 2019
 *      Author: abody
 *      -c "tpiu config internal <logfile_full_path> uart off <cpufreq>"
 */
#include <inttypes.h>
//#include <core_cm4.h>
#include "stm32f4xx.h"
#include "f4ll/strutil.h"
#include "f4ll/fault.h"

#ifdef __cplusplus
extern "C" {
#endif

typedef struct {
	uint32_t R0;
	uint32_t R1;
	uint32_t R2;
	uint32_t R3;
	uint32_t R4;
	uint32_t R5;
	uint32_t R6;
	uint32_t R7;
	uint32_t R8;
	uint32_t R9;
	uint32_t R10;
	uint32_t R11;
	uint32_t R12;
	uint32_t SP;
	uint32_t LR;
	uint32_t PC;
	uint32_t xPSR;
	uint32_t PSP;
	uint32_t MSP;
	uint32_t EXC_RETURN;
	uint32_t CONTROL;
} fault_context_t;

fault_context_t g_faultContext;

void __attribute__((weak)) app_fault_callback(uint32_t reason)
{
}

void SwoSendStr(char const *str, uint8_t len, uint8_t port)
{
	while(len) {
		if (((ITM->TCR & ITM_TCR_ITMENA_Msk) != 0UL) &&      // ITM enabled
		    ((ITM->TER & (1UL << port)     ) != 0UL)   )     // ITM Port enabled
		{
			// Wait until shift register is free
			while (ITM->PORT[port].u32 == 0UL) {
				__ASM volatile ("nop");
			}
			if(len >= 4) {
				ITM->PORT[port].u32 = *(uint32_t*)(str);
				str += 4;
				len -= 4;
			} else if(len >= 2) {
				ITM->PORT[port].u16 = *(uint16_t*)(str);
				str += 2;
				len -= 2;
			} else {
				ITM->PORT[port].u8 = *(uint8_t*)(str);
				++str;
				--len;
			}
		} else
			break;
	}
}

void fault_print_str(char const *fmtstr, uint32_t *values)
{
	char hex_str[9]={0};
	char const *nextChunk = fmtstr;

	while(*fmtstr) {
		if(*fmtstr == '%') {
			SwoSendStr(nextChunk, fmtstr-nextChunk, 0);
			uitohex(hex_str, *values++, 8);
			SwoSendStr(hex_str, 8, 0);
			nextChunk = fmtstr +1;
		}
		++fmtstr;
	}
	if(nextChunk != fmtstr)
		SwoSendStr(nextChunk, fmtstr-nextChunk, 0);
}



__attribute__((noreturn)) void FaultHandler(uint32_t type, fault_context_t *context)
{
	uint32_t FSR[9] = {
		SCB->HFSR,
		0xff & SCB->CFSR,
		(0xff00 & SCB->CFSR) >> 8,
		(0xffff0000 & SCB->CFSR) >> 16,
		SCB->DFSR,
		SCB->AFSR,
		SCB->SHCSR,
		SCB->MMFAR,
		SCB->BFAR
	};

	while(1) {
		fault_print_str("\n++ Fault Handler ++\n\nFaultType: ",NULL);
		switch( type ) {
		case FAULT_REASON_HARD_FAULT:
			fault_print_str("HardFault",NULL);
			break;
		case FAULT_REASON_MEMMANAGE_FAULT:
			fault_print_str("MemManageFault",NULL);
			break;
		case FAULT_REASON_BUS_FAULT:
			fault_print_str("BusFault",NULL);
			break;
		case FAULT_REASON_USAGE_FAULT:
			fault_print_str("UsageFault",NULL);
			break;
		default:
			fault_print_str("Unknown Fault",NULL);
			break;
		}

		fault_print_str("\n\nContext:",NULL);

		fault_print_str(
			"\nR0   : %"
			"\nR1   : %"
			"\nR2   : %"
			"\nR3   : %"
			"\nR4   : %"
			"\nR5   : %"
			"\nR6   : %"
			"\nR7   : %"
			"\nR8   : %"
			"\nR9   : %"
			"\nR10  : %"
			"\nR11  : %"
			"\nR12  : %"
			"\nSP   : %"
			"\nLR   : %"
			"\nPC   : %"
			"\nxPSR : %"
			"\nPSP  : %"
			"\nMSP  : %",
			(uint32_t *)context);

		//Capture CPUID to get core/cpu info
		fault_print_str("\nCPUID: %",(uint32_t *)&SCB->CPUID);

		fault_print_str(
			"\nHFSR : %"
			"\nMMFSR: %"
			"\nBFSR : %"
			"\nUFSR : %"
			"\nDFSR : %"
			"\nAFSR : %"
			"\nSHCSR: %",
			FSR);

		app_fault_callback(type);
	}
}

#ifdef __cplusplus
}
#endif