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