--- /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;
+}