+
+int stlink_erase_flash_page(stlink_t *sl, stm32_addr_t page) {
+ /* page an addr in the page to erase */
+
+ /* wait for ongoing op to finish */
+ wait_flash_busy(sl);
+
+ /* unlock if locked */
+ unlock_flash_if(sl);
+
+ /* set the page erase bit */
+ set_flash_cr_per(sl);
+
+ /* select the page to erase */
+ write_flash_ar(sl, page);
+
+ /* start erase operation, reset by hw with bsy bit */
+ set_flash_cr_strt(sl);
+
+ /* wait for completion */
+ wait_flash_busy(sl);
+
+ /* relock the flash */
+ lock_flash(sl);
+
+ /* todo: verify the erased page */
+
+ return 0;
+}
+
+int stlink_erase_flash_mass(stlink_t *sl) {
+ /* wait for ongoing op to finish */
+ wait_flash_busy(sl);
+
+ /* unlock if locked */
+ unlock_flash_if(sl);
+
+ /* set the mass erase bit */
+ set_flash_cr_mer(sl);
+
+ /* start erase operation, reset by hw with bsy bit */
+ set_flash_cr_strt(sl);
+
+ /* wait for completion */
+ wait_flash_busy(sl);
+
+ /* relock the flash */
+ lock_flash(sl);
+
+ /* todo: verify the erased memory */
+
+ return 0;
+}
+
+int init_flash_loader(stlink_t *sl, flash_loader_t* fl) {
+ size_t size;
+
+ /* allocate the loader in sram */
+ if (write_loader_to_sram(sl, &fl->loader_addr, &size) == -1) {
+ fprintf(stderr, "write_loader_to_sram() == -1\n");
+ return -1;
+ }
+
+ /* allocate a one page buffer in sram right after loader */
+ fl->buf_addr = fl->loader_addr + size;
+
+ return 0;
+}
+
+int write_loader_to_sram(stlink_t *sl, stm32_addr_t* addr, size_t* size) {
+ /* from openocd, contrib/loaders/flash/stm32.s */
+ static const uint8_t loader_code[] = {
+ 0x08, 0x4c, /* ldr r4, STM32_FLASH_BASE */
+ 0x1c, 0x44, /* add r4, r3 */
+ /* write_half_word: */
+ 0x01, 0x23, /* movs r3, #0x01 */
+ 0x23, 0x61, /* str r3, [r4, #STM32_FLASH_CR_OFFSET] */
+ 0x30, 0xf8, 0x02, 0x3b, /* ldrh r3, [r0], #0x02 */
+ 0x21, 0xf8, 0x02, 0x3b, /* strh r3, [r1], #0x02 */
+ /* busy: */
+ 0xe3, 0x68, /* ldr r3, [r4, #STM32_FLASH_SR_OFFSET] */
+ 0x13, 0xf0, 0x01, 0x0f, /* tst r3, #0x01 */
+ 0xfb, 0xd0, /* beq busy */
+ 0x13, 0xf0, 0x14, 0x0f, /* tst r3, #0x14 */
+ 0x01, 0xd1, /* bne exit */
+ 0x01, 0x3a, /* subs r2, r2, #0x01 */
+ 0xf0, 0xd1, /* bne write_half_word */
+ /* exit: */
+ 0x00, 0xbe, /* bkpt #0x00 */
+ 0x00, 0x20, 0x02, 0x40, /* STM32_FLASH_BASE: .word 0x40022000 */
+ };
+
+ memcpy(sl->q_buf, loader_code, sizeof (loader_code));
+ stlink_write_mem32(sl, sl->sram_base, sizeof (loader_code));
+
+ *addr = sl->sram_base;
+ *size = sizeof (loader_code);
+
+ /* success */
+ return 0;
+}
+
+int stlink_fcheck_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
+ /* check the contents of path are at addr */
+
+ int res;
+ mapped_file_t mf = MAPPED_FILE_INITIALIZER;
+
+ if (map_file(&mf, path) == -1)
+ return -1;
+
+ res = check_file(sl, &mf, addr);
+
+ unmap_file(&mf);
+
+ return res;
+}
+
+// The stlink_fwrite_flash should not muck with mmapped files inside itself,
+// and should use this function instead. (Hell, what's the reason behind mmap
+// there?!) But, as it is not actually used anywhere, nobody cares.
+
+#define WRITE_BLOCK_SIZE 0x40
+
+int stlink_write_flash(stlink_t *sl, stm32_addr_t addr, uint8_t* base, unsigned len) {
+ size_t off;
+ flash_loader_t fl;
+
+ /* check addr range is inside the flash */
+ if (addr < sl->flash_base) {
+ fprintf(stderr, "addr too low\n");
+ return -1;
+ } else if ((addr + len) < addr) {
+ fprintf(stderr, "addr overruns\n");
+ return -1;
+ } else if ((addr + len) > (sl->flash_base + sl->flash_size)) {
+ fprintf(stderr, "addr too high\n");
+ return -1;
+ } else if ((addr & 1) || (len & 1)) {
+ fprintf(stderr, "unaligned addr or size\n");
+ return -1;
+ }
+
+ /* flash loader initialization */
+ if (init_flash_loader(sl, &fl) == -1) {
+ fprintf(stderr, "init_flash_loader() == -1\n");
+ return -1;
+ }
+
+ /* write each page. above WRITE_BLOCK_SIZE fails? */
+ for (off = 0; off < len; off += WRITE_BLOCK_SIZE) {
+ /* adjust last write size */
+ size_t size = WRITE_BLOCK_SIZE;
+ if ((off + WRITE_BLOCK_SIZE) > len)
+ size = len - off;
+
+ if (run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
+ fprintf(stderr, "run_flash_loader(0x%zx) == -1\n", addr + off);
+ return -1;
+ }
+ }
+
+ for (off = 0; off < len; off += sl->flash_pgsz) {
+ size_t aligned_size;
+
+ /* adjust last page size */
+ size_t cmp_size = sl->flash_pgsz;
+ if ((off + sl->flash_pgsz) > len)
+ cmp_size = len - off;
+
+ aligned_size = cmp_size;
+ if (aligned_size & (4 - 1))
+ aligned_size = (cmp_size + 4) & ~(4 - 1);
+
+ stlink_read_mem32(sl, addr + off, aligned_size);
+
+ if (memcmp(sl->q_buf, base + off, cmp_size))
+ return -1;
+ }
+
+ return 0;
+}
+
+int stlink_fwrite_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
+ /* write the file in flash at addr */
+
+ int error = -1;
+ size_t off;
+ mapped_file_t mf = MAPPED_FILE_INITIALIZER;
+ flash_loader_t fl;
+
+ if (map_file(&mf, path) == -1) {
+ fprintf(stderr, "map_file() == -1\n");
+ return -1;
+ }
+
+ /* check addr range is inside the flash */
+ if (addr < sl->flash_base) {
+ fprintf(stderr, "addr too low\n");
+ goto on_error;
+ } else if ((addr + mf.len) < addr) {
+ fprintf(stderr, "addr overruns\n");
+ goto on_error;
+ } else if ((addr + mf.len) > (sl->flash_base + sl->flash_size)) {
+ fprintf(stderr, "addr too high\n");
+ goto on_error;
+ } else if ((addr & 1) || (mf.len & 1)) {
+ /* todo */
+ fprintf(stderr, "unaligned addr or size\n");
+ goto on_error;
+ }
+
+ /* erase each page. todo: mass erase faster? */
+ for (off = 0; off < mf.len; off += sl->flash_pgsz) {
+ /* addr must be an addr inside the page */
+ if (stlink_erase_flash_page(sl, addr + off) == -1) {
+ fprintf(stderr, "erase_flash_page(0x%zx) == -1\n", addr + off);
+ goto on_error;
+ }
+ }
+
+ /* flash loader initialization */
+ if (init_flash_loader(sl, &fl) == -1) {
+ fprintf(stderr, "init_flash_loader() == -1\n");
+ goto on_error;
+ }
+
+ /* write each page. above WRITE_BLOCK_SIZE fails? */
+#define WRITE_BLOCK_SIZE 0x40
+ for (off = 0; off < mf.len; off += WRITE_BLOCK_SIZE) {
+ /* adjust last write size */
+ size_t size = WRITE_BLOCK_SIZE;
+ if ((off + WRITE_BLOCK_SIZE) > mf.len)
+ size = mf.len - off;
+
+ if (run_flash_loader(sl, &fl, addr + off, mf.base + off, size) == -1) {
+ fprintf(stderr, "run_flash_loader(0x%zx) == -1\n", addr + off);
+ goto on_error;
+ }
+ }
+
+ /* check the file ha been written */
+ if (check_file(sl, &mf, addr) == -1) {
+ fprintf(stderr, "check_file() == -1\n");
+ goto on_error;
+ }
+
+ /* success */
+ error = 0;
+
+on_error:
+ unmap_file(&mf);
+ return error;
+}
+
+int run_flash_loader(stlink_t *sl, flash_loader_t* fl, stm32_addr_t target, const uint8_t* buf, size_t size) {
+ const size_t count = size / sizeof (uint16_t);
+
+ if (write_buffer_to_sram(sl, fl, buf, size) == -1) {
+ fprintf(stderr, "write_buffer_to_sram() == -1\n");
+ return -1;
+ }
+
+ /* setup core */
+ stlink_write_reg(sl, fl->buf_addr, 0); /* source */
+ stlink_write_reg(sl, target, 1); /* target */
+ stlink_write_reg(sl, count, 2); /* count (16 bits half words) */
+ stlink_write_reg(sl, 0, 3); /* flash bank 0 (input) */
+ stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
+
+ /* unlock and set programming mode */
+ unlock_flash_if(sl);
+ set_flash_cr_pg(sl);
+
+ /* run loader */
+ stlink_run(sl);
+
+ while (is_core_halted(sl) == 0)
+ ;
+
+ lock_flash(sl);
+
+ /* not all bytes have been written */
+ reg rr;
+ stlink_read_reg(sl, 2, &rr);
+ if (rr.r[2] != 0) {
+ fprintf(stderr, "write error, count == %u\n", rr.r[2]);
+ return -1;
+ }
+
+ return 0;
+}
\ No newline at end of file