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

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