arm64: Add support for FIQ interrupts

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 <hujun5@xiaomi.com>

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

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"

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 */

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),
+  regval = BIT(GICD_CTRL_ARE_S) | BIT(GICD_CTLR_ENABLE_G1NS);
-                 GICD_CTLR);
+
+#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),
+  regval = BIT(GICD_CTRL_ARE_NS) | BIT(GICD_CTLR_ENABLE_G1NS);
-           GICD_CTLR);
+
+#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;

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
 
 /****************************************************************************

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.

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 */
 

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)
+GTEXT(arm64_fiq_handler)
-SECTION_FUNC(text, arm64_irq_spurious)
+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

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