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

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