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, source addr
1505            r1, input, dest addr
1506            r2, input, word count
1507            r2, output, remaining word count
1508            */
1509
1510         0x04, 0xe0,
1511
1512         0x50, 0xf8, 0x04, 0xcb,
1513         0x41, 0xf8, 0x04, 0xcb,
1514         0x01, 0x3a,
1515
1516         0x00, 0x2a,
1517         0xf8, 0xd3,
1518         0x00, 0xbe
1519     };
1520
1521     static const uint8_t loader_code_stm32l0[] = {
1522
1523         /*
1524            r0, input, source addr
1525            r1, input, dest addr
1526            r2, input, word count
1527            r2, output, remaining word count
1528          */
1529
1530         0x04, 0xe0,
1531
1532         0x04, 0x68,
1533         0x0c, 0x60,
1534         0x01, 0x3a,
1535         0x04, 0x31,
1536         0x04, 0x30,
1537
1538         0x00, 0x2a,
1539         0xf8, 0xd3,
1540         0x00, 0xbe
1541     };
1542
1543     static const uint8_t loader_code_stm32f4[] = {
1544         // flashloaders/stm32f4.s
1545
1546         0x07, 0x4b,
1547
1548         0x62, 0xb1,
1549         0x04, 0x68,
1550         0x0c, 0x60,
1551
1552         0xdc, 0x89,
1553         0x14, 0xf0, 0x01, 0x0f,
1554         0xfb, 0xd1,
1555         0x00, 0xf1, 0x04, 0x00,
1556         0x01, 0xf1, 0x04, 0x01,
1557         0xa2, 0xf1, 0x01, 0x02,
1558         0xf1, 0xe7,
1559
1560         0x00, 0xbe,
1561
1562         0x00, 0x3c, 0x02, 0x40,
1563     };
1564
1565     static const uint8_t loader_code_stm32f4_lv[] = {
1566         // flashloaders/stm32f4lv.s
1567         0x92, 0x00,
1568
1569         0x08, 0x4b,
1570         0x62, 0xb1,
1571         0x04, 0x78,
1572         0x0c, 0x70,
1573
1574         0xdc, 0x89,
1575         0x14, 0xf0, 0x01, 0x0f,
1576         0xfb, 0xd1,
1577         0x00, 0xf1, 0x01, 0x00,
1578         0x01, 0xf1, 0x01, 0x01,
1579         0xa2, 0xf1, 0x01, 0x02,
1580         0xf1, 0xe7,
1581
1582         0x00, 0xbe,
1583         0x00, 0xbf,
1584
1585         0x00, 0x3c, 0x02, 0x40,
1586     };
1587
1588     static const uint8_t loader_code_stm32l4[] = {
1589         // flashloaders/stm32l4.s
1590         0x08, 0x4b,             // start: ldr   r3, [pc, #32] ; <flash_base>
1591         0x72, 0xb1,             // next:  cbz   r2, <done>
1592         0x04, 0x68,             //        ldr   r4, [r0, #0]
1593         0x45, 0x68,             //        ldr   r5, [r0, #4]
1594         0x0c, 0x60,             //        str   r4, [r1, #0]
1595         0x4d, 0x60,             //        str   r5, [r1, #4]
1596         0x5c, 0x8a,             // wait:  ldrh  r4, [r3, #18]
1597         0x14, 0xf0, 0x01, 0x0f, //        tst.w r4, #1
1598         0xfb, 0xd1,             //        bne.n <wait>
1599         0x00, 0xf1, 0x08, 0x00, //        add.w r0, r0, #8
1600         0x01, 0xf1, 0x08, 0x01, //        add.w r1, r1, #8
1601         0xa2, 0xf1, 0x01, 0x02, //        sub.w r2, r2, #1
1602         0xef, 0xe7,             //        b.n   <next>
1603         0x00, 0xbe,             // done:  bkpt  0x0000
1604         0x00, 0x20, 0x02, 0x40  // flash_base:  .word 0x40022000
1605     };
1606
1607         static const uint8_t loader_code_stm32f7[] = {
1608         0x08, 0x4b,
1609         0x72, 0xb1,
1610         0x04, 0x68,
1611         0x0c, 0x60,
1612         0xbf, 0xf3, 0x4f, 0x8f,        // DSB Memory barrier for in order flash write
1613         0xdc, 0x89,
1614         0x14, 0xf0, 0x01, 0x0f,
1615         0xfb, 0xd1,
1616         0x00, 0xf1, 0x04, 0x00,
1617         0x01, 0xf1, 0x04, 0x01,
1618         0xa2, 0xf1, 0x01, 0x02,
1619         0xef, 0xe7,
1620         0x00, 0xbe,                   //     bkpt       #0x00
1621         0x00, 0x3c, 0x02, 0x40,
1622     };
1623
1624     const uint8_t* loader_code;
1625     size_t loader_size;
1626
1627     if (sl->chip_id == STM32_CHIPID_L1_MEDIUM || sl->chip_id == STM32_CHIPID_L1_CAT2
1628             || sl->chip_id == STM32_CHIPID_L1_MEDIUM_PLUS || sl->chip_id == STM32_CHIPID_L1_HIGH
1629             || sl->chip_id == STM32_CHIPID_L152_RE) { /* stm32l */
1630         loader_code = loader_code_stm32l;
1631         loader_size = sizeof(loader_code_stm32l);
1632     } else if (sl->core_id == STM32VL_CORE_ID
1633             || sl->chip_id == STM32_CHIPID_F3
1634             || sl->chip_id == STM32_CHIPID_F3_SMALL
1635             || sl->chip_id == STM32_CHIPID_F303_HIGH
1636             || sl->chip_id == STM32_CHIPID_F37x
1637             || sl->chip_id == STM32_CHIPID_F334) {
1638         loader_code = loader_code_stm32vl;
1639         loader_size = sizeof(loader_code_stm32vl);
1640     } else if (sl->chip_id == STM32_CHIPID_F2 || sl->chip_id == STM32_CHIPID_F4 || (sl->chip_id == STM32_CHIPID_F4_DE) ||
1641             sl->chip_id == STM32_CHIPID_F4_LP || sl->chip_id == STM32_CHIPID_F4_HD || (sl->chip_id == STM32_CHIPID_F411RE) ||
1642             (sl->chip_id == STM32_CHIPID_F446) || (sl->chip_id == STM32_CHIPID_F4_DSI)){
1643         int voltage = stlink_target_voltage(sl);
1644         if (voltage == -1) {
1645             printf("Failed to read Target voltage\n");
1646             return voltage;
1647         } else if (voltage > 2700) {
1648             loader_code = loader_code_stm32f4;
1649             loader_size = sizeof(loader_code_stm32f4);
1650         } else {
1651             loader_code = loader_code_stm32f4_lv;
1652             loader_size = sizeof(loader_code_stm32f4_lv);
1653         }
1654     } else if (sl->chip_id == STM32_CHIPID_F7){
1655         loader_code = loader_code_stm32f7;
1656         loader_size = sizeof(loader_code_stm32f7);
1657     } 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) {
1658         loader_code = loader_code_stm32f0;
1659         loader_size = sizeof(loader_code_stm32f0);
1660     } else if (sl->chip_id == STM32_CHIPID_L0) {
1661         loader_code = loader_code_stm32l0;
1662         loader_size = sizeof(loader_code_stm32l0);
1663     } else if (sl->chip_id == STM32_CHIPID_L4) {
1664         loader_code = loader_code_stm32l4;
1665         loader_size = sizeof(loader_code_stm32l4);
1666     } else {
1667         ELOG("unknown coreid, not sure what flash loader to use, aborting!: %x\n", sl->core_id);
1668         return -1;
1669     }
1670
1671     memcpy(sl->q_buf, loader_code, loader_size);
1672     stlink_write_mem32(sl, sl->sram_base, loader_size);
1673
1674     *addr = sl->sram_base;
1675     *size = loader_size;
1676
1677     /* success */
1678     return 0;
1679 }
1680
1681 int stlink_fcheck_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
1682     /* check the contents of path are at addr */
1683
1684     int res;
1685     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1686
1687     if (map_file(&mf, path) == -1)
1688         return -1;
1689
1690     res = check_file(sl, &mf, addr);
1691
1692     unmap_file(&mf);
1693
1694     return res;
1695 }
1696
1697 /**
1698  * Verify addr..addr+len is binary identical to base...base+len
1699  * @param sl stlink context
1700  * @param address stm device address
1701  * @param data host side buffer to check against
1702  * @param length how much
1703  * @return 0 for success, -ve for failure
1704  */
1705 int stlink_verify_write_flash(stlink_t *sl, stm32_addr_t address, uint8_t *data, unsigned length) {
1706     size_t off;
1707     size_t cmp_size = (sl->flash_pgsz > 0x1800)? 0x1800:sl->flash_pgsz;
1708     ILOG("Starting verification of write complete\n");
1709     for (off = 0; off < length; off += cmp_size) {
1710         size_t aligned_size;
1711
1712         /* adjust last page size */
1713         if ((off + cmp_size) > length)
1714             cmp_size = length - off;
1715
1716         aligned_size = cmp_size;
1717         if (aligned_size & (4 - 1))
1718             aligned_size = (cmp_size + 4) & ~(4 - 1);
1719
1720         stlink_read_mem32(sl, address + off, aligned_size);
1721
1722         if (memcmp(sl->q_buf, data + off, cmp_size)) {
1723             ELOG("Verification of flash failed at offset: %zd\n", off);
1724             return -1;
1725         }
1726     }
1727     ILOG("Flash written and verified! jolly good!\n");
1728     return 0;
1729
1730 }
1731
1732 int stm32l1_write_half_pages(stlink_t *sl, stm32_addr_t addr, uint8_t* base, uint32_t len, uint32_t pagesize)
1733 {
1734     unsigned int count;
1735     unsigned int num_half_pages = len / pagesize;
1736     uint32_t val;
1737     uint32_t flash_regs_base;
1738     flash_loader_t fl;
1739
1740     if (sl->chip_id == STM32_CHIPID_L0) {
1741         flash_regs_base = STM32L0_FLASH_REGS_ADDR;
1742     } else {
1743         flash_regs_base = STM32L_FLASH_REGS_ADDR;
1744     }
1745
1746     ILOG("Starting Half page flash write for STM32L core id\n");
1747     /* flash loader initialization */
1748     if (init_flash_loader(sl, &fl) == -1) {
1749         WLOG("init_flash_loader() == -1\n");
1750         return -1;
1751     }
1752     /* Unlock already done */
1753     stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1754     val |= (1 << FLASH_L1_FPRG);
1755     stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
1756
1757     val |= (1 << FLASH_L1_PROG);
1758     stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
1759     do {
1760         stlink_read_debug32(sl, flash_regs_base + FLASH_SR_OFF, &val);
1761     } while ((val & (1 << 0)) != 0);
1762
1763     for (count = 0; count  < num_half_pages; count ++) {
1764         if (run_flash_loader(sl, &fl, addr + count * pagesize, base + count * pagesize, pagesize) == -1) {
1765             WLOG("l1_run_flash_loader(%#zx) failed! == -1\n", addr + count * pagesize);
1766             stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1767             val &= ~((1 << FLASH_L1_FPRG) |(1 << FLASH_L1_PROG));
1768             stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
1769             return -1;
1770         }
1771         /* wait for sr.busy to be cleared */
1772         if (sl->verbose >= 1) {
1773             /* show progress. writing procedure is slow
1774                and previous errors are misleading */
1775             fprintf(stdout, "\r%3u/%u halfpages written", count + 1, num_half_pages);
1776             fflush(stdout);
1777         }
1778         do {
1779             stlink_read_debug32(sl, flash_regs_base + FLASH_SR_OFF, &val);
1780         } while ((val & (1 << 0)) != 0);
1781     }
1782     stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1783     val &= ~(1 << FLASH_L1_PROG);
1784     stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
1785     stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1786     val &= ~(1 << FLASH_L1_FPRG);
1787     stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
1788
1789     return 0;
1790 }
1791
1792 int stlink_write_flash(stlink_t *sl, stm32_addr_t addr, uint8_t* base, uint32_t len, uint8_t eraseonly) {
1793     size_t off;
1794     flash_loader_t fl;
1795     ILOG("Attempting to write %d (%#x) bytes to stm32 address: %u (%#x)\n",
1796             len, len, addr, addr);
1797     /* check addr range is inside the flash */
1798     stlink_calculate_pagesize(sl, addr);
1799     if (addr < sl->flash_base) {
1800         ELOG("addr too low %#x < %#x\n", addr, sl->flash_base);
1801         return -1;
1802     } else if ((addr + len) < addr) {
1803         ELOG("addr overruns\n");
1804         return -1;
1805     } else if ((addr + len) > (sl->flash_base + sl->flash_size)) {
1806         ELOG("addr too high\n");
1807         return -1;
1808     } else if (addr & 1) {
1809         ELOG("unaligned addr 0x%x\n", addr);
1810         return -1;
1811     } else if (len & 1) {
1812         WLOG("unaligned len 0x%x -- padding with zero\n", len);
1813         len += 1;
1814     } else if (addr & (sl->flash_pgsz - 1)) {
1815         ELOG("addr not a multiple of pagesize, not supported\n");
1816         return -1;
1817     }
1818
1819     // Make sure we've loaded the context with the chip details
1820     stlink_core_id(sl);
1821     /* erase each page */
1822     int page_count = 0;
1823     for (off = 0; off < len; off += stlink_calculate_pagesize(sl, addr + off)) {
1824         /* addr must be an addr inside the page */
1825         if (stlink_erase_flash_page(sl, addr + off) == -1) {
1826             ELOG("Failed to erase_flash_page(%#zx) == -1\n", addr + off);
1827             return -1;
1828         }
1829         fprintf(stdout,"\rFlash page at addr: 0x%08lx erased",
1830                 (unsigned long)addr + off);
1831         fflush(stdout);
1832         page_count++;
1833     }
1834     fprintf(stdout,"\n");
1835     ILOG("Finished erasing %d pages of %d (%#x) bytes\n",
1836             page_count, sl->flash_pgsz, sl->flash_pgsz);
1837
1838     if (eraseonly)
1839         return 0;
1840
1841     if ((sl->flash_type == FLASH_TYPE_F4) || (sl->flash_type == FLASH_TYPE_L4)) {
1842         /* todo: check write operation */
1843
1844         ILOG("Starting Flash write for F2/F4/L4\n");
1845         /* flash loader initialization */
1846         if (init_flash_loader(sl, &fl) == -1) {
1847             ELOG("init_flash_loader() == -1\n");
1848             return -1;
1849         }
1850
1851         /* First unlock the cr */
1852         unlock_flash_if(sl);
1853
1854         /* TODO: Check that Voltage range is 2.7 - 3.6 V */
1855         if (sl->chip_id != STM32_CHIPID_L4) {
1856             /* set parallelisim to 32 bit*/
1857             int voltage = stlink_target_voltage(sl);
1858             if (voltage == -1) {
1859                 printf("Failed to read Target voltage\n");
1860                 return voltage;
1861             } else if (voltage > 2700) {
1862                 printf("enabling 32-bit flash writes\n");
1863                 write_flash_cr_psiz(sl, 2);
1864             } else {
1865                 printf("Target voltage (%d mV) too low for 32-bit flash, using 8-bit flash writes\n", voltage);
1866                 write_flash_cr_psiz(sl, 0);
1867             }
1868         } else {
1869             /* L4 does not have a byte-write mode */
1870             int voltage = stlink_target_voltage(sl);
1871             if (voltage == -1) {
1872                 printf("Failed to read Target voltage\n");
1873                 return voltage;
1874             } else if (voltage < 1710) {
1875                 printf("Target voltage (%d mV) too low for flash writes!\n", voltage);
1876                 return -1;
1877             }
1878         }
1879
1880         /* set programming mode */
1881         set_flash_cr_pg(sl);
1882
1883         for(off = 0; off < len;) {
1884             size_t size = len - off > 0x8000 ? 0x8000 : len - off;
1885
1886             printf("size: %zu\n", size);
1887
1888             if (run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
1889                 ELOG("run_flash_loader(%#zx) failed! == -1\n", addr + off);
1890                 return -1;
1891             }
1892
1893             off += size;
1894         }
1895
1896         /* Relock flash */
1897         lock_flash(sl);
1898
1899     }   //STM32F4END
1900
1901     else if (sl->flash_type == FLASH_TYPE_L0) {
1902         /* use fast word write. todo: half page. */
1903         uint32_t val;
1904         uint32_t flash_regs_base;
1905         uint32_t pagesize;
1906
1907         if (sl->chip_id == STM32_CHIPID_L0) {
1908             flash_regs_base = STM32L0_FLASH_REGS_ADDR;
1909             pagesize = L0_WRITE_BLOCK_SIZE;
1910         } else {
1911             flash_regs_base = STM32L_FLASH_REGS_ADDR;
1912             pagesize = L1_WRITE_BLOCK_SIZE;
1913         }
1914
1915         /* todo: check write operation */
1916
1917         /* disable pecr protection */
1918         stlink_write_debug32(sl, flash_regs_base + FLASH_PEKEYR_OFF, 0x89abcdef);
1919         stlink_write_debug32(sl, flash_regs_base + FLASH_PEKEYR_OFF, 0x02030405);
1920
1921         /* check pecr.pelock is cleared */
1922         stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1923         if (val & (1 << 0)) {
1924             fprintf(stderr, "pecr.pelock not clear\n");
1925             return -1;
1926         }
1927
1928         /* unlock program memory */
1929         stlink_write_debug32(sl, flash_regs_base + FLASH_PRGKEYR_OFF, 0x8c9daebf);
1930         stlink_write_debug32(sl, flash_regs_base + FLASH_PRGKEYR_OFF, 0x13141516);
1931
1932         /* check pecr.prglock is cleared */
1933         stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1934         if (val & (1 << 1)) {
1935             fprintf(stderr, "pecr.prglock not clear\n");
1936             return -1;
1937         }
1938         off = 0;
1939         if (len > pagesize) {
1940             if (stm32l1_write_half_pages(sl, addr, base, len, pagesize) == -1) {
1941                 /* This may happen on a blank device! */
1942                 WLOG("\nwrite_half_pages failed == -1\n");
1943             } else {
1944                 off = (len / pagesize)*pagesize;
1945             }
1946         }
1947
1948         /* write remainingword in program memory */
1949         for ( ; off < len; off += sizeof(uint32_t)) {
1950             uint32_t data;
1951             if (off > 254)
1952                 fprintf(stdout, "\r");
1953
1954             if ((off % sl->flash_pgsz) > (sl->flash_pgsz -5)) {
1955                 fprintf(stdout, "\r%3zd/%3zd pages written",
1956                         off/sl->flash_pgsz, len/sl->flash_pgsz);
1957                 fflush(stdout);
1958             }
1959
1960             write_uint32((unsigned char*) &data, *(uint32_t*) (base + off));
1961             stlink_write_debug32(sl, addr + off, data);
1962
1963             /* wait for sr.busy to be cleared */
1964             do {
1965                 stlink_read_debug32(sl, flash_regs_base + FLASH_SR_OFF, &val);
1966             } while ((val & (1 << 0)) != 0);
1967
1968             /* todo: check redo write operation */
1969
1970         }
1971         fprintf(stdout, "\n");
1972         /* reset lock bits */
1973         stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1974         val |= (1 << 0) | (1 << 1) | (1 << 2);
1975         stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
1976     } else if (sl->flash_type == FLASH_TYPE_F0) {
1977         ILOG("Starting Flash write for VL/F0/F3 core id\n");
1978         /* flash loader initialization */
1979         if (init_flash_loader(sl, &fl) == -1) {
1980             ELOG("init_flash_loader() == -1\n");
1981             return -1;
1982         }
1983
1984         int write_block_count = 0;
1985         for (off = 0; off < len; off += sl->flash_pgsz) {
1986             /* adjust last write size */
1987             size_t size = sl->flash_pgsz;
1988             if ((off + sl->flash_pgsz) > len) size = len - off;
1989
1990             /* unlock and set programming mode */
1991             unlock_flash_if(sl);
1992             set_flash_cr_pg(sl);
1993             //DLOG("Finished setting flash cr pg, running loader!\n");
1994             if (run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
1995                 ELOG("run_flash_loader(%#zx) failed! == -1\n", addr + off);
1996                 return -1;
1997             }
1998             lock_flash(sl);
1999             if (sl->verbose >= 1) {
2000                 /* show progress. writing procedure is slow
2001                    and previous errors are misleading */
2002                 fprintf(stdout, "\r%3u/%lu pages written", write_block_count++, (unsigned long)len/sl->flash_pgsz);
2003                 fflush(stdout);
2004             }
2005         }
2006         fprintf(stdout, "\n");
2007     } else {
2008         ELOG("unknown coreid, not sure how to write: %x\n", sl->core_id);
2009         return -1;
2010     }
2011
2012     return stlink_verify_write_flash(sl, addr, base, len);
2013 }
2014
2015 /**
2016  * Write the given binary file into flash at address "addr"
2017  * @param sl
2018  * @param path readable file path, should be binary image
2019  * @param addr where to start writing
2020  * @return 0 on success, -ve on failure.
2021  */
2022 int stlink_fwrite_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
2023     /* write the file in flash at addr */
2024     int err;
2025     unsigned int num_empty, index, val;
2026     unsigned char erased_pattern;
2027     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
2028
2029     if (map_file(&mf, path) == -1) {
2030         ELOG("map_file() == -1\n");
2031         return -1;
2032     }
2033
2034     if (sl->flash_type == FLASH_TYPE_L0)
2035         erased_pattern = 0x00;
2036     else
2037         erased_pattern = 0xff;
2038
2039     index = mf.len;
2040     for(num_empty = 0; num_empty != mf.len; ++num_empty) {
2041         if (mf.base[--index] != erased_pattern) {
2042             break;
2043         }
2044     }
2045     /* Round down to words */
2046     num_empty -= (num_empty & 3);
2047     if(num_empty != 0) {
2048         ILOG("Ignoring %d bytes of 0x%02x at end of file\n", num_empty, erased_pattern);
2049     }
2050     err = stlink_write_flash(sl, addr, mf.base, num_empty == mf.len? mf.len : mf.len - num_empty, num_empty == mf.len);
2051     /* set stack*/
2052     stlink_read_debug32(sl, addr, &val);
2053     stlink_write_reg(sl, val, 13);
2054     /* Set PC to the reset routine*/
2055     stlink_read_debug32(sl, addr + 4, &val);
2056     stlink_write_reg(sl, val, 15);
2057     stlink_run(sl);
2058     unmap_file(&mf);
2059     return err;
2060 }
2061
2062 int run_flash_loader(stlink_t *sl, flash_loader_t* fl, stm32_addr_t target, const uint8_t* buf, size_t size) {
2063
2064     reg rr;
2065     int i = 0;
2066     size_t count;
2067
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_F0) {
2077         count = size / sizeof(uint16_t);
2078         if (size % sizeof(uint16_t))
2079             ++count;
2080     } else if (sl->flash_type == FLASH_TYPE_F4 || sl->flash_type == FLASH_TYPE_L0) {
2081         count = size / sizeof(uint32_t);
2082         if (size % sizeof(uint32_t))
2083             ++count;
2084     } else if (sl->flash_type == FLASH_TYPE_L4) {
2085         count = size / sizeof(uint64_t);
2086         if (size % sizeof(uint64_t))
2087             ++count;
2088     } else {
2089         fprintf(stderr, "unknown coreid 0x%x, don't know what flash loader to use\n", sl->core_id);
2090         return -1;
2091     }
2092
2093     /* setup core */
2094     stlink_write_reg(sl, fl->buf_addr, 0); /* source */
2095     stlink_write_reg(sl, target, 1); /* target */
2096     stlink_write_reg(sl, count, 2); /* count */
2097     stlink_write_reg(sl, 0, 3); /* flash bank 0 (input), only used on F0, but armless fopr others */
2098     stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
2099
2100     /* run loader */
2101     stlink_run(sl);
2102
2103 #define WAIT_ROUNDS 10000
2104     /* wait until done (reaches breakpoint) */
2105     for (i = 0; i < WAIT_ROUNDS; i++) {
2106         usleep(10);
2107         if (is_core_halted(sl))
2108             break;
2109     }
2110
2111     if (i >= WAIT_ROUNDS) {
2112         ELOG("flash loader run error\n");
2113         return -1;
2114     }
2115
2116     /* check written byte count */
2117     stlink_read_reg(sl, 2, &rr);
2118     if (rr.r[2] != 0) {
2119         fprintf(stderr, "write error, count == %u\n", rr.r[2]);
2120         return -1;
2121     }
2122
2123     return 0;
2124 }