Index: arch/mips32/src/debugger.c
===================================================================
--- arch/mips32/src/debugger.c	(revision 36e7ee98b33f87f4fd7451460ffe749f290f85b7)
+++ arch/mips32/src/debugger.c	(revision d6e852916cb4672682728fa0540fe558bca857b2)
@@ -73,4 +73,63 @@
 };
 
+static cmd_arg_t adde_argv[] = {
+	{ .type = ARG_TYPE_INT },
+	{ .type = ARG_TYPE_INT }
+};
+static cmd_info_t addbkpte_info = {
+	.name = "addbkpte",
+	.description = "addebkpte <&symbol> <&func> - new bkpoint. Call func(or Nothing if 0).",
+	.func = cmd_add_breakpoint,
+	.argc = 2,
+	.argv = adde_argv
+};
+
+static struct {
+	__u32 andmask;
+	__u32 value;
+}jmpinstr[] = {
+	{0xf3ff0000, 0x41000000}, /* BCzF */
+	{0xf3ff0000, 0x41020000}, /* BCzFL */
+	{0xf3ff0000, 0x41010000}, /* BCzT */
+	{0xf3ff0000, 0x41030000}, /* BCzTL */
+	{0xfc000000, 0x10000000}, /* BEQ */
+	{0xfc000000, 0x50000000}, /* BEQL */
+	{0xfc1f0000, 0x04010000}, /* BEQL */
+	{0xfc1f0000, 0x04110000}, /* BGEZAL */
+	{0xfc1f0000, 0x04130000}, /* BGEZALL */
+	{0xfc1f0000, 0x04030000}, /* BGEZL */
+	{0xfc1f0000, 0x1c000000}, /* BGTZ */
+	{0xfc1f0000, 0x5c000000}, /* BGTZL */
+	{0xfc1f0000, 0x18000000}, /* BLEZ */
+	{0xfc1f0000, 0x58000000}, /* BLEZL */
+	{0xfc1f0000, 0x04000000}, /* BLTZ */
+	{0xfc1f0000, 0x04100000}, /* BLTZAL */
+	{0xfc1f0000, 0x04120000}, /* BLTZALL */
+	{0xfc1f0000, 0x04020000}, /* BLTZL */
+	{0xfc000000, 0x14000000}, /* BNE */
+	{0xfc000000, 0x54000000}, /* BNEL */
+	{0xfc000000, 0x08000000}, /* J */
+	{0xfc000000, 0x0c000000}, /* JAL */
+	{0xfc1f07ff, 0x00000009}, /* JALR */
+	{0,0} /* EndOfTable */
+};
+
+/** Test, if the given instruction is a jump or branch instruction
+ *
+ * @param instr Instruction code
+ * @return true - it is jump instruction, false otherwise
+ */
+static bool is_jump(__native instr)
+{
+	int i;
+
+	for (i=0;jmpinstr[i].andmask;i++) {
+		if ((instr & jmpinstr[i].andmask) == jmpinstr[i].value)
+			return true;
+	}
+
+	return false;
+}
+
 /** Add new breakpoint to table */
 int cmd_add_breakpoint(cmd_arg_t *argv)
@@ -117,5 +176,13 @@
 	cur->instruction = ((__native *)cur->address)[0];
 	cur->nextinstruction = ((__native *)cur->address)[1];
-	cur->executing = false;
+	if (argv == &add_argv) {
+		cur->flags = 0;
+	} else { /* We are add extended */
+		cur->flags = BKPOINT_FUNCCALL;
+		cur->bkfunc = 	(void (*)(void *, struct exception_regdump *)) argv[1].intval;
+	}
+	if (is_jump(cur->instruction))
+		cur->flags |= BKPOINT_ONESHOT;
+	cur->counter = 0;
 
 	/* Set breakpoint */
@@ -127,4 +194,6 @@
 	return 1;
 }
+
+
 
 /** Remove breakpoint from table */
@@ -148,5 +217,10 @@
 		return 0;
 	}
-	
+	if ((cur->flags & BKPOINT_INPROG) && (cur->flags & BKPOINT_ONESHOT)) {
+		printf("Cannot remove one-shot breakpoint in-progress\n");
+		spinlock_unlock(&bkpoint_lock);
+		interrupts_restore(ipl);
+		return 0;
+	}
 	((__u32 *)cur->address)[0] = cur->instruction;
 	((__u32 *)cur->address)[1] = cur->nextinstruction;
@@ -171,4 +245,12 @@
 			printf("%d. 0x%p in %s\n",i,
 			       breakpoints[i].address, symbol);
+			printf("     Count(%d) ", breakpoints[i].counter);
+			if (breakpoints[i].flags & BKPOINT_INPROG)
+				printf("INPROG ");
+			if (breakpoints[i].flags & BKPOINT_ONESHOT)
+				printf("ONESHOT ");
+			if (breakpoints[i].flags & BKPOINT_FUNCCALL)
+				printf("FUNCCALL ");
+			printf("\n");
 		}
 	return 1;
