[PATCH v2 1/2] arm/cortex-a: Fix cache flush/invalidate after u-boot.

[Chris Johns]

This is a copy of the patch from Pavel to fix some strange behaviour with
data cache, instruction cache and MMU being enabled by u-boot on the
RaspberryPi.

Closes #2774.
---
 .../libbsp/arm/shared/include/arm-a9mpcore-start.h | 38 ++++++++++++++++++++++
 c/src/lib/libbsp/arm/xilinx-zynq/Makefile.am       |  1 +
 c/src/lib/libbsp/arm/xilinx-zynq/preinstall.am     |  4 +++
 3 files changed, 43 insertions(+)

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 7d6185b..2304650 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
@@ -30,8 +30,10 @@
 #include <bsp.h>
 #include <bsp/start.h>
 #include <bsp/arm-a9mpcore-regs.h>
+#include <bsp/arm-cache-l1.h>
 #include <bsp/arm-errata.h>
 
+
 #ifdef __cplusplus
 extern "C" {
 #endif /* __cplusplus */
@@ -129,8 +131,44 @@ BSP_START_TEXT_SECTION static inline void arm_a9mpcore_start_hook_0(void)
   volatile a9mpcore_scu *scu =
     (volatile a9mpcore_scu *) BSP_ARM_A9MPCORE_SCU_BASE;
   uint32_t cpu_id = arm_cortex_a9_get_multiprocessor_cpu_id();
+  uint32_t sctlr_val;
+
+  sctlr_val = arm_cp15_get_control();
 
+  /*
+   * Current U-boot loader seems to start kernel image
+   * with I and D caches on and MMU enabled.
+   * If RTEMS application image finds that cache is on
+   * during startup then disable caches.
+   */
+  if (sctlr_val & (ARM_CP15_CTRL_I | ARM_CP15_CTRL_C | ARM_CP15_CTRL_M)) {
+    if (sctlr_val & (ARM_CP15_CTRL_C | ARM_CP15_CTRL_M)) {
+      /*
+       * If the data cache is on then ensure that it is clean
+       * before switching off to be extra carefull.
+       */
+      if (cpu_id == 0) {
+        rtems_cache_flush_entire_data();
+        rtems_cache_invalidate_entire_data();
+      }
+    }
+    arm_cp15_flush_prefetch_buffer();
+    sctlr_val &= ~(ARM_CP15_CTRL_I | ARM_CP15_CTRL_C | ARM_CP15_CTRL_M | ARM_CP15_CTRL_A);
+    arm_cp15_set_control(sctlr_val);
+  }
+  if (cpu_id == 0) {
+    rtems_cache_invalidate_entire_data();
+    rtems_cache_invalidate_entire_instruction();
+  } else {
+    arm_cache_l1_invalidate_entire_data();
+    arm_cache_l1_invalidate_entire_instruction();
+  }
   arm_cp15_branch_predictor_invalidate_all();
+  arm_cp15_tlb_invalidate();
+  arm_cp15_flush_prefetch_buffer();
+
+  /* Clear Translation Table Base Control Register */
+  arm_cp15_set_translation_table_base_control_register(0);
 
   if (cpu_id == 0) {
     arm_a9mpcore_start_scu_enable(scu);
diff --git a/c/src/lib/libbsp/arm/xilinx-zynq/Makefile.am b/c/src/lib/libbsp/arm/xilinx-zynq/Makefile.am
index 8e6f8c3..1ccc33c 100644
--- a/c/src/lib/libbsp/arm/xilinx-zynq/Makefile.am
+++ b/c/src/lib/libbsp/arm/xilinx-zynq/Makefile.am
@@ -43,6 +43,7 @@ include_bsp_HEADERS += ../shared/include/arm-gic-irq.h
 include_bsp_HEADERS += ../shared/include/arm-gic-regs.h
 include_bsp_HEADERS += ../shared/include/arm-gic-tm27.h
 include_bsp_HEADERS += ../shared/include/arm-release-id.h
+include_bsp_HEADERS += ../shared/include/arm-cache-l1.h
 include_bsp_HEADERS += include/cadence-i2c.h
 include_bsp_HEADERS += include/cadence-i2c-regs.h
 include_bsp_HEADERS += include/i2c.h
diff --git a/c/src/lib/libbsp/arm/xilinx-zynq/preinstall.am b/c/src/lib/libbsp/arm/xilinx-zynq/preinstall.am
index a75c344..f97a453 100644
--- a/c/src/lib/libbsp/arm/xilinx-zynq/preinstall.am
+++ b/c/src/lib/libbsp/arm/xilinx-zynq/preinstall.am
@@ -126,6 +126,10 @@ $(PROJECT_INCLUDE)/bsp/arm-release-id.h: ../shared/include/arm-release-id.h $(PR
 	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/arm-release-id.h
 PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/arm-release-id.h
 
+$(PROJECT_INCLUDE)/bsp/arm-cache-l1.h: ../shared/include/arm-cache-l1.h $(PROJECT_INCLUDE)/bsp/$(dirstamp)
+	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/arm-cache-l1.h
+PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/arm-cache-l1.h
+
 $(PROJECT_INCLUDE)/bsp/cadence-i2c.h: include/cadence-i2c.h $(PROJECT_INCLUDE)/bsp/$(dirstamp)
 	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/cadence-i2c.h
 PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/cadence-i2c.h
-- 
2.4.6