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 = FLASH_F4_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     if (params->flash_type == FLASH_TYPE_UNKNOWN) {
 595         WLOG("Invalid flash type, please check device declaration\n");
 596         return -1;
 597     }
 598
 599
 600     // These are fixed...
 601     sl->flash_base = STM32_FLASH_BASE;
 602     sl->sram_base = STM32_SRAM_BASE;
 603     stlink_read_debug32(sl,(params->flash_size_reg) & ~3, &flash_size);
 604     if (params->flash_size_reg & 2)
 605         flash_size = flash_size >>16;
 606     flash_size = flash_size & 0xffff;
 607
 608     if ((sl->chip_id == STM32_CHIPID_L1_MEDIUM || sl->chip_id == STM32_CHIPID_L1_MEDIUM_PLUS) && ( flash_size == 0 )) {
 609         sl->flash_size = 128 * 1024;
 610     } else if (sl->chip_id == STM32_CHIPID_L1_CAT2) {
 611         sl->flash_size = (flash_size & 0xff) * 1024;
 612     } else if ((sl->chip_id & 0xFFF) == STM32_CHIPID_L1_HIGH) {
 613         // 0 is 384k and 1 is 256k
 614         if ( flash_size == 0 ) {
 615             sl->flash_size = 384 * 1024;
 616         } else {
 617             sl->flash_size = 256 * 1024;
 618         }
 619     } else {
 620         sl->flash_size = flash_size * 1024;
 621     }
 622     sl->flash_type = params->flash_type;
 623     sl->flash_pgsz = params->flash_pagesize;
 624     sl->sram_size = params->sram_size;
 625     sl->sys_base = params->bootrom_base;
 626     sl->sys_size = params->bootrom_size;
 627
 628     //medium and low devices have the same chipid. ram size depends on flash size.
 629     //STM32F100xx datasheet Doc ID 16455 Table 2
 630     if(sl->chip_id == STM32_CHIPID_F1_VL_MEDIUM_LOW && sl->flash_size < 64 * 1024){
 631         sl->sram_size = 0x1000;
 632     }
 633
 634     ILOG("Device connected is: %s, id %#x\n", params->description, chip_id);
 635     // TODO make note of variable page size here.....
 636     ILOG("SRAM size: %#x bytes (%d KiB), Flash: %#x bytes (%d KiB) in pages of %zd bytes\n",
 637             sl->sram_size, sl->sram_size / 1024, sl->flash_size, sl->flash_size / 1024,
 638             sl->flash_pgsz);
 639     return 0;
 640 }
 641
 642 int stlink_reset(stlink_t *sl) {
 643     DLOG("*** stlink_reset ***\n");
 644     return sl->backend->reset(sl);
 645 }
 646
 647 int stlink_jtag_reset(stlink_t *sl, int value) {
 648     DLOG("*** stlink_jtag_reset ***\n");
 649     return sl->backend->jtag_reset(sl, value);
 650 }
 651
 652 int stlink_run(stlink_t *sl) {
 653     DLOG("*** stlink_run ***\n");
 654     return sl->backend->run(sl);
 655 }
 656
 657 int stlink_status(stlink_t *sl) {
 658     int ret;
 659
 660     DLOG("*** stlink_status ***\n");
 661     ret = sl->backend->status(sl);
 662     stlink_core_stat(sl);
 663
 664     return ret;
 665 }
 666
 667 /**
 668  * Decode the version bits, originally from -sg, verified with usb
 669  * @param sl stlink context, assumed to contain valid data in the buffer
 670  * @param slv output parsed version object
 671  */
 672 void _parse_version(stlink_t *sl, stlink_version_t *slv) {
 673     uint32_t b0 = sl->q_buf[0]; //lsb
 674     uint32_t b1 = sl->q_buf[1];
 675     uint32_t b2 = sl->q_buf[2];
 676     uint32_t b3 = sl->q_buf[3];
 677     uint32_t b4 = sl->q_buf[4];
 678     uint32_t b5 = sl->q_buf[5]; //msb
 679
 680     // b0 b1                       || b2 b3  | b4 b5
 681     // 4b        | 6b     | 6b     || 2B     | 2B
 682     // stlink_v  | jtag_v | swim_v || st_vid | stlink_pid
 683
 684     slv->stlink_v = (b0 & 0xf0) >> 4;
 685     slv->jtag_v = ((b0 & 0x0f) << 2) | ((b1 & 0xc0) >> 6);
 686     slv->swim_v = b1 & 0x3f;
 687     slv->st_vid = (b3 << 8) | b2;
 688     slv->stlink_pid = (b5 << 8) | b4;
 689     return;
 690 }
 691
 692 int stlink_version(stlink_t *sl) {
 693     DLOG("*** looking up stlink version\n");
 694     if (sl->backend->version(sl))
 695         return -1;
 696
 697     _parse_version(sl, &sl->version);
 698
 699     DLOG("st vid         = 0x%04x (expect 0x%04x)\n", sl->version.st_vid, USB_ST_VID);
 700     DLOG("stlink pid     = 0x%04x\n", sl->version.stlink_pid);
 701     DLOG("stlink version = 0x%x\n", sl->version.stlink_v);
 702     DLOG("jtag version   = 0x%x\n", sl->version.jtag_v);
 703     DLOG("swim version   = 0x%x\n", sl->version.swim_v);
 704     if (sl->version.jtag_v == 0) {
 705         DLOG("    notice: the firmware doesn't support a jtag/swd interface\n");
 706     }
 707     if (sl->version.swim_v == 0) {
 708         DLOG("    notice: the firmware doesn't support a swim interface\n");
 709     }
 710
 711     return 0;
 712 }
 713
 714 int stlink_target_voltage(stlink_t *sl) {
 715     int voltage = -1;
 716     DLOG("*** reading target voltage\n");
 717     if (sl->backend->target_voltage != NULL) {
 718         voltage = sl->backend->target_voltage(sl);
 719         if (voltage != -1) {
 720             DLOG("target voltage = %ldmV\n", voltage);
 721         } else {
 722             DLOG("error reading target voltage\n");
 723         }
 724     } else {
 725         DLOG("reading voltage not supported by backend\n");
 726     }
 727     return voltage;
 728 }
 729
 730 int stlink_read_debug32(stlink_t *sl, uint32_t addr, uint32_t *data) {
 731     int ret;
 732
 733     ret = sl->backend->read_debug32(sl, addr, data);
 734     if (!ret)
 735             DLOG("*** stlink_read_debug32 %x is %#x\n", data, addr);
 736
 737         return ret;
 738 }
 739
 740 int stlink_write_debug32(stlink_t *sl, uint32_t addr, uint32_t data) {
 741     DLOG("*** stlink_write_debug32 %x to %#x\n", data, addr);
 742     return sl->backend->write_debug32(sl, addr, data);
 743 }
 744
 745 int stlink_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 746     DLOG("*** stlink_write_mem32 %u bytes to %#x\n", len, addr);
 747     if (len % 4 != 0) {
 748         fprintf(stderr, "Error: Data length doesn't have a 32 bit alignment: +%d byte.\n", len % 4);
 749         abort();
 750     }
 751     return sl->backend->write_mem32(sl, addr, len);
 752 }
 753
 754 int stlink_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 755     DLOG("*** stlink_read_mem32 ***\n");
 756     if (len % 4 != 0) { // !!! never ever: fw gives just wrong values
 757         fprintf(stderr, "Error: Data length doesn't have a 32 bit alignment: +%d byte.\n",
 758                 len % 4);
 759         abort();
 760     }
 761     return sl->backend->read_mem32(sl, addr, len);
 762 }
 763
 764 int stlink_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
 765     DLOG("*** stlink_write_mem8 ***\n");
 766     if (len > 0x40 ) { // !!! never ever: Writing more then 0x40 bytes gives unexpected behaviour
 767         fprintf(stderr, "Error: Data length > 64: +%d byte.\n",
 768                 len);
 769         abort();
 770     }
 771     return sl->backend->write_mem8(sl, addr, len);
 772 }
 773
 774 int stlink_read_all_regs(stlink_t *sl, reg *regp) {
 775     DLOG("*** stlink_read_all_regs ***\n");
 776     return sl->backend->read_all_regs(sl, regp);
 777 }
 778
 779 int stlink_read_all_unsupported_regs(stlink_t *sl, reg *regp) {
 780     DLOG("*** stlink_read_all_unsupported_regs ***\n");
 781     return sl->backend->read_all_unsupported_regs(sl, regp);
 782 }
 783
 784 int stlink_write_reg(stlink_t *sl, uint32_t reg, int idx) {
 785     DLOG("*** stlink_write_reg\n");
 786     return sl->backend->write_reg(sl, reg, idx);
 787 }
 788
 789 int stlink_read_reg(stlink_t *sl, int r_idx, reg *regp) {
 790     DLOG("*** stlink_read_reg\n");
 791     DLOG(" (%d) ***\n", r_idx);
 792
 793     if (r_idx > 20 || r_idx < 0) {
 794         fprintf(stderr, "Error: register index must be in [0..20]\n");
 795         return -1;
 796     }
 797
 798     return sl->backend->read_reg(sl, r_idx, regp);
 799 }
 800
 801 int stlink_read_unsupported_reg(stlink_t *sl, int r_idx, reg *regp) {
 802     int r_convert;
 803
 804     DLOG("*** stlink_read_unsupported_reg\n");
 805     DLOG(" (%d) ***\n", r_idx);
 806
 807     /* Convert to values used by DCRSR */
 808     if (r_idx >= 0x1C && r_idx <= 0x1F) { /* primask, basepri, faultmask, or control */
 809         r_convert = 0x14;
 810     } else if (r_idx == 0x40) {     /* FPSCR */
 811         r_convert = 0x21;
 812     } else if (r_idx >= 0x20 && r_idx < 0x40) {
 813         r_convert = 0x40 + (r_idx - 0x20);
 814     } else {
 815         fprintf(stderr, "Error: register address must be in [0x1C..0x40]\n");
 816         return -1;
 817     }
 818
 819     return sl->backend->read_unsupported_reg(sl, r_convert, regp);
 820 }
 821
 822 int stlink_write_unsupported_reg(stlink_t *sl, uint32_t val, int r_idx, reg *regp) {
 823     int r_convert;
 824
 825     DLOG("*** stlink_write_unsupported_reg\n");
 826     DLOG(" (%d) ***\n", r_idx);
 827
 828     /* Convert to values used by DCRSR */
 829     if (r_idx >= 0x1C && r_idx <= 0x1F) { /* primask, basepri, faultmask, or control */
 830         r_convert = r_idx;  /* The backend function handles this */
 831     } else if (r_idx == 0x40) {     /* FPSCR */
 832         r_convert = 0x21;
 833     } else if (r_idx >= 0x20 && r_idx < 0x40) {
 834         r_convert = 0x40 + (r_idx - 0x20);
 835     } else {
 836         fprintf(stderr, "Error: register address must be in [0x1C..0x40]\n");
 837         return -1;
 838     }
 839
 840     return sl->backend->write_unsupported_reg(sl, val, r_convert, regp);
 841 }
 842
 843 unsigned int is_core_halted(stlink_t *sl) {
 844     /* return non zero if core is halted */
 845     stlink_status(sl);
 846     return sl->q_buf[0] == STLINK_CORE_HALTED;
 847 }
 848
 849 int stlink_step(stlink_t *sl) {
 850     DLOG("*** stlink_step ***\n");
 851     return sl->backend->step(sl);
 852 }
 853
 854 int stlink_current_mode(stlink_t *sl) {
 855     int mode = sl->backend->current_mode(sl);
 856     switch (mode) {
 857     case STLINK_DEV_DFU_MODE:
 858         DLOG("stlink current mode: dfu\n");
 859         return mode;
 860     case STLINK_DEV_DEBUG_MODE:
 861         DLOG("stlink current mode: debug (jtag or swd)\n");
 862         return mode;
 863     case STLINK_DEV_MASS_MODE:
 864         DLOG("stlink current mode: mass\n");
 865         return mode;
 866     }
 867     DLOG("stlink mode: unknown!\n");
 868     return STLINK_DEV_UNKNOWN_MODE;
 869 }
 870
 871
 872
 873
 874 // End of delegates....  Common code below here...
 875
 876 // Endianness
 877 // http://www.ibm.com/developerworks/aix/library/au-endianc/index.html
 878 // const int i = 1;
 879 // #define is_bigendian() ( (*(char*)&i) == 0 )
 880
 881 inline unsigned int is_bigendian(void) {
 882     static volatile const unsigned int i = 1;
 883     return *(volatile const char*) &i == 0;
 884 }
 885
 886 uint16_t read_uint16(const unsigned char *c, const int pt) {
 887     uint32_t ui;
 888     char *p = (char *) &ui;
 889
 890     if (!is_bigendian()) { // le -> le (don't swap)
 891         p[0] = c[pt + 0];
 892         p[1] = c[pt + 1];
 893     } else {
 894         p[0] = c[pt + 1];
 895         p[1] = c[pt + 0];
 896     }
 897     return ui;
 898 }
 899
 900 // same as above with entrypoint.
 901
 902 void stlink_run_at(stlink_t *sl, stm32_addr_t addr) {
 903     stlink_write_reg(sl, addr, 15); /* pc register */
 904
 905     stlink_run(sl);
 906
 907     while (is_core_halted(sl) == 0)
 908         usleep(3000000);
 909 }
 910
 911 void stlink_core_stat(stlink_t *sl) {
 912     if (sl->q_len <= 0)
 913         return;
 914
 915     switch (sl->q_buf[0]) {
 916     case STLINK_CORE_RUNNING:
 917         sl->core_stat = STLINK_CORE_RUNNING;
 918         DLOG("  core status: running\n");
 919         return;
 920     case STLINK_CORE_HALTED:
 921         sl->core_stat = STLINK_CORE_HALTED;
 922         DLOG("  core status: halted\n");
 923         return;
 924     default:
 925         sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
 926         fprintf(stderr, "  core status: unknown\n");
 927     }
 928 }
 929
 930 void stlink_print_data(stlink_t * sl) {
 931     if (sl->q_len <= 0 || sl->verbose < UDEBUG)
 932         return;
 933     if (sl->verbose > 2)
 934         fprintf(stdout, "data_len = %d 0x%x\n", sl->q_len, sl->q_len);
 935
 936     for (int i = 0; i < sl->q_len; i++) {
 937         if (i % 16 == 0) {
 938             /*
 939                if (sl->q_data_dir == Q_DATA_OUT)
 940                fprintf(stdout, "\n<- 0x%08x ", sl->q_addr + i);
 941                else
 942                fprintf(stdout, "\n-> 0x%08x ", sl->q_addr + i);
 943                */
 944         }
 945         fprintf(stdout, " %02x", (unsigned int) sl->q_buf[i]);
 946     }
 947     fputs("\n\n", stdout);
 948 }
 949
 950 /* memory mapped file */
 951
 952 typedef struct mapped_file {
 953     uint8_t* base;
 954     size_t len;
 955 } mapped_file_t;
 956
 957 #define MAPPED_FILE_INITIALIZER { NULL, 0 }
 958
 959 static int map_file(mapped_file_t* mf, const char* path) {
 960     int error = -1;
 961     struct stat st;
 962
 963     const int fd = open(path, O_RDONLY | O_BINARY);
 964     if (fd == -1) {
 965         fprintf(stderr, "open(%s) == -1\n", path);
 966         return -1;
 967     }
 968
 969     if (fstat(fd, &st) == -1) {
 970         fprintf(stderr, "fstat() == -1\n");
 971         goto on_error;
 972     }
 973
 974     mf->base = (uint8_t*) mmap(NULL, st.st_size, PROT_READ, MAP_SHARED, fd, 0);
 975     if (mf->base == MAP_FAILED) {
 976         fprintf(stderr, "mmap() == MAP_FAILED\n");
 977         goto on_error;
 978     }
 979
 980     mf->len = st.st_size;
 981
 982     /* success */
 983     error = 0;
 984
 985 on_error:
 986     close(fd);
 987
 988     return error;
 989 }
 990
 991 static void unmap_file(mapped_file_t * mf) {
 992     munmap((void*) mf->base, mf->len);
 993     mf->base = (unsigned char*) MAP_FAILED;
 994     mf->len = 0;
 995 }
 996
 997 /* Limit the block size to compare to 0x1800
 998    Anything larger will stall the STLINK2
 999    Maybe STLINK V1 needs smaller value!*/
