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