arm: traps: emulate a MRCC instruction reading CNTPCT register

A user space application is planned to support feature for
synchronized timestamp among debug packets across peripherals.
As part of the feature, it is responsible for providing physical
timer count value to user space. If memory mapped timer is used
in ARM arch, Usersapce can't read the physical timer count directly
with a MRCC ASM instruction. So Kernel traps the instruction and
returns the physical timer count.

Change-Id: Ia3f0d9c8c06ca9e2204187890c0c57c8640e4f7e
Signed-off-by: Se Wang (Patrick) Oh <sewango@codeaurora.org>

arch/arm/include/asm/traps.h

@@ -46,6 +46,7 @@ static inline int in_exception_text(unsigned long ptr)
 	return in ? : __in_irqentry_text(ptr);
 }
 
+extern void get_pct_hook_init(void);
 extern void __init early_trap_init(void *);
 extern void dump_backtrace_entry(unsigned long where, unsigned long from, unsigned long frame);
 extern void ptrace_break(struct task_struct *tsk, struct pt_regs *regs);

arch/arm/kernel/traps.c

@@ -28,6 +28,7 @@
 #include <linux/bug.h>
 
 #include <linux/atomic.h>
+#include <asm/arch_timer.h>
 #include <asm/cacheflush.h>
 #include <asm/exception.h>
 #include <asm/unistd.h>
@@ -733,6 +734,42 @@ late_initcall(arm_mrc_hook_init);
 
 #endif
 
+static int get_pct_trap(struct pt_regs *regs, unsigned int instr)
+{
+	u64 cntpct;
+	unsigned int res;
+	int rd = (instr >> 12) & 0xF;
+	int rn =  (instr >> 16) & 0xF;
+
+	res = arm_check_condition(instr, regs->ARM_cpsr);
+	if (res == ARM_OPCODE_CONDTEST_FAIL) {
+		regs->ARM_pc += 4;
+		return 0;
+	}
+
+	if (rd == 15 || rn == 15)
+		return 1;
+	cntpct = arch_counter_get_cntpct();
+	regs->uregs[rd] = cntpct;
+	regs->uregs[rn] = cntpct >> 32;
+	regs->ARM_pc += 4;
+	return 0;
+}
+
+static struct undef_hook get_pct_hook = {
+	.instr_mask	= 0x0ff00fff,
+	.instr_val	= 0x0c500f0e,
+	.cpsr_mask	= MODE_MASK,
+	.cpsr_val	= USR_MODE,
+	.fn		= get_pct_trap,
+};
+
+void get_pct_hook_init(void)
+{
+	register_undef_hook(&get_pct_hook);
+}
+EXPORT_SYMBOL(get_pct_hook_init);
+
 void __bad_xchg(volatile void *ptr, int size)
 {
 	printk("xchg: bad data size: pc 0x%p, ptr 0x%p, size %d\n",

arch/arm64/include/asm/traps.h

@@ -40,4 +40,5 @@ static inline int in_exception_text(unsigned long ptr)
 	       ptr < (unsigned long)&__exception_text_end;
 }
 
+static inline void get_pct_hook_init(void) {}
 #endif

drivers/clocksource/arm_arch_timer.c

@@ -23,6 +23,7 @@
 #include <linux/sched_clock.h>
 
 #include <asm/arch_timer.h>
+#include <asm/traps.h>
 #include <asm/virt.h>
 
 #include <clocksource/arm_arch_timer.h>
@@ -790,6 +791,7 @@ static void __init arch_timer_mem_init(struct device_node *np)
 	arch_timer_detect_rate(base, np);
 	arch_timer_mem_register(base, irq);
 	arch_timer_common_init();
+	get_pct_hook_init();
 }
 CLOCKSOURCE_OF_DECLARE(armv7_arch_timer_mem, "arm,armv7-timer-mem",
 		       arch_timer_mem_init);