From e7b878e42f0c358379206c025059feccd711804e Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Fri, 2 Oct 2020 11:36:20 +0200
Subject: bsps/arm: Workaround for Errata 845369

Add a workaround for Cortex-A9 Errata 845369: Under Very Rare Timing
Circumstances Transition into Streaming Mode Might Create Data Corruption.

Update #4115.
---
 bsps/arm/include/bsp/arm-a9mpcore-start.h      | 15 ++++++++++++
 cpukit/score/cpu/arm/include/libcpu/arm-cp15.h | 32 ++++++++++++++++++++++++++
 2 files changed, 47 insertions(+)

diff --git a/bsps/arm/include/bsp/arm-a9mpcore-start.h b/bsps/arm/include/bsp/arm-a9mpcore-start.h
index a03bc8fb33..8f9da486b5 100644
--- a/bsps/arm/include/bsp/arm-a9mpcore-start.h
+++ b/bsps/arm/include/bsp/arm-a9mpcore-start.h
@@ -123,6 +123,20 @@ arm_a9mpcore_start_enable_smp_in_auxiliary_control(void)
   actlr |= ARM_CORTEX_A9_ACTL_SMP | ARM_CORTEX_A9_ACTL_FW;
   arm_cp15_set_auxiliary_control(actlr);
 }
+
+BSP_START_TEXT_SECTION static inline void
+arm_a9mpcore_start_errata_845369_handler(void)
+{
+  uint32_t diag;
+
+  /*
+   * Workaround for Errata 845369: Under Very Rare Timing Circumstances
+   * Transition into Streaming Mode Might Create Data Corruption.
+   */
+  diag = arm_cp15_get_diagnostic_control();
+  diag |= 1U << 22;
+  arm_cp15_set_diagnostic_control(diag);
+}
 #endif
 
 BSP_START_TEXT_SECTION static inline void arm_a9mpcore_start_hook_0(void)
@@ -138,6 +152,7 @@ BSP_START_TEXT_SECTION static inline void arm_a9mpcore_start_hook_0(void)
   }
 
 #ifdef RTEMS_SMP
+  arm_a9mpcore_start_errata_845369_handler();
   arm_a9mpcore_start_enable_smp_in_auxiliary_control();
 #endif
 
diff --git a/cpukit/score/cpu/arm/include/libcpu/arm-cp15.h b/cpukit/score/cpu/arm/include/libcpu/arm-cp15.h
index 6097d60ba6..5bc01dcb32 100644
--- a/cpukit/score/cpu/arm/include/libcpu/arm-cp15.h
+++ b/cpukit/score/cpu/arm/include/libcpu/arm-cp15.h
@@ -2308,6 +2308,38 @@ arm_cp15_set_counter_virtual_offset(uint64_t val)
   );
 }
 
+/* Diagnostic Control Register */
+ARM_CP15_TEXT_SECTION static inline uint32_t
+arm_cp15_get_diagnostic_control(void)
+{
+  ARM_SWITCH_REGISTERS;
+  uint32_t val;
+
+  __asm__ volatile (
+    ARM_SWITCH_TO_ARM
+    "mrc p15, 0, %[val], c15, c0, 1\n"
+    ARM_SWITCH_BACK
+    : [val] "=&r" (val) ARM_SWITCH_ADDITIONAL_OUTPUT
+  );
+
+  return val;
+}
+
+/* Diagnostic Control Register */
+ARM_CP15_TEXT_SECTION static inline void
+arm_cp15_set_diagnostic_control(uint32_t val)
+{
+  ARM_SWITCH_REGISTERS;
+
+  __asm__ volatile (
+    ARM_SWITCH_TO_ARM
+    "mcr p15, 0, %[val], c15, c0, 1\n"
+    ARM_SWITCH_BACK
+    : ARM_SWITCH_OUTPUT
+    : [val] "r" (val)
+  );
+}
+
 /**
  * @brief Sets the @a section_flags for the address range [@a begin, @a end).
  *
-- 
cgit v1.2.3