--- /dev/null
+// SPDX-License-Identifier: GPL-2.0-or-later
+
+/***************************************************************************
+ * ESP32xx application tracing module for OpenOCD *
+ * Copyright (C) 2017 Espressif Systems Ltd. *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#ifdef HAVE_ARPA_INET_H
+#include <arpa/inet.h>
+#endif
+
+#ifdef HAVE_NETDB_H
+#include <netdb.h>
+#endif
+
+#ifndef _WIN32
+#include <netinet/tcp.h>
+#include <sys/ioctl.h>
+#endif
+
+#include <helper/list.h>
+#include <helper/time_support.h>
+#include <target/target.h>
+#include <target/target_type.h>
+#include <target/smp.h>
+#include <server/server.h>
+#include "esp_xtensa.h"
+#include "esp_xtensa_smp.h"
+#include "esp_xtensa_apptrace.h"
+#include "esp32_apptrace.h"
+
+#define ESP32_APPTRACE_USER_BLOCK_CORE(_v_) ((_v_) >> 15)
+#define ESP32_APPTRACE_USER_BLOCK_LEN(_v_) ((_v_) & ~BIT(15))
+
+#define ESP32_APPTRACE_USER_BLOCK_HDR_SZ 4
+
+#define ESP_APPTRACE_CMD_MODE_GEN 0
+#define ESP_APPTRACE_CMD_MODE_SYSVIEW 1
+#define ESP_APPTRACE_CMD_MODE_SYSVIEW_MCORE 2
+#define ESP_APPTRACE_CMD_MODE_SYNC 3
+
+#define ESP32_APPTRACE_TGT_STATE_TMO 5000
+#define ESP_APPTRACE_BLOCKS_POOL_SZ 10
+
+struct esp32_apptrace_dest_file_data {
+ int fout;
+};
+
+struct esp32_apptrace_dest_tcp_data {
+ int sockfd;
+};
+
+struct esp32_apptrace_target_state {
+ int running;
+ uint32_t block_id;
+ uint32_t data_len;
+};
+
+struct esp_apptrace_target2host_hdr {
+ uint16_t block_sz;
+ uint16_t wr_sz;
+};
+#define APPTRACE_BLOCK_SIZE_OFFSET 0
+#define APPTRACE_WR_SIZE_OFFSET 2
+
+struct esp32_apptrace_block {
+ struct list_head node;
+ uint8_t *data;
+ uint32_t data_len;
+};
+
+static int esp32_apptrace_data_processor(void *priv);
+static int esp32_apptrace_get_data_info(struct esp32_apptrace_cmd_ctx *ctx,
+ struct esp32_apptrace_target_state *target_state,
+ uint32_t *fired_target_num);
+static int esp32_apptrace_safe_halt_targets(struct esp32_apptrace_cmd_ctx *ctx,
+ struct esp32_apptrace_target_state *targets);
+static struct esp32_apptrace_block *esp32_apptrace_free_block_get(struct esp32_apptrace_cmd_ctx *ctx);
+static int esp32_apptrace_handle_trace_block(struct esp32_apptrace_cmd_ctx *ctx,
+ struct esp32_apptrace_block *block);
+
+static const bool s_time_stats_enable = true;
+
+/*********************************************************************
+* Trace destination API
+**********************************************************************/
+
+static int esp32_apptrace_file_dest_write(void *priv, uint8_t *data, int size)
+{
+ struct esp32_apptrace_dest_file_data *dest_data = (struct esp32_apptrace_dest_file_data *)priv;
+
+ int wr_sz = write(dest_data->fout, data, size);
+ if (wr_sz != size) {
+ LOG_ERROR("Failed to write %d bytes to out file (%d)! Written %d.", size, errno, wr_sz);
+ return ERROR_FAIL;
+ }
+ return ERROR_OK;
+}
+
+static int esp32_apptrace_file_dest_cleanup(void *priv)
+{
+ struct esp32_apptrace_dest_file_data *dest_data = (struct esp32_apptrace_dest_file_data *)priv;
+
+ if (dest_data->fout > 0)
+ close(dest_data->fout);
+ free(dest_data);
+ return ERROR_OK;
+}
+
+static int esp32_apptrace_file_dest_init(struct esp32_apptrace_dest *dest, const char *dest_name)
+{
+ struct esp32_apptrace_dest_file_data *dest_data = calloc(1, sizeof(*dest_data));
+ if (!dest_data) {
+ LOG_ERROR("Failed to alloc mem for file dest!");
+ return ERROR_FAIL;
+ }
+
+ LOG_INFO("Open file %s", dest_name);
+ dest_data->fout = open(dest_name, O_WRONLY | O_CREAT | O_TRUNC | O_BINARY, 0666);
+ if (dest_data->fout <= 0) {
+ LOG_ERROR("Failed to open file %s", dest_name);
+ free(dest_data);
+ return ERROR_FAIL;
+ }
+
+ dest->priv = dest_data;
+ dest->write = esp32_apptrace_file_dest_write;
+ dest->clean = esp32_apptrace_file_dest_cleanup;
+ dest->log_progress = true;
+
+ return ERROR_OK;
+}
+
+static int esp32_apptrace_console_dest_write(void *priv, uint8_t *data, int size)
+{
+ LOG_USER_N("%.*s", size, data);
+ return ERROR_OK;
+}
+
+static int esp32_apptrace_console_dest_cleanup(void *priv)
+{
+ return ERROR_OK;
+}
+
+static int esp32_apptrace_console_dest_init(struct esp32_apptrace_dest *dest, const char *dest_name)
+{
+ dest->priv = NULL;
+ dest->write = esp32_apptrace_console_dest_write;
+ dest->clean = esp32_apptrace_console_dest_cleanup;
+ dest->log_progress = false;
+
+ return ERROR_OK;
+}
+
+static int esp32_apptrace_tcp_dest_write(void *priv, uint8_t *data, int size)
+{
+ struct esp32_apptrace_dest_tcp_data *dest_data = (struct esp32_apptrace_dest_tcp_data *)priv;
+ int wr_sz = write_socket(dest_data->sockfd, data, size);
+ if (wr_sz != size) {
+ LOG_ERROR("Failed to write %u bytes to out socket (%d)! Written %d.", size, errno, wr_sz);
+ return ERROR_FAIL;
+ }
+ return ERROR_OK;
+}
+
+static int esp32_apptrace_tcp_dest_cleanup(void *priv)
+{
+ struct esp32_apptrace_dest_tcp_data *dest_data = (struct esp32_apptrace_dest_tcp_data *)priv;
+
+ if (dest_data->sockfd > 0)
+ close_socket(dest_data->sockfd);
+ free(dest_data);
+ return ERROR_OK;
+}
+
+static int esp32_apptrace_tcp_dest_init(struct esp32_apptrace_dest *dest, const char *dest_name)
+{
+ const char *port_sep = strchr(dest_name, ':');
+ /* separator not found, or was the first or the last character */
+ if (!port_sep || port_sep == dest_name || port_sep == dest_name + strlen(dest_name) - 1) {
+ LOG_ERROR("apptrace: Invalid connection URI, format should be tcp://host:port");
+ return ERROR_COMMAND_ARGUMENT_INVALID;
+ }
+ size_t hostname_len = port_sep - dest_name;
+
+ char hostname[64] = { 0 };
+ if (hostname_len >= sizeof(hostname)) {
+ LOG_ERROR("apptrace: Hostname too long");
+ return ERROR_COMMAND_ARGUMENT_INVALID;
+ }
+ memcpy(hostname, dest_name, hostname_len);
+
+ const char *port_str = port_sep + 1;
+ struct addrinfo *ai;
+ int flags = 0;
+#ifdef AI_NUMERICSERV
+ flags |= AI_NUMERICSERV;
+#endif /* AI_NUMERICSERV */
+ struct addrinfo hint = {
+ .ai_family = AF_UNSPEC,
+ .ai_socktype = SOCK_STREAM,
+ .ai_protocol = 0,
+ .ai_flags = flags
+ };
+ int res = getaddrinfo(hostname, port_str, &hint, &ai);
+ if (res != 0) {
+ LOG_ERROR("apptrace: Failed to resolve host name: %s", hostname);
+ return ERROR_FAIL;
+ }
+ int sockfd = -1;
+ for (struct addrinfo *ai_it = ai; ai_it; ai_it = ai_it->ai_next) {
+ sockfd = socket(ai_it->ai_family, ai_it->ai_socktype, ai_it->ai_protocol);
+ if (sockfd < 0) {
+ LOG_DEBUG("apptrace: Failed to create socket (%d, %d, %d) (%s)",
+ ai_it->ai_family,
+ ai_it->ai_socktype,
+ ai_it->ai_protocol,
+ strerror(errno));
+ continue;
+ }
+
+ char cur_hostname[NI_MAXHOST];
+ char cur_portname[NI_MAXSERV];
+ res =
+ getnameinfo(ai_it->ai_addr, ai_it->ai_addrlen, cur_hostname,
+ sizeof(cur_hostname),
+ cur_portname, sizeof(cur_portname),
+ NI_NUMERICHOST | NI_NUMERICSERV);
+ if (res != 0)
+ continue;
+
+ LOG_INFO("apptrace: Trying to connect to %s:%s", cur_hostname, cur_portname);
+ if (connect(sockfd, ai_it->ai_addr, ai_it->ai_addrlen) < 0) {
+ close_socket(sockfd);
+ sockfd = -1;
+ LOG_WARNING("apptrace: Connection failed (%s)", strerror(errno));
+ continue;
+ }
+ break;
+ }
+ freeaddrinfo(ai);
+ if (sockfd < 0) {
+ LOG_ERROR("apptrace: Could not connect to %s:%s", hostname, port_str);
+ return ERROR_FAIL;
+ }
+ LOG_INFO("apptrace: Connected!");
+
+ struct esp32_apptrace_dest_tcp_data *dest_data = calloc(1, sizeof(struct esp32_apptrace_dest_tcp_data));
+ if (!dest_data) {
+ LOG_ERROR("apptrace: Failed to alloc mem for tcp dest!");
+ close_socket(sockfd);
+ return ERROR_FAIL;
+ }
+
+ dest_data->sockfd = sockfd;
+ dest->priv = dest_data;
+ dest->write = esp32_apptrace_tcp_dest_write;
+ dest->clean = esp32_apptrace_tcp_dest_cleanup;
+ dest->log_progress = true;
+
+ return ERROR_OK;
+}
+
+int esp32_apptrace_dest_init(struct esp32_apptrace_dest dest[], const char *dest_paths[], unsigned int max_dests)
+{
+ int res;
+ unsigned int i;
+
+ for (i = 0; i < max_dests; i++) {
+ if (strncmp(dest_paths[i], "file://", 7) == 0)
+ res = esp32_apptrace_file_dest_init(&dest[i], &dest_paths[i][7]);
+ else if (strncmp(dest_paths[i], "con:", 4) == 0)
+ res = esp32_apptrace_console_dest_init(&dest[i], NULL);
+ else if (strncmp(dest_paths[i], "tcp://", 6) == 0)
+ res = esp32_apptrace_tcp_dest_init(&dest[i], &dest_paths[i][6]);
+ else
+ break;
+
+ if (res != ERROR_OK) {
+ LOG_ERROR("apptrace: Failed to init trace data destination '%s'!", dest_paths[i]);
+ return 0;
+ }
+ }
+
+ return i;
+}
+
+int esp32_apptrace_dest_cleanup(struct esp32_apptrace_dest dest[], unsigned int max_dests)
+{
+ for (unsigned int i = 0; i < max_dests; i++) {
+ if (dest[i].clean && dest[i].priv) {
+ int res = dest[i].clean(dest[i].priv);
+ dest[i].priv = NULL;
+ return res;
+ }
+ }
+ return ERROR_OK;
+}
+
+/*********************************************************************
+* Trace data blocks management API
+**********************************************************************/
+static void esp32_apptrace_blocks_pool_cleanup(struct esp32_apptrace_cmd_ctx *ctx)
+{
+ struct esp32_apptrace_block *cur;
+ struct list_head *head = &ctx->free_trace_blocks;
+ struct list_head *tmp, *pos;
+
+ list_for_each_safe(pos, tmp, head) {
+ cur = list_entry(pos, struct esp32_apptrace_block, node);
+ if (cur) {
+ list_del(&cur->node);
+ free(cur->data);
+ free(cur);
+ }
+ }
+
+ head = &ctx->ready_trace_blocks;
+
+ list_for_each_safe(pos, tmp, head) {
+ cur = list_entry(pos, struct esp32_apptrace_block, node);
+ if (cur) {
+ list_del(&cur->node);
+ free(cur->data);
+ free(cur);
+ }
+ }
+}
+
+struct esp32_apptrace_block *esp32_apptrace_free_block_get(struct esp32_apptrace_cmd_ctx *ctx)
+{
+ struct esp32_apptrace_block *block = NULL;
+
+ if (!list_empty(&ctx->free_trace_blocks)) {
+ /*get first */
+ block = list_first_entry(&ctx->free_trace_blocks, struct esp32_apptrace_block, node);
+ list_del(&block->node);
+ }
+
+ return block;
+}
+
+static int esp32_apptrace_ready_block_put(struct esp32_apptrace_cmd_ctx *ctx, struct esp32_apptrace_block *block)
+{
+ LOG_DEBUG("esp32_apptrace_ready_block_put");
+ /* add to ready blocks list */
+ INIT_LIST_HEAD(&block->node);
+ list_add(&block->node, &ctx->ready_trace_blocks);
+
+ return ERROR_OK;
+}
+
+static struct esp32_apptrace_block *esp32_apptrace_ready_block_get(struct esp32_apptrace_cmd_ctx *ctx)
+{
+ struct esp32_apptrace_block *block = NULL;
+
+ if (!list_empty(&ctx->ready_trace_blocks)) {
+ struct list_head *head = &ctx->ready_trace_blocks;
+ struct list_head *tmp, *pos;
+
+ list_for_each_safe(pos, tmp, head) {
+ block = list_entry(pos, struct esp32_apptrace_block, node);
+ }
+ /* remove it from ready list */
+ list_del(&block->node);
+ }
+
+ return block;
+}
+
+static int esp32_apptrace_block_free(struct esp32_apptrace_cmd_ctx *ctx, struct esp32_apptrace_block *block)
+{
+ /* add to free blocks list */
+ INIT_LIST_HEAD(&block->node);
+ list_add(&block->node, &ctx->free_trace_blocks);
+
+ return ERROR_OK;
+}
+
+static int esp32_apptrace_wait_tracing_finished(struct esp32_apptrace_cmd_ctx *ctx)
+{
+ int64_t timeout = timeval_ms() + (LOG_LEVEL_IS(LOG_LVL_DEBUG) ? 70000 : 5000);
+ while (!list_empty(&ctx->ready_trace_blocks)) {
+ alive_sleep(100);
+ if (timeval_ms() >= timeout) {
+ LOG_ERROR("Failed to wait for pended trace blocks!");
+ return ERROR_FAIL;
+ }
+ }
+ /* signal timer callback to stop */
+ ctx->running = 0;
+ target_unregister_timer_callback(esp32_apptrace_data_processor, ctx);
+ return ERROR_OK;
+}
+
+/*********************************************************************
+* Trace commands
+**********************************************************************/
+
+int esp32_apptrace_cmd_ctx_init(struct esp32_apptrace_cmd_ctx *cmd_ctx, struct command_invocation *cmd, int mode)
+{
+ struct target *target = get_current_target(CMD_CTX);
+
+ memset(cmd_ctx, 0, sizeof(struct esp32_apptrace_cmd_ctx));
+ cmd_ctx->target = target;
+ cmd_ctx->mode = mode;
+ cmd_ctx->target_state = target->state;
+ cmd_ctx->cmd = cmd;
+
+ if (target->smp) {
+ struct target_list *head;
+ struct target *curr;
+ unsigned int i = 0;
+ cmd_ctx->cores_num = 0;
+ foreach_smp_target(head, target->smp_targets) {
+ curr = head->target;
+ if (i == ESP32_APPTRACE_MAX_CORES_NUM) {
+ command_print(cmd, "Too many cores configured! Max %d cores are supported.",
+ ESP32_APPTRACE_MAX_CORES_NUM);
+ return ERROR_FAIL;
+ }
+ if (!target_was_examined(curr))
+ continue;
+ cmd_ctx->cores_num++;
+ cmd_ctx->cpus[i++] = curr;
+ }
+ } else {
+ cmd_ctx->cores_num = 1;
+ cmd_ctx->cpus[0] = target;
+ }
+ /* some relies on ESP32_APPTRACE_MAX_CORES_NUM
+ * TODO: remove that dependency */
+ assert(cmd_ctx->cores_num <= ESP32_APPTRACE_MAX_CORES_NUM && "Too many cores number!");
+
+ struct xtensa *xtensa = target->arch_info;
+ if (xtensa->common_magic == XTENSA_COMMON_MAGIC) {
+ cmd_ctx->hw = target_to_esp_xtensa(target)->apptrace.hw;
+ } else { /* TODO: riscv is not supported yet */
+ command_print(cmd, "Unsupported target arch 0x%X", xtensa->common_magic);
+ return ERROR_FAIL;
+ }
+
+ cmd_ctx->max_trace_block_sz = cmd_ctx->hw->max_block_size_get(cmd_ctx->cpus[0]);
+ if (cmd_ctx->max_trace_block_sz == 0) {
+ command_print(cmd, "Failed to get max trace block size!");
+ return ERROR_FAIL;
+ }
+ LOG_INFO("Total trace memory: %" PRIu32 " bytes", cmd_ctx->max_trace_block_sz);
+
+ INIT_LIST_HEAD(&cmd_ctx->ready_trace_blocks);
+ INIT_LIST_HEAD(&cmd_ctx->free_trace_blocks);
+ for (unsigned int i = 0; i < ESP_APPTRACE_BLOCKS_POOL_SZ; i++) {
+ struct esp32_apptrace_block *block = calloc(1, sizeof(struct esp32_apptrace_block));
+ if (!block) {
+ command_print(cmd, "Failed to alloc trace buffer entry!");
+ esp32_apptrace_blocks_pool_cleanup(cmd_ctx);
+ return ERROR_FAIL;
+ }
+ block->data = malloc(cmd_ctx->max_trace_block_sz);
+ if (!block->data) {
+ free(block);
+ command_print(cmd, "Failed to alloc trace buffer %" PRIu32 " bytes!", cmd_ctx->max_trace_block_sz);
+ esp32_apptrace_blocks_pool_cleanup(cmd_ctx);
+ return ERROR_FAIL;
+ }
+ INIT_LIST_HEAD(&block->node);
+ list_add(&block->node, &cmd_ctx->free_trace_blocks);
+ }
+
+ cmd_ctx->running = 1;
+ if (cmd_ctx->mode != ESP_APPTRACE_CMD_MODE_SYNC) {
+ int res = target_register_timer_callback(esp32_apptrace_data_processor,
+ 0,
+ TARGET_TIMER_TYPE_PERIODIC,
+ cmd_ctx);
+ if (res != ERROR_OK) {
+ command_print(cmd, "Failed to start trace data timer callback (%d)!", res);
+ esp32_apptrace_blocks_pool_cleanup(cmd_ctx);
+ return ERROR_FAIL;
+ }
+ }
+
+ if (s_time_stats_enable) {
+ cmd_ctx->stats.min_blk_read_time = 1000000.0;
+ cmd_ctx->stats.min_blk_proc_time = 1000000.0;
+ }
+ if (duration_start(&cmd_ctx->idle_time) != 0) {
+ command_print(cmd, "Failed to start idle time measurement!");
+ esp32_apptrace_cmd_ctx_cleanup(cmd_ctx);
+ return ERROR_FAIL;
+ }
+
+ return ERROR_OK;
+}
+
+int esp32_apptrace_cmd_ctx_cleanup(struct esp32_apptrace_cmd_ctx *cmd_ctx)
+{
+ esp32_apptrace_blocks_pool_cleanup(cmd_ctx);
+ return ERROR_OK;
+}
+
+#define ESP32_APPTRACE_CMD_NUM_ARG_CHECK(_cmd_, _arg_, _start_, _end_) \
+ do { \
+ if ((_arg_) == 0 && (_start_) == (_end_)) { \
+ command_print(_cmd_, "Invalid '" # _arg_ "' arg!"); \
+ return; \
+ } \
+ } while (0)
+
+void esp32_apptrace_cmd_args_parse(struct esp32_apptrace_cmd_ctx *cmd_ctx,
+ struct esp32_apptrace_cmd_data *cmd_data,
+ const char **argv,
+ int argc)
+{
+ char *end;
+
+ cmd_data->poll_period = strtoul(argv[0], &end, 10);
+ ESP32_APPTRACE_CMD_NUM_ARG_CHECK(cmd_ctx->cmd, cmd_data->poll_period, argv[0], end);
+ if (argc > 1) {
+ cmd_data->max_len = strtoul(argv[1], &end, 10);
+ ESP32_APPTRACE_CMD_NUM_ARG_CHECK(cmd_ctx->cmd, cmd_data->max_len, argv[1], end);
+ if (argc > 2) {
+ int32_t tmo = strtol(argv[2], &end, 10);
+ ESP32_APPTRACE_CMD_NUM_ARG_CHECK(cmd_ctx->cmd, tmo, argv[2], end);
+ cmd_ctx->stop_tmo = 1.0 * tmo;
+ if (argc > 3) {
+ cmd_data->wait4halt = strtoul(argv[3], &end, 10);
+ ESP32_APPTRACE_CMD_NUM_ARG_CHECK(cmd_ctx->cmd, cmd_data->wait4halt, argv[3], end);
+ if (argc > 4) {
+ cmd_data->skip_len = strtoul(argv[4], &end, 10);
+ ESP32_APPTRACE_CMD_NUM_ARG_CHECK(cmd_ctx->cmd, cmd_data->skip_len, argv[4], end);
+ }
+ }
+ }
+ }
+}
+
+static int esp32_apptrace_core_id_get(struct target *target, uint8_t *hdr_buf)
+{
+ return ESP32_APPTRACE_USER_BLOCK_CORE(target_buffer_get_u16(target, hdr_buf + APPTRACE_BLOCK_SIZE_OFFSET));
+}
+
+static uint32_t esp32_apptrace_usr_block_len_get(struct target *target, uint8_t *hdr_buf, uint32_t *wr_len)
+{
+ *wr_len = ESP32_APPTRACE_USER_BLOCK_LEN(target_buffer_get_u16(target, hdr_buf + APPTRACE_WR_SIZE_OFFSET));
+ return ESP32_APPTRACE_USER_BLOCK_LEN(target_buffer_get_u16(target, hdr_buf + APPTRACE_BLOCK_SIZE_OFFSET));
+}
+
+static int esp32_apptrace_cmd_init(struct esp32_apptrace_cmd_ctx *cmd_ctx,
+ struct command_invocation *cmd,
+ int mode,
+ const char **argv,
+ int argc)
+{
+ struct esp32_apptrace_cmd_data *cmd_data;
+
+ if (argc < 1) {
+ command_print(cmd, "Not enough args! Need trace data destination!");
+ return ERROR_FAIL;
+ }
+
+ int res = esp32_apptrace_cmd_ctx_init(cmd_ctx, cmd, mode);
+ if (res != ERROR_OK)
+ return res;
+
+ cmd_data = calloc(1, sizeof(*cmd_data));
+ assert(cmd_data && "No memory for command data!");
+ cmd_ctx->cmd_priv = cmd_data;
+
+ /*outfile1 [poll_period [trace_size [stop_tmo [wait4halt [skip_size]]]]] */
+ res = esp32_apptrace_dest_init(&cmd_data->data_dest, argv, 1);
+ if (res != 1) { /* only one destination needs to be initialized */
+ command_print(cmd, "Wrong args! Needs a trace data destination!");
+ free(cmd_data);
+ goto on_error;
+ }
+ cmd_ctx->stop_tmo = -1.0; /* infinite */
+ cmd_data->max_len = UINT32_MAX;
+ cmd_data->poll_period = 0 /*ms*/;
+ if (argc > 1)
+ /* parse remaining args */
+ esp32_apptrace_cmd_args_parse(cmd_ctx, cmd_data, &argv[1], argc - 1);
+
+ LOG_USER("App trace params: from %d cores, size %" PRId32 " bytes, stop_tmo %g s, poll period %" PRId32
+ " ms, wait_rst %d, skip %" PRId32 " bytes", cmd_ctx->cores_num,
+ cmd_data->max_len,
+ cmd_ctx->stop_tmo,
+ cmd_data->poll_period,
+ cmd_data->wait4halt,
+ cmd_data->skip_len);
+
+ cmd_ctx->trace_format.hdr_sz = ESP32_APPTRACE_USER_BLOCK_HDR_SZ;
+ cmd_ctx->trace_format.core_id_get = esp32_apptrace_core_id_get;
+ cmd_ctx->trace_format.usr_block_len_get = esp32_apptrace_usr_block_len_get;
+ return ERROR_OK;
+on_error:
+ command_print(cmd, "Not enough args! Need %d trace data destinations!", cmd_ctx->cores_num);
+ cmd_ctx->running = 0;
+ esp32_apptrace_cmd_ctx_cleanup(cmd_ctx);
+ return res;
+}
+
+static int esp32_apptrace_cmd_cleanup(struct esp32_apptrace_cmd_ctx *cmd_ctx)
+{
+ struct esp32_apptrace_cmd_data *cmd_data = cmd_ctx->cmd_priv;
+
+ esp32_apptrace_dest_cleanup(&cmd_data->data_dest, 1);
+ free(cmd_data);
+ cmd_ctx->cmd_priv = NULL;
+ esp32_apptrace_cmd_ctx_cleanup(cmd_ctx);
+ return ERROR_OK;
+}
+
+static void esp32_apptrace_print_stats(struct esp32_apptrace_cmd_ctx *ctx)
+{
+ struct esp32_apptrace_cmd_data *cmd_data = ctx->cmd_priv;
+ uint32_t trace_sz = 0;
+
+ if (cmd_data)
+ trace_sz = ctx->tot_len > cmd_data->skip_len ? ctx->tot_len - cmd_data->skip_len : 0;
+ LOG_USER("Tracing is %s. Size is %" PRId32 " of %" PRId32 " @ %f (%f) KiB/s",
+ !ctx->running ? "STOPPED" : "RUNNING",
+ trace_sz,
+ cmd_data ? cmd_data->max_len : 0,
+ duration_kbps(&ctx->read_time, ctx->tot_len),
+ duration_kbps(&ctx->read_time, ctx->raw_tot_len));
+ LOG_USER("Data: blocks incomplete %" PRId32 ", lost bytes: %" PRId32,
+ ctx->stats.incompl_blocks,
+ ctx->stats.lost_bytes);
+ if (s_time_stats_enable) {
+ LOG_USER("Block read time [%f..%f] ms",
+ 1000 * ctx->stats.min_blk_read_time,
+ 1000 * ctx->stats.max_blk_read_time);
+ LOG_USER("Block proc time [%f..%f] ms",
+ 1000 * ctx->stats.min_blk_proc_time,
+ 1000 * ctx->stats.max_blk_proc_time);
+ }
+}
+
+static int esp32_apptrace_wait4halt(struct esp32_apptrace_cmd_ctx *ctx, struct target *target)
+{
+ LOG_USER("Wait for halt...");
+ while (!openocd_is_shutdown_pending()) {
+ int res = target_poll(target);
+ if (res != ERROR_OK)
+ return res;
+ if (target->state == TARGET_HALTED) {
+ LOG_USER("%s: HALTED", target->cmd_name);
+ break;
+ }
+ alive_sleep(500);
+ }
+ return ERROR_OK;
+}
+
+int esp32_apptrace_safe_halt_targets(struct esp32_apptrace_cmd_ctx *ctx,
+ struct esp32_apptrace_target_state *targets)
+{
+ int res = ERROR_OK;
+
+ memset(targets, 0, ctx->cores_num * sizeof(struct esp32_apptrace_target_state));
+ /* halt all CPUs */
+ LOG_DEBUG("Halt all targets!");
+ for (unsigned int k = 0; k < ctx->cores_num; k++) {
+ if (!target_was_examined(ctx->cpus[k]))
+ continue;
+ if (ctx->cpus[k]->state == TARGET_HALTED)
+ continue;
+ res = target_halt(ctx->cpus[k]);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to halt target (%d)!", res);
+ return res;
+ }
+ res = target_wait_state(ctx->cpus[k], TARGET_HALTED, ESP32_APPTRACE_TGT_STATE_TMO);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to wait halt target %s / %d (%d)!",
+ target_name(ctx->cpus[k]),
+ ctx->cpus[k]->state,
+ res);
+ return res;
+ }
+ }
+ /* read current block statuses from CPUs */
+ LOG_DEBUG("Read current block statuses");
+ for (unsigned int k = 0; k < ctx->cores_num; k++) {
+ uint32_t stat;
+ res = ctx->hw->status_reg_read(ctx->cpus[k], &stat);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to read trace status (%d)!", res);
+ return res;
+ }
+ /* check if some CPU stopped inside tracing regs update critical section */
+ if (stat) {
+ if (ctx->hw->leave_trace_crit_section_start) {
+ res = ctx->hw->leave_trace_crit_section_start(ctx->cpus[k]);
+ if (res != ERROR_OK)
+ return res;
+ }
+ uint32_t bp_addr = stat;
+ res = breakpoint_add(ctx->cpus[k], bp_addr, 1, BKPT_HARD);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to set breakpoint (%d)!", res);
+ return res;
+ }
+ while (stat) {
+ /* allow this CPU to leave ERI write critical section */
+ res = target_resume(ctx->cpus[k], 1, 0, 1, 0);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to resume target (%d)!", res);
+ breakpoint_remove(ctx->cpus[k], bp_addr);
+ return res;
+ }
+ /* wait for CPU to be halted on BP */
+ enum target_debug_reason debug_reason = DBG_REASON_UNDEFINED;
+ while (debug_reason != DBG_REASON_BREAKPOINT) {
+ res = target_wait_state(ctx->cpus[k], TARGET_HALTED,
+ ESP32_APPTRACE_TGT_STATE_TMO);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to wait halt on bp (%d)!", res);
+ breakpoint_remove(ctx->cpus[k], bp_addr);
+ return res;
+ }
+ debug_reason = ctx->cpus[k]->debug_reason;
+ }
+ res = ctx->hw->status_reg_read(ctx->cpus[k], &stat);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to read trace status (%d)!", res);
+ breakpoint_remove(ctx->cpus[k], bp_addr);
+ return res;
+ }
+ }
+ breakpoint_remove(ctx->cpus[k], bp_addr);
+ if (ctx->hw->leave_trace_crit_section_stop) {
+ res = ctx->hw->leave_trace_crit_section_stop(ctx->cpus[k]);
+ if (res != ERROR_OK)
+ return res;
+ }
+ }
+ res = ctx->hw->data_len_read(ctx->cpus[k], &targets[k].block_id, &targets[k].data_len);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to read trace status (%d)!", res);
+ return res;
+ }
+ }
+
+ return ERROR_OK;
+}
+
+static int esp32_apptrace_connect_targets(struct esp32_apptrace_cmd_ctx *ctx,
+ bool conn,
+ bool resume_target)
+{
+ struct esp32_apptrace_target_state target_to_connect[ESP32_APPTRACE_MAX_CORES_NUM];
+
+ if (conn)
+ LOG_USER("Connect targets...");
+ else
+ LOG_USER("Disconnect targets...");
+
+ int res = esp32_apptrace_safe_halt_targets(ctx, target_to_connect);
+ if (res != ERROR_OK) {
+ command_print(ctx->cmd, "Failed to halt targets (%d)!", res);
+ return res;
+ }
+ if (ctx->cores_num > 1) {
+ /* set block ids to the highest value */
+ uint32_t max_id = 0;
+ for (unsigned int k = 0; k < ctx->cores_num; k++) {
+ if (target_to_connect[k].block_id > max_id)
+ max_id = target_to_connect[k].block_id;
+ }
+ for (unsigned int k = 0; k < ctx->cores_num; k++)
+ target_to_connect[k].block_id = max_id;
+ }
+ for (unsigned int k = 0; k < ctx->cores_num; k++) {
+ /* update host connected status */
+ res = ctx->hw->ctrl_reg_write(ctx->cpus[k],
+ target_to_connect[k].block_id,
+ 0 /*ack target data*/,
+ conn,
+ false /*no host data*/);
+ if (res != ERROR_OK) {
+ command_print(ctx->cmd, "Failed to read trace status (%d)!", res);
+ return res;
+ }
+ }
+ if (resume_target) {
+ LOG_DEBUG("Resume targets");
+ bool smp_resumed = false;
+ for (unsigned int k = 0; k < ctx->cores_num; k++) {
+ if (smp_resumed && ctx->cpus[k]->smp) {
+ /* in SMP mode we need to call target_resume for one core only */
+ continue;
+ }
+ res = target_resume(ctx->cpus[k], 1, 0, 1, 0);
+ if (res != ERROR_OK) {
+ command_print(ctx->cmd, "Failed to resume target (%d)!", res);
+ return res;
+ }
+ if (ctx->cpus[k]->smp)
+ smp_resumed = true;
+ }
+ }
+ if (conn)
+ LOG_INFO("Targets connected.");
+ else
+ LOG_INFO("Targets disconnected.");
+ return ERROR_OK;
+}
+
+int esp_apptrace_usr_block_write(const struct esp32_apptrace_hw *hw, struct target *target,
+ uint32_t block_id,
+ const uint8_t *data,
+ uint32_t size)
+{
+ struct esp_apptrace_host2target_hdr hdr = { .block_sz = size };
+ uint32_t buf_sz[2] = { sizeof(hdr), size };
+ const uint8_t *bufs[2] = { (const uint8_t *)&hdr, data };
+
+ if (size > hw->usr_block_max_size_get(target)) {
+ LOG_ERROR("Too large user block %" PRId32, size);
+ return ERROR_FAIL;
+ }
+
+ return hw->buffs_write(target,
+ ARRAY_SIZE(buf_sz),
+ buf_sz,
+ bufs,
+ block_id,
+ true /*ack target data*/,
+ true /*host data*/);
+}
+
+static uint32_t esp32_apptrace_usr_block_check(struct esp32_apptrace_cmd_ctx *ctx, uint8_t *hdr_buf)
+{
+ uint32_t wr_len = 0;
+ uint32_t usr_len = ctx->trace_format.usr_block_len_get(ctx->target, hdr_buf, &wr_len);
+ if (usr_len != wr_len) {
+ LOG_ERROR("Incomplete block sz %" PRId32 ", wr %" PRId32, usr_len, wr_len);
+ ctx->stats.incompl_blocks++;
+ ctx->stats.lost_bytes += usr_len - wr_len;
+ }
+ return usr_len;
+}
+
+int esp32_apptrace_get_data_info(struct esp32_apptrace_cmd_ctx *ctx,
+ struct esp32_apptrace_target_state *target_state,
+ uint32_t *fired_target_num)
+{
+ if (fired_target_num)
+ *fired_target_num = UINT32_MAX;
+
+ for (unsigned int i = 0; i < ctx->cores_num; i++) {
+ int res = ctx->hw->data_len_read(ctx->cpus[i], &target_state[i].block_id, &target_state[i].data_len);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to read data len on (%s)!", target_name(ctx->cpus[i]));
+ return res;
+ }
+ if (target_state[i].data_len) {
+ LOG_TARGET_DEBUG(ctx->cpus[i], "Block %" PRId32 ", len %" PRId32 " bytes on fired",
+ target_state[i].block_id, target_state[i].data_len);
+ if (fired_target_num)
+ *fired_target_num = i;
+ break;
+ }
+ }
+ return ERROR_OK;
+}
+
+static int esp32_apptrace_process_data(struct esp32_apptrace_cmd_ctx *ctx,
+ unsigned int core_id,
+ uint8_t *data,
+ uint32_t data_len)
+{
+ struct esp32_apptrace_cmd_data *cmd_data = ctx->cmd_priv;
+
+ LOG_DEBUG("Got block %" PRId32 " bytes [%x %x...%x %x]", data_len, data[12], data[13],
+ data[data_len - 2], data[data_len - 1]);
+ if (ctx->tot_len + data_len > cmd_data->skip_len) {
+ uint32_t wr_idx = 0, wr_chunk_len = data_len;
+ if (ctx->tot_len < cmd_data->skip_len) {
+ wr_chunk_len = (ctx->tot_len + wr_chunk_len) - cmd_data->skip_len;
+ wr_idx = cmd_data->skip_len - ctx->tot_len;
+ }
+ if (ctx->tot_len + wr_chunk_len > cmd_data->max_len)
+ wr_chunk_len -= (ctx->tot_len + wr_chunk_len - cmd_data->skip_len) - cmd_data->max_len;
+ if (wr_chunk_len > 0) {
+ int res = cmd_data->data_dest.write(cmd_data->data_dest.priv, data + wr_idx, wr_chunk_len);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to write %" PRId32 " bytes to dest 0!", data_len);
+ return res;
+ }
+ }
+ ctx->tot_len += wr_chunk_len;
+ } else {
+ ctx->tot_len += data_len;
+ }
+
+ if (cmd_data->data_dest.log_progress)
+ LOG_USER("%" PRId32 " ", ctx->tot_len);
+ /* check for stop condition */
+ if (ctx->tot_len > cmd_data->skip_len && (ctx->tot_len - cmd_data->skip_len >= cmd_data->max_len)) {
+ ctx->running = 0;
+ if (duration_measure(&ctx->read_time) != 0) {
+ LOG_ERROR("Failed to stop trace read time measure!");
+ return ERROR_FAIL;
+ }
+ }
+ return ERROR_OK;
+}
+
+static int esp32_apptrace_handle_trace_block(struct esp32_apptrace_cmd_ctx *ctx,
+ struct esp32_apptrace_block *block)
+{
+ uint32_t processed = 0;
+ uint32_t hdr_sz = ctx->trace_format.hdr_sz;
+
+ LOG_DEBUG("Got block %" PRId32 " bytes", block->data_len);
+ /* process user blocks one by one */
+ while (processed < block->data_len) {
+ LOG_DEBUG("Process usr block %" PRId32 "/%" PRId32, processed, block->data_len);
+ /* process user block */
+ uint32_t usr_len = esp32_apptrace_usr_block_check(ctx, block->data + processed);
+ int core_id = ctx->trace_format.core_id_get(ctx->target, block->data + processed);
+ /* process user data */
+ int res = ctx->process_data(ctx, core_id, block->data + processed + hdr_sz, usr_len);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to process %" PRId32 " bytes!", usr_len);
+ return res;
+ }
+ processed += usr_len + hdr_sz;
+ }
+ return ERROR_OK;
+}
+
+static int esp32_apptrace_data_processor(void *priv)
+{
+ struct esp32_apptrace_cmd_ctx *ctx = (struct esp32_apptrace_cmd_ctx *)priv;
+
+ if (!ctx->running)
+ return ERROR_OK;
+
+ struct esp32_apptrace_block *block = esp32_apptrace_ready_block_get(ctx);
+ if (!block)
+ return ERROR_OK;
+
+ int res = esp32_apptrace_handle_trace_block(ctx, block);
+ if (res != ERROR_OK) {
+ ctx->running = 0;
+ LOG_ERROR("Failed to process trace block %" PRId32 " bytes!", block->data_len);
+ return res;
+ }
+ res = esp32_apptrace_block_free(ctx, block);
+ if (res != ERROR_OK) {
+ ctx->running = 0;
+ LOG_ERROR("Failed to free ready block!");
+ return res;
+ }
+
+ return ERROR_OK;
+}
+
+static int esp32_apptrace_check_connection(struct esp32_apptrace_cmd_ctx *ctx)
+{
+ if (!ctx)
+ return ERROR_FAIL;
+
+ unsigned int busy_target_num = 0;
+
+ for (unsigned int i = 0; i < ctx->cores_num; i++) {
+ bool conn = true;
+ int res = ctx->hw->ctrl_reg_read(ctx->cpus[i], NULL, NULL, &conn);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to read apptrace control reg for cpu(%d) res(%d)!", i, res);
+ return res;
+ }
+ if (!conn) {
+ uint32_t stat = 0;
+ LOG_TARGET_WARNING(ctx->cpus[i], "apptrace connection is lost. Re-connect.");
+ res = ctx->hw->status_reg_read(ctx->cpus[i], &stat);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to read trace status (%d)!", res);
+ return res;
+ }
+ if (stat) {
+ LOG_TARGET_WARNING(ctx->cpus[i], "in critical state. Retry in next poll");
+ if (++busy_target_num == ctx->cores_num) {
+ LOG_WARNING("No available core");
+ return ERROR_WAIT;
+ }
+ continue;
+ }
+ res = ctx->hw->ctrl_reg_write(ctx->cpus[i],
+ 0,
+ 0,
+ true /*host connected*/,
+ false /*no host data*/);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to write apptrace control reg for cpu(%d) res(%d)!", i, res);
+ return res;
+ }
+ if (ctx->stop_tmo != -1.0) {
+ /* re-start idle time measurement */
+ if (duration_start(&ctx->idle_time) != 0) {
+ LOG_ERROR("Failed to re-start idle time measure!");
+ return ERROR_FAIL;
+ }
+ }
+ }
+ }
+
+ return ERROR_OK;
+}
+
+static int esp32_apptrace_poll(void *priv)
+{
+ struct esp32_apptrace_cmd_ctx *ctx = (struct esp32_apptrace_cmd_ctx *)priv;
+ int res;
+ uint32_t fired_target_num = 0;
+ struct esp32_apptrace_target_state target_state[ESP32_APPTRACE_MAX_CORES_NUM];
+ struct duration blk_proc_time;
+
+ if (!ctx->running) {
+ if (ctx->auto_clean)
+ ctx->auto_clean(ctx);
+ return ERROR_FAIL;
+ }
+
+ /* Check for connection is alive.For some reason target and therefore host_connected flag
+ * might have been reset */
+ res = esp32_apptrace_check_connection(ctx);
+ if (res != ERROR_OK) {
+ if (res != ERROR_WAIT)
+ ctx->running = 0;
+ return res;
+ }
+
+ /* check for data from target */
+ res = esp32_apptrace_get_data_info(ctx, target_state, &fired_target_num);
+ if (res != ERROR_OK) {
+ ctx->running = 0;
+ LOG_ERROR("Failed to read data len!");
+ return res;
+ }
+ /* LOG_DEBUG("Block %d (%d bytes) on target (%s)!", target_state[0].block_id,
+ * target_state[0].data_len, target_name(ctx->cpus[0])); */
+ if (fired_target_num == UINT32_MAX) {
+ /* no data has been received, but block could be switched due to the data transferred
+ * from host to target */
+ if (ctx->cores_num > 1) {
+ uint32_t max_block_id = 0, min_block_id = ctx->hw->max_block_id;
+ /* find maximum block ID and set the same ID in control reg for both cores
+ * */
+ for (unsigned int i = 0; i < ctx->cores_num; i++) {
+ if (max_block_id < target_state[i].block_id)
+ max_block_id = target_state[i].block_id;
+ if (min_block_id > target_state[i].block_id)
+ min_block_id = target_state[i].block_id;
+ }
+ /* handle block ID overflow */
+ if (max_block_id == ctx->hw->max_block_id && min_block_id == 0)
+ max_block_id = 0;
+ for (unsigned int i = 0; i < ctx->cores_num; i++) {
+ if (max_block_id != target_state[i].block_id) {
+ LOG_TARGET_DEBUG(ctx->cpus[i], "Ack empty block %" PRId32 "!", max_block_id);
+ res = ctx->hw->ctrl_reg_write(ctx->cpus[i],
+ max_block_id,
+ 0 /*all read*/,
+ true /*host connected*/,
+ false /*no host data*/);
+ if (res != ERROR_OK) {
+ ctx->running = 0;
+ LOG_TARGET_ERROR(ctx->cpus[i], "Failed to ack empty data block!");
+ return res;
+ }
+ }
+ }
+ ctx->last_blk_id = max_block_id;
+ }
+ if (ctx->stop_tmo != -1.0) {
+ if (duration_measure(&ctx->idle_time) != 0) {
+ ctx->running = 0;
+ LOG_ERROR("Failed to measure idle time!");
+ return ERROR_FAIL;
+ }
+ if (duration_elapsed(&ctx->idle_time) >= ctx->stop_tmo) {
+ ctx->running = 0;
+ LOG_ERROR("Data timeout!");
+ return ERROR_FAIL;
+ }
+ }
+ return ERROR_OK;/* no data */
+ }
+ /* sanity check */
+ if (target_state[fired_target_num].data_len > ctx->max_trace_block_sz) {
+ ctx->running = 0;
+ LOG_ERROR("Too large block size %" PRId32 "!", target_state[fired_target_num].data_len);
+ return ERROR_FAIL;
+ }
+ if (ctx->tot_len == 0) {
+ if (duration_start(&ctx->read_time) != 0) {
+ ctx->running = 0;
+ LOG_ERROR("Failed to start trace read time measurement!");
+ return ERROR_FAIL;
+ }
+ }
+ struct esp32_apptrace_block *block = esp32_apptrace_free_block_get(ctx);
+ if (!block) {
+ ctx->running = 0;
+ LOG_TARGET_ERROR(ctx->cpus[fired_target_num], "Failed to get free block for data!");
+ return ERROR_FAIL;
+ }
+ if (s_time_stats_enable) {
+ /* read block */
+ if (duration_start(&blk_proc_time) != 0) {
+ ctx->running = 0;
+ LOG_ERROR("Failed to start block read time measurement!");
+ return ERROR_FAIL;
+ }
+ }
+ res =
+ ctx->hw->data_read(ctx->cpus[fired_target_num],
+ target_state[fired_target_num].data_len,
+ block->data,
+ target_state[fired_target_num].block_id,
+ /* do not ack target data in sync mode,
+ esp32_apptrace_handle_trace_block() can write response data and will do ack thereafter */
+ ctx->mode != ESP_APPTRACE_CMD_MODE_SYNC);
+ if (res != ERROR_OK) {
+ ctx->running = 0;
+ LOG_TARGET_ERROR(ctx->cpus[fired_target_num], "Failed to read data!");
+ return res;
+ }
+ ctx->last_blk_id = target_state[fired_target_num].block_id;
+ block->data_len = target_state[fired_target_num].data_len;
+ ctx->raw_tot_len += block->data_len;
+ if (s_time_stats_enable) {
+ if (duration_measure(&blk_proc_time) != 0) {
+ ctx->running = 0;
+ LOG_ERROR("Failed to measure block read time!");
+ return ERROR_FAIL;
+ }
+ /* update stats */
+ float brt = duration_elapsed(&blk_proc_time);
+ if (brt > ctx->stats.max_blk_read_time)
+ ctx->stats.max_blk_read_time = brt;
+ if (brt < ctx->stats.min_blk_read_time)
+ ctx->stats.min_blk_read_time = brt;
+
+ if (duration_start(&blk_proc_time) != 0) {
+ ctx->running = 0;
+ LOG_ERROR("Failed to start block proc time measurement!");
+ return ERROR_FAIL;
+ }
+ }
+ /* in sync mode do not ack target data on other cores, esp32_apptrace_handle_trace_block() can write response
+ * data and will do ack thereafter */
+ if (ctx->mode != ESP_APPTRACE_CMD_MODE_SYNC) {
+ for (unsigned int i = 0; i < ctx->cores_num; i++) {
+ if (i == fired_target_num)
+ continue;
+ res = ctx->hw->ctrl_reg_write(ctx->cpus[i],
+ ctx->last_blk_id,
+ 0 /*all read*/,
+ true /*host connected*/,
+ false /*no host data*/);
+ if (res != ERROR_OK) {
+ ctx->running = 0;
+ LOG_TARGET_ERROR(ctx->cpus[i], "Failed to ack data!");
+ return res;
+ }
+ LOG_TARGET_DEBUG(ctx->cpus[i], "Ack block %" PRId32, ctx->last_blk_id);
+ }
+ res = esp32_apptrace_ready_block_put(ctx, block);
+ if (res != ERROR_OK) {
+ ctx->running = 0;
+ LOG_TARGET_ERROR(ctx->cpus[fired_target_num], "Failed to put ready block of data!");
+ return res;
+ }
+ } else {
+ res = esp32_apptrace_handle_trace_block(ctx, block);
+ if (res != ERROR_OK) {
+ ctx->running = 0;
+ LOG_ERROR("Failed to process trace block %" PRId32 " bytes!", block->data_len);
+ return res;
+ }
+ res = esp32_apptrace_block_free(ctx, block);
+ if (res != ERROR_OK) {
+ ctx->running = 0;
+ LOG_ERROR("Failed to free ready block!");
+ return res;
+ }
+ }
+ if (ctx->stop_tmo != -1.0) {
+ /* start idle time measurement */
+ if (duration_start(&ctx->idle_time) != 0) {
+ ctx->running = 0;
+ LOG_ERROR("Failed to start idle time measure!");
+ return ERROR_FAIL;
+ }
+ }
+ if (s_time_stats_enable) {
+ if (duration_measure(&blk_proc_time) != 0) {
+ ctx->running = 0;
+ LOG_ERROR("Failed to stop block proc time measure!");
+ return ERROR_FAIL;
+ }
+ /* update stats */
+ float bt = duration_elapsed(&blk_proc_time);
+ if (bt > ctx->stats.max_blk_proc_time)
+ ctx->stats.max_blk_proc_time = bt;
+ if (bt < ctx->stats.min_blk_proc_time)
+ ctx->stats.min_blk_proc_time = bt;
+ }
+ return ERROR_OK;
+}
+
+static void esp32_apptrace_cmd_stop(struct esp32_apptrace_cmd_ctx *ctx)
+{
+ if (duration_measure(&ctx->read_time) != 0)
+ LOG_ERROR("Failed to stop trace read time measurement!");
+ int res = target_unregister_timer_callback(esp32_apptrace_poll, ctx);
+ if (res != ERROR_OK)
+ LOG_ERROR("Failed to unregister target timer handler (%d)!", res);
+
+ /* data processor is alive, so wait for all received blocks to be processed */
+ res = esp32_apptrace_wait_tracing_finished(ctx);
+ if (res != ERROR_OK)
+ LOG_ERROR("Failed to wait for pended blocks (%d)!", res);
+ res = esp32_apptrace_connect_targets(ctx, false, ctx->target_state == TARGET_RUNNING);
+ if (res != ERROR_OK)
+ LOG_ERROR("Failed to disconnect targets (%d)!", res);
+ esp32_apptrace_print_stats(ctx);
+ res = esp32_apptrace_cmd_cleanup(ctx);
+ if (res != ERROR_OK)
+ LOG_ERROR("Failed to cleanup cmd ctx (%d)!", res);
+}
+
+int esp32_cmd_apptrace_generic(struct command_invocation *cmd, int mode, const char **argv, int argc)
+{
+ static struct esp32_apptrace_cmd_ctx s_at_cmd_ctx;
+ struct esp32_apptrace_cmd_data *cmd_data;
+ int res = ERROR_FAIL;
+ enum target_state old_state;
+ struct target *target = get_current_target(CMD_CTX);
+
+ if (argc < 1)
+ return ERROR_COMMAND_SYNTAX_ERROR;
+
+ /* command can be invoked on unexamined core, if so find examined one */
+ if (target->smp && !target_was_examined(target)) {
+ struct target_list *head;
+ struct target *curr;
+ LOG_WARNING("Current target '%s' was not examined!", target_name(target));
+ foreach_smp_target(head, target->smp_targets) {
+ curr = head->target;
+ if (target_was_examined(curr)) {
+ target = curr;
+ LOG_WARNING("Run command on target '%s'", target_name(target));
+ break;
+ }
+ }
+ }
+ old_state = target->state;
+
+ if (strcmp(argv[0], "start") == 0) {
+ res = esp32_apptrace_cmd_init(&s_at_cmd_ctx,
+ cmd,
+ mode,
+ &argv[1],
+ argc - 1);
+ if (res != ERROR_OK) {
+ command_print(cmd, "Failed to init cmd ctx (%d)!", res);
+ return res;
+ }
+ cmd_data = s_at_cmd_ctx.cmd_priv;
+ s_at_cmd_ctx.process_data = esp32_apptrace_process_data;
+ s_at_cmd_ctx.auto_clean = esp32_apptrace_cmd_stop;
+ if (cmd_data->wait4halt) {
+ res = esp32_apptrace_wait4halt(&s_at_cmd_ctx, target);
+ if (res != ERROR_OK) {
+ command_print(cmd, "Failed to wait for halt target (%d)!", res);
+ goto _on_start_error;
+ }
+ }
+ res = esp32_apptrace_connect_targets(&s_at_cmd_ctx, true, old_state == TARGET_RUNNING);
+ if (res != ERROR_OK) {
+ command_print(cmd, "Failed to connect to targets (%d)!", res);
+ goto _on_start_error;
+ }
+ res = target_register_timer_callback(esp32_apptrace_poll,
+ cmd_data->poll_period,
+ TARGET_TIMER_TYPE_PERIODIC,
+ &s_at_cmd_ctx);
+ if (res != ERROR_OK) {
+ command_print(cmd, "Failed to register target timer handler (%d)!", res);
+ goto _on_start_error;
+ }
+ } else if (strcmp(argv[0], "stop") == 0) {
+ if (!s_at_cmd_ctx.running) {
+ command_print(cmd, "Tracing is not running!");
+ return ERROR_FAIL;
+ }
+ esp32_apptrace_cmd_stop(&s_at_cmd_ctx);
+ return ERROR_OK;
+ } else if (strcmp(argv[0], "status") == 0) {
+ if (s_at_cmd_ctx.running && duration_measure(&s_at_cmd_ctx.read_time) != 0)
+ LOG_ERROR("Failed to measure trace read time!");
+ esp32_apptrace_print_stats(&s_at_cmd_ctx);
+ return ERROR_OK;
+ } else if (strcmp(argv[0], "dump") == 0) {
+ /* [dump outfile] - post-mortem dump without connection to targets */
+ res = esp32_apptrace_cmd_init(&s_at_cmd_ctx,
+ cmd,
+ mode,
+ &argv[1],
+ argc - 1);
+ if (res != ERROR_OK) {
+ command_print(cmd, "Failed to init cmd ctx (%d)!", res);
+ return res;
+ }
+ s_at_cmd_ctx.stop_tmo = 0.01; /* use small stop tmo */
+ s_at_cmd_ctx.process_data = esp32_apptrace_process_data;
+ /* check for exit signal and command completion */
+ while (!openocd_is_shutdown_pending() && s_at_cmd_ctx.running) {
+ res = esp32_apptrace_poll(&s_at_cmd_ctx);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to poll target for trace data (%d)!", res);
+ break;
+ }
+ /* let registered timer callbacks to run */
+ target_call_timer_callbacks();
+ }
+ if (s_at_cmd_ctx.running) {
+ /* data processor is alive, so wait for all received blocks to be processed */
+ res = esp32_apptrace_wait_tracing_finished(&s_at_cmd_ctx);
+ if (res != ERROR_OK)
+ LOG_ERROR("Failed to wait for pended blocks (%d)!", res);
+ }
+ esp32_apptrace_print_stats(&s_at_cmd_ctx);
+ res = esp32_apptrace_cmd_cleanup(&s_at_cmd_ctx);
+ if (res != ERROR_OK)
+ command_print(cmd, "Failed to cleanup cmd ctx (%d)!", res);
+ } else {
+ command_print(cmd, "Invalid action '%s'!", argv[0]);
+ }
+
+ return res;
+
+_on_start_error:
+ s_at_cmd_ctx.running = 0;
+ esp32_apptrace_cmd_cleanup(&s_at_cmd_ctx);
+ return res;
+}
+
+COMMAND_HANDLER(esp32_cmd_apptrace)
+{
+ return esp32_cmd_apptrace_generic(CMD, ESP_APPTRACE_CMD_MODE_GEN, CMD_ARGV, CMD_ARGC);
+}
+
+const struct command_registration esp32_apptrace_command_handlers[] = {
+ {
+ .name = "apptrace",
+ .handler = esp32_cmd_apptrace,
+ .mode = COMMAND_EXEC,
+ .help =
+ "App Tracing: application level trace control. Starts, stops or queries tracing process status.",
+ .usage =
+ "[start <destination> [poll_period [trace_size [stop_tmo [wait4halt [skip_size]]]]] | [stop] | [status] | [dump <destination>]",
+ },
+ COMMAND_REGISTRATION_DONE
+};
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0-or-later
+
+/***************************************************************************
+ * Xtensa application tracing module for OpenOCD *
+ * Copyright (C) 2017 Espressif Systems Ltd. *
+ ***************************************************************************/
+
+/*
+ How it works?
+ https://github.com/espressif/esp-idf/blob/master/components/app_trace/port/xtensa/port.c#L8
+*/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <helper/align.h>
+#include <target/xtensa/xtensa.h>
+#include <target/xtensa/xtensa_debug_module.h>
+#include "esp_xtensa_apptrace.h"
+
+/* TRAX is disabled, so we use its registers for our own purposes
+ * | 31..XXXXXX..24 | 23 .(host_connect). 23 | 22 .(host_data). 22| 21..(block_id)..15 | 14..(block_len)..0 |
+ */
+#define XTENSA_APPTRACE_CTRL_REG XDMREG_DELAYCNT
+#define XTENSA_APPTRACE_BLOCK_ID_MSK 0x7FUL
+#define XTENSA_APPTRACE_BLOCK_ID_MAX XTENSA_APPTRACE_BLOCK_ID_MSK
+/* if non-zero then apptrace code entered the critical section and the value is an address of the
+ * critical section's exit point */
+#define XTENSA_APPTRACE_STAT_REG XDMREG_TRIGGERPC
+
+#define XTENSA_APPTRACE_BLOCK_LEN_MSK 0x7FFFUL
+#define XTENSA_APPTRACE_BLOCK_LEN(_l_) ((_l_) & XTENSA_APPTRACE_BLOCK_LEN_MSK)
+#define XTENSA_APPTRACE_BLOCK_LEN_GET(_v_) ((_v_) & XTENSA_APPTRACE_BLOCK_LEN_MSK)
+#define XTENSA_APPTRACE_BLOCK_ID(_id_) (((_id_) & XTENSA_APPTRACE_BLOCK_ID_MSK) << 15)
+#define XTENSA_APPTRACE_BLOCK_ID_GET(_v_) (((_v_) >> 15) & XTENSA_APPTRACE_BLOCK_ID_MSK)
+#define XTENSA_APPTRACE_HOST_DATA BIT(22)
+#define XTENSA_APPTRACE_HOST_CONNECT BIT(23)
+
+static int esp_xtensa_apptrace_leave_crit_section_start(struct target *target);
+static int esp_xtensa_apptrace_leave_crit_section_stop(struct target *target);
+static int esp_xtensa_apptrace_buffs_write(struct target *target,
+ uint32_t bufs_num,
+ uint32_t buf_sz[],
+ const uint8_t *bufs[],
+ uint32_t block_id,
+ bool ack,
+ bool data);
+
+struct esp32_apptrace_hw esp_xtensa_apptrace_hw = {
+ .max_block_id = XTENSA_APPTRACE_BLOCK_ID_MAX,
+ .max_block_size_get = esp_xtensa_apptrace_block_max_size_get,
+ .status_reg_read = esp_xtensa_apptrace_status_reg_read,
+ .ctrl_reg_write = esp_xtensa_apptrace_ctrl_reg_write,
+ .ctrl_reg_read = esp_xtensa_apptrace_ctrl_reg_read,
+ .data_len_read = esp_xtensa_apptrace_data_len_read,
+ .data_read = esp_xtensa_apptrace_data_read,
+ .usr_block_max_size_get = esp_xtensa_apptrace_usr_block_max_size_get,
+ .buffs_write = esp_xtensa_apptrace_buffs_write,
+ .leave_trace_crit_section_start = esp_xtensa_apptrace_leave_crit_section_start,
+ .leave_trace_crit_section_stop = esp_xtensa_apptrace_leave_crit_section_stop,
+};
+
+uint32_t esp_xtensa_apptrace_block_max_size_get(struct target *target)
+{
+ struct xtensa *xtensa = target_to_xtensa(target);
+ struct xtensa_trace_status trace_status;
+ struct xtensa_trace_config trace_config;
+ uint32_t max_trace_block_sz;
+
+ int res = xtensa_dm_trace_status_read(&xtensa->dbg_mod, &trace_status);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to read TRAX status (%d)!", res);
+ return 0;
+ }
+
+ max_trace_block_sz = BIT(((trace_status.stat >> 8) & 0x1f) - 2) * 4;
+ res = xtensa_dm_trace_config_read(&xtensa->dbg_mod, &trace_config);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to read TRAX config (%d)!", res);
+ return 0;
+ }
+ LOG_DEBUG("ctrl=0x%" PRIx32 " memadrstart=0x%" PRIx32 " memadrend=0x%" PRIx32 " traxadr=0x%" PRIx32,
+ trace_config.ctrl,
+ trace_config.memaddr_start,
+ trace_config.memaddr_end,
+ trace_config.addr);
+
+ return max_trace_block_sz;
+}
+
+uint32_t esp_xtensa_apptrace_usr_block_max_size_get(struct target *target)
+{
+ return esp_xtensa_apptrace_block_max_size_get(target) - sizeof(struct esp_apptrace_host2target_hdr);
+}
+
+int esp_xtensa_apptrace_data_len_read(struct target *target,
+ uint32_t *block_id,
+ uint32_t *len)
+{
+ return esp_xtensa_apptrace_ctrl_reg_read(target, block_id, len, NULL);
+}
+
+int esp_xtensa_apptrace_usr_block_write(struct target *target,
+ uint32_t block_id,
+ const uint8_t *data,
+ uint32_t size)
+{
+ return esp_apptrace_usr_block_write(&esp_xtensa_apptrace_hw, target, block_id, data, size);
+}
+
+static int esp_xtensa_apptrace_data_reverse_read(struct xtensa *xtensa,
+ uint32_t size,
+ uint8_t *buffer,
+ uint8_t *unal_bytes)
+{
+ int res = 0;
+ uint32_t rd_sz = ALIGN_UP(size, 4);
+
+ res = xtensa_queue_dbg_reg_write(xtensa, XDMREG_TRAXADDR, (xtensa->core_config->trace.mem_sz - rd_sz) / 4);
+ if (res != ERROR_OK)
+ return res;
+ if (!IS_ALIGNED(size, 4)) {
+ res = xtensa_queue_dbg_reg_read(xtensa, XDMREG_TRAXDATA, unal_bytes);
+ if (res != ERROR_OK)
+ return res;
+ }
+ for (unsigned int i = size / 4; i != 0; i--) {
+ res = xtensa_queue_dbg_reg_read(xtensa, XDMREG_TRAXDATA, &buffer[(i - 1) * 4]);
+ if (res != ERROR_OK)
+ return res;
+ }
+ return ERROR_OK;
+}
+
+static int esp_xtensa_apptrace_data_normal_read(struct xtensa *xtensa,
+ uint32_t size,
+ uint8_t *buffer,
+ uint8_t *unal_bytes)
+{
+ int res = xtensa_queue_dbg_reg_write(xtensa, XDMREG_TRAXADDR, 0);
+ if (res != ERROR_OK)
+ return res;
+ for (unsigned int i = 0; i < size / 4; i++) {
+ res = xtensa_queue_dbg_reg_read(xtensa, XDMREG_TRAXDATA, &buffer[i * 4]);
+ if (res != ERROR_OK)
+ return res;
+ }
+ if (!IS_ALIGNED(size, 4)) {
+ res = xtensa_queue_dbg_reg_read(xtensa, XDMREG_TRAXDATA, unal_bytes);
+ if (res != ERROR_OK)
+ return res;
+ }
+ return ERROR_OK;
+}
+
+int esp_xtensa_apptrace_data_read(struct target *target,
+ uint32_t size,
+ uint8_t *buffer,
+ uint32_t block_id,
+ bool ack)
+{
+ struct xtensa *xtensa = target_to_xtensa(target);
+ int res;
+ uint32_t tmp = XTENSA_APPTRACE_HOST_CONNECT | XTENSA_APPTRACE_BLOCK_ID(block_id) |
+ XTENSA_APPTRACE_BLOCK_LEN(0);
+ uint8_t unal_bytes[4];
+
+ LOG_DEBUG("Read data on target (%s)", target_name(target));
+ if (xtensa->core_config->trace.reversed_mem_access)
+ res = esp_xtensa_apptrace_data_reverse_read(xtensa, size, buffer, unal_bytes);
+ else
+ res = esp_xtensa_apptrace_data_normal_read(xtensa, size, buffer, unal_bytes);
+ if (res != ERROR_OK)
+ return res;
+ if (ack) {
+ LOG_DEBUG("Ack block %" PRIu32 " target (%s)!", block_id, target_name(target));
+ res = xtensa_queue_dbg_reg_write(xtensa, XTENSA_APPTRACE_CTRL_REG, tmp);
+ if (res != ERROR_OK)
+ return res;
+ }
+ xtensa_dm_queue_tdi_idle(&xtensa->dbg_mod);
+ res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to exec JTAG queue!");
+ return res;
+ }
+ if (!IS_ALIGNED(size, 4)) {
+ /* copy the last unaligned bytes */
+ memcpy(buffer + ALIGN_DOWN(size, 4), unal_bytes, size & 0x3UL);
+ }
+ return ERROR_OK;
+}
+
+int esp_xtensa_apptrace_ctrl_reg_write(struct target *target,
+ uint32_t block_id,
+ uint32_t len,
+ bool conn,
+ bool data)
+{
+ struct xtensa *xtensa = target_to_xtensa(target);
+ uint32_t tmp = (conn ? XTENSA_APPTRACE_HOST_CONNECT : 0) |
+ (data ? XTENSA_APPTRACE_HOST_DATA : 0) | XTENSA_APPTRACE_BLOCK_ID(block_id) |
+ XTENSA_APPTRACE_BLOCK_LEN(len);
+
+ xtensa_queue_dbg_reg_write(xtensa, XTENSA_APPTRACE_CTRL_REG, tmp);
+ xtensa_dm_queue_tdi_idle(&xtensa->dbg_mod);
+ int res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to exec JTAG queue!");
+ return res;
+ }
+
+ return ERROR_OK;
+}
+
+int esp_xtensa_apptrace_ctrl_reg_read(struct target *target,
+ uint32_t *block_id,
+ uint32_t *len,
+ bool *conn)
+{
+ struct xtensa *xtensa = target_to_xtensa(target);
+ uint8_t tmp[4];
+
+ xtensa_queue_dbg_reg_read(xtensa, XTENSA_APPTRACE_CTRL_REG, tmp);
+ xtensa_dm_queue_tdi_idle(&xtensa->dbg_mod);
+ int res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
+ if (res != ERROR_OK)
+ return res;
+ uint32_t val = target_buffer_get_u32(target, tmp);
+ if (block_id)
+ *block_id = XTENSA_APPTRACE_BLOCK_ID_GET(val);
+ if (len)
+ *len = XTENSA_APPTRACE_BLOCK_LEN_GET(val);
+ if (conn)
+ *conn = val & XTENSA_APPTRACE_HOST_CONNECT;
+ return ERROR_OK;
+}
+
+int esp_xtensa_apptrace_status_reg_read(struct target *target, uint32_t *stat)
+{
+ struct xtensa *xtensa = target_to_xtensa(target);
+ uint8_t tmp[4];
+
+ xtensa_queue_dbg_reg_read(xtensa, XTENSA_APPTRACE_STAT_REG, tmp);
+ xtensa_dm_queue_tdi_idle(&xtensa->dbg_mod);
+ int res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to exec JTAG queue!");
+ return res;
+ }
+ *stat = buf_get_u32(tmp, 0, 32);
+ return ERROR_OK;
+}
+
+int esp_xtensa_apptrace_status_reg_write(struct target *target, uint32_t stat)
+{
+ struct xtensa *xtensa = target_to_xtensa(target);
+
+ xtensa_queue_dbg_reg_write(xtensa, XTENSA_APPTRACE_STAT_REG, stat);
+ xtensa_dm_queue_tdi_idle(&xtensa->dbg_mod);
+ int res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to exec JTAG queue!");
+ return res;
+ }
+ return ERROR_OK;
+}
+
+static int esp_xtensa_swdbg_activate(struct target *target, int enab)
+{
+ struct xtensa *xtensa = target_to_xtensa(target);
+
+ xtensa_queue_dbg_reg_write(xtensa, enab ? XDMREG_DCRSET : XDMREG_DCRCLR, OCDDCR_DEBUGSWACTIVE);
+ xtensa_dm_queue_tdi_idle(&xtensa->dbg_mod);
+ int res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
+ if (res != ERROR_OK) {
+ LOG_ERROR("%s: writing DCR failed!", target->cmd_name);
+ return res;
+ }
+
+ return ERROR_OK;
+}
+
+static int esp_xtensa_apptrace_leave_crit_section_start(struct target *target)
+{
+ /* TODO: not sure that we need this, but it seems that we fail to leave tracing critical
+ *section w/o this */
+ int res = esp_xtensa_swdbg_activate(target, 1 /*enable*/);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to activate SW debug (%d)!", res);
+ return res;
+ }
+ return ERROR_OK;
+}
+
+static int esp_xtensa_apptrace_leave_crit_section_stop(struct target *target)
+{
+ int res = esp_xtensa_swdbg_activate(target, 0 /*disable*/);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to activate SW debug (%d)!", res);
+ return res;
+ }
+ return ERROR_OK;
+}
+
+static int esp_xtensa_apptrace_queue_reverse_write(struct target *target, uint32_t bufs_num,
+ uint32_t buf_sz[], const uint8_t *bufs[])
+{
+ int res = ERROR_OK;
+ uint32_t cached_bytes = 0, total_sz = 0;
+ uint8_t cached_data8[sizeof(uint32_t)] = { 0 };
+ uint32_t cached_data32 = 0;
+
+ struct xtensa *xtensa = target_to_xtensa(target);
+
+ for (uint32_t i = 0; i < bufs_num; i++)
+ total_sz += buf_sz[i];
+ if (!IS_ALIGNED(total_sz, 4)) {
+ cached_bytes = sizeof(uint32_t) - (total_sz & 0x3UL);
+ total_sz = ALIGN_UP(total_sz, 4);
+ }
+ xtensa_queue_dbg_reg_write(xtensa, XDMREG_TRAXADDR, (xtensa->core_config->trace.mem_sz - total_sz) / 4);
+ for (uint32_t i = bufs_num; i > 0; i--) {
+ uint32_t bsz = buf_sz[i - 1];
+ const uint8_t *cur_buf = &bufs[i - 1][bsz];
+ uint32_t bytes_to_cache;
+ /* if there are cached bytes from the previous buffer, combine them with the last
+ * from the current buffer */
+ if (cached_bytes) {
+ if ((cached_bytes + bsz) < sizeof(uint32_t))
+ bytes_to_cache = bsz;
+ else
+ bytes_to_cache = sizeof(uint32_t) - cached_bytes;
+ memcpy(&cached_data8[sizeof(uint32_t) - cached_bytes - bytes_to_cache],
+ cur_buf - bytes_to_cache,
+ bytes_to_cache);
+ cached_data32 = target_buffer_get_u32(target, cached_data8);
+ cached_bytes += bytes_to_cache;
+ if (cached_bytes < sizeof(uint32_t))
+ continue;
+ res = xtensa_queue_dbg_reg_write(xtensa, XDMREG_TRAXDATA, cached_data32);
+ if (res != ERROR_OK)
+ return res;
+ bsz -= bytes_to_cache;
+ cur_buf -= bytes_to_cache;
+ memset(cached_data8, 0x00, sizeof(cached_data8));
+ cached_bytes = 0;
+ }
+ /* write full dwords */
+ for (unsigned int k = bsz; k >= sizeof(uint32_t); k -= sizeof(uint32_t)) {
+ uint32_t temp = target_buffer_get_u32(target, cur_buf - sizeof(uint32_t));
+ res = xtensa_queue_dbg_reg_write(xtensa, XDMREG_TRAXDATA, temp);
+ if (res != ERROR_OK)
+ return res;
+ cur_buf -= sizeof(uint32_t);
+ }
+ /* if there are bytes to be cached (1..3) */
+ bytes_to_cache = bsz & 0x3UL;
+ if (bytes_to_cache > 0) {
+ if (bytes_to_cache + cached_bytes >= sizeof(uint32_t)) {
+ /* filling the cache buffer from the end to beginning */
+ uint32_t to_copy = sizeof(uint32_t) - cached_bytes;
+ memcpy(&cached_data8[0], cur_buf - to_copy, to_copy);
+ cached_data32 = target_buffer_get_u32(target, cached_data8);
+ /* write full word of cached bytes */
+ res = xtensa_queue_dbg_reg_write(xtensa, XDMREG_TRAXDATA, cached_data32);
+ if (res != ERROR_OK)
+ return res;
+ /* cache remaining bytes */
+ memset(cached_data8, 0x00, sizeof(cached_data8));
+ cur_buf -= to_copy;
+ to_copy = bytes_to_cache + cached_bytes - sizeof(uint32_t);
+ memcpy(&cached_data8[sizeof(uint32_t) - to_copy], cur_buf - to_copy, to_copy);
+ cached_bytes = to_copy;
+ } else {
+ /* filling the cache buffer from the end to beginning */
+ memcpy(&cached_data8[sizeof(uint32_t) - cached_bytes - bytes_to_cache],
+ cur_buf - bytes_to_cache,
+ bytes_to_cache);
+ cached_bytes += bytes_to_cache;
+ }
+ }
+ }
+ return ERROR_OK;
+}
+
+static int esp_xtensa_apptrace_queue_normal_write(struct target *target, uint32_t bufs_num,
+ uint32_t buf_sz[], const uint8_t *bufs[])
+{
+ int res = ERROR_OK;
+ uint32_t cached_bytes = 0;
+ uint8_t cached_data8[4] = { 0 };
+ uint32_t cached_data32 = 0;
+
+ struct xtensa *xtensa = target_to_xtensa(target);
+
+ /* | 1 | 2 | 1 | 2 | 4 |.......|
+ * | 4 | 4 | 4 | */
+ xtensa_queue_dbg_reg_write(xtensa, XDMREG_TRAXADDR, 0);
+ for (unsigned int i = 0; i < bufs_num; i++) {
+ uint32_t bsz = buf_sz[i];
+ const uint8_t *cur_buf = bufs[i];
+ uint32_t bytes_to_cache;
+ /* if there are cached bytes from the previous buffer, combine them with the last
+ * from the current buffer */
+ if (cached_bytes) {
+ if ((cached_bytes + bsz) < sizeof(uint32_t))
+ bytes_to_cache = bsz;
+ else
+ bytes_to_cache = sizeof(uint32_t) - cached_bytes;
+ memcpy(&cached_data8[cached_bytes], cur_buf, bytes_to_cache);
+ cached_bytes += bytes_to_cache;
+ if (cached_bytes < sizeof(uint32_t))
+ continue;
+ cached_data32 = target_buffer_get_u32(target, cached_data8);
+ res = xtensa_queue_dbg_reg_write(xtensa, XDMREG_TRAXDATA, cached_data32);
+ if (res != ERROR_OK)
+ return res;
+ bsz -= bytes_to_cache;
+ cur_buf += bytes_to_cache;
+ memset(cached_data8, 0x00, sizeof(cached_data8));
+ cached_bytes = 0;
+ }
+ /* write full dwords */
+ for (unsigned int k = 0; (k + sizeof(uint32_t)) <= bsz; k += sizeof(uint32_t)) {
+ uint32_t temp = target_buffer_get_u32(target, cur_buf);
+ res = xtensa_queue_dbg_reg_write(xtensa, XDMREG_TRAXDATA, temp);
+ if (res != ERROR_OK)
+ return res;
+ cur_buf += sizeof(uint32_t);
+ }
+ /* if there are bytes to be cached (1..3) */
+ bytes_to_cache = bsz & 0x3UL;
+ if (bytes_to_cache > 0) {
+ if (bytes_to_cache + cached_bytes >= sizeof(uint32_t)) {
+ memcpy(&cached_data8[0], cur_buf, sizeof(uint32_t) - cached_bytes);
+ cached_data32 = target_buffer_get_u32(target, cached_data8);
+ /* write full word of cached bytes */
+ res = xtensa_queue_dbg_reg_write(xtensa, XDMREG_TRAXDATA, cached_data32);
+ if (res != ERROR_OK)
+ return res;
+ /* cache remaining bytes */
+ memset(cached_data8, 0x00, sizeof(cached_data8));
+ cur_buf += sizeof(uint32_t) - cached_bytes;
+ cached_bytes = bytes_to_cache + cached_bytes - sizeof(uint32_t);
+ memcpy(&cached_data8[0], cur_buf, cached_bytes);
+ } else {
+ memcpy(&cached_data8[cached_bytes], cur_buf, bytes_to_cache);
+ cached_bytes += bytes_to_cache;
+ }
+ }
+ }
+ if (cached_bytes) {
+ /* write remaining cached bytes */
+ cached_data32 = target_buffer_get_u32(target, cached_data8);
+ res = xtensa_queue_dbg_reg_write(xtensa, XDMREG_TRAXDATA, cached_data32);
+ if (res != ERROR_OK)
+ return res;
+ }
+ return ERROR_OK;
+}
+
+static int esp_xtensa_apptrace_buffs_write(struct target *target,
+ uint32_t bufs_num,
+ uint32_t buf_sz[],
+ const uint8_t *bufs[],
+ uint32_t block_id,
+ bool ack,
+ bool data)
+{
+ struct xtensa *xtensa = target_to_xtensa(target);
+ int res = ERROR_OK;
+ uint32_t tmp = XTENSA_APPTRACE_HOST_CONNECT |
+ (data ? XTENSA_APPTRACE_HOST_DATA : 0) | XTENSA_APPTRACE_BLOCK_ID(block_id) |
+ XTENSA_APPTRACE_BLOCK_LEN(0);
+
+ if (xtensa->core_config->trace.reversed_mem_access)
+ res = esp_xtensa_apptrace_queue_reverse_write(target, bufs_num, buf_sz, bufs);
+ else
+ res = esp_xtensa_apptrace_queue_normal_write(target, bufs_num, buf_sz, bufs);
+ if (res != ERROR_OK)
+ return res;
+ if (ack) {
+ LOG_DEBUG("Ack block %" PRId32 " on target (%s)!", block_id, target_name(target));
+ res = xtensa_queue_dbg_reg_write(xtensa, XTENSA_APPTRACE_CTRL_REG, tmp);
+ if (res != ERROR_OK)
+ return res;
+ }
+ xtensa_dm_queue_tdi_idle(&xtensa->dbg_mod);
+ res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
+ if (res != ERROR_OK) {
+ LOG_ERROR("Failed to exec JTAG queue!");
+ return res;
+ }
+ return ERROR_OK;
+}
Code Review / openocd.git / commitdiff