From 4fa21c47191f3a769ccb63153eedae330d92ef89 Mon Sep 17 00:00:00 2001
From: Oki Minabe <minabe.oki@gmail.com>
Date: Thu, 5 May 2022 20:34:40 +0900
Subject: [PATCH] armv7-a: Inner Shareable TLB maintenance operations

Summary:
- Use Inner Shareable for TLB maintenance operations
- Add config option as CONFIG_ARM_HAVE_MPCORE
- This PR is in preparation for smp with kernel build

Impact:
- armv7-a

Testing:
- sabre-6quad:smp w/ qemu
- sabre-6quad:knsh_smp w/ qemu (WIP)

Signed-off-by: Oki Minabe <minabe.oki@gmail.com>
---
 arch/arm/Kconfig                   |  9 +++++++++
 arch/arm/src/armv7-a/arm_cpuhead.S |  6 ++++++
 arch/arm/src/armv7-a/arm_head.S    |  6 ++++++
 arch/arm/src/armv7-a/mmu.h         | 23 +++++++++++++++++------
 4 files changed, 38 insertions(+), 6 deletions(-)

diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index 653d063920..f28065400f 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -662,6 +662,7 @@ config ARCH_CORTEXA5
 	select ARCH_HAVE_MMU
 	select ARCH_USE_MMU
 	select ARCH_HAVE_TESTSET
+	select ARM_HAVE_MPCORE
 
 config ARCH_CORTEXA7
 	bool
@@ -672,6 +673,7 @@ config ARCH_CORTEXA7
 	select ARCH_HAVE_MMU
 	select ARCH_USE_MMU
 	select ARCH_HAVE_TESTSET
+	select ARM_HAVE_MPCORE
 
 config ARCH_CORTEXA8
 	bool
@@ -692,6 +694,7 @@ config ARCH_CORTEXA9
 	select ARCH_HAVE_MMU
 	select ARCH_USE_MMU
 	select ARCH_HAVE_TESTSET
+	select ARM_HAVE_MPCORE
 
 config ARCH_ARMV7R
 	bool
@@ -924,6 +927,12 @@ config ARM_HAVE_MPU_UNIFIED
 		Automatically selected to indicate that the CPU supports a
 		unified MPU for both instruction and data addresses.
 
+config ARM_HAVE_MPCORE
+	bool
+	default n
+	---help---
+		Decide whether support MPCore extension
+
 config ARM_MPU
 	bool "MPU support"
 	default n
diff --git a/arch/arm/src/armv7-a/arm_cpuhead.S b/arch/arm/src/armv7-a/arm_cpuhead.S
index 9d71005a76..97948108ff 100644
--- a/arch/arm/src/armv7-a/arm_cpuhead.S
+++ b/arch/arm/src/armv7-a/arm_cpuhead.S
@@ -200,11 +200,17 @@ __cpu3_start:
 	 */
 
 	mov		r0, #0
+#ifdef CONFIG_ARM_HAVE_MPCORE
+	mcr		CP15_TLBIALLIS(r0)	/* Invalidate the entire unified TLB */
+	mcr		CP15_BPIALLIS(r0)	/* Invalidate entire branch prediction array */
+	mcr		CP15_ICIALLUIS(r0)	/* Invalidate I-cache */
+#else
 	mcr		CP15_TLBIALL(r0,c7)	/* Invalidate the entire unified TLB */
 	mcr		CP15_TLBIALL(r0,c6)
 	mcr		CP15_TLBIALL(r0,c5)
 	mcr		CP15_BPIALL(r0)		/* Invalidate entire branch prediction array */
 	mcr		CP15_ICIALLU(r0)	/* Invalidate I-cache */
+#endif
 
 	/* Load the page table address.
 	 *
diff --git a/arch/arm/src/armv7-a/arm_head.S b/arch/arm/src/armv7-a/arm_head.S
index 53ab81fb5f..e0061bd550 100644
--- a/arch/arm/src/armv7-a/arm_head.S
+++ b/arch/arm/src/armv7-a/arm_head.S
@@ -326,11 +326,17 @@ __start:
 	 */
 
 	mov		r0, #0
+#ifdef CONFIG_ARM_HAVE_MPCORE
+	mcr		CP15_TLBIALLIS(r0)	/* Invalidate the entire unified TLB */
+	mcr		CP15_BPIALLIS(r0)	/* Invalidate entire branch prediction array */
+	mcr		CP15_ICIALLUIS(r0)	/* Invalidate I-cache */
+#else
 	mcr		CP15_TLBIALL(r0,c7)	/* Invalidate the entire unified TLB */
 	mcr		CP15_TLBIALL(r0,c6)
 	mcr		CP15_TLBIALL(r0,c5)
 	mcr		CP15_BPIALL(r0)		/* Invalidate entire branch prediction array */
 	mcr		CP15_ICIALLU(r0)	/* Invalidate I-cache */
+#endif
 
 	/* Load the page table address.
 	 *
diff --git a/arch/arm/src/armv7-a/mmu.h b/arch/arm/src/armv7-a/mmu.h
index 62034462ca..dddb1ab566 100644
--- a/arch/arm/src/armv7-a/mmu.h
+++ b/arch/arm/src/armv7-a/mmu.h
@@ -963,10 +963,10 @@ struct section_mapping_s
 
   .macro  cp15_invalidate_tlb_bymva, vaddr
   dsb
-#if defined(CONFIG_ARCH_CORTEXA8)
-  mcr  p15, 0, \vaddr, c8, c7, 1  /* TLBIMVA */
-#else
+#ifdef CONFIG_ARM_HAVE_MPCORE
   mcr  p15, 0, \vaddr, c8, c3, 3  /* TLBIMVAAIS */
+#else
+  mcr  p15, 0, \vaddr, c8, c7, 1  /* TLBIMVA */
 #endif
   dsb
   isb
@@ -1227,7 +1227,16 @@ static inline void cp15_invalidate_tlbs(void)
 {
   __asm__ __volatile__
     (
+      "\tdsb\n"
+#ifdef CONFIG_ARM_HAVE_MPCORE
+      "\tmcr p15, 0, r0, c8, c3, 0\n" /* TLBIALLIS */
+      "\tmcr p15, 0, r0, c7, c1, 6\n" /* BPIALLIS */
+#else
       "\tmcr p15, 0, r0, c8, c7, 0\n" /* TLBIALL */
+      "\tmcr p15, 0, r0, c7, c5, 6\n" /* BPIALL */
+#endif
+      "\tdsb\n"
+      "\tisb\n"
       :
       :
       : "r0", "memory"
@@ -1250,10 +1259,12 @@ static inline void cp15_invalidate_tlb_bymva(uint32_t vaddr)
   __asm__ __volatile__
     (
       "\tdsb\n"
-#if defined(CONFIG_ARCH_CORTEXA8)
-      "\tmcr p15, 0, %0, c8, c7, 1\n" /* TLBIMVA */
-#else
+#ifdef CONFIG_ARM_HAVE_MPCORE
       "\tmcr p15, 0, %0, c8, c3, 3\n" /* TLBIMVAAIS */
+      "\tmcr p15, 0, r0, c7, c1, 6\n" /* BPIALLIS */
+#else
+      "\tmcr p15, 0, %0, c8, c7, 1\n" /* TLBIMVA */
+      "\tmcr p15, 0, r0, c7, c5, 6\n" /* BPIALL */
 #endif
       "\tdsb\n"
       "\tisb\n"