From e5388ad12713ba1804bece7834062ba686659365 Mon Sep 17 00:00:00 2001
From: Gregory Nutt <gnutt@nuttx.org>
Date: Thu, 12 May 2016 15:32:53 -0600
Subject: [PATCH] i.MX6:  Need to set VBAR register for each CPU

---
 arch/arm/src/imx6/imx_cpuboot.c | 39 ++++++++++++++++++++++++++++++++-
 1 file changed, 38 insertions(+), 1 deletion(-)

diff --git a/arch/arm/src/imx6/imx_cpuboot.c b/arch/arm/src/imx6/imx_cpuboot.c
index 650b9ddf5e..cc8494782e 100644
--- a/arch/arm/src/imx6/imx_cpuboot.c
+++ b/arch/arm/src/imx6/imx_cpuboot.c
@@ -43,10 +43,13 @@
 #include <assert.h>
 
 #include <nuttx/arch.h>
+#include <arch/irq.h>
 
 #include "up_arch.h"
+#include "up_internal.h"
 
 #include "chip/imx_src.h"
+#include "sctlr.h"
 #include "smp.h"
 #include "gic.h"
 
@@ -120,6 +123,14 @@ static const cpu_start_t g_cpu_boot[CONFIG_SMP_NCPUS] =
 #endif
 };
 
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/* Symbols defined via the linker script */
+
+extern uint32_t _vector_start; /* Beginning of vector block */
+
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
@@ -251,6 +262,33 @@ void arm_cpu_boot(int cpu)
 
   arm_gic_initialize();
 
+#ifdef CONFIG_ARCH_LOWVECTORS
+  /* If CONFIG_ARCH_LOWVECTORS is defined, then the vectors located at the
+   * beginning of the .text region must appear at address at the address
+   * specified in the VBAR.  There are two ways to accomplish this:
+   *
+   *   1. By explicitly mapping the beginning of .text region with a page
+   *      table entry so that the virtual address zero maps to the beginning
+   *      of the .text region.  VBAR == 0x0000:0000.
+   *
+   *   2. Set the Cortex-A5 VBAR register so that the vector table address
+   *      is moved to a location other than 0x0000:0000.
+   *
+   *  The second method is used by this logic.
+   */
+
+  /* Set the VBAR register to the address of the vector table */
+
+  DEBUGASSERT((((uintptr_t)&_vector_start) & ~VBAR_MASK) == 0);
+  cp15_wrvbar((uint32_t)&_vector_start);
+#endif /* CONFIG_ARCH_LOWVECTORS */
+
+#ifndef CONFIG_SUPPRESS_INTERRUPTS
+  /* And finally, enable interrupts */
+
+  (void)up_irq_enable();
+#endif
+
   /* The next thing that we expect to happen is for logic running on CPU0
    * to call up_cpu_start() which generate an SGI and a context switch to
    * the configured NuttX IDLE task.
@@ -261,5 +299,4 @@ void arm_cpu_boot(int cpu)
       asm("WFI");
     }
 }
-
 #endif /* CONFIG_SMP */