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

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