/*
 * Copyright 2018, 2020 NXP.
 * This software is owned or controlled by NXP and may only be used strictly in accordance with the
 * license terms that accompany it. By expressly accepting such terms or by downloading, installing,
 * activating and/or otherwise using the software, you are agreeing that you have read, and that you
 * agree to comply with and are bound by, such license terms. If you do not agree to be bound by the
 * applicable license terms, then you may not retain, install, activate or otherwise use the software.
 */

#ifdef NDEBUG
#include "fault_handlers.h"

/* Board includes */
#include "board.h"

/* Shell includes */
#include "fsl_debug_console.h"
#include "FreeRTOSConfig.h"

#pragma GCC push_options
#pragma GCC optimize ("O0")

/* Can't print when using UART */
#define faulthandlerENABLE_ERROR_LOGS 0

#if (faulthandlerENABLE_ERROR_LOGS == 1)
#define faultERROR_LOG(x) {DEBUG_CONSOLE_LOCK(); PRINTF x; DEBUG_CONSOLE_UNLOCK();}
#else
#define faultERROR_LOG(x)
#endif

static void Fault_GetStatusRegisters(fault_status_t *s_fault_status);
static void Fault_StackDump(fault_status_t s_fault_status);
static void HardFault_Handler_StatusRegisterDecode(fault_status_t s_fault_status);
static void MemManageFault_Handler_StatusRegisterDecode(fault_status_t s_fault_status);
static void BusFault_Handler_StatusRegisterDecode(fault_status_t s_fault_status);
static void UsageFault_Handler_StatusRegisterDecode(fault_status_t s_fault_status);

static void Fault_GetStatusRegisters(fault_status_t *s_fault_status)
{
    uint32_t *msp = (uint32_t *)(__get_MSP());
    uint32_t *psp = (uint32_t *)(__get_PSP());

    s_fault_status->hfsr  = SCB->HFSR;
    s_fault_status->cfsr  = SCB->CFSR;
    s_fault_status->mmfar = SCB->MMFAR;
    s_fault_status->bfar  = SCB->BFAR;

    memcpy(&s_fault_status->msr, msp, sizeof(stack_registers_t));
    memcpy(&s_fault_status->psr, psp, sizeof(stack_registers_t));
}

static void Fault_StackDump(fault_status_t s_fault_status)
{
    faultERROR_LOG(("[ERR] [FAULT] [MSP] Main Stack registers:\r\n"));
    faultERROR_LOG(("[ERR] [FAULT] r0  = 0x%08x\r\n", s_fault_status.msr.r0));
    faultERROR_LOG(("[ERR] [FAULT] r1  = 0x%08x\r\n", s_fault_status.msr.r1));
    faultERROR_LOG(("[ERR] [FAULT] r2  = 0x%08x\r\n", s_fault_status.msr.r2));
    faultERROR_LOG(("[ERR] [FAULT] r3  = 0x%08x\r\n", s_fault_status.msr.r3));
    faultERROR_LOG(("[ERR] [FAULT] r12 = 0x%08x\r\n", s_fault_status.msr.r12));
    faultERROR_LOG(("[ERR] [FAULT] lr  = 0x%08x\r\n", s_fault_status.msr.lr));
    faultERROR_LOG(("[ERR] [FAULT] pc  = 0x%08x\r\n", s_fault_status.msr.pc));
    faultERROR_LOG(("[ERR] [FAULT] psr = 0x%08x\r\n", s_fault_status.msr.psr));

    faultERROR_LOG(("[ERR] [FAULT] [PSP] Process Stack registers:\r\n"));
    faultERROR_LOG(("[ERR] [FAULT] r0  = 0x%08x\r\n", s_fault_status.psr.r0));
    faultERROR_LOG(("[ERR] [FAULT] r1  = 0x%08x\r\n", s_fault_status.psr.r1));
    faultERROR_LOG(("[ERR] [FAULT] r2  = 0x%08x\r\n", s_fault_status.psr.r2));
    faultERROR_LOG(("[ERR] [FAULT] r3  = 0x%08x\r\n", s_fault_status.psr.r3));
    faultERROR_LOG(("[ERR] [FAULT] r12 = 0x%08x\r\n", s_fault_status.psr.r12));
    faultERROR_LOG(("[ERR] [FAULT] lr  = 0x%08x\r\n", s_fault_status.psr.lr));
    faultERROR_LOG(("[ERR] [FAULT] pc  = 0x%08x\r\n", s_fault_status.psr.pc));
    faultERROR_LOG(("[ERR] [FAULT] psr = 0x%08x\r\n", s_fault_status.psr.psr));
}

