// SPDX-License-Identifier: GPL-3.0-only
/*
 * Copyright (c) 2008-2023 100askTeam : Dongshan WEI <weidongshan@qq.com> 
 * Discourse:  https://forums.100ask.net
 */
 
/*  Copyright (C) 2008-2023 深圳百问网科技有限公司
 *  All rights reserved
 *
 * 免责声明: 百问网编写的文档, 仅供学员学习使用, 可以转发或引用(请保留作者信息),禁止用于商业用途！
 * 免责声明: 百问网编写的程序, 可以用于商业用途, 但百问网不承担任何后果！
 * 
 * 本程序遵循GPL V3协议, 请遵循协议
 * 百问网学习平台   : https://www.100ask.net
 * 百问网交流社区   : https://forums.100ask.net
 * 百问网官方B站    : https://space.bilibili.com/275908810
 * 百问网官方淘宝   : https://100ask.taobao.com
 * 联系我们(E-mail): weidongshan@qq.com
 *
 *          版权所有，盗版必究。
 *  
 * 修改历史     版本号           作者        修改内容
 *-----------------------------------------------------
 * 2024.09.20      v01         百问科技      创建文件
 *-----------------------------------------------------
 */
#include <stdbool.h>
#include <stdint.h>
#include "main.h"

struct exception_stack_frame
{
    rt_uint32_t r0;
    rt_uint32_t r1;
    rt_uint32_t r2;
    rt_uint32_t r3;
    rt_uint32_t r12;
    rt_uint32_t lr;
    rt_uint32_t pc;
    rt_uint32_t psr;
};

struct stack_frame
{
#if USE_FPU
    rt_uint32_t flag;
#endif /* USE_FPU */

    /* r4 ~ r11 register */
    rt_uint32_t r4;
    rt_uint32_t r5;
    rt_uint32_t r6;
    rt_uint32_t r7;
    rt_uint32_t r8;
    rt_uint32_t r9;
    rt_uint32_t r10;
    rt_uint32_t r11;

    struct exception_stack_frame exception_stack_frame;
};

struct exception_info
{
    rt_uint32_t exc_return;
    struct stack_frame stack_frame;
};

#if defined(__ARMCC_VERSION)
    /* C stack block name, default is STACK */
    #ifndef CMB_CSTACK_BLOCK_NAME
    #define CMB_CSTACK_BLOCK_NAME          Image$$ARM_LIB_STACK$$ZI
    #endif
    /* code section name, default is ER_IROM1 */
    #ifndef CMB_CODE_SECTION_NAME
    #define CMB_CODE_SECTION_NAME          FLASH_CODE
    #endif
#elif defined(__ICCARM__)
    /* C stack block name, default is 'CSTACK' */
    #ifndef CMB_CSTACK_BLOCK_NAME
    #define CMB_CSTACK_BLOCK_NAME          "CSTACK"
    #endif
    /* code section name, default is '.text' */
    #ifndef CMB_CODE_SECTION_NAME
    #define CMB_CODE_SECTION_NAME          ".text"
    #endif
#elif defined(__GNUC__)
    /* C stack block start address, defined on linker script file, default is _sstack */
    #ifndef CMB_CSTACK_BLOCK_START
    #define CMB_CSTACK_BLOCK_START         _sstack
    #endif
    /* C stack block end address, defined on linker script file, default is _estack */
    #ifndef CMB_CSTACK_BLOCK_END
    #define CMB_CSTACK_BLOCK_END           _estack
    #endif
    /* code section start address, defined on linker script file, default is _stext */
    #ifndef CMB_CODE_SECTION_START
    #define CMB_CODE_SECTION_START         _stext
    #endif
    /* code section end address, defined on linker script file, default is _etext */
    #ifndef CMB_CODE_SECTION_END
    #define CMB_CODE_SECTION_END           _etext
    #endif
#else
    #error "not supported compiler"
#endif

#if __STDC_VERSION__ < 199901L
    #error "must be C99 or higher. try to add '-std=c99' to compile parameters"
#endif

#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  
    
static bool is_in_text(uint32_t addr)
{
#if 0
    uint32_t code_start_addr = (uint32_t)&CODE_SECTION_START(CMB_CODE_SECTION_NAME);
    uint32_t code_size = (uint32_t)&CODE_SECTION_END(CMB_CODE_SECTION_NAME) - code_start_addr;
#else
    uint32_t code_start_addr = (uint32_t)__section_begin(CMB_CODE_SECTION_NAME);
    uint32_t code_size = (uint32_t)__section_end(CMB_CODE_SECTION_NAME) - code_start_addr;
#endif
    
    if (code_start_addr < addr && addr <= code_start_addr + code_size)
    {
        return true;
    }
    
    return false;
}

