[RFC PATCH 11/18] ARM: LPAE: Add SMP support for the 3-level page table format

[Catalin Marinas <catalin.marinas@arm.com>]

With 3-level page tables, starting secondary CPUs required allocating
the pgd as well. Since LPAE Linux uses TTBR1 for the kernel page tables,
this patch reorders the CPU setup call in the head.S file so that the
swapper_pg_dir is used. TTBR0 is set to the value generated by the
primary CPU.

Signed-off-by: Catalin Marinas <catalin.marinas@arm.com>
---
 arch/arm/kernel/head.S |   10 +++++-----
 arch/arm/kernel/smp.c  |   34 +++++++++++++++++++++++++++++++---
 2 files changed, 36 insertions(+), 8 deletions(-)

diff --git a/arch/arm/kernel/head.S b/arch/arm/kernel/head.S
index eb59839..ff0aff9 100644
--- a/arch/arm/kernel/head.S
+++ b/arch/arm/kernel/head.S
@@ -126,6 +126,10 @@ ENTRY(secondary_startup)
 	moveq	r0, #'p'			@ yes, error 'p'
 	beq	__error
 
+	pgtbl	r4
+	add	r12, r10, #BSYM(PROCINFO_INITFUNC)
+	blx	r12				@ initialise processor
+						@ (return control reg)
 	/*
 	 * Use the page tables supplied from  __cpu_up.
 	 */
@@ -133,12 +137,8 @@ ENTRY(secondary_startup)
 	ldmia	r4, {r5, r7, r12}		@ address to jump to after
 	sub	r4, r4, r5			@ mmu has been enabled
 	ldr	r4, [r7, r4]			@ get secondary_data.pgdir
-	adr	lr, BSYM(__enable_mmu)		@ return address
 	mov	r13, r12			@ __secondary_switched address
- ARM(	add	pc, r10, #PROCINFO_INITFUNC	) @ initialise processor
-						  @ (return control reg)
- THUMB(	add	r12, r10, #PROCINFO_INITFUNC	)
- THUMB(	mov	pc, r12				)
+	b	__enable_mmu
 ENDPROC(secondary_startup)
 
 	/*
diff --git a/arch/arm/kernel/smp.c b/arch/arm/kernel/smp.c
index fde5497..2189ca6 100644
--- a/arch/arm/kernel/smp.c
+++ b/arch/arm/kernel/smp.c
@@ -71,7 +71,7 @@ int __cpuinit __cpu_up(unsigned int cpu)
 {
 	struct cpuinfo_arm *ci = &per_cpu(cpu_data, cpu);
 	struct task_struct *idle = ci->idle;
-	pgd_t *pgd;
+	pgd_t *pgd, *pgd_phys;
 	pmd_t *pmd;
 	int ret;
 
@@ -101,9 +101,32 @@ int __cpuinit __cpu_up(unsigned int cpu)
 	 * a 1:1 mapping for the physical address of the kernel.
 	 */
 	pgd = pgd_alloc(&init_mm);
-	pmd = pmd_offset(pgd + pgd_index(PHYS_OFFSET), PHYS_OFFSET);
+	if (!pgd) {
+		ret = -ENOMEM;
+		goto out;
+	}
+	pgd_phys = pgd + pgd_index(PHYS_OFFSET);
+#ifdef CONFIG_ARM_LPAE
+	/*
+	 * Check for overlapping between PHYS_OFFSET and PAGE_OFFSET and
+	 * duplicate the pmd to avoid overriding valid kernel mappings in the
+	 * init_mm page tables.
+	 */
+	pmd = pmd_alloc_one(NULL, PHYS_OFFSET);
+	if (!pmd) {
+		ret = -ENOMEM;
+		goto nopmd;
+	}
+	if (pgd_present(*pgd_phys))
+		memcpy(pmd, pmd_offset(pgd_phys, 0),
+		       PTRS_PER_PMD * sizeof(pmd_t));
+	pgd_populate(NULL, pgd_phys, pmd);
+#endif
+	pmd = pmd_offset(pgd_phys, PHYS_OFFSET);
 	*pmd = __pmd((PHYS_OFFSET & PMD_MASK) |
-		     PMD_TYPE_SECT | PMD_SECT_AP_WRITE);
+		     PMD_TYPE_SECT | PMD_SECT_AP_WRITE |
+		     PMD_SECT_AP_READ | PMD_SECT_AF |
+		     PMD_SECT_WBWA | PMD_SECT_S);
 	flush_pmd_entry(pmd);
 	outer_clean_range(__pa(pmd), __pa(pmd + 1));
 
@@ -145,8 +168,13 @@ int __cpuinit __cpu_up(unsigned int cpu)
 
 	*pmd = __pmd(0);
 	clean_pmd_entry(pmd);
+#ifdef CONFIG_ARM_LPAE
+	pmd_free(&init_mm, pmd_offset(pgd_phys, 0));
+nopmd:
+#endif
 	pgd_free(&init_mm, pgd);
 
+out:
 	if (ret) {
 		printk(KERN_CRIT "CPU%u: processor failed to boot\n", cpu);