#include <os_isr.h>
#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"
#include "cpu.h"

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


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

OS_STATIC_FORCE_INLINE
uint32_t _SysTick_Config(os_tick_t ticks)
{


    NVIC->CFGR = 0xFA050003; /* 关闭硬件压栈和嵌套 */

    NVIC_SetPriority(SysTick_IRQn,0xf0);
    NVIC_SetPriority(Software_IRQn,0xf0);

    SysTick->CTLR=0;
    SysTick->CNTL0=0;SysTick->CNTL1=0;SysTick->CNTL2=0;SysTick->CNTL3=0;
    SysTick->CNTH0=0;SysTick->CNTH1=0;SysTick->CNTH2=0;SysTick->CNTH3=0;
    SysTick->CMPLR0=(uint8_t)(ticks-1);
    SysTick->CMPLR1=(uint8_t)((ticks-1)>>8);
    SysTick->CMPLR2=(uint8_t)((ticks-1)>>16);
    SysTick->CMPLR3=(uint8_t)((ticks-1)>>24);
    SysTick->CMPHR0=0;SysTick->CMPHR1=0;SysTick->CMPHR2=0;SysTick->CMPHR3=0;
    SysTick->CTLR=0x1;

    NVIC_EnableIRQ(SysTick_IRQn);
    NVIC_EnableIRQ(Software_IRQn);
    return 0;
}

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

typedef struct {
    os_uintptr_t mstatus;       /* zero, 用于 mstatus 保存 */
    os_uintptr_t ra;            /* 返回地址 */
    os_uintptr_t mepc;          /* 栈指针, mepc */
    os_uintptr_t x3;            /* 全局指针 */
    os_uintptr_t x4;            /* 线程指针 */
    os_uintptr_t x5;            /* 临时寄存器0 */
    os_uintptr_t x6;            /* 临时寄存器1 */
    os_uintptr_t x7;            /* 临时寄存器2 */
    os_uintptr_t x8;            /* s0/fp */
    os_uintptr_t x9;            /* 保存寄存器1 */
    os_uintptr_t a0;            /* 函数参数/返回值 */
    os_uintptr_t x11;           /* 函数参数 */
    os_uintptr_t x12;           /* 函数参数 */
    os_uintptr_t x13;           /* 函数参数 */
    os_uintptr_t x14;           /* 函数参数 */
    os_uintptr_t x15;           /* 函数参数 */
    os_uintptr_t x16;           /* 函数参数 */
    os_uintptr_t x17;           /* 函数参数 */
    os_uintptr_t x18;           /* 保存寄存器 */
    os_uintptr_t x19;           /* 保存寄存器 */
    os_uintptr_t x20;           /* 保存寄存器 */
    os_uintptr_t x21;           /* 保存寄存器 */
    os_uintptr_t x22;           /* 保存寄存器 */
    os_uintptr_t x23;           /* 保存寄存器 */
    os_uintptr_t x24;           /* 保存寄存器 */
    os_uintptr_t x25;           /* 保存寄存器 */
    os_uintptr_t x26;           /* 保存寄存器 */
    os_uintptr_t x27;           /* 保存寄存器 */
    os_uintptr_t x28;           /* 临时寄存器 */
    os_uintptr_t x29;           /* 临时寄存器 */
    os_uintptr_t x30;           /* 临时寄存器 */
    os_uintptr_t x31;           /* 临时寄存器 */
#if (OS_CFG_CPU_FPU_PRESENT)
    os_uintptr_t f0;
    os_uintptr_t f1;
    os_uintptr_t f2;
    os_uintptr_t f3;
    os_uintptr_t f4;
    os_uintptr_t f5;
    os_uintptr_t f6;
    os_uintptr_t f7;
    os_uintptr_t f8;
    os_uintptr_t f9;
    os_uintptr_t f10;
    os_uintptr_t f11;
    os_uintptr_t f12;
    os_uintptr_t f13;
    os_uintptr_t f14;
    os_uintptr_t f15;
    os_uintptr_t f16;
    os_uintptr_t f17;
    os_uintptr_t f18;
    os_uintptr_t f19;
    os_uintptr_t f20;
    os_uintptr_t f21;
    os_uintptr_t f22;
    os_uintptr_t f23;
    os_uintptr_t f24;
    os_uintptr_t f25;
    os_uintptr_t f26;
    os_uintptr_t f27;
    os_uintptr_t f28;
    os_uintptr_t f29;
    os_uintptr_t f30;
    os_uintptr_t f31;
#endif
}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 value = 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] = (value<<24) | (value<<16) | (value << 8) | value;
        value++;
    }

    p_frame->mstatus = 0x1880;
    p_frame->mepc = (os_uintptr_t)entry;
    p_frame->a0 = (os_uintptr_t)param;
    p_frame->ra = (os_uintptr_t)exit_entry;

    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_sched(void){
    os_sched_in_isr();
}


OS_WEAK
void os_port_init(void){

}

OS_WEAK
void os_port_startup(void){
    _SysTick_Config(SystemCoreClock/8/OS_CFG_TICKS_PER_SECOND);
}

/*
 * 发出上下文切换的请求
 */
void os_port_send_context_switch_request(void){
    NVIC_SetPendingIRQ(Software_IRQn);
}

/*
 * 清除上下文切换的请求
 */
void os_port_clear_context_switch_request(void){
    NVIC_ClearPendingIRQ(Software_IRQn);
}

extern void os_port_context_switch_to(os_uintptr_t to);

OS_USED
void os_port_context_switch(void* from_sp, void* to_sp){
    os_critical_enter_in_isr();
    if(g_os_port_context_switch_flag==OS_TRUE){
        os_critical_leave_in_isr();
        return;
    }
    g_os_port_switch_from_sp = from_sp;
    g_os_port_switch_to_sp = to_sp;
    g_os_port_context_switch_flag = OS_TRUE;
    if(from_sp==0){
        os_critical_leave_in_isr();
        os_port_context_switch_to((os_uintptr_t)to_sp);
    }else{
        os_critical_leave_in_isr();
        os_port_send_context_switch_request();
    }
}


void SysTick_Handler_C(void){
    SysTick->CTLR=0;
    SysTick->CNTL0=0;SysTick->CNTL1=0;SysTick->CNTL2=0;SysTick->CNTL3=0;
    SysTick->CNTH0=0;SysTick->CNTH1=0;SysTick->CNTH2=0;SysTick->CNTH3=0;
    SysTick->CTLR=0x1;
    // 调用系统心跳处理
    os_systick_tick();
}