3 files changed, 26 insertions, 1 deletions

diff --git a/arch/arm64/Kconfig.debug b/arch/arm64/Kconfig.debug
index 79871cd78da8..51493430f142 100644
--- a/arch/arm64/Kconfig.debug
+++ b/arch/arm64/Kconfig.debug
@@ -33,4 +33,12 @@ config EARLY_PRINTK
           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 f68465dee026..e2bc385adb6b 100644
--- a/arch/arm64/include/asm/mmu_context.h
+++ b/arch/arm64/include/asm/mmu_context.h
@@ -35,6 +35,21 @@ extern unsigned int cpu_last_asid;
 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 cb0956bc96ed..a8fbd7eaa2ed 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 @@ struct task_struct *__switch_to(struct task_struct *prev,
         /* the actual thread switch */
         last = cpu_switch_to(prev, next);
 
+        contextidr_thread_switch(next);
         return last;
 }