path: root/src/target/espressif/esp32_apptrace.c

// 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"
#include "esp32_sysview.h"
#include "segger_sysview.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 int esp32_sysview_start(struct esp32_apptrace_cmd_ctx *ctx);
static int esp32_sysview_stop(struct esp32_apptrace_cmd_ctx *ctx);

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)
{
	if (list_empty(&ctx->ready_trace_blocks))
		return NULL;

	struct esp32_apptrace_block *block =
		list_last_entry(&ctx->ready_trace_blocks, 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 inline bool is_sysview_mode(int mode)
{
	return mode == ESP_APPTRACE_CMD_MODE_SYSVIEW || mode ==	ESP_APPTRACE_CMD_MODE_SYSVIEW_MCORE;
}

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);
	if (is_sysview_mode(ctx->mode)) {
		/* stop tracing */
		res = esp32_sysview_stop(ctx);
		if (res != ERROR_OK)
			LOG_ERROR("sysview: Failed to stop tracing!");
	}
	/* 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);
}

/* this function must be called after connecting to targets */
static int esp32_sysview_start(struct esp32_apptrace_cmd_ctx *ctx)
{
	uint8_t cmds[] = { SEGGER_SYSVIEW_COMMAND_ID_START };
	uint32_t fired_target_num = 0;
	struct esp32_apptrace_target_state target_state[ESP32_APPTRACE_MAX_CORES_NUM] = {{0}};
	struct esp32_sysview_cmd_data *cmd_data = ctx->cmd_priv;

	/* get current block id */
	int res = esp32_apptrace_get_data_info(ctx, target_state, &fired_target_num);
	if (res != ERROR_OK) {
		LOG_ERROR("sysview: Failed to read target data info!");
		return res;
	}
	if (fired_target_num == UINT32_MAX) {
		/* it can happen that there is no pending target data, but block was switched
		 * in this case block_ids on both CPUs are equal, so select the first one */
		fired_target_num = 0;
	}
	/* start tracing */
	res = esp_apptrace_usr_block_write(ctx->hw, ctx->cpus[fired_target_num], target_state[fired_target_num].block_id,
		cmds, sizeof(cmds));
	if (res != ERROR_OK) {
		LOG_ERROR("sysview: Failed to start tracing!");
		return res;
	}
	cmd_data->sv_trace_running = 1;
	return res;
}

static int esp32_sysview_stop(struct esp32_apptrace_cmd_ctx *ctx)
{
	uint32_t old_block_id, fired_target_num = 0, empty_target_num = 0;
	struct esp32_apptrace_target_state target_state[ESP32_APPTRACE_MAX_CORES_NUM];
	struct esp32_sysview_cmd_data *cmd_data = ctx->cmd_priv;
	uint8_t cmds[] = { SEGGER_SYSVIEW_COMMAND_ID_STOP };
	struct duration wait_time;

	struct esp32_apptrace_block *block = esp32_apptrace_free_block_get(ctx);
	if (!block) {
		LOG_ERROR("Failed to get free block for data on (%s)!", target_name(ctx->cpus[fired_target_num]));
		return ERROR_FAIL;
	}

	/* halt all CPUs (not only one), otherwise it can happen that there is no target data and
	 * while we are queueing commands another CPU switches tracing block */
	int res = esp32_apptrace_safe_halt_targets(ctx, target_state);
	if (res != ERROR_OK) {
		LOG_ERROR("sysview: Failed to halt targets (%d)!", res);
		return res;
	}
	/* it can happen that there is no pending target data
	 * in this case block_ids on both CPUs are equal, so the first one will be selected */
	for (unsigned int k = 0; k < ctx->cores_num; k++) {
		if (target_state[k].data_len) {
			fired_target_num = k;
			break;
		}
	}
	if (target_state[fired_target_num].data_len) {
		/* read pending data without ack, they will be acked when stop command is queued */
		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,
			false /*no ack target data*/);
		if (res != ERROR_OK) {
			LOG_ERROR("sysview: Failed to read data on (%s)!", target_name(ctx->cpus[fired_target_num]));
			return res;
		}
		/* process data */
		block->data_len = target_state[fired_target_num].data_len;
		res = esp32_apptrace_handle_trace_block(ctx, block);
		if (res != ERROR_OK) {
			LOG_ERROR("Failed to process trace block %" PRId32 " bytes!", block->data_len);
			return res;
		}
	}
	/* stop tracing and ack target data */
	res = esp_apptrace_usr_block_write(ctx->hw, ctx->cpus[fired_target_num], target_state[fired_target_num].block_id,
		cmds,
		sizeof(cmds));
	if (res != ERROR_OK) {
		LOG_ERROR("sysview: Failed to stop tracing!");
		return res;
	}
	if (ctx->cores_num > 1) {
		empty_target_num = fired_target_num ? 0 : 1;
		/* ack target data on another CPU */
		res = ctx->hw->ctrl_reg_write(ctx->cpus[empty_target_num], target_state[fired_target_num].block_id,
			0 /*target data ack*/,
			true /*host connected*/,
			false /*no host data*/);
		if (res != ERROR_OK) {
			LOG_ERROR("sysview: Failed to ack data on target '%s' (%d)!",
				target_name(ctx->cpus[empty_target_num]), res);
			return res;
		}
	}
	/* resume targets to allow command processing */
	LOG_INFO("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) {
			LOG_ERROR("sysview: Failed to resume target '%s' (%d)!", target_name(ctx->cpus[k]), res);
			return res;
		}
		if (ctx->cpus[k]->smp)
			smp_resumed = true;
	}
	/* wait for block switch (command sent), so we can disconnect from targets */
	old_block_id = target_state[fired_target_num].block_id;
	if (duration_start(&wait_time) != 0) {
		LOG_ERROR("Failed to start trace stop timeout measurement!");
		return ERROR_FAIL;
	}
	/* we are waiting for the last data from tracing block and also there can be data in the pended
	 * data buffer */
	/* so we are expecting two TRX block switches at most or stopping due to timeout */
	while (cmd_data->sv_trace_running) {
		res = esp32_apptrace_get_data_info(ctx, target_state, &fired_target_num);
		if (res != ERROR_OK) {
			LOG_ERROR("sysview: Failed to read targets data info!");
			return res;
		}
		if (fired_target_num == UINT32_MAX) {
			/* it can happen that there is no pending (last) target data, but block was
			 * switched */
			/* in this case block_ids on both CPUs are equal, so select the first one */
			fired_target_num = 0;
		}
		if (target_state[fired_target_num].block_id != old_block_id) {
			if (target_state[fired_target_num].data_len) {
				/* read last data and ack them */
				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,
					true /*ack target data*/);
				if (res != ERROR_OK) {
					LOG_ERROR("sysview: Failed to read last data on (%s)!", target_name(ctx->cpus[fired_target_num]));
				} else {
					if (ctx->cores_num > 1) {
						/* ack target data on another CPU */
						empty_target_num = fired_target_num ? 0 : 1;
						res = ctx->hw->ctrl_reg_write(ctx->cpus[empty_target_num],
							target_state[fired_target_num].block_id,
							0 /*all read*/,
							true /*host connected*/,
							false /*no host data*/);
						if (res != ERROR_OK) {
							LOG_ERROR("sysview: Failed to ack data on target '%s' (%d)!",
								target_name(ctx->cpus[empty_target_num]), res);
							return res;
						}
					}
					/* process data */
					block->data_len = target_state[fired_target_num].data_len;
					res = esp32_apptrace_handle_trace_block(ctx, block);
					if (res != ERROR_OK) {
						LOG_ERROR("Failed to process trace block %" PRId32 " bytes!",
							block->data_len);
						return res;
					}
				}
				old_block_id = target_state[fired_target_num].block_id;
			}
		}
		if (duration_measure(&wait_time) != 0) {
			LOG_ERROR("Failed to start trace stop timeout measurement!");
			return ERROR_FAIL;
		}
		const float stop_tmo = LOG_LEVEL_IS(LOG_LVL_DEBUG) ? 30.0 : 0.5;
		if (duration_elapsed(&wait_time) >= stop_tmo) {
			LOG_INFO("Stop waiting for the last data due to timeout.");
			break;
		}
	}
	return res;
}

