[PATCH v2 13/20] 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  |   39 +++++++++++++++++++++++++++++++++++++--
 2 files changed, 42 insertions(+), 7 deletions(-)

diff --git a/arch/arm/kernel/head.S b/arch/arm/kernel/head.S
index fd8a29e..b54d00e 100644
--- a/arch/arm/kernel/head.S
+++ b/arch/arm/kernel/head.S
@@ -321,6 +321,10 @@ ENTRY(secondary_startup)
 	moveq	r0, #'p'			@ yes, error 'p'
 	beq	__error_p
 
+	pgtbl	r4
+	add	r12, r10, #BSYM(PROCINFO_INITFUNC)
+	blx	r12				@ initialise processor
+						@ (return control reg)
 	/*
 	 * Use the page tables supplied from  __cpu_up.
 	 */
@@ -328,12 +332,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 40b386c..089e2ae 100644
--- a/arch/arm/kernel/smp.c
+++ b/arch/arm/kernel/smp.c
@@ -82,8 +82,10 @@ static inline void identity_mapping_add(pgd_t *pgd, unsigned long start,
 		pmd = pmd_offset(pgd + pgd_index(addr), addr);
 		pmd[0] = __pmd(addr | prot);
 		addr += SECTION_SIZE;
+#ifndef CONFIG_ARM_LPAE
 		pmd[1] = __pmd(addr | prot);
 		addr += SECTION_SIZE;
+#endif
 		flush_pmd_entry(pmd);
 		outer_clean_range(__pa(pmd), __pa(pmd + 1));
 	}
@@ -98,7 +100,9 @@ static inline void identity_mapping_del(pgd_t *pgd, unsigned long start,
 	for (addr = start & PMD_MASK; addr < end; addr += PMD_SIZE) {
 		pmd = pmd_offset(pgd + pgd_index(addr), addr);
 		pmd[0] = __pmd(0);
+#ifndef CONFIG_ARM_LPAE
 		pmd[1] = __pmd(0);
+#endif
 		clean_pmd_entry(pmd);
 		outer_clean_range(__pa(pmd), __pa(pmd + 1));
 	}
@@ -109,6 +113,10 @@ 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;
+#ifdef CONFIG_ARM_LPAE
+	pgd_t *pgd_phys;
+	pmd_t *pmd;
+#endif
 	int ret;
 
 	/*
@@ -137,8 +145,30 @@ int __cpuinit __cpu_up(unsigned int cpu)
 	 * a 1:1 mapping for the physical address of the kernel.
 	 */
 	pgd = pgd_alloc(&init_mm);
-	if (!pgd)
-		return -ENOMEM;
+	if (!pgd) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+#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. The code assumes that the kernel text and data
+	 * sections are within 1GB of PHYS_OFFSET (maximum range convered by
+	 * a PMD table with LPAE).
+	 */
+	pgd_phys = pgd + pgd_index(PHYS_OFFSET);
+	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
 
 	if (PHYS_OFFSET != PAGE_OFFSET) {
 #ifndef CONFIG_HOTPLUG_CPU
@@ -192,8 +222,13 @@ int __cpuinit __cpu_up(unsigned int cpu)
 		identity_mapping_del(pgd, __pa(_sdata), __pa(_edata));
 	}
 
+#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);