5 files changed, 171 insertions, 9 deletions

diff --git a/arch/avr32/kernel/Makefile b/arch/avr32/kernel/Makefile
index 2d6d48f35f69..bc224a4e39fe 100644
--- a/arch/avr32/kernel/Makefile
+++ b/arch/avr32/kernel/Makefile
@@ -6,7 +6,7 @@ extra-y				:= head.o vmlinux.lds
 
 obj-$(CONFIG_SUBARCH_AVR32B)    += entry-avr32b.o
 obj-y                           += syscall_table.o syscall-stubs.o irq.o
-obj-y                           += setup.o traps.o semaphore.o ptrace.o
+obj-y                           += setup.o traps.o semaphore.o ocd.o ptrace.o
 obj-y                           += signal.o sys_avr32.o process.o time.o
 obj-y                           += init_task.o switch_to.o cpu.o
 obj-$(CONFIG_MODULES)           += module.o avr32_ksyms.o
diff --git a/arch/avr32/kernel/kprobes.c b/arch/avr32/kernel/kprobes.c
index 799ba89b07a8..f820e9f25520 100644
--- a/arch/avr32/kernel/kprobes.c
+++ b/arch/avr32/kernel/kprobes.c
@@ -48,6 +48,7 @@ int __kprobes arch_prepare_kprobe(struct kprobe *p)
 void __kprobes arch_arm_kprobe(struct kprobe *p)
 {
         pr_debug("arming kprobe at %p\n", p->addr);
+        ocd_enable(NULL);
         *p->addr = BREAKPOINT_INSTRUCTION;
         flush_icache_range((unsigned long)p->addr,
                            (unsigned long)p->addr + sizeof(kprobe_opcode_t));
@@ -56,6 +57,7 @@ void __kprobes arch_arm_kprobe(struct kprobe *p)
 void __kprobes arch_disarm_kprobe(struct kprobe *p)
 {
         pr_debug("disarming kprobe at %p\n", p->addr);
+        ocd_disable(NULL);
         *p->addr = p->opcode;
         flush_icache_range((unsigned long)p->addr,
                            (unsigned long)p->addr + sizeof(kprobe_opcode_t));
@@ -260,9 +262,6 @@ int __kprobes longjmp_break_handler(struct kprobe *p, struct pt_regs *regs)
 
 int __init arch_init_kprobes(void)
 {
-        printk("KPROBES: Enabling monitor mode (MM|DBE)...\n");
-        ocd_write(DC, (1 << OCD_DC_MM_BIT) | (1 << OCD_DC_DBE_BIT));
-
         /* TODO: Register kretprobe trampoline */
         return 0;
 }
diff --git a/arch/avr32/kernel/ocd.c b/arch/avr32/kernel/ocd.c
new file mode 100644
index 000000000000..c4f023294d75
--- /dev/null
+++ b/arch/avr32/kernel/ocd.c
@@ -0,0 +1,163 @@
+/*
+ * Copyright (C) 2007 Atmel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/init.h>
+#include <linux/sched.h>
+#include <linux/spinlock.h>
+
+#include <asm/ocd.h>
+
+static long ocd_count;
+static spinlock_t ocd_lock;
+
+/**
+ * ocd_enable - enable on-chip debugging
+ * @child: task to be debugged
+ *
+ * If @child is non-NULL, ocd_enable() first checks if debugging has
+ * already been enabled for @child, and if it has, does nothing.
+ *
+ * If @child is NULL (e.g. when debugging the kernel), or debugging
+ * has not already been enabled for it, ocd_enable() increments the
+ * reference count and enables the debugging hardware.
+ */
+void ocd_enable(struct task_struct *child)
+{
+        u32 dc;
+
+        if (child)
+                pr_debug("ocd_enable: child=%s [%u]\n",
+                                child->comm, child->pid);
+        else
+                pr_debug("ocd_enable (no child)\n");
+
+        if (!child || !test_and_set_tsk_thread_flag(child, TIF_DEBUG)) {
+                spin_lock(&ocd_lock);
+                ocd_count++;
+                dc = ocd_read(DC);
+                dc |= (1 << OCD_DC_MM_BIT) | (1 << OCD_DC_DBE_BIT);
+                ocd_write(DC, dc);
+                spin_unlock(&ocd_lock);
+        }
+}
+
+/**
+ * ocd_disable - disable on-chip debugging
+ * @child: task that was being debugged, but isn't anymore
+ *
+ * If @child is non-NULL, ocd_disable() checks if debugging is enabled
+ * for @child, and if it isn't, does nothing.
+ *
+ * If @child is NULL (e.g. when debugging the kernel), or debugging is
+ * enabled, ocd_disable() decrements the reference count, and if it
+ * reaches zero, disables the debugging hardware.
+ */
+void ocd_disable(struct task_struct *child)
+{
+        u32 dc;
+
+        if (!child)
+                pr_debug("ocd_disable (no child)\n");
+        else if (test_tsk_thread_flag(child, TIF_DEBUG))
+                pr_debug("ocd_disable: child=%s [%u]\n",
+                                child->comm, child->pid);
+
+        if (!child || test_and_clear_tsk_thread_flag(child, TIF_DEBUG)) {
+                spin_lock(&ocd_lock);
+                ocd_count--;
+
+                WARN_ON(ocd_count < 0);
+
+                if (ocd_count <= 0) {
+                        dc = ocd_read(DC);
+                        dc &= ~((1 << OCD_DC_MM_BIT) | (1 << OCD_DC_DBE_BIT));
+                        ocd_write(DC, dc);
+                }
+                spin_unlock(&ocd_lock);
+        }
+}
+
+#ifdef CONFIG_DEBUG_FS
+#include <linux/debugfs.h>
+#include <linux/module.h>
+
+static struct dentry *ocd_debugfs_root;
+static struct dentry *ocd_debugfs_DC;
+static struct dentry *ocd_debugfs_DS;
+static struct dentry *ocd_debugfs_count;
+
+static u64 ocd_DC_get(void *data)
+{
+        return ocd_read(DC);
+}
+static void ocd_DC_set(void *data, u64 val)
+{
+        ocd_write(DC, val);
+}
+DEFINE_SIMPLE_ATTRIBUTE(fops_DC, ocd_DC_get, ocd_DC_set, "0x%08llx\n");
+
+static u64 ocd_DS_get(void *data)
+{
+        return ocd_read(DS);
+}
+DEFINE_SIMPLE_ATTRIBUTE(fops_DS, ocd_DS_get, NULL, "0x%08llx\n");
+
+static u64 ocd_count_get(void *data)
+{
+        return ocd_count;
+}
+DEFINE_SIMPLE_ATTRIBUTE(fops_count, ocd_count_get, NULL, "%lld\n");
+
+static void ocd_debugfs_init(void)
+{
+        struct dentry *root;
+
+        root = debugfs_create_dir("ocd", NULL);
+        if (IS_ERR(root) || !root)
+                goto err_root;
+        ocd_debugfs_root = root;
+
+        ocd_debugfs_DC = debugfs_create_file("DC", S_IRUSR | S_IWUSR,
+                                root, NULL, &fops_DC);
+        if (!ocd_debugfs_DC)
+                goto err_DC;
+
+        ocd_debugfs_DS = debugfs_create_file("DS", S_IRUSR, root,
+                                NULL, &fops_DS);
+        if (!ocd_debugfs_DS)
+                goto err_DS;
+
+        ocd_debugfs_count = debugfs_create_file("count", S_IRUSR, root,
+                                NULL, &fops_count);
+        if (!ocd_debugfs_count)
+                goto err_count;
+
+        return;
+
+err_count:
+        debugfs_remove(ocd_debugfs_DS);
+err_DS:
+        debugfs_remove(ocd_debugfs_DC);
+err_DC:
+        debugfs_remove(ocd_debugfs_root);
+err_root:
+        printk(KERN_WARNING "OCD: Failed to create debugfs entries\n");
+}
+#else
+static inline void ocd_debugfs_init(void)
+{
+
+}
+#endif
+
+static int __init ocd_init(void)
+{
+        spin_lock_init(&ocd_lock);
+        ocd_debugfs_init();
+        return 0;
+}
+arch_initcall(ocd_init);
diff --git a/arch/avr32/kernel/process.c b/arch/avr32/kernel/process.c
index 9d6dac8af7a2..eaaa69bbdc38 100644
--- a/arch/avr32/kernel/process.c
+++ b/arch/avr32/kernel/process.c
@@ -103,7 +103,7 @@ EXPORT_SYMBOL(kernel_thread);
  */
 void exit_thread(void)
 {
-        /* nothing to do */
+        ocd_disable(current);
 }
 
 void flush_thread(void)
@@ -345,6 +345,9 @@ int copy_thread(int nr, unsigned long clone_flags, unsigned long usp,
         p->thread.cpu_context.ksp = (unsigned long)childregs;
         p->thread.cpu_context.pc = (unsigned long)ret_from_fork;
 
+        if ((clone_flags & CLONE_PTRACE) && test_thread_flag(TIF_DEBUG))
+                ocd_enable(p);
+
         return 0;
 }
 
diff --git a/arch/avr32/kernel/ptrace.c b/arch/avr32/kernel/ptrace.c
index 002369e44093..1fed38fcf594 100644
--- a/arch/avr32/kernel/ptrace.c
+++ b/arch/avr32/kernel/ptrace.c
@@ -58,6 +58,7 @@ void ptrace_disable(struct task_struct *child)
 {
         clear_tsk_thread_flag(child, TIF_SINGLE_STEP);
         clear_tsk_thread_flag(child, TIF_BREAKPOINT);
+        ocd_disable(child);
 }
 
 /*
@@ -144,10 +145,6 @@ long arch_ptrace(struct task_struct *child, long request, long addr, long data)
 {
         int ret;
 
-        pr_debug("ptrace: Enabling monitor mode...\n");
-        ocd_write(DC, ocd_read(DC) | (1 << OCD_DC_MM_BIT)
-                        | (1 << OCD_DC_DBE_BIT));
-
         switch (request) {
         /* Read the word at location addr in the child process */
         case PTRACE_PEEKTEXT: