armv7-a: smp: allocate page table for each cpu

Summary:
- In case of SMP and ADDRENV, allocate the page table for each cpu
- Each cpu holds separated addrenv and MMU setting

Impact:
- armv7-a

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

Signed-off-by: Oki Minabe <minabe.oki@gmail.com>

arch/arm/src/armv7-a/arm_cpuhead.S

@@ -223,6 +223,10 @@ __cpu3_start:
 	 */
 
 	ldr		r1, .LCppgtable		/* r1=phys. page table */
+#ifdef CONFIG_ARCH_ADDRENV
+	mov		r2, #PGTABLE_SIZE
+	mla		r1, r2, r5, r1		/* page table of cpu1,2,3 */
+#endif
 	orr		r1, r1, #(TTBR0_RGN_WBWA | TTBR0_IRGN0)	/* Select cache properties */
 	mcr		CP15_TTBR0(r1)
 	mcr		CP15_TTBR1(r1)

arch/arm/src/armv7-a/arm_mmu.c

@@ -52,7 +52,7 @@
 #ifndef CONFIG_ARCH_ROMPGTABLE
 void mmu_l1_setentry(uint32_t paddr, uint32_t vaddr, uint32_t mmuflags)
 {
-  uint32_t *l1table = (uint32_t *)PGTABLE_BASE_VADDR;
+  uint32_t *l1table = mmu_l1_pgtable();
   uint32_t  index   = vaddr >> 20;
 
   /* Save the page table entry */
@@ -87,7 +87,7 @@ void mmu_l1_setentry(uint32_t paddr, uint32_t vaddr, uint32_t mmuflags)
 #if !defined(CONFIG_ARCH_ROMPGTABLE) && defined(CONFIG_ARCH_ADDRENV)
 void mmu_l1_restore(uintptr_t vaddr, uint32_t l1entry)
 {
-  uint32_t *l1table = (uint32_t *)PGTABLE_BASE_VADDR;
+  uint32_t *l1table = mmu_l1_pgtable();
   uint32_t  index   = vaddr >> 20;
 
   /* Set the encoded page table entry */

arch/arm/src/armv7-a/mmu.h

@@ -34,9 +34,9 @@
  ****************************************************************************/
 
 #include <nuttx/config.h>
+#include <sys/types.h>
 
 #ifndef __ASSEMBLY__
-#  include <sys/types.h>
 #  include <stdint.h>
 #  include "chip.h"
 #endif /* __ASSEMBLY__ */
@@ -91,13 +91,8 @@
 #define TTBR0_IRGN0          (1 << 6)               /* Bit 6:  Inner cacheability IRGN[0] (MP extensions) */
                                                     /* Bits 7-n: Reserved, n=7-13 */
 
-#define _TTBR0_LOWER(n)      (0xffffffff << (n))
+#define TTBR0_BASE_SHIFT(n)  (14 - (n)) /* Bits (14-n)-31: Translation table base 0 */
-
+#define TTBR0_BASE_MASK(n)   (0xffffffff << TTBR0_BASE_SHIFT(n))
-/*                                                     Bits (n+1)-31:
- *                                                   Translation table base 0
- */
-
-#define TTBR0_BASE_MASK(n)   (~_TTBR0_LOWER(n))
 
 /* Translation Table Base Register 1 (TTBR1) */
 
@@ -642,6 +637,7 @@
  */
 
 #define PGTABLE_SIZE       0x00004000
+#define ALL_PGTABLE_SIZE   (PGTABLE_SIZE * CONFIG_SMP_NCPUS)
 
 /* Virtual Page Table Location **********************************************/
 
@@ -1340,6 +1336,41 @@ static inline void cp15_wrttb(unsigned int ttb)
     );
 }
 
+/****************************************************************************
+ * Name: mmu_l1_pgtable
+ *
+ * Description:
+ *   Return the value of the L1 page table base address.
+ *   The TTBR0 register contains the phys address for each cpu.
+ *
+ * Input Parameters:
+ *   None
+ *
+ ****************************************************************************/
+
+#ifndef CONFIG_ARCH_ROMPGTABLE
+static inline uint32_t *mmu_l1_pgtable(void)
+{
+#if defined(CONFIG_SMP) && defined(CONFIG_ARCH_ADDRENV)
+  uint32_t ttbr0;
+  uint32_t pgtable;
+
+  __asm__ __volatile__
+    (
+      "\tmrc p15, 0, %0, c2, c0, 0\n"
+      : "=r" (ttbr0)
+      :
+      :
+    );
+
+  pgtable = ttbr0 & TTBR0_BASE_MASK(0);
+  return (uint32_t *)(pgtable - PGTABLE_BASE_PADDR + PGTABLE_BASE_VADDR);
+#else
+  return (uint32_t *)PGTABLE_BASE_VADDR;
+#endif
+}
+#endif
+
 /****************************************************************************
  * Name: mmu_l1_getentry
  *
@@ -1355,7 +1386,7 @@ static inline void cp15_wrttb(unsigned int ttb)
 #ifndef CONFIG_ARCH_ROMPGTABLE
 static inline uint32_t mmu_l1_getentry(uint32_t vaddr)
 {
-  uint32_t *l1table = (uint32_t *)PGTABLE_BASE_VADDR;
+  uint32_t *l1table = mmu_l1_pgtable();
   uint32_t  index   = vaddr >> 20;
 
   /* Return the address of the page table entry */