From 39ccb3fbe93ac6a0fa789511bec86aac0a6d5754 Mon Sep 17 00:00:00 2001
From: Angus Gratton <gus@projectgus.com>
Date: Fri, 27 Jun 2014 16:13:24 +1000
Subject: [PATCH 3/4] nrf51: Add flash loader code (doesn't yet work)

Change-Id: I4d242397bf8e916b5c6b6c9182c895308a9c7341
---
 contrib/loaders/flash/cortex-m0.S |  75 +++++++++++++++++
 src/flash/nor/nrf51.c             | 169 +++++++++++++++++++++++++++-----------
 tcl/target/nrf51_stlink.tcl       |   4 +-
 3 files changed, 196 insertions(+), 52 deletions(-)
 create mode 100644 contrib/loaders/flash/cortex-m0.S

diff --git a/contrib/loaders/flash/cortex-m0.S b/contrib/loaders/flash/cortex-m0.S
new file mode 100644
index 0000000..6a6c84d
--- /dev/null
+++ b/contrib/loaders/flash/cortex-m0.S
@@ -0,0 +1,75 @@
+/***************************************************************************
+ *   Copyright (C) 2014 by Angus Gratton                                   *
+ *   Derived from stm32f1x.S:
+ *   Copyright (C) 2011 by Andreas Fritiofson                              *
+ *   andreas.fritiofson@gmail.com                                          *
+ *   Copyright (C) 2013 by Roman Dmitrienko                                *
+ *   me@iamroman.org                                                       *
+ *                                                                         *
+ *   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 2 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, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.           *
+ ***************************************************************************/
+	.text
+	.syntax unified
+	.cpu cortex-m0
+	.thumb
+	.thumb_func
+
+/* Written for NRF51822 (src/flash/nor/nrf51.c) however the NRF NVMC is
+ * very generic (CPU blocks during flash writes), so this is actually
+ * just a generic word-oriented copy routine for ARMv6M/cortex-m0.
+ *
+ * To assemble:
+ * arm-none-eabi-gcc -c -o cortex-m0.o -x assembler-with-cpp cortex-m0.S
+ *
+ * To disassemble:
+ * arm-none-eabi-objdump -o cortex-m0.o
+ */
+
+	/* Params:
+	 * r0 - byte count (in)
+	 * r1 - workarea start
+	 * r2 - workarea end
+	 * r3 - target address
+	 * Clobbered:
+	 * r4 - rp
+	 * r5 - wp, tmp
+	 * r6 - tmp
+	 */
+
+wait_fifo:
+	ldr 	r5, [r1, #0]	/* read wp */
+	cmp 	r5, #0			/* abort if wp == 0 */
+	beq 	exit
+	ldr 	r4, [r1, #4]	/* read rp */
+	cmp 	r4, r5		/* wait until rp != wp */
+	beq 	wait_fifo
+
+	ldr	r5, [r4]	/* "*target_address++ = *rp++" */
+	str	r5, [r3]
+	adds	r4, #4
+	adds	r3, #4
+
+        cmp 	r4, r2			/* wrap rp at end of work area buffer */
+	bcc	no_wrap
+	mov	r4, r1
+	adds	r4, #8 /* skip rp,wp at start of work area */
+no_wrap:
+	str 	r4, [r1, #4]	/* *wp = *rp; */
+	subs	r0, #4      /* decrement byte count */
+	cmp     r0, #0
+	bne     wait_fifo		/* loop if not done */
+exit:
+	bkpt    #0
diff --git a/src/flash/nor/nrf51.c b/src/flash/nor/nrf51.c
index adb4004..b20e26e 100644
--- a/src/flash/nor/nrf51.c
+++ b/src/flash/nor/nrf51.c
@@ -23,6 +23,8 @@
 #endif
 
 #include "imp.h"
+#include <target/algorithm.h>
+#include <target/armv7m.h>
 
 enum {
 	NRF51_FLASH_BASE = 0x00000000,
@@ -600,55 +602,140 @@ static int nrf51_erase_page(struct nrf51_info *chip, struct flash_sector *sector
 	return res;
 }
 
-static int nrf51_ll_flash_write(struct nrf51_info *chip, uint32_t offset, const uint8_t *buffer, uint32_t buffer_size)
-{
-	int res;
-	assert(buffer_size % 4 == 0);
+static const uint16_t nrf51_flash_write_code[] = {
+	/* See contrib/loaders/flash/cortex-m0.S */
+/*<wait_fifo>:*/
+	0x680d,      	/* ldr	r5, [r1, #0] */
+	0x2d00,      	/* cmp	r5, #0 */
+	0xd00e,      	/* beq.n	24 <exit> */
+	0x684c,      	/* ldr	r4, [r1, #4] */
+	0x42ac,      	/* cmp	r4, r5 */
+	0xd0f9,      	/* beq.n	0 <wait_fifo> */
+	0x6825,      	/* ldr	r5, [r4, #0] */
+	0x601d,      	/* str	r5, [r3, #0] */
+	0x3404,      	/* adds	r4, #4 */
+	0x3304,      	/* adds	r3, #4 */
+	0x4294,      	/* cmp	r4, r2 */
+	0xd301,      	/* bcc.n	1c <no_wrap> */
+	0x460c,      	/* mov	r4, r1 */
+	0x3408,      	/* adds	r4, #8 */
+/*<no_wrap>:*/
+	0x604c,      	/* str	r4, [r1, #4] */
+	0x3804,      	/* subs	r0, #4 */
+	0x2800,      	/* cmp	r0, #0 */
+	0xd1ed,      	/* bne.n	0 <wait_fifo> */
+/*<exit>:*/
+	0xbe00,      	/* bkpt	0x0000 */
+};
 
-	for (; buffer_size > 0; buffer_size -= 4) {
-		res = target_write_memory(chip->target, offset, 4, 1, buffer);
-		if (res != ERROR_OK)
-			return res;
 
-		res = nrf51_wait_for_nvmc(chip);
-		if (res != ERROR_OK)
-			return res;
+/* Start a low level flash write for the specified region */
+static int nrf51_ll_flash_write(struct nrf51_info *chip, uint32_t offset, const uint8_t *buffer, uint32_t bytes)
+{
+	struct target *target = chip->target;
+	uint32_t buffer_size = 8192;
+	struct working_area *write_algorithm;
+	struct working_area *source;
+	uint32_t address = NRF51_FLASH_BASE + offset;
+	struct reg_param reg_params[4];
+	struct armv7m_algorithm armv7m_info = { 0 };
+	int retval = ERROR_OK;
+
+	assert(bytes % 4 == 0);
+
+	/* allocate working area with flash programming code */
+	if (target_alloc_working_area(target, sizeof(nrf51_flash_write_code),
+			&write_algorithm) != ERROR_OK) {
+		LOG_WARNING("no working area available, can't do block memory writes");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
 
-		offset += 4;
-		buffer += 4;
+	retval = target_write_buffer(target, write_algorithm->address,
+                                sizeof(nrf51_flash_write_code),
+                                (const uint8_t *)nrf51_flash_write_code);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* memory buffer */
+	while (target_alloc_working_area(target, buffer_size, &source) != ERROR_OK) {
+		buffer_size /= 4;
+		buffer_size &= ~3UL; /* Make sure it's 4 byte aligned */
+		if (buffer_size <= 256) {
+			/* free working area, write algorithm already allocated */
+			target_free_working_area(target, write_algorithm);
+
+			LOG_WARNING("No large enough working area available, can't do block memory writes");
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
 	}
 
-	return ERROR_OK;
+	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
+	armv7m_info.core_mode = ARM_MODE_THREAD;
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT);	/* byte count */
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);	/* buffer start */
+	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);	/* buffer end */
+	init_reg_param(&reg_params[3], "r3", 32, PARAM_IN_OUT);	/* target address */
+
+	buf_set_u32(reg_params[0].value, 0, 32, bytes);
+	buf_set_u32(reg_params[1].value, 0, 32, source->address);
+	buf_set_u32(reg_params[2].value, 0, 32, source->address + source->size);
+	buf_set_u32(reg_params[3].value, 0, 32, address);
+
+	retval = target_run_flash_async_algorithm(target, buffer, bytes/4, 4,
+			0, NULL,
+			4, reg_params,
+			source->address, source->size,
+			write_algorithm->address, 0,
+			&armv7m_info);
+
+	target_free_working_area(target, source);
+	target_free_working_area(target, write_algorithm);
+
+	destroy_reg_param(&reg_params[0]);
+	destroy_reg_param(&reg_params[1]);
+	destroy_reg_param(&reg_params[2]);
+	destroy_reg_param(&reg_params[3]);
+
+	return retval;
 }
 
-static int nrf51_write_page(struct flash_bank *bank, uint32_t offset, const uint8_t *buffer)
+/* Check and erase flash sectors in specified range then start a low level page write.
+   start/end must be sector aligned. */
+static int nrf51_write_pages(struct flash_bank *bank, uint32_t start, uint32_t end, const uint8_t *buffer)
 {
-	assert(offset % 4 == 0);
 	int res = ERROR_FAIL;
 	struct nrf51_info *chip = bank->driver_priv;
-	struct flash_sector *sector = nrf51_find_sector_by_address(bank, offset);
+	struct flash_sector *sector;
+	uint32_t offset;
 
-	if (!sector)
-		return ERROR_FLASH_SECTOR_INVALID;
+	assert(start % chip->code_page_size == 0);
+	assert(end % chip->code_page_size == 0);
 
-	if (sector->is_protected)
-		goto error;
+	/* Erase all sectors */
+	for(offset = start; offset < end; offset += chip->code_page_size) {
+		sector = nrf51_find_sector_by_address(bank, offset);
+		if (!sector)
+			return ERROR_FLASH_SECTOR_INVALID;
 
-	if (!sector->is_erased) {
-		res = nrf51_erase_page(chip, sector);
-		if (res != ERROR_OK) {
-			LOG_ERROR("Failed to erase sector @ 0x%08"PRIx32, sector->offset);
+		if (sector->is_protected)
 			goto error;
+
+		if (!sector->is_erased) {
+			res = nrf51_erase_page(chip, sector);
+			if (res != ERROR_OK) {
+				LOG_ERROR("Failed to erase sector @ 0x%08"PRIx32, sector->offset);
+				goto error;
+			}
 		}
+		sector->is_erased = 0;
 	}
 
 	res = nrf51_nvmc_write_enable(chip);
 	if (res != ERROR_OK)
 		goto error;
 
-	sector->is_erased = 0;
-
-	res = nrf51_ll_flash_write(chip, offset, buffer, chip->code_page_size);
+	res = nrf51_ll_flash_write(chip, start, buffer, (end - start));
 	if (res != ERROR_OK)
 		goto set_read_only;
 
@@ -657,7 +744,7 @@ static int nrf51_write_page(struct flash_bank *bank, uint32_t offset, const uint
 set_read_only:
 	nrf51_nvmc_read_only(chip);
 error:
-	LOG_ERROR("Failed to write sector @ 0x%08"PRIx32, sector->offset);
+	LOG_ERROR("Failed to write to nrf51 flash");
 	return res;
 }
 
@@ -712,11 +799,7 @@ static int nrf51_code_flash_write(struct flash_bank *bank,
 		       start_extra.buffer,
 		       chip->code_page_size - start_extra.length);
 
-		res = nrf51_write_page(bank,
-				       region.start - start_extra.length,
-				       page);
-		if (res != ERROR_OK)
-			return res;
+                region.start -= start_extra.length;
 	}
 
 	if (end_extra.length) {
@@ -733,25 +816,11 @@ static int nrf51_code_flash_write(struct flash_bank *bank,
 
 		memcpy(page, end_extra.buffer, end_extra.length);
 
-		res = nrf51_write_page(bank,
-				       region.end - end_extra.length,
-				       page);
-		if (res != ERROR_OK)
-			return res;
+                region.end += end_extra.length;
 	}
 
 
-	region.start += start_extra.length;
-	region.end   -= end_extra.length;
-
-	for (uint32_t address = region.start; address < region.end;
-	     address += chip->code_page_size) {
-		res = nrf51_write_page(bank, address, &buffer[address - region.start]);
-
-		if (res != ERROR_OK)
-			return res;
-
-	}
+	nrf51_write_pages(bank, region.start, region.end, buffer);
 
 	return ERROR_OK;
 }
diff --git a/tcl/target/nrf51_stlink.tcl b/tcl/target/nrf51_stlink.tcl
index 40d6d0c..579db00 100644
--- a/tcl/target/nrf51_stlink.tcl
+++ b/tcl/target/nrf51_stlink.tcl
@@ -17,11 +17,11 @@ if { [info exists ENDIAN] } {
 }
 
 # Work-area is a space in RAM used for flash programming
-# By default use 2kB
+# By default use 16kB
 if { [info exists WORKAREASIZE] } {
    set _WORKAREASIZE $WORKAREASIZE
 } else {
-   set _WORKAREASIZE 0x800
+   set _WORKAREASIZE 0x4000
 }
 
 if { [info exists CPUTAPID] } {
-- 
2.0.0