static bool disassembly_ins_is_bl_blx(uint32_t addr) 
{
    #define BL_INS_MASK         0xF800
    #define BL_INS_HIGH         0xF800
    #define BL_INS_LOW          0xF000
    #define BLX_INX_MASK        0xFF00
    #define BLX_INX             0x4700
  
    uint16_t ins1 = *((uint16_t *)addr);
    uint16_t ins2 = *((uint16_t *)(addr + 2));
    
    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;
    }
}

void svc_exception(struct exception_info *exception_info)
{
    //struct exception_stack_frame *exception_stack = &exception_info->stack_frame.exception_stack_frame;
    struct stack_frame *context = &exception_info->stack_frame;
    
    unsigned int addr;
    unsigned short swi_inst;
    unsigned int *old_sp;
    unsigned int value;
    
    /* 取出swi指令 */
    addr = context->exception_stack_frame.pc;
    addr -= 2;
    
    swi_inst = *((unsigned short *)addr);
    rt_kprintf("swi_inst = 0x%04x\n", swi_inst);
    swi_inst = swi_inst & 0xff;
    
    /* 分析swi指令 */
    old_sp = (unsigned int *)(exception_info + 1); // 得到栈顶
    unsigned int *p_reg = &context->exception_stack_frame.r0; // 得到r0的地址
    
    if (swi_inst == 0xff)
    {
        /* POP {PC} */
        value = *old_sp;
        old_sp++;
        context->exception_stack_frame.pc = value;
    }
    else
    {
        for (int i = 0; i < 8; i ++)
        {
            if (swi_inst & (1 << i))
            {
                /* POP {r0, PC} */
                value = *old_sp;
                old_sp++;
                p_reg[i] = value; // 哪个寄存器就保存哪个值
                
                /* POP {PC} */
                value = *old_sp;
                old_sp++;
                context->exception_stack_frame.pc = value;
            }
        }
    }
    
    rt_kprintf("old_sp = 0x%0x\n", old_sp);
    rt_kprintf("context->exception_stack_frame.pc= 0x%0x\n", context->exception_stack_frame.pc);
    /* 模拟swi指令 */
    struct exception_info stack_frame = *exception_info;
    unsigned int old_pos  = (unsigned int)(exception_info + 1);
    unsigned int new_pos = (unsigned int)old_sp;
    unsigned int offset = new_pos - old_pos;
    
    exception_info = (struct exception_info *)((unsigned int)exception_info + offset);
    *exception_info = stack_frame;
    /* 返回 */
}

void rt_hw_hard_fault_exception(struct exception_info *exception_info)
{
#if 0
    //struct exception_stack_frame *exception_stack = &exception_info->stack_frame.exception_stack_frame;
    struct stack_frame *context = &exception_info->stack_frame;
    rt_kprintf("psr: 0x%08x\n", context->exception_stack_frame.psr);

    rt_kprintf("r00: 0x%08x\n", context->exception_stack_frame.r0);
    rt_kprintf("r01: 0x%08x\n", context->exception_stack_frame.r1);
    rt_kprintf("r02: 0x%08x\n", context->exception_stack_frame.r2);
    rt_kprintf("r03: 0x%08x\n", context->exception_stack_frame.r3);
    rt_kprintf("r04: 0x%08x\n", context->r4);
    rt_kprintf("r05: 0x%08x\n", context->r5);
    rt_kprintf("r06: 0x%08x\n", context->r6);
    rt_kprintf("r07: 0x%08x\n", context->r7);
    rt_kprintf("r08: 0x%08x\n", context->r8);
    rt_kprintf("r09: 0x%08x\n", context->r9);
    rt_kprintf("r10: 0x%08x\n", context->r10);
    rt_kprintf("r11: 0x%08x\n", context->r11);
    rt_kprintf("r12: 0x%08x\n", context->exception_stack_frame.r12);
    rt_kprintf(" lr: 0x%08x\n", context->exception_stack_frame.lr);
    rt_kprintf(" pc: 0x%08x\n", context->exception_stack_frame.pc);
    
    rt_kprintf("use command: addr2line -e your.out -a -f ");
    rt_kprintf("%08x ", context->exception_stack_frame.pc);
    rt_kprintf("%08x ", context->exception_stack_frame.lr);
    uint32_t lr;
    uint32_t pc;
    uint32_t *app_sp = (unsigned int *)(exception_info + 1); // 得到栈顶;
    
    for(int i = 0; i < 1024; i++)
    {
        lr = *app_sp;
        app_sp++;
        
        if ((lr & 0x01) && true == is_in_text(lr))
        {
            pc = (lr & ~0x01) - 4;

            if (true == disassembly_ins_is_bl_blx(pc))
            {
                rt_kprintf("%08x ", pc);
            }
        }
    }
#else 
    void DumpCore(struct stack_frame *sp);
    DumpCore(&exception_info->stack_frame);
#endif
    while(1);
}

