[PATCH 21/23] sim/erc32: add data watchpoint support for all cpu targets.

[Jiri Gaisler <jiri@gaisler.se>]

	Add watchpoint to all processor targets (erc32, leon2, leon3).
---
 sim/erc32/exec.c | 54 +++++++++++++++++++++++++++------
 sim/erc32/func.c | 92 ++++++++++++++++++++++++++++++++++++++++++++++++++++++--
 sim/erc32/sis.c  | 32 +++++++++++++++-----
 sim/erc32/sis.h  | 23 +++++++++++++-
 4 files changed, 180 insertions(+), 21 deletions(-)

diff --git a/sim/erc32/exec.c b/sim/erc32/exec.c
index f812ad9..a81089a 100644
--- a/sim/erc32/exec.c
+++ b/sim/erc32/exec.c
@@ -395,6 +395,20 @@ extract_byte_signed(uint32 data, uint32 address)
     return(tmp);
 }
 
+/* Decode watchpoint address mask from opcode. Not correct for LDST,
+   SWAP and STFSR but watchpoints will work anyway. */
+
+static unsigned char
+wpmask(uint32 op3)
+{
+    switch (op3 & 3) {
+      case 0: return(3);  /* word */
+      case 1: return(0);  /* byte */
+      case 2: return(1);  /* half-word */
+      case 3: return(7);  /* double word */
+    }
+}
+
 int
 dispatch_instruction(sregs)
     struct pstate  *sregs;
@@ -698,6 +712,12 @@ dispatch_instruction(sregs)
 		}
 		if (eicc & 1) {
 		    sregs->trap = (0x80 | ((rs1 + operand2) & 0x7f));
+		    if ((sregs->trap == 129) && (sis_gdb_break) &&
+		    		(sregs->inst == 0x91d02001))
+		    {
+			sregs->trap = WPT_TRAP;
+			sregs->bphit = 1;
+		    }
 		}
 		break;
 
@@ -1211,18 +1231,25 @@ dispatch_instruction(sregs)
 
 	address = rs1 + operand2;
 
