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