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