--- /dev/null
+/*
+ * 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");
projects / platform / kernel / linux-amlogic.git / commitdiff