From 3f6eadc238eacb800c00782f112d11e42b9b4c04 Mon Sep 17 00:00:00 2001
From: Gregory Nutt <gnutt@nuttx.org>
Date: Sat, 26 Nov 2016 18:41:48 -0600
Subject: [PATCH] ARMv7-A: Fix some SCU SMP logic

---
 arch/arm/src/armv7-a/arm_scu.c | 95 +++++++++++++++++++++++++++++++++-
 arch/arm/src/armv7-a/scu.h     |  1 +
 2 files changed, 94 insertions(+), 2 deletions(-)

diff --git a/arch/arm/src/armv7-a/arm_scu.c b/arch/arm/src/armv7-a/arm_scu.c
index 24fa156e3c7..ce857e1b2d4 100644
--- a/arch/arm/src/armv7-a/arm_scu.c
+++ b/arch/arm/src/armv7-a/arm_scu.c
@@ -39,11 +39,78 @@
 
 #include <nuttx/config.h>
 
+#include <stdint.h>
+
+#include <nuttx/irq.h>
+
 #include "up_arch.h"
+#include "sctlr.h"
 #include "scu.h"
 
 #ifdef CONFIG_SMP
 
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: arm_get_actlr
+ *
+ * Description:
+ *   Get the contents of the ACTLR register
+ *
+ ****************************************************************************/
+
+static inline uint32_t arm_get_actlr(void)
+{
+  uint32_t actlr;
+
+  __asm__ __volatile__
+  (
+    "\tmrc  p15, 0, %0, c1, c0, 1\n"  /* Read ACTLR */
+    : "=r"(actlr)
+    :
+    :
+  );
+
+  return actlr;
+}
+
+/****************************************************************************
+ * Name: arm_set_actlr
+ *
+ * Description:
+ *   Set the contents of the ACTLR register
+ *
+ ****************************************************************************/
+
+static inline void arm_set_actlr(uint32_t actlr)
+{
+  __asm__ __volatile__
+  (
+    "\tmcr p15, 0, %0, c1, c0, 1\n" /* Write ACTLR */
+    :
+    : "r"(actlr)
+    :
+  );
+}
+
+/****************************************************************************
+ * Name: arm_modify_actlr
+ *
+ * Description:
+ *   Set the bits in the ACTLR register
+ *
+ ****************************************************************************/
+
+static inline void arm_modify_actlr(uint32_t setbits)
+{
+  irqstate_t flags = enter_critical_section();
+  uint32_t actlr = arm_get_actlr();
+  arm_set_actlr(actlr | setbits);
+  leave_critical_section(flags);
+}
+
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
@@ -59,8 +126,32 @@
 
 void arm_enable_smp(int cpu)
 {
- modifyreg32(SCU_CONFIG, 0, SCU_CONFIG_CPU_SMP(cpu));
- modifyreg32(SCU_CTRL, 0, SCU_CTRL_ENABLE);
+  /* Invalidate the data cache -- Missing logic. */
+
+  /* Handle actions unique to CPU0 */
+
+  if (cpu == 0)
+    {
+      /* Invalidate the SCU duplicate tags for all processors */
+
+      putreg32((SCU_INVALIDATE_ALL_WAYS << SCU_INVALIDATE_CPU0_SHIFT) |
+               (SCU_INVALIDATE_ALL_WAYS << SCU_INVALIDATE_CPU1_SHIFT) |
+               (SCU_INVALIDATE_ALL_WAYS << SCU_INVALIDATE_CPU2_SHIFT) |
+               (SCU_INVALIDATE_ALL_WAYS << SCU_INVALIDATE_CPU3_SHIFT),
+               SCU_INVALIDATE);
+
+      /* Invalidate the L2C-310 -- Missing logic. */
+
+      /* Enable the SCU */
+
+      modifyreg32(SCU_CTRL, 0, SCU_CTRL_ENABLE); /* CPU0 only */
+    }
+
+  /* Enable the data cache -- Missing logic. */
+
+  /* This CPU now participates the SMP cache coherency */
+
+  arm_modify_actlr(ACTLR_SMP);
 }
 
 #endif
diff --git a/arch/arm/src/armv7-a/scu.h b/arch/arm/src/armv7-a/scu.h
index a51f09cec48..a84fb0cc4f8 100644
--- a/arch/arm/src/armv7-a/scu.h
+++ b/arch/arm/src/armv7-a/scu.h
@@ -128,6 +128,7 @@
 
 /* SCU Invalidate All Registers in Secure State */
 
+#define SCU_INVALIDATE_ALL_WAYS      15
 #define SCU_INVALIDATE_CPU0_SHIFT    0         /* Ways that must be invalidated for CPU0 */
 #define SCU_INVALIDATE_CPU0_MASK     (15 << SCU_INVALIDATE_CPU0_SHIFT)
 #define SCU_INVALIDATE_CPU1_SHIFT    4         /* Ways that must be invalidated for CPU1 */