Fix reading of non-general registers for hwthread

Previously the code made the assumption (which is valid for conventional
RTOSs) that special registers (e.g. CSRs) are the same across threads.

26/45 tests pass.

Change-Id: Ibb3398790d7354a995d506772375d869f608f1f0

src/rtos/hwthread.c

@@ -31,6 +31,8 @@
 static bool hwthread_detect_rtos(struct target *target);
 static int hwthread_create(struct target *target);
 static int hwthread_update_threads(struct rtos *rtos);
+static int hwthread_get_thread_reg(struct rtos *rtos, int64_t thread_id,
+		uint32_t reg_num, struct rtos_reg *rtos_reg);
 static int hwthread_get_thread_reg_list(struct rtos *rtos, int64_t thread_id,
 		struct rtos_reg **reg_list, int *num_regs);
 static int hwthread_get_symbol_list_to_lookup(symbol_table_elem_t *symbol_list[]);
@@ -52,6 +54,7 @@ const struct rtos_type hwthread_rtos = {
 	.create = hwthread_create,
 	.update_threads = hwthread_update_threads,
 	.get_thread_reg_list = hwthread_get_thread_reg_list,
+	.get_thread_reg = hwthread_get_thread_reg,
 	.get_symbol_list_to_lookup = hwthread_get_symbol_list_to_lookup,
 	.smp_init = hwthread_smp_init,
 	.set_reg = hwthread_set_reg,
@@ -257,6 +260,38 @@ static int hwthread_get_thread_reg_list(struct rtos *rtos, int64_t thread_id,
 	return ERROR_OK;
 }
 
+static int hwthread_get_thread_reg(struct rtos *rtos, int64_t thread_id,
+		uint32_t reg_num, struct rtos_reg *rtos_reg)
+{
+	LOG_DEBUG(">>> thread %ld, reg %d", thread_id, reg_num);
+	if (rtos == NULL)
+		return ERROR_FAIL;
+
+	struct target *target = rtos->target;
+
+	struct target *curr = find_thread(target, thread_id);
+	if (curr == NULL)
+		return ERROR_FAIL;
+
+	if (!target_was_examined(curr))
+		return ERROR_FAIL;
+
+	struct reg *reg = register_get_by_number(curr->reg_cache, reg_num, true);
+	if (!reg)
+		return ERROR_FAIL;
+
+	if (reg->type->get(reg) != ERROR_OK)
+		return ERROR_FAIL;
+
+	rtos_reg->number = reg->number;
+	rtos_reg->size = reg->size;
+	unsigned bytes = (reg->size + 7) / 8;
+	assert(bytes <= sizeof(rtos_reg->value));
+	memcpy(rtos_reg->value, reg->value, bytes);
+
+	return ERROR_OK;
+}
+
 int hwthread_set_reg(struct rtos *rtos, int reg_num, uint8_t *reg_value)
 {
 	if (rtos == NULL)
@@ -265,14 +300,13 @@ int hwthread_set_reg(struct rtos *rtos, int reg_num, uint8_t *reg_value)
 	struct target *target = rtos->target;
 
 	struct target *curr = find_thread(target, rtos->current_thread);
-	LOG_DEBUG(">>> found %ld: %p", rtos->current_thread, curr);
 	if (curr == NULL)
 		return ERROR_FAIL;
 
 	struct reg **reg_list;
 	int reg_list_size;
 	if (target_get_gdb_reg_list(curr, &reg_list, &reg_list_size,
-				REG_CLASS_ALL) != ERROR_OK)
+				REG_CLASS_GENERAL) != ERROR_OK)
 		return ERROR_FAIL;
 
 	if (reg_list_size <= reg_num) {
@@ -332,7 +366,6 @@ static int hwthread_thread_packet(struct connection *connection, const char *pac
 			target->rtos->current_thread = threadid_from_target(target);
 
 		target->rtos->current_threadid = current_threadid;
-		LOG_DEBUG(">>> current_threadid=%ld", current_threadid);
 
 		gdb_put_packet(connection, "OK", 2);
 		return ERROR_OK;

src/rtos/rtos.c

@@ -463,6 +463,7 @@ static int rtos_put_gdb_reg_list(struct connection *connection,
 	return ERROR_OK;
 }
 
+/** Look through all registers to find this register. */
 int rtos_get_gdb_reg(struct connection *connection, int reg_num)
 {
 	struct target *target = get_target_from_connection(connection);
@@ -480,10 +481,18 @@ int rtos_get_gdb_reg(struct connection *connection, int reg_num)
 										current_threadid,
 										target->rtos->current_thread);
 
-		int retval = target->rtos->type->get_thread_reg_list(target->rtos,
+		int retval;
-				current_threadid,
+		if (target->rtos->type->get_thread_reg) {
-				&reg_list,
+			reg_list = calloc(1, sizeof(*reg_list));
-				&num_regs);
+			num_regs = 1;
+			retval = target->rtos->type->get_thread_reg(target->rtos,
+					current_threadid, reg_num, &reg_list[0]);
+		} else {
+			retval = target->rtos->type->get_thread_reg_list(target->rtos,
+					current_threadid,
+					&reg_list,
+					&num_regs);
+		}
 		if (retval != ERROR_OK) {
 			LOG_ERROR("RTOS: failed to get register list");
 			return retval;
@@ -502,6 +511,7 @@ int rtos_get_gdb_reg(struct connection *connection, int reg_num)
 	return ERROR_FAIL;
 }
 
+/** Return a list of general registers. */
 int rtos_get_gdb_reg_list(struct connection *connection)
 {
 	struct target *target = get_target_from_connection(connection);
@@ -540,7 +550,7 @@ int rtos_set_reg(struct connection *connection, int reg_num,
 {
 	struct target *target = get_target_from_connection(connection);
 	int64_t current_threadid = target->rtos->current_threadid;
-	LOG_DEBUG(">>> reg_num=%d", reg_num);
+	LOG_DEBUG(">>> thread %ld, reg %d", current_threadid, reg_num);
 	if ((target->rtos != NULL) &&
 			(target->rtos->type->set_reg != NULL) &&
 			(current_threadid != -1) &&

src/rtos/rtos.h

@@ -20,6 +20,7 @@
 #define OPENOCD_RTOS_RTOS_H
 
 #include "server/server.h"
+#include "target/target.h"
 #include <jim-nvp.h>
 
 typedef int64_t threadid_t;
@@ -71,11 +72,15 @@ struct rtos_type {
 	int (*create)(struct target *target);
 	int (*smp_init)(struct target *target);
 	int (*update_threads)(struct rtos *rtos);
+	/** Return a list of general registers, with their values filled out. */
 	int (*get_thread_reg_list)(struct rtos *rtos, int64_t thread_id,
 			struct rtos_reg **reg_list, int *num_regs);
+	int (*get_thread_reg)(struct rtos *rtos, int64_t thread_id,
+			uint32_t reg_num, struct rtos_reg *reg);
 	int (*get_symbol_list_to_lookup)(symbol_table_elem_t *symbol_list[]);
 	int (*clean)(struct target *target);
 	char * (*ps_command)(struct target *target);
+	// TODO: int or uint32_t for reg_num?
 	int (*set_reg)(struct rtos *rtos, int reg_num, uint8_t *reg_value);
 };
 

src/server/gdb_server.c

@@ -96,7 +96,7 @@ struct gdb_connection {
 	char *thread_list;
 };
 
-#if 0
+#if 1
 #define _DEBUG_GDB_IO_
 #endif
 

src/target/register.c

@@ -36,6 +36,29 @@
  * may be separate registers associated with debug or trace modules.
  */
 
+struct reg *register_get_by_number(struct reg_cache *first,
+		uint32_t reg_num, bool search_all)
+{
+	unsigned i;
+	struct reg_cache *cache = first;
+
+	while (cache) {
+		for (i = 0; i < cache->num_regs; i++) {
+			if (cache->reg_list[i].exist == false)
+				continue;
+			if (cache->reg_list[i].number == reg_num)
+				return &(cache->reg_list[i]);
+		}
+
+		if (search_all)
+			cache = cache->next;
+		else
+			break;
+	}
+
+	return NULL;
+}
+
 struct reg *register_get_by_name(struct reg_cache *first,
 		const char *name, bool search_all)
 {

src/target/register.h

@@ -159,6 +159,8 @@ struct reg_arch_type {
 	int (*set)(struct reg *reg, uint8_t *buf);
 };
 
+struct reg *register_get_by_number(struct reg_cache *first,
+		uint32_t reg_num, bool search_all);
 struct reg *register_get_by_name(struct reg_cache *first,
 		const char *name, bool search_all);
 struct reg_cache **register_get_last_cache_p(struct reg_cache **first);

src/target/riscv/riscv.c

@@ -931,7 +931,7 @@ static int riscv_get_gdb_reg_list(struct target *target,
 
 	switch (reg_class) {
 		case REG_CLASS_GENERAL:
-			*reg_list_size = 32;
+			*reg_list_size = 33;
 			break;
 		case REG_CLASS_ALL:
 			*reg_list_size = target->reg_cache->num_regs;
@@ -1218,6 +1218,22 @@ int riscv_openocd_poll(struct target *target)
 
 	target->state = TARGET_HALTED;
 
+	if (target->smp) {
+		LOG_DEBUG("Halt other targets in this SMP group.");
+		struct target_list *targets = target->head;
+		int result = ERROR_OK;
+		while (targets) {
+			struct target *t = targets->target;
+			targets = targets->next;
+			if (t->state != TARGET_HALTED) {
+				if (old_or_new_riscv_halt(t) != ERROR_OK)
+					result = ERROR_FAIL;
+			}
+		}
+		if (result != ERROR_OK)
+			return result;
+	}
+
 	if (target->debug_reason == DBG_REASON_BREAKPOINT) {
 		int retval;
 		if (riscv_semihosting(target, &retval) != 0)

src/target/target.c

@@ -1575,8 +1575,9 @@ int target_call_event_callbacks(struct target *target, enum target_event event)
 		target_call_event_callbacks(target, TARGET_EVENT_GDB_HALT);
 	}
 
-	LOG_DEBUG("target event %i (%s)", event,
+	LOG_DEBUG("target event %i (%s) for core %d", event,
-			Jim_Nvp_value2name_simple(nvp_target_event, event)->name);
+			Jim_Nvp_value2name_simple(nvp_target_event, event)->name,
+			target->coreid);
 
 	target_handle_event(target, event);