From 909198fa8bd460772d3729a85444333ab4ef38d7 Mon Sep 17 00:00:00 2001
From: Jakob Gruber <jakob.gruber@gmail.com>
Date: Fri, 13 Jul 2012 14:22:51 +0200
Subject: [PATCH] gdb: Implemented watchpoint handling

Per data access, we never signal more than one watchpoint. Possible
scenarios are several watchpoints at the same address (for example
WATCH_ACCESS and WATCH_WRITE) or overlapping watchpoint ranges. This is
not completely correct.
---
 simavr/sim/sim_core.c | 11 +++++++++++
 simavr/sim/sim_gdb.c  | 45 +++++++++++++++++++++++++++++++++++++++++++
 simavr/sim/sim_gdb.h  |  3 +++
 3 files changed, 59 insertions(+)

diff --git a/simavr/sim/sim_core.c b/simavr/sim/sim_core.c
index 5ceb7f3..f3106ef 100644
--- a/simavr/sim/sim_core.c
+++ b/simavr/sim/sim_core.c
@@ -25,6 +25,7 @@
 #include <ctype.h>
 #include "sim_avr.h"
 #include "sim_core.h"
+#include "sim_gdb.h"
 #include "avr_flash.h"
 #include "avr_watchdog.h"
 
@@ -119,6 +120,11 @@ void avr_core_watch_write(avr_t *avr, uint16_t addr, uint8_t v)
 		printf( FONT_RED "%04x : munching stack SP %04x, A=%04x <= %02x\n" FONT_DEFAULT, avr->pc, _avr_sp_get(avr), addr, v);
 	}
 #endif
+
+	if (avr->gdb) {
+		avr_gdb_handle_watchpoints(avr, addr, AVR_GDB_WATCH_WRITE);
+	}
+
 	avr->data[addr] = v;
 }
 
@@ -129,6 +135,11 @@ uint8_t avr_core_watch_read(avr_t *avr, uint16_t addr)
 				avr->pc, _avr_sp_get(avr), avr->flash[avr->pc + 1] | (avr->flash[avr->pc]<<8), addr, avr->ramend);
 		CRASH();
 	}
+
+	if (avr->gdb) {
+		avr_gdb_handle_watchpoints(avr, addr, AVR_GDB_WATCH_READ);
+	}
+
 	return avr->data[addr];
 }
 
diff --git a/simavr/sim/sim_gdb.c b/simavr/sim/sim_gdb.c
index 727b8ed..c4eb01b 100644
--- a/simavr/sim/sim_gdb.c
+++ b/simavr/sim/sim_gdb.c
@@ -73,6 +73,22 @@ static int gdb_watch_find(const avr_gdb_watchpoints_t * w, uint32_t addr)
 	return -1;
 }
 
+/**
+ * Contrary to gdb_watch_find, this actually checks the address against
+ * a watched memory _range_.
+ */
+static int gdb_watch_find_range(const avr_gdb_watchpoints_t * w, uint32_t addr)
+{
+	for (int i = 0; i < w->len; i++) {
+		if (w->points[i].addr <= addr &&
+				addr < w->points[i].addr + w->points[i].size) {
+			return i;
+		}
+	}
+
+	return -1;
+}
+
 /**
  * Returns -1 on error, 0 otherwise.
  */
@@ -444,6 +460,35 @@ static int gdb_network_handler(avr_gdb_t * g, uint32_t dosleep)
 	return 1;
 }
 
+/**
+ * If an applicable watchpoint exists for addr, stop the cpu and send a status report.
+ * type is one of AVR_GDB_WATCH_READ, AVR_GDB_WATCH_WRITE depending on the type of access.
+ */
+void avr_gdb_handle_watchpoints(avr_t * avr, uint16_t addr, enum avr_gdb_watch_type type)
+{
+	avr_gdb_t *g = avr->gdb;
+
+	int i = gdb_watch_find_range(&g->watchpoints, addr);
+	if (i == -1) {
+		return;
+	}
+
+	int kind = g->watchpoints.points[i].kind;
+	if (kind & type) {
+		/* Send gdb reply (see GDB user manual appendix E.3). */
+		char cmd[78];
+		sprintf(cmd, "T%02x20:%02x;21:%02x%02x;22:%02x%02x%02x00;%s:%06x;",
+				5, g->avr->data[R_SREG],
+				g->avr->data[R_SPL], g->avr->data[R_SPH],
+				g->avr->pc & 0xff, (g->avr->pc>>8)&0xff, (g->avr->pc>>16)&0xff,
+				kind & AVR_GDB_WATCH_ACCESS ? "awatch" : kind & AVR_GDB_WATCH_WRITE ? "watch" : "rwatch",
+				addr | 0x800000);
+		gdb_send_reply(g, cmd);
+
+		avr->state = cpu_Stopped;
+	}
+}
+
 int avr_gdb_processor(avr_t * avr, int sleep)
 {
 	if (!avr || !avr->gdb)
diff --git a/simavr/sim/sim_gdb.h b/simavr/sim/sim_gdb.h
index a9e8aa4..2c672c3 100644
--- a/simavr/sim/sim_gdb.h
+++ b/simavr/sim/sim_gdb.h
@@ -42,6 +42,9 @@ int avr_gdb_init(avr_t * avr);
 // call from the main AVR decoder thread
 int avr_gdb_processor(avr_t * avr, int sleep);
 
+// Called from sim_core.c
+void avr_gdb_handle_watchpoints(avr_t * g, uint16_t addr, enum avr_gdb_watch_type type);
+
 #ifdef __cplusplus
 };
 #endif
-- 
2.39.5