ST-Link 高速烧录的秘密.下（硬件实现）

.ramfunc :
{
    *(.ramfunc*)
} > RAM

__attribute__((section(".ramfunc")))
void flash_write_page(void)
{
    // 这里的代码会被放进 RAM
}

__RAM_FUNC void flash_write_page(void)
{
}

Flash 中的 .ramfunc
    ↓ memcpy
RAM 中的 .ramfunc

const uint32_t table[] = {...}; // 在 Flash

SCB->VTOR = RAM_VECTOR_TABLE_ADDR;

#define FLASH_REGS_BANK2_OFS      0x40
#define FLASH_BANK2_START_ADDR    0x08080000

#define STM32F0_WDG_KR            0x40003000
#define STM32H7_WDG_KR            0x58004800
#define STM32F0_WDG_KR_KEY_RELOAD 0xAAAA

stlink_write_debug32(DHCSR, DBGKEY|C_DEBUGEN|C_HALT);
stlink_write_debug32(DHCSR, DBGKEY|C_DEBUGEN|C_HALT|C_MASKINTS);

stlink_flash_loader_write_to_sram(...)
fl->buf_addr = fl->loader_addr + size;

DFSR/CFSR/HFSR

write_buffer_to_sram(sl, fl, buf, size, padded_size)

if(F1_XL && target >= 0x08080000) flash_base = 0x40;

R0 = buf_addr   // source
R1 = target     // target flash address
R2 = padded_size// count
R3 = flash_base // flash reg base offset
PC = loader_addr

stlink_write_debug32(iwdg_kr, 0xAAAA);

stlink_run(sl, RUN_FLASH_LOADER);

if (rr.r[2] > 0 || rr.r[2] < -7) -> error

/*
 * File: flash_loader.c
 *
 * Flash loaders
 */

#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>

#include <stm32.h>
#include <stm32_register.h>
#include <stlink.h>

#include "flash_loader.h"
#include "common_flash.h"
#include "helper.h"
#include "logging.h"
#include "read_write.h"

#define FLASH_REGS_BANK2_OFS      0x40
#define FLASH_BANK2_START_ADDR    0x08080000

#define STM32F0_WDG_KR            0x40003000
#define STM32H7_WDG_KR            0x58004800

#define STM32F0_WDG_KR_KEY_RELOAD 0xAAAA

/*
 * !!! DO NOT MODIFY FLASH LOADERS DIRECTLY !!!
 *
 * Edit assembly files in the '/flashloaders' instead. The sizes of binary
 * flash loaders must be aligned by 4 (it's written by stlink_write_mem32)
 */

// flashloaders/stm32f0.s -- compiled with thumb2
staticconstuint8_t loader_code_stm32vl[] = {
    0x00, 0xbf, 0x00, 0xbf,
    0x09, 0x4f, 0x1f, 0x44,
    0x09, 0x4d, 0x3d, 0x44,
    0x04, 0x88, 0x0c, 0x80,
    0x02, 0x30, 0x02, 0x31,
    0x4f, 0xf0, 0x01, 0x07,
    0x2c, 0x68, 0x3c, 0x42,
    0xfc, 0xd1, 0x4f, 0xf0,
    0x14, 0x07, 0x3c, 0x42,
    0x01, 0xd1, 0x02, 0x3a,
    0xf0, 0xdc, 0x00, 0xbe,
    0x00, 0x20, 0x02, 0x40,
    0x0c, 0x00, 0x00, 0x00
};

// flashloaders/stm32f0.s -- thumb1 only, same sequence as for STM32VL, bank ignored
staticconstuint8_t loader_code_stm32f0[] = {
    0xc0, 0x46, 0xc0, 0x46,
    0x08, 0x4f, 0x1f, 0x44,
    0x08, 0x4d, 0x3d, 0x44,
    0x04, 0x88, 0x0c, 0x80,
    0x02, 0x30, 0x02, 0x31,
    0x06, 0x4f, 0x2c, 0x68,
    0x3c, 0x42, 0xfc, 0xd1,
    0x05, 0x4f, 0x3c, 0x42,
    0x01, 0xd1, 0x02, 0x3a,
    0xf2, 0xdc, 0x00, 0xbe,
    0x00, 0x20, 0x02, 0x40,
    0x0c, 0x00, 0x00, 0x00,
    0x01, 0x00, 0x00, 0x00,
    0x14, 0x00, 0x00, 0x00
};

// flashloaders/stm32wb0.s
staticconstuint8_t loader_code_stm32wb0[] = {
    0x0b, 0x4b, 0xc9, 0x1a,
    0x89, 0x08, 0x09, 0x4f,
    0xb9, 0x61, 0x40, 0x37,
    0x78, 0xc8, 0x78, 0xc7,
    0x50, 0x3f, 0x0d, 0x25,
    0x3d, 0x61, 0xcc, 0x24,
    0x3c, 0x60, 0x3c, 0x69,
    0x2c, 0x40, 0xfc, 0xd0,
    0x64, 0x08, 0x02, 0xd1,
    0x04, 0x31, 0x10, 0x3a,
    0xee, 0xdc, 0x00, 0xbe,
    0x00, 0x10, 0x00, 0x40,
    0x00, 0x00, 0x04, 0x10
};

// flashloaders/stm32lx.s -- compiled for armv6-m for compatibility with both
// armv6-m cores (STM32L0) and armv7-m cores (STM32L1)
staticconstuint8_t loader_code_stm32lx[] = {
    0x04, 0x68, 0x0c, 0x60,
    0x04, 0x30, 0x04, 0x31,
    0x04, 0x3a, 0xf9, 0xdc,
    0x00, 0xbe, 0x00, 0x00
};

