git.gag.com Git - fw/openocd/blob - src/target/espressif/esp_xtensa_smp.c

   1 /* SPDX-License-Identifier: GPL-2.0-or-later */
   2
   3 /***************************************************************************
   4  *   ESP Xtensa SMP target API for OpenOCD                                 *
   5  *   Copyright (C) 2020 Espressif Systems Ltd. Co                          *
   6  ***************************************************************************/
   7
   8 #ifdef HAVE_CONFIG_H
   9 #include "config.h"
  10 #endif
  11
  12 #include "assert.h"
  13 #include <target/target.h>
  14 #include <target/target_type.h>
  15 #include <target/smp.h>
  16 #include <target/semihosting_common.h>
  17 #include "esp_xtensa_smp.h"
  18 #include "esp_xtensa_semihosting.h"
  19
  20 /*
  21 Multiprocessor stuff common:
  22
  23 The ESP Xtensa chip can have several cores in it, which can run in SMP-mode if an
  24 SMP-capable OS is running. The hardware has a few features which makes
  25 SMP debugging much easier.
  26
  27 First of all, there's something called a 'break network', consisting of a
  28 BreakIn input  and a BreakOut output on each CPU. The idea is that as soon
  29 as a CPU goes into debug mode for whatever reason, it'll signal that using
  30 its DebugOut pin. This signal is connected to the other CPU's DebugIn
  31 input, causing this CPU also to go into debugging mode. To resume execution
  32 when using only this break network, we will need to manually resume both
  33 CPUs.
  34
  35 An alternative to this is the XOCDMode output and the RunStall (or DebugStall)
  36 input. When these are cross-connected, a CPU that goes into debug mode will
  37 halt execution entirely on the other CPU. Execution on the other CPU can be
  38 resumed by either the first CPU going out of debug mode, or the second CPU
  39 going into debug mode: the stall is temporarily lifted as long as the stalled
  40 CPU is in debug mode.
  41
  42 A third, separate, signal is CrossTrigger. This is connected in the same way
  43 as the breakIn/breakOut network, but is for the TRAX (trace memory) feature;
  44 it does not affect OCD in any way.
  45 */
  46
  47 /*
  48 Multiprocessor stuff:
  49
  50 The ESP Xtensa chip has several Xtensa cores inside, but represent themself to the OCD
  51 as one chip that works in multithreading mode under FreeRTOS OS.
  52 The core that initiate the stop condition will be defined as an active cpu.
  53 When one core stops, then other core will be stopped automatically by smpbreak.
  54 The core that initiates stop condition will be defined as an active core, and
  55 registers of this core will be transferred.
  56 */
  57
  58 #define ESP_XTENSA_SMP_EXAMINE_OTHER_CORES      5
  59
  60 static int esp_xtensa_smp_update_halt_gdb(struct target *target, bool *need_resume);
  61
  62 static inline struct esp_xtensa_smp_common *target_to_esp_xtensa_smp(struct target *target)
  63 {
  64         return container_of(target->arch_info, struct esp_xtensa_smp_common, esp_xtensa);
  65 }
  66
  67 int esp_xtensa_smp_assert_reset(struct target *target)
  68 {
  69         return ERROR_OK;
  70 }
  71
  72 int esp_xtensa_smp_deassert_reset(struct target *target)
  73 {
  74         LOG_TARGET_DEBUG(target, "begin");
  75
  76         int ret = xtensa_deassert_reset(target);
  77         if (ret != ERROR_OK)
  78                 return ret;
  79         /* in SMP mode when chip was running single-core app the other core can be left un-examined,
  80            because examination is done before SOC reset. But after SOC reset it is functional and should be handled.
  81            So try to examine un-examined core just after SOC reset */
  82         if (target->smp && !target_was_examined(target))
  83                 ret = xtensa_examine(target);
  84         return ret;
  85 }
  86
  87 int esp_xtensa_smp_soft_reset_halt(struct target *target)
  88 {
  89         int res;
  90         struct target_list *head;
  91         struct esp_xtensa_smp_common *esp_xtensa_smp = target_to_esp_xtensa_smp(target);
  92
  93         LOG_TARGET_DEBUG(target, "begin");
  94         /* in SMP mode we need to ensure that at first we reset SOC on PRO-CPU
  95            and then call xtensa_assert_reset() for all cores */
  96         if (target->smp && target->coreid != 0)
  97                 return ERROR_OK;
  98         /* Reset the SoC first */
  99         if (esp_xtensa_smp->chip_ops->reset) {
 100                 res = esp_xtensa_smp->chip_ops->reset(target);
 101                 if (res != ERROR_OK)
 102                         return res;
 103         }
 104         if (!target->smp)
 105                 return xtensa_assert_reset(target);
 106
 107         foreach_smp_target(head, target->smp_targets) {
 108                 res = xtensa_assert_reset(head->target);
 109                 if (res != ERROR_OK)
 110                         return res;
 111         }
 112         return ERROR_OK;
 113 }
 114
 115 static struct target *get_halted_esp_xtensa_smp(struct target *target, int32_t coreid)
 116 {
 117         struct target_list *head;
 118         struct target *curr;
 119
 120         foreach_smp_target(head, target->smp_targets) {
 121                 curr = head->target;
 122                 if ((curr->coreid == coreid) && (curr->state == TARGET_HALTED))
 123                         return curr;
 124         }
 125
 126         return target;
 127 }
 128
 129 int esp_xtensa_smp_poll(struct target *target)
 130 {
 131         enum target_state old_state = target->state;
 132         struct esp_xtensa_smp_common *esp_xtensa_smp = target_to_esp_xtensa_smp(target);
 133         struct esp_xtensa_common *esp_xtensa = target_to_esp_xtensa(target);
 134         struct target_list *head;
 135         struct target *curr;
 136         bool other_core_resume_req = false;
 137
 138         if (target->state == TARGET_HALTED && target->smp && target->gdb_service && !target->gdb_service->target) {
 139                 target->gdb_service->target = get_halted_esp_xtensa_smp(target, target->gdb_service->core[1]);
 140                 LOG_INFO("Switch GDB target to '%s'", target_name(target->gdb_service->target));
 141                 if (esp_xtensa_smp->chip_ops->on_halt)
 142                         esp_xtensa_smp->chip_ops->on_halt(target);
 143                 target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 144                 return ERROR_OK;
 145         }
 146
 147         int ret = esp_xtensa_poll(target);
 148         if (ret != ERROR_OK)
 149                 return ret;
 150
 151         if (target->smp) {
 152                 if (target->state == TARGET_RESET) {
 153                         esp_xtensa_smp->examine_other_cores = ESP_XTENSA_SMP_EXAMINE_OTHER_CORES;
 154                 } else if (esp_xtensa_smp->examine_other_cores > 0 &&
 155                         (target->state == TARGET_RUNNING || target->state == TARGET_HALTED)) {
 156                         LOG_TARGET_DEBUG(target, "Check for unexamined cores after reset");
 157                         bool all_examined = true;
 158                         foreach_smp_target(head, target->smp_targets) {
 159                                 curr = head->target;
 160                                 if (curr == target)
 161                                         continue;
 162                                 if (!target_was_examined(curr)) {
 163                                         if (target_examine_one(curr) != ERROR_OK) {
 164                                                 LOG_DEBUG("Failed to examine!");
 165                                                 all_examined = false;
 166                                         }
 167                                 }
 168                         }
 169                         if (all_examined)
 170                                 esp_xtensa_smp->examine_other_cores = 0;
 171                         else
 172                                 esp_xtensa_smp->examine_other_cores--;
 173                 }
 174         }
 175
 176         if (old_state != TARGET_HALTED && target->state == TARGET_HALTED) {
 177                 if (target->smp) {
 178                         ret = esp_xtensa_smp_update_halt_gdb(target, &other_core_resume_req);
 179                         if (ret != ERROR_OK)
 180                                 return ret;
 181                 }
 182                 /* Call any event callbacks that are applicable */
 183                 if (old_state == TARGET_DEBUG_RUNNING) {
 184                         target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);
 185                 } else {
 186                         if (esp_xtensa_semihosting(target, &ret) == SEMIHOSTING_HANDLED) {
 187                                 if (ret == ERROR_OK && esp_xtensa->semihost.need_resume &&
 188                                         !esp_xtensa_smp->other_core_does_resume) {
 189                                         esp_xtensa->semihost.need_resume = false;
 190                                         /* Resume xtensa_resume will handle BREAK instruction. */
 191                                         ret = target_resume(target, 1, 0, 1, 0);
 192                                         if (ret != ERROR_OK) {
 193                                                 LOG_ERROR("Failed to resume target");
 194                                                 return ret;
 195                                         }
 196                                 }
 197                                 return ret;
 198                         }
 199                         /* check whether any core polled by esp_xtensa_smp_update_halt_gdb() requested resume */
 200                         if (target->smp && other_core_resume_req) {
 201                                 /* Resume xtensa_resume will handle BREAK instruction. */
 202                                 ret = target_resume(target, 1, 0, 1, 0);
 203                                 if (ret != ERROR_OK) {
 204                                         LOG_ERROR("Failed to resume target");
 205                                         return ret;
 206                                 }
 207                                 return ERROR_OK;
 208                         }
 209                         if (esp_xtensa_smp->chip_ops->on_halt)
 210                                 esp_xtensa_smp->chip_ops->on_halt(target);
 211                         target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 212                 }
 213         }
 214
 215         return ERROR_OK;
 216 }
 217
 218 static int esp_xtensa_smp_update_halt_gdb(struct target *target, bool *need_resume)
 219 {
 220         struct esp_xtensa_smp_common *esp_xtensa_smp;
 221         struct target *gdb_target = NULL;
 222         struct target_list *head;
 223         struct target *curr;
 224         int ret = ERROR_OK;
 225
 226         *need_resume = false;
 227
 228         if (target->gdb_service && target->gdb_service->target)
 229                 LOG_DEBUG("GDB target '%s'", target_name(target->gdb_service->target));
 230
 231         if (target->gdb_service && target->gdb_service->core[0] == -1) {
 232                 target->gdb_service->target = target;
 233                 target->gdb_service->core[0] = target->coreid;
 234                 LOG_INFO("Set GDB target to '%s'", target_name(target));
 235         }
 236
 237         if (target->gdb_service)
 238                 gdb_target = target->gdb_service->target;
 239
 240         /* due to smpbreak config other cores can also go to HALTED state */
 241         foreach_smp_target(head, target->smp_targets) {
 242                 curr = head->target;
 243                 LOG_DEBUG("Check target '%s'", target_name(curr));
 244                 /* skip calling context */
 245                 if (curr == target)
 246                         continue;
 247                 if (!target_was_examined(curr)) {
 248                         curr->state = TARGET_HALTED;
 249                         continue;
 250                 }
 251                 /* skip targets that were already halted */
 252                 if (curr->state == TARGET_HALTED)
 253                         continue;
 254                 /* Skip gdb_target; it alerts GDB so has to be polled as last one */
 255                 if (curr == gdb_target)
 256                         continue;
 257                 LOG_DEBUG("Poll target '%s'", target_name(curr));
 258
 259                 esp_xtensa_smp = target_to_esp_xtensa_smp(curr);
 260                 /* avoid auto-resume after syscall, it will be done later */
 261                 esp_xtensa_smp->other_core_does_resume = true;
 262                 /* avoid recursion in esp_xtensa_smp_poll() */
 263                 curr->smp = 0;
 264                 if (esp_xtensa_smp->chip_ops->poll)
 265                         ret = esp_xtensa_smp->chip_ops->poll(curr);
 266                 else
 267                         ret = esp_xtensa_smp_poll(curr);
 268                 curr->smp = 1;
 269                 if (ret != ERROR_OK)
 270                         return ret;
 271                 esp_xtensa_smp->other_core_does_resume = false;
 272                 struct esp_xtensa_common *curr_esp_xtensa = target_to_esp_xtensa(curr);
 273                 if (curr_esp_xtensa->semihost.need_resume) {
 274                         curr_esp_xtensa->semihost.need_resume = false;
 275                         *need_resume = true;
 276                 }
 277         }
 278
 279         /* after all targets were updated, poll the gdb serving target */
 280         if (gdb_target && gdb_target != target) {
 281                 esp_xtensa_smp = target_to_esp_xtensa_smp(gdb_target);
 282                 if (esp_xtensa_smp->chip_ops->poll)
 283                         ret = esp_xtensa_smp->chip_ops->poll(gdb_target);
 284                 else
 285                         ret = esp_xtensa_smp_poll(gdb_target);
 286         }
 287
 288         LOG_DEBUG("exit");
 289
 290         return ret;
 291 }
 292
 293 static inline int esp_xtensa_smp_smpbreak_disable(struct target *target, uint32_t *smp_break)
 294 {
 295         int res = xtensa_smpbreak_get(target, smp_break);
 296         if (res != ERROR_OK)
 297                 return res;
 298         return xtensa_smpbreak_set(target, 0);
 299 }
 300
 301 static inline int esp_xtensa_smp_smpbreak_restore(struct target *target, uint32_t smp_break)
 302 {
 303         return xtensa_smpbreak_set(target, smp_break);
 304 }
 305
 306 static int esp_xtensa_smp_resume_cores(struct target *target,
 307         int handle_breakpoints,
 308         int debug_execution)
 309 {
 310         struct target_list *head;
 311         struct target *curr;
 312
 313         LOG_TARGET_DEBUG(target, "begin");
 314
 315         foreach_smp_target(head, target->smp_targets) {
 316                 curr = head->target;
 317                 /* in single-core mode disabled core cannot be examined, but need to be resumed too*/
 318                 if ((curr != target) && (curr->state != TARGET_RUNNING) && target_was_examined(curr)) {
 319                         /*  resume current address, not in SMP mode */
 320                         curr->smp = 0;
 321                         int res = esp_xtensa_smp_resume(curr, 1, 0, handle_breakpoints, debug_execution);
 322                         curr->smp = 1;
 323                         if (res != ERROR_OK)
 324                                 return res;
 325                 }
 326         }
 327         return ERROR_OK;
 328 }
 329
 330 int esp_xtensa_smp_resume(struct target *target,
 331         int current,
 332         target_addr_t address,
 333         int handle_breakpoints,
 334         int debug_execution)
 335 {
 336         int res;
 337         uint32_t smp_break;
 338
 339         xtensa_smpbreak_get(target, &smp_break);
 340         LOG_TARGET_DEBUG(target, "smp_break=0x%" PRIx32, smp_break);
 341
 342         /* dummy resume for smp toggle in order to reduce gdb impact  */
 343         if ((target->smp) && (target->gdb_service) && (target->gdb_service->core[1] != -1)) {
 344                 /* simulate a start and halt of target */
 345                 target->gdb_service->target = NULL;
 346                 target->gdb_service->core[0] = target->gdb_service->core[1];
 347                 /* fake resume at next poll we play the  target core[1], see poll*/
 348                 LOG_TARGET_DEBUG(target, "Fake resume");
 349                 target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
 350                 return ERROR_OK;
 351         }
 352
 353         /* xtensa_prepare_resume() can step over breakpoint/watchpoint and generate signals on BreakInOut circuit for
 354          * other cores. So disconnect this core from BreakInOut circuit and do xtensa_prepare_resume(). */
 355         res = esp_xtensa_smp_smpbreak_disable(target, &smp_break);
 356         if (res != ERROR_OK)
 357                 return res;
 358         res = xtensa_prepare_resume(target, current, address, handle_breakpoints, debug_execution);
 359         /* restore configured BreakInOut signals config */
 360         int ret = esp_xtensa_smp_smpbreak_restore(target, smp_break);
 361         if (ret != ERROR_OK)
 362                 return ret;
 363         if (res != ERROR_OK) {
 364                 LOG_TARGET_ERROR(target, "Failed to prepare for resume!");
 365                 return res;
 366         }
 367
 368         if (target->smp) {
 369                 if (target->gdb_service)
 370                         target->gdb_service->core[0] = -1;
 371                 res = esp_xtensa_smp_resume_cores(target, handle_breakpoints, debug_execution);
 372                 if (res != ERROR_OK)
 373                         return res;
 374         }
 375
 376         res = xtensa_do_resume(target);
 377         if (res != ERROR_OK) {
 378                 LOG_TARGET_ERROR(target, "Failed to resume!");
 379                 return res;
 380         }
 381
 382         target->debug_reason = DBG_REASON_NOTHALTED;
 383         if (!debug_execution)
 384                 target->state = TARGET_RUNNING;
 385         else
 386                 target->state = TARGET_DEBUG_RUNNING;
 387
 388         target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
 389         return ERROR_OK;
 390 }
 391
 392 int esp_xtensa_smp_step(struct target *target,
 393         int current,
 394         target_addr_t address,
 395         int handle_breakpoints)
 396 {
 397         int res;
 398         uint32_t smp_break = 0;
 399         struct esp_xtensa_smp_common *esp_xtensa_smp = target_to_esp_xtensa_smp(target);
 400
 401         if (target->smp) {
 402                 res = esp_xtensa_smp_smpbreak_disable(target, &smp_break);
 403                 if (res != ERROR_OK)
 404                         return res;
 405         }
 406         res = xtensa_step(target, current, address, handle_breakpoints);
 407
 408         if (res == ERROR_OK) {
 409                 if (esp_xtensa_smp->chip_ops->on_halt)
 410                         esp_xtensa_smp->chip_ops->on_halt(target);
 411                 target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 412         }
 413
 414         if (target->smp) {
 415                 int ret = esp_xtensa_smp_smpbreak_restore(target, smp_break);
 416                 if (ret != ERROR_OK)
 417                         return ret;
 418         }
 419
 420         return res;
 421 }
 422
 423 int esp_xtensa_smp_watchpoint_add(struct target *target, struct watchpoint *watchpoint)
 424 {
 425         int res = xtensa_watchpoint_add(target, watchpoint);
 426         if (res != ERROR_OK)
 427                 return res;
 428
 429         if (!target->smp)
 430                 return ERROR_OK;
 431
 432         struct target_list *head;
 433         foreach_smp_target(head, target->smp_targets) {
 434                 struct target *curr = head->target;
 435                 if (curr == target || !target_was_examined(curr))
 436                         continue;
 437                 /* Need to use high level API here because every target for core contains list of watchpoints.
 438                  * GDB works with active core only, so we need to duplicate every watchpoint on other cores,
 439                  * otherwise watchpoint_free() on active core can fail if WP has been initially added on another core. */
 440                 curr->smp = 0;
 441                 res = watchpoint_add(curr, watchpoint->address, watchpoint->length,
 442                         watchpoint->rw, watchpoint->value, watchpoint->mask);
 443                 curr->smp = 1;
 444                 if (res != ERROR_OK)
 445                         return res;
 446         }
 447         return ERROR_OK;
 448 }
 449
 450 int esp_xtensa_smp_watchpoint_remove(struct target *target, struct watchpoint *watchpoint)
 451 {
 452         int res = xtensa_watchpoint_remove(target, watchpoint);
 453         if (res != ERROR_OK)
 454                 return res;
 455
 456         if (!target->smp)
 457                 return ERROR_OK;
 458
 459         struct target_list *head;
 460         foreach_smp_target(head, target->smp_targets) {
 461                 struct target *curr = head->target;
 462                 if (curr == target)
 463                         continue;
 464                 /* see big comment in esp_xtensa_smp_watchpoint_add() */
 465                 curr->smp = 0;
 466                 watchpoint_remove(curr, watchpoint->address);
 467                 curr->smp = 1;
 468         }
 469         return ERROR_OK;
 470 }
 471
 472 int esp_xtensa_smp_init_arch_info(struct target *target,
 473         struct esp_xtensa_smp_common *esp_xtensa_smp,
 474         struct xtensa_debug_module_config *dm_cfg,
 475         const struct esp_xtensa_smp_chip_ops *chip_ops,
 476         const struct esp_semihost_ops *semihost_ops)
 477 {
 478         int ret = esp_xtensa_init_arch_info(target, &esp_xtensa_smp->esp_xtensa, dm_cfg, semihost_ops);
 479         if (ret != ERROR_OK)
 480                 return ret;
 481         esp_xtensa_smp->chip_ops = chip_ops;
 482         esp_xtensa_smp->examine_other_cores = ESP_XTENSA_SMP_EXAMINE_OTHER_CORES;
 483         return ERROR_OK;
 484 }
 485
 486 int esp_xtensa_smp_target_init(struct command_context *cmd_ctx, struct target *target)
 487 {
 488         int ret = esp_xtensa_target_init(cmd_ctx, target);
 489         if (ret != ERROR_OK)
 490                 return ret;
 491
 492         if (target->smp) {
 493                 struct target_list *head;
 494                 foreach_smp_target(head, target->smp_targets) {
 495                         struct target *curr = head->target;
 496                         ret = esp_xtensa_semihosting_init(curr);
 497                         if (ret != ERROR_OK)
 498                                 return ret;
 499                 }
 500         } else {
 501                 ret = esp_xtensa_semihosting_init(target);
 502                 if (ret != ERROR_OK)
 503                         return ret;
 504         }
 505         return ERROR_OK;
 506 }
 507
 508 COMMAND_HANDLER(esp_xtensa_smp_cmd_xtdef)
 509 {
 510         struct target *target = get_current_target(CMD_CTX);
 511         if (target->smp && CMD_ARGC > 0) {
 512                 struct target_list *head;
 513                 struct target *curr;
 514                 foreach_smp_target(head, target->smp_targets) {
 515                         curr = head->target;
 516                         int ret = CALL_COMMAND_HANDLER(xtensa_cmd_xtdef_do,
 517                                 target_to_xtensa(curr));
 518                         if (ret != ERROR_OK)
 519                                 return ret;
 520                 }
 521                 return ERROR_OK;
 522         }
 523         return CALL_COMMAND_HANDLER(xtensa_cmd_xtdef_do,
 524                 target_to_xtensa(target));
 525 }
 526
 527 COMMAND_HANDLER(esp_xtensa_smp_cmd_xtopt)
 528 {
 529         struct target *target = get_current_target(CMD_CTX);
 530         if (target->smp && CMD_ARGC > 0) {
 531                 struct target_list *head;
 532                 struct target *curr;
 533                 foreach_smp_target(head, target->smp_targets) {
 534                         curr = head->target;
 535                         int ret = CALL_COMMAND_HANDLER(xtensa_cmd_xtopt_do,
 536                                 target_to_xtensa(curr));
 537                         if (ret != ERROR_OK)
 538                                 return ret;
 539                 }
 540                 return ERROR_OK;
 541         }
 542         return CALL_COMMAND_HANDLER(xtensa_cmd_xtopt_do,
 543                 target_to_xtensa(target));
 544 }
 545
 546 COMMAND_HANDLER(esp_xtensa_smp_cmd_xtmem)
 547 {
 548         struct target *target = get_current_target(CMD_CTX);
 549         if (target->smp && CMD_ARGC > 0) {
 550                 struct target_list *head;
 551                 struct target *curr;
 552                 foreach_smp_target(head, target->smp_targets) {
 553                         curr = head->target;
 554                         int ret = CALL_COMMAND_HANDLER(xtensa_cmd_xtmem_do,
 555                                 target_to_xtensa(curr));
 556                         if (ret != ERROR_OK)
 557                                 return ret;
 558                 }
 559                 return ERROR_OK;
 560         }
 561         return CALL_COMMAND_HANDLER(xtensa_cmd_xtmem_do,
 562                 target_to_xtensa(target));
 563 }
 564
 565 COMMAND_HANDLER(esp_xtensa_smp_cmd_xtmpu)
 566 {
 567         struct target *target = get_current_target(CMD_CTX);
 568         if (target->smp && CMD_ARGC > 0) {
 569                 struct target_list *head;
 570                 struct target *curr;
 571                 foreach_smp_target(head, target->smp_targets) {
 572                         curr = head->target;
 573                         int ret = CALL_COMMAND_HANDLER(xtensa_cmd_xtmpu_do,
 574                                 target_to_xtensa(curr));
 575                         if (ret != ERROR_OK)
 576                                 return ret;
 577                 }
 578                 return ERROR_OK;
 579         }
 580         return CALL_COMMAND_HANDLER(xtensa_cmd_xtmpu_do,
 581                 target_to_xtensa(target));
 582 }
 583
 584 COMMAND_HANDLER(esp_xtensa_smp_cmd_xtmmu)
 585 {
 586         struct target *target = get_current_target(CMD_CTX);
 587         if (target->smp && CMD_ARGC > 0) {
 588                 struct target_list *head;
 589                 struct target *curr;
 590                 foreach_smp_target(head, target->smp_targets) {
 591                         curr = head->target;
 592                         int ret = CALL_COMMAND_HANDLER(xtensa_cmd_xtmmu_do,
 593                                 target_to_xtensa(curr));
 594                         if (ret != ERROR_OK)
 595                                 return ret;
 596                 }
 597                 return ERROR_OK;
 598         }
 599         return CALL_COMMAND_HANDLER(xtensa_cmd_xtmmu_do,
 600                 target_to_xtensa(target));
 601 }
 602
 603 COMMAND_HANDLER(esp_xtensa_smp_cmd_xtreg)
 604 {
 605         struct target *target = get_current_target(CMD_CTX);
 606         if (target->smp && CMD_ARGC > 0) {
 607                 struct target_list *head;
 608                 struct target *curr;
 609                 foreach_smp_target(head, target->smp_targets) {
 610                         curr = head->target;
 611                         int ret = CALL_COMMAND_HANDLER(xtensa_cmd_xtreg_do,
 612                                 target_to_xtensa(curr));
 613                         if (ret != ERROR_OK)
 614                                 return ret;
 615                 }
 616                 return ERROR_OK;
 617         }
 618         return CALL_COMMAND_HANDLER(xtensa_cmd_xtreg_do,
 619                 target_to_xtensa(target));
 620 }
 621
 622 COMMAND_HANDLER(esp_xtensa_smp_cmd_xtregfmt)
 623 {
 624         struct target *target = get_current_target(CMD_CTX);
 625         if (target->smp && CMD_ARGC > 0) {
 626                 struct target_list *head;
 627                 struct target *curr;
 628                 foreach_smp_target(head, target->smp_targets) {
 629                         curr = head->target;
 630                         int ret = CALL_COMMAND_HANDLER(xtensa_cmd_xtregfmt_do,
 631                                 target_to_xtensa(curr));
 632                         if (ret != ERROR_OK)
 633                                 return ret;
 634                 }
 635                 return ERROR_OK;
 636         }
 637         return CALL_COMMAND_HANDLER(xtensa_cmd_xtregfmt_do,
 638                 target_to_xtensa(target));
 639 }
 640
 641 COMMAND_HANDLER(esp_xtensa_smp_cmd_permissive_mode)
 642 {
 643         struct target *target = get_current_target(CMD_CTX);
 644         if (target->smp && CMD_ARGC > 0) {
 645                 struct target_list *head;
 646                 struct target *curr;
 647                 foreach_smp_target(head, target->smp_targets) {
 648                         curr = head->target;
 649                         int ret = CALL_COMMAND_HANDLER(xtensa_cmd_permissive_mode_do,
 650                                 target_to_xtensa(curr));
 651                         if (ret != ERROR_OK)
 652                                 return ret;
 653                 }
 654                 return ERROR_OK;
 655         }
 656         return CALL_COMMAND_HANDLER(xtensa_cmd_permissive_mode_do,
 657                 target_to_xtensa(target));
 658 }
 659
 660 COMMAND_HANDLER(esp_xtensa_smp_cmd_smpbreak)
 661 {
 662         struct target *target = get_current_target(CMD_CTX);
 663         if (target->smp && CMD_ARGC > 0) {
 664                 struct target_list *head;
 665                 struct target *curr;
 666                 foreach_smp_target(head, target->smp_targets) {
 667                         curr = head->target;
 668                         int ret = CALL_COMMAND_HANDLER(xtensa_cmd_smpbreak_do, curr);
 669                         if (ret != ERROR_OK)
 670                                 return ret;
 671                 }
 672                 return ERROR_OK;
 673         }
 674         return CALL_COMMAND_HANDLER(xtensa_cmd_smpbreak_do, target);
 675 }
 676
 677 COMMAND_HANDLER(esp_xtensa_smp_cmd_mask_interrupts)
 678 {
 679         struct target *target = get_current_target(CMD_CTX);
 680         if (target->smp && CMD_ARGC > 0) {
 681                 struct target_list *head;
 682                 struct target *curr;
 683                 foreach_smp_target(head, target->smp_targets) {
 684                         curr = head->target;
 685                         int ret = CALL_COMMAND_HANDLER(xtensa_cmd_mask_interrupts_do,
 686                                 target_to_xtensa(curr));
 687                         if (ret != ERROR_OK)
 688                                 return ret;
 689                 }
 690                 return ERROR_OK;
 691         }
 692         return CALL_COMMAND_HANDLER(xtensa_cmd_mask_interrupts_do,
 693                 target_to_xtensa(target));
 694 }
 695
 696 COMMAND_HANDLER(esp_xtensa_smp_cmd_perfmon_enable)
 697 {
 698         struct target *target = get_current_target(CMD_CTX);
 699         if (target->smp && CMD_ARGC > 0) {
 700                 struct target_list *head;
 701                 struct target *curr;
 702                 foreach_smp_target(head, target->smp_targets) {
 703                         curr = head->target;
 704                         int ret = CALL_COMMAND_HANDLER(xtensa_cmd_perfmon_enable_do,
 705                                 target_to_xtensa(curr));
 706                         if (ret != ERROR_OK)
 707                                 return ret;
 708                 }
 709                 return ERROR_OK;
 710         }
 711         return CALL_COMMAND_HANDLER(xtensa_cmd_perfmon_enable_do,
 712                 target_to_xtensa(target));
 713 }
 714
 715 COMMAND_HANDLER(esp_xtensa_smp_cmd_perfmon_dump)
 716 {
 717         struct target *target = get_current_target(CMD_CTX);
 718         if (target->smp) {
 719                 struct target_list *head;
 720                 struct target *curr;
 721                 foreach_smp_target(head, target->smp_targets) {
 722                         curr = head->target;
 723                         LOG_INFO("CPU%d:", curr->coreid);
 724                         int ret = CALL_COMMAND_HANDLER(xtensa_cmd_perfmon_dump_do,
 725                                 target_to_xtensa(curr));
 726                         if (ret != ERROR_OK)
 727                                 return ret;
 728                 }
 729                 return ERROR_OK;
 730         }
 731         return CALL_COMMAND_HANDLER(xtensa_cmd_perfmon_dump_do,
 732                 target_to_xtensa(target));
 733 }
 734
 735 COMMAND_HANDLER(esp_xtensa_smp_cmd_tracestart)
 736 {
 737         struct target *target = get_current_target(CMD_CTX);
 738         if (target->smp) {
 739                 struct target_list *head;
 740                 struct target *curr;
 741                 foreach_smp_target(head, target->smp_targets) {
 742                         curr = head->target;
 743                         int ret = CALL_COMMAND_HANDLER(xtensa_cmd_tracestart_do,
 744                                 target_to_xtensa(curr));
 745                         if (ret != ERROR_OK)
 746                                 return ret;
 747                 }
 748                 return ERROR_OK;
 749         }
 750         return CALL_COMMAND_HANDLER(xtensa_cmd_tracestart_do,
 751                 target_to_xtensa(target));
 752 }
 753
 754 COMMAND_HANDLER(esp_xtensa_smp_cmd_tracestop)
 755 {
 756         struct target *target = get_current_target(CMD_CTX);
 757         if (target->smp) {
 758                 struct target_list *head;
 759                 struct target *curr;
 760                 foreach_smp_target(head, target->smp_targets) {
 761                         curr = head->target;
 762                         int ret = CALL_COMMAND_HANDLER(xtensa_cmd_tracestop_do,
 763                                 target_to_xtensa(curr));
 764                         if (ret != ERROR_OK)
 765                                 return ret;
 766                 }
 767                 return ERROR_OK;
 768         }
 769         return CALL_COMMAND_HANDLER(xtensa_cmd_tracestop_do,
 770                 target_to_xtensa(target));
 771 }
 772
 773 COMMAND_HANDLER(esp_xtensa_smp_cmd_tracedump)
 774 {
 775         struct target *target = get_current_target(CMD_CTX);
 776         if (target->smp) {
 777                 struct target_list *head;
 778                 struct target *curr;
 779                 int32_t cores_max_id = 0;
 780                 /* assume that core IDs are assigned to SMP targets sequentially: 0,1,2... */
 781                 foreach_smp_target(head, target->smp_targets) {
 782                         curr = head->target;
 783                         if (cores_max_id < curr->coreid)
 784                                 cores_max_id = curr->coreid;
 785                 }
 786                 if (CMD_ARGC < ((uint32_t)cores_max_id + 1)) {
 787                         command_print(CMD,
 788                                 "Need %d filenames to dump to as output!",
 789                                 cores_max_id + 1);
 790                         return ERROR_FAIL;
 791                 }
 792                 foreach_smp_target(head, target->smp_targets) {
 793                         curr = head->target;
 794                         int ret = CALL_COMMAND_HANDLER(xtensa_cmd_tracedump_do,
 795                                 target_to_xtensa(curr), CMD_ARGV[curr->coreid]);
 796                         if (ret != ERROR_OK)
 797                                 return ret;
 798                 }
 799                 return ERROR_OK;
 800         }
 801         return CALL_COMMAND_HANDLER(xtensa_cmd_tracedump_do,
 802                 target_to_xtensa(target), CMD_ARGV[0]);
 803 }
 804
 805 const struct command_registration esp_xtensa_smp_xtensa_command_handlers[] = {
 806         {
 807                 .name = "xtdef",
 808                 .handler = esp_xtensa_smp_cmd_xtdef,
 809                 .mode = COMMAND_CONFIG,
 810                 .help = "Configure Xtensa core type",
 811                 .usage = "<type>",
 812         },
 813         {
 814                 .name = "xtopt",
 815                 .handler = esp_xtensa_smp_cmd_xtopt,
 816                 .mode = COMMAND_CONFIG,
 817                 .help = "Configure Xtensa core option",
 818                 .usage = "<name> <value>",
 819         },
 820         {
 821                 .name = "xtmem",
 822                 .handler = esp_xtensa_smp_cmd_xtmem,
 823                 .mode = COMMAND_CONFIG,
 824                 .help = "Configure Xtensa memory/cache option",
 825                 .usage = "<type> [parameters]",
 826         },
 827         {
 828                 .name = "xtmmu",
 829                 .handler = esp_xtensa_smp_cmd_xtmmu,
 830                 .mode = COMMAND_CONFIG,
 831                 .help = "Configure Xtensa MMU option",
 832                 .usage = "<NIREFILLENTRIES> <NDREFILLENTRIES> <IVARWAY56> <DVARWAY56>",
 833         },
 834         {
 835                 .name = "xtmpu",
 836                 .handler = esp_xtensa_smp_cmd_xtmpu,
 837                 .mode = COMMAND_CONFIG,
 838                 .help = "Configure Xtensa MPU option",
 839                 .usage = "<num FG seg> <min seg size> <lockable> <executeonly>",
 840         },
 841         {
 842                 .name = "xtreg",
 843                 .handler = esp_xtensa_smp_cmd_xtreg,
 844                 .mode = COMMAND_CONFIG,
 845                 .help = "Configure Xtensa register",
 846                 .usage = "<regname> <regnum>",
 847         },
 848         {
 849                 .name = "xtregs",
 850                 .handler = esp_xtensa_smp_cmd_xtreg,
 851                 .mode = COMMAND_CONFIG,
 852                 .help = "Configure number of Xtensa registers",
 853                 .usage = "<numregs>",
 854         },
 855         {
 856                 .name = "xtregfmt",
 857                 .handler = esp_xtensa_smp_cmd_xtregfmt,
 858                 .mode = COMMAND_CONFIG,
 859                 .help = "Configure format of Xtensa register map",
 860                 .usage = "<numgregs>",
 861         },
 862         {
 863                 .name = "set_permissive",
 864                 .handler = esp_xtensa_smp_cmd_permissive_mode,
 865                 .mode = COMMAND_ANY,
 866                 .help = "When set to 1, enable Xtensa permissive mode (less client-side checks)",
 867                 .usage = "[0|1]",
 868         },
 869         {
 870                 .name = "maskisr",
 871                 .handler = esp_xtensa_smp_cmd_mask_interrupts,
 872                 .mode = COMMAND_ANY,
 873                 .help = "mask Xtensa interrupts at step",
 874                 .usage = "['on'|'off']",
 875         },
 876         {
 877                 .name = "smpbreak",
 878                 .handler = esp_xtensa_smp_cmd_smpbreak,
 879                 .mode = COMMAND_ANY,
 880                 .help = "Set the way the CPU chains OCD breaks",
 881                 .usage =
 882                         "[none|breakinout|runstall] | [BreakIn] [BreakOut] [RunStallIn] [DebugModeOut]",
 883         },
 884         {
 885                 .name = "perfmon_enable",
 886                 .handler = esp_xtensa_smp_cmd_perfmon_enable,
 887                 .mode = COMMAND_EXEC,
 888                 .help = "Enable and start performance counter",
 889                 .usage = "<counter_id> <select> [mask] [kernelcnt] [tracelevel]",
 890         },
 891         {
 892                 .name = "perfmon_dump",
 893                 .handler = esp_xtensa_smp_cmd_perfmon_dump,
 894                 .mode = COMMAND_EXEC,
 895                 .help =
 896                         "Dump performance counter value. If no argument specified, dumps all counters.",
 897                 .usage = "[counter_id]",
 898         },
 899         {
 900                 .name = "tracestart",
 901                 .handler = esp_xtensa_smp_cmd_tracestart,
 902                 .mode = COMMAND_EXEC,
 903                 .help =
 904                         "Tracing: Set up and start a trace. Optionally set stop trigger address and amount of data captured after.",
 905                 .usage = "[pc <pcval>/[maskbitcount]] [after <n> [ins|words]]",
 906         },
 907         {
 908                 .name = "tracestop",
 909                 .handler = esp_xtensa_smp_cmd_tracestop,
 910                 .mode = COMMAND_EXEC,
 911                 .help = "Tracing: Stop current trace as started by the tracestart command",
 912                 .usage = "",
 913         },
 914         {
 915                 .name = "tracedump",
 916                 .handler = esp_xtensa_smp_cmd_tracedump,
 917                 .mode = COMMAND_EXEC,
 918                 .help = "Tracing: Dump trace memory to a files. One file per core.",
 919                 .usage = "<outfile1> <outfile2>",
 920         },
 921         COMMAND_REGISTRATION_DONE
 922 };
 923
 924 const struct command_registration esp_xtensa_smp_command_handlers[] = {
 925         {
 926                 .name = "xtensa",
 927                 .usage = "",
 928                 .chain = esp_xtensa_smp_xtensa_command_handlers,
 929         },
 930         COMMAND_REGISTRATION_DONE
 931 };