// vim:ft=arm64asm
#include <gic.h>
#include <gic.S>
#include <rt/arch/pseudo.S>
#include <rt/arch/sysreg.h>
#include <rt/task.h>
.macro irq_disable
msr daifset, 0b0010
.endm
.macro irq_enable
msr daifclr, 0b0010
.endm
.macro irq_nest_inc scratch
mrs \scratch, tpidrro_el0
add \scratch, \scratch, 1
msr tpidrro_el0, \scratch
.endm
.macro irq_nest_dec scratch
mrs \scratch, tpidrro_el0
sub \scratch, \scratch, 1
msr tpidrro_el0, \scratch
.endm
.macro pushfp
sub sp, sp, 0x210
mrs x10, fpcr
mrs x11, fpsr
stp x10, x11, [sp, 0x0]
stm q, 0, 31, sp, 0x10
.endm
.macro popfp
ldp x10, x11, [sp, 0x0]
ldm q, 0, 31, sp, 0x10
add sp, sp, 0x210
msr fpcr, x10
msr fpsr, x11
.endm
.macro pushnvgpr
stp x19, x20, [sp, -0x50]!
stm x, 21, 28, sp, 0x10
.endm
.macro popnvgpr
ldm x, 21, 28, sp, 0x10
ldp x19, x20, [sp], 0x50
.endm
.macro popvgpr
ldp x2, x3, [sp, 0x10]
ldm x, 4, 17, sp, 0x20
ldr x18, [sp, 0x90]
ldp x0, x1, [sp], 0xa0
.endm
.macro swapcontext
mrs x1, cpacr_el1
tbz x1, CPACR_EL1_FPEN_SHIFT, .Lskip_fp_save\@
pushfp
.Lskip_fp_save\@:
pushnvgpr
stp x1, x2, [sp, -0x10]!
// Store the stack pointer with the saved context.
adrp x2, rt_context_prev
add x2, x2, :lo12:rt_context_prev
ldr x2, [x2]
mov x3, sp
str x3, [x2]
// Load new task's ctx
ldr x3, [x0, RT_TASK_CTX_OFFSET]
mov sp, x3
ldp x1, x2, [sp], 0x10
msr cpacr_el1, x1
popnvgpr
tbz x1, CPACR_EL1_FPEN_SHIFT, .Lskip_fp_restore\@
isb
popfp
.Lskip_fp_restore\@:
.endm
.section .text.rt_svc_handler, "ax", %progbits
.global rt_svc_handler
.type rt_svc_handler, %function
rt_svc_handler:
/* Save elr and spsr to clobbered volatile registers so we can re-enable
* interrupts as quickly as possible.
* NOTE: x0-x3 are system call arguments, so we don't use those here. */
gic_svc_start
mrs x4, elr_el1
mrs x5, spsr_el1
irq_enable
// Save elr and spsr to the handler stack.
stp x4, x5, [sp, -0x10]!
bl rt_syscall_run
ldp x4, x5, [sp], 0x10
cbnz x0, .Lsvc_switch
irq_disable
gic_svc_finish
msr elr_el1, x4
msr spsr_el1, x5
eret
.Lsvc_switch:
/* Save fp, lr, elr, and spsr on the task stack. The task expects other
* volatile state to be clobbered by the SVC interface. */
msr spsel, 0
stp fp, lr, [sp, -0x20]!
stp x4, x5, [sp, 0x10]
mov x2, xzr
swapcontext
cbz x2, .Lsvc_skip_popvgpr
popvgpr
.Lsvc_skip_popvgpr:
irq_disable
/* Use fp and lr as scratch registers for gic_svc_finish and to restore elr
* and spsr, before loading the new task's fp and lr. */
gic_svc_finish
ldp fp, lr, [sp, 0x10]
msr elr_el1, fp
msr spsr_el1, lr
ldp fp, lr, [sp], 0x20
eret
.size rt_svc_handler, .-rt_svc_handler
.macro syscall_sgi_eoir
.if GIC_INTID_SYSCALL == 0
msr icc_eoir1_el1, xzr
.else
mov lr, GIC_INTID_SYSCALL
msr icc_eoir1_el1, lr
.endif
.endm
.section .text.rt_irq_handler, "ax", %progbits
.global rt_irq_handler
.type rt_irq_handler, %function
rt_irq_handler:
stp x0, x1, [sp, -0xc0]!
mrs x0, icc_iar1_el1
mrs x1, elr_el1
stp x2, x3, [sp, 0x10]
mrs x2, spsr_el1
irq_nest_inc x3
irq_enable
.if GIC_INTID_SYSCALL == 0
cbz x0, .Lsyscall_irq
.else
cmp x0, GIC_INTID_SYSCALL
beq .Lsyscall_irq
.endif
// Ordinary interrupt: save remaining volatile state and call the handler.
stm x, 4, 17, sp, 0x20
stp x18, x0, [sp, 0x90] // Store interrupt id in padding slot.
stp fp, lr, [sp, 0xa0]
stp x1, x2, [sp, 0xb0] // elr, spsr
adrp x1, gic_vector
add x1, x1, :lo12:gic_vector
ldr x1, [x1, x0, lsl 3]
blr x1
ldm x, 4, 17, sp, 0x20
ldp x18, x0, [sp, 0x90] // Load saved interrupt id from padding slot.
ldp fp, lr, [sp, 0xa0]
ldp x1, x2, [sp, 0xb0] // elr, spsr
irq_disable
cmp x0, GIC_INTID_GROUP0
bhs .Lno_eoir
msr icc_eoir1_el1, x0
.Lno_eoir:
irq_nest_dec x3
msr elr_el1, x1
msr spsr_el1, x2
ldp x2, x3, [sp, 0x10]
ldp x0, x1, [sp], 0xc0
eret
.Lsyscall_irq:
/* Save the volatile context (excluding x0-x3) on the task stack in case
* a context switch is required. */
msr spsel, 0
stp x4, x5, [sp, -0xa0]!
stm x, 6, 17, sp, 0x10
str x18, [sp, 0x70]
stp fp, lr, [sp, 0x80]
stp x1, x2, [sp, 0x90]
msr spsel, 1
bl rt_syscall_run_pending
cbnz x0, .Lsyscall_irq_switch
// x0-x3 are still on the handler stack.
ldp x2, x3, [sp, 0x10]
ldp x0, x1, [sp], 0xc0
msr spsel, 0
ldm x, 6, 17, sp, 0x10
ldr x18, [sp, 0x70]
irq_disable
syscall_sgi_eoir
irq_nest_dec x3
ldp x4, x5, [sp, 0x90]
msr elr_el1, x4
msr spsr_el1, x5
ldp fp, lr, [sp, 0x80]
ldp x4, x5, [sp], 0xa0
eret
.Lsyscall_irq_switch:
/* Load the saved x0-x3 from the handler stack and extend the task stack
* to prepend them, making the frame popvgpr-compatible.
* NOTE: x0 still contains the new task pointer so we don't use it to do
* this move. We can use other volatile registers because they are already
* saved. */
ldp x6, x7, [sp, 0x10]
ldp x4, x5, [sp], 0xc0
msr spsel, 0
stp x4, x5, [sp, -0x20]!
stp x6, x7, [sp, 0x10]
mov x2, 1
swapcontext
cbz x2, .Lsyscall_irq_skip_popvgpr
popvgpr
.Lsyscall_irq_skip_popvgpr:
irq_disable
syscall_sgi_eoir
irq_nest_dec x3
ldp fp, lr, [sp, 0x10]
msr elr_el1, fp
msr spsr_el1, lr
ldp fp, lr, [sp], 0x20
eret
.size rt_irq_handler, .-rt_irq_handler
.section .text.rt_irq_handler_nested, "ax", %progbits
.global rt_irq_handler_nested
.type rt_irq_handler_nested, %function
rt_irq_handler_nested:
// Save x0-x3, then use them to read iar, elr, and spsr.
stp x0, x1, [sp, -0xc0]!
mrs x0, icc_iar1_el1
mrs x1, elr_el1
stp x2, x3, [sp, 0x10]
mrs x2, spsr_el1
irq_nest_inc x3
irq_enable
stm x, 4, 17, sp, 0x20
stp x18, x0, [sp, 0x90] // Store interrupt id in padding slot.
stp fp, lr, [sp, 0xa0]
stp x1, x2, [sp, 0xb0] // elr, spsr
adrp x1, gic_vector
add x1, x1, :lo12:gic_vector
ldr x1, [x1, x0, lsl 3]
blr x1
ldm x, 4, 17, sp, 0x20
ldp x18, x0, [sp, 0x90] // Load saved interrupt id from padding slot.
ldp fp, lr, [sp, 0xa0]
irq_disable
cmp x0, GIC_INTID_GROUP0
bhs .Lnested_no_eoir
msr icc_eoir1_el1, x0
.Lnested_no_eoir:
irq_nest_dec x3
ldp x1, x2, [sp, 0xb0]
msr elr_el1, x1
msr spsr_el1, x2
ldp x2, x3, [sp, 0x10]
ldp x0, x1, [sp], 0xc0
eret
.size rt_irq_handler_nested, .-rt_irq_handler_nested
.section .text.rt_start, "ax", %progbits
.global rt_start
.type rt_start, %function
rt_start:
bl rt_start_sched
ldr x4, [x0, RT_TASK_CTX_OFFSET]
msr spsel, 0
mov sp, x4
// Load the initial cpacr_el1, skipping non-volatile registers.
ldr x5, [sp], 0x60
msr cpacr_el1, x5
// Load the initialized parts of the context.
ldp fp, lr, [sp, 0xa0]
ldp x2, x3, [sp, 0xb0]
ldp x0, x1, [sp], 0xc0
msr elr_el1, x2
msr spsr_el1, x3
// Set initial interrupt nesting count.
msr tpidrro_el0, xzr
eret
.size rt_start, .-rt_start
.section .text.rt_task_entry, "ax", %progbits
.global rt_task_entry
.type rt_task_entry, %function
rt_task_entry:
blr x1
b rt_task_exit
.size rt_task_entry, .-rt_task_entry