From c03d5e15296e4d57cb9e98d5136701e0878b46ec Mon Sep 17 00:00:00 2001 From: Tao Zeng Date: Wed, 23 Jan 2019 19:39:09 +0800 Subject: [PATCH] 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 --- MAINTAINERS | 2 + arch/arm/kernel/hw_breakpoint.c | 4 + arch/arm64/kernel/hw_breakpoint.c | 10 +- drivers/amlogic/memory_ext/Kconfig | 10 + drivers/amlogic/memory_ext/Makefile | 1 + drivers/amlogic/memory_ext/watch_point.c | 485 +++++++++++++++++++++++ include/linux/amlogic/watch_point.h | 55 +++ 7 files changed, 566 insertions(+), 1 deletion(-) create mode 100644 drivers/amlogic/memory_ext/watch_point.c create mode 100644 include/linux/amlogic/watch_point.h diff --git a/MAINTAINERS b/MAINTAINERS index 2ffb9e3f3165..5917bfcd9a03 100644 --- a/MAINTAINERS +++ b/MAINTAINERS @@ -13527,6 +13527,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 diff --git a/arch/arm/kernel/hw_breakpoint.c b/arch/arm/kernel/hw_breakpoint.c index 25538a935874..bc8f9b1b752c 100644 --- a/arch/arm/kernel/hw_breakpoint.c +++ b/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; diff --git a/arch/arm64/kernel/hw_breakpoint.c b/arch/arm64/kernel/hw_breakpoint.c index 0798abd4d692..97e9b4d5999b 100644 --- a/arch/arm64/kernel/hw_breakpoint.c +++ b/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); diff --git a/drivers/amlogic/memory_ext/Kconfig b/drivers/amlogic/memory_ext/Kconfig index 46009b0f9c73..f0a8817fb005 100644 --- a/drivers/amlogic/memory_ext/Kconfig +++ b/drivers/amlogic/memory_ext/Kconfig @@ -69,3 +69,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 diff --git a/drivers/amlogic/memory_ext/Makefile b/drivers/amlogic/memory_ext/Makefile index 8d3c2666ac00..58406009fb2a 100644 --- a/drivers/amlogic/memory_ext/Makefile +++ b/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 diff --git a/drivers/amlogic/memory_ext/watch_point.c b/drivers/amlogic/memory_ext/watch_point.c new file mode 100644 index 000000000000..35b4a050b613 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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"); diff --git a/include/linux/amlogic/watch_point.h b/include/linux/amlogic/watch_point.h new file mode 100644 index 000000000000..f28fa37eb8b4 --- /dev/null +++ b/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 +#include +#include + +#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