git.gag.com Git - fw/stlink/blob - src/stlink-common.c

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