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