src/target/espressif/esp_algorithm.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2
   3 /***************************************************************************
   4  *   Espressif chips common algorithm API for OpenOCD                      *
   5  *   Copyright (C) 2022 Espressif Systems Ltd.                             *
   6  ***************************************************************************/
   7
   8 #ifdef HAVE_CONFIG_H
   9 #include "config.h"
  10 #endif
  11
  12 #include <helper/align.h>
  13 #include <target/algorithm.h>
  14 #include <target/target.h>
  15 #include "esp_algorithm.h"
  16
  17 #define DEFAULT_ALGORITHM_TIMEOUT_MS    40000   /* ms */
  18
  19 static int esp_algorithm_read_stub_logs(struct target *target, struct esp_algorithm_stub *stub)
  20 {
  21         if (!stub || stub->log_buff_addr == 0 || stub->log_buff_size == 0)
  22                 return ERROR_FAIL;
  23
  24         uint32_t len = 0;
  25         int retval = target_read_u32(target, stub->log_buff_addr, &len);
  26         if (retval != ERROR_OK)
  27                 return retval;
  28
  29         /* sanity check. log_buff_size = sizeof(len) + sizeof(log_buff) */
  30         if (len == 0 || len > stub->log_buff_size - 4)
  31                 return ERROR_FAIL;
  32
  33         uint8_t *log_buff = calloc(1, len);
  34         if (!log_buff) {
  35                 LOG_ERROR("Failed to allocate memory for the stub log!");
  36                 return ERROR_FAIL;
  37         }
  38         retval = target_read_memory(target, stub->log_buff_addr + 4, 1, len, log_buff);
  39         if (retval == ERROR_OK)
  40                 LOG_OUTPUT("%*.*s", len, len, log_buff);
  41         free(log_buff);
  42         return retval;
  43 }
  44
  45 static int esp_algorithm_run_image(struct target *target,
  46         struct esp_algorithm_run_data *run,
  47         uint32_t num_args,
  48         va_list ap)
  49 {
  50         struct working_area **mem_handles = NULL;
  51
  52         if (!run || !run->hw)
  53                 return ERROR_FAIL;
  54
  55         int retval = run->hw->algo_init(target, run, num_args, ap);
  56         if (retval != ERROR_OK)
  57                 return retval;
  58
  59         /* allocate memory arguments and fill respective reg params */
  60         if (run->mem_args.count > 0) {
  61                 mem_handles = calloc(run->mem_args.count, sizeof(*mem_handles));
  62                 if (!mem_handles) {
  63                         LOG_ERROR("Failed to alloc target mem handles!");
  64                         retval = ERROR_FAIL;
  65                         goto _cleanup;
  66                 }
  67                 /* alloc memory args target buffers */
  68                 for (uint32_t i = 0; i < run->mem_args.count; i++) {
  69                         /* small hack: if we need to update some reg param this field holds
  70                          * appropriate user argument number, */
  71                         /* otherwise should hold UINT_MAX */
  72                         uint32_t usr_param_num = run->mem_args.params[i].address;
  73                         static struct working_area *area;
  74                         retval = target_alloc_working_area(target, run->mem_args.params[i].size, &area);
  75                         if (retval != ERROR_OK) {
  76                                 LOG_ERROR("Failed to alloc target buffer!");
  77                                 retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
  78                                 goto _cleanup;
  79                         }
  80                         mem_handles[i] = area;
  81                         run->mem_args.params[i].address = area->address;
  82                         if (usr_param_num != UINT_MAX) /* if we need update some register param with mem param value */
  83                                 esp_algorithm_user_arg_set_uint(run, usr_param_num, run->mem_args.params[i].address);
  84                 }
  85         }
  86
  87         if (run->usr_func_init) {
  88                 retval = run->usr_func_init(target, run, run->usr_func_arg);
  89                 if (retval != ERROR_OK) {
  90                         LOG_ERROR("Failed to prepare algorithm host side args stub (%d)!", retval);
  91                         goto _cleanup;
  92                 }
  93         }
  94
  95         LOG_DEBUG("Algorithm start @ " TARGET_ADDR_FMT ", stack %d bytes @ " TARGET_ADDR_FMT,
  96                 run->stub.tramp_mapped_addr, run->stack_size, run->stub.stack_addr);
  97         retval = target_start_algorithm(target,
  98                 run->mem_args.count, run->mem_args.params,
  99                 run->reg_args.count, run->reg_args.params,
 100                 run->stub.tramp_mapped_addr, 0,
 101                 run->stub.ainfo);
 102         if (retval != ERROR_OK) {
 103                 LOG_ERROR("Failed to start algorithm (%d)!", retval);
 104                 goto _cleanup;
 105         }
 106
 107         if (run->usr_func) {
 108                 /* give target algorithm stub time to init itself, then user func can communicate to it safely */
 109                 alive_sleep(100);
 110                 retval = run->usr_func(target, run->usr_func_arg);
 111                 if (retval != ERROR_OK)
 112                         LOG_ERROR("Failed to exec algorithm user func (%d)!", retval);
 113         }
 114         uint32_t timeout_ms = 0;        /* do not wait if 'usr_func' returned error */
 115         if (retval == ERROR_OK)
 116                 timeout_ms = run->timeout_ms ? run->timeout_ms : DEFAULT_ALGORITHM_TIMEOUT_MS;
 117         LOG_DEBUG("Wait algorithm completion");
 118         retval = target_wait_algorithm(target,
 119                 run->mem_args.count, run->mem_args.params,
 120                 run->reg_args.count, run->reg_args.params,
 121                 0, timeout_ms,
 122                 run->stub.ainfo);
 123         if (retval != ERROR_OK) {
 124                 LOG_ERROR("Failed to wait algorithm (%d)!", retval);
 125                 /* target has been forced to stop in target_wait_algorithm() */
 126         }
 127         esp_algorithm_read_stub_logs(target, &run->stub);
 128
 129         if (run->usr_func_done)
 130                 run->usr_func_done(target, run, run->usr_func_arg);
 131
 132         if (retval != ERROR_OK) {
 133                 LOG_ERROR("Algorithm run failed (%d)!", retval);
 134         } else {
 135                 run->ret_code = esp_algorithm_user_arg_get_uint(run, 0);
 136                 LOG_DEBUG("Got algorithm RC 0x%" PRIx32, run->ret_code);
 137         }
 138
 139 _cleanup:
 140         /* free memory arguments */
 141         if (mem_handles) {
 142                 for (uint32_t i = 0; i < run->mem_args.count; i++) {
 143                         if (mem_handles[i])
 144                                 target_free_working_area(target, mem_handles[i]);
 145                 }
 146                 free(mem_handles);
 147         }
 148         run->hw->algo_cleanup(target, run);
 149
 150         return retval;
 151 }
 152
 153 static int esp_algorithm_run_debug_stub(struct target *target,
 154         struct esp_algorithm_run_data *run,
 155         uint32_t num_args,
 156         va_list ap)
 157 {
 158         if (!run || !run->hw)
 159                 return ERROR_FAIL;
 160
 161         int retval = run->hw->algo_init(target, run, num_args, ap);
 162         if (retval != ERROR_OK)
 163                 return retval;
 164
 165         LOG_DEBUG("Algorithm start @ " TARGET_ADDR_FMT ", stack %d bytes @ " TARGET_ADDR_FMT,
 166                 run->stub.tramp_mapped_addr, run->stack_size, run->stub.stack_addr);
 167         retval = target_start_algorithm(target,
 168                 run->mem_args.count, run->mem_args.params,
 169                 run->reg_args.count, run->reg_args.params,
 170                 run->stub.tramp_mapped_addr, 0,
 171                 run->stub.ainfo);
 172         if (retval != ERROR_OK) {
 173                 LOG_ERROR("Failed to start algorithm (%d)!", retval);
 174                 goto _cleanup;
 175         }
 176
 177         uint32_t timeout_ms = 0;        /* do not wait if 'usr_func' returned error */
 178         if (retval == ERROR_OK)
 179                 timeout_ms = run->timeout_ms ? run->timeout_ms : DEFAULT_ALGORITHM_TIMEOUT_MS;
 180         LOG_DEBUG("Wait algorithm completion");
 181         retval = target_wait_algorithm(target,
 182                 run->mem_args.count, run->mem_args.params,
 183                 run->reg_args.count, run->reg_args.params,
 184                 0, timeout_ms,
 185                 run->stub.ainfo);
 186         if (retval != ERROR_OK) {
 187                 LOG_ERROR("Failed to wait algorithm (%d)!", retval);
 188                 /* target has been forced to stop in target_wait_algorithm() */
 189         }
 190
 191         if (retval != ERROR_OK) {
 192                 LOG_ERROR("Algorithm run failed (%d)!", retval);
 193         } else {
 194                 run->ret_code = esp_algorithm_user_arg_get_uint(run, 0);
 195                 LOG_DEBUG("Got algorithm RC 0x%" PRIx32, run->ret_code);
 196         }
 197
 198 _cleanup:
 199         run->hw->algo_cleanup(target, run);
 200
 201         return retval;
 202 }
 203
 204 static void reverse_binary(const uint8_t *src, uint8_t *dest, size_t length)
 205 {
 206         size_t remaining = length % 4;
 207         size_t offset = 0;
 208         size_t aligned_len = ALIGN_UP(length, 4);
 209
 210         if (remaining > 0) {
 211                 /* Put extra bytes to the beginning with padding */
 212                 memset(dest + remaining, 0xFF, 4 - remaining);
 213                 for (size_t i = 0; i < remaining; i++)
 214                         dest[i] = src[length - remaining + i];
 215                 length -= remaining; /* reverse the others */
 216                 offset = 4;
 217         }
 218
 219         for (size_t i = offset; i < aligned_len; i += 4) {
 220                 dest[i + 0] = src[length - i + offset - 4];
 221                 dest[i + 1] = src[length - i + offset - 3];
 222                 dest[i + 2] = src[length - i + offset - 2];
 223                 dest[i + 3] = src[length - i + offset - 1];
 224         }
 225 }
 226
 227 static int load_section_from_image(struct target *target,
 228         struct esp_algorithm_run_data *run,
 229         int section_num,
 230         bool reverse)
 231 {
 232         if (!run)
 233                 return ERROR_FAIL;
 234
 235         struct imagesection *section = &run->image.image.sections[section_num];
 236         uint32_t sec_wr = 0;
 237         uint8_t buf[1024];
 238
 239         assert(sizeof(buf) % 4 == 0);
 240
 241         while (sec_wr < section->size) {
 242                 uint32_t nb = section->size - sec_wr > sizeof(buf) ? sizeof(buf) : section->size - sec_wr;
 243                 size_t size_read = 0;
 244                 int retval = image_read_section(&run->image.image, section_num, sec_wr, nb, buf, &size_read);
 245                 if (retval != ERROR_OK) {
 246                         LOG_ERROR("Failed to read stub section (%d)!", retval);
 247                         return retval;
 248                 }
 249
 250                 if (reverse) {
 251                         size_t aligned_len = ALIGN_UP(size_read, 4);
 252                         uint8_t reversed_buf[aligned_len];
 253
 254                         /* Send original size to allow padding */
 255                         reverse_binary(buf, reversed_buf, size_read);
 256
 257                         /*
 258                                 The address range accessed via the instruction bus is in reverse order (word-wise) compared to access
 259                                 via the data bus. That is to say, address
 260                                 0x3FFE_0000 and 0x400B_FFFC access the same word
 261                                 0x3FFE_0004 and 0x400B_FFF8 access the same word
 262                                 0x3FFE_0008 and 0x400B_FFF4 access the same word
 263                                 ...
 264                                 The data bus and instruction bus of the CPU are still both little-endian,
 265                                 so the byte order of individual words is not reversed between address spaces.
 266                                 For example, address
 267                                 0x3FFE_0000 accesses the least significant byte in the word accessed by 0x400B_FFFC.
 268                                 0x3FFE_0001 accesses the second least significant byte in the word accessed by 0x400B_FFFC.
 269                                 0x3FFE_0002 accesses the second most significant byte in the word accessed by 0x400B_FFFC.
 270                                 For more details, please refer to ESP32 TRM, Internal SRAM1 section.
 271                         */
 272                         retval = target_write_buffer(target, run->image.dram_org - sec_wr - aligned_len, aligned_len, reversed_buf);
 273                         if (retval != ERROR_OK) {
 274                                 LOG_ERROR("Failed to write stub section!");
 275                                 return retval;
 276                         }
 277                 } else {
 278                         retval = target_write_buffer(target, section->base_address + sec_wr, size_read, buf);
 279                         if (retval != ERROR_OK) {
 280                                 LOG_ERROR("Failed to write stub section!");
 281                                 return retval;
 282                         }
 283                 }
 284
 285                 sec_wr += size_read;
 286         }
 287
 288         return ERROR_OK;
 289 }
 290
 291 /*
 292  * Configuration:
 293  * ----------------------------
 294  * The linker scripts defines the memory layout for the stub code.
 295  * The OpenOCD script specifies the workarea address and it's size
 296  * Sections defined in the linker are organized to share the same addresses with the workarea.
 297  * code and data sections are located in Internal SRAM1 and OpenOCD fills these sections using the data bus.
 298  */
 299 int esp_algorithm_load_func_image(struct target *target, struct esp_algorithm_run_data *run)
 300 {
 301         int retval;
 302         size_t tramp_sz = 0;
 303         const uint8_t *tramp = NULL;
 304         struct duration algo_time;
 305         bool alloc_code_working_area = true;
 306
 307         if (!run || !run->hw)
 308                 return ERROR_FAIL;
 309
 310         if (duration_start(&algo_time) != 0) {
 311                 LOG_ERROR("Failed to start algo time measurement!");
 312                 return ERROR_FAIL;
 313         }
 314
 315         if (run->hw->stub_tramp_get) {
 316                 tramp = run->hw->stub_tramp_get(target, &tramp_sz);
 317                 if (!tramp)
 318                         return ERROR_FAIL;
 319         }
 320
 321         LOG_DEBUG("stub: base 0x%x, start 0x%" PRIx32 ", %d sections",
 322                 run->image.image.base_address_set ? (unsigned int)run->image.image.base_address : 0,
 323                 run->image.image.start_address,
 324                 run->image.image.num_sections);
 325         run->stub.entry = run->image.image.start_address;
 326
 327         /* [code + trampoline] + <padding> + [data] */
 328
 329         /* ESP32 has reversed memory region. It will use the last part of DRAM, the others will use the first part.
 330          * To avoid complexity for the backup/restore process, we will allocate a workarea for all IRAM region from
 331          * the beginning. In that case no need to have a padding area.
 332          */
 333         if (run->image.reverse) {
 334                 if (target_alloc_working_area(target, run->image.iram_len, &run->stub.code) != ERROR_OK) {
 335                         LOG_ERROR("no working area available, can't alloc space for stub code!");
 336                         retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 337                         goto _on_error;
 338                 }
 339                 alloc_code_working_area = false;
 340         }
 341
 342         uint32_t code_size = 0;
 343
 344         /* Load code section */
 345         for (unsigned int i = 0; i < run->image.image.num_sections; i++) {
 346                 struct imagesection *section = &run->image.image.sections[i];
 347
 348                 if (section->size == 0)
 349                         continue;
 350
 351                 if (section->flags & ESP_IMAGE_ELF_PHF_EXEC) {
 352                         LOG_DEBUG("addr " TARGET_ADDR_FMT ", sz %d, flags %" PRIx64,
 353                                 section->base_address, section->size, section->flags);
 354
 355                         if (alloc_code_working_area) {
 356                                 retval = target_alloc_working_area(target, section->size, &run->stub.code);
 357                                 if (retval != ERROR_OK) {
 358                                         LOG_ERROR("no working area available, can't alloc space for stub code!");
 359                                         retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 360                                         goto _on_error;
 361                                 }
 362                         }
 363
 364                         if (section->base_address == 0) {
 365                                 section->base_address = run->stub.code->address;
 366                                 /* sanity check, stub is compiled to be run from working area */
 367                         } else if (run->stub.code->address != section->base_address) {
 368                                 LOG_ERROR("working area " TARGET_ADDR_FMT " and stub code section " TARGET_ADDR_FMT
 369                                         " address mismatch!",
 370                                         section->base_address,
 371                                         run->stub.code->address);
 372                                 retval = ERROR_FAIL;
 373                                 goto _on_error;
 374                         }
 375
 376                         retval = load_section_from_image(target, run, i, run->image.reverse);
 377                         if (retval != ERROR_OK)
 378                                 goto _on_error;
 379
 380                         code_size += ALIGN_UP(section->size, 4);
 381                         break; /* Stub has one executable text section */
 382                 }
 383         }
 384
 385         /* If exists, load trampoline to the code area */
 386         if (tramp) {
 387                 if (run->stub.tramp_addr == 0) {
 388                         if (alloc_code_working_area) {
 389                                 /* alloc trampoline in code working area */
 390                                 if (target_alloc_working_area(target, tramp_sz, &run->stub.tramp) != ERROR_OK) {
 391                                         LOG_ERROR("no working area available, can't alloc space for stub jumper!");
 392                                         retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 393                                         goto _on_error;
 394                                 }
 395                                 run->stub.tramp_addr = run->stub.tramp->address;
 396                         }
 397                 }
 398
 399                 size_t al_tramp_size = ALIGN_UP(tramp_sz, 4);
 400
 401                 if (run->image.reverse) {
 402                         target_addr_t reversed_tramp_addr = run->image.dram_org - code_size;
 403                         uint8_t reversed_tramp[al_tramp_size];
 404
 405                         /* Send original size to allow padding */
 406                         reverse_binary(tramp, reversed_tramp, tramp_sz);
 407                         run->stub.tramp_addr = reversed_tramp_addr - al_tramp_size;
 408                         LOG_DEBUG("Write reversed tramp to addr " TARGET_ADDR_FMT ", sz %zu", run->stub.tramp_addr, al_tramp_size);
 409                         retval = target_write_buffer(target, run->stub.tramp_addr, al_tramp_size, reversed_tramp);
 410                 } else {
 411                         LOG_DEBUG("Write tramp to addr " TARGET_ADDR_FMT ", sz %zu", run->stub.tramp_addr, tramp_sz);
 412                         retval = target_write_buffer(target, run->stub.tramp_addr, tramp_sz, tramp);
 413                 }
 414
 415                 if (retval != ERROR_OK) {
 416                         LOG_ERROR("Failed to write stub jumper!");
 417                         goto _on_error;
 418                 }
 419
 420                 run->stub.tramp_mapped_addr = run->image.iram_org + code_size;
 421                 code_size += al_tramp_size;
 422                 LOG_DEBUG("Tramp mapped to addr " TARGET_ADDR_FMT, run->stub.tramp_mapped_addr);
 423         }
 424
 425         /* allocate dummy space until the data address */
 426         if (alloc_code_working_area) {
 427                 /* we dont need to restore padding area. */
 428                 uint32_t backup_working_area_prev = target->backup_working_area;
 429                 target->backup_working_area = 0;
 430                 if (target_alloc_working_area(target, run->image.iram_len - code_size, &run->stub.padding) != ERROR_OK) {
 431                         LOG_ERROR("no working area available, can't alloc space for stub code!");
 432                         retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 433                         goto _on_error;
 434                 }
 435                 target->backup_working_area = backup_working_area_prev;
 436         }
 437
 438         /*  Load the data section */
 439         for (unsigned int i = 0; i < run->image.image.num_sections; i++) {
 440                 struct imagesection *section = &run->image.image.sections[i];
 441
 442                 if (section->size == 0)
 443                         continue;
 444
 445                 if (!(section->flags & ESP_IMAGE_ELF_PHF_EXEC)) {
 446                         LOG_DEBUG("addr " TARGET_ADDR_FMT ", sz %d, flags %" PRIx64, section->base_address, section->size,
 447                                 section->flags);
 448                         /* target_alloc_working_area() aligns the whole working area size to 4-byte boundary.
 449                            We alloc one area for both DATA and BSS, so align each of them ourselves. */
 450                         uint32_t data_sec_sz = ALIGN_UP(section->size, 4);
 451                         LOG_DEBUG("DATA sec size %" PRIu32 " -> %" PRIu32, section->size, data_sec_sz);
 452                         uint32_t bss_sec_sz = ALIGN_UP(run->image.bss_size, 4);
 453                         LOG_DEBUG("BSS sec size %" PRIu32 " -> %" PRIu32, run->image.bss_size, bss_sec_sz);
 454                         if (target_alloc_working_area(target, data_sec_sz + bss_sec_sz, &run->stub.data) != ERROR_OK) {
 455                                 LOG_ERROR("no working area available, can't alloc space for stub data!");
 456                                 retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 457                                 goto _on_error;
 458                         }
 459                         if (section->base_address == 0) {
 460                                 section->base_address = run->stub.data->address;
 461                                 /* sanity check, stub is compiled to be run from working area */
 462                         } else if (run->stub.data->address != section->base_address) {
 463                                 LOG_ERROR("working area " TARGET_ADDR_FMT
 464                                         " and stub data section " TARGET_ADDR_FMT
 465                                         " address mismatch!",
 466                                         section->base_address,
 467                                         run->stub.data->address);
 468                                 retval = ERROR_FAIL;
 469                                 goto _on_error;
 470                         }
 471
 472                         retval = load_section_from_image(target, run, i, false);
 473                         if (retval != ERROR_OK)
 474                                 goto _on_error;
 475                 }
 476         }
 477
 478         /* stack */
 479         if (run->stub.stack_addr == 0 && run->stack_size > 0) {
 480                 /* allocate stack in data working area */
 481                 if (target_alloc_working_area(target, run->stack_size, &run->stub.stack) != ERROR_OK) {
 482                         LOG_ERROR("no working area available, can't alloc stub stack!");
 483                         retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 484                         goto _on_error;
 485                 }
 486                 run->stub.stack_addr = run->stub.stack->address + run->stack_size;
 487         }
 488
 489         if (duration_measure(&algo_time) != 0) {
 490                 LOG_ERROR("Failed to stop algo run measurement!");
 491                 retval = ERROR_FAIL;
 492                 goto _on_error;
 493         }
 494         LOG_DEBUG("Stub loaded in %g ms", duration_elapsed(&algo_time) * 1000);
 495         return ERROR_OK;
 496
 497 _on_error:
 498         esp_algorithm_unload_func_image(target, run);
 499         return retval;
 500 }
 501
 502 int esp_algorithm_unload_func_image(struct target *target, struct esp_algorithm_run_data *run)
 503 {
 504         if (!run)
 505                 return ERROR_FAIL;
 506
 507         target_free_all_working_areas(target);
 508
 509         run->stub.tramp = NULL;
 510         run->stub.stack = NULL;
 511         run->stub.code = NULL;
 512         run->stub.data = NULL;
 513         run->stub.padding = NULL;
 514
 515         return ERROR_OK;
 516 }
 517
 518 int esp_algorithm_exec_func_image_va(struct target *target,
 519         struct esp_algorithm_run_data *run,
 520         uint32_t num_args,
 521         va_list ap)
 522 {
 523         if (!run || !run->image.image.start_address_set || run->image.image.start_address == 0)
 524                 return ERROR_FAIL;
 525
 526         return esp_algorithm_run_image(target, run, num_args, ap);
 527 }
 528
 529 int esp_algorithm_load_onboard_func(struct target *target, target_addr_t func_addr, struct esp_algorithm_run_data *run)
 530 {
 531         int res;
 532         const uint8_t *tramp = NULL;
 533         size_t tramp_sz = 0;
 534         struct duration algo_time;
 535
 536         if (!run || !run->hw)
 537                 return ERROR_FAIL;
 538
 539         if (duration_start(&algo_time) != 0) {
 540                 LOG_ERROR("Failed to start algo time measurement!");
 541                 return ERROR_FAIL;
 542         }
 543
 544         if (run->hw->stub_tramp_get) {
 545                 tramp = run->hw->stub_tramp_get(target, &tramp_sz);
 546                 if (!tramp)
 547                         return ERROR_FAIL;
 548         }
 549
 550         if (tramp_sz > run->on_board.code_buf_size) {
 551                 LOG_ERROR("Stub tramp size %zu bytes exceeds target buf size %d bytes!",
 552                         tramp_sz, run->on_board.code_buf_size);
 553                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 554         }
 555
 556         if (run->stack_size > run->on_board.min_stack_size) {
 557                 LOG_ERROR("Algorithm stack size not fit into the allocated target stack!");
 558                 return ERROR_FAIL;
 559         }
 560
 561         run->stub.stack_addr = run->on_board.min_stack_addr + run->stack_size;
 562         run->stub.tramp_addr = run->on_board.code_buf_addr;
 563         run->stub.tramp_mapped_addr = run->stub.tramp_addr;
 564         run->stub.entry = func_addr;
 565
 566         if (tramp) {
 567                 res = target_write_buffer(target, run->stub.tramp_addr, tramp_sz, tramp);
 568                 if (res != ERROR_OK) {
 569                         LOG_ERROR("Failed to write stub jumper!");
 570                         esp_algorithm_unload_onboard_func(target, run);
 571                         return res;
 572                 }
 573         }
 574
 575         if (duration_measure(&algo_time) != 0) {
 576                 LOG_ERROR("Failed to stop algo run measurement!");
 577                 return ERROR_FAIL;
 578         }
 579         LOG_DEBUG("Stub loaded in %g ms", duration_elapsed(&algo_time) * 1000);
 580
 581         return ERROR_OK;
 582 }
 583
 584 int esp_algorithm_unload_onboard_func(struct target *target, struct esp_algorithm_run_data *run)
 585 {
 586         return ERROR_OK;
 587 }
 588
 589 int esp_algorithm_exec_onboard_func_va(struct target *target,
 590         struct esp_algorithm_run_data *run,
 591         uint32_t num_args,
 592         va_list ap)
 593 {
 594         return esp_algorithm_run_debug_stub(target, run, num_args, ap);
 595 }