Implement the "vCont" GDB packet

7 files changed, 74 insertions, 13 deletions

diff --git a/src/rtos/riscv_debug.c b/src/rtos/riscv_debug.c
index 32e3c64..4161f58 100644
--- a/src/rtos/riscv_debug.c
+++ b/src/rtos/riscv_debug.c
@@ -10,6 +10,7 @@
 
 static int riscv_update_threads(struct rtos *rtos);
 static int riscv_gdb_thread_packet(struct connection *connection, const char *packet, int packet_size);
+static int riscv_gdb_v_packet(struct connection *connection, const char *packet, int packet_size);
 
 static int riscv_detect_rtos(struct target *target)
 {
@@ -34,6 +35,7 @@ static int riscv_create_rtos(struct target *target)
 	riscv_update_threads(target->rtos);
 
 	target->rtos->gdb_thread_packet = riscv_gdb_thread_packet;
+	target->rtos->gdb_v_packet = riscv_gdb_v_packet;
 
 	return JIM_OK;
 }
@@ -67,10 +69,10 @@ static int riscv_gdb_thread_packet(struct connection *connection, const char *pa
 	struct rtos *rtos = target->rtos;
 	struct riscv_rtos *r = (struct riscv_rtos *)(target->rtos->rtos_specific_params);
 
-		char *packet_stttrr = malloc(packet_size + 1);
-		memset(packet_stttrr, '\0', packet_size + 1);
-		memcpy(packet_stttrr, packet, packet_size);
-		LOG_DEBUG("riscv_gdb_thread_packet(%s)", packet_stttrr);
+	char *packet_stttrr = malloc(packet_size + 1);
+	memset(packet_stttrr, '\0', packet_size + 1);
+	memcpy(packet_stttrr, packet, packet_size);
+	LOG_DEBUG("handling packet '%s'", packet_stttrr);
 
 	switch (packet[0]) {
 	case 'q':
@@ -205,7 +207,6 @@ static int riscv_gdb_thread_packet(struct connection *connection, const char *pa
 		}
 	}
 
-
 	case 'c':
 	case 's':
 		target->state = TARGET_HALTED;
@@ -227,6 +228,44 @@ static int riscv_gdb_thread_packet(struct connection *connection, const char *pa
 	}
 }
 
+static int riscv_gdb_v_packet(struct connection *connection, const char *packet, int packet_size)
+{
+	char *packet_stttrr = malloc(packet_size + 1);
+	memset(packet_stttrr, '\0', packet_size + 1);
+	memcpy(packet_stttrr, packet, packet_size);
+	LOG_DEBUG("handling packet '%s'", packet_stttrr);
+
+	struct target *target = get_target_from_connection(connection);
+
+	if (strcmp(packet_stttrr, "vCont?") == 0) {
+		static const char *message = "OK";
+		gdb_put_packet(connection, message, strlen(message));
+		return JIM_OK;
+	}
+
+	int threadid;
+	if (sscanf(packet_stttrr, "vCont;s:%d;c", &threadid) == 1) {
+		riscv_set_rtos_hartid(target, threadid - 1);
+		riscv_step_rtos_hart(target);
+
+		gdb_put_packet(connection, "S02", 3);
+		return JIM_OK;
+	}
+
+	if (strcmp(packet_stttrr, "vCont;c") == 0) {
+		riscv_resume_all_harts(target);
+		target->state = TARGET_RUNNING;
+		target_call_event_callbacks(target, TARGET_EVENT_GDB_START);
+		gdb_set_frontend_state_running(connection);
+		return JIM_OK;
+	}
+
+	if (strncmp(packet_stttrr, "vCont", 5) == 0)
+		LOG_ERROR("Got unknown vCont-type packet");
+
+	return GDB_THREAD_PACKET_NOT_CONSUMED;
+}
+
 static int riscv_get_thread_reg_list(struct rtos *rtos, int64_t thread_id, char **hex_reg_list)
 {
 	LOG_DEBUG("Updating RISC-V regiser list for hart %d", (int)(thread_id - 1));
diff --git a/src/rtos/rtos.c b/src/rtos/rtos.c
index 76443b6..9bf218e 100644
--- a/src/rtos/rtos.c
+++ b/src/rtos/rtos.c
@@ -72,6 +72,7 @@ static int os_alloc(struct target *target, struct rtos_type *ostype)
 
 	/* RTOS drivers can override the packet handler in _create(). */
 	os->gdb_thread_packet = rtos_thread_packet;
+	os->gdb_v_packet = NULL;
 
 	return JIM_OK;
 }
