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