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     fprintf(stdout, "WriteFlash - addr:%x len:%x\n", addr, len);
1108     fprintf(stdout, "CoreID:%X ChipID:%X\n", sl->core_id, sl->chip_id);
1109
1110     /* check addr range is inside the flash */
1111     if (addr < sl->flash_base) {
1112         fprintf(stderr, "addr too low\n");
1113         return -1;
1114     } else if ((addr + len) < addr) {
1115         fprintf(stderr, "addr overruns\n");
1116         return -1;
1117     } else if ((addr + len) > (sl->flash_base + sl->flash_size)) {
1118         fprintf(stderr, "addr too high\n");
1119         return -1;
1120     } else if ((addr & 1) || (len & 1)) {
1121         fprintf(stderr, "unaligned addr or size\n");
1122         return -1;
1123     } else if (addr & (sl->flash_pgsz - 1)) {
1124         fprintf(stderr, "addr not a multiple of pagesize, not supported\n");
1125         return -1;
1126     }
1127
1128     /* erase each page */
1129     for (off = 0; off < len; off += calculate_sectorsize(sl, addr + off) ) {
1130         /* addr must be an addr inside the page */
1131         if (stlink_erase_flash_page(sl, addr + off) == -1) {
1132            fprintf(stderr, "erase_flash_page(0x%zx) == -1\n", addr + off);
1133             return -1;
1134         }
1135     }
1136
1137     if (sl->chip_id == STM32F4_CHIP_ID) {
1138         /* todo: check write operation */
1139
1140         /* First unlock the cr */
1141         unlock_flash_if(sl);
1142
1143         /* set parallelisim to 32 bit*/
1144         write_flash_cr_psiz(sl, 2);
1145
1146         /* set programming mode */
1147         set_flash_cr_pg(sl);
1148
1149 #define PROGRESS_CHUNK_SIZE 0x1000
1150         /* write a word in program memory */
1151         for (off = 0; off < len; off += sizeof(uint32_t)) {
1152                 if (sl->verbose >= 1) {
1153                         if ((off & (PROGRESS_CHUNK_SIZE - 1)) == 0) {
1154                                 /* show progress. writing procedure is slow
1155                                            and previous errors are misleading */
1156                                 const uint32_t pgnum = (off / PROGRESS_CHUNK_SIZE)+1;
1157                                 const uint32_t pgcount = len / PROGRESS_CHUNK_SIZE;
1158                                 fprintf(stdout, "Writing %ukB chunk %u out of %u\n", PROGRESS_CHUNK_SIZE/1024, pgnum, pgcount);
1159                         }
1160                 }
1161
1162                 memcpy(sl->q_buf, (const void*)(base + off), sizeof(uint32_t));
1163                 stlink_write_mem32(sl, addr + off, sizeof(uint32_t));
1164
1165                 /* wait for sr.busy to be cleared */
1166             wait_flash_busy(sl);
1167
1168         }
1169         /* Relock flash */
1170         lock_flash(sl);
1171                 fprintf(stdout, "Final CR:%X\n", read_flash_cr(sl));
1172
1173
1174
1175     }   //STM32F4END
1176
1177     else if (sl->core_id == STM32L_CORE_ID)    {
1178         /* use fast word write. todo: half page. */
1179
1180         uint32_t val;
1181
1182 #if 0 /* todo: check write operation */
1183
1184         uint32_t nwrites = sl->flash_pgsz;
1185
1186         redo_write:
1187
1188 #endif /* todo: check write operation */
1189
1190         /* disable pecr protection */
1191         write_uint32(sl->q_buf, 0x89abcdef);
1192         stlink_write_mem32(sl, STM32L_FLASH_PEKEYR, sizeof(uint32_t));
1193         write_uint32(sl->q_buf, 0x02030405);
1194         stlink_write_mem32(sl, STM32L_FLASH_PEKEYR, sizeof(uint32_t));
1195
1196         /* check pecr.pelock is cleared */
1197         stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1198         val = read_uint32(sl->q_buf, 0);
1199         if (val & (1 << 0)) {
1200                 fprintf(stderr, "pecr.pelock not clear\n");
1201                 return -1;
1202         }
1203
1204         /* unlock program memory */
1205         write_uint32(sl->q_buf, 0x8c9daebf);
1206         stlink_write_mem32(sl, STM32L_FLASH_PRGKEYR, sizeof(uint32_t));
1207         write_uint32(sl->q_buf, 0x13141516);
1208         stlink_write_mem32(sl, STM32L_FLASH_PRGKEYR, sizeof(uint32_t));
1209
1210         /* check pecr.prglock is cleared */
1211         stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1212         val = read_uint32(sl->q_buf, 0);
1213         if (val & (1 << 1)) {
1214                 fprintf(stderr, "pecr.prglock not clear\n");
1215                 return -1;
1216         }
1217
1218         /* write a word in program memory */
1219         for (off = 0; off < len; off += sizeof(uint32_t)) {
1220                 if (sl->verbose >= 1) {
1221                         if ((off & (sl->flash_pgsz - 1)) == 0) {
1222                                 /* show progress. writing procedure is slow
1223                                    and previous errors are misleading */
1224                                 const uint32_t pgnum = off / sl->flash_pgsz;
1225                                 const uint32_t pgcount = len / sl->flash_pgsz;
1226                                 fprintf(stdout, "%u pages written out of %u\n", pgnum, pgcount);
1227                         }
1228                 }
1229
1230                 memcpy(sl->q_buf, (const void*)(base + off), sizeof(uint32_t));
1231                 stlink_write_mem32(sl, addr + off, sizeof(uint32_t));
1232
1233                 /* wait for sr.busy to be cleared */
1234                 while (1) {
1235                         stlink_read_mem32(sl, STM32L_FLASH_SR, sizeof(uint32_t));
1236                         if ((read_uint32(sl->q_buf, 0) & (1 << 0)) == 0) break ;
1237                 }
1238
1239 #if 0 /* todo: check redo write operation */
1240
1241                 /* check written bytes. todo: should be on a per page basis. */
1242                 stlink_read_mem32(sl, addr + off, sizeof(uint32_t));
1243                 if (memcmp(sl->q_buf, base + off, sizeof(uint32_t))) {
1244                         /* re erase the page and redo the write operation */
1245                         uint32_t page;
1246                         uint32_t val;
1247
1248                         /* fail if successive write count too low */
1249                         if (nwrites < sl->flash_pgsz) {
1250                                 fprintf(stderr, "writes operation failure count too high, aborting\n");
1251                                 return -1;
1252                         }
1253
1254                         nwrites = 0;
1255
1256                         /* assume addr aligned */
1257                         if (off % sl->flash_pgsz) off &= ~(sl->flash_pgsz - 1);
1258                         page = addr + off;
1259
1260                         fprintf(stderr, "invalid write @%x(%x): %x != %x. retrying.\n",
1261                                         page, addr + off, read_uint32(base + off, 0), read_uint32(sl->q_buf, 0));
1262
1263                         /* reset lock bits */
1264                         stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1265                         val = read_uint32(sl->q_buf, 0) | (1 << 0) | (1 << 1) | (1 << 2);
1266                         write_uint32(sl->q_buf, val);
1267                         stlink_write_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1268
1269                         stlink_erase_flash_page(sl, page);
1270
1271                         goto redo_write;
1272                 }
1273
1274                 /* increment successive writes counter */
1275                 ++nwrites;
1276
1277 #endif /* todo: check redo write operation */
1278         }
1279         /* reset lock bits */
1280         stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1281         val = read_uint32(sl->q_buf, 0) | (1 << 0) | (1 << 1) | (1 << 2);
1282         write_uint32(sl->q_buf, val);
1283         stlink_write_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1284         }
1285
1286
1287
1288
1289     else if (sl->core_id == STM32VL_CORE_ID) {
1290         /* flash loader initialization */
1291         if (init_flash_loader(sl, &fl) == -1) {
1292                 fprintf(stderr, "init_flash_loader() == -1\n");
1293                 return -1;
1294         }
1295         /* write each page. above WRITE_BLOCK_SIZE fails? */
1296 #define WRITE_BLOCK_SIZE 0x40
1297       for (off = 0; off < len; off += WRITE_BLOCK_SIZE)      {
1298           /* adjust last write size */
1299           size_t size = WRITE_BLOCK_SIZE;
1300           if ((off + WRITE_BLOCK_SIZE) > len) size = len - off;
1301
1302           /* unlock and set programming mode */
1303           unlock_flash_if(sl);
1304           set_flash_cr_pg(sl);
1305
1306           if (run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
1307                   fprintf(stderr, "run_flash_loader(0x%zx) == -1\n", addr + off);
1308                   return -1;
1309           }
1310           lock_flash(sl);
1311       }
1312     }
1313
1314
1315
1316
1317     else
1318     {
1319       fprintf(stderr, "unknown device!\n");
1320       return -1;
1321     }
1322
1323
1324
1325
1326
1327 #if(0)
1328     //todo: F4 Can't stlink_read_mem32 an entire sector, not enough ram!
1329     for (off = 0; off < len; off += sl->flash_pgsz) {
1330         size_t aligned_size;
1331
1332         /* adjust last page size */
1333         size_t cmp_size = sl->flash_pgsz;
1334         if ((off + sl->flash_pgsz) > len)
1335             cmp_size = len - off;
1336
1337         aligned_size = cmp_size;
1338         if (aligned_size & (4 - 1))
1339             aligned_size = (cmp_size + 4) & ~(4 - 1);
1340
1341                 fprintf(stdout, "AlignedSize:%x\n", aligned_size);
1342         stlink_read_mem32(sl, addr + off, aligned_size);
1343
1344         if (memcmp(sl->q_buf, base + off, cmp_size))
1345             return -1;
1346     }
1347 #endif
1348
1349     return 0;
1350 }
1351
1352
1353
1354
1355 int stlink_fwrite_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
1356     /* write the file in flash at addr */
1357
1358     int err;
1359     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1360
1361     if (map_file(&mf, path) == -1) {
1362         fprintf(stderr, "map_file() == -1\n");
1363         return -1;
1364     }
1365
1366     err = stlink_write_flash(sl, addr, mf.base, mf.len);
1367
1368     unmap_file(&mf);
1369
1370     return err;
1371 }
1372
1373 int run_flash_loader(stlink_t *sl, flash_loader_t* fl, stm32_addr_t target, const uint8_t* buf, size_t size) {
1374
1375     reg rr;
1376
1377     if (write_buffer_to_sram(sl, fl, buf, size) == -1) {
1378         fprintf(stderr, "write_buffer_to_sram() == -1\n");
1379         return -1;
1380     }
1381
1382     if (sl->core_id == STM32L_CORE_ID) {
1383
1384       size_t count = size / sizeof(uint32_t);
1385       if (size % sizeof(uint32_t)) ++count;
1386
1387       /* setup core */
1388       stlink_write_reg(sl, target, 0); /* target */
1389       stlink_write_reg(sl, fl->buf_addr, 1); /* source */
1390       stlink_write_reg(sl, count, 2); /* count (32 bits words) */
1391       stlink_write_reg(sl, 0, 3); /* output count */
1392       stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1393
1394     } else if (sl->core_id == STM32VL_CORE_ID) {
1395
1396       size_t count = size / sizeof(uint16_t);
1397       if (size % sizeof(uint16_t)) ++count;
1398
1399       /* setup core */
1400       stlink_write_reg(sl, fl->buf_addr, 0); /* source */
1401       stlink_write_reg(sl, target, 1); /* target */
1402       stlink_write_reg(sl, count, 2); /* count (16 bits half words) */
1403       stlink_write_reg(sl, 0, 3); /* flash bank 0 (input) */
1404       stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1405
1406     } else {
1407       fprintf(stderr, "unknown coreid: %x\n", sl->core_id);
1408       return -1;
1409     }
1410
1411     /* run loader */
1412     stlink_step(sl);
1413
1414     /* wait until done (reaches breakpoint) */
1415     while (is_core_halted(sl) == 0) ;
1416
1417     /* check written byte count */
1418     if (sl->core_id == STM32L_CORE_ID) {
1419
1420       size_t count = size / sizeof(uint32_t);
1421       if (size % sizeof(uint32_t)) ++count;
1422
1423       stlink_read_reg(sl, 3, &rr);
1424       if (rr.r[3] != count) {
1425         fprintf(stderr, "write error, count == %u\n", rr.r[3]);
1426         return -1;
1427       }
1428
1429     } else if (sl->core_id == STM32VL_CORE_ID) {
1430
1431       stlink_read_reg(sl, 2, &rr);
1432       if (rr.r[2] != 0) {
1433         fprintf(stderr, "write error, count == %u\n", rr.r[2]);
1434         return -1;
1435       }
1436
1437     } else {
1438
1439       fprintf(stderr, "unknown coreid: %x\n", sl->core_id);
1440       return -1;
1441
1442     }
1443
1444     return 0;
1445 }