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             fprintf(stderr, "EraseFlash - Page:0x%x Size:0x%x ", page, stlink_calculate_pagesize(sl, flashaddr));
1252
1253             write_flash_cr_bker_pnb(sl, page);
1254         } else if (sl->chip_id == STM32_CHIPID_F7) {
1255             // calculate the actual page from the address
1256             uint32_t sector=calculate_F7_sectornum(flashaddr);
1257
1258             fprintf(stderr, "EraseFlash - Sector:0x%x Size:0x%x ", sector, stlink_calculate_pagesize(sl, flashaddr));
1259
1260             write_flash_cr_snb(sl, sector);
1261         } else {
1262             // calculate the actual page from the address
1263             uint32_t sector=calculate_F4_sectornum(flashaddr);
1264
1265             fprintf(stderr, "EraseFlash - Sector:0x%x Size:0x%x ", sector, stlink_calculate_pagesize(sl, flashaddr));
1266
1267             //the SNB values for flash sectors in the second bank do not directly follow the values for the first bank on 2mb devices...
1268             if (sector >= 12) sector += 4;
1269
1270             write_flash_cr_snb(sl, sector);
1271         }
1272
1273         /* start erase operation */
1274         set_flash_cr_strt(sl);
1275
1276         /* wait for completion */
1277         wait_flash_busy(sl);
1278
1279         /* relock the flash */
1280         //todo: fails to program if this is in
1281         lock_flash(sl);
1282 #if DEBUG_FLASH
1283         fprintf(stdout, "Erase Final CR:0x%x\n", read_flash_cr(sl));
1284 #endif
1285     } else if (sl->flash_type == FLASH_TYPE_L0) {
1286
1287         uint32_t val;
1288         uint32_t flash_regs_base;
1289         if (sl->chip_id == STM32_CHIPID_L0) {
1290             flash_regs_base = STM32L0_FLASH_REGS_ADDR;
1291         } else {
1292             flash_regs_base = STM32L_FLASH_REGS_ADDR;
1293         }
1294
1295         /* check if the locks are set */
1296         stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1297         if((val & (1<<0))||(val & (1<<1))) {
1298             /* disable pecr protection */
1299             stlink_write_debug32(sl, flash_regs_base + FLASH_PEKEYR_OFF, 0x89abcdef);
1300             stlink_write_debug32(sl, flash_regs_base + FLASH_PEKEYR_OFF, 0x02030405);
1301
1302             /* check pecr.pelock is cleared */
1303             stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1304             if (val & (1 << 0)) {
1305                 WLOG("pecr.pelock not clear (%#x)\n", val);
1306                 return -1;
1307             }
1308
1309             /* unlock program memory */
1310             stlink_write_debug32(sl, flash_regs_base + FLASH_PRGKEYR_OFF, 0x8c9daebf);
1311             stlink_write_debug32(sl, flash_regs_base + FLASH_PRGKEYR_OFF, 0x13141516);
1312
1313             /* check pecr.prglock is cleared */
1314             stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1315             if (val & (1 << 1)) {
1316                 WLOG("pecr.prglock not clear (%#x)\n", val);
1317                 return -1;
1318             }
1319         }
1320
1321         /* set pecr.{erase,prog} */
1322         val |= (1 << 9) | (1 << 3);
1323         stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
1324 #if 0 /* fix_to_be_confirmed */
1325
1326         /* wait for sr.busy to be cleared
1327          * MP: Test shows that busy bit is not set here. Perhaps, PM0062 is
1328          * wrong and we do not need to wait here for clearing the busy bit.
1329          * TEXANE: ok, if experience says so and it works for you, we comment
1330          * it. If someone has a problem, please drop an email.
1331          */
1332         do {
1333             stlink_read_debug32(sl, STM32L_FLASH_SR, &val)
1334         } while((val & (1 << 0)) != 0);
1335
1336 #endif /* fix_to_be_confirmed */
1337
1338         /* write 0 to the first word of the page to be erased */
1339         stlink_write_debug32(sl, flashaddr, 0);
1340
1341         /* MP: It is better to wait for clearing the busy bit after issuing
1342            page erase command, even though PM0062 recommends to wait before it.
1343            Test shows that a few iterations is performed in the following loop
1344            before busy bit is cleared.*/
1345         do {
1346             stlink_read_debug32(sl, flash_regs_base + FLASH_SR_OFF, &val);
1347         } while ((val & (1 << 0)) != 0);
1348
1349         /* reset lock bits */
1350         stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1351         val |= (1 << 0) | (1 << 1) | (1 << 2);
1352         stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
1353     } else if (sl->flash_type == FLASH_TYPE_F0)  {
1354         /* wait for ongoing op to finish */
1355         wait_flash_busy(sl);
1356
1357         /* unlock if locked */
1358         unlock_flash_if(sl);
1359
1360         /* set the page erase bit */
1361         set_flash_cr_per(sl);
1362
1363         /* select the page to erase */
1364         write_flash_ar(sl, flashaddr);
1365
1366         /* start erase operation, reset by hw with bsy bit */
1367         set_flash_cr_strt(sl);
1368
1369         /* wait for completion */
1370         wait_flash_busy(sl);
1371
1372         /* relock the flash */
1373         lock_flash(sl);
1374     } else {
1375         WLOG("unknown coreid %x, page erase failed\n", sl->core_id);
1376         return -1;
1377     }
1378
1379     /* todo: verify the erased page */
1380
1381     return 0;
1382 }
1383
1384 int stlink_erase_flash_mass(stlink_t *sl) {
1385     if (sl->flash_type == FLASH_TYPE_L0) {
1386         /* erase each page */
1387         int i = 0, num_pages = sl->flash_size/sl->flash_pgsz;
1388         for (i = 0; i < num_pages; i++) {
1389             /* addr must be an addr inside the page */
1390             stm32_addr_t addr = sl->flash_base + i * sl->flash_pgsz;
1391             if (stlink_erase_flash_page(sl, addr) == -1) {
1392                 WLOG("Failed to erase_flash_page(%#zx) == -1\n", addr);
1393                 return -1;
1394             }
1395             fprintf(stdout,"-> Flash page at %5d/%5d erased\n", i, num_pages);
1396             fflush(stdout);
1397         }
1398         fprintf(stdout, "\n");
1399     } else {
1400         /* wait for ongoing op to finish */
1401         wait_flash_busy(sl);
1402
1403         /* unlock if locked */
1404         unlock_flash_if(sl);
1405
1406         /* set the mass erase bit */
1407         set_flash_cr_mer(sl);
1408
1409         /* start erase operation, reset by hw with bsy bit */
1410         set_flash_cr_strt(sl);
1411
1412         /* wait for completion */
1413         wait_flash_busy_progress(sl);
1414
1415         /* relock the flash */
1416         lock_flash(sl);
1417
1418         /* todo: verify the erased memory */
1419     }
1420     return 0;
1421 }
1422
1423 int init_flash_loader(stlink_t *sl, flash_loader_t* fl) {
1424     size_t size;
1425
1426     /* allocate the loader in sram */
1427     if (write_loader_to_sram(sl, &fl->loader_addr, &size) == -1) {
1428         WLOG("Failed to write flash loader to sram!\n");
1429         return -1;
1430     }
1431
1432     /* allocate a one page buffer in sram right after loader */
1433     fl->buf_addr = fl->loader_addr + size;
1434     ILOG("Successfully loaded flash loader in sram\n");
1435     return 0;
1436 }
1437
1438 int write_loader_to_sram(stlink_t *sl, stm32_addr_t* addr, size_t* size) {
1439     /* from openocd, contrib/loaders/flash/stm32.s */
1440     static const uint8_t loader_code_stm32vl[] = {
1441         0x08, 0x4c, /* ldr      r4, STM32_FLASH_BASE */
1442         0x1c, 0x44, /* add      r4, r3 */
1443         /* write_half_word: */
1444         0x01, 0x23, /* movs     r3, #0x01 */
1445         0x23, 0x61, /* str      r3, [r4, #STM32_FLASH_CR_OFFSET] */
1446         0x30, 0xf8, 0x02, 0x3b, /* ldrh r3, [r0], #0x02 */
1447         0x21, 0xf8, 0x02, 0x3b, /* strh r3, [r1], #0x02 */
1448         /* busy: */
1449         0xe3, 0x68, /* ldr      r3, [r4, #STM32_FLASH_SR_OFFSET] */
1450         0x13, 0xf0, 0x01, 0x0f, /* tst  r3, #0x01 */
1451         0xfb, 0xd0, /* beq      busy */
1452         0x13, 0xf0, 0x14, 0x0f, /* tst  r3, #0x14 */
1453         0x01, 0xd1, /* bne      exit */
1454         0x01, 0x3a, /* subs     r2, r2, #0x01 */
1455         0xf0, 0xd1, /* bne      write_half_word */
1456         /* exit: */
1457         0x00, 0xbe, /* bkpt     #0x00 */
1458         0x00, 0x20, 0x02, 0x40, /* STM32_FLASH_BASE: .word 0x40022000 */
1459     };
1460
1461     /* flashloaders/stm32f0.s -- thumb1 only, same sequence as for STM32VL, bank ignored */
1462     static const uint8_t loader_code_stm32f0[] = {
1463 #if 1
1464         /*
1465          * These two NOPs here are a safety precaution, added by Pekka Nikander
1466          * while debugging the STM32F05x support.  They may not be needed, but
1467          * there were strange problems with simpler programs, like a program
1468          * that had just a breakpoint or a program that first moved zero to register r2
1469          * and then had a breakpoint.  So, it appears safest to have these two nops.
1470          *
1471          * Feel free to remove them, if you dare, but then please do test the result
1472          * rigorously.  Also, if you remove these, it may be a good idea first to
1473          * #if 0 them out, with a comment when these were taken out, and to remove
1474          * these only a few months later...  But YMMV.
1475          */
1476         0x00, 0x30, //     nop     /* add r0,#0 */
1477         0x00, 0x30, //     nop     /* add r0,#0 */
1478 #endif
1479         0x0A, 0x4C, //     ldr     r4, STM32_FLASH_BASE
1480         0x01, 0x25, //     mov     r5, #1            /*  FLASH_CR_PG, FLASH_SR_BUSY */
1481         0x04, 0x26, //     mov     r6, #4            /*  PGERR  */
1482         // write_half_word:
1483         0x23, 0x69, //     ldr     r3, [r4, #16]     /*  FLASH->CR   */
1484         0x2B, 0x43, //     orr     r3, r5
1485         0x23, 0x61, //     str     r3, [r4, #16]     /*  FLASH->CR |= FLASH_CR_PG */
1486         0x03, 0x88, //     ldrh    r3, [r0]          /*  r3 = *sram */
1487         0x0B, 0x80, //     strh    r3, [r1]          /*  *flash = r3 */
1488         // busy:
1489         0xE3, 0x68, //     ldr     r3, [r4, #12]     /*  FLASH->SR  */
1490         0x2B, 0x42, //     tst     r3, r5            /*  FLASH_SR_BUSY  */
1491         0xFC, 0xD0, //     beq     busy
1492
1493         0x33, 0x42, //     tst     r3, r6            /*  PGERR  */
1494         0x04, 0xD1, //     bne     exit
1495
1496         0x02, 0x30, //     add     r0, r0, #2        /*  sram += 2  */
1497         0x02, 0x31, //     add     r1, r1, #2        /*  flash += 2  */
1498         0x01, 0x3A, //     sub     r2, r2, #0x01     /*  count--  */
1499         0x00, 0x2A, //     cmp     r2, #0
1500         0xF0, 0xD1, //     bne     write_half_word
1501         // exit:
1502         0x23, 0x69, //     ldr     r3, [r4, #16]     /*  FLASH->CR  */
1503         0xAB, 0x43, //     bic     r3, r5
1504         0x23, 0x61, //     str     r3, [r4, #16]     /*  FLASH->CR &= ~FLASH_CR_PG  */
1505         0x00, 0xBE, //     bkpt #0x00
1506         0x00, 0x20, 0x02, 0x40, /* STM32_FLASH_BASE: .word 0x40022000 */
1507     };
1508
1509     static const uint8_t loader_code_stm32l[] = {
1510
1511         /* openocd.git/contrib/loaders/flash/stm32lx.S
1512            r0, input, source addr
1513            r1, input, dest addr
1514            r2, input, word count
1515            r2, output, remaining word count
1516            */
1517
1518         0x04, 0xe0,
1519
1520         0x50, 0xf8, 0x04, 0xcb,
1521         0x41, 0xf8, 0x04, 0xcb,
1522         0x01, 0x3a,
1523
1524         0x00, 0x2a,
1525         0xf8, 0xd3,
1526         0x00, 0xbe
1527     };
1528
1529     static const uint8_t loader_code_stm32l0[] = {
1530
1531         /*
1532            r0, input, source addr
1533            r1, input, dest addr
1534            r2, input, word count
1535            r2, output, remaining word count
1536          */
1537
1538         0x04, 0xe0,
1539
1540         0x04, 0x68,
1541         0x0c, 0x60,
1542         0x01, 0x3a,
1543         0x04, 0x31,
1544         0x04, 0x30,
1545
1546         0x00, 0x2a,
1547         0xf8, 0xd3,
1548         0x00, 0xbe
1549     };
1550
1551     static const uint8_t loader_code_stm32f4[] = {
1552         // flashloaders/stm32f4.s
1553
1554         0x07, 0x4b,
1555
1556         0x62, 0xb1,
1557         0x04, 0x68,
1558         0x0c, 0x60,
1559
1560         0xdc, 0x89,
1561         0x14, 0xf0, 0x01, 0x0f,
1562         0xfb, 0xd1,
1563         0x00, 0xf1, 0x04, 0x00,
1564         0x01, 0xf1, 0x04, 0x01,
1565         0xa2, 0xf1, 0x01, 0x02,
1566         0xf1, 0xe7,
1567
1568         0x00, 0xbe,
1569
1570         0x00, 0x3c, 0x02, 0x40,
1571     };
1572
1573     static const uint8_t loader_code_stm32f4_lv[] = {
1574         // flashloaders/stm32f4lv.s
1575         0x92, 0x00,
1576
1577         0x08, 0x4b,
1578         0x62, 0xb1,
1579         0x04, 0x78,
1580         0x0c, 0x70,
1581
1582         0xdc, 0x89,
1583         0x14, 0xf0, 0x01, 0x0f,
1584         0xfb, 0xd1,
1585         0x00, 0xf1, 0x01, 0x00,
1586         0x01, 0xf1, 0x01, 0x01,
1587         0xa2, 0xf1, 0x01, 0x02,
1588         0xf1, 0xe7,
1589
1590         0x00, 0xbe,
1591         0x00, 0xbf,
1592
1593         0x00, 0x3c, 0x02, 0x40,
1594     };
1595
1596     static const uint8_t loader_code_stm32l4[] = {
1597         // flashloaders/stm32l4.s
1598         0x08, 0x4b,             // start: ldr   r3, [pc, #32] ; <flash_base>
1599         0x72, 0xb1,             // next:  cbz   r2, <done>
1600         0x04, 0x68,             //        ldr   r4, [r0, #0]
1601         0x45, 0x68,             //        ldr   r5, [r0, #4]
1602         0x0c, 0x60,             //        str   r4, [r1, #0]
1603         0x4d, 0x60,             //        str   r5, [r1, #4]
1604         0x5c, 0x8a,             // wait:  ldrh  r4, [r3, #18]
1605         0x14, 0xf0, 0x01, 0x0f, //        tst.w r4, #1
1606         0xfb, 0xd1,             //        bne.n <wait>
1607         0x00, 0xf1, 0x08, 0x00, //        add.w r0, r0, #8
1608         0x01, 0xf1, 0x08, 0x01, //        add.w r1, r1, #8
1609         0xa2, 0xf1, 0x01, 0x02, //        sub.w r2, r2, #1
1610         0xef, 0xe7,             //        b.n   <next>
1611         0x00, 0xbe,             // done:  bkpt  0x0000
1612         0x00, 0x20, 0x02, 0x40  // flash_base:  .word 0x40022000
1613     };
1614
1615         static const uint8_t loader_code_stm32f7[] = {
1616         0x08, 0x4b,
1617         0x72, 0xb1,
1618         0x04, 0x68,
1619         0x0c, 0x60,
1620         0xbf, 0xf3, 0x4f, 0x8f,        // DSB Memory barrier for in order flash write
1621         0xdc, 0x89,
1622         0x14, 0xf0, 0x01, 0x0f,
1623         0xfb, 0xd1,
1624         0x00, 0xf1, 0x04, 0x00,
1625         0x01, 0xf1, 0x04, 0x01,
1626         0xa2, 0xf1, 0x01, 0x02,
1627         0xef, 0xe7,
1628         0x00, 0xbe,                   //     bkpt       #0x00
1629         0x00, 0x3c, 0x02, 0x40,
1630     };
1631
1632     const uint8_t* loader_code;
1633     size_t loader_size;
1634
1635     if (sl->chip_id == STM32_CHIPID_L1_MEDIUM || sl->chip_id == STM32_CHIPID_L1_CAT2
1636             || sl->chip_id == STM32_CHIPID_L1_MEDIUM_PLUS || sl->chip_id == STM32_CHIPID_L1_HIGH
1637             || sl->chip_id == STM32_CHIPID_L152_RE) { /* stm32l */
1638         loader_code = loader_code_stm32l;
1639         loader_size = sizeof(loader_code_stm32l);
1640     } else if (sl->core_id == STM32VL_CORE_ID
1641             || sl->chip_id == STM32_CHIPID_F3
1642             || sl->chip_id == STM32_CHIPID_F3_SMALL
1643             || sl->chip_id == STM32_CHIPID_F303_HIGH
1644             || sl->chip_id == STM32_CHIPID_F37x
1645             || sl->chip_id == STM32_CHIPID_F334) {
1646         loader_code = loader_code_stm32vl;
1647         loader_size = sizeof(loader_code_stm32vl);
1648     } else if (sl->chip_id == STM32_CHIPID_F2 || sl->chip_id == STM32_CHIPID_F4 || (sl->chip_id == STM32_CHIPID_F4_DE) ||
1649             sl->chip_id == STM32_CHIPID_F4_LP || sl->chip_id == STM32_CHIPID_F4_HD || (sl->chip_id == STM32_CHIPID_F411RE) ||
1650             (sl->chip_id == STM32_CHIPID_F446) || (sl->chip_id == STM32_CHIPID_F4_DSI)){
1651         int voltage = stlink_target_voltage(sl);
1652         if (voltage == -1) {
1653             printf("Failed to read Target voltage\n");
1654             return voltage;
1655         } else if (voltage > 2700) {
1656             loader_code = loader_code_stm32f4;
1657             loader_size = sizeof(loader_code_stm32f4);
1658         } else {
1659             loader_code = loader_code_stm32f4_lv;
1660             loader_size = sizeof(loader_code_stm32f4_lv);
1661         }
1662     } else if (sl->chip_id == STM32_CHIPID_F7){
1663         loader_code = loader_code_stm32f7;
1664         loader_size = sizeof(loader_code_stm32f7);
1665     } 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) {
1666         loader_code = loader_code_stm32f0;
1667         loader_size = sizeof(loader_code_stm32f0);
1668     } else if (sl->chip_id == STM32_CHIPID_L0) {
1669         loader_code = loader_code_stm32l0;
1670         loader_size = sizeof(loader_code_stm32l0);
1671     } else if (sl->chip_id == STM32_CHIPID_L4) {
1672         loader_code = loader_code_stm32l4;
1673         loader_size = sizeof(loader_code_stm32l4);
1674     } else {
1675         ELOG("unknown coreid, not sure what flash loader to use, aborting!: %x\n", sl->core_id);
1676         return -1;
1677     }
1678
1679     memcpy(sl->q_buf, loader_code, loader_size);
1680     stlink_write_mem32(sl, sl->sram_base, loader_size);
1681
1682     *addr = sl->sram_base;
1683     *size = loader_size;
1684
1685     /* success */
1686     return 0;
1687 }
1688
1689 int stlink_fcheck_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
1690     /* check the contents of path are at addr */
1691
1692     int res;
1693     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1694
1695     if (map_file(&mf, path) == -1)
1696         return -1;
1697
1698     res = check_file(sl, &mf, addr);
1699
1700     unmap_file(&mf);
1701
1702     return res;
1703 }
1704
1705 /**
1706  * Verify addr..addr+len is binary identical to base...base+len
1707  * @param sl stlink context
1708  * @param address stm device address
1709  * @param data host side buffer to check against
1710  * @param length how much
1711  * @return 0 for success, -ve for failure
1712  */
1713 int stlink_verify_write_flash(stlink_t *sl, stm32_addr_t address, uint8_t *data, unsigned length) {
1714     size_t off;
1715     size_t cmp_size = (sl->flash_pgsz > 0x1800)? 0x1800:sl->flash_pgsz;
1716     ILOG("Starting verification of write complete\n");
1717     for (off = 0; off < length; off += cmp_size) {
1718         size_t aligned_size;
1719
1720         /* adjust last page size */
1721         if ((off + cmp_size) > length)
1722             cmp_size = length - off;
1723
1724         aligned_size = cmp_size;
1725         if (aligned_size & (4 - 1))
1726             aligned_size = (cmp_size + 4) & ~(4 - 1);
1727
1728         stlink_read_mem32(sl, address + off, aligned_size);
1729
1730         if (memcmp(sl->q_buf, data + off, cmp_size)) {
1731             ELOG("Verification of flash failed at offset: %zd\n", off);
1732             return -1;
1733         }
1734     }
1735     ILOG("Flash written and verified! jolly good!\n");
1736     return 0;
1737
1738 }
1739
1740 int stm32l1_write_half_pages(stlink_t *sl, stm32_addr_t addr, uint8_t* base, uint32_t len, uint32_t pagesize)
1741 {
1742     unsigned int count;
1743     unsigned int num_half_pages = len / pagesize;
1744     uint32_t val;
1745     uint32_t flash_regs_base;
1746     flash_loader_t fl;
1747
1748     if (sl->chip_id == STM32_CHIPID_L0) {
1749         flash_regs_base = STM32L0_FLASH_REGS_ADDR;
1750     } else {
1751         flash_regs_base = STM32L_FLASH_REGS_ADDR;
1752     }
1753
1754     ILOG("Starting Half page flash write for STM32L core id\n");
1755     /* flash loader initialization */
1756     if (init_flash_loader(sl, &fl) == -1) {
1757         WLOG("init_flash_loader() == -1\n");
1758         return -1;
1759     }
1760     /* Unlock already done */
1761     stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1762     val |= (1 << FLASH_L1_FPRG);
1763     stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
1764
1765     val |= (1 << FLASH_L1_PROG);
1766     stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
1767     do {
1768         stlink_read_debug32(sl, flash_regs_base + FLASH_SR_OFF, &val);
1769     } while ((val & (1 << 0)) != 0);
1770
1771     for (count = 0; count  < num_half_pages; count ++) {
1772         if (run_flash_loader(sl, &fl, addr + count * pagesize, base + count * pagesize, pagesize) == -1) {
1773             WLOG("l1_run_flash_loader(%#zx) failed! == -1\n", addr + count * pagesize);
1774             stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1775             val &= ~((1 << FLASH_L1_FPRG) |(1 << FLASH_L1_PROG));
1776             stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
1777             return -1;
1778         }
1779         /* wait for sr.busy to be cleared */
1780         if (sl->verbose >= 1) {
1781             /* show progress. writing procedure is slow
1782                and previous errors are misleading */
1783             fprintf(stdout, "\r%3u/%u halfpages written", count + 1, num_half_pages);
1784             fflush(stdout);
1785         }
1786         do {
1787             stlink_read_debug32(sl, flash_regs_base + FLASH_SR_OFF, &val);
1788         } while ((val & (1 << 0)) != 0);
1789     }
1790     stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1791     val &= ~(1 << FLASH_L1_PROG);
1792     stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
1793     stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1794     val &= ~(1 << FLASH_L1_FPRG);
1795     stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
1796
1797     return 0;
1798 }
1799
1800 int stlink_write_flash(stlink_t *sl, stm32_addr_t addr, uint8_t* base, uint32_t len, uint8_t eraseonly) {
1801     size_t off;
1802     flash_loader_t fl;
1803     ILOG("Attempting to write %d (%#x) bytes to stm32 address: %u (%#x)\n",
1804             len, len, addr, addr);
1805     /* check addr range is inside the flash */
1806     stlink_calculate_pagesize(sl, addr);
1807     if (addr < sl->flash_base) {
1808         ELOG("addr too low %#x < %#x\n", addr, sl->flash_base);
1809         return -1;
1810     } else if ((addr + len) < addr) {
1811         ELOG("addr overruns\n");
1812         return -1;
1813     } else if ((addr + len) > (sl->flash_base + sl->flash_size)) {
1814         ELOG("addr too high\n");
1815         return -1;
1816     } else if (addr & 1) {
1817         ELOG("unaligned addr 0x%x\n", addr);
1818         return -1;
1819     } else if (len & 1) {
1820         WLOG("unaligned len 0x%x -- padding with zero\n", len);
1821         len += 1;
1822     } else if (addr & (sl->flash_pgsz - 1)) {
1823         ELOG("addr not a multiple of pagesize, not supported\n");
1824         return -1;
1825     }
1826
1827     // Make sure we've loaded the context with the chip details
1828     stlink_core_id(sl);
1829     /* erase each page */
1830     int page_count = 0;
1831     for (off = 0; off < len; off += stlink_calculate_pagesize(sl, addr + off)) {
1832         /* addr must be an addr inside the page */
1833         if (stlink_erase_flash_page(sl, addr + off) == -1) {
1834             ELOG("Failed to erase_flash_page(%#zx) == -1\n", addr + off);
1835             return -1;
1836         }
1837         fprintf(stdout,"\rFlash page at addr: 0x%08lx erased",
1838                 (unsigned long)addr + off);
1839         fflush(stdout);
1840         page_count++;
1841     }
1842     fprintf(stdout,"\n");
1843     ILOG("Finished erasing %d pages of %d (%#x) bytes\n",
1844             page_count, sl->flash_pgsz, sl->flash_pgsz);
1845
1846     if (eraseonly)
1847         return 0;
1848
1849     if ((sl->flash_type == FLASH_TYPE_F4) || (sl->flash_type == FLASH_TYPE_L4)) {
1850         /* todo: check write operation */
1851
1852         ILOG("Starting Flash write for F2/F4/L4\n");
1853         /* flash loader initialization */
1854         if (init_flash_loader(sl, &fl) == -1) {
1855             ELOG("init_flash_loader() == -1\n");
1856             return -1;
1857         }
1858
1859         /* First unlock the cr */
1860         unlock_flash_if(sl);
1861
1862         /* TODO: Check that Voltage range is 2.7 - 3.6 V */
1863         if (sl->chip_id != STM32_CHIPID_L4) {
1864             /* set parallelisim to 32 bit*/
1865             int voltage = stlink_target_voltage(sl);
1866             if (voltage == -1) {
1867                 printf("Failed to read Target voltage\n");
1868                 return voltage;
1869             } else if (voltage > 2700) {
1870                 printf("enabling 32-bit flash writes\n");
1871                 write_flash_cr_psiz(sl, 2);
1872             } else {
1873                 printf("Target voltage (%d mV) too low for 32-bit flash, using 8-bit flash writes\n", voltage);
1874                 write_flash_cr_psiz(sl, 0);
1875             }
1876         } else {
1877             /* L4 does not have a byte-write mode */
1878             int voltage = stlink_target_voltage(sl);
1879             if (voltage == -1) {
1880                 printf("Failed to read Target voltage\n");
1881                 return voltage;
1882             } else if (voltage < 1710) {
1883                 printf("Target voltage (%d mV) too low for flash writes!\n", voltage);
1884                 return -1;
1885             }
1886         }
1887
1888         /* set programming mode */
1889         set_flash_cr_pg(sl);
1890
1891         for(off = 0; off < len;) {
1892             size_t size = len - off > 0x8000 ? 0x8000 : len - off;
1893
1894             printf("size: %zu\n", size);
1895
1896             if (run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
1897                 ELOG("run_flash_loader(%#zx) failed! == -1\n", addr + off);
1898                 return -1;
1899             }
1900
1901             off += size;
1902         }
1903
1904         /* Relock flash */
1905         lock_flash(sl);
1906
1907     }   //STM32F4END
1908
1909     else if (sl->flash_type == FLASH_TYPE_L0) {
1910         /* use fast word write. todo: half page. */
1911         uint32_t val;
1912         uint32_t flash_regs_base;
1913         uint32_t pagesize;
1914
1915         if (sl->chip_id == STM32_CHIPID_L0) {
1916             flash_regs_base = STM32L0_FLASH_REGS_ADDR;
1917             pagesize = L0_WRITE_BLOCK_SIZE;
1918         } else {
1919             flash_regs_base = STM32L_FLASH_REGS_ADDR;
1920             pagesize = L1_WRITE_BLOCK_SIZE;
1921         }
1922
1923         /* todo: check write operation */
1924
1925         /* disable pecr protection */
1926         stlink_write_debug32(sl, flash_regs_base + FLASH_PEKEYR_OFF, 0x89abcdef);
1927         stlink_write_debug32(sl, flash_regs_base + FLASH_PEKEYR_OFF, 0x02030405);
1928
1929         /* check pecr.pelock is cleared */
1930         stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1931         if (val & (1 << 0)) {
1932             fprintf(stderr, "pecr.pelock not clear\n");
1933             return -1;
1934         }
1935
1936         /* unlock program memory */
1937         stlink_write_debug32(sl, flash_regs_base + FLASH_PRGKEYR_OFF, 0x8c9daebf);
1938         stlink_write_debug32(sl, flash_regs_base + FLASH_PRGKEYR_OFF, 0x13141516);
1939
1940         /* check pecr.prglock is cleared */
1941         stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1942         if (val & (1 << 1)) {
1943             fprintf(stderr, "pecr.prglock not clear\n");
1944             return -1;
1945         }
1946         off = 0;
1947         if (len > pagesize) {
1948             if (stm32l1_write_half_pages(sl, addr, base, len, pagesize) == -1) {
1949                 /* This may happen on a blank device! */
1950                 WLOG("\nwrite_half_pages failed == -1\n");
1951             } else {
1952                 off = (len / pagesize)*pagesize;
1953             }
1954         }
1955
1956         /* write remainingword in program memory */
1957         for ( ; off < len; off += sizeof(uint32_t)) {
1958             uint32_t data;
1959             if (off > 254)
1960                 fprintf(stdout, "\r");
1961
1962             if ((off % sl->flash_pgsz) > (sl->flash_pgsz -5)) {
1963                 fprintf(stdout, "\r%3zd/%3zd pages written",
1964                         off/sl->flash_pgsz, len/sl->flash_pgsz);
1965                 fflush(stdout);
1966             }
1967
1968             write_uint32((unsigned char*) &data, *(uint32_t*) (base + off));
1969             stlink_write_debug32(sl, addr + off, data);
1970
1971             /* wait for sr.busy to be cleared */
1972             do {
1973                 stlink_read_debug32(sl, flash_regs_base + FLASH_SR_OFF, &val);
1974             } while ((val & (1 << 0)) != 0);
1975
1976             /* todo: check redo write operation */
1977
1978         }
1979         fprintf(stdout, "\n");
1980         /* reset lock bits */
1981         stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
1982         val |= (1 << 0) | (1 << 1) | (1 << 2);
1983         stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
1984     } else if (sl->flash_type == FLASH_TYPE_F0) {
1985         ILOG("Starting Flash write for VL/F0/F3 core id\n");
1986         /* flash loader initialization */
1987         if (init_flash_loader(sl, &fl) == -1) {
1988             ELOG("init_flash_loader() == -1\n");
1989             return -1;
1990         }
1991
1992         int write_block_count = 0;
1993         for (off = 0; off < len; off += sl->flash_pgsz) {
1994             /* adjust last write size */
1995             size_t size = sl->flash_pgsz;
1996             if ((off + sl->flash_pgsz) > len) size = len - off;
1997
1998             /* unlock and set programming mode */
1999             unlock_flash_if(sl);
2000             set_flash_cr_pg(sl);
2001             //DLOG("Finished setting flash cr pg, running loader!\n");
2002             if (run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
2003                 ELOG("run_flash_loader(%#zx) failed! == -1\n", addr + off);
2004                 return -1;
2005             }
2006             lock_flash(sl);
2007             if (sl->verbose >= 1) {
2008                 /* show progress. writing procedure is slow
2009                    and previous errors are misleading */
2010                 fprintf(stdout, "\r%3u/%lu pages written", write_block_count++, (unsigned long)len/sl->flash_pgsz);
2011                 fflush(stdout);
2012             }
2013         }
2014         fprintf(stdout, "\n");
2015     } else {
2016         ELOG("unknown coreid, not sure how to write: %x\n", sl->core_id);
2017         return -1;
2018     }
2019
2020     return stlink_verify_write_flash(sl, addr, base, len);
2021 }
2022
2023 /**
2024  * Write the given binary file into flash at address "addr"
2025  * @param sl
2026  * @param path readable file path, should be binary image
2027  * @param addr where to start writing
2028  * @return 0 on success, -ve on failure.
2029  */
2030 int stlink_fwrite_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
2031     /* write the file in flash at addr */
2032     int err;
2033     unsigned int num_empty, index, val;
2034     unsigned char erased_pattern;
2035     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
2036
2037     if (map_file(&mf, path) == -1) {
2038         ELOG("map_file() == -1\n");
2039         return -1;
2040     }
2041
2042     if (sl->flash_type == FLASH_TYPE_L0)
2043         erased_pattern = 0x00;
2044     else
2045         erased_pattern = 0xff;
2046
2047     index = mf.len;
2048     for(num_empty = 0; num_empty != mf.len; ++num_empty) {
2049         if (mf.base[--index] != erased_pattern) {
2050             break;
2051         }
2052     }
2053     /* Round down to words */
2054     num_empty -= (num_empty & 3);
2055     if(num_empty != 0) {
2056         ILOG("Ignoring %d bytes of 0x%02x at end of file\n", num_empty, erased_pattern);
2057     }
2058     err = stlink_write_flash(sl, addr, mf.base, num_empty == mf.len? mf.len : mf.len - num_empty, num_empty == mf.len);
2059     /* set stack*/
2060     stlink_read_debug32(sl, addr, &val);
2061     stlink_write_reg(sl, val, 13);
2062     /* Set PC to the reset routine*/
2063     stlink_read_debug32(sl, addr + 4, &val);
2064     stlink_write_reg(sl, val, 15);
2065     stlink_run(sl);
2066     unmap_file(&mf);
2067     return err;
2068 }
2069
2070 int run_flash_loader(stlink_t *sl, flash_loader_t* fl, stm32_addr_t target, const uint8_t* buf, size_t size) {
2071
2072     reg rr;
2073     int i = 0;
2074     size_t count = 0;
2075
2076     DLOG("Running flash loader, write address:%#x, size: %zd\n", target, size);
2077     // FIXME This can never return -1
2078     if (write_buffer_to_sram(sl, fl, buf, size) == -1) {
2079         // IMPOSSIBLE!
2080         ELOG("write_buffer_to_sram() == -1\n");
2081         return -1;
2082     }
2083
2084     if (sl->flash_type == FLASH_TYPE_F0) {
2085         count = size / sizeof(uint16_t);
2086         if (size % sizeof(uint16_t))
2087             ++count;
2088     } else if (sl->flash_type == FLASH_TYPE_F4 || sl->flash_type == FLASH_TYPE_L0) {
2089         count = size / sizeof(uint32_t);
2090         if (size % sizeof(uint32_t))
2091             ++count;
2092     } else if (sl->flash_type == FLASH_TYPE_L4) {
2093         count = size / sizeof(uint64_t);
2094         if (size % sizeof(uint64_t))
2095             ++count;
2096     }
2097
2098     /* setup core */
2099     stlink_write_reg(sl, fl->buf_addr, 0); /* source */
2100     stlink_write_reg(sl, target, 1); /* target */
2101     stlink_write_reg(sl, count, 2); /* count */
2102     stlink_write_reg(sl, 0, 3); /* flash bank 0 (input), only used on F0, but armless fopr others */
2103     stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
2104
2105     /* run loader */
2106     stlink_run(sl);
2107
2108 #define WAIT_ROUNDS 10000
2109     /* wait until done (reaches breakpoint) */
2110     for (i = 0; i < WAIT_ROUNDS; i++) {
2111         usleep(10);
2112         if (is_core_halted(sl))
2113             break;
2114     }
2115
2116     if (i >= WAIT_ROUNDS) {
2117         ELOG("flash loader run error\n");
2118         return -1;
2119     }
2120
2121     /* check written byte count */
2122     stlink_read_reg(sl, 2, &rr);
2123     if (rr.r[2] != 0) {
2124         fprintf(stderr, "write error, count == %u\n", rr.r[2]);
2125         return -1;
2126     }
2127
2128     return 0;
2129 }