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