// flashloaders/stm32f4.s
staticconstuint8_t loader_code_stm32f4[] = {
    0xdf, 0xf8, 0x24, 0xc0,
    0xdf, 0xf8, 0x24, 0xa0,
    0xe2, 0x44, 0x04, 0x68,
    0x0c, 0x60, 0x00, 0xf1,
    0x04, 0x00, 0x01, 0xf1,
    0x04, 0x01, 0xba, 0xf8,
    0x00, 0x40, 0x14, 0xf0,
    0x01, 0x0f, 0xfa, 0xd1,
    0x04, 0x3a, 0xf2, 0xdc,
    0x00, 0xbe, 0x00, 0xbf,
    0x00, 0x3c, 0x02, 0x40,
    0x0e, 0x00, 0x00, 0x00
};

// flashloaders/stm32f4lv.s
staticconstuint8_t loader_code_stm32f4_lv[] = {
    0xdf, 0xf8, 0x24, 0xc0,
    0xdf, 0xf8, 0x24, 0xa0,
    0xe2, 0x44, 0x04, 0x78,
    0x0c, 0x70, 0x00, 0xf1,
    0x01, 0x00, 0x01, 0xf1,
    0x01, 0x01, 0xba, 0xf8,
    0x00, 0x40, 0x14, 0xf0,
    0x01, 0x0f, 0xfa, 0xd1,
    0x01, 0x3a, 0xf2, 0xdc,
    0x00, 0xbe, 0x00, 0xbf,
    0x00, 0x3c, 0x02, 0x40,
    0x0e, 0x00, 0x00, 0x00
};

// flashloaders/stm32l4.s
staticconstuint8_t loader_code_stm32l4[] = {
    0xdf, 0xf8, 0x28, 0xc0,
    0xdf, 0xf8, 0x28, 0xa0,
    0xe2, 0x44, 0x05, 0x68,
    0x44, 0x68, 0x0d, 0x60,
    0x4c, 0x60, 0x00, 0xf1,
    0x08, 0x00, 0x01, 0xf1,
    0x08, 0x01, 0xda, 0xf8,
    0x00, 0x40, 0x14, 0xf4,
    0x80, 0x3f, 0xfa, 0xd1,
    0x08, 0x3a, 0xf0, 0xdc,
    0x00, 0xbe, 0x00, 0xbf,
    0x00, 0x20, 0x02, 0x40,
    0x10, 0x00, 0x00, 0x00
};

// flashloaders/stm32f7.s
staticconstuint8_t loader_code_stm32f7[] = {
    0xdf, 0xf8, 0x28, 0xc0,
    0xdf, 0xf8, 0x28, 0xa0,
    0xe2, 0x44, 0x04, 0x68,
    0x0c, 0x60, 0x00, 0xf1,
    0x04, 0x00, 0x01, 0xf1,
    0x04, 0x01, 0xbf, 0xf3,
    0x4f, 0x8f, 0xba, 0xf8,
    0x00, 0x40, 0x14, 0xf0,
    0x01, 0x0f, 0xfa, 0xd1,
    0x04, 0x3a, 0xf0, 0xdc,
    0x00, 0xbe, 0x00, 0xbf,
    0x00, 0x3c, 0x02, 0x40,
    0x0e, 0x00, 0x00, 0x00
};

// flashloaders/stm32f7lv.s
staticconstuint8_t loader_code_stm32f7_lv[] = {
    0xdf, 0xf8, 0x28, 0xc0,
    0xdf, 0xf8, 0x28, 0xa0,
    0xe2, 0x44, 0x04, 0x78,
    0x0c, 0x70, 0x00, 0xf1,
    0x01, 0x00, 0x01, 0xf1,
    0x01, 0x01, 0xbf, 0xf3,
    0x4f, 0x8f, 0xba, 0xf8,
    0x00, 0x40, 0x14, 0xf0,
    0x01, 0x0f, 0xfa, 0xd1,
    0x01, 0x3a, 0xf0, 0xdc,
    0x00, 0xbe, 0x00, 0xbf,
    0x00, 0x3c, 0x02, 0x40,
    0x0e, 0x00, 0x00, 0x00
};


int32_t stlink_flash_loader_init(stlink_t *sl, flash_loader_t *fl) {
    uint32_t size = 0;
    uint32_t dfsr, cfsr, hfsr;

    /* Interrupt masking according to DDI0419C, Table C1-7 firstly force halt */
    stlink_write_debug32(sl, STM32_REG_DHCSR,
                           STM32_REG_DHCSR_DBGKEY | STM32_REG_DHCSR_C_DEBUGEN |
                           STM32_REG_DHCSR_C_HALT);
    /* and only then disable interrupts */
    stlink_write_debug32(sl, STM32_REG_DHCSR,
                           STM32_REG_DHCSR_DBGKEY | STM32_REG_DHCSR_C_DEBUGEN |
                           STM32_REG_DHCSR_C_HALT | STM32_REG_DHCSR_C_MASKINTS);

    // allocate the loader in SRAM
    if(stlink_flash_loader_write_to_sram(sl, &fl->loader_addr, &size) == -1) {
        WLOG("Failed to write flash loader to sram!\n");
        return (-1);
    }

    // allocate a one page buffer in SRAM right after loader
    fl->buf_addr = fl->loader_addr + size;
    ILOG("Successfully loaded flash loader in sram\n");

    // set address of IWDG key register for reset it
    if(sl->flash_type == STM32_FLASH_TYPE_H7) {
        fl->iwdg_kr = STM32H7_WDG_KR;
    } else {
        fl->iwdg_kr = STM32F0_WDG_KR;
    }

    /* Clear Fault Status Register for handling flash loader error */
    if(!stlink_read_debug32(sl, STM32_REG_DFSR, &dfsr) && dfsr) {
        ILOG("Clear DFSR\n");
        stlink_write_debug32(sl, STM32_REG_DFSR, dfsr);
    }
    if(!stlink_read_debug32(sl, STM32_REG_CFSR, &cfsr) && cfsr) {
        ILOG("Clear CFSR\n");
        stlink_write_debug32(sl, STM32_REG_CFSR, cfsr);
    }
    if(!stlink_read_debug32(sl, STM32_REG_HFSR, &hfsr) && hfsr) {
        ILOG("Clear HFSR\n");
        stlink_write_debug32(sl, STM32_REG_HFSR, hfsr);
    }

    return (0);
}

