watchpoint: add watch point driver for cpu [1/1]

PD#SWPL-4351

Problem:
Our cpu has 4 watch point on each cpu. It can be used for debug
purpose. But currently there is no driver support it.

Solution:
Bring up driver for it.

Verify:
p212

Change-Id: Ifbcb7f9b77e10fcb03b3c1a5e18f06b1a56ec2f0
Signed-off-by: Tao Zeng <tao.zeng@amlogic.com>

MAINTAINERS

@@ -13522,6 +13522,8 @@ F: drivers/amlogic/memory_ext/*
 F: include/linux/amlogic/ramdump.h
 F: include/linux/amlogic/vmap_stack.h
 F: drivers/amlogic/memory_ext/vmap_stack.c
+F: drivers/amlogic/memory_ext/watch_point.c
+F: include/linux/amlogic/watch_point.h
 
 AMLOGIC driver for memory extend
 M: Tao Zeng <tao.zeng@amlogic.com>

arch/arm/kernel/hw_breakpoint.c

@@ -102,7 +102,11 @@ static u8 max_watchpoint_len;
 	WRITE_WB_REG_CASE(OP2, 14, VAL);	\
 	WRITE_WB_REG_CASE(OP2, 15, VAL)
 
+#ifdef CONFIG_AMLOGIC_MODIFY
+u32 read_wb_reg(int n)
+#else
 static u32 read_wb_reg(int n)
+#endif
 {
 	u32 val = 0;
 

arch/arm64/kernel/hw_breakpoint.c

@@ -114,7 +114,11 @@ int hw_breakpoint_slots(int type)
 	WRITE_WB_REG_CASE(OFF, 14, REG, VAL);	\
 	WRITE_WB_REG_CASE(OFF, 15, REG, VAL)
 
+#ifdef CONFIG_AMLOGIC_MODIFY
+u64 read_wb_reg(int reg, int n)
+#else
 static u64 read_wb_reg(int reg, int n)
+#endif
 {
 	u64 val = 0;
 
@@ -129,14 +133,18 @@ static u64 read_wb_reg(int reg, int n)
 
 	return val;
 }
+#ifndef CONFIG_AMLOGIC_MODIFY
 NOKPROBE_SYMBOL(read_wb_reg);
+#endif
 
 static void write_wb_reg(int reg, int n, u64 val)
 {
 #ifdef CONFIG_AMLOGIC_VMAP
 	/* avoid write DBGWVR since we use it for special purpose */
