git.gag.com Git - fw/stlink/blob - src/stlink-common.c

   1
   2
   3 #include <stdarg.h>
   4 #include <stdio.h>
   5 #include <stdlib.h>
   6 #include <string.h>
   7
   8 #include <unistd.h>
   9 #include <fcntl.h>
  10 #include <sys/types.h>
  11 #include <sys/stat.h>
  12 #include <sys/mman.h>
  13
  14
  15 #include "stlink-common.h"
  16
  17 void D(stlink_t *sl, char *txt) {
  18     if (sl->verbose > 1)
  19         fputs(txt, stderr);
  20 }
  21
  22 void DD(stlink_t *sl, char *format, ...) {
  23     if (sl->verbose > 0) {
  24         va_list list;
  25         va_start(list, format);
  26         vfprintf(stderr, format, list);
  27         va_end(list);
  28     }
  29 }
  30
  31
  32 /* todo: stm32l15xxx flash memory, pm0062 manual */
  33
  34 /* stm32f FPEC flash controller interface, pm0063 manual */
  35
  36 #define FLASH_REGS_ADDR 0x40022000
  37 #define FLASH_REGS_SIZE 0x28
  38
  39 #define FLASH_ACR (FLASH_REGS_ADDR + 0x00)
  40 #define FLASH_KEYR (FLASH_REGS_ADDR + 0x04)
  41 #define FLASH_SR (FLASH_REGS_ADDR + 0x0c)
  42 #define FLASH_CR (FLASH_REGS_ADDR + 0x10)
  43 #define FLASH_AR (FLASH_REGS_ADDR + 0x14)
  44 #define FLASH_OBR (FLASH_REGS_ADDR + 0x1c)
  45 #define FLASH_WRPR (FLASH_REGS_ADDR + 0x20)
  46
  47 #define FLASH_RDPTR_KEY 0x00a5
  48 #define FLASH_KEY1 0x45670123
  49 #define FLASH_KEY2 0xcdef89ab
  50
  51 #define FLASH_SR_BSY 0
  52 #define FLASH_SR_EOP 5
  53
  54 #define FLASH_CR_PG 0
  55 #define FLASH_CR_PER 1
  56 #define FLASH_CR_MER 2
  57 #define FLASH_CR_STRT 6
  58 #define FLASH_CR_LOCK 7
  59
  60 void write_uint32(unsigned char* buf, uint32_t ui) {
  61     if (!is_bigendian()) { // le -> le (don't swap)
  62         buf[0] = ((unsigned char*) &ui)[0];
  63         buf[1] = ((unsigned char*) &ui)[1];
  64         buf[2] = ((unsigned char*) &ui)[2];
  65         buf[3] = ((unsigned char*) &ui)[3];
  66     } else {
  67         buf[0] = ((unsigned char*) &ui)[3];
  68         buf[1] = ((unsigned char*) &ui)[2];
  69         buf[2] = ((unsigned char*) &ui)[1];
  70         buf[3] = ((unsigned char*) &ui)[0];
  71     }
  72 }
  73
  74 void write_uint16(unsigned char* buf, uint16_t ui) {
  75     if (!is_bigendian()) { // le -> le (don't swap)
  76         buf[0] = ((unsigned char*) &ui)[0];
  77         buf[1] = ((unsigned char*) &ui)[1];
  78     } else {
  79         buf[0] = ((unsigned char*) &ui)[1];
  80         buf[1] = ((unsigned char*) &ui)[0];
  81     }
  82 }
  83
  84 uint32_t read_uint32(const unsigned char *c, const int pt) {
  85     uint32_t ui;
  86     char *p = (char *) &ui;
  87
  88     if (!is_bigendian()) { // le -> le (don't swap)
  89         p[0] = c[pt + 0];
  90         p[1] = c[pt + 1];
  91         p[2] = c[pt + 2];
  92         p[3] = c[pt + 3];
  93     } else {
  94         p[0] = c[pt + 3];
  95         p[1] = c[pt + 2];
  96         p[2] = c[pt + 1];
  97         p[3] = c[pt + 0];
  98     }
  99     return ui;
 100 }
 101
 102 static uint32_t __attribute__((unused)) read_flash_rdp(stlink_t *sl) {
 103     stlink_read_mem32(sl, FLASH_WRPR, sizeof (uint32_t));
 104     return (*(uint32_t*) sl->q_buf) & 0xff;
 105 }
 106
 107 static inline uint32_t read_flash_wrpr(stlink_t *sl) {
 108     stlink_read_mem32(sl, FLASH_WRPR, sizeof (uint32_t));
 109     return *(uint32_t*) sl->q_buf;
 110 }
 111
 112 static inline uint32_t read_flash_obr(stlink_t *sl) {
 113     stlink_read_mem32(sl, FLASH_OBR, sizeof (uint32_t));
 114     return *(uint32_t*) sl->q_buf;
 115 }
 116
 117 static inline uint32_t read_flash_cr(stlink_t *sl) {
 118     stlink_read_mem32(sl, FLASH_CR, sizeof (uint32_t));
 119     return *(uint32_t*) sl->q_buf;
 120 }
 121
 122 static inline unsigned int is_flash_locked(stlink_t *sl) {
 123     /* return non zero for true */
 124     return read_flash_cr(sl) & (1 << FLASH_CR_LOCK);
 125 }
 126
 127 static void unlock_flash(stlink_t *sl) {
 128     /* the unlock sequence consists of 2 write cycles where
 129        2 key values are written to the FLASH_KEYR register.
 130        an invalid sequence results in a definitive lock of
 131        the FPEC block until next reset.
 132      */
 133
 134     write_uint32(sl->q_buf, FLASH_KEY1);
 135     stlink_write_mem32(sl, FLASH_KEYR, sizeof (uint32_t));
 136
 137     write_uint32(sl->q_buf, FLASH_KEY2);
 138     stlink_write_mem32(sl, FLASH_KEYR, sizeof (uint32_t));
 139 }
 140
 141 static int unlock_flash_if(stlink_t *sl) {
 142     /* unlock flash if already locked */
 143
 144     if (is_flash_locked(sl)) {
 145         unlock_flash(sl);
 146         if (is_flash_locked(sl))
 147             return -1;
 148     }
 149
 150     return 0;
 151 }
 152
 153 static void lock_flash(stlink_t *sl) {
 154     /* write to 1 only. reset by hw at unlock sequence */
 155
 156     const uint32_t n = read_flash_cr(sl) | (1 << FLASH_CR_LOCK);
 157
 158     write_uint32(sl->q_buf, n);
 159     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 160 }
 161
 162 static void set_flash_cr_pg(stlink_t *sl) {
 163     const uint32_t n = 1 << FLASH_CR_PG;
 164     write_uint32(sl->q_buf, n);
 165     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 166 }
 167
 168 static void __attribute__((unused)) clear_flash_cr_pg(stlink_t *sl) {
 169     const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_PG);
 170     write_uint32(sl->q_buf, n);
 171     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 172 }
 173
 174 static void set_flash_cr_per(stlink_t *sl) {
 175     const uint32_t n = 1 << FLASH_CR_PER;
 176     write_uint32(sl->q_buf, n);
 177     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 178 }
 179
 180 static void __attribute__((unused)) clear_flash_cr_per(stlink_t *sl) {
 181     const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_PER);
 182     write_uint32(sl->q_buf, n);
 183     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 184 }
 185
 186 static void set_flash_cr_mer(stlink_t *sl) {
 187     const uint32_t n = 1 << FLASH_CR_MER;
 188     write_uint32(sl->q_buf, n);
 189     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 190 }
 191
 192 static void __attribute__((unused)) clear_flash_cr_mer(stlink_t *sl) {
 193     const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_MER);
 194     write_uint32(sl->q_buf, n);
 195     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 196 }
 197
 198 static void set_flash_cr_strt(stlink_t *sl) {
 199     /* assume come on the flash_cr_per path */
 200     const uint32_t n = (1 << FLASH_CR_PER) | (1 << FLASH_CR_STRT);
 201     write_uint32(sl->q_buf, n);
 202     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 203 }
 204
 205 static inline uint32_t read_flash_acr(stlink_t *sl) {
 206     stlink_read_mem32(sl, FLASH_ACR, sizeof (uint32_t));
 207     return *(uint32_t*) sl->q_buf;
 208 }
 209
 210 static inline uint32_t read_flash_sr(stlink_t *sl) {
 211     stlink_read_mem32(sl, FLASH_SR, sizeof (uint32_t));
 212     return *(uint32_t*) sl->q_buf;
 213 }
 214
 215 static inline unsigned int is_flash_busy(stlink_t *sl) {
 216     return read_flash_sr(sl) & (1 << FLASH_SR_BSY);
 217 }
 218
 219 static void wait_flash_busy(stlink_t *sl) {
 220     /* todo: add some delays here */
 221     while (is_flash_busy(sl))
 222         ;
 223 }
 224
 225 static inline unsigned int is_flash_eop(stlink_t *sl) {
 226     return read_flash_sr(sl) & (1 << FLASH_SR_EOP);
 227 }
 228
 229 static void __attribute__((unused)) clear_flash_sr_eop(stlink_t *sl) {
 230     const uint32_t n = read_flash_sr(sl) & ~(1 << FLASH_SR_EOP);
 231     write_uint32(sl->q_buf, n);
 232     stlink_write_mem32(sl, FLASH_SR, sizeof (uint32_t));
 233 }
 234
 235 static void __attribute__((unused)) wait_flash_eop(stlink_t *sl) {
 236     /* todo: add some delays here */
 237     while (is_flash_eop(sl) == 0)
 238         ;
 239 }
 240
 241 static inline void write_flash_ar(stlink_t *sl, uint32_t n) {
 242     write_uint32(sl->q_buf, n);
 243     stlink_write_mem32(sl, FLASH_AR, sizeof (uint32_t));
 244 }
 245
 246 #if 0 /* todo */
 247
 248 static void disable_flash_read_protection(stlink_t *sl) {
 249     /* erase the option byte area */
 250     /* rdp = 0x00a5; */
 251     /* reset */
 252 }
 253 #endif /* todo */
 254
 255
 256 // Delegates to the backends...
 257
 258 void stlink_close(stlink_t *sl) {
 259     D(sl, "\n*** stlink_close ***\n");
 260     sl->backend->close(sl);
 261     free(sl);
 262 }
 263
 264 void stlink_exit_debug_mode(stlink_t *sl) {
 265     D(sl, "\n*** stlink_exit_debug_mode ***\n");
 266     sl->backend->exit_debug_mode(sl);
 267 }
 268
 269 void stlink_enter_swd_mode(stlink_t *sl) {
 270     D(sl, "\n*** stlink_enter_swd_mode ***\n");
 271     sl->backend->enter_swd_mode(sl);
 272 }
 273
 274 // Force the core into the debug mode -> halted state.
 275 void stlink_force_debug(stlink_t *sl) {
 276     D(sl, "\n*** stlink_force_debug_mode ***\n");
 277     sl->backend->force_debug(sl);
 278 }
 279
 280 void stlink_exit_dfu_mode(stlink_t *sl) {
 281     D(sl, "\n*** stlink_exit_dfu_mode ***\n");
 282     sl->backend->exit_dfu_mode(sl);
 283 }
 284
 285 uint32_t stlink_core_id(stlink_t *sl) {
 286     D(sl, "\n*** stlink_core_id ***\n");
 287     sl->backend->core_id(sl);
 288     if (sl->verbose > 2)
 289         stlink_print_data(sl);
 290     return sl->core_id;
 291 }
 292
 293 uint16_t stlink_chip_id(stlink_t *sl) {
 294     stlink_read_mem32(sl, 0xE0042000, 4);
 295     uint32_t chip_id = sl->q_buf[0] | (sl->q_buf[1] << 8) | (sl->q_buf[2] << 16) |
 296             (sl->q_buf[3] << 24);
 297     return chip_id;
 298 }
 299
 300 /**
 301  * Cortex m3 tech ref manual, CPUID register description
 302  * @param sl stlink context
 303  * @param cpuid pointer to the result object
 304  */
 305 void stlink_cpu_id(stlink_t *sl, cortex_m3_cpuid_t *cpuid) {
 306     stlink_read_mem32(sl, CM3_REG_CPUID, 4);
 307     uint32_t raw = read_uint32(sl->q_buf, 0);
 308     cpuid->implementer_id = (raw >> 24) & 0x7f;
 309     cpuid->variant = (raw >> 20) & 0xf;
 310     cpuid->part = (raw >> 4) & 0xfff;
 311     cpuid->revision = raw & 0xf;
 312     return;
 313 }
 314
 315 void stlink_reset(stlink_t *sl) {
 316     D(sl, "\n*** stlink_reset ***\n");
 317     sl->backend->reset(sl);
 318 }
 319
 320 void stlink_run(stlink_t *sl) {
 321     D(sl, "\n*** stlink_run ***\n");
 322     sl->backend->run(sl);
 323 }
 324
 325 void stlink_status(stlink_t *sl) {
 326     D(sl, "\n*** stlink_status ***\n");
 327     sl->backend->status(sl);
 328     stlink_core_stat(sl);
 329 }
 330
 331 /**
 332  * Decode the version bits, originally from -sg, verified with usb
 333  * @param sl stlink context, assumed to contain valid data in the buffer
 334  * @param slv output parsed version object
 335  */
 336 void _parse_version(stlink_t *sl, stlink_version_t *slv) {
 337     uint32_t b0 = sl->q_buf[0]; //lsb
 338     uint32_t b1 = sl->q_buf[1];
 339     uint32_t b2 = sl->q_buf[2];
 340     uint32_t b3 = sl->q_buf[3];
 341     uint32_t b4 = sl->q_buf[4];
 342     uint32_t b5 = sl->q_buf[5]; //msb
 343
 344     // b0 b1                       || b2 b3  | b4 b5
 345     // 4b        | 6b     | 6b     || 2B     | 2B
 346     // stlink_v  | jtag_v | swim_v || st_vid | stlink_pid
 347
 348     slv->stlink_v = (b0 & 0xf0) >> 4;
 349     slv->jtag_v = ((b0 & 0x0f) << 2) | ((b1 & 0xc0) >> 6);
 350     slv->swim_v = b1 & 0x3f;
 351     slv->st_vid = (b3 << 8) | b2;
 352     slv->stlink_pid = (b5 << 8) | b4;
 353     return;
 354 }
 355
 356 void stlink_version(stlink_t *sl) {
 357     D(sl, "*** looking up stlink version\n");
 358     stlink_version_t slv;
 359     sl->backend->version(sl);
 360     _parse_version(sl, &slv);
 361
 362     DD(sl, "st vid         = 0x%04x (expect 0x%04x)\n", slv.st_vid, USB_ST_VID);
 363     DD(sl, "stlink pid     = 0x%04x\n", slv.stlink_pid);
 364     DD(sl, "stlink version = 0x%x\n", slv.stlink_v);
 365     DD(sl, "jtag version   = 0x%x\n", slv.jtag_v);
 366     DD(sl, "swim version   = 0x%x\n", slv.swim_v);
 367     if (slv.jtag_v == 0) {
 368         DD(sl, "    notice: the firmware doesn't support a jtag/swd interface\n");
 369     }
 370     if (slv.swim_v == 0) {
 371         DD(sl, "    notice: the firmware doesn't support a swim interface\n");
 372     }
 373 }
 374
 375 void stlink_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 376     D(sl, "\n*** stlink_write_mem32 ***\n");
 377     if (len % 4 != 0) {
 378         fprintf(stderr, "Error: Data length doesn't have a 32 bit alignment: +%d byte.\n", len % 4);
 379         return;
 380     }
 381     sl->backend->write_mem32(sl, addr, len);
 382 }
 383
 384 void stlink_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 385     D(sl, "\n*** stlink_read_mem32 ***\n");
 386     if (len % 4 != 0) { // !!! never ever: fw gives just wrong values
 387         fprintf(stderr, "Error: Data length doesn't have a 32 bit alignment: +%d byte.\n",
 388                 len % 4);
 389         return;
 390     }
 391     sl->backend->read_mem32(sl, addr, len);
 392 }
 393
 394 void stlink_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
 395     D(sl, "\n*** stlink_write_mem8 ***\n");
 396     sl->backend->write_mem8(sl, addr, len);
 397 }
 398
 399 void stlink_read_all_regs(stlink_t *sl, reg *regp) {
 400     D(sl, "\n*** stlink_read_all_regs ***\n");
 401     sl->backend->read_all_regs(sl, regp);
 402 }
 403
 404 void stlink_write_reg(stlink_t *sl, uint32_t reg, int idx) {
 405     D(sl, "\n*** stlink_write_reg\n");
 406     sl->backend->write_reg(sl, reg, idx);
 407 }
 408
 409 void stlink_read_reg(stlink_t *sl, int r_idx, reg *regp) {
 410     D(sl, "\n*** stlink_read_reg\n");
 411     DD(sl, " (%d) ***\n", r_idx);
 412
 413     if (r_idx > 20 || r_idx < 0) {
 414         fprintf(stderr, "Error: register index must be in [0..20]\n");
 415         return;
 416     }
 417
 418     sl->backend->read_reg(sl, r_idx, regp);
 419 }
 420
 421 unsigned int is_core_halted(stlink_t *sl) {
 422     /* return non zero if core is halted */
 423     stlink_status(sl);
 424     return sl->q_buf[0] == STLINK_CORE_HALTED;
 425 }
 426
 427 void stlink_step(stlink_t *sl) {
 428     D(sl, "\n*** stlink_step ***\n");
 429     sl->backend->step(sl);
 430 }
 431
 432 int stlink_current_mode(stlink_t *sl) {
 433     int mode = sl->backend->current_mode(sl);
 434     switch (mode) {
 435         case STLINK_DEV_DFU_MODE:
 436             DD(sl, "stlink current mode: dfu\n");
 437             return mode;
 438         case STLINK_DEV_DEBUG_MODE:
 439             DD(sl, "stlink current mode: debug (jtag or swd)\n");
 440             return mode;
 441         case STLINK_DEV_MASS_MODE:
 442             DD(sl, "stlink current mode: mass\n");
 443             return mode;
 444     }
 445     DD(sl, "stlink mode: unknown!\n");
 446     return STLINK_DEV_UNKNOWN_MODE;
 447 }
 448
 449
 450
 451
 452 // End of delegates....  Common code below here...
 453
 454 // Endianness
 455 // http://www.ibm.com/developerworks/aix/library/au-endianc/index.html
 456 // const int i = 1;
 457 // #define is_bigendian() ( (*(char*)&i) == 0 )
 458
 459 inline unsigned int is_bigendian(void) {
 460     static volatile const unsigned int i = 1;
 461     return *(volatile const char*) &i == 0;
 462 }
 463
 464 uint16_t read_uint16(const unsigned char *c, const int pt) {
 465     uint32_t ui;
 466     char *p = (char *) &ui;
 467
 468     if (!is_bigendian()) { // le -> le (don't swap)
 469         p[0] = c[pt + 0];
 470         p[1] = c[pt + 1];
 471     } else {
 472         p[0] = c[pt + 1];
 473         p[1] = c[pt + 0];
 474     }
 475     return ui;
 476 }
 477
 478 // same as above with entrypoint.
 479
 480 void stlink_run_at(stlink_t *sl, stm32_addr_t addr) {
 481     stlink_write_reg(sl, addr, 15); /* pc register */
 482
 483     stlink_run(sl);
 484
 485     while (is_core_halted(sl) == 0)
 486         usleep(3000000);
 487 }
 488
 489 void stlink_core_stat(stlink_t *sl) {
 490     if (sl->q_len <= 0)
 491         return;
 492
 493     switch (sl->q_buf[0]) {
 494         case STLINK_CORE_RUNNING:
 495             sl->core_stat = STLINK_CORE_RUNNING;
 496             DD(sl, "  core status: running\n");
 497             return;
 498         case STLINK_CORE_HALTED:
 499             sl->core_stat = STLINK_CORE_HALTED;
 500             DD(sl, "  core status: halted\n");
 501             return;
 502         default:
 503             sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
 504             fprintf(stderr, "  core status: unknown\n");
 505     }
 506 }
 507
 508 void stlink_print_data(stlink_t * sl) {
 509     if (sl->q_len <= 0 || sl->verbose < 2)
 510         return;
 511     if (sl->verbose > 2)
 512         fprintf(stdout, "data_len = %d 0x%x\n", sl->q_len, sl->q_len);
 513
 514     for (int i = 0; i < sl->q_len; i++) {
 515         if (i % 16 == 0) {
 516             /*
 517                                     if (sl->q_data_dir == Q_DATA_OUT)
 518                                             fprintf(stdout, "\n<- 0x%08x ", sl->q_addr + i);
 519                                     else
 520                                             fprintf(stdout, "\n-> 0x%08x ", sl->q_addr + i);
 521              */
 522         }
 523         fprintf(stdout, " %02x", (unsigned int) sl->q_buf[i]);
 524     }
 525     fputs("\n\n", stdout);
 526 }
 527
 528 /* memory mapped file */
 529
 530 typedef struct mapped_file {
 531     uint8_t* base;
 532     size_t len;
 533 } mapped_file_t;
 534
 535 #define MAPPED_FILE_INITIALIZER { NULL, 0 }
 536
 537 static int map_file(mapped_file_t* mf, const char* path) {
 538     int error = -1;
 539     struct stat st;
 540
 541     const int fd = open(path, O_RDONLY);
 542     if (fd == -1) {
 543         fprintf(stderr, "open(%s) == -1\n", path);
 544         return -1;
 545     }
 546
 547     if (fstat(fd, &st) == -1) {
 548         fprintf(stderr, "fstat() == -1\n");
 549         goto on_error;
 550     }
 551
 552     mf->base = (uint8_t*) mmap(NULL, st.st_size, PROT_READ, MAP_SHARED, fd, 0);
 553     if (mf->base == MAP_FAILED) {
 554         fprintf(stderr, "mmap() == MAP_FAILED\n");
 555         goto on_error;
 556     }
 557
 558     mf->len = st.st_size;
 559
 560     /* success */
 561     error = 0;
 562
 563 on_error:
 564     close(fd);
 565
 566     return error;
 567 }
 568
 569 static void unmap_file(mapped_file_t * mf) {
 570     munmap((void*) mf->base, mf->len);
 571     mf->base = (unsigned char*) MAP_FAILED;
 572     mf->len = 0;
 573 }
 574
 575 static int check_file(stlink_t* sl, mapped_file_t* mf, stm32_addr_t addr) {
 576     size_t off;
 577
 578     for (off = 0; off < mf->len; off += sl->flash_pgsz) {
 579         size_t aligned_size;
 580
 581         /* adjust last page size */
 582         size_t cmp_size = sl->flash_pgsz;
 583         if ((off + sl->flash_pgsz) > mf->len)
 584             cmp_size = mf->len - off;
 585
 586         aligned_size = cmp_size;
 587         if (aligned_size & (4 - 1))
 588             aligned_size = (cmp_size + 4) & ~(4 - 1);
 589
 590         stlink_read_mem32(sl, addr + off, aligned_size);
 591
 592         if (memcmp(sl->q_buf, mf->base + off, cmp_size))
 593             return -1;
 594     }
 595
 596     return 0;
 597 }
 598
 599 int stlink_fwrite_sram
 600 (stlink_t * sl, const char* path, stm32_addr_t addr) {
 601     /* write the file in sram at addr */
 602
 603     int error = -1;
 604     size_t off;
 605     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
 606
 607     if (map_file(&mf, path) == -1) {
 608         fprintf(stderr, "map_file() == -1\n");
 609         return -1;
 610     }
 611
 612     /* check addr range is inside the sram */
 613     if (addr < sl->sram_base) {
 614         fprintf(stderr, "addr too low\n");
 615         goto on_error;
 616     } else if ((addr + mf.len) < addr) {
 617         fprintf(stderr, "addr overruns\n");
 618         goto on_error;
 619     } else if ((addr + mf.len) > (sl->sram_base + sl->sram_size)) {
 620         fprintf(stderr, "addr too high\n");
 621         goto on_error;
 622     } else if ((addr & 3) || (mf.len & 3)) {
 623         /* todo */
 624         fprintf(stderr, "unaligned addr or size\n");
 625         goto on_error;
 626     }
 627
 628     /* do the copy by 1k blocks */
 629     for (off = 0; off < mf.len; off += 1024) {
 630         size_t size = 1024;
 631         if ((off + size) > mf.len)
 632             size = mf.len - off;
 633
 634         memcpy(sl->q_buf, mf.base + off, size);
 635
 636         /* round size if needed */
 637         if (size & 3)
 638             size += 2;
 639
 640         stlink_write_mem32(sl, addr + off, size);
 641     }
 642
 643     /* check the file ha been written */
 644     if (check_file(sl, &mf, addr) == -1) {
 645         fprintf(stderr, "check_file() == -1\n");
 646         goto on_error;
 647     }
 648
 649     /* success */
 650     error = 0;
 651
 652 on_error:
 653     unmap_file(&mf);
 654     return error;
 655 }
 656
 657 int stlink_fread(stlink_t* sl, const char* path, stm32_addr_t addr, size_t size) {
 658     /* read size bytes from addr to file */
 659
 660     int error = -1;
 661     size_t off;
 662
 663     const int fd = open(path, O_RDWR | O_TRUNC | O_CREAT, 00700);
 664     if (fd == -1) {
 665         fprintf(stderr, "open(%s) == -1\n", path);
 666         return -1;
 667     }
 668
 669     /* do the copy by 1k blocks */
 670     for (off = 0; off < size; off += 1024) {
 671         size_t read_size = 1024;
 672         size_t rounded_size;
 673         if ((off + read_size) > size)
 674           read_size = size - off;
 675
 676         /* round size if needed */
 677         rounded_size = read_size;
 678         if (rounded_size & 3)
 679           rounded_size = (rounded_size + 4) & ~(3);
 680
 681         stlink_read_mem32(sl, addr + off, rounded_size);
 682
 683         if (write(fd, sl->q_buf, read_size) != (ssize_t) read_size) {
 684             fprintf(stderr, "write() != read_size\n");
 685             goto on_error;
 686         }
 687     }
 688
 689     /* success */
 690     error = 0;
 691
 692 on_error:
 693     close(fd);
 694
 695     return error;
 696 }
 697
 698 int write_buffer_to_sram(stlink_t *sl, flash_loader_t* fl, const uint8_t* buf, size_t size) {
 699     /* write the buffer right after the loader */
 700     memcpy(sl->q_buf, buf, size);
 701     stlink_write_mem8(sl, fl->buf_addr, size);
 702     return 0;
 703 }
 704
 705 int stlink_erase_flash_page(stlink_t *sl, stm32_addr_t page)
 706 {
 707   /* page an addr in the page to erase */
 708
 709   stlink_core_id(sl);
 710   if (sl->core_id == STM32L_CORE_ID)
 711   {
 712 #define STM32L_FLASH_REGS_ADDR ((uint32_t)0x40023c00)
 713 #define STM32L_FLASH_ACR (STM32L_FLASH_REGS_ADDR + 0x00)
 714 #define STM32L_FLASH_PECR (STM32L_FLASH_REGS_ADDR + 0x04)
 715 #define STM32L_FLASH_PDKEYR (STM32L_FLASH_REGS_ADDR + 0x08)
 716 #define STM32L_FLASH_PEKEYR (STM32L_FLASH_REGS_ADDR + 0x0c)
 717 #define STM32L_FLASH_PRGKEYR (STM32L_FLASH_REGS_ADDR + 0x10)
 718 #define STM32L_FLASH_OPTKEYR (STM32L_FLASH_REGS_ADDR + 0x14)
 719 #define STM32L_FLASH_SR (STM32L_FLASH_REGS_ADDR + 0x18)
 720 #define STM32L_FLASH_OBR (STM32L_FLASH_REGS_ADDR + 0x0c)
 721 #define STM32L_FLASH_WRPR (STM32L_FLASH_REGS_ADDR + 0x20)
 722
 723     uint32_t val;
 724
 725     /* disable pecr protection */
 726     write_uint32(sl->q_buf, 0x89abcdef);
 727     stlink_write_mem32(sl, STM32L_FLASH_PEKEYR, sizeof(uint32_t));
 728     write_uint32(sl->q_buf, 0x02030405);
 729     stlink_write_mem32(sl, STM32L_FLASH_PEKEYR, sizeof(uint32_t));
 730
 731     /* check pecr.pelock is cleared */
 732     stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
 733     val = read_uint32(sl->q_buf, 0);
 734     if (val & (1 << 0))
 735     {
 736       fprintf(stderr, "pecr.pelock not clear (0x%x)\n", val);
 737       return -1;
 738     }
 739
 740     /* unlock program memory */
 741     write_uint32(sl->q_buf, 0x8c9daebf);
 742     stlink_write_mem32(sl, STM32L_FLASH_PRGKEYR, sizeof(uint32_t));
 743     write_uint32(sl->q_buf, 0x13141516);
 744     stlink_write_mem32(sl, STM32L_FLASH_PRGKEYR, sizeof(uint32_t));
 745
 746     /* check pecr.prglock is cleared */
 747     stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
 748     val = read_uint32(sl->q_buf, 0);
 749     if (val & (1 << 1))
 750     {
 751       fprintf(stderr, "pecr.prglock not clear (0x%x)\n", val);
 752       return -1;
 753     }
 754
 755     /* unused: unlock the option byte block */
 756 #if 0
 757     write_uint32(sl->q_buf, 0xfbead9c8);
 758     stlink_write_mem32(sl, STM32L_FLASH_OPTKEYR, sizeof(uint32_t));
 759     write_uint32(sl->q_buf, 0x24252627);
 760     stlink_write_mem32(sl, STM32L_FLASH_OPTKEYR, sizeof(uint32_t));
 761
 762     /* check pecr.optlock is cleared */
 763     stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
 764     val = read_uint32(sl->q_buf, 0);
 765     if (val & (1 << 2))
 766     {
 767       fprintf(stderr, "pecr.prglock not clear\n");
 768       return -1;
 769     }
 770 #endif
 771
 772     /* set pecr.{erase,prog} */
 773     val |= (1 << 9) | (1 << 3);
 774     write_uint32(sl->q_buf, val);
 775     stlink_write_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
 776
 777     /* wait for sr.busy to be cleared */
 778     while (1)
 779     {
 780       stlink_read_mem32(sl, STM32L_FLASH_SR, sizeof(uint32_t));
 781       if ((read_uint32(sl->q_buf, 0) & (1 << 0)) == 0) break ;
 782     }
 783
 784     /* write 0 to the first word of the page to be erased */
 785     memset(sl->q_buf, 0, sizeof(uint32_t));
 786     stlink_write_mem32(sl, page, sizeof(uint32_t));
 787
 788     /* reset lock bits */
 789     stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
 790     val = read_uint32(sl->q_buf, 0) | (1 << 0) | (1 << 1) | (1 << 2);
 791     write_uint32(sl->q_buf, val);
 792     stlink_write_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
 793   }
 794   else if (sl->core_id == STM32VL_CORE_ID)
 795   {
 796     /* wait for ongoing op to finish */
 797     wait_flash_busy(sl);
 798
 799     /* unlock if locked */
 800     unlock_flash_if(sl);
 801
 802     /* set the page erase bit */
 803     set_flash_cr_per(sl);
 804
 805     /* select the page to erase */
 806     write_flash_ar(sl, page);
 807
 808     /* start erase operation, reset by hw with bsy bit */
 809     set_flash_cr_strt(sl);
 810
 811     /* wait for completion */
 812     wait_flash_busy(sl);
 813
 814     /* relock the flash */
 815     lock_flash(sl);
 816   }
 817   else {
 818     fprintf(stderr, "unknown coreid: %x\n", sl->core_id);
 819     return -1;
 820   }
 821
 822   /* todo: verify the erased page */
 823
 824   return 0;
 825 }
 826
 827 int stlink_erase_flash_mass(stlink_t *sl) {
 828     /* wait for ongoing op to finish */
 829     wait_flash_busy(sl);
 830
 831     /* unlock if locked */
 832     unlock_flash_if(sl);
 833
 834     /* set the mass erase bit */
 835     set_flash_cr_mer(sl);
 836
 837     /* start erase operation, reset by hw with bsy bit */
 838     set_flash_cr_strt(sl);
 839
 840     /* wait for completion */
 841     wait_flash_busy(sl);
 842
 843     /* relock the flash */
 844     lock_flash(sl);
 845
 846     /* todo: verify the erased memory */
 847
 848     return 0;
 849 }
 850
 851 int init_flash_loader(stlink_t *sl, flash_loader_t* fl) {
 852     size_t size;
 853
 854     /* allocate the loader in sram */
 855     if (write_loader_to_sram(sl, &fl->loader_addr, &size) == -1) {
 856         fprintf(stderr, "write_loader_to_sram() == -1\n");
 857         return -1;
 858     }
 859
 860     /* allocate a one page buffer in sram right after loader */
 861     fl->buf_addr = fl->loader_addr + size;
 862
 863     return 0;
 864 }
 865
 866 int write_loader_to_sram(stlink_t *sl, stm32_addr_t* addr, size_t* size) {
 867     /* from openocd, contrib/loaders/flash/stm32.s */
 868     static const uint8_t loader_code_stm32vl[] = {
 869         0x08, 0x4c, /* ldr      r4, STM32_FLASH_BASE */
 870         0x1c, 0x44, /* add      r4, r3 */
 871         /* write_half_word: */
 872         0x01, 0x23, /* movs     r3, #0x01 */
 873         0x23, 0x61, /* str      r3, [r4, #STM32_FLASH_CR_OFFSET] */
 874         0x30, 0xf8, 0x02, 0x3b, /* ldrh r3, [r0], #0x02 */
 875         0x21, 0xf8, 0x02, 0x3b, /* strh r3, [r1], #0x02 */
 876         /* busy: */
 877         0xe3, 0x68, /* ldr      r3, [r4, #STM32_FLASH_SR_OFFSET] */
 878         0x13, 0xf0, 0x01, 0x0f, /* tst  r3, #0x01 */
 879         0xfb, 0xd0, /* beq      busy */
 880         0x13, 0xf0, 0x14, 0x0f, /* tst  r3, #0x14 */
 881         0x01, 0xd1, /* bne      exit */
 882         0x01, 0x3a, /* subs     r2, r2, #0x01 */
 883         0xf0, 0xd1, /* bne      write_half_word */
 884         /* exit: */
 885         0x00, 0xbe, /* bkpt     #0x00 */
 886         0x00, 0x20, 0x02, 0x40, /* STM32_FLASH_BASE: .word 0x40022000 */
 887     };
 888
 889     static const uint8_t loader_code_stm32l[] = {
 890
 891       /* openocd.git/contrib/loaders/flash/stm32lx.S
 892          r0, input, dest addr
 893          r1, input, source addr
 894          r2, input, word count
 895          r3, output, word count
 896        */
 897
 898       0x00, 0x23,
 899       0x04, 0xe0,
 900
 901       0x51, 0xf8, 0x04, 0xcb,
 902       0x40, 0xf8, 0x04, 0xcb,
 903       0x01, 0x33,
 904
 905       0x93, 0x42,
 906       0xf8, 0xd3,
 907       0x00, 0xbe
 908     };
 909
 910     const uint8_t* loader_code;
 911     size_t loader_size;
 912
 913     if (sl->core_id == STM32L_CORE_ID) /* stm32l */
 914     {
 915       loader_code = loader_code_stm32l;
 916       loader_size = sizeof(loader_code_stm32l);
 917     }
 918     else if (sl->core_id == STM32VL_CORE_ID)
 919     {
 920       loader_code = loader_code_stm32vl;
 921       loader_size = sizeof(loader_code_stm32vl);
 922     }
 923     else
 924     {
 925       fprintf(stderr, "unknown coreid: %x\n", sl->core_id);
 926       return -1;
 927     }
 928
 929     memcpy(sl->q_buf, loader_code, loader_size);
 930     stlink_write_mem32(sl, sl->sram_base, loader_size);
 931
 932     *addr = sl->sram_base;
 933     *size = loader_size;
 934
 935     /* success */
 936     return 0;
 937 }
 938
 939 int stlink_fcheck_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
 940     /* check the contents of path are at addr */
 941
 942     int res;
 943     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
 944
 945     if (map_file(&mf, path) == -1)
 946         return -1;
 947
 948     res = check_file(sl, &mf, addr);
 949
 950     unmap_file(&mf);
 951
 952     return res;
 953 }
 954
 955
 956 int stlink_write_flash(stlink_t *sl, stm32_addr_t addr, uint8_t* base, unsigned len) {
 957     size_t off;
 958     flash_loader_t fl;
 959
 960     /* check addr range is inside the flash */
 961     if (addr < sl->flash_base) {
 962         fprintf(stderr, "addr too low\n");
 963         return -1;
 964     } else if ((addr + len) < addr) {
 965         fprintf(stderr, "addr overruns\n");
 966         return -1;
 967     } else if ((addr + len) > (sl->flash_base + sl->flash_size)) {
 968         fprintf(stderr, "addr too high\n");
 969         return -1;
 970     } else if ((addr & 1) || (len & 1)) {
 971         fprintf(stderr, "unaligned addr or size\n");
 972         return -1;
 973     } else if (addr & (sl->flash_pgsz - 1)) {
 974         fprintf(stderr, "addr not a multiple of pagesize, not supported\n");
 975         return -1;
 976     }
 977
 978     /* erase each page */
 979     for (off = 0; off < len; off += sl->flash_pgsz) {
 980         /* addr must be an addr inside the page */
 981         if (stlink_erase_flash_page(sl, addr + off) == -1) {
 982             fprintf(stderr, "erase_flash_page(0x%zx) == -1\n", addr + off);
 983             return -1;
 984         }
 985     }
 986
 987     stlink_core_id(sl);
 988     if (sl->core_id == STM32L_CORE_ID)
 989     {
 990       /* use fast word write. todo: half page. */
 991
 992       uint32_t val;
 993
 994 #if 0 /* todo: check write operation */
 995
 996       uint32_t nwrites = sl->flash_pgsz;
 997
 998     redo_write:
 999
1000 #endif /* todo: check write operation */
1001
1002       /* disable pecr protection */
1003       write_uint32(sl->q_buf, 0x89abcdef);
1004       stlink_write_mem32(sl, STM32L_FLASH_PEKEYR, sizeof(uint32_t));
1005       write_uint32(sl->q_buf, 0x02030405);
1006       stlink_write_mem32(sl, STM32L_FLASH_PEKEYR, sizeof(uint32_t));
1007
1008       /* check pecr.pelock is cleared */
1009       stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1010       val = read_uint32(sl->q_buf, 0);
1011       if (val & (1 << 0))
1012       {
1013         fprintf(stderr, "pecr.pelock not clear\n");
1014         return -1;
1015       }
1016
1017       /* unlock program memory */
1018       write_uint32(sl->q_buf, 0x8c9daebf);
1019       stlink_write_mem32(sl, STM32L_FLASH_PRGKEYR, sizeof(uint32_t));
1020       write_uint32(sl->q_buf, 0x13141516);
1021       stlink_write_mem32(sl, STM32L_FLASH_PRGKEYR, sizeof(uint32_t));
1022
1023       /* check pecr.prglock is cleared */
1024       stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1025       val = read_uint32(sl->q_buf, 0);
1026       if (val & (1 << 1))
1027       {
1028         fprintf(stderr, "pecr.prglock not clear\n");
1029         return -1;
1030       }
1031
1032       /* write a word in program memory */
1033       for (off = 0; off < len; off += sizeof(uint32_t))
1034       {
1035         if (sl->verbose >= 1)
1036         {
1037           if ((off & (sl->flash_pgsz - 1)) == 0)
1038           {
1039             /* show progress. writing procedure is slow
1040                and previous errors are misleading */
1041             const uint32_t pgnum = off / sl->flash_pgsz;
1042             const uint32_t pgcount = len / sl->flash_pgsz;
1043             fprintf(stdout, "%u pages written out of %u\n", pgnum, pgcount);
1044           }
1045         }
1046
1047         memcpy(sl->q_buf, (const void*)(base + off), sizeof(uint32_t));
1048         stlink_write_mem32(sl, addr + off, sizeof(uint32_t));
1049
1050         /* wait for sr.busy to be cleared */
1051         while (1)
1052         {
1053           stlink_read_mem32(sl, STM32L_FLASH_SR, sizeof(uint32_t));
1054           if ((read_uint32(sl->q_buf, 0) & (1 << 0)) == 0) break ;
1055         }
1056
1057 #if 0 /* todo: check redo write operation */
1058
1059         /* check written bytes. todo: should be on a per page basis. */
1060         stlink_read_mem32(sl, addr + off, sizeof(uint32_t));
1061         if (memcmp(sl->q_buf, base + off, sizeof(uint32_t)))
1062         {
1063           /* re erase the page and redo the write operation */
1064           uint32_t page;
1065           uint32_t val;
1066
1067           /* fail if successive write count too low */
1068           if (nwrites < sl->flash_pgsz) {
1069             fprintf(stderr, "writes operation failure count too high, aborting\n");
1070             return -1;
1071           }
1072
1073           nwrites = 0;
1074
1075           /* assume addr aligned */
1076           if (off % sl->flash_pgsz) off &= ~(sl->flash_pgsz - 1);
1077           page = addr + off;
1078
1079           fprintf(stderr, "invalid write @%x(%x): %x != %x. retrying.\n",
1080                   page, addr + off, read_uint32(base + off, 0), read_uint32(sl->q_buf, 0));
1081
1082           /* reset lock bits */
1083           stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1084           val = read_uint32(sl->q_buf, 0) | (1 << 0) | (1 << 1) | (1 << 2);
1085           write_uint32(sl->q_buf, val);
1086           stlink_write_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1087
1088           stlink_erase_flash_page(sl, page);
1089
1090           goto redo_write;
1091         }
1092
1093         /* increment successive writes counter */
1094         ++nwrites;
1095
1096 #endif /* todo: check redo write operation */
1097
1098       }
1099
1100       /* reset lock bits */
1101       stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1102       val = read_uint32(sl->q_buf, 0) | (1 << 0) | (1 << 1) | (1 << 2);
1103       write_uint32(sl->q_buf, val);
1104       stlink_write_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1105     }
1106     else if (sl->core_id == STM32VL_CORE_ID)
1107     {
1108       /* flash loader initialization */
1109       if (init_flash_loader(sl, &fl) == -1) {
1110         fprintf(stderr, "init_flash_loader() == -1\n");
1111         return -1;
1112       }
1113
1114       /* write each page. above WRITE_BLOCK_SIZE fails? */
1115 #define WRITE_BLOCK_SIZE 0x40
1116       for (off = 0; off < len; off += WRITE_BLOCK_SIZE)
1117       {
1118         /* adjust last write size */
1119         size_t size = WRITE_BLOCK_SIZE;
1120         if ((off + WRITE_BLOCK_SIZE) > len) size = len - off;
1121
1122         /* unlock and set programming mode */
1123         unlock_flash_if(sl);
1124         set_flash_cr_pg(sl);
1125
1126         if (run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
1127           fprintf(stderr, "run_flash_loader(0x%zx) == -1\n", addr + off);
1128           return -1;
1129         }
1130
1131         lock_flash(sl);
1132       }
1133     } else {
1134       fprintf(stderr, "unknown coreid: %x\n", sl->core_id);
1135       return -1;
1136     }
1137
1138     for (off = 0; off < len; off += sl->flash_pgsz) {
1139         size_t aligned_size;
1140
1141         /* adjust last page size */
1142         size_t cmp_size = sl->flash_pgsz;
1143         if ((off + sl->flash_pgsz) > len)
1144             cmp_size = len - off;
1145
1146         aligned_size = cmp_size;
1147         if (aligned_size & (4 - 1))
1148             aligned_size = (cmp_size + 4) & ~(4 - 1);
1149
1150         stlink_read_mem32(sl, addr + off, aligned_size);
1151
1152         if (memcmp(sl->q_buf, base + off, cmp_size))
1153             return -1;
1154     }
1155
1156     return 0;
1157 }
1158
1159 int stlink_fwrite_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
1160     /* write the file in flash at addr */
1161
1162     int err;
1163     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1164
1165     if (map_file(&mf, path) == -1) {
1166         fprintf(stderr, "map_file() == -1\n");
1167         return -1;
1168     }
1169
1170     err = stlink_write_flash(sl, addr, mf.base, mf.len);
1171
1172     unmap_file(&mf);
1173
1174     return err;
1175 }
1176
1177 int run_flash_loader(stlink_t *sl, flash_loader_t* fl, stm32_addr_t target, const uint8_t* buf, size_t size) {
1178
1179     reg rr;
1180
1181     if (write_buffer_to_sram(sl, fl, buf, size) == -1) {
1182         fprintf(stderr, "write_buffer_to_sram() == -1\n");
1183         return -1;
1184     }
1185
1186     if (sl->core_id == STM32L_CORE_ID) {
1187
1188       size_t count = size / sizeof(uint32_t);
1189       if (size % sizeof(uint32_t)) ++count;
1190
1191       /* setup core */
1192       stlink_write_reg(sl, target, 0); /* target */
1193       stlink_write_reg(sl, fl->buf_addr, 1); /* source */
1194       stlink_write_reg(sl, count, 2); /* count (32 bits words) */
1195       stlink_write_reg(sl, 0, 3); /* output count */
1196       stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1197
1198     } else if (sl->core_id == STM32VL_CORE_ID) {
1199
1200       size_t count = size / sizeof(uint16_t);
1201       if (size % sizeof(uint16_t)) ++count;
1202
1203       /* setup core */
1204       stlink_write_reg(sl, fl->buf_addr, 0); /* source */
1205       stlink_write_reg(sl, target, 1); /* target */
1206       stlink_write_reg(sl, count, 2); /* count (16 bits half words) */
1207       stlink_write_reg(sl, 0, 3); /* flash bank 0 (input) */
1208       stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1209
1210     } else {
1211       fprintf(stderr, "unknown coreid: %x\n", sl->core_id);
1212       return -1;
1213     }
1214
1215     /* run loader */
1216     stlink_step(sl);
1217
1218     /* wait until done (reaches breakpoint) */
1219     while (is_core_halted(sl) == 0) ;
1220
1221     /* check written byte count */
1222     if (sl->core_id == STM32L_CORE_ID) {
1223
1224       size_t count = size / sizeof(uint32_t);
1225       if (size % sizeof(uint32_t)) ++count;
1226
1227       stlink_read_reg(sl, 3, &rr);
1228       if (rr.r[3] != count) {
1229         fprintf(stderr, "write error, count == %u\n", rr.r[3]);
1230         return -1;
1231       }
1232
1233     } else if (sl->core_id == STM32VL_CORE_ID) {
1234
1235       stlink_read_reg(sl, 2, &rr);
1236       if (rr.r[2] != 0) {
1237         fprintf(stderr, "write error, count == %u\n", rr.r[2]);
1238         return -1;
1239       }
1240
1241     } else {
1242
1243       fprintf(stderr, "unknown coreid: %x\n", sl->core_id);
1244       return -1;
1245
1246     }
1247
1248     return 0;
1249 }