From 66d9a24b066c137b80677c1f6dd929b9c20cfc6b Mon Sep 17 00:00:00 2001
From: Tomas Vanek <vanekt@fbl.cz>
Date: Thu, 1 Oct 2015 23:35:12 +0200
Subject: [PATCH] Kinetis: fix preparation of FlexRAM before flash programming

FlexRAM should be requested before any section programming.
Test FCNFG RAMRDY bit before issuing FTFx_CMD_SETFLEXRAM
to speed up operation and to cover pflash only devices.

Change-Id: Ib0f2d8e8ab8b1507cbf2b7f8565178ab79941f5d
Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Reviewed-on: http://openocd.zylin.com/2990
Tested-by: jenkins
---
 src/flash/nor/kinetis.c | 66 ++++++++++++++++++++++++-----------------
 1 file changed, 38 insertions(+), 28 deletions(-)

diff --git a/src/flash/nor/kinetis.c b/src/flash/nor/kinetis.c
index 5b618c03e..a1625ffee 100644
--- a/src/flash/nor/kinetis.c
+++ b/src/flash/nor/kinetis.c
@@ -984,11 +984,41 @@ static int kinetis_erase(struct flash_bank *bank, int first, int last)
 	return ERROR_OK;
 }
 
+static int kinetis_make_ram_ready(struct target *target)
+{
+	int result;
+	uint8_t ftfx_fstat;
+	uint8_t ftfx_fcnfg;
+
+	/* check if ram ready */
+	result = target_read_memory(target, FTFx_FCNFG, 1, 1, &ftfx_fcnfg);
+	if (result != ERROR_OK)
+		return result;
+
+	if (ftfx_fcnfg & (1 << 1))
+		return ERROR_OK;	/* ram ready */
+
+	/* make flex ram available */
+	result = kinetis_ftfx_command(target, FTFx_CMD_SETFLEXRAM, 0x00ff0000,
+				 0, 0, 0, 0,  0, 0, 0, 0,  &ftfx_fstat);
+	if (result != ERROR_OK)
+		return ERROR_FLASH_OPERATION_FAILED;
+
+	/* check again */
+	result = target_read_memory(target, FTFx_FCNFG, 1, 1, &ftfx_fcnfg);
+	if (result != ERROR_OK)
+		return result;
+
+	if (ftfx_fcnfg & (1 << 1))
+		return ERROR_OK;	/* ram ready */
+
+	return ERROR_FLASH_OPERATION_FAILED;
+}
+
 static int kinetis_write(struct flash_bank *bank, const uint8_t *buffer,
 			 uint32_t offset, uint32_t count)
 {
 	unsigned int i, result, fallback = 0;
-	uint8_t buf[8];
 	uint32_t wc;
 	struct kinetis_flash_bank *kinfo = bank->driver_priv;
 	uint8_t *new_buffer = NULL;
@@ -1002,36 +1032,16 @@ static int kinetis_write(struct flash_bank *bank, const uint8_t *buffer,
 		/* fallback to longword write */
 		fallback = 1;
 		LOG_WARNING("This device supports Program Longword execution only.");
-		LOG_DEBUG("flash write into PFLASH @08%" PRIX32, offset);
-
-	} else if (kinfo->flash_class == FC_FLEX_NVM) {
-		uint8_t ftfx_fstat;
-
-		LOG_DEBUG("flash write into FlexNVM @%08" PRIX32, offset);
-
-		/* make flex ram available */
-		result = kinetis_ftfx_command(bank->target, FTFx_CMD_SETFLEXRAM, 0x00ff0000,
-				0, 0, 0, 0,  0, 0, 0, 0,  &ftfx_fstat);
-
-		if (result != ERROR_OK)
-			return ERROR_FLASH_OPERATION_FAILED;
-
-		/* check if ram ready */
-		result = target_read_memory(bank->target, FTFx_FCNFG, 1, 1, buf);
-
-		if (result != ERROR_OK)
-			return result;
-
-		if (!(buf[0] & (1 << 1))) {
-			/* fallback to longword write */
-			fallback = 1;
-
-			LOG_WARNING("ram not ready, fallback to slow longword write (FCNFG: %02X)", buf[0]);
-		}
 	} else {
-		LOG_DEBUG("flash write into PFLASH @08%" PRIX32, offset);
+		result = kinetis_make_ram_ready(bank->target);
+		if (result != ERROR_OK) {
+			fallback = 1;
+			LOG_WARNING("FlexRAM not ready, fallback to slow longword write.");
+		}
 	}
 
+	LOG_DEBUG("flash write @08%" PRIX32, offset);
+
 
 	/* program section command */
 	if (fallback == 0) {