#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include "libs/backtrace.h"
#include "bsp/gd32_bkp.h"
#include "os/os_task.h"

#if defined(__CC_ARM)
    #define SECTION_START(_name_)                _name_##$$Base
    #define SECTION_END(_name_)                  _name_##$$Limit
    #define IMAGE_SECTION_START(_name_)          Image$$##_name_##$$Base
    #define IMAGE_SECTION_END(_name_)            Image$$##_name_##$$Limit
    #define CSTACK_BLOCK_START(_name_)           SECTION_START(_name_)
    #define CSTACK_BLOCK_END(_name_)             SECTION_END(_name_)
    #define CODE_SECTION_START(_name_)           IMAGE_SECTION_START(_name_)
    #define CODE_SECTION_END(_name_)             IMAGE_SECTION_END(_name_)

    extern const int CSTACK_BLOCK_START(CMB_CSTACK_BLOCK_NAME);
    extern const int CSTACK_BLOCK_END(CMB_CSTACK_BLOCK_NAME);
    extern const int CODE_SECTION_START(CMB_CODE_SECTION_NAME);
    extern const int CODE_SECTION_END(CMB_CODE_SECTION_NAME);
#elif defined(__ICCARM__)
    #pragma section=CMB_CSTACK_BLOCK_NAME
    #pragma section=CMB_CODE_SECTION_NAME
#elif defined(__GNUC__)
    extern const int CMB_CSTACK_BLOCK_START;
    extern const int CMB_CSTACK_BLOCK_END;
    extern const int CMB_CODE_SECTION_START;
    extern const int CMB_CODE_SECTION_END;
#else
    #error "not supported compiler"
#endif

//extern int STACK$$Base;
static void get_cur_thread_stack_info(uint32_t sp, uint32_t *start_addr, size_t *size) {
    *start_addr = SECTION_START(STACK);//(uint32_t)vTaskStackAddr();
    *size = SECTION_END(STACK) - SECTION_START(STACK);//vTaskStackSize() * sizeof( StackType_t );
}

#ifdef __TARGET_FPU_VFP
static uint32_t statck_del_fpu_regs(uint32_t fault_handler_lr, uint32_t sp) {
    bool statck_has_fpu_regs = (fault_handler_lr & (1UL << 4)) == 0 ? true : false;

    /* the stack has S0~S15 and FPSCR registers when statck_has_fpu_regs is true, double word align */
    return statck_has_fpu_regs == true ? sp + sizeof(size_t) * 18 : sp;
}
#endif

/* check the disassembly instruction is 'BL' or 'BLX' */
static bool disassembly_ins_is_bl_blx(uint32_t addr) {
    uint16_t ins1 = *((uint16_t *)addr);
    uint16_t ins2 = *((uint16_t *)(addr + 2));

#define BL_INS_MASK         0xF800
#define BL_INS_HIGH         0xF800
#define BL_INS_LOW          0xF000
#define BLX_INX_MASK        0xFF00
#define BLX_INX             0x4700

    if ((ins2 & BL_INS_MASK) == BL_INS_HIGH && (ins1 & BL_INS_MASK) == BL_INS_LOW) {
        return true;
    } else if ((ins2 & BLX_INX_MASK) == BLX_INX) {
        return true;
    } else {
        return false;
    }
}
static uint32_t main_stack_start_addr;
static size_t main_stack_size;
static uint32_t code_start_addr;
static size_t code_size;
static uint32_t call_stack_buf[CMB_CALL_STACK_MAX_DEPTH] = {0};

