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 + 0x1c)
  63 #define STM32L_FLASH_WRPR (STM32L_FLASH_REGS_ADDR + 0x20)
  64 #define FLASH_L1_FPRG 10
  65 #define FLASH_L1_PROG 3
  66
  67
  68 //STM32F4
  69 #define FLASH_F4_REGS_ADDR ((uint32_t)0x40023c00)
  70 #define FLASH_F4_KEYR (FLASH_F4_REGS_ADDR + 0x04)
  71 #define FLASH_F4_OPT_KEYR (FLASH_F4_REGS_ADDR + 0x08)
  72 #define FLASH_F4_SR (FLASH_F4_REGS_ADDR + 0x0c)
  73 #define FLASH_F4_CR (FLASH_F4_REGS_ADDR + 0x10)
  74 #define FLASH_F4_OPT_CR (FLASH_F4_REGS_ADDR + 0x14)
  75 #define FLASH_F4_CR_STRT 16
  76 #define FLASH_F4_CR_LOCK 31
  77 #define FLASH_F4_CR_SER 1
  78 #define FLASH_F4_CR_SNB 3
  79 #define FLASH_F4_CR_SNB_MASK 0x38
  80 #define FLASH_F4_SR_BSY 16
  81
  82
  83 void write_uint32(unsigned char* buf, uint32_t ui) {
  84     if (!is_bigendian()) { // le -> le (don't swap)
  85         buf[0] = ((unsigned char*) &ui)[0];
  86         buf[1] = ((unsigned char*) &ui)[1];
  87         buf[2] = ((unsigned char*) &ui)[2];
  88         buf[3] = ((unsigned char*) &ui)[3];
  89     } else {
  90         buf[0] = ((unsigned char*) &ui)[3];
  91         buf[1] = ((unsigned char*) &ui)[2];
  92         buf[2] = ((unsigned char*) &ui)[1];
  93         buf[3] = ((unsigned char*) &ui)[0];
  94     }
  95 }
  96
  97 void write_uint16(unsigned char* buf, uint16_t ui) {
  98     if (!is_bigendian()) { // le -> le (don't swap)
  99         buf[0] = ((unsigned char*) &ui)[0];
 100         buf[1] = ((unsigned char*) &ui)[1];
 101     } else {
 102         buf[0] = ((unsigned char*) &ui)[1];
 103         buf[1] = ((unsigned char*) &ui)[0];
 104     }
 105 }
 106
 107 uint32_t read_uint32(const unsigned char *c, const int pt) {
 108     uint32_t ui;
 109     char *p = (char *) &ui;
 110
 111     if (!is_bigendian()) { // le -> le (don't swap)
 112         p[0] = c[pt + 0];
 113         p[1] = c[pt + 1];
 114         p[2] = c[pt + 2];
 115         p[3] = c[pt + 3];
 116     } else {
 117         p[0] = c[pt + 3];
 118         p[1] = c[pt + 2];
 119         p[2] = c[pt + 1];
 120         p[3] = c[pt + 0];
 121     }
 122     return ui;
 123 }
 124
 125 static uint32_t __attribute__((unused)) read_flash_rdp(stlink_t *sl) {
 126     return stlink_read_debug32(sl, FLASH_WRPR) & 0xff;
 127 }
 128
 129 static inline uint32_t read_flash_wrpr(stlink_t *sl) {
 130     return stlink_read_debug32(sl, FLASH_WRPR);
 131 }
 132
 133 static inline uint32_t read_flash_obr(stlink_t *sl) {
 134     return stlink_read_debug32(sl, FLASH_OBR);
 135 }
 136
 137 static inline uint32_t read_flash_cr(stlink_t *sl) {
 138         uint32_t res;
 139         if(sl->chip_id==STM32F4_CHIP_ID)
 140                 res = stlink_read_debug32(sl, FLASH_F4_CR);
 141         else
 142                 res = stlink_read_debug32(sl, FLASH_CR);
 143 #if DEBUG_FLASH
 144         fprintf(stdout, "CR:0x%x\n", res);
 145 #endif
 146         return res;
 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         stlink_write_debug32(sl, FLASH_F4_KEYR, FLASH_KEY1);
 165                 stlink_write_debug32(sl, FLASH_F4_KEYR, FLASH_KEY2);
 166     }
 167         else {
 168         stlink_write_debug32(sl, FLASH_KEYR, FLASH_KEY1);
 169                 stlink_write_debug32(sl, FLASH_KEYR, FLASH_KEY2);
 170         }
 171
 172 }
 173
 174 static int unlock_flash_if(stlink_t *sl) {
 175     /* unlock flash if already locked */
 176
 177     if (is_flash_locked(sl)) {
 178         unlock_flash(sl);
 179         if (is_flash_locked(sl)) {
 180             WLOG("Failed to unlock flash!\n");
 181             return -1;
 182         }
 183     }
 184     DLOG("Successfully unlocked flash\n");
 185     return 0;
 186 }
 187
 188 static void lock_flash(stlink_t *sl) {
 189     if(sl->chip_id==STM32F4_CHIP_ID) {
 190         const uint32_t n = read_flash_cr(sl) | (1 << FLASH_F4_CR_LOCK);
 191         stlink_write_debug32(sl, FLASH_F4_CR, n);
 192     }
 193     else {
 194         /* write to 1 only. reset by hw at unlock sequence */
 195         const uint32_t n = read_flash_cr(sl) | (1 << FLASH_CR_LOCK);
 196         stlink_write_debug32(sl, FLASH_CR, n);
 197     }
 198 }
 199
 200
 201 static void set_flash_cr_pg(stlink_t *sl) {
 202     if(sl->chip_id==STM32F4_CHIP_ID) {
 203                 uint32_t x = read_flash_cr(sl);
 204                 x |= (1 << FLASH_CR_PG);
 205         stlink_write_debug32(sl, FLASH_F4_CR, x);
 206     }
 207     else {
 208         const uint32_t n = 1 << FLASH_CR_PG;
 209         stlink_write_debug32(sl, FLASH_CR, n);
 210     }
 211 }
 212
 213 static void __attribute__((unused)) clear_flash_cr_pg(stlink_t *sl) {
 214     const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_PG);
 215     if(sl->chip_id==STM32F4_CHIP_ID)
 216         stlink_write_debug32(sl, FLASH_F4_CR, n);
 217     else
 218         stlink_write_debug32(sl, FLASH_CR, n);
 219 }
 220
 221 static void set_flash_cr_per(stlink_t *sl) {
 222     const uint32_t n = 1 << FLASH_CR_PER;
 223     stlink_write_debug32(sl, FLASH_CR, n);
 224 }
 225
 226 static void __attribute__((unused)) clear_flash_cr_per(stlink_t *sl) {
 227     const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_PER);
 228     stlink_write_debug32(sl, FLASH_CR, n);
 229 }
 230
 231 static void set_flash_cr_mer(stlink_t *sl) {
 232     if(sl->chip_id == STM32F4_CHIP_ID)
 233         stlink_write_debug32(sl, FLASH_F4_CR,
 234                              stlink_read_debug32(sl, FLASH_F4_CR) | (1 << FLASH_CR_MER));
 235     else
 236         stlink_write_debug32(sl, FLASH_CR,
 237                              stlink_read_debug32(sl, FLASH_CR) | (1 << FLASH_CR_MER));
 238 }
 239
 240 static void __attribute__((unused)) clear_flash_cr_mer(stlink_t *sl) {
 241     if(sl->chip_id == STM32F4_CHIP_ID)
 242         stlink_write_debug32(sl, FLASH_F4_CR,
 243                              stlink_read_debug32(sl, FLASH_F4_CR) & ~(1 << FLASH_CR_MER));
 244     else
 245         stlink_write_debug32(sl, FLASH_CR,
 246                              stlink_read_debug32(sl, FLASH_CR) & ~(1 << FLASH_CR_MER));
 247 }
 248
 249 static void set_flash_cr_strt(stlink_t *sl) {
 250         if(sl->chip_id == STM32F4_CHIP_ID)
 251         {
 252                 uint32_t x = read_flash_cr(sl);
 253                 x |= (1 << FLASH_F4_CR_STRT);
 254                 stlink_write_debug32(sl, FLASH_F4_CR, x);
 255         }
 256         else {
 257             stlink_write_debug32(
 258                 sl, FLASH_CR,
 259                 stlink_read_debug32(sl,FLASH_CR) |(1 << FLASH_CR_STRT) );
 260         }
 261 }
 262
 263 static inline uint32_t read_flash_acr(stlink_t *sl) {
 264     return stlink_read_debug32(sl, FLASH_ACR);
 265 }
 266
 267 static inline uint32_t read_flash_sr(stlink_t *sl) {
 268         uint32_t res;
 269         if(sl->chip_id==STM32F4_CHIP_ID)
 270                 res = stlink_read_debug32(sl, FLASH_F4_SR);
 271         else
 272                 res = stlink_read_debug32(sl, FLASH_SR);
 273     //fprintf(stdout, "SR:0x%x\n", *(uint32_t*) sl->q_buf);
 274     return res;
 275 }
 276
 277 static inline unsigned int is_flash_busy(stlink_t *sl) {
 278         if(sl->chip_id==STM32F4_CHIP_ID)
 279                 return read_flash_sr(sl) & (1 << FLASH_F4_SR_BSY);
 280         else
 281                 return read_flash_sr(sl) & (1 << FLASH_SR_BSY);
 282 }
 283
 284 static void wait_flash_busy(stlink_t *sl) {
 285     /* todo: add some delays here */
 286     while (is_flash_busy(sl))
 287         ;
 288 }
 289
 290 static void wait_flash_busy_progress(stlink_t *sl) {
 291     int i = 0;
 292     fprintf(stdout, "Mass erasing");
 293     fflush(stdout);
 294     while (is_flash_busy(sl))
 295     {
 296         usleep(10000);
 297         i++;
 298         if (i % 100 == 0) {
 299             fprintf(stdout, ".");
 300             fflush(stdout);
 301         }
 302     }
 303     fprintf(stdout, "\n");
 304 }
 305
 306 static inline unsigned int is_flash_eop(stlink_t *sl) {
 307     return read_flash_sr(sl) & (1 << FLASH_SR_EOP);
 308 }
 309
 310 static void __attribute__((unused)) clear_flash_sr_eop(stlink_t *sl) {
 311     const uint32_t n = read_flash_sr(sl) & ~(1 << FLASH_SR_EOP);
 312     stlink_write_debug32(sl, FLASH_SR, n);
 313 }
 314
 315 static void __attribute__((unused)) wait_flash_eop(stlink_t *sl) {
 316     /* todo: add some delays here */
 317     while (is_flash_eop(sl) == 0)
 318         ;
 319 }
 320
 321 static inline void write_flash_ar(stlink_t *sl, uint32_t n) {
 322     stlink_write_debug32(sl, FLASH_AR, n);
 323 }
 324
 325 static inline void write_flash_cr_psiz(stlink_t *sl, uint32_t n) {
 326     uint32_t x = read_flash_cr(sl);
 327     x &= ~(0x03 << 8);
 328     x |= (n << 8);
 329 #if DEBUG_FLASH
 330     fprintf(stdout, "PSIZ:0x%x 0x%x\n", x, n);
 331 #endif
 332     stlink_write_debug32(sl, FLASH_F4_CR, x);
 333 }
 334
 335
 336 static inline void write_flash_cr_snb(stlink_t *sl, uint32_t n) {
 337     uint32_t x = read_flash_cr(sl);
 338     x &= ~FLASH_F4_CR_SNB_MASK;
 339     x |= (n << FLASH_F4_CR_SNB);
 340     x |= (1 << FLASH_F4_CR_SER);
 341 #if DEBUG_FLASH
 342     fprintf(stdout, "SNB:0x%x 0x%x\n", x, n);
 343 #endif
 344     stlink_write_debug32(sl, FLASH_F4_CR, x);
 345 }
 346
 347 #if 0 /* todo */
 348
 349 static void disable_flash_read_protection(stlink_t *sl) {
 350     /* erase the option byte area */
 351     /* rdp = 0x00a5; */
 352     /* reset */
 353 }
 354 #endif /* todo */
 355
 356
 357 // Delegates to the backends...
 358
 359 void stlink_close(stlink_t *sl) {
 360     DLOG("*** stlink_close ***\n");
 361     sl->backend->close(sl);
 362     free(sl);
 363 }
 364
 365 void stlink_exit_debug_mode(stlink_t *sl) {
 366     DLOG("*** stlink_exit_debug_mode ***\n");
 367     stlink_write_debug32(sl, DHCSR, DBGKEY);
 368     sl->backend->exit_debug_mode(sl);
 369 }
 370
 371 void stlink_enter_swd_mode(stlink_t *sl) {
 372     DLOG("*** stlink_enter_swd_mode ***\n");
 373     sl->backend->enter_swd_mode(sl);
 374 }
 375
 376 // Force the core into the debug mode -> halted state.
 377 void stlink_force_debug(stlink_t *sl) {
 378     DLOG("*** stlink_force_debug_mode ***\n");
 379     sl->backend->force_debug(sl);
 380 }
 381
 382 void stlink_exit_dfu_mode(stlink_t *sl) {
 383     DLOG("*** stlink_exit_dfu_mode ***\n");
 384     sl->backend->exit_dfu_mode(sl);
 385 }
 386
 387 uint32_t stlink_core_id(stlink_t *sl) {
 388     DLOG("*** stlink_core_id ***\n");
 389     sl->backend->core_id(sl);
 390     if (sl->verbose > 2)
 391         stlink_print_data(sl);
 392     DLOG("core_id = 0x%08x\n", sl->core_id);
 393     return sl->core_id;
 394 }
 395
 396 uint32_t stlink_chip_id(stlink_t *sl) {
 397     uint32_t chip_id = stlink_read_debug32(sl, 0xE0042000);
 398     return chip_id;
 399 }
 400
 401 /**
 402  * Cortex m3 tech ref manual, CPUID register description
 403  * @param sl stlink context
 404  * @param cpuid pointer to the result object
 405  */
 406 void stlink_cpu_id(stlink_t *sl, cortex_m3_cpuid_t *cpuid) {
 407     uint32_t raw = stlink_read_debug32(sl, CM3_REG_CPUID);
 408     cpuid->implementer_id = (raw >> 24) & 0x7f;
 409     cpuid->variant = (raw >> 20) & 0xf;
 410     cpuid->part = (raw >> 4) & 0xfff;
 411     cpuid->revision = raw & 0xf;
 412     return;
 413 }
 414
 415 /**
 416  * reads and decodes the flash parameters, as dynamically as possible
 417  * @param sl
 418  * @return 0 for success, or -1 for unsupported core type.
 419  */
 420 int stlink_load_device_params(stlink_t *sl) {
 421     ILOG("Loading device parameters....\n");
 422     const chip_params_t *params = NULL;
 423
 424     sl->core_id = stlink_core_id(sl);
 425     uint32_t chip_id = stlink_chip_id(sl);
 426
 427     /* Fix chip_id for F4 rev A errata */
 428     if (((chip_id & 0xFFF) == 0x411) && (sl->core_id == CORE_M4_R0)) {
 429       chip_id = 0x413;
 430     }
 431
 432     sl->chip_id = chip_id & 0xfff;
 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 = 0; // FIXME - need to work this out some other way, just set to max possible?
 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 == STM32F4_CHIP_ID) {
 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 == STM32F4_CHIP_ID)
 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->core_id == STM32L_CORE_ID)
 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->core_id == STM32L_CORE_ID) /* 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     if (sl->chip_id == STM32_CHIPID_F4) {
1240         DLOG("(FIXME)Skipping verification for F4, not enough ram (yet)\n");
1241         return 0;
1242     }
1243     ILOG("Starting verification of write complete\n");
1244     for (off = 0; off < length; off += sl->flash_pgsz) {
1245         size_t aligned_size;
1246
1247         /* adjust last page size */
1248         size_t cmp_size = sl->flash_pgsz;
1249         if ((off + sl->flash_pgsz) > length)
1250             cmp_size = length - off;
1251
1252         aligned_size = cmp_size;
1253         if (aligned_size & (4 - 1))
1254             aligned_size = (cmp_size + 4) & ~(4 - 1);
1255
1256         stlink_read_mem32(sl, address + off, aligned_size);
1257
1258         if (memcmp(sl->q_buf, data + off, cmp_size)) {
1259             WLOG("Verification of flash failed at offset: %zd\n", off);
1260             return -1;
1261         }
1262     }
1263     ILOG("Flash written and verified! jolly good!\n");
1264     return 0;
1265
1266 }
1267
1268 int stm32l1_write_half_pages(stlink_t *sl, stm32_addr_t addr, uint8_t* base, unsigned num_half_pages)
1269 {
1270     unsigned int count;
1271     uint32_t val;
1272     flash_loader_t fl;
1273
1274     ILOG("Starting Half page flash write for STM32L core id\n");
1275     /* flash loader initialization */
1276     if (init_flash_loader(sl, &fl) == -1) {
1277         WLOG("init_flash_loader() == -1\n");
1278         return -1;
1279     }
1280     /* Unlock already done */
1281     val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1282     val |= (1 << FLASH_L1_FPRG);
1283     stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1284
1285     val |= (1 << FLASH_L1_PROG);
1286     stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1287     while ((stlink_read_debug32(sl, STM32L_FLASH_SR) & (1 << 0)) != 0) {}
1288
1289 #define L1_WRITE_BLOCK_SIZE 0x80
1290     for (count = 0; count  < num_half_pages; count ++) {
1291         if (run_flash_loader(sl, &fl, addr + count * L1_WRITE_BLOCK_SIZE, base + count * L1_WRITE_BLOCK_SIZE, L1_WRITE_BLOCK_SIZE) == -1) {
1292             WLOG("l1_run_flash_loader(%#zx) failed! == -1\n", addr + count * L1_WRITE_BLOCK_SIZE);
1293             val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1294             val &= ~((1 << FLASH_L1_FPRG) |(1 << FLASH_L1_PROG));
1295             stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1296             return -1;
1297         }
1298         /* wait for sr.busy to be cleared */
1299         if (sl->verbose >= 1) {
1300             /* show progress. writing procedure is slow
1301                and previous errors are misleading */
1302             fprintf(stdout, "\r%3u/%u halfpages written", count, num_half_pages);
1303             fflush(stdout);
1304         }
1305         while ((stlink_read_debug32(sl, STM32L_FLASH_SR) & (1 << 0)) != 0) {
1306         }
1307     }
1308     val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1309     val &= ~(1 << FLASH_L1_PROG);
1310     stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1311     val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1312     val &= ~(1 << FLASH_L1_FPRG);
1313     stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1314
1315     return 0;
1316 }
1317
1318 int stlink_write_flash(stlink_t *sl, stm32_addr_t addr, uint8_t* base, unsigned len) {
1319     size_t off;
1320     flash_loader_t fl;
1321     ILOG("Attempting to write %d (%#x) bytes to stm32 address: %u (%#x)\n",
1322         len, len, addr, addr);
1323     /* check addr range is inside the flash */
1324     stlink_calculate_pagesize(sl, addr);
1325     if (addr < sl->flash_base) {
1326         WLOG("addr too low %#x < %#x\n", addr, sl->flash_base);
1327         return -1;
1328     } else if ((addr + len) < addr) {
1329         WLOG("addr overruns\n");
1330         return -1;
1331     } else if ((addr + len) > (sl->flash_base + sl->flash_size)) {
1332         WLOG("addr too high\n");
1333         return -1;
1334     } else if ((addr & 1) || (len & 1)) {
1335         WLOG("unaligned addr or size\n");
1336         return -1;
1337     } else if (addr & (sl->flash_pgsz - 1)) {
1338         WLOG("addr not a multiple of pagesize, not supported\n");
1339         return -1;
1340     }
1341
1342     // Make sure we've loaded the context with the chip details
1343     stlink_core_id(sl);
1344     /* erase each page */
1345     int page_count = 0;
1346     for (off = 0; off < len; off += stlink_calculate_pagesize(sl, addr + off)) {
1347         /* addr must be an addr inside the page */
1348         if (stlink_erase_flash_page(sl, addr + off) == -1) {
1349             WLOG("Failed to erase_flash_page(%#zx) == -1\n", addr + off);
1350             return -1;
1351         }
1352         fprintf(stdout,"\rFlash page at addr: 0x%08lx erased",
1353                 (unsigned long)addr + off);
1354         fflush(stdout);
1355         page_count++;
1356     }
1357     fprintf(stdout,"\n");
1358     ILOG("Finished erasing %d pages of %d (%#x) bytes\n",
1359         page_count, sl->flash_pgsz, sl->flash_pgsz);
1360
1361     if (sl->chip_id == STM32F4_CHIP_ID) {
1362         /* todo: check write operation */
1363
1364         /* First unlock the cr */
1365         unlock_flash_if(sl);
1366
1367         /* TODO: Check that Voltage range is 2.7 - 3.6 V */
1368         /* set parallelisim to 32 bit*/
1369         write_flash_cr_psiz(sl, 2);
1370
1371         /* set programming mode */
1372         set_flash_cr_pg(sl);
1373
1374 #define PROGRESS_CHUNK_SIZE 0x1000
1375         /* write a word in program memory */
1376         for (off = 0; off < len; off += sizeof(uint32_t)) {
1377                 uint32_t data;
1378                 if (sl->verbose >= 1) {
1379                         if ((off & (PROGRESS_CHUNK_SIZE - 1)) == 0) {
1380                                 /* show progress. writing procedure is slow
1381                                            and previous errors are misleading */
1382                                 const uint32_t pgnum = (off / PROGRESS_CHUNK_SIZE)+1;
1383                                 const uint32_t pgcount = len / PROGRESS_CHUNK_SIZE;
1384                                 fprintf(stdout, "Writing %ukB chunk %u out of %u\n", PROGRESS_CHUNK_SIZE/1024, pgnum, pgcount);
1385                         }
1386                 }
1387
1388                 write_uint32((unsigned char*) &data, *(uint32_t*) (base + off));
1389                 stlink_write_debug32(sl, addr + off, data);
1390
1391                 /* wait for sr.busy to be cleared */
1392             wait_flash_busy(sl);
1393
1394         }
1395         /* Relock flash */
1396         lock_flash(sl);
1397
1398 #if 0 /* todo: debug mode */
1399         fprintf(stdout, "Final CR:0x%x\n", read_flash_cr(sl));
1400 #endif
1401
1402
1403
1404     }   //STM32F4END
1405
1406     else if (sl->core_id == STM32L_CORE_ID)    {
1407         /* use fast word write. todo: half page. */
1408         uint32_t val;
1409
1410 #if 0 /* todo: check write operation */
1411
1412         uint32_t nwrites = sl->flash_pgsz;
1413
1414         redo_write:
1415
1416 #endif /* todo: check write operation */
1417
1418         /* disable pecr protection */
1419         stlink_write_debug32(sl, STM32L_FLASH_PEKEYR, 0x89abcdef);
1420         stlink_write_debug32(sl, STM32L_FLASH_PEKEYR, 0x02030405);
1421
1422         /* check pecr.pelock is cleared */
1423         val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1424         if (val & (1 << 0)) {
1425                 fprintf(stderr, "pecr.pelock not clear\n");
1426                 return -1;
1427         }
1428
1429         /* unlock program memory */
1430         stlink_write_debug32(sl, STM32L_FLASH_PRGKEYR, 0x8c9daebf);
1431         stlink_write_debug32(sl, STM32L_FLASH_PRGKEYR, 0x13141516);
1432
1433         /* check pecr.prglock is cleared */
1434         val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1435         if (val & (1 << 1)) {
1436                 fprintf(stderr, "pecr.prglock not clear\n");
1437                 return -1;
1438         }
1439         off = 0;
1440         if (len > L1_WRITE_BLOCK_SIZE) {
1441             if (stm32l1_write_half_pages(sl, addr, base, len/L1_WRITE_BLOCK_SIZE) == -1){
1442                 /* This may happen on a blank device! */
1443                 WLOG("\nwrite_half_pages failed == -1\n");
1444             }
1445             else{
1446                 off = (len /L1_WRITE_BLOCK_SIZE)*L1_WRITE_BLOCK_SIZE;
1447             }
1448         }
1449
1450         /* write remainingword in program memory */
1451         for ( ; off < len; off += sizeof(uint32_t)) {
1452                 uint32_t data;
1453                 if (off > 254)
1454                     fprintf(stdout, "\r");
1455
1456                 if ((off % sl->flash_pgsz) > (sl->flash_pgsz -5)) {
1457                     fprintf(stdout, "\r%3u/%u pages written",
1458                             off/sl->flash_pgsz, len/sl->flash_pgsz);
1459                     fflush(stdout);
1460                 }
1461
1462                 write_uint32((unsigned char*) &data, *(uint32_t*) (base + off));
1463                 stlink_write_debug32(sl, addr + off, data);
1464
1465                 /* wait for sr.busy to be cleared */
1466                 while ((stlink_read_debug32(sl, STM32L_FLASH_SR) & (1 << 0)) != 0) {
1467                 }
1468
1469 #if 0 /* todo: check redo write operation */
1470
1471                 /* check written bytes. todo: should be on a per page basis. */
1472                 data = stlink_read_debug32(sl, addr + off);
1473                 if (data == *(uint32_t*)(base + off)) {
1474                         /* re erase the page and redo the write operation */
1475                         uint32_t page;
1476                         uint32_t val;
1477
1478                         /* fail if successive write count too low */
1479                         if (nwrites < sl->flash_pgsz) {
1480                                 fprintf(stderr, "writes operation failure count too high, aborting\n");
1481                                 return -1;
1482                         }
1483
1484                         nwrites = 0;
1485
1486                         /* assume addr aligned */
1487                         if (off % sl->flash_pgsz) off &= ~(sl->flash_pgsz - 1);
1488                         page = addr + off;
1489
1490                         fprintf(stderr, "invalid write @0x%x(0x%x): 0x%x != 0x%x. retrying.\n",
1491                                         page, addr + off, read_uint32(base + off, 0), read_uint32(sl->q_buf, 0));
1492
1493                         /* reset lock bits */
1494                         val = stlink_read_debug32(sl, STM32L_FLASH_PECR)
1495                              | (1 << 0) | (1 << 1) | (1 << 2);
1496                         stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1497
1498                         stlink_erase_flash_page(sl, page);
1499
1500                         goto redo_write;
1501                 }
1502
1503                 /* increment successive writes counter */
1504                 ++nwrites;
1505
1506 #endif /* todo: check redo write operation */
1507         }
1508         fprintf(stdout, "\n");
1509         /* reset lock bits */
1510         val = stlink_read_debug32(sl, STM32L_FLASH_PECR)
1511              | (1 << 0) | (1 << 1) | (1 << 2);
1512         stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1513     } else if (sl->core_id == STM32VL_CORE_ID) {
1514         ILOG("Starting Flash write for VL core id\n");
1515         /* flash loader initialization */
1516         if (init_flash_loader(sl, &fl) == -1) {
1517             WLOG("init_flash_loader() == -1\n");
1518             return -1;
1519         }
1520
1521         int write_block_count = 0;
1522         for (off = 0; off < len; off += sl->flash_pgsz) {
1523             /* adjust last write size */
1524             size_t size = sl->flash_pgsz;
1525             if ((off + sl->flash_pgsz) > len) size = len - off;
1526
1527             /* unlock and set programming mode */
1528             unlock_flash_if(sl);
1529             set_flash_cr_pg(sl);
1530             //DLOG("Finished setting flash cr pg, running loader!\n");
1531             if (run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
1532                 WLOG("run_flash_loader(%#zx) failed! == -1\n", addr + off);
1533                 return -1;
1534             }
1535             lock_flash(sl);
1536             if (sl->verbose >= 1) {
1537                 /* show progress. writing procedure is slow
1538                    and previous errors are misleading */
1539               fprintf(stdout, "\r%3u/%lu pages written", write_block_count++, (unsigned long)len/sl->flash_pgsz);
1540                 fflush(stdout);
1541             }
1542         }
1543         fprintf(stdout, "\n");
1544     } else {
1545         WLOG("unknown coreid, not sure how to write: %x\n", sl->core_id);
1546         return -1;
1547     }
1548
1549     return stlink_verify_write_flash(sl, addr, base, len);
1550 }
1551
1552 /**
1553  * Write the given binary file into flash at address "addr"
1554  * @param sl
1555  * @param path readable file path, should be binary image
1556  * @param addr where to start writing
1557  * @return 0 on success, -ve on failure.
1558  */
1559 int stlink_fwrite_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
1560     /* write the file in flash at addr */
1561     int err;
1562     unsigned int num_empty = 0, index;
1563     unsigned char erased_pattern =(sl->chip_id == STM32_CHIPID_L1_MEDIUM)?0:0xff;
1564     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1565     if (map_file(&mf, path) == -1) {
1566         WLOG("map_file() == -1\n");
1567         return -1;
1568     }
1569     for(index = 0; index < mf.len; index ++) {
1570         if (mf.base[index] == erased_pattern)
1571             num_empty ++;
1572         else
1573             num_empty = 0;
1574     }
1575     if(num_empty != 0) {
1576         ILOG("Ignoring %d bytes of Zeros at end of file\n",num_empty);
1577         mf.len -= num_empty;
1578     }
1579     err = stlink_write_flash(sl, addr, mf.base, mf.len);
1580     /* set stack*/
1581     stlink_write_reg(sl, stlink_read_debug32(sl, addr    ),13);
1582     /* Set PC to the reset routine*/
1583     stlink_write_reg(sl, stlink_read_debug32(sl, addr + 4),15);
1584     stlink_run(sl);
1585     unmap_file(&mf);
1586     return err;
1587 }
1588
1589 int run_flash_loader(stlink_t *sl, flash_loader_t* fl, stm32_addr_t target, const uint8_t* buf, size_t size) {
1590
1591     reg rr;
1592     int i = 0;
1593     DLOG("Running flash loader, write address:%#x, size: %zd\n", target, size);
1594     // FIXME This can never return -1
1595     if (write_buffer_to_sram(sl, fl, buf, size) == -1) {
1596         // IMPOSSIBLE!
1597         WLOG("write_buffer_to_sram() == -1\n");
1598         return -1;
1599     }
1600
1601     if (sl->core_id == STM32L_CORE_ID) {
1602
1603       size_t count = size / sizeof(uint32_t);
1604       if (size % sizeof(uint32_t)) ++count;
1605
1606       /* setup core */
1607       stlink_write_reg(sl, target, 0); /* target */
1608       stlink_write_reg(sl, fl->buf_addr, 1); /* source */
1609       stlink_write_reg(sl, count, 2); /* count (32 bits words) */
1610       stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1611
1612     } else if (sl->core_id == STM32VL_CORE_ID) {
1613
1614       size_t count = size / sizeof(uint16_t);
1615       if (size % sizeof(uint16_t)) ++count;
1616
1617       /* setup core */
1618       stlink_write_reg(sl, fl->buf_addr, 0); /* source */
1619       stlink_write_reg(sl, target, 1); /* target */
1620       stlink_write_reg(sl, count, 2); /* count (16 bits half words) */
1621       stlink_write_reg(sl, 0, 3); /* flash bank 0 (input) */
1622       stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1623
1624     } else {
1625       fprintf(stderr, "unknown coreid: 0x%x\n", sl->core_id);
1626       return -1;
1627     }
1628
1629     /* run loader */
1630     stlink_run(sl);
1631
1632     /* wait until done (reaches breakpoint) */
1633     while ((is_core_halted(sl) == 0) && (i <1000))
1634     {
1635         i++;
1636     }
1637
1638     if ( i > 999) {
1639         fprintf(stderr, "run error\n");
1640         return -1;
1641     }
1642
1643     /* check written byte count */
1644     if (sl->core_id == STM32L_CORE_ID) {
1645
1646       size_t count = size / sizeof(uint32_t);
1647       if (size % sizeof(uint32_t)) ++count;
1648
1649       stlink_read_reg(sl, 3, &rr);
1650       if (rr.r[3] != count) {
1651         fprintf(stderr, "write error, count == %u\n", rr.r[3]);
1652         return -1;
1653       }
1654
1655     } else if (sl->core_id == STM32VL_CORE_ID) {
1656
1657       stlink_read_reg(sl, 2, &rr);
1658       if (rr.r[2] != 0) {
1659         fprintf(stderr, "write error, count == %u\n", rr.r[2]);
1660         return -1;
1661       }
1662
1663     } else {
1664
1665       fprintf(stderr, "unknown coreid: 0x%x\n", sl->core_id);
1666       return -1;
1667
1668     }
1669
1670     return 0;
1671 }