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