static void HardFault_Handler_StatusRegisterDecode(fault_status_t s_fault_status)
{
    faultERROR_LOG(("[ERR] [HARD FAULT] Decode Status Register: SCB->HFSR = 0x%08x\r\n", s_fault_status.hfsr));

    /* Decode the HardFault Status Register (HFSR) */
    if (SCB_HFSR_FORCED_Msk == (SCB_HFSR_FORCED_Msk & s_fault_status.hfsr))
    {
        /* Forced HardFault
         * Generated by escalation of a fault with configurable priority that
         * cannot be handled, either because of priority or because it is disabled
         * The handler must read the other fault status registers to find the cause of the fault.
         */
        faultERROR_LOG(("[ERR] [HARD FAULT] SCB->HFSR[30] - Forced HardFault\r\n"));

        MemManageFault_Handler_StatusRegisterDecode(s_fault_status);
        BusFault_Handler_StatusRegisterDecode(s_fault_status);
        UsageFault_Handler_StatusRegisterDecode(s_fault_status);
    }

    if (SCB_HFSR_VECTTBL_Msk == (SCB_HFSR_VECTTBL_Msk & s_fault_status.hfsr))
    {
        /* BusFault on vector table read */
        faultERROR_LOG(
            ("[ERR] [HARD FAULT] SCB->HFSR[1] - BusFault on a vector table read during exception processing\r\n"));
    }
}

static void MemManageFault_Handler_StatusRegisterDecode(fault_status_t s_fault_status)
{
    faultERROR_LOG(("[ERR] [MEMMANAGE FAULT] Decode Status Register: SCB->CFSR = 0x%08x\r\n", s_fault_status.cfsr));

    /* Decode the MemManage Fault Status Register (MMFSR subregister of CFSR) */
    if (SCB_CFSR_MMARVALID_Msk == (SCB_CFSR_MMARVALID_Msk & s_fault_status.cfsr))
    {
        /* MMAR holds a valid fault address */
        faultERROR_LOG(
            ("[ERR] [MEMMANAGE FAULT] SCB->CFSR[7](MMFSR[7]) - MemManage Fault Address Register valid flag\r\n"));
        faultERROR_LOG(("[ERR] [MEMMANAGE FAULT] SCB->MMFAR[31:0] - 0x%08x\r\n", s_fault_status.mmfar));
    }

    if (SCB_CFSR_MLSPERR_Msk == (SCB_CFSR_MLSPERR_Msk & s_fault_status.cfsr))
    {
        /* A MemManage fault occurred during floating-point lazy state preservation */
        faultERROR_LOG(("[ERR] [MEMMANAGE FAULT] SCB->CFSR[5](MMFSR[5]) - MemManage fault Floating-point\r\n"));
    }

    if (SCB_CFSR_MSTKERR_Msk == (SCB_CFSR_MSTKERR_Msk & s_fault_status.cfsr))
    {
        /* Stacking for an exception entry has caused one or more access violations */
        faultERROR_LOG(
            ("[ERR] [MEMMANAGE FAULT] SCB->CFSR[4](MMFSR[4]) - MemManage fault on stacking for exception entry\r\n"));
    }

    if (SCB_CFSR_MUNSTKERR_Msk == (SCB_CFSR_MUNSTKERR_Msk & s_fault_status.cfsr))
    {
        /* Unstack for an exception return has caused one or more access violations */
        faultERROR_LOG(
            ("[ERR] [MEMMANAGE FAULT] SCB->CFSR[3](MMFSR[3]) - MemManage fault on unstacking for a return from "
             "exception\r\n"));
    }

    if (SCB_CFSR_DACCVIOL_Msk == (SCB_CFSR_DACCVIOL_Msk & s_fault_status.cfsr))
    {
        /* The processor attempted a load or store at a location that does not permit the operation */
        faultERROR_LOG(("[ERR] [MEMMANAGE FAULT] SCB->CFSR[1](MMFSR[1]) - Data access violation flag\r\n"));
    }

    if (SCB_CFSR_IACCVIOL_Msk == (SCB_CFSR_IACCVIOL_Msk & s_fault_status.cfsr))
    {
        /* The processor attempted an instruction fetch from a location that does not permit execution */
        faultERROR_LOG(("[ERR] [MEMMANAGE FAULT] SCB->CFSR[0](MMFSR[0]) - Instruction access violation flag\r\n"));
    }
}

