/**
 * @file port.c
 * @brief RISC-V RV32 architecture specific port
 * @note RISC-V is emerging in automotive applications
 */

#include "kernel.h"
#include "task.h"
#include "scheduler.h"
#include "portmacro.h"

/* Context storage */
uint32_t* current_task_sp = NULL;
uint32_t* next_task_sp = NULL;

/* Machine Timer registers */
#define MTIME_ADDR      0x0200BFF8
#define MTIMECMP_ADDR   0x02004000

/* Initialize Architecture Port */
void port_init(void) {
    /* Configure machine timer for 1ms interrupts */
    volatile uint32_t* mtime = (uint32_t*)MTIME_ADDR;
    volatile uint32_t* mtimecmp = (uint32_t*)MTIMECMP_ADDR;

    uint32_t current_time = *mtime;
    *mtimecmp = current_time + (SystemCoreClock / 1000);

    /* Enable machine timer interrupt */
    __asm volatile (
        "li t0, 0x80\n"          /* Machine Timer Interrupt Enable */
        "csrs mie, t0\n"
    );

    /* Enable global interrupts */
    __asm volatile (
        "csrsi mstatus, 0x8\n"   /* Machine Interrupt Enable */
    );
}

/* Start First Task */
void port_start_first_task(void) {
    TaskHandle_t first_task = scheduler_get_current_task();

    if (first_task == NULL) {
        return;
    }

    /* Load task stack pointer */
    __asm volatile (
        "mv sp, %0\n"
        "ret\n"
        :
        : "r" (first_task->stack_pointer)
    );
}

/* Context Switch */
void port_context_switch(uint32_t* current_context, uint32_t* next_context) {
    current_task_sp = current_context;
    next_task_sp = next_context;

    /* Trigger software interrupt */
    __asm volatile (
        "li t0, 0x8\n"           /* Machine Software Interrupt */
        "csrs mip, t0\n"
    );
}

/* Initialize Task Stack */
uint32_t* port_initialize_task_stack(TaskFunction_t task_function,
                                     void* parameters,
                                     uint32_t* stack_top) {
    uint32_t* stack_ptr = stack_top;

    /* Align to 16 bytes */
    stack_ptr = (uint32_t*)((uint32_t)stack_ptr & ~0xF);

    /* Initial stack frame */
    *(--stack_ptr) = (uint32_t)parameters;           /* a0 */
    *(--stack_ptr) = 0;                              /* a1 */
    *(--stack_ptr) = 0;                              /* a2 */
    *(--stack_ptr) = 0;                              /* a3 */
    *(--stack_ptr) = 0;                              /* a4 */
    *(--stack_ptr) = 0;                              /* a5 */
    *(--stack_ptr) = 0;                              /* a6 */
    *(--stack_ptr) = 0;                              /* a7 */

    *(--stack_ptr) = (uint32_t)task_function;        /* ra */
    *(--stack_ptr) = 0;                              /* t0 */
    *(--stack_ptr) = 0;                              /* t1 */
    *(--stack_ptr) = 0;                              /* t2 */
    *(--stack_ptr) = 0;                              /* t3 */
    *(--stack_ptr) = 0;                              /* t4 */
    *(--stack_ptr) = 0;                              /* t5 */
    *(--stack_ptr) = 0;                              /* t6 */

    *(--stack_ptr) = 0;                              /* s0 */
    *(--stack_ptr) = 0;                              /* s1 */
    *(--stack_ptr) = 0;                              /* s2 */
    *(--stack_ptr) = 0;                              /* s3 */
    *(--stack_ptr) = 0;                              /* s4 */
    *(--stack_ptr) = 0;                              /* s5 */
    *(--stack_ptr) = 0;                              /* s6 */
    *(--stack_ptr) = 0;                              /* s7 */
    *(--stack_ptr) = 0;                              /* s8 */
    *(--stack_ptr) = 0;                              /* s9 */
    *(--stack_ptr) = 0;                              /* s10 */
    *(--stack_ptr) = 0;                              /* s11 */

    *(--stack_ptr) = 0;                              /* gp */
    *(--stack_ptr) = 0;                              /* tp */

    /* mstatus */
    *(--stack_ptr) = 0x00001880;                     /* MPP = Machine mode */

    return stack_ptr;
}

