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 uint32_t stlink_read_debug32(stlink_t *sl, uint32_t addr) {
 533     uint32_t data = sl->backend->read_debug32(sl, addr);
 534     DLOG("*** stlink_read_debug32 %x is %#x\n", data, addr);
 535     return data;
 536 }
 537
 538 void stlink_write_debug32(stlink_t *sl, uint32_t addr, uint32_t data) {
 539     DLOG("*** stlink_write_debug32 %x to %#x\n", data, addr);
 540     sl->backend->write_debug32(sl, addr, data);
 541 }
 542
 543 void stlink_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 544     DLOG("*** stlink_write_mem32 %u bytes to %#x\n", len, addr);
 545     if (len % 4 != 0) {
 546         fprintf(stderr, "Error: Data length doesn't have a 32 bit alignment: +%d byte.\n", len % 4);
 547         return;
 548     }
 549     sl->backend->write_mem32(sl, addr, len);
 550 }
 551
 552 void stlink_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 553     DLOG("*** stlink_read_mem32 ***\n");
 554     if (len % 4 != 0) { // !!! never ever: fw gives just wrong values
 555         fprintf(stderr, "Error: Data length doesn't have a 32 bit alignment: +%d byte.\n",
 556                 len % 4);
 557         return;
 558     }
 559     sl->backend->read_mem32(sl, addr, len);
 560 }
 561
 562 void stlink_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
 563     DLOG("*** stlink_write_mem8 ***\n");
 564     sl->backend->write_mem8(sl, addr, len);
 565 }
 566
 567 void stlink_read_all_regs(stlink_t *sl, reg *regp) {
 568     DLOG("*** stlink_read_all_regs ***\n");
 569     sl->backend->read_all_regs(sl, regp);
 570 }
 571
 572 void stlink_write_reg(stlink_t *sl, uint32_t reg, int idx) {
 573     DLOG("*** stlink_write_reg\n");
 574     sl->backend->write_reg(sl, reg, idx);
 575 }
 576
 577 void stlink_read_reg(stlink_t *sl, int r_idx, reg *regp) {
 578     DLOG("*** stlink_read_reg\n");
 579     DLOG(" (%d) ***\n", r_idx);
 580
 581     if (r_idx > 20 || r_idx < 0) {
 582         fprintf(stderr, "Error: register index must be in [0..20]\n");
 583         return;
 584     }
 585
 586     sl->backend->read_reg(sl, r_idx, regp);
 587 }
 588
 589 unsigned int is_core_halted(stlink_t *sl) {
 590     /* return non zero if core is halted */
 591     stlink_status(sl);
 592     return sl->q_buf[0] == STLINK_CORE_HALTED;
 593 }
 594
 595 void stlink_step(stlink_t *sl) {
 596     DLOG("*** stlink_step ***\n");
 597     sl->backend->step(sl);
 598 }
 599
 600 int stlink_current_mode(stlink_t *sl) {
 601     int mode = sl->backend->current_mode(sl);
 602     switch (mode) {
 603         case STLINK_DEV_DFU_MODE:
 604             DLOG("stlink current mode: dfu\n");
 605             return mode;
 606         case STLINK_DEV_DEBUG_MODE:
 607             DLOG("stlink current mode: debug (jtag or swd)\n");
 608             return mode;
 609         case STLINK_DEV_MASS_MODE:
 610             DLOG("stlink current mode: mass\n");
 611             return mode;
 612     }
 613     DLOG("stlink mode: unknown!\n");
 614     return STLINK_DEV_UNKNOWN_MODE;
 615 }
 616
 617
 618
 619
 620 // End of delegates....  Common code below here...
 621
 622 // Endianness
 623 // http://www.ibm.com/developerworks/aix/library/au-endianc/index.html
 624 // const int i = 1;
 625 // #define is_bigendian() ( (*(char*)&i) == 0 )
 626
 627 inline unsigned int is_bigendian(void) {
 628     static volatile const unsigned int i = 1;
 629     return *(volatile const char*) &i == 0;
 630 }
 631
 632 uint16_t read_uint16(const unsigned char *c, const int pt) {
 633     uint32_t ui;
 634     char *p = (char *) &ui;
 635
 636     if (!is_bigendian()) { // le -> le (don't swap)
 637         p[0] = c[pt + 0];
 638         p[1] = c[pt + 1];
 639     } else {
 640         p[0] = c[pt + 1];
 641         p[1] = c[pt + 0];
 642     }
 643     return ui;
 644 }
 645
 646 // same as above with entrypoint.
 647
 648 void stlink_run_at(stlink_t *sl, stm32_addr_t addr) {
 649     stlink_write_reg(sl, addr, 15); /* pc register */
 650
 651     stlink_run(sl);
 652
 653     while (is_core_halted(sl) == 0)
 654         usleep(3000000);
 655 }
 656
 657 void stlink_core_stat(stlink_t *sl) {
 658     if (sl->q_len <= 0)
 659         return;
 660
 661     switch (sl->q_buf[0]) {
 662         case STLINK_CORE_RUNNING:
 663             sl->core_stat = STLINK_CORE_RUNNING;
 664             DLOG("  core status: running\n");
 665             return;
 666         case STLINK_CORE_HALTED:
 667             sl->core_stat = STLINK_CORE_HALTED;
 668             DLOG("  core status: halted\n");
 669             return;
 670         default:
 671             sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
 672             fprintf(stderr, "  core status: unknown\n");
 673     }
 674 }
 675
 676 void stlink_print_data(stlink_t * sl) {
 677     if (sl->q_len <= 0 || sl->verbose < UDEBUG)
 678         return;
 679     if (sl->verbose > 2)
 680         fprintf(stdout, "data_len = %d 0x%x\n", sl->q_len, sl->q_len);
 681
 682     for (int i = 0; i < sl->q_len; i++) {
 683         if (i % 16 == 0) {
 684             /*
 685                                     if (sl->q_data_dir == Q_DATA_OUT)
 686                                             fprintf(stdout, "\n<- 0x%08x ", sl->q_addr + i);
 687                                     else
 688                                             fprintf(stdout, "\n-> 0x%08x ", sl->q_addr + i);
 689              */
 690         }
 691         fprintf(stdout, " %02x", (unsigned int) sl->q_buf[i]);
 692     }
 693     fputs("\n\n", stdout);
 694 }
 695
 696 /* memory mapped file */
 697
 698 typedef struct mapped_file {
 699     uint8_t* base;
 700     size_t len;
 701 } mapped_file_t;
 702
 703 #define MAPPED_FILE_INITIALIZER { NULL, 0 }
 704
 705 static int map_file(mapped_file_t* mf, const char* path) {
 706     int error = -1;
 707     struct stat st;
 708
 709     const int fd = open(path, O_RDONLY);
 710     if (fd == -1) {
 711         fprintf(stderr, "open(%s) == -1\n", path);
 712         return -1;
 713     }
 714
 715     if (fstat(fd, &st) == -1) {
 716         fprintf(stderr, "fstat() == -1\n");
 717         goto on_error;
 718     }
 719
 720     mf->base = (uint8_t*) mmap(NULL, st.st_size, PROT_READ, MAP_SHARED, fd, 0);
 721     if (mf->base == MAP_FAILED) {
 722         fprintf(stderr, "mmap() == MAP_FAILED\n");
 723         goto on_error;
 724     }
 725
 726     mf->len = st.st_size;
 727
 728     /* success */
 729     error = 0;
 730
 731 on_error:
 732     close(fd);
 733
 734     return error;
 735 }
 736
 737 static void unmap_file(mapped_file_t * mf) {
 738     munmap((void*) mf->base, mf->len);
 739     mf->base = (unsigned char*) MAP_FAILED;
 740     mf->len = 0;
 741 }
 742
 743 static int check_file(stlink_t* sl, mapped_file_t* mf, stm32_addr_t addr) {
 744     size_t off;
 745
 746     for (off = 0; off < mf->len; off += sl->flash_pgsz) {
 747         size_t aligned_size;
 748
 749         /* adjust last page size */
 750         size_t cmp_size = sl->flash_pgsz;
 751         if ((off + sl->flash_pgsz) > mf->len)
 752             cmp_size = mf->len - off;
 753
 754         aligned_size = cmp_size;
 755         if (aligned_size & (4 - 1))
 756             aligned_size = (cmp_size + 4) & ~(4 - 1);
 757
 758         stlink_read_mem32(sl, addr + off, aligned_size);
 759
 760         if (memcmp(sl->q_buf, mf->base + off, cmp_size))
 761             return -1;
 762     }
 763
 764     return 0;
 765 }
 766
 767 int stlink_fwrite_sram
 768 (stlink_t * sl, const char* path, stm32_addr_t addr) {
 769     /* write the file in sram at addr */
 770
 771     int error = -1;
 772     size_t off;
 773     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
 774
 775     if (map_file(&mf, path) == -1) {
 776         fprintf(stderr, "map_file() == -1\n");
 777         return -1;
 778     }
 779
 780     /* check addr range is inside the sram */
 781     if (addr < sl->sram_base) {
 782         fprintf(stderr, "addr too low\n");
 783         goto on_error;
 784     } else if ((addr + mf.len) < addr) {
 785         fprintf(stderr, "addr overruns\n");
 786         goto on_error;
 787     } else if ((addr + mf.len) > (sl->sram_base + sl->sram_size)) {
 788         fprintf(stderr, "addr too high\n");
 789         goto on_error;
 790     } else if ((addr & 3) || (mf.len & 3)) {
 791         /* todo */
 792         fprintf(stderr, "unaligned addr or size\n");
 793         goto on_error;
 794     }
 795
 796     /* do the copy by 1k blocks */
 797     for (off = 0; off < mf.len; off += 1024) {
 798         size_t size = 1024;
 799         if ((off + size) > mf.len)
 800             size = mf.len - off;
 801
 802         memcpy(sl->q_buf, mf.base + off, size);
 803
 804         /* round size if needed */
 805         if (size & 3)
 806             size += 2;
 807
 808         stlink_write_mem32(sl, addr + off, size);
 809     }
 810
 811     /* check the file ha been written */
 812     if (check_file(sl, &mf, addr) == -1) {
 813         fprintf(stderr, "check_file() == -1\n");
 814         goto on_error;
 815     }
 816
 817     /* success */
 818     error = 0;
 819
 820 on_error:
 821     unmap_file(&mf);
 822     return error;
 823 }
 824
 825 int stlink_fread(stlink_t* sl, const char* path, stm32_addr_t addr, size_t size) {
 826     /* read size bytes from addr to file */
 827
 828     int error = -1;
 829     size_t off;
 830
 831     const int fd = open(path, O_RDWR | O_TRUNC | O_CREAT, 00700);
 832     if (fd == -1) {
 833         fprintf(stderr, "open(%s) == -1\n", path);
 834         return -1;
 835     }
 836
 837     /* do the copy by 1k blocks */
 838     for (off = 0; off < size; off += 1024) {
 839         size_t read_size = 1024;
 840         size_t rounded_size;
 841         if ((off + read_size) > size)
 842           read_size = size - off;
 843
 844         /* round size if needed */
 845         rounded_size = read_size;
 846         if (rounded_size & 3)
 847           rounded_size = (rounded_size + 4) & ~(3);
 848
 849         stlink_read_mem32(sl, addr + off, rounded_size);
 850
 851         if (write(fd, sl->q_buf, read_size) != (ssize_t) read_size) {
 852             fprintf(stderr, "write() != read_size\n");
 853             goto on_error;
 854         }
 855     }
 856
 857     /* success */
 858     error = 0;
 859
 860 on_error:
 861     close(fd);
 862
 863     return error;
 864 }
 865
 866 int write_buffer_to_sram(stlink_t *sl, flash_loader_t* fl, const uint8_t* buf, size_t size) {
 867     /* write the buffer right after the loader */
 868     memcpy(sl->q_buf, buf, size);
 869     stlink_write_mem8(sl, fl->buf_addr, size);
 870     return 0;
 871 }
 872
 873 uint32_t calculate_F4_sectornum(uint32_t flashaddr){
 874     flashaddr &= ~STM32_FLASH_BASE;     //Page now holding the actual flash address
 875     if (flashaddr<0x4000) return (0);
 876     else if(flashaddr<0x8000) return(1);
 877     else if(flashaddr<0xc000) return(2);
 878     else if(flashaddr<0x10000) return(3);
 879     else if(flashaddr<0x20000) return(4);
 880     else return(flashaddr/0x20000)+4;
 881
 882 }
 883
 884 uint32_t stlink_calculate_pagesize(stlink_t *sl, uint32_t flashaddr){
 885         if(sl->chip_id == STM32F4_CHIP_ID) {
 886                 uint32_t sector=calculate_F4_sectornum(flashaddr);
 887                 if (sector<4) sl->flash_pgsz=0x4000;
 888                 else if(sector<5) sl->flash_pgsz=0x10000;
 889                 else sl->flash_pgsz=0x20000;
 890         }
 891         return (sl->flash_pgsz);
 892 }
 893
 894 /**
 895  * Erase a page of flash, assumes sl is fully populated with things like chip/core ids
 896  * @param sl stlink context
 897  * @param flashaddr an address in the flash page to erase
 898  * @return 0 on success -ve on failure
 899  */
 900 int stlink_erase_flash_page(stlink_t *sl, stm32_addr_t flashaddr)
 901 {
 902   ILOG("Erasing flash page at addr: %#x\n", flashaddr);
 903   if (sl->chip_id == STM32F4_CHIP_ID)
 904   {
 905     /* wait for ongoing op to finish */
 906     wait_flash_busy(sl);
 907
 908     /* unlock if locked */
 909     unlock_flash_if(sl);
 910
 911     /* select the page to erase */
 912     // calculate the actual page from the address
 913     uint32_t sector=calculate_F4_sectornum(flashaddr);
 914
 915     fprintf(stderr, "EraseFlash - Sector:0x%x Size:0x%x\n", sector, stlink_calculate_pagesize(sl, flashaddr));
 916     write_flash_cr_snb(sl, sector);
 917
 918     /* start erase operation */
 919     set_flash_cr_strt(sl);
 920
 921     /* wait for completion */
 922     wait_flash_busy(sl);
 923
 924     /* relock the flash */
 925     //todo: fails to program if this is in
 926     lock_flash(sl);
 927 #if DEBUG_FLASH
 928         fprintf(stdout, "Erase Final CR:0x%x\n", read_flash_cr(sl));
 929 #endif
 930   }
 931   else if (sl->core_id == STM32L_CORE_ID)
 932   {
 933
 934     uint32_t val;
 935
 936     /* disable pecr protection */
 937     write_uint32(sl->q_buf, 0x89abcdef);
 938     stlink_write_mem32(sl, STM32L_FLASH_PEKEYR, sizeof(uint32_t));
 939     write_uint32(sl->q_buf, 0x02030405);
 940     stlink_write_mem32(sl, STM32L_FLASH_PEKEYR, sizeof(uint32_t));
 941
 942     /* check pecr.pelock is cleared */
 943     stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
 944     val = read_uint32(sl->q_buf, 0);
 945     if (val & (1 << 0))
 946     {
 947       WLOG("pecr.pelock not clear (%#x)\n", val);
 948       return -1;
 949     }
 950
 951     /* unlock program memory */
 952     write_uint32(sl->q_buf, 0x8c9daebf);
 953     stlink_write_mem32(sl, STM32L_FLASH_PRGKEYR, sizeof(uint32_t));
 954     write_uint32(sl->q_buf, 0x13141516);
 955     stlink_write_mem32(sl, STM32L_FLASH_PRGKEYR, sizeof(uint32_t));
 956
 957     /* check pecr.prglock is cleared */
 958     stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
 959     val = read_uint32(sl->q_buf, 0);
 960     if (val & (1 << 1))
 961     {
 962       WLOG("pecr.prglock not clear (%#x)\n", val);
 963       return -1;
 964     }
 965
 966     /* unused: unlock the option byte block */
 967 #if 0
 968     write_uint32(sl->q_buf, 0xfbead9c8);
 969     stlink_write_mem32(sl, STM32L_FLASH_OPTKEYR, sizeof(uint32_t));
 970     write_uint32(sl->q_buf, 0x24252627);
 971     stlink_write_mem32(sl, STM32L_FLASH_OPTKEYR, sizeof(uint32_t));
 972
 973     /* check pecr.optlock is cleared */
 974     stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
 975     val = read_uint32(sl->q_buf, 0);
 976     if (val & (1 << 2))
 977     {
 978       fprintf(stderr, "pecr.prglock not clear\n");
 979       return -1;
 980     }
 981 #endif
 982
 983     /* set pecr.{erase,prog} */
 984     val |= (1 << 9) | (1 << 3);
 985     write_uint32(sl->q_buf, val);
 986     stlink_write_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
 987
 988 #if 0 /* fix_to_be_confirmed */
 989
 990     /* wait for sr.busy to be cleared
 991        MP: Test shows that busy bit is not set here. Perhaps, PM0062 is
 992        wrong and we do not need to wait here for clearing the busy bit.
 993        TEXANE: ok, if experience says so and it works for you, we comment
 994        it. If someone has a problem, please drop an email.
 995      */
 996     while (1)
 997     {
 998       stlink_read_mem32(sl, STM32L_FLASH_SR, sizeof(uint32_t));
 999       if ((read_uint32(sl->q_buf, 0) & (1 << 0)) == 0) break ;
1000     }
1001
1002 #endif /* fix_to_be_confirmed */
1003
1004     /* write 0 to the first word of the page to be erased */
1005     memset(sl->q_buf, 0, sizeof(uint32_t));
1006     stlink_write_mem32(sl, flashaddr, sizeof(uint32_t));
1007
1008     /* MP: It is better to wait for clearing the busy bit after issuing
1009     page erase command, even though PM0062 recommends to wait before it.
1010     Test shows that a few iterations is performed in the following loop
1011     before busy bit is cleared.*/
1012     while (1)
1013     {
1014       stlink_read_mem32(sl, STM32L_FLASH_SR, sizeof(uint32_t));
1015       if ((read_uint32(sl->q_buf, 0) & (1 << 0)) == 0) break;
1016     }
1017
1018     /* reset lock bits */
1019     stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1020     val = read_uint32(sl->q_buf, 0) | (1 << 0) | (1 << 1) | (1 << 2);
1021     write_uint32(sl->q_buf, val);
1022     stlink_write_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1023   }
1024   else if (sl->core_id == STM32VL_CORE_ID)
1025   {
1026     /* wait for ongoing op to finish */
1027     wait_flash_busy(sl);
1028
1029     /* unlock if locked */
1030     unlock_flash_if(sl);
1031
1032     /* set the page erase bit */
1033     set_flash_cr_per(sl);
1034
1035     /* select the page to erase */
1036     write_flash_ar(sl, flashaddr);
1037
1038     /* start erase operation, reset by hw with bsy bit */
1039     set_flash_cr_strt(sl);
1040
1041     /* wait for completion */
1042     wait_flash_busy(sl);
1043
1044     /* relock the flash */
1045     lock_flash(sl);
1046   }
1047
1048   else {
1049     WLOG("unknown coreid: %x\n", sl->core_id);
1050     return -1;
1051   }
1052
1053   /* todo: verify the erased page */
1054
1055   return 0;
1056 }
1057
1058 int stlink_erase_flash_mass(stlink_t *sl) {
1059     /* wait for ongoing op to finish */
1060     wait_flash_busy(sl);
1061
1062     /* unlock if locked */
1063     unlock_flash_if(sl);
1064
1065     /* set the mass erase bit */
1066     set_flash_cr_mer(sl);
1067
1068     /* start erase operation, reset by hw with bsy bit */
1069     set_flash_cr_strt(sl);
1070
1071     /* wait for completion */
1072     wait_flash_busy(sl);
1073
1074     /* relock the flash */
1075     lock_flash(sl);
1076
1077     /* todo: verify the erased memory */
1078
1079     return 0;
1080 }
1081
1082 int init_flash_loader(stlink_t *sl, flash_loader_t* fl) {
1083     size_t size;
1084
1085     /* allocate the loader in sram */
1086     if (write_loader_to_sram(sl, &fl->loader_addr, &size) == -1) {
1087         WLOG("Failed to write flash loader to sram!\n");
1088         return -1;
1089     }
1090
1091     /* allocate a one page buffer in sram right after loader */
1092     fl->buf_addr = fl->loader_addr + size;
1093     ILOG("Successfully loaded flash loader in sram\n");
1094     return 0;
1095 }
1096
1097 int write_loader_to_sram(stlink_t *sl, stm32_addr_t* addr, size_t* size) {
1098     /* from openocd, contrib/loaders/flash/stm32.s */
1099     static const uint8_t loader_code_stm32vl[] = {
1100         0x08, 0x4c, /* ldr      r4, STM32_FLASH_BASE */
1101         0x1c, 0x44, /* add      r4, r3 */
1102         /* write_half_word: */
1103         0x01, 0x23, /* movs     r3, #0x01 */
1104         0x23, 0x61, /* str      r3, [r4, #STM32_FLASH_CR_OFFSET] */
1105         0x30, 0xf8, 0x02, 0x3b, /* ldrh r3, [r0], #0x02 */
1106         0x21, 0xf8, 0x02, 0x3b, /* strh r3, [r1], #0x02 */
1107         /* busy: */
1108         0xe3, 0x68, /* ldr      r3, [r4, #STM32_FLASH_SR_OFFSET] */
1109         0x13, 0xf0, 0x01, 0x0f, /* tst  r3, #0x01 */
1110         0xfb, 0xd0, /* beq      busy */
1111         0x13, 0xf0, 0x14, 0x0f, /* tst  r3, #0x14 */
1112         0x01, 0xd1, /* bne      exit */
1113         0x01, 0x3a, /* subs     r2, r2, #0x01 */
1114         0xf0, 0xd1, /* bne      write_half_word */
1115         /* exit: */
1116         0x00, 0xbe, /* bkpt     #0x00 */
1117         0x00, 0x20, 0x02, 0x40, /* STM32_FLASH_BASE: .word 0x40022000 */
1118     };
1119
1120     static const uint8_t loader_code_stm32l[] = {
1121
1122       /* openocd.git/contrib/loaders/flash/stm32lx.S
1123          r0, input, dest addr
1124          r1, input, source addr
1125          r2, input, word count
1126          r3, output, word count
1127        */
1128
1129       0x00, 0x23,
1130       0x04, 0xe0,
1131
1132       0x51, 0xf8, 0x04, 0xcb,
1133       0x40, 0xf8, 0x04, 0xcb,
1134       0x01, 0x33,
1135
1136       0x93, 0x42,
1137       0xf8, 0xd3,
1138       0x00, 0xbe
1139     };
1140
1141     const uint8_t* loader_code;
1142     size_t loader_size;
1143
1144     if (sl->core_id == STM32L_CORE_ID) /* stm32l */
1145     {
1146       loader_code = loader_code_stm32l;
1147       loader_size = sizeof(loader_code_stm32l);
1148     }
1149     else if (sl->core_id == STM32VL_CORE_ID)
1150     {
1151       loader_code = loader_code_stm32vl;
1152       loader_size = sizeof(loader_code_stm32vl);
1153     }
1154     else
1155     {
1156       WLOG("unknown coreid, not sure what flash loader to use, aborting!: %x\n", sl->core_id);
1157       return -1;
1158     }
1159
1160     memcpy(sl->q_buf, loader_code, loader_size);
1161     stlink_write_mem32(sl, sl->sram_base, loader_size);
1162
1163     *addr = sl->sram_base;
1164     *size = loader_size;
1165
1166     /* success */
1167     return 0;
1168 }
1169
1170 int stlink_fcheck_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
1171     /* check the contents of path are at addr */
1172
1173     int res;
1174     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1175
1176     if (map_file(&mf, path) == -1)
1177         return -1;
1178
1179     res = check_file(sl, &mf, addr);
1180
1181     unmap_file(&mf);
1182
1183     return res;
1184 }
1185
1186 /**
1187  * Verify addr..addr+len is binary identical to base...base+len
1188  * @param sl stlink context
1189  * @param address stm device address
1190  * @param data host side buffer to check against
1191  * @param length how much
1192  * @return 0 for success, -ve for failure
1193  */
1194 int stlink_verify_write_flash(stlink_t *sl, stm32_addr_t address, uint8_t *data, unsigned length) {
1195     size_t off;
1196     if ((sl->chip_id & 0xFFF) == STM32_CHIPID_F4) {
1197         DLOG("(FIXME)Skipping verification for F4, not enough ram (yet)\n");
1198         return 0;
1199     }
1200     ILOG("Starting verification of write complete\n");
1201     for (off = 0; off < length; off += sl->flash_pgsz) {
1202         size_t aligned_size;
1203
1204         /* adjust last page size */
1205         size_t cmp_size = sl->flash_pgsz;
1206         if ((off + sl->flash_pgsz) > length)
1207             cmp_size = length - off;
1208
1209         aligned_size = cmp_size;
1210         if (aligned_size & (4 - 1))
1211             aligned_size = (cmp_size + 4) & ~(4 - 1);
1212
1213         stlink_read_mem32(sl, address + off, aligned_size);
1214
1215         if (memcmp(sl->q_buf, data + off, cmp_size)) {
1216             WLOG("Verification of flash failed at offset: %zd\n", off);
1217             return -1;
1218         }
1219     }
1220     ILOG("Flash written and verified! jolly good!\n");
1221     return 0;
1222
1223 }
1224
1225 int stlink_write_flash(stlink_t *sl, stm32_addr_t addr, uint8_t* base, unsigned len) {
1226     size_t off;
1227     flash_loader_t fl;
1228     ILOG("Attempting to write %d (%#x) bytes to stm32 address: %u (%#x)\n",
1229         len, len, addr, addr);
1230     /* check addr range is inside the flash */
1231     stlink_calculate_pagesize(sl, addr);
1232     if (addr < sl->flash_base) {
1233         WLOG("addr too low %#x < %#x\n", addr, sl->flash_base);
1234         return -1;
1235     } else if ((addr + len) < addr) {
1236         WLOG("addr overruns\n");
1237         return -1;
1238     } else if ((addr + len) > (sl->flash_base + sl->flash_size)) {
1239         WLOG("addr too high\n");
1240         return -1;
1241     } else if ((addr & 1) || (len & 1)) {
1242         WLOG("unaligned addr or size\n");
1243         return -1;
1244     } else if (addr & (sl->flash_pgsz - 1)) {
1245         WLOG("addr not a multiple of pagesize, not supported\n");
1246         return -1;
1247     }
1248
1249     // Make sure we've loaded the context with the chip details
1250     stlink_core_id(sl);
1251     /* erase each page */
1252     int page_count = 0;
1253     for (off = 0; off < len; off += stlink_calculate_pagesize(sl, addr + off)) {
1254         /* addr must be an addr inside the page */
1255         if (stlink_erase_flash_page(sl, addr + off) == -1) {
1256             WLOG("Failed to erase_flash_page(%#zx) == -1\n", addr + off);
1257             return -1;
1258         }
1259         page_count++;
1260     }
1261     ILOG("Finished erasing %d pages of %d (%#x) bytes\n",
1262         page_count, sl->flash_pgsz, sl->flash_pgsz);
1263
1264     if (sl->chip_id == STM32F4_CHIP_ID) {
1265         /* todo: check write operation */
1266
1267         /* First unlock the cr */
1268         unlock_flash_if(sl);
1269
1270         /* set parallelisim to 32 bit*/
1271         write_flash_cr_psiz(sl, 2);
1272
1273         /* set programming mode */
1274         set_flash_cr_pg(sl);
1275
1276 #define PROGRESS_CHUNK_SIZE 0x1000
1277         /* write a word in program memory */
1278         for (off = 0; off < len; off += sizeof(uint32_t)) {
1279                 if (sl->verbose >= 1) {
1280                         if ((off & (PROGRESS_CHUNK_SIZE - 1)) == 0) {
1281                                 /* show progress. writing procedure is slow
1282                                            and previous errors are misleading */
1283                                 const uint32_t pgnum = (off / PROGRESS_CHUNK_SIZE)+1;
1284                                 const uint32_t pgcount = len / PROGRESS_CHUNK_SIZE;
1285                                 fprintf(stdout, "Writing %ukB chunk %u out of %u\n", PROGRESS_CHUNK_SIZE/1024, pgnum, pgcount);
1286                         }
1287                 }
1288
1289                 memcpy(sl->q_buf, (const void*)(base + off), sizeof(uint32_t));
1290                 stlink_write_mem32(sl, addr + off, sizeof(uint32_t));
1291
1292                 /* wait for sr.busy to be cleared */
1293             wait_flash_busy(sl);
1294
1295         }
1296         /* Relock flash */
1297         lock_flash(sl);
1298
1299 #if 0 /* todo: debug mode */
1300         fprintf(stdout, "Final CR:0x%x\n", read_flash_cr(sl));
1301 #endif
1302
1303
1304
1305     }   //STM32F4END
1306
1307     else if (sl->core_id == STM32L_CORE_ID)    {
1308         /* use fast word write. todo: half page. */
1309         uint32_t val;
1310
1311 #if 0 /* todo: check write operation */
1312
1313         uint32_t nwrites = sl->flash_pgsz;
1314
1315         redo_write:
1316
1317 #endif /* todo: check write operation */
1318
1319         /* disable pecr protection */
1320         write_uint32(sl->q_buf, 0x89abcdef);
1321         stlink_write_mem32(sl, STM32L_FLASH_PEKEYR, sizeof(uint32_t));
1322         write_uint32(sl->q_buf, 0x02030405);
1323         stlink_write_mem32(sl, STM32L_FLASH_PEKEYR, sizeof(uint32_t));
1324
1325         /* check pecr.pelock is cleared */
1326         stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1327         val = read_uint32(sl->q_buf, 0);
1328         if (val & (1 << 0)) {
1329                 fprintf(stderr, "pecr.pelock not clear\n");
1330                 return -1;
1331         }
1332
1333         /* unlock program memory */
1334         write_uint32(sl->q_buf, 0x8c9daebf);
1335         stlink_write_mem32(sl, STM32L_FLASH_PRGKEYR, sizeof(uint32_t));
1336         write_uint32(sl->q_buf, 0x13141516);
1337         stlink_write_mem32(sl, STM32L_FLASH_PRGKEYR, sizeof(uint32_t));
1338
1339         /* check pecr.prglock is cleared */
1340         stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1341         val = read_uint32(sl->q_buf, 0);
1342         if (val & (1 << 1)) {
1343                 fprintf(stderr, "pecr.prglock not clear\n");
1344                 return -1;
1345         }
1346
1347         /* write a word in program memory */
1348         for (off = 0; off < len; off += sizeof(uint32_t)) {
1349                 if (sl->verbose >= 1) {
1350                         if ((off & (sl->flash_pgsz - 1)) == 0) {
1351                                 /* show progress. writing procedure is slow
1352                                    and previous errors are misleading */
1353                                 const uint32_t pgnum = off / sl->flash_pgsz;
1354                                 const uint32_t pgcount = len / sl->flash_pgsz;
1355                                 fprintf(stdout, "%u pages written out of %u\n", pgnum, pgcount);
1356                         }
1357                 }
1358
1359                 memcpy(sl->q_buf, (const void*)(base + off), sizeof(uint32_t));
1360                 stlink_write_mem32(sl, addr + off, sizeof(uint32_t));
1361
1362                 /* wait for sr.busy to be cleared */
1363                 while (1) {
1364                         stlink_read_mem32(sl, STM32L_FLASH_SR, sizeof(uint32_t));
1365                         if ((read_uint32(sl->q_buf, 0) & (1 << 0)) == 0) break ;
1366                 }
1367
1368 #if 0 /* todo: check redo write operation */
1369
1370                 /* check written bytes. todo: should be on a per page basis. */
1371                 stlink_read_mem32(sl, addr + off, sizeof(uint32_t));
1372                 if (memcmp(sl->q_buf, base + off, sizeof(uint32_t))) {
1373                         /* re erase the page and redo the write operation */
1374                         uint32_t page;
1375                         uint32_t val;
1376
1377                         /* fail if successive write count too low */
1378                         if (nwrites < sl->flash_pgsz) {
1379                                 fprintf(stderr, "writes operation failure count too high, aborting\n");
1380                                 return -1;
1381                         }
1382
1383                         nwrites = 0;
1384
1385                         /* assume addr aligned */
1386                         if (off % sl->flash_pgsz) off &= ~(sl->flash_pgsz - 1);
1387                         page = addr + off;
1388
1389                         fprintf(stderr, "invalid write @0x%x(0x%x): 0x%x != 0x%x. retrying.\n",
1390                                         page, addr + off, read_uint32(base + off, 0), read_uint32(sl->q_buf, 0));
1391
1392                         /* reset lock bits */
1393                         stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1394                         val = read_uint32(sl->q_buf, 0) | (1 << 0) | (1 << 1) | (1 << 2);
1395                         write_uint32(sl->q_buf, val);
1396                         stlink_write_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1397
1398                         stlink_erase_flash_page(sl, page);
1399
1400                         goto redo_write;
1401                 }
1402
1403                 /* increment successive writes counter */
1404                 ++nwrites;
1405
1406 #endif /* todo: check redo write operation */
1407         }
1408         /* reset lock bits */
1409         stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1410         val = read_uint32(sl->q_buf, 0) | (1 << 0) | (1 << 1) | (1 << 2);
1411         write_uint32(sl->q_buf, val);
1412         stlink_write_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1413     } else if (sl->core_id == STM32VL_CORE_ID) {
1414         ILOG("Starting Flash write for VL core id\n");
1415         /* flash loader initialization */
1416         if (init_flash_loader(sl, &fl) == -1) {
1417             WLOG("init_flash_loader() == -1\n");
1418             return -1;
1419         }
1420
1421         /* write each page. above WRITE_BLOCK_SIZE fails? */
1422 #define WRITE_BLOCK_SIZE 0x40
1423         int write_block_count = 0;
1424         for (off = 0; off < len; off += WRITE_BLOCK_SIZE) {
1425             ILOG("Writing flash block %d of size %d (%#x)\n", write_block_count,
1426                 WRITE_BLOCK_SIZE, WRITE_BLOCK_SIZE);
1427             /* adjust last write size */
1428             size_t size = WRITE_BLOCK_SIZE;
1429             if ((off + WRITE_BLOCK_SIZE) > len) size = len - off;
1430
1431             /* unlock and set programming mode */
1432             unlock_flash_if(sl);
1433             set_flash_cr_pg(sl);
1434             //DLOG("Finished setting flash cr pg, running loader!\n");
1435             if (run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
1436                 WLOG("run_flash_loader(%#zx) failed! == -1\n", addr + off);
1437                 return -1;
1438             }
1439             lock_flash(sl);
1440             DLOG("Finished writing block %d\n", write_block_count++);
1441         }
1442     } else {
1443         WLOG("unknown coreid, not sure how to write: %x\n", sl->core_id);
1444         return -1;
1445     }
1446
1447     return stlink_verify_write_flash(sl, addr, base, len);
1448 }
1449
1450 /**
1451  * Write the given binary file into flash at address "addr"
1452  * @param sl
1453  * @param path readable file path, should be binary image
1454  * @param addr where to start writing
1455  * @return 0 on success, -ve on failure.
1456  */
1457 int stlink_fwrite_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
1458     /* write the file in flash at addr */
1459     int err;
1460     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1461     if (map_file(&mf, path) == -1) {
1462         WLOG("map_file() == -1\n");
1463         return -1;
1464     }
1465     err = stlink_write_flash(sl, addr, mf.base, mf.len);
1466     unmap_file(&mf);
1467     return err;
1468 }
1469
1470 int run_flash_loader(stlink_t *sl, flash_loader_t* fl, stm32_addr_t target, const uint8_t* buf, size_t size) {
1471
1472     reg rr;
1473     DLOG("Running flash loader, write address:%#x, size: %zd\n", target, size);
1474     // FIXME This can never return -1
1475     if (write_buffer_to_sram(sl, fl, buf, size) == -1) {
1476         // IMPOSSIBLE!
1477         WLOG("write_buffer_to_sram() == -1\n");
1478         return -1;
1479     }
1480
1481     if (sl->core_id == STM32L_CORE_ID) {
1482
1483       size_t count = size / sizeof(uint32_t);
1484       if (size % sizeof(uint32_t)) ++count;
1485
1486       /* setup core */
1487       stlink_write_reg(sl, target, 0); /* target */
1488       stlink_write_reg(sl, fl->buf_addr, 1); /* source */
1489       stlink_write_reg(sl, count, 2); /* count (32 bits words) */
1490       stlink_write_reg(sl, 0, 3); /* output count */
1491       stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1492
1493     } else if (sl->core_id == STM32VL_CORE_ID) {
1494
1495       size_t count = size / sizeof(uint16_t);
1496       if (size % sizeof(uint16_t)) ++count;
1497
1498       /* setup core */
1499       stlink_write_reg(sl, fl->buf_addr, 0); /* source */
1500       stlink_write_reg(sl, target, 1); /* target */
1501       stlink_write_reg(sl, count, 2); /* count (16 bits half words) */
1502       stlink_write_reg(sl, 0, 3); /* flash bank 0 (input) */
1503       stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1504
1505     } else {
1506       fprintf(stderr, "unknown coreid: 0x%x\n", sl->core_id);
1507       return -1;
1508     }
1509
1510     /* run loader */
1511     stlink_run(sl);
1512
1513     /* wait until done (reaches breakpoint) */
1514     while (is_core_halted(sl) == 0) ;
1515
1516     /* check written byte count */
1517     if (sl->core_id == STM32L_CORE_ID) {
1518
1519       size_t count = size / sizeof(uint32_t);
1520       if (size % sizeof(uint32_t)) ++count;
1521
1522       stlink_read_reg(sl, 3, &rr);
1523       if (rr.r[3] != count) {
1524         fprintf(stderr, "write error, count == %u\n", rr.r[3]);
1525         return -1;
1526       }
1527
1528     } else if (sl->core_id == STM32VL_CORE_ID) {
1529
1530       stlink_read_reg(sl, 2, &rr);
1531       if (rr.r[2] != 0) {
1532         fprintf(stderr, "write error, count == %u\n", rr.r[2]);
1533         return -1;
1534       }
1535
1536     } else {
1537
1538       fprintf(stderr, "unknown coreid: 0x%x\n", sl->core_id);
1539       return -1;
1540
1541     }
1542
1543     return 0;
1544 }