1000 static int check_file(stlink_t* sl, mapped_file_t* mf, stm32_addr_t addr) {
1001     size_t off;
1002     size_t n_cmp = sl->flash_pgsz;
1003     if ( n_cmp > 0x1800)
1004         n_cmp = 0x1800;
1005
1006     for (off = 0; off < mf->len; off += n_cmp) {
1007         size_t aligned_size;
1008
1009         /* adjust last page size */
1010         size_t cmp_size = n_cmp;
1011         if ((off + n_cmp) > mf->len)
1012             cmp_size = mf->len - off;
1013
1014         aligned_size = cmp_size;
1015         if (aligned_size & (4 - 1))
1016             aligned_size = (cmp_size + 4) & ~(4 - 1);
1017
1018         stlink_read_mem32(sl, addr + off, aligned_size);
1019
1020         if (memcmp(sl->q_buf, mf->base + off, cmp_size))
1021             return -1;
1022     }
1023
1024     return 0;
1025 }
1026
1027 int stlink_fwrite_sram
1028 (stlink_t * sl, const char* path, stm32_addr_t addr) {
1029     /* write the file in sram at addr */
1030
1031     int error = -1;
1032     size_t off;
1033     size_t len;
1034     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1035     uint32_t val;
1036
1037
1038     if (map_file(&mf, path) == -1) {
1039         fprintf(stderr, "map_file() == -1\n");
1040         return -1;
1041     }
1042
1043     /* check addr range is inside the sram */
1044     if (addr < sl->sram_base) {
1045         fprintf(stderr, "addr too low\n");
1046         goto on_error;
1047     } else if ((addr + mf.len) < addr) {
1048         fprintf(stderr, "addr overruns\n");
1049         goto on_error;
1050     } else if ((addr + mf.len) > (sl->sram_base + sl->sram_size)) {
1051         fprintf(stderr, "addr too high\n");
1052         goto on_error;
1053     } else if (addr & 3) {
1054         /* todo */
1055         fprintf(stderr, "unaligned addr\n");
1056         goto on_error;
1057     }
1058
1059     len = mf.len;
1060
1061     if(len & 3) {
1062       len -= len & 3;
1063     }
1064
1065     /* do the copy by 1k blocks */
1066     for (off = 0; off < len; off += 1024) {
1067         size_t size = 1024;
1068         if ((off + size) > len)
1069             size = len - off;
1070
1071         memcpy(sl->q_buf, mf.base + off, size);
1072
1073         /* round size if needed */
1074         if (size & 3)
1075             size += 2;
1076
1077         stlink_write_mem32(sl, addr + off, size);
1078     }
1079
1080     if(mf.len > len) {
1081         memcpy(sl->q_buf, mf.base + len, mf.len - len);
1082         stlink_write_mem8(sl, addr + len, mf.len - len);
1083     }
1084
1085     /* check the file ha been written */
1086     if (check_file(sl, &mf, addr) == -1) {
1087         fprintf(stderr, "check_file() == -1\n");
1088         goto on_error;
1089     }
1090
1091     /* success */
1092     error = 0;
1093     /* set stack*/
1094     stlink_read_debug32(sl, addr, &val);
1095     stlink_write_reg(sl, val, 13);
1096     /* Set PC to the reset routine*/
1097     stlink_read_debug32(sl, addr + 4, &val);
1098     stlink_write_reg(sl, val, 15);
1099     stlink_run(sl);
1100
1101 on_error:
1102     unmap_file(&mf);
1103     return error;
1104 }
1105
1106 int stlink_fread(stlink_t* sl, const char* path, stm32_addr_t addr, size_t size) {
1107     /* read size bytes from addr to file */
1108
1109     int error = -1;
1110     size_t off;
1111
1112     const int fd = open(path, O_RDWR | O_TRUNC | O_CREAT, 00700);
1113     if (fd == -1) {
1114         fprintf(stderr, "open(%s) == -1\n", path);
1115         return -1;
1116     }
1117
1118     if (size <1)
1119         size = sl->flash_size;
1120
1121     if (size > sl->flash_size)
1122         size = sl->flash_size;
1123
1124     size_t cmp_size = (sl->flash_pgsz > 0x1800)? 0x1800:sl->flash_pgsz;
1125     for (off = 0; off < size; off += cmp_size) {
1126         size_t aligned_size;
1127
1128         /* adjust last page size */
1129         if ((off + cmp_size) > size)
1130             cmp_size = size - off;
1131
1132         aligned_size = cmp_size;
1133         if (aligned_size & (4 - 1))
1134             aligned_size = (cmp_size + 4) & ~(4 - 1);
1135
1136         stlink_read_mem32(sl, addr + off, aligned_size);
1137
1138         if (write(fd, sl->q_buf, sl->q_len) != (ssize_t) aligned_size) {
1139             fprintf(stderr, "write() != aligned_size\n");
1140             goto on_error;
1141         }
1142     }
1143
1144     /* success */
1145     error = 0;
1146
1147 on_error:
1148     close(fd);
1149
1150     return error;
1151 }
1152
1153 int write_buffer_to_sram(stlink_t *sl, flash_loader_t* fl, const uint8_t* buf, size_t size) {
1154     /* write the buffer right after the loader */
1155     size_t chunk = size & ~0x3;
1156     size_t rem   = size & 0x3;
1157     if (chunk) {
1158         memcpy(sl->q_buf, buf, chunk);
1159         stlink_write_mem32(sl, fl->buf_addr, chunk);
1160     }
1161     if (rem) {
1162         memcpy(sl->q_buf, buf+chunk, rem);
1163         stlink_write_mem8(sl, (fl->buf_addr)+chunk, rem);
1164     }
1165     return 0;
1166 }
1167
1168 uint32_t calculate_F4_sectornum(uint32_t flashaddr){
1169     uint32_t offset = 0;
1170     flashaddr &= ~STM32_FLASH_BASE;     //Page now holding the actual flash address
1171     if (flashaddr >= 0x100000) {
1172         offset = 12;
1173         flashaddr -= 0x100000;
1174     }
1175     if (flashaddr<0x4000) return (offset + 0);
1176     else if(flashaddr<0x8000) return(offset + 1);
1177     else if(flashaddr<0xc000) return(offset + 2);
1178     else if(flashaddr<0x10000) return(offset + 3);
1179     else if(flashaddr<0x20000) return(offset + 4);
1180     else return offset + (flashaddr/0x20000) +4;
1181
1182 }
1183
1184 uint32_t calculate_F7_sectornum(uint32_t flashaddr){
1185     flashaddr &= ~STM32_FLASH_BASE;     //Page now holding the actual flash address
1186         if(flashaddr<0x20000) return(flashaddr/0x8000);
1187     else if(flashaddr<0x40000) return(4);
1188     else return(flashaddr/0x40000) +4;
1189
1190 }
1191
1192 // Returns BKER:PNB for the given page address
1193 uint32_t calculate_L4_page(stlink_t *sl, uint32_t flashaddr) {
1194     uint32_t bker = 0;
1195     uint32_t flashopt;
1196     stlink_read_debug32(sl, STM32L4_FLASH_OPTR, &flashopt);
1197     flashaddr -= STM32_FLASH_BASE;
1198     if (flashopt & (1lu << STM32L4_FLASH_OPTR_DUALBANK)) {
1199         uint32_t banksize = sl->flash_size / 2;
1200         if (flashaddr > banksize) {
1201             flashaddr -= banksize;
1202             bker = 0x100;
1203         }
1204     }
1205     // For 1MB chips without the dual-bank option set, the page address will
1206     // overflow into the BKER bit, which gives us the correct bank:page value.
1207     return bker | flashaddr/sl->flash_pgsz;
1208 }
1209
1210 uint32_t stlink_calculate_pagesize(stlink_t *sl, uint32_t flashaddr){
1211     if ((sl->chip_id == STM32_CHIPID_F2) || (sl->chip_id == STM32_CHIPID_F4) || (sl->chip_id == STM32_CHIPID_F4_DE) ||
1212             (sl->chip_id == STM32_CHIPID_F4_LP) || (sl->chip_id == STM32_CHIPID_F4_HD) || (sl->chip_id == STM32_CHIPID_F411RE) ||
1213             (sl->chip_id == STM32_CHIPID_F446) || (sl->chip_id == STM32_CHIPID_F4_DSI)) {
1214         uint32_t sector=calculate_F4_sectornum(flashaddr);
1215         if (sector>= 12) {
1216             sector -= 12;
1217         }
1218         if (sector<4) sl->flash_pgsz=0x4000;
1219         else if(sector<5) sl->flash_pgsz=0x10000;
1220         else sl->flash_pgsz=0x20000;
1221     }
1222     else if (sl->chip_id == STM32_CHIPID_F7) {
1223         uint32_t sector=calculate_F7_sectornum(flashaddr);
1224         if (sector<4) sl->flash_pgsz=0x8000;
1225         else if(sector<5) sl->flash_pgsz=0x20000;
1226         else sl->flash_pgsz=0x40000;
1227     }
1228     return (sl->flash_pgsz);
1229 }
1230
1231 /**
1232  * Erase a page of flash, assumes sl is fully populated with things like chip/core ids
1233  * @param sl stlink context
1234  * @param flashaddr an address in the flash page to erase
1235  * @return 0 on success -ve on failure
1236  */
1237 int stlink_erase_flash_page(stlink_t *sl, stm32_addr_t flashaddr)
1238 {
1239     if (sl->flash_type == FLASH_TYPE_F4 || sl->flash_type == FLASH_TYPE_L4) {
1240         /* wait for ongoing op to finish */
1241         wait_flash_busy(sl);
1242
1243         /* unlock if locked */
1244         unlock_flash_if(sl);
1245
1246         /* select the page to erase */
1247         if (sl->chip_id == STM32_CHIPID_L4) {
1248             // calculate the actual bank+page from the address
1249             uint32_t page = calculate_L4_page(sl, flashaddr);
1250
1251             write_flash_cr_bker_pnb(sl, page);
1252         } else if (sl->chip_id == STM32_CHIPID_F7) {
1253             // calculate the actual page from the address
1254             uint32_t sector=calculate_F7_sectornum(flashaddr);
1255
1256             fprintf(stderr, "EraseFlash - Sector:0x%x Size:0x%x\n", sector, stlink_calculate_pagesize(sl, flashaddr));
1257
1258             write_flash_cr_snb(sl, sector);
1259         } else {
1260             // calculate the actual page from the address
1261             uint32_t sector=calculate_F4_sectornum(flashaddr);
1262
1263             fprintf(stderr, "EraseFlash - Sector:0x%x Size:0x%x\n", sector, stlink_calculate_pagesize(sl, flashaddr));
1264
1265             //the SNB values for flash sectors in the second bank do not directly follow the values for the first bank on 2mb devices...
1266             if (sector >= 12) sector += 4;
1267
1268             write_flash_cr_snb(sl, sector);
1269         }
1270
1271         /* start erase operation */
1272         set_flash_cr_strt(sl);
1273
1274         /* wait for completion */
1275         wait_flash_busy(sl);
1276
1277         /* relock the flash */
1278         //todo: fails to program if this is in
1279         lock_flash(sl);
1280 #if DEBUG_FLASH
1281         fprintf(stdout, "Erase Final CR:0x%x\n", read_flash_cr(sl));
1282 #endif
1283     } else if (sl->flash_type == FLASH_TYPE_L0) {
1284
1285         uint32_t val;
1286         uint32_t flash_regs_base;
1287         if (sl->chip_id == STM32_CHIPID_L0) {
1288             flash_regs_base = STM32L0_FLASH_REGS_ADDR;
1289         } else {
1290             flash_regs_base = STM32L_FLASH_REGS_ADDR;
1291         }
1292
1293         /* check if the locks are set */
1294         stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1295         if((val & (1<<0))||(val & (1<<1))) {
1296             /* disable pecr protection */
1297             stlink_write_debug32(sl, flash_regs_base + FLASH_PEKEYR_OFF, 0x89abcdef);
1298             stlink_write_debug32(sl, flash_regs_base + FLASH_PEKEYR_OFF, 0x02030405);
1299
1300             /* check pecr.pelock is cleared */
1301             stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1302             if (val & (1 << 0)) {
1303                 WLOG("pecr.pelock not clear (%#x)\n", val);
1304                 return -1;
1305             }
1306
1307             /* unlock program memory */
1308             stlink_write_debug32(sl, flash_regs_base + FLASH_PRGKEYR_OFF, 0x8c9daebf);
1309             stlink_write_debug32(sl, flash_regs_base + FLASH_PRGKEYR_OFF, 0x13141516);
1310
1311             /* check pecr.prglock is cleared */
1312             stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1313             if (val & (1 << 1)) {
1314                 WLOG("pecr.prglock not clear (%#x)\n", val);
1315                 return -1;
1316             }
1317         }
1318
1319         /* set pecr.{erase,prog} */
1320         val |= (1 << 9) | (1 << 3);
1321         stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
1322 #if 0 /* fix_to_be_confirmed */
1323
1324         /* wait for sr.busy to be cleared
1325          * MP: Test shows that busy bit is not set here. Perhaps, PM0062 is
1326          * wrong and we do not need to wait here for clearing the busy bit.
1327          * TEXANE: ok, if experience says so and it works for you, we comment
1328          * it. If someone has a problem, please drop an email.
1329          */
1330         do {
1331             stlink_read_debug32(sl, STM32L_FLASH_SR, &val)
1332         } while((val & (1 << 0)) != 0);
1333
1334 #endif /* fix_to_be_confirmed */
1335
1336         /* write 0 to the first word of the page to be erased */
1337         stlink_write_debug32(sl, flashaddr, 0);
1338
1339         /* MP: It is better to wait for clearing the busy bit after issuing
1340            page erase command, even though PM0062 recommends to wait before it.
1341            Test shows that a few iterations is performed in the following loop
1342            before busy bit is cleared.*/
1343         do {
1344             stlink_read_debug32(sl, flash_regs_base + FLASH_SR_OFF, &val);
1345         } while ((val & (1 << 0)) != 0);
1346
1347         /* reset lock bits */
1348         stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1349         val |= (1 << 0) | (1 << 1) | (1 << 2);
1350         stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
1351     } else if (sl->flash_type == FLASH_TYPE_F0)  {
1352         /* wait for ongoing op to finish */
1353         wait_flash_busy(sl);
1354
1355         /* unlock if locked */
1356         unlock_flash_if(sl);
1357
1358         /* set the page erase bit */
1359         set_flash_cr_per(sl);
1360
1361         /* select the page to erase */
1362         write_flash_ar(sl, flashaddr);
1363
1364         /* start erase operation, reset by hw with bsy bit */
1365         set_flash_cr_strt(sl);
1366
1367         /* wait for completion */
1368         wait_flash_busy(sl);
1369
1370         /* relock the flash */
1371         lock_flash(sl);
1372     } else {
1373         WLOG("unknown coreid %x, page erase failed\n", sl->core_id);
1374         return -1;
1375     }
1376
1377     /* todo: verify the erased page */
1378
1379     return 0;
1380 }
1381
1382 int stlink_erase_flash_mass(stlink_t *sl) {
1383     if (sl->flash_type == FLASH_TYPE_L0) {
1384         /* erase each page */
1385         int i = 0, num_pages = sl->flash_size/sl->flash_pgsz;
1386         for (i = 0; i < num_pages; i++) {
1387             /* addr must be an addr inside the page */
1388             stm32_addr_t addr = sl->flash_base + i * sl->flash_pgsz;
1389             if (stlink_erase_flash_page(sl, addr) == -1) {
1390                 WLOG("Failed to erase_flash_page(%#zx) == -1\n", addr);
1391                 return -1;
1392             }
1393             fprintf(stdout,"\rFlash page at %5d/%5d erased", i, num_pages);
1394             fflush(stdout);
1395         }
1396         fprintf(stdout, "\n");
1397     } else {
1398         /* wait for ongoing op to finish */
1399         wait_flash_busy(sl);
1400
1401         /* unlock if locked */
1402         unlock_flash_if(sl);
1403
1404         /* set the mass erase bit */
1405         set_flash_cr_mer(sl);
1406
1407         /* start erase operation, reset by hw with bsy bit */
1408         set_flash_cr_strt(sl);
1409
1410         /* wait for completion */
1411         wait_flash_busy_progress(sl);
1412
1413         /* relock the flash */
1414         lock_flash(sl);
1415
1416         /* todo: verify the erased memory */
1417     }
1418     return 0;
1419 }
1420
1421 int init_flash_loader(stlink_t *sl, flash_loader_t* fl) {
1422     size_t size;
1423
1424     /* allocate the loader in sram */
1425     if (write_loader_to_sram(sl, &fl->loader_addr, &size) == -1) {
1426         WLOG("Failed to write flash loader to sram!\n");
1427         return -1;
1428     }
1429
1430     /* allocate a one page buffer in sram right after loader */
1431     fl->buf_addr = fl->loader_addr + size;
1432     ILOG("Successfully loaded flash loader in sram\n");
1433     return 0;
1434 }
1435
1436 int write_loader_to_sram(stlink_t *sl, stm32_addr_t* addr, size_t* size) {
1437     /* from openocd, contrib/loaders/flash/stm32.s */
1438     static const uint8_t loader_code_stm32vl[] = {
1439         0x08, 0x4c, /* ldr      r4, STM32_FLASH_BASE */
1440         0x1c, 0x44, /* add      r4, r3 */
1441         /* write_half_word: */
1442         0x01, 0x23, /* movs     r3, #0x01 */
1443         0x23, 0x61, /* str      r3, [r4, #STM32_FLASH_CR_OFFSET] */
1444         0x30, 0xf8, 0x02, 0x3b, /* ldrh r3, [r0], #0x02 */
1445         0x21, 0xf8, 0x02, 0x3b, /* strh r3, [r1], #0x02 */
1446         /* busy: */
1447         0xe3, 0x68, /* ldr      r3, [r4, #STM32_FLASH_SR_OFFSET] */
1448         0x13, 0xf0, 0x01, 0x0f, /* tst  r3, #0x01 */
1449         0xfb, 0xd0, /* beq      busy */
1450         0x13, 0xf0, 0x14, 0x0f, /* tst  r3, #0x14 */
1451         0x01, 0xd1, /* bne      exit */
1452         0x01, 0x3a, /* subs     r2, r2, #0x01 */
1453         0xf0, 0xd1, /* bne      write_half_word */
1454         /* exit: */
1455         0x00, 0xbe, /* bkpt     #0x00 */
1456         0x00, 0x20, 0x02, 0x40, /* STM32_FLASH_BASE: .word 0x40022000 */
1457     };
1458
1459     /* flashloaders/stm32f0.s -- thumb1 only, same sequence as for STM32VL, bank ignored */
1460     static const uint8_t loader_code_stm32f0[] = {
1461 #if 1
1462         /*
1463          * These two NOPs here are a safety precaution, added by Pekka Nikander
1464          * while debugging the STM32F05x support.  They may not be needed, but
1465          * there were strange problems with simpler programs, like a program
1466          * that had just a breakpoint or a program that first moved zero to register r2
1467          * and then had a breakpoint.  So, it appears safest to have these two nops.
1468          *
1469          * Feel free to remove them, if you dare, but then please do test the result
1470          * rigorously.  Also, if you remove these, it may be a good idea first to
1471          * #if 0 them out, with a comment when these were taken out, and to remove
1472          * these only a few months later...  But YMMV.
1473          */
1474         0x00, 0x30, //     nop     /* add r0,#0 */
1475         0x00, 0x30, //     nop     /* add r0,#0 */
1476 #endif
1477         0x0A, 0x4C, //     ldr     r4, STM32_FLASH_BASE
1478         0x01, 0x25, //     mov     r5, #1            /*  FLASH_CR_PG, FLASH_SR_BUSY */
1479         0x04, 0x26, //     mov     r6, #4            /*  PGERR  */
1480         // write_half_word:
1481         0x23, 0x69, //     ldr     r3, [r4, #16]     /*  FLASH->CR   */
1482         0x2B, 0x43, //     orr     r3, r5
1483         0x23, 0x61, //     str     r3, [r4, #16]     /*  FLASH->CR |= FLASH_CR_PG */
1484         0x03, 0x88, //     ldrh    r3, [r0]          /*  r3 = *sram */
1485         0x0B, 0x80, //     strh    r3, [r1]          /*  *flash = r3 */
1486         // busy:
1487         0xE3, 0x68, //     ldr     r3, [r4, #12]     /*  FLASH->SR  */
1488         0x2B, 0x42, //     tst     r3, r5            /*  FLASH_SR_BUSY  */
1489         0xFC, 0xD0, //     beq     busy
1490
1491         0x33, 0x42, //     tst     r3, r6            /*  PGERR  */
1492         0x04, 0xD1, //     bne     exit
1493
1494         0x02, 0x30, //     add     r0, r0, #2        /*  sram += 2  */
1495         0x02, 0x31, //     add     r1, r1, #2        /*  flash += 2  */
1496         0x01, 0x3A, //     sub     r2, r2, #0x01     /*  count--  */
1497         0x00, 0x2A, //     cmp     r2, #0
1498         0xF0, 0xD1, //     bne     write_half_word
1499         // exit:
1500         0x23, 0x69, //     ldr     r3, [r4, #16]     /*  FLASH->CR  */
1501         0xAB, 0x43, //     bic     r3, r5
1502         0x23, 0x61, //     str     r3, [r4, #16]     /*  FLASH->CR &= ~FLASH_CR_PG  */
1503         0x00, 0xBE, //     bkpt #0x00
1504         0x00, 0x20, 0x02, 0x40, /* STM32_FLASH_BASE: .word 0x40022000 */
1505     };
1506
1507     static const uint8_t loader_code_stm32l[] = {
1508
1509         /* openocd.git/contrib/loaders/flash/stm32lx.S
1510            r0, input, source addr
1511            r1, input, dest addr
1512            r2, input, word count
1513            r2, output, remaining word count
1514            */
1515
1516         0x04, 0xe0,
1517
1518         0x50, 0xf8, 0x04, 0xcb,
1519         0x41, 0xf8, 0x04, 0xcb,
1520         0x01, 0x3a,
1521
1522         0x00, 0x2a,
1523         0xf8, 0xd3,
1524         0x00, 0xbe
1525     };
1526
1527     static const uint8_t loader_code_stm32l0[] = {
1528
1529         /*
1530            r0, input, source addr
1531            r1, input, dest addr
1532            r2, input, word count
1533            r2, output, remaining word count
1534          */
1535
1536         0x04, 0xe0,
1537
1538         0x04, 0x68,
1539         0x0c, 0x60,
1540         0x01, 0x3a,
1541         0x04, 0x31,
1542         0x04, 0x30,
1543
1544         0x00, 0x2a,
1545         0xf8, 0xd3,
1546         0x00, 0xbe
1547     };
1548
1549     static const uint8_t loader_code_stm32f4[] = {
1550         // flashloaders/stm32f4.s
1551
1552         0x07, 0x4b,
1553
1554         0x62, 0xb1,
1555         0x04, 0x68,
1556         0x0c, 0x60,
1557
1558         0xdc, 0x89,
1559         0x14, 0xf0, 0x01, 0x0f,
1560         0xfb, 0xd1,
1561         0x00, 0xf1, 0x04, 0x00,
1562         0x01, 0xf1, 0x04, 0x01,
1563         0xa2, 0xf1, 0x01, 0x02,
1564         0xf1, 0xe7,
1565
1566         0x00, 0xbe,
1567
1568         0x00, 0x3c, 0x02, 0x40,
1569     };
1570
1571     static const uint8_t loader_code_stm32f4_lv[] = {
1572         // flashloaders/stm32f4lv.s
1573         0x92, 0x00,
1574
1575         0x08, 0x4b,
1576         0x62, 0xb1,
1577         0x04, 0x78,
1578         0x0c, 0x70,
1579
1580         0xdc, 0x89,
1581         0x14, 0xf0, 0x01, 0x0f,
1582         0xfb, 0xd1,
1583         0x00, 0xf1, 0x01, 0x00,
1584         0x01, 0xf1, 0x01, 0x01,
1585         0xa2, 0xf1, 0x01, 0x02,
1586         0xf1, 0xe7,
1587
1588         0x00, 0xbe,
1589         0x00, 0xbf,
1590
1591         0x00, 0x3c, 0x02, 0x40,
1592     };
1593
1594     static const uint8_t loader_code_stm32l4[] = {
1595         // flashloaders/stm32l4.s
1596         0x08, 0x4b,             // start: ldr   r3, [pc, #32] ; <flash_base>
1597         0x72, 0xb1,             // next:  cbz   r2, <done>
1598         0x04, 0x68,             //        ldr   r4, [r0, #0]
1599         0x45, 0x68,             //        ldr   r5, [r0, #4]
1600         0x0c, 0x60,             //        str   r4, [r1, #0]
1601         0x4d, 0x60,             //        str   r5, [r1, #4]
1602         0x5c, 0x8a,             // wait:  ldrh  r4, [r3, #18]
1603         0x14, 0xf0, 0x01, 0x0f, //        tst.w r4, #1
1604         0xfb, 0xd1,             //        bne.n <wait>
1605         0x00, 0xf1, 0x08, 0x00, //        add.w r0, r0, #8
1606         0x01, 0xf1, 0x08, 0x01, //        add.w r1, r1, #8
1607         0xa2, 0xf1, 0x01, 0x02, //        sub.w r2, r2, #1
1608         0xef, 0xe7,             //        b.n   <next>
1609         0x00, 0xbe,             // done:  bkpt  0x0000
1610         0x00, 0x20, 0x02, 0x40  // flash_base:  .word 0x40022000
1611     };
1612
1613         static const uint8_t loader_code_stm32f7[] = {
1614         0x08, 0x4b,
1615         0x72, 0xb1,
1616         0x04, 0x68,
1617         0x0c, 0x60,
1618         0xbf, 0xf3, 0x4f, 0x8f,        // DSB Memory barrier for in order flash write
1619         0xdc, 0x89,
1620         0x14, 0xf0, 0x01, 0x0f,
1621         0xfb, 0xd1,
1622         0x00, 0xf1, 0x04, 0x00,
1623         0x01, 0xf1, 0x04, 0x01,
1624         0xa2, 0xf1, 0x01, 0x02,
1625         0xef, 0xe7,
1626         0x00, 0xbe,                   //     bkpt       #0x00
1627         0x00, 0x3c, 0x02, 0x40,
1628     };
1629
1630     const uint8_t* loader_code;
1631     size_t loader_size;
1632
1633     if (sl->chip_id == STM32_CHIPID_L1_MEDIUM || sl->chip_id == STM32_CHIPID_L1_CAT2
1634             || sl->chip_id == STM32_CHIPID_L1_MEDIUM_PLUS || sl->chip_id == STM32_CHIPID_L1_HIGH
1635             || sl->chip_id == STM32_CHIPID_L152_RE) { /* stm32l */
1636         loader_code = loader_code_stm32l;
1637         loader_size = sizeof(loader_code_stm32l);
1638     } else if (sl->core_id == STM32VL_CORE_ID
1639             || sl->chip_id == STM32_CHIPID_F3
1640             || sl->chip_id == STM32_CHIPID_F3_SMALL
1641             || sl->chip_id == STM32_CHIPID_F303_HIGH
1642             || sl->chip_id == STM32_CHIPID_F37x
1643             || sl->chip_id == STM32_CHIPID_F334) {
1644         loader_code = loader_code_stm32vl;
1645         loader_size = sizeof(loader_code_stm32vl);
1646     } else if (sl->chip_id == STM32_CHIPID_F2 || sl->chip_id == STM32_CHIPID_F4 || (sl->chip_id == STM32_CHIPID_F4_DE) ||
1647             sl->chip_id == STM32_CHIPID_F4_LP || sl->chip_id == STM32_CHIPID_F4_HD || (sl->chip_id == STM32_CHIPID_F411RE) ||
1648             (sl->chip_id == STM32_CHIPID_F446) || (sl->chip_id == STM32_CHIPID_F4_DSI)){
1649         int voltage = stlink_target_voltage(sl);
1650         if (voltage == -1) {
1651             printf("Failed to read Target voltage\n");
1652             return voltage;
1653         } else if (voltage > 2700) {
1654             loader_code = loader_code_stm32f4;
1655             loader_size = sizeof(loader_code_stm32f4);
1656         } else {
1657             loader_code = loader_code_stm32f4_lv;
1658             loader_size = sizeof(loader_code_stm32f4_lv);
1659         }
1660     } else if (sl->chip_id == STM32_CHIPID_F7){
1661         loader_code = loader_code_stm32f7;
1662         loader_size = sizeof(loader_code_stm32f7);
1663     } 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) {
1664         loader_code = loader_code_stm32f0;
1665         loader_size = sizeof(loader_code_stm32f0);
1666     } else if (sl->chip_id == STM32_CHIPID_L0) {
1667         loader_code = loader_code_stm32l0;
1668         loader_size = sizeof(loader_code_stm32l0);
1669     } else if (sl->chip_id == STM32_CHIPID_L4) {
1670         loader_code = loader_code_stm32l4;
1671         loader_size = sizeof(loader_code_stm32l4);
1672     } else {
1673         ELOG("unknown coreid, not sure what flash loader to use, aborting!: %x\n", sl->core_id);
1674         return -1;
1675     }
1676
1677     memcpy(sl->q_buf, loader_code, loader_size);
1678     stlink_write_mem32(sl, sl->sram_base, loader_size);
1679
1680     *addr = sl->sram_base;
1681     *size = loader_size;
1682
1683     /* success */
1684     return 0;
1685 }
1686
1687 int stlink_fcheck_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
1688     /* check the contents of path are at addr */
1689
1690     int res;
1691     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1692
1693     if (map_file(&mf, path) == -1)
1694         return -1;
1695
1696     res = check_file(sl, &mf, addr);
1697
1698     unmap_file(&mf);
1699
1700     return res;
1701 }
1702
1703 /**
1704  * Verify addr..addr+len is binary identical to base...base+len
1705  * @param sl stlink context
1706  * @param address stm device address
1707  * @param data host side buffer to check against
1708  * @param length how much
1709  * @return 0 for success, -ve for failure
1710  */
1711 int stlink_verify_write_flash(stlink_t *sl, stm32_addr_t address, uint8_t *data, unsigned length) {
1712     size_t off;
1713     size_t cmp_size = (sl->flash_pgsz > 0x1800)? 0x1800:sl->flash_pgsz;
1714     ILOG("Starting verification of write complete\n");
1715     for (off = 0; off < length; off += cmp_size) {
1716         size_t aligned_size;
1717
1718         /* adjust last page size */
1719         if ((off + cmp_size) > length)
1720             cmp_size = length - off;
1721
1722         aligned_size = cmp_size;
1723         if (aligned_size & (4 - 1))
1724             aligned_size = (cmp_size + 4) & ~(4 - 1);
1725
1726         stlink_read_mem32(sl, address + off, aligned_size);
1727
1728         if (memcmp(sl->q_buf, data + off, cmp_size)) {
1729             ELOG("Verification of flash failed at offset: %zd\n", off);
1730             return -1;
1731         }
1732     }
1733     ILOG("Flash written and verified! jolly good!\n");
1734     return 0;
1735
1736 }
1737
1738 int stm32l1_write_half_pages(stlink_t *sl, stm32_addr_t addr, uint8_t* base, uint32_t len, uint32_t pagesize)
1739 {
1740     unsigned int count;
1741     unsigned int num_half_pages = len / pagesize;
1742     uint32_t val;
1743     uint32_t flash_regs_base;
1744     flash_loader_t fl;
1745
1746     if (sl->chip_id == STM32_CHIPID_L0) {
1747         flash_regs_base = STM32L0_FLASH_REGS_ADDR;
1748     } else {
1749         flash_regs_base = STM32L_FLASH_REGS_ADDR;
1750     }
1751
1752     ILOG("Starting Half page flash write for STM32L core id\n");
1753     /* flash loader initialization */
1754     if (init_flash_loader(sl, &fl) == -1) {
1755         WLOG("init_flash_loader() == -1\n");
1756         return -1;
1757     }
1758     /* Unlock already done */
1759     stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1760     val |= (1 << FLASH_L1_FPRG);
1761     stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
1762
1763     val |= (1 << FLASH_L1_PROG);
1764     stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
1765     do {
1766         stlink_read_debug32(sl, flash_regs_base + FLASH_SR_OFF, &val);
1767     } while ((val & (1 << 0)) != 0);
1768
1769     for (count = 0; count  < num_half_pages; count ++) {
1770         if (run_flash_loader(sl, &fl, addr + count * pagesize, base + count * pagesize, pagesize) == -1) {
1771             WLOG("l1_run_flash_loader(%#zx) failed! == -1\n", addr + count * pagesize);
1772             stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1773             val &= ~((1 << FLASH_L1_FPRG) |(1 << FLASH_L1_PROG));
1774             stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
1775             return -1;
1776         }
1777         /* wait for sr.busy to be cleared */
1778         if (sl->verbose >= 1) {
1779             /* show progress. writing procedure is slow
1780                and previous errors are misleading */
1781             fprintf(stdout, "\r%3u/%u halfpages written", count + 1, num_half_pages);
1782             fflush(stdout);
1783         }
1784         do {
1785             stlink_read_debug32(sl, flash_regs_base + FLASH_SR_OFF, &val);
1786         } while ((val & (1 << 0)) != 0);
1787     }
1788     stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1789     val &= ~(1 << FLASH_L1_PROG);
1790     stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
1791     stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1792     val &= ~(1 << FLASH_L1_FPRG);
1793     stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
1794
1795     return 0;
1796 }
1797
1798 int stlink_write_flash(stlink_t *sl, stm32_addr_t addr, uint8_t* base, uint32_t len, uint8_t eraseonly) {
1799     size_t off;
1800     flash_loader_t fl;
1801     ILOG("Attempting to write %d (%#x) bytes to stm32 address: %u (%#x)\n",
1802             len, len, addr, addr);
1803     /* check addr range is inside the flash */
1804     stlink_calculate_pagesize(sl, addr);
1805     if (addr < sl->flash_base) {
1806         ELOG("addr too low %#x < %#x\n", addr, sl->flash_base);
1807         return -1;
1808     } else if ((addr + len) < addr) {
1809         ELOG("addr overruns\n");
1810         return -1;
1811     } else if ((addr + len) > (sl->flash_base + sl->flash_size)) {
1812         ELOG("addr too high\n");
1813         return -1;
1814     } else if (addr & 1) {
1815         ELOG("unaligned addr 0x%x\n", addr);
1816         return -1;
1817     } else if (len & 1) {
1818         WLOG("unaligned len 0x%x -- padding with zero\n", len);
1819         len += 1;
1820     } else if (addr & (sl->flash_pgsz - 1)) {
1821         ELOG("addr not a multiple of pagesize, not supported\n");
1822         return -1;
1823     }
1824
1825     // Make sure we've loaded the context with the chip details
1826     stlink_core_id(sl);
1827     /* erase each page */
1828     int page_count = 0;
1829     for (off = 0; off < len; off += stlink_calculate_pagesize(sl, addr + off)) {
1830         /* addr must be an addr inside the page */
1831         if (stlink_erase_flash_page(sl, addr + off) == -1) {
1832             ELOG("Failed to erase_flash_page(%#zx) == -1\n", addr + off);
1833             return -1;
1834         }
1835         fprintf(stdout,"\rFlash page at addr: 0x%08lx erased",
1836                 (unsigned long)addr + off);
1837         fflush(stdout);
1838         page_count++;
1839     }
1840     fprintf(stdout,"\n");
1841     ILOG("Finished erasing %d pages of %d (%#x) bytes\n",
1842             page_count, sl->flash_pgsz, sl->flash_pgsz);
1843
1844     if (eraseonly)
1845         return 0;
1846
1847     if ((sl->flash_type == FLASH_TYPE_F4) || (sl->flash_type == FLASH_TYPE_L4)) {
1848         /* todo: check write operation */
1849
1850         ILOG("Starting Flash write for F2/F4/L4\n");
1851         /* flash loader initialization */
1852         if (init_flash_loader(sl, &fl) == -1) {
1853             ELOG("init_flash_loader() == -1\n");
1854             return -1;
1855         }
1856
1857         /* First unlock the cr */
1858         unlock_flash_if(sl);
1859
1860         /* TODO: Check that Voltage range is 2.7 - 3.6 V */
1861         if (sl->chip_id != STM32_CHIPID_L4) {
1862             /* set parallelisim to 32 bit*/
1863             int voltage = stlink_target_voltage(sl);
1864             if (voltage == -1) {
1865                 printf("Failed to read Target voltage\n");
1866                 return voltage;
1867             } else if (voltage > 2700) {
1868                 printf("enabling 32-bit flash writes\n");
1869                 write_flash_cr_psiz(sl, 2);
1870             } else {
1871                 printf("Target voltage (%d mV) too low for 32-bit flash, using 8-bit flash writes\n", voltage);
1872                 write_flash_cr_psiz(sl, 0);
1873             }
1874         } else {
1875             /* L4 does not have a byte-write mode */
1876             int voltage = stlink_target_voltage(sl);
1877             if (voltage == -1) {
1878                 printf("Failed to read Target voltage\n");
1879                 return voltage;
1880             } else if (voltage < 1710) {
1881                 printf("Target voltage (%d mV) too low for flash writes!\n", voltage);
1882                 return -1;
1883             }
1884         }
1885
1886         /* set programming mode */
1887         set_flash_cr_pg(sl);
1888
1889         for(off = 0; off < len;) {
1890             size_t size = len - off > 0x8000 ? 0x8000 : len - off;
1891
1892             printf("size: %zu\n", size);
1893
1894             if (run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
1895                 ELOG("run_flash_loader(%#zx) failed! == -1\n", addr + off);
1896                 return -1;
1897             }
1898
1899             off += size;
1900         }
1901
1902         /* Relock flash */
1903         lock_flash(sl);
1904
1905     }   //STM32F4END
1906
1907     else if (sl->flash_type == FLASH_TYPE_L0) {
1908         /* use fast word write. todo: half page. */
1909         uint32_t val;
1910         uint32_t flash_regs_base;
1911         uint32_t pagesize;
1912
1913         if (sl->chip_id == STM32_CHIPID_L0) {
1914             flash_regs_base = STM32L0_FLASH_REGS_ADDR;
1915             pagesize = L0_WRITE_BLOCK_SIZE;
1916         } else {
1917             flash_regs_base = STM32L_FLASH_REGS_ADDR;
1918             pagesize = L1_WRITE_BLOCK_SIZE;
1919         }
1920
1921         /* todo: check write operation */
1922
1923         /* disable pecr protection */
1924         stlink_write_debug32(sl, flash_regs_base + FLASH_PEKEYR_OFF, 0x89abcdef);
1925         stlink_write_debug32(sl, flash_regs_base + FLASH_PEKEYR_OFF, 0x02030405);
1926
1927         /* check pecr.pelock is cleared */
1928         stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1929         if (val & (1 << 0)) {
1930             fprintf(stderr, "pecr.pelock not clear\n");
1931             return -1;
1932         }
1933
1934         /* unlock program memory */
1935         stlink_write_debug32(sl, flash_regs_base + FLASH_PRGKEYR_OFF, 0x8c9daebf);
1936         stlink_write_debug32(sl, flash_regs_base + FLASH_PRGKEYR_OFF, 0x13141516);
1937
1938         /* check pecr.prglock is cleared */
1939         stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1940         if (val & (1 << 1)) {
1941             fprintf(stderr, "pecr.prglock not clear\n");
1942             return -1;
1943         }
1944         off = 0;
1945         if (len > pagesize) {
1946             if (stm32l1_write_half_pages(sl, addr, base, len, pagesize) == -1) {
1947                 /* This may happen on a blank device! */
1948                 WLOG("\nwrite_half_pages failed == -1\n");
1949             } else {
1950                 off = (len / pagesize)*pagesize;
1951             }
1952         }
1953
1954         /* write remainingword in program memory */
1955         for ( ; off < len; off += sizeof(uint32_t)) {
1956             uint32_t data;
1957             if (off > 254)
1958                 fprintf(stdout, "\r");
1959
1960             if ((off % sl->flash_pgsz) > (sl->flash_pgsz -5)) {
1961                 fprintf(stdout, "\r%3zd/%3zd pages written",
1962                         off/sl->flash_pgsz, len/sl->flash_pgsz);
1963                 fflush(stdout);
1964             }
1965
1966             write_uint32((unsigned char*) &data, *(uint32_t*) (base + off));
1967             stlink_write_debug32(sl, addr + off, data);
1968
1969             /* wait for sr.busy to be cleared */
1970             do {
1971                 stlink_read_debug32(sl, flash_regs_base + FLASH_SR_OFF, &val);
1972             } while ((val & (1 << 0)) != 0);
1973
1974             /* todo: check redo write operation */
1975
1976         }
1977         fprintf(stdout, "\n");
1978         /* reset lock bits */
1979         stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1980         val |= (1 << 0) | (1 << 1) | (1 << 2);
1981         stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
1982     } else if (sl->flash_type == FLASH_TYPE_F0) {
1983         ILOG("Starting Flash write for VL/F0/F3 core id\n");
1984         /* flash loader initialization */
1985         if (init_flash_loader(sl, &fl) == -1) {
1986             ELOG("init_flash_loader() == -1\n");
1987             return -1;
1988         }
1989
1990         int write_block_count = 0;
1991         for (off = 0; off < len; off += sl->flash_pgsz) {
1992             /* adjust last write size */
1993             size_t size = sl->flash_pgsz;
1994             if ((off + sl->flash_pgsz) > len) size = len - off;
1995
1996             /* unlock and set programming mode */
1997             unlock_flash_if(sl);
1998             set_flash_cr_pg(sl);
1999             //DLOG("Finished setting flash cr pg, running loader!\n");
2000             if (run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
2001                 ELOG("run_flash_loader(%#zx) failed! == -1\n", addr + off);
2002                 return -1;
2003             }
2004             lock_flash(sl);
2005             if (sl->verbose >= 1) {
2006                 /* show progress. writing procedure is slow
2007                    and previous errors are misleading */
2008                 fprintf(stdout, "\r%3u/%lu pages written", write_block_count++, (unsigned long)len/sl->flash_pgsz);
2009                 fflush(stdout);
2010             }
2011         }
2012         fprintf(stdout, "\n");
2013     } else {
2014         ELOG("unknown coreid, not sure how to write: %x\n", sl->core_id);
2015         return -1;
2016     }
2017
2018     return stlink_verify_write_flash(sl, addr, base, len);
2019 }
2020
2021 /**
2022  * Write the given binary file into flash at address "addr"
2023  * @param sl
2024  * @param path readable file path, should be binary image
2025  * @param addr where to start writing
2026  * @return 0 on success, -ve on failure.
2027  */
2028 int stlink_fwrite_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
2029     /* write the file in flash at addr */
2030     int err;
2031     unsigned int num_empty, index, val;
2032     unsigned char erased_pattern;
2033     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
2034
2035     if (map_file(&mf, path) == -1) {
2036         ELOG("map_file() == -1\n");
2037         return -1;
2038     }
2039
2040     if (sl->flash_type == FLASH_TYPE_L0)
2041         erased_pattern = 0x00;
2042     else
2043         erased_pattern = 0xff;
2044
2045     index = mf.len;
2046     for(num_empty = 0; num_empty != mf.len; ++num_empty) {
2047         if (mf.base[--index] != erased_pattern) {
2048             break;
2049         }
2050     }
2051     /* Round down to words */
2052     num_empty -= (num_empty & 3);
2053     if(num_empty != 0) {
2054         ILOG("Ignoring %d bytes of 0x%02x at end of file\n", num_empty, erased_pattern);
2055     }
2056     err = stlink_write_flash(sl, addr, mf.base, num_empty == mf.len? mf.len : mf.len - num_empty, num_empty == mf.len);
2057     /* set stack*/
2058     stlink_read_debug32(sl, addr, &val);
2059     stlink_write_reg(sl, val, 13);
2060     /* Set PC to the reset routine*/
2061     stlink_read_debug32(sl, addr + 4, &val);
2062     stlink_write_reg(sl, val, 15);
2063     stlink_run(sl);
2064     unmap_file(&mf);
2065     return err;
2066 }
2067
2068 int run_flash_loader(stlink_t *sl, flash_loader_t* fl, stm32_addr_t target, const uint8_t* buf, size_t size) {
2069
2070     reg rr;
2071     int i = 0;
2072     size_t count = 0;
2073
2074     DLOG("Running flash loader, write address:%#x, size: %zd\n", target, size);
2075     // FIXME This can never return -1
2076     if (write_buffer_to_sram(sl, fl, buf, size) == -1) {
2077         // IMPOSSIBLE!
2078         ELOG("write_buffer_to_sram() == -1\n");
2079         return -1;
2080     }
2081
2082     if (sl->flash_type == FLASH_TYPE_F0) {
2083         count = size / sizeof(uint16_t);
2084         if (size % sizeof(uint16_t))
2085             ++count;
2086     } else if (sl->flash_type == FLASH_TYPE_F4 || sl->flash_type == FLASH_TYPE_L0) {
2087         count = size / sizeof(uint32_t);
2088         if (size % sizeof(uint32_t))
2089             ++count;
2090     } else if (sl->flash_type == FLASH_TYPE_L4) {
2091         count = size / sizeof(uint64_t);
2092         if (size % sizeof(uint64_t))
2093             ++count;
2094     }
2095
2096     /* setup core */
2097     stlink_write_reg(sl, fl->buf_addr, 0); /* source */
2098     stlink_write_reg(sl, target, 1); /* target */
2099     stlink_write_reg(sl, count, 2); /* count */
2100     stlink_write_reg(sl, 0, 3); /* flash bank 0 (input), only used on F0, but armless fopr others */
2101     stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
2102
2103     /* run loader */
2104     stlink_run(sl);
2105
2106 #define WAIT_ROUNDS 10000
2107     /* wait until done (reaches breakpoint) */
2108     for (i = 0; i < WAIT_ROUNDS; i++) {
2109         usleep(10);
2110         if (is_core_halted(sl))
2111             break;
2112     }
2113
2114     if (i >= WAIT_ROUNDS) {
2115         ELOG("flash loader run error\n");
2116         return -1;
2117     }
2118
2119     /* check written byte count */
2120     stlink_read_reg(sl, 2, &rr);
2121     if (rr.r[2] != 0) {
2122         fprintf(stderr, "write error, count == %u\n", rr.r[2]);
2123         return -1;
2124     }
2125
2126     return 0;
2127 }