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     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 ***\n");
 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
 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     fprintf(stderr, "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         fprintf(stderr, "write_loader_to_sram() == -1\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
 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       fprintf(stderr, "unknown coreid: %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
 998     /* check addr range is inside the flash */
 999     if (addr < sl->flash_base) {
1000         fprintf(stderr, "addr too low\n");
1001         return -1;
1002     } else if ((addr + len) < addr) {
1003         fprintf(stderr, "addr overruns\n");
1004         return -1;
1005     } else if ((addr + len) > (sl->flash_base + sl->flash_size)) {
1006         fprintf(stderr, "addr too high\n");
1007         return -1;
1008     } else if ((addr & 1) || (len & 1)) {
1009         fprintf(stderr, "unaligned addr or size\n");
1010         return -1;
1011     } else if (addr & (sl->flash_pgsz - 1)) {
1012         fprintf(stderr, "addr not a multiple of pagesize, not supported\n");
1013         return -1;
1014     }
1015
1016     /* erase each page */
1017     for (off = 0; off < len; off += sl->flash_pgsz) {
1018         /* addr must be an addr inside the page */
1019         if (stlink_erase_flash_page(sl, addr + off) == -1) {
1020             fprintf(stderr, "erase_flash_page(0x%zx) == -1\n", addr + off);
1021             return -1;
1022         }
1023     }
1024
1025     if (sl->core_id == STM32L_CORE_ID)
1026     {
1027       /* use fast word write. todo: half page. */
1028
1029       uint32_t val;
1030
1031 #if 0 /* todo: check write operation */
1032
1033       uint32_t nwrites = sl->flash_pgsz;
1034
1035     redo_write:
1036
1037 #endif /* todo: check write operation */
1038
1039       /* disable pecr protection */
1040       write_uint32(sl->q_buf, 0x89abcdef);
1041       stlink_write_mem32(sl, STM32L_FLASH_PEKEYR, sizeof(uint32_t));
1042       write_uint32(sl->q_buf, 0x02030405);
1043       stlink_write_mem32(sl, STM32L_FLASH_PEKEYR, sizeof(uint32_t));
1044
1045       /* check pecr.pelock is cleared */
1046       stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1047       val = read_uint32(sl->q_buf, 0);
1048       if (val & (1 << 0))
1049       {
1050         fprintf(stderr, "pecr.pelock not clear\n");
1051         return -1;
1052       }
1053
1054       /* unlock program memory */
1055       write_uint32(sl->q_buf, 0x8c9daebf);
1056       stlink_write_mem32(sl, STM32L_FLASH_PRGKEYR, sizeof(uint32_t));
1057       write_uint32(sl->q_buf, 0x13141516);
1058       stlink_write_mem32(sl, STM32L_FLASH_PRGKEYR, sizeof(uint32_t));
1059
1060       /* check pecr.prglock 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 << 1))
1064       {
1065         fprintf(stderr, "pecr.prglock not clear\n");
1066         return -1;
1067       }
1068
1069       /* write a word in program memory */
1070       for (off = 0; off < len; off += sizeof(uint32_t))
1071       {
1072         if (sl->verbose >= 1)
1073         {
1074           if ((off & (sl->flash_pgsz - 1)) == 0)
1075           {
1076             /* show progress. writing procedure is slow
1077                and previous errors are misleading */
1078             const uint32_t pgnum = off / sl->flash_pgsz;
1079             const uint32_t pgcount = len / sl->flash_pgsz;
1080             fprintf(stdout, "%u pages written out of %u\n", pgnum, pgcount);
1081           }
1082         }
1083
1084         memcpy(sl->q_buf, (const void*)(base + off), sizeof(uint32_t));
1085         stlink_write_mem32(sl, addr + off, sizeof(uint32_t));
1086
1087         /* wait for sr.busy to be cleared */
1088         while (1)
1089         {
1090           stlink_read_mem32(sl, STM32L_FLASH_SR, sizeof(uint32_t));
1091           if ((read_uint32(sl->q_buf, 0) & (1 << 0)) == 0) break ;
1092         }
1093
1094 #if 0 /* todo: check redo write operation */
1095
1096         /* check written bytes. todo: should be on a per page basis. */
1097         stlink_read_mem32(sl, addr + off, sizeof(uint32_t));
1098         if (memcmp(sl->q_buf, base + off, sizeof(uint32_t)))
1099         {
1100           /* re erase the page and redo the write operation */
1101           uint32_t page;
1102           uint32_t val;
1103
1104           /* fail if successive write count too low */
1105           if (nwrites < sl->flash_pgsz) {
1106             fprintf(stderr, "writes operation failure count too high, aborting\n");
1107             return -1;
1108           }
1109
1110           nwrites = 0;
1111
1112           /* assume addr aligned */
1113           if (off % sl->flash_pgsz) off &= ~(sl->flash_pgsz - 1);
1114           page = addr + off;
1115
1116           fprintf(stderr, "invalid write @%x(%x): %x != %x. retrying.\n",
1117                   page, addr + off, read_uint32(base + off, 0), read_uint32(sl->q_buf, 0));
1118
1119           /* reset lock bits */
1120           stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1121           val = read_uint32(sl->q_buf, 0) | (1 << 0) | (1 << 1) | (1 << 2);
1122           write_uint32(sl->q_buf, val);
1123           stlink_write_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1124
1125           stlink_erase_flash_page(sl, page);
1126
1127           goto redo_write;
1128         }
1129
1130         /* increment successive writes counter */
1131         ++nwrites;
1132
1133 #endif /* todo: check redo write operation */
1134
1135       }
1136
1137       /* reset lock bits */
1138       stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1139       val = read_uint32(sl->q_buf, 0) | (1 << 0) | (1 << 1) | (1 << 2);
1140       write_uint32(sl->q_buf, val);
1141       stlink_write_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1142     }
1143     else if (sl->core_id == STM32VL_CORE_ID)
1144     {
1145       /* flash loader initialization */
1146       if (init_flash_loader(sl, &fl) == -1) {
1147         fprintf(stderr, "init_flash_loader() == -1\n");
1148         return -1;
1149       }
1150
1151       /* write each page. above WRITE_BLOCK_SIZE fails? */
1152 #define WRITE_BLOCK_SIZE 0x40
1153       for (off = 0; off < len; off += WRITE_BLOCK_SIZE)
1154       {
1155         /* adjust last write size */
1156         size_t size = WRITE_BLOCK_SIZE;
1157         if ((off + WRITE_BLOCK_SIZE) > len) size = len - off;
1158
1159         /* unlock and set programming mode */
1160         unlock_flash_if(sl);
1161         set_flash_cr_pg(sl);
1162
1163         if (run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
1164           fprintf(stderr, "run_flash_loader(0x%zx) == -1\n", addr + off);
1165           return -1;
1166         }
1167
1168         lock_flash(sl);
1169       }
1170     } else {
1171       fprintf(stderr, "unknown coreid: %x\n", sl->core_id);
1172       return -1;
1173     }
1174
1175     for (off = 0; off < len; off += sl->flash_pgsz) {
1176         size_t aligned_size;
1177
1178         /* adjust last page size */
1179         size_t cmp_size = sl->flash_pgsz;
1180         if ((off + sl->flash_pgsz) > len)
1181             cmp_size = len - off;
1182
1183         aligned_size = cmp_size;
1184         if (aligned_size & (4 - 1))
1185             aligned_size = (cmp_size + 4) & ~(4 - 1);
1186
1187         stlink_read_mem32(sl, addr + off, aligned_size);
1188
1189         if (memcmp(sl->q_buf, base + off, cmp_size))
1190             return -1;
1191     }
1192
1193     return 0;
1194 }
1195
1196 int stlink_fwrite_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
1197     /* write the file in flash at addr */
1198
1199     int err;
1200     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1201
1202     if (map_file(&mf, path) == -1) {
1203         fprintf(stderr, "map_file() == -1\n");
1204         return -1;
1205     }
1206
1207     err = stlink_write_flash(sl, addr, mf.base, mf.len);
1208
1209     unmap_file(&mf);
1210
1211     return err;
1212 }
1213
1214 int run_flash_loader(stlink_t *sl, flash_loader_t* fl, stm32_addr_t target, const uint8_t* buf, size_t size) {
1215
1216     reg rr;
1217
1218     if (write_buffer_to_sram(sl, fl, buf, size) == -1) {
1219         fprintf(stderr, "write_buffer_to_sram() == -1\n");
1220         return -1;
1221     }
1222
1223     if (sl->core_id == STM32L_CORE_ID) {
1224
1225       size_t count = size / sizeof(uint32_t);
1226       if (size % sizeof(uint32_t)) ++count;
1227
1228       /* setup core */
1229       stlink_write_reg(sl, target, 0); /* target */
1230       stlink_write_reg(sl, fl->buf_addr, 1); /* source */
1231       stlink_write_reg(sl, count, 2); /* count (32 bits words) */
1232       stlink_write_reg(sl, 0, 3); /* output count */
1233       stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1234
1235     } else if (sl->core_id == STM32VL_CORE_ID) {
1236
1237       size_t count = size / sizeof(uint16_t);
1238       if (size % sizeof(uint16_t)) ++count;
1239
1240       /* setup core */
1241       stlink_write_reg(sl, fl->buf_addr, 0); /* source */
1242       stlink_write_reg(sl, target, 1); /* target */
1243       stlink_write_reg(sl, count, 2); /* count (16 bits half words) */
1244       stlink_write_reg(sl, 0, 3); /* flash bank 0 (input) */
1245       stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1246
1247     } else {
1248       fprintf(stderr, "unknown coreid: %x\n", sl->core_id);
1249       return -1;
1250     }
1251
1252     /* run loader */
1253     stlink_step(sl);
1254
1255     /* wait until done (reaches breakpoint) */
1256     while (is_core_halted(sl) == 0) ;
1257
1258     /* check written byte count */
1259     if (sl->core_id == STM32L_CORE_ID) {
1260
1261       size_t count = size / sizeof(uint32_t);
1262       if (size % sizeof(uint32_t)) ++count;
1263
1264       stlink_read_reg(sl, 3, &rr);
1265       if (rr.r[3] != count) {
1266         fprintf(stderr, "write error, count == %u\n", rr.r[3]);
1267         return -1;
1268       }
1269
1270     } else if (sl->core_id == STM32VL_CORE_ID) {
1271
1272       stlink_read_reg(sl, 2, &rr);
1273       if (rr.r[2] != 0) {
1274         fprintf(stderr, "write error, count == %u\n", rr.r[2]);
1275         return -1;
1276       }
1277
1278     } else {
1279
1280       fprintf(stderr, "unknown coreid: %x\n", sl->core_id);
1281       return -1;
1282
1283     }
1284
1285     return 0;
1286 }