ARM: 7502/1: contextidr: avoid using bfi instruction during notifier

[firefly-linux-kernel-4.4.55.git] / arch / arm / mm / context.c

diff --git a/arch/arm/mm/context.c b/arch/arm/mm/context.c
index 806cc4f63516fd4bfddecad4ec93be51a830f49a..4e07eec1270dd3b3fa51860fbc735a5d1fb9e18b 100644 (file)
--- a/arch/arm/mm/context.c
+++ b/arch/arm/mm/context.c
@@ -14,6 +14,7 @@
 #include <linux/percpu.h>
 
 #include <asm/mmu_context.h>
+#include <asm/thread_notify.h>
 #include <asm/tlbflush.h>
 
 static DEFINE_RAW_SPINLOCK(cpu_asid_lock);
@@ -48,6 +49,41 @@ void cpu_set_reserved_ttbr0(void)
 }
 #endif
 
+#ifdef CONFIG_PID_IN_CONTEXTIDR
+static int contextidr_notifier(struct notifier_block *unused, unsigned long cmd,
+                              void *t)
+{
+       u32 contextidr;
+       pid_t pid;
+       struct thread_info *thread = t;
+
+       if (cmd != THREAD_NOTIFY_SWITCH)
+               return NOTIFY_DONE;
+
+       pid = task_pid_nr(thread->task) << ASID_BITS;
+       asm volatile(
+       "       mrc     p15, 0, %0, c13, c0, 1\n"
+       "       and     %0, %0, %2\n"
+       "       orr     %0, %0, %1\n"
+       "       mcr     p15, 0, %0, c13, c0, 1\n"
+       : "=r" (contextidr), "+r" (pid)
+       : "I" (~ASID_MASK));
+       isb();
+
+       return NOTIFY_OK;
+}
+
+static struct notifier_block contextidr_notifier_block = {
+       .notifier_call = contextidr_notifier,
+};
+
+static int __init contextidr_notifier_init(void)
+{
+       return thread_register_notifier(&contextidr_notifier_block);
+}
+arch_initcall(contextidr_notifier_init);
+#endif
+
 /*
  * We fork()ed a process, and we need a new context for the child
  * to run in.