rtos support to write registers on current thread

I don't understand how it was ever possible to change the registers on a
thread that's not the current active one when a halt happened. Really
instead of the RTOS tracking what the currently selected thread is, it
would make more sense to have gdb_server do that and simply pass it
along in every call to the RTOS layer.

Now MulticoreRegTest passes.

Change-Id: I399b9b2b05a147aa6b41463714ed3a39534b1fc8

4 files changed, 93 insertions, 61 deletions

diff --git a/src/rtos/hwthread.c b/src/rtos/hwthread.c
index 7050ba3..a9fc1d9 100644
--- a/src/rtos/hwthread.c
+++ b/src/rtos/hwthread.c
@@ -35,6 +35,7 @@ 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[]);
 static int hwthread_smp_init(struct target *target);
+int hwthread_set_reg(struct rtos *rtos, int reg_num, uint8_t *reg_value);
 
 #define HW_THREAD_NAME_STR_SIZE (32)
 
@@ -53,6 +54,7 @@ const struct rtos_type hwthread_rtos = {
 	.get_thread_reg_list = hwthread_get_thread_reg_list,
 	.get_symbol_list_to_lookup = hwthread_get_symbol_list_to_lookup,
 	.smp_init = hwthread_smp_init,
+	.set_reg = hwthread_set_reg,
 };
 
 struct hwthread_params {
@@ -201,44 +203,33 @@ static int hwthread_smp_init(struct target *target)
 	return hwthread_update_threads(target->rtos);
 }
 
-static inline int gdb_reg_pos(struct target *target, int pos, int len)
+static struct target *find_thread(struct target *target, int64_t thread_id)
 {
-	if (target->endianness == TARGET_LITTLE_ENDIAN)
-		return pos;
-	else
-		return len - 1 - pos;
+	/* Find the thread with that thread_id */
+	if (target == NULL)
+		return NULL;
+	if (target->smp) {
+		for (struct target_list *head = target->head; head != NULL; head = head->next) {
+			if (thread_id == threadid_from_target(head->target))
+				return head->target;
+		}
+	} else if (thread_id == threadid_from_target(target)) {
+		return target;
+	}
+	return NULL;
 }
 
