/**
 * @file port_asm.s
 * @brief NXP S32K assembly routines
 */

.syntax unified
.thumb
.arch armv7e-m

.section .text

/* Global symbols */
.global current_task_sp
.global next_task_sp
.global port_context_switch
.global port_start_first_task
.global port_disable_interrupts
.global port_enable_interrupts

/* Variables */
.section .bss
.align 3
current_task_sp: .word 0
next_task_sp: .word 0

.section .text
.thumb_func

/* Start First Task */
port_start_first_task:
    /* Load task stack pointer */
    ldr r0, =next_task_sp
    ldr r1, [r0]

    /* Set PSP */
    msr PSP, r1

    /* Switch to PSP */
    mov r0, #2
    msr CONTROL, r0
    isb

    /* Restore context */
    ldmia r1!, {r4-r11}
    msr PSP, r1
    mov lr, #0xFFFFFFFD
    bx lr

/* PendSV Handler */
PendSV_Handler:
    /* Save context */
    mrs r0, PSP
    stmdb r0!, {r4-r11}
    ldr r1, =current_task_sp
    str r0, [r1]

    /* Load next context */
    ldr r0, =next_task_sp
    ldr r1, [r0]
    ldmia r1!, {r4-r11}
    msr PSP, r1
    bx lr

/* SVC Handler */
SVC_Handler:
    tst lr, #4
    ite eq
    mrseq r0, MSP
    mrsne r0, PSP
    ldr r0, [r0, #24]
    ldrb r0, [r0, #-2]
    push {lr}
    bl svc_handler
    pop {lr}
    bx lr

/* Disable Interrupts */
port_disable_interrupts:
    cpsid i
    bx lr

/* Enable Interrupts */
port_enable_interrupts:
    cpsie i
    bx lr

/* LPIT Interrupt Handler */
LPIT0_IRQHandler:
    /* Save context */
    push {r4-r11, lr}

    /* Clear interrupt flag */
    ldr r0, =0x40037008  /* LPIT0->MSR */
    movs r1, #1
    str r1, [r0]

    /* Call kernel tick handler */
    bl kernel_tick_handler

    /* Service watchdog */
    ldr r0, =0x40052000  /* WDOG->CNT */
    ldr r1, =0xB480
    str r1, [r0]
    ldr r1, =0x4B80
    str r1, [r0]

    /* Restore context */
    pop {r4-r11, lr}
    bx lr

/* Hard Fault Handler */
HardFault_Handler:
    /* Save context */
    mrs r0, PSP
    stmdb r0!, {r4-r11}

    /* Get fault status */
    ldr r2, =0xE000ED2C  /* HFSR */
    ldr r2, [r2]

    /* Call fault handler */
    movs r0, #1
    movs r1, #0
    bl fault_handler_process

    /* Safe state */
    b .

/* Memory Management Fault */
MemManage_Handler:
    /* Save context */
    mrs r0, PSP
    stmdb r0!, {r4-r11}

    /* Get fault information */
    ldr r1, =0xE000ED34  /* MMFAR */
    ldr r1, [r1]
    ldr r2, =0xE000ED28  /* CFSR */
    ldr r2, [r2]

    /* Call fault handler */
    movs r0, #2
    bl fault_handler_process

    /* Safe state */
    b .

/* Bus Fault Handler */
BusFault_Handler:
    /* Save context */
    mrs r0, PSP
    stmdb r0!, {r4-r11}

    /* Get fault information */
    ldr r1, =0xE000ED38  /* BFAR */
    ldr r1, [r1]
    ldr r2, =0xE000ED28  /* CFSR */
    ldr r2, [r2]

    /* Call fault handler */
    movs r0, #3
    bl fault_handler_process

    /* Safe state */
    b .

/* Usage Fault Handler */
UsageFault_Handler:
    /* Save context */
    mrs r0, PSP
    stmdb r0!, {r4-r11}

    /* Get fault status */
    ldr r2, =0xE000ED28  /* CFSR */
    ldr r2, [r2]

    /* Call fault handler */
    movs r0, #4
    movs r1, #0
    bl fault_handler_process

    /* Safe state */
    b .

.end