Writing registers in other threads appears to work. (#585)

Change-Id: Ie2a8bef5e9aa24fc85e9b6c9093021731c58c3e7
Signed-off-by: Tim Newsome <tim@sifive.com>

src/rtos/FreeRTOS.c

@@ -190,6 +190,7 @@ static int FreeRTOS_get_thread_reg_list(struct rtos *rtos, threadid_t thread_id,
 		struct rtos_reg **reg_list, int *num_regs);
 static int FreeRTOS_get_thread_reg(struct rtos *rtos, threadid_t thread_id,
 		uint32_t reg_num, struct rtos_reg *reg);
+static int FreeRTOS_set_reg(struct rtos *rtos, uint32_t reg_num, uint8_t *reg_value);
 static int FreeRTOS_get_symbol_list_to_lookup(symbol_table_elem_t *symbol_list[]);
 
 struct rtos_type FreeRTOS_rtos = {
@@ -200,6 +201,7 @@ struct rtos_type FreeRTOS_rtos = {
 	.update_threads = FreeRTOS_update_threads,
 	.get_thread_reg_list = FreeRTOS_get_thread_reg_list,
 	.get_thread_reg = FreeRTOS_get_thread_reg,
+	.set_reg = FreeRTOS_set_reg,
 	.get_symbol_list_to_lookup = FreeRTOS_get_symbol_list_to_lookup,
 };
 
@@ -585,6 +587,24 @@ static int FreeRTOS_get_thread_reg(struct rtos *rtos, threadid_t thread_id,
 	return rtos_generic_stack_read_reg(rtos->target, stacking_info, stack_ptr, reg_num, reg);
 }
 
+static int FreeRTOS_set_reg(struct rtos *rtos, uint32_t reg_num, uint8_t *reg_value)
+{
+	LOG_DEBUG("[%" PRId64 "] reg_num=%" PRId32, rtos->current_threadid, reg_num);
+
+	/* Let the caller write registers directly for the current thread. */
+	if (rtos->current_threadid == rtos->current_thread)
+		return ERROR_FAIL;
+
+	const struct rtos_register_stacking *stacking_info;
+	target_addr_t stack_ptr;
+	if (FreeRTOS_get_stacking_info(rtos, rtos->current_threadid,
+								   &stacking_info, &stack_ptr) != ERROR_OK)
+		return ERROR_FAIL;
+
+	return rtos_generic_stack_write_reg(rtos->target, stacking_info, stack_ptr,
+										reg_num, reg_value);
+}
+
 static int FreeRTOS_get_symbol_list_to_lookup(symbol_table_elem_t *symbol_list[])
 {
 	unsigned int i;

src/rtos/rtos.c

@@ -653,7 +653,7 @@ int rtos_generic_stack_read_reg(struct target *target,
 	}
 	if (i >= total_count) {
 		/* This register is not on the stack. Return error so a caller somewhere
-		 * will just read the register directly fromt he target. */
+		 * will just read the register directly from the target. */
 		return ERROR_FAIL;
 	}
 
@@ -680,6 +680,58 @@ int rtos_generic_stack_read_reg(struct target *target,
 	return ERROR_OK;
 }
 
+int rtos_generic_stack_write_reg(struct target *target,
+								const struct rtos_register_stacking *stacking,
+								target_addr_t stack_ptr,
+								uint32_t reg_num, uint8_t *reg_value)
+{
+	LOG_DEBUG("stack_ptr=" TARGET_ADDR_FMT ", reg_num=%d", stack_ptr, reg_num);
+	unsigned total_count = MAX(stacking->total_register_count, stacking->num_output_registers);
+	unsigned i;
+	for (i = 0; i < total_count; i++) {
+		if (stacking->register_offsets[i].number == reg_num)
+			break;
+	}
+	if (i >= total_count) {
+		/* This register is not on the stack. Return error so a caller somewhere
+		 * will just read the register directly from the target. */
+		return ERROR_FAIL;
+	}
+
+	const struct stack_register_offset *offsets = &stacking->register_offsets[i];
+
+	unsigned width_bytes = DIV_ROUND_UP(offsets->width_bits, 8);
+	if (offsets->offset >= 0) {
+		target_addr_t address = stack_ptr;
+
+		if (stacking->stack_growth_direction == 1)
+			address -= stacking->stack_registers_size;
+
+		LOG_DEBUG("write 0x%" PRIx64 " to register %d",
+				  buf_get_u64(reg_value, 0, offsets->width_bits), reg_num);
+		if (target_write_buffer(
+				target, address + offsets->offset,
+				width_bytes, reg_value) != ERROR_OK)
+			return ERROR_FAIL;
+	} else if (offsets->offset == -1) {
+		/* This register isn't on the stack, but is listed as one of those. We
+		 * read it as 0, and ignore writes. */
+	} else if (offsets->offset == -2) {
+		/* This register requires computation when we "read" it. I'm not sure
+		 * how to handle writes. We can't simply return error here because then
+		 * the higher level code will end up writing the register in the halted
+		 * core, which is definitely not the same as writing it for a thread. */
+		LOG_ERROR("Don't know how to write register %d with offset -2 in a thread.",
+				  reg_num);
+		assert(0);
+	} else {
+		LOG_ERROR("Don't know how to handle offset <2.");
+		assert(0);
+	}
+
+	return ERROR_OK;
+}
+
 static int rtos_try_next(struct target *target)
 {
 	struct rtos *os = target->rtos;

src/rtos/rtos.h

@@ -143,6 +143,10 @@ int rtos_generic_stack_read_reg(struct target *target,
 								const struct rtos_register_stacking *stacking,
 								target_addr_t stack_ptr,
 								uint32_t reg_num, struct rtos_reg *reg);
+int rtos_generic_stack_write_reg(struct target *target,
+								const struct rtos_register_stacking *stacking,
+								target_addr_t stack_ptr,
+								uint32_t reg_num, uint8_t *reg_value);
 int gdb_thread_packet(struct connection *connection, char const *packet, int packet_size);
 int rtos_get_gdb_reg(struct connection *connection, int reg_num);
 int rtos_get_gdb_reg_list(struct connection *connection);

src/server/gdb_server.c

@@ -2942,6 +2942,7 @@ static bool gdb_handle_vcont_packet(struct connection *connection, const char *p
 				char sig_reply[128];
 
 				LOG_DEBUG("fake step thread %"PRIx64, thread_id);
+				target->rtos->current_threadid = thread_id;
 
 				sig_reply_len = snprintf(sig_reply, sizeof(sig_reply),
 										 "T05thread:%016"PRIx64";", thread_id);