diff options
Diffstat (limited to 'src/target/espressif/esp_algorithm.c')
-rw-r--r-- | src/target/espressif/esp_algorithm.c | 595 |
1 files changed, 595 insertions, 0 deletions
diff --git a/src/target/espressif/esp_algorithm.c b/src/target/espressif/esp_algorithm.c new file mode 100644 index 0000000..79f610b --- /dev/null +++ b/src/target/espressif/esp_algorithm.c @@ -0,0 +1,595 @@ +// SPDX-License-Identifier: GPL-2.0-or-later + +/*************************************************************************** + * Espressif chips common algorithm API for OpenOCD * + * Copyright (C) 2022 Espressif Systems Ltd. * + ***************************************************************************/ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include <helper/align.h> +#include <target/algorithm.h> +#include <target/target.h> +#include "esp_algorithm.h" + +#define DEFAULT_ALGORITHM_TIMEOUT_MS 40000 /* ms */ + +static int esp_algorithm_read_stub_logs(struct target *target, struct esp_algorithm_stub *stub) +{ + if (!stub || stub->log_buff_addr == 0 || stub->log_buff_size == 0) + return ERROR_FAIL; + + uint32_t len = 0; + int retval = target_read_u32(target, stub->log_buff_addr, &len); + if (retval != ERROR_OK) + return retval; + + /* sanity check. log_buff_size = sizeof(len) + sizeof(log_buff) */ + if (len == 0 || len > stub->log_buff_size - 4) + return ERROR_FAIL; + + uint8_t *log_buff = calloc(1, len); + if (!log_buff) { + LOG_ERROR("Failed to allocate memory for the stub log!"); + return ERROR_FAIL; + } + retval = target_read_memory(target, stub->log_buff_addr + 4, 1, len, log_buff); + if (retval == ERROR_OK) + LOG_OUTPUT("%*.*s", len, len, log_buff); + free(log_buff); + return retval; +} + +static int esp_algorithm_run_image(struct target *target, + struct esp_algorithm_run_data *run, + uint32_t num_args, + va_list ap) +{ + struct working_area **mem_handles = NULL; + + if (!run || !run->hw) + return ERROR_FAIL; + + int retval = run->hw->algo_init(target, run, num_args, ap); + if (retval != ERROR_OK) + return retval; + + /* allocate memory arguments and fill respective reg params */ + if (run->mem_args.count > 0) { + mem_handles = calloc(run->mem_args.count, sizeof(*mem_handles)); + if (!mem_handles) { + LOG_ERROR("Failed to alloc target mem handles!"); + retval = ERROR_FAIL; + goto _cleanup; + } + /* alloc memory args target buffers */ + for (uint32_t i = 0; i < run->mem_args.count; i++) { + /* small hack: if we need to update some reg param this field holds + * appropriate user argument number, */ + /* otherwise should hold UINT_MAX */ + uint32_t usr_param_num = run->mem_args.params[i].address; + static struct working_area *area; + retval = target_alloc_working_area(target, run->mem_args.params[i].size, &area); + if (retval != ERROR_OK) { + LOG_ERROR("Failed to alloc target buffer!"); + retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE; + goto _cleanup; + } + mem_handles[i] = area; + run->mem_args.params[i].address = area->address; + if (usr_param_num != UINT_MAX) /* if we need update some register param with mem param value */ + esp_algorithm_user_arg_set_uint(run, usr_param_num, run->mem_args.params[i].address); + } + } + + if (run->usr_func_init) { + retval = run->usr_func_init(target, run, run->usr_func_arg); + if (retval != ERROR_OK) { + LOG_ERROR("Failed to prepare algorithm host side args stub (%d)!", retval); + goto _cleanup; + } + } + + LOG_DEBUG("Algorithm start @ " TARGET_ADDR_FMT ", stack %d bytes @ " TARGET_ADDR_FMT, + run->stub.tramp_mapped_addr, run->stack_size, run->stub.stack_addr); + retval = target_start_algorithm(target, + run->mem_args.count, run->mem_args.params, + run->reg_args.count, run->reg_args.params, + run->stub.tramp_mapped_addr, 0, + run->stub.ainfo); + if (retval != ERROR_OK) { + LOG_ERROR("Failed to start algorithm (%d)!", retval); + goto _cleanup; + } + + if (run->usr_func) { + /* give target algorithm stub time to init itself, then user func can communicate to it safely */ + alive_sleep(100); + retval = run->usr_func(target, run->usr_func_arg); + if (retval != ERROR_OK) + LOG_ERROR("Failed to exec algorithm user func (%d)!", retval); + } + uint32_t timeout_ms = 0; /* do not wait if 'usr_func' returned error */ + if (retval == ERROR_OK) + timeout_ms = run->timeout_ms ? run->timeout_ms : DEFAULT_ALGORITHM_TIMEOUT_MS; + LOG_DEBUG("Wait algorithm completion"); + retval = target_wait_algorithm(target, + run->mem_args.count, run->mem_args.params, + run->reg_args.count, run->reg_args.params, + 0, timeout_ms, + run->stub.ainfo); + if (retval != ERROR_OK) { + LOG_ERROR("Failed to wait algorithm (%d)!", retval); + /* target has been forced to stop in target_wait_algorithm() */ + } + esp_algorithm_read_stub_logs(target, &run->stub); + + if (run->usr_func_done) + run->usr_func_done(target, run, run->usr_func_arg); + + if (retval != ERROR_OK) { + LOG_ERROR("Algorithm run failed (%d)!", retval); + } else { + run->ret_code = esp_algorithm_user_arg_get_uint(run, 0); + LOG_DEBUG("Got algorithm RC 0x%" PRIx32, run->ret_code); + } + +_cleanup: + /* free memory arguments */ + if (mem_handles) { + for (uint32_t i = 0; i < run->mem_args.count; i++) { + if (mem_handles[i]) + target_free_working_area(target, mem_handles[i]); + } + free(mem_handles); + } + run->hw->algo_cleanup(target, run); + + return retval; +} + +static int esp_algorithm_run_debug_stub(struct target *target, + struct esp_algorithm_run_data *run, + uint32_t num_args, + va_list ap) +{ + if (!run || !run->hw) + return ERROR_FAIL; + + int retval = run->hw->algo_init(target, run, num_args, ap); + if (retval != ERROR_OK) + return retval; + + LOG_DEBUG("Algorithm start @ " TARGET_ADDR_FMT ", stack %d bytes @ " TARGET_ADDR_FMT, + run->stub.tramp_mapped_addr, run->stack_size, run->stub.stack_addr); + retval = target_start_algorithm(target, + run->mem_args.count, run->mem_args.params, + run->reg_args.count, run->reg_args.params, + run->stub.tramp_mapped_addr, 0, + run->stub.ainfo); + if (retval != ERROR_OK) { + LOG_ERROR("Failed to start algorithm (%d)!", retval); + goto _cleanup; + } + + uint32_t timeout_ms = 0; /* do not wait if 'usr_func' returned error */ + if (retval == ERROR_OK) + timeout_ms = run->timeout_ms ? run->timeout_ms : DEFAULT_ALGORITHM_TIMEOUT_MS; + LOG_DEBUG("Wait algorithm completion"); + retval = target_wait_algorithm(target, + run->mem_args.count, run->mem_args.params, + run->reg_args.count, run->reg_args.params, + 0, timeout_ms, + run->stub.ainfo); + if (retval != ERROR_OK) { + LOG_ERROR("Failed to wait algorithm (%d)!", retval); + /* target has been forced to stop in target_wait_algorithm() */ + } + + if (retval != ERROR_OK) { + LOG_ERROR("Algorithm run failed (%d)!", retval); + } else { + run->ret_code = esp_algorithm_user_arg_get_uint(run, 0); + LOG_DEBUG("Got algorithm RC 0x%" PRIx32, run->ret_code); + } + +_cleanup: + run->hw->algo_cleanup(target, run); + + return retval; +} + +static void reverse_binary(const uint8_t *src, uint8_t *dest, size_t length) +{ + size_t remaining = length % 4; + size_t offset = 0; + size_t aligned_len = ALIGN_UP(length, 4); + + if (remaining > 0) { + /* Put extra bytes to the beginning with padding */ + memset(dest + remaining, 0xFF, 4 - remaining); + for (size_t i = 0; i < remaining; i++) + dest[i] = src[length - remaining + i]; + length -= remaining; /* reverse the others */ + offset = 4; + } + + for (size_t i = offset; i < aligned_len; i += 4) { + dest[i + 0] = src[length - i + offset - 4]; + dest[i + 1] = src[length - i + offset - 3]; + dest[i + 2] = src[length - i + offset - 2]; + dest[i + 3] = src[length - i + offset - 1]; + } +} + +static int load_section_from_image(struct target *target, + struct esp_algorithm_run_data *run, + int section_num, + bool reverse) +{ + if (!run) + return ERROR_FAIL; + + struct imagesection *section = &run->image.image.sections[section_num]; + uint32_t sec_wr = 0; + uint8_t buf[1024]; + + assert(sizeof(buf) % 4 == 0); + + while (sec_wr < section->size) { + uint32_t nb = section->size - sec_wr > sizeof(buf) ? sizeof(buf) : section->size - sec_wr; + size_t size_read = 0; + int retval = image_read_section(&run->image.image, section_num, sec_wr, nb, buf, &size_read); + if (retval != ERROR_OK) { + LOG_ERROR("Failed to read stub section (%d)!", retval); + return retval; + } + + if (reverse) { + size_t aligned_len = ALIGN_UP(size_read, 4); + uint8_t reversed_buf[aligned_len]; + + /* Send original size to allow padding */ + reverse_binary(buf, reversed_buf, size_read); + + /* + The address range accessed via the instruction bus is in reverse order (word-wise) compared to access + via the data bus. That is to say, address + 0x3FFE_0000 and 0x400B_FFFC access the same word + 0x3FFE_0004 and 0x400B_FFF8 access the same word + 0x3FFE_0008 and 0x400B_FFF4 access the same word + ... + The data bus and instruction bus of the CPU are still both little-endian, + so the byte order of individual words is not reversed between address spaces. + For example, address + 0x3FFE_0000 accesses the least significant byte in the word accessed by 0x400B_FFFC. + 0x3FFE_0001 accesses the second least significant byte in the word accessed by 0x400B_FFFC. + 0x3FFE_0002 accesses the second most significant byte in the word accessed by 0x400B_FFFC. + For more details, please refer to ESP32 TRM, Internal SRAM1 section. + */ + retval = target_write_buffer(target, run->image.dram_org - sec_wr - aligned_len, aligned_len, reversed_buf); + if (retval != ERROR_OK) { + LOG_ERROR("Failed to write stub section!"); + return retval; + } + } else { + retval = target_write_buffer(target, section->base_address + sec_wr, size_read, buf); + if (retval != ERROR_OK) { + LOG_ERROR("Failed to write stub section!"); + return retval; + } + } + + sec_wr += size_read; + } + + return ERROR_OK; +} + +/* + * Configuration: + * ---------------------------- + * The linker scripts defines the memory layout for the stub code. + * The OpenOCD script specifies the workarea address and it's size + * Sections defined in the linker are organized to share the same addresses with the workarea. + * code and data sections are located in Internal SRAM1 and OpenOCD fills these sections using the data bus. + */ +int esp_algorithm_load_func_image(struct target *target, struct esp_algorithm_run_data *run) +{ + int retval; + size_t tramp_sz = 0; + const uint8_t *tramp = NULL; + struct duration algo_time; + bool alloc_code_working_area = true; + + if (!run || !run->hw) + return ERROR_FAIL; + + if (duration_start(&algo_time) != 0) { + LOG_ERROR("Failed to start algo time measurement!"); + return ERROR_FAIL; + } + + if (run->hw->stub_tramp_get) { + tramp = run->hw->stub_tramp_get(target, &tramp_sz); + if (!tramp) + return ERROR_FAIL; + } + + LOG_DEBUG("stub: base 0x%x, start 0x%" PRIx32 ", %d sections", + run->image.image.base_address_set ? (unsigned int)run->image.image.base_address : 0, + run->image.image.start_address, + run->image.image.num_sections); + run->stub.entry = run->image.image.start_address; + + /* [code + trampoline] + <padding> + [data] */ + + /* ESP32 has reversed memory region. It will use the last part of DRAM, the others will use the first part. + * To avoid complexity for the backup/restore process, we will allocate a workarea for all IRAM region from + * the beginning. In that case no need to have a padding area. + */ + if (run->image.reverse) { + if (target_alloc_working_area(target, run->image.iram_len, &run->stub.code) != ERROR_OK) { + LOG_ERROR("no working area available, can't alloc space for stub code!"); + retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE; + goto _on_error; + } + alloc_code_working_area = false; + } + + uint32_t code_size = 0; + + /* Load code section */ + for (unsigned int i = 0; i < run->image.image.num_sections; i++) { + struct imagesection *section = &run->image.image.sections[i]; + + if (section->size == 0) + continue; + + if (section->flags & ESP_IMAGE_ELF_PHF_EXEC) { + LOG_DEBUG("addr " TARGET_ADDR_FMT ", sz %d, flags %" PRIx64, + section->base_address, section->size, section->flags); + + if (alloc_code_working_area) { + retval = target_alloc_working_area(target, section->size, &run->stub.code); + if (retval != ERROR_OK) { + LOG_ERROR("no working area available, can't alloc space for stub code!"); + retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE; + goto _on_error; + } + } + + if (section->base_address == 0) { + section->base_address = run->stub.code->address; + /* sanity check, stub is compiled to be run from working area */ + } else if (run->stub.code->address != section->base_address) { + LOG_ERROR("working area " TARGET_ADDR_FMT " and stub code section " TARGET_ADDR_FMT + " address mismatch!", + section->base_address, + run->stub.code->address); + retval = ERROR_FAIL; + goto _on_error; + } + + retval = load_section_from_image(target, run, i, run->image.reverse); + if (retval != ERROR_OK) + goto _on_error; + + code_size += ALIGN_UP(section->size, 4); + break; /* Stub has one executable text section */ + } + } + + /* If exists, load trampoline to the code area */ + if (tramp) { + if (run->stub.tramp_addr == 0) { + if (alloc_code_working_area) { + /* alloc trampoline in code working area */ + if (target_alloc_working_area(target, tramp_sz, &run->stub.tramp) != ERROR_OK) { + LOG_ERROR("no working area available, can't alloc space for stub jumper!"); + retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE; + goto _on_error; + } + run->stub.tramp_addr = run->stub.tramp->address; + } + } + + size_t al_tramp_size = ALIGN_UP(tramp_sz, 4); + + if (run->image.reverse) { + target_addr_t reversed_tramp_addr = run->image.dram_org - code_size; + uint8_t reversed_tramp[al_tramp_size]; + + /* Send original size to allow padding */ + reverse_binary(tramp, reversed_tramp, tramp_sz); + run->stub.tramp_addr = reversed_tramp_addr - al_tramp_size; + LOG_DEBUG("Write reversed tramp to addr " TARGET_ADDR_FMT ", sz %zu", run->stub.tramp_addr, al_tramp_size); + retval = target_write_buffer(target, run->stub.tramp_addr, al_tramp_size, reversed_tramp); + } else { + LOG_DEBUG("Write tramp to addr " TARGET_ADDR_FMT ", sz %zu", run->stub.tramp_addr, tramp_sz); + retval = target_write_buffer(target, run->stub.tramp_addr, tramp_sz, tramp); + } + + if (retval != ERROR_OK) { + LOG_ERROR("Failed to write stub jumper!"); + goto _on_error; + } + + run->stub.tramp_mapped_addr = run->image.iram_org + code_size; + code_size += al_tramp_size; + LOG_DEBUG("Tramp mapped to addr " TARGET_ADDR_FMT, run->stub.tramp_mapped_addr); + } + + /* allocate dummy space until the data address */ + if (alloc_code_working_area) { + /* we dont need to restore padding area. */ + uint32_t backup_working_area_prev = target->backup_working_area; + target->backup_working_area = 0; + if (target_alloc_working_area(target, run->image.iram_len - code_size, &run->stub.padding) != ERROR_OK) { + LOG_ERROR("no working area available, can't alloc space for stub code!"); + retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE; + goto _on_error; + } + target->backup_working_area = backup_working_area_prev; + } + + /* Load the data section */ + for (unsigned int i = 0; i < run->image.image.num_sections; i++) { + struct imagesection *section = &run->image.image.sections[i]; + + if (section->size == 0) + continue; + + if (!(section->flags & ESP_IMAGE_ELF_PHF_EXEC)) { + LOG_DEBUG("addr " TARGET_ADDR_FMT ", sz %d, flags %" PRIx64, section->base_address, section->size, + section->flags); + /* target_alloc_working_area() aligns the whole working area size to 4-byte boundary. + We alloc one area for both DATA and BSS, so align each of them ourselves. */ + uint32_t data_sec_sz = ALIGN_UP(section->size, 4); + LOG_DEBUG("DATA sec size %" PRIu32 " -> %" PRIu32, section->size, data_sec_sz); + uint32_t bss_sec_sz = ALIGN_UP(run->image.bss_size, 4); + LOG_DEBUG("BSS sec size %" PRIu32 " -> %" PRIu32, run->image.bss_size, bss_sec_sz); + if (target_alloc_working_area(target, data_sec_sz + bss_sec_sz, &run->stub.data) != ERROR_OK) { + LOG_ERROR("no working area available, can't alloc space for stub data!"); + retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE; + goto _on_error; + } + if (section->base_address == 0) { + section->base_address = run->stub.data->address; + /* sanity check, stub is compiled to be run from working area */ + } else if (run->stub.data->address != section->base_address) { + LOG_ERROR("working area " TARGET_ADDR_FMT + " and stub data section " TARGET_ADDR_FMT + " address mismatch!", + section->base_address, + run->stub.data->address); + retval = ERROR_FAIL; + goto _on_error; + } + + retval = load_section_from_image(target, run, i, false); + if (retval != ERROR_OK) + goto _on_error; + } + } + + /* stack */ + if (run->stub.stack_addr == 0 && run->stack_size > 0) { + /* allocate stack in data working area */ + if (target_alloc_working_area(target, run->stack_size, &run->stub.stack) != ERROR_OK) { + LOG_ERROR("no working area available, can't alloc stub stack!"); + retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE; + goto _on_error; + } + run->stub.stack_addr = run->stub.stack->address + run->stack_size; + } + + if (duration_measure(&algo_time) != 0) { + LOG_ERROR("Failed to stop algo run measurement!"); + retval = ERROR_FAIL; + goto _on_error; + } + LOG_DEBUG("Stub loaded in %g ms", duration_elapsed(&algo_time) * 1000); + return ERROR_OK; + +_on_error: + esp_algorithm_unload_func_image(target, run); + return retval; +} + +int esp_algorithm_unload_func_image(struct target *target, struct esp_algorithm_run_data *run) +{ + if (!run) + return ERROR_FAIL; + + target_free_all_working_areas(target); + + run->stub.tramp = NULL; + run->stub.stack = NULL; + run->stub.code = NULL; + run->stub.data = NULL; + run->stub.padding = NULL; + + return ERROR_OK; +} + +int esp_algorithm_exec_func_image_va(struct target *target, + struct esp_algorithm_run_data *run, + uint32_t num_args, + va_list ap) +{ + if (!run || !run->image.image.start_address_set || run->image.image.start_address == 0) + return ERROR_FAIL; + + return esp_algorithm_run_image(target, run, num_args, ap); +} + +int esp_algorithm_load_onboard_func(struct target *target, target_addr_t func_addr, struct esp_algorithm_run_data *run) +{ + int res; + const uint8_t *tramp = NULL; + size_t tramp_sz = 0; + struct duration algo_time; + + if (!run || !run->hw) + return ERROR_FAIL; + + if (duration_start(&algo_time) != 0) { + LOG_ERROR("Failed to start algo time measurement!"); + return ERROR_FAIL; + } + + if (run->hw->stub_tramp_get) { + tramp = run->hw->stub_tramp_get(target, &tramp_sz); + if (!tramp) + return ERROR_FAIL; + } + + if (tramp_sz > run->on_board.code_buf_size) { + LOG_ERROR("Stub tramp size %zu bytes exceeds target buf size %d bytes!", + tramp_sz, run->on_board.code_buf_size); + return ERROR_TARGET_RESOURCE_NOT_AVAILABLE; + } + + if (run->stack_size > run->on_board.min_stack_size) { + LOG_ERROR("Algorithm stack size not fit into the allocated target stack!"); + return ERROR_FAIL; + } + + run->stub.stack_addr = run->on_board.min_stack_addr + run->stack_size; + run->stub.tramp_addr = run->on_board.code_buf_addr; + run->stub.tramp_mapped_addr = run->stub.tramp_addr; + run->stub.entry = func_addr; + + if (tramp) { + res = target_write_buffer(target, run->stub.tramp_addr, tramp_sz, tramp); + if (res != ERROR_OK) { + LOG_ERROR("Failed to write stub jumper!"); + esp_algorithm_unload_onboard_func(target, run); + return res; + } + } + + if (duration_measure(&algo_time) != 0) { + LOG_ERROR("Failed to stop algo run measurement!"); + return ERROR_FAIL; + } + LOG_DEBUG("Stub loaded in %g ms", duration_elapsed(&algo_time) * 1000); + + return ERROR_OK; +} + +int esp_algorithm_unload_onboard_func(struct target *target, struct esp_algorithm_run_data *run) +{ + return ERROR_OK; +} + +int esp_algorithm_exec_onboard_func_va(struct target *target, + struct esp_algorithm_run_data *run, + uint32_t num_args, + va_list ap) +{ + return esp_algorithm_run_debug_stub(target, run, num_args, ap); +} |