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

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