static int32_t loader_v_dependent_assignment(stlink_t *sl,
                                            const uint8_t **loader_code, uint32_t *loader_size,
                                            const uint8_t *high_v_loader, uint32_t high_v_loader_size,
                                            const uint8_t *low_v_loader, uint32_t low_v_loader_size) {
    int32_t retval = 0;

    if( sl->version.stlink_v == 1) {
        printf("STLINK V1 cannot read voltage, defaulting to 32-bit writes\n");
        *loader_code = high_v_loader;
        *loader_size = high_v_loader_size;
    } else {
        int32_t voltage = stlink_target_voltage(sl);

        if(voltage == -1) {
            retval = -1;
            printf("Failed to read Target voltage\n");
        } else   {
            if(voltage > 2700) {
                *loader_code = high_v_loader;
                *loader_size = high_v_loader_size;
            } else {
                *loader_code = low_v_loader;
                *loader_size = low_v_loader_size;
            }
        }
    }

    return (retval);
}

int32_t stlink_flash_loader_write_to_sram(stlink_t *sl, stm32_addr_t* addr, uint32_t* size) {
    constuint8_t* loader_code;
    uint32_t loader_size;

    if(sl->chip_id == STM32_CHIPID_L1_MD ||
        sl->chip_id == STM32_CHIPID_L1_CAT2 ||
        sl->chip_id == STM32_CHIPID_L1_MD_PLUS ||
        sl->chip_id == STM32_CHIPID_L1_MD_PLUS_HD ||
        sl->chip_id == STM32_CHIPID_L152_RE ||
        sl->chip_id == STM32_CHIPID_L0_CAT1 ||
        sl->chip_id == STM32_CHIPID_L0_CAT2 ||
        sl->chip_id == STM32_CHIPID_L0_CAT3 ||
        sl->chip_id == STM32_CHIPID_L0_CAT5) {
        loader_code = loader_code_stm32lx;
        loader_size = sizeof(loader_code_stm32lx);
    } elseif(sl->core_id == STM32_CORE_ID_M3_r1p1_SWD ||
               sl->chip_id == STM32_CHIPID_F1_MD ||
               sl->chip_id == STM32_CHIPID_F1_HD ||
               sl->chip_id == STM32_CHIPID_F1_LD ||
               sl->chip_id == STM32_CHIPID_F1_VL_MD_LD ||
               sl->chip_id == STM32_CHIPID_F1_VL_HD ||
               sl->chip_id == STM32_CHIPID_F1_XLD ||
               sl->chip_id == STM32_CHIPID_F1_CONN ||
               sl->chip_id == STM32_CHIPID_F3 ||
               sl->chip_id == STM32_CHIPID_F3xx_SMALL ||
               sl->chip_id == STM32_CHIPID_F303_HD ||
               sl->chip_id == STM32_CHIPID_F37x ||
               sl->chip_id == STM32_CHIPID_F334) {
        loader_code = loader_code_stm32vl;
        loader_size = sizeof(loader_code_stm32vl);
    } elseif(sl->chip_id == STM32_CHIPID_F2 ||
               sl->chip_id == STM32_CHIPID_F4 ||
               sl->chip_id == STM32_CHIPID_F4_DE ||
               sl->chip_id == STM32_CHIPID_F4_LP ||
               sl->chip_id == STM32_CHIPID_F4_HD ||
               sl->chip_id == STM32_CHIPID_F4_DSI ||
               sl->chip_id == STM32_CHIPID_F410 ||
               sl->chip_id == STM32_CHIPID_F411xx ||
               sl->chip_id == STM32_CHIPID_F412 ||
               sl->chip_id == STM32_CHIPID_F413 ||
               sl->chip_id == STM32_CHIPID_F446) {
        int32_t retval;
        retval = loader_v_dependent_assignment(sl,
                                               &loader_code, &loader_size,
                                               loader_code_stm32f4, sizeof(loader_code_stm32f4),
                                               loader_code_stm32f4_lv, sizeof(loader_code_stm32f4_lv));

        if(retval == -1) { return (retval); }
    } elseif(sl->core_id == STM32_CORE_ID_M7F_SWD ||
               sl->chip_id == STM32_CHIPID_F7 ||
               sl->chip_id == STM32_CHIPID_F76xxx ||
               sl->chip_id == STM32_CHIPID_F72xxx) {
        int32_t retval;
        retval = loader_v_dependent_assignment(sl,
                                               &loader_code, &loader_size,
                                               loader_code_stm32f7, sizeof(loader_code_stm32f7),
                                               loader_code_stm32f7_lv, sizeof(loader_code_stm32f7_lv));

        if(retval == -1) { return (retval); }
    } elseif(sl->chip_id == STM32_CHIPID_F0 ||
               sl->chip_id == STM32_CHIPID_F04 ||
               sl->chip_id == STM32_CHIPID_F0_CAN ||
               sl->chip_id == STM32_CHIPID_F0xx_SMALL ||
               sl->chip_id == STM32_CHIPID_F09x) {
        loader_code = loader_code_stm32f0;
        loader_size = sizeof(loader_code_stm32f0);
    } elseif(sl->flash_type == STM32_FLASH_TYPE_WB0) {
        loader_code = loader_code_stm32wb0;
        loader_size = sizeof(loader_code_stm32wb0);
    } elseif((sl->chip_id == STM32_CHIPID_L4) ||
               (sl->chip_id == STM32_CHIPID_L41x_L42x) ||
               (sl->chip_id == STM32_CHIPID_L43x_L44x) ||
               (sl->chip_id == STM32_CHIPID_L45x_L46x) ||
               (sl->chip_id == STM32_CHIPID_L4PX) ||
               (sl->chip_id == STM32_CHIPID_L4Rx) ||
               (sl->chip_id == STM32_CHIPID_L496x_L4A6x)) {
        loader_code = loader_code_stm32l4;
        loader_size = sizeof(loader_code_stm32l4);
    } else {
        ELOG("unknown coreid, not sure what flash loader to use, aborting! coreid: %x, chipid: %x\n",
            sl->core_id, sl->chip_id);
        return (-1);
    }

    memcpy(sl->q_buf, loader_code, loader_size);
    int32_t ret = stlink_write_mem32(sl, sl->sram_base, (uint16_t) loader_size);

    if(ret) { return (ret); }

    *addr = sl->sram_base;
    *size = loader_size;

    return (0); // success
}

