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