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