int32_t stlink_flash_loader_run(stlink_t *sl, flash_loader_t* fl, stm32_addr_t target, const uint8_t* buf, uint32_t size) {
    struct stlink_reg rr;
    uint32_t timeout;
    uint32_t flash_base = 0;
    uint32_t dhcsr, dfsr, cfsr, hfsr;
    uint16_t padded_size = size, pad_modulo = 1;
    
    if(sl->flash_type == STM32_FLASH_TYPE_WB0) {
        pad_modulo = 16;
    }
    uint16_t unaligned_cnt = size % pad_modulo;
    if (unaligned_cnt) {
      padded_size = size + (pad_modulo - unaligned_cnt);
    }

    DLOG("Running flash loader, write address:%#x, size: %u, padded_size: %u\n", target, size, padded_size);

    if(write_buffer_to_sram(sl, fl, buf, size, padded_size) == -1) {
        ELOG("write_buffer_to_sram() == -1\n");
        return (-1);
    }

    if((sl->flash_type == STM32_FLASH_TYPE_F1_XL) && (target >= FLASH_BANK2_START_ADDR)) {
        flash_base = FLASH_REGS_BANK2_OFS;
    }

/* Setup core */
  stlink_write_reg(sl, fl->buf_addr, 0);     // source
  stlink_write_reg(sl, target, 1);           // target
  stlink_write_reg(sl, padded_size, 2);      // count
  stlink_write_reg(sl, flash_base, 3);       // flash register base
                                             // only used on VL/F1_XL, but harmless for others
  stlink_write_reg(sl, fl->loader_addr, 15); // pc register

/* Reset IWDG */
if(fl->iwdg_kr) {
      stlink_write_debug32(sl, fl->iwdg_kr, STM32F0_WDG_KR_KEY_RELOAD);
  }

/* Run loader */
  stlink_run(sl, RUN_FLASH_LOADER);

/*
 * This piece of code used to try to spin for .1 second by waiting doing 10000 rounds of 10 µs.
 * But because this usually runs on Unix-like OSes, the 10 µs get rounded up to the "tick"
 * (actually almost two ticks) of the system. 1 ms. Thus, the ten thousand attempts, when
 * "something goes wrong" that requires the error message "flash loader run error" would wait
 * for something like 20 seconds before coming up with the error.
 * By increasing the sleep-per-round to the same order-of-magnitude as the tick-rounding that
 * the OS uses, the wait until the error message is reduced to the same order of magnitude
 * as what was intended. -- REW.
 */

// wait until done (reaches breakpoint)
  timeout = time_ms() + 500;
while (time_ms() < timeout) {
      usleep(10000);

      if(stlink_is_core_halted(sl)) {
          timeout = 0;
          break;
      }
  }

if(timeout) {
      ELOG("Flash loader run error\n");
      goto error;
  }

// check written byte count
  stlink_read_reg(sl, 2, &rr);

/*
  * The chunk size for loading is not rounded. The flash loader
  * subtracts the size of the written block (1-8 bytes) from
  * the remaining size each time. A negative value may mean that
  * several bytes garbage have been written due to the unaligned
  * firmware size.
  */
if((int32_t) rr.r[2] > 0 || (int32_t) rr.r[2] < -7) {
      ELOG("Flash loader write error at 0x%08X\n", target + padded_size - rr.r[2]);
      goto error;
  }

return (0);

  error:
      dhcsr = dfsr = cfsr = hfsr = 0;
      stlink_force_debug(sl); // ensure we can read regs, even after timeout
      stlink_read_debug32(sl, STM32_REG_DHCSR, &dhcsr);
      stlink_read_debug32(sl, STM32_REG_DFSR, &dfsr);
      stlink_read_debug32(sl, STM32_REG_CFSR, &cfsr);
      stlink_read_debug32(sl, STM32_REG_HFSR, &hfsr);
      stlink_read_all_regs(sl, &rr);

      WLOG("Loader state: PC 0x%08X\n", rr.r[15]);
      WLOG("              R0 0x%08X R1 0x%08X\n", rr.r[0], rr.r[1]);
      WLOG("              R2 0x%08X R3 0x%08X\n", rr.r[2], rr.r[3]);
      WLOG("              R4 0x%08X R5 0x%08X\n", rr.r[4], rr.r[5]);
      WLOG("              R6 0x%08X R7 0x%08X\n", rr.r[6], rr.r[7]);
      if(dhcsr != 0x3000B || dfsr || cfsr || hfsr) {
          WLOG("MCU state: DHCSR 0x%X DFSR 0x%X CFSR 0x%X HFSR 0x%X\n", dhcsr, dfsr, cfsr, hfsr);
      }

return (-1);
}


/* === Content from old source file flashloader.c === */

#define L1_WRITE_BLOCK_SIZE 0x80
#define L0_WRITE_BLOCK_SIZE 0x40

