Great cedrus MMIO tracing that works

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

4 files changed, 362 insertions, 9 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))
diff --git a/arch/arm/probes/Makefile b/arch/arm/probes/Makefile
index 8b0ea5ace100..fffdf62a5d54 100644
--- a/arch/arm/probes/Makefile
+++ b/arch/arm/probes/Makefile
@@ -1,5 +1,5 @@
 # SPDX-License-Identifier: GPL-2.0
-obj-$(CONFIG_UPROBES)		+= decode.o decode-arm.o uprobes/
+obj-$(CONFIG_UPROBES)		+= decode.o decode-arm.o decode-thumb.o uprobes/
 obj-$(CONFIG_KPROBES)		+= decode.o kprobes/
 ifdef CONFIG_THUMB2_KERNEL
 obj-$(CONFIG_KPROBES)		+= decode-thumb.o
diff --git a/arch/arm/probes/decode.c b/arch/arm/probes/decode.c
index c84053a81358..4464ff89f9a8 100644
--- a/arch/arm/probes/decode.c
+++ b/arch/arm/probes/decode.c
@@ -327,6 +327,7 @@ static bool __kprobes decode_regs(probes_opcode_t *pinsn, u32 regs, bool modify)
 	return true;
 
 reject:
+	printk(KERN_ERR "decode_regs: reject\n");
 	return false;
 }
 
