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 "mmap.h"
  13
  14 #include "stlink-common.h"
  15 #include "uglylogging.h"
  16
  17 #define LOG_TAG __FILE__
  18 #define DLOG(format, args...)         ugly_log(UDEBUG, LOG_TAG, format, ## args)
  19 #define ILOG(format, args...)         ugly_log(UINFO, LOG_TAG, format, ## args)
  20 #define WLOG(format, args...)         ugly_log(UWARN, LOG_TAG, format, ## args)
  21 #define ELOG(format, args...)         ugly_log(UERROR, LOG_TAG, format, ## args)
  22 #define fatal(format, args...)        ugly_log(UFATAL, LOG_TAG, format, ## args)
  23
  24 /* todo: stm32l15xxx flash memory, pm0062 manual */
  25
  26 /* stm32f FPEC flash controller interface, pm0063 manual */
  27 // TODO - all of this needs to be abstracted out....
  28 #define FLASH_REGS_ADDR 0x40022000
  29 #define FLASH_REGS_SIZE 0x28
  30
  31 #define FLASH_ACR (FLASH_REGS_ADDR + 0x00)
  32 #define FLASH_KEYR (FLASH_REGS_ADDR + 0x04)
  33 #define FLASH_SR (FLASH_REGS_ADDR + 0x0c)
  34 #define FLASH_CR (FLASH_REGS_ADDR + 0x10)
  35 #define FLASH_AR (FLASH_REGS_ADDR + 0x14)
  36 #define FLASH_OBR (FLASH_REGS_ADDR + 0x1c)
  37 #define FLASH_WRPR (FLASH_REGS_ADDR + 0x20)
  38
  39 #define FLASH_RDPTR_KEY 0x00a5
  40 #define FLASH_KEY1 0x45670123
  41 #define FLASH_KEY2 0xcdef89ab
  42
  43 #define FLASH_SR_BSY 0
  44 #define FLASH_SR_EOP 5
  45
  46 #define FLASH_CR_PG 0
  47 #define FLASH_CR_PER 1
  48 #define FLASH_CR_MER 2
  49 #define FLASH_CR_STRT 6
  50 #define FLASH_CR_LOCK 7
  51
  52
  53 //32L = 32F1 same CoreID as 32F4!
  54 #define STM32L_FLASH_REGS_ADDR ((uint32_t)0x40023c00)
  55 #define STM32L_FLASH_ACR (STM32L_FLASH_REGS_ADDR + 0x00)
  56 #define STM32L_FLASH_PECR (STM32L_FLASH_REGS_ADDR + 0x04)
  57 #define STM32L_FLASH_PDKEYR (STM32L_FLASH_REGS_ADDR + 0x08)
  58 #define STM32L_FLASH_PEKEYR (STM32L_FLASH_REGS_ADDR + 0x0c)
  59 #define STM32L_FLASH_PRGKEYR (STM32L_FLASH_REGS_ADDR + 0x10)
  60 #define STM32L_FLASH_OPTKEYR (STM32L_FLASH_REGS_ADDR + 0x14)
  61 #define STM32L_FLASH_SR (STM32L_FLASH_REGS_ADDR + 0x18)
  62 #define STM32L_FLASH_OBR (STM32L_FLASH_REGS_ADDR + 0x1c)
  63 #define STM32L_FLASH_WRPR (STM32L_FLASH_REGS_ADDR + 0x20)
  64 #define FLASH_L1_FPRG 10
  65 #define FLASH_L1_PROG 3
  66
  67
  68 //STM32F4
  69 #define FLASH_F4_REGS_ADDR ((uint32_t)0x40023c00)
  70 #define FLASH_F4_KEYR (FLASH_F4_REGS_ADDR + 0x04)
  71 #define FLASH_F4_OPT_KEYR (FLASH_F4_REGS_ADDR + 0x08)
  72 #define FLASH_F4_SR (FLASH_F4_REGS_ADDR + 0x0c)
  73 #define FLASH_F4_CR (FLASH_F4_REGS_ADDR + 0x10)
  74 #define FLASH_F4_OPT_CR (FLASH_F4_REGS_ADDR + 0x14)
  75 #define FLASH_F4_CR_STRT 16
  76 #define FLASH_F4_CR_LOCK 31
  77 #define FLASH_F4_CR_SER 1
  78 #define FLASH_F4_CR_SNB 3
  79 #define FLASH_F4_CR_SNB_MASK 0x38
  80 #define FLASH_F4_SR_BSY 16
  81
  82
  83 void write_uint32(unsigned char* buf, uint32_t ui) {
  84     if (!is_bigendian()) { // le -> le (don't swap)
  85         buf[0] = ((unsigned char*) &ui)[0];
  86         buf[1] = ((unsigned char*) &ui)[1];
  87         buf[2] = ((unsigned char*) &ui)[2];
  88         buf[3] = ((unsigned char*) &ui)[3];
  89     } else {
  90         buf[0] = ((unsigned char*) &ui)[3];
  91         buf[1] = ((unsigned char*) &ui)[2];
  92         buf[2] = ((unsigned char*) &ui)[1];
  93         buf[3] = ((unsigned char*) &ui)[0];
  94     }
  95 }
  96
  97 void write_uint16(unsigned char* buf, uint16_t ui) {
  98     if (!is_bigendian()) { // le -> le (don't swap)
  99         buf[0] = ((unsigned char*) &ui)[0];
 100         buf[1] = ((unsigned char*) &ui)[1];
 101     } else {
 102         buf[0] = ((unsigned char*) &ui)[1];
 103         buf[1] = ((unsigned char*) &ui)[0];
 104     }
 105 }
 106
 107 uint32_t read_uint32(const unsigned char *c, const int pt) {
 108     uint32_t ui;
 109     char *p = (char *) &ui;
 110
 111     if (!is_bigendian()) { // le -> le (don't swap)
 112         p[0] = c[pt + 0];
 113         p[1] = c[pt + 1];
 114         p[2] = c[pt + 2];
 115         p[3] = c[pt + 3];
 116     } else {
 117         p[0] = c[pt + 3];
 118         p[1] = c[pt + 2];
 119         p[2] = c[pt + 1];
 120         p[3] = c[pt + 0];
 121     }
 122     return ui;
 123 }
 124
 125 static uint32_t __attribute__((unused)) read_flash_rdp(stlink_t *sl) {
 126     return stlink_read_debug32(sl, FLASH_WRPR) & 0xff;
 127 }
 128
 129 static inline uint32_t read_flash_wrpr(stlink_t *sl) {
 130     return stlink_read_debug32(sl, FLASH_WRPR);
 131 }
 132
 133 static inline uint32_t read_flash_obr(stlink_t *sl) {
 134     return stlink_read_debug32(sl, FLASH_OBR);
 135 }
 136
 137 static inline uint32_t read_flash_cr(stlink_t *sl) {
 138     uint32_t res;
 139     if ((sl->chip_id == STM32_CHIPID_F2) || (sl->chip_id == STM32_CHIPID_F4))
 140         res = stlink_read_debug32(sl, FLASH_F4_CR);
 141     else
 142         res = stlink_read_debug32(sl, FLASH_CR);
 143 #if DEBUG_FLASH
 144     fprintf(stdout, "CR:0x%x\n", res);
 145 #endif
 146     return res;
 147 }
 148
 149 static inline unsigned int is_flash_locked(stlink_t *sl) {
 150     /* return non zero for true */
 151     if ((sl->chip_id == STM32_CHIPID_F2) || (sl->chip_id == STM32_CHIPID_F4))
 152         return read_flash_cr(sl) & (1 << FLASH_F4_CR_LOCK);
 153     else
 154         return read_flash_cr(sl) & (1 << FLASH_CR_LOCK);
 155 }
 156
 157 static void unlock_flash(stlink_t *sl) {
 158     /* the unlock sequence consists of 2 write cycles where
 159        2 key values are written to the FLASH_KEYR register.
 160        an invalid sequence results in a definitive lock of
 161        the FPEC block until next reset.
 162      */
 163     if ((sl->chip_id == STM32_CHIPID_F2) || (sl->chip_id == STM32_CHIPID_F4)) {
 164         stlink_write_debug32(sl, FLASH_F4_KEYR, FLASH_KEY1);
 165         stlink_write_debug32(sl, FLASH_F4_KEYR, FLASH_KEY2);
 166     } else {
 167         stlink_write_debug32(sl, FLASH_KEYR, FLASH_KEY1);
 168         stlink_write_debug32(sl, FLASH_KEYR, FLASH_KEY2);
 169     }
 170
 171 }
 172
 173 static int unlock_flash_if(stlink_t *sl) {
 174     /* unlock flash if already locked */
 175
 176     if (is_flash_locked(sl)) {
 177         unlock_flash(sl);
 178         if (is_flash_locked(sl)) {
 179             WLOG("Failed to unlock flash!\n");
 180             return -1;
 181         }
 182     }
 183     DLOG("Successfully unlocked flash\n");
 184     return 0;
 185 }
 186
 187 static void lock_flash(stlink_t *sl) {
 188     if ((sl->chip_id == STM32_CHIPID_F2) || (sl->chip_id == STM32_CHIPID_F4)) {
 189         const uint32_t n = read_flash_cr(sl) | (1 << FLASH_F4_CR_LOCK);
 190         stlink_write_debug32(sl, FLASH_F4_CR, n);
 191     } else {
 192         /* write to 1 only. reset by hw at unlock sequence */
 193         const uint32_t n = read_flash_cr(sl) | (1 << FLASH_CR_LOCK);
 194         stlink_write_debug32(sl, FLASH_CR, n);
 195     }
 196 }
 197
 198
 199 static void set_flash_cr_pg(stlink_t *sl) {
 200     if ((sl->chip_id == STM32_CHIPID_F2) || (sl->chip_id == STM32_CHIPID_F4)) {
 201                 uint32_t x = read_flash_cr(sl);
 202                 x |= (1 << FLASH_CR_PG);
 203         stlink_write_debug32(sl, FLASH_F4_CR, x);
 204     } else {
 205         const uint32_t n = 1 << FLASH_CR_PG;
 206         stlink_write_debug32(sl, FLASH_CR, n);
 207     }
 208 }
 209
 210 static void __attribute__((unused)) clear_flash_cr_pg(stlink_t *sl) {
 211     const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_PG);
 212     if ((sl->chip_id == STM32_CHIPID_F2) || (sl->chip_id == STM32_CHIPID_F4))
 213         stlink_write_debug32(sl, FLASH_F4_CR, n);
 214     else
 215         stlink_write_debug32(sl, FLASH_CR, n);
 216 }
 217
 218 static void set_flash_cr_per(stlink_t *sl) {
 219     const uint32_t n = 1 << FLASH_CR_PER;
 220     stlink_write_debug32(sl, FLASH_CR, n);
 221 }
 222
 223 static void __attribute__((unused)) clear_flash_cr_per(stlink_t *sl) {
 224     const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_PER);
 225     stlink_write_debug32(sl, FLASH_CR, n);
 226 }
 227
 228 static void set_flash_cr_mer(stlink_t *sl) {
 229     if ((sl->chip_id == STM32_CHIPID_F2) || (sl->chip_id == STM32_CHIPID_F4))
 230         stlink_write_debug32(sl, FLASH_F4_CR,
 231                              stlink_read_debug32(sl, FLASH_F4_CR) | (1 << FLASH_CR_MER));
 232     else
 233         stlink_write_debug32(sl, FLASH_CR,
 234                              stlink_read_debug32(sl, FLASH_CR) | (1 << FLASH_CR_MER));
 235 }
 236
 237 static void __attribute__((unused)) clear_flash_cr_mer(stlink_t *sl) {
 238     if ((sl->chip_id == STM32_CHIPID_F2) || (sl->chip_id == STM32_CHIPID_F4))
 239         stlink_write_debug32(sl, FLASH_F4_CR,
 240                              stlink_read_debug32(sl, FLASH_F4_CR) & ~(1 << FLASH_CR_MER));
 241     else
 242         stlink_write_debug32(sl, FLASH_CR,
 243                              stlink_read_debug32(sl, FLASH_CR) & ~(1 << FLASH_CR_MER));
 244 }
 245
 246 static void set_flash_cr_strt(stlink_t *sl) {
 247     if ((sl->chip_id == STM32_CHIPID_F2) || (sl->chip_id == STM32_CHIPID_F4)) {
 248         uint32_t x = read_flash_cr(sl);
 249         x |= (1 << FLASH_F4_CR_STRT);
 250         stlink_write_debug32(sl, FLASH_F4_CR, x);
 251     } else {
 252         stlink_write_debug32(sl, FLASH_CR,
 253                              stlink_read_debug32(sl, FLASH_CR) | (1 << FLASH_CR_STRT) );
 254     }
 255 }
 256
 257 static inline uint32_t read_flash_acr(stlink_t *sl) {
 258     return stlink_read_debug32(sl, FLASH_ACR);
 259 }
 260
 261 static inline uint32_t read_flash_sr(stlink_t *sl) {
 262     uint32_t res;
 263     if ((sl->chip_id == STM32_CHIPID_F2) || (sl->chip_id == STM32_CHIPID_F4))
 264         res = stlink_read_debug32(sl, FLASH_F4_SR);
 265     else
 266         res = stlink_read_debug32(sl, FLASH_SR);
 267     //fprintf(stdout, "SR:0x%x\n", *(uint32_t*) sl->q_buf);
 268     return res;
 269 }
 270
 271 static inline unsigned int is_flash_busy(stlink_t *sl) {
 272     if ((sl->chip_id == STM32_CHIPID_F2) || (sl->chip_id == STM32_CHIPID_F4))
 273         return read_flash_sr(sl) & (1 << FLASH_F4_SR_BSY);
 274     else
 275         return read_flash_sr(sl) & (1 << FLASH_SR_BSY);
 276 }
 277
 278 static void wait_flash_busy(stlink_t *sl) {
 279     /* todo: add some delays here */
 280     while (is_flash_busy(sl))
 281         ;
 282 }
 283
 284 static void wait_flash_busy_progress(stlink_t *sl) {
 285     int i = 0;
 286     fprintf(stdout, "Mass erasing");
 287     fflush(stdout);
 288     while (is_flash_busy(sl)) {
 289         usleep(10000);
 290         i++;
 291         if (i % 100 == 0) {
 292             fprintf(stdout, ".");
 293             fflush(stdout);
 294         }
 295     }
 296     fprintf(stdout, "\n");
 297 }
 298
 299 static inline unsigned int is_flash_eop(stlink_t *sl) {
 300     return read_flash_sr(sl) & (1 << FLASH_SR_EOP);
 301 }
 302
 303 static void __attribute__((unused)) clear_flash_sr_eop(stlink_t *sl) {
 304     const uint32_t n = read_flash_sr(sl) & ~(1 << FLASH_SR_EOP);
 305     stlink_write_debug32(sl, FLASH_SR, n);
 306 }
 307
 308 static void __attribute__((unused)) wait_flash_eop(stlink_t *sl) {
 309     /* todo: add some delays here */
 310     while (is_flash_eop(sl) == 0)
 311         ;
 312 }
 313
 314 static inline void write_flash_ar(stlink_t *sl, uint32_t n) {
 315     stlink_write_debug32(sl, FLASH_AR, n);
 316 }
 317
 318 static inline void write_flash_cr_psiz(stlink_t *sl, uint32_t n) {
 319     uint32_t x = read_flash_cr(sl);
 320     x &= ~(0x03 << 8);
 321     x |= (n << 8);
 322 #if DEBUG_FLASH
 323     fprintf(stdout, "PSIZ:0x%x 0x%x\n", x, n);
 324 #endif
 325     stlink_write_debug32(sl, FLASH_F4_CR, x);
 326 }
 327
 328
 329 static inline void write_flash_cr_snb(stlink_t *sl, uint32_t n) {
 330     uint32_t x = read_flash_cr(sl);
 331     x &= ~FLASH_F4_CR_SNB_MASK;
 332     x |= (n << FLASH_F4_CR_SNB);
 333     x |= (1 << FLASH_F4_CR_SER);
 334 #if DEBUG_FLASH
 335     fprintf(stdout, "SNB:0x%x 0x%x\n", x, n);
 336 #endif
 337     stlink_write_debug32(sl, FLASH_F4_CR, x);
 338 }
 339
 340 #if 0 /* todo */
 341
 342 static void disable_flash_read_protection(stlink_t *sl) {
 343     /* erase the option byte area */
 344     /* rdp = 0x00a5; */
 345     /* reset */
 346 }
 347 #endif /* todo */
 348
 349
 350 // Delegates to the backends...
 351
 352 void stlink_close(stlink_t *sl) {
 353     DLOG("*** stlink_close ***\n");
 354     sl->backend->close(sl);
 355     free(sl);
 356 }
 357
 358 void stlink_exit_debug_mode(stlink_t *sl) {
 359     DLOG("*** stlink_exit_debug_mode ***\n");
 360     stlink_write_debug32(sl, DHCSR, DBGKEY);
 361     sl->backend->exit_debug_mode(sl);
 362 }
 363
 364 void stlink_enter_swd_mode(stlink_t *sl) {
 365     DLOG("*** stlink_enter_swd_mode ***\n");
 366     sl->backend->enter_swd_mode(sl);
 367 }
 368
 369 // Force the core into the debug mode -> halted state.
 370 void stlink_force_debug(stlink_t *sl) {
 371     DLOG("*** stlink_force_debug_mode ***\n");
 372     sl->backend->force_debug(sl);
 373 }
 374
 375 void stlink_exit_dfu_mode(stlink_t *sl) {
 376     DLOG("*** stlink_exit_dfu_mode ***\n");
 377     sl->backend->exit_dfu_mode(sl);
 378 }
 379
 380 uint32_t stlink_core_id(stlink_t *sl) {
 381     DLOG("*** stlink_core_id ***\n");
 382     sl->backend->core_id(sl);
 383     if (sl->verbose > 2)
 384         stlink_print_data(sl);
 385     DLOG("core_id = 0x%08x\n", sl->core_id);
 386     return sl->core_id;
 387 }
 388
 389 uint32_t stlink_chip_id(stlink_t *sl) {
 390     uint32_t chip_id = stlink_read_debug32(sl, 0xE0042000);
 391     if (chip_id == 0) chip_id = stlink_read_debug32(sl, 0x40015800);    //Try Corex M0 DBGMCU_IDCODE register address
 392     return chip_id;
 393 }
 394
 395 /**
 396  * Cortex m3 tech ref manual, CPUID register description
 397  * @param sl stlink context
 398  * @param cpuid pointer to the result object
 399  */
 400 void stlink_cpu_id(stlink_t *sl, cortex_m3_cpuid_t *cpuid) {
 401     uint32_t raw = stlink_read_debug32(sl, CM3_REG_CPUID);
 402     cpuid->implementer_id = (raw >> 24) & 0x7f;
 403     cpuid->variant = (raw >> 20) & 0xf;
 404     cpuid->part = (raw >> 4) & 0xfff;
 405     cpuid->revision = raw & 0xf;
 406     return;
 407 }
 408
 409 /**
 410  * reads and decodes the flash parameters, as dynamically as possible
 411  * @param sl
 412  * @return 0 for success, or -1 for unsupported core type.
 413  */
 414 int stlink_load_device_params(stlink_t *sl) {
 415     ILOG("Loading device parameters....\n");
 416     const chip_params_t *params = NULL;
 417     sl->core_id = stlink_core_id(sl);
 418     uint32_t chip_id = stlink_chip_id(sl);
 419
 420     sl->chip_id = chip_id & 0xfff;
 421     /* Fix chip_id for F4 rev A errata , Read CPU ID, as CoreID is the same for F2/F4*/
 422     if (sl->chip_id == 0x411) {
 423         uint32_t cpuid = stlink_read_debug32(sl, 0xE000ED00);
 424         if ((cpuid  & 0xfff0) == 0xc240)
 425             sl->chip_id = 0x413;
 426     }
 427
 428     for (size_t i = 0; i < sizeof(devices) / sizeof(devices[0]); i++) {
 429         if(devices[i].chip_id == sl->chip_id) {
 430             params = &devices[i];
 431             break;
 432         }
 433     }
 434     if (params == NULL) {
 435         WLOG("unknown chip id! %#x\n", chip_id);
 436         return -1;
 437     }
 438
 439     // These are fixed...
 440     sl->flash_base = STM32_FLASH_BASE;
 441     sl->sram_base = STM32_SRAM_BASE;
 442
 443     // read flash size from hardware, if possible...
 444     if (sl->chip_id == STM32_CHIPID_F2) {
 445         sl->flash_size = 0x100000; /* Use maximum, User must care!*/
 446     } else if (sl->chip_id == STM32_CHIPID_F4) {
 447                 sl->flash_size = 0x100000;                      //todo: RM0090 error; size register same address as unique ID
 448     } else {
 449         uint32_t flash_size = stlink_read_debug32(sl, params->flash_size_reg) & 0xffff;
 450         sl->flash_size = flash_size * 1024;
 451     }
 452     sl->flash_pgsz = params->flash_pagesize;
 453     sl->sram_size = params->sram_size;
 454     sl->sys_base = params->bootrom_base;
 455     sl->sys_size = params->bootrom_size;
 456
 457     ILOG("Device connected is: %s, id %#x\n", params->description, chip_id);
 458     // TODO make note of variable page size here.....
 459     ILOG("SRAM size: %#x bytes (%d KiB), Flash: %#x bytes (%d KiB) in pages of %zd bytes\n",
 460         sl->sram_size, sl->sram_size / 1024, sl->flash_size, sl->flash_size / 1024,
 461         sl->flash_pgsz);
 462     return 0;
 463 }
 464
 465 void stlink_reset(stlink_t *sl) {
 466     DLOG("*** stlink_reset ***\n");
 467     sl->backend->reset(sl);
 468 }
 469
 470 void stlink_jtag_reset(stlink_t *sl, int value) {
 471     DLOG("*** stlink_jtag_reset ***\n");
 472     sl->backend->jtag_reset(sl, value);
 473 }
 474
 475 void stlink_run(stlink_t *sl) {
 476     DLOG("*** stlink_run ***\n");
 477     sl->backend->run(sl);
 478 }
 479
 480 void stlink_status(stlink_t *sl) {
 481     DLOG("*** stlink_status ***\n");
 482     sl->backend->status(sl);
 483     stlink_core_stat(sl);
 484 }
 485
 486 /**
 487  * Decode the version bits, originally from -sg, verified with usb
 488  * @param sl stlink context, assumed to contain valid data in the buffer
 489  * @param slv output parsed version object
 490  */
 491 void _parse_version(stlink_t *sl, stlink_version_t *slv) {
 492     uint32_t b0 = sl->q_buf[0]; //lsb
 493     uint32_t b1 = sl->q_buf[1];
 494     uint32_t b2 = sl->q_buf[2];
 495     uint32_t b3 = sl->q_buf[3];
 496     uint32_t b4 = sl->q_buf[4];
 497     uint32_t b5 = sl->q_buf[5]; //msb
 498
 499     // b0 b1                       || b2 b3  | b4 b5
 500     // 4b        | 6b     | 6b     || 2B     | 2B
 501     // stlink_v  | jtag_v | swim_v || st_vid | stlink_pid
 502
 503     slv->stlink_v = (b0 & 0xf0) >> 4;
 504     slv->jtag_v = ((b0 & 0x0f) << 2) | ((b1 & 0xc0) >> 6);
 505     slv->swim_v = b1 & 0x3f;
 506     slv->st_vid = (b3 << 8) | b2;
 507     slv->stlink_pid = (b5 << 8) | b4;
 508     return;
 509 }
 510
 511 void stlink_version(stlink_t *sl) {
 512     DLOG("*** looking up stlink version\n");
 513     sl->backend->version(sl);
 514     _parse_version(sl, &sl->version);
 515
 516     DLOG("st vid         = 0x%04x (expect 0x%04x)\n", sl->version.st_vid, USB_ST_VID);
 517     DLOG("stlink pid     = 0x%04x\n", sl->version.stlink_pid);
 518     DLOG("stlink version = 0x%x\n", sl->version.stlink_v);
 519     DLOG("jtag version   = 0x%x\n", sl->version.jtag_v);
 520     DLOG("swim version   = 0x%x\n", sl->version.swim_v);
 521     if (sl->version.jtag_v == 0) {
 522         DLOG("    notice: the firmware doesn't support a jtag/swd interface\n");
 523     }
 524     if (sl->version.swim_v == 0) {
 525         DLOG("    notice: the firmware doesn't support a swim interface\n");
 526     }
 527 }
 528
 529 uint32_t stlink_read_debug32(stlink_t *sl, uint32_t addr) {
 530     uint32_t data = sl->backend->read_debug32(sl, addr);
 531     DLOG("*** stlink_read_debug32 %x is %#x\n", data, addr);
 532     return data;
 533 }
 534
 535 void stlink_write_debug32(stlink_t *sl, uint32_t addr, uint32_t data) {
 536     DLOG("*** stlink_write_debug32 %x to %#x\n", data, addr);
 537     sl->backend->write_debug32(sl, addr, data);
 538 }
 539
 540 void stlink_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 541     DLOG("*** stlink_write_mem32 %u bytes to %#x\n", len, addr);
 542     if (len % 4 != 0) {
 543         fprintf(stderr, "Error: Data length doesn't have a 32 bit alignment: +%d byte.\n", len % 4);
 544         return;
 545     }
 546     sl->backend->write_mem32(sl, addr, len);
 547 }
 548
 549 void stlink_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 550     DLOG("*** stlink_read_mem32 ***\n");
 551     if (len % 4 != 0) { // !!! never ever: fw gives just wrong values
 552         fprintf(stderr, "Error: Data length doesn't have a 32 bit alignment: +%d byte.\n",
 553                 len % 4);
 554         return;
 555     }
 556     sl->backend->read_mem32(sl, addr, len);
 557 }
 558
 559 void stlink_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
 560     DLOG("*** stlink_write_mem8 ***\n");
 561     if (len > 0x40 ) { // !!! never ever: Writing more then 0x40 bytes gives unexpected behaviour
 562         fprintf(stderr, "Error: Data length > 64: +%d byte.\n",
 563                 len);
 564         return;
 565     }
 566     sl->backend->write_mem8(sl, addr, len);
 567 }
 568
 569 void stlink_read_all_regs(stlink_t *sl, reg *regp) {
 570     DLOG("*** stlink_read_all_regs ***\n");
 571     sl->backend->read_all_regs(sl, regp);
 572 }
 573
 574 void stlink_read_all_unsupported_regs(stlink_t *sl, reg *regp) {
 575     DLOG("*** stlink_read_all_unsupported_regs ***\n");
 576     sl->backend->read_all_unsupported_regs(sl, regp);
 577 }
 578
 579 void stlink_write_reg(stlink_t *sl, uint32_t reg, int idx) {
 580     DLOG("*** stlink_write_reg\n");
 581     sl->backend->write_reg(sl, reg, idx);
 582 }
 583
 584 void stlink_read_reg(stlink_t *sl, int r_idx, reg *regp) {
 585     DLOG("*** stlink_read_reg\n");
 586     DLOG(" (%d) ***\n", r_idx);
 587
 588     if (r_idx > 20 || r_idx < 0) {
 589         fprintf(stderr, "Error: register index must be in [0..20]\n");
 590         return;
 591     }
 592
 593     sl->backend->read_reg(sl, r_idx, regp);
 594 }
 595
 596 void stlink_read_unsupported_reg(stlink_t *sl, int r_idx, reg *regp) {
 597     int r_convert;
 598
 599     DLOG("*** stlink_read_unsupported_reg\n");
 600     DLOG(" (%d) ***\n", r_idx);
 601
 602     /* Convert to values used by DCRSR */
 603     if (r_idx >= 0x1C && r_idx <= 0x1F) { /* primask, basepri, faultmask, or control */
 604         r_convert = 0x14;
 605     } else if (r_idx == 0x40) {     /* FPSCR */
 606         r_convert = 0x21;
 607     } else if (r_idx >= 0x20 && r_idx < 0x40) {
 608         r_convert = 0x40 + (r_idx - 0x20);
 609     } else {
 610         fprintf(stderr, "Error: register address must be in [0x1C..0x40]\n");
 611         return;
 612     }
 613
 614     sl->backend->read_unsupported_reg(sl, r_convert, regp);
 615 }
 616
 617 void stlink_write_unsupported_reg(stlink_t *sl, uint32_t val, int r_idx, reg *regp) {
 618     int r_convert;
 619
 620     DLOG("*** stlink_write_unsupported_reg\n");
 621     DLOG(" (%d) ***\n", r_idx);
 622
 623     /* Convert to values used by DCRSR */
 624     if (r_idx >= 0x1C && r_idx <= 0x1F) { /* primask, basepri, faultmask, or control */
 625         r_convert = r_idx;  /* The backend function handles this */
 626     } else if (r_idx == 0x40) {     /* FPSCR */
 627         r_convert = 0x21;
 628     } else if (r_idx >= 0x20 && r_idx < 0x40) {
 629         r_convert = 0x40 + (r_idx - 0x20);
 630     } else {
 631         fprintf(stderr, "Error: register address must be in [0x1C..0x40]\n");
 632         return;
 633     }
 634
 635     sl->backend->write_unsupported_reg(sl, val, r_convert, regp);
 636 }
 637
 638 unsigned int is_core_halted(stlink_t *sl) {
 639     /* return non zero if core is halted */
 640     stlink_status(sl);
 641     return sl->q_buf[0] == STLINK_CORE_HALTED;
 642 }
 643
 644 void stlink_step(stlink_t *sl) {
 645     DLOG("*** stlink_step ***\n");
 646     sl->backend->step(sl);
 647 }
 648
 649 int stlink_current_mode(stlink_t *sl) {
 650     int mode = sl->backend->current_mode(sl);
 651     switch (mode) {
 652         case STLINK_DEV_DFU_MODE:
 653             DLOG("stlink current mode: dfu\n");
 654             return mode;
 655         case STLINK_DEV_DEBUG_MODE:
 656             DLOG("stlink current mode: debug (jtag or swd)\n");
 657             return mode;
 658         case STLINK_DEV_MASS_MODE:
 659             DLOG("stlink current mode: mass\n");
 660             return mode;
 661     }
 662     DLOG("stlink mode: unknown!\n");
 663     return STLINK_DEV_UNKNOWN_MODE;
 664 }
 665
 666
 667
 668
 669 // End of delegates....  Common code below here...
 670
 671 // Endianness
 672 // http://www.ibm.com/developerworks/aix/library/au-endianc/index.html
 673 // const int i = 1;
 674 // #define is_bigendian() ( (*(char*)&i) == 0 )
 675
 676 inline unsigned int is_bigendian(void) {
 677     static volatile const unsigned int i = 1;
 678     return *(volatile const char*) &i == 0;
 679 }
 680
 681 uint16_t read_uint16(const unsigned char *c, const int pt) {
 682     uint32_t ui;
 683     char *p = (char *) &ui;
 684
 685     if (!is_bigendian()) { // le -> le (don't swap)
 686         p[0] = c[pt + 0];
 687         p[1] = c[pt + 1];
 688     } else {
 689         p[0] = c[pt + 1];
 690         p[1] = c[pt + 0];
 691     }
 692     return ui;
 693 }
 694
 695 // same as above with entrypoint.
 696
 697 void stlink_run_at(stlink_t *sl, stm32_addr_t addr) {
 698     stlink_write_reg(sl, addr, 15); /* pc register */
 699
 700     stlink_run(sl);
 701
 702     while (is_core_halted(sl) == 0)
 703         usleep(3000000);
 704 }
 705
 706 void stlink_core_stat(stlink_t *sl) {
 707     if (sl->q_len <= 0)
 708         return;
 709
 710     switch (sl->q_buf[0]) {
 711         case STLINK_CORE_RUNNING:
 712             sl->core_stat = STLINK_CORE_RUNNING;
 713             DLOG("  core status: running\n");
 714             return;
 715         case STLINK_CORE_HALTED:
 716             sl->core_stat = STLINK_CORE_HALTED;
 717             DLOG("  core status: halted\n");
 718             return;
 719         default:
 720             sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
 721             fprintf(stderr, "  core status: unknown\n");
 722     }
 723 }
 724
 725 void stlink_print_data(stlink_t * sl) {
 726     if (sl->q_len <= 0 || sl->verbose < UDEBUG)
 727         return;
 728     if (sl->verbose > 2)
 729         fprintf(stdout, "data_len = %d 0x%x\n", sl->q_len, sl->q_len);
 730
 731     for (int i = 0; i < sl->q_len; i++) {
 732         if (i % 16 == 0) {
 733             /*
 734                                     if (sl->q_data_dir == Q_DATA_OUT)
 735                                             fprintf(stdout, "\n<- 0x%08x ", sl->q_addr + i);
 736                                     else
 737                                             fprintf(stdout, "\n-> 0x%08x ", sl->q_addr + i);
 738              */
 739         }
 740         fprintf(stdout, " %02x", (unsigned int) sl->q_buf[i]);
 741     }
 742     fputs("\n\n", stdout);
 743 }
 744
 745 /* memory mapped file */
 746
 747 typedef struct mapped_file {
 748     uint8_t* base;
 749     size_t len;
 750 } mapped_file_t;
 751
 752 #define MAPPED_FILE_INITIALIZER { NULL, 0 }
 753
 754 static int map_file(mapped_file_t* mf, const char* path) {
 755     int error = -1;
 756     struct stat st;
 757
 758     const int fd = open(path, O_RDONLY);
 759     if (fd == -1) {
 760         fprintf(stderr, "open(%s) == -1\n", path);
 761         return -1;
 762     }
 763
 764     if (fstat(fd, &st) == -1) {
 765         fprintf(stderr, "fstat() == -1\n");
 766         goto on_error;
 767     }
 768
 769     mf->base = (uint8_t*) mmap(NULL, st.st_size, PROT_READ, MAP_SHARED, fd, 0);
 770     if (mf->base == MAP_FAILED) {
 771         fprintf(stderr, "mmap() == MAP_FAILED\n");
 772         goto on_error;
 773     }
 774
 775     mf->len = st.st_size;
 776
 777     /* success */
 778     error = 0;
 779
 780 on_error:
 781     close(fd);
 782
 783     return error;
 784 }
 785
 786 static void unmap_file(mapped_file_t * mf) {
 787     munmap((void*) mf->base, mf->len);
 788     mf->base = (unsigned char*) MAP_FAILED;
 789     mf->len = 0;
 790 }
 791
 792 /* Limit the block size to compare to 0x1800
 793    Anything larger will stall the STLINK2
 794    Maybe STLINK V1 needs smaller value!*/
 795 static int check_file(stlink_t* sl, mapped_file_t* mf, stm32_addr_t addr) {
 796     size_t off;
 797     size_t n_cmp = sl->flash_pgsz;
 798     if ( n_cmp > 0x1800)
 799         n_cmp = 0x1800;
 800
 801     for (off = 0; off < mf->len; off += n_cmp) {
 802         size_t aligned_size;
 803
 804         /* adjust last page size */
 805         size_t cmp_size = n_cmp;
 806         if ((off + n_cmp) > mf->len)
 807             cmp_size = mf->len - off;
 808
 809         aligned_size = cmp_size;
 810         if (aligned_size & (4 - 1))
 811             aligned_size = (cmp_size + 4) & ~(4 - 1);
 812
 813         stlink_read_mem32(sl, addr + off, aligned_size);
 814
 815         if (memcmp(sl->q_buf, mf->base + off, cmp_size))
 816             return -1;
 817     }
 818
 819     return 0;
 820 }
 821
 822 int stlink_fwrite_sram
 823 (stlink_t * sl, const char* path, stm32_addr_t addr) {
 824     /* write the file in sram at addr */
 825
 826     int error = -1;
 827     size_t off;
 828     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
 829
 830     if (map_file(&mf, path) == -1) {
 831         fprintf(stderr, "map_file() == -1\n");
 832         return -1;
 833     }
 834
 835     /* check addr range is inside the sram */
 836     if (addr < sl->sram_base) {
 837         fprintf(stderr, "addr too low\n");
 838         goto on_error;
 839     } else if ((addr + mf.len) < addr) {
 840         fprintf(stderr, "addr overruns\n");
 841         goto on_error;
 842     } else if ((addr + mf.len) > (sl->sram_base + sl->sram_size)) {
 843         fprintf(stderr, "addr too high\n");
 844         goto on_error;
 845     } else if ((addr & 3) || (mf.len & 3)) {
 846         /* todo */
 847         fprintf(stderr, "unaligned addr or size\n");
 848         goto on_error;
 849     }
 850
 851     /* do the copy by 1k blocks */
 852     for (off = 0; off < mf.len; off += 1024) {
 853         size_t size = 1024;
 854         if ((off + size) > mf.len)
 855             size = mf.len - off;
 856
 857         memcpy(sl->q_buf, mf.base + off, size);
 858
 859         /* round size if needed */
 860         if (size & 3)
 861             size += 2;
 862
 863         stlink_write_mem32(sl, addr + off, size);
 864     }
 865
 866     /* check the file ha been written */
 867     if (check_file(sl, &mf, addr) == -1) {
 868         fprintf(stderr, "check_file() == -1\n");
 869         goto on_error;
 870     }
 871
 872     /* success */
 873     error = 0;
 874     /* set stack*/
 875     stlink_write_reg(sl, stlink_read_debug32(sl, addr    ),13);
 876     /* Set PC to the reset routine*/
 877     stlink_write_reg(sl, stlink_read_debug32(sl, addr + 4),15);
 878     stlink_run(sl);
 879
 880 on_error:
 881     unmap_file(&mf);
 882     return error;
 883 }
 884
 885 int stlink_fread(stlink_t* sl, const char* path, stm32_addr_t addr, size_t size) {
 886     /* read size bytes from addr to file */
 887
 888     int error = -1;
 889     size_t off;
 890     int num_empty = 0;
 891     unsigned char erased_pattern = (sl->chip_id == STM32_CHIPID_L1_MEDIUM)?0:0xff;
 892
 893     const int fd = open(path, O_RDWR | O_TRUNC | O_CREAT, 00700);
 894     if (fd == -1) {
 895         fprintf(stderr, "open(%s) == -1\n", path);
 896         return -1;
 897     }
 898
 899     if (size <1)
 900         size = sl->flash_size;
 901
 902     if (size > sl->flash_size)
 903         size = sl->flash_size;
 904
 905     /* do the copy by 1k blocks */
 906     for (off = 0; off < size; off += 1024) {
 907         size_t read_size = 1024;
 908         size_t rounded_size;
 909         size_t index;
 910         if ((off + read_size) > size)
 911           read_size = size - off;
 912
 913         /* round size if needed */
 914         rounded_size = read_size;
 915         if (rounded_size & 3)
 916           rounded_size = (rounded_size + 4) & ~(3);
 917
 918         stlink_read_mem32(sl, addr + off, rounded_size);
 919
 920         for(index = 0; index < read_size; index ++) {
 921             if (sl->q_buf[index] == erased_pattern)
 922                 num_empty ++;
 923             else
 924                 num_empty = 0;
 925         }
 926         if (write(fd, sl->q_buf, read_size) != (ssize_t) read_size) {
 927             fprintf(stderr, "write() != read_size\n");
 928             goto on_error;
 929         }
 930     }
 931
 932     /* Ignore NULL Bytes at end of file */
 933     if (!ftruncate(fd, size - num_empty)) {
 934         error = -1;
 935     }
 936
 937     /* success */
 938     error = 0;
 939
 940 on_error:
 941     close(fd);
 942
 943     return error;
 944 }
 945
 946 int write_buffer_to_sram(stlink_t *sl, flash_loader_t* fl, const uint8_t* buf, size_t size) {
 947     /* write the buffer right after the loader */
 948     size_t chunk = size & ~0x3;
 949     size_t rem   = size & 0x3;
 950     if (chunk) {
 951         memcpy(sl->q_buf, buf, chunk);
 952         stlink_write_mem32(sl, fl->buf_addr, chunk);
 953     }
 954     if (rem) {
 955         memcpy(sl->q_buf, buf+chunk, rem);
 956         stlink_write_mem8(sl, (fl->buf_addr)+chunk, rem);
 957     }
 958     return 0;
 959 }
 960
 961 uint32_t calculate_F4_sectornum(uint32_t flashaddr){
 962     flashaddr &= ~STM32_FLASH_BASE;     //Page now holding the actual flash address
 963     if (flashaddr<0x4000) return (0);
 964     else if(flashaddr<0x8000) return(1);
 965     else if(flashaddr<0xc000) return(2);
 966     else if(flashaddr<0x10000) return(3);
 967     else if(flashaddr<0x20000) return(4);
 968     else return(flashaddr/0x20000)+4;
 969
 970 }
 971
 972 uint32_t stlink_calculate_pagesize(stlink_t *sl, uint32_t flashaddr){
 973         if ((sl->chip_id == STM32_CHIPID_F2) || (sl->chip_id == STM32_CHIPID_F4)) {
 974                 uint32_t sector=calculate_F4_sectornum(flashaddr);
 975                 if (sector<4) sl->flash_pgsz=0x4000;
 976                 else if(sector<5) sl->flash_pgsz=0x10000;
 977                 else sl->flash_pgsz=0x20000;
 978         }
 979         return (sl->flash_pgsz);
 980 }
 981
 982 /**
 983  * Erase a page of flash, assumes sl is fully populated with things like chip/core ids
 984  * @param sl stlink context
 985  * @param flashaddr an address in the flash page to erase
 986  * @return 0 on success -ve on failure
 987  */
 988 int stlink_erase_flash_page(stlink_t *sl, stm32_addr_t flashaddr)
 989 {
 990   if ((sl->chip_id == STM32_CHIPID_F2) || (sl->chip_id == STM32_CHIPID_F4)) {
 991     /* wait for ongoing op to finish */
 992     wait_flash_busy(sl);
 993
 994     /* unlock if locked */
 995     unlock_flash_if(sl);
 996
 997     /* select the page to erase */
 998     // calculate the actual page from the address
 999     uint32_t sector=calculate_F4_sectornum(flashaddr);
1000
1001     fprintf(stderr, "EraseFlash - Sector:0x%x Size:0x%x\n", sector, stlink_calculate_pagesize(sl, flashaddr));
1002     write_flash_cr_snb(sl, sector);
1003
1004     /* start erase operation */
1005     set_flash_cr_strt(sl);
1006
1007     /* wait for completion */
1008     wait_flash_busy(sl);
1009
1010     /* relock the flash */
1011     //todo: fails to program if this is in
1012     lock_flash(sl);
1013 #if DEBUG_FLASH
1014         fprintf(stdout, "Erase Final CR:0x%x\n", read_flash_cr(sl));
1015 #endif
1016   } else if (sl->chip_id == STM32_CHIPID_L1_MEDIUM) {
1017
1018     uint32_t val;
1019
1020     /* disable pecr protection */
1021     stlink_write_debug32(sl, STM32L_FLASH_PEKEYR, 0x89abcdef);
1022     stlink_write_debug32(sl, STM32L_FLASH_PEKEYR, 0x02030405);
1023
1024     /* check pecr.pelock is cleared */
1025     val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1026     if (val & (1 << 0)) {
1027       WLOG("pecr.pelock not clear (%#x)\n", val);
1028       return -1;
1029     }
1030
1031     /* unlock program memory */
1032     stlink_write_debug32(sl, STM32L_FLASH_PRGKEYR, 0x8c9daebf);
1033     stlink_write_debug32(sl, STM32L_FLASH_PRGKEYR, 0x13141516);
1034
1035     /* check pecr.prglock is cleared */
1036     val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1037     if (val & (1 << 1)) {
1038       WLOG("pecr.prglock not clear (%#x)\n", val);
1039       return -1;
1040     }
1041
1042     /* unused: unlock the option byte block */
1043 #if 0
1044     stlink_write_debug32(sl, STM32L_FLASH_OPTKEYR, 0xfbead9c8);
1045     stlink_write_debug32(sl, STM32L_FLASH_OPTKEYR, 0x24252627);
1046
1047     /* check pecr.optlock is cleared */
1048     val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1049     if (val & (1 << 2)) {
1050       fprintf(stderr, "pecr.prglock not clear\n");
1051       return -1;
1052     }
1053 #endif
1054
1055     /* set pecr.{erase,prog} */
1056     val |= (1 << 9) | (1 << 3);
1057     stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1058
1059 #if 0 /* fix_to_be_confirmed */
1060
1061     /* wait for sr.busy to be cleared
1062        MP: Test shows that busy bit is not set here. Perhaps, PM0062 is
1063        wrong and we do not need to wait here for clearing the busy bit.
1064        TEXANE: ok, if experience says so and it works for you, we comment
1065        it. If someone has a problem, please drop an email.
1066      */
1067     while ((stlink_read_debug32(sl, STM32L_FLASH_SR) & (1 << 0)) != 0)
1068         ;
1069
1070 #endif /* fix_to_be_confirmed */
1071
1072     /* write 0 to the first word of the page to be erased */
1073     stlink_write_debug32(sl, flashaddr, 0);
1074
1075     /* MP: It is better to wait for clearing the busy bit after issuing
1076     page erase command, even though PM0062 recommends to wait before it.
1077     Test shows that a few iterations is performed in the following loop
1078     before busy bit is cleared.*/
1079     while ((stlink_read_debug32(sl, STM32L_FLASH_SR) & (1 << 0)) != 0)
1080         ;
1081
1082     /* reset lock bits */
1083     val = stlink_read_debug32(sl, STM32L_FLASH_PECR)
1084         | (1 << 0) | (1 << 1) | (1 << 2);
1085     stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1086   } else if (sl->core_id == STM32VL_CORE_ID) {
1087     /* wait for ongoing op to finish */
1088     wait_flash_busy(sl);
1089
1090     /* unlock if locked */
1091     unlock_flash_if(sl);
1092
1093     /* set the page erase bit */
1094     set_flash_cr_per(sl);
1095
1096     /* select the page to erase */
1097     write_flash_ar(sl, flashaddr);
1098
1099     /* start erase operation, reset by hw with bsy bit */
1100     set_flash_cr_strt(sl);
1101
1102     /* wait for completion */
1103     wait_flash_busy(sl);
1104
1105     /* relock the flash */
1106     lock_flash(sl);
1107   } else {
1108     WLOG("unknown coreid %x, page erase failed\n", sl->core_id);
1109     return -1;
1110   }
1111
1112   /* todo: verify the erased page */
1113
1114   return 0;
1115 }
1116
1117 int stlink_erase_flash_mass(stlink_t *sl) {
1118     if (sl->chip_id == STM32_CHIPID_L1_MEDIUM) {
1119         /* erase each page */
1120         int i = 0, num_pages = sl->flash_size/sl->flash_pgsz;
1121         for (i = 0; i < num_pages; i++) {
1122             /* addr must be an addr inside the page */
1123             stm32_addr_t addr = sl->flash_base + i * sl->flash_pgsz;
1124             if (stlink_erase_flash_page(sl, addr) == -1) {
1125                 WLOG("Failed to erase_flash_page(%#zx) == -1\n", addr);
1126                 return -1;
1127             }
1128             fprintf(stdout,"\rFlash page at %5d/%5d erased", i, num_pages);
1129             fflush(stdout);
1130         }
1131         fprintf(stdout, "\n");
1132     } else {
1133         /* wait for ongoing op to finish */
1134         wait_flash_busy(sl);
1135
1136         /* unlock if locked */
1137         unlock_flash_if(sl);
1138
1139         /* set the mass erase bit */
1140         set_flash_cr_mer(sl);
1141
1142         /* start erase operation, reset by hw with bsy bit */
1143         set_flash_cr_strt(sl);
1144
1145         /* wait for completion */
1146         wait_flash_busy_progress(sl);
1147
1148         /* relock the flash */
1149         lock_flash(sl);
1150
1151         /* todo: verify the erased memory */
1152     }
1153     return 0;
1154 }
1155
1156 int init_flash_loader(stlink_t *sl, flash_loader_t* fl) {
1157     size_t size;
1158
1159     /* allocate the loader in sram */
1160     if (write_loader_to_sram(sl, &fl->loader_addr, &size) == -1) {
1161         WLOG("Failed to write flash loader to sram!\n");
1162         return -1;
1163     }
1164
1165     /* allocate a one page buffer in sram right after loader */
1166     fl->buf_addr = fl->loader_addr + size;
1167     ILOG("Successfully loaded flash loader in sram\n");
1168     return 0;
1169 }
1170
1171 int write_loader_to_sram(stlink_t *sl, stm32_addr_t* addr, size_t* size) {
1172     /* from openocd, contrib/loaders/flash/stm32.s */
1173     static const uint8_t loader_code_stm32vl[] = {
1174         0x08, 0x4c, /* ldr      r4, STM32_FLASH_BASE */
1175         0x1c, 0x44, /* add      r4, r3 */
1176         /* write_half_word: */
1177         0x01, 0x23, /* movs     r3, #0x01 */
1178         0x23, 0x61, /* str      r3, [r4, #STM32_FLASH_CR_OFFSET] */
1179         0x30, 0xf8, 0x02, 0x3b, /* ldrh r3, [r0], #0x02 */
1180         0x21, 0xf8, 0x02, 0x3b, /* strh r3, [r1], #0x02 */
1181         /* busy: */
1182         0xe3, 0x68, /* ldr      r3, [r4, #STM32_FLASH_SR_OFFSET] */
1183         0x13, 0xf0, 0x01, 0x0f, /* tst  r3, #0x01 */
1184         0xfb, 0xd0, /* beq      busy */
1185         0x13, 0xf0, 0x14, 0x0f, /* tst  r3, #0x14 */
1186         0x01, 0xd1, /* bne      exit */
1187         0x01, 0x3a, /* subs     r2, r2, #0x01 */
1188         0xf0, 0xd1, /* bne      write_half_word */
1189         /* exit: */
1190         0x00, 0xbe, /* bkpt     #0x00 */
1191         0x00, 0x20, 0x02, 0x40, /* STM32_FLASH_BASE: .word 0x40022000 */
1192     };
1193
1194     static const uint8_t loader_code_stm32l[] = {
1195
1196         /* openocd.git/contrib/loaders/flash/stm32lx.S
1197            r0, input, dest addr
1198            r1, input, source addr
1199            r2, input, word count
1200            r3, output, word count
1201         */
1202
1203         0x00, 0x23,
1204         0x04, 0xe0,
1205
1206         0x51, 0xf8, 0x04, 0xcb,
1207         0x40, 0xf8, 0x04, 0xcb,
1208         0x01, 0x33,
1209
1210         0x93, 0x42,
1211         0xf8, 0xd3,
1212         0x00, 0xbe
1213     };
1214
1215     static const uint8_t loader_code_stm32f4[] = {
1216         // flashloaders/stm32f4.s
1217
1218         0x07, 0x4b,
1219
1220         0x62, 0xb1,
1221         0x04, 0x68,
1222         0x0c, 0x60,
1223
1224         0xdc, 0x89,
1225         0x14, 0xf0, 0x01, 0x0f,
1226         0xfb, 0xd1,
1227         0x00, 0xf1, 0x04, 0x00,
1228         0x01, 0xf1, 0x04, 0x01,
1229         0xa2, 0xf1, 0x01, 0x02,
1230         0xf1, 0xe7,
1231
1232         0x00, 0xbe,
1233
1234         0x00, 0x3c, 0x02, 0x40,
1235     };
1236
1237     const uint8_t* loader_code;
1238     size_t loader_size;
1239
1240     if (sl->chip_id == STM32_CHIPID_L1_MEDIUM) { /* stm32l */
1241         loader_code = loader_code_stm32l;
1242         loader_size = sizeof(loader_code_stm32l);
1243     } else if (sl->core_id == STM32VL_CORE_ID) {
1244         loader_code = loader_code_stm32vl;
1245         loader_size = sizeof(loader_code_stm32vl);
1246     } else if (sl->chip_id == STM32_CHIPID_F2 || sl->chip_id == STM32_CHIPID_F4) {
1247         loader_code = loader_code_stm32f4;
1248         loader_size = sizeof(loader_code_stm32f4);
1249     } else {
1250         ELOG("unknown coreid, not sure what flash loader to use, aborting!: %x\n", sl->core_id);
1251         return -1;
1252     }
1253
1254     memcpy(sl->q_buf, loader_code, loader_size);
1255     stlink_write_mem32(sl, sl->sram_base, loader_size);
1256
1257     *addr = sl->sram_base;
1258     *size = loader_size;
1259
1260     /* success */
1261     return 0;
1262 }
1263
1264 int stlink_fcheck_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
1265     /* check the contents of path are at addr */
1266
1267     int res;
1268     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1269
1270     if (map_file(&mf, path) == -1)
1271         return -1;
1272
1273     res = check_file(sl, &mf, addr);
1274
1275     unmap_file(&mf);
1276
1277     return res;
1278 }
1279
1280 /**
1281  * Verify addr..addr+len is binary identical to base...base+len
1282  * @param sl stlink context
1283  * @param address stm device address
1284  * @param data host side buffer to check against
1285  * @param length how much
1286  * @return 0 for success, -ve for failure
1287  */
1288 int stlink_verify_write_flash(stlink_t *sl, stm32_addr_t address, uint8_t *data, unsigned length) {
1289     size_t off;
1290     size_t cmp_size = (sl->flash_pgsz > 0x1800)? 0x1800:sl->flash_pgsz;
1291     ILOG("Starting verification of write complete\n");
1292     for (off = 0; off < length; off += cmp_size) {
1293         size_t aligned_size;
1294
1295         /* adjust last page size */
1296         if ((off + cmp_size) > length)
1297             cmp_size = length - off;
1298
1299         aligned_size = cmp_size;
1300         if (aligned_size & (4 - 1))
1301             aligned_size = (cmp_size + 4) & ~(4 - 1);
1302
1303         stlink_read_mem32(sl, address + off, aligned_size);
1304
1305         if (memcmp(sl->q_buf, data + off, cmp_size)) {
1306             ELOG("Verification of flash failed at offset: %zd\n", off);
1307             return -1;
1308         }
1309     }
1310     ILOG("Flash written and verified! jolly good!\n");
1311     return 0;
1312
1313 }
1314
1315 int stm32l1_write_half_pages(stlink_t *sl, stm32_addr_t addr, uint8_t* base, unsigned num_half_pages)
1316 {
1317     unsigned int count;
1318     uint32_t val;
1319     flash_loader_t fl;
1320
1321     ILOG("Starting Half page flash write for STM32L core id\n");
1322     /* flash loader initialization */
1323     if (init_flash_loader(sl, &fl) == -1) {
1324         WLOG("init_flash_loader() == -1\n");
1325         return -1;
1326     }
1327     /* Unlock already done */
1328     val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1329     val |= (1 << FLASH_L1_FPRG);
1330     stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1331
1332     val |= (1 << FLASH_L1_PROG);
1333     stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1334     while ((stlink_read_debug32(sl, STM32L_FLASH_SR) & (1 << 0)) != 0) {}
1335
1336 #define L1_WRITE_BLOCK_SIZE 0x80
1337     for (count = 0; count  < num_half_pages; count ++) {
1338         if (run_flash_loader(sl, &fl, addr + count * L1_WRITE_BLOCK_SIZE, base + count * L1_WRITE_BLOCK_SIZE, L1_WRITE_BLOCK_SIZE) == -1) {
1339             WLOG("l1_run_flash_loader(%#zx) failed! == -1\n", addr + count * L1_WRITE_BLOCK_SIZE);
1340             val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1341             val &= ~((1 << FLASH_L1_FPRG) |(1 << FLASH_L1_PROG));
1342             stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1343             return -1;
1344         }
1345         /* wait for sr.busy to be cleared */
1346         if (sl->verbose >= 1) {
1347             /* show progress. writing procedure is slow
1348                and previous errors are misleading */
1349             fprintf(stdout, "\r%3u/%u halfpages written", count + 1, num_half_pages);
1350             fflush(stdout);
1351         }
1352         while ((stlink_read_debug32(sl, STM32L_FLASH_SR) & (1 << 0)) != 0) {
1353         }
1354     }
1355     val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1356     val &= ~(1 << FLASH_L1_PROG);
1357     stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1358     val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1359     val &= ~(1 << FLASH_L1_FPRG);
1360     stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1361
1362     return 0;
1363 }
1364
1365 int stlink_write_flash(stlink_t *sl, stm32_addr_t addr, uint8_t* base, unsigned len) {
1366     size_t off;
1367     flash_loader_t fl;
1368     ILOG("Attempting to write %d (%#x) bytes to stm32 address: %u (%#x)\n",
1369         len, len, addr, addr);
1370     /* check addr range is inside the flash */
1371     stlink_calculate_pagesize(sl, addr);
1372     if (addr < sl->flash_base) {
1373         ELOG("addr too low %#x < %#x\n", addr, sl->flash_base);
1374         return -1;
1375     } else if ((addr + len) < addr) {
1376         ELOG("addr overruns\n");
1377         return -1;
1378     } else if ((addr + len) > (sl->flash_base + sl->flash_size)) {
1379         ELOG("addr too high\n");
1380         return -1;
1381     } else if (addr & 1) {
1382         ELOG("unaligned addr 0x%x\n", addr);
1383         return -1;
1384     } else if (len & 1) {
1385         WLOG("unaligned len 0x%x -- padding with zero\n", len);
1386         len += 1;
1387     } else if (addr & (sl->flash_pgsz - 1)) {
1388         ELOG("addr not a multiple of pagesize, not supported\n");
1389         return -1;
1390     }
1391
1392     // Make sure we've loaded the context with the chip details
1393     stlink_core_id(sl);
1394     /* erase each page */
1395     int page_count = 0;
1396     for (off = 0; off < len; off += stlink_calculate_pagesize(sl, addr + off)) {
1397         /* addr must be an addr inside the page */
1398         if (stlink_erase_flash_page(sl, addr + off) == -1) {
1399             ELOG("Failed to erase_flash_page(%#zx) == -1\n", addr + off);
1400             return -1;
1401         }
1402         fprintf(stdout,"\rFlash page at addr: 0x%08lx erased",
1403                 (unsigned long)addr + off);
1404         fflush(stdout);
1405         page_count++;
1406     }
1407     fprintf(stdout,"\n");
1408     ILOG("Finished erasing %d pages of %d (%#x) bytes\n",
1409         page_count, sl->flash_pgsz, sl->flash_pgsz);
1410
1411     if ((sl->chip_id == STM32_CHIPID_F2) || (sl->chip_id == STM32_CHIPID_F4)) {
1412         /* todo: check write operation */
1413
1414         ILOG("Starting Flash write for F2/F4\n");
1415         /* flash loader initialization */
1416         if (init_flash_loader(sl, &fl) == -1) {
1417             ELOG("init_flash_loader() == -1\n");
1418             return -1;
1419         }
1420
1421         /* First unlock the cr */
1422         unlock_flash_if(sl);
1423
1424         /* TODO: Check that Voltage range is 2.7 - 3.6 V */
1425         /* set parallelisim to 32 bit*/
1426         write_flash_cr_psiz(sl, 2);
1427
1428         /* set programming mode */
1429         set_flash_cr_pg(sl);
1430
1431         for(off = 0; off < len;) {
1432             size_t size = len - off > 0x8000 ? 0x8000 : len - off;
1433
1434             printf("size: %zu\n", size);
1435
1436             if (run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
1437                 ELOG("run_flash_loader(%#zx) failed! == -1\n", addr + off);
1438                 return -1;
1439             }
1440
1441             off += size;
1442         }
1443
1444 #if 0
1445 #define PROGRESS_CHUNK_SIZE 0x1000
1446         /* write a word in program memory */
1447         for (off = 0; off < len; off += sizeof(uint32_t)) {
1448             uint32_t data;
1449             if (sl->verbose >= 1) {
1450                 if ((off & (PROGRESS_CHUNK_SIZE - 1)) == 0) {
1451                     /* show progress. writing procedure is slow
1452                        and previous errors are misleading */
1453                     const uint32_t pgnum = (off / PROGRESS_CHUNK_SIZE)+1;
1454                     const uint32_t pgcount = len / PROGRESS_CHUNK_SIZE +1;
1455                     fprintf(stdout, "Writing %ukB chunk %u out of %u\n",
1456                             PROGRESS_CHUNK_SIZE/1024, pgnum, pgcount);
1457                 }
1458             }
1459
1460             write_uint32((unsigned char*) &data, *(uint32_t*) (base + off));
1461             stlink_write_debug32(sl, addr + off, data);
1462
1463             /* wait for sr.busy to be cleared */
1464             wait_flash_busy(sl);
1465
1466         }
1467 #endif
1468         /* Relock flash */
1469         lock_flash(sl);
1470
1471 #if 0 /* todo: debug mode */
1472         fprintf(stdout, "Final CR:0x%x\n", read_flash_cr(sl));
1473 #endif
1474
1475     }   //STM32F4END
1476
1477     else if (sl->chip_id == STM32_CHIPID_L1_MEDIUM)    {
1478         /* use fast word write. todo: half page. */
1479         uint32_t val;
1480
1481 #if 0 /* todo: check write operation */
1482
1483         uint32_t nwrites = sl->flash_pgsz;
1484
1485         redo_write:
1486
1487 #endif /* todo: check write operation */
1488
1489         /* disable pecr protection */
1490         stlink_write_debug32(sl, STM32L_FLASH_PEKEYR, 0x89abcdef);
1491         stlink_write_debug32(sl, STM32L_FLASH_PEKEYR, 0x02030405);
1492
1493         /* check pecr.pelock is cleared */
1494         val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1495         if (val & (1 << 0)) {
1496                 fprintf(stderr, "pecr.pelock not clear\n");
1497                 return -1;
1498         }
1499
1500         /* unlock program memory */
1501         stlink_write_debug32(sl, STM32L_FLASH_PRGKEYR, 0x8c9daebf);
1502         stlink_write_debug32(sl, STM32L_FLASH_PRGKEYR, 0x13141516);
1503
1504         /* check pecr.prglock is cleared */
1505         val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1506         if (val & (1 << 1)) {
1507                 fprintf(stderr, "pecr.prglock not clear\n");
1508                 return -1;
1509         }
1510         off = 0;
1511         if (len > L1_WRITE_BLOCK_SIZE) {
1512             if (stm32l1_write_half_pages(sl, addr, base, len/L1_WRITE_BLOCK_SIZE) == -1) {
1513                 /* This may happen on a blank device! */
1514                 WLOG("\nwrite_half_pages failed == -1\n");
1515             } else {
1516                 off = (len /L1_WRITE_BLOCK_SIZE)*L1_WRITE_BLOCK_SIZE;
1517             }
1518         }
1519
1520         /* write remainingword in program memory */
1521         for ( ; off < len; off += sizeof(uint32_t)) {
1522             uint32_t data;
1523             if (off > 254)
1524                 fprintf(stdout, "\r");
1525
1526             if ((off % sl->flash_pgsz) > (sl->flash_pgsz -5)) {
1527                 fprintf(stdout, "\r%3zd/%3zd pages written",
1528                         off/sl->flash_pgsz, len/sl->flash_pgsz);
1529                 fflush(stdout);
1530             }
1531
1532             write_uint32((unsigned char*) &data, *(uint32_t*) (base + off));
1533             stlink_write_debug32(sl, addr + off, data);
1534
1535             /* wait for sr.busy to be cleared */
1536             while ((stlink_read_debug32(sl, STM32L_FLASH_SR) & (1 << 0)) != 0)
1537                 ;
1538
1539 #if 0 /* todo: check redo write operation */
1540
1541             /* check written bytes. todo: should be on a per page basis. */
1542             data = stlink_read_debug32(sl, addr + off);
1543             if (data == *(uint32_t*)(base + off)) {
1544                 /* re erase the page and redo the write operation */
1545                 uint32_t page;
1546                 uint32_t val;
1547
1548                 /* fail if successive write count too low */
1549                 if (nwrites < sl->flash_pgsz) {
1550                     fprintf(stderr, "writes operation failure count too high, aborting\n");
1551                     return -1;
1552                 }
1553
1554                 nwrites = 0;
1555
1556                 /* assume addr aligned */
1557                 if (off % sl->flash_pgsz) off &= ~(sl->flash_pgsz - 1);
1558                 page = addr + off;
1559
1560                 fprintf(stderr, "invalid write @0x%x(0x%x): 0x%x != 0x%x. retrying.\n",
1561                         page, addr + off, read_uint32(base + off, 0), read_uint32(sl->q_buf, 0));
1562
1563                 /* reset lock bits */
1564                 val = stlink_read_debug32(sl, STM32L_FLASH_PECR)
1565                     | (1 << 0) | (1 << 1) | (1 << 2);
1566                 stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1567
1568                 stlink_erase_flash_page(sl, page);
1569
1570                 goto redo_write;
1571             }
1572
1573             /* increment successive writes counter */
1574             ++nwrites;
1575
1576 #endif /* todo: check redo write operation */
1577         }
1578         fprintf(stdout, "\n");
1579         /* reset lock bits */
1580         val = stlink_read_debug32(sl, STM32L_FLASH_PECR)
1581              | (1 << 0) | (1 << 1) | (1 << 2);
1582         stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1583     } else if (sl->core_id == STM32VL_CORE_ID) {
1584         ILOG("Starting Flash write for VL core id\n");
1585         /* flash loader initialization */
1586         if (init_flash_loader(sl, &fl) == -1) {
1587             ELOG("init_flash_loader() == -1\n");
1588             return -1;
1589         }
1590
1591         int write_block_count = 0;
1592         for (off = 0; off < len; off += sl->flash_pgsz) {
1593             /* adjust last write size */
1594             size_t size = sl->flash_pgsz;
1595             if ((off + sl->flash_pgsz) > len) size = len - off;
1596
1597             /* unlock and set programming mode */
1598             unlock_flash_if(sl);
1599             set_flash_cr_pg(sl);
1600             //DLOG("Finished setting flash cr pg, running loader!\n");
1601             if (run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
1602                 ELOG("run_flash_loader(%#zx) failed! == -1\n", addr + off);
1603                 return -1;
1604             }
1605             lock_flash(sl);
1606             if (sl->verbose >= 1) {
1607                 /* show progress. writing procedure is slow
1608                    and previous errors are misleading */
1609               fprintf(stdout, "\r%3u/%lu pages written", write_block_count++, (unsigned long)len/sl->flash_pgsz);
1610                 fflush(stdout);
1611             }
1612         }
1613         fprintf(stdout, "\n");
1614     } else {
1615         ELOG("unknown coreid, not sure how to write: %x\n", sl->core_id);
1616         return -1;
1617     }
1618
1619     return stlink_verify_write_flash(sl, addr, base, len);
1620 }
1621
1622 /**
1623  * Write the given binary file into flash at address "addr"
1624  * @param sl
1625  * @param path readable file path, should be binary image
1626  * @param addr where to start writing
1627  * @return 0 on success, -ve on failure.
1628  */
1629 int stlink_fwrite_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
1630     /* write the file in flash at addr */
1631     int err;
1632     unsigned int num_empty = 0, index;
1633     unsigned char erased_pattern =(sl->chip_id == STM32_CHIPID_L1_MEDIUM)?0:0xff;
1634     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1635     if (map_file(&mf, path) == -1) {
1636         ELOG("map_file() == -1\n");
1637         return -1;
1638     }
1639     for(index = 0; index < mf.len; index ++) {
1640         if (mf.base[index] == erased_pattern)
1641             num_empty ++;
1642         else
1643             num_empty = 0;
1644     }
1645     if(num_empty != 0) {
1646         ILOG("Ignoring %d bytes of Zeros at end of file\n",num_empty);
1647         mf.len -= num_empty;
1648     }
1649     err = stlink_write_flash(sl, addr, mf.base, mf.len);
1650     /* set stack*/
1651     stlink_write_reg(sl, stlink_read_debug32(sl, addr    ),13);
1652     /* Set PC to the reset routine*/
1653     stlink_write_reg(sl, stlink_read_debug32(sl, addr + 4),15);
1654     stlink_run(sl);
1655     unmap_file(&mf);
1656     return err;
1657 }
1658
1659 int run_flash_loader(stlink_t *sl, flash_loader_t* fl, stm32_addr_t target, const uint8_t* buf, size_t size) {
1660
1661     reg rr;
1662     int i = 0;
1663     DLOG("Running flash loader, write address:%#x, size: %zd\n", target, size);
1664     // FIXME This can never return -1
1665     if (write_buffer_to_sram(sl, fl, buf, size) == -1) {
1666         // IMPOSSIBLE!
1667         ELOG("write_buffer_to_sram() == -1\n");
1668         return -1;
1669     }
1670
1671     if (sl->chip_id == STM32_CHIPID_L1_MEDIUM) {
1672
1673         size_t count = size / sizeof(uint32_t);
1674         if (size % sizeof(uint32_t)) ++count;
1675
1676         /* setup core */
1677         stlink_write_reg(sl, target, 0); /* target */
1678         stlink_write_reg(sl, fl->buf_addr, 1); /* source */
1679         stlink_write_reg(sl, count, 2); /* count (32 bits words) */
1680         stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1681
1682     } else if (sl->core_id == STM32VL_CORE_ID) {
1683
1684         size_t count = size / sizeof(uint16_t);
1685         if (size % sizeof(uint16_t)) ++count;
1686
1687         /* setup core */
1688         stlink_write_reg(sl, fl->buf_addr, 0); /* source */
1689         stlink_write_reg(sl, target, 1); /* target */
1690         stlink_write_reg(sl, count, 2); /* count (16 bits half words) */
1691         stlink_write_reg(sl, 0, 3); /* flash bank 0 (input) */
1692         stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1693
1694     } else if (sl->chip_id == STM32_CHIPID_F2 || sl->chip_id == STM32_CHIPID_F4) {
1695
1696         size_t count = size / sizeof(uint32_t);
1697         if (size % sizeof(uint32_t)) ++count;
1698
1699         /* setup core */
1700         stlink_write_reg(sl, fl->buf_addr, 0); /* source */
1701         stlink_write_reg(sl, target, 1); /* target */
1702         stlink_write_reg(sl, count, 2); /* count (32 bits words) */
1703         stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1704
1705     } else {
1706         fprintf(stderr, "unknown coreid 0x%x, don't know what flash loader to use\n", sl->core_id);
1707         return -1;
1708     }
1709
1710     /* run loader */
1711     stlink_run(sl);
1712
1713 #define WAIT_ROUNDS 1000
1714     /* wait until done (reaches breakpoint) */
1715     for (i = 0; i < WAIT_ROUNDS; i++) {
1716         if (is_core_halted(sl))
1717             break;
1718     }
1719
1720     if (i >= WAIT_ROUNDS) {
1721         fatal("flash loader run error\n");
1722         return -1;
1723     }
1724
1725     /* check written byte count */
1726     if (sl->chip_id == STM32_CHIPID_L1_MEDIUM) {
1727
1728       size_t count = size / sizeof(uint32_t);
1729       if (size % sizeof(uint32_t)) ++count;
1730
1731       stlink_read_reg(sl, 3, &rr);
1732       if (rr.r[3] != count) {
1733         fprintf(stderr, "write error, count == %u\n", rr.r[3]);
1734         return -1;
1735       }
1736
1737     } else if (sl->core_id == STM32VL_CORE_ID) {
1738
1739       stlink_read_reg(sl, 2, &rr);
1740       if (rr.r[2] != 0) {
1741         fprintf(stderr, "write error, count == %u\n", rr.r[2]);
1742         return -1;
1743       }
1744
1745     } else if (sl->chip_id == STM32_CHIPID_F2 || sl->chip_id == STM32_CHIPID_F4) {
1746
1747         stlink_read_reg(sl, 2, &rr);
1748         if (rr.r[2] != 0) {
1749             fprintf(stderr, "write error, count == %u\n", rr.r[2]);
1750             return -1;
1751         }
1752
1753     } else {
1754
1755       fprintf(stderr, "unknown coreid 0x%x, can't check written byte count\n", sl->core_id);
1756       return -1;
1757
1758     }
1759
1760     return 0;
1761 }