From 31f148a88ef024bacde6ca794a076a1d1aff0080 Mon Sep 17 00:00:00 2001
From: Daniel Leung <daniel.leung@intel.com>
Date: Wed, 27 Oct 2021 15:08:10 -0700
Subject: [PATCH] debug: gdbstub: add stubs to support breakpoint/watchpoint

This adds the architecture interface so that the GDB stub can
deal with breakpoints and watchpoints. By default, weak
functions are implemented to indicate breakpoints and
watchpoints are not supported.

Signed-off-by: Daniel Leung <daniel.leung@intel.com>
---
 include/sys/arch_interface.h | 30 +++++++++++++++++++++++
 subsys/debug/gdbstub.c       | 47 ++++++++++++++++++++++++++++++++++++
 2 files changed, 77 insertions(+)

diff --git a/include/sys/arch_interface.h b/include/sys/arch_interface.h
index ccaedaff701..e3d61ba020e 100644
--- a/include/sys/arch_interface.h
+++ b/include/sys/arch_interface.h
@@ -907,6 +907,36 @@ size_t arch_gdb_reg_readone(struct gdb_ctx *ctx, uint8_t *buf, size_t buflen,
 size_t arch_gdb_reg_writeone(struct gdb_ctx *ctx, uint8_t *hex, size_t hexlen,
 			     uint32_t regno);
 
+/**
+ * @brief Add breakpoint or watchpoint.
+ *
+ * @param ctx GDB context
+ * @param type Breakpoint or watchpoint type
+ * @param addr Address of breakpoint or watchpoint
+ * @param kind Size of breakpoint/watchpoint in bytes
+ *
+ * @retval 0  Operation successful
+ * @retval -1 Error encountered
+ * @retval -2 Not supported
+ */
+int arch_gdb_add_breakpoint(struct gdb_ctx *ctx, uint8_t type,
+			    uintptr_t addr, uint32_t kind);
+
+/**
+ * @brief Remove breakpoint or watchpoint.
+ *
+ * @param ctx GDB context
+ * @param type Breakpoint or watchpoint type
+ * @param addr Address of breakpoint or watchpoint
+ * @param kind Size of breakpoint/watchpoint in bytes
+ *
+ * @retval 0  Operation successful
+ * @retval -1 Error encountered
+ * @retval -2 Not supported
+ */
+int arch_gdb_remove_breakpoint(struct gdb_ctx *ctx, uint8_t type,
+			       uintptr_t addr, uint32_t kind);
+
 #endif
 /** @} */
 
diff --git a/subsys/debug/gdbstub.c b/subsys/debug/gdbstub.c
index ed46c1e08ec..04f3caf8145 100644
--- a/subsys/debug/gdbstub.c
+++ b/subsys/debug/gdbstub.c
@@ -157,6 +157,21 @@ size_t gdb_bin2hex(const uint8_t *buf, size_t buflen, char *hex, size_t hexlen)
 	return 2 * buflen;
 }
 
+__weak
+int arch_gdb_add_breakpoint(struct gdb_ctx *ctx, uint8_t type,
+			    uintptr_t addr, uint32_t kind)
+{
+	return -2;
+}
+
+__weak
+int arch_gdb_remove_breakpoint(struct gdb_ctx *ctx, uint8_t type,
+			       uintptr_t addr, uint32_t kind)
+{
+	return -2;
+}
+
+
 /**
  * Add preamble and termination to the given data.
  *
@@ -608,6 +623,7 @@ int z_gdb_main_loop(struct gdb_ctx *ctx)
 		uint8_t *ptr;
 		size_t data_len, pkt_len;
 		uintptr_t addr;
+		uint32_t type;
 		int ret;
 
 		ret = gdb_get_packet(buf, sizeof(buf), &pkt_len);
@@ -742,6 +758,37 @@ int z_gdb_main_loop(struct gdb_ctx *ctx)
 			gdb_send_packet("OK", 2);
 			break;
 
+		/*
+		 * Breakpoints and Watchpoints
+		 */
+		case 'z':
+			__fallthrough;
+		case 'Z':
+			CHECK_INT(type);
+			CHECK_SYMBOL(',');
+			CHECK_INT(addr);
+			CHECK_SYMBOL(',');
+			CHECK_INT(data_len);
+
+			if (buf[0] == 'Z') {
+				ret = arch_gdb_add_breakpoint(ctx, type,
+							      addr, data_len);
+			} else if (buf[0] == 'z') {
+				ret = arch_gdb_remove_breakpoint(ctx, type,
+								 addr, data_len);
+			}
+
+			if (ret == -2) {
+				/* breakpoint/watchpoint not supported */
+				gdb_send_packet(NULL, 0);
+			} else if (ret == -1) {
+				state = ERROR;
+			} else {
+				gdb_send_packet("OK", 2);
+			}
+
+			break;
+
 
 		/* What cause the pause  */
 		case '?':