int32_t stm32l1_write_half_pages(stlink_t *sl, flash_loader_t *fl, stm32_addr_t addr, uint8_t *base, uint32_t len, uint32_t pagesize) {
uint32_t count, off;
uint32_t num_half_pages = len / pagesize;
uint32_t val;
uint32_t flash_regs_base = get_stm32l0_flash_base(sl);
bool use_loader = true;
int32_t ret = 0;

// enable half page write
  stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
  val |= (1 << STM32_FLASH_L1_FPRG);
  stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
  val |= (1 << STM32_FLASH_L1_PROG);
  stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);

  wait_flash_busy(sl);

for(count = 0; count < num_half_pages; count++) {
    if(use_loader) {
      ret = stlink_flash_loader_run(sl, fl, addr + count * pagesize, base + count * pagesize, pagesize);
      if(ret && count == 0) {
        /* It seems that stm32lx devices have a problem when it is blank */
        WLOG("Failed to use flash loader, fallback to soft write\n");
        use_loader = false;
      }
    }
    if(!use_loader) {
      ret = 0;
      for(off = 0; off < pagesize && !ret; off += 64) {
        uint32_t chunk = (pagesize - off > 64) ? 64 : pagesize - off;
        memcpy(sl->q_buf, base + count * pagesize + off, chunk);
        ret = stlink_write_mem32(sl, addr + count * pagesize + off, (uint16_t) chunk);
      }
    }

    if(ret) {
      WLOG("l1_stlink_flash_loader_run(%#x) failed! == -1\n", addr + count * pagesize);
      break;
    }

    if(sl->verbose >= 1) {
      // show progress; writing procedure is slow and previous errors are misleading
      fprintf(stdout, "%3u/%3u halfpages written\n", count + 1, num_half_pages);
      fflush(stdout);
    }

    // wait for sr.busy to be cleared
    wait_flash_busy(sl);
  }

// disable half page write
  stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
  val &= ~((1 << STM32_FLASH_L1_FPRG) | (1 << STM32_FLASH_L1_PROG));
  stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
return (ret);
}

static void set_flash_cr_pg(stlink_t *sl, uint32_t bank) {
uint32_t cr_reg, x;

  x = read_flash_cr(sl, bank);

if(sl->flash_type == STM32_FLASH_TYPE_C0) {
    cr_reg = STM32_FLASH_C0_CR;
    x |= (1 << FLASH_CR_PG);
  } elseif(sl->flash_type == STM32_FLASH_TYPE_F2_F4) {
    cr_reg = STM32_FLASH_F4_CR;
    x |= (1 << FLASH_CR_PG);
  } elseif(sl->flash_type == STM32_FLASH_TYPE_F7) {
    cr_reg = STM32_FLASH_F7_CR;
    x |= (1 << FLASH_CR_PG);
  } elseif(sl->flash_type == STM32_FLASH_TYPE_L4) {
    cr_reg = STM32_FLASH_L4_CR;
    x &= ~STM32_FLASH_L4_CR_OPBITS;
    x |= (1 << STM32_FLASH_L4_CR_PG);
  } elseif(sl->flash_type == STM32_FLASH_TYPE_L5_U5_H5) {
    cr_reg = STM32_FLASH_L5_NSCR;
    x |= (1 << FLASH_CR_PG);
  } elseif(sl->flash_type == STM32_FLASH_TYPE_G0 ||
             sl->flash_type == STM32_FLASH_TYPE_G4) {
    cr_reg = STM32_FLASH_Gx_CR;
    x |= (1 << FLASH_CR_PG);
  } elseif(sl->flash_type == STM32_FLASH_TYPE_WB_WL) {
    cr_reg = STM32_FLASH_WB_CR;
    x |= (1 << FLASH_CR_PG);
  } elseif(sl->flash_type == STM32_FLASH_TYPE_H7) {
    cr_reg = (bank == BANK_1) ? STM32_FLASH_H7_CR1 : STM32_FLASH_H7_CR2;
    x |= (1 << STM32_FLASH_H7_CR_PG);
  } else {
    cr_reg = (bank == BANK_1) ? FLASH_CR : FLASH_CR2;
    x = (1 << FLASH_CR_PG);
  }

  stlink_write_debug32(sl, cr_reg, x);
}

static void set_dma_state(stlink_t *sl, flash_loader_t *fl, int32_t bckpRstr) {
uint32_t rcc, rcc_dma_mask, value;

  rcc = rcc_dma_mask = value = 0;

switch (sl->flash_type) {
case STM32_FLASH_TYPE_C0:
    rcc = STM32C0_RCC_AHBENR;
    rcc_dma_mask = STM32C0_RCC_DMAEN;
    break;
case STM32_FLASH_TYPE_F0_F1_F3:
case STM32_FLASH_TYPE_F1_XL:
    rcc = STM32F1_RCC_AHBENR;
    rcc_dma_mask = STM32F1_RCC_DMAEN;
    break;
case STM32_FLASH_TYPE_F2_F4:
case STM32_FLASH_TYPE_F7:
    rcc = STM32F4_RCC_AHB1ENR;
    rcc_dma_mask = STM32F4_RCC_DMAEN;
    break;
case STM32_FLASH_TYPE_G0:
    rcc = STM32G0_RCC_AHBENR;
    rcc_dma_mask = STM32G0_RCC_DMAEN;
    break;
case STM32_FLASH_TYPE_G4:
case STM32_FLASH_TYPE_L4:
    rcc = STM32G4_RCC_AHB1ENR;
    rcc_dma_mask = STM32G4_RCC_DMAEN;
    break;
case STM32_FLASH_TYPE_L0_L1:
    if(get_stm32l0_flash_base(sl) == STM32_FLASH_Lx_REGS_ADDR) {
      rcc = STM32L1_RCC_AHBENR;
      rcc_dma_mask = STM32L1_RCC_DMAEN;
    } else {
      rcc = STM32L0_RCC_AHBENR;
      rcc_dma_mask = STM32L0_RCC_DMAEN;
    }
    break;
case STM32_FLASH_TYPE_L5_U5_H5:
    rcc = STM32L5_RCC_AHB1ENR;
    rcc_dma_mask = STM32L5_RCC_DMAEN;
    break;
case STM32_FLASH_TYPE_H7:
    rcc = STM32H7_RCC_AHB1ENR;
    rcc_dma_mask = STM32H7_RCC_DMAEN;
    break;
case STM32_FLASH_TYPE_WB_WL:
    rcc = STM32WB_RCC_AHB1ENR;
    rcc_dma_mask = STM32WB_RCC_DMAEN;
    break;
case STM32_FLASH_TYPE_WB0:
    rcc = STM32WB0_RCC_AHBENR;
    rcc_dma_mask = STM32WB0_RCC_AHB_DMAEN;
    break;
default:
    return;
  }

if(!stlink_read_debug32(sl, rcc, &value)) {
    if(bckpRstr) {
      value = (value & (~rcc_dma_mask)) | fl->rcc_dma_bkp;
    } else {
      fl->rcc_dma_bkp = value & rcc_dma_mask;
      value &= ~rcc_dma_mask;
    }
    stlink_write_debug32(sl, rcc, value);
  }
}