void backtrace_init(void){
	main_stack_start_addr = (uint32_t)&CSTACK_BLOCK_START(CMB_CSTACK_BLOCK_NAME);
	main_stack_size = (uint32_t)&CSTACK_BLOCK_END(CMB_CSTACK_BLOCK_NAME) - main_stack_start_addr;
	code_start_addr = (uint32_t)&CODE_SECTION_START(CMB_CODE_SECTION_NAME);
	code_size = (uint32_t)&CODE_SECTION_END(CMB_CODE_SECTION_NAME) - code_start_addr;
}
void backtrace_assert(int line) {
    uint32_t stack_start_addr = main_stack_start_addr, pc;
    size_t depth = 0, stack_size = main_stack_size;
    bool regs_saved_lr_is_valid = false;
	bool stack_is_overflow = false;
	uint32_t sp = cmb_get_sp();
        /* OS environment */
    get_cur_thread_stack_info(sp, &stack_start_addr, &stack_size);
    if ((sp < stack_start_addr) || (sp > (stack_start_addr + stack_size))) {
        stack_is_overflow = true;
    }	
    if (stack_is_overflow) {
        if (sp < stack_start_addr) {
            sp = stack_start_addr;
        } else if (sp > stack_start_addr + stack_size) {
            sp = stack_start_addr + stack_size;
        }
    }

    /* copy called function address */
    for (; sp < stack_start_addr + stack_size; sp += sizeof(size_t)) {
        /* the *sp value may be LR, so need decrease a word to PC */
        pc = *((uint32_t *) sp) - sizeof(size_t);
        /* the Cortex-M using thumb instruction, so the pc must be an odd number */
        if (pc % 2 == 0) {
            continue;
        }
        /* fix the PC address in thumb mode */
        pc = *((uint32_t *) sp) - 1;
        if ((pc >= code_start_addr) && (pc <= code_start_addr + code_size) && (depth < CMB_CALL_STACK_MAX_DEPTH)
                /* check the the instruction before PC address is 'BL' or 'BLX' */
                && disassembly_ins_is_bl_blx(pc - sizeof(size_t)) && (depth < CMB_CALL_STACK_MAX_DEPTH)) {
            /* the second depth function may be already saved, so need ignore repeat */
            if ((depth == 2) && regs_saved_lr_is_valid && (pc == call_stack_buf[1])) {
                continue;
            }
            call_stack_buf[depth++] = pc;
        }
    }
	gd32_bkp_save_backtrace(call_stack_buf, stack_is_overflow, depth, line);
	NVIC_SystemReset();
}


