From 4282244fb4ddee37d1e8dd7c29947cf3e4e23dcf Mon Sep 17 00:00:00 2001
From: Gareth McMullin <gareth@blacksphere.co.nz>
Date: Wed, 8 Feb 2012 20:43:12 +1300
Subject: [PATCH] stm32f4: Fixed chunk padding for flash programming.

---
 flashstub/stm32f4.s | 44 ++++++++++++++++++++++++++++++++++++++++++++
 src/stm32f4.c       |  4 ++--
 2 files changed, 46 insertions(+), 2 deletions(-)
 create mode 100644 flashstub/stm32f4.s

diff --git a/flashstub/stm32f4.s b/flashstub/stm32f4.s
new file mode 100644
index 0000000..fa3fda0
--- /dev/null
+++ b/flashstub/stm32f4.s
@@ -0,0 +1,44 @@
+.global _start
+
+_start:
+	ldr r0, _flashbase
+	ldr r1, _addr
+	mov r2, pc
+	add r2, #(_data - . - 2)
+	ldr r3, _size
+	ldr r5, _cr
+_next:
+	cbz r3, _done
+	@ Write PG command to FLASH_CR
+	str r5, [r0, #0x10]
+	@ Write data to flash (word)
+	ldr r4, [r2]
+	str r4, [r1]
+
+_wait:	@ Wait for BSY bit to clear
+	ldrh r4, [r0, #0x0E]
+	mov r6, #1
+	tst r4, r6
+	bne _wait
+
+	sub r3, #4
+	add r1, #4
+	add r2, #4
+	b _next
+_done:
+	bkpt
+
+@.align 4
+.org 0x28
+_cr:
+	.word 0x00000201
+_flashbase:
+	.word 0x40023C00
+_addr:
+	.word 0x0800bf78
+_size:
+	.word 8
+_data:
+	.word 0xAAAAAAAA
+	.word 0xBBBBBBBB
+	.word 0xCCCCCCCC
diff --git a/src/stm32f4.c b/src/stm32f4.c
index 8a34ac8..b2d3747 100644
--- a/src/stm32f4.c
+++ b/src/stm32f4.c
@@ -100,7 +100,7 @@ uint16_t stm32f4_flash_write_stub[] = {
 	0x467a, // mov r2, pc
 	0x3230, // adds r2, #48
 	0x4b0a, // ldr r3, [pc, #36] // _size
- 	0x4d07, // movs r5, #1
+ 	0x4d07, // ldr r5, [pc, #28] // _cr
 // _next:
  	0xb153, // cbz r3, _done
 	0x6105, // str r5, [r0, #16]
@@ -212,7 +212,7 @@ static int stm32f4_flash_write_words(struct target_s *target, uint32_t dest,
 	data[0] = dest - offset;
 	data[1] = words * 4;		/* length must always be a multiple of 4 */
 	data[2] = 0xFFFFFFFF;		/* pad partial words with all 1s to avoid */
-	data[words - 1] = 0xFFFFFFFF;	/* damaging overlapping areas */
+	data[words + 1] = 0xFFFFFFFF;	/* damaging overlapping areas */
 	memcpy((uint8_t *)&data[2] + offset, src, len);
 
 	/* Write stub and data to target ram and set PC */