#include "os_port.h"
#include "os_stack.h"
#include "os_macros.h"
#include "os_align.h"
#include "os_compiler.h"
#include "os_systick.h"
#include "os_sched.h"


/* ------------------------------------------------------------------------------------------------------------------ */
/*  */

volatile os_uint_t svc_exc_return;

/* ------------------------------------------------------------------------------------------------------------------ */
/*  */

typedef struct {
#if(OS_CFG_CPU_FPU_PRESENT)
    os_uintptr_t s16;
    os_uintptr_t s17;
    os_uintptr_t s18;
    os_uintptr_t s19;
    os_uintptr_t s20;
    os_uintptr_t s21;
    os_uintptr_t s22;
    os_uintptr_t s23;
    os_uintptr_t s24;
    os_uintptr_t s25;
    os_uintptr_t s26;
    os_uintptr_t s27;
    os_uintptr_t s28;
    os_uintptr_t s29;
    os_uintptr_t s30;
    os_uintptr_t s31;
#endif
/* ==== 以下为手动压栈部分 ==== */
    os_uintptr_t exc_return;    /* r2 - LR */           /* 这是地址最小的位置 */
    os_uintptr_t control;       /* r3 */
    os_uintptr_t r4;
    os_uintptr_t r5;
    os_uintptr_t r6;
    os_uintptr_t r7;
    os_uintptr_t r8;
    os_uintptr_t r9;
    os_uintptr_t r10;
    os_uintptr_t r11;
/* ==== 以下为自动压栈部分 ==== */
    os_uintptr_t r0;
    os_uintptr_t r1;
    os_uintptr_t r2;
    os_uintptr_t r3;
    os_uintptr_t r12;
    os_uintptr_t lr;
    os_uintptr_t pc;
    os_uintptr_t xPSR;
}os_stack_frame_t;

#define STACK_FRAME_SIZE sizeof(os_stack_frame_t)

os_stack_t os_port_cpu_stack_init(os_task_entry_t entry, void* param
        , void* stack_base, os_size_t stack_size, void(*exit_entry)(void)){
    os_uintptr_t stack_max = (os_uintptr_t)((uint8_t *) stack_base + stack_size);
    os_uintptr_t real_max = OS_ALIGN_DOWN(stack_max, OS_ALIGN_SIZE); /* 实际栈顶的位置，对齐后的位置 */
    stack_size -= (stack_max - real_max); /* 实际栈大小 */

    uint8_t flag = 0x01;
    os_stack_frame_t* p_frame = (os_stack_frame_t*)(real_max - STACK_FRAME_SIZE);
    for(os_size_t i=0; i<STACK_FRAME_SIZE/sizeof(os_uintptr_t); i++){
        ((os_uintptr_t*)(p_frame))[i] = ((os_uintptr_t)flag<<24)|((os_uintptr_t)flag<<16)|((os_uintptr_t)flag<<8)|flag;
        flag++;
    }

    p_frame->r0 = (os_uintptr_t)param;          /* 任务参数 */
    p_frame->pc = (os_uintptr_t)entry;          /* 任务入口 */
    p_frame->lr = (os_uintptr_t)exit_entry;     /* 退出地址 */

    p_frame->xPSR = (1u << 24);             /* THUMB */
    p_frame->control = 0x03;                /* 任务使用用户权限，栈使用 PSP */
    p_frame->exc_return = 0xFFFFFFFDul;     /* 任务在中断中切换，这里是退出中断的设置 */

    os_stack_t stack;
    stack.min = (os_uintptr_t)stack_base;
    stack.max = (os_uintptr_t)real_max;
    stack.type = kOsStackType_MaxToMin;
    stack.sp = p_frame;
    return stack;
}

void os_port_init(void){

}

void os_port_startup(void){
    NVIC_SetPriority(PendSV_IRQn, 0xff);
    NVIC_SetPriority(SVCall_IRQn, OS_CFG_SYSCALL_PRIORITY);
    SysTick_Config(SystemCoreClock/OS_CFG_TICKS_PER_SECOND);
}

void os_port_send_context_switch_request(void){
    SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
}

void os_port_clear_context_switch_request(void){
    SCB->ICSR &= ~SCB_ICSR_PENDSVSET_Msk;
}

void os_sched(void){
    if(is_cpu_in_privilege()){
        os_sched_in_isr();
    }else{
        cpu_sched_in_svc();
    }
}

void SysTick_Handler(void){
    os_systick_tick();
}

