Great cedrus MMIO tracing that works

Signed-off-by: Paul Kocialkowski <paul.kocialkowski@bootlin.com>

1 files changed, 247 insertions, 0 deletions

diff --git a/arch/arm/mm/fault.c b/arch/arm/mm/fault.c
index b0db85310331..34a9410e40bb 100644
--- a/arch/arm/mm/fault.c
+++ b/arch/arm/mm/fault.c
@@ -23,6 +23,8 @@
 #include <asm/system_info.h>
 #include <asm/tlbflush.h>
 
+#include "../probes/decode-arm.h"
+#include "../probes/decode-thumb.h"
 #include "fault.h"
 
 #ifdef CONFIG_MMU
@@ -147,6 +149,8 @@ __do_kernel_fault(struct mm_struct *mm, unsigned long addr, unsigned int fsr,
 	if (fixup_exception(regs))
 		return;
 
+	printk(KERN_ERR "fault: kernel-side\n");
+
 	/*
 	 * No handler, we'll have to terminate things with extreme prejudice.
 	 */
@@ -173,6 +177,8 @@ __do_user_fault(unsigned long addr, unsigned int fsr, unsigned int sig,
 {
 	struct task_struct *tsk = current;
 
+	printk(KERN_ERR "fault: user-side at %#x, current %#x\n", addr, current);
+
 	if (addr > TASK_SIZE)
 		harden_branch_predictor();
 
@@ -231,6 +237,245 @@ static inline bool is_permission_fault(unsigned int fsr)
 	return false;
 }
 
+static unsigned long cedar_vma_real_start;
+static unsigned long cedar_vma_dummy_start;
+static struct pt_regs *cedar_regs;
+
+static void cedar_ldrdstrd(probes_opcode_t insn, struct arch_probes_insn *asi,
+			   struct pt_regs *regs)
+{
+	printk(KERN_ERR "fault: ldrdstrd\n");
+}
+
+static void cedar_ldr(probes_opcode_t insn, struct arch_probes_insn *asi,
+		      struct pt_regs *regs)
+{
+	printk(KERN_ERR "fault: ldr\n");
+}
+
+static void cedar_str(probes_opcode_t insn, struct arch_probes_insn *asi,
+		      struct pt_regs *regs)
+{
+	printk(KERN_ERR "fault: ldr\n");
+}
+
+static const union decode_action cedar_arm_actions[NUM_PROBES_ARM_ACTIONS] = {
+	[PROBES_LDRSTRD] = {.handler = cedar_ldrdstrd},
+	[PROBES_LOAD_EXTRA] = {.handler = cedar_ldr},
+	[PROBES_LOAD] = {.handler = cedar_ldr},
+	[PROBES_STORE_EXTRA] = {.handler = cedar_str},
+	[PROBES_STORE] = {.handler = cedar_str},
+};
+
+unsigned long cedar_reg_read(unsigned long offset);
+void cedar_reg_write(unsigned long offset, unsigned long value);
+
+static unsigned long cedar_addr_correct(unsigned long addr)
+{
+	long addr_diff = (long)cedar_vma_real_start - (long)cedar_vma_dummy_start;
+	unsigned long addr_correct = ((long)addr + addr_diff);
+
+	return addr_correct;
+}
+
+static unsigned long cedar_addr_offset(unsigned long addr)
+{
+	return addr - cedar_vma_dummy_start;
+}
+
+static void cedar_load_im(struct pt_regs *regs, int rt, int rn, int im)
+{
+	unsigned long addr = cedar_addr_correct(regs->uregs[rn]) + im;
+	unsigned long offset = cedar_addr_offset(regs->uregs[rn]) + im;
+	unsigned long value;
+
+	// copy_from_user(&value, (unsigned long *)addr, sizeof(value));
+	value = cedar_reg_read(offset);
+	regs->uregs[rt] = value;
+
+	printk(KERN_ERR ">>>> cedar-read [%#04x] %#08x\n", offset, value);
+}
+
+static void cedar_store_im(struct pt_regs *regs, int rt, int rn, int im)
+{
+	unsigned long addr = cedar_addr_correct(regs->uregs[rn]) + im;
+	unsigned long offset = cedar_addr_offset(regs->uregs[rn]) + im;
+	unsigned long value;
+
+	value = regs->uregs[rt];
+//	copy_to_user((unsigned long *)addr, &value, sizeof(value));
+
+/*
+	if (offset == 0x0a14) {
+		printk(KERN_ERR "register value overwrite\n");
+		value = 0;
+	}
+*/
+
+	cedar_reg_write(offset, value);
+
+	printk(KERN_ERR ">>>> cedar-write [%#04x] %#08x\n", offset, value);
+
+	// copy_from_user(&value, (unsigned long *)addr, sizeof(value));
+//	value = cedar_reg_read(offset);
+//	printk(KERN_ERR ">>>> cedar-write-readback [%#04x] %#08x\n", offset, value);
+}
+
+/* called as decoder in case of !emulate */
+static void cedar_t32_ldrstr(probes_opcode_t insn, struct arch_probes_insn *asi,
+			     struct pt_regs *entry)
+{
+	int ldr_flag = insn & BIT(20); // 1: ldr, 0: str
+	int im_flag = insn & BIT(23); // 1: immediate, 0: register
+	int rt = (insn >> 12) & 0xf;
+	int rn = (insn >> 16) & 0xf;
+	int rm = insn & 0xf;
+	int im = insn & 0xfff;
+	char *opcode = ldr_flag ? "ldr" : "str";
+
+
+	if (im_flag) {
+//		printk(KERN_ERR "fault-decode: %s r%u [r%u, %#x]\n", opcode, rt, rn, im);
+//		printk(KERN_ERR "fault-decode: *r%u=%#x, *r%u=%#x\n", rt, cedar_regs->uregs[rt], rn, cedar_regs->uregs[rn]);
+
+		if (ldr_flag)
+			cedar_load_im(cedar_regs, rt, rn, im);
+		else
+			cedar_store_im(cedar_regs, rt, rn, im);
+
+		cedar_regs->ARM_pc += 4;
+	} else {
+//		printk(KERN_ERR "fault-decode: %s r%u [r%u, r%u]\n", opcode, rt, rn, rm);
+//		printk(KERN_ERR "fault-decode: *r%u=%#x, *r%u=%#x, *r%u=%#x\n", rt, cedar_regs->uregs[rt], rn, cedar_regs->uregs[rn], rm, cedar_regs->uregs[rm]);
+
+		printk(KERN_ERR "XXXX TODO XXXX\n");
+	}
+}
+
+static void cedar_t32_ldr_lit(probes_opcode_t insn, struct arch_probes_insn *asi,
+			     struct pt_regs *regs)
+{
+	printk(KERN_ERR "fault: ldr_lit\n");
+}
+
+static const union decode_action cedar_t32_actions[NUM_PROBES_T32_ACTIONS] = {
+	[PROBES_T32_LDRSTR] = { .handler = cedar_t32_ldrstr },
+	[PROBES_T32_LDR_LIT] = { .handler = cedar_t32_ldr_lit },
+};
+
+static void cedar_t16_hiregops(probes_opcode_t insn, struct arch_probes_insn *asi,
+		      struct pt_regs *regs)
+{
+	int rdn = (insn & 0x7) | ((insn & 0x80) >> 4);
+	int rm = (insn >> 3) & 0xf;
+	unsigned long addr_base;
+	unsigned long opcode_mask = (insn & 0xff00);
+	long addr_diff;
+	char *opcode;
+
+	printk(KERN_ERR "fault: hiregops at %#x\n", regs->uregs[rm]);
+
+	if (opcode_mask == 0x4400)
+		opcode = "add";
+	else if (opcode_mask == 0x4500)
+		opcode = "cmp";
+	else if (opcode_mask == 0x4600)
+		opcode = "mov";
+	else
+		opcode = "unk";
+
+	printk(KERN_ERR "fault-decode: %s r%u, r%u\n", opcode, rdn, rm);
+	printk(KERN_ERR "fault-decode: *r%u=%#x, *r%u=%#x\n", rdn, cedar_regs->uregs[rdn], rm, cedar_regs->uregs[rm]);
+
+/*
+	addr_diff = (long)cedar_vma_real_start - (long)cedar_vma_dummy_start;
+	addr_base = ((long)regs->uregs[rm] + addr_diff);
+
+	regs->uregs[rm] = addr_base;
+*/
+}
+
+static void cedar_t16_ldrhstrh(probes_opcode_t insn, struct arch_probes_insn *asi,
+		      struct pt_regs *entry)
+{
+	int ldr_flag = insn & BIT(11); // 1: ldr, 0: str
+	int im_flag = (insn & 0xf000) != 0x5000;
+	char *opcode = ldr_flag ? "ldr" : "str";
+
+	if ((insn & 0xf000) == 0x6000) {
+		int rt = insn & 0x7;
+		int rn = (insn >> 3) & 0x7;
+		int im = ((insn >> 6) & 0x1f) << 2;
+
+//		printk(KERN_ERR "fault-decode: %s r%u [r%u, %#x]\n", opcode, rt, rn, im);
+//		printk(KERN_ERR "fault-decode: *r%u=%#x, *r%u=%#x\n", rt, cedar_regs->uregs[rt], rn, cedar_regs->uregs[rn]);
+
+		if (ldr_flag)
+			cedar_load_im(cedar_regs, rt, rn, im);
+		else
+			cedar_store_im(cedar_regs, rt, rn, im);
+
+		cedar_regs->ARM_pc += 2;
+	} else {
+		printk(KERN_ERR "XXXX TODO XXXX\n");
+	}
+
+}
+
+static const union decode_action cedar_t16_actions[NUM_PROBES_T16_ACTIONS] = {
+//	[PROBES_T16_HIREGOPS] = { .handler = cedar_t16_hiregops },
+	[PROBES_T16_LDRHSTRH] = { .handler = cedar_t16_ldrhstrh },
+};
+
+
+static int cedar_page_fault(struct mm_struct *mm, unsigned long addr,
+			    struct pt_regs *regs)
+{
+	struct vm_area_struct *vma = find_vma(mm, addr);
+	struct arch_probes_insn asi = { 0 };
+	unsigned long pc_addr = regs->ARM_pc;
+	unsigned long insn, insn_orig;
+	int ret;
+
+	if (!vma->vm_private_data || ((unsigned long *)vma->vm_private_data)[0] != 0xb0cad0)
+		return 0;
+
+	cedar_vma_dummy_start = vma->vm_start;
+	cedar_vma_real_start = ((unsigned long *)vma->vm_private_data)[1];
+	cedar_regs = regs;
+
+//	printk(KERN_ERR "fault: addr %#x pc %#x\n", addr, pc_addr);
+
+	copy_from_user_nofault(&insn_orig, (unsigned long *)pc_addr, sizeof(insn_orig));
+
+	/* No emulate, no checkers */
+
+	if (regs->ARM_cpsr & PSR_T_BIT) {
+//		printk(KERN_ERR "fault: user in thumb mode\n");
+		u16 insn1 = __mem_to_opcode_thumb16(((u16 *)&insn_orig)[0]);
+		if (is_wide_instruction(insn1)) {
+			u16 insn2 = __mem_to_opcode_thumb16(((u16 *)&insn_orig)[1]);
+			insn = __opcode_thumb32_compose(insn1, insn2);
+//			printk(KERN_ERR "thumb32 instruction %#x\n", insn);
+
+			ret = thumb32_probes_decode_insn(insn, &asi, false, cedar_t32_actions, NULL);
+//			printk(KERN_ERR "thumb32 decode ret %d\n", ret);
+		} else {
+			insn = insn1;
+//			printk(KERN_ERR "thumb16 instruction %#x\n", insn);
+			ret = thumb16_probes_decode_insn(insn, &asi, false, cedar_t16_actions, NULL);
+//			printk(KERN_ERR "thumb16 decode ret %d\n", ret);
+		}
+	} else {
+//		printk(KERN_ERR "fault: user in arm mode\n");
+		insn = __mem_to_opcode_arm(insn_orig);
+		ret = arm_probes_decode_insn(insn, &asi, false, cedar_arm_actions, NULL);
+//		printk(KERN_ERR "arm decode ret %d\n", ret);
+	}
+
+	return 1;
+}
+
 static int __kprobes
 do_page_fault(unsigned long addr, unsigned int fsr, struct pt_regs *regs)
 {
@@ -244,6 +489,8 @@ do_page_fault(unsigned long addr, unsigned int fsr, struct pt_regs *regs)
 	if (kprobe_page_fault(regs, fsr))
 		return 0;
 
+	if (cedar_page_fault(mm, addr, regs))
+		return 0;
 
 	/* Enable interrupts if they were enabled in the parent context. */
 	if (interrupts_enabled(regs))