From ca49f403eff5187d5d983efa8637f73da7dccf41 Mon Sep 17 00:00:00 2001 From: Tim Newsome Date: Wed, 17 Mar 2021 12:32:29 -0700 Subject: [PATCH] Writing registers in other threads appears to work. (#585) Change-Id: Ie2a8bef5e9aa24fc85e9b6c9093021731c58c3e7 Signed-off-by: Tim Newsome --- src/rtos/FreeRTOS.c | 20 +++++++++++++++ src/rtos/rtos.c | 54 ++++++++++++++++++++++++++++++++++++++++- src/rtos/rtos.h | 4 +++ src/server/gdb_server.c | 1 + 4 files changed, 78 insertions(+), 1 deletion(-) diff --git a/src/rtos/FreeRTOS.c b/src/rtos/FreeRTOS.c index 697f18946..ee7faa0d3 100644 --- a/src/rtos/FreeRTOS.c +++ b/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; diff --git a/src/rtos/rtos.c b/src/rtos/rtos.c index 86ea088ee..5e83333d1 100644 --- a/src/rtos/rtos.c +++ b/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; diff --git a/src/rtos/rtos.h b/src/rtos/rtos.h index 9f91e19a1..3ea64160b 100644 --- a/src/rtos/rtos.h +++ b/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); diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c index c1d46fb04..ac00dd008 100644 --- a/src/server/gdb_server.c +++ b/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);