void fault_backtrace(uint32_t fault_handler_lr, uint32_t fault_handler_sp) {
    uint32_t stack_pointer = fault_handler_sp, saved_regs_addr = stack_pointer;
	struct cmb_hard_fault_regs regs;
    uint32_t stack_start_addr = main_stack_start_addr;
    size_t stack_size = main_stack_size;
	bool stack_is_overflow = false;
    bool on_fault = true;
    bool on_thread_before_fault = fault_handler_lr & (1UL << 2);
    /* check which stack was used before (MSP or PSP) */
    if (on_thread_before_fault) {
        saved_regs_addr = stack_pointer = cmb_get_psp();
        get_cur_thread_stack_info(stack_pointer, &stack_start_addr, &stack_size);

    }
    /* delete saved R0~R3, R12, LR,PC,xPSR registers space */
    stack_pointer += sizeof(size_t) * 8;

#ifdef __TARGET_FPU_VFP
    stack_pointer = statck_del_fpu_regs(fault_handler_lr, stack_pointer);
#endif /* (CMB_CPU_PLATFORM_TYPE == CMB_CPU_ARM_CORTEX_M4) || (CMB_CPU_PLATFORM_TYPE == CMB_CPU_ARM_CORTEX_M7) */

    /* check stack overflow */
    if ((stack_pointer < stack_start_addr) || (stack_pointer > (stack_start_addr + stack_size))) {
        stack_is_overflow = true;
    }

    /* the stack frame may be get failed when it is overflow  */
    if (!stack_is_overflow) {
        /* dump register */
        regs.saved.r0        = ((uint32_t *)saved_regs_addr)[0];  // Register R0
        regs.saved.r1        = ((uint32_t *)saved_regs_addr)[1];  // Register R1
        regs.saved.r2        = ((uint32_t *)saved_regs_addr)[2];  // Register R2
        regs.saved.r3        = ((uint32_t *)saved_regs_addr)[3];  // Register R3
        regs.saved.r12       = ((uint32_t *)saved_regs_addr)[4];  // Register R12
        regs.saved.lr        = ((uint32_t *)saved_regs_addr)[5];  // Link register LR
        regs.saved.pc        = ((uint32_t *)saved_regs_addr)[6];  // Program counter PC
        regs.saved.psr.value = ((uint32_t *)saved_regs_addr)[7];  // Program status word PSR
    }

    /* the Cortex-M0 is not support fault diagnosis */
    regs.syshndctrl.value = CMB_SYSHND_CTRL;  // System Handler Control and State Register
    regs.mfsr.value       = CMB_NVIC_MFSR;    // Memory Fault Status Register
    regs.mmar             = CMB_NVIC_MMAR;    // Memory Management Fault Address Register
    regs.bfsr.value       = CMB_NVIC_BFSR;    // Bus Fault Status Register
    regs.bfar             = CMB_NVIC_BFAR;    // Bus Fault Manage Address Register
    regs.ufsr.value       = CMB_NVIC_UFSR;    // Usage Fault Status Register
    regs.hfsr.value       = CMB_NVIC_HFSR;    // Hard Fault Status Register
    regs.dfsr.value       = CMB_NVIC_DFSR;    // Debug Fault Status Register
    regs.afsr             = CMB_NVIC_AFSR;    // Auxiliary Fault Status Register
	{
		uint32_t stack_start_addr = main_stack_start_addr, pc;
		size_t depth = 0, stack_size = main_stack_size;
		bool regs_saved_lr_is_valid = false;
		uint32_t sp = stack_pointer;
		if (on_fault) {
			if (!stack_is_overflow) {
				/* first depth is PC */
				call_stack_buf[depth++] = regs.saved.pc;
				/* fix the LR address in thumb mode */
#if 0	
				pc = regs.saved.lr - 1;
				if ((pc >= code_start_addr) && (pc <= code_start_addr + code_size) && (depth < CMB_CALL_STACK_MAX_DEPTH)
						&& (depth < CMB_CALL_STACK_MAX_DEPTH)) {
					call_stack_buf[depth++] = pc;
					regs_saved_lr_is_valid = true;
				}
#endif				
			}
			/* program is running on thread before fault */
			if (on_thread_before_fault) {
				get_cur_thread_stack_info(sp, &stack_start_addr, &stack_size);
			}
		} else {
			/* OS environment */
			if (cmb_get_sp() == cmb_get_psp()) {
				get_cur_thread_stack_info(sp, &stack_start_addr, &stack_size);
			}		
		}
		
		if (stack_is_overflow) {
			if (sp < stack_start_addr) {
				sp = stack_start_addr;
			} else if (sp > stack_start_addr + stack_size) {
				sp = stack_start_addr + stack_size;
			}
		}
		
		/* copy called function address */
		for (; sp < stack_start_addr + stack_size; sp += sizeof(size_t)) {
			/* the *sp value may be LR, so need decrease a word to PC */
			pc = *((uint32_t *) sp) - sizeof(size_t);
			/* the Cortex-M using thumb instruction, so the pc must be an odd number */
			if (pc % 2 == 0) {
				continue;
			}
			/* fix the PC address in thumb mode */
			pc = *((uint32_t *) sp) - 1;
			if ((pc >= code_start_addr) && (pc <= code_start_addr + code_size) && (depth < CMB_CALL_STACK_MAX_DEPTH)
					/* check the the instruction before PC address is 'BL' or 'BLX' */
					&& disassembly_ins_is_bl_blx(pc - sizeof(size_t)) && (depth < CMB_CALL_STACK_MAX_DEPTH)) {
				/* the second depth function may be already saved, so need ignore repeat */
				if ((depth == 2) && regs_saved_lr_is_valid && (pc == call_stack_buf[1])) {
					continue;
				}
				call_stack_buf[depth++] = pc;
			}
		}
		gd32_bkp_save_backtrace(call_stack_buf, stack_is_overflow, depth, 0);		
	}
	
	NVIC_SystemReset();
}