path: root/components/lwp/arch/aarch64/cortex-a/lwp_gcc.S

/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-05-18     Jesven       first version
 */

#include "rtconfig.h"

#include "asm-fpu.h"

/*********************
 *      SPSR BIT     *
 *********************/

#define  SPSR_Mode(v)          ((v) << 0)
#define  SPSR_A64              (0 << 4)
#define  SPSR_RESEVRED_5       (0 << 5)
#define  SPSR_FIQ_MASKED(v)    ((v) << 6)
#define  SPSR_IRQ_MASKED(v)    ((v) << 7)
#define  SPSR_SERROR_MASKED(v) ((v) << 8)
#define  SPSR_D_MASKED(v)      ((v) << 9)
#define  SPSR_RESEVRED_10_19   (0 << 10)
#define  SPSR_IL(v)            ((v) << 20)
#define  SPSR_SS(v)            ((v) << 21)
#define  SPSR_RESEVRED_22_27   (0 << 22)
#define  SPSR_V(v)             ((v) << 28)
#define  SPSR_C(v)             ((v) << 29)
#define  SPSR_Z(v)             ((v) << 30)
#define  SPSR_N(v)             ((v) << 31)

/*********************
 *   CONTEXT_OFFSET  *
 *********************/

#define CONTEXT_OFFSET_ELR_EL1    0x0
#define CONTEXT_OFFSET_SPSR_EL1   0x8
#define CONTEXT_OFFSET_SP_EL0     0x10
#define CONTEXT_OFFSET_X30        0x18
#define CONTEXT_OFFSET_FPCR       0x20
#define CONTEXT_OFFSET_FPSR       0x28
#define CONTEXT_OFFSET_X28        0x30
#define CONTEXT_OFFSET_X29        0x38
#define CONTEXT_OFFSET_X26        0x40
#define CONTEXT_OFFSET_X27        0x48
#define CONTEXT_OFFSET_X24        0x50
#define CONTEXT_OFFSET_X25        0x58
#define CONTEXT_OFFSET_X22        0x60
#define CONTEXT_OFFSET_X23        0x68
#define CONTEXT_OFFSET_X20        0x70
#define CONTEXT_OFFSET_X21        0x78
#define CONTEXT_OFFSET_X18        0x80
#define CONTEXT_OFFSET_X19        0x88
#define CONTEXT_OFFSET_X16        0x90
#define CONTEXT_OFFSET_X17        0x98
#define CONTEXT_OFFSET_X14        0xa0
#define CONTEXT_OFFSET_X15        0xa8
#define CONTEXT_OFFSET_X12        0xb0
#define CONTEXT_OFFSET_X13        0xb8
#define CONTEXT_OFFSET_X10        0xc0
#define CONTEXT_OFFSET_X11        0xc8
#define CONTEXT_OFFSET_X8         0xd0
#define CONTEXT_OFFSET_X9         0xd8
#define CONTEXT_OFFSET_X6         0xe0
#define CONTEXT_OFFSET_X7         0xe8
#define CONTEXT_OFFSET_X4         0xf0
#define CONTEXT_OFFSET_X5         0xf8
#define CONTEXT_OFFSET_X2         0x100
#define CONTEXT_OFFSET_X3         0x108
#define CONTEXT_OFFSET_X0         0x110
#define CONTEXT_OFFSET_X1         0x118

#define CONTEXT_OFFSET_Q15        0x120
#define CONTEXT_OFFSET_Q14        0x130
#define CONTEXT_OFFSET_Q13        0x140
#define CONTEXT_OFFSET_Q12        0x150
#define CONTEXT_OFFSET_Q11        0x160
#define CONTEXT_OFFSET_Q10        0x170
#define CONTEXT_OFFSET_Q9         0x180
#define CONTEXT_OFFSET_Q8         0x190
#define CONTEXT_OFFSET_Q7         0x1a0
#define CONTEXT_OFFSET_Q6         0x1b0
#define CONTEXT_OFFSET_Q5         0x1c0
#define CONTEXT_OFFSET_Q4         0x1d0
#define CONTEXT_OFFSET_Q3         0x1e0
#define CONTEXT_OFFSET_Q2         0x1f0
#define CONTEXT_OFFSET_Q1         0x200
#define CONTEXT_OFFSET_Q0         0x210