static 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) {
		if (is_sysview_mode(mode)) {
			/* init cmd context */
			res = esp32_sysview_cmd_init(&s_at_cmd_ctx,
				cmd,
				mode,
				mode == ESP_APPTRACE_CMD_MODE_SYSVIEW_MCORE,
				&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;
			if (cmd_data->skip_len != 0) {
				s_at_cmd_ctx.running = 0;
				esp32_sysview_cmd_cleanup(&s_at_cmd_ctx);
				command_print(cmd, "Data skipping not supported!");
				return ERROR_FAIL;
			}
			s_at_cmd_ctx.process_data = esp32_sysview_process_data;
		} else {
			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;
		}
		if (is_sysview_mode(mode)) {
			/* start tracing */
			res = esp32_sysview_start(&s_at_cmd_ctx);
			if (res != ERROR_OK) {
				esp32_apptrace_connect_targets(&s_at_cmd_ctx, false, old_state == TARGET_RUNNING);
				s_at_cmd_ctx.running = 0;
				esp32_apptrace_cmd_cleanup(&s_at_cmd_ctx);
				command_print(cmd, "sysview: Failed to start tracing!");
				return res;
			}
		}
		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) {
		if (is_sysview_mode(mode)) {
			command_print(cmd, "Not supported!");
			return ERROR_FAIL;
		}
		/* [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;
	if (is_sysview_mode(mode))
		esp32_sysview_cmd_cleanup(&s_at_cmd_ctx);
	else
		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);
}

COMMAND_HANDLER(esp32_cmd_sysview)
{
	return esp32_cmd_apptrace_generic(CMD, ESP_APPTRACE_CMD_MODE_SYSVIEW, CMD_ARGV, CMD_ARGC);
}

COMMAND_HANDLER(esp32_cmd_sysview_mcore)
{
	return esp32_cmd_apptrace_generic(CMD, ESP_APPTRACE_CMD_MODE_SYSVIEW_MCORE, 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>)",
	},
	{
		.name = "sysview",
		.handler = esp32_cmd_sysview,
		.mode = COMMAND_EXEC,
		.help =
			"App Tracing: SEGGER SystemView compatible trace control. Starts, stops or queries tracing process status.",
		.usage =
			"(start file://<outfile1> [file://<outfile2>] [poll_period [trace_size [stop_tmo [wait4halt [skip_size]]]]) | (stop) | (status)",
	},
	{
		.name = "sysview_mcore",
		.handler = esp32_cmd_sysview_mcore,
		.mode = COMMAND_EXEC,
		.help =
			"App Tracing: Espressif multi-core SystemView trace control. Starts, stops or queries tracing process status.",
		.usage =
			"(start file://<outfile> [poll_period [trace_size [stop_tmo [wait4halt [skip_size]]]]) | (stop) | (status)",
	},
	COMMAND_REGISTRATION_DONE
};