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