void SVC_Handler_C(os_uintptr_t* svc_args){
    // 读取系统调用号：它存储在原本的 PC 指令前两个字节
    uint8_t svc_number = ((uint8_t *)svc_args[6])[-2];
    switch (svc_number) {
        case CPU_SYSCALL_ID:{
            os_uint_t op = svc_args[0];
            switch (op) {
                case CPU_SYSCALL_OP_SAVE_DISABLE_IRQ:{
                    svc_args[0] = os_port_save_disable_irq_in_isr();
                    break;
                }
                case CPU_SYSCALL_OP_RESTORE_IRQ:{
                    os_port_restore_irq_in_isr(svc_args[1]);
                    break;
                }
                case CPU_SYSCALL_OP_SCHED:{
                    os_sched_in_isr();
                    break;
                }
                default:
                    break;
            }
            break;
        }
        default: break;
    }
}

/* ------------------------------------------------------------------------------------------------------------------ */
/*  */

// https://interrupt.memfault.com/blog/cortex-m-hardfault-debug
// Logic for dealing with the exception. Typically:
//  - log the fault which occurred for postmortem analysis
//  - If the fault is recoverable,
//    - clear errors and return back to Thread Mode
//  - else
//    - reboot system


//#define AUTO_REBOOT_ENABLE                  /* 允许自动重启 */
//#define OUTPUT_FAULT_TO_CONSOLE_ENABLE      /* 在调试终端输出运行时信息 */
//#define RECOVER_FROM_FAULT_ENABLE           /* 尝试从错误中继续运行 */

#if defined(RECOVER_FROM_FAULT_ENABLE)
static void recover_from_task_fault(void){
    while(1){
        os_task_yield();
    }
}
#endif

typedef OS_PACKED_STRUCT(ContextStateFrame) {
    uint32_t r0;
    uint32_t r1;
    uint32_t r2;
    uint32_t r3;
    uint32_t r12;
    uint32_t lr;
    uint32_t return_address;
    uint32_t xpsr;
} ContextStateFrame_t;

void HardFault_Handler_C(ContextStateFrame_t* frame, uint32_t lr_value){
    uint32_t bus_fault_address = SCB->BFAR;
    uint32_t mem_manage_fault_address = SCB->MMFAR;
    uint32_t cfsr = SCB->CFSR;
    uint32_t hfsr = SCB->HFSR;
    uint32_t dfsr = SCB->DFSR;
    uint32_t afsr = SCB->AFSR;

#if defined(AUTO_REBOOT_ENABLE)
    const uint32_t usage_fault_mask = 0xffff0000;
    const bool non_usage_fault_occurred =
            (cfsr & ~usage_fault_mask) != 0;
    // the bottom 8 bits of the xpsr hold the exception number of the
    // executing exception or 0 if the processor is in Thread mode
    const bool faulted_from_exception = ((frame->xpsr & 0xFF) != 0);

    if (faulted_from_exception || non_usage_fault_occurred) {
        // For any fault within an ISR or non-usage faults
        // let's reboot the system
        volatile uint32_t *aircr = (volatile uint32_t *)0xE000ED0C;
        *aircr = (0x05FA << 16) | 0x1 << 2;
        while (1) { } // should be unreachable
    }
#endif

#if defined(OUTPUT_FAULT_TO_CONSOLE_ENABLE)
    printf("[HardFault]\n");
    printf("- Stack frame:\n");
    printf(" R0    = %08"OS_PRIx"\n", frame->r0);
    printf(" R1    = %08"OS_PRIx"\n", frame->r1);
    printf(" R2    = %08"OS_PRIx"\n", frame->r2);
    printf(" R3    = %08"OS_PRIx"\n", frame->r3);
    printf(" R12   = %08"OS_PRIx"\n", frame->r12);
    printf(" LR    = %08"OS_PRIx"\n", frame->lr);
    printf(" PC    = %08"OS_PRIx"\n", frame->return_address);
    printf(" PSR   = %08"OS_PRIx"\n", frame->xpsr);
    printf("- FSR/FAR:\n");
    printf(" CFSR  = %08"OS_PRIx"\n", cfsr);
    printf(" HFSR  = %08"OS_PRIx"\n", hfsr);
    printf(" DFSR  = %08"OS_PRIx"\n", dfsr);
    printf(" AFSR  = %08"OS_PRIx"\n", afsr);
    if(cfsr & 0x0080){
        printf(" MMFAR = %08"OS_PRIx"\n", mem_manage_fault_address);
    }
    if(cfsr & 0x8000){
        printf(" BFAR  = %08"OS_PRIx"\n", bus_fault_address);
    }
    printf("- Misc:\n");
    printf(" LR/EXC_RETURN = %08"OS_PRIx"\n", lr_value);
#endif

#if defined(RECOVER_FROM_FAULT_ENABLE)
    // Clear any logged faults from the CFSR
    SCB->CFSR |= SCB->CFSR;
    // the instruction we will return to when we exit from the exception
    frame->return_address = (uint32_t)recover_from_task_fault;
    // the function we are returning to should never branch
    // so set lr to a pattern that would fault if it did
    frame->lr = 0xdeadbeef;
    // reset the psr state and only leave the
    // "thumb instruction interworking" bit set
    frame->xpsr = (1 << 24);
#else
    while(1){}
#endif
}