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