-	if (reg >= AARCH64_DBG_REG_WVR && reg < AARCH64_DBG_REG_WCR)
+	if ((reg + n) >= (AARCH64_DBG_REG_WVR + 2) &&
+	    (reg + n) < AARCH64_DBG_REG_WCR) {
 		return;
+	}
 #endif
 	switch (reg + n) {
 	GEN_WRITE_WB_REG_CASES(AARCH64_DBG_REG_BVR, AARCH64_DBG_REG_NAME_BVR, val);

drivers/amlogic/memory_ext/Kconfig

@@ -58,3 +58,13 @@ config AMLOGIC_SLUB_DEBUG
 	help
 		This option open trace debug for each slub objects. And will give
 		slub objdect allocator information when detected bad objects
+
+config AMLOGIC_WATCHPOINT
+	tristate "Amlogic point driver"
+	depends on HAVE_HW_BREAKPOINT
+	depends on AMLOGIC_MEMORY_EXTEND
+	default y
+	help
+		This driver export a debug sysfs in order
+		to using watch point function on ARMv8.
+		say y to enable Amlogic watch point driver

drivers/amlogic/memory_ext/Makefile

@@ -4,3 +4,4 @@ obj-$(CONFIG_AMLOGIC_CMA)         += aml_cma.o
 obj-$(CONFIG_AMLOGIC_SLUB_DEBUG)  += aml_slub_debug.o
 obj-$(CONFIG_AMLOGIC_RAMDUMP)     += ram_dump.o
 obj-$(CONFIG_AMLOGIC_VMAP)        += vmap_stack.o
+obj-$(CONFIG_AMLOGIC_WATCHPOINT)  += watch_point.o

drivers/amlogic/memory_ext/watch_point.c

@@ -0,0 +1,485 @@
+/*
+ * drivers/amlogic/memory_ext/watch_point.c
+ *
+ * Copyright (C) 2017 Amlogic, Inc. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ */
+
+#include <linux/cdev.h>
+#include <linux/types.h>
+#include <linux/fs.h>
+#include <linux/device.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/uaccess.h>
+#include <linux/sched.h>
+#include <linux/platform_device.h>
+#include <linux/amlogic/iomap.h>
+#include <linux/of.h>
+#include <linux/of_fdt.h>
+
+#include <linux/platform_device.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/compat.h>
+#include <linux/random.h>
+#include <linux/ptrace.h>
+#include <linux/hw_breakpoint.h>
+#include <linux/perf_event.h>
+#include <linux/cpu.h>
+#include <linux/smp.h>
+#include <linux/amlogic/watch_point.h>
+
+struct aml_watch_points {
+	struct perf_event * __percpu *wp_event[MAX_WATCH_POINTS];
+	perf_overflow_handler_t handler[MAX_WATCH_POINTS];
+	int num_watch_points;
+	struct work_struct replace_work;
+	spinlock_t lock;
+};
+
+struct aml_watch_points *awp;
+
+static void get_cpu_wb_reg(void *info)
+{
+	unsigned long *p, r;
+
+	p = (unsigned long *)info;
+	r = *p;
+
+#ifdef CONFIG_ARM64
+	if (r < AARCH64_DBG_REG_WCR)
+		*p = read_wb_reg(AARCH64_DBG_REG_WVR, r - AARCH64_DBG_REG_WVR);
+	else
+		*p = read_wb_reg(AARCH64_DBG_REG_WCR, r - AARCH64_DBG_REG_WCR);
+#else
+	*p = read_wb_reg(r);
+#endif
+}
+
+#ifdef CONFIG_ARM
+static struct perf_event **wp_flag(struct perf_event **event, int set)
+{
+	unsigned long tmp;
+
+	tmp = (unsigned long)event;
+
+	if (set)
+		tmp |= 0x01;
+	else
+		tmp &= ~0x01;
+	return (struct perf_event **)tmp;
+}
+
+static void wp_del(void *data)
+{
+	struct perf_event *bp;
+
+	bp = (struct perf_event *)data;
+	bp->pmu->del(bp, PERF_EF_UPDATE);
+	pr_info("del for wp:%lx, wp:%p\n",
+		(unsigned long)bp->attr.bp_addr, bp);
+}
+
+static void wp_add(void *data)
+{
+	struct perf_event *bp;
+
+	bp = (struct perf_event *)data;
+	bp->pmu->add(bp, PERF_EF_START);
+	pr_info("add for wp:%lx, wp:%p\n",
+		(unsigned long)bp->attr.bp_addr, bp);
+}
+#endif
+
+static int dump_watch_point_reg(char *buf)
+{
+	int i, cpu = 0;
+	unsigned long addr, wvr, wcr;
+	int len, type, size = 0;
+	struct perf_event *bp;
+
+	size += sprintf(buf + size,
+			"idx,            addr, type, len, event, handler\n");
+	for (i = 0; i < awp->num_watch_points; i++) {
+		if (awp->wp_event[i]) {
+			bp = get_cpu_var(*awp->wp_event[i]);
+			addr = bp->attr.bp_addr;
+			len  = bp->attr.bp_len;
+			type = bp->attr.bp_type;
+			put_cpu_var(*awp->wp_event[i]);
+		} else {
+			addr = 0;
+			len  = 0;
+			type = 0;
+		}
+		size += sprintf(buf + size, "%2d, %16lx,   %x,   %x, %p, %pf\n",
+				i, addr, type, len, awp->wp_event[i],
+				awp->handler[i]);
+	}
+	for_each_online_cpu(cpu) {
+		size += sprintf(buf + size, "CPU:%d\n", cpu);
+		for (i = 0; i < awp->num_watch_points; i++) {
+		#ifdef CONFIG_ARM64
+			wvr = AARCH64_DBG_REG_WVR + i;
+		#else
+			wvr = ARM_BASE_WVR + i;
+		#endif
+			smp_call_function_single(cpu, get_cpu_wb_reg, &wvr, 1);
+
+		#ifdef CONFIG_ARM64
+			wcr = AARCH64_DBG_REG_WCR + i;
+		#else
+			wcr = ARM_BASE_WCR + i;
+		#endif
+			smp_call_function_single(cpu, get_cpu_wb_reg, &wcr, 1);
+			size += sprintf(buf + size, "  WVR:%16lx WCR:%16lx\n",
+					wvr, wcr);
+		}
+	}
+	return size;
+}
+
+static void wp_replace_back(struct work_struct *data)
+{
+	int i, cpu;
+	struct perf_event *bp;
+
+	for (i = 0; i < awp->num_watch_points; i++) {
+		if (!awp->wp_event[i])
+			continue;
+	#ifdef CONFIG_ARM64
+		get_online_cpus();
+		for_each_online_cpu(cpu) {
+			bp = per_cpu(*awp->wp_event[i], cpu);
+			if (is_default_overflow_handler(bp)) {
+				bp->overflow_handler = awp->handler[i];
+				pr_info("replace handler for wp:%lx\n",
+					(unsigned long)bp->attr.bp_addr);
+			}
+		}
+		put_online_cpus();
+	#else
+		if (!(((unsigned long)awp->wp_event[i]) & 0x01))
+			continue;
+
+		awp->wp_event[i] = wp_flag(awp->wp_event[i], 0);
+		get_online_cpus();
+		for_each_online_cpu(cpu) {
+			bp = per_cpu(*awp->wp_event[i], cpu);
+			smp_call_function_single(cpu, wp_add, bp, 1);
+		}
+		put_online_cpus();
+	#endif
+	}
+}
+
+static void aml_default_hbp_handler(struct perf_event *bp,
+				    struct perf_sample_data *data,
+				    struct pt_regs *regs)
+{
+#ifdef CONFIG_ARM64
+	pr_info("watch addr %llx triggerd, pc:%pf, lr:%pf\n",
+		bp->attr.bp_addr, (void *)regs->pc,
+		(void *)regs->compat_lr_fiq);
+	bp->overflow_handler = perf_event_output_forward;
+	show_regs(regs);
+	dump_stack();
+#else
+	struct perf_event * __percpu *event = NULL;
+	int i, cpu;
+
+	pr_info("watch addr %llx triggerd, pc:%pf, lr:%pf\n",
+		bp->attr.bp_addr, (void *)regs->ARM_pc,
+		(void *)regs->ARM_lr);
+
+	for (i = 0; i < awp->num_watch_points; i++) {
+		if (!awp->wp_event[i])
+			continue;
+		for_each_online_cpu(cpu) {
+			if (bp == per_cpu(*awp->wp_event[i], cpu)) {
+				event = awp->wp_event[i];
+				break;
+			}
+		}
+		if (event) {
+			for_each_online_cpu(cpu) {
+				bp = per_cpu(*event, cpu);
+				smp_call_function_single(cpu, wp_del, bp, 1);
+			}
+			break;
+		}
+	}
+	if (event)
+		awp->wp_event[i] = wp_flag(awp->wp_event[i], 1);
+	show_regs(regs);
+#endif
+	schedule_work_on(smp_processor_id(), &awp->replace_work);
+}
+
+/* register a watch pointer */
+int aml_watch_point_register(unsigned long addr,
+			     unsigned int len,
+			     unsigned int type,
+			     perf_overflow_handler_t handle)
+{
+	int i;
+	struct perf_event_attr attr;
+	struct perf_event * __percpu *event;
+
+	if (!awp)
+		return -ENOMEM;
+
+	/* parameter check */
+	if ((len > HW_BREAKPOINT_LEN_8) || (len < HW_BREAKPOINT_LEN_1)) {
+		pr_err("bad input len:%d\n", len);
+		return -EINVAL;
+	}
+	if (type & ~(HW_BREAKPOINT_W | HW_BREAKPOINT_R)) {
+		pr_err("bad input type:%d\n", type);
+		return -EINVAL;
+	}
+
+	/* check if all watch points are used */
+	spin_lock(&awp->lock);
+	for (i = 0; i < awp->num_watch_points; i++) {
+		if (!awp->wp_event[i]) {
+			awp->wp_event[i]++;
+			break;
+		}
+	}
+	if (i == awp->num_watch_points) {
+		spin_unlock(&awp->lock);
+		pr_err("%s, watch point is all used\n", __func__);
+		return -ENODEV;
+	}
+	spin_unlock(&awp->lock);
+
+	hw_breakpoint_init(&attr);
+	attr.bp_addr = addr;
+	attr.bp_len  = len;
+	attr.bp_type = type;
+	if (!handle)
+		handle = aml_default_hbp_handler;
+
+	event = register_wide_hw_breakpoint(&attr, handle, NULL);
+	spin_lock(&awp->lock);
+	if (IS_ERR_OR_NULL(event)) {
+		awp->wp_event[i] = NULL;
+		awp->handler[i]  = NULL;
+	} else {
+		awp->wp_event[i] = event;
+		awp->handler[i]  = handle;
+	}
+	spin_unlock(&awp->lock);
+
+	pr_info("watch point[%d], addr:%lx, len:%d, type:%x, event:%p\n",
+		i, addr, len, type, awp->wp_event[i]);
+	return awp->wp_event[i] ? 0 : -EINVAL;
+}
+EXPORT_SYMBOL(aml_watch_point_register);
+
+/* remove watch point according given address */
+void aml_watch_point_remove(unsigned long addr)
+{
+	int i;
+	struct perf_event *bp;
+	struct perf_event * __percpu *event = NULL;
+
+	if (!awp)
+		return;
+
+	spin_lock(&awp->lock);
+	for (i = 0; i < awp->num_watch_points; i++) {
+		if (awp->wp_event[i]) {
+			bp = get_cpu_var(*awp->wp_event[i]);
+			if (bp->attr.bp_addr == addr) {
+				event = awp->wp_event[i];
+				awp->wp_event[i] = NULL;
+				awp->handler[i] = NULL;
+				put_cpu_var(*awp->wp_event[i]);
+				break;
+			}
+			put_cpu_var(*awp->wp_event[i]);
+		}
+	}
+	spin_unlock(&awp->lock);
+	if (event)
+		unregister_wide_hw_breakpoint(event);
+}
+EXPORT_SYMBOL(aml_watch_point_remove);
+
+/*
+ * force clear a watch point
+ */
+static void aml_watch_point_clear(int idx)
+{
+	struct perf_event * __percpu *event = NULL;
+
+	if (idx >= awp->num_watch_points)
+		return;
+
+	spin_lock(&awp->lock);
+	if (awp->wp_event[idx]) {
+		event = awp->wp_event[idx];
+		awp->wp_event[idx] = NULL;
+		awp->handler[idx] = NULL;
+	}
+	spin_unlock(&awp->lock);
+	if (event)
+		unregister_wide_hw_breakpoint(event);
+}
+
+static ssize_t num_watch_points_show(struct class *cla,
+	struct class_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%d\n", awp->num_watch_points);
+}
+
+static ssize_t watch_addr_store(struct class *cla,
+	struct class_attribute *attr, const char *buf, size_t count)
+{
+	unsigned long addr;
+	u32 len = HW_BREAKPOINT_LEN_8;
+	u32 type = HW_BREAKPOINT_W;
+	int ret;
+
+	ret = sscanf(buf, "%lx %x %x", &addr, &len, &type);
+	if (ret < 1)
+		return -EINVAL;
+
+	ret = aml_watch_point_register(addr, len, type, NULL);
+
+	return count;
+}
+
+static ssize_t clear_store(struct class *cla,
+	struct class_attribute *attr, const char *buf, size_t count)
+{
+	int i;
+	int idx = -1;
+
+	if (kstrtoint(buf, 10, &idx))
+		return count;
+
+	if (idx >= awp->num_watch_points) {
+		pr_err("input index %d out of range:[0 - %d]\n",
+			idx, awp->num_watch_points);
+		return -EINVAL;
+	}
+
+	/* negative value means clear all watch point */
+	if (idx < 0) {
+		for (i = 0; i < awp->num_watch_points; i++)
+			aml_watch_point_clear(i);
+	} else {
+		aml_watch_point_clear(idx);
+	}
+
+	return count;
+}
+
+static ssize_t dump_show(struct class *cla,
+	struct class_attribute *attr, char *buf)
+{
+	return dump_watch_point_reg(buf);
+}
+
+static struct class_attribute watch_point_attr[] = {
+	__ATTR(watch_addr, 0664, dump_show, watch_addr_store),
+	__ATTR_RO(num_watch_points),
+	__ATTR_WO(clear),
+	__ATTR_NULL
+};
+
+static struct class watch_point_class = {
+	.name = "watch_point",
+	.class_attrs = watch_point_attr,
+};
+
+/*
+ *    aml_watch_point_probe only executes before the init process starts
+ * to run, so add __ref to indicate it is okay to call __init function
+ * hook_debug_fault_code
+ */
+static int __init aml_watch_point_probe(struct platform_device *pdev)
+{
+	int r;
+
+	r = hw_breakpoint_slots(TYPE_DATA);
+	pr_info("%s, in, wp:%d\n", __func__, r);
+	if (!r)
+		return -ENODEV;
+
+	awp = devm_kzalloc(&pdev->dev, sizeof(*awp), GFP_KERNEL);
+	if (awp == NULL)
+		return -ENOMEM;
+
+	awp->num_watch_points = r;
+	r = class_register(&watch_point_class);
+	if (r) {
+		pr_err("regist watch_point_class failed\n");
+		return -EINVAL;
+	}
+	INIT_WORK(&awp->replace_work, wp_replace_back);
+
+	return 0;
+}
+
+static int aml_watch_point_drv_remove(struct platform_device *pdev)
+{
+	class_unregister(&watch_point_class);
+	return 0;
+}
+
+static struct platform_driver aml_watch_point_driver = {
+	.driver = {
+		.name  = "aml_watch_point",
+		.owner = THIS_MODULE,
+	},
+	.probe = aml_watch_point_probe,
+	.remove = aml_watch_point_drv_remove,
+};
+
+static int __init aml_watch_pint_init(void)
+{
+	struct platform_device *pdev;
+	int ret;
+
+	pdev = platform_device_alloc("aml_watch_point", 0);
+	if (!pdev) {
+		pr_err("alloc pdev aml_watch_point failed\n");
+		return -EINVAL;
+	}
+	ret = platform_device_add(pdev);
+	if (ret) {
+		pr_err("regist pdev failed, ret:%d\n", ret);
+		platform_device_del(pdev);
+		return ret;
+	}
+	ret = platform_driver_probe(&aml_watch_point_driver,
+				    aml_watch_point_probe);
+	if (ret)
+		platform_device_del(pdev);
+	return ret;
+}
+
+static void __exit aml_watch_point_uninit(void)
+{
+	platform_driver_unregister(&aml_watch_point_driver);
+}
+
+arch_initcall(aml_watch_pint_init);
+module_exit(aml_watch_point_uninit);
+MODULE_DESCRIPTION("amlogic watch point driver");
+MODULE_LICENSE("GPL");

include/linux/amlogic/watch_point.h

@@ -0,0 +1,55 @@
+/*
+ * include/linux/amlogic/watch_point.h
+ *
+ * Copyright (C) 2017 Amlogic, Inc. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ */
+
+#ifndef __AML_WATCH_POINT_H__
+#define __AML_WATCH_POINT_H__
+
+#include <uapi/linux/elf.h>
+#include <uapi/linux/hw_breakpoint.h>
+#include <linux/perf_event.h>
+
+#define MAX_WATCH_POINTS	16
+
+#ifdef CONFIG_ARM64
+extern u64 read_wb_reg(int reg, int n);
+#else
+extern u32 read_wb_reg(int n);
+#endif
+
+#ifdef CONFIG_HAVE_HW_BREAKPOINT
+extern int aml_watch_point_register(unsigned long addr,
+				    unsigned int len,
+				    unsigned int type,
+				    perf_overflow_handler_t handle);
+
+extern void aml_watch_point_remove(unsigned long addr);
+#else
+static inline int aml_watch_point_register(unsigned long addr,
+					   unsigned int len,
+					   unsigned int type,
+					   perf_overflow_handler_t handle)
+{
+	return -1;
+}
+
+static inline void aml_watch_point_remove(unsigned long addr)
+{
+
+}
+#endif
+
+#endif