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