#define CONTEXT_FPU_SIZE          0x100
#define CONTEXT_SIZE              0x220

/**************************************************/

.text

/*
 * void arch_start_umode(args, text, ustack, kstack);
 */
.global arch_start_umode
.type arch_start_umode, % function
arch_start_umode:
    mov sp, x3
    mov x4, #(SPSR_Mode(0) | SPSR_A64)
    mov x3, x2 ;/* user stack top */
    msr daifset, #3
    dsb sy
    mrs x30, sp_el0
    msr spsr_el1, x4
    msr elr_el1, x1
    eret

/*
 * void arch_crt_start_umode(args, text, ustack, kstack);
 */
.global arch_crt_start_umode
.type arch_crt_start_umode, % function
arch_crt_start_umode:
    sub x4, x2, #0x10
    adr x2, lwp_thread_return
    ldr x5, [x2]
    str x5, [x4]
    ldr x5, [x2, #4]
    str x5, [x4, #4]
    ldr x5, [x2, #8]
    str x5, [x4, #8]

    mov x5, x4
    dc cvau, x5
    add x5, x5, #8
    dc cvau, x5
    dsb sy
    ic ialluis
    dsb sy

    msr sp_el0, x4

    mov sp, x3
    mov x4, #(SPSR_Mode(0) | SPSR_A64)
    msr daifset, #3
    dsb sy
    mrs x30, sp_el0
    msr spsr_el1, x4
    msr elr_el1, x1
    eret

/*
void arch_set_thread_context(void *exit_addr, void *new_thread_stack, void *user_stack, void **thread_sp);
*/
.global arch_set_thread_context
arch_set_thread_context:
    sub x1, x1, #CONTEXT_SIZE
    str x2, [x1, #CONTEXT_OFFSET_SP_EL0]
    sub x1, x1, #CONTEXT_SIZE
    str xzr, [x1, #CONTEXT_OFFSET_X0]       /* new thread return 0 */
    mov x4, #((3 << 6) | 0x4 | 0x1)         /* el1h, disable interrupt */
    str x4, [x1, #CONTEXT_OFFSET_SPSR_EL1]
    str x0, [x1, #CONTEXT_OFFSET_ELR_EL1]
    str x1, [x3]
    ret

.global arch_get_user_sp
arch_get_user_sp:
    mrs x0, sp_el0
    ret

.global arch_fork_exit
.global arch_clone_exit
arch_fork_exit:
arch_clone_exit:
    b  arch_syscall_exit

/*
void lwp_exec_user(void *args, void *kernel_stack, void *user_entry)
*/
.global lwp_exec_user
lwp_exec_user:
    mov sp, x1
    mov x4, #(SPSR_Mode(0) | SPSR_A64)
    ldr x3, =0x0000ffff80000000
    msr daifset, #3
    msr spsr_el1, x4
    msr elr_el1, x2
    eret

/*
 * void SVC_Handler(regs);
 */
.global SVC_Handler
.type SVC_Handler, % function
SVC_Handler:
    /* x0 is initial sp */
    mov sp, x0

    msr daifclr, #3  /* enable interrupt */

    bl  rt_thread_self
    bl  lwp_user_setting_save

    ldp x8, x9, [sp, #(CONTEXT_OFFSET_X8)]
    and x0, x8, #0xf000
    cmp x0, #0xe000
    beq arch_signal_quit

    cmp x0, #0xf000
    beq ret_from_user

    uxtb x0, w8
    bl lwp_get_sys_api
    cmp x0, xzr
    mov x30, x0
    beq arch_syscall_exit
    ldp x0, x1, [sp, #(CONTEXT_OFFSET_X0)]
    ldp x2, x3, [sp, #(CONTEXT_OFFSET_X2)]
    ldp x4, x5, [sp, #(CONTEXT_OFFSET_X4)]
    ldp x6, x7, [sp, #(CONTEXT_OFFSET_X6)]
    blr x30
    /* jump explictly, make this code position independant */
    b arch_syscall_exit

.global arch_syscall_exit
arch_syscall_exit:
    msr daifset, #3

    ldp x2, x3, [sp], #0x10  /* SPSR and ELR. */
    msr spsr_el1, x3
    msr elr_el1, x2

    ldp x29, x30, [sp], #0x10
    msr sp_el0, x29
    ldp x28, x29, [sp], #0x10
    msr fpcr, x28
    msr fpsr, x29
    ldp x28, x29, [sp], #0x10
    ldp x26, x27, [sp], #0x10
    ldp x24, x25, [sp], #0x10
    ldp x22, x23, [sp], #0x10
    ldp x20, x21, [sp], #0x10
    ldp x18, x19, [sp], #0x10
    ldp x16, x17, [sp], #0x10
    ldp x14, x15, [sp], #0x10
    ldp x12, x13, [sp], #0x10
    ldp x10, x11, [sp], #0x10
    ldp x8, x9, [sp], #0x10
    add sp, sp, #0x40
    RESTORE_FPU sp

.global arch_ret_to_user
arch_ret_to_user:
    SAVE_FPU sp
    stp x0, x1, [sp, #-0x10]!
    stp x2, x3, [sp, #-0x10]!
    stp x4, x5, [sp, #-0x10]!
    stp x6, x7, [sp, #-0x10]!
    stp x8, x9, [sp, #-0x10]!
    stp x10, x11, [sp, #-0x10]!
    stp x12, x13, [sp, #-0x10]!
    stp x14, x15, [sp, #-0x10]!
    stp x16, x17, [sp, #-0x10]!
    stp x18, x19, [sp, #-0x10]!
    stp x20, x21, [sp, #-0x10]!
    stp x22, x23, [sp, #-0x10]!
    stp x24, x25, [sp, #-0x10]!
    stp x26, x27, [sp, #-0x10]!
    stp x28, x29, [sp, #-0x10]!

    mrs x0, fpcr
    mrs x1, fpsr
    stp x0, x1, [sp, #-0x10]!
    stp x29, x30, [sp, #-0x10]!

    bl lwp_check_debug
    bl lwp_check_exit_request
    cbz w0, 1f
    mov x0, xzr
    b sys_exit
1:
    ldr x0, =rt_dbg_ops
    ldr x0, [x0]
    cbz x0, 3f
    bl dbg_thread_in_debug
    mov x1, #(1 << 21)
    mrs x2, spsr_el1
    cbz w0, 2f
    orr x2, x2, x1
    msr spsr_el1, x2
    b 3f
2:
    bic x2, x2, x1
    msr spsr_el1, x2
3:
    bl lwp_signal_check
    cmp x0, xzr

    ldp x29, x30, [sp], #0x10
    ldp x0, x1, [sp], #0x10
    msr fpcr, x0
    msr fpsr, x1

    ldp x28, x29, [sp], #0x10
    ldp x26, x27, [sp], #0x10
    ldp x24, x25, [sp], #0x10
    ldp x22, x23, [sp], #0x10
    ldp x20, x21, [sp], #0x10
    ldp x18, x19, [sp], #0x10
    ldp x16, x17, [sp], #0x10
    ldp x14, x15, [sp], #0x10
    ldp x12, x13, [sp], #0x10
    ldp x10, x11, [sp], #0x10
    ldp x8, x9, [sp], #0x10
    ldp x6, x7, [sp], #0x10
    ldp x4, x5, [sp], #0x10
    ldp x2, x3, [sp], #0x10
    ldp x0, x1, [sp], #0x10
    RESTORE_FPU sp

    bne user_do_signal

    stp x0, x1, [sp, #-0x10]!
    ldr x0, =rt_dbg_ops
    ldr x0, [x0]
    cmp x0, xzr
    ldp x0, x1, [sp], #0x10
    beq 1f
    SAVE_FPU sp
    stp x0, x1, [sp, #-0x10]!
    stp x2, x3, [sp, #-0x10]!
    stp x4, x5, [sp, #-0x10]!
    stp x6, x7, [sp, #-0x10]!
    stp x8, x9, [sp, #-0x10]!
    stp x10, x11, [sp, #-0x10]!
    stp x12, x13, [sp, #-0x10]!
    stp x14, x15, [sp, #-0x10]!
    stp x16, x17, [sp, #-0x10]!
    stp x18, x19, [sp, #-0x10]!
    stp x20, x21, [sp, #-0x10]!
    stp x22, x23, [sp, #-0x10]!
    stp x24, x25, [sp, #-0x10]!
    stp x26, x27, [sp, #-0x10]!
    stp x28, x29, [sp, #-0x10]!
    mrs x0, fpcr
    mrs x1, fpsr
    stp x0, x1, [sp, #-0x10]!
    stp x29, x30, [sp, #-0x10]!
    mrs x0, elr_el1
    bl dbg_attach_req
    ldp x29, x30, [sp], #0x10
    ldp x0, x1, [sp], #0x10
    msr fpcr, x0
    msr fpsr, x1
    ldp x28, x29, [sp], #0x10
    ldp x26, x27, [sp], #0x10
    ldp x24, x25, [sp], #0x10
    ldp x22, x23, [sp], #0x10
    ldp x20, x21, [sp], #0x10
    ldp x18, x19, [sp], #0x10
    ldp x16, x17, [sp], #0x10
    ldp x14, x15, [sp], #0x10
    ldp x12, x13, [sp], #0x10
    ldp x10, x11, [sp], #0x10
    ldp x8, x9, [sp], #0x10
    ldp x6, x7, [sp], #0x10
    ldp x4, x5, [sp], #0x10
    ldp x2, x3, [sp], #0x10
    ldp x0, x1, [sp], #0x10
    RESTORE_FPU sp
1:
    eret

/*
struct rt_hw_exp_stack
{
    unsigned long pc;     0
    unsigned long cpsr;
    unsigned long sp_el0; 0x10
    unsigned long x30;
    unsigned long fpcr;   0x20
    unsigned long fpsr;
    unsigned long x28;    0x30
    unsigned long x29;
    unsigned long x26;    0x40
    unsigned long x27;
    unsigned long x24;    0x50
    unsigned long x25;
    unsigned long x22;    0x60
    unsigned long x23;
    unsigned long x20;    0x70
    unsigned long x21;
    unsigned long x18;    0x80
    unsigned long x19;
    unsigned long x16;    0x90
    unsigned long x17;
    unsigned long x14;    0xa0
    unsigned long x15;
    unsigned long x12;    0xb0
    unsigned long x13;
    unsigned long x10;    0xc0
    unsigned long x11;
    unsigned long x8;     0xd0
    unsigned long x9;
    unsigned long x6;     0xe0
    unsigned long x7;
    unsigned long x4;     0xf0
    unsigned long x5;
    unsigned long x2;     0x100
    unsigned long x3;
    unsigned long x0;     0x110
    unsigned long x1;

    unsigned long long fpu[16]; 0x120
                                0x220 = 0x120 + 0x10 * 0x10
};
*/
.global lwp_check_debug
lwp_check_debug:
    ldr x0, =rt_dbg_ops
    ldr x0, [x0]
    cbnz x0, 1f
    ret
1:
    stp x29, x30, [sp, #-0x10]!
    bl dbg_check_suspend
    cbz w0, lwp_check_debug_quit

    mrs x2, sp_el0
    sub x2, x2, #0x10
    mov x3, x2
    msr sp_el0, x2
    ldr x0, =lwp_debugreturn
    ldr w1, [x0]
    str w1, [x2]
    ldr w1, [x0, #4]
    str w1, [x2, #4]

    dc cvau, x2
    add x2, x2, #4
    dc cvau, x2

    dsb sy
    isb sy

    ic ialluis
    isb sy

    mrs x0, elr_el1
    mrs x1, spsr_el1
    stp x0, x1, [sp, #-0x10]!
    msr elr_el1, x3 /* lwp_debugreturn */
    mov x1, #(SPSR_Mode(0) | SPSR_A64)
    orr x1, x1, #(1 << 21)
    msr spsr_el1, x1
    eret
ret_from_user:
    /* sp_el0 += 16 for drop ins lwp_debugreturn */
    mrs x0, sp_el0
    add x0, x0, #0x10
    msr sp_el0, x0
    /* now is el1, sp is pos(empty) - sizeof(context) */
    mov x0, sp
    add x0, x0, #0x220
    mov sp, x0
    ldp x0, x1, [sp], #0x10  /* x1 is origin spsr_el1 */
    msr elr_el1, x0          /* x0 is origin elr_el1 */
    msr spsr_el1, x1
lwp_check_debug_quit:
    ldp x29, x30, [sp], #0x10
    ret

arch_signal_quit:
    msr daifset, #3
/*
    drop stack data
*/
    add sp, sp, #CONTEXT_SIZE
    bl lwp_signal_restore
    /* x0 is user_ctx : ori sp, pc, cpsr */
    ldr x1, [x0]
    ldr x2, [x0, #8]
    ldr x3, [x0, #16]
    msr spsr_el1, x3
    msr elr_el1, x2
    add x1, x1, #16
    msr sp_el0, x1

    msr spsel, #0

    ldp x29, x30, [sp], #0x10
    ldp x28, x29, [sp], #0x10
    msr fpcr, x28
    msr fpsr, x29
    ldp x28, x29, [sp], #0x10
    ldp x26, x27, [sp], #0x10
    ldp x24, x25, [sp], #0x10
    ldp x22, x23, [sp], #0x10
    ldp x20, x21, [sp], #0x10
    ldp x18, x19, [sp], #0x10
    ldp x16, x17, [sp], #0x10
    ldp x14, x15, [sp], #0x10
    ldp x12, x13, [sp], #0x10
    ldp x10, x11, [sp], #0x10
    ldp x8, x9, [sp], #0x10
    ldp x6, x7, [sp], #0x10
    ldp x4, x5, [sp], #0x10
    ldp x2, x3, [sp], #0x10
    ldp x0, x1, [sp], #0x10
    RESTORE_FPU sp

    msr spsel, #1

    b arch_ret_to_user

user_do_signal:
    msr spsel, #0
    SAVE_FPU sp
    stp x0, x1, [sp, #-0x10]!
    stp x2, x3, [sp, #-0x10]!
    stp x4, x5, [sp, #-0x10]!
    stp x6, x7, [sp, #-0x10]!
    stp x8, x9, [sp, #-0x10]!
    stp x10, x11, [sp, #-0x10]!
    stp x12, x13, [sp, #-0x10]!
    stp x14, x15, [sp, #-0x10]!
    stp x16, x17, [sp, #-0x10]!
    stp x18, x19, [sp, #-0x10]!
    stp x20, x21, [sp, #-0x10]!
    stp x22, x23, [sp, #-0x10]!
    stp x24, x25, [sp, #-0x10]!
    stp x26, x27, [sp, #-0x10]!
    stp x28, x29, [sp, #-0x10]!
    mrs x28, fpcr
    mrs x29, fpsr
    stp x28, x29, [sp, #-0x10]!
    stp x29, x30, [sp, #-0x10]!

    sub sp, sp, #0x10
    adr x0, lwp_sigreturn
    ldr w1, [x0]
    str w1, [sp]
    ldr w1, [x0, #4]
    str w1, [sp, #4]

    mov x20, sp /* lwp_sigreturn */
    mov x0, sp

    dc cvau, x0
    dsb sy
    ic ialluis
    dsb sy

    msr spsel, #1

    mrs x1, elr_el1
    mrs x2, spsr_el1
    bl lwp_signal_backup
    /* x0 is signal */
    mov x19, x0
    bl lwp_sighandler_get
    adds x1, x0, xzr
    mov x0, x19
    bne 1f
    mov x1, x20
1:
    msr elr_el1, x1
    mov x30, x20
    eret

lwp_debugreturn:
    mov x8, 0xf000
    svc #0

lwp_sigreturn:
    mov x8, #0xe000
    svc #0

lwp_thread_return:
    mov x0, xzr
    mov x8, #0x01
    svc #0

.globl arch_get_tidr
arch_get_tidr:
    mrs x0, tpidr_el0
    ret

.global arch_set_thread_area
arch_set_thread_area:
.globl arch_set_tidr
arch_set_tidr:
    msr tpidr_el0, x0
    ret