int32_t stlink_flashloader_start(stlink_t *sl, flash_loader_t *fl) {
// disable DMA
  set_dma_state(sl, fl, 0);

// wait for ongoing op to finish
  wait_flash_busy(sl);
// Clear errors
  clear_flash_error(sl);

if((sl->flash_type == STM32_FLASH_TYPE_F2_F4) ||
      (sl->flash_type == STM32_FLASH_TYPE_F7) ||
      (sl->flash_type == STM32_FLASH_TYPE_L4)) {
    ILOG("Starting Flash write for F2/F4/F7/L4\n");

    // Flash loader initialisation
    if(stlink_flash_loader_init(sl, fl) == -1) {
      ELOG("stlink_flash_loader_init() == -1\n");
      return (-1);
    }

    unlock_flash_if(sl); // first unlock the cr

    int32_t voltage;
    if(sl->version.stlink_v == 1) {
      WLOG("STLINK V1 cannot read voltage, use default voltage 3.2 V\n");
      voltage = 3200;
    } else {
      voltage = stlink_target_voltage(sl);
    }

    if(voltage == -1) {
      ELOG("Failed to read Target voltage\n");
      return (-1);
    }

    if(sl->flash_type == STM32_FLASH_TYPE_L4) {
      // L4 does not have a byte-write mode
      if(voltage < 1710) {
        ELOG("Target voltage (%d mV) too low for flash writes!\n", voltage);
        return (-1);
      }
    } else {
      if(voltage > 2700) {
        ILOG("enabling 32-bit flash writes\n");
        write_flash_cr_psiz(sl, 2, BANK_1);
      } else {
        ILOG("Target voltage (%d mV) too low for 32-bit flash, using 8-bit flash writes\n", voltage);
        write_flash_cr_psiz(sl, 0, BANK_1);
      }
    }

    // set programming mode
    set_flash_cr_pg(sl, BANK_1);
  } elseif(sl->flash_type == STM32_FLASH_TYPE_WB_WL ||
             sl->flash_type == STM32_FLASH_TYPE_G0 ||
             sl->flash_type == STM32_FLASH_TYPE_G4 ||
             sl->flash_type == STM32_FLASH_TYPE_L5_U5_H5 ||
             sl->flash_type == STM32_FLASH_TYPE_C0) {
    ILOG("Starting Flash write for WB/G0/G4/L5/U5/H5/C0\n");

    unlock_flash_if(sl);         // unlock flash if necessary
    set_flash_cr_pg(sl, BANK_1); // set PG 'allow programming' bit
  } elseif(sl->flash_type == STM32_FLASH_TYPE_L0_L1) {
    ILOG("Starting Flash write for L0\n");

    uint32_t val;
    uint32_t flash_regs_base = get_stm32l0_flash_base(sl);

    // disable pecr protection
    stlink_write_debug32(sl, flash_regs_base + FLASH_PEKEYR_OFF, STM32_FLASH_L0_PEKEY1);
    stlink_write_debug32(sl, flash_regs_base + FLASH_PEKEYR_OFF, STM32_FLASH_L0_PEKEY2);

    // check pecr.pelock is cleared
    stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
    if(val & (1 << 0)) {
      ELOG("pecr.pelock not clear\n");
      return (-1);
    }

    // unlock program memory
    stlink_write_debug32(sl, flash_regs_base + FLASH_PRGKEYR_OFF, STM32_FLASH_L0_PRGKEY1);
    stlink_write_debug32(sl, flash_regs_base + FLASH_PRGKEYR_OFF, STM32_FLASH_L0_PRGKEY2);

    // check pecr.prglock is cleared
    stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
    if(val & (1 << 1)) {
      ELOG("pecr.prglock not clear\n");
      return (-1);
    }

    /* Flash loader initialisation */
    if(stlink_flash_loader_init(sl, fl) == -1) {
      // L0/L1 have fallback to soft write
      WLOG("stlink_flash_loader_init() == -1\n");
    }
  } elseif(sl->flash_type == STM32_FLASH_TYPE_WB0) {
    ILOG("Starting Flash write for WB0\n");

    if(stlink_flash_loader_init(sl, fl) == -1) {
      ELOG("stlink_flash_loader_init() == -1\n");
      return (-1);
    }
  } elseif((sl->flash_type == STM32_FLASH_TYPE_F0_F1_F3) ||
             (sl->flash_type == STM32_FLASH_TYPE_F1_XL)) {
    ILOG("Starting Flash write for VL/F0/F3/F1_XL\n");

    // flash loader initialisation
    if(stlink_flash_loader_init(sl, fl) == -1) {
      ELOG("stlink_flash_loader_init() == -1\n");
      return (-1);
    }

    // unlock flash
    unlock_flash_if(sl);

    // set programming mode
    set_flash_cr_pg(sl, BANK_1);
    if(sl->flash_type == STM32_FLASH_TYPE_F1_XL) {
      set_flash_cr_pg(sl, BANK_2);
    }
  } elseif(sl->flash_type == STM32_FLASH_TYPE_H7) {
    ILOG("Starting Flash write for H7\n");

    unlock_flash_if(sl);         // unlock the cr
    set_flash_cr_pg(sl, BANK_1); // set programming mode
    if(sl->chip_flags & CHIP_F_HAS_DUAL_BANK) {
      set_flash_cr_pg(sl, BANK_2);
    }
    if(sl->chip_id != STM32_CHIPID_H7Ax) {
      // set parallelism
      write_flash_cr_psiz(sl, 3/* 64 bit */, BANK_1);
      if(sl->chip_flags & CHIP_F_HAS_DUAL_BANK) {
        write_flash_cr_psiz(sl, 3/* 64 bit */, BANK_2);
      }
    }
  } else {
    ELOG("unknown coreid, not sure how to write: %x\n", sl->core_id);
    return (-1);
  }

return (0);
}

