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