Merge pull request #82 from gsmcmullin/flashstubs

Generalise flash stub calls

flashstub/Makefile

@@ -0,0 +1,41 @@
+CROSS_COMPILE ?= arm-none-eabi-
+AS = $(CROSS_COMPILE)as
+CC = $(CROSS_COMPILE)gcc
+OBJCOPY = $(CROSS_COMPILE)objcopy
+HEXDUMP = hexdump
+
+ifneq ($(V), 1)
+Q = @
+endif
+
+CFLAGS=-Os -std=gnu99 -mcpu=cortex-m3 -mthumb -I../libopencm3/include
+ASFLAGS=-mcpu=cortex-m3 -mthumb
+
+all:	lmi.stub stm32f4.stub nrf51.stub stm32f1.stub
+
+stm32f1.o: stm32f1.c
+	$(Q)echo "  CC      $<"
+	$(Q)$(CC) $(CFLAGS) -DSTM32F1 -o $@ -c $<
+
+stm32f4.o: stm32f4.c
+	$(Q)echo "  CC      $<"
+	$(Q)$(CC) $(CFLAGS) -DSTM32F4 -o $@ -c $<
+
+%.o:	%.s
+	$(Q)echo "  AS      $<"
+	$(Q)$(AS) $(ASFLAGS) -o $@ $<
+
+%.bin:	%.o
+	$(Q)echo "  OBJCOPY $@"
+	$(Q)$(OBJCOPY) -O binary $< $@
+
+%.stub:	%.bin
+	$(Q)echo "  HEXDUMP $@"
+	$(Q)$(HEXDUMP) -v -e '/2 "0x%04X, "' $< > $@
+
+.PHONY: clean
+
+clean:
+	$(Q)echo "  CLEAN"
+	-$(Q)rm -f *.o *.bin *.stub
+

flashstub/stm32.s