int32_t stlink_flashloader_write(stlink_t *sl, flash_loader_t *fl, stm32_addr_t addr, uint8_t *base, uint32_t len) {
uint32_t off;
bool is_exclusively_otp = addr >= sl->otp_base && addr < sl->otp_base + sl->otp_size;
bool is_exclusively_flash = addr >= sl->flash_base && addr < sl->flash_base + sl->flash_size;

if((sl->flash_type == STM32_FLASH_TYPE_F2_F4) ||
      (sl->flash_type == STM32_FLASH_TYPE_F7) ||
      (sl->flash_type == STM32_FLASH_TYPE_L4) ||
      (sl->flash_type == STM32_FLASH_TYPE_WB0 && is_exclusively_flash)) {
    uint32_t buf_size = sl->sram_size - 0x1000;
    buf_size = buf_size > 0x8000 ? 0x8000 : buf_size;
    for(off = 0; off < len;) {
      uint32_t size = len - off > buf_size ? buf_size : len - off;
      if(stlink_flash_loader_run(sl, fl, addr + off, base + off, size) == -1) {
        ELOG("stlink_flash_loader_run(%#x) failed! == -1\n", (addr + off));
        check_flash_error(sl);
        return (-1);
      }

      off += size;
    }
  } elseif(sl->flash_type == STM32_FLASH_TYPE_WB_WL ||
             sl->flash_type == STM32_FLASH_TYPE_G0 ||
             sl->flash_type == STM32_FLASH_TYPE_G4 ||
             sl->flash_type == STM32_FLASH_TYPE_L5_U5_H5 ||
             sl->flash_type == STM32_FLASH_TYPE_C0) {

    if(sl->flash_type == STM32_FLASH_TYPE_L5_U5_H5 && (len % 16)) {
        WLOG("Aligning data size to 16 bytes\n");
        len += 16 - len % 16;
    }
    DLOG("Starting %3u page write\n", len / sl->flash_pgsz);
    for(off = 0; off < len; off += sizeof(uint32_t)) {
      uint32_t data;

      if((off % sl->flash_pgsz) > (sl->flash_pgsz - 5)) {
        fprintf(stdout, "%3u/%-3u pages written\n", (off / sl->flash_pgsz + 1), (len / sl->flash_pgsz));
        fflush(stdout);
      }

      // write_uint32((unsigned char *)&data, *(uint32_t *)(base + off));
      data = 0;
      memcpy(&data, base + off, (len - off) < 4 ? (len - off) : 4);
      stlink_write_debug32(sl, addr + off, data);
      wait_flash_busy(sl); // wait for 'busy' bit in FLASH_SR to clear
    }
    fprintf(stdout, "\n");

    // flash writes happen as 2 words at a time
    if((off / sizeof(uint32_t)) % 2 != 0) {
      stlink_write_debug32(sl, addr + off, 0); // write a single word of zeros
      wait_flash_busy(sl); // wait for 'busy' bit in FLASH_SR to clear
    }
  } elseif(sl->flash_type == STM32_FLASH_TYPE_L0_L1) {
    uint32_t val;
    uint32_t flash_regs_base = get_stm32l0_flash_base(sl);
    uint32_t pagesize = (flash_regs_base == STM32_FLASH_L0_REGS_ADDR)? L0_WRITE_BLOCK_SIZE : L1_WRITE_BLOCK_SIZE;

    DLOG("Starting %3u page write\n", len / sl->flash_pgsz);

    off = 0;

    if(len > pagesize) {
      if(stm32l1_write_half_pages(sl, fl, addr, base, len, pagesize)) {
        return (-1);
      } else {
        off = (uint32_t) ((uint64_t) (len / pagesize) * pagesize);
      }
    }

    // write remaining word in program memory
    for(; off < len; off += sizeof(uint32_t)) {
      uint32_t data;

      if((off % sl->flash_pgsz) > (sl->flash_pgsz - 5)) {
        fprintf(stdout, "%3u/%-3u pages written\n", (off / sl->flash_pgsz + 1), (len / sl->flash_pgsz));
        fflush(stdout);
      }

      write_uint32((unsignedchar *)&data, *(uint32_t *)(base + off));
      stlink_write_debug32(sl, addr + off, data);

      // wait for sr.busy to be cleared
      do {
        stlink_read_debug32(sl, flash_regs_base + FLASH_SR_OFF, &val);
      } while ((val & (1 << 0)) != 0);

      // TODO: check redo write operation
    }
    fprintf(stdout, "\n");
  } elseif((sl->flash_type == STM32_FLASH_TYPE_F0_F1_F3) || (sl->flash_type == STM32_FLASH_TYPE_F1_XL)) {
    int32_t write_block_count = 0;
    for(off = 0; off < len; off += sl->flash_pgsz) {
      // adjust last write size
      uint32_t size = len - off > sl->flash_pgsz ? sl->flash_pgsz : len - off;

      // unlock and set programming mode
      unlock_flash_if(sl);

      DLOG("Finished unlocking flash, running loader!\n");

      if(stlink_flash_loader_run(sl, fl, addr + off, base + off, size) == -1) {
        ELOG("stlink_flash_loader_run(%#x) failed! == -1\n", (addr + off));
        check_flash_error(sl);
        return (-1);
      }

      lock_flash(sl);

      if(sl->verbose >= 1) {
        // show progress; writing procedure is slow and previous errors are
        // misleading
        fprintf(stdout, "%3u/%-3u pages written\n", ++write_block_count,
                (len + sl->flash_pgsz - 1) / sl->flash_pgsz);
        fflush(stdout);
      }
    }
    if(sl->verbose >= 1) {
      fprintf(stdout, "\n");
    }
  } elseif(sl->flash_type == STM32_FLASH_TYPE_H7) {
    for(off = 0; off < len;) {
      // Program STM32H7x with 64-byte Flash words
      uint32_t chunk = (len - off > 64) ? 64 : len - off;
      memcpy(sl->q_buf, base + off, chunk);
      stlink_write_mem32(sl, addr + off, 64);
      wait_flash_busy(sl);

      off += chunk;

      if(sl->verbose >= 1) {
        // show progress
        fprintf(stdout, "%u/%u bytes written\n", off, len);
        fflush(stdout);
      }
    }
    if(sl->verbose >= 1) {
      fprintf(stdout, "\n");
    }
  } elseif((sl->flash_type == STM32_FLASH_TYPE_WB0) && is_exclusively_otp) {
    // WB0 OTP area can not be written with BURSTWRITE as implemented in flashloader
    // Writes are done as 32bit words, out of bounds bytes are written as 0xFF (no change to flash)
    for(off = 0; off < len; ) {
      uint32_t current_addr = addr + off;
      uint32_t remaining = len - off;
      uint32_t word_base = current_addr & (~0x03);
      uint32_t unused_front = current_addr - word_base;
      uint32_t max_bytes = 4 - unused_front;
      uint32_t bytes_to_copy = remaining < max_bytes ? remaining : max_bytes;

      uint32_t data_word = 0xFFFFFFFF;
      memcpy(((uint8_t*)&data_word) + unused_front, base + off, bytes_to_copy);
      
      stlink_write_debug32(sl, STM32_FLASH_WB0_IRQRAW, STM32_FLASH_WB0_IRQ_ALL);
      stlink_write_debug32(sl, STM32_FLASH_WB0_DATA0, data_word);
      stlink_write_debug32(sl, STM32_FLASH_WB0_ADDRESS, (current_addr) >> 2);
      stlink_write_debug32(sl, STM32_FLASH_WB0_COMMAND, STM32_FLASH_WB0_CMD_OTPWRITE);
      wait_flash_busy(sl);
      
      if (check_flash_error(sl)) {
        ELOG("Failed to writing OTP word at 0x%08X!\n", current_addr);
        return (-1);
      }
      off += bytes_to_copy;
    }
  } else {
    return (-1);
  }

return check_flash_error(sl);
}

