[SPARC64]: Initial KPROBES implementation.

Signed-off-by: David S. Miller <davem@davemloft.net>

arch/sparc64/Kconfig.debug

@@ -11,6 +11,16 @@ config DEBUG_STACK_USAGE
 
 	  This option will slow down process creation somewhat.
 
+config KPROBES
+	bool "Kprobes"
+	depends on DEBUG_KERNEL
+	help
+	  Kprobes allows you to trap at almost any kernel address and
+	  execute a callback function.  register_kprobe() establishes
+	  a probepoint and specifies the callback.  Kprobes is useful
+	  for kernel debugging, non-intrusive instrumentation and testing.
+	  If in doubt, say "N".
+
 config DEBUG_DCFLUSH
 	bool "D-cache flush debugging"
 	depends on DEBUG_KERNEL

arch/sparc64/kernel/Makefile

@@ -22,6 +22,7 @@ obj-$(CONFIG_BINFMT_AOUT32) += binfmt_aout32.o
 obj-$(CONFIG_MODULES) += module.o
 obj-$(CONFIG_US3_FREQ) += us3_cpufreq.o
 obj-$(CONFIG_US2E_FREQ) += us2e_cpufreq.o
+obj-$(CONFIG_KPROBES) += kprobes.o
 
 ifdef CONFIG_SUNOS_EMUL
   obj-y += sys_sunos32.o sunos_ioctl32.o

arch/sparc64/kernel/kprobes.c