/* Machine Timer Interrupt Handler */
void machine_timer_interrupt_handler(void) {
    /* Clear interrupt */
    volatile uint32_t* mtimecmp = (uint32_t*)MTIMECMP_ADDR;
    volatile uint32_t* mtime = (uint32_t*)MTIME_ADDR;
    *mtimecmp = *mtime + (SystemCoreClock / 1000);

    /* Call kernel tick handler */
    extern void kernel_tick_handler(void);
    kernel_tick_handler();
}

/* Machine Software Interrupt Handler */
void machine_software_interrupt_handler(void) {
    /* Clear interrupt */
    __asm volatile (
        "li t0, 0x8\n"
        "csrc mip, t0\n"
    );

    /* Perform context switch */
    __asm volatile (
        /* Save current context */
        "addi sp, sp, -128\n"
        "sw ra, 0(sp)\n"
        "sw t0, 4(sp)\n"
        "sw t1, 8(sp)\n"
        "sw t2, 12(sp)\n"
        "sw s0, 16(sp)\n"
        "sw s1, 20(sp)\n"
        "sw a0, 24(sp)\n"
        "sw a1, 28(sp)\n"
        "sw a2, 32(sp)\n"
        "sw a3, 36(sp)\n"
        "sw a4, 40(sp)\n"
        "sw a5, 44(sp)\n"
        "sw a6, 48(sp)\n"
        "sw a7, 52(sp)\n"
        "sw s2, 56(sp)\n"
        "sw s3, 60(sp)\n"
        "sw s4, 64(sp)\n"
        "sw s5, 68(sp)\n"
        "sw s6, 72(sp)\n"
        "sw s7, 76(sp)\n"
        "sw s8, 80(sp)\n"
        "sw s9, 84(sp)\n"
        "sw s10, 88(sp)\n"
        "sw s11, 92(sp)\n"
        "sw t3, 96(sp)\n"
        "sw t4, 100(sp)\n"
        "sw t5, 104(sp)\n"
        "sw t6, 108(sp)\n"
        "csrr t0, mstatus\n"
        "sw t0, 112(sp)\n"
        "csrr t0, mepc\n"
        "sw t0, 116(sp)\n"

        /* Save current stack pointer */
        "la t0, current_task_sp\n"
        "sw sp, 0(t0)\n"

        /* Load next stack pointer */
        "la t0, next_task_sp\n"
        "lw sp, 0(t0)\n"

        /* Restore next context */
        "lw ra, 0(sp)\n"
        "lw t0, 4(sp)\n"
        "lw t1, 8(sp)\n"
        "lw t2, 12(sp)\n"
        "lw s0, 16(sp)\n"
        "lw s1, 20(sp)\n"
        "lw a0, 24(sp)\n"
        "lw a1, 28(sp)\n"
        "lw a2, 32(sp)\n"
        "lw a3, 36(sp)\n"
        "lw a4, 40(sp)\n"
        "lw a5, 44(sp)\n"
        "lw a6, 48(sp)\n"
        "lw a7, 52(sp)\n"
        "lw s2, 56(sp)\n"
        "lw s3, 60(sp)\n"
        "lw s4, 64(sp)\n"
        "lw s5, 68(sp)\n"
        "lw s6, 72(sp)\n"
        "lw s7, 76(sp)\n"
        "lw s8, 80(sp)\n"
        "lw s9, 84(sp)\n"
        "lw s10, 88(sp)\n"
        "lw s11, 92(sp)\n"
        "lw t3, 96(sp)\n"
        "lw t4, 100(sp)\n"
        "lw t5, 104(sp)\n"
        "lw t6, 108(sp)\n"
        "lw t0, 112(sp)\n"
        "csrw mstatus, t0\n"
        "lw t0, 116(sp)\n"
        "csrw mepc, t0\n"
        "addi sp, sp, 128\n"

        /* Return from interrupt */
        "mret\n"
    );
}

/* Enter Critical Section */
void port_disable_interrupts(void) {
    __asm volatile (
        "csrci mstatus, 0x8\n"   /* Clear Machine Interrupt Enable */
    );
}

/* Exit Critical Section */
void port_enable_interrupts(void) {
    __asm volatile (
        "csrsi mstatus, 0x8\n"   /* Set Machine Interrupt Enable */
    );
}