arm64: mm: update CONTEXTIDR register to contain PID of current process

This patch is a port of 575320d62 ("ARM: 7445/1: mm: update CONTEXTIDR
register to contain PID of current process") from ARM that introduces a
new Kconfig option which, when enabled, causes the kernel to write the
PID of the current task into the CONTEXTIDR register on context switch.
This is useful when analysing hardware trace, since writes to this
register can be configured to emit an event into the trace stream.

Signed-off-by: Will Deacon <will.deacon@arm.com>
[catalin.marinas@arm.com: contextidr_thread_switch() moved to mmu_context.h]
Signed-off-by: Catalin Marinas <catalin.marinas@arm.com>
diff --git a/arch/arm64/Kconfig.debug b/arch/arm64/Kconfig.debug
index 79871cd..5149343 100644
--- a/arch/arm64/Kconfig.debug
+++ b/arch/arm64/Kconfig.debug
@@ -33,4 +33,12 @@
 	  is assumed that the early console device has been initialised
 	  by the boot loader prior to starting the Linux kernel.
 
+config PID_IN_CONTEXTIDR
+	bool "Write the current PID to the CONTEXTIDR register"
+	help
+	  Enabling this option causes the kernel to write the current PID to
+	  the CONTEXTIDR register, at the expense of some additional
+	  instructions during context switch. Say Y here only if you are
+	  planning to use hardware trace tools with this kernel.
+
 endmenu
diff --git a/arch/arm64/include/asm/mmu_context.h b/arch/arm64/include/asm/mmu_context.h
index f68465d..e2bc385 100644
--- a/arch/arm64/include/asm/mmu_context.h
+++ b/arch/arm64/include/asm/mmu_context.h
@@ -35,6 +35,21 @@
 void __init_new_context(struct task_struct *tsk, struct mm_struct *mm);
 void __new_context(struct mm_struct *mm);
 
+#ifdef CONFIG_PID_IN_CONTEXTIDR
+static inline void contextidr_thread_switch(struct task_struct *next)
+{
+	asm(
+	"	msr	contextidr_el1, %0\n"
+	"	isb"
+	:
+	: "r" (task_pid_nr(next)));
+}
+#else
+static inline void contextidr_thread_switch(struct task_struct *next)
+{
+}
+#endif
+
 /*
  * Set TTBR0 to empty_zero_page. No translations will be possible via TTBR0.
  */
diff --git a/arch/arm64/kernel/process.c b/arch/arm64/kernel/process.c
index cb0956b..a8fbd7e 100644
--- a/arch/arm64/kernel/process.c
+++ b/arch/arm64/kernel/process.c
@@ -45,9 +45,10 @@
 
 #include <asm/compat.h>
 #include <asm/cacheflush.h>
+#include <asm/fpsimd.h>
+#include <asm/mmu_context.h>
 #include <asm/processor.h>
 #include <asm/stacktrace.h>
-#include <asm/fpsimd.h>
 
 static void setup_restart(void)
 {
@@ -319,6 +320,7 @@
 	/* the actual thread switch */
 	last = cpu_switch_to(prev, next);
 
+	contextidr_thread_switch(next);
 	return last;
 }