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

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