+/* arch/sparc64/kernel/kprobes.c
+ *
+ * Copyright (C) 2004 David S. Miller <davem@davemloft.net>
+ */
+
+#include <linux/config.h>
+#include <linux/kernel.h>
+#include <linux/kprobes.h>
+
+#include <asm/kdebug.h>
+#include <asm/signal.h>
+
+/* We do not have hardware single-stepping, so in order
+ * to implement post handlers correctly we use two breakpoint
+ * instructions.
+ *
+ *          1) ta 0x70 --> 0x91d02070
+ *          2) ta 0x71 --> 0x91d02071
+ *
+ * When these are hit, control is transferred to kprobe_trap()
+ * below.  The arg 'level' tells us which of the two traps occurred.
+ *
+ * Initially, the instruction at p->addr gets set to "ta 0x70"
+ * by code in register_kprobe() by setting that memory address
+ * to BREAKPOINT_INSTRUCTION.  When this breakpoint is hit
+ * the following happens:
+ *
+ * 1) We run the pre-handler
+ * 2) We replace p->addr with the original opcode
+ * 3) We set the instruction at "regs->npc" to "ta 0x71"
+ * 4) We mark that we are waiting for the second breakpoint
+ *    to hit and return from the trap.
+ *
+ * At this point we wait for the second breakpoint to hit.
+ * When it does:
+ *
+ * 1) We run the post-handler
+ * 2) We re-install "ta 0x70" at p->addr
+ * 3) We restore the opcode at the "ta 0x71" breakpoint
+ * 4) We reset our "waiting for "ta 0x71" state
+ * 5) We return from the trap
+ *
+ * We could use the trick used by the i386 kprobe code but I
+ * think that scheme has problems with exception tables.  On i386
+ * they single-step over the original instruction stored at
+ * kprobe->insn.  So they set the processor to single step, and
+ * set the program counter to kprobe->insn.
+ *
+ * But that explodes if the original opcode is a user space
+ * access instruction and that faults.  It will go wrong because
+ * since the location of the instruction being executed is
+ * different from that recorded in the exception tables, the
+ * kernel will not find it and this will cause an erroneous
+ * kernel OOPS.
+ */
+
+void arch_prepare_kprobe(struct kprobe *p)
+{
+	p->insn[0] = *p->addr;
+	p->insn[1] = 0xdeadbeef;
+}
+
+static void prepare_singlestep(struct kprobe *p, struct pt_regs *regs)
+{
+	u32 *insn2 = (u32 *) regs->tpc;
+
+	p->insn[1] = *insn2;
+
+	*insn2 = BREAKPOINT_INSTRUCTION_2;
+	flushi(insn2);
+}
+
+static void undo_singlestep(struct kprobe *p, struct pt_regs *regs)
+{
+	u32 *insn2 = (u32 *) regs->tpc;
+
+	BUG_ON(p->insn[1] == 0xdeadbeef);
+
+	*insn2 = p->insn[1];
+	flushi(insn2);
+
+	p->insn[1] = 0xdeadbeef;
+}
+
+/* kprobe_status settings */
+#define KPROBE_HIT_ACTIVE	0x00000001
+#define KPROBE_HIT_SS		0x00000002
+
+static struct kprobe *current_kprobe;
+static unsigned int kprobe_status;
+
+static int kprobe_handler(struct pt_regs *regs)
+{
+	struct kprobe *p;
+	void *addr = (void *) regs->tpc;
+	int ret = 0;
+
+	preempt_disable();
+
+	if (kprobe_running()) {
+		p = get_kprobe(addr);
+		if (p) {
+			*p->addr = p->opcode;
+			flushi(p->addr);
+			ret = 1;
+		} else {
+			p = current_kprobe;
+			if (p->break_handler && p->break_handler(p, regs))
+				goto ss_probe;
+		}
+		goto no_kprobe;
+	}
+
+	lock_kprobes();
+	p = get_kprobe(addr);
+	if (!p) {
+		unlock_kprobes();
+		if (*(u32 *)addr != BREAKPOINT_INSTRUCTION)
+			ret = 1;
+		goto no_kprobe;
+	}
+
+	kprobe_status = KPROBE_HIT_ACTIVE;
+	current_kprobe = p;
+	if (p->pre_handler(p, regs))
+		return 1;
+
+ss_probe:
+	prepare_singlestep(p, regs);
+	kprobe_status = KPROBE_HIT_SS;
+	return 1;
+
+no_kprobe:
+	preempt_enable_no_resched();
+	return ret;
+}
+
+static int post_kprobe_handler(struct pt_regs *regs)
+{
+	u32 *insn_p = (u32 *) regs->tpc;
+
+	if (!kprobe_running() || (*insn_p != BREAKPOINT_INSTRUCTION_2))
+		return 0;
+
+	if (current_kprobe->post_handler)
+		current_kprobe->post_handler(current_kprobe, regs, 0);
+
+	undo_singlestep(current_kprobe, regs);
+
+	unlock_kprobes();
+	preempt_enable_no_resched();
+
+	return 1;
+}
+
+/* Interrupts disabled, kprobe_lock held. */
+static inline int kprobe_fault_handler(struct pt_regs *regs, int trapnr)
+{
+	if (current_kprobe->fault_handler
+	    && current_kprobe->fault_handler(current_kprobe, regs, trapnr))
+		return 1;
+
+	if (kprobe_status & KPROBE_HIT_SS) {
+		undo_singlestep(current_kprobe, regs);
+
+		unlock_kprobes();
+		preempt_enable_no_resched();
+	}
+	return 0;
+}
+
+/*
+ * Wrapper routine to for handling exceptions.
+ */
+int kprobe_exceptions_notify(struct notifier_block *self, unsigned long val,
+			     void *data)
+{
+	struct die_args *args = (struct die_args *)data;
+	switch (val) {
+	case DIE_DEBUG:
+		if (kprobe_handler(args->regs))
+			return NOTIFY_OK;
+		break;
+	case DIE_DEBUG_2:
+		if (post_kprobe_handler(args->regs))
+			return NOTIFY_OK;
+		break;
+	case DIE_GPF:
+		if (kprobe_running() &&
+		    kprobe_fault_handler(args->regs, args->trapnr))
+			return NOTIFY_OK;
+		break;
+	case DIE_PAGE_FAULT:
+		if (kprobe_running() &&
+		    kprobe_fault_handler(args->regs, args->trapnr))
+			return NOTIFY_OK;
+		break;
+	default:
+		break;
+	}
+	return NOTIFY_BAD;
+}
+
+asmlinkage void kprobe_trap(unsigned long trap_level, struct pt_regs *regs)
+{
+	BUG_ON(trap_level != 0x170 && trap_level != 0x171);
+
+	if (user_mode(regs)) {
+		local_irq_enable();
+		bad_trap(regs, trap_level);
+		return;
+	}
+
+	/* trap_level == 0x170 --> ta 0x70
+	 * trap_level == 0x171 --> ta 0x71
+	 */
+	if (notify_die((trap_level == 0x170) ? DIE_DEBUG : DIE_DEBUG_2,
+		       (trap_level == 0x170) ? "debug" : "debug_2",
+		       regs, 0, trap_level, SIGTRAP) != NOTIFY_OK)
+		bad_trap(regs, trap_level);
+}
+
+/* Jprobes support.  */
+static struct pt_regs jprobe_saved_regs;
+static struct sparc_stackf jprobe_saved_stack;
+
+int setjmp_pre_handler(struct kprobe *p, struct pt_regs *regs)
+{
+	struct jprobe *jp = container_of(p, struct jprobe, kp);
+
+	memcpy(&jprobe_saved_regs, regs, sizeof(*regs));
+
+	/* Save a whole stack frame, this gets arguments
+	 * pushed onto the stack after using up all the
+	 * arg registers.
+	 */
+	memcpy(&jprobe_saved_stack,
+	       (char *) (regs->u_regs[UREG_FP] + STACK_BIAS),
+	       sizeof(jprobe_saved_stack));
+
+	regs->tpc  = (unsigned long) jp->entry;
+	regs->tnpc = ((unsigned long) jp->entry) + 0x4UL;
+
+	return 1;
+}
+
+void jprobe_return(void)
+{
+	preempt_enable_no_resched();
+	__asm__ __volatile__(
+		".globl	jprobe_return_trap_instruction\n"
+"jprobe_return_trap_instruction:\n\t"
+		"ta 0x70");
+}
+
+extern void jprobe_return_trap_instruction(void);
+
+int longjmp_break_handler(struct kprobe *p, struct pt_regs *regs)
+{
+	u32 *addr = (u32 *) regs->tpc;
+
+	if (addr == (u32 *) jprobe_return_trap_instruction) {
+		/* Restore old register state.  Do pt_regs
+		 * first so that UREG_FP is the original one for
+		 * the stack frame restore.
+		 */
+		memcpy(regs, &jprobe_saved_regs, sizeof(*regs));
+
+		memcpy((char *) (regs->u_regs[UREG_FP] + STACK_BIAS),
+		       &jprobe_saved_stack,
+		       sizeof(jprobe_saved_stack));
+
+		return 1;
+	}
+	return 0;
+}

