In write_buffer_to_sram() write as much as possible with write_mem32

[fw/stlink] / src / stlink-common.c

diff --git a/src/stlink-common.c b/src/stlink-common.c
index 28be2d25e06465f692454e3afda219b333d56628..97bc3e14066e98b577e729a067e6f595aadb5e54 100644 (file)
--- a/src/stlink-common.c
+++ b/src/stlink-common.c
@@ -59,7 +59,7 @@
 #define STM32L_FLASH_PRGKEYR (STM32L_FLASH_REGS_ADDR + 0x10)
 #define STM32L_FLASH_OPTKEYR (STM32L_FLASH_REGS_ADDR + 0x14)
 #define STM32L_FLASH_SR (STM32L_FLASH_REGS_ADDR + 0x18)
-#define STM32L_FLASH_OBR (STM32L_FLASH_REGS_ADDR + 0x0c)
+#define STM32L_FLASH_OBR (STM32L_FLASH_REGS_ADDR + 0x1c)
 #define STM32L_FLASH_WRPR (STM32L_FLASH_REGS_ADDR + 0x20)
 
 
@@ -537,6 +537,11 @@ void stlink_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 
 void stlink_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
     DLOG("*** stlink_write_mem8 ***\n");
+    if (len > 0x40 ) { // !!! never ever: Writing more then 0x40 bytes gives unexpected behaviour
+        fprintf(stderr, "Error: Data length > 64: +%d byte.\n",
+                len);
+        return;
+    }
     sl->backend->write_mem8(sl, addr, len);
 }
 
@@ -859,8 +864,16 @@ on_error:
 
 int write_buffer_to_sram(stlink_t *sl, flash_loader_t* fl, const uint8_t* buf, size_t size) {
     /* write the buffer right after the loader */
-    memcpy(sl->q_buf, buf, size);
-    stlink_write_mem8(sl, fl->buf_addr, size);
+    size_t chunk = size & ~0x3;
+    size_t rem   = size & 0x3;
+    if (chunk) {
+        memcpy(sl->q_buf, buf, chunk);
+        stlink_write_mem32(sl, fl->buf_addr, chunk);
+    }
+    if (rem) {
+        memcpy(sl->q_buf, buf+chunk, rem);
+        stlink_write_mem8(sl, (fl->buf_addr)+chunk, rem);
+    }
     return 0;
 }