@@ -194,4 +276,8 @@
 	if (!cmd_register(&addbkpt_info))
 		panic("could not register command %s\n", addbkpt_info.name);
+
+	cmd_initialize(&addbkpte_info);
+	if (!cmd_register(&addbkpte_info))
+		panic("could not register command %s\n", addbkpte_info.name);
 }
 
@@ -206,10 +292,7 @@
 void debugger_bpoint(struct exception_regdump *pstate)
 {
-	char *symbol;
 	bpinfo_t *cur = NULL;
-	int i;
-
-	symbol = get_symtab_entry(pstate->epc);
-	
+	__address fireaddr = pstate->epc;
+	int i;
 
 	/* test branch delay slot */
@@ -219,44 +302,78 @@
 	spinlock_lock(&bkpoint_lock);
 	for (i=0; i<BKPOINTS_MAX; i++) {
-		if (pstate->epc == breakpoints[i].address || \
-		    (pstate->epc == breakpoints[i].address+sizeof(__native) &&\
-		     breakpoints[i].executing))
+		/* Normal breakpoint */
+		if (fireaddr == breakpoints[i].address \
+		    && !(breakpoints[i].flags & BKPOINT_REINST)) {
 			cur = &breakpoints[i];
-		break;
-
+			break;
+		}
+		/* Reinst only breakpoint */
+		if ((breakpoints[i].flags & BKPOINT_REINST) \
+		    && (fireaddr ==breakpoints[i].address+sizeof(__native))) {
+			cur = &breakpoints[i];
+			break;
+		}
 	}
 	if (cur) {
-		if ((cur->executing && pstate->epc==cur->address) ||
-		    (!cur->executing && pstate->epc==cur->address+sizeof(__native)))
-			panic("Weird breakpoint state.\n");
-		if (!cur->executing) {
-			printf("***Breakpoint %d: 0x%p in %s.\n", i, 
-			       pstate->epc,symbol);
-			/* Return first instruction back */
-			((__u32 *)cur->address)[0] = cur->instruction;
-			/* Set Breakpoint on second */
-			((__u32 *)cur->address)[1] = 0x0d;
-			cur->executing = true;
-		} else {
+		if (cur->flags & BKPOINT_REINST) {
 			/* Set breakpoint on first instruction */
 			((__u32 *)cur->address)[0] = 0x0d;
 			/* Return back the second */
 			((__u32 *)cur->address)[1] = cur->nextinstruction;
-			cur->executing = false;
+			cur->flags &= ~BKPOINT_REINST;
 			spinlock_unlock(&bkpoint_lock);
 			return;
-		}
+		} 
+		if (cur->flags & BKPOINT_INPROG)
+			printf("Warning: breakpoint recursion\n");
+		
+		if (!(cur->flags & BKPOINT_FUNCCALL))
+			printf("***Breakpoint %d: 0x%p in %s.\n", i, 
+			       fireaddr, get_symtab_entry(pstate->epc));
+
+		/* Return first instruction back */
+		((__u32 *)cur->address)[0] = cur->instruction;
+
+		if (! (cur->flags & BKPOINT_ONESHOT)) {
+			/* Set Breakpoint on next instruction */
+			((__u32 *)cur->address)[1] = 0x0d;
+			cur->flags |= BKPOINT_REINST;
+		} 
+		cur->flags |= BKPOINT_INPROG;
 	} else {
-		printf("***Breakpoint 0x%p in %s.\n", pstate->epc, symbol);
+		printf("***Breakpoint 0x%p in %s.\n", fireaddr, 
+		       get_symtab_entry(fireaddr));
 		/* Move on to next instruction */
 		pstate->epc += 4;
 	}
+	
+	cur->counter++;
+	if (cur && (cur->flags & BKPOINT_FUNCCALL)) {
+		/* Allow zero bkfunc, just for counting */
+		if (cur->bkfunc)
+			cur->bkfunc(cur, pstate);
+	} else {
+		printf("***Type 'exit' to exit kconsole.\n");
+		/* This disables all other processors - we are not SMP,
+		 * actually this gets us to cpu_halt, if scheduler() is run
+		 * - we generally do not want scheduler to be run from debug,
+		 *   so this is a good idea
+		 */	
+		atomic_set(&haltstate,1);
+		spinlock_unlock(&bkpoint_lock);
+
+		kconsole("debug");
+
+		spinlock_lock(&bkpoint_lock);
+		atomic_set(&haltstate,0);
+	}
+
+	if (cur && cur->address == fireaddr && (cur->flags & BKPOINT_INPROG)) {
+		/* Remove one-shot breakpoint */
+		if ((cur->flags & BKPOINT_ONESHOT))
+			cur->address = NULL;
+		/* Remove in-progress flag */
+		cur->flags &= ~BKPOINT_INPROG;
+	} 
 	spinlock_unlock(&bkpoint_lock);
-
-			
-	printf("***Type 'exit' to exit kconsole.\n");
-	/* Umm..we should rather set some 'debugstate' here */
-	atomic_set(&haltstate,1);
-	kconsole("debug");
-	atomic_set(&haltstate,0);
-}
+}