[PATCH] Add v850 support for hardware single-step (via ptrace)

arch/v850/kernel/ptrace.c

 /*
  * arch/v850/kernel/ptrace.c -- `ptrace' system call
  *
- *  Copyright (C) 2002  NEC Corporation
- *  Copyright (C) 2002  Miles Bader <miles@gnu.org>
+ *  Copyright (C) 2002,03  NEC Electronics Corporation
+ *  Copyright (C) 2002,03  Miles Bader <miles@gnu.org>
  *
  * Derived from arch/mips/kernel/ptrace.c:
  *
@@ -29,6 +29,89 @@
 #include <asm/processor.h>
 #include <asm/uaccess.h>
 
+/* Returns the address where the register at REG_OFFS in P is stashed away.  */
+static v850_reg_t *reg_save_addr (unsigned reg_offs, struct task_struct *t)
+{
+	struct pt_regs *regs;
+
+	/* Three basic cases:
+
+	   (1) A register normally saved before calling the scheduler, is
+	       available in the kernel entry pt_regs structure at the top
+	       of the kernel stack.  The kernel trap/irq exit path takes
+	       care to save/restore almost all registers for ptrace'd
+	       processes.
+
+	   (2) A call-clobbered register, where the process P entered the
+	       kernel via [syscall] trap, is not stored anywhere; that's
+	       OK, because such registers are not expected to be preserved
+	       when the trap returns anyway (so we don't actually bother to
+	       test for this case).
+
+	   (3) A few registers not used at all by the kernel, and so
+	       normally never saved except by context-switches, are in the
+	       context switch state.  */
+
+	if (reg_offs == PT_CTPC || reg_offs == PT_CTPSW || reg_offs == PT_CTBP)
+		/* Register saved during context switch.  */
+		regs = thread_saved_regs (t);
+	else
+		/* Register saved during kernel entry (or not available).  */
+		regs = task_regs (t);
+
+	return (v850_reg_t *)((char *)regs + reg_offs);
+}
+
+/* Set the bits SET and clear the bits CLEAR in the v850e DIR
+   (`debug information register').  Returns the new value of DIR.  */
+static inline v850_reg_t set_dir (v850_reg_t set, v850_reg_t clear)
+{
+	register v850_reg_t rval asm ("r10");
+	register v850_reg_t arg0 asm ("r6") = set;
+	register v850_reg_t arg1 asm ("r7") = clear;
+
+	/* The dbtrap handler has exactly this functionality when called
+	   from kernel mode.  0xf840 is a `dbtrap' insn.  */
+	asm (".short 0xf840" : "=r" (rval) : "r" (arg0), "r" (arg1));
+
+	return rval;
+}
+
+/* Makes sure hardware single-stepping is (globally) enabled.
+   Returns true if successful.  */
+static inline int enable_single_stepping (void)
+{
+	static int enabled = 0;	/* Remember whether we already did it.  */
+	if (! enabled) {
+		/* Turn on the SE (`single-step enable') bit, 0x100, in the
+		   DIR (`debug information register').  This may fail if a
+		   processor doesn't support it or something.  We also try
+		   to clear bit 0x40 (`INI'), which is necessary to use the
+		   debug stuff on the v850e2; on the v850e, clearing 0x40
+		   shouldn't cause any problem.  */
+		v850_reg_t dir = set_dir (0x100, 0x40);
+		/* Make sure it really got set.  */
+		if (dir & 0x100)
+			enabled = 1;
+	}
+	return enabled;
+}
+
+/* Try to set CHILD's single-step flag to VAL.  Returns true if successful.  */
+static int set_single_step (struct task_struct *t, int val)
+{
+	v850_reg_t *psw_addr = reg_save_addr(PT_PSW, t);
+	if (val) {
+		/* Make sure single-stepping is enabled.  */
+		if (! enable_single_stepping ())
+			return 0;
+		/* Set T's single-step flag.  */
+		*psw_addr |= 0x800;
+	} else
+		*psw_addr &= ~0x800;
+	return 1;
+}
+
 int sys_ptrace(long request, long pid, long addr, long data)
 {
 	struct task_struct *child;
@@ -36,12 +119,6 @@ int sys_ptrace(long request, long pid, long addr, long data)
 
 	lock_kernel();
 
-#if 0
-	printk("ptrace(r=%d,pid=%d,addr=%08lx,data=%08lx)\n",
-	       (int) request, (int) pid, (unsigned long) addr,
-	       (unsigned long) data);
-#endif
-
 	if (request == PTRACE_TRACEME) {
 		/* are we already being traced? */
 		if (current->ptrace & PT_PTRACED) {
@@ -81,31 +158,15 @@ int sys_ptrace(long request, long pid, long addr, long data)
 		goto out_tsk;
 
 	switch (request) {
-	case PTRACE_PEEKTEXT: /* read word at location addr. */ 
-	case PTRACE_PEEKDATA:{
-		unsigned long tmp;
-		int copied;
+		unsigned long val, copied;
 
-		copied = access_process_vm(child, addr, &tmp, sizeof(tmp), 0);
+	case PTRACE_PEEKTEXT: /* read word at location addr. */
+	case PTRACE_PEEKDATA:
+		copied = access_process_vm(child, addr, &val, sizeof(val), 0);
 		rval = -EIO;
-		if (copied != sizeof(tmp))
+		if (copied != sizeof(val))
 			break;
-		rval = put_user(tmp,(unsigned long *) data);
-
-		goto out;
-		}
-
-	/* Read the word at location addr in the USER area.  */
-	case PTRACE_PEEKUSR: 
-		if (addr >= 0 && addr < PT_SIZE && (addr & 0x3) == 0) {
-			struct pt_regs *regs = task_regs (child);
-			unsigned long val =
-				*(unsigned long *)((char *)regs + addr);
-			rval = put_user (val, (unsigned long *)data);
-		} else {
-			rval = 0;
-			rval = -EIO;			
-		}
+		rval = put_user(val, (unsigned long *)data);
 		goto out;
 
 	case PTRACE_POKETEXT: /* write the word at location addr. */
@@ -117,27 +178,54 @@ int sys_ptrace(long request, long pid, long addr, long data)
 		rval = -EIO;
 		goto out;
 
+	/* Read/write the word at location ADDR in the registers.  */
+	case PTRACE_PEEKUSR:
 	case PTRACE_POKEUSR:
-		if (addr >= 0 && addr < PT_SIZE && (addr & 0x3) == 0) {
-			struct pt_regs *regs = task_regs (child);
-			unsigned long *loc =
-				(unsigned long *)((char *)regs + addr);
-			*loc = data;
-		} else {
 		rval = 0;
+		if (addr >= PT_SIZE && request == PTRACE_PEEKUSR) {
+			/* Special requests that don't actually correspond
+			   to offsets in struct pt_regs.  */
+			if (addr == PT_TEXT_ADDR)
+				val = child->mm->start_code;
+			else if (addr == PT_DATA_ADDR)
+				val = child->mm->start_data;
+			else if (addr == PT_TEXT_LEN)
+				val = child->mm->end_code
+					- child->mm->start_code;
+			else
 				rval = -EIO;
-		}
+		} else if (addr >= 0 && addr < PT_SIZE && (addr & 0x3) == 0) {
+			v850_reg_t *reg_addr = reg_save_addr(addr, child);
+			if (request == PTRACE_PEEKUSR)
+				val = *reg_addr;
+			else
+				*reg_addr = data;
+		} else
+			rval = -EIO;
+
+		if (rval == 0 && request == PTRACE_PEEKUSR)
+			rval = put_user (val, (unsigned long *)data);
 		goto out;
 
-	case PTRACE_SYSCALL: /* continue and stop at next (return from) syscall */
-	case PTRACE_CONT: /* rvaltart after signal. */
+	/* Continue and stop at next (return from) syscall */
+	case PTRACE_SYSCALL:
+	/* Restart after a signal.  */
+	case PTRACE_CONT:
+	/* Execute a single instruction. */
+	case PTRACE_SINGLESTEP:
 		rval = -EIO;
 		if ((unsigned long) data > _NSIG)
 			break;
+
+		/* Turn CHILD's single-step flag on or off.  */
+		if (! set_single_step (child, request == PTRACE_SINGLESTEP))
+			break;
+
 		if (request == PTRACE_SYSCALL)
 			set_tsk_thread_flag(child, TIF_SYSCALL_TRACE);
 		else
 			clear_tsk_thread_flag(child, TIF_SYSCALL_TRACE);
+
 		child->exit_code = data;
 		wake_up_process(child);
 		rval = 0;
@@ -157,6 +245,7 @@ int sys_ptrace(long request, long pid, long addr, long data)
 		break;
 
 	case PTRACE_DETACH: /* detach a process that was attached. */
+		set_single_step (child, 0);  /* Clear single-step flag */
 		rval = ptrace_detach(child, data);
 		break;