-	if (sregs->psr & PSR_S)
-	    asi = 11;
-	 else
-	    asi = 10;
-
 	if (op3 & 4) {
 	    sregs->icnt = T_ST;	/* Set store instruction count */
+	    if (sregs->wpwnum) {
+	       if (sregs->wphit = check_wpw(sregs, address, wpmask(op3))) {
+	           sregs->trap = WPT_TRAP;
+		   break;
+	       }
+	    }
 #ifdef STAT
 	    sregs->nstore++;
 #endif
 	} else {
 	    sregs->icnt = T_LD;	/* Set load instruction count */
+	    if (sregs->wprnum) {
+	       if (sregs->wphit = check_wpr(sregs, address, wpmask(op3))) {
+	           sregs->trap = WPT_TRAP;
+		   break;
+	       }
+	    }
 #ifdef STAT
 	    sregs->nload++;
 #endif
@@ -2133,12 +2160,18 @@ execute_trap(sregs)
 {
     int32           cwp;
 
-    if (sregs->trap == 256) {
-	sregs->pc = 0;
-	sregs->npc = 4;
-	sregs->trap = 0;
-    } else if (sregs->trap == 257) {
+    if (sregs->trap >= 256) {
+	switch (sregs->trap) {
+	case 256:
+	    sregs->pc = 0;
+	    sregs->npc = 4;
+	    sregs->trap = 0;
+	    break;
+	case ERROR_TRAP:
 	    return (ERROR);
+	case WPT_TRAP:
+	    return (WPT_HIT);
+	}
     } else {
 
 	if ((sregs->psr & PSR_ET) == 0)
@@ -2231,6 +2264,7 @@ init_regs(sregs)
     sregs->fpu_pres = !nfp;
     set_fsr(sregs->fsr);
     sregs->bphit = 0;
+    sregs->wphit = 0;
     sregs->ildreg = 0;
     sregs->ildtime = 0;
 
diff --git a/sim/erc32/func.c b/sim/erc32/func.c
index 6f985a1..b4e0755 100644
--- a/sim/erc32/func.c
+++ b/sim/erc32/func.c
@@ -60,7 +60,8 @@ uint32		last_load_addr = 0;
 int		nouartrx = 0;
 host_callback 	*sim_callback;
 struct memsys *ms = &erc32sys;
-int		cputype = 0;		/* 0 = erc32, 3 = leon3 */
+int		cputype = 0;		/* 0 = erc32, 2 = leon2,3 = leon3 */
+int             sis_gdb_break;
 
 #ifdef ERRINJ
 uint32		errcnt = 0;
@@ -619,7 +620,54 @@ exec_cmd(sregs, cmd)
 	    stat = run_sim(sregs, UINT64_MAX, 0);
 	    daddr = sregs->pc;
 	    ms->sim_halt();
-	} else
+	} else if (strncmp(cmd1, "wp", clen) == 0) {
+	    for (i = 0; i < sregs->wprnum; i++) {
+		printf("  %d : 0x%08x (read)\n", i + 1, sregs->wprs[i]);
+	    }
+	    for (i = 0; i < sregs->wpwnum; i++) {
+		printf("  %d : 0x%08x (write)\n", i + 1, sregs->wpws[i]);
+	    }
+	} else if (strncmp(cmd1, "+wpr", clen) == 0) {
+	    if ((cmd1 = strtok(NULL, " \t\n\r")) != NULL) {
+		sregs->wprs[sregs->wprnum] = VAL(cmd1) & ~0x3;
+		sregs->wprm[sregs->wprnum] = 3;
+		printf("added read watchpoint %d at 0x%08x\n",
+		       sregs->wprnum + 1, sregs->wprs[sregs->wprnum]);
+		sregs->wprnum += 1;
+	    }
+	} else if (strncmp(cmd1, "-wpr", clen) == 0) {
+	    if ((cmd1 = strtok(NULL, " \t\n\r")) != NULL) {
+		i = VAL(cmd1) - 1;
+		if ((i >= 0) && (i < sregs->wprnum)) {
+		    printf("deleted read watchpoint %d at 0x%08x\n", i + 1,
+			   sregs->wprs[i]);
+		    for (; i < sregs->wprnum - 1; i++) {
+			sregs->wprs[i] = sregs->wprs[i + 1];
+		    }
+		    sregs->wprnum -= 1;
+		}
+	    }
+	} else if (strncmp(cmd1, "+wpw", clen) == 0) {
+	    if ((cmd1 = strtok(NULL, " \t\n\r")) != NULL) {
+		sregs->wpws[sregs->wpwnum] = VAL(cmd1) & ~0x3;
+		sregs->wpwm[sregs->wpwnum] = 3;
+		printf("added write watchpoint %d at 0x%08x\n",
+		       sregs->wpwnum + 1, sregs->wpws[sregs->wpwnum]);
+		sregs->wpwnum += 1;
+	    }
+	} else if (strncmp(cmd1, "-wpw", clen) == 0) {
+	    if ((cmd1 = strtok(NULL, " \t\n\r")) != NULL) {
+		i = VAL(cmd1) - 1;
+		if ((i >= 0) && (i < sregs->wpwnum)) {
+		    printf("deleted write watchpoint %d at 0x%08x\n", i + 1,
+			   sregs->wpws[i]);
+		    for (; i < sregs->wpwnum - 1; i++) {
+			sregs->wpws[i] = sregs->wpws[i + 1];
+		    }
+		    sregs->wpwnum -= 1;
+		}
+	    }
+	} else 
 	    printf("syntax error\n");
     }
     if (cmdsave != NULL)
@@ -703,6 +751,8 @@ init_bpt(sregs)
     struct pstate  *sregs;
 {
     sregs->bptnum = 0;
+    sregs->wprnum = 0;
+    sregs->wpwnum = 0;
     sregs->histlen = 0;
     sregs->histind = 0;
     sregs->histbuf = NULL;
@@ -1012,6 +1062,44 @@ check_bpt(sregs)
     return (0);
 }
 
+int
+check_wpr(sregs, address, mask)
+    struct pstate  *sregs;
+    int32          address;
+    unsigned char  mask;
+{
+    int32           i, msk;
+
+    for (i = 0; i < sregs->wprnum; i++) {
+	msk = ~(mask | sregs->wprm[i]);
+	if (((address ^ sregs->wprs[i]) & msk) == 0) {
+	    sregs->wpaddress = address;
+            if (sregs->wphit) return (0);
+	    return (WPT_HIT);
+	}
+    }
+    return (0);
+}
+
+int
+check_wpw(sregs, address, mask)
+    struct pstate  *sregs;
+    int32          address;
+    unsigned char  mask;
+{
+    int32           i, msk;
+
+    for (i = 0; i < sregs->wpwnum; i++) {
+	msk = ~(mask | sregs->wpwm[i]);
+	if (((address ^ sregs->wpws[i]) & msk) == 0) {
+	    sregs->wpaddress = address;
+            if (sregs->wphit) return (0);
+	    return (WPT_HIT);
+	}
+    }
+    return (0);
+}
+
 void
 reset_all()
 {
diff --git a/sim/erc32/sis.c b/sim/erc32/sis.c
index f21ddde..345f867 100644
--- a/sim/erc32/sis.c
+++ b/sim/erc32/sis.c
@@ -74,11 +74,7 @@ run_sim(sregs, icount, dis)
 		    sregs->trap = I_ACC_EXC;
 		} else {
 		    if (deb) {
-	    		if ((sregs->bphit = check_bpt(sregs)) != 0) {
-            		    ms->restore_stdio();
-	    		    return (BPT_HIT);
-	    		}
-		        if (sregs->histlen) {
+			if (sregs->histlen) {
 			    sregs->histbuf[sregs->histind].addr = sregs->pc;
 			    sregs->histbuf[sregs->histind].time = ebase.simtime;
 			    sregs->histind++;
@@ -89,14 +85,25 @@ run_sim(sregs, icount, dis)
 			    printf(" %8llu ", ebase.simtime);
 			    dis_mem(sregs->pc, 1, &dinfo);
 		        }
+			if ((sregs->bptnum) && (sregs->bphit = check_bpt(sregs)))
+	                    icount = 0;
+			else {
+		            dispatch_instruction(sregs);
+		            icount--;
+			}
+		    } else {
+		        dispatch_instruction(sregs);
+		        icount--;
 		    }
-		    dispatch_instruction(sregs);
-		    icount--;
 		}
 	    }
 	    if (sregs->trap) {
 		irq = 0;
-		sregs->err_mode = execute_trap(sregs);
+		if ((sregs->err_mode = execute_trap(sregs)) == WPT_HIT) {
+		    sregs->err_mode = 0;
+                    sregs->trap = 0;
+	            icount = 0;
+		}
         	if (sregs->err_mode) {
 	            ms->error_mode(sregs->pc);
 	            icount = 0;
@@ -117,6 +124,10 @@ run_sim(sregs, icount, dis)
 	ctrl_c = 0;
 	return (CTRL_C);
     }
+    if (sregs->bphit)
+        return (BPT_HIT);
+    if (sregs->wphit)
+        return (WPT_HIT);
     return (TIME_OUT);
 }
 
@@ -283,6 +294,11 @@ main(argc, argv)
 	    printf(" %8llu ", ebase.simtime);
 	    dis_mem(sregs.pc, 1, &dinfo);
 	    break;
+	case WPT_HIT:
+	    printf("watchpoint at 0x%08x reached, pc = 0x%08x\n",
+	        sregs.wpaddress, sregs.pc);
+	    sregs.wphit = 1;
+	    break;
 	default:
 	    break;
 	}
