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