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     if (!ftruncate(fd, size - num_empty)) {
 940         error = -1;
 941     }
 942
 943     /* success */
 944     error = 0;
 945
 946 on_error:
 947     close(fd);
 948
 949     return error;
 950 }
 951
 952 int write_buffer_to_sram(stlink_t *sl, flash_loader_t* fl, const uint8_t* buf, size_t size) {
 953     /* write the buffer right after the loader */
 954     size_t chunk = size & ~0x3;
 955     size_t rem   = size & 0x3;
 956     if (chunk) {
 957         memcpy(sl->q_buf, buf, chunk);
 958         stlink_write_mem32(sl, fl->buf_addr, chunk);
 959     }
 960     if (rem) {
 961         memcpy(sl->q_buf, buf+chunk, rem);
 962         stlink_write_mem8(sl, (fl->buf_addr)+chunk, rem);
 963     }
 964     return 0;
 965 }
 966
 967 uint32_t calculate_F4_sectornum(uint32_t flashaddr){
 968     flashaddr &= ~STM32_FLASH_BASE;     //Page now holding the actual flash address
 969     if (flashaddr<0x4000) return (0);
 970     else if(flashaddr<0x8000) return(1);
 971     else if(flashaddr<0xc000) return(2);
 972     else if(flashaddr<0x10000) return(3);
 973     else if(flashaddr<0x20000) return(4);
 974     else return(flashaddr/0x20000)+4;
 975
 976 }
 977
 978 uint32_t stlink_calculate_pagesize(stlink_t *sl, uint32_t flashaddr){
 979         if((sl->chip_id == STM32_CHIPID_F2) ||(sl->chip_id == STM32_CHIPID_F4)) {
 980                 uint32_t sector=calculate_F4_sectornum(flashaddr);
 981                 if (sector<4) sl->flash_pgsz=0x4000;
 982                 else if(sector<5) sl->flash_pgsz=0x10000;
 983                 else sl->flash_pgsz=0x20000;
 984         }
 985         return (sl->flash_pgsz);
 986 }
 987
 988 /**
 989  * Erase a page of flash, assumes sl is fully populated with things like chip/core ids
 990  * @param sl stlink context
 991  * @param flashaddr an address in the flash page to erase
 992  * @return 0 on success -ve on failure
 993  */
 994 int stlink_erase_flash_page(stlink_t *sl, stm32_addr_t flashaddr)
 995 {
 996   if ((sl->chip_id == STM32_CHIPID_F2) ||(sl->chip_id == STM32_CHIPID_F4))
 997   {
 998     /* wait for ongoing op to finish */
 999     wait_flash_busy(sl);
1000
1001     /* unlock if locked */
1002     unlock_flash_if(sl);
1003
1004     /* select the page to erase */
1005     // calculate the actual page from the address
1006     uint32_t sector=calculate_F4_sectornum(flashaddr);
1007
1008     fprintf(stderr, "EraseFlash - Sector:0x%x Size:0x%x\n", sector, stlink_calculate_pagesize(sl, flashaddr));
1009     write_flash_cr_snb(sl, sector);
1010
1011     /* start erase operation */
1012     set_flash_cr_strt(sl);
1013
1014     /* wait for completion */
1015     wait_flash_busy(sl);
1016
1017     /* relock the flash */
1018     //todo: fails to program if this is in
1019     lock_flash(sl);
1020 #if DEBUG_FLASH
1021         fprintf(stdout, "Erase Final CR:0x%x\n", read_flash_cr(sl));
1022 #endif
1023   }
1024   else if (sl->chip_id == STM32_CHIPID_L1_MEDIUM)
1025   {
1026
1027     uint32_t val;
1028
1029     /* disable pecr protection */
1030     stlink_write_debug32(sl, STM32L_FLASH_PEKEYR, 0x89abcdef);
1031     stlink_write_debug32(sl, STM32L_FLASH_PEKEYR, 0x02030405);
1032
1033     /* check pecr.pelock is cleared */
1034     val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1035     if (val & (1 << 0))
1036     {
1037       WLOG("pecr.pelock not clear (%#x)\n", val);
1038       return -1;
1039     }
1040
1041     /* unlock program memory */
1042     stlink_write_debug32(sl, STM32L_FLASH_PRGKEYR, 0x8c9daebf);
1043     stlink_write_debug32(sl, STM32L_FLASH_PRGKEYR, 0x13141516);
1044
1045     /* check pecr.prglock is cleared */
1046     val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1047     if (val & (1 << 1))
1048     {
1049       WLOG("pecr.prglock not clear (%#x)\n", val);
1050       return -1;
1051     }
1052
1053     /* unused: unlock the option byte block */
1054 #if 0
1055     stlink_write_debug32(sl, STM32L_FLASH_OPTKEYR, 0xfbead9c8);
1056     stlink_write_debug32(sl, STM32L_FLASH_OPTKEYR, 0x24252627);
1057
1058     /* check pecr.optlock is cleared */
1059     val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1060     if (val & (1 << 2))
1061     {
1062       fprintf(stderr, "pecr.prglock not clear\n");
1063       return -1;
1064     }
1065 #endif
1066
1067     /* set pecr.{erase,prog} */
1068     val |= (1 << 9) | (1 << 3);
1069     stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1070
1071 #if 0 /* fix_to_be_confirmed */
1072
1073     /* wait for sr.busy to be cleared
1074        MP: Test shows that busy bit is not set here. Perhaps, PM0062 is
1075        wrong and we do not need to wait here for clearing the busy bit.
1076        TEXANE: ok, if experience says so and it works for you, we comment
1077        it. If someone has a problem, please drop an email.
1078      */
1079     while ((stlink_read_debug32(sl, STM32L_FLASH_SR) & (1 << 0)) != 0)
1080     {
1081     }
1082
1083 #endif /* fix_to_be_confirmed */
1084
1085     /* write 0 to the first word of the page to be erased */
1086     stlink_write_debug32(sl, flashaddr, 0);
1087
1088     /* MP: It is better to wait for clearing the busy bit after issuing
1089     page erase command, even though PM0062 recommends to wait before it.
1090     Test shows that a few iterations is performed in the following loop
1091     before busy bit is cleared.*/
1092     while ((stlink_read_debug32(sl, STM32L_FLASH_SR) & (1 << 0)) != 0)
1093     {
1094     }
1095
1096     /* reset lock bits */
1097     val = stlink_read_debug32(sl, STM32L_FLASH_PECR)
1098         | (1 << 0) | (1 << 1) | (1 << 2);
1099     stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1100   }
1101   else if (sl->core_id == STM32VL_CORE_ID)
1102   {
1103     /* wait for ongoing op to finish */
1104     wait_flash_busy(sl);
1105
1106     /* unlock if locked */
1107     unlock_flash_if(sl);
1108
1109     /* set the page erase bit */
1110     set_flash_cr_per(sl);
1111
1112     /* select the page to erase */
1113     write_flash_ar(sl, flashaddr);
1114
1115     /* start erase operation, reset by hw with bsy bit */
1116     set_flash_cr_strt(sl);
1117
1118     /* wait for completion */
1119     wait_flash_busy(sl);
1120
1121     /* relock the flash */
1122     lock_flash(sl);
1123   }
1124
1125   else {
1126     WLOG("unknown coreid: %x\n", sl->core_id);
1127     return -1;
1128   }
1129
1130   /* todo: verify the erased page */
1131
1132   return 0;
1133 }
1134
1135 int stlink_erase_flash_mass(stlink_t *sl) {
1136      if (sl->chip_id == STM32_CHIPID_L1_MEDIUM) {
1137          /* erase each page */
1138          int i = 0, num_pages = sl->flash_size/sl->flash_pgsz;
1139          for (i = 0; i < num_pages; i++) {
1140              /* addr must be an addr inside the page */
1141              stm32_addr_t addr = sl->flash_base + i * sl->flash_pgsz;
1142              if (stlink_erase_flash_page(sl, addr) == -1) {
1143                  WLOG("Failed to erase_flash_page(%#zx) == -1\n", addr);
1144                  return -1;
1145              }
1146              fprintf(stdout,"\rFlash page at %5d/%5d erased", i, num_pages);
1147              fflush(stdout);
1148          }
1149          fprintf(stdout, "\n");
1150      }
1151      else {
1152          /* wait for ongoing op to finish */
1153          wait_flash_busy(sl);
1154
1155          /* unlock if locked */
1156          unlock_flash_if(sl);
1157
1158          /* set the mass erase bit */
1159          set_flash_cr_mer(sl);
1160
1161          /* start erase operation, reset by hw with bsy bit */
1162          set_flash_cr_strt(sl);
1163
1164          /* wait for completion */
1165          wait_flash_busy_progress(sl);
1166
1167          /* relock the flash */
1168          lock_flash(sl);
1169
1170          /* todo: verify the erased memory */
1171      }
1172     return 0;
1173 }
1174
1175 int init_flash_loader(stlink_t *sl, flash_loader_t* fl) {
1176     size_t size;
1177
1178     /* allocate the loader in sram */
1179     if (write_loader_to_sram(sl, &fl->loader_addr, &size) == -1) {
1180         WLOG("Failed to write flash loader to sram!\n");
1181         return -1;
1182     }
1183
1184     /* allocate a one page buffer in sram right after loader */
1185     fl->buf_addr = fl->loader_addr + size;
1186     ILOG("Successfully loaded flash loader in sram\n");
1187     return 0;
1188 }
1189
1190 int write_loader_to_sram(stlink_t *sl, stm32_addr_t* addr, size_t* size) {
1191     /* from openocd, contrib/loaders/flash/stm32.s */
1192     static const uint8_t loader_code_stm32vl[] = {
1193         0x08, 0x4c, /* ldr      r4, STM32_FLASH_BASE */
1194         0x1c, 0x44, /* add      r4, r3 */
1195         /* write_half_word: */
1196         0x01, 0x23, /* movs     r3, #0x01 */
1197         0x23, 0x61, /* str      r3, [r4, #STM32_FLASH_CR_OFFSET] */
1198         0x30, 0xf8, 0x02, 0x3b, /* ldrh r3, [r0], #0x02 */
1199         0x21, 0xf8, 0x02, 0x3b, /* strh r3, [r1], #0x02 */
1200         /* busy: */
1201         0xe3, 0x68, /* ldr      r3, [r4, #STM32_FLASH_SR_OFFSET] */
1202         0x13, 0xf0, 0x01, 0x0f, /* tst  r3, #0x01 */
1203         0xfb, 0xd0, /* beq      busy */
1204         0x13, 0xf0, 0x14, 0x0f, /* tst  r3, #0x14 */
1205         0x01, 0xd1, /* bne      exit */
1206         0x01, 0x3a, /* subs     r2, r2, #0x01 */
1207         0xf0, 0xd1, /* bne      write_half_word */
1208         /* exit: */
1209         0x00, 0xbe, /* bkpt     #0x00 */
1210         0x00, 0x20, 0x02, 0x40, /* STM32_FLASH_BASE: .word 0x40022000 */
1211     };
1212
1213     static const uint8_t loader_code_stm32l[] = {
1214
1215       /* openocd.git/contrib/loaders/flash/stm32lx.S
1216          r0, input, dest addr
1217          r1, input, source addr
1218          r2, input, word count
1219          r3, output, word count
1220        */
1221
1222       0x00, 0x23,
1223       0x04, 0xe0,
1224
1225       0x51, 0xf8, 0x04, 0xcb,
1226       0x40, 0xf8, 0x04, 0xcb,
1227       0x01, 0x33,
1228
1229       0x93, 0x42,
1230       0xf8, 0xd3,
1231       0x00, 0xbe
1232     };
1233
1234         static const uint8_t loader_code_stm32f4[] = {
1235                 // flashloaders/stm32f4.s
1236
1237                 0x07, 0x4b,
1238
1239                 0x62, 0xb1,
1240                 0x04, 0x68,
1241                 0x0c, 0x60,
1242
1243                 0xdc, 0x89,
1244                 0x14, 0xf0, 0x01, 0x0f,
1245                 0xfb, 0xd1,
1246                 0x00, 0xf1, 0x04, 0x00,
1247                 0x01, 0xf1, 0x04, 0x01,
1248                 0xa2, 0xf1, 0x01, 0x02,
1249                 0xf1, 0xe7,
1250
1251                 0x00, 0xbe,
1252
1253                 0x00, 0x3c, 0x02, 0x40,
1254         };
1255
1256     const uint8_t* loader_code;
1257     size_t loader_size;
1258
1259     if (sl->chip_id == STM32_CHIPID_L1_MEDIUM) /* stm32l */
1260     {
1261       loader_code = loader_code_stm32l;
1262       loader_size = sizeof(loader_code_stm32l);
1263     }
1264     else if (sl->core_id == STM32VL_CORE_ID)
1265     {
1266       loader_code = loader_code_stm32vl;
1267       loader_size = sizeof(loader_code_stm32vl);
1268     }
1269         else if (sl->chip_id == STM32_CHIPID_F2 || sl->chip_id == STM32_CHIPID_F4)
1270         {
1271                 loader_code = loader_code_stm32f4;
1272                 loader_size = sizeof(loader_code_stm32f4);
1273         }
1274     else
1275     {
1276       WLOG("unknown coreid, not sure what flash loader to use, aborting!: %x\n", sl->core_id);
1277       return -1;
1278     }
1279
1280     memcpy(sl->q_buf, loader_code, loader_size);
1281     stlink_write_mem32(sl, sl->sram_base, loader_size);
1282
1283     *addr = sl->sram_base;
1284     *size = loader_size;
1285
1286     /* success */
1287     return 0;
1288 }
1289
1290 int stlink_fcheck_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
1291     /* check the contents of path are at addr */
1292
1293     int res;
1294     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1295
1296     if (map_file(&mf, path) == -1)
1297         return -1;
1298
1299     res = check_file(sl, &mf, addr);
1300
1301     unmap_file(&mf);
1302
1303     return res;
1304 }
1305
1306 /**
1307  * Verify addr..addr+len is binary identical to base...base+len
1308  * @param sl stlink context
1309  * @param address stm device address
1310  * @param data host side buffer to check against
1311  * @param length how much
1312  * @return 0 for success, -ve for failure
1313  */
1314 int stlink_verify_write_flash(stlink_t *sl, stm32_addr_t address, uint8_t *data, unsigned length) {
1315     size_t off;
1316     size_t cmp_size = (sl->flash_pgsz > 0x1800)? 0x1800:sl->flash_pgsz;
1317     ILOG("Starting verification of write complete\n");
1318     for (off = 0; off < length; off += cmp_size) {
1319         size_t aligned_size;
1320
1321         /* adjust last page size */
1322         if ((off + cmp_size) > length)
1323             cmp_size = length - off;
1324
1325         aligned_size = cmp_size;
1326         if (aligned_size & (4 - 1))
1327             aligned_size = (cmp_size + 4) & ~(4 - 1);
1328
1329         stlink_read_mem32(sl, address + off, aligned_size);
1330
1331         if (memcmp(sl->q_buf, data + off, cmp_size)) {
1332             WLOG("Verification of flash failed at offset: %zd\n", off);
1333             return -1;
1334         }
1335     }
1336     ILOG("Flash written and verified! jolly good!\n");
1337     return 0;
1338
1339 }
1340
1341 int stm32l1_write_half_pages(stlink_t *sl, stm32_addr_t addr, uint8_t* base, unsigned num_half_pages)
1342 {
1343     unsigned int count;
1344     uint32_t val;
1345     flash_loader_t fl;
1346
1347     ILOG("Starting Half page flash write for STM32L core id\n");
1348     /* flash loader initialization */
1349     if (init_flash_loader(sl, &fl) == -1) {
1350         WLOG("init_flash_loader() == -1\n");
1351         return -1;
1352     }
1353     /* Unlock already done */
1354     val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1355     val |= (1 << FLASH_L1_FPRG);
1356     stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1357
1358     val |= (1 << FLASH_L1_PROG);
1359     stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1360     while ((stlink_read_debug32(sl, STM32L_FLASH_SR) & (1 << 0)) != 0) {}
1361
1362 #define L1_WRITE_BLOCK_SIZE 0x80
1363     for (count = 0; count  < num_half_pages; count ++) {
1364         if (run_flash_loader(sl, &fl, addr + count * L1_WRITE_BLOCK_SIZE, base + count * L1_WRITE_BLOCK_SIZE, L1_WRITE_BLOCK_SIZE) == -1) {
1365             WLOG("l1_run_flash_loader(%#zx) failed! == -1\n", addr + count * L1_WRITE_BLOCK_SIZE);
1366             val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1367             val &= ~((1 << FLASH_L1_FPRG) |(1 << FLASH_L1_PROG));
1368             stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1369             return -1;
1370         }
1371         /* wait for sr.busy to be cleared */
1372         if (sl->verbose >= 1) {
1373             /* show progress. writing procedure is slow
1374                and previous errors are misleading */
1375             fprintf(stdout, "\r%3u/%u halfpages written", count + 1, num_half_pages);
1376             fflush(stdout);
1377         }
1378         while ((stlink_read_debug32(sl, STM32L_FLASH_SR) & (1 << 0)) != 0) {
1379         }
1380     }
1381     val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1382     val &= ~(1 << FLASH_L1_PROG);
1383     stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1384     val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1385     val &= ~(1 << FLASH_L1_FPRG);
1386     stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1387
1388     return 0;
1389 }
1390
1391 int stlink_write_flash(stlink_t *sl, stm32_addr_t addr, uint8_t* base, unsigned len) {
1392     size_t off;
1393     flash_loader_t fl;
1394     ILOG("Attempting to write %d (%#x) bytes to stm32 address: %u (%#x)\n",
1395         len, len, addr, addr);
1396     /* check addr range is inside the flash */
1397     stlink_calculate_pagesize(sl, addr);
1398     if (addr < sl->flash_base) {
1399         WLOG("addr too low %#x < %#x\n", addr, sl->flash_base);
1400         return -1;
1401     } else if ((addr + len) < addr) {
1402         WLOG("addr overruns\n");
1403         return -1;
1404     } else if ((addr + len) > (sl->flash_base + sl->flash_size)) {
1405         WLOG("addr too high\n");
1406         return -1;
1407     } else if ((addr & 1) || (len & 1)) {
1408         WLOG("unaligned addr or size\n");
1409         return -1;
1410     } else if (addr & (sl->flash_pgsz - 1)) {
1411         WLOG("addr not a multiple of pagesize, not supported\n");
1412         return -1;
1413     }
1414
1415     // Make sure we've loaded the context with the chip details
1416     stlink_core_id(sl);
1417     /* erase each page */
1418     int page_count = 0;
1419     for (off = 0; off < len; off += stlink_calculate_pagesize(sl, addr + off)) {
1420         /* addr must be an addr inside the page */
1421         if (stlink_erase_flash_page(sl, addr + off) == -1) {
1422             WLOG("Failed to erase_flash_page(%#zx) == -1\n", addr + off);
1423             return -1;
1424         }
1425         fprintf(stdout,"\rFlash page at addr: 0x%08lx erased",
1426                 (unsigned long)addr + off);
1427         fflush(stdout);
1428         page_count++;
1429     }
1430     fprintf(stdout,"\n");
1431     ILOG("Finished erasing %d pages of %d (%#x) bytes\n",
1432         page_count, sl->flash_pgsz, sl->flash_pgsz);
1433
1434     if ((sl->chip_id == STM32_CHIPID_F2) ||(sl->chip_id == STM32_CHIPID_F4)) {
1435         /* todo: check write operation */
1436
1437                 ILOG("Starting Flash write for F2/F4\n");
1438                 /* flash loader initialization */
1439                 if (init_flash_loader(sl, &fl) == -1) {
1440                         WLOG("init_flash_loader() == -1\n");
1441                         return -1;
1442                 }
1443
1444         /* First unlock the cr */
1445         unlock_flash_if(sl);
1446
1447         /* TODO: Check that Voltage range is 2.7 - 3.6 V */
1448         /* set parallelisim to 32 bit*/
1449         write_flash_cr_psiz(sl, 2);
1450
1451         /* set programming mode */
1452         set_flash_cr_pg(sl);
1453
1454                 for(off = 0; off < len;) {
1455                         size_t size = len - off > 0x8000 ? 0x8000 : len - off;
1456
1457                         printf("size: %zu\n", size);
1458
1459                         if (run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
1460                                 WLOG("run_flash_loader(%#zx) failed! == -1\n", addr + off);
1461                                 return -1;
1462                         }
1463
1464                         off += size;
1465                 }
1466
1467 #if 0
1468 #define PROGRESS_CHUNK_SIZE 0x1000
1469         /* write a word in program memory */
1470         for (off = 0; off < len; off += sizeof(uint32_t)) {
1471                 uint32_t data;
1472                 if (sl->verbose >= 1) {
1473                         if ((off & (PROGRESS_CHUNK_SIZE - 1)) == 0) {
1474                                 /* show progress. writing procedure is slow
1475                                            and previous errors are misleading */
1476                                 const uint32_t pgnum = (off / PROGRESS_CHUNK_SIZE)+1;
1477                                 const uint32_t pgcount = len / PROGRESS_CHUNK_SIZE +1;
1478                                 fprintf(stdout, "Writing %ukB chunk %u out of %u\n", PROGRESS_CHUNK_SIZE/1024, pgnum, pgcount);
1479                         }
1480                 }
1481
1482                 write_uint32((unsigned char*) &data, *(uint32_t*) (base + off));
1483                 stlink_write_debug32(sl, addr + off, data);
1484
1485                 /* wait for sr.busy to be cleared */
1486             wait_flash_busy(sl);
1487
1488         }
1489 #endif
1490         /* Relock flash */
1491         lock_flash(sl);
1492
1493 #if 0 /* todo: debug mode */
1494         fprintf(stdout, "Final CR:0x%x\n", read_flash_cr(sl));
1495 #endif
1496
1497
1498
1499     }   //STM32F4END
1500
1501     else if (sl->chip_id == STM32_CHIPID_L1_MEDIUM)    {
1502         /* use fast word write. todo: half page. */
1503         uint32_t val;
1504
1505 #if 0 /* todo: check write operation */
1506
1507         uint32_t nwrites = sl->flash_pgsz;
1508
1509         redo_write:
1510
1511 #endif /* todo: check write operation */
1512
1513         /* disable pecr protection */
1514         stlink_write_debug32(sl, STM32L_FLASH_PEKEYR, 0x89abcdef);
1515         stlink_write_debug32(sl, STM32L_FLASH_PEKEYR, 0x02030405);
1516
1517         /* check pecr.pelock is cleared */
1518         val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1519         if (val & (1 << 0)) {
1520                 fprintf(stderr, "pecr.pelock not clear\n");
1521                 return -1;
1522         }
1523
1524         /* unlock program memory */
1525         stlink_write_debug32(sl, STM32L_FLASH_PRGKEYR, 0x8c9daebf);
1526         stlink_write_debug32(sl, STM32L_FLASH_PRGKEYR, 0x13141516);
1527
1528         /* check pecr.prglock is cleared */
1529         val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1530         if (val & (1 << 1)) {
1531                 fprintf(stderr, "pecr.prglock not clear\n");
1532                 return -1;
1533         }
1534         off = 0;
1535         if (len > L1_WRITE_BLOCK_SIZE) {
1536             if (stm32l1_write_half_pages(sl, addr, base, len/L1_WRITE_BLOCK_SIZE) == -1){
1537                 /* This may happen on a blank device! */
1538                 WLOG("\nwrite_half_pages failed == -1\n");
1539             }
1540             else{
1541                 off = (len /L1_WRITE_BLOCK_SIZE)*L1_WRITE_BLOCK_SIZE;
1542             }
1543         }
1544
1545         /* write remainingword in program memory */
1546         for ( ; off < len; off += sizeof(uint32_t)) {
1547                 uint32_t data;
1548                 if (off > 254)
1549                     fprintf(stdout, "\r");
1550
1551                 if ((off % sl->flash_pgsz) > (sl->flash_pgsz -5)) {
1552                     fprintf(stdout, "\r%3zd/%3zd pages written",
1553                             off/sl->flash_pgsz, len/sl->flash_pgsz);
1554                     fflush(stdout);
1555                 }
1556
1557                 write_uint32((unsigned char*) &data, *(uint32_t*) (base + off));
1558                 stlink_write_debug32(sl, addr + off, data);
1559
1560                 /* wait for sr.busy to be cleared */
1561                 while ((stlink_read_debug32(sl, STM32L_FLASH_SR) & (1 << 0)) != 0) {
1562                 }
1563
1564 #if 0 /* todo: check redo write operation */
1565
1566                 /* check written bytes. todo: should be on a per page basis. */
1567                 data = stlink_read_debug32(sl, addr + off);
1568                 if (data == *(uint32_t*)(base + off)) {
1569                         /* re erase the page and redo the write operation */
1570                         uint32_t page;
1571                         uint32_t val;
1572
1573                         /* fail if successive write count too low */
1574                         if (nwrites < sl->flash_pgsz) {
1575                                 fprintf(stderr, "writes operation failure count too high, aborting\n");
1576                                 return -1;
1577                         }
1578
1579                         nwrites = 0;
1580
1581                         /* assume addr aligned */
1582                         if (off % sl->flash_pgsz) off &= ~(sl->flash_pgsz - 1);
1583                         page = addr + off;
1584
1585                         fprintf(stderr, "invalid write @0x%x(0x%x): 0x%x != 0x%x. retrying.\n",
1586                                         page, addr + off, read_uint32(base + off, 0), read_uint32(sl->q_buf, 0));
1587
1588                         /* reset lock bits */
1589                         val = stlink_read_debug32(sl, STM32L_FLASH_PECR)
1590                              | (1 << 0) | (1 << 1) | (1 << 2);
1591                         stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1592
1593                         stlink_erase_flash_page(sl, page);
1594
1595                         goto redo_write;
1596                 }
1597
1598                 /* increment successive writes counter */
1599                 ++nwrites;
1600
1601 #endif /* todo: check redo write operation */
1602         }
1603         fprintf(stdout, "\n");
1604         /* reset lock bits */
1605         val = stlink_read_debug32(sl, STM32L_FLASH_PECR)
1606              | (1 << 0) | (1 << 1) | (1 << 2);
1607         stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1608     } else if (sl->core_id == STM32VL_CORE_ID) {
1609         ILOG("Starting Flash write for VL core id\n");
1610         /* flash loader initialization */
1611         if (init_flash_loader(sl, &fl) == -1) {
1612             WLOG("init_flash_loader() == -1\n");
1613             return -1;
1614         }
1615
1616         int write_block_count = 0;
1617         for (off = 0; off < len; off += sl->flash_pgsz) {
1618             /* adjust last write size */
1619             size_t size = sl->flash_pgsz;
1620             if ((off + sl->flash_pgsz) > len) size = len - off;
1621
1622             /* unlock and set programming mode */
1623             unlock_flash_if(sl);
1624             set_flash_cr_pg(sl);
1625             //DLOG("Finished setting flash cr pg, running loader!\n");
1626             if (run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
1627                 WLOG("run_flash_loader(%#zx) failed! == -1\n", addr + off);
1628                 return -1;
1629             }
1630             lock_flash(sl);
1631             if (sl->verbose >= 1) {
1632                 /* show progress. writing procedure is slow
1633                    and previous errors are misleading */
1634               fprintf(stdout, "\r%3u/%lu pages written", write_block_count++, (unsigned long)len/sl->flash_pgsz);
1635                 fflush(stdout);
1636             }
1637         }
1638         fprintf(stdout, "\n");
1639     } else {
1640         WLOG("unknown coreid, not sure how to write: %x\n", sl->core_id);
1641         return -1;
1642     }
1643
1644     return stlink_verify_write_flash(sl, addr, base, len);
1645 }
1646
1647 /**
1648  * Write the given binary file into flash at address "addr"
1649  * @param sl
1650  * @param path readable file path, should be binary image
1651  * @param addr where to start writing
1652  * @return 0 on success, -ve on failure.
1653  */
1654 int stlink_fwrite_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
1655     /* write the file in flash at addr */
1656     int err;
1657     unsigned int num_empty = 0, index;
1658     unsigned char erased_pattern =(sl->chip_id == STM32_CHIPID_L1_MEDIUM)?0:0xff;
1659     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1660     if (map_file(&mf, path) == -1) {
1661         WLOG("map_file() == -1\n");
1662         return -1;
1663     }
1664     for(index = 0; index < mf.len; index ++) {
1665         if (mf.base[index] == erased_pattern)
1666             num_empty ++;
1667         else
1668             num_empty = 0;
1669     }
1670     if(num_empty != 0) {
1671         ILOG("Ignoring %d bytes of Zeros at end of file\n",num_empty);
1672         mf.len -= num_empty;
1673     }
1674     err = stlink_write_flash(sl, addr, mf.base, mf.len);
1675     /* set stack*/
1676     stlink_write_reg(sl, stlink_read_debug32(sl, addr    ),13);
1677     /* Set PC to the reset routine*/
1678     stlink_write_reg(sl, stlink_read_debug32(sl, addr + 4),15);
1679     stlink_run(sl);
1680     unmap_file(&mf);
1681     return err;
1682 }
1683
1684 int run_flash_loader(stlink_t *sl, flash_loader_t* fl, stm32_addr_t target, const uint8_t* buf, size_t size) {
1685
1686     reg rr;
1687     int i = 0;
1688     DLOG("Running flash loader, write address:%#x, size: %zd\n", target, size);
1689     // FIXME This can never return -1
1690     if (write_buffer_to_sram(sl, fl, buf, size) == -1) {
1691         // IMPOSSIBLE!
1692         WLOG("write_buffer_to_sram() == -1\n");
1693         return -1;
1694     }
1695
1696     if (sl->chip_id == STM32_CHIPID_L1_MEDIUM) {
1697
1698       size_t count = size / sizeof(uint32_t);
1699       if (size % sizeof(uint32_t)) ++count;
1700
1701       /* setup core */
1702       stlink_write_reg(sl, target, 0); /* target */
1703       stlink_write_reg(sl, fl->buf_addr, 1); /* source */
1704       stlink_write_reg(sl, count, 2); /* count (32 bits words) */
1705       stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1706
1707     } else if (sl->core_id == STM32VL_CORE_ID) {
1708
1709       size_t count = size / sizeof(uint16_t);
1710       if (size % sizeof(uint16_t)) ++count;
1711
1712       /* setup core */
1713       stlink_write_reg(sl, fl->buf_addr, 0); /* source */
1714       stlink_write_reg(sl, target, 1); /* target */
1715       stlink_write_reg(sl, count, 2); /* count (16 bits half words) */
1716       stlink_write_reg(sl, 0, 3); /* flash bank 0 (input) */
1717       stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1718
1719         } else if (sl->chip_id == STM32_CHIPID_F2 || sl->chip_id == STM32_CHIPID_F4) {
1720
1721                 size_t count = size / sizeof(uint32_t);
1722                 if (size % sizeof(uint32_t)) ++count;
1723
1724                 /* setup core */
1725                 stlink_write_reg(sl, fl->buf_addr, 0); /* source */
1726                 stlink_write_reg(sl, target, 1); /* target */
1727                 stlink_write_reg(sl, count, 2); /* count (32 bits words) */
1728                 stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1729
1730     } else {
1731       fprintf(stderr, "unknown coreid: 0x%x\n", sl->core_id);
1732       return -1;
1733     }
1734
1735     /* run loader */
1736     stlink_run(sl);
1737
1738     /* wait until done (reaches breakpoint) */
1739     while ((is_core_halted(sl) == 0) && (i <1000))
1740     {
1741         i++;
1742     }
1743
1744     if ( i > 999) {
1745         fprintf(stderr, "run error\n");
1746         return -1;
1747     }
1748
1749     /* check written byte count */
1750     if (sl->chip_id == STM32_CHIPID_L1_MEDIUM) {
1751
1752       size_t count = size / sizeof(uint32_t);
1753       if (size % sizeof(uint32_t)) ++count;
1754
1755       stlink_read_reg(sl, 3, &rr);
1756       if (rr.r[3] != count) {
1757         fprintf(stderr, "write error, count == %u\n", rr.r[3]);
1758         return -1;
1759       }
1760
1761     } else if (sl->core_id == STM32VL_CORE_ID) {
1762
1763       stlink_read_reg(sl, 2, &rr);
1764       if (rr.r[2] != 0) {
1765         fprintf(stderr, "write error, count == %u\n", rr.r[2]);
1766         return -1;
1767       }
1768
1769         } else if (sl->chip_id == STM32_CHIPID_F2 || sl->chip_id == STM32_CHIPID_F4) {
1770
1771                 stlink_read_reg(sl, 2, &rr);
1772                 if (rr.r[2] != 0) {
1773                         fprintf(stderr, "write error, count == %u\n", rr.r[2]);
1774                         return -1;
1775                 }
1776
1777     } else {
1778
1779       fprintf(stderr, "unknown coreid: 0x%x\n", sl->core_id);
1780       return -1;
1781
1782     }
1783
1784     return 0;
1785 }