arch/sparc64/kernel/traps.c

@@ -85,11 +85,20 @@ static void dump_tl1_traplog(struct tl1_traplog *p)
 	}
 }
 
-void bad_trap (struct pt_regs *regs, long lvl)
+void do_call_debug(struct pt_regs *regs) 
+{ 
+	notify_die(DIE_CALL, "debug call", regs, 0, 255, SIGINT); 
+}
+
+void bad_trap(struct pt_regs *regs, long lvl)
 {
 	char buffer[32];
 	siginfo_t info;
 
+	if (notify_die(DIE_TRAP, "bad trap", regs,
+		       0, lvl, SIGTRAP) == NOTIFY_OK)
+		return;
+
 	if (lvl < 0x100) {
 		sprintf(buffer, "Bad hw trap %lx at tl0\n", lvl);
 		die_if_kernel(buffer, regs);
@@ -98,7 +107,7 @@ void bad_trap (struct pt_regs *regs, long lvl)
 	lvl -= 0x100;
 	if (regs->tstate & TSTATE_PRIV) {
 		sprintf(buffer, "Kernel bad sw trap %lx", lvl);
-		die_if_kernel (buffer, regs);
+		die_if_kernel(buffer, regs);
 	}
 	if (test_thread_flag(TIF_32BIT)) {
 		regs->tpc &= 0xffffffff;
@@ -112,10 +121,14 @@ void bad_trap (struct pt_regs *regs, long lvl)
 	force_sig_info(SIGILL, &info, current);
 }
 
-void bad_trap_tl1 (struct pt_regs *regs, long lvl)
+void bad_trap_tl1(struct pt_regs *regs, long lvl)
 {
 	char buffer[32];
 	
+	if (notify_die(DIE_TRAP_TL1, "bad trap tl1", regs,
+		       0, lvl, SIGTRAP) == NOTIFY_OK)
+		return;
+
 	dump_tl1_traplog((struct tl1_traplog *)(regs + 1));
 
 	sprintf (buffer, "Bad trap %lx at tl>0", lvl);
@@ -135,6 +148,10 @@ void instruction_access_exception(struct pt_regs *regs,
 {
 	siginfo_t info;
 
+	if (notify_die(DIE_TRAP, "instruction access exception", regs,
+		       0, 0x8, SIGTRAP) == NOTIFY_OK)
+		return;
+
 	if (regs->tstate & TSTATE_PRIV) {
 		printk("instruction_access_exception: SFSR[%016lx] SFAR[%016lx], going.\n",
 		       sfsr, sfar);
@@ -155,15 +172,23 @@ void instruction_access_exception(struct pt_regs *regs,
 void instruction_access_exception_tl1(struct pt_regs *regs,
 				      unsigned long sfsr, unsigned long sfar)
 {
+	if (notify_die(DIE_TRAP_TL1, "instruction access exception tl1", regs,
+		       0, 0x8, SIGTRAP) == NOTIFY_OK)
+		return;
+
 	dump_tl1_traplog((struct tl1_traplog *)(regs + 1));
 	instruction_access_exception(regs, sfsr, sfar);
 }
 
-void data_access_exception (struct pt_regs *regs,
-			    unsigned long sfsr, unsigned long sfar)
+void data_access_exception(struct pt_regs *regs,
+			   unsigned long sfsr, unsigned long sfar)
 {
 	siginfo_t info;
 
+	if (notify_die(DIE_TRAP, "data access exception", regs,
+		       0, 0x30, SIGTRAP) == NOTIFY_OK)
+		return;
+
 	if (regs->tstate & TSTATE_PRIV) {
 		/* Test if this comes from uaccess places. */
 		unsigned long fixup;
@@ -234,6 +259,10 @@ void do_iae(struct pt_regs *regs)
 
 	spitfire_clean_and_reenable_l1_caches();
 
+	if (notify_die(DIE_TRAP, "instruction access exception", regs,
+		       0, 0x8, SIGTRAP) == NOTIFY_OK)
+		return;
+
 	info.si_signo = SIGBUS;
 	info.si_errno = 0;
 	info.si_code = BUS_OBJERR;
@@ -244,6 +273,8 @@ void do_iae(struct pt_regs *regs)
 
 void do_dae(struct pt_regs *regs)
 {
+	siginfo_t info;
+
 #ifdef CONFIG_PCI
 	if (pci_poke_in_progress && pci_poke_cpu == smp_processor_id()) {
 		spitfire_clean_and_reenable_l1_caches();
@@ -258,7 +289,18 @@ void do_dae(struct pt_regs *regs)
 		return;
 	}
 #endif
-	do_iae(regs);
+	spitfire_clean_and_reenable_l1_caches();
+
+	if (notify_die(DIE_TRAP, "data access exception", regs,
+		       0, 0x30, SIGTRAP) == NOTIFY_OK)
+		return;
+
+	info.si_signo = SIGBUS;
+	info.si_errno = 0;
+	info.si_code = BUS_OBJERR;
+	info.si_addr = (void *)0;
+	info.si_trapno = 0;
+	force_sig_info(SIGBUS, &info, current);
 }
 
 static char ecc_syndrome_table[] = {
@@ -1652,6 +1694,10 @@ void do_fpe_common(struct pt_regs *regs)
 
 void do_fpieee(struct pt_regs *regs)
 {
+	if (notify_die(DIE_TRAP, "fpu exception ieee", regs,
+		       0, 0x24, SIGFPE) == NOTIFY_OK)
+		return;
+
 	do_fpe_common(regs);
 }
 
@@ -1662,6 +1708,10 @@ void do_fpother(struct pt_regs *regs)
 	struct fpustate *f = FPUSTATE;
 	int ret = 0;
 
+	if (notify_die(DIE_TRAP, "fpu exception other", regs,
+		       0, 0x25, SIGFPE) == NOTIFY_OK)
+		return;
+
 	switch ((current_thread_info()->xfsr[0] & 0x1c000)) {
 	case (2 << 14): /* unfinished_FPop */
 	case (3 << 14): /* unimplemented_FPop */
@@ -1677,6 +1727,10 @@ void do_tof(struct pt_regs *regs)
 {
 	siginfo_t info;
 
+	if (notify_die(DIE_TRAP, "tagged arithmetic overflow", regs,
+		       0, 0x26, SIGEMT) == NOTIFY_OK)
+		return;
+
 	if (regs->tstate & TSTATE_PRIV)
 		die_if_kernel("Penguin overflow trap from kernel mode", regs);
 	if (test_thread_flag(TIF_32BIT)) {
@@ -1695,6 +1749,10 @@ void do_div0(struct pt_regs *regs)
 {
 	siginfo_t info;
 
+	if (notify_die(DIE_TRAP, "integer division by zero", regs,
+		       0, 0x28, SIGFPE) == NOTIFY_OK)
+		return;
+
 	if (regs->tstate & TSTATE_PRIV)
 		die_if_kernel("TL0: Kernel divide by zero.", regs);
 	if (test_thread_flag(TIF_32BIT)) {
@@ -1800,6 +1858,7 @@ void die_if_kernel(char *str, struct pt_regs *regs)
 "                 \\__U_/\n");
 
 	printk("%s(%d): %s [#%d]\n", current->comm, current->pid, str, ++die_counter);
+	notify_die(DIE_OOPS, str, regs, 0, 255, SIGSEGV);
 	__asm__ __volatile__("flushw");
 	__show_regs(regs);
 	if (regs->tstate & TSTATE_PRIV) {
@@ -1848,6 +1907,10 @@ void do_illegal_instruction(struct pt_regs *regs)
 	u32 insn;
 	siginfo_t info;
 
+	if (notify_die(DIE_TRAP, "illegal instruction", regs,
+		       0, 0x10, SIGILL) == NOTIFY_OK)
+		return;
+
 	if (tstate & TSTATE_PRIV)
 		die_if_kernel("Kernel illegal instruction", regs);
 	if (test_thread_flag(TIF_32BIT))
@@ -1873,6 +1936,10 @@ void mem_address_unaligned(struct pt_regs *regs, unsigned long sfar, unsigned lo
 {
 	siginfo_t info;
 
+	if (notify_die(DIE_TRAP, "memory address unaligned", regs,
+		       0, 0x34, SIGSEGV) == NOTIFY_OK)
+		return;
+
 	if (regs->tstate & TSTATE_PRIV) {
 		extern void kernel_unaligned_trap(struct pt_regs *regs,
 						  unsigned int insn, 
@@ -1895,6 +1962,10 @@ void do_privop(struct pt_regs *regs)
 {
 	siginfo_t info;
 
+	if (notify_die(DIE_TRAP, "privileged operation", regs,
+		       0, 0x11, SIGILL) == NOTIFY_OK)
+		return;
+
 	if (test_thread_flag(TIF_32BIT)) {
 		regs->tpc &= 0xffffffff;
 		regs->tnpc &= 0xffffffff;

arch/sparc64/kernel/ttable.S

@@ -158,7 +158,7 @@ tl0_resv164:	BTRAP(0x164) BTRAP(0x165) BTRAP(0x166) BTRAP(0x167) BTRAP(0x168)
 tl0_resv169:	BTRAP(0x169) BTRAP(0x16a) BTRAP(0x16b) BTRAP(0x16c)
 tl0_linux64:	LINUX_64BIT_SYSCALL_TRAP
 tl0_gsctx:	TRAP(sparc64_get_context) TRAP(sparc64_set_context)
-tl0_resv170:	BTRAP(0x170) BTRAP(0x171) BTRAP(0x172)
+tl0_resv170:	KPROBES_TRAP(0x170) KPROBES_TRAP(0x171) BTRAP(0x172)
 tl0_resv173:	BTRAP(0x173) BTRAP(0x174) BTRAP(0x175) BTRAP(0x176) BTRAP(0x177)
 tl0_resv178:	BTRAP(0x178) BTRAP(0x179) BTRAP(0x17a) BTRAP(0x17b) BTRAP(0x17c)
 tl0_resv17d:	BTRAP(0x17d) BTRAP(0x17e) BTRAP(0x17f)

arch/sparc64/mm/fault.c

@@ -27,6 +27,7 @@
 #include <asm/asi.h>
 #include <asm/lsu.h>
 #include <asm/sections.h>
+#include <asm/kdebug.h>
 
 #define ELEMENTS(arr) (sizeof (arr)/sizeof (arr[0]))
 
@@ -147,6 +148,9 @@ static void unhandled_fault(unsigned long address, struct task_struct *tsk,
 	printk(KERN_ALERT "tsk->{mm,active_mm}->pgd = %016lx\n",
 	       (tsk->mm ? (unsigned long) tsk->mm->pgd :
 		          (unsigned long) tsk->active_mm->pgd));
+	if (notify_die(DIE_GPF, "general protection fault", regs,
+		       0, 0, SIGSEGV) == NOTIFY_OK)
+		return;
 	die_if_kernel("Oops", regs);
 }
 
@@ -318,8 +322,13 @@ asmlinkage void do_sparc64_fault(struct pt_regs *regs)
 	int si_code, fault_code;
 	unsigned long address;
 
-	si_code = SEGV_MAPERR;
 	fault_code = get_thread_fault_code();
+
+	if (notify_die(DIE_PAGE_FAULT, "page_fault", regs,
+		       fault_code, 0, SIGSEGV) == NOTIFY_OK)
+		return;
+
+	si_code = SEGV_MAPERR;
 	address = current_thread_info()->fault_address;
 
 	if ((fault_code & FAULT_CODE_ITLB) &&

include/asm-sparc64/kdebug.h

@@ -22,15 +22,16 @@ struct die_args {
 int register_die_notifier(struct notifier_block *nb);
 extern struct notifier_block *sparc64die_chain;
 
+extern void bad_trap(struct pt_regs *, long);
+
 /* Grossly misnamed. */
 enum die_val {
 	DIE_OOPS = 1,
-	DIE_DEBUG,
-	DIE_PANIC,
-	DIE_NMI,
+	DIE_DEBUG,	/* ta 0x70 */
+	DIE_DEBUG_2,	/* ta 0x71 */
 	DIE_DIE,
-	DIE_KERNELDEBUG,
 	DIE_TRAP,
+	DIE_TRAP_TL1,
 	DIE_GPF,
 	DIE_CALL,
 	DIE_PAGE_FAULT,

include/asm-sparc64/kprobes.h

+#ifndef _SPARC64_KPROBES_H
+#define _SPARC64_KPROBES_H
+
+#include <linux/config.h>
+#include <linux/types.h>
+
+typedef u32 kprobe_opcode_t;
+
+#define BREAKPOINT_INSTRUCTION   0x91d02070 /* ta 0x70 */
+#define BREAKPOINT_INSTRUCTION_2 0x91d02071 /* ta 0x71 */
+#define MAX_INSN_SIZE 2
+
+#ifdef CONFIG_KPROBES
+extern int kprobe_exceptions_notify(struct notifier_block *self,
+				    unsigned long val, void *data);
+#else				/* !CONFIG_KPROBES */
+static inline int kprobe_exceptions_notify(struct notifier_block *self,
+					   unsigned long val, void *data)
+{
+	return 0;
+}
+#endif
+
+#endif /* _SPARC64_KPROBES_H */

include/asm-sparc64/ttable.h

@@ -176,6 +176,12 @@
 	ba,pt	%xcc, rtrap_clr_l6;					\
 	 stx	%l2, [%sp + PTREGS_OFF + PT_V9_TNPC];
 	        
+#ifdef CONFIG_KPROBES
+#define KPROBES_TRAP(lvl) TRAP_IRQ(kprobe_trap, lvl)
+#else
+#define KPROBES_TRAP(lvl) TRAP_ARG(bad_trap, lvl)
+#endif
+
 /* Before touching these macros, you owe it to yourself to go and
  * see how arch/sparc64/kernel/winfixup.S works... -DaveM
  *