[PATCH 6/7] ARM: Use TPIDRPRW for current

[Keith Packard]

Store current task pointer in CPU thread ID register TPIDRPRW so that
accessing it doesn't depend on being able to locate thread_info off of
the kernel stack pointer.

Signed-off-by: Keith Packard <keithpac@xxxxxxxxxx>
---
 arch/arm/Kconfig                 |  4 +++
 arch/arm/include/asm/assembler.h |  8 +++++
 arch/arm/include/asm/current.h   | 52 ++++++++++++++++++++++++++++++++
 arch/arm/kernel/entry-armv.S     |  4 +++
 arch/arm/kernel/setup.c          |  1 +
 arch/arm/kernel/smp.c            |  1 +
 6 files changed, 70 insertions(+)
 create mode 100644 arch/arm/include/asm/current.h

diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index 24804f11302d..414fe23fd5ac 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -1172,6 +1172,10 @@ config SMP_ON_UP
 
 	  If you don't know what to do here, say Y.
 
+config CURRENT_POINTER_IN_TPIDRPRW
+	def_bool y
+	depends on (CPU_V6K || CPU_V7) && !CPU_V6
+
 config ARM_CPU_TOPOLOGY
 	bool "Support cpu topology definition"
 	depends on SMP && CPU_V7
diff --git a/arch/arm/include/asm/assembler.h b/arch/arm/include/asm/assembler.h
index e2b1fd558bf3..ea12fe3bb589 100644
--- a/arch/arm/include/asm/assembler.h
+++ b/arch/arm/include/asm/assembler.h
@@ -209,6 +209,14 @@
 	mov	\rd, \rd, lsl #THREAD_SIZE_ORDER + PAGE_SHIFT
 	.endm
 
+/*
+ * Set current task_info
+ * @src: Source register containing task_struct pointer
+ */
+	.macro	set_current src : req
+	mcr	p15, 0, \src, c13, c0, 4
+	.endm
+
 /*
  * Increment/decrement the preempt count.
  */
diff --git a/arch/arm/include/asm/current.h b/arch/arm/include/asm/current.h
new file mode 100644
index 000000000000..153a2ea18747
--- /dev/null
+++ b/arch/arm/include/asm/current.h
@@ -0,0 +1,52 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright © 2021 Keith Packard <keithp@xxxxxxxxxx>
+ */
+
+#ifndef _ASM_ARM_CURRENT_H_
+#define _ASM_ARM_CURRENT_H_
+
+#ifndef __ASSEMBLY__
+
+register unsigned long current_stack_pointer asm ("sp");
+
+/*
+ * Same as asm-generic/current.h, except that we store current
+ * in TPIDRPRW. TPIDRPRW only exists on V6K and V7
+ */
+#ifdef CONFIG_CURRENT_POINTER_IN_TPIDRPRW
+
+struct task_struct;
+
+static inline void set_current(struct task_struct *tsk)
+{
+	/* Set TPIDRPRW */
+	asm volatile("mcr p15, 0, %0, c13, c0, 4" : : "r" (tsk) : "memory");
+}
+
+static __always_inline struct task_struct *get_current(void)
+{
+	struct task_struct *tsk;
+
+	/*
+	 * Read TPIDRPRW.
+	 * 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" (tsk)
+		: "Q" (*(const unsigned long *)current_stack_pointer));
+
+	return tsk;
+}
+#define current get_current()
+#else
+
+#define set_current(tsk) do {} while (0)
+
+#include <asm-generic/current.h>
+
+#endif /* CONFIG_SMP */
+
+#endif /* __ASSEMBLY__ */
+
+#endif /* _ASM_ARM_CURRENT_H_ */
diff --git a/arch/arm/kernel/entry-armv.S b/arch/arm/kernel/entry-armv.S
index 0ea8529a4872..db3947ee9c3e 100644
--- a/arch/arm/kernel/entry-armv.S
+++ b/arch/arm/kernel/entry-armv.S
@@ -761,6 +761,10 @@ ENTRY(__switch_to)
 	ldr	r6, [r2, #TI_CPU_DOMAIN]
 #endif
 	switch_tls r1, r4, r5, r3, r7
+#ifdef CONFIG_CURRENT_POINTER_IN_TPIDRPRW
+	ldr	r7, [r2, #TI_TASK]
+	set_current r7
+#endif
 #if defined(CONFIG_STACKPROTECTOR) && !defined(CONFIG_SMP)
 	ldr	r7, [r2, #TI_TASK]
 	ldr	r8, =__stack_chk_guard
diff --git a/arch/arm/kernel/setup.c b/arch/arm/kernel/setup.c
index d0dc60afe54f..2fdf8c31d6c9 100644
--- a/arch/arm/kernel/setup.c
+++ b/arch/arm/kernel/setup.c
@@ -586,6 +586,7 @@ void __init smp_setup_processor_id(void)
 	u32 mpidr = is_smp() ? read_cpuid_mpidr() & MPIDR_HWID_BITMASK : 0;
 	u32 cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
 
+	set_current(&init_task);
 	cpu_logical_map(0) = cpu;
 	for (i = 1; i < nr_cpu_ids; ++i)
 		cpu_logical_map(i) = i == cpu ? 0 : i;
diff --git a/arch/arm/kernel/smp.c b/arch/arm/kernel/smp.c
index 8ccf10b34f08..09771916442a 100644
--- a/arch/arm/kernel/smp.c
+++ b/arch/arm/kernel/smp.c
@@ -410,6 +410,7 @@ asmlinkage void secondary_start_kernel(unsigned int cpu, struct task_struct *tas
 {
 	struct mm_struct *mm = &init_mm;
 
+	set_current(task);
 	secondary_biglittle_init();
 
 	/*
-- 
2.33.0