diff --git a/src/rtos/rtos.h b/src/rtos/rtos.h
index 70c1193..70cec87 100644
--- a/src/rtos/rtos.h
+++ b/src/rtos/rtos.h
@@ -54,6 +54,7 @@ struct rtos {
 	struct thread_detail *thread_details;
 	int thread_count;
 	int (*gdb_thread_packet)(struct connection *connection, char const *packet, int packet_size);
+	int (*gdb_v_packet)(struct connection *connection, char const *packet, int packet_size);
 	void *rtos_specific_params;
 };
 
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index 2387496..4828c21 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -2445,6 +2445,13 @@ static int gdb_v_packet(struct connection *connection,
 	struct gdb_service *gdb_service = connection->service->priv;
 	int result;
 
+	struct target *target = get_target_from_connection(connection);
+	if (target->rtos != NULL && target->rtos->gdb_v_packet != NULL) {
+		int out = target->rtos->gdb_v_packet(connection, packet, packet_size);
+		if (out != GDB_THREAD_PACKET_NOT_CONSUMED)
+			return out;
+	}
+
 	/* if flash programming disabled - send a empty reply */
 
 	if (gdb_flash_program == 0) {
@@ -2643,7 +2650,7 @@ static void gdb_log_callback(void *priv, const char *file, unsigned line,
 	gdb_output_con(connection, string);
 }
 
-static void gdb_sig_halted(struct connection *connection)
+void gdb_sig_halted(struct connection *connection)
 {
 	char sig_reply[4];
 	snprintf(sig_reply, 4, "T%2.2x", 2);
@@ -3195,3 +3202,9 @@ int gdb_register_commands(struct command_context *cmd_ctx)
 	gdb_port_next = strdup("3333");
 	return register_commands(cmd_ctx, NULL, gdb_command_handlers);
 }
+
+void gdb_set_frontend_state_running(struct connection *connection)
+{
+	struct gdb_connection *gdb_con = connection->priv;
+	gdb_con->frontend_state = TARGET_RUNNING;
+}
diff --git a/src/server/gdb_server.h b/src/server/gdb_server.h
index 2b4ac4e..1dc30e0 100644
--- a/src/server/gdb_server.h
+++ b/src/server/gdb_server.h
@@ -45,6 +45,9 @@ static inline struct target *get_target_from_connection(struct connection *conne
 	return gdb_service->target;
 }
 
+void gdb_set_frontend_state_running(struct connection *connection);
+void gdb_sig_halted(struct connection *connection);
+
 #define ERROR_GDB_BUFFER_TOO_SMALL (-800)
 #define ERROR_GDB_TIMEOUT (-801)
 
diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index ad69263..485a385 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -1355,8 +1355,6 @@ static int read_memory(struct target *target, uint32_t address,
 		  return ERROR_FAIL;
 		}
 
-		LOG_INFO("read 0x%08x from 0x%08x", value, t_addr);
-
 		if (check_dmi_error(target)) {
 			LOG_ERROR("DMI error");
 			return ERROR_FAIL;
@@ -1424,8 +1422,6 @@ static int write_memory(struct target *target, uint32_t address,
 		abstract_write_register(target, S1, riscv_xlen(target), value);
 		program_set_write(program, S1, value);
 
-		LOG_INFO("writing 0x%08x to 0x%08x", value, t_addr);
-
 		write_program(target, program);
 		execute_program(target, program);
 		uint32_t abstractcs;
@@ -1622,7 +1618,7 @@ static void riscv013_on_step_or_resume(struct target *target, bool step)
 static void riscv013_step_or_resume_current_hart(struct target *target, bool step)
 {
 	RISCV_INFO(r);
-	LOG_DEBUG("resuming hart %d", r->current_hartid);
+	LOG_DEBUG("resuming hart %d (for step?=%d)", r->current_hartid, step);
 	assert(riscv_is_halted(target));
 
 	/* Issue the halt command, and then wait for the current hart to halt. */
@@ -1654,4 +1650,7 @@ static void riscv013_step_or_resume_current_hart(struct target *target, bool ste
 
 	dmcontrol = set_field(dmcontrol, DMI_DMCONTROL_RESUMEREQ, 0);
 	dmi_write(target, DMI_DMCONTROL, dmcontrol);
+
+	/* When stepping we need to go and restore the relevant registers. */
+	if (step) riscv013_on_halt(target);
 }
diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index bb132d6..c1855b0 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -726,6 +726,8 @@ int riscv_openocd_step(
         uint32_t address,
         int handle_breakpoints
 ) {
+	RISCV_INFO(r);
+
 	if (!current) {
 		LOG_ERROR("step-at-pc unimplemented");
 		return ERROR_FAIL;
@@ -735,7 +737,11 @@ int riscv_openocd_step(
 	if (out != ERROR_OK)
 		return out;
 
+	/* step_rtos_hart blocks until the hart has actually stepped, but we
+	 * need to cycle through OpenOCD to  */
 	target->state = TARGET_RUNNING;
+	riscv_openocd_poll(target);
+
 	return out;
 }
 
@@ -888,8 +894,6 @@ int riscv_step_rtos_hart(struct target *target)
 	assert(r->hart_state[hartid] == RISCV_HART_HALTED);
 	r->on_step(target);
 	r->step_current_hart(target);
-	/* FIXME: There's a race condition with step. */
-	r->hart_state[hartid] = RISCV_HART_RUNNING;
 	return ERROR_OK;
 }
 
@@ -957,6 +961,7 @@ void riscv_set_all_rtos_harts(struct target *target)
 
 void riscv_set_rtos_hartid(struct target *target, int hartid)
 {
+	LOG_DEBUG("setting RTOS hartid %d", hartid);
 	RISCV_INFO(r);
 	r->rtos_hartid = hartid;
 }