static void BusFault_Handler_StatusRegisterDecode(fault_status_t s_fault_status)
{
    faultERROR_LOG(("[ERR] [BUS FAULT] Decode Status Register: SCB->CFSR = 0x%08x\r\n", s_fault_status.cfsr));

    /* Decode the MemManage Fault Status Register (MMFSR subregister of CFSR) */
    if (SCB_CFSR_BFARVALID_Msk == (SCB_CFSR_BFARVALID_Msk & s_fault_status.cfsr))
    {
        /* BFAR holds a valid fault address */
        faultERROR_LOG(("[ERR] [BUS FAULT] SCB->CFSR[15](BFSR[7]) - BusFault Address Register valid flag\r\n"));
        faultERROR_LOG(("[ERR] [BUS FAULT] SCB->BFAR[31:0] - 0x%08x\r\n", s_fault_status.bfar));
    }

    if (SCB_CFSR_LSPERR_Msk == (SCB_CFSR_LSPERR_Msk & s_fault_status.cfsr))
    {
        /* A bus fault occurred during floating-point lazy state preservation */
        faultERROR_LOG(("[ERR] [BUS FAULT] SCB->CFSR[13](BFSR[5]) - BusFault Floating-point\r\n"));
    }

    if (SCB_CFSR_STKERR_Msk == (SCB_CFSR_STKERR_Msk & s_fault_status.cfsr))
    {
        /* Stacking for an exception entry has caused one or more BusFaults */
        faultERROR_LOG(("[ERR] [BUS FAULT] SCB->CFSR[12](BFSR[4]) - BusFault on stacking for exception entry\r\n"));
    }

    if (SCB_CFSR_UNSTKERR_Msk == (SCB_CFSR_UNSTKERR_Msk & s_fault_status.cfsr))
    {
        /* Unstack for an exception return has caused one or more BusFaults */
        faultERROR_LOG(
            ("[ERR] [BUS FAULT] SCB->CFSR[11](BFSR[3]) - BusFault on unstacking for a return from exception\r\n"));
    }

    if (SCB_CFSR_IMPRECISERR_Msk == (SCB_CFSR_IMPRECISERR_Msk & s_fault_status.cfsr))
    {
        /* A data bus error has occurred, but the return address in the stack
         * frame is not related to the instruction that caused the error */
        faultERROR_LOG(("[ERR] [BUS FAULT] SCB->CFSR[10](BFSR[2]) - Imprecise data bus error\r\n"));
    }

    if (SCB_CFSR_PRECISERR_Msk == (SCB_CFSR_PRECISERR_Msk & s_fault_status.cfsr))
    {
        /* A data bus error has occurred, and the PC value stacked for the
         * exception return points to the instruction that caused the fault */
        faultERROR_LOG(("[ERR] [BUS FAULT] SCB->CFSR[9](BFSR[1]) - Precise data bus error\r\n"));
    }

    if (SCB_CFSR_IBUSERR_Msk == (SCB_CFSR_IBUSERR_Msk & s_fault_status.cfsr))
    {
        /* The processor detects the instruction bus error on prefetching an instruction,
         * but it sets the IBUSERR flag to 1 only if it attempts to issue the faulting instruction */
        faultERROR_LOG(("[ERR] [BUS FAULT] SCB->CFSR[8](BFSR[0]) - Instruction bus error\r\n"));
    }
}