/**********************************************************************
 * 函数名称： DumpRegisters
 * 功能描述： 打印寄存器的值
 * 输入参数： sp     - hardfault.s里设置的这个参数,它是栈的地址
 *            thread - 线程名
 * 输出参数： 无
 * 返 回 值： 无
 * 修改日期        版本号     修改人	      修改内容
 * -----------------------------------------------
 * 2023/09/20	     V1.0	  韦东山	      创建
 ***********************************************************************/
static void DumpRegisters(struct stack_frame *sp, char *thread)
{
	rt_kprintf("Registers@%s\n", thread);
	rt_kprintf("R0: 0x%08X\n", sp->exception_stack_frame.r0);
	rt_kprintf("R1: 0x%08X\n", sp->exception_stack_frame.r1); 
	rt_kprintf("R2: 0x%08X\n", sp->exception_stack_frame.r2); 
	rt_kprintf("R3: 0x%08X\n", sp->exception_stack_frame.r3); 
	rt_kprintf("R4: 0x%08X\n", sp->r4); 
	rt_kprintf("R5: 0x%08X\n", sp->r5); 
	rt_kprintf("R6: 0x%08X\n", sp->r6); 
	rt_kprintf("R7: 0x%08X\n", sp->r7); 
	rt_kprintf("R8: 0x%08X\n", sp->r8); 
	rt_kprintf("R9: 0x%08X\n", sp->r9); 
	rt_kprintf("R10: 0x%08X\n", sp->r10); 
	rt_kprintf("R11: 0x%08X\n", sp->r11); 
	rt_kprintf("R12: 0x%08X\n", sp->exception_stack_frame.r12); 
	rt_kprintf("R13(SP): 0x%08X\n", (uint32_t)sp + sizeof(*sp)); 
	rt_kprintf("R14(LR): 0x%08X\n", sp->exception_stack_frame.lr); 
	rt_kprintf("R15(PC): 0x%08X\n", sp->exception_stack_frame.pc); 
	rt_kprintf("xPSR: 0x%08X\n", sp->exception_stack_frame.psr); 
}

/**********************************************************************
 * 函数名称： DumpMem
 * 功能描述： 打印内存的值
 * 输入参数： addr - 内存起始地址(4对齐)
 *            len  - 内存长度(4对齐)
 * 输出参数： 无
 * 返 回 值： 无
 * 修改日期        版本号     修改人	      修改内容
 * -----------------------------------------------
 * 2023/09/20	     V1.0	  韦东山	      创建
 ***********************************************************************/
static void DumpMem(uint32_t addr, uint32_t len)
{
	uint32_t *paddr;
	uint32_t i;
	
	paddr = (uint32_t *)addr;
	rt_kprintf("mem@0x%08X, 0x%08X\n", addr, len);
	
	for (i = 0; i < len;)
	{
		rt_kprintf("0x%08X", *paddr);
		paddr++;

		i+= 4;
		if (i % 16 == 0)
			rt_kprintf("\n");
		else
			rt_kprintf(" ");
	}

	rt_kprintf("\n");
}

/**********************************************************************
 * 函数名称： DumpCore
 * 功能描述： 打印寄存器、内存的值
 * 输入参数： sp     - hardfault.s里设置的这个参数,它是栈的地址
 * 输出参数： 无
 * 返 回 值： 无
 * 修改日期        版本号     修改人	      修改内容
 * -----------------------------------------------
 * 2023/09/20	     V1.0	  韦东山	      创建
 ***********************************************************************/
#pragma section=".data"
#pragma section=".bss"
void DumpCore(struct stack_frame *sp)
{
	/* 打印寄存器 */
	DumpRegisters(sp, "current_thread");
	
	/* 打印数据段 */
        uint32_t start = (uint32_t)__section_begin(".data");
        uint32_t end = (uint32_t)__section_end(".data");
	rt_kprintf("Data segment:\n");
	DumpMem(start, end - start);

	/* 打印ZI段 */
	rt_kprintf("ZI segment:\n");
        start = (uint32_t)__section_begin(".bss");
        end = (uint32_t)__section_end(".bss");
	DumpMem(start, end - start);

	/* 打印栈 */
	rt_kprintf("Stack segment:\n");
	DumpMem((uint32_t)sp + sizeof(*sp), 1024);
}