diff options
Diffstat (limited to 'libcpu')
41 files changed, 3540 insertions, 1091 deletions
diff --git a/libcpu/Kconfig b/libcpu/Kconfig index 0257f44190..daaad9af90 100644 --- a/libcpu/Kconfig +++ b/libcpu/Kconfig @@ -82,6 +82,7 @@ config ARCH_ARM_MMU if RT_USING_SMART config KERNEL_VADDR_START hex "The virtural address of kernel start" + default 0xffff000000000000 if ARCH_ARMV8 default 0xc0000000 if ARCH_ARM default 0x80000000 if ARCH_RISCV depends on ARCH_MM_MMU @@ -159,6 +160,7 @@ config RT_BACKTRACE_FUNCTION_NAME config ARCH_ARMV8 bool select ARCH_ARM + select ARCH_ARM_MMU config ARCH_MIPS bool diff --git a/libcpu/aarch64/common/armv8.h b/libcpu/aarch64/common/armv8.h index 99baa81c6e..d96a5af72a 100644 --- a/libcpu/aarch64/common/armv8.h +++ b/libcpu/aarch64/common/armv8.h @@ -1,67 +1,64 @@ /* - * Copyright (c) 2006-2021, RT-Thread Development Team + * Copyright (c) 2006-2020, RT-Thread Development Team * * SPDX-License-Identifier: Apache-2.0 * * Change Logs: * Date Author Notes * 2011-09-15 Bernard first version - * 2021-12-28 GuEe-GUI add fpu support */ #ifndef __ARMV8_H__ #define __ARMV8_H__ -#include <rtdef.h> - /* the exception stack without VFP registers */ struct rt_hw_exp_stack { - unsigned long long pc; - unsigned long long spsr; - unsigned long long x30; - unsigned long long xzr; - unsigned long long fpcr; - unsigned long long fpsr; - unsigned long long x28; - unsigned long long x29; - unsigned long long x26; - unsigned long long x27; - unsigned long long x24; - unsigned long long x25; - unsigned long long x22; - unsigned long long x23; - unsigned long long x20; - unsigned long long x21; - unsigned long long x18; - unsigned long long x19; - unsigned long long x16; - unsigned long long x17; - unsigned long long x14; - unsigned long long x15; - unsigned long long x12; - unsigned long long x13; - unsigned long long x10; - unsigned long long x11; - unsigned long long x8; - unsigned long long x9; - unsigned long long x6; - unsigned long long x7; - unsigned long long x4; - unsigned long long x5; - unsigned long long x2; - unsigned long long x3; - unsigned long long x0; - unsigned long long x1; + unsigned long pc; + unsigned long cpsr; + unsigned long sp_el0; + unsigned long x30; + unsigned long fpcr; + unsigned long fpsr; + unsigned long x28; + unsigned long x29; + unsigned long x26; + unsigned long x27; + unsigned long x24; + unsigned long x25; + unsigned long x22; + unsigned long x23; + unsigned long x20; + unsigned long x21; + unsigned long x18; + unsigned long x19; + unsigned long x16; + unsigned long x17; + unsigned long x14; + unsigned long x15; + unsigned long x12; + unsigned long x13; + unsigned long x10; + unsigned long x11; + unsigned long x8; + unsigned long x9; + unsigned long x6; + unsigned long x7; + unsigned long x4; + unsigned long x5; + unsigned long x2; + unsigned long x3; + unsigned long x0; + unsigned long x1; unsigned long long fpu[16]; }; -#define SP_ELx ( ( unsigned long long ) 0x01 ) -#define SP_EL0 ( ( unsigned long long ) 0x00 ) -#define PSTATE_EL1 ( ( unsigned long long ) 0x04 ) -#define PSTATE_EL2 ( ( unsigned long long ) 0x08 ) -#define PSTATE_EL3 ( ( unsigned long long ) 0x0c ) +#define SP_ELx ((unsigned long)0x01) +#define SP_EL0 ((unsigned long)0x00) +#define PSTATE_EL1 ((unsigned long)0x04) +#define PSTATE_EL2 ((unsigned long)0x08) +#define PSTATE_EL3 ((unsigned long)0x0c) rt_ubase_t rt_hw_get_current_el(void); void rt_hw_set_elx_env(void); diff --git a/libcpu/aarch64/common/asm-fpu.h b/libcpu/aarch64/common/asm-fpu.h new file mode 100644 index 0000000000..8ac4ab9bd8 --- /dev/null +++ b/libcpu/aarch64/common/asm-fpu.h @@ -0,0 +1,46 @@ +/* + * Copyright (c) 2006-2021, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2021-05-18 Jesven the first version + */ + +.macro SAVE_FPU, reg + STR Q0, [\reg, #-0x10]! + STR Q1, [\reg, #-0x10]! + STR Q2, [\reg, #-0x10]! + STR Q3, [\reg, #-0x10]! + STR Q4, [\reg, #-0x10]! + STR Q5, [\reg, #-0x10]! + STR Q6, [\reg, #-0x10]! + STR Q7, [\reg, #-0x10]! + STR Q8, [\reg, #-0x10]! + STR Q9, [\reg, #-0x10]! + STR Q10, [\reg, #-0x10]! + STR Q11, [\reg, #-0x10]! + STR Q12, [\reg, #-0x10]! + STR Q13, [\reg, #-0x10]! + STR Q14, [\reg, #-0x10]! + STR Q15, [\reg, #-0x10]! +.endm +.macro RESTORE_FPU, reg + LDR Q15, [\reg], #0x10 + LDR Q14, [\reg], #0x10 + LDR Q13, [\reg], #0x10 + LDR Q12, [\reg], #0x10 + LDR Q11, [\reg], #0x10 + LDR Q10, [\reg], #0x10 + LDR Q9, [\reg], #0x10 + LDR Q8, [\reg], #0x10 + LDR Q7, [\reg], #0x10 + LDR Q6, [\reg], #0x10 + LDR Q5, [\reg], #0x10 + LDR Q4, [\reg], #0x10 + LDR Q3, [\reg], #0x10 + LDR Q2, [\reg], #0x10 + LDR Q1, [\reg], #0x10 + LDR Q0, [\reg], #0x10 +.endm diff --git a/libcpu/aarch64/common/backtrace.c b/libcpu/aarch64/common/backtrace.c new file mode 100644 index 0000000000..c3e3803f93 --- /dev/null +++ b/libcpu/aarch64/common/backtrace.c @@ -0,0 +1,57 @@ +/* + * Copyright (c) 2006-2022, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2022-06-02 Jesven the first version + */ + +#include <rtthread.h> +#include <backtrace.h> + +#define BT_NESTING_MAX 100 + +static int unwind_frame(struct bt_frame *frame) +{ + unsigned long fp = frame->fp; + + if ((fp & 0x7) +#ifdef RT_USING_LWP + || fp < KERNEL_VADDR_START +#endif + ) + { + return 1; + } + frame->fp = *(unsigned long *)fp; + frame->pc = *(unsigned long *)(fp + 8); + return 0; +} + +static void walk_unwind(unsigned long pc, unsigned long fp) +{ + struct bt_frame frame; + unsigned long lr = pc; + int nesting = 0; + + frame.fp = fp; + while (nesting < BT_NESTING_MAX) + { + rt_kprintf(" %p", (void *)lr); + if (unwind_frame(&frame)) + { + break; + } + lr = frame.pc; + nesting++; + } +} + +void backtrace(unsigned long pc, unsigned long lr, unsigned long fp) +{ + rt_kprintf("please use: addr2line -e rtthread.elf -a -f %p", (void *)pc); + walk_unwind(lr, fp); + rt_kprintf("\n"); +} diff --git a/libcpu/aarch64/common/backtrace.h b/libcpu/aarch64/common/backtrace.h new file mode 100644 index 0000000000..b34601da58 --- /dev/null +++ b/libcpu/aarch64/common/backtrace.h @@ -0,0 +1,22 @@ +/* + * Copyright (c) 2006-2022, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2022-06-02 Jesven the first version + */ + +#ifndef __BACKTRACE_H__ +#define __BACKTRACE_H__ + +struct bt_frame +{ + unsigned long fp; + unsigned long pc; +}; + +void backtrace(unsigned long pc, unsigned long lr, unsigned long fp); + +#endif /*__BACKTRACE_H__*/ diff --git a/libcpu/aarch64/common/cache.S b/libcpu/aarch64/common/cache.S index 81416544f1..bd8504e9ff 100644 --- a/libcpu/aarch64/common/cache.S +++ b/libcpu/aarch64/common/cache.S @@ -1,5 +1,5 @@ /* - * Copyright (c) 2006-2022, RT-Thread Development Team + * Copyright (c) 2006-2020, RT-Thread Development Team * * SPDX-License-Identifier: Apache-2.0 * @@ -127,7 +127,8 @@ __asm_flush_dcache_range: /* x2 <- minimal cache line size in cache system */ sub x3, x2, #1 bic x0, x0, x3 -1: dc civac, x0 /* clean & invalidate data or unified cache */ + +1: dc civac, x0 /* clean & invalidate data or unified cache */ add x0, x0, x2 cmp x0, x1 b.lo 1b diff --git a/libcpu/aarch64/common/cache.h b/libcpu/aarch64/common/cache.h new file mode 100644 index 0000000000..35b9893802 --- /dev/null +++ b/libcpu/aarch64/common/cache.h @@ -0,0 +1,23 @@ +/* + * Copyright (c) 2006-2022, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2022-12-18 RT-Thread the first version + */ + +#ifndef __CACHE_H__ +#define __CACHE_H__ + +void rt_hw_dcache_flush_all(void); +void rt_hw_dcache_invalidate_all(void); +void rt_hw_dcache_flush_range(unsigned long start_addr, unsigned long size); +void rt_hw_cpu_dcache_clean(void *addr, int size); +void rt_hw_cpu_dcache_invalidate(unsigned long start_addr,unsigned long size); + +void rt_hw_icache_invalidate_all(); +void rt_hw_icache_invalidate_range(unsigned long start_addr, int size); + +#endif /* __CACHE_H__ */ diff --git a/libcpu/aarch64/common/cache_ops.c b/libcpu/aarch64/common/cache_ops.c index 517845e924..018f0be4a4 100644 --- a/libcpu/aarch64/common/cache_ops.c +++ b/libcpu/aarch64/common/cache_ops.c @@ -1,59 +1,58 @@ /* - * Copyright (c) 2006-2021, RT-Thread Development Team + * Copyright (c) 2006-2018, RT-Thread Development Team * * SPDX-License-Identifier: Apache-2.0 * * Change Logs: * Date Author Notes - * 2021-12-28 GuEe-GUI the first version + * 2019-03-29 quanzhao the first version */ - #include <rthw.h> #include <rtdef.h> +void __asm_invalidate_icache_all(void); void __asm_flush_dcache_all(void); -void __asm_invalidate_dcache_all(void); void __asm_flush_dcache_range(unsigned long start, unsigned long end); void __asm_invalidate_dcache_range(unsigned long start, unsigned long end); - -void __asm_invalidate_icache_all(void); void __asm_invalidate_icache_range(unsigned long start, unsigned long end); +void __asm_invalidate_dcache_all(void); +void __asm_invalidate_icache_all(void); -void rt_hw_dcache_flush_all(void) +rt_inline rt_uint32_t rt_cpu_icache_line_size(void) { - __asm_flush_dcache_all(); + return 0; } -void rt_hw_dcache_invalidate_all(void) +rt_inline rt_uint32_t rt_cpu_dcache_line_size(void) { - __asm_invalidate_dcache_all(); + return 0; } -void rt_hw_dcache_flush_range(unsigned long start_addr, unsigned long size) +void rt_hw_cpu_icache_invalidate(void *addr, int size) { - __asm_flush_dcache_range(start_addr, start_addr + size); + __asm_invalidate_icache_range((unsigned long)addr, (unsigned long)addr + size); } -void rt_hw_dcache_invalidate_range(unsigned long start_addr,unsigned long size) +void rt_hw_cpu_dcache_invalidate(void *addr, int size) { - __asm_invalidate_dcache_range(start_addr, start_addr + size); + __asm_invalidate_dcache_range((unsigned long)addr, (unsigned long)addr + size); } -void rt_hw_icache_invalidate_all() +void rt_hw_cpu_dcache_clean(void *addr, int size) { - __asm_invalidate_icache_all(); + __asm_flush_dcache_range((unsigned long)addr, (unsigned long)addr + size); } -void rt_hw_icache_invalidate_range(unsigned long start_addr, int size) +void rt_hw_cpu_dcache_clean_and_invalidate(void *addr, int size) { - __asm_invalidate_icache_range(start_addr, start_addr + size); + __asm_flush_dcache_range((unsigned long)addr, (unsigned long)addr + size); } void rt_hw_cpu_icache_ops(int ops, void *addr, int size) { if (ops == RT_HW_CACHE_INVALIDATE) { - rt_hw_icache_invalidate_range((unsigned long)addr, size); + rt_hw_cpu_icache_invalidate(addr, size); } } @@ -61,10 +60,43 @@ void rt_hw_cpu_dcache_ops(int ops, void *addr, int size) { if (ops == RT_HW_CACHE_FLUSH) { - rt_hw_dcache_flush_range((unsigned long)addr, size); + rt_hw_cpu_dcache_clean(addr, size); } else if (ops == RT_HW_CACHE_INVALIDATE) { - rt_hw_dcache_invalidate_range((unsigned long)addr, size); + rt_hw_cpu_dcache_invalidate(addr, size); + } +} + +rt_base_t rt_hw_cpu_icache_status(void) +{ + return 0; +} + +rt_base_t rt_hw_cpu_dcache_status(void) +{ + return 0; +} + +#ifdef RT_USING_LWP +#define ICACHE (1<<0) +#define DCACHE (1<<1) +#define BCACHE (ICACHE|DCACHE) + +int sys_cacheflush(void *addr, int size, int cache) +{ + if ((size_t)addr < KERNEL_VADDR_START && (size_t)addr + size <= KERNEL_VADDR_START) + { + if ((cache & DCACHE) != 0) + { + rt_hw_cpu_dcache_clean_and_invalidate(addr, size); + } + if ((cache & ICACHE) != 0) + { + rt_hw_cpu_icache_invalidate(addr, size); + } + return 0; } + return -1; } +#endif diff --git a/libcpu/aarch64/common/context_gcc.S b/libcpu/aarch64/common/context_gcc.S index 20f65ffd35..344d7317c6 100644 --- a/libcpu/aarch64/common/context_gcc.S +++ b/libcpu/aarch64/common/context_gcc.S @@ -1,24 +1,44 @@ /* - * Copyright (c) 2006-2022, RT-Thread Development Team + * Copyright (c) 2006-2021, RT-Thread Development Team * * SPDX-License-Identifier: Apache-2.0 * * Change Logs: * Date Author Notes - * 2018-10-06 ZhaoXiaowei the first version - * 2021-11-04 GuEe-GUI set sp with SP_ELx - * 2021-12-28 GuEe-GUI add fpu and smp support + * 2021-05-18 Jesven the first version */ #include "rtconfig.h" -#include "asm_fpu.h" -#ifdef RT_USING_SMP -#define rt_hw_interrupt_disable rt_hw_local_irq_disable -#define rt_hw_interrupt_enable rt_hw_local_irq_enable -#endif +#include "asm-fpu.h" + +.text +.weak rt_hw_cpu_id_set +.type rt_hw_cpu_id_set, @function +rt_hw_cpu_id_set: + mrs x0, mpidr_el1 /* MPIDR_EL1: Multi-Processor Affinity Register */ + and x0, x0, #15 + msr tpidr_el1, x0 + ret /* +int rt_hw_cpu_id(void) +*/ +.global rt_hw_cpu_id +.type rt_hw_cpu_id, @function +rt_hw_cpu_id: + mrs x0, tpidr_el1 /* MPIDR_EL1: Multi-Processor Affinity Register */ + ret + +/* +void rt_hw_set_process_id(size_t id) +*/ +.global rt_hw_set_process_id +rt_hw_set_process_id: + msr CONTEXTIDR_EL1, x0 + ret + + /* *enable gtimer */ .globl rt_hw_gtimer_enable @@ -28,14 +48,6 @@ rt_hw_gtimer_enable: RET /* - *disable gtimer - */ -.globl rt_hw_gtimer_disable -rt_hw_gtimer_disable: - MSR CNTP_CTL_EL0,XZR - RET - -/* *set gtimer CNTP_TVAL_EL0 value */ .globl rt_hw_set_gtimer_val @@ -65,19 +77,6 @@ rt_hw_get_gtimer_frq: MRS X0,CNTFRQ_EL0 RET -/* - *set gtimer frq value (only in EL3) - */ -.globl rt_hw_set_gtimer_frq -rt_hw_set_gtimer_frq: - MRS X1, CurrentEL - CMP X1, 0xc - BNE rt_hw_set_gtimer_frq_exit - MSR CNTFRQ_EL0, X0 - MOV X0, XZR -rt_hw_set_gtimer_frq_exit: - RET - .macro SAVE_CONTEXT /* Save the entire context. */ SAVE_FPU SP @@ -99,39 +98,18 @@ rt_hw_set_gtimer_frq_exit: MRS X28, FPCR MRS X29, FPSR STP X28, X29, [SP, #-0x10]! - STP X30, XZR, [SP, #-0x10]! + MRS X29, SP_EL0 + STP X29, X30, [SP, #-0x10]! - MRS X0, CurrentEL - CMP X0, 0xc - B.EQ 3f - CMP X0, 0x8 - B.EQ 2f - CMP X0, 0x4 - B.EQ 1f - B . -3: - MRS X3, SPSR_EL3 - /* Save the ELR. */ - MRS X2, ELR_EL3 - B 0f -2: - MRS X3, SPSR_EL2 - /* Save the ELR. */ - MRS X2, ELR_EL2 - B 0f -1: MRS X3, SPSR_EL1 MRS X2, ELR_EL1 - B 0f -0: STP X2, X3, [SP, #-0x10]! MOV X0, SP /* Move SP into X0 for saving. */ +.endm - .endm - -.macro SAVE_CONTEXT_T +.macro SAVE_CONTEXT_FROM_EL1 /* Save the entire context. */ SAVE_FPU SP STP X0, X1, [SP, #-0x10]! @@ -152,66 +130,71 @@ rt_hw_set_gtimer_frq_exit: MRS X28, FPCR MRS X29, FPSR STP X28, X29, [SP, #-0x10]! - STP X30, XZR, [SP, #-0x10]! + MRS X29, SP_EL0 + STP X29, X30, [SP, #-0x10]! - MRS X0, CurrentEL - CMP X0, 0xc - B.EQ 3f - CMP X0, 0x8 - B.EQ 2f - CMP X0, 0x4 - B.EQ 1f - B . -3: - MOV X3, #((3 << 6) | 0x0d) /* EL3h */ - MOV X2, X30 - B 0f -2: - MOV X3, #((3 << 6) | 0x09) /* EL2h */ - MOV X2, X30 - B 0f -1: - MOV X3, #((3 << 6) | 0x05) /* EL1h */ - MOV X2, X30 - B 0f -0: + MOV X19, #((3 << 6) | 0x4 | 0x1) /* el1h, disable interrupt */ + MOV X18, X30 - STP X2, X3, [SP, #-0x10]! + STP X18, X19, [SP, #-0x10]! +.endm - MOV X0, SP /* Move SP into X0 for saving. */ +#ifdef RT_USING_SMP +.macro RESTORE_CONTEXT + /* Set the SP to point to the stack of the task being restored. */ + MOV SP, X0 - .endm + LDP X2, X3, [SP], #0x10 /* SPSR and ELR. */ -.macro RESTORE_CONTEXT + TST X3, #0x1f + 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 + LDP X6, X7, [SP], #0x10 + LDP X4, X5, [SP], #0x10 + LDP X2, X3, [SP], #0x10 + LDP X0, X1, [SP], #0x10 + RESTORE_FPU SP +#ifdef RT_USING_LWP + BEQ arch_ret_to_user +#endif + ERET +.endm +#else +.macro RESTORE_CONTEXT /* Set the SP to point to the stack of the task being restored. */ MOV SP, X0 - +#ifdef RT_USING_LWP + BL rt_thread_self + MOV X19, X0 + BL lwp_mmu_switch + MOV X0, X19 + BL lwp_user_setting_restore +#endif LDP X2, X3, [SP], #0x10 /* SPSR and ELR. */ - MRS X0, CurrentEL - CMP X0, 0xc - B.EQ 3f - CMP X0, 0x8 - B.EQ 2f - CMP X0, 0x4 - B.EQ 1f - B . -3: - MSR SPSR_EL3, X3 - MSR ELR_EL3, X2 - B 0f -2: - MSR SPSR_EL2, X3 - MSR ELR_EL2, X2 - B 0f -1: + TST X3, #0x1f MSR SPSR_EL1, X3 MSR ELR_EL1, X2 - B 0f -0: - LDP X30, XZR, [SP], #0x10 + LDP X29, X30, [SP], #0x10 + MSR SP_EL0, X29 LDP X28, X29, [SP], #0x10 MSR FPCR, X28 MSR FPSR, X29 @@ -231,10 +214,52 @@ rt_hw_set_gtimer_frq_exit: LDP X2, X3, [SP], #0x10 LDP X0, X1, [SP], #0x10 RESTORE_FPU SP +#ifdef RT_USING_LWP + BEQ arch_ret_to_user +#endif + ERET +.endm +#endif + +.macro RESTORE_CONTEXT_WITHOUT_MMU_SWITCH + /* the SP is already ok */ + LDP X2, X3, [SP], #0x10 /* SPSR and ELR. */ + + TST X3, #0x1f + 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 + LDP X6, X7, [SP], #0x10 + LDP X4, X5, [SP], #0x10 + LDP X2, X3, [SP], #0x10 + LDP X0, X1, [SP], #0x10 + RESTORE_FPU SP +#ifdef RT_USING_LWP + BEQ arch_ret_to_user +#endif ERET +.endm - .endm +#ifdef RT_USING_SMP +#define rt_hw_interrupt_disable rt_hw_local_irq_disable +#define rt_hw_interrupt_enable rt_hw_local_irq_enable +#endif .text /* @@ -242,9 +267,9 @@ rt_hw_set_gtimer_frq_exit: */ .globl rt_hw_interrupt_disable rt_hw_interrupt_disable: - MRS X0, DAIF - MSR DAIFSet, #3 - DSB SY + MRS X0, DAIF + MSR DAIFSet, #3 + DSB SY RET /* @@ -253,99 +278,166 @@ rt_hw_interrupt_disable: .globl rt_hw_interrupt_enable rt_hw_interrupt_enable: DSB SY - MOV X1, #0xC0 - ANDS X0, X0, X1 - B.NE rt_hw_interrupt_enable_exit - MSR DAIFClr, #3 -rt_hw_interrupt_enable_exit: + AND X0, X0, #0xc0 + MRS X1, DAIF + BIC X1, X1, #0xc0 + ORR X0, X0, X1 + MSR DAIF, X0 RET +.text + +#ifdef RT_USING_SMP + /* - * #ifdef RT_USING_SMP - * void rt_hw_context_switch_to(rt_ubase_t to, stuct rt_thread *to_thread); - * #else - * void rt_hw_context_switch_to(rt_ubase_t to); - * #endif - * X0 --> to + * void rt_hw_context_switch_to(rt_uint3 to, struct rt_thread *to_thread); + * X0 --> to (thread stack) * X1 --> to_thread */ + .globl rt_hw_context_switch_to rt_hw_context_switch_to: -#ifdef RT_USING_SMP - STR X0, [SP, #-0x8]! + LDR X0, [X0] + MOV SP, X0 MOV X0, X1 BL rt_cpus_lock_status_restore - LDR X0, [SP], #0x8 -#endif /*RT_USING_SMP*/ - LDR X0, [X0] - RESTORE_CONTEXT +#ifdef RT_USING_LWP + BL rt_thread_self + BL lwp_user_setting_restore +#endif + B rt_hw_context_switch_exit -.text /* - * #ifdef RT_USING_SMP - * void rt_hw_context_switch(rt_ubase_t from, rt_ubase_t to, struct rt_thread *to_thread); - * #else - * void rt_hw_context_switch(rt_ubase_t from, rt_ubase_t to); - * #endif - * X0 --> from - * X1 --> to + * void rt_hw_context_switch(rt_uint32 from, rt_uint32 +to, struct rt_thread *to_thread); + * X0 --> from (from_thread stack) + * X1 --> to (to_thread stack) * X2 --> to_thread */ .globl rt_hw_context_switch rt_hw_context_switch: -#ifdef RT_USING_SMP + SAVE_CONTEXT_FROM_EL1 + MOV X3, SP + STR X3, [X0] // store sp in preempted tasks TCB + LDR X0, [X1] // get new task stack pointer + MOV SP, X0 + MOV X0, X2 + BL rt_cpus_lock_status_restore +#ifdef RT_USING_LWP + BL rt_thread_self + BL lwp_user_setting_restore +#endif + B rt_hw_context_switch_exit + +/* + * void rt_hw_context_switch_interrupt(context, from sp, to sp, tp tcb) + * X0 :interrupt context + * X1 :addr of from_thread's sp + * X2 :addr of to_thread's sp + * X3 :to_thread's tcb + */ +.globl rt_hw_context_switch_interrupt +rt_hw_context_switch_interrupt: STP X0, X1, [SP, #-0x10]! - STR X30, [SP, #-0x8]! - MOV X0, X2 + STP X2, X3, [SP, #-0x10]! + STP X29, X30, [SP, #-0x10]! +#ifdef RT_USING_LWP + BL rt_thread_self + BL lwp_user_setting_save +#endif + LDP X29, X30, [SP], #0x10 + LDP X2, X3, [SP], #0x10 + LDP X0, X1, [SP], #0x10 + STR X0, [X1] + LDR X0, [X2] + MOV SP, X0 + MOV X0, X3 + MOV X19, X0 BL rt_cpus_lock_status_restore - LDR X30, [SP], #0x8 + MOV X0, X19 +#ifdef RT_USING_LWP + BL lwp_user_setting_restore +#endif + B rt_hw_context_switch_exit + +.globl vector_fiq +vector_fiq: + B . + +.globl vector_irq +vector_irq: + CLREX + SAVE_CONTEXT + STP X0, X1, [SP, #-0x10]! /* X0 is thread sp */ + + BL rt_interrupt_enter + BL rt_hw_trap_irq + BL rt_interrupt_leave + LDP X0, X1, [SP], #0x10 -#endif /*RT_USING_SMP*/ + BL rt_scheduler_do_irq_switch + B rt_hw_context_switch_exit - MOV X8,X0 - MOV X9,X1 +.global rt_hw_context_switch_exit +rt_hw_context_switch_exit: + MOV X0, SP + RESTORE_CONTEXT - SAVE_CONTEXT_T +#else - STR X0, [X8] // store sp in preempted tasks TCB - LDR X0, [X9] // get new task stack pointer +/* + * void rt_hw_context_switch_to(rt_ubase_t to); + * X0 --> to sp + */ +.globl rt_hw_context_switch_to +rt_hw_context_switch_to: + LDR X0, [X0] + RESTORE_CONTEXT + +/* + * void rt_hw_context_switch(rt_ubase_t from, rt_ubase_t to); + * X0 --> from sp + * X1 --> to sp + * X2 --> to thread + */ +.globl rt_hw_context_switch +rt_hw_context_switch: + + SAVE_CONTEXT_FROM_EL1 + + MOV X2, SP + STR X2, [X0] // store sp in preempted tasks TCB + LDR X0, [X1] // get new task stack pointer RESTORE_CONTEXT /* - * void rt_hw_context_switch_interrupt(rt_ubase_t from, rt_ubase_t to); + * void rt_hw_context_switch_interrupt(rt_ubase_t from, rt_ubase_t to, rt_thread_t from_thread, rt_thread_t to_thread); */ .globl rt_thread_switch_interrupt_flag .globl rt_interrupt_from_thread .globl rt_interrupt_to_thread .globl rt_hw_context_switch_interrupt rt_hw_context_switch_interrupt: -#ifdef RT_USING_SMP - /* x0 = context */ - /* x1 = ¤t_thread->sp */ - /* x2 = &to_thread->sp, */ - /* x3 = to_thread TCB */ - STR X0, [X1] - LDR X0, [x2] - MOV SP, X0 - MOV X0, X3 - BL rt_cpus_lock_status_restore - MOV X0, SP - RESTORE_CONTEXT -#else - LDR X2, =rt_thread_switch_interrupt_flag - LDR X3, [X2] - CMP X3, #1 - B.EQ _reswitch - LDR X4, =rt_interrupt_from_thread // set rt_interrupt_from_thread - MOV X3, #1 // set rt_thread_switch_interrupt_flag to 1 + LDR X6, =rt_thread_switch_interrupt_flag + LDR X7, [X6] + CMP X7, #1 + B.EQ _reswitch + LDR X4, =rt_interrupt_from_thread // set rt_interrupt_from_thread STR X0, [X4] - STR X3, [X2] + MOV X7, #1 // set rt_thread_switch_interrupt_flag to 1 + STR X7, [X6] + STP X1, X30, [SP, #-0x10]! +#ifdef RT_USING_LWP + MOV X0, X2 + BL lwp_user_setting_save +#endif + LDP X1, X30, [SP], #0x10 _reswitch: - LDR X2, =rt_interrupt_to_thread // set rt_interrupt_to_thread - STR X1, [X2] + LDR X6, =rt_interrupt_to_thread // set rt_interrupt_to_thread + STR X1, [X6] RET -#endif + .text // -- Exception handlers ---------------------------------- @@ -372,45 +464,78 @@ vector_fiq: .globl vector_irq vector_irq: SAVE_CONTEXT - STP X0, X1, [SP, #-0x10]! + STP X0, X1, [SP, #-0x10]! /* X0 is thread sp */ BL rt_interrupt_enter BL rt_hw_trap_irq BL rt_interrupt_leave LDP X0, X1, [SP], #0x10 -#ifdef RT_USING_SMP - /* Never reture If can switch */ - BL rt_scheduler_do_irq_switch - MOV X0, SP -#endif // if rt_thread_switch_interrupt_flag set, jump to // rt_hw_context_switch_interrupt_do and don't return LDR X1, =rt_thread_switch_interrupt_flag LDR X2, [X1] CMP X2, #1 - B.NE vector_irq_exit + B.NE vector_irq_exit MOV X2, #0 // clear flag STR X2, [X1] - LDR X3, =rt_interrupt_from_thread + LDR X3, =rt_interrupt_from_thread LDR X4, [X3] STR x0, [X4] // store sp in preempted tasks's TCB - LDR x3, =rt_interrupt_to_thread + LDR x3, =rt_interrupt_to_thread LDR X4, [X3] LDR x0, [X4] // get new task's stack pointer -vector_irq_exit: RESTORE_CONTEXT +vector_irq_exit: + MOV SP, X0 + RESTORE_CONTEXT_WITHOUT_MMU_SWITCH +#endif + // ------------------------------------------------- - .align 8 - .globl vector_error -vector_error: + .globl vector_exception +vector_exception: SAVE_CONTEXT - BL rt_hw_trap_error - B . + STP X0, X1, [SP, #-0x10]! + BL rt_hw_trap_exception + LDP X0, X1, [SP], #0x10 + MOV SP, X0 + RESTORE_CONTEXT_WITHOUT_MMU_SWITCH + + .globl vector_serror +vector_serror: + SAVE_CONTEXT + STP X0, X1, [SP, #-0x10]! + BL rt_hw_trap_serror + b . + +.global rt_hw_mmu_switch +rt_hw_mmu_switch: + MSR TTBR0_EL1, X0 + MRS X1, TCR_EL1 + CMP X0, XZR + ORR X1, X1, #(1 << 7) + BEQ 1f + BIC X1, X1, #(1 << 7) +1: + MSR TCR_EL1, X1 + DSB SY + ISB + TLBI VMALLE1 + DSB SY + ISB + IC IALLUIS + DSB SY + ISB + RET + +.global rt_hw_mmu_tbl_get +rt_hw_mmu_tbl_get: + MRS X0, TTBR0_EL1 + RET diff --git a/libcpu/aarch64/common/cp15.h b/libcpu/aarch64/common/cp15.h new file mode 100644 index 0000000000..cb1458ceec --- /dev/null +++ b/libcpu/aarch64/common/cp15.h @@ -0,0 +1,65 @@ +/* + * Copyright (c) 2006-2020, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2011-09-15 Bernard first version + */ + +#ifndef __CP15_H__ +#define __CP15_H__ + +#ifndef __STATIC_FORCEINLINE +#define __STATIC_FORCEINLINE __attribute__((always_inline)) static inline +#endif + +#define __WFI() __asm__ volatile ("wfi":::"memory") + +#define __WFE() __asm__ volatile ("wfe":::"memory") + +#define __SEV() __asm__ volatile ("sev") + +__STATIC_FORCEINLINE void __ISB(void) +{ + __asm__ volatile ("isb 0xF":::"memory"); +} + +/** + \brief Data Synchronization Barrier + \details Acts as a special kind of Data Memory Barrier. + It completes when all explicit memory accesses before this instruction complete. + */ +__STATIC_FORCEINLINE void __DSB(void) +{ + __asm__ volatile ("dsb 0xF":::"memory"); +} + +/** + \brief Data Memory Barrier + \details Ensures the apparent order of the explicit memory operations before + and after the instruction, without ensuring their completion. + */ + +__STATIC_FORCEINLINE void __DMB(void) +{ + __asm__ volatile ("dmb 0xF":::"memory"); +} + +unsigned long rt_cpu_get_smp_id(void); + +void rt_cpu_mmu_disable(void); +void rt_cpu_mmu_enable(void); +void rt_cpu_tlb_set(volatile unsigned long*); + +void rt_cpu_dcache_clean_flush(void); +void rt_cpu_icache_flush(void); + +void rt_cpu_vector_set_base(rt_ubase_t addr); +void rt_hw_mmu_init(void); +void rt_hw_vector_init(void); + +void set_timer_counter(unsigned int counter); +void set_timer_control(unsigned int control); +#endif diff --git a/libcpu/aarch64/common/cpu.c b/libcpu/aarch64/common/cpu.c index 63731a05ba..e8d55fd644 100644 --- a/libcpu/aarch64/common/cpu.c +++ b/libcpu/aarch64/common/cpu.c @@ -1,5 +1,5 @@ /* - * Copyright (c) 2006-2021, RT-Thread Development Team + * Copyright (c) 2006-2019, RT-Thread Development Team * * SPDX-License-Identifier: Apache-2.0 * @@ -7,15 +7,43 @@ * Date Author Notes * 2011-09-15 Bernard first version * 2019-07-28 zdzn add smp support - * 2021-12-21 GuEe-GUI set tpidr_el1 as multiprocessor id instead of mpidr_el1 - * 2021-12-28 GuEe-GUI add spinlock for aarch64 */ #include <rthw.h> #include <rtthread.h> -#include <cpuport.h> +#include <board.h> +#include "cp15.h" + +#define DBG_TAG "libcpu.aarch64.cpu" +#define DBG_LVL DBG_INFO +#include <rtdbg.h> +#include <string.h> +#include "cpu.h" +#include "psci_api.h" + +void (*system_off)(void); #ifdef RT_USING_SMP + +#ifdef RT_USING_FDT +#include "dtb_node.h" +struct dtb_node *_cpu_node[RT_CPUS_NR]; +#endif /* RT_USING_FDT */ + +#define MPIDR_AFF_MASK 0x000000FF00FFFFFFul +#define REPORT_ERR(retval) LOG_E("got error code %d in %s(), %s:%d", (retval), __func__, __FILE__, __LINE__) +#define CHECK_RETVAL(retval) if (retval) {REPORT_ERR(retval);} + +/** + * cpu_ops_tbl contains cpu_ops_t for each cpu kernel observed, + * given cpu logical id 'i', its cpu_ops_t is 'cpu_ops_tbl[i]' + */ +struct cpu_ops_t *cpu_ops_tbl[RT_CPUS_NR]; + +#ifdef RT_USING_SMART +// _id_to_mpidr is a table translate logical id to mpid, which is a 64-bit value +rt_uint64_t rt_cpu_mpidr_early[RT_CPUS_NR] rt_weak = {[0 ... RT_CPUS_NR - 1] = ID_ERROR}; +#else /* The more common mpidr_el1 table, redefine it in BSP if it is in other cases */ rt_weak rt_uint64_t rt_cpu_mpidr_early[] = { @@ -29,65 +57,290 @@ rt_weak rt_uint64_t rt_cpu_mpidr_early[] = [7] = 0x80000007, [RT_CPUS_NR] = 0 }; -#endif - -int rt_hw_cpu_id(void) -{ - rt_base_t value; - - __asm__ volatile ("mrs %0, tpidr_el1":"=r"(value)); - - return value; -} +#endif /* RT_USING_SMART */ -#ifdef RT_USING_SMP void rt_hw_spin_lock_init(rt_hw_spinlock_t *lock) { lock->slock = 0; } +#define TICKET_SHIFT 16 void rt_hw_spin_lock(rt_hw_spinlock_t *lock) { - rt_hw_spinlock_t lock_val, new_lockval; unsigned int tmp; + struct __arch_tickets lockval, newval; - __asm__ volatile ( - /* Increment the next ticket. */ + asm volatile( + /* Atomically increment the next ticket. */ " prfm pstl1strm, %3\n" "1: ldaxr %w0, %3\n" " add %w1, %w0, %w5\n" " stxr %w2, %w1, %3\n" " cbnz %w2, 1b\n" - /* Check wether we get the lock */ - " eor %w1, %w0, %w0, ror #16\n" - " cbz %w1, 3f\n" + /* Did we get the lock? */ + " eor %w1, %w0, %w0, ror #16\n" + " cbz %w1, 3f\n" /* - * Didn't get lock and spin on the owner. - * Should send a local event to avoid missing an + * No: spin on the owner. Send a local event to avoid missing an * unlock before the exclusive load. */ " sevl\n" "2: wfe\n" " ldaxrh %w2, %4\n" - " eor %w1, %w2, %w0, lsr #16\n" + " eor %w1, %w2, %w0, lsr #16\n" " cbnz %w1, 2b\n" - /* got the lock. */ + /* We got the lock. Critical section starts here. */ "3:" - : "=&r" (lock_val), "=&r" (new_lockval), "=&r" (tmp), "+Q" (*lock) - : "Q" (lock->tickets.owner), "I" (1 << 16) + : "=&r"(lockval), "=&r"(newval), "=&r"(tmp), "+Q"(*lock) + : "Q"(lock->tickets.owner), "I"(1 << TICKET_SHIFT) : "memory"); - __DMB(); + rt_hw_dmb(); } void rt_hw_spin_unlock(rt_hw_spinlock_t *lock) { - __DMB(); - __asm__ volatile ( - "stlrh %w1, %0\n" - : "=Q" (lock->tickets.owner) - : "r" (lock->tickets.owner + 1) + rt_hw_dmb(); + asm volatile( + " stlrh %w1, %0\n" + : "=Q"(lock->tickets.owner) + : "r"(lock->tickets.owner + 1) : "memory"); } + +static int _cpus_init_data_hardcoded(int num_cpus, rt_uint64_t *cpu_hw_ids, struct cpu_ops_t *cpu_ops[]) +{ + // load in cpu_hw_ids in cpuid_to_hwid, + // cpu_ops to cpu_ops_tbl + if (num_cpus > RT_CPUS_NR) + { + LOG_W("num_cpus (%d) greater than RT_CPUS_NR (%d)\n", num_cpus, RT_CPUS_NR); + num_cpus = RT_CPUS_NR; + } + + for (int i = 0; i < num_cpus; i++) + { + set_hwid(i, cpu_hw_ids[i]); + cpu_ops_tbl[i] = cpu_ops[i]; + } + return 0; +} + +#ifdef RT_USING_FDT + +/** read ('size' * 4) bytes number from start, big-endian format */ +static rt_uint64_t _read_be_number(void *start, int size) +{ + rt_uint64_t buf = 0; + for (; size > 0; size--) + buf = (buf << 32) | fdt32_to_cpu(*(uint32_t *)start++); + return buf; +} + +/** check device-type of the node, */ +static bool _node_is_cpu(struct dtb_node *node) +{ + char *device_type = dtb_node_get_dtb_node_property_value(node, "device_type", NULL); + if (device_type) + { + return !strcmp(device_type, "cpu"); + } + return false; +} + +static int _read_and_set_hwid(struct dtb_node *cpu, int *id_pool, int *pcpuid) +{ + // size/address_cells is number of elements in reg array + int size; + static int address_cells, size_cells; + if (!address_cells && !size_cells) + dtb_node_get_dtb_node_cells(cpu, &address_cells, &size_cells); + + void *id_start = dtb_node_get_dtb_node_property_value(cpu, "reg", &size); + rt_uint64_t mpid = _read_be_number(id_start, address_cells); + + *pcpuid = *id_pool; + *id_pool = *id_pool + 1; + set_hwid(*pcpuid, mpid); + + LOG_I("Using MPID 0x%lx as cpu %d", mpid, *pcpuid); + + // setting _cpu_node for cpu_init use + _cpu_node[*pcpuid] = cpu; + + return 0; +} + +static int _read_and_set_cpuops(struct dtb_node *cpu, int cpuid) +{ + char *method = dtb_node_get_dtb_node_property_value(cpu, "enable-method", NULL); + if (!method) + { + LOG_E("Cannot read method from cpu node"); + return -1; + } + + struct cpu_ops_t *cpu_ops; + if (!strcmp(method, cpu_ops_psci.method)) + { + cpu_ops = &cpu_ops_psci; + } + else if (!strcmp(method, cpu_ops_spin_tbl.method)) + { + cpu_ops = &cpu_ops_spin_tbl; + } + else + { + cpu_ops = RT_NULL; + LOG_E("Not supported cpu_ops: %s", method); + } + cpu_ops_tbl[cpuid] = cpu_ops; + + LOG_D("Using boot method [%s] for cpu %d", cpu_ops->method, cpuid); + return 0; +} + +static int _cpus_init_data_fdt() +{ + // cpuid_to_hwid and cpu_ops_tbl with fdt + void *root = get_dtb_node_head(); + int id_pool = 0; + int cpuid; + struct dtb_node *cpus = dtb_node_get_dtb_node_by_path(root, "/cpus"); + + // for each cpu node (device-type is cpu), read its mpid and set its cpuid_to_hwid + for_each_node_child(cpus) + { + if (!_node_is_cpu(cpus)) + { + continue; + } + + if (id_pool > RT_CPUS_NR) + { + LOG_W("Reading more cpus from FDT than RT_CPUS_NR" + "\n Parsing will not continue and only %d cpus will be used.", RT_CPUS_NR); + break; + } + + _read_and_set_hwid(cpus, &id_pool, &cpuid); + + _read_and_set_cpuops(cpus, cpuid); + } + return 0; +} + +#endif /* RT_USING_FDT */ + +/** init cpu with hardcoded infomation or parsing from FDT */ +static int _cpus_init(int num_cpus, rt_uint64_t *cpu_hw_ids, struct cpu_ops_t *cpu_ops[]) +{ + int retval; + + // first setup cpu_ops_tbl and cpuid_to_hwid + if (num_cpus > 0) + retval = _cpus_init_data_hardcoded(num_cpus, cpu_hw_ids, cpu_ops); + else + { + retval = -1; +#ifdef RT_USING_FDT + retval = _cpus_init_data_fdt(); +#endif + } + + if (retval) + return retval; + + // using cpuid_to_hwid and cpu_ops_tbl to call method_init and cpu_init + // assuming that cpuid 0 has already init + for (int i = 1; i < RT_CPUS_NR; i++) + { + if (cpuid_to_hwid(i) == ID_ERROR) + { + LOG_E("Failed to find hardware id of CPU %d", i); + continue; + } + + if (cpu_ops_tbl[i] && cpu_ops_tbl[i]->cpu_init) + { + retval = cpu_ops_tbl[i]->cpu_init(i); + CHECK_RETVAL(retval); + } + else + { + LOG_E("Failed to find cpu_init for cpu %d with cpu_ops[%p], cpu_ops->cpu_init[%p]" + , cpuid_to_hwid(i), cpu_ops_tbl[i], cpu_ops_tbl[i] ? cpu_ops_tbl[i]->cpu_init : NULL); + } + } + return 0; +} + +static void _boot_secondary(void) +{ + for (int i = 1; i < RT_CPUS_NR; i++) + { + int retval = -0xbad0; // mark no support operation + if (cpu_ops_tbl[i] && cpu_ops_tbl[i]->cpu_boot) + retval = cpu_ops_tbl[i]->cpu_boot(i); + if (retval) + { + if (retval == -0xbad0) + LOG_E("No cpu_ops was probed for CPU %d. Try to configure it or use fdt", i); + else + LOG_E("Failed to boot secondary CPU %d, error code %d", i, retval); + } else { + LOG_I("Secondary CPU %d booted", i); + } + } +} + +rt_weak void rt_hw_secondary_cpu_up(void) +{ + _boot_secondary(); +} + +/** + * @brief boot cpu with hardcoded data + * + * @param num_cpus number of cpus + * @param cpu_hw_ids each element represents a hwid of cpu[i] + * @param cpu_ops each element represents a pointer to cpu_ops of cpu[i] + * @return int 0 on success, + */ +int rt_hw_cpu_boot_secondary(int num_cpus, rt_uint64_t *cpu_hw_ids, struct cpu_ops_t *cpu_ops[]) +{ + int retval = 0; + if (num_cpus < 1 || !cpu_hw_ids || !cpu_ops) + return -1; + + retval = _cpus_init(num_cpus, cpu_hw_ids, cpu_ops); + CHECK_RETVAL(retval); + + return retval; +} + +#define CPU_INIT_USING_FDT 0,0,0 + +/** + * @brief Initialize cpu infomation from fdt + * + * @return int + */ +int rt_hw_cpu_init() +{ +#ifdef RT_USING_FDT + return _cpus_init(CPU_INIT_USING_FDT); +#else + LOG_E("CPU init failed since RT_USING_FDT was not defined"); + return -0xa; /* no fdt support */ +#endif /* RT_USING_FDT */ +} + +rt_weak void rt_hw_secondary_cpu_idle_exec(void) +{ + asm volatile("wfe" :: + : "memory", "cc"); +} + #endif /*RT_USING_SMP*/ /** @@ -98,14 +351,19 @@ void rt_hw_spin_unlock(rt_hw_spinlock_t *lock) /** shutdown CPU */ rt_weak void rt_hw_cpu_shutdown() { - register rt_int32_t level; + rt_uint32_t level; rt_kprintf("shutdown...\n"); + if (system_off) + system_off(); + LOG_E("system shutdown failed"); + level = rt_hw_interrupt_disable(); while (level) { RT_ASSERT(0); } } +MSH_CMD_EXPORT_ALIAS(rt_hw_cpu_shutdown, shutdown, shutdown machine); /*@}*/ diff --git a/libcpu/aarch64/common/cpu.h b/libcpu/aarch64/common/cpu.h new file mode 100644 index 0000000000..28044bb185 --- /dev/null +++ b/libcpu/aarch64/common/cpu.h @@ -0,0 +1,63 @@ +/* + * Copyright (c) 2006-2019, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + */ +#ifndef __RT_HW_CPU_H__ +#define __RT_HW_CPU_H__ + +#include <rthw.h> +#include <rtthread.h> +#include <stdbool.h> + +#ifndef RT_CPUS_NR +#define RT_CPUS_NR 1 +#endif /* RT_CPUS_NR */ + +#ifdef RT_USING_SMP +struct cpu_ops_t +{ + const char *method; + int (*cpu_init)(rt_uint32_t id); + int (*cpu_boot)(rt_uint32_t id); + void (*cpu_shutdown)(void); +}; + +/** + * Identifier to mark a wrong CPU MPID. + * All elements in rt_cpu_mpidr_early[] should be initialized with this value + */ +#define ID_ERROR __INT64_MAX__ + +extern rt_uint64_t rt_cpu_mpidr_early[]; +extern struct dtb_node *_cpu_node[]; + +#define cpuid_to_hwid(cpuid) \ + ((((cpuid) >= 0) && ((cpuid) < RT_CPUS_NR)) ? rt_cpu_mpidr_early[cpuid] : ID_ERROR) +#define set_hwid(cpuid, hwid) \ + ((((cpuid) >= 0) && ((cpuid) < RT_CPUS_NR)) ? (rt_cpu_mpidr_early[cpuid] = (hwid)) : ID_ERROR) +#define get_cpu_node(cpuid) \ + ((((cpuid) >= 0) && ((cpuid) < RT_CPUS_NR)) ? _cpu_node[cpuid] : NULL) +#define set_cpu_node(cpuid, node) \ + ((((cpuid) >= 0) && ((cpuid) < RT_CPUS_NR)) ? (_cpu_node[cpuid] = node) : NULL) + +extern int rt_hw_cpu_init(); + +extern int rt_hw_cpu_boot_secondary(int num_cpus, rt_uint64_t *cpu_hw_ids, struct cpu_ops_t *cpu_ops[]); + +extern void rt_hw_secondary_cpu_idle_exec(void); + +extern struct cpu_ops_t cpu_ops_psci; + +extern struct cpu_ops_t cpu_ops_spin_tbl; + +#endif /* RT_USING_SMP */ + +extern void rt_hw_cpu_shutdown(void); + +extern void (*system_off)(void); + +#endif /* __RT_HW_CPU_H__ */
\ No newline at end of file diff --git a/libcpu/aarch64/common/cpu_gcc.S b/libcpu/aarch64/common/cpu_gcc.S index 17dea1a481..445c104dce 100644 --- a/libcpu/aarch64/common/cpu_gcc.S +++ b/libcpu/aarch64/common/cpu_gcc.S @@ -1,82 +1,103 @@ /* - * Copyright (c) 2006-2022, RT-Thread Development Team + * Copyright (c) 2006-2020, RT-Thread Development Team * * SPDX-License-Identifier: Apache-2.0 * * Date Author Notes * 2018-10-06 ZhaoXiaowei the first version */ - + .text .globl rt_hw_get_current_el rt_hw_get_current_el: - MRS X0, CurrentEL - CMP X0, 0xc - B.EQ 3f - CMP X0, 0x8 - B.EQ 2f - CMP X0, 0x4 - B.EQ 1f - - LDR X0, =0 - B 0f + MRS X0, CurrentEL + CMP X0, 0xc + B.EQ 3f + CMP X0, 0x8 + B.EQ 2f + CMP X0, 0x4 + B.EQ 1f + + LDR X0, =0 + B 0f 3: - LDR X0, =3 - B 0f + LDR X0, =3 + B 0f 2: - LDR X0, =2 - B 0f + LDR X0, =2 + B 0f 1: - LDR X0, =1 - B 0f + LDR X0, =1 + B 0f 0: - RET + RET .globl rt_hw_set_current_vbar rt_hw_set_current_vbar: - MRS X1, CurrentEL - CMP X1, 0xc - B.EQ 3f - CMP X1, 0x8 - B.EQ 2f - CMP X1, 0x4 - B.EQ 1f - B 0f + MRS X1, CurrentEL + CMP X1, 0xc + B.EQ 3f + CMP X1, 0x8 + B.EQ 2f + CMP X1, 0x4 + B.EQ 1f + B 0f 3: - MSR VBAR_EL3,X0 - B 0f + MSR VBAR_EL3,X0 + B 0f 2: - MSR VBAR_EL2,X0 - B 0f + MSR VBAR_EL2,X0 + B 0f 1: - MSR VBAR_EL1,X0 - B 0f + MSR VBAR_EL1,X0 + B 0f 0: - RET - + RET .globl rt_hw_set_elx_env rt_hw_set_elx_env: - MRS X1, CurrentEL - CMP X1, 0xc - B.EQ 3f - CMP X1, 0x8 - B.EQ 2f - CMP X1, 0x4 - B.EQ 1f - B 0f + MRS X1, CurrentEL + CMP X1, 0xc + B.EQ 3f + CMP X1, 0x8 + B.EQ 2f + CMP X1, 0x4 + B.EQ 1f + B 0f 3: - MRS X0, SCR_EL3 - ORR X0, X0, #0xF /* SCR_EL3.NS|IRQ|FIQ|EA */ - MSR SCR_EL3, X0 - B 0f + MRS X0, SCR_EL3 + ORR X0, X0, #0xF /* SCR_EL3.NS|IRQ|FIQ|EA */ + MSR SCR_EL3, X0 + B 0f 2: - MRS X0, HCR_EL2 - ORR X0, X0, #0x38 - MSR HCR_EL2, X0 - B 0f + MRS X0, HCR_EL2 + ORR X0, X0, #0x38 + MSR HCR_EL2, X0 + B 0f 1: - B 0f + B 0f 0: + RET + +.global rt_cpu_vector_set_base +rt_cpu_vector_set_base: + MSR VBAR_EL1,X0 RET + + +/** + * unsigned long rt_hw_ffz(unsigned long x) + */ +.global rt_hw_ffz +rt_hw_ffz: + mvn x1, x0 + clz x0, x1 + mov x1, #0x3f + sub x0, x1, x0 + ret + +.global rt_hw_clz +rt_hw_clz: + clz x0, x0 + ret diff --git a/libcpu/aarch64/common/cpu_ops_common.h b/libcpu/aarch64/common/cpu_ops_common.h new file mode 100644 index 0000000000..3ac0a363c3 --- /dev/null +++ b/libcpu/aarch64/common/cpu_ops_common.h @@ -0,0 +1,21 @@ +#ifndef __CPU_OPS_COMMON_H__ +#define __CPU_OPS_COMMON_H__ + +#include <rthw.h> +#include <rtthread.h> +#include <mmu.h> +#include "entry_point.h" + +static inline rt_uint64_t get_secondary_entry_pa(void) +{ + rt_uint64_t secondary_entry_pa = (rt_uint64_t)rt_hw_mmu_v2p(&mmu_info, _secondary_cpu_entry); + + if (!secondary_entry_pa) + { + LOG_E("Failed to translate 'secondary_entry_pa' to physical address"); + return 0; + } + return secondary_entry_pa; +} + +#endif /* __CPU_OPS_COMMON_H__ */
\ No newline at end of file diff --git a/libcpu/aarch64/common/cpu_psci.c b/libcpu/aarch64/common/cpu_psci.c new file mode 100644 index 0000000000..ddb8ae4a47 --- /dev/null +++ b/libcpu/aarch64/common/cpu_psci.c @@ -0,0 +1,70 @@ +/* + * Copyright (c) 2006-2019, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + */ +#include <rthw.h> +#include <rtthread.h> +#include <stdint.h> + +#ifdef RT_USING_SMP + +#define DBG_TAG "libcpu.aarch64.cpu_psci" +#define DBG_LVL DBG_INFO +#include <rtdbg.h> +#include "cpu_ops_common.h" + +#include "cpu.h" +#include "errno.h" +#include "psci.h" +#include "psci_api.h" + +static int (*_psci_init)(void) = psci_init; + +static int __call_method_init() +{ + int (*init)(void) = _psci_init; + _psci_init = RT_NULL; + + return init(); +} + +/** return 0 on success, otherwise failed */ +#define _call_method_init() ((_psci_init) ? __call_method_init() : 0); + +static int cpu_psci_cpu_init(rt_uint32_t cpuid) +{ + // init psci only once + return _call_method_init(); +} + +static int cpu_psci_cpu_boot(rt_uint32_t cpuid) +{ + rt_uint64_t secondary_entry_pa = get_secondary_entry_pa(); + + if (!secondary_entry_pa) + return -1; + + if (!psci_ops.cpu_on) { + LOG_E("Uninitialized psci operation"); + return -1; + } + return psci_ops.cpu_on(cpuid_to_hwid(cpuid), secondary_entry_pa); +} + +static void cpu_psci_cpu_shutdown() +{ + psci_ops.cpu_off(cpuid_to_hwid(rt_hw_cpu_id())); +} + +struct cpu_ops_t cpu_ops_psci = { + .method = "psci", + .cpu_boot = cpu_psci_cpu_boot, + .cpu_init = cpu_psci_cpu_init, + .cpu_shutdown = cpu_psci_cpu_shutdown, +}; + +#endif /* RT_USING_SMP */
\ No newline at end of file diff --git a/libcpu/aarch64/common/cpu_spin_table.c b/libcpu/aarch64/common/cpu_spin_table.c new file mode 100644 index 0000000000..67e788225f --- /dev/null +++ b/libcpu/aarch64/common/cpu_spin_table.c @@ -0,0 +1,80 @@ +/* + * Copyright (c) 2006-2022, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + */ +#include <rthw.h> +#include <rtthread.h> + +#include "cpu.h" + +#ifdef RT_USING_SMART +#include <ioremap.h> +#else +#define rt_ioremap(x, ...) (x) +#define rt_iounmap(x) +#endif + +#define DBG_TAG "libcpu.aarch64.cpu_spin_table" +#define DBG_LVL DBG_INFO +#include <rtdbg.h> +#include "cpu_ops_common.h" + +#ifdef RT_USING_SMP +#ifdef RT_USING_FDT +#include <dtb_node.h> + +static rt_uint64_t cpu_release_addr[RT_CPUS_NR]; + +static int spin_table_cpu_init(rt_uint32_t cpuid) +{ + struct dtb_node *cpu = get_cpu_node(cpuid); + if (!cpu) + return -1; /* uninitialized cpu node in fdt */ + + int size; + rt_uint64_t *phead = (rt_uint64_t*)dtb_node_get_dtb_node_property_value(cpu, "cpu-release-addr", &size); + cpu_release_addr[cpuid] = fdt64_to_cpu(*phead); + + LOG_D("Using release address 0x%p for CPU %d", cpu_release_addr[cpuid], cpuid); + return 0; +} + +static int spin_table_cpu_boot(rt_uint32_t cpuid) +{ + rt_uint64_t secondary_entry_pa = get_secondary_entry_pa(); + if (!secondary_entry_pa) + return -1; + + // map release_addr to addressable place + void *rel_va = (void *)cpu_release_addr[cpuid]; + +#ifdef RT_USING_SMART + rel_va = rt_ioremap(rel_va, sizeof(cpu_release_addr[0])); +#endif + if (!rel_va) + { + LOG_E("IO remap failing"); + return -1; + } + + __asm__ volatile("str %0, [%1]" ::"rZ"(secondary_entry_pa), "r"(rel_va)); + __asm__ volatile("dsb sy"); + __asm__ volatile("sev"); + rt_iounmap(rel_va); + return 0; +} +#endif /* RT_USING_FDT */ + +struct cpu_ops_t cpu_ops_spin_tbl = { + .method = "spin-table", +#ifdef RT_USING_FDT + .cpu_init = spin_table_cpu_init, + .cpu_boot = spin_table_cpu_boot, +#endif +}; + +#endif /* RT_USING_SMP */
\ No newline at end of file diff --git a/libcpu/aarch64/common/cpuport.h b/libcpu/aarch64/common/cpuport.h index 36d3ac7b65..9f209069ac 100644 --- a/libcpu/aarch64/common/cpuport.h +++ b/libcpu/aarch64/common/cpuport.h @@ -5,20 +5,12 @@ * * Change Logs: * Date Author Notes - * 2021-09-10 GuEe-GUI first version */ -#ifndef __CPUPORT_H__ -#define __CPUPORT_H__ +#ifndef CPUPORT_H__ +#define CPUPORT_H__ -#include <rtdef.h> - -#define __WFI() __asm__ volatile ("wfi":::"memory") -#define __WFE() __asm__ volatile ("wfe":::"memory") -#define __SEV() __asm__ volatile ("sev") -#define __ISB() __asm__ volatile ("isb 0xf":::"memory") -#define __DSB() __asm__ volatile ("dsb 0xf":::"memory") -#define __DMB() __asm__ volatile ("dmb 0xf":::"memory") +#include <armv8.h> #ifdef RT_USING_SMP typedef union { @@ -32,17 +24,17 @@ typedef union { rt_inline void rt_hw_isb(void) { - __asm__ volatile ("isb":::"memory"); + asm volatile ("isb":::"memory"); } rt_inline void rt_hw_dmb(void) { - __asm__ volatile ("dmb sy":::"memory"); + asm volatile ("dmb sy":::"memory"); } rt_inline void rt_hw_dsb(void) { - __asm__ volatile ("dsb sy":::"memory"); + asm volatile ("dsb sy":::"memory"); } -#endif /* __CPUPORT_H__ */ +#endif /*CPUPORT_H__*/ diff --git a/libcpu/aarch64/common/entry_point.h b/libcpu/aarch64/common/entry_point.h new file mode 100644 index 0000000000..fc3e6c061f --- /dev/null +++ b/libcpu/aarch64/common/entry_point.h @@ -0,0 +1,13 @@ +/* + * Copyright (c) 2006-2022, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + */ +#ifndef __ENTRY_POINT_H__ +#define __ENTRY_POINT_H__ + +extern void _secondary_cpu_entry(void); +#endif /* __ENTRY_POINT_H__ */
\ No newline at end of file diff --git a/libcpu/aarch64/common/exception.c b/libcpu/aarch64/common/exception.c new file mode 100644 index 0000000000..c2ad726bdd --- /dev/null +++ b/libcpu/aarch64/common/exception.c @@ -0,0 +1,232 @@ +#include "rtthread.h" + +static void data_abort(unsigned long far, unsigned long iss) +{ + rt_kprintf("fault addr = 0x%016lx\n", far); + if (iss & 0x40) + { + rt_kprintf("abort caused by write instruction\n"); + } + else + { + rt_kprintf("abort caused by read instruction\n"); + } + switch (iss & 0x3f) + { + case 0b000000: + rt_kprintf("Address size fault, zeroth level of translation or translation table base register\n"); + break; + + case 0b000001: + rt_kprintf("Address size fault, first level\n"); + break; + + case 0b000010: + rt_kprintf("Address size fault, second level\n"); + break; + + case 0b000011: + rt_kprintf("Address size fault, third level\n"); + break; + + case 0b000100: + rt_kprintf("Translation fault, zeroth level\n"); + break; + + case 0b000101: + rt_kprintf("Translation fault, first level\n"); + break; + + case 0b000110: + rt_kprintf("Translation fault, second level\n"); + break; + + case 0b000111: + rt_kprintf("Translation fault, third level\n"); + break; + + case 0b001001: + rt_kprintf("Access flag fault, first level\n"); + break; + + case 0b001010: + rt_kprintf("Access flag fault, second level\n"); + break; + + case 0b001011: + rt_kprintf("Access flag fault, third level\n"); + break; + + case 0b001101: + rt_kprintf("Permission fault, first level\n"); + break; + + case 0b001110: + rt_kprintf("Permission fault, second level\n"); + break; + + case 0b001111: + rt_kprintf("Permission fault, third level\n"); + break; + + case 0b010000: + rt_kprintf("Synchronous external abort, not on translation table walk\n"); + break; + + case 0b011000: + rt_kprintf("Synchronous parity or ECC error on memory access, not on translation table walk\n"); + break; + + case 0b010100: + rt_kprintf("Synchronous external abort on translation table walk, zeroth level\n"); + break; + + case 0b010101: + rt_kprintf("Synchronous external abort on translation table walk, first level\n"); + break; + + case 0b010110: + rt_kprintf("Synchronous external abort on translation table walk, second level\n"); + break; + + case 0b010111: + rt_kprintf("Synchronous external abort on translation table walk, third level\n"); + break; + + case 0b011100: + rt_kprintf("Synchronous parity or ECC error on memory access on translation table walk, zeroth level\n"); + break; + + case 0b011101: + rt_kprintf("Synchronous parity or ECC error on memory access on translation table walk, first level\n"); + break; + + case 0b011110: + rt_kprintf("Synchronous parity or ECC error on memory access on translation table walk, second level\n"); + break; + + case 0b011111: + rt_kprintf("Synchronous parity or ECC error on memory access on translation table walk, third level\n"); + break; + + case 0b100001: + rt_kprintf("Alignment fault\n"); + break; + + case 0b110000: + rt_kprintf("TLB conflict abort\n"); + break; + + case 0b110100: + rt_kprintf("IMPLEMENTATION DEFINED fault (Lockdown fault)\n"); + break; + + case 0b110101: + rt_kprintf("IMPLEMENTATION DEFINED fault (Unsupported Exclusive access fault)\n"); + break; + + case 0b111101: + rt_kprintf("Section Domain Fault, used only for faults reported in the PAR_EL1\n"); + break; + + case 0b111110: + rt_kprintf("Page Domain Fault, used only for faults reported in the PAR_EL1\n"); + break; + + default: + rt_kprintf("unknow abort\n"); + break; + } +} + +void process_exception(unsigned long esr, unsigned long epc) +{ + rt_uint8_t ec; + rt_uint32_t iss; + unsigned long fault_addr; + rt_kprintf("\nexception info:\n"); + ec = (unsigned char)((esr >> 26) & 0x3fU); + iss = (unsigned int)(esr & 0x00ffffffU); + rt_kprintf("esr.EC :0x%02x\n", ec); + rt_kprintf("esr.IL :0x%02x\n", (unsigned char)((esr >> 25) & 0x01U)); + rt_kprintf("esr.ISS:0x%08x\n", iss); + rt_kprintf("epc :0x%016p\n", (void *)epc); + switch (ec) + { + case 0x00: + rt_kprintf("Exceptions with an unknow reason\n"); + break; + + case 0x01: + rt_kprintf("Exceptions from an WFI or WFE instruction\n"); + break; + + case 0x03: + rt_kprintf("Exceptions from an MCR or MRC access to CP15 from AArch32\n"); + break; + + case 0x04: + rt_kprintf("Exceptions from an MCRR or MRRC access to CP15 from AArch32\n"); + break; + + case 0x05: + rt_kprintf("Exceptions from an MCR or MRC access to CP14 from AArch32\n"); + break; + + case 0x06: + rt_kprintf("Exceptions from an LDC or STC access to CP14 from AArch32\n"); + break; + + case 0x07: + rt_kprintf("Exceptions from Access to Advanced SIMD or floating-point registers\n"); + break; + + case 0x08: + rt_kprintf("Exceptions from an MRC (or VMRS) access to CP10 from AArch32\n"); + break; + + case 0x0c: + rt_kprintf("Exceptions from an MCRR or MRRC access to CP14 from AArch32\n"); + break; + + case 0x0e: + rt_kprintf("Exceptions that occur because ther value of PSTATE.IL is 1\n"); + break; + + case 0x11: + rt_kprintf("SVC call from AArch32 state\n"); + break; + + case 0x15: + rt_kprintf("SVC call from AArch64 state\n"); + break; + + case 0x20: + rt_kprintf("Instruction abort from lower exception level\n"); + break; + + case 0x21: + rt_kprintf("Instruction abort from current exception level\n"); + break; + + case 0x22: + rt_kprintf("PC alignment fault\n"); + break; + + case 0x24: + rt_kprintf("Data abort from a lower Exception level\n"); + __asm__ volatile("mrs %0, far_el1":"=r"(fault_addr)); + data_abort(fault_addr, iss); + break; + + case 0x25: + rt_kprintf("Data abort\n"); + __asm__ volatile("mrs %0, far_el1":"=r"(fault_addr)); + data_abort(fault_addr, iss); + break; + + default: + rt_kprintf("Other error\n"); + break; + } +} diff --git a/libcpu/aarch64/common/gic.c b/libcpu/aarch64/common/gic.c index 95a710c480..3cb9052cfc 100644 --- a/libcpu/aarch64/common/gic.c +++ b/libcpu/aarch64/common/gic.c @@ -9,7 +9,6 @@ * 2014-04-03 Grissiom many enhancements * 2018-11-22 Jesven add rt_hw_ipi_send() * add rt_hw_ipi_handler_install() - * 2022-03-08 GuEe-GUI add BSP bind SPI CPU self support */ #include <rthw.h> @@ -17,17 +16,15 @@ #if defined(BSP_USING_GIC) && defined(BSP_USING_GICV2) -#include <gic.h> -#include <cpuport.h> - -#include <board.h> +#include "gic.h" +#include "cp15.h" struct arm_gic { rt_uint64_t offset; /* the first interrupt index in the vector table */ rt_uint64_t dist_hw_base; /* the base address of the gic distributor */ - rt_uint64_t cpu_hw_base; /* the base address of the gic cpu interface */ + rt_uint64_t cpu_hw_base; /* the base addrees of the gic cpu interface */ }; /* 'ARM_GIC_MAX_NR' is the number of cores */ @@ -35,33 +32,33 @@ static struct arm_gic _gic_table[ARM_GIC_MAX_NR]; /** Macro to access the Generic Interrupt Controller Interface (GICC) */ -#define GIC_CPU_CTRL(hw_base) HWREG32((hw_base) + 0x00U) -#define GIC_CPU_PRIMASK(hw_base) HWREG32((hw_base) + 0x04U) -#define GIC_CPU_BINPOINT(hw_base) HWREG32((hw_base) + 0x08U) -#define GIC_CPU_INTACK(hw_base) HWREG32((hw_base) + 0x0cU) -#define GIC_CPU_EOI(hw_base) HWREG32((hw_base) + 0x10U) -#define GIC_CPU_RUNNINGPRI(hw_base) HWREG32((hw_base) + 0x14U) -#define GIC_CPU_HIGHPRI(hw_base) HWREG32((hw_base) + 0x18U) -#define GIC_CPU_IIDR(hw_base) HWREG32((hw_base) + 0xFCU) +#define GIC_CPU_CTRL(hw_base) __REG32((hw_base) + 0x00U) +#define GIC_CPU_PRIMASK(hw_base) __REG32((hw_base) + 0x04U) +#define GIC_CPU_BINPOINT(hw_base) __REG32((hw_base) + 0x08U) +#define GIC_CPU_INTACK(hw_base) __REG32((hw_base) + 0x0cU) +#define GIC_CPU_EOI(hw_base) __REG32((hw_base) + 0x10U) +#define GIC_CPU_RUNNINGPRI(hw_base) __REG32((hw_base) + 0x14U) +#define GIC_CPU_HIGHPRI(hw_base) __REG32((hw_base) + 0x18U) +#define GIC_CPU_IIDR(hw_base) __REG32((hw_base) + 0xFCU) /** Macro to access the Generic Interrupt Controller Distributor (GICD) */ -#define GIC_DIST_CTRL(hw_base) HWREG32((hw_base) + 0x000U) -#define GIC_DIST_TYPE(hw_base) HWREG32((hw_base) + 0x004U) -#define GIC_DIST_IGROUP(hw_base, n) HWREG32((hw_base) + 0x080U + ((n)/32U) * 4U) -#define GIC_DIST_ENABLE_SET(hw_base, n) HWREG32((hw_base) + 0x100U + ((n)/32U) * 4U) -#define GIC_DIST_ENABLE_CLEAR(hw_base, n) HWREG32((hw_base) + 0x180U + ((n)/32U) * 4U) -#define GIC_DIST_PENDING_SET(hw_base, n) HWREG32((hw_base) + 0x200U + ((n)/32U) * 4U) -#define GIC_DIST_PENDING_CLEAR(hw_base, n) HWREG32((hw_base) + 0x280U + ((n)/32U) * 4U) -#define GIC_DIST_ACTIVE_SET(hw_base, n) HWREG32((hw_base) + 0x300U + ((n)/32U) * 4U) -#define GIC_DIST_ACTIVE_CLEAR(hw_base, n) HWREG32((hw_base) + 0x380U + ((n)/32U) * 4U) -#define GIC_DIST_PRI(hw_base, n) HWREG32((hw_base) + 0x400U + ((n)/4U) * 4U) -#define GIC_DIST_TARGET(hw_base, n) HWREG32((hw_base) + 0x800U + ((n)/4U) * 4U) -#define GIC_DIST_CONFIG(hw_base, n) HWREG32((hw_base) + 0xc00U + ((n)/16U) * 4U) -#define GIC_DIST_SOFTINT(hw_base) HWREG32((hw_base) + 0xf00U) -#define GIC_DIST_CPENDSGI(hw_base, n) HWREG32((hw_base) + 0xf10U + ((n)/4U) * 4U) -#define GIC_DIST_SPENDSGI(hw_base, n) HWREG32((hw_base) + 0xf20U + ((n)/4U) * 4U) -#define GIC_DIST_ICPIDR2(hw_base) HWREG32((hw_base) + 0xfe8U) +#define GIC_DIST_CTRL(hw_base) __REG32((hw_base) + 0x000U) +#define GIC_DIST_TYPE(hw_base) __REG32((hw_base) + 0x004U) +#define GIC_DIST_IGROUP(hw_base, n) __REG32((hw_base) + 0x080U + ((n)/32U) * 4U) +#define GIC_DIST_ENABLE_SET(hw_base, n) __REG32((hw_base) + 0x100U + ((n)/32U) * 4U) +#define GIC_DIST_ENABLE_CLEAR(hw_base, n) __REG32((hw_base) + 0x180U + ((n)/32U) * 4U) +#define GIC_DIST_PENDING_SET(hw_base, n) __REG32((hw_base) + 0x200U + ((n)/32U) * 4U) +#define GIC_DIST_PENDING_CLEAR(hw_base, n) __REG32((hw_base) + 0x280U + ((n)/32U) * 4U) +#define GIC_DIST_ACTIVE_SET(hw_base, n) __REG32((hw_base) + 0x300U + ((n)/32U) * 4U) +#define GIC_DIST_ACTIVE_CLEAR(hw_base, n) __REG32((hw_base) + 0x380U + ((n)/32U) * 4U) +#define GIC_DIST_PRI(hw_base, n) __REG32((hw_base) + 0x400U + ((n)/4U) * 4U) +#define GIC_DIST_TARGET(hw_base, n) __REG32((hw_base) + 0x800U + ((n)/4U) * 4U) +#define GIC_DIST_CONFIG(hw_base, n) __REG32((hw_base) + 0xc00U + ((n)/16U) * 4U) +#define GIC_DIST_SOFTINT(hw_base) __REG32((hw_base) + 0xf00U) +#define GIC_DIST_CPENDSGI(hw_base, n) __REG32((hw_base) + 0xf10U + ((n)/4U) * 4U) +#define GIC_DIST_SPENDSGI(hw_base, n) __REG32((hw_base) + 0xf20U + ((n)/4U) * 4U) +#define GIC_DIST_ICPIDR2(hw_base) __REG32((hw_base) + 0xfe8U) static unsigned int _gic_max_irq; @@ -184,7 +181,7 @@ void arm_gic_clear_pending_irq(rt_uint64_t index, int irq) } } -void arm_gic_set_configuration(rt_uint64_t index, int irq, rt_uint32_t config) +void arm_gic_set_configuration(rt_uint64_t index, int irq, uint32_t config) { rt_uint64_t icfgr; rt_uint64_t shift; @@ -328,8 +325,6 @@ void arm_gic_send_sgi(rt_uint64_t index, int irq, rt_uint64_t target_list, rt_ui GIC_DIST_SOFTINT(_gic_table[index].dist_hw_base) = ((filter_list & 0x3U) << 24U) | ((target_list & 0xFFUL) << 16U) | (irq & 0x0FUL); - - __DSB(); } rt_uint64_t arm_gic_get_high_pending_irq(rt_uint64_t index) @@ -348,8 +343,8 @@ rt_uint64_t arm_gic_get_interface_id(rt_uint64_t index) void arm_gic_set_group(rt_uint64_t index, int irq, rt_uint64_t group) { - rt_uint32_t igroupr; - rt_uint32_t shift; + uint32_t igroupr; + uint32_t shift; RT_ASSERT(index < ARM_GIC_MAX_NR); RT_ASSERT(group <= 1U); @@ -380,10 +375,6 @@ int arm_gic_dist_init(rt_uint64_t index, rt_uint64_t dist_base, int irq_start) unsigned int gic_type, i; rt_uint64_t cpumask = 1U << 0U; -#ifdef ARM_SPI_BIND_CPU_ID - cpumask = 1U << ARM_SPI_BIND_CPU_ID; -#endif - RT_ASSERT(index < ARM_GIC_MAX_NR); _gic_table[index].dist_hw_base = dist_base; @@ -438,6 +429,12 @@ int arm_gic_dist_init(rt_uint64_t index, rt_uint64_t dist_base, int irq_start) } /* All interrupts defaults to IGROUP1(IRQ). */ + /* + for (i = 0; i < _gic_max_irq; i += 32) + { + GIC_DIST_IGROUP(dist_base, i) = 0xffffffffU; + } + */ for (i = 0U; i < _gic_max_irq; i += 32U) { GIC_DIST_IGROUP(dist_base, i) = 0U; @@ -489,17 +486,23 @@ void arm_gic_dump(rt_uint64_t index) rt_kprintf("--- hw mask ---\n"); for (i = 0U; i < _gic_max_irq / 32U; i++) { - rt_kprintf("0x%08x, ", GIC_DIST_ENABLE_SET(_gic_table[index].dist_hw_base, i * 32U)); + rt_kprintf("0x%08x, ", + GIC_DIST_ENABLE_SET(_gic_table[index].dist_hw_base, + i * 32U)); } rt_kprintf("\n--- hw pending ---\n"); for (i = 0U; i < _gic_max_irq / 32U; i++) { - rt_kprintf("0x%08x, ", GIC_DIST_PENDING_SET(_gic_table[index].dist_hw_base, i * 32U)); + rt_kprintf("0x%08x, ", + GIC_DIST_PENDING_SET(_gic_table[index].dist_hw_base, + i * 32U)); } rt_kprintf("\n--- hw active ---\n"); for (i = 0U; i < _gic_max_irq / 32U; i++) { - rt_kprintf("0x%08x, ", GIC_DIST_ACTIVE_SET(_gic_table[index].dist_hw_base, i * 32U)); + rt_kprintf("0x%08x, ", + GIC_DIST_ACTIVE_SET(_gic_table[index].dist_hw_base, + i * 32U)); } rt_kprintf("\n"); } diff --git a/libcpu/aarch64/common/gic.h b/libcpu/aarch64/common/gic.h index 7ea0d6d3af..4cdcb44028 100644 --- a/libcpu/aarch64/common/gic.h +++ b/libcpu/aarch64/common/gic.h @@ -11,9 +11,8 @@ #ifndef __GIC_H__ #define __GIC_H__ -#include <rtdef.h> - -#if defined(BSP_USING_GIC) && defined(BSP_USING_GICV2) +#include <rthw.h> +#include <board.h> int arm_gic_get_active_irq(rt_uint64_t index); void arm_gic_ack(rt_uint64_t index, int irq); @@ -25,7 +24,7 @@ rt_uint64_t arm_gic_get_pending_irq(rt_uint64_t index, int irq); void arm_gic_set_pending_irq(rt_uint64_t index, int irq); void arm_gic_clear_pending_irq(rt_uint64_t index, int irq); -void arm_gic_set_configuration(rt_uint64_t index, int irq, rt_uint32_t config); +void arm_gic_set_configuration(rt_uint64_t index, int irq, uint32_t config); rt_uint64_t arm_gic_get_configuration(rt_uint64_t index, int irq); void arm_gic_clear_active(rt_uint64_t index, int irq); @@ -59,7 +58,5 @@ int arm_gic_cpu_init(rt_uint64_t index, rt_uint64_t cpu_base); void arm_gic_dump_type(rt_uint64_t index); void arm_gic_dump(rt_uint64_t index); -#endif /* defined(BSP_USING_GIC) && defined(BSP_USING_GICV2) */ - #endif diff --git a/libcpu/aarch64/common/gicv3.c b/libcpu/aarch64/common/gicv3.c index 67010f1d57..eb7c8f2f6e 100644 --- a/libcpu/aarch64/common/gicv3.c +++ b/libcpu/aarch64/common/gicv3.c @@ -27,7 +27,7 @@ #if defined(BSP_USING_GIC) && defined(BSP_USING_GICV3) #include <gicv3.h> -#include <cpuport.h> +#include <cp15.h> #include <board.h> @@ -479,7 +479,7 @@ rt_uint64_t arm_gic_get_irq_status(rt_uint64_t index, int irq) } #ifdef RT_USING_SMP -void arm_gic_send_affinity_sgi(rt_uint64_t index, int irq, rt_uint64_t cpu_masks[], rt_uint64_t routing_mode) +void arm_gic_send_affinity_sgi(rt_uint64_t index, int irq, rt_uint32_t cpu_masks[], rt_uint64_t routing_mode) { const int cpu_mask_cpu_max_nr = sizeof(cpu_masks[0]) * 8; rt_uint64_t int_id = (irq & 0xf) << 24; @@ -731,9 +731,22 @@ int arm_gic_redist_init(rt_uint64_t index, rt_uint64_t redist_base) { int i; int cpu_id = rt_hw_cpu_id(); + static int master_cpu_id = -1; RT_ASSERT(index < ARM_GIC_MAX_NR); + if (master_cpu_id < 0) + { + master_cpu_id = cpu_id; + rt_hw_cpu_dcache_ops(RT_HW_CACHE_FLUSH, &master_cpu_id, sizeof(master_cpu_id)); + } + + if (!_gic_table[index].redist_hw_base[master_cpu_id]) + { + _gic_table[index].redist_hw_base[master_cpu_id] = redist_base; + } + redist_base = _gic_table[index].redist_hw_base[master_cpu_id]; + redist_base += cpu_id * (2 << 16); _gic_table[index].redist_hw_base[cpu_id] = redist_base; diff --git a/libcpu/aarch64/common/gicv3.h b/libcpu/aarch64/common/gicv3.h index 0cb6bda03a..cdc7720b37 100644 --- a/libcpu/aarch64/common/gicv3.h +++ b/libcpu/aarch64/common/gicv3.h @@ -51,7 +51,7 @@ rt_uint64_t arm_gic_get_binary_point(rt_uint64_t index); rt_uint64_t arm_gic_get_irq_status(rt_uint64_t index, int irq); #ifdef RT_USING_SMP -void arm_gic_send_affinity_sgi(rt_uint64_t index, int irq, rt_uint64_t cpu_masks[], rt_uint64_t routing_mode); +void arm_gic_send_affinity_sgi(rt_uint64_t index, int irq, rt_uint32_t cpu_masks[], rt_uint64_t routing_mode); #endif rt_uint64_t arm_gic_get_high_pending_irq(rt_uint64_t index); diff --git a/libcpu/aarch64/common/gtimer.c b/libcpu/aarch64/common/gtimer.c index 74187a92cb..881655c2cf 100644 --- a/libcpu/aarch64/common/gtimer.c +++ b/libcpu/aarch64/common/gtimer.c @@ -26,9 +26,9 @@ static void rt_hw_timer_isr(int vector, void *parameter) void rt_hw_gtimer_init(void) { rt_hw_interrupt_install(EL1_PHY_TIMER_IRQ_NUM, rt_hw_timer_isr, RT_NULL, "tick"); - __ISB(); + rt_hw_isb(); timer_step = rt_hw_get_gtimer_frq(); - __DSB(); + rt_hw_dsb(); timer_step /= RT_TICK_PER_SECOND; rt_hw_gtimer_local_enable(); } diff --git a/libcpu/aarch64/common/gtimer.h b/libcpu/aarch64/common/gtimer.h index c420c1f886..83a0d53165 100644 --- a/libcpu/aarch64/common/gtimer.h +++ b/libcpu/aarch64/common/gtimer.h @@ -18,7 +18,12 @@ void rt_hw_gtimer_local_enable(void); void rt_hw_gtimer_local_disable(void); void rt_hw_gtimer_enable(); -void rt_hw_gtimer_disable(); + +rt_inline void rt_hw_gtimer_disable(void) +{ + __asm__ volatile ("msr CNTP_CTL_EL0, xzr":::"memory"); +} + void rt_hw_set_gtimer_val(rt_uint64_t value); rt_uint64_t rt_hw_get_gtimer_val(); rt_uint64_t rt_hw_get_cntpct_val(); diff --git a/libcpu/aarch64/common/hypercall.c b/libcpu/aarch64/common/hypercall.c new file mode 100644 index 0000000000..73b64d5aae --- /dev/null +++ b/libcpu/aarch64/common/hypercall.c @@ -0,0 +1,16 @@ +/* + * Copyright (c) 2006-2020, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2022-02-24 GuEe-GUI first version + */ + +#include <hypercall.h> + +rt_err_t rt_hv_stage2_map(unsigned long paddr, unsigned long size) +{ + return rt_hw_hypercall(120, paddr & (~4095), (paddr & (~4095)) + size, (1 << 0) | (1 << 1) | (1 << 4), 0, 0, 0, 0); +} diff --git a/libcpu/aarch64/common/hypercall.h b/libcpu/aarch64/common/hypercall.h new file mode 100644 index 0000000000..e9599d7f78 --- /dev/null +++ b/libcpu/aarch64/common/hypercall.h @@ -0,0 +1,27 @@ +/* + * Copyright (c) 2006-2020, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2022-02-24 GuEe-GUI first version + */ + +#ifndef __HYPERCALL_H__ +#define __HYPERCALL_H__ + +#include <rtdef.h> + +rt_inline rt_uint32_t rt_hw_hypercall(rt_uint32_t w0, rt_uint64_t x1, rt_uint64_t x2, + rt_uint64_t x3, rt_uint64_t x4, rt_uint64_t x5, rt_uint64_t x6, rt_uint32_t w7) +{ + register rt_uint64_t ret __asm__ ("x0"); + __asm__ volatile ("hvc #0"); + + return (rt_uint32_t)ret; +} + +rt_err_t rt_hv_stage2_map(unsigned long paddr, unsigned long size); + +#endif diff --git a/libcpu/aarch64/common/interrupt.c b/libcpu/aarch64/common/interrupt.c index c5db2d46af..3e527640b2 100644 --- a/libcpu/aarch64/common/interrupt.c +++ b/libcpu/aarch64/common/interrupt.c @@ -1,5 +1,5 @@ /* - * Copyright (c) 2006-2021, RT-Thread Development Team + * Copyright (c) 2006-2022, RT-Thread Development Team * * SPDX-License-Identifier: Apache-2.0 * @@ -14,23 +14,45 @@ #include "interrupt.h" #include "gic.h" #include "gicv3.h" -#include "armv8.h" -#include "mmu.h" -#include "cpuport.h" /* exception and interrupt handler table */ struct rt_irq_desc isr_table[MAX_HANDLERS]; +#ifndef RT_USING_SMP /* Those variables will be accessed in ISR, so we need to share them. */ rt_ubase_t rt_interrupt_from_thread = 0; rt_ubase_t rt_interrupt_to_thread = 0; rt_ubase_t rt_thread_switch_interrupt_flag = 0; +#endif + +#ifndef RT_CPUS_NR +#define RT_CPUS_NR 1 +#endif -extern int system_vectors; +const unsigned int VECTOR_BASE = 0x00; +extern void rt_cpu_vector_set_base(void *addr); +extern void *system_vectors; + +#ifdef RT_USING_SMP +#define rt_interrupt_nest rt_cpu_self()->irq_nest +#else +extern volatile rt_uint8_t rt_interrupt_nest; +#endif + +#ifdef SOC_BCM283x +static void default_isr_handler(int vector, void *param) +{ +#ifdef RT_USING_SMP + rt_kprintf("cpu %d unhandled irq: %d\n", rt_hw_cpu_id(),vector); +#else + rt_kprintf("unhandled irq: %d\n",vector); +#endif +} +#endif void rt_hw_vector_init(void) { - rt_hw_set_current_vbar((rt_ubase_t)&system_vectors); + rt_cpu_vector_set_base(&system_vectors); } /** @@ -38,23 +60,69 @@ void rt_hw_vector_init(void) */ void rt_hw_interrupt_init(void) { +#ifdef SOC_BCM283x + rt_uint32_t index; /* initialize vector table */ rt_hw_vector_init(); /* initialize exceptions table */ rt_memset(isr_table, 0x00, sizeof(isr_table)); -#ifndef BSP_USING_GIC /* mask all of interrupts */ IRQ_DISABLE_BASIC = 0x000000ff; IRQ_DISABLE1 = 0xffffffff; IRQ_DISABLE2 = 0xffffffff; + for (index = 0; index < MAX_HANDLERS; index ++) + { + isr_table[index].handler = default_isr_handler; + isr_table[index].param = RT_NULL; +#ifdef RT_USING_INTERRUPT_INFO + rt_strncpy(isr_table[index].name, "unknown", RT_NAME_MAX); + isr_table[index].counter = 0; +#endif + } + + /* init interrupt nest, and context in thread sp */ + rt_interrupt_nest = 0; + rt_interrupt_from_thread = 0; + rt_interrupt_to_thread = 0; + rt_thread_switch_interrupt_flag = 0; #else + rt_uint64_t gic_cpu_base; + rt_uint64_t gic_dist_base; +#ifdef BSP_USING_GICV3 + rt_uint64_t gic_rdist_base; +#endif + rt_uint64_t gic_irq_start; + + /* initialize vector table */ + rt_hw_vector_init(); + + /* initialize exceptions table */ + rt_memset(isr_table, 0x00, sizeof(isr_table)); + /* initialize ARM GIC */ - arm_gic_dist_init(0, platform_get_gic_dist_base(), GIC_IRQ_START); - arm_gic_cpu_init(0, platform_get_gic_cpu_base()); +#ifdef RT_USING_SMART + gic_dist_base = (rt_uint64_t)rt_hw_mmu_map(&mmu_info, 0, (void*)platform_get_gic_dist_base(), 0x2000, MMU_MAP_K_DEVICE); + gic_cpu_base = (rt_uint64_t)rt_hw_mmu_map(&mmu_info, 0, (void*)platform_get_gic_cpu_base(), 0x1000, MMU_MAP_K_DEVICE); +#ifdef BSP_USING_GICV3 + gic_rdist_base = (rt_uint64_t)rt_hw_mmu_map(&mmu_info, 0, (void*)platform_get_gic_redist_base(), + RT_CPUS_NR * (2 << 16), MMU_MAP_K_DEVICE); +#endif +#else + gic_dist_base = platform_get_gic_dist_base(); + gic_cpu_base = platform_get_gic_cpu_base(); +#ifdef BSP_USING_GICV3 + gic_rdist_base = platform_get_gic_redist_base(); +#endif +#endif + + gic_irq_start = GIC_IRQ_START; + + arm_gic_dist_init(0, gic_dist_base, gic_irq_start); + arm_gic_cpu_init(0, gic_cpu_base); #ifdef BSP_USING_GICV3 - arm_gic_redist_init(0, platform_get_gic_redist_base()); + arm_gic_redist_init(0, gic_rdist_base); #endif #endif } @@ -65,7 +133,7 @@ void rt_hw_interrupt_init(void) */ void rt_hw_interrupt_mask(int vector) { -#ifndef BSP_USING_GIC +#ifdef SOC_BCM283x if (vector < 32) { IRQ_DISABLE1 = (1 << vector); @@ -91,8 +159,8 @@ void rt_hw_interrupt_mask(int vector) */ void rt_hw_interrupt_umask(int vector) { -#ifndef BSP_USING_GIC - if (vector < 32) +#ifdef SOC_BCM283x +if (vector < 32) { IRQ_ENABLE1 = (1 << vector); } @@ -117,7 +185,7 @@ void rt_hw_interrupt_umask(int vector) */ int rt_hw_interrupt_get_irq(void) { -#ifdef BSP_USING_GIC +#ifndef SOC_BCM283x return arm_gic_get_active_irq(0); #else return 0; @@ -130,11 +198,12 @@ int rt_hw_interrupt_get_irq(void) */ void rt_hw_interrupt_ack(int vector) { -#ifdef BSP_USING_GIC +#ifndef SOC_BCM283x arm_gic_ack(0, vector); #endif } +#ifndef SOC_BCM283x /** * This function set interrupt CPU targets. * @param vector: the interrupt number @@ -142,9 +211,7 @@ void rt_hw_interrupt_ack(int vector) */ void rt_hw_interrupt_set_target_cpus(int vector, unsigned int cpu_mask) { -#ifdef BSP_USING_GIC arm_gic_set_cpu(0, vector, cpu_mask); -#endif } /** @@ -154,11 +221,7 @@ void rt_hw_interrupt_set_target_cpus(int vector, unsigned int cpu_mask) */ unsigned int rt_hw_interrupt_get_target_cpus(int vector) { -#ifdef BSP_USING_GIC return arm_gic_get_target_cpu(0, vector); -#else - return -RT_ERROR; -#endif } /** @@ -168,9 +231,7 @@ unsigned int rt_hw_interrupt_get_target_cpus(int vector) */ void rt_hw_interrupt_set_triger_mode(int vector, unsigned int mode) { -#ifdef BSP_USING_GIC arm_gic_set_configuration(0, vector, mode); -#endif } /** @@ -180,11 +241,7 @@ void rt_hw_interrupt_set_triger_mode(int vector, unsigned int mode) */ unsigned int rt_hw_interrupt_get_triger_mode(int vector) { -#ifdef BSP_USING_GIC return arm_gic_get_configuration(0, vector); -#else - return -RT_ERROR; -#endif } /** @@ -193,9 +250,7 @@ unsigned int rt_hw_interrupt_get_triger_mode(int vector) */ void rt_hw_interrupt_set_pending(int vector) { -#ifdef BSP_USING_GIC arm_gic_set_pending_irq(0, vector); -#endif } /** @@ -205,11 +260,7 @@ void rt_hw_interrupt_set_pending(int vector) */ unsigned int rt_hw_interrupt_get_pending(int vector) { -#ifdef BSP_USING_GIC return arm_gic_get_pending_irq(0, vector); -#else - return -RT_ERROR; -#endif } /** @@ -218,9 +269,7 @@ unsigned int rt_hw_interrupt_get_pending(int vector) */ void rt_hw_interrupt_clear_pending(int vector) { -#ifdef BSP_USING_GIC arm_gic_clear_pending_irq(0, vector); -#endif } /** @@ -230,9 +279,7 @@ void rt_hw_interrupt_clear_pending(int vector) */ void rt_hw_interrupt_set_priority(int vector, unsigned int priority) { -#ifdef BSP_USING_GIC arm_gic_set_priority(0, vector, priority); -#endif } /** @@ -242,11 +289,7 @@ void rt_hw_interrupt_set_priority(int vector, unsigned int priority) */ unsigned int rt_hw_interrupt_get_priority(int vector) { -#ifdef BSP_USING_GIC return arm_gic_get_priority(0, vector); -#else - return -RT_ERROR; -#endif } /** @@ -255,9 +298,7 @@ unsigned int rt_hw_interrupt_get_priority(int vector) */ void rt_hw_interrupt_set_priority_mask(unsigned int priority) { -#ifdef BSP_USING_GIC arm_gic_set_interface_prior_mask(0, priority); -#endif } /** @@ -267,11 +308,7 @@ void rt_hw_interrupt_set_priority_mask(unsigned int priority) */ unsigned int rt_hw_interrupt_get_priority_mask(void) { -#ifdef BSP_USING_GIC return arm_gic_get_interface_prior_mask(0); -#else - return -RT_ERROR; -#endif } /** @@ -281,7 +318,6 @@ unsigned int rt_hw_interrupt_get_priority_mask(void) */ int rt_hw_interrupt_set_prior_group_bits(unsigned int bits) { -#ifdef BSP_USING_GIC int status; if (bits < 8) @@ -295,9 +331,6 @@ int rt_hw_interrupt_set_prior_group_bits(unsigned int bits) } return (status); -#else - return -RT_ERROR; -#endif } /** @@ -307,16 +340,13 @@ int rt_hw_interrupt_set_prior_group_bits(unsigned int bits) */ unsigned int rt_hw_interrupt_get_prior_group_bits(void) { -#ifdef BSP_USING_GIC unsigned int bp; bp = arm_gic_get_binary_point(0) & 0x07; return (7 - bp); -#else - return -RT_ERROR; -#endif } +#endif /* SOC_BCM283x */ /** * This function will install a interrupt service routine to a interrupt. @@ -349,26 +379,10 @@ rt_isr_handler_t rt_hw_interrupt_install(int vector, rt_isr_handler_t handler, #ifdef RT_USING_SMP void rt_hw_ipi_send(int ipi_vector, unsigned int cpu_mask) { -#ifdef BSP_USING_GIC #ifdef BSP_USING_GICV2 arm_gic_send_sgi(0, ipi_vector, cpu_mask, 0); -#else - arm_gic_send_affinity_sgi(0, ipi_vector, (rt_uint64_t *)&cpu_mask, GICV3_ROUTED_TO_SPEC); -#endif -#else - int i; - - __DSB(); - - for (i = 0; i < RT_CPUS_NR; ++i) - { - if (cpu_mask & (1 << i)) - { - IPI_MAILBOX_SET(i) = 1 << ipi_vector; - } - } - - __DSB(); +#elif defined(BSP_USING_GICV3) + arm_gic_send_affinity_sgi(0, ipi_vector, (unsigned int *)&cpu_mask, GICV3_ROUTED_TO_SPEC); #endif } @@ -378,4 +392,3 @@ void rt_hw_ipi_handler_install(int ipi_vector, rt_isr_handler_t ipi_isr_handler) rt_hw_interrupt_install(ipi_vector, ipi_isr_handler, 0, "IPI_HANDLER"); } #endif - diff --git a/libcpu/aarch64/common/interrupt.h b/libcpu/aarch64/common/interrupt.h index dda6c5534a..1c4199c61b 100644 --- a/libcpu/aarch64/common/interrupt.h +++ b/libcpu/aarch64/common/interrupt.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2006-2021, RT-Thread Development Team + * Copyright (c) 2006-2018, RT-Thread Development Team * * SPDX-License-Identifier: Apache-2.0 * diff --git a/libcpu/aarch64/common/mmu.c b/libcpu/aarch64/common/mmu.c index 777a0a5b1a..cac70d2767 100644 --- a/libcpu/aarch64/common/mmu.c +++ b/libcpu/aarch64/common/mmu.c @@ -1,49 +1,83 @@ /* - * Copyright (c) 2006-2021, RT-Thread Development Team + * Copyright (c) 2006-2018, RT-Thread Development Team * * SPDX-License-Identifier: Apache-2.0 * * Change Logs: * Date Author Notes - * 2021-11-28 GuEe-GUI first version + * 2012-01-10 bernard porting to AM1808 */ #include <rtthread.h> #include <rthw.h> +#include <board.h> -#include <cpuport.h> -#include <mmu.h> +#include "mmu.h" -#define ARCH_SECTION_SHIFT 21 -#define ARCH_SECTION_SIZE (1 << ARCH_SECTION_SHIFT) -#define ARCH_SECTION_MASK (ARCH_SECTION_SIZE - 1) -#define ARCH_PAGE_SHIFT 12 -#define ARCH_PAGE_SIZE (1 << ARCH_PAGE_SHIFT) -#define ARCH_PAGE_MASK (ARCH_PAGE_SIZE - 1) +#ifdef RT_USING_SMART +#include <lwp_mm.h> +#include <page.h> +#endif -#define MMU_LEVEL_MASK 0x1ffUL -#define MMU_LEVEL_SHIFT 9 -#define MMU_ADDRESS_BITS 39 -#define MMU_ADDRESS_MASK 0x0000fffffffff000UL -#define MMU_ATTRIB_MASK 0xfff0000000000ffcUL +#define MMU_LEVEL_MASK 0x1ffUL +#define MMU_LEVEL_SHIFT 9 +#define MMU_ADDRESS_BITS 39 +#define MMU_ADDRESS_MASK 0x0000fffffffff000UL +#define MMU_ATTRIB_MASK 0xfff0000000000ffcUL -#define MMU_TYPE_MASK 3UL -#define MMU_TYPE_USED 1UL -#define MMU_TYPE_BLOCK 1UL -#define MMU_TYPE_TABLE 3UL -#define MMU_TYPE_PAGE 3UL +#define MMU_TYPE_MASK 3UL +#define MMU_TYPE_USED 1UL +#define MMU_TYPE_BLOCK 1UL +#define MMU_TYPE_TABLE 3UL +#define MMU_TYPE_PAGE 3UL -#define MMU_TBL_BLOCK_2M_LEVEL 2 +#define MMU_TBL_BLOCK_2M_LEVEL 2 +#define MMU_TBL_PAGE_4k_LEVEL 3 +#define MMU_TBL_LEVEL_NR 4 + +void *_rt_hw_mmu_v2p(rt_mmu_info *mmu_info, void *v_addr); + +struct page_table +{ + unsigned long page[512]; +}; + +#ifndef RT_USING_SMART #define MMU_TBL_PAGE_NR_MAX 32 -/* only map 4G io/memory */ -static volatile unsigned long MMUTable[512] __attribute__((aligned(4096))); -static volatile struct +static rt_mutex_t mm_lock = RT_NULL; + +void rt_mm_lock(void) +{ + if (rt_thread_self()) + { + if (!mm_lock) + { + mm_lock = rt_mutex_create("mm_lock", RT_IPC_FLAG_FIFO); + } + if (mm_lock) + { + rt_mutex_take(mm_lock, RT_WAITING_FOREVER); + } + } +} + +void rt_mm_unlock(void) { - unsigned long entry[512]; -} MMUPage[MMU_TBL_PAGE_NR_MAX] __attribute__((aligned(4096))); + if (rt_thread_self()) + { + if (mm_lock) + { + rt_mutex_release(mm_lock); + } + } +} + +static volatile struct page_table MMUPage[MMU_TBL_PAGE_NR_MAX] __attribute__((aligned(4096))); -static unsigned long _kernel_free_page(void) +#define rt_page_ref_inc(...) + +unsigned long rt_pages_alloc(rt_size_t size_bits) { static unsigned long i = 0; @@ -54,13 +88,37 @@ static unsigned long _kernel_free_page(void) ++i; - return (unsigned long)&MMUPage[i - 1].entry; + return (unsigned long)&MMUPage[i - 1].page; +} +#endif + +static struct page_table *__init_page_array; +static unsigned long __page_off = 0UL; +unsigned long get_free_page(void) +{ + if (!__init_page_array) + { + unsigned long temp_page_start; + asm volatile("mov %0, sp":"=r"(temp_page_start)); + __init_page_array = (struct page_table *)(temp_page_start & ~(ARCH_SECTION_MASK)); + __page_off = 2; /* 0, 1 for ttbr0, ttrb1 */ + } + __page_off++; + return (unsigned long)(__init_page_array[__page_off - 1].page); +} + +void mmu_memset(char *dst, char v, size_t len) +{ + while (len--) + { + *dst++ = v; + } } -static int _kenrel_map_2M(unsigned long *tbl, unsigned long va, unsigned long pa, unsigned long attr) +static int _map_single_page_2M(unsigned long *lv0_tbl, unsigned long va, unsigned long pa, unsigned long attr) { int level; - unsigned long *cur_lv_tbl = tbl; + unsigned long *cur_lv_tbl = lv0_tbl; unsigned long page; unsigned long off; int level_shift = MMU_ADDRESS_BITS; @@ -73,46 +131,115 @@ static int _kenrel_map_2M(unsigned long *tbl, unsigned long va, unsigned long pa { return MMU_MAP_ERROR_PANOTALIGN; } - - for (level = 0; level < MMU_TBL_BLOCK_2M_LEVEL; ++level) + for (level = 0; level < MMU_TBL_BLOCK_2M_LEVEL; level++) { off = (va >> level_shift); off &= MMU_LEVEL_MASK; - if (!(cur_lv_tbl[off] & MMU_TYPE_USED)) { - page = _kernel_free_page(); - + page = get_free_page(); if (!page) { return MMU_MAP_ERROR_NOPAGE; } + mmu_memset((char *)page, 0, ARCH_PAGE_SIZE); + cur_lv_tbl[off] = page | MMU_TYPE_TABLE; + } + page = cur_lv_tbl[off]; + if ((page & MMU_TYPE_MASK) == MMU_TYPE_BLOCK) + { + /* is block! error! */ + return MMU_MAP_ERROR_CONFLICT; + } + cur_lv_tbl = (unsigned long *)(page & MMU_ADDRESS_MASK); + level_shift -= MMU_LEVEL_SHIFT; + } + attr &= MMU_ATTRIB_MASK; + pa |= (attr | MMU_TYPE_BLOCK); /* block */ + off = (va >> ARCH_SECTION_SHIFT); + off &= MMU_LEVEL_MASK; + cur_lv_tbl[off] = pa; + return 0; +} +int armv8_init_map_2M(unsigned long *lv0_tbl, unsigned long va, unsigned long pa, unsigned long count, unsigned long attr) +{ + unsigned long i; + int ret; + + if (va & ARCH_SECTION_MASK) + { + return -1; + } + if (pa & ARCH_SECTION_MASK) + { + return -1; + } + for (i = 0; i < count; i++) + { + ret = _map_single_page_2M(lv0_tbl, va, pa, attr); + va += ARCH_SECTION_SIZE; + pa += ARCH_SECTION_SIZE; + if (ret != 0) + { + return ret; + } + } + return 0; +} + +static int _kenrel_map_2M(unsigned long *lv0_tbl, unsigned long va, unsigned long pa, unsigned long attr) +{ + int level; + unsigned long *cur_lv_tbl = lv0_tbl; + unsigned long page; + unsigned long off; + int level_shift = MMU_ADDRESS_BITS; + + if (va & ARCH_SECTION_MASK) + { + return MMU_MAP_ERROR_VANOTALIGN; + } + if (pa & ARCH_SECTION_MASK) + { + return MMU_MAP_ERROR_PANOTALIGN; + } + for (level = 0; level < MMU_TBL_BLOCK_2M_LEVEL; level++) + { + off = (va >> level_shift); + off &= MMU_LEVEL_MASK; + if (!(cur_lv_tbl[off] & MMU_TYPE_USED)) + { + page = (unsigned long)rt_pages_alloc(0); + if (!page) + { + return MMU_MAP_ERROR_NOPAGE; + } rt_memset((char *)page, 0, ARCH_PAGE_SIZE); rt_hw_cpu_dcache_ops(RT_HW_CACHE_FLUSH, (void *)page, ARCH_PAGE_SIZE); - cur_lv_tbl[off] = page | MMU_TYPE_TABLE; + cur_lv_tbl[off] = (page + PV_OFFSET) | MMU_TYPE_TABLE; rt_hw_cpu_dcache_ops(RT_HW_CACHE_FLUSH, cur_lv_tbl + off, sizeof(void *)); } else { page = cur_lv_tbl[off]; page &= MMU_ADDRESS_MASK; + /* page to va */ + page -= PV_OFFSET; + rt_page_ref_inc((void *)page, 0); } - page = cur_lv_tbl[off]; if ((page & MMU_TYPE_MASK) == MMU_TYPE_BLOCK) { /* is block! error! */ return MMU_MAP_ERROR_CONFLICT; } - - /* next level */ cur_lv_tbl = (unsigned long *)(page & MMU_ADDRESS_MASK); + cur_lv_tbl = (unsigned long *)((unsigned long)cur_lv_tbl - PV_OFFSET); level_shift -= MMU_LEVEL_SHIFT; } - attr &= MMU_ATTRIB_MASK; - pa |= (attr | MMU_TYPE_BLOCK); + pa |= (attr | MMU_TYPE_BLOCK); /* block */ off = (va >> ARCH_SECTION_SHIFT); off &= MMU_LEVEL_MASK; cur_lv_tbl[off] = pa; @@ -121,48 +248,640 @@ static int _kenrel_map_2M(unsigned long *tbl, unsigned long va, unsigned long pa return 0; } -int rt_hw_mmu_setmtt(unsigned long vaddr_start, unsigned long vaddr_end, - unsigned long paddr_start, unsigned long attr) +struct mmu_level_info { - int ret = -1; - int i; - unsigned long count; - unsigned long map_attr = MMU_MAP_CUSTOM(MMU_AP_KAUN, attr); + unsigned long *pos; + void *page; +}; - if (vaddr_start > vaddr_end) +#ifdef RT_USING_LWP +static void _kenrel_unmap_4K(unsigned long *lv0_tbl, void *v_addr) +{ + int level; + unsigned long va = (unsigned long)v_addr; + unsigned long *cur_lv_tbl = lv0_tbl; + unsigned long page; + unsigned long off; + struct mmu_level_info level_info[4]; + int ref; + int level_shift = MMU_ADDRESS_BITS; + unsigned long *pos; + + rt_memset(level_info, 0, sizeof level_info); + for (level = 0; level < MMU_TBL_LEVEL_NR; level++) { - goto end; + off = (va >> level_shift); + off &= MMU_LEVEL_MASK; + page = cur_lv_tbl[off]; + if (!(page & MMU_TYPE_USED)) + { + break; + } + if ((page & MMU_TYPE_MASK) == MMU_TYPE_BLOCK) + { + break; + } + level_info[level].pos = cur_lv_tbl + off; + cur_lv_tbl = (unsigned long *)(page & MMU_ADDRESS_MASK); + cur_lv_tbl = (unsigned long *)((unsigned long)cur_lv_tbl - PV_OFFSET); + level_info[level].page = cur_lv_tbl; + level_shift -= MMU_LEVEL_SHIFT; } - if (vaddr_start % ARCH_SECTION_SIZE) + + level = MMU_TBL_PAGE_4k_LEVEL; + pos = level_info[level].pos; + if (pos) { - vaddr_start = (vaddr_start / ARCH_SECTION_SIZE) * ARCH_SECTION_SIZE; + *pos = (unsigned long)RT_NULL; + rt_hw_cpu_dcache_ops(RT_HW_CACHE_FLUSH, pos, sizeof(void *)); } - if (paddr_start % ARCH_SECTION_SIZE) + level--; + + while (level >= 0) { - paddr_start = (paddr_start / ARCH_SECTION_SIZE) * ARCH_SECTION_SIZE; + pos = level_info[level].pos; + if (pos) + { + void *cur_page = level_info[level].page; + ref = rt_page_ref_get(cur_page, 0); + if (ref == 1) + { + *pos = (unsigned long)RT_NULL; + rt_hw_cpu_dcache_ops(RT_HW_CACHE_FLUSH, pos, sizeof(void *)); + } + rt_pages_free(cur_page, 0); + } + level--; } - if (vaddr_end % ARCH_SECTION_SIZE) + asm volatile("tlbi vae1, %0\ndsb sy"::"r"(v_addr):"memory"); + return; +} + +static int _kenrel_map_4K(unsigned long *lv0_tbl, unsigned long va, unsigned long pa, unsigned long attr) +{ + int ret = 0; + int level; + unsigned long *cur_lv_tbl = lv0_tbl; + unsigned long page; + unsigned long off; + int level_shift = MMU_ADDRESS_BITS; + + if (va & ARCH_PAGE_MASK) { - vaddr_end = (vaddr_end / ARCH_SECTION_SIZE + 1) * ARCH_SECTION_SIZE; + return MMU_MAP_ERROR_VANOTALIGN; + } + if (pa & ARCH_PAGE_MASK) + { + return MMU_MAP_ERROR_PANOTALIGN; + } + for (level = 0; level < MMU_TBL_PAGE_4k_LEVEL; level++) + { + off = (va >> level_shift); + off &= MMU_LEVEL_MASK; + if (!(cur_lv_tbl[off] & MMU_TYPE_USED)) + { + page = (unsigned long)rt_pages_alloc(0); + if (!page) + { + ret = MMU_MAP_ERROR_NOPAGE; + goto err; + } + rt_memset((void *)page, 0, ARCH_PAGE_SIZE); + rt_hw_cpu_dcache_ops(RT_HW_CACHE_FLUSH, (void *)page, ARCH_PAGE_SIZE); + cur_lv_tbl[off] = (page + PV_OFFSET) | MMU_TYPE_TABLE; + rt_hw_cpu_dcache_ops(RT_HW_CACHE_FLUSH, cur_lv_tbl + off, sizeof(void *)); + } + else + { + page = cur_lv_tbl[off]; + page &= MMU_ADDRESS_MASK; + /* page to va */ + page -= PV_OFFSET; + rt_page_ref_inc((void *)page, 0); + } + page = cur_lv_tbl[off]; + if ((page & MMU_TYPE_MASK) == MMU_TYPE_BLOCK) + { + /* is block! error! */ + ret = MMU_MAP_ERROR_CONFLICT; + goto err; + } + cur_lv_tbl = (unsigned long *)(page & MMU_ADDRESS_MASK); + cur_lv_tbl = (unsigned long *)((unsigned long)cur_lv_tbl - PV_OFFSET); + level_shift -= MMU_LEVEL_SHIFT; } + /* now is level page */ + attr &= MMU_ATTRIB_MASK; + pa |= (attr | MMU_TYPE_PAGE); /* page */ + off = (va >> ARCH_PAGE_SHIFT); + off &= MMU_LEVEL_MASK; + cur_lv_tbl[off] = pa; /* page */ + rt_hw_cpu_dcache_ops(RT_HW_CACHE_FLUSH, cur_lv_tbl + off, sizeof(void *)); + return ret; +err: + _kenrel_unmap_4K(lv0_tbl, (void *)va); + return ret; +} +#endif - count = (vaddr_end - vaddr_start) >> ARCH_SECTION_SHIFT; +static int _kernel_map_fixed(unsigned long *lv0_tbl, unsigned long va, unsigned long pa, unsigned long count, unsigned long attr) +{ + unsigned long i; + int ret; + unsigned long _attr = MMU_MAP_CUSTOM(MMU_AP_KAUN, attr); + if (va & ARCH_SECTION_MASK) + { + return -1; + } + if (pa & ARCH_SECTION_MASK) + { + return -1; + } for (i = 0; i < count; i++) { - ret = _kenrel_map_2M((void *)MMUTable, vaddr_start, paddr_start, map_attr); - vaddr_start += ARCH_SECTION_SIZE; - paddr_start += ARCH_SECTION_SIZE; - + ret = _kenrel_map_2M(lv0_tbl, va, pa, _attr); + va += ARCH_SECTION_SIZE; + pa += ARCH_SECTION_SIZE; if (ret != 0) { - goto end; + return ret; + } + } + return 0; +} + +/************ setting el1 mmu register************** + MAIR_EL1 + index 0 : memory outer writeback, write/read alloc + index 1 : memory nocache + index 2 : device nGnRnE + *****************************************************/ +void mmu_tcr_init(void) +{ + unsigned long val64; + + val64 = 0x00447fUL; + __asm__ volatile("msr MAIR_EL1, %0\n dsb sy\n"::"r"(val64)); + + /* TCR_EL1 */ + val64 = (16UL << 0) /* t0sz 48bit */ + | (0x0UL << 6) /* reserved */ + | (0x0UL << 7) /* epd0 */ + | (0x3UL << 8) /* t0 wb cacheable */ + | (0x3UL << 10) /* inner shareable */ + | (0x2UL << 12) /* t0 outer shareable */ + | (0x0UL << 14) /* t0 4K */ + | (16UL << 16) /* t1sz 48bit */ + | (0x0UL << 22) /* define asid use ttbr0.asid */ + | (0x0UL << 23) /* epd1 */ + | (0x3UL << 24) /* t1 inner wb cacheable */ + | (0x3UL << 26) /* t1 outer wb cacheable */ + | (0x2UL << 28) /* t1 outer shareable */ + | (0x2UL << 30) /* t1 4k */ + | (0x1UL << 32) /* 001b 64GB PA */ + | (0x0UL << 35) /* reserved */ + | (0x1UL << 36) /* as: 0:8bit 1:16bit */ + | (0x0UL << 37) /* tbi0 */ + | (0x0UL << 38); /* tbi1 */ + __asm__ volatile("msr TCR_EL1, %0\n"::"r"(val64)); +} + +/* dump 2nd level page table */ +void rt_hw_cpu_dump_page_table_2nd(rt_uint32_t *ptb) +{ +} + +void rt_hw_cpu_dump_page_table(rt_uint32_t *ptb) +{ +} + +volatile unsigned long MMUTable[512] __attribute__((aligned(4 * 1024))); +void rt_hw_mmu_setmtt(unsigned long vaddrStart, + unsigned long vaddrEnd, + unsigned long paddrStart, + unsigned long attr) +{ + unsigned long count; + + if (vaddrStart & ARCH_SECTION_MASK) + { + while (1); + } + if (paddrStart & ARCH_SECTION_MASK) + { + while (1); + } + if (vaddrStart > vaddrEnd) + { + while (1); + } + count = vaddrEnd + 1; + if (count & ARCH_SECTION_MASK) + { + while (1); + } + count -= vaddrStart; + if (count == 0) + { + while (1); + } + count >>= ARCH_SECTION_SHIFT; + _kernel_map_fixed((unsigned long *)MMUTable, vaddrStart, paddrStart, count, attr); +} + +void rt_hw_mmu_ktbl_set(unsigned long tbl) +{ +#ifdef RT_USING_SMART + tbl += PV_OFFSET; + __asm__ volatile("msr TTBR1_EL1, %0\n dsb sy\nisb"::"r"(tbl):"memory"); +#else + __asm__ volatile("msr TTBR0_EL1, %0\n dsb sy\nisb"::"r"(tbl):"memory"); +#endif + __asm__ volatile("tlbi vmalle1\n dsb sy\nisb":::"memory"); + __asm__ volatile("ic ialluis\n dsb sy\nisb":::"memory"); +} + +void rt_hw_mmu_setup(struct mem_desc *mdesc, int desc_nr) +{ + /* set page table */ + for (; desc_nr > 0; desc_nr--) + { + rt_hw_mmu_setmtt(mdesc->vaddr_start, mdesc->vaddr_end, + mdesc->paddr_start, mdesc->attr); + mdesc++; + } + rt_hw_cpu_dcache_ops(RT_HW_CACHE_FLUSH, (void *)MMUTable, sizeof MMUTable); + rt_hw_mmu_ktbl_set((unsigned long)MMUTable); +} + +/** + * This function will initialize rt_mmu_info structure. + * + * @param mmu_info rt_mmu_info structure + * @param v_address virtual address + * @param size map size + * @param vtable mmu table + * @param pv_off pv offset in kernel space + * + * @return 0 on successful and -1 for fail + */ +int rt_hw_mmu_map_init(rt_mmu_info *mmu_info, void *v_address, size_t size, size_t *vtable, size_t pv_off) +{ + size_t va_s, va_e; + + if (!mmu_info || !vtable) + { + return -1; + } + + va_s = (size_t)v_address; + va_e = (size_t)v_address + size - 1; + + if (va_e < va_s) + { + return -1; + } + + va_s >>= ARCH_SECTION_SHIFT; + va_e >>= ARCH_SECTION_SHIFT; + + if (va_s == 0) + { + return -1; + } + + mmu_info->vtable = vtable; + mmu_info->vstart = va_s; + mmu_info->vend = va_e; + mmu_info->pv_off = pv_off; + + return 0; +} + +int rt_hw_mmu_ioremap_init(rt_mmu_info *mmu_info, void *v_address, size_t size) +{ + return 0; +} + +static size_t find_vaddr(rt_mmu_info *mmu_info, int pages) +{ + size_t loop_pages; + size_t va; + size_t find_va = 0; + int n = 0; + size_t i; + + if (!pages) + { + return 0; + } + + if (!mmu_info) + { + return 0; + } + + loop_pages = mmu_info->vend - mmu_info->vstart + 1; + loop_pages <<= (ARCH_SECTION_SHIFT - ARCH_PAGE_SHIFT); + va = mmu_info->vstart; + va <<= ARCH_SECTION_SHIFT; + for (i = 0; i < loop_pages; i++, va += ARCH_PAGE_SIZE) + { + if (_rt_hw_mmu_v2p(mmu_info, (void *)va)) + { + n = 0; + find_va = 0; + continue; + } + if (!find_va) + { + find_va = va; + } + n++; + if (n >= pages) + { + return find_va; } } + return 0; +} -end: +#ifdef RT_USING_SMART +static int check_vaddr(rt_mmu_info *mmu_info, void *va, int pages) +{ + size_t loop_va; + + if (!pages) + { + return -1; + } + + if (!mmu_info) + { + return -1; + } + + loop_va = ((size_t)va >> ARCH_SECTION_SHIFT); + if (loop_va < mmu_info->vstart || loop_va > mmu_info->vend) + { + return -1; + } + loop_va += ((pages << ARCH_PAGE_SHIFT) >> ARCH_SECTION_SHIFT); + if (loop_va < mmu_info->vstart || loop_va > mmu_info->vend + 1) + { + return -1; + } + + loop_va = (size_t)va & ~ARCH_PAGE_MASK; + while (pages--) + { + if (_rt_hw_mmu_v2p(mmu_info, (void *)loop_va)) + { + return -1; + } + loop_va += ARCH_PAGE_SIZE; + } + return 0; +} + +static void __rt_hw_mmu_unmap(rt_mmu_info *mmu_info, void *v_addr, size_t npages) +{ + size_t loop_va = (size_t)v_addr & ~ARCH_PAGE_MASK; + + if (!mmu_info || !mmu_info->vtable) + { + return; + } + + while (npages--) + { + _kenrel_unmap_4K(mmu_info->vtable, (void *)loop_va); + loop_va += ARCH_PAGE_SIZE; + } +} + +static int __rt_hw_mmu_map(rt_mmu_info *mmu_info, void *v_addr, void *p_addr, size_t npages, size_t attr) +{ + int ret = -1; + size_t loop_va = (size_t)v_addr & ~ARCH_PAGE_MASK; + size_t loop_pa = (size_t)p_addr & ~ARCH_PAGE_MASK; + size_t unmap_va = loop_va; + + if (mmu_info) + { + while (npages--) + { + ret = _kenrel_map_4K(mmu_info->vtable, loop_va, loop_pa, attr); + if (ret != 0) + { + /* error, undo map */ + while (unmap_va != loop_va) + { + _kenrel_unmap_4K(mmu_info->vtable, (void *)unmap_va); + unmap_va += ARCH_PAGE_SIZE; + } + break; + } + loop_va += ARCH_PAGE_SIZE; + loop_pa += ARCH_PAGE_SIZE; + } + } return ret; } +#endif + +static void rt_hw_cpu_tlb_invalidate(void) +{ + __asm__ volatile("tlbi vmalle1\n dsb sy\n isb sy\n"); +} + +#ifdef RT_USING_SMART +void *_rt_hw_mmu_map(rt_mmu_info *mmu_info, void *v_addr, void *p_addr, size_t size, size_t attr) +{ + size_t pa_s, pa_e; + size_t vaddr; + int pages; + int ret; + + if (!size) + { + return 0; + } + pa_s = (size_t)p_addr; + pa_e = (size_t)p_addr + size - 1; + pa_s >>= ARCH_PAGE_SHIFT; + pa_e >>= ARCH_PAGE_SHIFT; + pages = pa_e - pa_s + 1; + if (v_addr) + { + vaddr = (size_t)v_addr; + pa_s = (size_t)p_addr; + if ((vaddr & ARCH_PAGE_MASK) != (pa_s & ARCH_PAGE_MASK)) + { + return 0; + } + vaddr &= ~ARCH_PAGE_MASK; + if (check_vaddr(mmu_info, (void *)vaddr, pages) != 0) + { + return 0; + } + } + else + { + vaddr = find_vaddr(mmu_info, pages); + } + if (vaddr) + { + rt_enter_critical(); + ret = __rt_hw_mmu_map(mmu_info, (void *)vaddr, p_addr, pages, attr); + if (ret == 0) + { + rt_hw_cpu_tlb_invalidate(); + rt_exit_critical(); + return (void *)(vaddr + ((size_t)p_addr & ARCH_PAGE_MASK)); + } + rt_exit_critical(); + } + return 0; +} +#else +void *_rt_hw_mmu_map(rt_mmu_info *mmu_info, void* p_addr, size_t size, size_t attr) +{ + return p_addr; +} +#endif + +#ifdef RT_USING_SMART +static int __rt_hw_mmu_map_auto(rt_mmu_info *mmu_info, void *v_addr, size_t npages, size_t attr) +{ + size_t loop_va = (size_t)v_addr & ~ARCH_PAGE_MASK; + size_t loop_pa; + + if (!mmu_info) + { + return -1; + } + + while (npages--) + { + loop_pa = (size_t)rt_pages_alloc(0); + if (!loop_pa) + { + goto err; + } + loop_pa += mmu_info->pv_off; + _kenrel_map_4K(mmu_info->vtable, loop_va, loop_pa, attr); + loop_va += ARCH_PAGE_SIZE; + } + return 0; +err: + { + /* error, unmap and quit */ + int i; + void *va, *pa; + + va = (void *)((size_t)v_addr & ~ARCH_PAGE_MASK); + for (i = 0; i < npages; i++) + { + pa = rt_hw_mmu_v2p(mmu_info, va); + pa = (void *)((char *)pa - mmu_info->pv_off); + rt_pages_free(pa, 0); + va = (void *)((char *)va + ARCH_PAGE_SIZE); + } + + __rt_hw_mmu_unmap(mmu_info, v_addr, npages); + return -1; + } +} + +void *_rt_hw_mmu_map_auto(rt_mmu_info *mmu_info, void *v_addr, size_t size, size_t attr) +{ + size_t vaddr; + size_t offset; + int pages; + int ret; + + if (!size) + { + return 0; + } + offset = (size_t)v_addr & ARCH_PAGE_MASK; + size += (offset + ARCH_PAGE_SIZE - 1); + pages = (size >> ARCH_PAGE_SHIFT); + if (v_addr) + { + vaddr = (size_t)v_addr; + vaddr &= ~ARCH_PAGE_MASK; + if (check_vaddr(mmu_info, (void *)vaddr, pages) != 0) + { + return 0; + } + } + else + { + vaddr = find_vaddr(mmu_info, pages); + } + if (vaddr) + { + rt_enter_critical(); + ret = __rt_hw_mmu_map_auto(mmu_info, (void *)vaddr, pages, attr); + if (ret == 0) + { + rt_hw_cpu_tlb_invalidate(); + rt_exit_critical(); + return (void *)((char *)vaddr + offset); + } + rt_exit_critical(); + } + return 0; +} + +void _rt_hw_mmu_unmap(rt_mmu_info *mmu_info, void *v_addr, size_t size) +{ + size_t va_s, va_e; + int pages; + + va_s = (size_t)v_addr; + va_e = (size_t)v_addr + size - 1; + va_s >>= ARCH_PAGE_SHIFT; + va_e >>= ARCH_PAGE_SHIFT; + pages = va_e - va_s + 1; + rt_enter_critical(); + __rt_hw_mmu_unmap(mmu_info, v_addr, pages); + rt_hw_cpu_tlb_invalidate(); + rt_exit_critical(); +} + +void *rt_hw_mmu_map(rt_mmu_info *mmu_info, void *v_addr, void *p_addr, size_t size, size_t attr) +{ + void *ret; + + rt_mm_lock(); + ret = _rt_hw_mmu_map(mmu_info, v_addr, p_addr, size, attr); + rt_mm_unlock(); + return ret; +} + +void *rt_hw_mmu_map_auto(rt_mmu_info *mmu_info, void *v_addr, size_t size, size_t attr) +{ + void *ret; + + rt_mm_lock(); + ret = _rt_hw_mmu_map_auto(mmu_info, v_addr, size, attr); + rt_mm_unlock(); + return ret; +} + +void rt_hw_mmu_unmap(rt_mmu_info *mmu_info, void *v_addr, size_t size) +{ + rt_mm_lock(); + _rt_hw_mmu_unmap(mmu_info, v_addr, size); + rt_mm_unlock(); +} +#else + +#include <cache.h> void rt_hw_init_mmu_table(struct mem_desc *mdesc, rt_size_t desc_nr) { @@ -176,19 +895,9 @@ void rt_hw_init_mmu_table(struct mem_desc *mdesc, rt_size_t desc_nr) ++mdesc; } - rt_hw_dcache_flush_range((unsigned long)MMUTable, sizeof(MMUTable)); + rt_hw_cpu_dcache_clean((void *)MMUTable, sizeof(MMUTable)); } -void rt_hw_mmu_tlb_invalidate(void) -{ - __asm__ volatile ( - "tlbi vmalle1\n\r" - "dsb sy\n\r" - "isb sy\n\r" - "ic ialluis\n\r" - "dsb sy\n\r" - "isb sy"); -} void rt_hw_mmu_init(void) { @@ -234,18 +943,89 @@ void rt_hw_mmu_init(void) "isb sy\n\r" ::"r"(MMUTable), "r"(reg_val) :"memory"); - rt_hw_mmu_tlb_invalidate(); + rt_hw_cpu_tlb_invalidate(); } +#endif -int rt_hw_mmu_map(unsigned long addr, unsigned long size, unsigned long attr) +void *_rt_hw_mmu_v2p(rt_mmu_info *mmu_info, void *v_addr) { - int ret; - rt_base_t level; + int level; + unsigned long va = (unsigned long)v_addr; + unsigned long pa; + unsigned long *cur_lv_tbl; + unsigned long page; + unsigned long off; + unsigned long off_addr; + int level_shift = MMU_ADDRESS_BITS; - level = rt_hw_interrupt_disable(); - ret = rt_hw_mmu_setmtt(addr, addr + size, addr, attr); + if (!mmu_info) + { + return (void *)0; + } + cur_lv_tbl = mmu_info->vtable; + for (level = 0; level < MMU_TBL_PAGE_4k_LEVEL; level++) + { + off = (va >> level_shift); + off &= MMU_LEVEL_MASK; + if (!(cur_lv_tbl[off] & MMU_TYPE_USED)) + { + return (void *)0; + } + page = cur_lv_tbl[off]; + if ((page & MMU_TYPE_MASK) == MMU_TYPE_BLOCK) + { + off_addr = va & ((1UL << level_shift) - 1); + pa = (page & MMU_ADDRESS_MASK); + pa += off_addr; + return (void *)pa; + } + cur_lv_tbl = (unsigned long *)(page & MMU_ADDRESS_MASK); + cur_lv_tbl = (unsigned long *)((unsigned long)cur_lv_tbl - PV_OFFSET); + level_shift -= MMU_LEVEL_SHIFT; + } + /* now is level MMU_TBL_PAGE_4k_LEVEL */ + off = (va >> ARCH_PAGE_SHIFT); + off &= MMU_LEVEL_MASK; + page = cur_lv_tbl[off]; + if (!(page & MMU_TYPE_USED)) + { + return (void *)0; + } + pa = (page & MMU_ADDRESS_MASK); + pa += (va & ARCH_PAGE_MASK); + return (void *)pa; +} - rt_hw_interrupt_enable(level); +void *rt_hw_mmu_v2p(rt_mmu_info *mmu_info, void *v_addr) +{ + void *ret; + rt_mm_lock(); + ret = _rt_hw_mmu_v2p(mmu_info, v_addr); + rt_mm_unlock(); return ret; } + + +void rt_hw_mmu_setup_early(unsigned long *tbl0, unsigned long *tbl1, unsigned long size, unsigned long pv_off) +{ + int ret; + unsigned long va = KERNEL_VADDR_START; + unsigned long count = (size + ARCH_SECTION_MASK) >> ARCH_SECTION_SHIFT; + unsigned long normal_attr = MMU_MAP_CUSTOM(MMU_AP_KAUN, NORMAL_MEM); + + /* clean the first two pages */ + mmu_memset((char *)tbl0, 0, ARCH_PAGE_SIZE); + mmu_memset((char *)tbl1, 0, ARCH_PAGE_SIZE); + + ret = armv8_init_map_2M(tbl1, va, va + pv_off, count, normal_attr); + if (ret != 0) + { + while (1); + } + ret = armv8_init_map_2M(tbl0, va + pv_off, va + pv_off, count, normal_attr); + if (ret != 0) + { + while (1); + } +} diff --git a/libcpu/aarch64/common/mmu.h b/libcpu/aarch64/common/mmu.h index c2837a7dce..0e61eeac70 100644 --- a/libcpu/aarch64/common/mmu.h +++ b/libcpu/aarch64/common/mmu.h @@ -1,29 +1,23 @@ /* - * Copyright (c) 2006-2021, RT-Thread Development Team + * Copyright (c) 2006-2018, RT-Thread Development Team * * SPDX-License-Identifier: Apache-2.0 * * Change Logs: * Date Author Notes - * 2021-11-28 GuEe-GUI the first version + * 2021-05-12 RT-Thread the first version */ - #ifndef __MMU_H_ #define __MMU_H_ #include <rtthread.h> /* normal memory wra mapping type */ -#define NORMAL_MEM 0 +#define NORMAL_MEM 0 /* normal nocache memory mapping type */ -#define NORMAL_NOCACHE_MEM 1 +#define NORMAL_NOCACHE_MEM 1 /* device mapping type */ -#define DEVICE_MEM 2 - -#define MMU_MAP_ERROR_VANOTALIGN (-1) -#define MMU_MAP_ERROR_PANOTALIGN (-2) -#define MMU_MAP_ERROR_NOPAGE (-3) -#define MMU_MAP_ERROR_CONFLICT (-4) +#define DEVICE_MEM 2 struct mem_desc { @@ -33,42 +27,119 @@ struct mem_desc unsigned long attr; }; -#define MMU_AF_SHIFT 10 -#define MMU_SHARED_SHIFT 8 -#define MMU_AP_SHIFT 6 -#define MMU_MA_SHIFT 2 +#define MMU_AF_SHIFT 10 +#define MMU_SHARED_SHIFT 8 +#define MMU_AP_SHIFT 6 +#define MMU_MA_SHIFT 2 + +#define MMU_AP_KAUN 0UL /* kernel r/w, user none */ +#define MMU_AP_KAUA 1UL /* kernel r/w, user r/w */ +#define MMU_AP_KRUN 2UL /* kernel r, user none */ +#define MMU_AP_KRUR 3UL /* kernel r, user r */ + +#define MMU_MAP_K_RO (\ + (0x1UL << MMU_AF_SHIFT) |\ + (0x2UL << MMU_SHARED_SHIFT) |\ + (MMU_AP_KRUN << MMU_AP_SHIFT) |\ + (NORMAL_MEM << MMU_MA_SHIFT)\ + ) +#define MMU_MAP_K_RWCB (\ + (0x1UL << MMU_AF_SHIFT) |\ + (0x2UL << MMU_SHARED_SHIFT) |\ + (MMU_AP_KAUN << MMU_AP_SHIFT) |\ + (NORMAL_MEM << MMU_MA_SHIFT)\ + ) +#define MMU_MAP_K_RW (\ + (0x1UL << MMU_AF_SHIFT) |\ + (0x2UL << MMU_SHARED_SHIFT) |\ + (MMU_AP_KAUN << MMU_AP_SHIFT) |\ + (NORMAL_NOCACHE_MEM << MMU_MA_SHIFT)\ + ) +#define MMU_MAP_K_DEVICE (\ + (0x1UL << MMU_AF_SHIFT) |\ + (0x2UL << MMU_SHARED_SHIFT) |\ + (MMU_AP_KAUN << MMU_AP_SHIFT) |\ + (DEVICE_MEM << MMU_MA_SHIFT)\ + ) +#define MMU_MAP_U_RO (\ + (0x1UL << MMU_AF_SHIFT) |\ + (0x2UL << MMU_SHARED_SHIFT) |\ + (MMU_AP_KRUR << MMU_AP_SHIFT) |\ + (NORMAL_NOCACHE_MEM << MMU_MA_SHIFT)\ + ) +#define MMU_MAP_U_RWCB (\ + (0x1UL << MMU_AF_SHIFT) |\ + (0x2UL << MMU_SHARED_SHIFT) |\ + (MMU_AP_KAUA << MMU_AP_SHIFT) |\ + (NORMAL_MEM << MMU_MA_SHIFT)\ + ) +#define MMU_MAP_U_RW (\ + (0x1UL << MMU_AF_SHIFT) |\ + (0x2UL << MMU_SHARED_SHIFT) |\ + (MMU_AP_KAUA << MMU_AP_SHIFT) |\ + (NORMAL_NOCACHE_MEM << MMU_MA_SHIFT)\ + ) +#define MMU_MAP_U_DEVICE (\ + (0x1UL << MMU_AF_SHIFT) |\ + (0x2UL << MMU_SHARED_SHIFT) |\ + (MMU_AP_KAUA << MMU_AP_SHIFT) |\ + (DEVICE_MEM << MMU_MA_SHIFT)\ + ) +#define MMU_MAP_CUSTOM(ap, mtype) (\ + (0x1UL << MMU_AF_SHIFT) |\ + (0x2UL << MMU_SHARED_SHIFT) |\ + ((ap) << MMU_AP_SHIFT) |\ + ((mtype) << MMU_MA_SHIFT)\ + ) -#define MMU_AP_KAUN 0UL /* kernel r/w, user none */ -#define MMU_AP_KAUA 1UL /* kernel r/w, user r/w */ -#define MMU_AP_KRUN 2UL /* kernel r, user none */ -#define MMU_AP_KRUR 3UL /* kernel r, user r */ +#define ARCH_SECTION_SHIFT 21 +#define ARCH_SECTION_SIZE (1 << ARCH_SECTION_SHIFT) +#define ARCH_SECTION_MASK (ARCH_SECTION_SIZE - 1) +#define ARCH_PAGE_SHIFT 12 +#define ARCH_PAGE_SIZE (1 << ARCH_PAGE_SHIFT) +#define ARCH_PAGE_MASK (ARCH_PAGE_SIZE - 1) +#define ARCH_PAGE_TBL_SHIFT 12 +#define ARCH_PAGE_TBL_SIZE (1 << ARCH_PAGE_TBL_SHIFT) +#define ARCH_PAGE_TBL_MASK (ARCH_PAGE_TBL_SIZE - 1) -#define MMU_MAP_CUSTOM(ap, mtype) \ -(\ - (0x1UL << MMU_AF_SHIFT) |\ - (0x2UL << MMU_SHARED_SHIFT) |\ - ((ap) << MMU_AP_SHIFT) |\ - ((mtype) << MMU_MA_SHIFT)\ -) -#define MMU_MAP_K_RO MMU_MAP_CUSTOM(MMU_AP_KRUN, NORMAL_MEM) -#define MMU_MAP_K_RWCB MMU_MAP_CUSTOM(MMU_AP_KAUN, NORMAL_MEM) -#define MMU_MAP_K_RW MMU_MAP_CUSTOM(MMU_AP_KAUN, NORMAL_NOCACHE_MEM) -#define MMU_MAP_K_DEVICE MMU_MAP_CUSTOM(MMU_AP_KAUN, DEVICE_MEM) -#define MMU_MAP_U_RO MMU_MAP_CUSTOM(MMU_AP_KRUR, NORMAL_NOCACHE_MEM) -#define MMU_MAP_U_RWCB MMU_MAP_CUSTOM(MMU_AP_KAUA, NORMAL_MEM) -#define MMU_MAP_U_RW MMU_MAP_CUSTOM(MMU_AP_KAUA, NORMAL_NOCACHE_MEM) -#define MMU_MAP_U_DEVICE MMU_MAP_CUSTOM(MMU_AP_KAUA, DEVICE_MEM) +#define ARCH_ADDRESS_WIDTH_BITS 64 + +#define MMU_MAP_ERROR_VANOTALIGN -1 +#define MMU_MAP_ERROR_PANOTALIGN -2 +#define MMU_MAP_ERROR_NOPAGE -3 +#define MMU_MAP_ERROR_CONFLICT -4 + +typedef struct +{ + size_t *vtable; + size_t vstart; + size_t vend; + size_t pv_off; +} rt_mmu_info; + +void rt_hw_mmu_setup_early(unsigned long *tbl0, unsigned long *tbl1, unsigned long size, unsigned long pv_off); +void rt_hw_mmu_setup(struct mem_desc *mdesc, int desc_nr); + +int rt_hw_mmu_map_init(rt_mmu_info *mmu_info, void* v_address, size_t size, size_t *vtable, size_t pv_off); +int rt_hw_mmu_ioremap_init(rt_mmu_info *mmu_info, void* v_address, size_t size); + +#ifdef RT_USING_SMART +void *rt_hw_mmu_map(rt_mmu_info *mmu_info, void *v_addr, void* p_addr, size_t size, size_t attr); +void *rt_hw_mmu_map_auto(rt_mmu_info *mmu_info, void *v_addr, size_t size, size_t attr); +#else +void *rt_hw_mmu_map(rt_mmu_info *mmu_info, void* p_addr, size_t size, size_t attr); +#endif + +void rt_hw_mmu_unmap(rt_mmu_info *mmu_info, void* v_addr, size_t size); +void *rt_hw_mmu_v2p(rt_mmu_info *mmu_info, void* v_addr); + +void rt_hw_mmu_ktbl_set(unsigned long tbl); +void *rt_hw_mmu_tbl_get(); +void rt_hw_mmu_switch(void *mmu_table); void rt_hw_init_mmu_table(struct mem_desc *mdesc, rt_size_t desc_nr); void rt_hw_mmu_init(void); -int rt_hw_mmu_map(unsigned long addr, unsigned long size, unsigned long attr); - -void rt_hw_dcache_flush_all(void); -void rt_hw_dcache_invalidate_all(void); -void rt_hw_dcache_flush_range(unsigned long start_addr, unsigned long size); -void rt_hw_dcache_invalidate_range(unsigned long start_addr,unsigned long size); -void rt_hw_icache_invalidate_all(); -void rt_hw_icache_invalidate_range(unsigned long start_addr, int size); +extern rt_mmu_info mmu_info; -#endif /* __MMU_H_ */ +#endif diff --git a/libcpu/aarch64/common/psci.c b/libcpu/aarch64/common/psci.c index 65e84ebf0a..e18a51a766 100644 --- a/libcpu/aarch64/common/psci.c +++ b/libcpu/aarch64/common/psci.c @@ -1,103 +1,288 @@ /* - * Copyright (c) 2006-2021, RT-Thread Development Team + * Copyright (c) 2006-2022, RT-Thread Development Team * * SPDX-License-Identifier: Apache-2.0 * * Change Logs: * Date Author Notes - * 2021-09-09 GuEe-GUI The first version */ +#include <rthw.h> +#include <rtthread.h> +#include <stdint.h> +#include <stdio.h> +#include <string.h> +#include "cpu.h" +#include "psci.h" +#include "psci_api.h" +#include "smccc.h" -#include <psci.h> -#include <smccc.h> -#include <armv8.h> +#define DBG_TAG "libcpu.aarch64.psci" +#define DBG_LVL DBG_INFO +#include <rtdbg.h> -typedef uint64_t (*psci_call_handle)(uint32_t fn, uint64_t arg0, uint64_t arg1, uint64_t arg2); +/** template for creating 4 PSCI ops: SUSPEND, OFF, ON, MIGRATE */ +#define COMMON_PSCI_OPS_TEMPLATE(VER, SUSPEND, OFF, ON, MIGRATE) \ + static int psci_##VER##_cpu_suspend(uint32_t state, unsigned long entry_point) \ + { \ + return psci_call((SUSPEND), state, entry_point, 0); \ + } \ + static int psci_##VER##_cpu_off(uint32_t state) \ + { \ + return psci_call((OFF), state, 0, 0); \ + } \ + static int psci_##VER##_cpu_on(unsigned long cpuid, unsigned long entry_point) \ + { \ + return psci_call((ON), cpuid, entry_point, 0); \ + } \ + static int psci_##VER##_migrate(unsigned long cpuid) \ + { \ + return psci_call((MIGRATE), cpuid, 0, 0); \ + } -static uint64_t psci_smc_call(uint32_t fn, uint64_t arg0, uint64_t arg1, uint64_t arg2) -{ - return arm_smc_call(fn, arg0, arg1, arg2, 0, 0, 0, 0).x0; -} +#ifdef RT_USING_FDT +#include "dtb_node.h" + +struct psci_ops_t psci_ops; + +#if __SIZE_WIDTH__ == 64 +#define PSCI_FN_NATIVE(version, name) PSCI_##version##_FN64_##name +#else +#define PSCI_FN_NATIVE(version, name) PSCI_##version##_FN_##name +#endif + +/** + * SMCCC can use either smc or hvc method + * smccc_call will be init to proper interface when psci_init() was executed + */ +static void (*smccc_call)(unsigned long a0, unsigned long a1, unsigned long a2, + unsigned long a3, unsigned long a4, unsigned long a5, + unsigned long a6, unsigned long a7, struct arm_smccc_res_t *res, + struct arm_smccc_quirk_t *quirk); -static uint64_t psci_hvc_call(uint32_t fn, uint64_t arg0, uint64_t arg1, uint64_t arg2) +static rt_uint32_t psci_call(unsigned long a0, unsigned long a1, unsigned long a2, unsigned long a3) { - return arm_hvc_call(fn, arg0, arg1, arg2, 0, 0, 0, 0).x0; + struct arm_smccc_res_t res; + smccc_call(a0, a1, a2, a3, 0, 0, 0, 0, &res, (void *)0); + return res.a0; } -static psci_call_handle psci_call = psci_smc_call; +static int _psci_probe_version(char *version, int *major, int *minor); +static int _psci_init_with_version(int major, int minor); -static uint64_t shutdown_args[3] = {0, 0, 0}; -static uint64_t reboot_args[3] = {0, 0, 0}; +static struct dtb_node *psci_node; -void arm_psci_init(uint64_t method, uint64_t *platform_shutdown_args, uint64_t *platform_reboot_args) +static int psci_ver_major; +static int psci_ver_minor; + +/** + * @brief init psci operations. + * using device tree to probe version and psci-method, + * setup psci ops for future use + * + * @return int 0 on success + */ +int psci_init() { - switch (method) + void *root = get_dtb_node_head(); + psci_node = dtb_node_get_dtb_node_by_path(root, "/psci"); + if (!psci_node) { - case PSCI_METHOD_SMC: - psci_call = psci_smc_call; - break; - case PSCI_METHOD_HVC: - if (rt_hw_get_current_el() < 2) - { - psci_call = psci_hvc_call; - } - break; + LOG_E("No PSCI node found"); + return -1; } + char *compatible = dtb_node_get_dtb_node_property_value(psci_node, "compatible", NULL); + char *method = dtb_node_get_dtb_node_property_value(psci_node, "method", NULL); + + int retval = 0; - if (platform_shutdown_args != RT_NULL) + // setup psci-method + if (!strcmp("hvc", method)) { - shutdown_args[0] = platform_shutdown_args[0]; - shutdown_args[1] = platform_shutdown_args[1]; - shutdown_args[2] = platform_shutdown_args[2]; + smccc_call = arm_smccc_hvc; } - - if (platform_reboot_args != RT_NULL) + else if (!strcmp("smc", method)) { - reboot_args[0] = platform_reboot_args[0]; - reboot_args[1] = platform_reboot_args[1]; - reboot_args[2] = platform_reboot_args[2]; + smccc_call = arm_smccc_smc; } + else + { + LOG_E("Unknown PSCI method: %s", method); + return -1; + } + LOG_D("Using psci method %s", method); + + retval = _psci_probe_version(compatible, &psci_ver_major, &psci_ver_minor); + if (retval != 0) + return retval; + + // init psci_ops with specified psci version + retval = _psci_init_with_version(psci_ver_major, psci_ver_minor); + + return retval; } -uint32_t arm_psci_get_version() +/* function id of PSCI v0.1 should be probed in FDT, they are implementation defined value */ +static rt_uint32_t cpu_suspend_0_1; +static rt_uint32_t cpu_off_0_1; +static rt_uint32_t cpu_on_0_1; +static rt_uint32_t migrate_0_1; + +/* basic operations TEMPLATE for API since 0.1 version */ +COMMON_PSCI_OPS_TEMPLATE(0_1, cpu_suspend_0_1, cpu_off_0_1, cpu_on_0_1, migrate_0_1); + +/* used for v0.1 only, rely on FDT to probe function id */ +#define PROBE_AND_SET(FUNC_NAME) \ + do \ + { \ + int num_of_elem; \ + funcid = \ + dtb_node_get_dtb_node_property_value(psci_node, #FUNC_NAME, &num_of_elem); \ + if (num_of_elem != 4 || funcid == 0 || *funcid == 0) \ + { \ + LOG_E("Failed to probe " #FUNC_NAME " in FDT"); \ + } \ + else \ + { \ + FUNC_NAME##_0_1 = (rt_uint32_t)fdt32_to_cpu(*funcid); \ + psci_ops.FUNC_NAME = psci_0_1_##FUNC_NAME; \ + } \ + } while (0) + +static int psci_0_1_init() +{ + // reading function id from fdt + rt_uint32_t *funcid; + PROBE_AND_SET(cpu_suspend); + PROBE_AND_SET(cpu_off); + PROBE_AND_SET(cpu_on); + PROBE_AND_SET(migrate); + return 0; +} + +COMMON_PSCI_OPS_TEMPLATE(0_2, PSCI_FN_NATIVE(0_2, CPU_SUSPEND), PSCI_0_2_FN_CPU_OFF, PSCI_FN_NATIVE(0_2, CPU_ON), PSCI_FN_NATIVE(0_2, MIGRATE)); + +static rt_uint32_t psci_0_2_get_version(void) { - return (uint32_t)psci_call(PSCI_0_2_FN_PSCI_VERSION, 0, 0, 0); + return psci_call(PSCI_0_2_FN_PSCI_VERSION, 0, 0, 0); } -uint32_t arm_psci_get_affinity_info(uint64_t target_affinity, uint64_t lowest_affinity_level) +static void psci_0_2_set_basic_ops() { - return (uint32_t)psci_call(PSCI_0_2_FN_AFFINITY_INFO, target_affinity, lowest_affinity_level, 0); + psci_ops = (struct psci_ops_t){ + .get_version = psci_0_2_get_version, + + // followings API are v0.1 compatible + .cpu_suspend = psci_0_2_cpu_suspend, + .cpu_off = psci_0_2_cpu_off, + .cpu_on = psci_0_2_cpu_on, + .migrate = psci_0_2_migrate, + }; } -uint32_t arm_psci_get_feature(uint32_t psci_func_id) +static void psci_0_2_system_off(void) { - return (uint32_t)psci_call(PSCI_1_0_FN_PSCI_FEATURES, psci_func_id, 0, 0); + psci_call(PSCI_0_2_FN_SYSTEM_OFF, 0, 0, 0); } -uint32_t arm_psci_cpu_off(uint64_t state) +static void psci_0_2_system_reset(void) { - return (uint32_t)psci_call(PSCI_0_2_FN_CPU_OFF, state, 0, 0); + psci_call(PSCI_0_2_FN_SYSTEM_RESET, 0, 0, 0); } -uint32_t arm_psci_cpu_on(uint64_t mpid, uint64_t entry) +static int psci_0_2_init() { - /* [40:63] and [24:31] must be zero, other is aff3, aff2, aff1, aff0 */ - mpid = mpid & 0xff00ffffff; + psci_0_2_set_basic_ops(); - return (uint32_t)psci_call(PSCI_0_2_FN_CPU_ON, mpid, entry, 0); + // TODO init other version 0.2 features... + // psci system off and reset which controlling machine + psci_ops.system_off = psci_0_2_system_off; + psci_ops.system_reset = psci_0_2_system_reset; + + system_off = psci_0_2_system_off; + return 0; } -uint32_t arm_psci_cpu_suspend(uint32_t power_state, uint64_t entry) +/* PSCI v1.0 & after */ +static int psci_1_0_features(uint32_t psci_func_id) { - return (uint32_t)psci_call(PSCI_0_2_FN_CPU_SUSPEND, power_state, entry, 0); + return psci_call(PSCI_1_0_FN_PSCI_FEATURES, + psci_func_id, 0, 0); } -void arm_psci_system_off() +static int psci_1_0_init() { - psci_call(PSCI_0_2_FN_SYSTEM_OFF, shutdown_args[0], shutdown_args[1], shutdown_args[2]); + psci_0_2_init(); + + // TODO init other version 1.0 features... + // remove unsupported features + if (psci_1_0_features(PSCI_0_2_FN_SYSTEM_OFF) == PSCI_RET_NOT_SUPPORTED) + { + psci_ops.system_off = RT_NULL; + system_off = RT_NULL; + } + else + LOG_D("Using SYSTEM OFF feature"); + if (psci_1_0_features(PSCI_0_2_FN_SYSTEM_RESET) == PSCI_RET_NOT_SUPPORTED) + psci_ops.system_reset = RT_NULL; + else + LOG_D("Using SYSTEM RESET feature"); + + return 0; +} + +/* probe psci version from fdt or SMC call */ +static int _psci_probe_version(char *version, int *major, int *minor) +{ + int retval = 0; + // if strcmp compatible 'arm,psci-0.1' + if (!strcmp(version, "arm,psci")) + { + *major = 0; + *minor = 1; + } + else if (!strncmp(version, "arm,psci-", 8)) + { + // since psci-0.2, using psci call to probe version + rt_uint32_t ret = psci_0_2_get_version(); + *major = PSCI_VERSION_MAJOR(ret); + *minor = PSCI_VERSION_MINOR(ret); + } + else + { + LOG_E("[%s] was not a proper PSCI version", version); + retval = -1; + } + LOG_D("Using PSCI v%d.%d", *major, *minor); + return retval; } -void arm_psci_system_reboot() +/* init psci ops with version info */ +static int _psci_init_with_version(int major, int minor) { - psci_call(PSCI_0_2_FN_SYSTEM_RESET, reboot_args[0], reboot_args[1], reboot_args[2]); + int retval = -0xbeef; // mark unsupported + if (major == 0) + { + // for v0.1, psci function id was provided fdt + if (minor == 1) + { + retval = psci_0_1_init(); + } + else if (minor == 2) + { + retval = psci_0_2_init(); + } + } + else if (major == 1) + { + // psci_1_0_init is a base setup for version after v1.0 + retval = psci_1_0_init(); + } + + if (retval == -0xbeef) + { + LOG_E("PSCI init with incompatible version %d.%d", major, minor); + } + return retval; } + +#endif /* RT_USING_FDT */
\ No newline at end of file diff --git a/libcpu/aarch64/common/psci.h b/libcpu/aarch64/common/psci.h index 999489a801..3cce66211f 100644 --- a/libcpu/aarch64/common/psci.h +++ b/libcpu/aarch64/common/psci.h @@ -1,34 +1,25 @@ /* - * Copyright (c) 2006-2021, RT-Thread Development Team + * Copyright (c) 2006-2022, RT-Thread Development Team * * SPDX-License-Identifier: Apache-2.0 * * Change Logs: * Date Author Notes - * 2021-09-09 GuEe-GUI The first version */ - #ifndef __PSCI_H__ #define __PSCI_H__ -#include <stdint.h> - -/* - * Non-Confidential PSCI 1.0 release (30 January 2015), and errata fix for PSCI 0.2, unsupport PSCI 0.1 +/** + * PSCI protocol content + * For PSCI v0.1, only return values below are protocol defined */ -#define PSCI_VER_0_2 0x00000002 -#define PSCI_METHOD_SMC 3 -#define PSCI_METHOD_HVC 2 - -/* PSCI 0.2 interface */ +/* PSCI v0.2 interface */ #define PSCI_0_2_FN_BASE 0x84000000 #define PSCI_0_2_FN(n) (PSCI_0_2_FN_BASE + (n)) -#define PSCI_0_2_FN_END 0x8400001F - -#define PSCI_0_2_FN64_BASE 0xC4000000 +#define PSCI_0_2_64BIT 0x40000000 +#define PSCI_0_2_FN64_BASE (PSCI_0_2_FN_BASE + PSCI_0_2_64BIT) #define PSCI_0_2_FN64(n) (PSCI_0_2_FN64_BASE + (n)) -#define PSCI_0_2_FN64_END 0xC400001F #define PSCI_0_2_FN_PSCI_VERSION PSCI_0_2_FN(0) #define PSCI_0_2_FN_CPU_SUSPEND PSCI_0_2_FN(1) @@ -44,90 +35,69 @@ #define PSCI_0_2_FN64_CPU_SUSPEND PSCI_0_2_FN64(1) #define PSCI_0_2_FN64_CPU_ON PSCI_0_2_FN64(3) #define PSCI_0_2_FN64_AFFINITY_INFO PSCI_0_2_FN64(4) -#define PSCI_0_2_FN64_MIGRATE PSCI_0_2_FN64(5) +#define PSCI_0_2_FN64_MIGRATE (5) #define PSCI_0_2_FN64_MIGRATE_INFO_UP_CPU PSCI_0_2_FN64(7) -/* PSCI 1.0 interface */ #define PSCI_1_0_FN_PSCI_FEATURES PSCI_0_2_FN(10) -#define PSCI_1_0_FN_CPU_FREEZE PSCI_0_2_FN(11) -#define PSCI_1_0_FN_CPU_DEFAULT_SUSPEND PSCI_0_2_FN(12) -#define PSCI_1_0_FN_NODE_HW_STATE PSCI_0_2_FN(13) #define PSCI_1_0_FN_SYSTEM_SUSPEND PSCI_0_2_FN(14) #define PSCI_1_0_FN_SET_SUSPEND_MODE PSCI_0_2_FN(15) -#define PSCI_1_0_FN_STAT_RESIDENCY PSCI_0_2_FN(16) -#define PSCI_1_0_FN_STAT_COUNT PSCI_0_2_FN(17) +#define PSCI_1_1_FN_SYSTEM_RESET2 PSCI_0_2_FN(18) -#define PSCI_1_0_FN64_CPU_DEFAULT_SUSPEND PSCI_0_2_FN64(12) -#define PSCI_1_0_FN64_NODE_HW_STATE PSCI_0_2_FN64(13) #define PSCI_1_0_FN64_SYSTEM_SUSPEND PSCI_0_2_FN64(14) -#define PSCI_1_0_FN64_STAT_RESIDENCY PSCI_0_2_FN64(16) -#define PSCI_1_0_FN64_STAT_COUNT PSCI_0_2_FN64(17) +#define PSCI_1_1_FN64_SYSTEM_RESET2 PSCI_0_2_FN64(18) -/* 1KB stack per core */ -#define PSCI_STACK_SHIFT 10 -#define PSCI_STACK_SIZE (1 << PSCI_STACK_SHIFT) +/* PSCI v0.2 power state encoding for CPU_SUSPEND function */ +#define PSCI_0_2_POWER_STATE_ID_MASK 0xffff +#define PSCI_0_2_POWER_STATE_ID_SHIFT 0 +#define PSCI_0_2_POWER_STATE_TYPE_SHIFT 16 +#define PSCI_0_2_POWER_STATE_TYPE_MASK (0x1 << PSCI_0_2_POWER_STATE_TYPE_SHIFT) +#define PSCI_0_2_POWER_STATE_AFFL_SHIFT 24 +#define PSCI_0_2_POWER_STATE_AFFL_MASK (0x3 << PSCI_0_2_POWER_STATE_AFFL_SHIFT) -/* PSCI affinity level state returned by AFFINITY_INFO */ -#define PSCI_AFFINITY_LEVEL_ON 0 -#define PSCI_AFFINITY_LEVEL_OFF 1 -#define PSCI_AFFINITY_LEVEL_ON_PENDING 2 +/* PSCI extended power state encoding for CPU_SUSPEND function */ +#define PSCI_1_0_EXT_POWER_STATE_ID_MASK 0xfffffff +#define PSCI_1_0_EXT_POWER_STATE_ID_SHIFT 0 +#define PSCI_1_0_EXT_POWER_STATE_TYPE_SHIFT 30 +#define PSCI_1_0_EXT_POWER_STATE_TYPE_MASK (0x1 << PSCI_1_0_EXT_POWER_STATE_TYPE_SHIFT) -/* - * PSCI power state - * power_level: - * Level 0: cores - * Level 1: clusters - * Level 2: system - * state_type: - * value 0: standby or retention state - * value 1: powerdown state(entry and context_id is valid) - * state_id: - * StateID - */ -#define PSCI_POWER_STATE(power_level, state_type, state_id) \ -( \ - ((power_level) << 24) | \ - ((state_type) << 16) | \ - ((state_id) << 24) \ -) - -/* - * For system, cluster, core - * 0: run - * 1: standby(only core) - * 2: retention - * 3: powerdown - */ -#define PSCI_POWER_STATE_ID(state_id_power_level, system, cluster, core) \ -( \ - ((state_id_power_level) << 12) | \ - ((system) << 8) | \ - ((cluster) << 4) | \ - (core) \ -) +/* PSCI v0.2 affinity level state returned by AFFINITY_INFO */ +#define PSCI_0_2_AFFINITY_LEVEL_ON 0 +#define PSCI_0_2_AFFINITY_LEVEL_OFF 1 +#define PSCI_0_2_AFFINITY_LEVEL_ON_PENDING 2 -#define PSCI_RET_SUCCESS 0 -#define PSCI_RET_NOT_SUPPORTED (-1) -#define PSCI_RET_INVALID_PARAMETERS (-2) -#define PSCI_RET_DENIED (-3) -#define PSCI_RET_ALREADY_ON (-4) -#define PSCI_RET_ON_PENDING (-5) -#define PSCI_RET_INTERNAL_FAILURE (-6) -#define PSCI_RET_NOT_PRESENT (-7) -#define PSCI_RET_DISABLED (-8) -#define PSCI_RET_INVALID_ADDRESS (-9) +/* PSCI v0.2 multicore support in Trusted OS returned by MIGRATE_INFO_TYPE */ +#define PSCI_0_2_TOS_UP_MIGRATE 0 +#define PSCI_0_2_TOS_UP_NO_MIGRATE 1 +#define PSCI_0_2_TOS_MP 2 -void arm_psci_init(uint64_t method, uint64_t *platform_shutdown_args, uint64_t *platform_reboot_args); +/* PSCI version decoding (independent of PSCI version) */ +#define PSCI_VERSION_MAJOR_SHIFT 16 +#define PSCI_VERSION_MINOR_MASK ((1U << PSCI_VERSION_MAJOR_SHIFT) - 1) +#define PSCI_VERSION_MAJOR_MASK ~PSCI_VERSION_MINOR_MASK +#define PSCI_VERSION_MAJOR(ver) (((ver) & PSCI_VERSION_MAJOR_MASK) >> PSCI_VERSION_MAJOR_SHIFT) +#define PSCI_VERSION_MINOR(ver) ((ver) & PSCI_VERSION_MINOR_MASK) +#define PSCI_VERSION(maj, min) \ + ((((maj) << PSCI_VERSION_MAJOR_SHIFT) & PSCI_VERSION_MAJOR_MASK) | \ + ((min) & PSCI_VERSION_MINOR_MASK)) -uint32_t arm_psci_get_version(); -uint32_t arm_psci_get_affinity_info(uint64_t target_affinity, uint64_t lowest_affinity_level); -uint32_t arm_psci_get_feature(uint32_t psci_func_id); +/* PSCI features decoding (>=1.0) */ +#define PSCI_1_0_FEATURES_CPU_SUSPEND_PF_SHIFT 1 +#define PSCI_1_0_FEATURES_CPU_SUSPEND_PF_MASK (0x1 << PSCI_1_0_FEATURES_CPU_SUSPEND_PF_SHIFT) -uint32_t arm_psci_cpu_off(uint64_t state); -uint32_t arm_psci_cpu_on(uint64_t mpid, uint64_t entry); -uint32_t arm_psci_cpu_suspend(uint32_t power_state, uint64_t entry); +#define PSCI_1_0_OS_INITIATED BIT(0) +#define PSCI_1_0_SUSPEND_MODE_PC 0 +#define PSCI_1_0_SUSPEND_MODE_OSI 1 -void arm_psci_system_off(); -void arm_psci_system_reboot(); +/* PSCI return values (inclusive of all PSCI versions) */ +#define PSCI_RET_SUCCESS 0 +#define PSCI_RET_NOT_SUPPORTED -1 +#define PSCI_RET_INVALID_PARAMS -2 +#define PSCI_RET_DENIED -3 +#define PSCI_RET_ALREADY_ON -4 +#define PSCI_RET_ON_PENDING -5 +#define PSCI_RET_INTERNAL_FAILURE -6 +#define PSCI_RET_NOT_PRESENT -7 +#define PSCI_RET_DISABLED -8 +#define PSCI_RET_INVALID_ADDRESS -9 #endif /*__PSCI_H__*/ diff --git a/libcpu/aarch64/common/psci_api.h b/libcpu/aarch64/common/psci_api.h new file mode 100644 index 0000000000..36566c3e6f --- /dev/null +++ b/libcpu/aarch64/common/psci_api.h @@ -0,0 +1,34 @@ +/* + * Copyright (c) 2006-2022, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + */ +#ifndef __PSCI_API_H__ +#define __PSCI_API_H__ + +#include <rthw.h> +#include <rtthread.h> +#include <stdint.h> +#include "psci_api.h" + +/** generic psci ops supported v0.1 v0.2 v1.0 v1.1 */ +struct psci_ops_t +{ + uint32_t (*get_version)(void); + int32_t (*cpu_suspend)(uint32_t state, unsigned long entry_point); + int32_t (*cpu_off)(uint32_t state); + int32_t (*cpu_on)(unsigned long cpuid, unsigned long entry_point); + int32_t (*migrate)(unsigned long cpuid); + + void (*system_off)(void); + void (*system_reset)(void); +}; + +extern struct psci_ops_t psci_ops; + +extern int psci_init(void); + +#endif // __PSCI_API_H__ diff --git a/libcpu/aarch64/common/smccc.S b/libcpu/aarch64/common/smccc.S index 501d210c16..6428bc988b 100644 --- a/libcpu/aarch64/common/smccc.S +++ b/libcpu/aarch64/common/smccc.S @@ -1,37 +1,32 @@ /* - * Copyright (c) 2006-2021, RT-Thread Development Team + * Copyright (c) 2006-2022, RT-Thread Development Team * * SPDX-License-Identifier: Apache-2.0 * * Change Logs: * Date Author Notes - * 2021-09-09 GuEe-GUI The first version */ - -/* - * smc calling convention call +/** + * SMCCC v0.2 + * ARM DEN0028E chapter 2.6 */ -.macro SMCCC_CALL INS - stp x8, x29, [sp,#-16]! /* push the frame pointer (x29) for the purposes of AAPCS64 compatibility */ - \INS #0 - ldp x8, x29, [sp], #16 - - stp x0, x1, [x8] - stp x2, x3, [x8, #16] - str x6, [x8, #32] + .macro SMCCC instr + stp x29, x30, [sp, #-16]! + mov x29, sp + \instr #0 + // store in arm_smccc_res + ldr x4, [sp, #16] + stp x0, x1, [x4, #0] + stp x2, x3, [x4, #16] +1: + ldp x29, x30, [sp], #16 ret -.endm + .endm -/* - * smc call - */ -.globl arm_smc_call -arm_smc_call: - SMCCC_CALL smc +.global arm_smccc_smc +arm_smccc_smc: + SMCCC smc -/* - * hvc call - */ -.globl arm_hvc_call -arm_hvc_call: - SMCCC_CALL hvc +.global arm_smccc_hvc +arm_smccc_hvc: + SMCCC hvc diff --git a/libcpu/aarch64/common/smccc.h b/libcpu/aarch64/common/smccc.h index 9a84fd1452..3b2283ac50 100644 --- a/libcpu/aarch64/common/smccc.h +++ b/libcpu/aarch64/common/smccc.h @@ -1,33 +1,45 @@ /* - * Copyright (c) 2006-2021, RT-Thread Development Team + * Copyright (c) 2006-2019, RT-Thread Development Team * * SPDX-License-Identifier: Apache-2.0 * * Change Logs: * Date Author Notes - * 2021-09-09 GuEe-GUI The first version */ +#ifndef __SMCCC_H__ +#define __SMCCC_H__ -#include <stdint.h> - -/* - * The ARM SMCCC v1.0 calling convention provides the following guarantees about registers: - * Register Modified Return State - * X0...X3 Yes Result values - * X4...X17 Yes Unpredictable - * X18...X30 No Preserved - * SP_EL0 No Preserved - * SP_ELx No Preserved +/** + * result from SMC/HVC call + * ARM DEN0028E chapter 5, */ +typedef struct arm_smccc_res_t +{ + unsigned long a0; + // reserved for ARM SMC and HVC Fast Call services + unsigned long a1; + unsigned long a2; + unsigned long a3; +} arm_smccc_res_t; -struct arm_smccc_ret +/** + * quirk is a structure contains vendor specified information, + * it just a placeholder currently + */ +struct arm_smccc_quirk_t { - uint64_t x0; /* Parameter registers */ - uint64_t x1; /* Parameter registers */ - uint64_t x2; /* Parameter registers */ - uint64_t x3; /* Parameter registers */ - uint64_t x6; /* Parameter register: Optional Session ID register */ }; -struct arm_smccc_ret arm_smc_call(uint32_t w0, uint64_t x1, uint64_t x2, uint64_t x3, uint64_t x4, uint64_t x5, uint64_t x6, uint32_t w7); -struct arm_smccc_ret arm_hvc_call(uint32_t w0, uint64_t x1, uint64_t x2, uint64_t x3, uint64_t x4, uint64_t x5, uint64_t x6, uint32_t w7); +/* smccc version 0.2 */ + +void arm_smccc_smc(unsigned long a0, unsigned long a1, unsigned long a2, + unsigned long a3, unsigned long a4, unsigned long a5, + unsigned long a6, unsigned long a7, struct arm_smccc_res_t *res, + struct arm_smccc_quirk_t *quirk); + +void arm_smccc_hvc(unsigned long a0, unsigned long a1, unsigned long a2, + unsigned long a3, unsigned long a4, unsigned long a5, + unsigned long a6, unsigned long a7, struct arm_smccc_res_t *res, + struct arm_smccc_quirk_t *quirk); + +#endif /* __SMCCC_H__ */ diff --git a/libcpu/aarch64/common/stack.c b/libcpu/aarch64/common/stack.c index 2743fd3eee..eaa9a4d158 100644 --- a/libcpu/aarch64/common/stack.c +++ b/libcpu/aarch64/common/stack.c @@ -5,14 +5,15 @@ * * Change Logs: * Date Author Notes - * 2011-09-23 Bernard the first version - * 2011-10-05 Bernard add thumb mode - * 2021-11-04 GuEe-GUI set sp with SP_ELx - * 2021-12-28 GuEe-GUI add fpu support + * 2021-05-12 RT-Thread init */ +#include <board.h> #include <rtthread.h> + #include <armv8.h> +#define INITIAL_SPSR_EL1 (PSTATE_EL1 | SP_ELx) + /** * This function will initialize thread stack * @@ -23,87 +24,84 @@ * * @return stack address */ -rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter, rt_uint8_t *stack_addr, void *texit) +rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter, + rt_uint8_t *stack_addr, void *texit) { - static const rt_ubase_t initial_spsr[] = - { - [1] = PSTATE_EL1 | SP_ELx, - [2] = PSTATE_EL2 | SP_ELx, - [3] = PSTATE_EL3 | SP_ELx - }; - /* The AAPCS64 requires 128-bit (16 byte) stack alignment */ - rt_ubase_t *stk = (rt_ubase_t*)RT_ALIGN_DOWN((rt_ubase_t)stack_addr, 16); + rt_ubase_t *stk; + + stk = (rt_ubase_t *)stack_addr; + + *(--stk) = (rt_ubase_t)0; /* Q0 */ + *(--stk) = (rt_ubase_t)0; /* Q0 */ + *(--stk) = (rt_ubase_t)0; /* Q1 */ + *(--stk) = (rt_ubase_t)0; /* Q1 */ + *(--stk) = (rt_ubase_t)0; /* Q2 */ + *(--stk) = (rt_ubase_t)0; /* Q2 */ + *(--stk) = (rt_ubase_t)0; /* Q3 */ + *(--stk) = (rt_ubase_t)0; /* Q3 */ + *(--stk) = (rt_ubase_t)0; /* Q4 */ + *(--stk) = (rt_ubase_t)0; /* Q4 */ + *(--stk) = (rt_ubase_t)0; /* Q5 */ + *(--stk) = (rt_ubase_t)0; /* Q5 */ + *(--stk) = (rt_ubase_t)0; /* Q6 */ + *(--stk) = (rt_ubase_t)0; /* Q6 */ + *(--stk) = (rt_ubase_t)0; /* Q7 */ + *(--stk) = (rt_ubase_t)0; /* Q7 */ + *(--stk) = (rt_ubase_t)0; /* Q8 */ + *(--stk) = (rt_ubase_t)0; /* Q8 */ + *(--stk) = (rt_ubase_t)0; /* Q9 */ + *(--stk) = (rt_ubase_t)0; /* Q9 */ + *(--stk) = (rt_ubase_t)0; /* Q10 */ + *(--stk) = (rt_ubase_t)0; /* Q10 */ + *(--stk) = (rt_ubase_t)0; /* Q11 */ + *(--stk) = (rt_ubase_t)0; /* Q11 */ + *(--stk) = (rt_ubase_t)0; /* Q12 */ + *(--stk) = (rt_ubase_t)0; /* Q12 */ + *(--stk) = (rt_ubase_t)0; /* Q13 */ + *(--stk) = (rt_ubase_t)0; /* Q13 */ + *(--stk) = (rt_ubase_t)0; /* Q14 */ + *(--stk) = (rt_ubase_t)0; /* Q14 */ + *(--stk) = (rt_ubase_t)0; /* Q15 */ + *(--stk) = (rt_ubase_t)0; /* Q15 */ - *(--stk) = (rt_ubase_t) 0; /* Q0 */ - *(--stk) = (rt_ubase_t) 0; /* Q0 */ - *(--stk) = (rt_ubase_t) 0; /* Q1 */ - *(--stk) = (rt_ubase_t) 0; /* Q1 */ - *(--stk) = (rt_ubase_t) 0; /* Q2 */ - *(--stk) = (rt_ubase_t) 0; /* Q2 */ - *(--stk) = (rt_ubase_t) 0; /* Q3 */ - *(--stk) = (rt_ubase_t) 0; /* Q3 */ - *(--stk) = (rt_ubase_t) 0; /* Q4 */ - *(--stk) = (rt_ubase_t) 0; /* Q4 */ - *(--stk) = (rt_ubase_t) 0; /* Q5 */ - *(--stk) = (rt_ubase_t) 0; /* Q5 */ - *(--stk) = (rt_ubase_t) 0; /* Q6 */ - *(--stk) = (rt_ubase_t) 0; /* Q6 */ - *(--stk) = (rt_ubase_t) 0; /* Q7 */ - *(--stk) = (rt_ubase_t) 0; /* Q7 */ - *(--stk) = (rt_ubase_t) 0; /* Q8 */ - *(--stk) = (rt_ubase_t) 0; /* Q8 */ - *(--stk) = (rt_ubase_t) 0; /* Q9 */ - *(--stk) = (rt_ubase_t) 0; /* Q9 */ - *(--stk) = (rt_ubase_t) 0; /* Q10 */ - *(--stk) = (rt_ubase_t) 0; /* Q10 */ - *(--stk) = (rt_ubase_t) 0; /* Q11 */ - *(--stk) = (rt_ubase_t) 0; /* Q11 */ - *(--stk) = (rt_ubase_t) 0; /* Q12 */ - *(--stk) = (rt_ubase_t) 0; /* Q12 */ - *(--stk) = (rt_ubase_t) 0; /* Q13 */ - *(--stk) = (rt_ubase_t) 0; /* Q13 */ - *(--stk) = (rt_ubase_t) 0; /* Q14 */ - *(--stk) = (rt_ubase_t) 0; /* Q14 */ - *(--stk) = (rt_ubase_t) 0; /* Q15 */ - *(--stk) = (rt_ubase_t) 0; /* Q15 */ + *(--stk) = (rt_ubase_t)1; /* X1 */ + *(--stk) = (rt_ubase_t)parameter; /* X0 */ + *(--stk) = (rt_ubase_t)3; /* X3 */ + *(--stk) = (rt_ubase_t)2; /* X2 */ + *(--stk) = (rt_ubase_t)5; /* X5 */ + *(--stk) = (rt_ubase_t)4; /* X4 */ + *(--stk) = (rt_ubase_t)7; /* X7 */ + *(--stk) = (rt_ubase_t)6; /* X6 */ + *(--stk) = (rt_ubase_t)9; /* X9 */ + *(--stk) = (rt_ubase_t)8; /* X8 */ + *(--stk) = (rt_ubase_t)11; /* X11 */ + *(--stk) = (rt_ubase_t)10; /* X10 */ + *(--stk) = (rt_ubase_t)13; /* X13 */ + *(--stk) = (rt_ubase_t)12; /* X12 */ + *(--stk) = (rt_ubase_t)15; /* X15 */ + *(--stk) = (rt_ubase_t)14; /* X14 */ + *(--stk) = (rt_ubase_t)17; /* X17 */ + *(--stk) = (rt_ubase_t)16; /* X16 */ + *(--stk) = (rt_ubase_t)19; /* X19 */ + *(--stk) = (rt_ubase_t)18; /* X18 */ + *(--stk) = (rt_ubase_t)21; /* X21 */ + *(--stk) = (rt_ubase_t)20; /* X20 */ + *(--stk) = (rt_ubase_t)23; /* X23 */ + *(--stk) = (rt_ubase_t)22; /* X22 */ + *(--stk) = (rt_ubase_t)25; /* X25 */ + *(--stk) = (rt_ubase_t)24; /* X24 */ + *(--stk) = (rt_ubase_t)27; /* X27 */ + *(--stk) = (rt_ubase_t)26; /* X26 */ + *(--stk) = (rt_ubase_t)29; /* X29 */ + *(--stk) = (rt_ubase_t)28; /* X28 */ + *(--stk) = (rt_ubase_t)0; /* FPSR */ + *(--stk) = (rt_ubase_t)0; /* FPCR */ + *(--stk) = (rt_ubase_t)texit; /* X30 - procedure call link register. */ + *(--stk) = (rt_ubase_t)0; /* sp_el0 */ - *(--stk) = ( rt_ubase_t ) 11; /* X1 */ - *(--stk) = ( rt_ubase_t ) parameter; /* X0 */ - *(--stk) = ( rt_ubase_t ) 33; /* X3 */ - *(--stk) = ( rt_ubase_t ) 22; /* X2 */ - *(--stk) = ( rt_ubase_t ) 55; /* X5 */ - *(--stk) = ( rt_ubase_t ) 44; /* X4 */ - *(--stk) = ( rt_ubase_t ) 77; /* X7 */ - *(--stk) = ( rt_ubase_t ) 66; /* X6 */ - *(--stk) = ( rt_ubase_t ) 99; /* X9 */ - *(--stk) = ( rt_ubase_t ) 88; /* X8 */ - *(--stk) = ( rt_ubase_t ) 11; /* X11 */ - *(--stk) = ( rt_ubase_t ) 10; /* X10 */ - *(--stk) = ( rt_ubase_t ) 13; /* X13 */ - *(--stk) = ( rt_ubase_t ) 12; /* X12 */ - *(--stk) = ( rt_ubase_t ) 15; /* X15 */ - *(--stk) = ( rt_ubase_t ) 14; /* X14 */ - *(--stk) = ( rt_ubase_t ) 17; /* X17 */ - *(--stk) = ( rt_ubase_t ) 16; /* X16 */ - *(--stk) = ( rt_ubase_t ) 19; /* X19 */ - *(--stk) = ( rt_ubase_t ) 18; /* X18 */ - *(--stk) = ( rt_ubase_t ) 21; /* X21 */ - *(--stk) = ( rt_ubase_t ) 20; /* X20 */ - *(--stk) = ( rt_ubase_t ) 23; /* X23 */ - *(--stk) = ( rt_ubase_t ) 22; /* X22 */ - *(--stk) = ( rt_ubase_t ) 25; /* X25 */ - *(--stk) = ( rt_ubase_t ) 24; /* X24 */ - *(--stk) = ( rt_ubase_t ) 27; /* X27 */ - *(--stk) = ( rt_ubase_t ) 26; /* X26 */ - *(--stk) = ( rt_ubase_t ) 29; /* X29 */ - *(--stk) = ( rt_ubase_t ) 28; /* X28 */ - *(--stk) = ( rt_ubase_t ) 0; /* FPSR */ - *(--stk) = ( rt_ubase_t ) 0; /* FPCR */ - *(--stk) = ( rt_ubase_t ) 0; /* XZR - has no effect, used so there are an even number of registers. */ - *(--stk) = ( rt_ubase_t ) texit; /* X30 - procedure call link register. */ + *(--stk) = INITIAL_SPSR_EL1; - *(--stk) = initial_spsr[rt_hw_get_current_el()]; - *(--stk) = ( rt_ubase_t ) tentry; /* Exception return address. */ + *(--stk) = (rt_ubase_t)tentry; /* Exception return address. */ /* return task's current stack address */ return (rt_uint8_t *)stk; diff --git a/libcpu/aarch64/common/startup_gcc.S b/libcpu/aarch64/common/startup_gcc.S new file mode 100644 index 0000000000..9674b57d57 --- /dev/null +++ b/libcpu/aarch64/common/startup_gcc.S @@ -0,0 +1,18 @@ +/* + * Copyright (c) 2006-2018, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + */ + + .global Reset_Handler + .section ".start", "ax" +Reset_Handler: + nop + +.text +.weak SVC_Handler +SVC_Handler: + ret diff --git a/libcpu/aarch64/common/trap.c b/libcpu/aarch64/common/trap.c index f828f7e01f..0a449efeb8 100644 --- a/libcpu/aarch64/common/trap.c +++ b/libcpu/aarch64/common/trap.c @@ -1,5 +1,5 @@ /* - * Copyright (c) 2006-2021, RT-Thread Development Team + * Copyright (c) 2006-2018, RT-Thread Development Team * * SPDX-License-Identifier: Apache-2.0 * @@ -13,12 +13,66 @@ #include <board.h> #include <armv8.h> -#include <interrupt.h> +#include "interrupt.h" + +#include <backtrace.h> + +void rt_unwind(struct rt_hw_exp_stack *regs, int pc_adj) +{ +} #ifdef RT_USING_FINSH extern long list_thread(void); #endif +#ifdef RT_USING_LWP +#include <lwp.h> +#include <lwp_arch.h> + +#ifdef LWP_USING_CORE_DUMP +#include <lwp_core_dump.h> +#endif + +void sys_exit(int value); +void check_user_fault(struct rt_hw_exp_stack *regs, uint32_t pc_adj, char *info) +{ + uint32_t mode = regs->cpsr; + + if ((mode & 0x1f) == 0x00) + { + rt_kprintf("%s! pc = 0x%08x\n", info, regs->pc - pc_adj); +#ifdef LWP_USING_CORE_DUMP + lwp_core_dump(regs, pc_adj); +#endif + sys_exit(-1); + } +} + +int check_user_stack(unsigned long esr, struct rt_hw_exp_stack *regs) +{ + unsigned char ec; + void *dfar; + int ret = 0; + + ec = (unsigned char)((esr >> 26) & 0x3fU); + switch (ec) + { + case 0x20: + case 0x21: + case 0x24: + asm volatile("mrs %0, far_el1":"=r"(dfar)); + if (arch_expand_user_stack(dfar)) + { + ret = 1; + } + break; + default: + break; + } + return ret; +} +#endif + /** * this function will show registers of CPU * @@ -35,84 +89,42 @@ void rt_hw_show_register(struct rt_hw_exp_stack *regs) rt_kprintf("X20:0x%16.16p X21:0x%16.16p X22:0x%16.16p X23:0x%16.16p\n", (void *)regs->x20, (void *)regs->x21, (void *)regs->x22, (void *)regs->x23); rt_kprintf("X24:0x%16.16p X25:0x%16.16p X26:0x%16.16p X27:0x%16.16p\n", (void *)regs->x24, (void *)regs->x25, (void *)regs->x26, (void *)regs->x27); rt_kprintf("X28:0x%16.16p X29:0x%16.16p X30:0x%16.16p\n", (void *)regs->x28, (void *)regs->x29, (void *)regs->x30); - rt_kprintf("SPSR :0x%16.16p\n", (void *)regs->spsr); + rt_kprintf("SP_EL0:0x%16.16p\n", (void *)regs->sp_el0); + rt_kprintf("SPSR :0x%16.16p\n", (void *)regs->cpsr); rt_kprintf("EPC :0x%16.16p\n", (void *)regs->pc); } -/** - * When comes across an instruction which it cannot handle, - * it takes the undefined instruction trap. - * - * @param regs system registers - * - * @note never invoke this function in application - */ -void rt_hw_trap_error(struct rt_hw_exp_stack *regs) -{ - rt_kprintf("error exception:\n"); - rt_hw_show_register(regs); -#ifdef RT_USING_FINSH - list_thread(); -#endif - rt_hw_cpu_shutdown(); -} - void rt_hw_trap_irq(void) { -#ifndef BSP_USING_GIC +#ifdef SOC_BCM283x + extern rt_uint8_t core_timer_flag; void *param; uint32_t irq; rt_isr_handler_t isr_func; extern struct rt_irq_desc isr_table[]; - uint32_t value = IRQ_PEND_BASIC & 0x3ff; - -#ifdef RT_USING_SMP - uint32_t cpu_id = rt_hw_cpu_id(); - uint32_t mailbox_data = IPI_MAILBOX_CLEAR(cpu_id); -#else - uint32_t cpu_id = 0; -#endif - uint32_t int_source = CORE_IRQSOURCE(cpu_id) & 0x3ff; - - if (int_source & 0x02) - { - isr_func = isr_table[IRQ_ARM_TIMER].handler; -#ifdef RT_USING_INTERRUPT_INFO - isr_table[IRQ_ARM_TIMER].counter++; -#endif - if (isr_func) - { - param = isr_table[IRQ_ARM_TIMER].param; - isr_func(IRQ_ARM_TIMER, param); - } - return; - } + uint32_t value = 0; + value = IRQ_PEND_BASIC & 0x3ff; -#ifdef RT_USING_SMP - if (int_source & 0xf0) + if(core_timer_flag != 0) { - /* it's a ipi interrupt */ - if (mailbox_data & 0x1) + uint32_t cpu_id = rt_hw_cpu_id(); + uint32_t int_source = CORE_IRQSOURCE(cpu_id); + if (int_source & 0x0f) { - /* clear mailbox */ - IPI_MAILBOX_CLEAR(cpu_id) = mailbox_data; - isr_func = isr_table[IRQ_ARM_MAILBOX].handler; + if (int_source & 0x08) + { + isr_func = isr_table[IRQ_ARM_TIMER].handler; #ifdef RT_USING_INTERRUPT_INFO - isr_table[IRQ_ARM_MAILBOX].counter++; + isr_table[IRQ_ARM_TIMER].counter++; #endif - if (isr_func) - { - param = isr_table[IRQ_ARM_MAILBOX].param; - isr_func(IRQ_ARM_MAILBOX, param); + if (isr_func) + { + param = isr_table[IRQ_ARM_TIMER].param; + isr_func(IRQ_ARM_TIMER, param); + } } } - else - { - CORE_MAILBOX3_CLEAR(cpu_id) = mailbox_data; - } - return; } -#endif /* RT_USING_SMP */ /* local interrupt*/ if (value) @@ -184,19 +196,74 @@ void rt_hw_trap_irq(void) void rt_hw_trap_fiq(void) { void *param; - int ir; + int ir, ir_self; rt_isr_handler_t isr_func; extern struct rt_irq_desc isr_table[]; ir = rt_hw_interrupt_get_irq(); + /* bit 10~12 is cpuid, bit 0~9 is interrup id */ + ir_self = ir & 0x3ffUL; + /* get interrupt service routine */ - isr_func = isr_table[ir].handler; - param = isr_table[ir].param; + isr_func = isr_table[ir_self].handler; + param = isr_table[ir_self].param; /* turn to interrupt service routine */ - isr_func(ir, param); + isr_func(ir_self, param); /* end of interrupt */ rt_hw_interrupt_ack(ir); } + +void process_exception(unsigned long esr, unsigned long epc); +void SVC_Handler(struct rt_hw_exp_stack *regs); +void rt_hw_trap_exception(struct rt_hw_exp_stack *regs) +{ + unsigned long esr; + unsigned char ec; + + asm volatile("mrs %0, esr_el1":"=r"(esr)); + ec = (unsigned char)((esr >> 26) & 0x3fU); + +#ifdef RT_USING_LWP + if (dbg_check_event(regs, esr)) + { + return; + } + else +#endif + if (ec == 0x15) /* is 64bit syscall ? */ + { + SVC_Handler(regs); + /* never return here */ + } +#ifdef RT_USING_LWP + if (check_user_stack(esr, regs)) + { + return; + } +#endif + process_exception(esr, regs->pc); + rt_hw_show_register(regs); + rt_kprintf("current: %s\n", rt_thread_self()->name); +#ifdef RT_USING_LWP + check_user_fault(regs, 0, "user fault"); +#endif +#ifdef RT_USING_FINSH + list_thread(); +#endif + backtrace((unsigned long)regs->pc, (unsigned long)regs->x30, (unsigned long)regs->x29); + rt_hw_cpu_shutdown(); +} + +void rt_hw_trap_serror(struct rt_hw_exp_stack *regs) +{ + rt_kprintf("SError\n"); + rt_hw_show_register(regs); + rt_kprintf("current: %s\n", rt_thread_self()->name); +#ifdef RT_USING_FINSH + list_thread(); +#endif + rt_hw_cpu_shutdown(); +} diff --git a/libcpu/aarch64/common/vector_gcc.S b/libcpu/aarch64/common/vector_gcc.S index 35fc742bee..d765fdd89c 100644 --- a/libcpu/aarch64/common/vector_gcc.S +++ b/libcpu/aarch64/common/vector_gcc.S @@ -1,44 +1,60 @@ /* - * Copyright (c) 2006-2022, RT-Thread Development Team + * Copyright (c) 2006-2020, RT-Thread Development Team * * SPDX-License-Identifier: Apache-2.0 * * Date Author Notes - * 2018-10-06 ZhaoXiaowei the first version - * 2022-02-16 GuEe-GUI replace vectors entry to macro + * 2018-10-06 ZhaoXiaowei the first version */ - -.macro ventry label - .align 7 - b \label -.endm + +.text .globl system_vectors -.globl vector_error +.globl vector_exception .globl vector_irq .globl vector_fiq -.align 11 + system_vectors: - /* Exception from CurrentEL (EL1t) with SP_EL0 (SPSEL = 0) */ - ventry vector_error /* Synchronous */ - ventry vector_irq /* IRQ/vIRQ */ - ventry vector_fiq /* FIQ/vFIQ */ - ventry vector_error /* SError/vSError */ +.align 11 + .set VBAR, system_vectors + .org VBAR + + /* Exception from CurrentEL (EL1) with SP_EL0 (SPSEL=1) */ + .org (VBAR + 0x00 + 0) + B vector_serror /* Synchronous */ + .org (VBAR + 0x80 + 0) + B vector_serror /* IRQ/vIRQ */ + .org (VBAR + 0x100 + 0) + B vector_serror /* FIQ/vFIQ */ + .org (VBAR + 0x180 + 0) + B vector_serror /* Error/vError */ - /* Exception from CurrentEL (EL1h) with SP_ELn */ - ventry vector_error /* Synchronous */ - ventry vector_irq /* IRQ/vIRQ */ - ventry vector_fiq /* FIQ/vFIQ */ - ventry vector_error /* SError/vSError */ + /* Exception from CurrentEL (EL1) with SP_ELn */ + .org (VBAR + 0x200 + 0) + B vector_exception /* Synchronous */ + .org (VBAR + 0x280 + 0) + B vector_irq /* IRQ/vIRQ */ + .org (VBAR + 0x300 + 0) + B vector_fiq /* FIQ/vFIQ */ + .org (VBAR + 0x380 + 0) + B vector_serror /* Exception from lower EL, aarch64 */ - ventry vector_error /* Synchronous */ - ventry vector_error /* IRQ/vIRQ */ - ventry vector_error /* FIQ/vFIQ */ - ventry vector_error /* SError/vSError */ + .org (VBAR + 0x400 + 0) + B vector_exception + .org (VBAR + 0x480 + 0) + B vector_irq + .org (VBAR + 0x500 + 0) + B vector_fiq + .org (VBAR + 0x580 + 0) + B vector_serror /* Exception from lower EL, aarch32 */ - ventry vector_error /* Synchronous */ - ventry vector_error /* IRQ/vIRQ */ - ventry vector_error /* FIQ/vFIQ */ - ventry vector_error /* SError/vSError */ + .org (VBAR + 0x600 + 0) + B vector_serror + .org (VBAR + 0x680 + 0) + B vector_serror + .org (VBAR + 0x700 + 0) + B vector_serror + .org (VBAR + 0x780 + 0) + B vector_serror diff --git a/libcpu/aarch64/cortex-a/entry_point.S b/libcpu/aarch64/cortex-a/entry_point.S index c605a41631..ae3fa02ccd 100644 --- a/libcpu/aarch64/cortex-a/entry_point.S +++ b/libcpu/aarch64/cortex-a/entry_point.S @@ -1,182 +1,291 @@ /* - * Copyright (c) 2006-2022, RT-Thread Development Team + * Copyright (c) 2006-2020, RT-Thread Development Team * * SPDX-License-Identifier: Apache-2.0 * * Date Author Notes * 2020-01-15 bigmagic the first version * 2020-08-10 SummerGift support clang compiler - * 2021-11-04 GuEe-GUI set sp with SP_ELx - * 2021-12-28 GuEe-GUI add smp support */ #include "rtconfig.h" - -#define SECONDARY_STACK_SIZE 4096 - .section ".text.entrypoint","ax" +.global __start -.globl _start -.globl secondary_cpu_start -_start: -#ifdef RT_USING_SMP - mrs x1, mpidr_el1 - adr x4, .boot_cpu_mpidr - str x1, [x4] - dsb sy +__start: +#ifdef ARCH_ARM_BOOTWITH_FLUSH_CACHE + bl __asm_flush_dcache_all #endif + bl rt_hw_cpu_id_set + /* read cpu id, stop slave cores */ + mrs x0, tpidr_el1 + cbz x0, .L__cpu_0 /* .L prefix is the local label in ELF */ - bl __asm_flush_dcache_all /* The kernel and data must flush to DDR */ + /* cpu id > 0, stop */ + /* cpu id == 0 will also goto here after returned from entry() if possible */ +.L__current_cpu_idle: + wfe + b .L__current_cpu_idle -secondary_cpu_start: -#ifdef RT_USING_SMP - adr x4, .boot_cpu_mpidr - ldr x4, [x4] - dsb sy - /* Read cpu mpidr_el1 */ - mrs x1, mpidr_el1 +.L__cpu_0: + /* set stack before our code, Define stack pointer for current exception level */ + adr x1, __start + + /* set up EL1 */ + mrs x0, CurrentEL /* CurrentEL Register. bit 2, 3. Others reserved */ + and x0, x0, #12 /* clear reserved bits */ - /* Read cpu id */ - ldr x0, =rt_cpu_mpidr_early /* BSP must be defined `rt_cpu_mpidr_early' table in smp */ - mov x2, #0 + /* running at EL3? */ + cmp x0, #12 /* 1100b. So, EL3 */ + bne .L__not_in_el3 /* 11? !EL3 -> 5: */ -cpu_id_confirm: - add x2, x2, #1 /* Next cpu id inc */ - ldr x3, [x0], #8 - dsb sy - cmp x3, #0 - beq cpu_idle /* Mean that `rt_cpu_mpidr_early' table is end */ - cmp x3, x1 - bne cpu_id_confirm + /* should never be executed, just for completeness. (EL3) */ + mov x2, #0x5b1 + msr scr_el3, x2 /* SCR_ELn Secure Configuration Register */ + mov x2, #0x3c9 + msr spsr_el3, x2 /* SPSR_ELn. Saved Program Status Register. 1111001001 */ + adr x2, .L__not_in_el3 + msr elr_el3, x2 + eret /* Exception Return: from EL3, continue from .L__not_in_el3 */ + +.L__not_in_el3: /* running at EL2 or EL1 */ + cmp x0, #4 /* 0x04 0100 EL1 */ + beq .L__in_el1 /* EL1 -> 5: */ - /* Get cpu id success */ - sub x0, x2, #1 - msr tpidr_el1, x0 /* Save cpu id global */ - cmp x3, x4 /* If it is boot cpu */ - beq boot_cpu_setup + mrs x0, hcr_el2 + bic x0, x0, #0xff + msr hcr_el2, x0 - /* Set current cpu's stack top */ - sub x0, x0, #1 - mov x1, #SECONDARY_STACK_SIZE - adr x2, .secondary_cpu_stack_top - msub x1, x0, x1, x2 + msr sp_el1, x1 /* in EL2, set sp of EL1 to _start */ - b cpu_check_el + /* enable CNTP for EL1 */ + mrs x0, cnthctl_el2 /* Counter-timer Hypervisor Control register */ + orr x0, x0, #3 + msr cnthctl_el2, x0 + msr cntvoff_el2, xzr + + /* enable AArch64 in EL1 */ + mov x0, #(1 << 31) /* AArch64 */ + orr x0, x0, #(1 << 1) /* SWIO hardwired on Pi3 */ + msr hcr_el2, x0 + mrs x0, hcr_el2 + + /* change execution level to EL1 */ + mov x2, #0x3c4 + msr spsr_el2, x2 /* 1111000100 */ + adr x2, .L__in_el1 + msr elr_el2, x2 + + eret /* exception return. from EL2. continue from .L__in_el1 */ + +.L__in_el1: +#ifdef RT_USING_LWP + ldr x9, =PV_OFFSET #else - msr tpidr_el1, xzr -#endif /* RT_USING_SMP */ + mov x9, #0 +#endif + mov sp, x1 /* in EL1. Set sp to _start */ + + /* Set CPACR_EL1 (Architecture Feature Access Control Register) to avoid trap from SIMD or float point instruction */ + mov x1, #0x00300000 /* Don't trap any SIMD/FP instructions in both EL0 and EL1 */ + msr cpacr_el1, x1 -boot_cpu_setup: - ldr x1, =_start + /* clear bss */ + ldr x1, =__bss_start /* get bss start address */ + ldr x2, =__bss_end + sub x2, x2, x1 /* get bss size */ + add x1, x1, x9 -cpu_check_el: - mrs x0, CurrentEL /* CurrentEL Register. bit 2, 3. Others reserved */ - and x0, x0, #12 /* Clear reserved bits */ + and x3, x2, #7 /* x3 is < 7 */ + ldr x4, =~0x7 + and x2, x2, x4 /* mask ~7 */ + +.L__clean_bss_loop: + cbz x2, .L__clean_bss_loop_1 + str xzr, [x1], #8 + sub x2, x2, #8 + b .L__clean_bss_loop - /* Running at EL3? */ - cmp x0, #12 /* EL3 value is 0b1100 */ - bne cpu_not_in_el3 +.L__clean_bss_loop_1: + cbz x3, .L__jump_to_entry + strb wzr, [x1], #1 + sub x3, x3, #1 + b .L__clean_bss_loop_1 - /* Should never be executed, just for completeness. (EL3) */ - mov x2, #(1 << 0) /* EL0 and EL1 are in Non-Secure state */ - orr x2, x2, #(1 << 4) /* RES1 */ - orr x2, x2, #(1 << 5) /* RES1 */ - bic x2, x2, #(1 << 7) /* SMC instructions are enabled at EL1 and above */ - orr x2, x2, #(1 << 8) /* HVC instructions are enabled at EL1 and above */ - orr x2, x2, #(1 << 10) /* The next lower level is AArch64 */ - msr scr_el3, x2 +.L__jump_to_entry: /* jump to C code, should not return */ + bl mmu_tcr_init - mov x2, #9 /* Next level is 0b1001->EL2h */ - orr x2, x2, #(1 << 6) /* Mask FIQ */ - orr x2, x2, #(1 << 7) /* Mask IRQ */ - orr x2, x2, #(1 << 8) /* Mask SError */ - orr x2, x2, #(1 << 9) /* Mask Debug Exception */ - msr spsr_el3, x2 - adr x2, cpu_in_el2 + adr x1, __start + ldr x0, =~0x1fffff + and x0, x1, x0 + add x1, x0, #0x1000 + + msr ttbr0_el1, x0 + msr ttbr1_el1, x1 + dsb sy + + ldr x2, =0x40000000 /* map 1G memory for kernel space */ +#ifdef RT_USING_LWP + ldr x3, =PV_OFFSET +#endif + bl rt_hw_mmu_setup_early + + ldr x30, =after_mmu_enable /* set LR to after_mmu_enable function, it's a v_addr */ + + mrs x1, sctlr_el1 + bic x1, x1, #(3 << 3) /* dis SA, SA0 */ + bic x1, x1, #(1 << 1) /* dis A */ + orr x1, x1, #(1 << 12) /* I */ + orr x1, x1, #(1 << 2) /* C */ + orr x1, x1, #(1 << 0) /* M */ + msr sctlr_el1, x1 /* enable MMU */ + + dsb sy + isb sy + ic ialluis /* Invalidate all instruction caches in Inner Shareable domain to Point of Unification */ + dsb sy + isb sy + tlbi vmalle1 /* Invalidate all stage 1 translations used at EL1 with the current VMID */ + dsb sy + isb sy + ret + +after_mmu_enable: +#ifdef RT_USING_LWP + mrs x0, tcr_el1 /* disable ttbr0, only using kernel space */ + orr x0, x0, #(1 << 7) + msr tcr_el1, x0 + msr ttbr0_el1, xzr + dsb sy +#endif + + mov x0, #1 + msr spsel, x0 + adr x1, __start + mov sp, x1 /* sp_el1 set to _start */ + + b rtthread_startup + +#ifdef RT_USING_SMP +/** + * secondary cpu + */ + +.globl _secondary_cpu_entry +_secondary_cpu_entry: + bl rt_hw_cpu_id_set + adr x1, __start + + /* set up EL1 */ + mrs x0, CurrentEL /* CurrentEL Register. bit 2, 3. Others reserved */ + and x0, x0, #12 /* clear reserved bits */ + + /* running at EL3? */ + cmp x0, #12 /* 1100b. So, EL3 */ + bne .L__not_in_el3_cpux /* 11? !EL3 -> 5: */ + + /* should never be executed, just for completeness. (EL3) */ + mov x2, #0x5b1 + msr scr_el3, x2 /* SCR_ELn Secure Configuration Register */ + mov x2, #0x3c9 + msr spsr_el3, x2 /* SPSR_ELn. Saved Program Status Register. 1111001001 */ + adr x2, .L__not_in_el3_cpux msr elr_el3, x2 - eret + eret /* Exception Return: from EL3, continue from .L__not_in_el3 */ + +.L__not_in_el3_cpux: /* running at EL2 or EL1 */ + cmp x0, #4 /* 0x04 0100 EL1 */ + beq .L__in_el1_cpux /* EL1 -> 5: */ + + mrs x0, hcr_el2 + bic x0, x0, #0xff + msr hcr_el2, x0 -cpu_not_in_el3: /* Running at EL2 or EL1 */ - cmp x0, #4 /* EL1 = 0100 */ - beq cpu_in_el1 + msr sp_el1, x1 /* in EL2, set sp of EL1 to _start */ -cpu_in_el2: - /* Enable CNTP for EL1 */ + /* enable CNTP for EL1 */ mrs x0, cnthctl_el2 /* Counter-timer Hypervisor Control register */ orr x0, x0, #3 msr cnthctl_el2, x0 msr cntvoff_el2, xzr - mov x0, #(1 << 31) /* Enable AArch64 in EL1 */ + /* enable AArch64 in EL1 */ + mov x0, #(1 << 31) /* AArch64 */ orr x0, x0, #(1 << 1) /* SWIO hardwired on Pi3 */ msr hcr_el2, x0 + mrs x0, hcr_el2 - mov x2, #5 /* Next level is 0b0101->EL1h */ - orr x2, x2, #(1 << 6) /* Mask FIQ */ - orr x2, x2, #(1 << 7) /* Mask IRQ */ - orr x2, x2, #(1 << 8) /* Mask SError */ - orr x2, x2, #(1 << 9) /* Mask Debug Exception */ - msr spsr_el2, x2 - adr x2, cpu_in_el1 + /* change execution level to EL1 */ + mov x2, #0x3c4 + msr spsr_el2, x2 /* 1111000100 */ + adr x2, .L__in_el1_cpux msr elr_el2, x2 - eret -cpu_in_el1: - msr spsel, #1 - mov sp, x1 /* Set sp in el1 */ + eret /* exception return. from EL2. continue from .L__in_el1 */ - /* Avoid trap from SIMD or float point instruction */ +.L__in_el1_cpux: + adr x19, .L__in_el1_cpux + ldr x8, =.L__in_el1_cpux + sub x19, x19, x8 /* get PV_OFFSET */ + + mrs x0, tpidr_el1 + /* each cpu init stack is 8k */ + sub x1, x1, x0, lsl #13 + mov sp, x1 /* in EL1. Set sp to _start */ + + /* Set CPACR_EL1 (Architecture Feature Access Control Register) to avoid trap from SIMD or float point instruction */ mov x1, #0x00300000 /* Don't trap any SIMD/FP instructions in both EL0 and EL1 */ msr cpacr_el1, x1 - mrs x1, sctlr_el1 - orr x1, x1, #(1 << 12) /* Enable Instruction */ - bic x1, x1, #(3 << 3) /* Disable SP Alignment check */ - bic x1, x1, #(1 << 1) /* Disable Alignment check */ - msr sctlr_el1, x1 +.L__jump_to_entry_cpux: /* jump to C code, should not return */ -#ifdef RT_USING_SMP - ldr x1, =_start - cmp sp, x1 - bne secondary_cpu_c_start -#endif /* RT_USING_SMP */ + /* init mmu early */ - ldr x0, =__bss_start - ldr x1, =__bss_end - sub x2, x1, x0 - mov x3, x1 - cmp x2, #7 - bls clean_bss_check + bl mmu_tcr_init -clean_bss_loop_quad: - str xzr, [x0], #8 - sub x2, x3, x0 - cmp x2, #7 - bhi clean_bss_loop_quad - cmp x1, x0 - bls jump_to_entry + adr x1, __start + ldr x0, =~0x1fffff + and x0, x1, x0 + add x1, x0, #0x1000 -clean_bss_loop_byte: - str xzr, [x0], #1 + msr ttbr0_el1, x0 + msr ttbr1_el1, x1 + dsb sy -clean_bss_check: - cmp x1, x0 - bhi clean_bss_loop_byte + ldr x30, =after_mmu_enable_cpux /* set LR to after_mmu_enable function, it's a v_addr */ -jump_to_entry: - b rtthread_startup + mrs x1, sctlr_el1 + bic x1, x1, #(3 << 3) /* dis SA, SA0 */ + bic x1, x1, #(1 << 1) /* dis A */ + orr x1, x1, #(1 << 12) /* I */ + orr x1, x1, #(1 << 2) /* C */ + orr x1, x1, #(1 << 0) /* M */ + msr sctlr_el1, x1 /* enable MMU */ -cpu_idle: - wfe - b cpu_idle + dsb sy + isb sy + ic ialluis /* Invalidate all instruction caches in Inner Shareable domain to Point of Unification */ + dsb sy + isb sy + tlbi vmalle1 /* Invalidate all stage 1 translations used at EL1 with the current VMID */ + dsb sy + isb sy + ret -#ifdef RT_USING_SMP -.align 3 -.boot_cpu_mpidr: - .quad 0x0 +after_mmu_enable_cpux: + mrs x0, tcr_el1 /* disable ttbr0, only using kernel space */ + orr x0, x0, #(1 << 7) + msr tcr_el1, x0 + msr ttbr0_el1, xzr + dsb sy + + mov x0, #1 + msr spsel, x0 + mrs x0, tpidr_el1 + /* each cpu init stack is 8k */ + adr x1, __start + sub x1, x1, x0, lsl #13 + mov sp, x1 /* in EL1. Set sp to _start */ -.align 12 -.secondary_cpu_stack: -.space (SECONDARY_STACK_SIZE * (RT_CPUS_NR - 1)) -.secondary_cpu_stack_top: + b rt_hw_secondary_cpu_bsp_start #endif |