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

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