diff --git a/sim/erc32/sis.h b/sim/erc32/sis.h
index 637549a..93e0da9 100644
--- a/sim/erc32/sis.h
+++ b/sim/erc32/sis.h
@@ -38,8 +38,10 @@
 /* Maximum # of floating point queue */
 #define FPUQN	1
 
-/* Maximum # of breakpoints */
+/* Maximum # of breakpoints and watchpoints */
 #define BPT_MAX	256
+#define WPR_MAX	256
+#define WPW_MAX	256
 
 struct histype {
     unsigned        addr;
@@ -102,6 +104,14 @@ struct pstate {
     uint32          bptnum;
     uint32          bphit;
     uint32          bpts[BPT_MAX];	/* Breakpoints */
+    uint32          wprnum;
+    uint32          wphit;
+    uint32          wprs[WPR_MAX];	/* Read Watchpoints */
+    unsigned char   wprm[WPR_MAX];	/* Read Watchpoint masks*/
+    uint32          wpwnum;
+    uint32          wpws[WPW_MAX];	/* Write Watchpoints */
+    unsigned char   wpwm[WPW_MAX];	/* Write Watchpoint masks */
+    uint32          wpaddress;
 
     uint32          ltime;	/* Load interlock time */
     uint32          hold;	/* IU hold cycles in current inst */
@@ -178,12 +188,19 @@ struct memsys {
 };
 
 
+/* return values for run_sim */
 #define OK 0
 #define TIME_OUT 1
 #define BPT_HIT 2
 #define ERROR 3
 #define CTRL_C 4
+#define WPT_HIT 5
 
+/* special simulator trap types */
+#define ERROR_TRAP 257
+#define WPT_TRAP   258
+
+/* cpu type defines */
 #define CPU_LEON2  2
 #define CPU_LEON3  3
 
@@ -234,6 +251,9 @@ extern void	advance_time (struct pstate  *sregs);
 extern uint32	now (void);
 extern int	wait_for_irq (void);
 extern int	check_bpt (struct pstate *sregs);
+extern int 	check_wpr(struct pstate *sregs, int32 address, unsigned char mask);
+extern int 	check_wpw(struct pstate *sregs, int32 address, unsigned char mask);
+
 extern void	reset_all (void);
 extern void	sys_reset (void);
 extern void	sys_halt (void);
@@ -243,6 +263,7 @@ extern          host_callback *sim_callback;
 extern int	current_target_byte_order;
 extern int      dumbio;
 extern int      cputype;
+extern int	sis_gdb_break;
 
 /* exec.c */
 extern int	dispatch_instruction (struct pstate *sregs);
-- 
2.1.0