@@ -455,8 +456,12 @@ probes_decode_insn(probes_opcode_t insn, struct arch_probes_insn *asi,
 		if (!matched && (insn & h->mask.bits) != h->value.bits)
 			continue;
 
-		if (!decode_regs(&insn, regs, emulate))
+//		printk(KERN_ERR "probes_decode_insn: matched instruction, type %x\n", type);
+
+		if (!decode_regs(&insn, regs, emulate)) {
+			printk(KERN_ERR "probes_decode_insn: decode regs failed\n");
 			return INSN_REJECTED;
+		}
 
 		switch (type) {
 
@@ -498,6 +503,9 @@ probes_decode_insn(probes_opcode_t insn, struct arch_probes_insn *asi,
 			if (err == INSN_REJECTED)
 				return INSN_REJECTED;
 
+//			printk(KERN_ERR "decode-emulate: action %#x\n", action);
+
+			/* decoder and handler are unionized */
 			if (!emulate)
 				return actions[action].decoder(insn, asi, h);
 
diff --git a/drivers/staging/media/sunxi/cedar/ve/cedar_ve.c b/drivers/staging/media/sunxi/cedar/ve/cedar_ve.c
index 16a20da05a1d..7dbd49929562 100644
--- a/drivers/staging/media/sunxi/cedar/ve/cedar_ve.c
+++ b/drivers/staging/media/sunxi/cedar/ve/cedar_ve.c
@@ -1061,10 +1061,16 @@ static long compat_cedardev_ioctl(struct file *filp, unsigned int cmd,
 	return ret;
 }
 
+static void cedardev_vma_prepare(void);
+
 static int cedardev_open(struct inode *inode, struct file *filp)
 {
 	struct ve_info *info;
 
+	printk(KERN_ERR "%s()\n", __func__);
+
+	cedardev_vma_prepare();
+
 	info = kmalloc(sizeof(struct ve_info), GFP_KERNEL);
 	if (!info)
 		return -ENOMEM;
@@ -1092,12 +1098,17 @@ static int cedardev_open(struct inode *inode, struct file *filp)
 	return 0;
 }
 
+static void cedardev_vma_unprepare(void);
+
 static int cedardev_release(struct inode *inode, struct file *filp)
 {
 	struct ve_info *info;
 
 	info = filp->private_data;
 
+	printk(KERN_ERR "%s()\n", __func__);
+	cedardev_vma_unprepare();
+
 	mutex_lock(&info->lock_flag_io);
 	/* lock status */
 	if (info->lock_flags) {
@@ -1190,9 +1201,94 @@ static int snd_sw_cedar_resume(struct platform_device *pdev)
 }
 #endif
 
-static int cedardev_mmap(struct file *filp, struct vm_area_struct *vma)
+static unsigned long cedar_vma_info[2] = { 0 };
+struct vm_area_struct *cedar_vma_real = NULL;
+
+static void *cedar_kernel_map;
+
+unsigned long cedar_reg_read(unsigned long offset)
+{
+	return readl(cedar_kernel_map + offset);
+}
+EXPORT_SYMBOL(cedar_reg_read);
+
+void cedar_reg_write(unsigned long offset, unsigned long value)
 {
+	writel(value, cedar_kernel_map + offset);
+}
+EXPORT_SYMBOL(cedar_reg_write);
+
+static void cedardev_vma_prepare(void)
+{
+	struct vm_area_struct *vma_real;
+	unsigned long addr_real;
 	unsigned long temp_pfn;
+	struct vm_unmapped_area_info info;
+	unsigned long size = 0x1000;
+
+	printk(KERN_ERR "%s()\n", __func__);
+
+/*
+	if (cedar_vma_info[1] != 0) {
+		printk(KERN_ERR "cedar: remove previous mapping\n");
+		vm_munmap(cedar_vma_info[1], size);
+	}
+*/
+
+	info.flags = 0;
+	info.length = size;
+	info.low_limit = get_current()->mm->mmap_base;
+	info.high_limit = arch_get_mmap_end(0, size, 0);
+	info.align_mask = 0;
+	info.align_offset = 0;
+
+	addr_real = vm_unmapped_area(&info);
+//	addr_real = get_unmapped_area(filp, vma->vm_end + size, size, 0, 0);
+
+	printk(KERN_ERR "cedar: real mapping at %#x\n", addr_real);
+
+	vma_real = vm_area_alloc(get_current()->mm);
+
+	vma_real->vm_flags = 0x40fb | VM_IO;
+	vma_real->vm_page_prot = pgprot_noncached(0x703); //pgprot_noncached(vma_real->vm_page_prot);
+	vma_real->vm_start = addr_real;
+	vma_real->vm_end = addr_real + size;
+	vma_real->vm_pgoff = 0;
+	vma_real->vm_ops = &cedardev_remap_vm_ops;
+
+	temp_pfn = cedar_devp->phy_addr >> 12; /* Shift by page size */
+
+	if (io_remap_pfn_range(vma_real, vma_real->vm_start, temp_pfn,
+			       size,
+			       vma_real->vm_page_prot)) {
+		printk(KERN_ERR "cedar: real mapping failed!\n");
+		return;
+	}
+
+	printk(KERN_ERR "cedar: real area at %#x size %#x\n", vma_real->vm_start, vma_real->vm_end - vma_real->vm_start);
+
+	cedar_vma_info[0] = 0xb0cad0;
+	cedar_vma_info[1] = vma_real->vm_start;
+
+
+	cedar_vma_real = vma_real;
+}
+
+static void cedardev_vma_unprepare(void)
+{
+	unsigned long size = 0x1000;
+
+	printk(KERN_ERR "%s()\n", __func__);
+
+	if (cedar_vma_info[1] != 0) {
+		printk(KERN_ERR "cedar: remove previous mapping\n");
+		//vm_munmap(cedar_vma_info[1], size);
+		zap_page_range(cedar_vma_real, cedar_vma_info[1], size);
+	}
+}
+
+static int cedardev_mmap(struct file *filp, struct vm_area_struct *vma)
+{
 
 	if (vma->vm_end - vma->vm_start == 0) {
 		dev_warn(cedar_devp->platform_dev,
@@ -1207,18 +1303,18 @@ static int cedardev_mmap(struct file *filp, struct vm_area_struct *vma)
 		return -EINVAL;
 	}
 
-	temp_pfn = cedar_devp->phy_addr >> 12;
+	printk(KERN_ERR "%s() current %#x\n", __func__, current);
 
 	/* Set reserved and I/O flag for the area. */
 	vma->vm_flags |= /*VM_RESERVED | */ VM_IO;
 	/* Select uncached access. */
 	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+	printk(KERN_ERR "cedar: prot %x flags %#x\n", vma->vm_page_prot, vma->vm_flags);
+	printk(KERN_ERR "cedar: dummy area at %#x size %#x\n", vma->vm_start, vma->vm_end - vma->vm_start);
 
-	if (io_remap_pfn_range(vma, vma->vm_start, temp_pfn,
-			       vma->vm_end - vma->vm_start,
-			       vma->vm_page_prot)) {
-		return -EAGAIN;
-	}
+//	printk(KERN_ERR "cedar: not mapping area\n");
+
+	vma->vm_private_data = cedar_vma_info;
 
 	vma->vm_ops = &cedardev_remap_vm_ops;
 	cedardev_vma_open(vma);
@@ -1407,6 +1503,8 @@ static int cedardev_init(struct platform_device *pdev)
 	}
 	cedar_devp->phy_addr = res->start;
 
+	cedar_kernel_map = cedar_devp->regs_macc;
+
 	ret = clk_set_rate(cedar_devp->mod_clk, variant->mod_rate);
 	if (ret) {
 		dev_err(cedar_devp->platform_dev, "Failed to set clock rate\n");