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

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