From 9a037da9664793334cc5da4447a0b1232dc2e1ff Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Wed, 19 Jun 2013 11:20:06 +0200
Subject: bsps/arm: Set vector base address if necessary

---
 .../arm/realview-pbx-a9/startup/bspstarthooks.c    |  1 +
 .../libbsp/arm/shared/include/arm-a9mpcore-start.h | 29 +++++++++++++
 .../libbsp/arm/xilinx-zynq/startup/bspstarthooks.c |  1 +
 c/src/lib/libcpu/arm/shared/include/arm-cp15.h     | 47 ++++++++++++++++++++++
 4 files changed, 78 insertions(+)

diff --git a/c/src/lib/libbsp/arm/realview-pbx-a9/startup/bspstarthooks.c b/c/src/lib/libbsp/arm/realview-pbx-a9/startup/bspstarthooks.c
index 93ebe20f48..cabfe64d45 100644
--- a/c/src/lib/libbsp/arm/realview-pbx-a9/startup/bspstarthooks.c
+++ b/c/src/lib/libbsp/arm/realview-pbx-a9/startup/bspstarthooks.c
@@ -100,6 +100,7 @@ BSP_START_TEXT_SECTION void bsp_start_hook_0(void)
 
 BSP_START_TEXT_SECTION void bsp_start_hook_1(void)
 {
+  arm_a9mpcore_start_hook_1();
   bsp_start_copy_sections();
   setup_mmu_and_cache();
   bsp_start_clear_bss();
diff --git a/c/src/lib/libbsp/arm/shared/include/arm-a9mpcore-start.h b/c/src/lib/libbsp/arm/shared/include/arm-a9mpcore-start.h
index b42c549c91..36b76f5e49 100644
--- a/c/src/lib/libbsp/arm/shared/include/arm-a9mpcore-start.h
+++ b/c/src/lib/libbsp/arm/shared/include/arm-a9mpcore-start.h
@@ -27,12 +27,21 @@
 extern "C" {
 #endif /* __cplusplus */
 
+BSP_START_TEXT_SECTION static inline uint32_t
+arm_cp15_get_control(void);
+
+BSP_START_TEXT_SECTION static inline void
+arm_cp15_set_control(uint32_t val);
+
 BSP_START_TEXT_SECTION static inline uint32_t
 arm_cp15_get_auxiliary_control(void);
 
 BSP_START_TEXT_SECTION static inline void
 arm_cp15_set_auxiliary_control(uint32_t val);
 
+BSP_START_TEXT_SECTION static inline void
+arm_cp15_set_vector_base_address(void *base);
+
 BSP_START_TEXT_SECTION static inline arm_a9mpcore_start_hook_0(void)
 {
 #ifdef RTEMS_SMP
@@ -81,6 +90,26 @@ BSP_START_TEXT_SECTION static inline arm_a9mpcore_start_hook_0(void)
 #endif
 }
 
+BSP_START_TEXT_SECTION static inline arm_a9mpcore_start_hook_1(void)
+{
+  /*
+   * Do not use bsp_vector_table_begin == 0, since this will get optimized away.
+  */
+  if (bsp_vector_table_end != bsp_vector_table_size) {
+    uint32_t ctrl;
+
+    /*
+     * For now we assume that every Cortex-A9 MPCore has the Security Extensions.
+     * Later it might be necessary to evaluate the ID_PFR1 register.
+     */
+    arm_cp15_set_vector_base_address(bsp_vector_table_begin);
+
+    ctrl = arm_cp15_get_control();
+    ctrl &= ~ARM_CP15_CTRL_V;
+    arm_cp15_set_control(ctrl);
+  }
+}
+
 #ifdef __cplusplus
 }
 #endif /* __cplusplus */
diff --git a/c/src/lib/libbsp/arm/xilinx-zynq/startup/bspstarthooks.c b/c/src/lib/libbsp/arm/xilinx-zynq/startup/bspstarthooks.c
index 32585fecc6..84723234c7 100644
--- a/c/src/lib/libbsp/arm/xilinx-zynq/startup/bspstarthooks.c
+++ b/c/src/lib/libbsp/arm/xilinx-zynq/startup/bspstarthooks.c
@@ -100,6 +100,7 @@ BSP_START_TEXT_SECTION void bsp_start_hook_0(void)
 
 BSP_START_TEXT_SECTION void bsp_start_hook_1(void)
 {
+  arm_a9mpcore_start_hook_1();
   bsp_start_copy_sections();
   setup_mmu_and_cache();
   bsp_start_clear_bss();
diff --git a/c/src/lib/libcpu/arm/shared/include/arm-cp15.h b/c/src/lib/libcpu/arm/shared/include/arm-cp15.h
index 6d7cf394cd..3f032f22c1 100644
--- a/c/src/lib/libcpu/arm/shared/include/arm-cp15.h
+++ b/c/src/lib/libcpu/arm/shared/include/arm-cp15.h
@@ -899,6 +899,53 @@ static inline void arm_cp15_set_auxiliary_control(uint32_t val)
   );
 }
 
+/* ID_PFR1, Processor Feature Register 1 */
+
+static inline uint32_t arm_cp15_get_processor_feature_1(void)
+{
+  ARM_SWITCH_REGISTERS;
+  uint32_t val;
+
+  __asm__ volatile (
+    ARM_SWITCH_TO_ARM
+    "mrc p15, 0, %[val], c0, c1, 1\n"
+    ARM_SWITCH_BACK
+    : [val] "=&r" (val) ARM_SWITCH_ADDITIONAL_OUTPUT
+  );
+
+  return val;
+}
+
+/* VBAR, Vector Base Address Register, Security Extensions */
+
+static inline void *arm_cp15_get_vector_base_address(void)
+{
+  ARM_SWITCH_REGISTERS;
+  void *base;
+
+  __asm__ volatile (
+    ARM_SWITCH_TO_ARM
+    "mrc p15, 0, %[base], c12, c0, 0\n"
+    ARM_SWITCH_BACK
+    : [base] "=&r" (base) ARM_SWITCH_ADDITIONAL_OUTPUT
+  );
+
+  return base;
+}
+
+static inline void arm_cp15_set_vector_base_address(void *base)
+{
+  ARM_SWITCH_REGISTERS;
+
+  __asm__ volatile (
+    ARM_SWITCH_TO_ARM
+    "mcr p15, 0, %[base], c12, c0, 0\n"
+    ARM_SWITCH_BACK
+    : ARM_SWITCH_OUTPUT
+    : [base] "r" (base)
+  );
+}
+
 /**
  * @brief Sets the @a section_flags for the address range [@a begin, @a end).
  *
-- 
cgit v1.2.3