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