From 353f717037b57f663ddce9447fd05700f3087a66 Mon Sep 17 00:00:00 2001 From: GuEe-GUI <2991707448@qq.com> Date: Fri, 7 Jan 2022 13:49:06 +0800 Subject: [libcpu/aarch64] add smp support --- libcpu/aarch64/common/armv8.h | 9 +- libcpu/aarch64/common/asm_fpu.h | 47 ++++ libcpu/aarch64/common/cache.S | 53 ++++- libcpu/aarch64/common/cache_ops.c | 70 ++++++ libcpu/aarch64/common/context_gcc.S | 85 ++++++- libcpu/aarch64/common/cp15.h | 166 -------------- libcpu/aarch64/common/cpu.c | 90 +++++--- libcpu/aarch64/common/cpuport.h | 7 + libcpu/aarch64/common/gic.c | 2 +- libcpu/aarch64/common/gtimer.c | 48 ++++ libcpu/aarch64/common/gtimer.h | 27 +++ libcpu/aarch64/common/interrupt.c | 22 +- libcpu/aarch64/common/mmu.c | 432 +++++++++++++----------------------- libcpu/aarch64/common/mmu.h | 108 +++++---- libcpu/aarch64/common/psci.c | 95 ++++++++ libcpu/aarch64/common/psci.h | 130 +++++++++++ libcpu/aarch64/common/smccc.S | 37 +++ libcpu/aarch64/common/smccc.h | 33 +++ libcpu/aarch64/common/stack.c | 38 +++- libcpu/aarch64/common/trap.c | 37 ++- 20 files changed, 979 insertions(+), 557 deletions(-) create mode 100644 libcpu/aarch64/common/asm_fpu.h create mode 100644 libcpu/aarch64/common/cache_ops.c delete mode 100644 libcpu/aarch64/common/cp15.h create mode 100644 libcpu/aarch64/common/gtimer.c create mode 100644 libcpu/aarch64/common/gtimer.h create mode 100644 libcpu/aarch64/common/psci.c create mode 100644 libcpu/aarch64/common/psci.h create mode 100644 libcpu/aarch64/common/smccc.S create mode 100644 libcpu/aarch64/common/smccc.h (limited to 'libcpu/aarch64/common') diff --git a/libcpu/aarch64/common/armv8.h b/libcpu/aarch64/common/armv8.h index 77fb4de8a2..99baa81c6e 100644 --- a/libcpu/aarch64/common/armv8.h +++ b/libcpu/aarch64/common/armv8.h @@ -6,18 +6,23 @@ * 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 + /* 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 xz; + unsigned long long xzr; + unsigned long long fpcr; + unsigned long long fpsr; unsigned long long x28; unsigned long long x29; unsigned long long x26; @@ -48,6 +53,8 @@ struct rt_hw_exp_stack unsigned long long x3; unsigned long long x0; unsigned long long x1; + + unsigned long long fpu[16]; }; #define SP_ELx ( ( unsigned long long ) 0x01 ) diff --git a/libcpu/aarch64/common/asm_fpu.h b/libcpu/aarch64/common/asm_fpu.h new file mode 100644 index 0000000000..f40ac7a9a2 --- /dev/null +++ b/libcpu/aarch64/common/asm_fpu.h @@ -0,0 +1,47 @@ +/* + * 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/cache.S b/libcpu/aarch64/common/cache.S index 7f295a2b02..38c476ac47 100644 --- a/libcpu/aarch64/common/cache.S +++ b/libcpu/aarch64/common/cache.S @@ -1,5 +1,5 @@ /* - * Copyright (c) 2006-2020, RT-Thread Development Team + * Copyright (c) 2006-2021, RT-Thread Development Team * * SPDX-License-Identifier: Apache-2.0 * @@ -134,6 +134,57 @@ __asm_flush_dcache_range: dsb sy ret +/* void __asm_invalidate_dcache_range(start, end) + * + * invalidate data cache in the range + * + * x0: start address + * x1: end address + */ +.globl __asm_invalidate_dcache_range +__asm_invalidate_dcache_range: + mrs x3, ctr_el0 + lsr x3, x3, #16 + and x3, x3, #0xf + mov x2, #4 + lsl x2, x2, x3 /* cache line size */ + + /* x2 <- minimal cache line size in cache system */ + sub x3, x2, #1 + bic x0, x0, x3 + +1: dc ivac, x0 /* invalidate data or unified cache */ + add x0, x0, x2 + cmp x0, x1 + b.lo 1b + dsb sy + ret + +/* void __asm_invalidate_icache_range(start, end) + * + * invalidate icache in the range + * + * x0: start address + * x1: end address + */ +.globl __asm_invalidate_icache_range +__asm_invalidate_icache_range: + mrs x3, ctr_el0 + and x3, x3, #0xf + mov x2, #4 + lsl x2, x2, x3 /* cache line size */ + + /* x2 <- minimal cache line size in cache system */ + sub x3, x2, #1 + bic x0, x0, x3 + +1: ic ivau, x0 /* invalidate instruction or unified cache */ + add x0, x0, x2 + cmp x0, x1 + b.lo 1b + dsb sy + ret + /* * void __asm_invalidate_icache_all(void) * diff --git a/libcpu/aarch64/common/cache_ops.c b/libcpu/aarch64/common/cache_ops.c new file mode 100644 index 0000000000..517845e924 --- /dev/null +++ b/libcpu/aarch64/common/cache_ops.c @@ -0,0 +1,70 @@ +/* + * Copyright (c) 2006-2021, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2021-12-28 GuEe-GUI the first version + */ + +#include +#include + +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 rt_hw_dcache_flush_all(void) +{ + __asm_flush_dcache_all(); +} + +void rt_hw_dcache_invalidate_all(void) +{ + __asm_invalidate_dcache_all(); +} + +void rt_hw_dcache_flush_range(unsigned long start_addr, unsigned long size) +{ + __asm_flush_dcache_range(start_addr, start_addr + size); +} + +void rt_hw_dcache_invalidate_range(unsigned long start_addr,unsigned long size) +{ + __asm_invalidate_dcache_range(start_addr, start_addr + size); +} + +void rt_hw_icache_invalidate_all() +{ + __asm_invalidate_icache_all(); +} + +void rt_hw_icache_invalidate_range(unsigned long start_addr, int size) +{ + __asm_invalidate_icache_range(start_addr, start_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); + } +} + +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); + } + else if (ops == RT_HW_CACHE_INVALIDATE) + { + rt_hw_dcache_invalidate_range((unsigned long)addr, size); + } +} diff --git a/libcpu/aarch64/common/context_gcc.S b/libcpu/aarch64/common/context_gcc.S index b1a8313916..9d7f52b585 100644 --- a/libcpu/aarch64/common/context_gcc.S +++ b/libcpu/aarch64/common/context_gcc.S @@ -1,5 +1,5 @@ /* - * Copyright (c) 2006-2020, RT-Thread Development Team + * Copyright (c) 2006-2021, RT-Thread Development Team * * SPDX-License-Identifier: Apache-2.0 * @@ -7,8 +7,17 @@ * 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 */ +#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 + /* *enable gtimer */ @@ -58,6 +67,7 @@ rt_hw_get_gtimer_frq: .macro SAVE_CONTEXT /* Save the entire context. */ + SAVE_FPU SP STP X0, X1, [SP, #-0x10]! STP X2, X3, [SP, #-0x10]! STP X4, X5, [SP, #-0x10]! @@ -73,6 +83,9 @@ rt_hw_get_gtimer_frq: STP X24, X25, [SP, #-0x10]! STP X26, X27, [SP, #-0x10]! STP X28, X29, [SP, #-0x10]! + MRS X28, FPCR + MRS X29, FPSR + STP X28, X29, [SP, #-0x10]! STP X30, XZR, [SP, #-0x10]! MRS X0, CurrentEL @@ -107,6 +120,7 @@ rt_hw_get_gtimer_frq: .macro SAVE_CONTEXT_T /* Save the entire context. */ + SAVE_FPU SP STP X0, X1, [SP, #-0x10]! STP X2, X3, [SP, #-0x10]! STP X4, X5, [SP, #-0x10]! @@ -122,6 +136,9 @@ rt_hw_get_gtimer_frq: STP X24, X25, [SP, #-0x10]! STP X26, X27, [SP, #-0x10]! STP X28, X29, [SP, #-0x10]! + MRS X28, FPCR + MRS X29, FPSR + STP X28, X29, [SP, #-0x10]! STP X30, XZR, [SP, #-0x10]! MRS X0, CurrentEL @@ -182,6 +199,9 @@ rt_hw_get_gtimer_frq: 0: LDP X30, XZR, [SP], #0x10 + LDP X28, X29, [SP], #0x10 + MSR FPCR, X28 + MSR FPSR, X29 LDP X28, X29, [SP], #0x10 LDP X26, X27, [SP], #0x10 LDP X24, X25, [SP], #0x10 @@ -197,6 +217,7 @@ rt_hw_get_gtimer_frq: LDP X4, X5, [SP], #0x10 LDP X2, X3, [SP], #0x10 LDP X0, X1, [SP], #0x10 + RESTORE_FPU SP ERET @@ -227,22 +248,46 @@ rt_hw_interrupt_enable_exit: RET /* + * #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); - * r0 --> to + * #endif + * X0 --> to + * X1 --> to_thread */ .globl rt_hw_context_switch_to rt_hw_context_switch_to: +#ifdef RT_USING_SMP + STR X0, [SP, #-0x8]! + MOV X0, X1 + BL rt_cpus_lock_status_restore + LDR X0, [SP], #0x8 +#endif /*RT_USING_SMP*/ LDR X0, [X0] RESTORE_CONTEXT .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); - * r0 --> from - * r1 --> to + * #endif + * X0 --> from + * X1 --> to + * X2 --> to_thread */ .globl rt_hw_context_switch rt_hw_context_switch: +#ifdef RT_USING_SMP + STP X0, X1, [SP, #-0x10]! + STR X30, [SP, #-0x8]! + MOV X0, X2 + BL rt_cpus_lock_status_restore + LDR X30, [SP], #0x8 + LDP X0, X1, [SP], #0x10 +#endif /*RT_USING_SMP*/ MOV X8,X0 MOV X9,X1 @@ -262,19 +307,32 @@ rt_hw_context_switch: .globl rt_interrupt_to_thread .globl rt_hw_context_switch_interrupt rt_hw_context_switch_interrupt: - ADR X2, rt_thread_switch_interrupt_flag +#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 - ADR X4, rt_interrupt_from_thread // set rt_interrupt_from_thread + LDR X4, =rt_interrupt_from_thread // set rt_interrupt_from_thread MOV X3, #1 // set rt_thread_switch_interrupt_flag to 1 STR X0, [X4] STR X3, [X2] _reswitch: - ADR X2, rt_interrupt_to_thread // set rt_interrupt_to_thread + LDR X2, =rt_interrupt_to_thread // set rt_interrupt_to_thread STR X1, [X2] RET - +#endif .text // -- Exception handlers ---------------------------------- @@ -308,10 +366,15 @@ vector_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 - ADR X1, rt_thread_switch_interrupt_flag + LDR X1, =rt_thread_switch_interrupt_flag LDR X2, [X1] CMP X2, #1 B.NE vector_irq_exit @@ -319,11 +382,11 @@ vector_irq: MOV X2, #0 // clear flag STR X2, [X1] - ADR X3, rt_interrupt_from_thread + LDR X3, =rt_interrupt_from_thread LDR X4, [X3] STR x0, [X4] // store sp in preempted tasks's TCB - ADR x3, rt_interrupt_to_thread + LDR x3, =rt_interrupt_to_thread LDR X4, [X3] LDR x0, [X4] // get new task's stack pointer diff --git a/libcpu/aarch64/common/cp15.h b/libcpu/aarch64/common/cp15.h deleted file mode 100644 index e6d8626ab9..0000000000 --- a/libcpu/aarch64/common/cp15.h +++ /dev/null @@ -1,166 +0,0 @@ -/* - * Copyright (c) 2006-2021, 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"); -} - -#ifdef RT_USING_SMP -static inline void send_ipi_msg(int cpu, int ipi_vector) -{ - IPI_MAILBOX_SET(cpu) = 1 << ipi_vector; -} - -static inline void setup_bootstrap_addr(int cpu, int addr) -{ - CORE_MAILBOX3_SET(cpu) = addr; -} - -static inline void enable_cpu_ipi_intr(int cpu) -{ - COREMB_INTCTL(cpu) = IPI_MAILBOX_INT_MASK; -} - -static inline void enable_cpu_timer_intr(int cpu) -{ - CORETIMER_INTCTL(cpu) = 0x8; -} - -static inline void enable_cntv(void) -{ - rt_uint32_t cntv_ctl; - cntv_ctl = 1; - asm volatile ("mcr p15, 0, %0, c14, c3, 1" :: "r"(cntv_ctl) ); // write CNTV_CTL -} - -static inline void disable_cntv(void) -{ - rt_uint32_t cntv_ctl; - cntv_ctl = 0; - asm volatile ("mcr p15, 0, %0, c14, c3, 1" :: "r"(cntv_ctl) ); // write CNTV_CTL -} - -static inline void mask_cntv(void) -{ - rt_uint32_t cntv_ctl; - cntv_ctl = 2; - asm volatile ("mcr p15, 0, %0, c14, c3, 1" :: "r"(cntv_ctl) ); // write CNTV_CTL -} - -static inline void unmask_cntv(void) -{ - rt_uint32_t cntv_ctl; - cntv_ctl = 1; - asm volatile ("mcr p15, 0, %0, c14, c3, 1" :: "r"(cntv_ctl) ); // write CNTV_CTL -} - -static inline rt_uint64_t read_cntvct(void) -{ - rt_uint32_t val,val1; - asm volatile("mrrc p15, 1, %0, %1, c14" : "=r" (val),"=r" (val1)); - return (val); -} - -static inline rt_uint64_t read_cntvoff(void) -{ - - rt_uint64_t val; - asm volatile("mrrc p15, 4, %Q0, %R0, c14" : "=r" (val)); - return (val); -} - -static inline rt_uint32_t read_cntv_tval(void) -{ - rt_uint32_t val; - asm volatile ("mrc p15, 0, %0, c14, c3, 0" : "=r"(val) ); - return val; -} - - -static inline void write_cntv_tval(rt_uint32_t val) -{ - asm volatile ("mcr p15, 0, %0, c14, c3, 0" :: "r"(val) ); - return; -} - -static inline rt_uint32_t read_cntfrq(void) -{ - rt_uint32_t val; - asm volatile ("mrc p15, 0, %0, c14, c0, 0" : "=r"(val) ); - return val; -} - - -static inline rt_uint32_t read_cntctrl(void) -{ - rt_uint32_t val; - asm volatile ("mrc p15, 0, %0, c14, c1, 0" : "=r"(val) ); - return val; -} - -static inline uint32_t write_cntctrl(uint32_t val) -{ - - asm volatile ("mcr p15, 0, %0, c14, c1, 0" : :"r"(val) ); - return val; -} -#endif - -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 66b540a5e5..3c1bb69b77 100644 --- a/libcpu/aarch64/common/cpu.c +++ b/libcpu/aarch64/common/cpu.c @@ -7,25 +7,38 @@ * Date Author Notes * 2011-09-15 Bernard first version * 2019-07-28 zdzn add smp support + * 2021-12-21 GuEe-GUI set tpidr_el2 as multiprocessor id instead of mpidr_el1 + * 2021-12-28 GuEe-GUI add spinlock for aarch64 */ #include #include -#include -#include "cp15.h" +#include + +#ifdef RT_USING_SMP +/* 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[] = +{ + [0] = 0x80000000, + [1] = 0x80000001, + [2] = 0x80000002, + [3] = 0x80000003, + [4] = 0x80000004, + [5] = 0x80000005, + [6] = 0x80000006, + [7] = 0x80000007, + [RT_CPUS_NR] = 0 +}; +#endif int rt_hw_cpu_id(void) { - int cpu_id; rt_base_t value; - __asm__ volatile ( - "mrs %0, mpidr_el1" - :"=r"(value) - ); - cpu_id = value & 0xf; - return cpu_id; -}; + __asm__ volatile ("mrs %0, tpidr_el1":"=r"(value)); + + return value; +} #ifdef RT_USING_SMP void rt_hw_spin_lock_init(rt_hw_spinlock_t *lock) @@ -35,38 +48,45 @@ void rt_hw_spin_lock_init(rt_hw_spinlock_t *lock) void rt_hw_spin_lock(rt_hw_spinlock_t *lock) { - unsigned long tmp; - unsigned long newval; - rt_hw_spinlock_t lockval; - __asm__ __volatile__( - "pld [%0]" - ::"r"(&lock->slock) - ); - - __asm__ __volatile__( - "1: ldrex %0, [%3]\n" - " add %1, %0, %4\n" - " strex %2, %1, [%3]\n" - " teq %2, #0\n" - " bne 1b" - : "=&r" (lockval), "=&r" (newval), "=&r" (tmp) - : "r" (&lock->slock), "I" (1 << 16) - : "cc"); - - while (lockval.tickets.next != lockval.tickets.owner) { - __WFE(); - lockval.tickets.owner = *(volatile unsigned short *)(&lock->tickets.owner); - } + rt_hw_spinlock_t lock_val, new_lockval; + unsigned int tmp; + __asm__ volatile ( + /* 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" + /* + * Didn't get lock and spin on the owner. + * Should 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" + " cbnz %w1, 2b\n" + /* got the lock. */ + "3:" + : "=&r" (lock_val), "=&r" (new_lockval), "=&r" (tmp), "+Q" (*lock) + : "Q" (lock->tickets.owner), "I" (1 << 16) + : "memory"); __DMB(); } void rt_hw_spin_unlock(rt_hw_spinlock_t *lock) { __DMB(); - lock->tickets.owner++; - __DSB(); - __SEV(); + __asm__ volatile ( + "stlrh %w1, %0\n" + : "=Q" (lock->tickets.owner) + : "r" (lock->tickets.owner + 1) + : "memory"); } #endif /*RT_USING_SMP*/ diff --git a/libcpu/aarch64/common/cpuport.h b/libcpu/aarch64/common/cpuport.h index 91c4a7f77b..ab398573ff 100644 --- a/libcpu/aarch64/common/cpuport.h +++ b/libcpu/aarch64/common/cpuport.h @@ -13,6 +13,13 @@ #include +#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") + rt_inline void rt_hw_isb(void) { __asm__ volatile ("isb":::"memory"); diff --git a/libcpu/aarch64/common/gic.c b/libcpu/aarch64/common/gic.c index d23ece42df..7a2c368d23 100644 --- a/libcpu/aarch64/common/gic.c +++ b/libcpu/aarch64/common/gic.c @@ -15,7 +15,7 @@ #include #include -#include +#include struct arm_gic { diff --git a/libcpu/aarch64/common/gtimer.c b/libcpu/aarch64/common/gtimer.c new file mode 100644 index 0000000000..74187a92cb --- /dev/null +++ b/libcpu/aarch64/common/gtimer.c @@ -0,0 +1,48 @@ +/* + * Copyright (c) 2006-2021, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2011-12-20 GuEe-GUI first version + */ + +#include +#include +#include +#include + +#define EL1_PHY_TIMER_IRQ_NUM 30 + +static volatile rt_uint64_t timer_step; + +static void rt_hw_timer_isr(int vector, void *parameter) +{ + rt_hw_set_gtimer_val(timer_step); + rt_tick_increase(); +} + +void rt_hw_gtimer_init(void) +{ + rt_hw_interrupt_install(EL1_PHY_TIMER_IRQ_NUM, rt_hw_timer_isr, RT_NULL, "tick"); + __ISB(); + timer_step = rt_hw_get_gtimer_frq(); + __DSB(); + timer_step /= RT_TICK_PER_SECOND; + rt_hw_gtimer_local_enable(); +} + +void rt_hw_gtimer_local_enable(void) +{ + rt_hw_gtimer_disable(); + rt_hw_set_gtimer_val(timer_step); + rt_hw_interrupt_umask(EL1_PHY_TIMER_IRQ_NUM); + rt_hw_gtimer_enable(); +} + +void rt_hw_gtimer_local_disable(void) +{ + rt_hw_gtimer_disable(); + rt_hw_interrupt_mask(EL1_PHY_TIMER_IRQ_NUM); +} diff --git a/libcpu/aarch64/common/gtimer.h b/libcpu/aarch64/common/gtimer.h new file mode 100644 index 0000000000..956e2ed5aa --- /dev/null +++ b/libcpu/aarch64/common/gtimer.h @@ -0,0 +1,27 @@ +/* + * Copyright (c) 2006-2021, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2011-12-20 GuEe-GUI first version + */ + +#ifndef __GTIMER_H__ +#define __GTIMER_H__ + +#include + +void rt_hw_gtimer_init(void); +void rt_hw_gtimer_local_enable(void); +void rt_hw_gtimer_local_disable(void); + +void rt_hw_gtimer_enable(); +void rt_hw_gtimer_disable(); +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(); +rt_uint64_t rt_hw_get_gtimer_frq(); + +#endif /* __GTIMER_H__ */ diff --git a/libcpu/aarch64/common/interrupt.c b/libcpu/aarch64/common/interrupt.c index c4e3f20b46..895088c805 100644 --- a/libcpu/aarch64/common/interrupt.c +++ b/libcpu/aarch64/common/interrupt.c @@ -15,16 +15,15 @@ #include "gic.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 const unsigned int VECTOR_BASE = 0x00; extern int system_vectors; @@ -39,9 +38,9 @@ extern volatile rt_uint8_t rt_interrupt_nest; 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); + rt_kprintf("cpu %d unhandled irq: %d\n", rt_hw_cpu_id(), vector); #else - rt_kprintf("unhandled irq: %d\n",vector); + rt_kprintf("unhandled irq: %d\n", vector); #endif } #endif @@ -138,7 +137,7 @@ void rt_hw_interrupt_mask(int vector) void rt_hw_interrupt_umask(int vector) { #ifndef BSP_USING_GIC -if (vector < 32) + if (vector < 32) { IRQ_ENABLE1 = (1 << vector); } @@ -397,7 +396,20 @@ void rt_hw_ipi_send(int ipi_vector, unsigned int cpu_mask) { #ifdef BSP_USING_GIC arm_gic_send_sgi(0, ipi_vector, cpu_mask, 0); +#else + int i; + + __DSB(); + + for (i = 0; i < RT_CPUS_NR; ++i) + { + if (cpu_mask & (1 << i)) + { + IPI_MAILBOX_SET(i) = 1 << ipi_vector; + } + } #endif + __DSB(); } void rt_hw_ipi_handler_install(int ipi_vector, rt_isr_handler_t ipi_isr_handler) diff --git a/libcpu/aarch64/common/mmu.c b/libcpu/aarch64/common/mmu.c index 2a41c4f813..59292c6db2 100644 --- a/libcpu/aarch64/common/mmu.c +++ b/libcpu/aarch64/common/mmu.c @@ -4,364 +4,248 @@ * SPDX-License-Identifier: Apache-2.0 * * Change Logs: - * Date Author Notes - * 2020-02-20 bigmagic first version + * Date Author Notes + * 2021-11-28 GuEe-GUI first version */ -#include -#include -#include - -#define TTBR_CNP 1 - -typedef unsigned long int uint64_t; - -static unsigned long main_tbl[512 * 20] __attribute__((aligned (4096))); - -#define IS_ALIGNED(x, a) (((x) & ((typeof(x))(a) - 1)) == 0) - -#define PMD_TYPE_SECT (1 << 0) -#define PMD_TYPE_TABLE (3 << 0) - -#define PTE_TYPE_PAGE (3 << 0) - -#define BITS_PER_VA 39 - -/* Granule size of 4KB is being used */ -#define GRANULE_SIZE_SHIFT 12 -#define GRANULE_SIZE (1 << GRANULE_SIZE_SHIFT) -#define XLAT_ADDR_MASK ((1UL << BITS_PER_VA) - GRANULE_SIZE) - -#define PMD_TYPE_MASK (3 << 0) - -int free_idx = 1; +#include +#include -void __asm_invalidate_icache_all(void); -void __asm_flush_dcache_all(void); -int __asm_flush_l3_cache(void); -void __asm_flush_dcache_range(unsigned long long start, unsigned long long end); -void __asm_invalidate_dcache_all(void); -void __asm_invalidate_icache_all(void); +#include +#include -void mmu_memset(char *dst, char v, size_t len) -{ - while (len--) - { - *dst++ = v; - } -} +#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) -static unsigned long __page_off = 0; -static unsigned long get_free_page(void) -{ - __page_off += 512; - return (unsigned long)(main_tbl + __page_off); -} +#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 -static inline unsigned int get_sctlr(void) -{ - unsigned int val; - asm volatile("mrs %0, sctlr_el1" : "=r" (val) : : "cc"); - return val; -} +#define MMU_TBL_BLOCK_2M_LEVEL 2 +#define MMU_TBL_PAGE_NR_MAX 32 -static inline void set_sctlr(unsigned int val) +/* only map 4G io/memory */ +static volatile unsigned long MMUTable[512] __attribute__((aligned(4096))); +static volatile struct { - asm volatile("msr sctlr_el1, %0" : : "r" (val) : "cc"); - asm volatile("isb"); -} + unsigned long entry[512]; +} MMUPage[MMU_TBL_PAGE_NR_MAX] __attribute__((aligned(4096))); -void mmu_init(void) +static unsigned long _kernel_free_page(void) { - unsigned long val64; - unsigned long val32; - - val64 = 0x007f6eUL; - __asm__ volatile("msr MAIR_EL1, %0\n dsb sy\n"::"r"(val64)); - __asm__ volatile("mrs %0, MAIR_EL1\n dsb sy\n":"=r"(val64)); - - //TCR_EL1 - val32 = (16UL << 0)//48bit - | (0x0UL << 6) - | (0x0UL << 7) - | (0x3UL << 8) - | (0x3UL << 10)//Inner Shareable - | (0x2UL << 12) - | (0x0UL << 14)//4K - | (0x0UL << 16) - | (0x0UL << 22) - | (0x1UL << 23) - | (0x2UL << 30) - | (0x1UL << 32) - | (0x0UL << 35) - | (0x0UL << 36) - | (0x0UL << 37) - | (0x0UL << 38); - __asm__ volatile("msr TCR_EL1, %0\n"::"r"(val32)); - __asm__ volatile("mrs %0, TCR_EL1\n":"=r"(val32)); - - __asm__ volatile("msr TTBR0_EL1, %0\n dsb sy\n"::"r"(main_tbl)); - __asm__ volatile("mrs %0, TTBR0_EL1\n dsb sy\n":"=r"(val64)); - - mmu_memset((char *)main_tbl, 0, 4096); -} + static unsigned long i = 0; -void mmu_enable(void) -{ - unsigned long val64; - unsigned long val32; - - __asm__ volatile("mrs %0, SCTLR_EL1\n":"=r"(val64)); - val64 &= ~0x1000; //disable I - __asm__ volatile("dmb sy\n msr SCTLR_EL1, %0\n isb sy\n"::"r"(val64)); - - __asm__ volatile("IC IALLUIS\n dsb sy\n isb sy\n"); - __asm__ volatile("tlbi vmalle1\n dsb sy\n isb sy\n"); + if (i >= MMU_TBL_PAGE_NR_MAX) + { + return RT_NULL; + } - //SCTLR_EL1, turn on mmu - __asm__ volatile("mrs %0, SCTLR_EL1\n":"=r"(val32)); - val32 |= 0x1005; //enable mmu, I C M - __asm__ volatile("dmb sy\n msr SCTLR_EL1, %0\nisb sy\n"::"r"(val32)); - rt_hw_icache_enable(); - rt_hw_dcache_enable(); + ++i; + return (unsigned long)&MMUPage[i - 1].entry; } -static int map_single_page_2M(unsigned long* lv0_tbl, unsigned long va, unsigned long pa, unsigned long attr) +static int _kenrel_map_2M(unsigned long *tbl, unsigned long va, unsigned long pa, unsigned long attr) { int level; - unsigned long* cur_lv_tbl = lv0_tbl; + unsigned long *cur_lv_tbl = tbl; unsigned long page; unsigned long off; - int level_shift = 39; + int level_shift = MMU_ADDRESS_BITS; - if (va & (0x200000UL - 1)) + if (va & ARCH_SECTION_MASK) { return MMU_MAP_ERROR_VANOTALIGN; } - if (pa & (0x200000UL - 1)) + if (pa & ARCH_SECTION_MASK) { return MMU_MAP_ERROR_PANOTALIGN; } - for (level = 0; level < 2; level++) + + for (level = 0; level < MMU_TBL_BLOCK_2M_LEVEL; ++level) { off = (va >> level_shift); off &= MMU_LEVEL_MASK; - if ((cur_lv_tbl[off] & 1) == 0) + + if (!(cur_lv_tbl[off] & MMU_TYPE_USED)) { - page = get_free_page(); + page = _kernel_free_page(); + if (!page) { return MMU_MAP_ERROR_NOPAGE; } - mmu_memset((char *)page, 0, 4096); - cur_lv_tbl[off] = page | 0x3UL; + + 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; + 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 = cur_lv_tbl[off]; - if (!(page & 0x2)) + if ((page & MMU_TYPE_MASK) == MMU_TYPE_BLOCK) { - //is block! error! + /* is block! error! */ return MMU_MAP_ERROR_CONFLICT; } - cur_lv_tbl = (unsigned long*)(page & 0x0000fffffffff000UL); - level_shift -= 9; + + /* next level */ + cur_lv_tbl = (unsigned long *)(page & MMU_ADDRESS_MASK); + level_shift -= MMU_LEVEL_SHIFT; } - attr &= 0xfff0000000000ffcUL; - pa |= (attr | 0x1UL); //block - off = (va >> 21); + + attr &= MMU_ATTRIB_MASK; + pa |= (attr | MMU_TYPE_BLOCK); + off = (va >> ARCH_SECTION_SHIFT); off &= MMU_LEVEL_MASK; cur_lv_tbl[off] = pa; + rt_hw_cpu_dcache_ops(RT_HW_CACHE_FLUSH, cur_lv_tbl + off, sizeof(void *)); + return 0; } -int armv8_map_2M(unsigned long va, unsigned long pa, int count, unsigned long attr) +int rt_hw_mmu_setmtt(unsigned long vaddr_start, unsigned long vaddr_end, + unsigned long paddr_start, unsigned long attr) { + int ret = -1; int i; - int ret; + unsigned long count; + unsigned long map_attr = MMU_MAP_CUSTOM(MMU_AP_KAUN, attr); - if (va & (0x200000 - 1)) + if (vaddr_start > vaddr_end) + { + goto end; + } + if (vaddr_start % ARCH_SECTION_SIZE) + { + vaddr_start = (vaddr_start / ARCH_SECTION_SIZE) * ARCH_SECTION_SIZE; + } + if (paddr_start % ARCH_SECTION_SIZE) { - return -1; + paddr_start = (paddr_start / ARCH_SECTION_SIZE) * ARCH_SECTION_SIZE; } - if (pa & (0x200000 - 1)) + if (vaddr_end % ARCH_SECTION_SIZE) { - return -1; + vaddr_end = (vaddr_end / ARCH_SECTION_SIZE + 1) * ARCH_SECTION_SIZE; } + + count = (vaddr_end - vaddr_start) >> ARCH_SECTION_SHIFT; + for (i = 0; i < count; i++) { - ret = map_single_page_2M((unsigned long *)main_tbl, va, pa, attr); - va += 0x200000; - pa += 0x200000; + ret = _kenrel_map_2M((void *)MMUTable, vaddr_start, paddr_start, map_attr); + vaddr_start += ARCH_SECTION_SIZE; + paddr_start += ARCH_SECTION_SIZE; + if (ret != 0) { - return ret; + goto end; } } - return 0; -} -static void set_table(uint64_t *pt, uint64_t *table_addr) -{ - uint64_t val; - val = (0x3UL | (uint64_t)table_addr); - *pt = val; +end: + return ret; } -void mmu_memset2(unsigned char *dst, char v, int len) +void rt_hw_init_mmu_table(struct mem_desc *mdesc, rt_size_t desc_nr) { - while (len--) + rt_memset((void *)MMUTable, 0, sizeof(MMUTable)); + rt_memset((void *)MMUPage, 0, sizeof(MMUPage)); + + /* set page table */ + for (; desc_nr > 0; --desc_nr) { - *dst++ = v; + rt_hw_mmu_setmtt(mdesc->vaddr_start, mdesc->vaddr_end, mdesc->paddr_start, mdesc->attr); + ++mdesc; } -} - -static uint64_t *create_table(void) -{ - uint64_t *new_table = (uint64_t *)((unsigned char *)&main_tbl[0] + free_idx * 4096); //+ free_idx * GRANULE_SIZE; - /* Mark all entries as invalid */ - mmu_memset2((unsigned char *)new_table, 0, 4096); - free_idx++; - return new_table; -} -static int pte_type(uint64_t *pte) -{ - return *pte & PMD_TYPE_MASK; + rt_hw_dcache_flush_range((unsigned long)MMUTable, sizeof(MMUTable)); } -static int level2shift(int level) +void rt_hw_mmu_tlb_invalidate(void) { - /* Page is 12 bits wide, every level translates 9 bits */ - return (12 + 9 * (3 - level)); + __asm__ volatile ( + "tlbi vmalle1\n\r" + "dsb sy\n\r" + "isb sy\n\r" + "ic ialluis\n\r" + "dsb sy\n\r" + "isb sy"); } -static uint64_t *get_level_table(uint64_t *pte) +void rt_hw_mmu_init(void) { - uint64_t *table = (uint64_t *)(*pte & XLAT_ADDR_MASK); - - if (pte_type(pte) != PMD_TYPE_TABLE) - { - table = create_table(); - set_table(pte, table); - } - return table; -} + unsigned long reg_val; -static void map_region(uint64_t virt, uint64_t phys, uint64_t size, uint64_t attr) -{ - uint64_t block_size = 0; - uint64_t block_shift = 0; - uint64_t *pte; - uint64_t idx = 0; - uint64_t addr = 0; - uint64_t *table = 0; - int level = 0; - - addr = virt; - while (size) - { - table = &main_tbl[0]; - for (level = 0; level < 4; level++) - { - block_shift = level2shift(level); - idx = addr >> block_shift; - idx = idx%512; - block_size = (uint64_t)(1L << block_shift); - pte = table + idx; + reg_val = 0x00447fUL; + __asm__ volatile("msr mair_el1, %0"::"r"(reg_val)); - if (size >= block_size && IS_ALIGNED(addr, block_size)) - { - attr &= 0xfff0000000000ffcUL; - if(level != 3) - { - *pte = phys | (attr | 0x1UL); - } - else - { - *pte = phys | (attr | 0x3UL); - } - addr += block_size; - phys += block_size; - size -= block_size; - break; - } - table = get_level_table(pte); - } - } -} + rt_hw_isb(); -void armv8_map(unsigned long va, unsigned long pa, unsigned long size, unsigned long attr) -{ - map_region(va, pa, size, attr); -} + reg_val = (16UL << 0) /* t0sz 48bit */ + | (0UL << 6) /* reserved */ + | (0UL << 7) /* epd0 */ + | (3UL << 8) /* t0 wb cacheable */ + | (3UL << 10) /* inner shareable */ + | (2UL << 12) /* t0 outer shareable */ + | (0UL << 14) /* t0 4K */ + | (16UL << 16) /* t1sz 48bit */ + | (0UL << 22) /* define asid use ttbr0.asid */ + | (0UL << 23) /* epd1 */ + | (3UL << 24) /* t1 inner wb cacheable */ + | (3UL << 26) /* t1 outer wb cacheable */ + | (2UL << 28) /* t1 outer shareable */ + | (2UL << 30) /* t1 4k */ + | (1UL << 32) /* 001b 64GB PA */ + | (0UL << 35) /* reserved */ + | (1UL << 36) /* as: 0:8bit 1:16bit */ + | (0UL << 37) /* tbi0 */ + | (0UL << 38); /* tbi1 */ + __asm__ volatile("msr tcr_el1, %0"::"r"(reg_val)); -void rt_hw_dcache_enable(void) -{ - if (!(get_sctlr() & CR_M)) - { - rt_kprintf("please init mmu!\n"); - } - else - { - set_sctlr(get_sctlr() | CR_C); - } -} + rt_hw_isb(); -void rt_hw_dcache_flush_all(void) -{ - int ret; + __asm__ volatile ("mrs %0, sctlr_el1":"=r"(reg_val)); - __asm_flush_dcache_all(); - ret = __asm_flush_l3_cache(); - if (ret) - { - rt_kprintf("flushing dcache returns 0x%x\n", ret); - } - else - { - rt_kprintf("flushing dcache successfully.\n"); - } -} + reg_val |= 1 << 2; /* enable dcache */ + reg_val |= 1 << 0; /* enable mmu */ -void rt_hw_dcache_flush_range(unsigned long start_addr, unsigned long size) -{ - __asm_flush_dcache_range(start_addr, start_addr + size); -} -void rt_hw_dcache_invalidate_range(unsigned long start_addr,unsigned long size) -{ - __asm_flush_dcache_range(start_addr, start_addr + size); -} + __asm__ volatile ( + "msr ttbr0_el1, %0\n\r" + "msr sctlr_el1, %1\n\r" + "dsb sy\n\r" + "isb sy\n\r" + ::"r"(MMUTable), "r"(reg_val) :"memory"); -void rt_hw_dcache_invalidate_all(void) -{ - __asm_invalidate_dcache_all(); + rt_hw_mmu_tlb_invalidate(); } -void rt_hw_dcache_disable(void) +int rt_hw_mmu_map(unsigned long addr, unsigned long size, unsigned long attr) { - /* if cache isn't enabled no need to disable */ - if(!(get_sctlr() & CR_C)) - { - rt_kprintf("need enable cache!\n"); - return; - } - set_sctlr(get_sctlr() & ~CR_C); -} + int ret; + rt_ubase_t level; -//icache -void rt_hw_icache_enable(void) -{ - __asm_invalidate_icache_all(); - set_sctlr(get_sctlr() | CR_I); -} + level = rt_hw_interrupt_disable(); + ret = rt_hw_mmu_setmtt(addr, addr + size, addr, attr); -void rt_hw_icache_invalidate_all(void) -{ - __asm_invalidate_icache_all(); -} + rt_hw_interrupt_enable(level); -void rt_hw_icache_disable(void) -{ - set_sctlr(get_sctlr() & ~CR_I); + return ret; } diff --git a/libcpu/aarch64/common/mmu.h b/libcpu/aarch64/common/mmu.h index f3261c3a1d..c2837a7dce 100644 --- a/libcpu/aarch64/common/mmu.h +++ b/libcpu/aarch64/common/mmu.h @@ -4,75 +4,71 @@ * SPDX-License-Identifier: Apache-2.0 * * Change Logs: - * Date Author Notes - * 2020-02-20 bigmagic first version + * Date Author Notes + * 2021-11-28 GuEe-GUI the first version */ -#ifndef __MMU_H__ -#define __MMU_H__ +#ifndef __MMU_H_ +#define __MMU_H_ -/* - * CR1 bits (CP#15 CR1) - */ -#define CR_M (1 << 0) /* MMU enable */ -#define CR_A (1 << 1) /* Alignment abort enable */ -#define CR_C (1 << 2) /* Dcache enable */ -#define CR_W (1 << 3) /* Write buffer enable */ -#define CR_P (1 << 4) /* 32-bit exception handler */ -#define CR_D (1 << 5) /* 32-bit data address range */ -#define CR_L (1 << 6) /* Implementation defined */ -#define CR_B (1 << 7) /* Big endian */ -#define CR_S (1 << 8) /* System MMU protection */ -#define CR_R (1 << 9) /* ROM MMU protection */ -#define CR_F (1 << 10) /* Implementation defined */ -#define CR_Z (1 << 11) /* Implementation defined */ -#define CR_I (1 << 12) /* Icache enable */ -#define CR_V (1 << 13) /* Vectors relocated to 0xffff0000 */ -#define CR_RR (1 << 14) /* Round Robin cache replacement */ -#define CR_L4 (1 << 15) /* LDR pc can set T bit */ -#define CR_DT (1 << 16) -#define CR_IT (1 << 18) -#define CR_ST (1 << 19) -#define CR_FI (1 << 21) /* Fast interrupt (lower latency mode) */ -#define CR_U (1 << 22) /* Unaligned access operation */ -#define CR_XP (1 << 23) /* Extended page tables */ -#define CR_VE (1 << 24) /* Vectored interrupts */ -#define CR_EE (1 << 25) /* Exception (Big) Endian */ -#define CR_TRE (1 << 28) /* TEX remap enable */ -#define CR_AFE (1 << 29) /* Access flag enable */ -#define CR_TE (1 << 30) /* Thumb exception enable */ +#include -#define MMU_LEVEL_MASK 0x1ffUL -#define MMU_MAP_ERROR_VANOTALIGN -1 -#define MMU_MAP_ERROR_PANOTALIGN -2 -#define MMU_MAP_ERROR_NOPAGE -3 -#define MMU_MAP_ERROR_CONFLICT -4 +/* normal memory wra mapping type */ +#define NORMAL_MEM 0 +/* normal nocache memory mapping type */ +#define NORMAL_NOCACHE_MEM 1 +/* device mapping type */ +#define DEVICE_MEM 2 -#define MEM_ATTR_MEMORY ((0x1UL << 10) | (0x2UL << 8) | (0x0UL << 6) | (0x1UL << 2)) -#define MEM_ATTR_IO ((0x1UL << 10) | (0x2UL << 8) | (0x0UL << 6) | (0x2UL << 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 BUS_ADDRESS(phys) (((phys) & ~0xC0000000) | 0xC0000000) +struct mem_desc +{ + unsigned long vaddr_start; + unsigned long vaddr_end; + unsigned long paddr_start; + unsigned long attr; +}; -void mmu_init(void); +#define MMU_AF_SHIFT 10 +#define MMU_SHARED_SHIFT 8 +#define MMU_AP_SHIFT 6 +#define MMU_MA_SHIFT 2 -void mmu_enable(void); +#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 */ -int armv8_map_2M(unsigned long va, unsigned long pa, int count, unsigned long attr); +#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) -void armv8_map(unsigned long va, unsigned long pa, unsigned long size, unsigned long attr); +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); -//dcache -void rt_hw_dcache_enable(void); 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_dcache_invalidate_all(void); -void rt_hw_dcache_disable(void); - -//icache -void rt_hw_icache_enable(void); -void rt_hw_icache_invalidate_all(void); -void rt_hw_icache_disable(void); +void rt_hw_icache_invalidate_all(); +void rt_hw_icache_invalidate_range(unsigned long start_addr, int size); -#endif /*__MMU_H__*/ +#endif /* __MMU_H_ */ diff --git a/libcpu/aarch64/common/psci.c b/libcpu/aarch64/common/psci.c new file mode 100644 index 0000000000..400db977ee --- /dev/null +++ b/libcpu/aarch64/common/psci.c @@ -0,0 +1,95 @@ +/* + * Copyright (c) 2006-2021, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2021-09-09 GuEe-GUI The first version + */ + +#include +#include +#include + +typedef uint64_t (*psci_call_handle)(uint32_t fn, uint64_t arg0, uint64_t arg1, uint64_t arg2); + +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; +} + +static uint64_t psci_hvc_call(uint32_t fn, uint64_t arg0, uint64_t arg1, uint64_t arg2) +{ + return arm_hvc_call(fn, arg0, arg1, arg2, 0, 0, 0, 0).x0; +} + +static psci_call_handle psci_call = psci_smc_call; + +static uint64_t shutdown_args[3] = {0, 0, 0}; +static uint64_t reboot_args[3] = {0, 0, 0}; + +void arm_psci_init(uint64_t *platform_shutdown_args, uint64_t *platform_reboot_args) +{ + if (rt_hw_get_current_el() < 2) + { + psci_call = psci_hvc_call; + } + + if (platform_shutdown_args != RT_NULL) + { + shutdown_args[0] = platform_shutdown_args[0]; + shutdown_args[1] = platform_shutdown_args[1]; + shutdown_args[2] = platform_shutdown_args[2]; + } + + if (platform_reboot_args != RT_NULL) + { + reboot_args[0] = platform_reboot_args[0]; + reboot_args[1] = platform_reboot_args[1]; + reboot_args[2] = platform_reboot_args[2]; + } +} + +uint32_t arm_psci_get_version() +{ + return (uint32_t)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) +{ + return (uint32_t)psci_call(PSCI_0_2_FN_AFFINITY_INFO, target_affinity, lowest_affinity_level, 0); +} + +uint32_t arm_psci_get_feature(uint32_t psci_func_id) +{ + return (uint32_t)psci_call(PSCI_1_0_FN_PSCI_FEATURES, psci_func_id, 0, 0); +} + +uint32_t arm_psci_cpu_off(uint64_t state) +{ + return (uint32_t)psci_call(PSCI_0_2_FN_CPU_OFF, state, 0, 0); +} + +uint32_t arm_psci_cpu_on(uint64_t mpid, uint64_t entry) +{ + /* [40:63] and [24:31] must be zero, other is aff3, aff2, aff1, aff0 */ + mpid = mpid & 0xff00ffffff; + + return (uint32_t)psci_call(PSCI_0_2_FN_CPU_ON, mpid, entry, 0); +} + +uint32_t arm_psci_cpu_suspend(uint32_t power_state, uint64_t entry) +{ + return (uint32_t)psci_call(PSCI_0_2_FN_CPU_SUSPEND, power_state, entry, 0); +} + +void arm_psci_system_off() +{ + psci_call(PSCI_0_2_FN_SYSTEM_OFF, shutdown_args[0], shutdown_args[1], shutdown_args[2]); +} + +void arm_psci_system_reboot() +{ + psci_call(PSCI_0_2_FN_SYSTEM_RESET, reboot_args[0], reboot_args[1], reboot_args[2]); +} diff --git a/libcpu/aarch64/common/psci.h b/libcpu/aarch64/common/psci.h new file mode 100644 index 0000000000..e8b2e3129d --- /dev/null +++ b/libcpu/aarch64/common/psci.h @@ -0,0 +1,130 @@ +/* + * Copyright (c) 2006-2021, 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 + +/* + * Non-Confidential PSCI 1.0 release (30 January 2015), and errata fix for PSCI 0.2, unsupport PSCI 0.1 + */ +#define PSCI_VER_0_2 0x00000002 + +/* PSCI 0.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_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) +#define PSCI_0_2_FN_CPU_OFF PSCI_0_2_FN(2) +#define PSCI_0_2_FN_CPU_ON PSCI_0_2_FN(3) +#define PSCI_0_2_FN_AFFINITY_INFO PSCI_0_2_FN(4) +#define PSCI_0_2_FN_MIGRATE PSCI_0_2_FN(5) +#define PSCI_0_2_FN_MIGRATE_INFO_TYPE PSCI_0_2_FN(6) +#define PSCI_0_2_FN_MIGRATE_INFO_UP_CPU PSCI_0_2_FN(7) +#define PSCI_0_2_FN_SYSTEM_OFF PSCI_0_2_FN(8) +#define PSCI_0_2_FN_SYSTEM_RESET PSCI_0_2_FN(9) + +#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_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_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) + +/* 1KB stack per core */ +#define PSCI_STACK_SHIFT 10 +#define PSCI_STACK_SIZE (1 << PSCI_STACK_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 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) \ +) + +#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) + +void arm_psci_init(uint64_t *platform_shutdown_args, uint64_t *platform_reboot_args); + +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); + +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); + +void arm_psci_system_off(); +void arm_psci_system_reboot(); + +#endif /*__PSCI_H__*/ diff --git a/libcpu/aarch64/common/smccc.S b/libcpu/aarch64/common/smccc.S new file mode 100644 index 0000000000..501d210c16 --- /dev/null +++ b/libcpu/aarch64/common/smccc.S @@ -0,0 +1,37 @@ +/* + * Copyright (c) 2006-2021, 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 + */ +.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] + ret +.endm + +/* + * smc call + */ +.globl arm_smc_call +arm_smc_call: + SMCCC_CALL smc + +/* + * hvc call + */ +.globl arm_hvc_call +arm_hvc_call: + SMCCC_CALL hvc diff --git a/libcpu/aarch64/common/smccc.h b/libcpu/aarch64/common/smccc.h new file mode 100644 index 0000000000..9a84fd1452 --- /dev/null +++ b/libcpu/aarch64/common/smccc.h @@ -0,0 +1,33 @@ +/* + * Copyright (c) 2006-2021, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2021-09-09 GuEe-GUI The first version + */ + +#include + +/* + * 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 + */ + +struct arm_smccc_ret +{ + 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); diff --git a/libcpu/aarch64/common/stack.c b/libcpu/aarch64/common/stack.c index d93706fcb9..a0ec935e5d 100644 --- a/libcpu/aarch64/common/stack.c +++ b/libcpu/aarch64/common/stack.c @@ -8,10 +8,9 @@ * 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 */ #include -#include - #include #define INITIAL_SPSR_EL3 (PSTATE_EL3 | SP_ELx) @@ -35,6 +34,39 @@ rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter, rt_uint8_t *stack_ad 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 ) 11; /* X1 */ *(--stk) = ( rt_ubase_t ) parameter; /* X0 */ *(--stk) = ( rt_ubase_t ) 33; /* X3 */ @@ -65,6 +97,8 @@ rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter, rt_uint8_t *stack_ad *(--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. */ diff --git a/libcpu/aarch64/common/trap.c b/libcpu/aarch64/common/trap.c index e93ad78fc5..a648c2dd88 100644 --- a/libcpu/aarch64/common/trap.c +++ b/libcpu/aarch64/common/trap.c @@ -64,15 +64,16 @@ void rt_hw_trap_irq(void) uint32_t irq; rt_isr_handler_t isr_func; extern struct rt_irq_desc isr_table[]; - uint32_t value = 0; - value = IRQ_PEND_BASIC & 0x3ff; + uint32_t value = IRQ_PEND_BASIC & 0x3ff; -#ifdef BSP_USING_CORETIMER - uint32_t cpu_id = 0; #ifdef RT_USING_SMP - cpu_id = rt_hw_cpu_id(); + 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; @@ -84,8 +85,34 @@ void rt_hw_trap_irq(void) param = isr_table[IRQ_ARM_TIMER].param; isr_func(IRQ_ARM_TIMER, param); } + return; } + +#ifdef RT_USING_SMP + if (int_source & 0xf0) + { + /* it's a ipi interrupt */ + if (mailbox_data & 0x1) + { + /* clear mailbox */ + IPI_MAILBOX_CLEAR(cpu_id) = mailbox_data; + isr_func = isr_table[IRQ_ARM_MAILBOX].handler; +#ifdef RT_USING_INTERRUPT_INFO + isr_table[IRQ_ARM_MAILBOX].counter++; #endif + if (isr_func) + { + param = isr_table[IRQ_ARM_MAILBOX].param; + isr_func(IRQ_ARM_MAILBOX, param); + } + } + else + { + CORE_MAILBOX3_CLEAR(cpu_id) = mailbox_data; + } + return; + } +#endif /* RT_USING_SMP */ /* local interrupt*/ if (value) -- cgit v1.3.1