esp_xtensa_apptrace.c

Go to the documentation of this file.

// 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];
 
    int res = xtensa_queue_dbg_reg_read(xtensa, XTENSA_APPTRACE_STAT_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;
    }
    *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;
}