+/**
+ * Returns the index of the watchpoint if found, -1 otherwise.
+ */
+static int gdb_watch_find(const avr_gdb_watchpoints_t * w, uint32_t addr)
+{
+ for (int i = 0; i < w->len; i++) {
+ if (w->points[i].addr > addr) {
+ return -1;
+ } else if (w->points[i].addr == addr) {
+ return i;
+ }
+ }
+
+ 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) {
+ return -1;
+ } else 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.
+ */
+static int gdb_watch_add_or_update(avr_gdb_watchpoints_t * w, enum avr_gdb_watch_type kind, uint32_t addr,
+ uint32_t size)
+{
+ /* If the watchpoint exists, update it. */
+ int i = gdb_watch_find(w, addr);
+ if (i != -1) {
+ w->points[i].size = size;
+ w->points[i].kind |= kind;
+ return 0;
+ }
+
+ /* Otherwise add it. */
+ if (w->len == WATCH_LIMIT) {
+ return -1;
+ }
+
+ /* Find the insertion point. */
+ for (i = 0; i < w->len; i++) {
+ if (w->points[i].addr > addr) {
+ break;
+ }
+ }
+
+ w->len++;
+
+ /* Make space for new element. */
+ for (int j = i + 1; j < w->len; j++) {
+ w->points[j] = w->points[j - 1];
+ }
+
+ /* Insert it. */
+ w->points[i].kind = kind;
+ w->points[i].addr = addr;
+ w->points[i].size = size;
+
+ return 0;
+}
+
+/**
+ * Returns -1 on error or if the specified point does not exist, 0 otherwise.
+ */
+static int gdb_watch_rm(avr_gdb_watchpoints_t * w, enum avr_gdb_watch_type kind, uint32_t addr)
+{
+ int i = gdb_watch_find(w, addr);
+ if (i == -1) {
+ return -1;
+ }
+
+ w->points[i].kind &= ~kind;
+ if (w->points[i].kind) {
+ return 0;
+ }
+
+ for (i = i + 1; i < w->len; i++) {
+ w->points[i - 1] = w->points[i];
+ }
+
+ w->len--;
+
+ return 0;
+}
+
+static void gdb_watch_clear(avr_gdb_watchpoints_t * w)
+{
+ w->len = 0;
+}
+