nrf51 - Add async loader. Performance on nrf51822QAA/stlink-v2 from ~3.5KiB/s to ~19.5KiB/s.

Change-Id: Ib0bf41a0cec85f0bd5728551f8ad7f6255e4ea04
Signed-off-by: Angus Gratton <gus@projectgus.com>
[spamjunkeater@gmail.com: Cleanup buffer allocation, detect -1 for unknown pages]
Signed-off-by: Erdem U. Altunyurt <spamjunkeater@gmail.com>
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/2204
Tested-by: jenkins
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>

1 files changed, 177 insertions, 88 deletions

diff --git a/src/flash/nor/nrf51.c b/src/flash/nor/nrf51.c
index c5ffa832..af412e28 100644
--- a/src/flash/nor/nrf51.c
+++ b/src/flash/nor/nrf51.c
@@ -1,6 +1,8 @@
 /***************************************************************************
  *   Copyright (C) 2013 Synapse Product Development                        *
  *   Andrey Smirnov <andrew.smironv@gmail.com>                             *
+ *   Angus Gratton <gus@projectgus.com>                                    *
+ *   Erdem U. Altunyurt <spamjunkeater@gmail.com>                          *
  *                                                                         *
  *   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  *
@@ -23,6 +25,9 @@
 #endif
 
 #include "imp.h"
+#include <target/algorithm.h>
+#include <target/armv7m.h>
+#include <helper/types.h>
 
 enum {
 	NRF51_FLASH_BASE = 0x00000000,
@@ -248,6 +253,7 @@ static int nrf51_wait_for_nvmc(struct nrf51_info *chip)
 		alive_sleep(1);
 	} while (timeout--);
 
+	LOG_DEBUG("Timed out waiting for NVMC_READY");
 	return ERROR_FLASH_BUSY;
 }
 
@@ -557,6 +563,7 @@ static struct flash_sector *nrf51_find_sector_by_address(struct flash_bank *bank
 
 static int nrf51_erase_all(struct nrf51_info *chip)
 {
+	LOG_DEBUG("Erasing all non-volatile memory");
 	return nrf51_nvmc_generic_erase(chip,
 					NRF51_NVMC_ERASEALL,
 					0x00000001);
@@ -568,8 +575,11 @@ static int nrf51_erase_page(struct flash_bank *bank,
 {
 	int res;
 
-	if (sector->is_protected)
+	LOG_DEBUG("Erasing page at 0x%"PRIx32, sector->offset);
+	if (sector->is_protected) {
+		LOG_ERROR("Cannot erase protected sector at 0x%x", sector->offset);
 		return ERROR_FAIL;
+	}
 
 	if (bank->base == NRF51_UICR_BASE) {
 		uint32_t ppfc;
@@ -608,55 +618,162 @@ static int nrf51_erase_page(struct flash_bank *bank,
 	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 uint8_t nrf51_flash_write_code[] = {
+	/* See contrib/loaders/flash/cortex-m0.S */
+/* <wait_fifo>: */
+	0x0d, 0x68,		/* ldr	r5,	[r1,	#0] */
+	0x00, 0x2d,		/* cmp	r5,	#0 */
+	0x0b, 0xd0,		/* beq.n	1e <exit> */
+	0x4c, 0x68,		/* ldr	r4,	[r1,	#4] */
+	0xac, 0x42,		/* cmp	r4,	r5 */
+	0xf9, 0xd0,		/* beq.n	0 <wait_fifo> */
+	0x20, 0xcc,		/* ldmia	r4!,	{r5} */
+	0x20, 0xc3,		/* stmia	r3!,	{r5} */
+	0x94, 0x42,		/* cmp	r4,	r2 */
+	0x01, 0xd3,		/* bcc.n	18 <no_wrap> */
+	0x0c, 0x46,		/* mov	r4,	r1 */
+	0x08, 0x34,		/* adds	r4,	#8 */
+/* <no_wrap>: */
+	0x4c, 0x60,		/* str	r4, [r1,	#4] */
+	0x04, 0x38,		/* subs	r0, #4 */
+	0xf0, 0xd1,		/* bne.n	0 <wait_fifo> */
+/* <exit>: */
+	0x00, 0xbe		/* 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;
+	int retval = ERROR_OK;
+
+
+	LOG_DEBUG("Writing buffer to flash offset=0x%"PRIx32" bytes=0x%"PRIx32, offset, bytes);
+	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, falling back to slow memory writes");
+
+		for (; bytes > 0; bytes -= 4) {
+			retval = target_write_memory(chip->target, offset, 4, 1, buffer);
+			if (retval != ERROR_OK)
+				return retval;
+
+			retval = nrf51_wait_for_nvmc(chip);
+			if (retval != ERROR_OK)
+				return retval;
+
+			offset += 4;
+			buffer += 4;
+		}
 
-		offset += 4;
-		buffer += 4;
+		return ERROR_OK;
 	}
 
-	return ERROR_OK;
+	LOG_WARNING("using fast async flash loader. This is currently supported");
+	LOG_WARNING("only with ST-Link and CMSIS-DAP. If you have issues, add");
+	LOG_WARNING("\"set WORKAREASIZE 0\" before sourcing nrf51.cfg to disable it");
+
+	retval = target_write_buffer(target, write_algorithm->address,
+				sizeof(nrf51_flash_write_code),
+				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 /= 2;
+		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;
+		}
+	}
+
+	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);
-
-	if (!sector)
-		return ERROR_FLASH_SECTOR_INVALID;
-
-	if (sector->is_protected)
-		goto error;
+	struct flash_sector *sector;
+	uint32_t offset;
+
+	assert(start % chip->code_page_size == 0);
+	assert(end % chip->code_page_size == 0);
+
+	/* Erase all sectors */
+	for (offset = start; offset < end; offset += chip->code_page_size) {
+		sector = nrf51_find_sector_by_address(bank, offset);
+		if (!sector) {
+			LOG_ERROR("Invalid sector @ 0x%08"PRIx32, offset);
+			return ERROR_FLASH_SECTOR_INVALID;
+		}
 
-	if (sector->is_erased != 1) {
-		res = nrf51_erase_page(bank, chip, sector);
-		if (res != ERROR_OK) {
-			LOG_ERROR("Failed to erase sector @ 0x%08"PRIx32, sector->offset);
+		if (sector->is_protected) {
+			LOG_ERROR("Can't erase protected sector @ 0x%08"PRIx32, offset);
 			goto error;
 		}
+
+		if (sector->is_erased != 1) {	/* 1 = erased, 0= not erased, -1 = unknown */
+			res = nrf51_erase_page(bank, 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;
 
@@ -665,7 +782,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;
 }
 
@@ -689,79 +806,51 @@ static int nrf51_code_flash_write(struct flash_bank *bank,
 				  struct nrf51_info *chip,
 				  const uint8_t *buffer, uint32_t offset, uint32_t count)
 {
-	int res;
-	struct {
-		uint32_t start, end;
-	} region;
 
-	region.start = offset;
-	region.end   = offset + count;
+	int res;
+	/* Need to perform reads to fill any gaps we need to preserve in the first page,
+	   before the start of buffer, or in the last page, after the end of buffer */
+	uint32_t first_page = offset/chip->code_page_size;
+	uint32_t last_page = DIV_ROUND_UP(offset+count, chip->code_page_size);
 
-	struct {
-		size_t   length;
-		const uint8_t *buffer;
-	}  start_extra, end_extra;
+	uint32_t first_page_offset = first_page * chip->code_page_size;
+	uint32_t last_page_offset = last_page * chip->code_page_size;
 
-	start_extra.length	= region.start % chip->code_page_size;
-	start_extra.buffer	= buffer;
-	end_extra.length	= region.end  % chip->code_page_size;
-	end_extra.buffer	= buffer + count - end_extra.length;
+	LOG_DEBUG("Padding write from 0x%08"PRIx32"-0x%08"PRIx32" as 0x%08"PRIx32"-0x%08"PRIx32,
+		offset, offset+count, first_page_offset, last_page_offset);
 
-	if (start_extra.length) {
-		uint8_t page[chip->code_page_size];
+	uint32_t page_cnt = last_page - first_page;
+	uint8_t buffer_to_flash[page_cnt*chip->code_page_size];
 
+	/* Fill in any space between start of first page and start of buffer */
+	uint32_t pre = offset - first_page_offset;
+	if (pre > 0) {
 		res = target_read_memory(bank->target,
-					 region.start - start_extra.length,
-					 1, start_extra.length, page);
-		if (res != ERROR_OK)
-			return res;
-
-		memcpy(page + start_extra.length,
-		       start_extra.buffer,
-		       chip->code_page_size - start_extra.length);
-
-		res = nrf51_write_page(bank,
-				       region.start - start_extra.length,
-				       page);
+					first_page_offset,
+					1,
+					pre,
+					buffer_to_flash);
 		if (res != ERROR_OK)
 			return res;
 	}
 
-	if (end_extra.length) {
-		uint8_t page[chip->code_page_size];
+	/* Fill in main contents of buffer */
+	memcpy(buffer_to_flash+pre, buffer, count);
 
+	/* Fill in any space between end of buffer and end of last page */
+	uint32_t post = last_page_offset - (offset+count);
+	if (post > 0) {
 		/* Retrieve the full row contents from Flash */
 		res = target_read_memory(bank->target,
-					 region.end,
-					 1,
-					 (chip->code_page_size - end_extra.length),
-					 page + end_extra.length);
-		if (res != ERROR_OK)
-			return res;
-
-		memcpy(page, end_extra.buffer, end_extra.length);
-
-		res = nrf51_write_page(bank,
-				       region.end - end_extra.length,
-				       page);
+					offset + count,
+					1,
+					post,
+					buffer_to_flash+pre+count);
 		if (res != ERROR_OK)
 			return res;
 	}
 
-
-	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;
-
-	}
-
-	return ERROR_OK;
+	return nrf51_write_pages(bank, first_page_offset, last_page_offset, buffer_to_flash);
 }
 
 static int nrf51_uicr_flash_write(struct flash_bank *bank,