From dc79e35d65dbace7763317ad95a2995340e3dac1 Mon Sep 17 00:00:00 2001
From: Gregory Nutt <gnutt@nuttx.org>
Date: Wed, 7 Dec 2016 09:06:41 -0600
Subject: [PATCH] For Cortex-A9, should also set ACTLR.FW in SMP mode to enble
 TLB and cache broadcasts.  Does not fix SMP cache problem.

---
 arch/arm/src/armv7-a/arm_scu.c | 16 +++++++++++++++-
 arch/arm/src/armv7-a/cache.h   | 10 ++++++++++
 arch/arm/src/armv7-r/cache.h   |  4 ++--
 3 files changed, 27 insertions(+), 3 deletions(-)

diff --git a/arch/arm/src/armv7-a/arm_scu.c b/arch/arm/src/armv7-a/arm_scu.c
index e1fdd52925e..eedf179e731 100644
--- a/arch/arm/src/armv7-a/arm_scu.c
+++ b/arch/arm/src/armv7-a/arm_scu.c
@@ -44,6 +44,7 @@
 #include "up_arch.h"
 #include "cp15_cacheops.h"
 #include "sctlr.h"
+#include "cache.h"
 #include "scu.h"
 
 #ifdef CONFIG_SMP
@@ -174,6 +175,7 @@ void arm_enable_smp(int cpu)
        */
 
       cp15_invalidate_dcache_all();
+      ARM_DSB();
 
       /* Invalidate the L2C-310 -- Missing logic. */
 
@@ -193,16 +195,28 @@ void arm_enable_smp(int cpu)
        */
 
       cp15_invalidate_dcache_all();
+      ARM_DSB();
 
       /* Wait for the SCU to be enabled by the primary processor -- should
        * not be necessary.
        */
     }
 
-  /* Enable the data cache, set the SMP mode with ACTLR.SMP=1. */
+  /* Enable the data cache, set the SMP mode with ACTLR.SMP=1.
+   *
+   *   SMP - Sgnals if the Cortex-A9 processor is taking part in coherency
+   *         or not.
+   *
+   * Cortex-A9 also needs  ACTLR.FW=1
+   *
+   *   FW  - Cache and TLB maintenance broadcast.
+   */
 
   regval  = arm_get_actlr();
   regval |= ACTLR_SMP;
+#ifdef CONFIG_ARCH_CORTEXA9
+  regval |= ACTLR_FW;
+#endif
   arm_set_actlr(regval);
 
   regval  = arm_get_sctlr();
diff --git a/arch/arm/src/armv7-a/cache.h b/arch/arm/src/armv7-a/cache.h
index dda36271e2d..c9af0611f7b 100644
--- a/arch/arm/src/armv7-a/cache.h
+++ b/arch/arm/src/armv7-a/cache.h
@@ -50,6 +50,16 @@
  * Pre-processor Definitions
  ************************************************************************************/
 
+/* Intrinsics are used in these inline functions */
+
+#define arm_isb(n) __asm__ __volatile__ ("isb " #n : : : "memory")
+#define arm_dsb(n) __asm__ __volatile__ ("dsb " #n : : : "memory")
+#define arm_dmb(n) __asm__ __volatile__ ("dmb " #n : : : "memory")
+
+#define ARM_DSB()  arm_dsb(15)
+#define ARM_ISB()  arm_isb(15)
+#define ARM_DMB()  arm_dmb(15)
+
  /************************************************************************************
  * Inline Functions
  ************************************************************************************/
diff --git a/arch/arm/src/armv7-r/cache.h b/arch/arm/src/armv7-r/cache.h
index a0d25c89410..721f40313f2 100644
--- a/arch/arm/src/armv7-r/cache.h
+++ b/arch/arm/src/armv7-r/cache.h
@@ -51,7 +51,7 @@
  * Pre-processor Definitions
  ************************************************************************************/
 
-/* intrinsics are used in these inline functions */
+/* Intrinsics are used in these inline functions */
 
 #define arm_isb(n) __asm__ __volatile__ ("isb " #n : : : "memory")
 #define arm_dsb(n) __asm__ __volatile__ ("dsb " #n : : : "memory")
@@ -61,7 +61,7 @@
 #define ARM_ISB()  arm_isb(15)
 #define ARM_DMB()  arm_dmb(15)
 
- /************************************************************************************
+/************************************************************************************
  * Inline Functions
  ************************************************************************************/