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