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 /* #define FLASH_REGS_ADDR 0x40022000 */
  34
  35 /* stm32f FPEC flash controller interface, pm0063 manual */
  36
  37 #define FLASH_REGS_ADDR 0x40022000
  38 #define FLASH_REGS_SIZE 0x28
  39
  40 #define FLASH_ACR (FLASH_REGS_ADDR + 0x00)
  41 #define FLASH_KEYR (FLASH_REGS_ADDR + 0x04)
  42 #define FLASH_SR (FLASH_REGS_ADDR + 0x0c)
  43 #define FLASH_CR (FLASH_REGS_ADDR + 0x10)
  44 #define FLASH_AR (FLASH_REGS_ADDR + 0x14)
  45 #define FLASH_OBR (FLASH_REGS_ADDR + 0x1c)
  46 #define FLASH_WRPR (FLASH_REGS_ADDR + 0x20)
  47
  48 #define FLASH_RDPTR_KEY 0x00a5
  49 #define FLASH_KEY1 0x45670123
  50 #define FLASH_KEY2 0xcdef89ab
  51
  52 #define FLASH_SR_BSY 0
  53 #define FLASH_SR_EOP 5
  54
  55 #define FLASH_CR_PG 0
  56 #define FLASH_CR_PER 1
  57 #define FLASH_CR_MER 2
  58 #define FLASH_CR_STRT 6
  59 #define FLASH_CR_LOCK 7
  60
  61 void write_uint32(unsigned char* buf, uint32_t ui) {
  62     if (!is_bigendian()) { // le -> le (don't swap)
  63         buf[0] = ((unsigned char*) &ui)[0];
  64         buf[1] = ((unsigned char*) &ui)[1];
  65         buf[2] = ((unsigned char*) &ui)[2];
  66         buf[3] = ((unsigned char*) &ui)[3];
  67     } else {
  68         buf[0] = ((unsigned char*) &ui)[3];
  69         buf[1] = ((unsigned char*) &ui)[2];
  70         buf[2] = ((unsigned char*) &ui)[1];
  71         buf[3] = ((unsigned char*) &ui)[0];
  72     }
  73 }
  74
  75 void write_uint16(unsigned char* buf, uint16_t ui) {
  76     if (!is_bigendian()) { // le -> le (don't swap)
  77         buf[0] = ((unsigned char*) &ui)[0];
  78         buf[1] = ((unsigned char*) &ui)[1];
  79     } else {
  80         buf[0] = ((unsigned char*) &ui)[1];
  81         buf[1] = ((unsigned char*) &ui)[0];
  82     }
  83 }
  84
  85 uint32_t read_uint32(const unsigned char *c, const int pt) {
  86     uint32_t ui;
  87     char *p = (char *) &ui;
  88
  89     if (!is_bigendian()) { // le -> le (don't swap)
  90         p[0] = c[pt];
  91         p[1] = c[pt + 1];
  92         p[2] = c[pt + 2];
  93         p[3] = c[pt + 3];
  94     } else {
  95         p[0] = c[pt + 3];
  96         p[1] = c[pt + 2];
  97         p[2] = c[pt + 1];
  98         p[3] = c[pt];
  99     }
 100     return ui;
 101 }
 102
 103 static uint32_t __attribute__((unused)) read_flash_rdp(stlink_t *sl) {
 104     stlink_read_mem32(sl, FLASH_WRPR, sizeof (uint32_t));
 105     return (*(uint32_t*) sl->q_buf) & 0xff;
 106 }
 107
 108 static inline uint32_t read_flash_wrpr(stlink_t *sl) {
 109     stlink_read_mem32(sl, FLASH_WRPR, sizeof (uint32_t));
 110     return *(uint32_t*) sl->q_buf;
 111 }
 112
 113 static inline uint32_t read_flash_obr(stlink_t *sl) {
 114     stlink_read_mem32(sl, FLASH_OBR, sizeof (uint32_t));
 115     return *(uint32_t*) sl->q_buf;
 116 }
 117
 118 static inline uint32_t read_flash_cr(stlink_t *sl) {
 119     stlink_read_mem32(sl, FLASH_CR, sizeof (uint32_t));
 120     return *(uint32_t*) sl->q_buf;
 121 }
 122
 123 static inline unsigned int is_flash_locked(stlink_t *sl) {
 124     /* return non zero for true */
 125     return read_flash_cr(sl) & (1 << FLASH_CR_LOCK);
 126 }
 127
 128 static void unlock_flash(stlink_t *sl) {
 129     /* the unlock sequence consists of 2 write cycles where
 130        2 key values are written to the FLASH_KEYR register.
 131        an invalid sequence results in a definitive lock of
 132        the FPEC block until next reset.
 133      */
 134
 135     write_uint32(sl->q_buf, FLASH_KEY1);
 136     stlink_write_mem32(sl, FLASH_KEYR, sizeof (uint32_t));
 137
 138     write_uint32(sl->q_buf, FLASH_KEY2);
 139     stlink_write_mem32(sl, FLASH_KEYR, sizeof (uint32_t));
 140 }
 141
 142 static int unlock_flash_if(stlink_t *sl) {
 143     /* unlock flash if already locked */
 144
 145     if (is_flash_locked(sl)) {
 146         unlock_flash(sl);
 147         if (is_flash_locked(sl))
 148             return -1;
 149     }
 150
 151     return 0;
 152 }
 153
 154 static void lock_flash(stlink_t *sl) {
 155     /* write to 1 only. reset by hw at unlock sequence */
 156
 157     const uint32_t n = read_flash_cr(sl) | (1 << FLASH_CR_LOCK);
 158
 159     write_uint32(sl->q_buf, n);
 160     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 161 }
 162
 163 static void set_flash_cr_pg(stlink_t *sl) {
 164     const uint32_t n = 1 << FLASH_CR_PG;
 165     write_uint32(sl->q_buf, n);
 166     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 167 }
 168
 169 static void __attribute__((unused)) clear_flash_cr_pg(stlink_t *sl) {
 170     const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_PG);
 171     write_uint32(sl->q_buf, n);
 172     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 173 }
 174
 175 static void set_flash_cr_per(stlink_t *sl) {
 176     const uint32_t n = 1 << FLASH_CR_PER;
 177     write_uint32(sl->q_buf, n);
 178     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 179 }
 180
 181 static void __attribute__((unused)) clear_flash_cr_per(stlink_t *sl) {
 182     const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_PER);
 183     write_uint32(sl->q_buf, n);
 184     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 185 }
 186
 187 static void set_flash_cr_mer(stlink_t *sl) {
 188     const uint32_t n = 1 << FLASH_CR_MER;
 189     write_uint32(sl->q_buf, n);
 190     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 191 }
 192
 193 static void __attribute__((unused)) clear_flash_cr_mer(stlink_t *sl) {
 194     const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_MER);
 195     write_uint32(sl->q_buf, n);
 196     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 197 }
 198
 199 static void set_flash_cr_strt(stlink_t *sl) {
 200     /* assume come on the flash_cr_per path */
 201     const uint32_t n = (1 << FLASH_CR_PER) | (1 << FLASH_CR_STRT);
 202     write_uint32(sl->q_buf, n);
 203     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 204 }
 205
 206 static inline uint32_t read_flash_acr(stlink_t *sl) {
 207     stlink_read_mem32(sl, FLASH_ACR, sizeof (uint32_t));
 208     return *(uint32_t*) sl->q_buf;
 209 }
 210
 211 static inline uint32_t read_flash_sr(stlink_t *sl) {
 212     stlink_read_mem32(sl, FLASH_SR, sizeof (uint32_t));
 213     return *(uint32_t*) sl->q_buf;
 214 }
 215
 216 static inline unsigned int is_flash_busy(stlink_t *sl) {
 217     return read_flash_sr(sl) & (1 << FLASH_SR_BSY);
 218 }
 219
 220 static void wait_flash_busy(stlink_t *sl) {
 221     /* todo: add some delays here */
 222     while (is_flash_busy(sl))
 223         ;
 224 }
 225
 226 static inline unsigned int is_flash_eop(stlink_t *sl) {
 227     return read_flash_sr(sl) & (1 << FLASH_SR_EOP);
 228 }
 229
 230 static void __attribute__((unused)) clear_flash_sr_eop(stlink_t *sl) {
 231     const uint32_t n = read_flash_sr(sl) & ~(1 << FLASH_SR_EOP);
 232     write_uint32(sl->q_buf, n);
 233     stlink_write_mem32(sl, FLASH_SR, sizeof (uint32_t));
 234 }
 235
 236 static void __attribute__((unused)) wait_flash_eop(stlink_t *sl) {
 237     /* todo: add some delays here */
 238     while (is_flash_eop(sl) == 0)
 239         ;
 240 }
 241
 242 static inline void write_flash_ar(stlink_t *sl, uint32_t n) {
 243     write_uint32(sl->q_buf, n);
 244     stlink_write_mem32(sl, FLASH_AR, sizeof (uint32_t));
 245 }
 246
 247 #if 0 /* todo */
 248
 249 static void disable_flash_read_protection(stlink_t *sl) {
 250     /* erase the option byte area */
 251     /* rdp = 0x00a5; */
 252     /* reset */
 253 }
 254 #endif /* todo */
 255
 256
 257 // Delegates to the backends...
 258
 259 void stlink_close(stlink_t *sl) {
 260     D(sl, "\n*** stlink_close ***\n");
 261     sl->backend->close(sl);
 262     free(sl);
 263 }
 264
 265 void stlink_exit_debug_mode(stlink_t *sl) {
 266     D(sl, "\n*** stlink_exit_debug_mode ***\n");
 267     sl->backend->exit_debug_mode(sl);
 268 }
 269
 270 void stlink_enter_swd_mode(stlink_t *sl) {
 271     D(sl, "\n*** stlink_enter_swd_mode ***\n");
 272     sl->backend->enter_swd_mode(sl);
 273 }
 274
 275 // Force the core into the debug mode -> halted state.
 276 void stlink_force_debug(stlink_t *sl) {
 277     D(sl, "\n*** stlink_force_debug_mode ***\n");
 278     sl->backend->force_debug(sl);
 279 }
 280
 281 void stlink_exit_dfu_mode(stlink_t *sl) {
 282     D(sl, "\n*** stlink_exit_dfu_mode ***\n");
 283     sl->backend->exit_dfu_mode(sl);
 284 }
 285
 286 uint32_t stlink_core_id(stlink_t *sl) {
 287     D(sl, "\n*** stlink_core_id ***\n");
 288     sl->backend->core_id(sl);
 289     if (sl->verbose > 2)
 290         stlink_print_data(sl);
 291     DD(sl, "core_id = 0x%08x\n", sl->core_id);
 292     return sl->core_id;
 293 }
 294
 295 uint16_t stlink_chip_id(stlink_t *sl) {
 296     stlink_read_mem32(sl, 0xE0042000, 4);
 297     uint32_t chip_id = sl->q_buf[0] | (sl->q_buf[1] << 8) | (sl->q_buf[2] << 16) |
 298             (sl->q_buf[3] << 24);
 299     return chip_id;
 300 }
 301
 302 /**
 303  * Cortex m3 tech ref manual, CPUID register description
 304  * @param sl stlink context
 305  * @param cpuid pointer to the result object
 306  */
 307 void stlink_cpu_id(stlink_t *sl, cortex_m3_cpuid_t *cpuid) {
 308     stlink_read_mem32(sl, CM3_REG_CPUID, 4);
 309     uint32_t raw = read_uint32(sl->q_buf, 0);
 310     cpuid->implementer_id = (raw >> 24) & 0x7f;
 311     cpuid->variant = (raw >> 20) & 0xf;
 312     cpuid->part = (raw >> 4) & 0xfff;
 313     cpuid->revision = raw & 0xf;
 314     return;
 315 }
 316
 317 void stlink_reset(stlink_t *sl) {
 318     D(sl, "\n*** stlink_reset ***\n");
 319     sl->backend->reset(sl);
 320 }
 321
 322 void stlink_run(stlink_t *sl) {
 323     D(sl, "\n*** stlink_run ***\n");
 324     sl->backend->run(sl);
 325 }
 326
 327 void stlink_status(stlink_t *sl) {
 328     D(sl, "\n*** stlink_status ***\n");
 329     sl->backend->status(sl);
 330     stlink_core_stat(sl);
 331 }
 332
 333 /**
 334  * Decode the version bits, originally from -sg, verified with usb
 335  * @param sl stlink context, assumed to contain valid data in the buffer
 336  * @param slv output parsed version object
 337  */
 338 void _parse_version(stlink_t *sl, stlink_version_t *slv) {
 339     uint32_t b0 = sl->q_buf[0]; //lsb
 340     uint32_t b1 = sl->q_buf[1];
 341     uint32_t b2 = sl->q_buf[2];
 342     uint32_t b3 = sl->q_buf[3];
 343     uint32_t b4 = sl->q_buf[4];
 344     uint32_t b5 = sl->q_buf[5]; //msb
 345
 346     // b0 b1                       || b2 b3  | b4 b5
 347     // 4b        | 6b     | 6b     || 2B     | 2B
 348     // stlink_v  | jtag_v | swim_v || st_vid | stlink_pid
 349
 350     slv->stlink_v = (b0 & 0xf0) >> 4;
 351     slv->jtag_v = ((b0 & 0x0f) << 2) | ((b1 & 0xc0) >> 6);
 352     slv->swim_v = b1 & 0x3f;
 353     slv->st_vid = (b3 << 8) | b2;
 354     slv->stlink_pid = (b5 << 8) | b4;
 355     return;
 356 }
 357
 358 void stlink_version(stlink_t *sl) {
 359     D(sl, "*** looking up stlink version\n");
 360     stlink_version_t slv;
 361     sl->backend->version(sl);
 362     _parse_version(sl, &slv);
 363
 364     DD(sl, "st vid         = 0x%04x (expect 0x%04x)\n", slv.st_vid, USB_ST_VID);
 365     DD(sl, "stlink pid     = 0x%04x\n", slv.stlink_pid);
 366     DD(sl, "stlink version = 0x%x\n", slv.stlink_v);
 367     DD(sl, "jtag version   = 0x%x\n", slv.jtag_v);
 368     DD(sl, "swim version   = 0x%x\n", slv.swim_v);
 369     if (slv.jtag_v == 0) {
 370         DD(sl, "    notice: the firmware doesn't support a jtag/swd interface\n");
 371     }
 372     if (slv.swim_v == 0) {
 373         DD(sl, "    notice: the firmware doesn't support a swim interface\n");
 374     }
 375 }
 376
 377 void stlink_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 378     D(sl, "\n*** stlink_write_mem32 ***\n");
 379     if (len % 4 != 0) {
 380         fprintf(stderr, "Error: Data length doesn't have a 32 bit alignment: +%d byte.\n", len % 4);
 381         return;
 382     }
 383     sl->backend->write_mem32(sl, addr, len);
 384 }
 385
 386 void stlink_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 387     D(sl, "\n*** stlink_read_mem32 ***\n");
 388     if (len % 4 != 0) { // !!! never ever: fw gives just wrong values
 389         fprintf(stderr, "Error: Data length doesn't have a 32 bit alignment: +%d byte.\n",
 390                 len % 4);
 391         return;
 392     }
 393     sl->backend->read_mem32(sl, addr, len);
 394 }
 395
 396 void stlink_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
 397     D(sl, "\n*** stlink_write_mem8 ***\n");
 398     sl->backend->write_mem8(sl, addr, len);
 399 }
 400
 401 void stlink_read_all_regs(stlink_t *sl, reg *regp) {
 402     D(sl, "\n*** stlink_read_all_regs ***\n");
 403     sl->backend->read_all_regs(sl, regp);
 404 }
 405
 406 void stlink_write_reg(stlink_t *sl, uint32_t reg, int idx) {
 407     D(sl, "\n*** stlink_write_reg\n");
 408     sl->backend->write_reg(sl, reg, idx);
 409 }
 410
 411 void stlink_read_reg(stlink_t *sl, int r_idx, reg *regp) {
 412     D(sl, "\n*** stlink_read_reg\n");
 413     DD(sl, " (%d) ***\n", r_idx);
 414
 415     if (r_idx > 20 || r_idx < 0) {
 416         fprintf(stderr, "Error: register index must be in [0..20]\n");
 417         return;
 418     }
 419
 420     sl->backend->read_reg(sl, r_idx, regp);
 421 }
 422
 423 unsigned int is_core_halted(stlink_t *sl) {
 424     /* return non zero if core is halted */
 425     stlink_status(sl);
 426     return sl->q_buf[0] == STLINK_CORE_HALTED;
 427 }
 428
 429 void stlink_step(stlink_t *sl) {
 430     D(sl, "\n*** stlink_step ***\n");
 431     sl->backend->step(sl);
 432 }
 433
 434 int stlink_current_mode(stlink_t *sl) {
 435     int mode = sl->backend->current_mode(sl);
 436     switch (mode) {
 437         case STLINK_DEV_DFU_MODE:
 438             DD(sl, "stlink current mode: dfu\n");
 439             return mode;
 440         case STLINK_DEV_DEBUG_MODE:
 441             DD(sl, "stlink current mode: debug (jtag or swd)\n");
 442             return mode;
 443         case STLINK_DEV_MASS_MODE:
 444             DD(sl, "stlink current mode: mass\n");
 445             return mode;
 446     }
 447     DD(sl, "stlink mode: unknown!\n");
 448     return STLINK_DEV_UNKNOWN_MODE;
 449 }
 450
 451
 452
 453
 454 // End of delegates....  Common code below here...
 455
 456 // Endianness
 457 // http://www.ibm.com/developerworks/aix/library/au-endianc/index.html
 458 // const int i = 1;
 459 // #define is_bigendian() ( (*(char*)&i) == 0 )
 460
 461 inline unsigned int is_bigendian(void) {
 462     static volatile const unsigned int i = 1;
 463     return *(volatile const char*) &i == 0;
 464 }
 465
 466 uint16_t read_uint16(const unsigned char *c, const int pt) {
 467     uint32_t ui;
 468     char *p = (char *) &ui;
 469
 470     if (!is_bigendian()) { // le -> le (don't swap)
 471         p[0] = c[pt];
 472         p[1] = c[pt + 1];
 473     } else {
 474         p[0] = c[pt + 1];
 475         p[1] = c[pt];
 476     }
 477     return ui;
 478 }
 479
 480 // same as above with entrypoint.
 481
 482 void stlink_run_at(stlink_t *sl, stm32_addr_t addr) {
 483     stlink_write_reg(sl, addr, 15); /* pc register */
 484
 485     stlink_run(sl);
 486
 487     while (is_core_halted(sl) == 0)
 488         usleep(3000000);
 489 }
 490
 491 void stlink_core_stat(stlink_t *sl) {
 492     if (sl->q_len <= 0)
 493         return;
 494
 495     stlink_print_data(sl);
 496
 497     switch (sl->q_buf[0]) {
 498         case STLINK_CORE_RUNNING:
 499             sl->core_stat = STLINK_CORE_RUNNING;
 500             DD(sl, "  core status: running\n");
 501             return;
 502         case STLINK_CORE_HALTED:
 503             sl->core_stat = STLINK_CORE_HALTED;
 504             DD(sl, "  core status: halted\n");
 505             return;
 506         default:
 507             sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
 508             fprintf(stderr, "  core status: unknown\n");
 509     }
 510 }
 511
 512 void stlink_print_data(stlink_t * sl) {
 513     if (sl->q_len <= 0 || sl->verbose < 2)
 514         return;
 515     if (sl->verbose > 2)
 516         fprintf(stdout, "data_len = %d 0x%x\n", sl->q_len, sl->q_len);
 517
 518     for (int i = 0; i < sl->q_len; i++) {
 519         if (i % 16 == 0) {
 520             /*
 521                                     if (sl->q_data_dir == Q_DATA_OUT)
 522                                             fprintf(stdout, "\n<- 0x%08x ", sl->q_addr + i);
 523                                     else
 524                                             fprintf(stdout, "\n-> 0x%08x ", sl->q_addr + i);
 525              */
 526         }
 527         fprintf(stdout, " %02x", (unsigned int) sl->q_buf[i]);
 528     }
 529     fputs("\n\n", stdout);
 530 }
 531
 532 /* memory mapped file */
 533
 534 typedef struct mapped_file {
 535     uint8_t* base;
 536     size_t len;
 537 } mapped_file_t;
 538
 539 #define MAPPED_FILE_INITIALIZER { NULL, 0 }
 540
 541 static int map_file(mapped_file_t* mf, const char* path) {
 542     int error = -1;
 543     struct stat st;
 544
 545     const int fd = open(path, O_RDONLY);
 546     if (fd == -1) {
 547         fprintf(stderr, "open(%s) == -1\n", path);
 548         return -1;
 549     }
 550
 551     if (fstat(fd, &st) == -1) {
 552         fprintf(stderr, "fstat() == -1\n");
 553         goto on_error;
 554     }
 555
 556     mf->base = (uint8_t*) mmap(NULL, st.st_size, PROT_READ, MAP_SHARED, fd, 0);
 557     if (mf->base == MAP_FAILED) {
 558         fprintf(stderr, "mmap() == MAP_FAILED\n");
 559         goto on_error;
 560     }
 561
 562     mf->len = st.st_size;
 563
 564     /* success */
 565     error = 0;
 566
 567 on_error:
 568     close(fd);
 569
 570     return error;
 571 }
 572
 573 static void unmap_file(mapped_file_t * mf) {
 574     munmap((void*) mf->base, mf->len);
 575     mf->base = (unsigned char*) MAP_FAILED;
 576     mf->len = 0;
 577 }
 578
 579 static int check_file(stlink_t* sl, mapped_file_t* mf, stm32_addr_t addr) {
 580     size_t off;
 581
 582     for (off = 0; off < mf->len; off += sl->flash_pgsz) {
 583         size_t aligned_size;
 584
 585         /* adjust last page size */
 586         size_t cmp_size = sl->flash_pgsz;
 587         if ((off + sl->flash_pgsz) > mf->len)
 588             cmp_size = mf->len - off;
 589
 590         aligned_size = cmp_size;
 591         if (aligned_size & (4 - 1))
 592             aligned_size = (cmp_size + 4) & ~(4 - 1);
 593
 594         stlink_read_mem32(sl, addr + off, aligned_size);
 595
 596         if (memcmp(sl->q_buf, mf->base + off, cmp_size))
 597             return -1;
 598     }
 599
 600     return 0;
 601 }
 602
 603 int stlink_fwrite_sram
 604 (stlink_t * sl, const char* path, stm32_addr_t addr) {
 605     /* write the file in sram at addr */
 606
 607     int error = -1;
 608     size_t off;
 609     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
 610
 611     if (map_file(&mf, path) == -1) {
 612         fprintf(stderr, "map_file() == -1\n");
 613         return -1;
 614     }
 615
 616     /* check addr range is inside the sram */
 617     if (addr < sl->sram_base) {
 618         fprintf(stderr, "addr too low\n");
 619         goto on_error;
 620     } else if ((addr + mf.len) < addr) {
 621         fprintf(stderr, "addr overruns\n");
 622         goto on_error;
 623     } else if ((addr + mf.len) > (sl->sram_base + sl->sram_size)) {
 624         fprintf(stderr, "addr too high\n");
 625         goto on_error;
 626     } else if ((addr & 3) || (mf.len & 3)) {
 627         /* todo */
 628         fprintf(stderr, "unaligned addr or size\n");
 629         goto on_error;
 630     }
 631
 632     /* do the copy by 1k blocks */
 633     for (off = 0; off < mf.len; off += 1024) {
 634         size_t size = 1024;
 635         if ((off + size) > mf.len)
 636             size = mf.len - off;
 637
 638         memcpy(sl->q_buf, mf.base + off, size);
 639
 640         /* round size if needed */
 641         if (size & 3)
 642             size += 2;
 643
 644         stlink_write_mem32(sl, addr + off, size);
 645     }
 646
 647     /* check the file ha been written */
 648     if (check_file(sl, &mf, addr) == -1) {
 649         fprintf(stderr, "check_file() == -1\n");
 650         goto on_error;
 651     }
 652
 653     /* success */
 654     error = 0;
 655
 656 on_error:
 657     unmap_file(&mf);
 658     return error;
 659 }
 660
 661 int stlink_fread(stlink_t* sl, const char* path, stm32_addr_t addr, size_t size) {
 662     /* read size bytes from addr to file */
 663
 664     int error = -1;
 665     size_t off;
 666
 667     const int fd = open(path, O_RDWR | O_TRUNC | O_CREAT, 00700);
 668     if (fd == -1) {
 669         fprintf(stderr, "open(%s) == -1\n", path);
 670         return -1;
 671     }
 672
 673     /* do the copy by 1k blocks */
 674     for (off = 0; off < size; off += 1024) {
 675         size_t read_size = 1024;
 676         if ((off + read_size) > size)
 677             read_size = off + read_size;
 678
 679         /* round size if needed */
 680         if (read_size & 3)
 681             read_size = (read_size + 4) & ~(3);
 682
 683         stlink_read_mem32(sl, addr + off, read_size);
 684
 685         if (write(fd, sl->q_buf, read_size) != (ssize_t) read_size) {
 686             fprintf(stderr, "write() != read_size\n");
 687             goto on_error;
 688         }
 689     }
 690
 691     /* success */
 692     error = 0;
 693
 694 on_error:
 695     close(fd);
 696
 697     return error;
 698 }
 699
 700 int write_buffer_to_sram(stlink_t *sl, flash_loader_t* fl, const uint8_t* buf, size_t size) {
 701     /* write the buffer right after the loader */
 702     memcpy(sl->q_buf, buf, size);
 703     stlink_write_mem8(sl, fl->buf_addr, size);
 704     return 0;
 705 }
 706
 707 int stlink_erase_flash_page(stlink_t *sl, stm32_addr_t page) {
 708     /* page an addr in the page to erase */
 709
 710     /* wait for ongoing op to finish */
 711     wait_flash_busy(sl);
 712
 713     /* unlock if locked */
 714     unlock_flash_if(sl);
 715
 716     /* set the page erase bit */
 717     set_flash_cr_per(sl);
 718
 719     /* select the page to erase */
 720     write_flash_ar(sl, page);
 721
 722     /* start erase operation, reset by hw with bsy bit */
 723     set_flash_cr_strt(sl);
 724
 725     /* wait for completion */
 726     wait_flash_busy(sl);
 727
 728     /* relock the flash */
 729     lock_flash(sl);
 730
 731     /* todo: verify the erased page */
 732
 733     return 0;
 734 }
 735
 736 int stlink_erase_flash_mass(stlink_t *sl) {
 737     /* wait for ongoing op to finish */
 738     wait_flash_busy(sl);
 739
 740     /* unlock if locked */
 741     unlock_flash_if(sl);
 742
 743     /* set the mass erase bit */
 744     set_flash_cr_mer(sl);
 745
 746     /* start erase operation, reset by hw with bsy bit */
 747     set_flash_cr_strt(sl);
 748
 749     /* wait for completion */
 750     wait_flash_busy(sl);
 751
 752     /* relock the flash */
 753     lock_flash(sl);
 754
 755     /* todo: verify the erased memory */
 756
 757     return 0;
 758 }
 759
 760 int init_flash_loader(stlink_t *sl, flash_loader_t* fl) {
 761     size_t size;
 762
 763     /* allocate the loader in sram */
 764     if (write_loader_to_sram(sl, &fl->loader_addr, &size) == -1) {
 765         fprintf(stderr, "write_loader_to_sram() == -1\n");
 766         return -1;
 767     }
 768
 769     /* allocate a one page buffer in sram right after loader */
 770     fl->buf_addr = fl->loader_addr + size;
 771
 772     return 0;
 773 }
 774
 775 int write_loader_to_sram(stlink_t *sl, stm32_addr_t* addr, size_t* size) {
 776     /* from openocd, contrib/loaders/flash/stm32.s */
 777     static const uint8_t loader_code_stm32vl[] = {
 778         0x08, 0x4c, /* ldr      r4, STM32_FLASH_BASE */
 779         0x1c, 0x44, /* add      r4, r3 */
 780         /* write_half_word: */
 781         0x01, 0x23, /* movs     r3, #0x01 */
 782         0x23, 0x61, /* str      r3, [r4, #STM32_FLASH_CR_OFFSET] */
 783         0x30, 0xf8, 0x02, 0x3b, /* ldrh r3, [r0], #0x02 */
 784         0x21, 0xf8, 0x02, 0x3b, /* strh r3, [r1], #0x02 */
 785         /* busy: */
 786         0xe3, 0x68, /* ldr      r3, [r4, #STM32_FLASH_SR_OFFSET] */
 787         0x13, 0xf0, 0x01, 0x0f, /* tst  r3, #0x01 */
 788         0xfb, 0xd0, /* beq      busy */
 789         0x13, 0xf0, 0x14, 0x0f, /* tst  r3, #0x14 */
 790         0x01, 0xd1, /* bne      exit */
 791         0x01, 0x3a, /* subs     r2, r2, #0x01 */
 792         0xf0, 0xd1, /* bne      write_half_word */
 793         /* exit: */
 794         0x00, 0xbe, /* bkpt     #0x00 */
 795         0x00, 0x20, 0x02, 0x40, /* STM32_FLASH_BASE: .word 0x40022000 */
 796     };
 797
 798     static const uint8_t loader_code_stm32l[] = {
 799       /* see openocd.git/contib/loaders/flash/stm32lx.s for src */
 800       0x00, 0x23,
 801       0x04, 0xe0,
 802
 803       0x51, 0xf8, 0x04, 0xcb,
 804       0x40, 0xf8, 0x04, 0xcb,
 805       0x01, 0x33,
 806
 807       0x93, 0x42,
 808       0xf8, 0xd3,
 809       0x00, 0xbe
 810     };
 811
 812     const uint8_t* loader_code;
 813     size_t loader_size;
 814
 815     if (sl->core_id == 0x2ba01477) /* stm32l */
 816     {
 817       loader_code = loader_code_stm32l;
 818       loader_size = sizeof(loader_code_stm32l);
 819     }
 820     else /* stm32vl */
 821     {
 822       loader_code = loader_code_stm32vl;
 823       loader_size = sizeof(loader_code_stm32vl);
 824     }
 825
 826     memcpy(sl->q_buf, loader_code, loader_size);
 827     stlink_write_mem32(sl, sl->sram_base, loader_size);
 828
 829     *addr = sl->sram_base;
 830     *size = loader_size;
 831
 832     /* success */
 833     return 0;
 834 }
 835
 836 int stlink_fcheck_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
 837     /* check the contents of path are at addr */
 838
 839     int res;
 840     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
 841
 842     if (map_file(&mf, path) == -1)
 843         return -1;
 844
 845     res = check_file(sl, &mf, addr);
 846
 847     unmap_file(&mf);
 848
 849     return res;
 850 }
 851
 852 // The stlink_fwrite_flash should not muck with mmapped files inside itself,
 853 // and should use this function instead. (Hell, what's the reason behind mmap
 854 // there?!) But, as it is not actually used anywhere, nobody cares.
 855
 856 #define WRITE_BLOCK_SIZE 0x40
 857
 858 int stlink_write_flash(stlink_t *sl, stm32_addr_t addr, uint8_t* base, unsigned len) {
 859     size_t off;
 860     flash_loader_t fl;
 861
 862     /* check addr range is inside the flash */
 863     if (addr < sl->flash_base) {
 864         fprintf(stderr, "addr too low\n");
 865         return -1;
 866     } else if ((addr + len) < addr) {
 867         fprintf(stderr, "addr overruns\n");
 868         return -1;
 869     } else if ((addr + len) > (sl->flash_base + sl->flash_size)) {
 870         fprintf(stderr, "addr too high\n");
 871         return -1;
 872     } else if ((addr & 1) || (len & 1)) {
 873         fprintf(stderr, "unaligned addr or size\n");
 874         return -1;
 875     }
 876
 877     /* needed for specializing loader */
 878     stlink_core_id(sl);
 879
 880     /* flash loader initialization */
 881     if (init_flash_loader(sl, &fl) == -1) {
 882         fprintf(stderr, "init_flash_loader() == -1\n");
 883         return -1;
 884     }
 885
 886     /* write each page. above WRITE_BLOCK_SIZE fails? */
 887     for (off = 0; off < len; off += WRITE_BLOCK_SIZE) {
 888         /* adjust last write size */
 889         size_t size = WRITE_BLOCK_SIZE;
 890         if ((off + WRITE_BLOCK_SIZE) > len)
 891             size = len - off;
 892
 893         if (run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
 894             fprintf(stderr, "run_flash_loader(0x%zx) == -1\n", addr + off);
 895             return -1;
 896         }
 897     }
 898
 899     for (off = 0; off < len; off += sl->flash_pgsz) {
 900         size_t aligned_size;
 901
 902         /* adjust last page size */
 903         size_t cmp_size = sl->flash_pgsz;
 904         if ((off + sl->flash_pgsz) > len)
 905             cmp_size = len - off;
 906
 907         aligned_size = cmp_size;
 908         if (aligned_size & (4 - 1))
 909             aligned_size = (cmp_size + 4) & ~(4 - 1);
 910
 911         stlink_read_mem32(sl, addr + off, aligned_size);
 912
 913         if (memcmp(sl->q_buf, base + off, cmp_size))
 914             return -1;
 915     }
 916
 917     return 0;
 918 }
 919
 920 int stlink_fwrite_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
 921     /* write the file in flash at addr */
 922
 923     int error = -1;
 924     size_t off;
 925     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
 926     flash_loader_t fl;
 927
 928     if (map_file(&mf, path) == -1) {
 929         fprintf(stderr, "map_file() == -1\n");
 930         return -1;
 931     }
 932
 933     /* check addr range is inside the flash */
 934     if (addr < sl->flash_base) {
 935         fprintf(stderr, "addr too low\n");
 936         goto on_error;
 937     } else if ((addr + mf.len) < addr) {
 938         fprintf(stderr, "addr overruns\n");
 939         goto on_error;
 940     } else if ((addr + mf.len) > (sl->flash_base + sl->flash_size)) {
 941         fprintf(stderr, "addr too high\n");
 942         goto on_error;
 943     } else if ((addr & 1) || (mf.len & 1)) {
 944         /* todo */
 945         fprintf(stderr, "unaligned addr or size\n");
 946         goto on_error;
 947     }
 948
 949     /* needed for specializing loader */
 950     stlink_core_id(sl);
 951
 952     /* erase each page. todo: mass erase faster? */
 953     for (off = 0; off < mf.len; off += sl->flash_pgsz) {
 954         /* addr must be an addr inside the page */
 955         if (stlink_erase_flash_page(sl, addr + off) == -1) {
 956             fprintf(stderr, "erase_flash_page(0x%zx) == -1\n", addr + off);
 957             goto on_error;
 958         }
 959     }
 960
 961     /* flash loader initialization */
 962     if (init_flash_loader(sl, &fl) == -1) {
 963         fprintf(stderr, "init_flash_loader() == -1\n");
 964         goto on_error;
 965     }
 966
 967     /* write each page. above WRITE_BLOCK_SIZE fails? */
 968 #define WRITE_BLOCK_SIZE 0x40
 969     for (off = 0; off < mf.len; off += WRITE_BLOCK_SIZE) {
 970         /* adjust last write size */
 971         size_t size = WRITE_BLOCK_SIZE;
 972         if ((off + WRITE_BLOCK_SIZE) > mf.len)
 973             size = mf.len - off;
 974
 975         if (run_flash_loader(sl, &fl, addr + off, mf.base + off, size) == -1) {
 976             fprintf(stderr, "run_flash_loader(0x%zx) == -1\n", addr + off);
 977             goto on_error;
 978         }
 979     }
 980
 981     /* check the file ha been written */
 982     if (check_file(sl, &mf, addr) == -1) {
 983         fprintf(stderr, "check_file() == -1\n");
 984         goto on_error;
 985     }
 986
 987     /* success */
 988     error = 0;
 989
 990 on_error:
 991     unmap_file(&mf);
 992     return error;
 993 }
 994
 995 int run_flash_loader(stlink_t *sl, flash_loader_t* fl, stm32_addr_t target, const uint8_t* buf, size_t size) {
 996     const size_t count = size / sizeof (uint16_t);
 997
 998     if (write_buffer_to_sram(sl, fl, buf, size) == -1) {
 999         fprintf(stderr, "write_buffer_to_sram() == -1\n");
1000         return -1;
1001     }
1002
1003     /* setup core */
1004     stlink_write_reg(sl, fl->buf_addr, 0); /* source */
1005     stlink_write_reg(sl, target, 1); /* target */
1006     stlink_write_reg(sl, count, 2); /* count (16 bits half words) */
1007     stlink_write_reg(sl, 0, 3); /* flash bank 0 (input) */
1008     stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1009
1010     /* unlock and set programming mode */
1011     unlock_flash_if(sl);
1012     set_flash_cr_pg(sl);
1013
1014     /* run loader */
1015     stlink_run(sl);
1016
1017     while (is_core_halted(sl) == 0)
1018         ;
1019
1020     lock_flash(sl);
1021
1022     /* not all bytes have been written */
1023     reg rr;
1024     stlink_read_reg(sl, 2, &rr);
1025     if (rr.r[2] != 0) {
1026         fprintf(stderr, "write error, count == %u\n", rr.r[2]);
1027         return -1;
1028     }
1029
1030     return 0;
1031 }