-
 static int hwthread_get_thread_reg_list(struct rtos *rtos, int64_t thread_id,
 		struct rtos_reg **rtos_reg_list, int *rtos_reg_list_size)
 {
-	struct target_list *head;
-	struct target *target;
-	struct target *curr;
-
 	if (rtos == NULL)
 		return ERROR_FAIL;
 
-	target = rtos->target;
-
-	/* Find the thread with that thread_id */
-	if (target->smp) {
-		curr = NULL;
-		for (head = target->head; head != NULL; head = head->next) {
-			curr = head->target;
-
-			if (thread_id == threadid_from_target(curr))
-				break;
-		}
+	struct target *target = rtos->target;
 
-		if (head == NULL)
-			return ERROR_FAIL;
-	} else {
-		curr = target;
-		if (thread_id != threadid_from_target(curr))
-			return ERROR_FAIL;
-	}
+	struct target *curr = find_thread(target, thread_id);
+	if (curr == NULL)
+		return ERROR_FAIL;
 
 	if (!target_was_examined(curr))
 		return ERROR_FAIL;
@@ -266,6 +257,32 @@ static int hwthread_get_thread_reg_list(struct rtos *rtos, int64_t thread_id,
 	return ERROR_OK;
 }
 
+int hwthread_set_reg(struct rtos *rtos, int reg_num, uint8_t *reg_value)
+{
+	if (rtos == NULL)
+		return ERROR_FAIL;
+
+	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)
+		return ERROR_FAIL;
+
+	if (reg_list_size <= reg_num) {
+		LOG_ERROR("Register %d requested, but only %d registers exist.",
+				reg_num, reg_list_size);
+		return ERROR_FAIL;
+	}
+	return reg_list[reg_num]->type->set(reg_list[reg_num], reg_value);
+}
+
 static int hwthread_get_symbol_list_to_lookup(symbol_table_elem_t *symbol_list[])
 {
 	/* return an empty list, we don't have any symbols to look up */
@@ -277,26 +294,10 @@ static int hwthread_get_symbol_list_to_lookup(symbol_table_elem_t *symbol_list[]
 static int hwthread_target_for_threadid(struct connection *connection, int64_t thread_id, struct target **p_target)
 {
 	struct target *target = get_target_from_connection(connection);
-	struct target_list *head;
-	struct target *curr;
-
-	if (target->smp) {
-		/* Find the thread with that thread_id */
-		curr = NULL;
-		for (head = target->head; head != NULL; head = head->next) {
-			curr = head->target;
 
-			if (thread_id == threadid_from_target(curr))
-				break;
-		}
-
-		if (head == NULL)
-			return ERROR_FAIL;
-	} else {
-		curr = target;
-		if (thread_id != threadid_from_target(curr))
-			return ERROR_FAIL;
-	}
+	struct target *curr = find_thread(target, thread_id);
+	if (curr == NULL)
+		return ERROR_FAIL;
 
 	*p_target = curr;
 
@@ -331,6 +332,7 @@ 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;
diff --git a/src/rtos/rtos.c b/src/rtos/rtos.c
index 9032f81..eb48ab8 100644
--- a/src/rtos/rtos.c
+++ b/src/rtos/rtos.c
@@ -535,6 +535,21 @@ int rtos_get_gdb_reg_list(struct connection *connection)
 	return ERROR_FAIL;
 }
 
+int rtos_set_reg(struct connection *connection, int reg_num,
+		uint8_t *reg_value)
+{
+	struct target *target = get_target_from_connection(connection);
+	int64_t current_threadid = target->rtos->current_threadid;
+	LOG_DEBUG(">>> reg_num=%d", reg_num);
+	if ((target->rtos != NULL) &&
+			(target->rtos->type->set_reg != NULL) &&
+			(current_threadid != -1) &&
+			(current_threadid != 0)) {
+		return target->rtos->type->set_reg(target->rtos, reg_num, reg_value);
+	}
+	return ERROR_FAIL;
+}
+
 int rtos_generic_stack_read(struct target *target,
 	const struct rtos_register_stacking *stacking,
 	int64_t stack_ptr,
diff --git a/src/rtos/rtos.h b/src/rtos/rtos.h
index 22de33f..8574b4a 100644
--- a/src/rtos/rtos.h
+++ b/src/rtos/rtos.h
@@ -76,6 +76,7 @@ struct rtos_type {
 	int (*get_symbol_list_to_lookup)(symbol_table_elem_t *symbol_list[]);
 	int (*clean)(struct target *target);
 	char * (*ps_command)(struct target *target);
+	int (*set_reg)(struct rtos *rtos, int reg_num, uint8_t *reg_value);
 };
 
 struct stack_register_offset {
@@ -105,6 +106,8 @@ struct rtos_register_stacking {
 #define GDB_THREAD_PACKET_NOT_CONSUMED (-40)
 
 int rtos_create(Jim_GetOptInfo *goi, struct target *target);
+int rtos_set_reg(struct connection *connection, int reg_num,
+		uint8_t *reg_value);
 int rtos_generic_stack_read(struct target *target,
 		const struct rtos_register_stacking *stacking,
 		int64_t stack_ptr,
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index b95c1ec..0b43155 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -1340,37 +1340,49 @@ static int gdb_set_register_packet(struct connection *connection,
 {
 	struct target *target = get_target_from_connection(connection);
 	char *separator;
-	uint8_t *bin_buf;
 	int reg_num = strtoul(packet + 1, &separator, 16);
 	struct reg **reg_list;
 	int reg_list_size;
 	int retval;
 
+#ifdef _DEBUG_GDB_IO_
 	LOG_DEBUG("-");
+#endif
+
+	if (*separator != '=') {
+		LOG_ERROR("GDB 'set register packet', but no '=' following the register number");
+		return ERROR_SERVER_REMOTE_CLOSED;
+	}
+	size_t chars = strlen(separator + 1);
+	uint8_t *bin_buf = malloc(chars / 2);
+	gdb_target_to_reg(target, separator + 1, chars, bin_buf);
+
+	if ((target->rtos != NULL) &&
+			(ERROR_OK == rtos_set_reg(connection, reg_num, bin_buf))) {
+		free(bin_buf);
+		gdb_put_packet(connection, "OK", 2);
+		return ERROR_OK;
+	}
 
 	retval = target_get_gdb_reg_list(target, &reg_list, &reg_list_size,
 			REG_CLASS_ALL);
-	if (retval != ERROR_OK)
+	if (retval != ERROR_OK) {
+		free(bin_buf);
 		return gdb_error(connection, retval);
+	}
 
 	if (reg_list_size <= reg_num) {
 		LOG_ERROR("gdb requested a non-existing register");
+		free(bin_buf);
+		free(reg_list);
 		return ERROR_SERVER_REMOTE_CLOSED;
 	}
 
-	if (*separator != '=') {
-		LOG_ERROR("GDB 'set register packet', but no '=' following the register number");
-		return ERROR_SERVER_REMOTE_CLOSED;
-	}
-
-	/* convert from GDB-string (target-endian) to hex-string (big-endian) */
-	bin_buf = malloc(DIV_ROUND_UP(reg_list[reg_num]->size, 8));
-	int chars = (DIV_ROUND_UP(reg_list[reg_num]->size, 8) * 2);
-
-	if ((unsigned int)chars != strlen(separator + 1)) {
-		LOG_ERROR("gdb sent %zu bits for a %d-bit register (%s)",
-				strlen(separator + 1) * 4, chars * 4, reg_list[reg_num]->name);
+	if (chars != (DIV_ROUND_UP(reg_list[reg_num]->size, 8) * 2)) {
+		LOG_ERROR("gdb sent %d bits for a %d-bit register (%s)",
+				(int) chars * 4, reg_list[reg_num]->size, reg_list[reg_num]->name);
 		free(bin_buf);
+		free(reg_list);
 		return ERROR_SERVER_REMOTE_CLOSED;
 	}