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

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