@ vim:ft=arm

/* This variant of the syscall handler supports armv6-m and armv8-m.base.
 *
 * armv8-m.base also supports cbz while armv6-m does not, but otherwise they
 * support the same subset of armv{7,8}-m that makes the save/restore procedure
 * different, mainly due to the lack of stmdb and ldm.w/stm.w. */

#include <rt/mpu.h>

#include <rt/arch/pseudo.S>
#include <rt/arch/control.h>

#include "exc_return.h"

    .syntax unified

#if RT_MPU_TASK_REGIONS_ENABLE
.macro mpuconfigure
    mov32 r1, rt_active_task
    ldr r1, [r1]
    adds r1, RT_MPU_TASK_CONFIG_OFFSET
#if __ARM_ARCH == 8
    /* In armv8-m, the region number isn't part of the base address
     * register and must be set separately. */
    mov32 r2, RT_MPU_BASE + RT_MPU_REGION_NUMBER_OFFSET
#else // __ARM_ARCH == 6
    mov32 r2, RT_MPU_BASE + RT_MPU_REGION_OFFSET
#endif // __ARM_ARCH
    mpuset RT_MPU_TASK_REGION_START_ID, RT_MPU_NUM_TASK_REGIONS
    dsb
.endm

.macro mpuset r, n
    .if \n == 0
    .exitm
    .endif
    ldmia r1!, {r4-r5}
#if __ARM_ARCH == 8
    movs r3, \r
    stmia r2!, {r3, r4, r5}
    subs r2, 12
#else // __ARM_ARCH == 6
    stmia r2!, {r4, r5}
    subs r2, 8
#endif // __ARM_ARCH
    mpuset (\r + 1), (\n - 1)
.endm
#else // !RT_MPU_TASK_REGIONS_ENABLE
.macro mpuconfigure
.endm
#endif // RT_MPU_TASK_REGIONS_ENABLE

#if (__ARM_ARCH >= 7) && RT_MPU_TASK_REGIONS_ENABLE
#define CONTROL_SIZE 4
#define controltemp r2,
#define getcontrol mrs r2, control
#define setcontrol msr control, r2
#else // __ARM_ARCH <= 6 || !RT_MPU_TASK_REGIONS_ENABLE
#define CONTROL_SIZE 0
#define controltemp
#define getcontrol
#define setcontrol
#endif // (__ARM_ARCH >= 7) && RT_MPU_TASK_REGIONS_ENABLE

#if __ARM_ARCH == 8
#define PSPLIM_SIZE 4
#define psplimtemp r3,
#define getpsplim mrs r3, psplim
#define setpsplim msr psplim, r3
#else // __ARM_ARCH == 6
#define PSPLIM_SIZE 0
#define psplimtemp
#define getpsplim
#define setpsplim
#endif // __ARM_ARCH

#define NONVOLATILE_CONTEXT_SIZE (32 + PSPLIM_SIZE + CONTROL_SIZE)

.macro swapcontext
    // Store the suspending task's context to its stack.
    mrs r1, psp
    subs r1, NONVOLATILE_CONTEXT_SIZE

    mov32 r2, rt_context_prev
    ldr r2, [r2]
    str r1, [r2]

    getcontrol
    getpsplim
    stmia r1!, {controltemp psplimtemp r4-r7}
    mov r4, r8
    mov r5, r9
    mov r6, r10
    mov r7, r11
    stmia r1!, {r4-r7}

    mpuconfigure

    // Load the new context returned by rt_syscall_run_pending.
    mov r1, r0
    adds r0, NONVOLATILE_CONTEXT_SIZE - 16
    ldmia r0!, {r4-r7}
    mov r8, r4
    mov r9, r5
    mov r10, r6
    mov r11, r7
    ldmia r1!, {controltemp psplimtemp r4-r7}

    // Set the new stack pointer.
    msr psp, r0
    setcontrol
    setpsplim
.endm

.macro return
    movs r0, TASK_INITIAL_EXC_RETURN_NEG
    negs r0, r0
    bx r0
.endm

    .section .text.rt_svcall_handler, "ax", %progbits
    .global rt_svcall_handler
    .type rt_svcall_handler, %function
rt_svcall_handler:
    mrs r0, psp
    ldmia r0, {r0-r3}
    bl rt_syscall_run
    cbz r0, 0f
    swapcontext
0:  return

    .size rt_svcall_handler, .-rt_svcall_handler


    .section .text.rt_pendsv_handler, "ax", %progbits
    .global rt_pendsv_handler
    .type rt_pendsv_handler, %function
rt_pendsv_handler:
    bl rt_syscall_run_pending
    cbz r0, 0f
    swapcontext
0:  return

    .size rt_pendsv_handler, .-rt_pendsv_handler


    .section .text.rt_start, "ax", %progbits
    .global rt_start
    .type rt_start, %function
rt_start:
    bl rt_start_context
    mpuconfigure
#if RT_ARM_V8M
    ldr psplimtemp [r0, CONTROL_SIZE]
    setpsplim
#endif
    adds r0, NONVOLATILE_CONTEXT_SIZE
    msr psp, r0
    // Set the SPSEL bit in control to switch to the process stack pointer.
    movs r0, CONTROL_SPSEL
    msr control, r0
    isb
    // sp now points at the beginning of: {r0-r3, r12, lr, pc, psr}.
    // Load the arguments to rt_task_entry.
    pop {r0, r1}
    // Skip past r2, r12, lr, pc, psr.
    add sp, 24
    cpsie i
    // Fall through to rt_task_entry in the same section.

    .size rt_start, .-rt_start

    .global rt_task_entry
    .type rt_task_entry, %function
rt_task_entry:
    blx r1
    /* The call to rt_task_exit is a tail call to a non-returning function, so b or
     * bl could be used, but v6m and v8m.base do not have a long form of b. */
    bl rt_task_exit

    .size rt_task_entry, .-rt_task_entry