[RFC PATCH 3/6] ARM: percpu: add SMP_ON_UP support

[Ard Biesheuvel]

Permit the use of the TPIDRPRW system register for carrying the per-CPU
offset in generic SMP configurations that also target non-SMP capable
ARMv6 cores. This uses the SMP_ON_UP code patching framework to turn all
TPIDRPRW accesses into reads/writes of entry #0 in the __per_cpu_offset
array.

While at it, switch over some existing direct TPIDRPRW accesses in asm
code to invocations of a new helper that is patched in the same way when
necessary.

Note that CPU_V6+SMP without SMP_ON_UP results in a kernel that does not
boot on v6 CPUs without SMP extensions, so add this dependency to
Kconfig as well.

Signed-off-by: Ard Biesheuvel <ardb at kernel.org>
---
 arch/arm/include/asm/assembler.h | 21 ++++++++++++++
 arch/arm/include/asm/percpu.h    | 29 ++++++++++++++++++--
 arch/arm/kernel/entry-armv.S     |  4 +--
 arch/arm/mm/Kconfig              |  1 +
 4 files changed, 50 insertions(+), 5 deletions(-)

diff --git a/arch/arm/include/asm/assembler.h b/arch/arm/include/asm/assembler.h
index 1b9d4df331aa..c4c1d5b2edf5 100644
--- a/arch/arm/include/asm/assembler.h
+++ b/arch/arm/include/asm/assembler.h
@@ -312,6 +312,27 @@ THUMB(	fpreg	.req	r7	)
 #define ALT_UP_B(label) b label
 #endif
 
+	/*
+	 * this_cpu_offset - load the per-CPU offset of this CPU into
+	 * 		     register 'rd'
+	 */
+	.macro	this_cpu_offset, rd:req
+#ifdef CONFIG_SMP
+ALT_SMP(mrc p15, 0, \rd, c13, c0, 4)
+#ifdef CONFIG_CPU_V6
+ALT_UP_B(.L1_\@)
+.L0_\@:
+	.subsection 1
+.L1_\@: ldr	\rd, =__per_cpu_offset
+	ldr	\rd, [\rd]
+	b	.L0_\@
+	.previous
+#endif
+#else
+	mov	\rd, #0
+#endif
+	.endm
+
 /*
  * Instruction barrier
  */
diff --git a/arch/arm/include/asm/percpu.h b/arch/arm/include/asm/percpu.h
index e2fcb3cfd3de..7b984352e402 100644
--- a/arch/arm/include/asm/percpu.h
+++ b/arch/arm/include/asm/percpu.h
@@ -5,15 +5,25 @@
 #ifndef _ASM_ARM_PERCPU_H_
 #define _ASM_ARM_PERCPU_H_
 
+#include <linux/threads.h>
+
 register unsigned long current_stack_pointer asm ("sp");
 
 /*
  * Same as asm-generic/percpu.h, except that we store the per cpu offset
  * in the TPIDRPRW. TPIDRPRW only exists on V6K and V7
  */
-#if defined(CONFIG_SMP) && !defined(CONFIG_CPU_V6)
+#ifdef CONFIG_SMP
+extern unsigned long __per_cpu_offset[NR_CPUS];
+extern unsigned int smp_on_up;
+
 static inline void set_my_cpu_offset(unsigned long off)
 {
+	if (IS_ENABLED(CONFIG_CPU_V6) && !smp_on_up) {
+		__per_cpu_offset[0] = off;
+		return;
+	}
+
 	/* Set TPIDRPRW */
 	asm volatile("mcr p15, 0, %0, c13, c0, 4" : : "r" (off) : "memory");
 }
@@ -27,8 +37,21 @@ static inline unsigned long __my_cpu_offset(void)
 	 * We want to allow caching the value, so avoid using volatile and
 	 * instead use a fake stack read to hazard against barrier().
 	 */
-	asm("mrc p15, 0, %0, c13, c0, 4" : "=r" (off)
-		: "Q" (*(const unsigned long *)current_stack_pointer));
+	asm("0:	mrc p15, 0, %0, c13, c0, 4			\n\t"
+#ifdef CONFIG_CPU_V6
+	    "1:							\n\t"
+	    "	.subsection 1					\n\t"
+	    "2: ldr	%0, =__per_cpu_offset			\n\t"
+	    "	ldr	%0, [%0]				\n\t"
+	    "	b	1b					\n\t"
+	    "	.previous					\n\t"
+	    "	.pushsection \".alt.smp.init\", \"a\"		\n\t"
+	    "	.long	0b - .					\n\t"
+	    "	b	. + (2b - 0b)				\n\t"
+	    "	.popsection					\n\t"
+#endif
+	     : "=r" (off)
+	     : "Q" (*(const unsigned long *)current_stack_pointer));
 
 	return off;
 }
diff --git a/arch/arm/kernel/entry-armv.S b/arch/arm/kernel/entry-armv.S
index 1e26d69ebbf1..09a9fe501094 100644
--- a/arch/arm/kernel/entry-armv.S
+++ b/arch/arm/kernel/entry-armv.S
@@ -41,7 +41,7 @@
 	mov	r0, sp
 #ifdef CONFIG_IRQSTACKS
 	mov_l	r2, irq_stack_ptr	@ Take base address
-	mrc	p15, 0, r3, c13, c0, 4	@ Get CPU offset
+	this_cpu_offset r3
 #ifdef CONFIG_UNWINDER_ARM
 	mov	fpreg, sp		@ Preserve original SP
 #else
@@ -884,7 +884,7 @@ __bad_stack:
 THUMB(	bx	pc		)
 THUMB(	nop			)
 THUMB(	.arm			)
-	mrc	p15, 0, ip, c13, c0, 4		@ Get per-CPU offset
+	this_cpu_offset ip
 
 	.globl	overflow_stack_ptr
 	.reloc	0f, R_ARM_ALU_PC_G0_NC, overflow_stack_ptr
diff --git a/arch/arm/mm/Kconfig b/arch/arm/mm/Kconfig
index 58afba346729..a91ff22c6c2e 100644
--- a/arch/arm/mm/Kconfig
+++ b/arch/arm/mm/Kconfig
@@ -386,6 +386,7 @@ config CPU_V6
 	select CPU_PABRT_V6
 	select CPU_THUMB_CAPABLE
 	select CPU_TLB_V6 if MMU
+	select SMP_ON_UP if SMP
 
 # ARMv6k
 config CPU_V6K
-- 
2.30.2