int32_t stlink_flashloader_stop(stlink_t *sl, flash_loader_t *fl) {
uint32_t dhcsr;

if((sl->flash_type == STM32_FLASH_TYPE_C0) ||
      (sl->flash_type == STM32_FLASH_TYPE_F0_F1_F3) ||
      (sl->flash_type == STM32_FLASH_TYPE_F1_XL) ||
      (sl->flash_type == STM32_FLASH_TYPE_F2_F4) ||
      (sl->flash_type == STM32_FLASH_TYPE_F7) ||
      (sl->flash_type == STM32_FLASH_TYPE_G0) ||
      (sl->flash_type == STM32_FLASH_TYPE_G4) ||
      (sl->flash_type == STM32_FLASH_TYPE_H7) ||
      (sl->flash_type == STM32_FLASH_TYPE_L4) ||
      (sl->flash_type == STM32_FLASH_TYPE_L5_U5_H5) ||
      (sl->flash_type == STM32_FLASH_TYPE_WB_WL)) {

    clear_flash_cr_pg(sl, BANK_1);
    if((sl->flash_type == STM32_FLASH_TYPE_H7 && sl->chip_flags & CHIP_F_HAS_DUAL_BANK) ||
        sl->flash_type == STM32_FLASH_TYPE_F1_XL) {
      clear_flash_cr_pg(sl, BANK_2);
    }
    lock_flash(sl);
  } elseif(sl->flash_type == STM32_FLASH_TYPE_L0_L1) {
    uint32_t val;
    uint32_t flash_regs_base = get_stm32l0_flash_base(sl);

    // reset lock bits
    stlink_read_debug32(sl, flash_regs_base + FLASH_PECR_OFF, &val);
    val |= (1 << 0) | (1 << 1) | (1 << 2);
    stlink_write_debug32(sl, flash_regs_base + FLASH_PECR_OFF, val);
  }

// enable interrupt
if(!stlink_read_debug32(sl, STM32_REG_DHCSR, &dhcsr)) {
    stlink_write_debug32(sl, STM32_REG_DHCSR, STM32_REG_DHCSR_DBGKEY | STM32_REG_DHCSR_C_DEBUGEN |
                         (dhcsr & (~STM32_REG_DHCSR_C_MASKINTS)));
  }

// restore DMA state
  set_dma_state(sl, fl, 1);

return (0);
}