From cef8c598c7019f04d283f8399f5314a318564b30 Mon Sep 17 00:00:00 2001 From: hujun5 Date: Mon, 18 Sep 2023 12:28:22 +0800 Subject: [PATCH] arm64: Add support for FIQ interrupts MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit To compile arm64 NuttX, use the following command: ./tools/configure.sh -l qemu-armv8a:nsh_fiq To run，use the following command qemu-system-aarch64 -cpu cortex-a53 -nographic -machine virt,virtualization=on,gic-version=3 -net none -chardev stdio,id=con,mux=on -serial chardev:con -mon chardev=con,mode=readline -kernel ./nuttx Signed-off-by: hujun5 --- arch/arm64/Kconfig | 8 ++ arch/arm64/include/irq.h | 8 ++ arch/arm64/src/common/arm64_cpustart.c | 12 ++ arch/arm64/src/common/arm64_gicv3.c | 122 ++++++++++++++++-- arch/arm64/src/common/arm64_initialize.c | 11 ++ arch/arm64/src/common/arm64_internal.h | 11 ++ arch/arm64/src/common/arm64_vector_table.S | 6 +- arch/arm64/src/common/arm64_vectors.S | 74 ++++++++++- .../qemu-armv8a/configs/nsh_fiq/defconfig | 75 +++++++++++ 9 files changed, 310 insertions(+), 17 deletions(-) create mode 100644 boards/arm64/qemu/qemu-armv8a/configs/nsh_fiq/defconfig diff --git a/arch/arm64/Kconfig b/arch/arm64/Kconfig index fedc987820..1db0658eb4 100644 --- a/arch/arm64/Kconfig +++ b/arch/arm64/Kconfig @@ -233,6 +233,14 @@ config ARM_HAVE_NEON ---help--- Decide whether support NEON instruction +config ARM64_DECODEFIQ + bool "FIQ Handler" + default n + ---help--- + Select this option if your platform supports the function + arm_decodefiq(). This is used primarily to support secure TrustZone + interrupts received on the FIQ vector. + config ARM_GIC_VERSION int "GIC version" default 2 if ARCH_CHIP_A64 diff --git a/arch/arm64/include/irq.h b/arch/arm64/include/irq.h index 5e5fa781fb..ccaf44e1b5 100644 --- a/arch/arm64/include/irq.h +++ b/arch/arm64/include/irq.h @@ -314,7 +314,11 @@ static inline irqstate_t up_irq_save(void) __asm__ __volatile__ ( "mrs %0, daif\n" +#ifdef CONFIG_ARM64_DECODEFIQ + "msr daifset, #3\n" +#else "msr daifset, #2\n" +#endif : "=r" (flags) : : "memory" @@ -332,7 +336,11 @@ static inline irqstate_t up_irq_enable(void) __asm__ __volatile__ ( "mrs %0, daif\n" +#ifdef CONFIG_ARM64_DECODEFIQ + "msr daifclr, #3\n" +#else "msr daifclr, #2\n" +#endif : "=r" (flags) : : "memory" diff --git a/arch/arm64/src/common/arm64_cpustart.c b/arch/arm64/src/common/arm64_cpustart.c index a57eb93c09..231203276e 100644 --- a/arch/arm64/src/common/arm64_cpustart.c +++ b/arch/arm64/src/common/arm64_cpustart.c @@ -78,6 +78,13 @@ volatile uint64_t *g_cpu_int_stacktop[CONFIG_SMP_NCPUS] = (uint64_t *)(g_interrupt_stacks[0] + INTSTACK_SIZE), }; +#ifdef CONFIG_ARM64_DECODEFIQ +volatile uint64_t *g_cpu_int_fiq_stacktop[CONFIG_SMP_NCPUS] = +{ + (uint64_t *)(g_interrupt_fiq_stacks[0] + INTSTACK_SIZE), +}; +#endif + /**************************************************************************** * Private data ****************************************************************************/ @@ -158,6 +165,11 @@ static void arm64_start_cpu(int cpu_num, char *stack, int stack_sz, g_cpu_int_stacktop[cpu_num] = (uint64_t *)(g_interrupt_stacks[cpu_num] + INTSTACK_SIZE); +#ifdef CONFIG_ARM64_DECODEFIQ + g_cpu_int_fiq_stacktop[cpu_num] = + (uint64_t *)(g_interrupt_fiq_stacks[cpu_num] + INTSTACK_SIZE); +#endif + ARM64_DSB(); /* store mpid last as this is our synchronization point */ diff --git a/arch/arm64/src/common/arm64_gicv3.c b/arch/arm64/src/common/arm64_gicv3.c index e4dc93212b..4453fc7ee9 100644 --- a/arch/arm64/src/common/arm64_gicv3.c +++ b/arch/arm64/src/common/arm64_gicv3.c @@ -60,6 +60,15 @@ #define IGROUPR_VAL 0xFFFFFFFFU +#ifdef CONFIG_ARM64_DECODEFIQ + +/* Config SGI8 ~ SGI15 as group0, to signal fiq */ + +#define IGROUPR_SGI_VAL 0xFFFF00FFU +#else +#define IGROUPR_SGI_VAL 0xFFFFFFFFU +#endif + /*************************************************************************** * Private Data ***************************************************************************/ @@ -273,7 +282,7 @@ bool arm64_gic_irq_is_enabled(unsigned int intid) return (val & mask) != 0; } -unsigned int arm64_gic_get_active(void) +unsigned int arm64_gic_get_active_irq(void) { int intid; @@ -284,7 +293,20 @@ unsigned int arm64_gic_get_active(void) return intid; } -void arm64_gic_eoi(unsigned int intid) +#ifdef CONFIG_ARM64_DECODEFIQ +unsigned int arm64_gic_get_active_fiq(void) +{ + int intid; + + /* (Pending -> Active / AP) or (AP -> AP) */ + + intid = read_sysreg(ICC_IAR0_EL1); + + return intid; +} +#endif + +void aarm64_gic_eoi_irq(unsigned int intid) { /* Interrupt request deassertion from peripheral to GIC happens * by clearing interrupt condition by a write to the peripheral @@ -304,6 +326,28 @@ void arm64_gic_eoi(unsigned int intid) write_sysreg(intid, ICC_EOIR1_EL1); } +#ifdef CONFIG_ARM64_DECODEFIQ +void arm64_gic_eoi_fiq(unsigned int intid) +{ + /* Interrupt request deassertion from peripheral to GIC happens + * by clearing interrupt condition by a write to the peripheral + * register. It is desired that the write transfer is complete + * before the core tries to change GIC state from 'AP/Active' to + * a new state on seeing 'EOI write'. + * Since ICC interface writes are not ordered against Device + * memory writes, a barrier is required to ensure the ordering. + * The dsb will also ensure *completion* of previous writes with + * DEVICE nGnRnE attribute. + */ + + ARM64_DSB(); + + /* (AP -> Pending) Or (Active -> Inactive) or (AP to AP) nested case */ + + write_sysreg(intid, ICC_EOIR0_EL1); +} +#endif + static int arm64_gic_send_sgi(unsigned int sgi_id, uint64_t target_aff, uint16_t target_list) { @@ -311,7 +355,10 @@ static int arm64_gic_send_sgi(unsigned int sgi_id, uint64_t target_aff, uint32_t aff2; uint32_t aff1; uint64_t sgi_val; + uint32_t regval; + unsigned long base; + base = gic_get_rdist() + GICR_SGI_BASE_OFF; assert(GIC_IS_SGI(sgi_id)); /* Extract affinity fields from target */ @@ -324,7 +371,18 @@ static int arm64_gic_send_sgi(unsigned int sgi_id, uint64_t target_aff, target_list); ARM64_DSB(); - write_sysreg(sgi_val, ICC_SGI1R); + + regval = getreg32(IGROUPR(base, 0)); + + if (regval & BIT(sgi_id)) + { + write_sysreg(sgi_val, ICC_SGI1R); /* Group 1 */ + } + else + { + write_sysreg(sgi_val, ICC_SGI0R_EL1); /* Group 0 */ + } + ARM64_ISB(); return 0; @@ -416,7 +474,7 @@ static void gicv3_cpuif_init(void) * All interrupts will be delivered as irq */ - putreg32(IGROUPR_VAL, IGROUPR(base, 0)); + putreg32(IGROUPR_SGI_VAL, IGROUPR(base, 0)); putreg32(BIT64_MASK(GIC_NUM_INTR_PER_REG), IGROUPMODR(base, 0)); /* Configure default priorities for SGI 0:15 and PPI 0:15. */ @@ -455,6 +513,10 @@ static void gicv3_cpuif_init(void) /* Allow group1 interrupts */ write_sysreg(1, ICC_IGRPEN1_EL1); + +#ifdef CONFIG_ARM64_DECODEFIQ + write_sysreg(1, ICC_IGRPEN0_EL1); +#endif } static void gicv3_dist_init(void) @@ -462,6 +524,7 @@ static void gicv3_dist_init(void) unsigned int num_ints; unsigned int intid; unsigned int idx; + unsigned int regval; unsigned long base = GIC_DIST_BASE; num_ints = getreg32(GICD_TYPER); @@ -546,14 +609,24 @@ static void gicv3_dist_init(void) * BIT(1), we can reuse them. */ - putreg32(BIT(GICD_CTRL_ARE_S) | BIT(GICD_CTLR_ENABLE_G1NS), - GICD_CTLR); + regval = BIT(GICD_CTRL_ARE_S) | BIT(GICD_CTLR_ENABLE_G1NS); + +#ifdef CONFIG_ARM64_DECODEFIQ + regval |= BIT(GICD_CTLR_ENABLE_G0); +#endif + + putreg32(regval, GICD_CTLR); #else /* Enable distributor with ARE */ - putreg32(BIT(GICD_CTRL_ARE_NS) | BIT(GICD_CTLR_ENABLE_G1NS), - GICD_CTLR); + regval = BIT(GICD_CTRL_ARE_NS) | BIT(GICD_CTLR_ENABLE_G1NS); + +#ifdef CONFIG_ARM64_DECODEFIQ + regval |= BIT(GICD_CTLR_ENABLE_G0); +#endif + + putreg32(regval, GICD_CTLR); #endif #ifdef CONFIG_SMP @@ -682,7 +755,7 @@ uint64_t * arm64_decodeirq(uint64_t * regs) /* Read the interrupt acknowledge register and get the interrupt ID */ - irq = arm64_gic_get_active(); + irq = arm64_gic_get_active_irq(); /* Ignore spurions IRQs. ICCIAR will report 1023 if there is no pending * interrupt. @@ -698,11 +771,40 @@ uint64_t * arm64_decodeirq(uint64_t * regs) /* Write to the end-of-interrupt register */ - arm64_gic_eoi(irq); + aarm64_gic_eoi_irq(irq); return regs; } +#ifdef CONFIG_ARM64_DECODEFIQ +uint64_t * arm64_decodefiq(uint64_t * regs) +{ + int irq; + + /* Read the interrupt acknowledge register and get the interrupt ID */ + + irq = arm64_gic_get_active_fiq(); + + /* Ignore spurions IRQs. ICCIAR will report 1023 if there is no pending + * interrupt. + */ + + DEBUGASSERT(irq < NR_IRQS || irq == 1023); + if (irq < NR_IRQS) + { + /* Dispatch the interrupt */ + + regs = arm64_doirq(irq, regs); + } + + /* Write to the end-of-interrupt register */ + + arm64_gic_eoi_fiq(irq); + + return regs; +} +#endif + static int gic_validate_dist_version(void) { uint32_t typer; diff --git a/arch/arm64/src/common/arm64_initialize.c b/arch/arm64/src/common/arm64_initialize.c index 6d0f9a039f..3cb4aaa325 100644 --- a/arch/arm64/src/common/arm64_initialize.c +++ b/arch/arm64/src/common/arm64_initialize.c @@ -71,11 +71,22 @@ INIT_STACK_ARRAY_DEFINE(g_cpu_idlestackalloc, CONFIG_SMP_NCPUS, SMP_STACK_SIZE); INIT_STACK_ARRAY_DEFINE(g_interrupt_stacks, CONFIG_SMP_NCPUS, INTSTACK_SIZE); + +#ifdef CONFIG_ARM64_DECODEFIQ +INIT_STACK_ARRAY_DEFINE(g_interrupt_fiq_stacks, CONFIG_SMP_NCPUS, + INTSTACK_SIZE); +#endif + #else /* idle thread stack for primary core */ INIT_STACK_DEFINE(g_idle_stack, CONFIG_IDLETHREAD_STACKSIZE); INIT_STACK_DEFINE(g_interrupt_stack, INTSTACK_SIZE); + +#ifdef CONFIG_ARM64_DECODEFIQ +INIT_STACK_DEFINE(g_interrupt_fiq_stack, INTSTACK_SIZE); +#endif + #endif /**************************************************************************** diff --git a/arch/arm64/src/common/arm64_internal.h b/arch/arm64/src/common/arm64_internal.h index 5ead8ca714..a1a98d456f 100644 --- a/arch/arm64/src/common/arm64_internal.h +++ b/arch/arm64/src/common/arm64_internal.h @@ -162,6 +162,12 @@ INIT_STACK_ARRAY_DEFINE_EXTERN(g_cpu_idlestackalloc, CONFIG_SMP_NCPUS, SMP_STACK_SIZE); INIT_STACK_ARRAY_DEFINE_EXTERN(g_interrupt_stacks, CONFIG_SMP_NCPUS, INTSTACK_SIZE); + +#ifdef CONFIG_ARM64_DECODEFIQ +INIT_STACK_ARRAY_DEFINE_EXTERN(g_interrupt_fiq_stacks, CONFIG_SMP_NCPUS, + INTSTACK_SIZE); +#endif + uintptr_t arm64_intstack_alloc(void); uintptr_t arm64_intstack_top(void); #else @@ -169,6 +175,11 @@ uintptr_t arm64_intstack_top(void); INIT_STACK_DEFINE_EXTERN(g_idle_stack, CONFIG_IDLETHREAD_STACKSIZE); INIT_STACK_DEFINE_EXTERN(g_interrupt_stack, INTSTACK_SIZE); + +#ifdef CONFIG_ARM64_DECODEFIQ +INIT_STACK_DEFINE_EXTERN(g_interrupt_fiq_stack, INTSTACK_SIZE); +#endif + #endif /* This is the beginning of heap as provided from arm64_head.S. diff --git a/arch/arm64/src/common/arm64_vector_table.S b/arch/arm64/src/common/arm64_vector_table.S index f153bbe69f..1f9fd33243 100644 --- a/arch/arm64/src/common/arm64_vector_table.S +++ b/arch/arm64/src/common/arm64_vector_table.S @@ -151,7 +151,7 @@ SECTION_SUBSEC_FUNC(exc_vector_table,_vector_table_section,_vector_table) .align 7 arm64_enter_exception x0, x1 - b arm64_irq_spurious + b arm64_fiq_handler /* Current EL with SP0 / SError */ @@ -175,7 +175,7 @@ SECTION_SUBSEC_FUNC(exc_vector_table,_vector_table_section,_vector_table) .align 7 arm64_enter_exception x0, x1 - b arm64_irq_spurious + b arm64_fiq_handler /* Current EL with SPx / SError */ @@ -199,7 +199,7 @@ SECTION_SUBSEC_FUNC(exc_vector_table,_vector_table_section,_vector_table) .align 7 arm64_enter_exception x0, x1 - b arm64_irq_spurious + b arm64_fiq_handler /* Lower EL using AArch64 / SError */ diff --git a/arch/arm64/src/common/arm64_vectors.S b/arch/arm64/src/common/arm64_vectors.S index e44ef8aa69..08b6a6f901 100644 --- a/arch/arm64/src/common/arm64_vectors.S +++ b/arch/arm64/src/common/arm64_vectors.S @@ -336,7 +336,7 @@ exc_handle: GTEXT(arm64_irq_handler) SECTION_FUNC(text, arm64_irq_handler) - /* switch to IRQ stack and save current sp on it. */ + /* Switch to IRQ stack and save current sp on it. */ #ifdef CONFIG_SMP get_cpu_id x1 ldr x0, =(g_cpu_int_stacktop) @@ -345,7 +345,7 @@ SECTION_FUNC(text, arm64_irq_handler) #else ldr x0, =(g_interrupt_stack + CONFIG_ARCH_INTERRUPTSTACK) #endif - /* save the task's stack and switch irq stack */ + /* Save the task's stack and switch irq stack */ mov x1, sp mov sp, x0 @@ -427,8 +427,9 @@ SECTION_FUNC(text, arm64_mode32_error) b arm64_exit_exception -GTEXT(arm64_irq_spurious) -SECTION_FUNC(text, arm64_irq_spurious) +GTEXT(arm64_fiq_handler) +SECTION_FUNC(text, arm64_fiq_handler) +#ifndef CONFIG_ARM64_DECODEFIQ arm64_exception_context_save x0 x1 sp mov x1, sp @@ -439,3 +440,68 @@ SECTION_FUNC(text, arm64_irq_spurious) /* Return here only in case of recoverable error */ b arm64_exit_exception +#else + /* Switch to FIQ stack and save current sp on it. */ +#ifdef CONFIG_SMP + get_cpu_id x1 + ldr x0, =(g_cpu_int_fiq_stacktop) + lsl x1, x1, #3 + ldr x0, [x0, x1] +#else + ldr x0, =(g_interrupt_fiq_stack + CONFIG_ARCH_INTERRUPTSTACK) +#endif + /* Save the task's stack and switch fiq stack */ + + mov x1, sp + mov sp, x0 + str x1, [sp, #-16]! + + mov x0, x1 /* x0 = reg frame */ + + /* Call arm64_decodefiq() on the interrupt stack + * with interrupts disabled + */ + + bl arm64_decodefiq + + /* Upon return from arm64_decodefiq, x0 holds the pointer to the + * call reg context area, which can be use to restore context. + * This may or may not be the same value that was passed to arm64_decodefiq: + * It will differ if a context switch is required. + */ + + ldr x1, [sp], #16 + + /* retrieve the task's stack. */ + + mov sp, x1 + + cmp x0, x1 + beq fiq_exit + + +#ifdef CONFIG_SMP + /* Notes: + * Complete any pending TLB or cache maintenance on this CPU in case + * the thread migrates to a different CPU. + * This full barrier is also required by the membarrier system + * call. + */ + dsb ish + +#endif + + /* Switch thread + * - x0: restore task reg context, return by arm64_decodefiq, + * - x1: save task reg context, save before call arm64_decodefiq + * call arm64_context_switch(x0) to switch + */ + bl arm64_context_switch +#ifdef CONFIG_ARCH_FPU + /* when the fpu trap is handled */ + + b arm64_exit_exc_fpu_done +#endif +fiq_exit: + b arm64_exit_exception +#endif diff --git a/boards/arm64/qemu/qemu-armv8a/configs/nsh_fiq/defconfig b/boards/arm64/qemu/qemu-armv8a/configs/nsh_fiq/defconfig new file mode 100644 index 0000000000..8976c6914c --- /dev/null +++ b/boards/arm64/qemu/qemu-armv8a/configs/nsh_fiq/defconfig @@ -0,0 +1,75 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +CONFIG_ARCH="arm64" +CONFIG_ARCH_ARM64=y +CONFIG_ARCH_BOARD="qemu-armv8a" +CONFIG_ARCH_BOARD_QEMU_ARMV8A=y +CONFIG_ARCH_CHIP="qemu" +CONFIG_ARCH_CHIP_QEMU=y +CONFIG_ARCH_CHIP_QEMU_A53=y +CONFIG_ARCH_EARLY_PRINT=y +CONFIG_ARCH_INTERRUPTSTACK=4096 +CONFIG_ARM64_DECODEFIQ=y +CONFIG_ARM64_SEMIHOSTING_HOSTFS=y +CONFIG_ARM64_SEMIHOSTING_HOSTFS_CACHE_COHERENCE=y +CONFIG_ARM64_STRING_FUNCTION=y +CONFIG_BUILTIN=y +CONFIG_DEBUG_ASSERTIONS=y +CONFIG_DEBUG_FEATURES=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SCHED=y +CONFIG_DEBUG_SCHED_ERROR=y +CONFIG_DEBUG_SCHED_INFO=y +CONFIG_DEBUG_SCHED_WARN=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_TASK_STACKSIZE=8192 +CONFIG_DEVICE_TREE=y +CONFIG_DEV_ZERO=y +CONFIG_EXAMPLES_HELLO=y +CONFIG_EXPERIMENTAL=y +CONFIG_FS_HOSTFS=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=8192 +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_INTELHEX_BINARY=y +CONFIG_LIBC_FDT=y +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_READLINE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_PREALLOC_TIMERS=4 +CONFIG_PTHREAD_STACK_MIN=8192 +CONFIG_RAMLOG=y +CONFIG_RAM_SIZE=134217728 +CONFIG_RAM_START=0x40000000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_RR_INTERVAL=200 +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=192 +CONFIG_SPINLOCK=y +CONFIG_STACK_COLORATION=y +CONFIG_START_MONTH=3 +CONFIG_START_YEAR=2022 +CONFIG_SYMTAB_ORDEREDBYNAME=y +CONFIG_SYSTEM_NSH=y +CONFIG_SYSTEM_SYSTEM=y +CONFIG_SYSTEM_TIME64=y +CONFIG_TESTING_GETPRIME=y +CONFIG_TESTING_OSTEST=y +CONFIG_UART1_BASE=0x9000000 +CONFIG_UART1_IRQ=33 +CONFIG_UART1_PL011=y +CONFIG_UART1_SERIAL_CONSOLE=y +CONFIG_UART_PL011=y +CONFIG_USEC_PER_TICK=1000