@@ -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
  */

@@ -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;
 }

@@ -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

@@ -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