src/target/espressif/esp32_apptrace.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2
   3 /***************************************************************************
   4  *   ESP32xx application tracing module for OpenOCD                        *
   5  *   Copyright (C) 2017 Espressif Systems Ltd.                             *
   6  ***************************************************************************/
   7
   8 #ifdef HAVE_CONFIG_H
   9 #include "config.h"
  10 #endif
  11
  12 #ifdef HAVE_ARPA_INET_H
  13 #include <arpa/inet.h>
  14 #endif
  15
  16 #ifdef HAVE_NETDB_H
  17 #include <netdb.h>
  18 #endif
  19
  20 #ifndef _WIN32
  21 #include <netinet/tcp.h>
  22 #include <sys/ioctl.h>
  23 #endif
  24
  25 #include <helper/list.h>
  26 #include <helper/time_support.h>
  27 #include <target/target.h>
  28 #include <target/target_type.h>
  29 #include <target/smp.h>
  30 #include <server/server.h>
  31 #include "esp_xtensa.h"
  32 #include "esp_xtensa_smp.h"
  33 #include "esp_xtensa_apptrace.h"
  34 #include "esp32_apptrace.h"
  35
  36 #define ESP32_APPTRACE_USER_BLOCK_CORE(_v_)     ((_v_) >> 15)
  37 #define ESP32_APPTRACE_USER_BLOCK_LEN(_v_)      ((_v_) & ~BIT(15))
  38
  39 #define ESP32_APPTRACE_USER_BLOCK_HDR_SZ        4
  40
  41 #define ESP_APPTRACE_CMD_MODE_GEN               0
  42 #define ESP_APPTRACE_CMD_MODE_SYSVIEW           1
  43 #define ESP_APPTRACE_CMD_MODE_SYSVIEW_MCORE     2
  44 #define ESP_APPTRACE_CMD_MODE_SYNC              3
  45
  46 #define ESP32_APPTRACE_TGT_STATE_TMO            5000
  47 #define ESP_APPTRACE_BLOCKS_POOL_SZ             10
  48
  49 struct esp32_apptrace_dest_file_data {
  50         int fout;
  51 };
  52
  53 struct esp32_apptrace_dest_tcp_data {
  54         int sockfd;
  55 };
  56
  57 struct esp32_apptrace_target_state {
  58         int running;
  59         uint32_t block_id;
  60         uint32_t data_len;
  61 };
  62
  63 struct esp_apptrace_target2host_hdr {
  64         uint16_t block_sz;
  65         uint16_t wr_sz;
  66 };
  67 #define APPTRACE_BLOCK_SIZE_OFFSET      0
  68 #define APPTRACE_WR_SIZE_OFFSET         2
  69
  70 struct esp32_apptrace_block {
  71         struct list_head node;
  72         uint8_t *data;
  73         uint32_t data_len;
  74 };
  75
  76 static int esp32_apptrace_data_processor(void *priv);
  77 static int esp32_apptrace_get_data_info(struct esp32_apptrace_cmd_ctx *ctx,
  78         struct esp32_apptrace_target_state *target_state,
  79         uint32_t *fired_target_num);
  80 static int esp32_apptrace_safe_halt_targets(struct esp32_apptrace_cmd_ctx *ctx,
  81         struct esp32_apptrace_target_state *targets);
  82 static struct esp32_apptrace_block *esp32_apptrace_free_block_get(struct esp32_apptrace_cmd_ctx *ctx);
  83 static int esp32_apptrace_handle_trace_block(struct esp32_apptrace_cmd_ctx *ctx,
  84         struct esp32_apptrace_block *block);
  85
  86 static const bool s_time_stats_enable = true;
  87
  88 /*********************************************************************
  89 *                       Trace destination API
  90 **********************************************************************/
  91
  92 static int esp32_apptrace_file_dest_write(void *priv, uint8_t *data, int size)
  93 {
  94         struct esp32_apptrace_dest_file_data *dest_data = (struct esp32_apptrace_dest_file_data *)priv;
  95
  96         int wr_sz = write(dest_data->fout, data, size);
  97         if (wr_sz != size) {
  98                 LOG_ERROR("Failed to write %d bytes to out file (%d)! Written %d.", size, errno, wr_sz);
  99                 return ERROR_FAIL;
 100         }
 101         return ERROR_OK;
 102 }
 103
 104 static int esp32_apptrace_file_dest_cleanup(void *priv)
 105 {
 106         struct esp32_apptrace_dest_file_data *dest_data = (struct esp32_apptrace_dest_file_data *)priv;
 107
 108         if (dest_data->fout > 0)
 109                 close(dest_data->fout);
 110         free(dest_data);
 111         return ERROR_OK;
 112 }
 113
 114 static int esp32_apptrace_file_dest_init(struct esp32_apptrace_dest *dest, const char *dest_name)
 115 {
 116         struct esp32_apptrace_dest_file_data *dest_data = calloc(1, sizeof(*dest_data));
 117         if (!dest_data) {
 118                 LOG_ERROR("Failed to alloc mem for file dest!");
 119                 return ERROR_FAIL;
 120         }
 121
 122         LOG_INFO("Open file %s", dest_name);
 123         dest_data->fout = open(dest_name, O_WRONLY | O_CREAT | O_TRUNC | O_BINARY, 0666);
 124         if (dest_data->fout <= 0) {
 125                 LOG_ERROR("Failed to open file %s", dest_name);
 126                 free(dest_data);
 127                 return ERROR_FAIL;
 128         }
 129
 130         dest->priv = dest_data;
 131         dest->write = esp32_apptrace_file_dest_write;
 132         dest->clean = esp32_apptrace_file_dest_cleanup;
 133         dest->log_progress = true;
 134
 135         return ERROR_OK;
 136 }
 137
 138 static int esp32_apptrace_console_dest_write(void *priv, uint8_t *data, int size)
 139 {
 140         LOG_USER_N("%.*s", size, data);
 141         return ERROR_OK;
 142 }
 143
 144 static int esp32_apptrace_console_dest_cleanup(void *priv)
 145 {
 146         return ERROR_OK;
 147 }
 148
 149 static int esp32_apptrace_console_dest_init(struct esp32_apptrace_dest *dest, const char *dest_name)
 150 {
 151         dest->priv = NULL;
 152         dest->write = esp32_apptrace_console_dest_write;
 153         dest->clean = esp32_apptrace_console_dest_cleanup;
 154         dest->log_progress = false;
 155
 156         return ERROR_OK;
 157 }
 158
 159 static int esp32_apptrace_tcp_dest_write(void *priv, uint8_t *data, int size)
 160 {
 161         struct esp32_apptrace_dest_tcp_data *dest_data = (struct esp32_apptrace_dest_tcp_data *)priv;
 162         int wr_sz = write_socket(dest_data->sockfd, data, size);
 163         if (wr_sz != size) {
 164                 LOG_ERROR("Failed to write %u bytes to out socket (%d)! Written %d.", size, errno, wr_sz);
 165                 return ERROR_FAIL;
 166         }
 167         return ERROR_OK;
 168 }
 169
 170 static int esp32_apptrace_tcp_dest_cleanup(void *priv)
 171 {
 172         struct esp32_apptrace_dest_tcp_data *dest_data = (struct esp32_apptrace_dest_tcp_data *)priv;
 173
 174         if (dest_data->sockfd > 0)
 175                 close_socket(dest_data->sockfd);
 176         free(dest_data);
 177         return ERROR_OK;
 178 }
 179
 180 static int esp32_apptrace_tcp_dest_init(struct esp32_apptrace_dest *dest, const char *dest_name)
 181 {
 182         const char *port_sep = strchr(dest_name, ':');
 183         /* separator not found, or was the first or the last character */
 184         if (!port_sep || port_sep == dest_name || port_sep == dest_name + strlen(dest_name) - 1) {
 185                 LOG_ERROR("apptrace: Invalid connection URI, format should be tcp://host:port");
 186                 return ERROR_COMMAND_ARGUMENT_INVALID;
 187         }
 188         size_t hostname_len = port_sep - dest_name;
 189
 190         char hostname[64] = { 0 };
 191         if (hostname_len >= sizeof(hostname)) {
 192                 LOG_ERROR("apptrace: Hostname too long");
 193                 return ERROR_COMMAND_ARGUMENT_INVALID;
 194         }
 195         memcpy(hostname, dest_name, hostname_len);
 196
 197         const char *port_str = port_sep + 1;
 198         struct addrinfo *ai;
 199         int flags = 0;
 200 #ifdef AI_NUMERICSERV
 201         flags |= AI_NUMERICSERV;
 202 #endif  /* AI_NUMERICSERV */
 203         struct addrinfo hint = {
 204                 .ai_family = AF_UNSPEC,
 205                 .ai_socktype = SOCK_STREAM,
 206                 .ai_protocol = 0,
 207                 .ai_flags = flags
 208         };
 209         int res = getaddrinfo(hostname, port_str, &hint, &ai);
 210         if (res != 0) {
 211                 LOG_ERROR("apptrace: Failed to resolve host name: %s", hostname);
 212                 return ERROR_FAIL;
 213         }
 214         int sockfd = -1;
 215         for (struct addrinfo *ai_it = ai; ai_it; ai_it = ai_it->ai_next) {
 216                 sockfd = socket(ai_it->ai_family, ai_it->ai_socktype, ai_it->ai_protocol);
 217                 if (sockfd < 0) {
 218                         LOG_DEBUG("apptrace: Failed to create socket (%d, %d, %d) (%s)",
 219                                 ai_it->ai_family,
 220                                 ai_it->ai_socktype,
 221                                 ai_it->ai_protocol,
 222                                 strerror(errno));
 223                         continue;
 224                 }
 225
 226                 char cur_hostname[NI_MAXHOST];
 227                 char cur_portname[NI_MAXSERV];
 228                 res =
 229                         getnameinfo(ai_it->ai_addr, ai_it->ai_addrlen, cur_hostname,
 230                         sizeof(cur_hostname),
 231                         cur_portname, sizeof(cur_portname),
 232                         NI_NUMERICHOST | NI_NUMERICSERV);
 233                 if (res != 0)
 234                         continue;
 235
 236                 LOG_INFO("apptrace: Trying to connect to %s:%s", cur_hostname, cur_portname);
 237                 if (connect(sockfd, ai_it->ai_addr, ai_it->ai_addrlen) < 0) {
 238                         close_socket(sockfd);
 239                         sockfd = -1;
 240                         LOG_WARNING("apptrace: Connection failed (%s)", strerror(errno));
 241                         continue;
 242                 }
 243                 break;
 244         }
 245         freeaddrinfo(ai);
 246         if (sockfd < 0) {
 247                 LOG_ERROR("apptrace: Could not connect to %s:%s", hostname, port_str);
 248                 return ERROR_FAIL;
 249         }
 250         LOG_INFO("apptrace: Connected!");
 251
 252         struct esp32_apptrace_dest_tcp_data *dest_data = calloc(1, sizeof(struct esp32_apptrace_dest_tcp_data));
 253         if (!dest_data) {
 254                 LOG_ERROR("apptrace: Failed to alloc mem for tcp dest!");
 255                 close_socket(sockfd);
 256                 return ERROR_FAIL;
 257         }
 258
 259         dest_data->sockfd = sockfd;
 260         dest->priv = dest_data;
 261         dest->write = esp32_apptrace_tcp_dest_write;
 262         dest->clean = esp32_apptrace_tcp_dest_cleanup;
 263         dest->log_progress = true;
 264
 265         return ERROR_OK;
 266 }
 267
 268 int esp32_apptrace_dest_init(struct esp32_apptrace_dest dest[], const char *dest_paths[], unsigned int max_dests)
 269 {
 270         int res;
 271         unsigned int i;
 272
 273         for (i = 0; i < max_dests; i++) {
 274                 if (strncmp(dest_paths[i], "file://", 7) == 0)
 275                         res = esp32_apptrace_file_dest_init(&dest[i], &dest_paths[i][7]);
 276                 else if (strncmp(dest_paths[i], "con:", 4) == 0)
 277                         res = esp32_apptrace_console_dest_init(&dest[i], NULL);
 278                 else if (strncmp(dest_paths[i], "tcp://", 6) == 0)
 279                         res = esp32_apptrace_tcp_dest_init(&dest[i], &dest_paths[i][6]);
 280                 else
 281                         break;
 282
 283                 if (res != ERROR_OK) {
 284                         LOG_ERROR("apptrace: Failed to init trace data destination '%s'!", dest_paths[i]);
 285                         return 0;
 286                 }
 287         }
 288
 289         return i;
 290 }
 291
 292 int esp32_apptrace_dest_cleanup(struct esp32_apptrace_dest dest[], unsigned int max_dests)
 293 {
 294         for (unsigned int i = 0; i < max_dests; i++) {
 295                 if (dest[i].clean && dest[i].priv) {
 296                         int res = dest[i].clean(dest[i].priv);
 297                         dest[i].priv = NULL;
 298                         return res;
 299                 }
 300         }
 301         return ERROR_OK;
 302 }
 303
 304 /*********************************************************************
 305 *                 Trace data blocks management API
 306 **********************************************************************/
 307 static void esp32_apptrace_blocks_pool_cleanup(struct esp32_apptrace_cmd_ctx *ctx)
 308 {
 309         struct esp32_apptrace_block *cur;
 310         struct list_head *head = &ctx->free_trace_blocks;
 311         struct list_head *tmp, *pos;
 312
 313         list_for_each_safe(pos, tmp, head) {
 314                 cur = list_entry(pos, struct esp32_apptrace_block, node);
 315                 if (cur) {
 316                         list_del(&cur->node);
 317                         free(cur->data);
 318                         free(cur);
 319                 }
 320         }
 321
 322         head = &ctx->ready_trace_blocks;
 323
 324         list_for_each_safe(pos, tmp, head) {
 325                 cur = list_entry(pos, struct esp32_apptrace_block, node);
 326                 if (cur) {
 327                         list_del(&cur->node);
 328                         free(cur->data);
 329                         free(cur);
 330                 }
 331         }
 332 }
 333
 334 struct esp32_apptrace_block *esp32_apptrace_free_block_get(struct esp32_apptrace_cmd_ctx *ctx)
 335 {
 336         struct esp32_apptrace_block *block = NULL;
 337
 338         if (!list_empty(&ctx->free_trace_blocks)) {
 339                 /*get first */
 340                 block = list_first_entry(&ctx->free_trace_blocks, struct esp32_apptrace_block, node);
 341                 list_del(&block->node);
 342         }
 343
 344         return block;
 345 }
 346
 347 static int esp32_apptrace_ready_block_put(struct esp32_apptrace_cmd_ctx *ctx, struct esp32_apptrace_block *block)
 348 {
 349         LOG_DEBUG("esp32_apptrace_ready_block_put");
 350         /* add to ready blocks list */
 351         INIT_LIST_HEAD(&block->node);
 352         list_add(&block->node, &ctx->ready_trace_blocks);
 353
 354         return ERROR_OK;
 355 }
 356
 357 static struct esp32_apptrace_block *esp32_apptrace_ready_block_get(struct esp32_apptrace_cmd_ctx *ctx)
 358 {
 359         if (list_empty(&ctx->ready_trace_blocks))
 360                 return NULL;
 361
 362         struct esp32_apptrace_block *block =
 363                 list_last_entry(&ctx->ready_trace_blocks, struct esp32_apptrace_block, node);
 364
 365         /* remove it from ready list */
 366         list_del(&block->node);
 367
 368         return block;
 369 }
 370
 371 static int esp32_apptrace_block_free(struct esp32_apptrace_cmd_ctx *ctx, struct esp32_apptrace_block *block)
 372 {
 373         /* add to free blocks list */
 374         INIT_LIST_HEAD(&block->node);
 375         list_add(&block->node, &ctx->free_trace_blocks);
 376
 377         return ERROR_OK;
 378 }
 379
 380 static int esp32_apptrace_wait_tracing_finished(struct esp32_apptrace_cmd_ctx *ctx)
 381 {
 382         int64_t timeout = timeval_ms() + (LOG_LEVEL_IS(LOG_LVL_DEBUG) ? 70000 : 5000);
 383         while (!list_empty(&ctx->ready_trace_blocks)) {
 384                 alive_sleep(100);
 385                 if (timeval_ms() >= timeout) {
 386                         LOG_ERROR("Failed to wait for pended trace blocks!");
 387                         return ERROR_FAIL;
 388                 }
 389         }
 390         /* signal timer callback to stop */
 391         ctx->running = 0;
 392         target_unregister_timer_callback(esp32_apptrace_data_processor, ctx);
 393         return ERROR_OK;
 394 }
 395
 396 /*********************************************************************
 397 *                          Trace commands
 398 **********************************************************************/
 399
 400 int esp32_apptrace_cmd_ctx_init(struct esp32_apptrace_cmd_ctx *cmd_ctx, struct command_invocation *cmd, int mode)
 401 {
 402         struct target *target = get_current_target(CMD_CTX);
 403
 404         memset(cmd_ctx, 0, sizeof(struct esp32_apptrace_cmd_ctx));
 405         cmd_ctx->target = target;
 406         cmd_ctx->mode = mode;
 407         cmd_ctx->target_state = target->state;
 408         cmd_ctx->cmd = cmd;
 409
 410         if (target->smp) {
 411                 struct target_list *head;
 412                 struct target *curr;
 413                 unsigned int i = 0;
 414                 cmd_ctx->cores_num = 0;
 415                 foreach_smp_target(head, target->smp_targets) {
 416                         curr = head->target;
 417                         if (i == ESP32_APPTRACE_MAX_CORES_NUM) {
 418                                 command_print(cmd, "Too many cores configured! Max %d cores are supported.",
 419                                         ESP32_APPTRACE_MAX_CORES_NUM);
 420                                 return ERROR_FAIL;
 421                         }
 422                         if (!target_was_examined(curr))
 423                                 continue;
 424                         cmd_ctx->cores_num++;
 425                         cmd_ctx->cpus[i++] = curr;
 426                 }
 427         } else {
 428                 cmd_ctx->cores_num = 1;
 429                 cmd_ctx->cpus[0] = target;
 430         }
 431         /* some relies on ESP32_APPTRACE_MAX_CORES_NUM
 432          * TODO: remove that dependency */
 433         assert(cmd_ctx->cores_num <= ESP32_APPTRACE_MAX_CORES_NUM && "Too many cores number!");
 434
 435         struct xtensa *xtensa = target->arch_info;
 436         if (xtensa->common_magic == XTENSA_COMMON_MAGIC) {
 437                 cmd_ctx->hw = target_to_esp_xtensa(target)->apptrace.hw;
 438         } else { /* TODO: riscv is not supported yet */
 439                 command_print(cmd, "Unsupported target arch 0x%X", xtensa->common_magic);
 440                 return ERROR_FAIL;
 441         }
 442
 443         cmd_ctx->max_trace_block_sz = cmd_ctx->hw->max_block_size_get(cmd_ctx->cpus[0]);
 444         if (cmd_ctx->max_trace_block_sz == 0) {
 445                 command_print(cmd, "Failed to get max trace block size!");
 446                 return ERROR_FAIL;
 447         }
 448         LOG_INFO("Total trace memory: %" PRIu32 " bytes", cmd_ctx->max_trace_block_sz);
 449
 450         INIT_LIST_HEAD(&cmd_ctx->ready_trace_blocks);
 451         INIT_LIST_HEAD(&cmd_ctx->free_trace_blocks);
 452         for (unsigned int i = 0; i < ESP_APPTRACE_BLOCKS_POOL_SZ; i++) {
 453                 struct esp32_apptrace_block *block = calloc(1, sizeof(struct esp32_apptrace_block));
 454                 if (!block) {
 455                         command_print(cmd, "Failed to alloc trace buffer entry!");
 456                         esp32_apptrace_blocks_pool_cleanup(cmd_ctx);
 457                         return ERROR_FAIL;
 458                 }
 459                 block->data = malloc(cmd_ctx->max_trace_block_sz);
 460                 if (!block->data) {
 461                         free(block);
 462                         command_print(cmd, "Failed to alloc trace buffer %" PRIu32 " bytes!", cmd_ctx->max_trace_block_sz);
 463                         esp32_apptrace_blocks_pool_cleanup(cmd_ctx);
 464                         return ERROR_FAIL;
 465                 }
 466                 INIT_LIST_HEAD(&block->node);
 467                 list_add(&block->node, &cmd_ctx->free_trace_blocks);
 468         }
 469
 470         cmd_ctx->running = 1;
 471         if (cmd_ctx->mode != ESP_APPTRACE_CMD_MODE_SYNC) {
 472                 int res = target_register_timer_callback(esp32_apptrace_data_processor,
 473                         0,
 474                         TARGET_TIMER_TYPE_PERIODIC,
 475                         cmd_ctx);
 476                 if (res != ERROR_OK) {
 477                         command_print(cmd, "Failed to start trace data timer callback (%d)!", res);
 478                         esp32_apptrace_blocks_pool_cleanup(cmd_ctx);
 479                         return ERROR_FAIL;
 480                 }
 481         }
 482
 483         if (s_time_stats_enable) {
 484                 cmd_ctx->stats.min_blk_read_time = 1000000.0;
 485                 cmd_ctx->stats.min_blk_proc_time = 1000000.0;
 486         }
 487         if (duration_start(&cmd_ctx->idle_time) != 0) {
 488                 command_print(cmd, "Failed to start idle time measurement!");
 489                 esp32_apptrace_cmd_ctx_cleanup(cmd_ctx);
 490                 return ERROR_FAIL;
 491         }
 492
 493         return ERROR_OK;
 494 }
 495
 496 int esp32_apptrace_cmd_ctx_cleanup(struct esp32_apptrace_cmd_ctx *cmd_ctx)
 497 {
 498         esp32_apptrace_blocks_pool_cleanup(cmd_ctx);
 499         return ERROR_OK;
 500 }
 501
 502 #define ESP32_APPTRACE_CMD_NUM_ARG_CHECK(_cmd_, _arg_, _start_, _end_)     \
 503         do { \
 504                 if ((_arg_) == 0 && (_start_) == (_end_)) { \
 505                         command_print(_cmd_, "Invalid '" # _arg_ "' arg!"); \
 506                         return; \
 507                 } \
 508         } while (0)
 509
 510 void esp32_apptrace_cmd_args_parse(struct esp32_apptrace_cmd_ctx *cmd_ctx,
 511         struct esp32_apptrace_cmd_data *cmd_data,
 512         const char **argv,
 513         int argc)
 514 {
 515         char *end;
 516
 517         cmd_data->poll_period = strtoul(argv[0], &end, 10);
 518         ESP32_APPTRACE_CMD_NUM_ARG_CHECK(cmd_ctx->cmd, cmd_data->poll_period, argv[0], end);
 519         if (argc > 1) {
 520                 cmd_data->max_len = strtoul(argv[1], &end, 10);
 521                 ESP32_APPTRACE_CMD_NUM_ARG_CHECK(cmd_ctx->cmd, cmd_data->max_len, argv[1], end);
 522                 if (argc > 2) {
 523                         int32_t tmo = strtol(argv[2], &end, 10);
 524                         ESP32_APPTRACE_CMD_NUM_ARG_CHECK(cmd_ctx->cmd, tmo, argv[2], end);
 525                         cmd_ctx->stop_tmo = 1.0 * tmo;
 526                         if (argc > 3) {
 527                                 cmd_data->wait4halt = strtoul(argv[3], &end, 10);
 528                                 ESP32_APPTRACE_CMD_NUM_ARG_CHECK(cmd_ctx->cmd, cmd_data->wait4halt, argv[3], end);
 529                                 if (argc > 4) {
 530                                         cmd_data->skip_len = strtoul(argv[4], &end, 10);
 531                                         ESP32_APPTRACE_CMD_NUM_ARG_CHECK(cmd_ctx->cmd, cmd_data->skip_len, argv[4], end);
 532                                 }
 533                         }
 534                 }
 535         }
 536 }
 537
 538 static int esp32_apptrace_core_id_get(struct target *target, uint8_t *hdr_buf)
 539 {
 540         return ESP32_APPTRACE_USER_BLOCK_CORE(target_buffer_get_u16(target, hdr_buf + APPTRACE_BLOCK_SIZE_OFFSET));
 541 }
 542
 543 static uint32_t esp32_apptrace_usr_block_len_get(struct target *target, uint8_t *hdr_buf, uint32_t *wr_len)
 544 {
 545         *wr_len = ESP32_APPTRACE_USER_BLOCK_LEN(target_buffer_get_u16(target, hdr_buf + APPTRACE_WR_SIZE_OFFSET));
 546         return ESP32_APPTRACE_USER_BLOCK_LEN(target_buffer_get_u16(target, hdr_buf + APPTRACE_BLOCK_SIZE_OFFSET));
 547 }
 548
 549 static int esp32_apptrace_cmd_init(struct esp32_apptrace_cmd_ctx *cmd_ctx,
 550         struct command_invocation *cmd,
 551         int mode,
 552         const char **argv,
 553         int argc)
 554 {
 555         struct esp32_apptrace_cmd_data *cmd_data;
 556
 557         if (argc < 1) {
 558                 command_print(cmd, "Not enough args! Need trace data destination!");
 559                 return ERROR_FAIL;
 560         }
 561
 562         int res = esp32_apptrace_cmd_ctx_init(cmd_ctx, cmd, mode);
 563         if (res != ERROR_OK)
 564                 return res;
 565
 566         cmd_data = calloc(1, sizeof(*cmd_data));
 567         assert(cmd_data && "No memory for command data!");
 568         cmd_ctx->cmd_priv = cmd_data;
 569
 570         /*outfile1 [poll_period [trace_size [stop_tmo [wait4halt [skip_size]]]]] */
 571         res = esp32_apptrace_dest_init(&cmd_data->data_dest, argv, 1);
 572         if (res != 1) { /* only one destination needs to be initialized */
 573                 command_print(cmd, "Wrong args! Needs a trace data destination!");
 574                 free(cmd_data);
 575                 goto on_error;
 576         }
 577         cmd_ctx->stop_tmo = -1.0;       /* infinite */
 578         cmd_data->max_len = UINT32_MAX;
 579         cmd_data->poll_period = 0 /*ms*/;
 580         if (argc > 1)
 581                 /* parse remaining args */
 582                 esp32_apptrace_cmd_args_parse(cmd_ctx, cmd_data, &argv[1], argc - 1);
 583
 584         LOG_USER("App trace params: from %d cores, size %" PRId32 " bytes, stop_tmo %g s, poll period %" PRId32
 585                 " ms, wait_rst %d, skip %" PRId32 " bytes", cmd_ctx->cores_num,
 586                 cmd_data->max_len,
 587                 cmd_ctx->stop_tmo,
 588                 cmd_data->poll_period,
 589                 cmd_data->wait4halt,
 590                 cmd_data->skip_len);
 591
 592         cmd_ctx->trace_format.hdr_sz = ESP32_APPTRACE_USER_BLOCK_HDR_SZ;
 593         cmd_ctx->trace_format.core_id_get = esp32_apptrace_core_id_get;
 594         cmd_ctx->trace_format.usr_block_len_get = esp32_apptrace_usr_block_len_get;
 595         return ERROR_OK;
 596 on_error:
 597         command_print(cmd, "Not enough args! Need %d trace data destinations!", cmd_ctx->cores_num);
 598         cmd_ctx->running = 0;
 599         esp32_apptrace_cmd_ctx_cleanup(cmd_ctx);
 600         return res;
 601 }
 602
 603 static int esp32_apptrace_cmd_cleanup(struct esp32_apptrace_cmd_ctx *cmd_ctx)
 604 {
 605         struct esp32_apptrace_cmd_data *cmd_data = cmd_ctx->cmd_priv;
 606
 607         esp32_apptrace_dest_cleanup(&cmd_data->data_dest, 1);
 608         free(cmd_data);
 609         cmd_ctx->cmd_priv = NULL;
 610         esp32_apptrace_cmd_ctx_cleanup(cmd_ctx);
 611         return ERROR_OK;
 612 }
 613
 614 static void esp32_apptrace_print_stats(struct esp32_apptrace_cmd_ctx *ctx)
 615 {
 616         struct esp32_apptrace_cmd_data *cmd_data = ctx->cmd_priv;
 617         uint32_t trace_sz = 0;
 618
 619         if (cmd_data)
 620                 trace_sz = ctx->tot_len > cmd_data->skip_len ? ctx->tot_len - cmd_data->skip_len : 0;
 621         LOG_USER("Tracing is %s. Size is %" PRId32 " of %" PRId32 " @ %f (%f) KiB/s",
 622                 !ctx->running ? "STOPPED" : "RUNNING",
 623                 trace_sz,
 624                 cmd_data ? cmd_data->max_len : 0,
 625                 duration_kbps(&ctx->read_time, ctx->tot_len),
 626                 duration_kbps(&ctx->read_time, ctx->raw_tot_len));
 627         LOG_USER("Data: blocks incomplete %" PRId32 ", lost bytes: %" PRId32,
 628                 ctx->stats.incompl_blocks,
 629                 ctx->stats.lost_bytes);
 630         if (s_time_stats_enable) {
 631                 LOG_USER("Block read time [%f..%f] ms",
 632                         1000 * ctx->stats.min_blk_read_time,
 633                         1000 * ctx->stats.max_blk_read_time);
 634                 LOG_USER("Block proc time [%f..%f] ms",
 635                         1000 * ctx->stats.min_blk_proc_time,
 636                         1000 * ctx->stats.max_blk_proc_time);
 637         }
 638 }
 639
 640 static int esp32_apptrace_wait4halt(struct esp32_apptrace_cmd_ctx *ctx, struct target *target)
 641 {
 642         LOG_USER("Wait for halt...");
 643         while (!openocd_is_shutdown_pending()) {
 644                 int res = target_poll(target);
 645                 if (res != ERROR_OK)
 646                         return res;
 647                 if (target->state == TARGET_HALTED) {
 648                         LOG_USER("%s: HALTED", target->cmd_name);
 649                         break;
 650                 }
 651                 alive_sleep(500);
 652         }
 653         return ERROR_OK;
 654 }
 655
 656 int esp32_apptrace_safe_halt_targets(struct esp32_apptrace_cmd_ctx *ctx,
 657         struct esp32_apptrace_target_state *targets)
 658 {
 659         int res = ERROR_OK;
 660
 661         memset(targets, 0, ctx->cores_num * sizeof(struct esp32_apptrace_target_state));
 662         /* halt all CPUs */
 663         LOG_DEBUG("Halt all targets!");
 664         for (unsigned int k = 0; k < ctx->cores_num; k++) {
 665                 if (!target_was_examined(ctx->cpus[k]))
 666                         continue;
 667                 if (ctx->cpus[k]->state == TARGET_HALTED)
 668                         continue;
 669                 res = target_halt(ctx->cpus[k]);
 670                 if (res != ERROR_OK) {
 671                         LOG_ERROR("Failed to halt target (%d)!", res);
 672                         return res;
 673                 }
 674                 res = target_wait_state(ctx->cpus[k], TARGET_HALTED, ESP32_APPTRACE_TGT_STATE_TMO);
 675                 if (res != ERROR_OK) {
 676                         LOG_ERROR("Failed to wait halt target %s / %d (%d)!",
 677                                 target_name(ctx->cpus[k]),
 678                                 ctx->cpus[k]->state,
 679                                 res);
 680                         return res;
 681                 }
 682         }
 683         /* read current block statuses from CPUs */
 684         LOG_DEBUG("Read current block statuses");
 685         for (unsigned int k = 0; k < ctx->cores_num; k++) {
 686                 uint32_t stat;
 687                 res = ctx->hw->status_reg_read(ctx->cpus[k], &stat);
 688                 if (res != ERROR_OK) {
 689                         LOG_ERROR("Failed to read trace status (%d)!", res);
 690                         return res;
 691                 }
 692                 /* check if some CPU stopped inside tracing regs update critical section */
 693                 if (stat) {
 694                         if (ctx->hw->leave_trace_crit_section_start) {
 695                                 res = ctx->hw->leave_trace_crit_section_start(ctx->cpus[k]);
 696                                 if (res != ERROR_OK)
 697                                         return res;
 698                         }
 699                         uint32_t bp_addr = stat;
 700                         res = breakpoint_add(ctx->cpus[k], bp_addr, 1, BKPT_HARD);
 701                         if (res != ERROR_OK) {
 702                                 LOG_ERROR("Failed to set breakpoint (%d)!", res);
 703                                 return res;
 704                         }
 705                         while (stat) {
 706                                 /* allow this CPU to leave ERI write critical section */
 707                                 res = target_resume(ctx->cpus[k], 1, 0, 1, 0);
 708                                 if (res != ERROR_OK) {
 709                                         LOG_ERROR("Failed to resume target (%d)!", res);
 710                                         breakpoint_remove(ctx->cpus[k], bp_addr);
 711                                         return res;
 712                                 }
 713                                 /* wait for CPU to be halted on BP */
 714                                 enum target_debug_reason debug_reason = DBG_REASON_UNDEFINED;
 715                                 while (debug_reason != DBG_REASON_BREAKPOINT) {
 716                                         res = target_wait_state(ctx->cpus[k], TARGET_HALTED,
 717                                                 ESP32_APPTRACE_TGT_STATE_TMO);
 718                                         if (res != ERROR_OK) {
 719                                                 LOG_ERROR("Failed to wait halt on bp (%d)!", res);
 720                                                 breakpoint_remove(ctx->cpus[k], bp_addr);
 721                                                 return res;
 722                                         }
 723                                         debug_reason = ctx->cpus[k]->debug_reason;
 724                                 }
 725                                 res = ctx->hw->status_reg_read(ctx->cpus[k], &stat);
 726                                 if (res != ERROR_OK) {
 727                                         LOG_ERROR("Failed to read trace status (%d)!", res);
 728                                         breakpoint_remove(ctx->cpus[k], bp_addr);
 729                                         return res;
 730                                 }
 731                         }
 732                         breakpoint_remove(ctx->cpus[k], bp_addr);
 733                         if (ctx->hw->leave_trace_crit_section_stop) {
 734                                 res = ctx->hw->leave_trace_crit_section_stop(ctx->cpus[k]);
 735                                 if (res != ERROR_OK)
 736                                         return res;
 737                         }
 738                 }
 739                 res = ctx->hw->data_len_read(ctx->cpus[k], &targets[k].block_id, &targets[k].data_len);
 740                 if (res != ERROR_OK) {
 741                         LOG_ERROR("Failed to read trace status (%d)!", res);
 742                         return res;
 743                 }
 744         }
 745
 746         return ERROR_OK;
 747 }
 748
 749 static int esp32_apptrace_connect_targets(struct esp32_apptrace_cmd_ctx *ctx,
 750         bool conn,
 751         bool resume_target)
 752 {
 753         struct esp32_apptrace_target_state target_to_connect[ESP32_APPTRACE_MAX_CORES_NUM];
 754
 755         if (conn)
 756                 LOG_USER("Connect targets...");
 757         else
 758                 LOG_USER("Disconnect targets...");
 759
 760         int res = esp32_apptrace_safe_halt_targets(ctx, target_to_connect);
 761         if (res != ERROR_OK) {
 762                 command_print(ctx->cmd, "Failed to halt targets (%d)!", res);
 763                 return res;
 764         }
 765         if (ctx->cores_num > 1) {
 766                 /* set block ids to the highest value */
 767                 uint32_t max_id = 0;
 768                 for (unsigned int k = 0; k < ctx->cores_num; k++) {
 769                         if (target_to_connect[k].block_id > max_id)
 770                                 max_id = target_to_connect[k].block_id;
 771                 }
 772                 for (unsigned int k = 0; k < ctx->cores_num; k++)
 773                         target_to_connect[k].block_id = max_id;
 774         }
 775         for (unsigned int k = 0; k < ctx->cores_num; k++) {
 776                 /* update host connected status */
 777                 res = ctx->hw->ctrl_reg_write(ctx->cpus[k],
 778                         target_to_connect[k].block_id,
 779                         0 /*ack target data*/,
 780                         conn,
 781                         false /*no host data*/);
 782                 if (res != ERROR_OK) {
 783                         command_print(ctx->cmd, "Failed to read trace status (%d)!", res);
 784                         return res;
 785                 }
 786         }
 787         if (resume_target) {
 788                 LOG_DEBUG("Resume targets");
 789                 bool smp_resumed = false;
 790                 for (unsigned int k = 0; k < ctx->cores_num; k++) {
 791                         if (smp_resumed && ctx->cpus[k]->smp) {
 792                                 /* in SMP mode we need to call target_resume for one core only */
 793                                 continue;
 794                         }
 795                         res = target_resume(ctx->cpus[k], 1, 0, 1, 0);
 796                         if (res != ERROR_OK) {
 797                                 command_print(ctx->cmd, "Failed to resume target (%d)!", res);
 798                                 return res;
 799                         }
 800                         if (ctx->cpus[k]->smp)
 801                                 smp_resumed = true;
 802                 }
 803         }
 804         if (conn)
 805                 LOG_INFO("Targets connected.");
 806         else
 807                 LOG_INFO("Targets disconnected.");
 808         return ERROR_OK;
 809 }
 810
 811 int esp_apptrace_usr_block_write(const struct esp32_apptrace_hw *hw, struct target *target,
 812         uint32_t block_id,
 813         const uint8_t *data,
 814         uint32_t size)
 815 {
 816         struct esp_apptrace_host2target_hdr hdr = { .block_sz = size };
 817         uint32_t buf_sz[2] = { sizeof(hdr), size };
 818         const uint8_t *bufs[2] = { (const uint8_t *)&hdr, data };
 819
 820         if (size > hw->usr_block_max_size_get(target)) {
 821                 LOG_ERROR("Too large user block %" PRId32, size);
 822                 return ERROR_FAIL;
 823         }
 824
 825         return hw->buffs_write(target,
 826                 ARRAY_SIZE(buf_sz),
 827                 buf_sz,
 828                 bufs,
 829                 block_id,
 830                 true /*ack target data*/,
 831                 true /*host data*/);
 832 }
 833
 834 static uint32_t esp32_apptrace_usr_block_check(struct esp32_apptrace_cmd_ctx *ctx, uint8_t *hdr_buf)
 835 {
 836         uint32_t wr_len = 0;
 837         uint32_t usr_len = ctx->trace_format.usr_block_len_get(ctx->target, hdr_buf, &wr_len);
 838         if (usr_len != wr_len) {
 839                 LOG_ERROR("Incomplete block sz %" PRId32 ", wr %" PRId32, usr_len, wr_len);
 840                 ctx->stats.incompl_blocks++;
 841                 ctx->stats.lost_bytes += usr_len - wr_len;
 842         }
 843         return usr_len;
 844 }
 845
 846 int esp32_apptrace_get_data_info(struct esp32_apptrace_cmd_ctx *ctx,
 847         struct esp32_apptrace_target_state *target_state,
 848         uint32_t *fired_target_num)
 849 {
 850         if (fired_target_num)
 851                 *fired_target_num = UINT32_MAX;
 852
 853         for (unsigned int i = 0; i < ctx->cores_num; i++) {
 854                 int res = ctx->hw->data_len_read(ctx->cpus[i], &target_state[i].block_id, &target_state[i].data_len);
 855                 if (res != ERROR_OK) {
 856                         LOG_ERROR("Failed to read data len on (%s)!", target_name(ctx->cpus[i]));
 857                         return res;
 858                 }
 859                 if (target_state[i].data_len) {
 860                         LOG_TARGET_DEBUG(ctx->cpus[i], "Block %" PRId32 ", len %" PRId32 " bytes on fired",
 861                                 target_state[i].block_id, target_state[i].data_len);
 862                         if (fired_target_num)
 863                                 *fired_target_num = i;
 864                         break;
 865                 }
 866         }
 867         return ERROR_OK;
 868 }
 869
 870 static int esp32_apptrace_process_data(struct esp32_apptrace_cmd_ctx *ctx,
 871         unsigned int core_id,
 872         uint8_t *data,
 873         uint32_t data_len)
 874 {
 875         struct esp32_apptrace_cmd_data *cmd_data = ctx->cmd_priv;
 876
 877         LOG_DEBUG("Got block %" PRId32 " bytes [%x %x...%x %x]", data_len, data[12], data[13],
 878                 data[data_len - 2], data[data_len - 1]);
 879         if (ctx->tot_len + data_len > cmd_data->skip_len) {
 880                 uint32_t wr_idx = 0, wr_chunk_len = data_len;
 881                 if (ctx->tot_len < cmd_data->skip_len) {
 882                         wr_chunk_len = (ctx->tot_len + wr_chunk_len) - cmd_data->skip_len;
 883                         wr_idx = cmd_data->skip_len - ctx->tot_len;
 884                 }
 885                 if (ctx->tot_len + wr_chunk_len > cmd_data->max_len)
 886                         wr_chunk_len -= (ctx->tot_len + wr_chunk_len - cmd_data->skip_len) - cmd_data->max_len;
 887                 if (wr_chunk_len > 0) {
 888                         int res = cmd_data->data_dest.write(cmd_data->data_dest.priv, data + wr_idx, wr_chunk_len);
 889                         if (res != ERROR_OK) {
 890                                 LOG_ERROR("Failed to write %" PRId32 " bytes to dest 0!", data_len);
 891                                 return res;
 892                         }
 893                 }
 894                 ctx->tot_len += wr_chunk_len;
 895         } else {
 896                 ctx->tot_len += data_len;
 897         }
 898
 899         if (cmd_data->data_dest.log_progress)
 900                 LOG_USER("%" PRId32 " ", ctx->tot_len);
 901         /* check for stop condition */
 902         if (ctx->tot_len > cmd_data->skip_len && (ctx->tot_len - cmd_data->skip_len >= cmd_data->max_len)) {
 903                 ctx->running = 0;
 904                 if (duration_measure(&ctx->read_time) != 0) {
 905                         LOG_ERROR("Failed to stop trace read time measure!");
 906                         return ERROR_FAIL;
 907                 }
 908         }
 909         return ERROR_OK;
 910 }
 911
 912 static int esp32_apptrace_handle_trace_block(struct esp32_apptrace_cmd_ctx *ctx,
 913         struct esp32_apptrace_block *block)
 914 {
 915         uint32_t processed = 0;
 916         uint32_t hdr_sz = ctx->trace_format.hdr_sz;
 917
 918         LOG_DEBUG("Got block %" PRId32 " bytes", block->data_len);
 919         /* process user blocks one by one */
 920         while (processed < block->data_len) {
 921                 LOG_DEBUG("Process usr block %" PRId32 "/%" PRId32, processed, block->data_len);
 922                 /* process user block */
 923                 uint32_t usr_len = esp32_apptrace_usr_block_check(ctx, block->data + processed);
 924                 int core_id = ctx->trace_format.core_id_get(ctx->target, block->data + processed);
 925                 /* process user data */
 926                 int res = ctx->process_data(ctx, core_id, block->data + processed + hdr_sz, usr_len);
 927                 if (res != ERROR_OK) {
 928                         LOG_ERROR("Failed to process %" PRId32 " bytes!", usr_len);
 929                         return res;
 930                 }
 931                 processed += usr_len + hdr_sz;
 932         }
 933         return ERROR_OK;
 934 }
 935
 936 static int esp32_apptrace_data_processor(void *priv)
 937 {
 938         struct esp32_apptrace_cmd_ctx *ctx = (struct esp32_apptrace_cmd_ctx *)priv;
 939
 940         if (!ctx->running)
 941                 return ERROR_OK;
 942
 943         struct esp32_apptrace_block *block = esp32_apptrace_ready_block_get(ctx);
 944         if (!block)
 945                 return ERROR_OK;
 946
 947         int res = esp32_apptrace_handle_trace_block(ctx, block);
 948         if (res != ERROR_OK) {
 949                 ctx->running = 0;
 950                 LOG_ERROR("Failed to process trace block %" PRId32 " bytes!", block->data_len);
 951                 return res;
 952         }
 953         res = esp32_apptrace_block_free(ctx, block);
 954         if (res != ERROR_OK) {
 955                 ctx->running = 0;
 956                 LOG_ERROR("Failed to free ready block!");
 957                 return res;
 958         }
 959
 960         return ERROR_OK;
 961 }
 962
 963 static int esp32_apptrace_check_connection(struct esp32_apptrace_cmd_ctx *ctx)
 964 {
 965         if (!ctx)
 966                 return ERROR_FAIL;
 967
 968         unsigned int busy_target_num = 0;
 969
 970         for (unsigned int i = 0; i < ctx->cores_num; i++) {
 971                 bool conn = true;
 972                 int res = ctx->hw->ctrl_reg_read(ctx->cpus[i], NULL, NULL, &conn);
 973                 if (res != ERROR_OK) {
 974                         LOG_ERROR("Failed to read apptrace control reg for cpu(%d) res(%d)!", i, res);
 975                         return res;
 976                 }
 977                 if (!conn) {
 978                         uint32_t stat = 0;
 979                         LOG_TARGET_WARNING(ctx->cpus[i], "apptrace connection is lost. Re-connect.");
 980                         res = ctx->hw->status_reg_read(ctx->cpus[i], &stat);
 981                         if (res != ERROR_OK) {
 982                                 LOG_ERROR("Failed to read trace status (%d)!", res);
 983                                 return res;
 984                         }
 985                         if (stat) {
 986                                 LOG_TARGET_WARNING(ctx->cpus[i], "in critical state. Retry in next poll");
 987                                 if (++busy_target_num == ctx->cores_num) {
 988                                         LOG_WARNING("No available core");
 989                                         return ERROR_WAIT;
 990                                 }
 991                                 continue;
 992                         }
 993                         res = ctx->hw->ctrl_reg_write(ctx->cpus[i],
 994                                 0,
 995                                 0,
 996                                 true /*host connected*/,
 997                                 false /*no host data*/);
 998                         if (res != ERROR_OK) {
 999                                 LOG_ERROR("Failed to write apptrace control reg for cpu(%d) res(%d)!", i, res);
1000                                 return res;
1001                         }
1002                         if (ctx->stop_tmo != -1.0) {
1003                                 /* re-start idle time measurement */
1004                                 if (duration_start(&ctx->idle_time) != 0) {
1005                                         LOG_ERROR("Failed to re-start idle time measure!");
1006                                         return ERROR_FAIL;
1007                                 }
1008                         }
1009                 }
1010         }
1011
1012         return ERROR_OK;
1013 }
1014
1015 static int esp32_apptrace_poll(void *priv)
1016 {
1017         struct esp32_apptrace_cmd_ctx *ctx = (struct esp32_apptrace_cmd_ctx *)priv;
1018         int res;
1019         uint32_t fired_target_num = 0;
1020         struct esp32_apptrace_target_state target_state[ESP32_APPTRACE_MAX_CORES_NUM];
1021         struct duration blk_proc_time;
1022
1023         if (!ctx->running) {
1024                 if (ctx->auto_clean)
1025                         ctx->auto_clean(ctx);
1026                 return ERROR_FAIL;
1027         }
1028
1029         /*  Check for connection is alive.For some reason target and therefore host_connected flag
1030          *  might have been reset */
1031         res = esp32_apptrace_check_connection(ctx);
1032         if (res != ERROR_OK) {
1033                 if (res != ERROR_WAIT)
1034                         ctx->running = 0;
1035                 return res;
1036         }
1037
1038         /* check for data from target */
1039         res = esp32_apptrace_get_data_info(ctx, target_state, &fired_target_num);
1040         if (res != ERROR_OK) {
1041                 ctx->running = 0;
1042                 LOG_ERROR("Failed to read data len!");
1043                 return res;
1044         }
1045         /* LOG_DEBUG("Block %d (%d bytes) on target (%s)!", target_state[0].block_id,
1046          * target_state[0].data_len, target_name(ctx->cpus[0])); */
1047         if (fired_target_num == UINT32_MAX) {
1048                 /* no data has been received, but block could be switched due to the data transferred
1049                  * from host to target */
1050                 if (ctx->cores_num > 1) {
1051                         uint32_t max_block_id = 0, min_block_id = ctx->hw->max_block_id;
1052                         /* find maximum block ID and set the same ID in control reg for both cores
1053                          * */
1054                         for (unsigned int i = 0; i < ctx->cores_num; i++) {
1055                                 if (max_block_id < target_state[i].block_id)
1056                                         max_block_id = target_state[i].block_id;
1057                                 if (min_block_id > target_state[i].block_id)
1058                                         min_block_id = target_state[i].block_id;
1059                         }
1060                         /* handle block ID overflow */
1061                         if (max_block_id == ctx->hw->max_block_id && min_block_id == 0)
1062                                 max_block_id = 0;
1063                         for (unsigned int i = 0; i < ctx->cores_num; i++) {
1064                                 if (max_block_id != target_state[i].block_id) {
1065                                         LOG_TARGET_DEBUG(ctx->cpus[i], "Ack empty block %" PRId32 "!", max_block_id);
1066                                         res = ctx->hw->ctrl_reg_write(ctx->cpus[i],
1067                                                 max_block_id,
1068                                                 0 /*all read*/,
1069                                                 true /*host connected*/,
1070                                                 false /*no host data*/);
1071                                         if (res != ERROR_OK) {
1072                                                 ctx->running = 0;
1073                                                 LOG_TARGET_ERROR(ctx->cpus[i], "Failed to ack empty data block!");
1074                                                 return res;
1075                                         }
1076                                 }
1077                         }
1078                         ctx->last_blk_id = max_block_id;
1079                 }
1080                 if (ctx->stop_tmo != -1.0) {
1081                         if (duration_measure(&ctx->idle_time) != 0) {
1082                                 ctx->running = 0;
1083                                 LOG_ERROR("Failed to measure idle time!");
1084                                 return ERROR_FAIL;
1085                         }
1086                         if (duration_elapsed(&ctx->idle_time) >= ctx->stop_tmo) {
1087                                 ctx->running = 0;
1088                                 LOG_ERROR("Data timeout!");
1089                                 return ERROR_FAIL;
1090                         }
1091                 }
1092                 return ERROR_OK;/* no data */
1093         }
1094         /* sanity check */
1095         if (target_state[fired_target_num].data_len > ctx->max_trace_block_sz) {
1096                 ctx->running = 0;
1097                 LOG_ERROR("Too large block size %" PRId32 "!", target_state[fired_target_num].data_len);
1098                 return ERROR_FAIL;
1099         }
1100         if (ctx->tot_len == 0) {
1101                 if (duration_start(&ctx->read_time) != 0) {
1102                         ctx->running = 0;
1103                         LOG_ERROR("Failed to start trace read time measurement!");
1104                         return ERROR_FAIL;
1105                 }
1106         }
1107         struct esp32_apptrace_block *block = esp32_apptrace_free_block_get(ctx);
1108         if (!block) {
1109                 ctx->running = 0;
1110                 LOG_TARGET_ERROR(ctx->cpus[fired_target_num], "Failed to get free block for data!");
1111                 return ERROR_FAIL;
1112         }
1113         if (s_time_stats_enable) {
1114                 /* read block */
1115                 if (duration_start(&blk_proc_time) != 0) {
1116                         ctx->running = 0;
1117                         LOG_ERROR("Failed to start block read time measurement!");
1118                         return ERROR_FAIL;
1119                 }
1120         }
1121         res =
1122                 ctx->hw->data_read(ctx->cpus[fired_target_num],
1123                 target_state[fired_target_num].data_len,
1124                 block->data,
1125                 target_state[fired_target_num].block_id,
1126                 /* do not ack target data in sync mode,
1127                    esp32_apptrace_handle_trace_block() can write response data and will do ack thereafter */
1128                 ctx->mode != ESP_APPTRACE_CMD_MODE_SYNC);
1129         if (res != ERROR_OK) {
1130                 ctx->running = 0;
1131                 LOG_TARGET_ERROR(ctx->cpus[fired_target_num], "Failed to read data!");
1132                 return res;
1133         }
1134         ctx->last_blk_id = target_state[fired_target_num].block_id;
1135         block->data_len = target_state[fired_target_num].data_len;
1136         ctx->raw_tot_len += block->data_len;
1137         if (s_time_stats_enable) {
1138                 if (duration_measure(&blk_proc_time) != 0) {
1139                         ctx->running = 0;
1140                         LOG_ERROR("Failed to measure block read time!");
1141                         return ERROR_FAIL;
1142                 }
1143                 /* update stats */
1144                 float brt = duration_elapsed(&blk_proc_time);
1145                 if (brt > ctx->stats.max_blk_read_time)
1146                         ctx->stats.max_blk_read_time = brt;
1147                 if (brt < ctx->stats.min_blk_read_time)
1148                         ctx->stats.min_blk_read_time = brt;
1149
1150                 if (duration_start(&blk_proc_time) != 0) {
1151                         ctx->running = 0;
1152                         LOG_ERROR("Failed to start block proc time measurement!");
1153                         return ERROR_FAIL;
1154                 }
1155         }
1156         /* in sync mode do not ack target data on other cores, esp32_apptrace_handle_trace_block() can write response
1157          * data and will do ack thereafter */
1158         if (ctx->mode != ESP_APPTRACE_CMD_MODE_SYNC) {
1159                 for (unsigned int i = 0; i < ctx->cores_num; i++) {
1160                         if (i == fired_target_num)
1161                                 continue;
1162                         res = ctx->hw->ctrl_reg_write(ctx->cpus[i],
1163                                 ctx->last_blk_id,
1164                                 0 /*all read*/,
1165                                 true /*host connected*/,
1166                                 false /*no host data*/);
1167                         if (res != ERROR_OK) {
1168                                 ctx->running = 0;
1169                                 LOG_TARGET_ERROR(ctx->cpus[i], "Failed to ack data!");
1170                                 return res;
1171                         }
1172                         LOG_TARGET_DEBUG(ctx->cpus[i], "Ack block %" PRId32, ctx->last_blk_id);
1173                 }
1174                 res = esp32_apptrace_ready_block_put(ctx, block);
1175                 if (res != ERROR_OK) {
1176                         ctx->running = 0;
1177                         LOG_TARGET_ERROR(ctx->cpus[fired_target_num], "Failed to put ready block of data!");
1178                         return res;
1179                 }
1180         } else {
1181                 res = esp32_apptrace_handle_trace_block(ctx, block);
1182                 if (res != ERROR_OK) {
1183                         ctx->running = 0;
1184                         LOG_ERROR("Failed to process trace block %" PRId32 " bytes!", block->data_len);
1185                         return res;
1186                 }
1187                 res = esp32_apptrace_block_free(ctx, block);
1188                 if (res != ERROR_OK) {
1189                         ctx->running = 0;
1190                         LOG_ERROR("Failed to free ready block!");
1191                         return res;
1192                 }
1193         }
1194         if (ctx->stop_tmo != -1.0) {
1195                 /* start idle time measurement */
1196                 if (duration_start(&ctx->idle_time) != 0) {
1197                         ctx->running = 0;
1198                         LOG_ERROR("Failed to start idle time measure!");
1199                         return ERROR_FAIL;
1200                 }
1201         }
1202         if (s_time_stats_enable) {
1203                 if (duration_measure(&blk_proc_time) != 0) {
1204                         ctx->running = 0;
1205                         LOG_ERROR("Failed to stop block proc time measure!");
1206                         return ERROR_FAIL;
1207                 }
1208                 /* update stats */
1209                 float bt = duration_elapsed(&blk_proc_time);
1210                 if (bt > ctx->stats.max_blk_proc_time)
1211                         ctx->stats.max_blk_proc_time = bt;
1212                 if (bt < ctx->stats.min_blk_proc_time)
1213                         ctx->stats.min_blk_proc_time = bt;
1214         }
1215         return ERROR_OK;
1216 }
1217
1218 static void esp32_apptrace_cmd_stop(struct esp32_apptrace_cmd_ctx *ctx)
1219 {
1220         if (duration_measure(&ctx->read_time) != 0)
1221                 LOG_ERROR("Failed to stop trace read time measurement!");
1222         int res = target_unregister_timer_callback(esp32_apptrace_poll, ctx);
1223         if (res != ERROR_OK)
1224                 LOG_ERROR("Failed to unregister target timer handler (%d)!", res);
1225
1226         /* data processor is alive, so wait for all received blocks to be processed */
1227         res = esp32_apptrace_wait_tracing_finished(ctx);
1228         if (res != ERROR_OK)
1229                 LOG_ERROR("Failed to wait for pended blocks (%d)!", res);
1230         res = esp32_apptrace_connect_targets(ctx, false, ctx->target_state == TARGET_RUNNING);
1231         if (res != ERROR_OK)
1232                 LOG_ERROR("Failed to disconnect targets (%d)!", res);
1233         esp32_apptrace_print_stats(ctx);
1234         res = esp32_apptrace_cmd_cleanup(ctx);
1235         if (res != ERROR_OK)
1236                 LOG_ERROR("Failed to cleanup cmd ctx (%d)!", res);
1237 }
1238
1239 int esp32_cmd_apptrace_generic(struct command_invocation *cmd, int mode, const char **argv, int argc)
1240 {
1241         static struct esp32_apptrace_cmd_ctx s_at_cmd_ctx;
1242         struct esp32_apptrace_cmd_data *cmd_data;
1243         int res = ERROR_FAIL;
1244         enum target_state old_state;
1245         struct target *target = get_current_target(CMD_CTX);
1246
1247         if (argc < 1)
1248                 return ERROR_COMMAND_SYNTAX_ERROR;
1249
1250         /* command can be invoked on unexamined core, if so find examined one */
1251         if (target->smp && !target_was_examined(target)) {
1252                 struct target_list *head;
1253                 struct target *curr;
1254                 LOG_WARNING("Current target '%s' was not examined!", target_name(target));
1255                 foreach_smp_target(head, target->smp_targets) {
1256                         curr = head->target;
1257                         if (target_was_examined(curr)) {
1258                                 target = curr;
1259                                 LOG_WARNING("Run command on target '%s'", target_name(target));
1260                                 break;
1261                         }
1262                 }
1263         }
1264         old_state = target->state;
1265
1266         if (strcmp(argv[0], "start") == 0) {
1267                 res = esp32_apptrace_cmd_init(&s_at_cmd_ctx,
1268                         cmd,
1269                         mode,
1270                         &argv[1],
1271                         argc - 1);
1272                 if (res != ERROR_OK) {
1273                         command_print(cmd, "Failed to init cmd ctx (%d)!", res);
1274                         return res;
1275                 }
1276                 cmd_data = s_at_cmd_ctx.cmd_priv;
1277                 s_at_cmd_ctx.process_data = esp32_apptrace_process_data;
1278                 s_at_cmd_ctx.auto_clean = esp32_apptrace_cmd_stop;
1279                 if (cmd_data->wait4halt) {
1280                         res = esp32_apptrace_wait4halt(&s_at_cmd_ctx, target);
1281                         if (res != ERROR_OK) {
1282                                 command_print(cmd, "Failed to wait for halt target (%d)!", res);
1283                                 goto _on_start_error;
1284                         }
1285                 }
1286                 res = esp32_apptrace_connect_targets(&s_at_cmd_ctx, true, old_state == TARGET_RUNNING);
1287                 if (res != ERROR_OK) {
1288                         command_print(cmd, "Failed to connect to targets (%d)!", res);
1289                         goto _on_start_error;
1290                 }
1291                 res = target_register_timer_callback(esp32_apptrace_poll,
1292                         cmd_data->poll_period,
1293                         TARGET_TIMER_TYPE_PERIODIC,
1294                         &s_at_cmd_ctx);
1295                 if (res != ERROR_OK) {
1296                         command_print(cmd, "Failed to register target timer handler (%d)!", res);
1297                         goto _on_start_error;
1298                 }
1299         } else if (strcmp(argv[0], "stop") == 0) {
1300                 if (!s_at_cmd_ctx.running) {
1301                         command_print(cmd, "Tracing is not running!");
1302                         return ERROR_FAIL;
1303                 }
1304                 esp32_apptrace_cmd_stop(&s_at_cmd_ctx);
1305                 return ERROR_OK;
1306         } else if (strcmp(argv[0], "status") == 0) {
1307                 if (s_at_cmd_ctx.running && duration_measure(&s_at_cmd_ctx.read_time) != 0)
1308                         LOG_ERROR("Failed to measure trace read time!");
1309                 esp32_apptrace_print_stats(&s_at_cmd_ctx);
1310                 return ERROR_OK;
1311         } else if (strcmp(argv[0], "dump") == 0) {
1312                 /* [dump outfile] - post-mortem dump without connection to targets */
1313                 res = esp32_apptrace_cmd_init(&s_at_cmd_ctx,
1314                         cmd,
1315                         mode,
1316                         &argv[1],
1317                         argc - 1);
1318                 if (res != ERROR_OK) {
1319                         command_print(cmd, "Failed to init cmd ctx (%d)!", res);
1320                         return res;
1321                 }
1322                 s_at_cmd_ctx.stop_tmo = 0.01;   /* use small stop tmo */
1323                 s_at_cmd_ctx.process_data = esp32_apptrace_process_data;
1324                 /* check for exit signal and command completion */
1325                 while (!openocd_is_shutdown_pending() && s_at_cmd_ctx.running) {
1326                         res = esp32_apptrace_poll(&s_at_cmd_ctx);
1327                         if (res != ERROR_OK) {
1328                                 LOG_ERROR("Failed to poll target for trace data (%d)!", res);
1329                                 break;
1330                         }
1331                         /* let registered timer callbacks to run */
1332                         target_call_timer_callbacks();
1333                 }
1334                 if (s_at_cmd_ctx.running) {
1335                         /* data processor is alive, so wait for all received blocks to be processed */
1336                         res = esp32_apptrace_wait_tracing_finished(&s_at_cmd_ctx);
1337                         if (res != ERROR_OK)
1338                                 LOG_ERROR("Failed to wait for pended blocks (%d)!", res);
1339                 }
1340                 esp32_apptrace_print_stats(&s_at_cmd_ctx);
1341                 res = esp32_apptrace_cmd_cleanup(&s_at_cmd_ctx);
1342                 if (res != ERROR_OK)
1343                         command_print(cmd, "Failed to cleanup cmd ctx (%d)!", res);
1344         } else {
1345                 command_print(cmd, "Invalid action '%s'!", argv[0]);
1346         }
1347
1348         return res;
1349
1350 _on_start_error:
1351         s_at_cmd_ctx.running = 0;
1352         esp32_apptrace_cmd_cleanup(&s_at_cmd_ctx);
1353         return res;
1354 }
1355
1356 COMMAND_HANDLER(esp32_cmd_apptrace)
1357 {
1358         return esp32_cmd_apptrace_generic(CMD, ESP_APPTRACE_CMD_MODE_GEN, CMD_ARGV, CMD_ARGC);
1359 }
1360
1361 const struct command_registration esp32_apptrace_command_handlers[] = {
1362         {
1363                 .name = "apptrace",
1364                 .handler = esp32_cmd_apptrace,
1365                 .mode = COMMAND_EXEC,
1366                 .help =
1367                         "App Tracing: application level trace control. Starts, stops or queries tracing process status.",
1368                 .usage =
1369                         "[start <destination> [poll_period [trace_size [stop_tmo [wait4halt [skip_size]]]]] | [stop] | [status] | [dump <destination>]",
1370         },
1371         COMMAND_REGISTRATION_DONE
1372 };