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

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