static void UsageFault_Handler_StatusRegisterDecode(fault_status_t s_fault_status)
{
    faultERROR_LOG(("[ERR] [USAGE FAULT] Decode Status Register: SCB->CFSR = 0x%08x\r\n", s_fault_status.cfsr));

    if (SCB_CFSR_DIVBYZERO_Msk == (SCB_CFSR_DIVBYZERO_Msk & s_fault_status.cfsr))
    {
        /* The processor has executed an SDIV or UDIV instruction with a divisor of 0 */
        faultERROR_LOG(("[ERR] [USAGE FAULT] SCB->CFSR[25](UFSR[9]) - Divide by zero UsageFault\r\n"));
    }

    if (SCB_CFSR_UNALIGNED_Msk == (SCB_CFSR_UNALIGNED_Msk & s_fault_status.cfsr))
    {
        /* The processor has made an unaligned memory access */
        faultERROR_LOG(("[ERR] [USAGE FAULT] SCB->CFSR[24](UFSR[8]) - Unaligned access UsageFault\r\n"));
    }

    if (SCB_CFSR_NOCP_Msk == (SCB_CFSR_NOCP_Msk & s_fault_status.cfsr))
    {
        /* The processor has attempted an illegal load of EXC_RETURN to the PC,
         * as a result of an invalid context, or an invalid EXC_RETURN value */
        faultERROR_LOG(("[ERR] [USAGE FAULT] SCB->CFSR[19](UFSR[3]) - No coprocessor UsageFault\r\n"));
    }

    if (SCB_CFSR_INVPC_Msk == (SCB_CFSR_INVPC_Msk & s_fault_status.cfsr))
    {
        /* The processor has attempted to execute an instruction that makes illegal use of the EPSR */
        faultERROR_LOG(("[ERR] [USAGE FAULT] SCB->CFSR[18](UFSR[2]) - Invalid PC load UsageFault\r\n"));
    }

    if (SCB_CFSR_INVSTATE_Msk == (SCB_CFSR_INVSTATE_Msk & s_fault_status.cfsr))
    {
        /* The processor has attempted to execute an instruction that makes illegal use of the EPSR */
        faultERROR_LOG(("[ERR] [USAGE FAULT] SCB->CFSR[17](UFSR[1]) - Invalid state UsageFault\r\n"));
    }

    if (SCB_CFSR_UNDEFINSTR_Msk == (SCB_CFSR_UNDEFINSTR_Msk & s_fault_status.cfsr))
    {
        /* The processor has attempted to execute an undefined instruction */
        faultERROR_LOG(("[ERR] [USAGE FAULT] SCB->CFSR[16](UFSR[0]) - Undefined instruction UsageFault\r\n"));
    }
}

void HardFault_Handler(void)
{
    volatile size_t heapSize = xPortGetFreeHeapSize();
    volatile size_t minEver  = xPortGetMinimumEverFreeHeapSize();
    volatile size_t usedHeap = 0;
    volatile size_t maxUsed  = 0;

    fault_status_t s_fault_status = {kFaultType_None};
    Fault_GetStatusRegisters(&s_fault_status);
    s_fault_status.fault_type = kFaultType_Hard;

    faultERROR_LOG(("[ERR] [HARD FAULT] Reached\r\n"));
    HardFault_Handler_StatusRegisterDecode(s_fault_status);
    Fault_StackDump(s_fault_status);

    usedHeap = configTOTAL_HEAP_SIZE - heapSize;
    maxUsed  = configTOTAL_HEAP_SIZE - minEver;

    // Something to break on if we want to check if there is some overhead left
    if (maxUsed - usedHeap)
    {
        __asm("nop");
    }

    __asm("BKPT #0");

}

void MemManage_Handler(void)
{
    fault_status_t s_fault_status = {kFaultType_None};
    Fault_GetStatusRegisters(&s_fault_status);
    s_fault_status.fault_type = kFaultType_MemManage;

    faultERROR_LOG(("[ERR] [MEMMANAGE FAULT] Reached\r\n"));
    MemManageFault_Handler_StatusRegisterDecode(s_fault_status);
    Fault_StackDump(s_fault_status);

    __asm("BKPT #1");
}

void BusFault_Handler(void)
{
    fault_status_t s_fault_status = {kFaultType_None};
    Fault_GetStatusRegisters(&s_fault_status);
    s_fault_status.fault_type = kFaultType_Bus;

    faultERROR_LOG(("[ERR] [BUS FAULT] Reached\r\n"));
    BusFault_Handler_StatusRegisterDecode(s_fault_status);
    Fault_StackDump(s_fault_status);

    __asm("BKPT #2");
}

void UsageFault_Handler(void)
{
    fault_status_t s_fault_status = {kFaultType_None};
    Fault_GetStatusRegisters(&s_fault_status);
    s_fault_status.fault_type = kFaultType_Usage;

    faultERROR_LOG(("[ERR] [USAGE FAULT] Reached\r\n"));
    UsageFault_Handler_StatusRegisterDecode(s_fault_status);
    Fault_StackDump(s_fault_status);

    __asm("BKPT #3");
}

#endif /* NDEBUG */
