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 <sys/mman.h>
  13
  14
  15 #include "stlink-common.h"
  16 #include "uglylogging.h"
  17
  18 #define LOG_TAG __FILE__
  19 #define DLOG(format, args...)         ugly_log(UDEBUG, LOG_TAG, format, ## args)
  20 #define ILOG(format, args...)         ugly_log(UINFO, LOG_TAG, format, ## args)
  21 #define WLOG(format, args...)         ugly_log(UWARN, LOG_TAG, format, ## args)
  22 #define fatal(format, args...)        ugly_log(UFATAL, LOG_TAG, format, ## args)
  23
  24 /* todo: stm32l15xxx flash memory, pm0062 manual */
  25
  26 /* stm32f FPEC flash controller interface, pm0063 manual */
  27 // TODO - all of this needs to be abstracted out....
  28 #define FLASH_REGS_ADDR 0x40022000
  29 #define FLASH_REGS_SIZE 0x28
  30
  31 #define FLASH_ACR (FLASH_REGS_ADDR + 0x00)
  32 #define FLASH_KEYR (FLASH_REGS_ADDR + 0x04)
  33 #define FLASH_SR (FLASH_REGS_ADDR + 0x0c)
  34 #define FLASH_CR (FLASH_REGS_ADDR + 0x10)
  35 #define FLASH_AR (FLASH_REGS_ADDR + 0x14)
  36 #define FLASH_OBR (FLASH_REGS_ADDR + 0x1c)
  37 #define FLASH_WRPR (FLASH_REGS_ADDR + 0x20)
  38
  39 #define FLASH_RDPTR_KEY 0x00a5
  40 #define FLASH_KEY1 0x45670123
  41 #define FLASH_KEY2 0xcdef89ab
  42
  43 #define FLASH_SR_BSY 0
  44 #define FLASH_SR_EOP 5
  45
  46 #define FLASH_CR_PG 0
  47 #define FLASH_CR_PER 1
  48 #define FLASH_CR_MER 2
  49 #define FLASH_CR_STRT 6
  50 #define FLASH_CR_LOCK 7
  51
  52
  53 //32L = 32F1 same CoreID as 32F4!
  54 #define STM32L_FLASH_REGS_ADDR ((uint32_t)0x40023c00)
  55 #define STM32L_FLASH_ACR (STM32L_FLASH_REGS_ADDR + 0x00)
  56 #define STM32L_FLASH_PECR (STM32L_FLASH_REGS_ADDR + 0x04)
  57 #define STM32L_FLASH_PDKEYR (STM32L_FLASH_REGS_ADDR + 0x08)
  58 #define STM32L_FLASH_PEKEYR (STM32L_FLASH_REGS_ADDR + 0x0c)
  59 #define STM32L_FLASH_PRGKEYR (STM32L_FLASH_REGS_ADDR + 0x10)
  60 #define STM32L_FLASH_OPTKEYR (STM32L_FLASH_REGS_ADDR + 0x14)
  61 #define STM32L_FLASH_SR (STM32L_FLASH_REGS_ADDR + 0x18)
  62 #define STM32L_FLASH_OBR (STM32L_FLASH_REGS_ADDR + 0x0c)
  63 #define STM32L_FLASH_WRPR (STM32L_FLASH_REGS_ADDR + 0x20)
  64
  65
  66 //STM32F4
  67 #define FLASH_F4_REGS_ADDR ((uint32_t)0x40023c00)
  68 #define FLASH_F4_KEYR (FLASH_F4_REGS_ADDR + 0x04)
  69 #define FLASH_F4_OPT_KEYR (FLASH_F4_REGS_ADDR + 0x08)
  70 #define FLASH_F4_SR (FLASH_F4_REGS_ADDR + 0x0c)
  71 #define FLASH_F4_CR (FLASH_F4_REGS_ADDR + 0x10)
  72 #define FLASH_F4_OPT_CR (FLASH_F4_REGS_ADDR + 0x14)
  73 #define FLASH_F4_CR_STRT 16
  74 #define FLASH_F4_CR_LOCK 31
  75 #define FLASH_F4_CR_SER 1
  76 #define FLASH_F4_CR_SNB 3
  77 #define FLASH_F4_CR_SNB_MASK 0x38
  78 #define FLASH_F4_SR_BSY 16
  79
  80
  81 void write_uint32(unsigned char* buf, uint32_t ui) {
  82     if (!is_bigendian()) { // le -> le (don't swap)
  83         buf[0] = ((unsigned char*) &ui)[0];
  84         buf[1] = ((unsigned char*) &ui)[1];
  85         buf[2] = ((unsigned char*) &ui)[2];
  86         buf[3] = ((unsigned char*) &ui)[3];
  87     } else {
  88         buf[0] = ((unsigned char*) &ui)[3];
  89         buf[1] = ((unsigned char*) &ui)[2];
  90         buf[2] = ((unsigned char*) &ui)[1];
  91         buf[3] = ((unsigned char*) &ui)[0];
  92     }
  93 }
  94
  95 void write_uint16(unsigned char* buf, uint16_t ui) {
  96     if (!is_bigendian()) { // le -> le (don't swap)
  97         buf[0] = ((unsigned char*) &ui)[0];
  98         buf[1] = ((unsigned char*) &ui)[1];
  99     } else {
 100         buf[0] = ((unsigned char*) &ui)[1];
 101         buf[1] = ((unsigned char*) &ui)[0];
 102     }
 103 }
 104
 105 uint32_t read_uint32(const unsigned char *c, const int pt) {
 106     uint32_t ui;
 107     char *p = (char *) &ui;
 108
 109     if (!is_bigendian()) { // le -> le (don't swap)
 110         p[0] = c[pt + 0];
 111         p[1] = c[pt + 1];
 112         p[2] = c[pt + 2];
 113         p[3] = c[pt + 3];
 114     } else {
 115         p[0] = c[pt + 3];
 116         p[1] = c[pt + 2];
 117         p[2] = c[pt + 1];
 118         p[3] = c[pt + 0];
 119     }
 120     return ui;
 121 }
 122
 123 static uint32_t __attribute__((unused)) read_flash_rdp(stlink_t *sl) {
 124     stlink_read_mem32(sl, FLASH_WRPR, sizeof (uint32_t));
 125     return read_uint32(sl->q_buf, 0) & 0xff;
 126 }
 127
 128 static inline uint32_t read_flash_wrpr(stlink_t *sl) {
 129     stlink_read_mem32(sl, FLASH_WRPR, sizeof (uint32_t));
 130     return read_uint32(sl->q_buf, 0);
 131 }
 132
 133 static inline uint32_t read_flash_obr(stlink_t *sl) {
 134     stlink_read_mem32(sl, FLASH_OBR, sizeof (uint32_t));
 135     return read_uint32(sl->q_buf, 0);
 136 }
 137
 138 static inline uint32_t read_flash_cr(stlink_t *sl) {
 139         if(sl->chip_id==STM32F4_CHIP_ID)
 140                 stlink_read_mem32(sl, FLASH_F4_CR, sizeof (uint32_t));
 141         else
 142                 stlink_read_mem32(sl, FLASH_CR, sizeof (uint32_t));
 143 #if DEBUG_FLASH
 144         fprintf(stdout, "CR:0x%x\n", *(uint32_t*) sl->q_buf);
 145 #endif
 146         return read_uint32(sl->q_buf, 0);
 147 }
 148
 149 static inline unsigned int is_flash_locked(stlink_t *sl) {
 150     /* return non zero for true */
 151         if(sl->chip_id==STM32F4_CHIP_ID)
 152                 return read_flash_cr(sl) & (1 << FLASH_F4_CR_LOCK);
 153         else
 154                 return read_flash_cr(sl) & (1 << FLASH_CR_LOCK);
 155 }
 156
 157 static void unlock_flash(stlink_t *sl) {
 158     /* the unlock sequence consists of 2 write cycles where
 159        2 key values are written to the FLASH_KEYR register.
 160        an invalid sequence results in a definitive lock of
 161        the FPEC block until next reset.
 162      */
 163     if(sl->chip_id==STM32F4_CHIP_ID) {
 164         write_uint32(sl->q_buf, FLASH_KEY1);
 165         stlink_write_mem32(sl, FLASH_F4_KEYR, sizeof (uint32_t));
 166                 write_uint32(sl->q_buf, FLASH_KEY2);
 167                 stlink_write_mem32(sl, FLASH_F4_KEYR, sizeof (uint32_t));
 168     }
 169         else {
 170         write_uint32(sl->q_buf, FLASH_KEY1);
 171         stlink_write_mem32(sl, FLASH_KEYR, sizeof (uint32_t));
 172                 write_uint32(sl->q_buf, FLASH_KEY2);
 173                 stlink_write_mem32(sl, FLASH_KEYR, sizeof (uint32_t));
 174         }
 175
 176 }
 177
 178 static int unlock_flash_if(stlink_t *sl) {
 179     /* unlock flash if already locked */
 180
 181     if (is_flash_locked(sl)) {
 182         unlock_flash(sl);
 183         if (is_flash_locked(sl)) {
 184             WLOG("Failed to unlock flash!\n");
 185             return -1;
 186         }
 187     }
 188     ILOG("Successfully unlocked flash\n");
 189     return 0;
 190 }
 191
 192 static void lock_flash(stlink_t *sl) {
 193     if(sl->chip_id==STM32F4_CHIP_ID) {
 194         const uint32_t n = read_flash_cr(sl) | (1 << FLASH_F4_CR_LOCK);
 195         write_uint32(sl->q_buf, n);
 196         stlink_write_mem32(sl, FLASH_F4_CR, sizeof (uint32_t));
 197     }
 198     else {
 199         /* write to 1 only. reset by hw at unlock sequence */
 200         const uint32_t n = read_flash_cr(sl) | (1 << FLASH_CR_LOCK);
 201         write_uint32(sl->q_buf, n);
 202         stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 203     }
 204 }
 205
 206
 207 static void set_flash_cr_pg(stlink_t *sl) {
 208     if(sl->chip_id==STM32F4_CHIP_ID) {
 209                 uint32_t x = read_flash_cr(sl);
 210                 x |= (1 << FLASH_CR_PG);
 211                 write_uint32(sl->q_buf, x);
 212         stlink_write_mem32(sl, FLASH_F4_CR, sizeof (uint32_t));
 213     }
 214     else {
 215         const uint32_t n = 1 << FLASH_CR_PG;
 216         write_uint32(sl->q_buf, n);
 217         stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 218     }
 219 }
 220
 221 static void __attribute__((unused)) clear_flash_cr_pg(stlink_t *sl) {
 222     const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_PG);
 223     write_uint32(sl->q_buf, n);
 224     if(sl->chip_id==STM32F4_CHIP_ID)
 225         stlink_write_mem32(sl, FLASH_F4_CR, sizeof (uint32_t));
 226     else
 227         stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 228 }
 229
 230 static void set_flash_cr_per(stlink_t *sl) {
 231     const uint32_t n = 1 << FLASH_CR_PER;
 232     write_uint32(sl->q_buf, n);
 233     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 234 }
 235
 236 static void __attribute__((unused)) clear_flash_cr_per(stlink_t *sl) {
 237     const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_PER);
 238     write_uint32(sl->q_buf, n);
 239     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 240 }
 241
 242 static void set_flash_cr_mer(stlink_t *sl) {
 243     const uint32_t n = 1 << FLASH_CR_MER;
 244     write_uint32(sl->q_buf, n);
 245     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 246 }
 247
 248 static void __attribute__((unused)) clear_flash_cr_mer(stlink_t *sl) {
 249     const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_MER);
 250     write_uint32(sl->q_buf, n);
 251     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 252 }
 253
 254 static void set_flash_cr_strt(stlink_t *sl) {
 255         if(sl->chip_id == STM32F4_CHIP_ID)
 256         {
 257                 uint32_t x = read_flash_cr(sl);
 258                 x |= (1 << FLASH_F4_CR_STRT);
 259                 write_uint32(sl->q_buf, x);
 260                 stlink_write_mem32(sl, FLASH_F4_CR, sizeof (uint32_t));
 261         }
 262         else {
 263                 /* assume come on the flash_cr_per path */
 264             const uint32_t n = (1 << FLASH_CR_PER) | (1 << FLASH_CR_STRT);
 265             write_uint32(sl->q_buf, n);
 266             stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 267         }
 268 }
 269
 270 static inline uint32_t read_flash_acr(stlink_t *sl) {
 271     stlink_read_mem32(sl, FLASH_ACR, sizeof (uint32_t));
 272     return read_uint32(sl->q_buf, 0);
 273 }
 274
 275 static inline uint32_t read_flash_sr(stlink_t *sl) {
 276         if(sl->chip_id==STM32F4_CHIP_ID)
 277                 stlink_read_mem32(sl, FLASH_F4_SR, sizeof (uint32_t));
 278         else
 279                 stlink_read_mem32(sl, FLASH_SR, sizeof (uint32_t));
 280     //fprintf(stdout, "SR:0x%x\n", *(uint32_t*) sl->q_buf);
 281     return read_uint32(sl->q_buf, 0);
 282 }
 283
 284 static inline unsigned int is_flash_busy(stlink_t *sl) {
 285         if(sl->chip_id==STM32F4_CHIP_ID)
 286                 return read_flash_sr(sl) & (1 << FLASH_F4_SR_BSY);
 287         else
 288                 return read_flash_sr(sl) & (1 << FLASH_SR_BSY);
 289 }
 290
 291 static void wait_flash_busy(stlink_t *sl) {
 292     /* todo: add some delays here */
 293     while (is_flash_busy(sl))
 294         ;
 295 }
 296
 297 static inline unsigned int is_flash_eop(stlink_t *sl) {
 298     return read_flash_sr(sl) & (1 << FLASH_SR_EOP);
 299 }
 300
 301 static void __attribute__((unused)) clear_flash_sr_eop(stlink_t *sl) {
 302     const uint32_t n = read_flash_sr(sl) & ~(1 << FLASH_SR_EOP);
 303     write_uint32(sl->q_buf, n);
 304     stlink_write_mem32(sl, FLASH_SR, sizeof (uint32_t));
 305 }
 306
 307 static void __attribute__((unused)) wait_flash_eop(stlink_t *sl) {
 308     /* todo: add some delays here */
 309     while (is_flash_eop(sl) == 0)
 310         ;
 311 }
 312
 313 static inline void write_flash_ar(stlink_t *sl, uint32_t n) {
 314     write_uint32(sl->q_buf, n);
 315     stlink_write_mem32(sl, FLASH_AR, sizeof (uint32_t));
 316 }
 317
 318 static inline void write_flash_cr_psiz(stlink_t *sl, uint32_t n) {
 319     uint32_t x = read_flash_cr(sl);
 320     x &= ~(0x03 << 8);
 321     x |= (n << 8);
 322 #if DEBUG_FLASH
 323     fprintf(stdout, "PSIZ:0x%x 0x%x\n", x, n);
 324 #endif
 325     write_uint32(sl->q_buf, x);
 326     stlink_write_mem32(sl, FLASH_F4_CR, sizeof (uint32_t));
 327 }
 328
 329
 330 static inline void write_flash_cr_snb(stlink_t *sl, uint32_t n) {
 331     uint32_t x = read_flash_cr(sl);
 332     x &= ~FLASH_F4_CR_SNB_MASK;
 333     x |= (n << FLASH_F4_CR_SNB);
 334     x |= (1 << FLASH_F4_CR_SER);
 335 #if DEBUG_FLASH
 336     fprintf(stdout, "SNB:0x%x 0x%x\n", x, n);
 337 #endif
 338     write_uint32(sl->q_buf, x);
 339     stlink_write_mem32(sl, FLASH_F4_CR, sizeof (uint32_t));
 340 }
 341
 342 #if 0 /* todo */
 343
 344 static void disable_flash_read_protection(stlink_t *sl) {
 345     /* erase the option byte area */
 346     /* rdp = 0x00a5; */
 347     /* reset */
 348 }
 349 #endif /* todo */
 350
 351
 352 // Delegates to the backends...
 353
 354 void stlink_close(stlink_t *sl) {
 355     DLOG("*** stlink_close ***\n");
 356     sl->backend->close(sl);
 357     free(sl);
 358 }
 359
 360 void stlink_exit_debug_mode(stlink_t *sl) {
 361     DLOG("*** stlink_exit_debug_mode ***\n");
 362     sl->backend->exit_debug_mode(sl);
 363 }
 364
 365 void stlink_enter_swd_mode(stlink_t *sl) {
 366     DLOG("*** stlink_enter_swd_mode ***\n");
 367     sl->backend->enter_swd_mode(sl);
 368 }
 369
 370 // Force the core into the debug mode -> halted state.
 371 void stlink_force_debug(stlink_t *sl) {
 372     DLOG("*** stlink_force_debug_mode ***\n");
 373     sl->backend->force_debug(sl);
 374 }
 375
 376 void stlink_exit_dfu_mode(stlink_t *sl) {
 377     DLOG("*** stlink_exit_dfu_mode ***\n");
 378     sl->backend->exit_dfu_mode(sl);
 379 }
 380
 381 uint32_t stlink_core_id(stlink_t *sl) {
 382     DLOG("*** stlink_core_id ***\n");
 383     sl->backend->core_id(sl);
 384     if (sl->verbose > 2)
 385         stlink_print_data(sl);
 386     DLOG("core_id = 0x%08x\n", sl->core_id);
 387     return sl->core_id;
 388 }
 389
 390 uint32_t stlink_chip_id(stlink_t *sl) {
 391     stlink_read_mem32(sl, 0xE0042000, 4);
 392     uint32_t chip_id = sl->q_buf[0] | (sl->q_buf[1] << 8) | (sl->q_buf[2] << 16) |
 393             (sl->q_buf[3] << 24);
 394     return chip_id;
 395 }
 396
 397 /**
 398  * Cortex m3 tech ref manual, CPUID register description
 399  * @param sl stlink context
 400  * @param cpuid pointer to the result object
 401  */
 402 void stlink_cpu_id(stlink_t *sl, cortex_m3_cpuid_t *cpuid) {
 403     stlink_read_mem32(sl, CM3_REG_CPUID, 4);
 404     uint32_t raw = read_uint32(sl->q_buf, 0);
 405     cpuid->implementer_id = (raw >> 24) & 0x7f;
 406     cpuid->variant = (raw >> 20) & 0xf;
 407     cpuid->part = (raw >> 4) & 0xfff;
 408     cpuid->revision = raw & 0xf;
 409     return;
 410 }
 411
 412 /**
 413  * reads and decodes the flash parameters, as dynamically as possible
 414  * @param sl
 415  * @return 0 for success, or -1 for unsupported core type.
 416  */
 417 int stlink_load_device_params(stlink_t *sl) {
 418     ILOG("Loading device parameters....\n");
 419     const chip_params_t *params = NULL;
 420
 421     sl->core_id = stlink_core_id(sl);
 422     uint32_t chip_id = stlink_chip_id(sl);
 423
 424     /* Fix chip_id for F4 rev A errata */
 425     if (((chip_id & 0xFFF) == 0x411) && (sl->core_id == CORE_M4_R0)) {
 426       chip_id = 0x413;
 427     }
 428
 429     sl->chip_id = chip_id;
 430         for(size_t i = 0; i < sizeof(devices) / sizeof(devices[0]); i++) {
 431                 if(devices[i].chip_id == (chip_id & 0xFFF)) {
 432                         params = &devices[i];
 433                         break;
 434                 }
 435         }
 436     if (params == NULL) {
 437         WLOG("unknown chip id! %#x\n", chip_id);
 438         return -1;
 439     }
 440
 441     // These are fixed...
 442     sl->flash_base = STM32_FLASH_BASE;
 443     sl->sram_base = STM32_SRAM_BASE;
 444
 445     // read flash size from hardware, if possible...
 446     if ((chip_id & 0xFFF) == STM32_CHIPID_F2) {
 447         sl->flash_size = 0; // FIXME - need to work this out some other way, just set to max possible?
 448     } else if ((chip_id & 0xFFF) == STM32_CHIPID_F4) {
 449                 sl->flash_size = 0x100000;                      //todo: RM0090 error; size register same address as unique ID
 450     } else {
 451         stlink_read_mem32(sl, params->flash_size_reg, 4);
 452         uint32_t flash_size = sl->q_buf[0] | (sl->q_buf[1] << 8);
 453         sl->flash_size = flash_size * 1024;
 454     }
 455     sl->flash_pgsz = params->flash_pagesize;
 456     sl->sram_size = params->sram_size;
 457     sl->sys_base = params->bootrom_base;
 458     sl->sys_size = params->bootrom_size;
 459
 460     ILOG("Device connected is: %s, id %#x\n", params->description, chip_id);
 461     // TODO make note of variable page size here.....
 462     ILOG("SRAM size: %#x bytes (%d KiB), Flash: %#x bytes (%d KiB) in pages of %zd bytes\n",
 463         sl->sram_size, sl->sram_size / 1024, sl->flash_size, sl->flash_size / 1024,
 464         sl->flash_pgsz);
 465     return 0;
 466 }
 467
 468 void stlink_reset(stlink_t *sl) {
 469     DLOG("*** stlink_reset ***\n");
 470     sl->backend->reset(sl);
 471 }
 472
 473 void stlink_jtag_reset(stlink_t *sl, int value) {
 474     DLOG("*** stlink_jtag_reset ***\n");
 475     sl->backend->jtag_reset(sl, value);
 476 }
 477
 478 void stlink_run(stlink_t *sl) {
 479     DLOG("*** stlink_run ***\n");
 480     sl->backend->run(sl);
 481 }
 482
 483 void stlink_status(stlink_t *sl) {
 484     DLOG("*** stlink_status ***\n");
 485     sl->backend->status(sl);
 486     stlink_core_stat(sl);
 487 }
 488
 489 /**
 490  * Decode the version bits, originally from -sg, verified with usb
 491  * @param sl stlink context, assumed to contain valid data in the buffer
 492  * @param slv output parsed version object
 493  */
 494 void _parse_version(stlink_t *sl, stlink_version_t *slv) {
 495     uint32_t b0 = sl->q_buf[0]; //lsb
 496     uint32_t b1 = sl->q_buf[1];
 497     uint32_t b2 = sl->q_buf[2];
 498     uint32_t b3 = sl->q_buf[3];
 499     uint32_t b4 = sl->q_buf[4];
 500     uint32_t b5 = sl->q_buf[5]; //msb
 501
 502     // b0 b1                       || b2 b3  | b4 b5
 503     // 4b        | 6b     | 6b     || 2B     | 2B
 504     // stlink_v  | jtag_v | swim_v || st_vid | stlink_pid
 505
 506     slv->stlink_v = (b0 & 0xf0) >> 4;
 507     slv->jtag_v = ((b0 & 0x0f) << 2) | ((b1 & 0xc0) >> 6);
 508     slv->swim_v = b1 & 0x3f;
 509     slv->st_vid = (b3 << 8) | b2;
 510     slv->stlink_pid = (b5 << 8) | b4;
 511     return;
 512 }
 513
 514 void stlink_version(stlink_t *sl) {
 515     DLOG("*** looking up stlink version\n");
 516     sl->backend->version(sl);
 517     _parse_version(sl, &sl->version);
 518
 519     DLOG("st vid         = 0x%04x (expect 0x%04x)\n", sl->version.st_vid, USB_ST_VID);
 520     DLOG("stlink pid     = 0x%04x\n", sl->version.stlink_pid);
 521     DLOG("stlink version = 0x%x\n", sl->version.stlink_v);
 522     DLOG("jtag version   = 0x%x\n", sl->version.jtag_v);
 523     DLOG("swim version   = 0x%x\n", sl->version.swim_v);
 524     if (sl->version.jtag_v == 0) {
 525         DLOG("    notice: the firmware doesn't support a jtag/swd interface\n");
 526     }
 527     if (sl->version.swim_v == 0) {
 528         DLOG("    notice: the firmware doesn't support a swim interface\n");
 529     }
 530 }
 531
 532 void stlink_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 533     DLOG("*** stlink_write_mem32 %u bytes to %#x\n", len, addr);
 534     if (len % 4 != 0) {
 535         fprintf(stderr, "Error: Data length doesn't have a 32 bit alignment: +%d byte.\n", len % 4);
 536         return;
 537     }
 538     sl->backend->write_mem32(sl, addr, len);
 539 }
 540
 541 void stlink_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 542     DLOG("*** stlink_read_mem32 ***\n");
 543     if (len % 4 != 0) { // !!! never ever: fw gives just wrong values
 544         fprintf(stderr, "Error: Data length doesn't have a 32 bit alignment: +%d byte.\n",
 545                 len % 4);
 546         return;
 547     }
 548     sl->backend->read_mem32(sl, addr, len);
 549 }
 550
 551 void stlink_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
 552     DLOG("*** stlink_write_mem8 ***\n");
 553     sl->backend->write_mem8(sl, addr, len);
 554 }
 555
 556 void stlink_read_all_regs(stlink_t *sl, reg *regp) {
 557     DLOG("*** stlink_read_all_regs ***\n");
 558     sl->backend->read_all_regs(sl, regp);
 559 }
 560
 561 void stlink_write_reg(stlink_t *sl, uint32_t reg, int idx) {
 562     DLOG("*** stlink_write_reg\n");
 563     sl->backend->write_reg(sl, reg, idx);
 564 }
 565
 566 void stlink_read_reg(stlink_t *sl, int r_idx, reg *regp) {
 567     DLOG("*** stlink_read_reg\n");
 568     DLOG(" (%d) ***\n", r_idx);
 569
 570     if (r_idx > 20 || r_idx < 0) {
 571         fprintf(stderr, "Error: register index must be in [0..20]\n");
 572         return;
 573     }
 574
 575     sl->backend->read_reg(sl, r_idx, regp);
 576 }
 577
 578 unsigned int is_core_halted(stlink_t *sl) {
 579     /* return non zero if core is halted */
 580     stlink_status(sl);
 581     return sl->q_buf[0] == STLINK_CORE_HALTED;
 582 }
 583
 584 void stlink_step(stlink_t *sl) {
 585     DLOG("*** stlink_step ***\n");
 586     sl->backend->step(sl);
 587 }
 588
 589 int stlink_current_mode(stlink_t *sl) {
 590     int mode = sl->backend->current_mode(sl);
 591     switch (mode) {
 592         case STLINK_DEV_DFU_MODE:
 593             DLOG("stlink current mode: dfu\n");
 594             return mode;
 595         case STLINK_DEV_DEBUG_MODE:
 596             DLOG("stlink current mode: debug (jtag or swd)\n");
 597             return mode;
 598         case STLINK_DEV_MASS_MODE:
 599             DLOG("stlink current mode: mass\n");
 600             return mode;
 601     }
 602     DLOG("stlink mode: unknown!\n");
 603     return STLINK_DEV_UNKNOWN_MODE;
 604 }
 605
 606
 607
 608
 609 // End of delegates....  Common code below here...
 610
 611 // Endianness
 612 // http://www.ibm.com/developerworks/aix/library/au-endianc/index.html
 613 // const int i = 1;
 614 // #define is_bigendian() ( (*(char*)&i) == 0 )
 615
 616 inline unsigned int is_bigendian(void) {
 617     static volatile const unsigned int i = 1;
 618     return *(volatile const char*) &i == 0;
 619 }
 620
 621 uint16_t read_uint16(const unsigned char *c, const int pt) {
 622     uint32_t ui;
 623     char *p = (char *) &ui;
 624
 625     if (!is_bigendian()) { // le -> le (don't swap)
 626         p[0] = c[pt + 0];
 627         p[1] = c[pt + 1];
 628     } else {
 629         p[0] = c[pt + 1];
 630         p[1] = c[pt + 0];
 631     }
 632     return ui;
 633 }
 634
 635 // same as above with entrypoint.
 636
 637 void stlink_run_at(stlink_t *sl, stm32_addr_t addr) {
 638     stlink_write_reg(sl, addr, 15); /* pc register */
 639
 640     stlink_run(sl);
 641
 642     while (is_core_halted(sl) == 0)
 643         usleep(3000000);
 644 }
 645
 646 void stlink_core_stat(stlink_t *sl) {
 647     if (sl->q_len <= 0)
 648         return;
 649
 650     switch (sl->q_buf[0]) {
 651         case STLINK_CORE_RUNNING:
 652             sl->core_stat = STLINK_CORE_RUNNING;
 653             DLOG("  core status: running\n");
 654             return;
 655         case STLINK_CORE_HALTED:
 656             sl->core_stat = STLINK_CORE_HALTED;
 657             DLOG("  core status: halted\n");
 658             return;
 659         default:
 660             sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
 661             fprintf(stderr, "  core status: unknown\n");
 662     }
 663 }
 664
 665 void stlink_print_data(stlink_t * sl) {
 666     if (sl->q_len <= 0 || sl->verbose < UDEBUG)
 667         return;
 668     if (sl->verbose > 2)
 669         fprintf(stdout, "data_len = %d 0x%x\n", sl->q_len, sl->q_len);
 670
 671     for (int i = 0; i < sl->q_len; i++) {
 672         if (i % 16 == 0) {
 673             /*
 674                                     if (sl->q_data_dir == Q_DATA_OUT)
 675                                             fprintf(stdout, "\n<- 0x%08x ", sl->q_addr + i);
 676                                     else
 677                                             fprintf(stdout, "\n-> 0x%08x ", sl->q_addr + i);
 678              */
 679         }
 680         fprintf(stdout, " %02x", (unsigned int) sl->q_buf[i]);
 681     }
 682     fputs("\n\n", stdout);
 683 }
 684
 685 /* memory mapped file */
 686
 687 typedef struct mapped_file {
 688     uint8_t* base;
 689     size_t len;
 690 } mapped_file_t;
 691
 692 #define MAPPED_FILE_INITIALIZER { NULL, 0 }
 693
 694 static int map_file(mapped_file_t* mf, const char* path) {
 695     int error = -1;
 696     struct stat st;
 697
 698     const int fd = open(path, O_RDONLY);
 699     if (fd == -1) {
 700         fprintf(stderr, "open(%s) == -1\n", path);
 701         return -1;
 702     }
 703
 704     if (fstat(fd, &st) == -1) {
 705         fprintf(stderr, "fstat() == -1\n");
 706         goto on_error;
 707     }
 708
 709     mf->base = (uint8_t*) mmap(NULL, st.st_size, PROT_READ, MAP_SHARED, fd, 0);
 710     if (mf->base == MAP_FAILED) {
 711         fprintf(stderr, "mmap() == MAP_FAILED\n");
 712         goto on_error;
 713     }
 714
 715     mf->len = st.st_size;
 716
 717     /* success */
 718     error = 0;
 719
 720 on_error:
 721     close(fd);
 722
 723     return error;
 724 }
 725
 726 static void unmap_file(mapped_file_t * mf) {
 727     munmap((void*) mf->base, mf->len);
 728     mf->base = (unsigned char*) MAP_FAILED;
 729     mf->len = 0;
 730 }
 731
 732 static int check_file(stlink_t* sl, mapped_file_t* mf, stm32_addr_t addr) {
 733     size_t off;
 734
 735     for (off = 0; off < mf->len; off += sl->flash_pgsz) {
 736         size_t aligned_size;
 737
 738         /* adjust last page size */
 739         size_t cmp_size = sl->flash_pgsz;
 740         if ((off + sl->flash_pgsz) > mf->len)
 741             cmp_size = mf->len - off;
 742
 743         aligned_size = cmp_size;
 744         if (aligned_size & (4 - 1))
 745             aligned_size = (cmp_size + 4) & ~(4 - 1);
 746
 747         stlink_read_mem32(sl, addr + off, aligned_size);
 748
 749         if (memcmp(sl->q_buf, mf->base + off, cmp_size))
 750             return -1;
 751     }
 752
 753     return 0;
 754 }
 755
 756 int stlink_fwrite_sram
 757 (stlink_t * sl, const char* path, stm32_addr_t addr) {
 758     /* write the file in sram at addr */
 759
 760     int error = -1;
 761     size_t off;
 762     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
 763
 764     if (map_file(&mf, path) == -1) {
 765         fprintf(stderr, "map_file() == -1\n");
 766         return -1;
 767     }
 768
 769     /* check addr range is inside the sram */
 770     if (addr < sl->sram_base) {
 771         fprintf(stderr, "addr too low\n");
 772         goto on_error;
 773     } else if ((addr + mf.len) < addr) {
 774         fprintf(stderr, "addr overruns\n");
 775         goto on_error;
 776     } else if ((addr + mf.len) > (sl->sram_base + sl->sram_size)) {
 777         fprintf(stderr, "addr too high\n");
 778         goto on_error;
 779     } else if ((addr & 3) || (mf.len & 3)) {
 780         /* todo */
 781         fprintf(stderr, "unaligned addr or size\n");
 782         goto on_error;
 783     }
 784
 785     /* do the copy by 1k blocks */
 786     for (off = 0; off < mf.len; off += 1024) {
 787         size_t size = 1024;
 788         if ((off + size) > mf.len)
 789             size = mf.len - off;
 790
 791         memcpy(sl->q_buf, mf.base + off, size);
 792
 793         /* round size if needed */
 794         if (size & 3)
 795             size += 2;
 796
 797         stlink_write_mem32(sl, addr + off, size);
 798     }
 799
 800     /* check the file ha been written */
 801     if (check_file(sl, &mf, addr) == -1) {
 802         fprintf(stderr, "check_file() == -1\n");
 803         goto on_error;
 804     }
 805
 806     /* success */
 807     error = 0;
 808
 809 on_error:
 810     unmap_file(&mf);
 811     return error;
 812 }
 813
 814 int stlink_fread(stlink_t* sl, const char* path, stm32_addr_t addr, size_t size) {
 815     /* read size bytes from addr to file */
 816
 817     int error = -1;
 818     size_t off;
 819
 820     const int fd = open(path, O_RDWR | O_TRUNC | O_CREAT, 00700);
 821     if (fd == -1) {
 822         fprintf(stderr, "open(%s) == -1\n", path);
 823         return -1;
 824     }
 825
 826     /* do the copy by 1k blocks */
 827     for (off = 0; off < size; off += 1024) {
 828         size_t read_size = 1024;
 829         size_t rounded_size;
 830         if ((off + read_size) > size)
 831           read_size = size - off;
 832
 833         /* round size if needed */
 834         rounded_size = read_size;
 835         if (rounded_size & 3)
 836           rounded_size = (rounded_size + 4) & ~(3);
 837
 838         stlink_read_mem32(sl, addr + off, rounded_size);
 839
 840         if (write(fd, sl->q_buf, read_size) != (ssize_t) read_size) {
 841             fprintf(stderr, "write() != read_size\n");
 842             goto on_error;
 843         }
 844     }
 845
 846     /* success */
 847     error = 0;
 848
 849 on_error:
 850     close(fd);
 851
 852     return error;
 853 }
 854
 855 int write_buffer_to_sram(stlink_t *sl, flash_loader_t* fl, const uint8_t* buf, size_t size) {
 856     /* write the buffer right after the loader */
 857     memcpy(sl->q_buf, buf, size);
 858     stlink_write_mem8(sl, fl->buf_addr, size);
 859     return 0;
 860 }
 861
 862 uint32_t calculate_F4_sectornum(uint32_t flashaddr){
 863     flashaddr &= ~STM32_FLASH_BASE;     //Page now holding the actual flash address
 864     if (flashaddr<0x4000) return (0);
 865     else if(flashaddr<0x8000) return(1);
 866     else if(flashaddr<0xc000) return(2);
 867     else if(flashaddr<0x10000) return(3);
 868     else if(flashaddr<0x20000) return(4);
 869     else return(flashaddr/0x20000)+4;
 870
 871 }
 872
 873 uint32_t stlink_calculate_pagesize(stlink_t *sl, uint32_t flashaddr){
 874         if(sl->chip_id == STM32F4_CHIP_ID) {
 875                 uint32_t sector=calculate_F4_sectornum(flashaddr);
 876                 if (sector<4) sl->flash_pgsz=0x4000;
 877                 else if(sector<5) sl->flash_pgsz=0x10000;
 878                 else sl->flash_pgsz=0x20000;
 879         }
 880         return (sl->flash_pgsz);
 881 }
 882
 883 /**
 884  * Erase a page of flash, assumes sl is fully populated with things like chip/core ids
 885  * @param sl stlink context
 886  * @param flashaddr an address in the flash page to erase
 887  * @return 0 on success -ve on failure
 888  */
 889 int stlink_erase_flash_page(stlink_t *sl, stm32_addr_t flashaddr)
 890 {
 891   ILOG("Erasing flash page at addr: %#x\n", flashaddr);
 892   if (sl->chip_id == STM32F4_CHIP_ID)
 893   {
 894     /* wait for ongoing op to finish */
 895     wait_flash_busy(sl);
 896
 897     /* unlock if locked */
 898     unlock_flash_if(sl);
 899
 900     /* select the page to erase */
 901     // calculate the actual page from the address
 902     uint32_t sector=calculate_F4_sectornum(flashaddr);
 903
 904     fprintf(stderr, "EraseFlash - Sector:0x%x Size:0x%x\n", sector, stlink_calculate_pagesize(sl, flashaddr));
 905     write_flash_cr_snb(sl, sector);
 906
 907     /* start erase operation */
 908     set_flash_cr_strt(sl);
 909
 910     /* wait for completion */
 911     wait_flash_busy(sl);
 912
 913     /* relock the flash */
 914     //todo: fails to program if this is in
 915     lock_flash(sl);
 916 #if DEBUG_FLASH
 917         fprintf(stdout, "Erase Final CR:0x%x\n", read_flash_cr(sl));
 918 #endif
 919   }
 920   else if (sl->core_id == STM32L_CORE_ID)
 921   {
 922
 923     uint32_t val;
 924
 925     /* disable pecr protection */
 926     write_uint32(sl->q_buf, 0x89abcdef);
 927     stlink_write_mem32(sl, STM32L_FLASH_PEKEYR, sizeof(uint32_t));
 928     write_uint32(sl->q_buf, 0x02030405);
 929     stlink_write_mem32(sl, STM32L_FLASH_PEKEYR, sizeof(uint32_t));
 930
 931     /* check pecr.pelock is cleared */
 932     stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
 933     val = read_uint32(sl->q_buf, 0);
 934     if (val & (1 << 0))
 935     {
 936       WLOG("pecr.pelock not clear (%#x)\n", val);
 937       return -1;
 938     }
 939
 940     /* unlock program memory */
 941     write_uint32(sl->q_buf, 0x8c9daebf);
 942     stlink_write_mem32(sl, STM32L_FLASH_PRGKEYR, sizeof(uint32_t));
 943     write_uint32(sl->q_buf, 0x13141516);
 944     stlink_write_mem32(sl, STM32L_FLASH_PRGKEYR, sizeof(uint32_t));
 945
 946     /* check pecr.prglock is cleared */
 947     stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
 948     val = read_uint32(sl->q_buf, 0);
 949     if (val & (1 << 1))
 950     {
 951       WLOG("pecr.prglock not clear (%#x)\n", val);
 952       return -1;
 953     }
 954
 955     /* unused: unlock the option byte block */
 956 #if 0
 957     write_uint32(sl->q_buf, 0xfbead9c8);
 958     stlink_write_mem32(sl, STM32L_FLASH_OPTKEYR, sizeof(uint32_t));
 959     write_uint32(sl->q_buf, 0x24252627);
 960     stlink_write_mem32(sl, STM32L_FLASH_OPTKEYR, sizeof(uint32_t));
 961
 962     /* check pecr.optlock is cleared */
 963     stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
 964     val = read_uint32(sl->q_buf, 0);
 965     if (val & (1 << 2))
 966     {
 967       fprintf(stderr, "pecr.prglock not clear\n");
 968       return -1;
 969     }
 970 #endif
 971
 972     /* set pecr.{erase,prog} */
 973     val |= (1 << 9) | (1 << 3);
 974     write_uint32(sl->q_buf, val);
 975     stlink_write_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
 976
 977 #if 0 /* fix_to_be_confirmed */
 978
 979     /* wait for sr.busy to be cleared
 980        MP: Test shows that busy bit is not set here. Perhaps, PM0062 is
 981        wrong and we do not need to wait here for clearing the busy bit.
 982        TEXANE: ok, if experience says so and it works for you, we comment
 983        it. If someone has a problem, please drop an email.
 984      */
 985     while (1)
 986     {
 987       stlink_read_mem32(sl, STM32L_FLASH_SR, sizeof(uint32_t));
 988       if ((read_uint32(sl->q_buf, 0) & (1 << 0)) == 0) break ;
 989     }
 990
 991 #endif /* fix_to_be_confirmed */
 992
 993     /* write 0 to the first word of the page to be erased */
 994     memset(sl->q_buf, 0, sizeof(uint32_t));
 995     stlink_write_mem32(sl, flashaddr, sizeof(uint32_t));
 996
 997     /* MP: It is better to wait for clearing the busy bit after issuing
 998     page erase command, even though PM0062 recommends to wait before it.
 999     Test shows that a few iterations is performed in the following loop
1000     before busy bit is cleared.*/
1001     while (1)
1002     {
1003       stlink_read_mem32(sl, STM32L_FLASH_SR, sizeof(uint32_t));
1004       if ((read_uint32(sl->q_buf, 0) & (1 << 0)) == 0) break;
1005     }
1006
1007     /* reset lock bits */
1008     stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1009     val = read_uint32(sl->q_buf, 0) | (1 << 0) | (1 << 1) | (1 << 2);
1010     write_uint32(sl->q_buf, val);
1011     stlink_write_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1012   }
1013   else if (sl->core_id == STM32VL_CORE_ID)
1014   {
1015     /* wait for ongoing op to finish */
1016     wait_flash_busy(sl);
1017
1018     /* unlock if locked */
1019     unlock_flash_if(sl);
1020
1021     /* set the page erase bit */
1022     set_flash_cr_per(sl);
1023
1024     /* select the page to erase */
1025     write_flash_ar(sl, flashaddr);
1026
1027     /* start erase operation, reset by hw with bsy bit */
1028     set_flash_cr_strt(sl);
1029
1030     /* wait for completion */
1031     wait_flash_busy(sl);
1032
1033     /* relock the flash */
1034     lock_flash(sl);
1035   }
1036
1037   else {
1038     WLOG("unknown coreid: %x\n", sl->core_id);
1039     return -1;
1040   }
1041
1042   /* todo: verify the erased page */
1043
1044   return 0;
1045 }
1046
1047 int stlink_erase_flash_mass(stlink_t *sl) {
1048     /* wait for ongoing op to finish */
1049     wait_flash_busy(sl);
1050
1051     /* unlock if locked */
1052     unlock_flash_if(sl);
1053
1054     /* set the mass erase bit */
1055     set_flash_cr_mer(sl);
1056
1057     /* start erase operation, reset by hw with bsy bit */
1058     set_flash_cr_strt(sl);
1059
1060     /* wait for completion */
1061     wait_flash_busy(sl);
1062
1063     /* relock the flash */
1064     lock_flash(sl);
1065
1066     /* todo: verify the erased memory */
1067
1068     return 0;
1069 }
1070
1071 int init_flash_loader(stlink_t *sl, flash_loader_t* fl) {
1072     size_t size;
1073
1074     /* allocate the loader in sram */
1075     if (write_loader_to_sram(sl, &fl->loader_addr, &size) == -1) {
1076         WLOG("Failed to write flash loader to sram!\n");
1077         return -1;
1078     }
1079
1080     /* allocate a one page buffer in sram right after loader */
1081     fl->buf_addr = fl->loader_addr + size;
1082     ILOG("Successfully loaded flash loader in sram\n");
1083     return 0;
1084 }
1085
1086 int write_loader_to_sram(stlink_t *sl, stm32_addr_t* addr, size_t* size) {
1087     /* from openocd, contrib/loaders/flash/stm32.s */
1088     static const uint8_t loader_code_stm32vl[] = {
1089         0x08, 0x4c, /* ldr      r4, STM32_FLASH_BASE */
1090         0x1c, 0x44, /* add      r4, r3 */
1091         /* write_half_word: */
1092         0x01, 0x23, /* movs     r3, #0x01 */
1093         0x23, 0x61, /* str      r3, [r4, #STM32_FLASH_CR_OFFSET] */
1094         0x30, 0xf8, 0x02, 0x3b, /* ldrh r3, [r0], #0x02 */
1095         0x21, 0xf8, 0x02, 0x3b, /* strh r3, [r1], #0x02 */
1096         /* busy: */
1097         0xe3, 0x68, /* ldr      r3, [r4, #STM32_FLASH_SR_OFFSET] */
1098         0x13, 0xf0, 0x01, 0x0f, /* tst  r3, #0x01 */
1099         0xfb, 0xd0, /* beq      busy */
1100         0x13, 0xf0, 0x14, 0x0f, /* tst  r3, #0x14 */
1101         0x01, 0xd1, /* bne      exit */
1102         0x01, 0x3a, /* subs     r2, r2, #0x01 */
1103         0xf0, 0xd1, /* bne      write_half_word */
1104         /* exit: */
1105         0x00, 0xbe, /* bkpt     #0x00 */
1106         0x00, 0x20, 0x02, 0x40, /* STM32_FLASH_BASE: .word 0x40022000 */
1107     };
1108
1109     static const uint8_t loader_code_stm32l[] = {
1110
1111       /* openocd.git/contrib/loaders/flash/stm32lx.S
1112          r0, input, dest addr
1113          r1, input, source addr
1114          r2, input, word count
1115          r3, output, word count
1116        */
1117
1118       0x00, 0x23,
1119       0x04, 0xe0,
1120
1121       0x51, 0xf8, 0x04, 0xcb,
1122       0x40, 0xf8, 0x04, 0xcb,
1123       0x01, 0x33,
1124
1125       0x93, 0x42,
1126       0xf8, 0xd3,
1127       0x00, 0xbe
1128     };
1129
1130     const uint8_t* loader_code;
1131     size_t loader_size;
1132
1133     if (sl->core_id == STM32L_CORE_ID) /* stm32l */
1134     {
1135       loader_code = loader_code_stm32l;
1136       loader_size = sizeof(loader_code_stm32l);
1137     }
1138     else if (sl->core_id == STM32VL_CORE_ID)
1139     {
1140       loader_code = loader_code_stm32vl;
1141       loader_size = sizeof(loader_code_stm32vl);
1142     }
1143     else
1144     {
1145       WLOG("unknown coreid, not sure what flash loader to use, aborting!: %x\n", sl->core_id);
1146       return -1;
1147     }
1148
1149     memcpy(sl->q_buf, loader_code, loader_size);
1150     stlink_write_mem32(sl, sl->sram_base, loader_size);
1151
1152     *addr = sl->sram_base;
1153     *size = loader_size;
1154
1155     /* success */
1156     return 0;
1157 }
1158
1159 int stlink_fcheck_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
1160     /* check the contents of path are at addr */
1161
1162     int res;
1163     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1164
1165     if (map_file(&mf, path) == -1)
1166         return -1;
1167
1168     res = check_file(sl, &mf, addr);
1169
1170     unmap_file(&mf);
1171
1172     return res;
1173 }
1174
1175 /**
1176  * Verify addr..addr+len is binary identical to base...base+len
1177  * @param sl stlink context
1178  * @param address stm device address
1179  * @param data host side buffer to check against
1180  * @param length how much
1181  * @return 0 for success, -ve for failure
1182  */
1183 int stlink_verify_write_flash(stlink_t *sl, stm32_addr_t address, uint8_t *data, unsigned length) {
1184     size_t off;
1185     if ((sl->chip_id & 0xFFF) == STM32_CHIPID_F4) {
1186         DLOG("(FIXME)Skipping verification for F4, not enough ram (yet)\n");
1187         return 0;
1188     }
1189     ILOG("Starting verification of write complete\n");
1190     for (off = 0; off < length; off += sl->flash_pgsz) {
1191         size_t aligned_size;
1192
1193         /* adjust last page size */
1194         size_t cmp_size = sl->flash_pgsz;
1195         if ((off + sl->flash_pgsz) > length)
1196             cmp_size = length - off;
1197
1198         aligned_size = cmp_size;
1199         if (aligned_size & (4 - 1))
1200             aligned_size = (cmp_size + 4) & ~(4 - 1);
1201
1202         stlink_read_mem32(sl, address + off, aligned_size);
1203
1204         if (memcmp(sl->q_buf, data + off, cmp_size)) {
1205             WLOG("Verification of flash failed at offset: %zd\n", off);
1206             return -1;
1207         }
1208     }
1209     ILOG("Flash written and verified! jolly good!\n");
1210     return 0;
1211
1212 }
1213
1214 int stlink_write_flash(stlink_t *sl, stm32_addr_t addr, uint8_t* base, unsigned len) {
1215     size_t off;
1216     flash_loader_t fl;
1217     ILOG("Attempting to write %d (%#x) bytes to stm32 address: %u (%#x)\n",
1218         len, len, addr, addr);
1219     /* check addr range is inside the flash */
1220     stlink_calculate_pagesize(sl, addr);
1221     if (addr < sl->flash_base) {
1222         WLOG("addr too low %#x < %#x\n", addr, sl->flash_base);
1223         return -1;
1224     } else if ((addr + len) < addr) {
1225         WLOG("addr overruns\n");
1226         return -1;
1227     } else if ((addr + len) > (sl->flash_base + sl->flash_size)) {
1228         WLOG("addr too high\n");
1229         return -1;
1230     } else if ((addr & 1) || (len & 1)) {
1231         WLOG("unaligned addr or size\n");
1232         return -1;
1233     } else if (addr & (sl->flash_pgsz - 1)) {
1234         WLOG("addr not a multiple of pagesize, not supported\n");
1235         return -1;
1236     }
1237
1238     // Make sure we've loaded the context with the chip details
1239     stlink_core_id(sl);
1240     /* erase each page */
1241     int page_count = 0;
1242     for (off = 0; off < len; off += stlink_calculate_pagesize(sl, addr + off)) {
1243         /* addr must be an addr inside the page */
1244         if (stlink_erase_flash_page(sl, addr + off) == -1) {
1245             WLOG("Failed to erase_flash_page(%#zx) == -1\n", addr + off);
1246             return -1;
1247         }
1248         page_count++;
1249     }
1250     ILOG("Finished erasing %d pages of %d (%#x) bytes\n",
1251         page_count, sl->flash_pgsz, sl->flash_pgsz);
1252
1253     if (sl->chip_id == STM32F4_CHIP_ID) {
1254         /* todo: check write operation */
1255
1256         /* First unlock the cr */
1257         unlock_flash_if(sl);
1258
1259         /* set parallelisim to 32 bit*/
1260         write_flash_cr_psiz(sl, 2);
1261
1262         /* set programming mode */
1263         set_flash_cr_pg(sl);
1264
1265 #define PROGRESS_CHUNK_SIZE 0x1000
1266         /* write a word in program memory */
1267         for (off = 0; off < len; off += sizeof(uint32_t)) {
1268                 if (sl->verbose >= 1) {
1269                         if ((off & (PROGRESS_CHUNK_SIZE - 1)) == 0) {
1270                                 /* show progress. writing procedure is slow
1271                                            and previous errors are misleading */
1272                                 const uint32_t pgnum = (off / PROGRESS_CHUNK_SIZE)+1;
1273                                 const uint32_t pgcount = len / PROGRESS_CHUNK_SIZE;
1274                                 fprintf(stdout, "Writing %ukB chunk %u out of %u\n", PROGRESS_CHUNK_SIZE/1024, pgnum, pgcount);
1275                         }
1276                 }
1277
1278                 memcpy(sl->q_buf, (const void*)(base + off), sizeof(uint32_t));
1279                 stlink_write_mem32(sl, addr + off, sizeof(uint32_t));
1280
1281                 /* wait for sr.busy to be cleared */
1282             wait_flash_busy(sl);
1283
1284         }
1285         /* Relock flash */
1286         lock_flash(sl);
1287
1288 #if 0 /* todo: debug mode */
1289         fprintf(stdout, "Final CR:0x%x\n", read_flash_cr(sl));
1290 #endif
1291
1292
1293
1294     }   //STM32F4END
1295
1296     else if (sl->core_id == STM32L_CORE_ID)    {
1297         /* use fast word write. todo: half page. */
1298         uint32_t val;
1299
1300 #if 0 /* todo: check write operation */
1301
1302         uint32_t nwrites = sl->flash_pgsz;
1303
1304         redo_write:
1305
1306 #endif /* todo: check write operation */
1307
1308         /* disable pecr protection */
1309         write_uint32(sl->q_buf, 0x89abcdef);
1310         stlink_write_mem32(sl, STM32L_FLASH_PEKEYR, sizeof(uint32_t));
1311         write_uint32(sl->q_buf, 0x02030405);
1312         stlink_write_mem32(sl, STM32L_FLASH_PEKEYR, sizeof(uint32_t));
1313
1314         /* check pecr.pelock is cleared */
1315         stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1316         val = read_uint32(sl->q_buf, 0);
1317         if (val & (1 << 0)) {
1318                 fprintf(stderr, "pecr.pelock not clear\n");
1319                 return -1;
1320         }
1321
1322         /* unlock program memory */
1323         write_uint32(sl->q_buf, 0x8c9daebf);
1324         stlink_write_mem32(sl, STM32L_FLASH_PRGKEYR, sizeof(uint32_t));
1325         write_uint32(sl->q_buf, 0x13141516);
1326         stlink_write_mem32(sl, STM32L_FLASH_PRGKEYR, sizeof(uint32_t));
1327
1328         /* check pecr.prglock is cleared */
1329         stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1330         val = read_uint32(sl->q_buf, 0);
1331         if (val & (1 << 1)) {
1332                 fprintf(stderr, "pecr.prglock not clear\n");
1333                 return -1;
1334         }
1335
1336         /* write a word in program memory */
1337         for (off = 0; off < len; off += sizeof(uint32_t)) {
1338                 if (sl->verbose >= 1) {
1339                         if ((off & (sl->flash_pgsz - 1)) == 0) {
1340                                 /* show progress. writing procedure is slow
1341                                    and previous errors are misleading */
1342                                 const uint32_t pgnum = off / sl->flash_pgsz;
1343                                 const uint32_t pgcount = len / sl->flash_pgsz;
1344                                 fprintf(stdout, "%u pages written out of %u\n", pgnum, pgcount);
1345                         }
1346                 }
1347
1348                 memcpy(sl->q_buf, (const void*)(base + off), sizeof(uint32_t));
1349                 stlink_write_mem32(sl, addr + off, sizeof(uint32_t));
1350
1351                 /* wait for sr.busy to be cleared */
1352                 while (1) {
1353                         stlink_read_mem32(sl, STM32L_FLASH_SR, sizeof(uint32_t));
1354                         if ((read_uint32(sl->q_buf, 0) & (1 << 0)) == 0) break ;
1355                 }
1356
1357 #if 0 /* todo: check redo write operation */
1358
1359                 /* check written bytes. todo: should be on a per page basis. */
1360                 stlink_read_mem32(sl, addr + off, sizeof(uint32_t));
1361                 if (memcmp(sl->q_buf, base + off, sizeof(uint32_t))) {
1362                         /* re erase the page and redo the write operation */
1363                         uint32_t page;
1364                         uint32_t val;
1365
1366                         /* fail if successive write count too low */
1367                         if (nwrites < sl->flash_pgsz) {
1368                                 fprintf(stderr, "writes operation failure count too high, aborting\n");
1369                                 return -1;
1370                         }
1371
1372                         nwrites = 0;
1373
1374                         /* assume addr aligned */
1375                         if (off % sl->flash_pgsz) off &= ~(sl->flash_pgsz - 1);
1376                         page = addr + off;
1377
1378                         fprintf(stderr, "invalid write @0x%x(0x%x): 0x%x != 0x%x. retrying.\n",
1379                                         page, addr + off, read_uint32(base + off, 0), read_uint32(sl->q_buf, 0));
1380
1381                         /* reset lock bits */
1382                         stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1383                         val = read_uint32(sl->q_buf, 0) | (1 << 0) | (1 << 1) | (1 << 2);
1384                         write_uint32(sl->q_buf, val);
1385                         stlink_write_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1386
1387                         stlink_erase_flash_page(sl, page);
1388
1389                         goto redo_write;
1390                 }
1391
1392                 /* increment successive writes counter */
1393                 ++nwrites;
1394
1395 #endif /* todo: check redo write operation */
1396         }
1397         /* reset lock bits */
1398         stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1399         val = read_uint32(sl->q_buf, 0) | (1 << 0) | (1 << 1) | (1 << 2);
1400         write_uint32(sl->q_buf, val);
1401         stlink_write_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1402     } else if (sl->core_id == STM32VL_CORE_ID) {
1403         ILOG("Starting Flash write for VL core id\n");
1404         /* flash loader initialization */
1405         if (init_flash_loader(sl, &fl) == -1) {
1406             WLOG("init_flash_loader() == -1\n");
1407             return -1;
1408         }
1409
1410         /* write each page. above WRITE_BLOCK_SIZE fails? */
1411 #define WRITE_BLOCK_SIZE 0x40
1412         int write_block_count = 0;
1413         for (off = 0; off < len; off += WRITE_BLOCK_SIZE) {
1414             ILOG("Writing flash block %d of size %d (%#x)\n", write_block_count,
1415                 WRITE_BLOCK_SIZE, WRITE_BLOCK_SIZE);
1416             /* adjust last write size */
1417             size_t size = WRITE_BLOCK_SIZE;
1418             if ((off + WRITE_BLOCK_SIZE) > len) size = len - off;
1419
1420             /* unlock and set programming mode */
1421             unlock_flash_if(sl);
1422             set_flash_cr_pg(sl);
1423             //DLOG("Finished setting flash cr pg, running loader!\n");
1424             if (run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
1425                 WLOG("run_flash_loader(%#zx) failed! == -1\n", addr + off);
1426                 return -1;
1427             }
1428             lock_flash(sl);
1429             DLOG("Finished writing block %d\n", write_block_count++);
1430         }
1431     } else {
1432         WLOG("unknown coreid, not sure how to write: %x\n", sl->core_id);
1433         return -1;
1434     }
1435
1436     return stlink_verify_write_flash(sl, addr, base, len);
1437 }
1438
1439 /**
1440  * Write the given binary file into flash at address "addr"
1441  * @param sl
1442  * @param path readable file path, should be binary image
1443  * @param addr where to start writing
1444  * @return 0 on success, -ve on failure.
1445  */
1446 int stlink_fwrite_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
1447     /* write the file in flash at addr */
1448     int err;
1449     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1450     if (map_file(&mf, path) == -1) {
1451         WLOG("map_file() == -1\n");
1452         return -1;
1453     }
1454     err = stlink_write_flash(sl, addr, mf.base, mf.len);
1455     unmap_file(&mf);
1456     return err;
1457 }
1458
1459 int run_flash_loader(stlink_t *sl, flash_loader_t* fl, stm32_addr_t target, const uint8_t* buf, size_t size) {
1460
1461     reg rr;
1462     DLOG("Running flash loader, write address:%#x, size: %zd\n", target, size);
1463     // FIXME This can never return -1
1464     if (write_buffer_to_sram(sl, fl, buf, size) == -1) {
1465         // IMPOSSIBLE!
1466         WLOG("write_buffer_to_sram() == -1\n");
1467         return -1;
1468     }
1469
1470     if (sl->core_id == STM32L_CORE_ID) {
1471
1472       size_t count = size / sizeof(uint32_t);
1473       if (size % sizeof(uint32_t)) ++count;
1474
1475       /* setup core */
1476       stlink_write_reg(sl, target, 0); /* target */
1477       stlink_write_reg(sl, fl->buf_addr, 1); /* source */
1478       stlink_write_reg(sl, count, 2); /* count (32 bits words) */
1479       stlink_write_reg(sl, 0, 3); /* output count */
1480       stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1481
1482     } else if (sl->core_id == STM32VL_CORE_ID) {
1483
1484       size_t count = size / sizeof(uint16_t);
1485       if (size % sizeof(uint16_t)) ++count;
1486
1487       /* setup core */
1488       stlink_write_reg(sl, fl->buf_addr, 0); /* source */
1489       stlink_write_reg(sl, target, 1); /* target */
1490       stlink_write_reg(sl, count, 2); /* count (16 bits half words) */
1491       stlink_write_reg(sl, 0, 3); /* flash bank 0 (input) */
1492       stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1493
1494     } else {
1495       fprintf(stderr, "unknown coreid: 0x%x\n", sl->core_id);
1496       return -1;
1497     }
1498
1499     /* run loader */
1500     stlink_run(sl);
1501
1502     /* wait until done (reaches breakpoint) */
1503     while (is_core_halted(sl) == 0) ;
1504
1505     /* check written byte count */
1506     if (sl->core_id == STM32L_CORE_ID) {
1507
1508       size_t count = size / sizeof(uint32_t);
1509       if (size % sizeof(uint32_t)) ++count;
1510
1511       stlink_read_reg(sl, 3, &rr);
1512       if (rr.r[3] != count) {
1513         fprintf(stderr, "write error, count == %u\n", rr.r[3]);
1514         return -1;
1515       }
1516
1517     } else if (sl->core_id == STM32VL_CORE_ID) {
1518
1519       stlink_read_reg(sl, 2, &rr);
1520       if (rr.r[2] != 0) {
1521         fprintf(stderr, "write error, count == %u\n", rr.r[2]);
1522         return -1;
1523       }
1524
1525     } else {
1526
1527       fprintf(stderr, "unknown coreid: 0x%x\n", sl->core_id);
1528       return -1;
1529
1530     }
1531
1532     return 0;
1533 }