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