@@ -1,43 +0,0 @@
-.global _start
-
-_start:
-	ldr r0, _flashbase
-	ldr r1, _addr
-	mov r2, pc
-	add r2, #(_data - . - 2)
-	ldr r3, _size
-	mov r5, #1
-_next:
-	cmp r3, #0
-	beq _done
-	@ Write PG command to FLASH_CR
-	str r5, [r0, #0x10]
-	@ Write data to flash (half-word)
-	ldrh r4, [r2]
-	strh r4, [r1]
-
-_wait:	@ Wait for BSY bit to clear
-	ldr r4, [r0, #0x0C]
-	mov r6, #1
-	tst r4, r6
-	bne _wait
-
-	sub r3, #2
-	add r1, #2
-	add r2, #2
-	b _next
-_done:
-	bkpt
-
-@.align 4
-.org 0x28
-_flashbase:
-	.word 0x40022000
-_addr:
-	.word 0
-_size:
-	.word 12
-_data:
-	.word 0xAAAAAAAA
-	.word 0xBBBBBBBB
-	.word 0xCCCCCCCC

flashstub/stm32f1.c

@@ -0,0 +1,40 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2015  Black Sphere Technologies Ltd.
+ * Written by Gareth McMullin <gareth@blacksphere.co.nz>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include "libopencm3/stm32/flash.h"
+#include "stub.h"
+
+#define SR_ERROR_MASK 0x14
+
+void __attribute__((naked))
+stm32f1_flash_write_stub(uint16_t *dest, uint16_t *src, uint32_t size)
+{
+	for (int i; i < size; i += 2) {
+		FLASH_CR = FLASH_CR_PG;
+		*dest++ = *src++;
+		while (FLASH_SR & FLASH_SR_BSY)
+			;
+	}
+
+	if (FLASH_SR & SR_ERROR_MASK)
+		stub_exit(1);
+
+	stub_exit(0);
+}
+

flashstub/stm32f1.stub

@@ -0,0 +1 @@
+0x2300, 0x4293, 0x4C09, 0xD20A, 0x4D09, 0x2601, 0x602E, 0x5ACD, 0x52C5, 0x6825, 0xF015, 0x0F01, 0xD1FB, 0x3302, 0xE7F1, 0x6823, 0xF013, 0x0F14, 0xD000, 0xBE01, 0xBE00, 0xBF00, 0x200C, 0x4002, 0x2010, 0x4002, 

flashstub/stm32f4.c

@@ -0,0 +1,40 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2015  Black Sphere Technologies Ltd.
+ * Written by Gareth McMullin <gareth@blacksphere.co.nz>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include "libopencm3/stm32/flash.h"
+#include "stub.h"
+
+#define SR_ERROR_MASK 0xF2
+
+void __attribute__((naked))
+stm32f4_flash_write_stub(uint32_t *dest, uint32_t *src, uint32_t size)
+{
+	for (int i = 0; i < size; i += 4) {
+		FLASH_CR = FLASH_CR_PROGRAM_X32 | FLASH_CR_PG;
+		*dest++ = *src++;
+		while (FLASH_SR & FLASH_SR_BSY)
+			;
+	}
+
+	if (FLASH_SR & SR_ERROR_MASK)
+		stub_exit(1);
+
+	stub_exit(0);
+}
+

flashstub/stm32f4.s

@@ -1,44 +0,0 @@
-.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

flashstub/stm32f4.stub

@@ -0,0 +1 @@
+0x2300, 0x4293, 0x4C09, 0xD20B, 0x4D09, 0xF240, 0x2601, 0x602E, 0x58CD, 0x50C5, 0x6825, 0xF415, 0x3F80, 0xD1FB, 0x3304, 0xE7F0, 0x6823, 0xF013, 0x0FF2, 0xD000, 0xBE01, 0xBE00, 0x3C0C, 0x4002, 0x3C10, 0x4002, 

flashstub/stub.h

@@ -0,0 +1,30 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2015  Black Sphere Technologies Ltd.
+ * Written by Gareth McMullin <gareth@blacksphere.co.nz>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#ifndef __STUB_H
+#define __STUB_H
+
+static inline void __attribute__((always_inline))
+stub_exit(const int code)
+{
+	asm("bkpt %0"::"i"(code));
+}
+
+#endif
+

src/cortexm.c

@@ -60,6 +60,7 @@ const struct command_s cortexm_cmd_list[] = {
 static int cortexm_regs_read(struct target_s *target, void *data);
 static int cortexm_regs_write(struct target_s *target, const void *data);
 static int cortexm_pc_write(struct target_s *target, const uint32_t val);
+static uint32_t cortexm_pc_read(struct target_s *target);
 
 static void cortexm_reset(struct target_s *target);
 static int cortexm_halt_wait(struct target_s *target);
@@ -216,6 +217,7 @@ cortexm_probe(struct target_s *target)
 	target->regs_read = cortexm_regs_read;
 	target->regs_write = cortexm_regs_write;
 	target->pc_write = cortexm_pc_write;
+	target->pc_read = cortexm_pc_read;
 
 	target->reset = cortexm_reset;
 	target->halt_request = cortexm_halt_request;
@@ -603,6 +605,40 @@ static int cortexm_fault_unwind(struct target_s *target)
 	return 0;
 }
 
+int cortexm_run_stub(struct target_s *target, uint32_t loadaddr,
+                     const uint16_t *stub, uint32_t stublen,
+                     uint32_t r0, uint32_t r1, uint32_t r2, uint32_t r3)
+{
+	uint32_t regs[target->regs_size / 4];
+
+	memset(regs, 0, sizeof(regs));
+	regs[0] = r0;
+	regs[1] = r1;
+	regs[2] = r2;
+	regs[3] = r3;
+	regs[15] = loadaddr;
+	regs[16] = 0x1000000;
+	regs[19] = 0;
+
+	target_mem_write(target, loadaddr, stub, stublen);
+	cortexm_regs_write(target, regs);
+
+	if (target_check_error(target))
+		return -1;
+
+	/* Execute the stub */
+	cortexm_halt_resume(target, 0);
+	while (!cortexm_halt_wait(target))
+		;
+
+	uint32_t pc = cortexm_pc_read(target);
+	uint16_t bkpt_instr = target_mem_read16(target, pc);
+	if (bkpt_instr >> 8 != 0xbe)
+		return -2;
+
+	return bkpt_instr & 0xff;
+}
+
 /* The following routines implement hardware breakpoints.
  * The Flash Patch and Breakpoint (FPB) system is used. */
 
@@ -648,7 +684,6 @@ cortexm_clear_hw_bp(struct target_s *target, uint32_t addr)
 	return 0;
 }
 
-
 /* The following routines implement hardware watchpoints.
  * The Data Watch and Trace (DWT) system is used. */
 

src/include/cortexm.h

@@ -146,6 +146,9 @@
 bool cortexm_attach(struct target_s *target);
 void cortexm_detach(struct target_s *target);
 void cortexm_halt_resume(struct target_s *target, bool step);
+int cortexm_run_stub(struct target_s *target, uint32_t loadaddr,
+                     const uint16_t *stub, uint32_t stublen,
+                     uint32_t r0, uint32_t r1, uint32_t r2, uint32_t r3);
 
 #endif
 

src/include/general.h

@@ -38,5 +38,7 @@
 #define DEBUG	printf
 #endif
 
+#define ALIGN(x, n) (((x) + (n) - 1) & ~((n) - 1))
+
 #endif
 

src/stm32f1.c

@@ -32,6 +32,7 @@
 #include "general.h"
 #include "adiv5.h"
 #include "target.h"
+#include "cortexm.h"
 #include "command.h"
 #include "gdb_packet.h"
 
@@ -120,42 +121,12 @@ static const char stm32hd_xml_memory_map[] = "<?xml version=\"1.0\"?>"
 #define DBGMCU_IDCODE_F0	0x40015800
 
 static const uint16_t stm32f1_flash_write_stub[] = {
-// _start:
+#include "../flashstub/stm32f1.stub"
-	0x4809,	// ldr r0, [pc, #36] // _flashbase
-	0x490a,	// ldr r1, [pc, #40] // _addr
-	0x467a, // mov r2, pc
-	0x322c, // adds r2, #44
-	0x4b09, // ldr r3, [pc, #36] // _size
- 	0x2501, // movs r5, #1
-// _next:
-	0x2b00, // cmp r3, #0
-	0xd00a, // beq _done
-	0x6105, // str r5, [r0, #16]
-	0x8814, // ldrh r4, [r2]
-	0x800c, // strh r4, [r1]
-// _wait:
-	0x68c4, // ldr r4, [r0, #12]
-	0x2601, // movs r6, #1
-	0x4234, // tst r4, r6
-	0xd1fb, // bne _wait
-
-	0x3b02, // subs r3, #2
-	0x3102, // adds r1, #2
-	0x3202, // adds r2, #2
-	0xe7f2, // b _next
-// _done:
-	0xbe00, // bkpt
-// .org 0x28
-// _flashbase:
- 	0x2000, 0x4002, // .word 0x40022000 (FPEC_BASE)
-// _addr:
-// 	0x0000, 0x0000,
-// _size:
-// 	0x0000, 0x0000,
-// _data:
-// 	...
 };
 
+#define SRAM_BASE 0x20000000
+#define STUB_BUFFER_BASE ALIGN(SRAM_BASE + sizeof(stm32f1_flash_write_stub), 4)
+
 bool stm32f1_probe(struct target_s *target)
 {
 	target->idcode = target_mem_read32(target, DBGMCU_IDCODE) & 0xfff;
@@ -277,34 +248,19 @@ static int stm32f1_flash_write(struct target_s *target, uint32_t dest,
                                const uint8_t *src, size_t len)
 {
 	uint32_t offset = dest % 4;
-	uint32_t words = (offset + len + 3) / 4;
+	uint8_t data[ALIGN(offset + len, 4)];
-	if (words > 256)
-		return -1;
-	uint32_t data[2 + words];
 
 	/* Construct data buffer used by stub */
-	data[0] = dest - offset;
+	/* pad partial words with all 1s to avoid damaging overlapping areas */
-	data[1] = words * 4;		/* length must always be a multiple of 4 */
+	memset(data, 0xff, sizeof(data));
-	data[2] = 0xFFFFFFFF;		/* pad partial words with all 1s to avoid */
+	memcpy((uint8_t *)data + offset, src, len);
-	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 */
-	target_mem_write(target, 0x20000000, stm32f1_flash_write_stub, 0x2C);
+	target_mem_write(target, STUB_BUFFER_BASE, (void*)data, sizeof(data));
-	target_mem_write(target, 0x2000002C, data, sizeof(data));
+	return cortexm_run_stub(target, SRAM_BASE, stm32f1_flash_write_stub,
-	target_pc_write(target, 0x20000000);
+	                        sizeof(stm32f1_flash_write_stub),
-	if(target_check_error(target))
+	                        dest - offset, STUB_BUFFER_BASE, sizeof(data),
-		return -1;
+	                        0);
-
-	/* Execute the stub */
-	target_halt_resume(target, 0);
-	while(!target_halt_wait(target));
-
-	/* Check for error */
-	if (target_mem_read32(target, FLASH_SR) & SR_ERROR_MASK)
-		return -1;
-
-	return 0;
 }
 
 static bool stm32f1_cmd_erase_mass(target *t)

src/stm32f4.c

@@ -33,6 +33,7 @@
 #include "general.h"
 #include "adiv5.h"
 #include "target.h"
+#include "cortexm.h"
 #include "command.h"
 #include "gdb_packet.h"
 
@@ -120,44 +121,12 @@ static const char stm32f4_xml_memory_map[] = "<?xml version=\"1.0\"?>"
 
 /* This routine is uses word access.  Only usable on target voltage >2.7V */
 static const uint16_t stm32f4_flash_write_stub[] = {
-// _start:
+#include "../flashstub/stm32f4.stub"
-	0x480a,	// ldr r0, [pc, #40] // _flashbase
-	0x490b,	// ldr r1, [pc, #44] // _addr
-	0x467a, // mov r2, pc
-	0x3230, // adds r2, #48
-	0x4b0a, // ldr r3, [pc, #36] // _size
- 	0x4d07, // ldr r5, [pc, #28] // _cr
-// _next:
- 	0xb153, // cbz r3, _done
-	0x6105, // str r5, [r0, #16]
-	0x6814, // ldr r4, [r2]
-	0x600c, // str r4, [r1]
-// _wait:
-	0x89c4, // ldrb r4, [r0, #14]
-	0x2601, // movs r6, #1
-	0x4234, // tst r4, r6
-	0xd1fb, // bne _wait
-
-	0x3b04, // subs r3, #4
-	0x3104, // adds r1, #4
-	0x3204, // adds r2, #4
-	0xe7f3, // b _next
-// _done:
-	0xbe00, // bkpt
-	0x0000,
-// .org 0x28
-//_cr:
-	0x0201, 0x0000, //.word 0x00000201 (Value to write to FLASH_CR) */
-// _flashbase:
- 	0x3c00, 0x4002, // .word 0x40023c00 (FPEC_BASE)
-// _addr:
-// 	0x0000, 0x0000,
-// _size:
-// 	0x0000, 0x0000,
-// _data:
-// 	...
 };
 
+#define SRAM_BASE 0x20000000
+#define STUB_BUFFER_BASE ALIGN(SRAM_BASE + sizeof(stm32f4_flash_write_stub), 4)
+
 bool stm32f4_probe(struct target_s *target)
 {
 	uint32_t idcode;
@@ -239,34 +208,19 @@ static int stm32f4_flash_write(struct target_s *target, uint32_t dest,
                                const uint8_t *src, size_t len)
 {
 	uint32_t offset = dest % 4;
-	uint32_t words = (offset + len + 3) / 4;
+	uint8_t data[ALIGN(offset + len, 4)];
-	uint32_t data[2 + words];
-	uint16_t sr;
 
 	/* Construct data buffer used by stub */
-	data[0] = dest - offset;
+	/* pad partial words with all 1s to avoid damaging overlapping areas */
-	data[1] = words * 4;		/* length must always be a multiple of 4 */
+	memset(data, 0xff, sizeof(data));
-	data[2] = 0xFFFFFFFF;		/* pad partial words with all 1s to avoid */
+	memcpy((uint8_t *)data + offset, src, len);
-	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 */
+	/* Write buffer to target ram call stub */
-	target_mem_write(target, 0x20000000, stm32f4_flash_write_stub, 0x30);
+	target_mem_write(target, STUB_BUFFER_BASE, data, sizeof(data));
-	target_mem_write(target, 0x20000030, data, sizeof(data));
+	return cortexm_run_stub(target, SRAM_BASE, stm32f4_flash_write_stub,
-	target_pc_write(target, 0x20000000);
+	                        sizeof(stm32f4_flash_write_stub),
-	if(target_check_error(target))
+	                        dest - offset, STUB_BUFFER_BASE, sizeof(data),
-		return -1;
+	                        0);
-
-	/* Execute the stub */
-	target_halt_resume(target, 0);
-	while(!target_halt_wait(target));
-
-	/* Check for error */
-	sr = target_mem_read32(target, FLASH_SR);
-	if(sr & SR_ERROR_MASK)
-		return -1;
-
-	return 0;
 }
 
 static bool stm32f4_cmd_erase_mass(target *t)