From 87e7e135ba23b50f4820b4a1dd1377fb84d064bb Mon Sep 17 00:00:00 2001
From: Gregory Nutt <gnutt@nuttx.org>
Date: Fri, 11 Mar 2016 09:49:00 -0600
Subject: [PATCH] i.MX6: GIC decode and prioritization logic

---
 arch/arm/src/armv7-a/arm_gic.c    | 51 +++++++++++++++++++++++++++++--
 arch/arm/src/armv7-a/gic.h        |  2 ++
 arch/arm/src/common/up_internal.h |  2 ++
 arch/arm/src/common/up_vfork.c    |  3 +-
 4 files changed, 54 insertions(+), 4 deletions(-)

diff --git a/arch/arm/src/armv7-a/arm_gic.c b/arch/arm/src/armv7-a/arm_gic.c
index 243ae9c645..39e63d4cac 100644
--- a/arch/arm/src/armv7-a/arm_gic.c
+++ b/arch/arm/src/armv7-a/arm_gic.c
@@ -41,10 +41,12 @@
 
 #include <sys/types.h>
 #include <stdint.h>
+#include <errno.h>
 
 #include <arch/irq.h>
 
 #include "up_arch.h"
+#include "up_internal.h"
 #include "gic.h"
 
 #ifdef CONFIG_ARMV7A_HAVE_GIC
@@ -209,7 +211,29 @@ void arm_gic_initialize(void)
 
 uint32_t *arm_decodeirq(uint32_t *regs)
 {
-#  warning Missing logic
+  uint32_t regval;
+  int irq;
+
+  /* Read the interrupt acknowledge register and get the interrupt ID */
+
+  regval = getreg32(GIC_ICCIAR);
+  irq    = (regval & GIC_ICCIAR_INTID_MASK) >> GIC_ICCIAR_INTID_SHIFT;
+
+  /* Ignore spurions IRQs.  ICCIAR will report 1023 if there is no pending
+   * interrupt.
+   */
+
+  DEBUGASSERT(irg < NR_IRQS || irq == 1023);
+  if (irq < NR_IRQS)
+    {
+      /* Dispatch the interrupt */
+
+      regs = arm_doirq(irq, regs);
+    }
+
+  /* Write to the end-of-interrupt register */
+
+  putreg32(regval, GIC_ICCEOIR);
   return regs;
 }
 
@@ -299,8 +323,29 @@ void up_disable_irq(int irq)
 
 int up_prioritize_irq(int irq, int priority)
 {
-#  warning Missing logic
-  return OK;
+  DEBUGASSERT(irq >= 0 && irq < NR_IRQS && priority >= 0 && priority <= 255);
+
+  /* Ignore invalid interrupt IDs */
+
+  if (irq >= 0 && irq < NR_IRQS)
+    {
+      uintptr_t regaddr;
+      uint32_t regval;
+
+      /* Write the new priority to the corresponding field in the in the
+       * distributor Interrupt Priority Register (GIC_ICDIPR).
+       */
+
+      regaddr  = GIC_ICDIPR(irq);
+      regval   = getreg32(regaddr);
+      regval  &= ~GIC_ICDIPR_ID_MASK(irq);
+      regval  |= GIC_ICDIPR_ID(irq, priority);
+      putreg32(regval, regaddr);
+
+      return OK;
+    }
+
+  return -EINVAL;
 }
 
 #endif /* CONFIG_ARMV7A_HAVE_GIC */
diff --git a/arch/arm/src/armv7-a/gic.h b/arch/arm/src/armv7-a/gic.h
index 1447d4eab1..2fb696fa0f 100644
--- a/arch/arm/src/armv7-a/gic.h
+++ b/arch/arm/src/armv7-a/gic.h
@@ -256,6 +256,8 @@
                                              /* Bits 13-31: Reserved */
 /* End of Interrupt Register */
 
+#define GIC_ICCEOIR_SPURIOUS       (0x3ff)
+
 #define GIC_ICCEOIR_INTID_SHIFT    (0)       /* Bits 0-9: Interrupt ID */
 #define GIC_ICCEOIR_INTID_MASK     (0x3ff << GIC_ICCEOIR_INTID_SHIFT)
 #  define GIC_ICCEOIR_INTID(n)     ((uint32_t)(n) << GIC_ICCEOIR_INTID_SHIFT)
diff --git a/arch/arm/src/common/up_internal.h b/arch/arm/src/common/up_internal.h
index 1ebeac1dad..ac999d6034 100644
--- a/arch/arm/src/common/up_internal.h
+++ b/arch/arm/src/common/up_internal.h
@@ -140,6 +140,7 @@
  */
 
 #elif defined(CONFIG_ARCH_CORTEXA5) || defined(CONFIG_ARCH_CORTEXA8)  || \
+      defined(CONFIG_ARCH_CORTEXA9) || \
       defined(CONFIG_ARCH_CORTEXR4) || defined(CONFIG_ARCH_CORTEXR4F) || \
       defined(CONFIG_ARCH_CORTEXR5) || defined(CONFIG_ARCH_CORTEXR5F) || \
       defined(CONFIG_ARCH_CORTEXR7) || defined(CONFIG_ARCH_CORTEXR7F)
@@ -371,6 +372,7 @@ int  up_memfault(int irq, FAR void *context);
  */
 
 #elif defined(CONFIG_ARCH_CORTEXA5) || defined(CONFIG_ARCH_CORTEXA8)  || \
+      defined(CONFIG_ARCH_CORTEXA9) || \
       defined(CONFIG_ARCH_CORTEXR4) || defined(CONFIG_ARCH_CORTEXR4F) || \
       defined(CONFIG_ARCH_CORTEXR5) || defined(CONFIG_ARCH_CORTEXR5F) || \
       defined(CONFIG_ARCH_CORTEXR7) || defined(CONFIG_ARCH_CORTEXR7F)
diff --git a/arch/arm/src/common/up_vfork.c b/arch/arm/src/common/up_vfork.c
index c1312c8641..b902ccba7b 100644
--- a/arch/arm/src/common/up_vfork.c
+++ b/arch/arm/src/common/up_vfork.c
@@ -238,7 +238,8 @@ pid_t up_vfork(const struct vfork_s *context)
 
           /* REVISIT:  This logic is *not* common. */
 
-#if defined(CONFIG_ARCH_CORTEXA5) || defined(CONFIG_ARCH_CORTEXA8)
+#if defined(CONFIG_ARCH_CORTEXA5) || defined(CONFIG_ARCH_CORTEXA8) || \
+    defined(CONFIG_ARCH_CORTEXA9)
 #  ifdef CONFIG_BUILD_KERNEL
 
           child->cmn.xcp.syscall[index].cpsr =