X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Fflash%2Fcfi.c;h=60d1834da199b0153c0e6b308a0e7b1c46c13555;hp=fea75a741ff4470440c375ae3d03b9ddffdccaa0;hb=8959de9f679cfd0436d731fd91b88a68b9a75fa6;hpb=996acf082c07c654c850317ca35b88c8c86f6935

diff --git a/src/flash/cfi.c b/src/flash/cfi.c
index fea75a741f..60d1834da1 100644
--- a/src/flash/cfi.c
+++ b/src/flash/cfi.c
@@ -1,6 +1,8 @@
 /***************************************************************************
  *   Copyright (C) 2005, 2007 by Dominic Rath                              *
  *   Dominic.Rath@gmx.de                                                   *
+ *   Copyright (C) 2009 Michael Schwingen                                  *
+ *   michael@schwingen.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  *
@@ -21,34 +23,23 @@
 #include "config.h"
 #endif
 
-#include "replacements.h"
-
 #include "cfi.h"
 #include "non_cfi.h"
-
-#include "flash.h"
-#include "target.h"
-#include "log.h"
 #include "armv4_5.h"
-#include "algorithm.h"
 #include "binarybuffer.h"
-#include "types.h"
 
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
 
-int cfi_register_commands(struct command_context_s *cmd_ctx);
-int cfi_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);
-int cfi_erase(struct flash_bank_s *bank, int first, int last);
-int cfi_protect(struct flash_bank_s *bank, int set, int first, int last);
-int cfi_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);
-int cfi_probe(struct flash_bank_s *bank);
-int cfi_auto_probe(struct flash_bank_s *bank);
-int cfi_protect_check(struct flash_bank_s *bank);
-int cfi_info(struct flash_bank_s *bank, char *buf, int buf_size);
+static int cfi_register_commands(struct command_context_s *cmd_ctx);
+static int cfi_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);
+static int cfi_erase(struct flash_bank_s *bank, int first, int last);
+static int cfi_protect(struct flash_bank_s *bank, int set, int first, int last);
+static int cfi_write(struct flash_bank_s *bank, uint8_t *buffer, uint32_t offset, uint32_t count);
+static int cfi_probe(struct flash_bank_s *bank);
+static int cfi_auto_probe(struct flash_bank_s *bank);
+static int cfi_protect_check(struct flash_bank_s *bank);
+static int cfi_info(struct flash_bank_s *bank, char *buf, int buf_size);
 
-int cfi_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+//static int cfi_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 
 #define CFI_MAX_BUS_WIDTH	4
 #define CFI_MAX_CHIP_WIDTH	4
@@ -71,34 +62,19 @@ flash_driver_t cfi_flash =
 	.info = cfi_info
 };
 
-cfi_unlock_addresses_t cfi_unlock_addresses[] =
+static cfi_unlock_addresses_t cfi_unlock_addresses[] =
 {
 	[CFI_UNLOCK_555_2AA] = { .unlock1 = 0x555, .unlock2 = 0x2aa },
 	[CFI_UNLOCK_5555_2AAA] = { .unlock1 = 0x5555, .unlock2 = 0x2aaa },
 };
 
 /* CFI fixups foward declarations */
-void cfi_fixup_0002_erase_regions(flash_bank_t *flash, void *param);
-void cfi_fixup_0002_unlock_addresses(flash_bank_t *flash, void *param);
-void cfi_fixup_atmel_reversed_erase_regions(flash_bank_t *flash, void *param);
-
-/* fixup after identifying JEDEC manufactuer and ID */
-cfi_fixup_t cfi_jedec_fixups[] = {
-	{CFI_MFR_SST, 0x00D4, cfi_fixup_non_cfi, NULL},
-	{CFI_MFR_SST, 0x00D5, cfi_fixup_non_cfi, NULL},
-	{CFI_MFR_SST, 0x00D6, cfi_fixup_non_cfi, NULL},
-	{CFI_MFR_SST, 0x00D7, cfi_fixup_non_cfi, NULL},
-	{CFI_MFR_SST, 0x2780, cfi_fixup_non_cfi, NULL},
-	{CFI_MFR_ST, 0x00D5, cfi_fixup_non_cfi, NULL},
-	{CFI_MFR_ST, 0x00D6, cfi_fixup_non_cfi, NULL},
-	{CFI_MFR_AMD, 0x2223, cfi_fixup_non_cfi, NULL},
-	{CFI_MFR_AMD, 0x22ab, cfi_fixup_non_cfi, NULL},
-	{CFI_MFR_FUJITSU, 0x226b, cfi_fixup_non_cfi, NULL},
-	{0, 0, NULL, NULL}
-};
+static void cfi_fixup_0002_erase_regions(flash_bank_t *flash, void *param);
+static void cfi_fixup_0002_unlock_addresses(flash_bank_t *flash, void *param);
+static void cfi_fixup_atmel_reversed_erase_regions(flash_bank_t *flash, void *param);
 
 /* fixup after reading cmdset 0002 primary query table */
-cfi_fixup_t cfi_0002_fixups[] = {
+static cfi_fixup_t cfi_0002_fixups[] = {
 	{CFI_MFR_SST, 0x00D4, cfi_fixup_0002_unlock_addresses, &cfi_unlock_addresses[CFI_UNLOCK_5555_2AAA]},
 	{CFI_MFR_SST, 0x00D5, cfi_fixup_0002_unlock_addresses, &cfi_unlock_addresses[CFI_UNLOCK_5555_2AAA]},
 	{CFI_MFR_SST, 0x00D6, cfi_fixup_0002_unlock_addresses, &cfi_unlock_addresses[CFI_UNLOCK_5555_2AAA]},
@@ -106,16 +82,19 @@ cfi_fixup_t cfi_0002_fixups[] = {
 	{CFI_MFR_SST, 0x2780, cfi_fixup_0002_unlock_addresses, &cfi_unlock_addresses[CFI_UNLOCK_5555_2AAA]},
 	{CFI_MFR_ATMEL, 0x00C8, cfi_fixup_atmel_reversed_erase_regions, NULL},
 	{CFI_MFR_FUJITSU, 0x226b, cfi_fixup_0002_unlock_addresses, &cfi_unlock_addresses[CFI_UNLOCK_5555_2AAA]},
+	{CFI_MFR_AMIC, 0xb31a, cfi_fixup_0002_unlock_addresses, &cfi_unlock_addresses[CFI_UNLOCK_555_2AA]},
+	{CFI_MFR_MX, 0x225b, cfi_fixup_0002_unlock_addresses, &cfi_unlock_addresses[CFI_UNLOCK_555_2AA]},
+	{CFI_MFR_AMD, 0x225b, cfi_fixup_0002_unlock_addresses, &cfi_unlock_addresses[CFI_UNLOCK_555_2AA]},
 	{CFI_MFR_ANY, CFI_ID_ANY, cfi_fixup_0002_erase_regions, NULL},
 	{0, 0, NULL, NULL}
 };
 
 /* fixup after reading cmdset 0001 primary query table */
-cfi_fixup_t cfi_0001_fixups[] = {
+static cfi_fixup_t cfi_0001_fixups[] = {
 	{0, 0, NULL, NULL}
 };
 
-void cfi_fixup(flash_bank_t *bank, cfi_fixup_t *fixups)
+static void cfi_fixup(flash_bank_t *bank, cfi_fixup_t *fixups)
 {
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 	cfi_fixup_t *f;
@@ -130,9 +109,13 @@ void cfi_fixup(flash_bank_t *bank, cfi_fixup_t *fixups)
 	}
 }
 
-/* inline u32 flash_address(flash_bank_t *bank, int sector, u32 offset) */
-__inline__ u32 flash_address(flash_bank_t *bank, int sector, u32 offset)
+/* inline uint32_t flash_address(flash_bank_t *bank, int sector, uint32_t offset) */
+static __inline__ uint32_t flash_address(flash_bank_t *bank, int sector, uint32_t offset)
 {
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+
+	if (cfi_info->x16_as_x8) offset *= 2;
+
 	/* while the sector list isn't built, only accesses to sector 0 work */
 	if (sector == 0)
 		return bank->base + offset * bank->bus_width;
@@ -148,7 +131,7 @@ __inline__ u32 flash_address(flash_bank_t *bank, int sector, u32 offset)
 
 }
 
-void cfi_command(flash_bank_t *bank, u8 cmd, u8 *cmd_buf)
+static void cfi_command(flash_bank_t *bank, uint8_t cmd, uint8_t *cmd_buf)
 {
 	int i;
 
@@ -178,12 +161,12 @@ void cfi_command(flash_bank_t *bank, u8 cmd, u8 *cmd_buf)
  * flash banks are expected to be made of similar chips
  * the query result should be the same for all
  */
-u8 cfi_query_u8(flash_bank_t *bank, int sector, u32 offset)
+static uint8_t cfi_query_u8(flash_bank_t *bank, int sector, uint32_t offset)
 {
 	target_t *target = bank->target;
-	u8 data[CFI_MAX_BUS_WIDTH];
+	uint8_t data[CFI_MAX_BUS_WIDTH];
 
-	target->type->read_memory(target, flash_address(bank, sector, offset), bank->bus_width, 1, data);
+	target_read_memory(target, flash_address(bank, sector, offset), bank->bus_width, 1, data);
 
 	if (bank->target->endianness == TARGET_LITTLE_ENDIAN)
 		return data[0];
@@ -195,13 +178,13 @@ u8 cfi_query_u8(flash_bank_t *bank, int sector, u32 offset)
  * in case of a bank made of multiple chips,
  * the individual values are ORed
  */
-u8 cfi_get_u8(flash_bank_t *bank, int sector, u32 offset)
+static uint8_t cfi_get_u8(flash_bank_t *bank, int sector, uint32_t offset)
 {
 	target_t *target = bank->target;
-	u8 data[CFI_MAX_BUS_WIDTH];
+	uint8_t data[CFI_MAX_BUS_WIDTH];
 	int i;
 
-	target->type->read_memory(target, flash_address(bank, sector, offset), bank->bus_width, 1, data);
+	target_read_memory(target, flash_address(bank, sector, offset), bank->bus_width, 1, data);
 
 	if (bank->target->endianness == TARGET_LITTLE_ENDIAN)
 	{
@@ -212,7 +195,7 @@ u8 cfi_get_u8(flash_bank_t *bank, int sector, u32 offset)
 	}
 	else
 	{
-		u8 value = 0;
+		uint8_t value = 0;
 		for (i = 0; i < bank->bus_width / bank->chip_width; i++)
 			value |= data[bank->bus_width - 1 - i];
 
@@ -220,12 +203,21 @@ u8 cfi_get_u8(flash_bank_t *bank, int sector, u32 offset)
 	}
 }
 
-u16 cfi_query_u16(flash_bank_t *bank, int sector, u32 offset)
+static uint16_t cfi_query_u16(flash_bank_t *bank, int sector, uint32_t offset)
 {
 	target_t *target = bank->target;
-	u8 data[CFI_MAX_BUS_WIDTH * 2];
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	uint8_t data[CFI_MAX_BUS_WIDTH * 2];
 
-	target->type->read_memory(target, flash_address(bank, sector, offset), bank->bus_width, 2, data);
+	if (cfi_info->x16_as_x8)
+	{
+		uint8_t i;
+		for (i = 0;i<2;i++)
+			target_read_memory(target, flash_address(bank, sector, offset + i), bank->bus_width, 1,
+				&data[i*bank->bus_width] );
+	}
+	else
+		target_read_memory(target, flash_address(bank, sector, offset), bank->bus_width, 2, data);
 
 	if (bank->target->endianness == TARGET_LITTLE_ENDIAN)
 		return data[0] | data[bank->bus_width] << 8;
@@ -233,12 +225,21 @@ u16 cfi_query_u16(flash_bank_t *bank, int sector, u32 offset)
 		return data[bank->bus_width - 1] | data[(2 * bank->bus_width) - 1] << 8;
 }
 
-u32 cfi_query_u32(flash_bank_t *bank, int sector, u32 offset)
+static uint32_t cfi_query_u32(flash_bank_t *bank, int sector, uint32_t offset)
 {
 	target_t *target = bank->target;
-	u8 data[CFI_MAX_BUS_WIDTH * 4];
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	uint8_t data[CFI_MAX_BUS_WIDTH * 4];
 
-	target->type->read_memory(target, flash_address(bank, sector, offset), bank->bus_width, 4, data);
+	if (cfi_info->x16_as_x8)
+	{
+		uint8_t i;
+		for (i = 0;i<4;i++)
+			target_read_memory(target, flash_address(bank, sector, offset + i), bank->bus_width, 1,
+				&data[i*bank->bus_width] );
+	}
+	else
+		target_read_memory(target, flash_address(bank, sector, offset), bank->bus_width, 4, data);
 
 	if (bank->target->endianness == TARGET_LITTLE_ENDIAN)
 		return data[0] | data[bank->bus_width] << 8 | data[bank->bus_width * 2] << 16 | data[bank->bus_width * 3] << 24;
@@ -247,10 +248,10 @@ u32 cfi_query_u32(flash_bank_t *bank, int sector, u32 offset)
 				data[(3 * bank->bus_width) - 1] << 16 | data[(4 * bank->bus_width) - 1] << 24;
 }
 
-void cfi_intel_clear_status_register(flash_bank_t *bank)
+static void cfi_intel_clear_status_register(flash_bank_t *bank)
 {
 	target_t *target = bank->target;
-	u8 command[8];
+	uint8_t command[8];
 
 	if (target->state != TARGET_HALTED)
 	{
@@ -259,17 +260,17 @@ void cfi_intel_clear_status_register(flash_bank_t *bank)
 	}
 
 	cfi_command(bank, 0x50, command);
-	target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+	target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
 }
 
-u8 cfi_intel_wait_status_busy(flash_bank_t *bank, int timeout)
+uint8_t cfi_intel_wait_status_busy(flash_bank_t *bank, int timeout)
 {
-	u8 status;
+	uint8_t status;
 
 	while ((!((status = cfi_get_u8(bank, 0, 0x0)) & 0x80)) && (timeout-- > 0))
 	{
 		LOG_DEBUG("status: 0x%x", status);
-		usleep(1000);
+		alive_sleep(1);
 	}
 
 	/* mask out bit 0 (reserved) */
@@ -305,14 +306,15 @@ u8 cfi_intel_wait_status_busy(flash_bank_t *bank, int timeout)
 
 int cfi_spansion_wait_status_busy(flash_bank_t *bank, int timeout)
 {
-	u8 status, oldstatus;
+	uint8_t status, oldstatus;
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 
 	oldstatus = cfi_get_u8(bank, 0, 0x0);
 
 	do {
 		status = cfi_get_u8(bank, 0, 0x0);
 		if ((status ^ oldstatus) & 0x40) {
-			if (status & 0x20) {
+			if (status & cfi_info->status_poll_mask & 0x20) {
 				oldstatus = cfi_get_u8(bank, 0, 0x0);
 				status = cfi_get_u8(bank, 0, 0x0);
 				if ((status ^ oldstatus) & 0x40) {
@@ -323,13 +325,13 @@ int cfi_spansion_wait_status_busy(flash_bank_t *bank, int timeout)
 					return(ERROR_OK);
 				}
 			}
-		} else {
+		} else { /* no toggle: finished, OK */
 			LOG_DEBUG("status: 0x%x", status);
 			return(ERROR_OK);
 		}
 
 		oldstatus = status;
-		usleep(1000);
+		alive_sleep(1);
 	} while (timeout-- > 0);
 
 	LOG_ERROR("timeout, status: 0x%x", status);
@@ -337,12 +339,13 @@ int cfi_spansion_wait_status_busy(flash_bank_t *bank, int timeout)
 	return(ERROR_FLASH_BUSY);
 }
 
-int cfi_read_intel_pri_ext(flash_bank_t *bank)
+static int cfi_read_intel_pri_ext(flash_bank_t *bank)
 {
+	int retval;
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 	cfi_intel_pri_ext_t *pri_ext = malloc(sizeof(cfi_intel_pri_ext_t));
 	target_t *target = bank->target;
-	u8 command[8];
+	uint8_t command[8];
 
 	cfi_info->pri_ext = pri_ext;
 
@@ -353,9 +356,15 @@ int cfi_read_intel_pri_ext(flash_bank_t *bank)
 	if ((pri_ext->pri[0] != 'P') || (pri_ext->pri[1] != 'R') || (pri_ext->pri[2] != 'I'))
 	{
 		cfi_command(bank, 0xf0, command);
-		target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+		if ((retval = target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+		{
+			return retval;
+		}
 		cfi_command(bank, 0xff, command);
-		target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+		if ((retval = target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+		{
+			return retval;
+		}
 		LOG_ERROR("Could not read bank flash bank information");
 		return ERROR_FLASH_BANK_INVALID;
 	}
@@ -369,7 +378,10 @@ int cfi_read_intel_pri_ext(flash_bank_t *bank)
 	pri_ext->suspend_cmd_support = cfi_query_u8(bank, 0, cfi_info->pri_addr + 9);
 	pri_ext->blk_status_reg_mask = cfi_query_u16(bank, 0, cfi_info->pri_addr + 0xa);
 
-	LOG_DEBUG("feature_support: 0x%x, suspend_cmd_support: 0x%x, blk_status_reg_mask: 0x%x", pri_ext->feature_support, pri_ext->suspend_cmd_support, pri_ext->blk_status_reg_mask);
+	LOG_DEBUG("feature_support: 0x%" PRIx32 ", suspend_cmd_support: 0x%x, blk_status_reg_mask: 0x%x", 
+		  pri_ext->feature_support, 
+		  pri_ext->suspend_cmd_support, 
+		  pri_ext->blk_status_reg_mask);
 
 	pri_ext->vcc_optimal = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0xc);
 	pri_ext->vpp_optimal = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0xd);
@@ -393,12 +405,13 @@ int cfi_read_intel_pri_ext(flash_bank_t *bank)
 	return ERROR_OK;
 }
 
-int cfi_read_spansion_pri_ext(flash_bank_t *bank)
+static int cfi_read_spansion_pri_ext(flash_bank_t *bank)
 {
+	int retval;
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 	cfi_spansion_pri_ext_t *pri_ext = malloc(sizeof(cfi_spansion_pri_ext_t));
 	target_t *target = bank->target;
-	u8 command[8];
+	uint8_t command[8];
 
 	cfi_info->pri_ext = pri_ext;
 
@@ -409,7 +422,10 @@ int cfi_read_spansion_pri_ext(flash_bank_t *bank)
 	if ((pri_ext->pri[0] != 'P') || (pri_ext->pri[1] != 'R') || (pri_ext->pri[2] != 'I'))
 	{
 		cfi_command(bank, 0xf0, command);
-		target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+		if ((retval = target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+		{
+			return retval;
+		}
 		LOG_ERROR("Could not read spansion bank information");
 		return ERROR_FLASH_BANK_INVALID;
 	}
@@ -454,13 +470,14 @@ int cfi_read_spansion_pri_ext(flash_bank_t *bank)
 	return ERROR_OK;
 }
 
-int cfi_read_atmel_pri_ext(flash_bank_t *bank)
+static int cfi_read_atmel_pri_ext(flash_bank_t *bank)
 {
+	int retval;
 	cfi_atmel_pri_ext_t atmel_pri_ext;
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 	cfi_spansion_pri_ext_t *pri_ext = malloc(sizeof(cfi_spansion_pri_ext_t));
 	target_t *target = bank->target;
-	u8 command[8];
+	uint8_t command[8];
 
 	/* ATMEL devices use the same CFI primary command set (0x2) as AMD/Spansion,
 	 * but a different primary extended query table.
@@ -478,7 +495,10 @@ int cfi_read_atmel_pri_ext(flash_bank_t *bank)
 	if ((atmel_pri_ext.pri[0] != 'P') || (atmel_pri_ext.pri[1] != 'R') || (atmel_pri_ext.pri[2] != 'I'))
 	{
 		cfi_command(bank, 0xf0, command);
-		target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+		if ((retval = target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+		{
+			return retval;
+		}
 		LOG_ERROR("Could not read atmel bank information");
 		return ERROR_FLASH_BANK_INVALID;
 	}
@@ -517,7 +537,7 @@ int cfi_read_atmel_pri_ext(flash_bank_t *bank)
 	return ERROR_OK;
 }
 
-int cfi_read_0002_pri_ext(flash_bank_t *bank)
+static int cfi_read_0002_pri_ext(flash_bank_t *bank)
 {
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 
@@ -531,7 +551,7 @@ int cfi_read_0002_pri_ext(flash_bank_t *bank)
 	}
 }
 
-int cfi_spansion_info(struct flash_bank_s *bank, char *buf, int buf_size)
+static int cfi_spansion_info(struct flash_bank_s *bank, char *buf, int buf_size)
 {
 	int printed;
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
@@ -566,7 +586,7 @@ int cfi_spansion_info(struct flash_bank_s *bank, char *buf, int buf_size)
 	return ERROR_OK;
 }
 
-int cfi_intel_info(struct flash_bank_s *bank, char *buf, int buf_size)
+static int cfi_intel_info(struct flash_bank_s *bank, char *buf, int buf_size)
 {
 	int printed;
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
@@ -580,7 +600,7 @@ int cfi_intel_info(struct flash_bank_s *bank, char *buf, int buf_size)
 	buf += printed;
 	buf_size -= printed;
 
-	printed = snprintf(buf, buf_size, "feature_support: 0x%x, suspend_cmd_support: 0x%x, blk_status_reg_mask: 0x%x\n", pri_ext->feature_support, pri_ext->suspend_cmd_support, pri_ext->blk_status_reg_mask);
+	printed = snprintf(buf, buf_size, "feature_support: 0x%" PRIx32 ", suspend_cmd_support: 0x%x, blk_status_reg_mask: 0x%x\n", pri_ext->feature_support, pri_ext->suspend_cmd_support, pri_ext->blk_status_reg_mask);
 	buf += printed;
 	buf_size -= printed;
 
@@ -595,7 +615,7 @@ int cfi_intel_info(struct flash_bank_s *bank, char *buf, int buf_size)
 	return ERROR_OK;
 }
 
-int cfi_register_commands(struct command_context_s *cmd_ctx)
+static int cfi_register_commands(struct command_context_s *cmd_ctx)
 {
 	/*command_t *cfi_cmd = */
 	register_command(cmd_ctx, NULL, "cfi", NULL, COMMAND_ANY, "flash bank cfi <base> <size> <chip_width> <bus_width> <targetNum> [jedec_probe/x16_as_x8]");
@@ -608,10 +628,12 @@ int cfi_register_commands(struct command_context_s *cmd_ctx)
 
 /* flash_bank cfi <base> <size> <chip_width> <bus_width> <target#> [options]
  */
-int cfi_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)
+static int cfi_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)
 {
 	cfi_flash_bank_t *cfi_info;
 	int i;
+	(void) cmd_ctx;
+	(void) cmd;
 
 	if (argc < 6)
 	{
@@ -656,11 +678,12 @@ int cfi_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **
 	return ERROR_OK;
 }
 
-int cfi_intel_erase(struct flash_bank_s *bank, int first, int last)
+static int cfi_intel_erase(struct flash_bank_s *bank, int first, int last)
 {
+	int retval;
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 	target_t *target = bank->target;
-	u8 command[8];
+	uint8_t command[8];
 	int i;
 
 	cfi_intel_clear_status_register(bank);
@@ -668,81 +691,110 @@ int cfi_intel_erase(struct flash_bank_s *bank, int first, int last)
 	for (i = first; i <= last; i++)
 	{
 		cfi_command(bank, 0x20, command);
-		target->type->write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command);
+		if ((retval = target_write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+		{
+			return retval;
+		}
 
 		cfi_command(bank, 0xd0, command);
-		target->type->write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command);
+		if ((retval = target_write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+		{
+			return retval;
+		}
 
 		if (cfi_intel_wait_status_busy(bank, 1000 * (1 << cfi_info->block_erase_timeout_typ)) == 0x80)
 			bank->sectors[i].is_erased = 1;
 		else
 		{
 			cfi_command(bank, 0xff, command);
-			target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+			if ((retval = target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+			{
+				return retval;
+			}
 
-			LOG_ERROR("couldn't erase block %i of flash bank at base 0x%x", i, bank->base);
+			LOG_ERROR("couldn't erase block %i of flash bank at base 0x%" PRIx32 , i, bank->base);
 			return ERROR_FLASH_OPERATION_FAILED;
 		}
 	}
 
 	cfi_command(bank, 0xff, command);
-	target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+	return target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
 
-	return ERROR_OK;
 }
 
-int cfi_spansion_erase(struct flash_bank_s *bank, int first, int last)
+static int cfi_spansion_erase(struct flash_bank_s *bank, int first, int last)
 {
+	int retval;
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 	cfi_spansion_pri_ext_t *pri_ext = cfi_info->pri_ext;
 	target_t *target = bank->target;
-	u8 command[8];
+	uint8_t command[8];
 	int i;
 
 	for (i = first; i <= last; i++)
 	{
 		cfi_command(bank, 0xaa, command);
-		target->type->write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command);
+		if ((retval = target_write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command)) != ERROR_OK)
+		{
+			return retval;
+		}
 
 		cfi_command(bank, 0x55, command);
-		target->type->write_memory(target, flash_address(bank, 0, pri_ext->_unlock2), bank->bus_width, 1, command);
+		if ((retval = target_write_memory(target, flash_address(bank, 0, pri_ext->_unlock2), bank->bus_width, 1, command)) != ERROR_OK)
+		{
+			return retval;
+		}
 
 		cfi_command(bank, 0x80, command);
-		target->type->write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command);
+		if ((retval = target_write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command)) != ERROR_OK)
+		{
+			return retval;
+		}
 
 		cfi_command(bank, 0xaa, command);
-		target->type->write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command);
+		if ((retval = target_write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command)) != ERROR_OK)
+		{
+			return retval;
+		}
 
 		cfi_command(bank, 0x55, command);
-		target->type->write_memory(target, flash_address(bank, 0, pri_ext->_unlock2), bank->bus_width, 1, command);
+		if ((retval = target_write_memory(target, flash_address(bank, 0, pri_ext->_unlock2), bank->bus_width, 1, command)) != ERROR_OK)
+		{
+			return retval;
+		}
 
 		cfi_command(bank, 0x30, command);
-		target->type->write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command);
+		if ((retval = target_write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+		{
+			return retval;
+		}
 
 		if (cfi_spansion_wait_status_busy(bank, 1000 * (1 << cfi_info->block_erase_timeout_typ)) == ERROR_OK)
 			bank->sectors[i].is_erased = 1;
 		else
 		{
 			cfi_command(bank, 0xf0, command);
-			target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+			if ((retval = target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+			{
+				return retval;
+			}
 
-			LOG_ERROR("couldn't erase block %i of flash bank at base 0x%x", i, bank->base);
+			LOG_ERROR("couldn't erase block %i of flash bank at base 0x%" PRIx32, i, bank->base);
 			return ERROR_FLASH_OPERATION_FAILED;
 		}
 	}
 
 	cfi_command(bank, 0xf0, command);
-	target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
-
-	return ERROR_OK;
+	return target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
 }
 
-int cfi_erase(struct flash_bank_s *bank, int first, int last)
+static int cfi_erase(struct flash_bank_s *bank, int first, int last)
 {
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 
 	if (bank->target->state != TARGET_HALTED)
 	{
+		LOG_ERROR("Target not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -754,7 +806,7 @@ int cfi_erase(struct flash_bank_s *bank, int first, int last)
 	if (cfi_info->qry[0] != 'Q')
 		return ERROR_FLASH_BANK_NOT_PROBED;
 
-	switch(cfi_info->pri_id)
+	switch (cfi_info->pri_id)
 	{
 		case 1:
 		case 3:
@@ -771,12 +823,13 @@ int cfi_erase(struct flash_bank_s *bank, int first, int last)
 	return ERROR_OK;
 }
 
-int cfi_intel_protect(struct flash_bank_s *bank, int set, int first, int last)
+static int cfi_intel_protect(struct flash_bank_s *bank, int set, int first, int last)
 {
+	int retval;
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 	cfi_intel_pri_ext_t *pri_ext = cfi_info->pri_ext;
 	target_t *target = bank->target;
-	u8 command[8];
+	uint8_t command[8];
 	int retry = 0;
 	int i;
 
@@ -791,20 +844,29 @@ int cfi_intel_protect(struct flash_bank_s *bank, int set, int first, int last)
 	for (i = first; i <= last; i++)
 	{
 		cfi_command(bank, 0x60, command);
-		LOG_DEBUG("address: 0x%4.4x, command: 0x%4.4x", flash_address(bank, i, 0x0), target_buffer_get_u32(target, command));
-		target->type->write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command);
+		LOG_DEBUG("address: 0x%4.4" PRIx32 ", command: 0x%4.4" PRIx32, flash_address(bank, i, 0x0), target_buffer_get_u32(target, command));
+		if ((retval = target_write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+		{
+			return retval;
+		}
 		if (set)
 		{
 			cfi_command(bank, 0x01, command);
-			LOG_DEBUG("address: 0x%4.4x, command: 0x%4.4x", flash_address(bank, i, 0x0), target_buffer_get_u32(target, command));
-			target->type->write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command);
+			LOG_DEBUG("address: 0x%4.4" PRIx32 ", command: 0x%4.4" PRIx32 , flash_address(bank, i, 0x0), target_buffer_get_u32(target, command));
+			if ((retval = target_write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+			{
+				return retval;
+			}
 			bank->sectors[i].is_protected = 1;
 		}
 		else
 		{
 			cfi_command(bank, 0xd0, command);
-			LOG_DEBUG("address: 0x%4.4x, command: 0x%4.4x", flash_address(bank, i, 0x0), target_buffer_get_u32(target, command));
-			target->type->write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command);
+			LOG_DEBUG("address: 0x%4.4" PRIx32 ", command: 0x%4.4" PRIx32, flash_address(bank, i, 0x0), target_buffer_get_u32(target, command));
+			if ((retval = target_write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+			{
+				return retval;
+			}
 			bank->sectors[i].is_protected = 0;
 		}
 
@@ -816,17 +878,23 @@ int cfi_intel_protect(struct flash_bank_s *bank, int set, int first, int last)
 		}
 		else
 		{
-			u8 block_status;
+			uint8_t block_status;
 			/* read block lock bit, to verify status */
 			cfi_command(bank, 0x90, command);
-			target->type->write_memory(target, flash_address(bank, 0, 0x55), bank->bus_width, 1, command);
+			if ((retval = target_write_memory(target, flash_address(bank, 0, 0x55), bank->bus_width, 1, command)) != ERROR_OK)
+			{
+				return retval;
+			}
 			block_status = cfi_get_u8(bank, i, 0x2);
 
 			if ((block_status & 0x1) != set)
 			{
 				LOG_ERROR("couldn't change block lock status (set = %i, block_status = 0x%2.2x)", set, block_status);
 				cfi_command(bank, 0x70, command);
-				target->type->write_memory(target, flash_address(bank, 0, 0x55), bank->bus_width, 1, command);
+				if ((retval = target_write_memory(target, flash_address(bank, 0, 0x55), bank->bus_width, 1, command)) != ERROR_OK)
+				{
+					return retval;
+				}
 				cfi_intel_wait_status_busy(bank, 10);
 
 				if (retry > 10)
@@ -852,10 +920,16 @@ int cfi_intel_protect(struct flash_bank_s *bank, int set, int first, int last)
 				cfi_intel_clear_status_register(bank);
 
 				cfi_command(bank, 0x60, command);
-				target->type->write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command);
+				if ((retval = target_write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+				{
+					return retval;
+				}
 
 				cfi_command(bank, 0x01, command);
-				target->type->write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command);
+				if ((retval = target_write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+				{
+					return retval;
+				}
 
 				cfi_intel_wait_status_busy(bank, 100);
 			}
@@ -863,17 +937,16 @@ int cfi_intel_protect(struct flash_bank_s *bank, int set, int first, int last)
 	}
 
 	cfi_command(bank, 0xff, command);
-	target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
-
-	return ERROR_OK;
+	return target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
 }
 
-int cfi_protect(struct flash_bank_s *bank, int set, int first, int last)
+static int cfi_protect(struct flash_bank_s *bank, int set, int first, int last)
 {
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 
 	if (bank->target->state != TARGET_HALTED)
 	{
+		LOG_ERROR("Target not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -885,14 +958,14 @@ int cfi_protect(struct flash_bank_s *bank, int set, int first, int last)
 	if (cfi_info->qry[0] != 'Q')
 		return ERROR_FLASH_BANK_NOT_PROBED;
 
-	switch(cfi_info->pri_id)
+	switch (cfi_info->pri_id)
 	{
 		case 1:
 		case 3:
 			cfi_intel_protect(bank, set, first, last);
 			break;
 		default:
-			LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
+			LOG_ERROR("protect: cfi primary command set %i unsupported", cfi_info->pri_id);
 			break;
 	}
 
@@ -900,7 +973,7 @@ int cfi_protect(struct flash_bank_s *bank, int set, int first, int last)
 }
 
 /* FIXME Replace this by a simple memcpy() - still unsure about sideeffects */
-static void cfi_add_byte(struct flash_bank_s *bank, u8 *word, u8 byte)
+static void cfi_add_byte(struct flash_bank_s *bank, uint8_t *word, uint8_t byte)
 {
 	/* target_t *target = bank->target; */
 
@@ -909,8 +982,8 @@ static void cfi_add_byte(struct flash_bank_s *bank, u8 *word, u8 byte)
 	/* NOTE:
 	 * The data to flash must not be changed in endian! We write a bytestrem in
 	 * target byte order already. Only the control and status byte lane of the flash
-	 * WSM is interpreted by the CPU in different ways, when read a u16 or u32
-	 * word (data seems to be in the upper or lower byte lane for u16 accesses).
+	 * WSM is interpreted by the CPU in different ways, when read a uint16_t or uint32_t
+	 * word (data seems to be in the upper or lower byte lane for uint16_t accesses).
 	 */
 
 #if 0
@@ -935,22 +1008,22 @@ static void cfi_add_byte(struct flash_bank_s *bank, u8 *word, u8 byte)
 
 /* Convert code image to target endian */
 /* FIXME create general block conversion fcts in target.c?) */
-static void cfi_fix_code_endian(target_t *target, u8 *dest, const u32 *src, u32 count)
+static void cfi_fix_code_endian(target_t *target, uint8_t *dest, const uint32_t *src, uint32_t count)
 {
-	u32 i;
-	for (i=0; i< count; i++)
+	uint32_t i;
+	for (i = 0; i< count; i++)
 	{
 		target_buffer_set_u32(target, dest, *src);
-		dest+=4;
+		dest += 4;
 		src++;
 	}
 }
 
-u32 cfi_command_val(flash_bank_t *bank, u8 cmd)
+static uint32_t cfi_command_val(flash_bank_t *bank, uint8_t cmd)
 {
 	target_t *target = bank->target;
 
-	u8 buf[CFI_MAX_BUS_WIDTH];
+	uint8_t buf[CFI_MAX_BUS_WIDTH];
 	cfi_command(bank, cmd, buf);
 	switch (bank->bus_width)
 	{
@@ -969,15 +1042,15 @@ u32 cfi_command_val(flash_bank_t *bank, u8 cmd)
 	}
 }
 
-int cfi_intel_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address, u32 count)
+static int cfi_intel_write_block(struct flash_bank_s *bank, uint8_t *buffer, uint32_t address, uint32_t count)
 {
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 	target_t *target = bank->target;
 	reg_param_t reg_params[7];
 	armv4_5_algorithm_t armv4_5_info;
 	working_area_t *source;
-	u32 buffer_size = 32768;
-	u32 write_command_val, busy_pattern_val, error_pattern_val;
+	uint32_t buffer_size = 32768;
+	uint32_t write_command_val, busy_pattern_val, error_pattern_val;
 
 	/* algorithm register usage:
 	 * r0: source address (in RAM)
@@ -989,7 +1062,7 @@ int cfi_intel_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address, u3
 	 * r6: error test pattern
 	 */
 
-	static const u32 word_32_code[] = {
+	static const uint32_t word_32_code[] = {
 		0xe4904004,   /* loop:	ldr r4, [r0], #4 */
 		0xe5813000,   /* 		str r3, [r1] */
 		0xe5814000,   /* 		str r4, [r1] */
@@ -1006,7 +1079,7 @@ int cfi_intel_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address, u3
 		0xeafffffe    /* done:	b -2 */
 	};
 
-	static const u32 word_16_code[] = {
+	static const uint32_t word_16_code[] = {
 		0xe0d040b2,   /* loop:	ldrh r4, [r0], #2 */
 		0xe1c130b0,   /* 		strh r3, [r1] */
 		0xe1c140b0,   /* 		strh r4, [r1] */
@@ -1023,7 +1096,7 @@ int cfi_intel_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address, u3
 		0xeafffffe    /* done:	b -2 */
 	};
 
-	static const u32 word_8_code[] = {
+	static const uint32_t word_8_code[] = {
 		0xe4d04001,   /* loop:	ldrb r4, [r0], #1 */
 		0xe5c13000,   /* 		strb r3, [r1] */
 		0xe5c14000,   /* 		strb r4, [r1] */
@@ -1039,9 +1112,9 @@ int cfi_intel_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address, u3
 		0xeafffff2,   /* 		b loop */
 		0xeafffffe    /* done:	b -2 */
 	};
-	u8 target_code[4*CFI_MAX_INTEL_CODESIZE];
-	const u32 *target_code_src;
-	int target_code_size;
+	uint8_t target_code[4*CFI_MAX_INTEL_CODESIZE];
+	const uint32_t *target_code_src;
+	uint32_t target_code_size;
 	int retval = ERROR_OK;
 
 
@@ -1133,15 +1206,18 @@ int cfi_intel_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address, u3
 	busy_pattern_val  = cfi_command_val(bank, 0x80);
 	error_pattern_val = cfi_command_val(bank, 0x7e);
 
-	LOG_INFO("Using target buffer at 0x%08x and of size 0x%04x", source->address, buffer_size );
+	LOG_INFO("Using target buffer at 0x%08" PRIx32 " and of size 0x%04" PRIx32, source->address, buffer_size );
 
 	/* Programming main loop */
 	while (count > 0)
 	{
-		u32 thisrun_count = (count > buffer_size) ? buffer_size : count;
-		u32 wsm_error;
+		uint32_t thisrun_count = (count > buffer_size) ? buffer_size : count;
+		uint32_t wsm_error;
 
-		target_write_buffer(target, source->address, thisrun_count, buffer);
+		if ((retval = target_write_buffer(target, source->address, thisrun_count, buffer)) != ERROR_OK)
+		{
+			goto cleanup;
+		}
 
 		buf_set_u32(reg_params[0].value, 0, 32, source->address);
 		buf_set_u32(reg_params[1].value, 0, 32, address);
@@ -1151,12 +1227,12 @@ int cfi_intel_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address, u3
 		buf_set_u32(reg_params[5].value, 0, 32, busy_pattern_val);
 		buf_set_u32(reg_params[6].value, 0, 32, error_pattern_val);
 
-		LOG_INFO("Write 0x%04x bytes to flash at 0x%08x", thisrun_count, address );
+		LOG_INFO("Write 0x%04" PRIx32 " bytes to flash at 0x%08" PRIx32 , thisrun_count, address );
 
 		/* Execute algorithm, assume breakpoint for last instruction */
-		retval = target->type->run_algorithm(target, 0, NULL, 7, reg_params,
+		retval = target_run_algorithm(target, 0, NULL, 7, reg_params,
 			cfi_info->write_algorithm->address,
-			cfi_info->write_algorithm->address + target_code_size - sizeof(u32),
+			cfi_info->write_algorithm->address + target_code_size - sizeof(uint32_t),
 			10000, /* 10s should be enough for max. 32k of data */
 			&armv4_5_info);
 
@@ -1210,7 +1286,7 @@ cleanup:
 	return retval;
 }
 
-int cfi_spansion_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address, u32 count)
+static int cfi_spansion_write_block(struct flash_bank_s *bank, uint8_t *buffer, uint32_t address, uint32_t count)
 {
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 	cfi_spansion_pri_ext_t *pri_ext = cfi_info->pri_ext;
@@ -1218,9 +1294,9 @@ int cfi_spansion_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address,
 	reg_param_t reg_params[10];
 	armv4_5_algorithm_t armv4_5_info;
 	working_area_t *source;
-	u32 buffer_size = 32768;
-	u32 status;
-	int retval;
+	uint32_t buffer_size = 32768;
+	uint32_t status;
+	int retval, retvaltemp;
 	int exit_code = ERROR_OK;
 
 	/* input parameters - */
@@ -1240,7 +1316,7 @@ int cfi_spansion_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address,
 	/*  R10 = unlock2_addr */
 	/*  R11 = unlock2_cmd */
 
-	static const u32 word_32_code[] = {
+	static const uint32_t word_32_code[] = {
 						/* 00008100 <sp_32_code>:		*/
 		0xe4905004,		/* ldr	r5, [r0], #4			*/
 		0xe5889000, 	/* str	r9, [r8]				*/
@@ -1274,7 +1350,7 @@ int cfi_spansion_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address,
 		0xeafffffe		/* b	8154 <sp_32_done>		*/
 		};
 
-		static const u32 word_16_code[] = {
+		static const uint32_t word_16_code[] = {
 				/* 00008158 <sp_16_code>:              */
 		0xe0d050b2, 	/* ldrh	r5, [r0], #2		   */
 		0xe1c890b0, 	/* strh	r9, [r8]				*/
@@ -1308,7 +1384,7 @@ int cfi_spansion_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address,
 		0xeafffffe 	/* b	81ac <sp_16_done>              */
 		};
 
-		static const u32 word_8_code[] = {
+		static const uint32_t word_8_code[] = {
 				/* 000081b0 <sp_16_code_end>:          */
 		0xe4d05001, 	/* ldrb	r5, [r0], #1		       */
 		0xe5c89000, 	/* strb	r9, [r8]				*/
@@ -1349,9 +1425,9 @@ int cfi_spansion_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address,
 	/* flash write code */
 	if (!cfi_info->write_algorithm)
 	{
-		u8 *target_code;
+		uint8_t *target_code;
 		int target_code_size;
-		const u32 *src;
+		const uint32_t *src;
 
 		/* convert bus-width dependent algorithm code to correct endiannes */
 		switch (bank->bus_width)
@@ -1376,14 +1452,21 @@ int cfi_spansion_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address,
 		cfi_fix_code_endian(target, target_code, src, target_code_size / 4);
 
 		/* allocate working area */
-		retval=target_alloc_working_area(target, target_code_size,
+		retval = target_alloc_working_area(target, target_code_size,
 				&cfi_info->write_algorithm);
 		if (retval != ERROR_OK)
+		{
+			free(target_code);
 			return retval;
+		}
 
 		/* write algorithm code to working area */
-		target_write_buffer(target, cfi_info->write_algorithm->address,
-		                    target_code_size, target_code);
+		if ((retval = target_write_buffer(target, cfi_info->write_algorithm->address,
+		                    target_code_size, target_code)) != ERROR_OK)
+		{
+			free(target_code);
+			return retval;
+		}
 
 		free(target_code);
 	}
@@ -1416,9 +1499,9 @@ int cfi_spansion_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address,
 
 	while (count > 0)
 	{
-		u32 thisrun_count = (count > buffer_size) ? buffer_size : count;
+		uint32_t thisrun_count = (count > buffer_size) ? buffer_size : count;
 
-		target_write_buffer(target, source->address, thisrun_count, buffer);
+		retvaltemp = target_write_buffer(target, source->address, thisrun_count, buffer);
 
 		buf_set_u32(reg_params[0].value, 0, 32, source->address);
 		buf_set_u32(reg_params[1].value, 0, 32, address);
@@ -1430,16 +1513,16 @@ int cfi_spansion_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address,
 		buf_set_u32(reg_params[8].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock2));
 		buf_set_u32(reg_params[9].value, 0, 32, 0x55555555);
 
-		retval = target->type->run_algorithm(target, 0, NULL, 10, reg_params,
+		retval = target_run_algorithm(target, 0, NULL, 10, reg_params,
 						     cfi_info->write_algorithm->address,
 						     cfi_info->write_algorithm->address + ((24 * 4) - 4),
 						     10000, &armv4_5_info);
 
 		status = buf_get_u32(reg_params[5].value, 0, 32);
 
-		if ((retval != ERROR_OK) || status != 0x80)
+		if ((retval != ERROR_OK) || (retvaltemp != ERROR_OK) || status != 0x80)
 		{
-			LOG_DEBUG("status: 0x%x", status);
+			LOG_DEBUG("status: 0x%" PRIx32 , status);
 			exit_code = ERROR_FLASH_OPERATION_FAILED;
 			break;
 		}
@@ -1465,48 +1548,60 @@ int cfi_spansion_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address,
 	return exit_code;
 }
 
-int cfi_intel_write_word(struct flash_bank_s *bank, u8 *word, u32 address)
+static int cfi_intel_write_word(struct flash_bank_s *bank, uint8_t *word, uint32_t address)
 {
+	int retval;
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 	target_t *target = bank->target;
-	u8 command[8];
+	uint8_t command[8];
 
 	cfi_intel_clear_status_register(bank);
 	cfi_command(bank, 0x40, command);
-	target->type->write_memory(target, address, bank->bus_width, 1, command);
+	if ((retval = target_write_memory(target, address, bank->bus_width, 1, command)) != ERROR_OK)
+	{
+		return retval;
+	}
 
-	target->type->write_memory(target, address, bank->bus_width, 1, word);
+	if ((retval = target_write_memory(target, address, bank->bus_width, 1, word)) != ERROR_OK)
+	{
+		return retval;
+	}
 
 	if (cfi_intel_wait_status_busy(bank, 1000 * (1 << cfi_info->word_write_timeout_max)) != 0x80)
 	{
 		cfi_command(bank, 0xff, command);
-		target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+		if ((retval = target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+		{
+			return retval;
+		}
 
-		LOG_ERROR("couldn't write word at base 0x%x, address %x", bank->base, address);
+		LOG_ERROR("couldn't write word at base 0x%" PRIx32 ", address %" PRIx32 , bank->base, address);
 		return ERROR_FLASH_OPERATION_FAILED;
 	}
 
 	return ERROR_OK;
 }
 
-int cfi_intel_write_words(struct flash_bank_s *bank, u8 *word, u32 wordcount, u32 address)
+static int cfi_intel_write_words(struct flash_bank_s *bank, uint8_t *word, uint32_t wordcount, uint32_t address)
 {
+	int retval;
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 	target_t *target = bank->target;
-	u8 command[8];
+	uint8_t command[8];
 
 	/* Calculate buffer size and boundary mask */
-	u32 buffersize = 1UL << cfi_info->max_buf_write_size;
-	u32 buffermask = buffersize-1;
-	u32 bufferwsize;
+	uint32_t buffersize = (1UL << cfi_info->max_buf_write_size) * (bank->bus_width / bank->chip_width);
+	uint32_t buffermask = buffersize-1;
+	uint32_t bufferwsize;
 
 	/* Check for valid range */
 	if (address & buffermask)
 	{
-		LOG_ERROR("Write address at base 0x%x, address %x not aligned to 2^%d boundary", bank->base, address, cfi_info->max_buf_write_size);
+		LOG_ERROR("Write address at base 0x%" PRIx32 ", address %" PRIx32 " not aligned to 2^%d boundary", 
+			  bank->base, address, cfi_info->max_buf_write_size);
 		return ERROR_FLASH_OPERATION_FAILED;
 	}
-	switch(bank->chip_width)
+	switch (bank->chip_width)
 	{
 	case 4 : bufferwsize = buffersize / 4; break;
 	case 2 : bufferwsize = buffersize / 2; break;
@@ -1516,10 +1611,13 @@ int cfi_intel_write_words(struct flash_bank_s *bank, u8 *word, u32 wordcount, u3
 		return ERROR_FLASH_OPERATION_FAILED;
 	}
 
+	bufferwsize/=(bank->bus_width / bank->chip_width);
+
+
 	/* Check for valid size */
 	if (wordcount > bufferwsize)
 	{
-		LOG_ERROR("Number of data words %d exceeds available buffersize %d", wordcount, buffersize);
+		LOG_ERROR("Number of data words %" PRId32 " exceeds available buffersize %" PRId32 , wordcount, buffersize);
 		return ERROR_FLASH_OPERATION_FAILED;
 	}
 
@@ -1528,72 +1626,198 @@ int cfi_intel_write_words(struct flash_bank_s *bank, u8 *word, u32 wordcount, u3
 
 	/* Initiate buffer operation _*/
 	cfi_command(bank, 0xE8, command);
-	target->type->write_memory(target, address, bank->bus_width, 1, command);
+	if ((retval = target_write_memory(target, address, bank->bus_width, 1, command)) != ERROR_OK)
+	{
+		return retval;
+	}
 	if (cfi_intel_wait_status_busy(bank, 1000 * (1 << cfi_info->buf_write_timeout_max)) != 0x80)
 	{
 		cfi_command(bank, 0xff, command);
-		target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+		if ((retval = target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+		{
+			return retval;
+		}
 
-		LOG_ERROR("couldn't start buffer write operation at base 0x%x, address %x", bank->base, address);
+		LOG_ERROR("couldn't start buffer write operation at base 0x%" PRIx32 ", address %" PRIx32 , bank->base, address);
 		return ERROR_FLASH_OPERATION_FAILED;
 	}
 
 	/* Write buffer wordcount-1 and data words */
 	cfi_command(bank, bufferwsize-1, command);
-	target->type->write_memory(target, address, bank->bus_width, 1, command);
+	if ((retval = target_write_memory(target, address, bank->bus_width, 1, command)) != ERROR_OK)
+	{
+		return retval;
+	}
 
-	target->type->write_memory(target, address, bank->bus_width, bufferwsize, word);
+	if ((retval = target_write_memory(target, address, bank->bus_width, bufferwsize, word)) != ERROR_OK)
+	{
+		return retval;
+	}
 
 	/* Commit write operation */
 	cfi_command(bank, 0xd0, command);
-	target->type->write_memory(target, address, bank->bus_width, 1, command);
+	if ((retval = target_write_memory(target, address, bank->bus_width, 1, command)) != ERROR_OK)
+	{
+		return retval;
+	}
 	if (cfi_intel_wait_status_busy(bank, 1000 * (1 << cfi_info->buf_write_timeout_max)) != 0x80)
 	{
 		cfi_command(bank, 0xff, command);
-		target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+		if ((retval = target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+		{
+			return retval;
+		}
 
-		LOG_ERROR("Buffer write at base 0x%x, address %x failed.", bank->base, address);
+		LOG_ERROR("Buffer write at base 0x%" PRIx32 ", address %" PRIx32 " failed.", bank->base, address);
 		return ERROR_FLASH_OPERATION_FAILED;
 	}
 
 	return ERROR_OK;
 }
 
-int cfi_spansion_write_word(struct flash_bank_s *bank, u8 *word, u32 address)
+static int cfi_spansion_write_word(struct flash_bank_s *bank, uint8_t *word, uint32_t address)
 {
+	int retval;
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 	cfi_spansion_pri_ext_t *pri_ext = cfi_info->pri_ext;
 	target_t *target = bank->target;
-	u8 command[8];
+	uint8_t command[8];
 
 	cfi_command(bank, 0xaa, command);
-	target->type->write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command);
+	if ((retval = target_write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command)) != ERROR_OK)
+	{
+		return retval;
+	}
 
 	cfi_command(bank, 0x55, command);
-	target->type->write_memory(target, flash_address(bank, 0, pri_ext->_unlock2), bank->bus_width, 1, command);
+	if ((retval = target_write_memory(target, flash_address(bank, 0, pri_ext->_unlock2), bank->bus_width, 1, command)) != ERROR_OK)
+	{
+		return retval;
+	}
 
 	cfi_command(bank, 0xa0, command);
-	target->type->write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command);
+	if ((retval = target_write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command)) != ERROR_OK)
+	{
+		return retval;
+	}
 
-	target->type->write_memory(target, address, bank->bus_width, 1, word);
+	if ((retval = target_write_memory(target, address, bank->bus_width, 1, word)) != ERROR_OK)
+	{
+		return retval;
+	}
 
 	if (cfi_spansion_wait_status_busy(bank, 1000 * (1 << cfi_info->word_write_timeout_max)) != ERROR_OK)
 	{
 		cfi_command(bank, 0xf0, command);
-		target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+		if ((retval = target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+		{
+			return retval;
+		}
 
-		LOG_ERROR("couldn't write word at base 0x%x, address %x", bank->base, address);
+		LOG_ERROR("couldn't write word at base 0x%" PRIx32 ", address %" PRIx32 , bank->base, address);
 		return ERROR_FLASH_OPERATION_FAILED;
 	}
 
 	return ERROR_OK;
 }
 
-int cfi_write_word(struct flash_bank_s *bank, u8 *word, u32 address)
+static int cfi_spansion_write_words(struct flash_bank_s *bank, uint8_t *word, uint32_t wordcount, uint32_t address)
 {
+	int retval;
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	target_t *target = bank->target;
+	uint8_t command[8];
+	cfi_spansion_pri_ext_t *pri_ext = cfi_info->pri_ext;
 
-	switch(cfi_info->pri_id)
+	/* Calculate buffer size and boundary mask */
+	uint32_t buffersize = (1UL << cfi_info->max_buf_write_size) * (bank->bus_width / bank->chip_width);
+	uint32_t buffermask = buffersize-1;
+	uint32_t bufferwsize;
+
+	/* Check for valid range */
+	if (address & buffermask)
+	{
+		LOG_ERROR("Write address at base 0x%" PRIx32 ", address %" PRIx32 " not aligned to 2^%d boundary", bank->base, address, cfi_info->max_buf_write_size);
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+	switch (bank->chip_width)
+	{
+	case 4 : bufferwsize = buffersize / 4; break;
+	case 2 : bufferwsize = buffersize / 2; break;
+	case 1 : bufferwsize = buffersize; break;
+	default:
+		LOG_ERROR("Unsupported chip width %d", bank->chip_width);
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	bufferwsize/=(bank->bus_width / bank->chip_width);
+
+	/* Check for valid size */
+	if (wordcount > bufferwsize)
+	{
+		LOG_ERROR("Number of data words %" PRId32 " exceeds available buffersize %" PRId32, wordcount, buffersize);
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	// Unlock
+	cfi_command(bank, 0xaa, command);
+	if ((retval = target_write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command)) != ERROR_OK)
+	{
+		return retval;
+	}
+
+	cfi_command(bank, 0x55, command);
+	if ((retval = target_write_memory(target, flash_address(bank, 0, pri_ext->_unlock2), bank->bus_width, 1, command)) != ERROR_OK)
+	{
+		return retval;
+	}
+
+	// Buffer load command
+	cfi_command(bank, 0x25, command);
+	if ((retval = target_write_memory(target, address, bank->bus_width, 1, command)) != ERROR_OK)
+	{
+		return retval;
+	}
+
+	/* Write buffer wordcount-1 and data words */
+	cfi_command(bank, bufferwsize-1, command);
+	if ((retval = target_write_memory(target, address, bank->bus_width, 1, command)) != ERROR_OK)
+	{
+		return retval;
+	}
+
+	if ((retval = target_write_memory(target, address, bank->bus_width, bufferwsize, word)) != ERROR_OK)
+	{
+		return retval;
+	}
+
+	/* Commit write operation */
+	cfi_command(bank, 0x29, command);
+	if ((retval = target_write_memory(target, address, bank->bus_width, 1, command)) != ERROR_OK)
+	{
+		return retval;
+	}
+
+	if (cfi_spansion_wait_status_busy(bank, 1000 * (1 << cfi_info->word_write_timeout_max)) != ERROR_OK)
+	{
+		cfi_command(bank, 0xf0, command);
+		if ((retval = target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+		{
+			return retval;
+		}
+
+		LOG_ERROR("couldn't write block at base 0x%" PRIx32 ", address %" PRIx32 ", size %" PRIx32 , bank->base, address, bufferwsize);
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	return ERROR_OK;
+}
+
+static int cfi_write_word(struct flash_bank_s *bank, uint8_t *word, uint32_t address)
+{
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+
+	switch (cfi_info->pri_id)
 	{
 		case 1:
 		case 3:
@@ -1610,19 +1834,18 @@ int cfi_write_word(struct flash_bank_s *bank, u8 *word, u32 address)
 	return ERROR_FLASH_OPERATION_FAILED;
 }
 
-int cfi_write_words(struct flash_bank_s *bank, u8 *word, u32 wordcount, u32 address)
+static int cfi_write_words(struct flash_bank_s *bank, uint8_t *word, uint32_t wordcount, uint32_t address)
 {
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 
-	switch(cfi_info->pri_id)
+	switch (cfi_info->pri_id)
 	{
 		case 1:
 		case 3:
 			return cfi_intel_write_words(bank, word, wordcount, address);
 			break;
 		case 2:
-			/* return cfi_spansion_write_words(bank, word, address); */
-			LOG_ERROR("cfi primary command set %i unimplemented - FIXME", cfi_info->pri_id);
+			return cfi_spansion_write_words(bank, word, wordcount, address);
 			break;
 		default:
 			LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
@@ -1632,20 +1855,23 @@ int cfi_write_words(struct flash_bank_s *bank, u8 *word, u32 wordcount, u32 addr
 	return ERROR_FLASH_OPERATION_FAILED;
 }
 
-int cfi_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
+int cfi_write(struct flash_bank_s *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
 {
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 	target_t *target = bank->target;
-	u32 address = bank->base + offset;	/* address of first byte to be programmed */
-	u32 write_p, copy_p;
+	uint32_t address = bank->base + offset;	/* address of first byte to be programmed */
+	uint32_t write_p, copy_p;
 	int align;	/* number of unaligned bytes */
 	int blk_count; /* number of bus_width bytes for block copy */
-	u8 current_word[CFI_MAX_BUS_WIDTH * 4];	/* word (bus_width size) currently being programmed */
+	uint8_t current_word[CFI_MAX_BUS_WIDTH * 4];	/* word (bus_width size) currently being programmed */
 	int i;
 	int retval;
 
 	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
 		return ERROR_TARGET_NOT_HALTED;
+	}
 
 	if (offset + count > bank->size)
 		return ERROR_FLASH_DST_OUT_OF_BANK;
@@ -1666,8 +1892,11 @@ int cfi_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
 		/* copy bytes before the first write address */
 		for (i = 0; i < align; ++i, ++copy_p)
 		{
-			u8 byte;
-			target->type->read_memory(target, copy_p, 1, 1, &byte);
+			uint8_t byte;
+			if ((retval = target_read_memory(target, copy_p, 1, 1, &byte)) != ERROR_OK)
+			{
+				return retval;
+			}
 			cfi_add_byte(bank, current_word, byte);
 		}
 
@@ -1682,8 +1911,11 @@ int cfi_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
 		/* if the buffer is already finished, copy bytes after the last write address */
 		for (; (count == 0) && (i < bank->bus_width); ++i, ++copy_p)
 		{
-			u8 byte;
-			target->type->read_memory(target, copy_p, 1, 1, &byte);
+			uint8_t byte;
+			if ((retval = target_read_memory(target, copy_p, 1, 1, &byte)) != ERROR_OK)
+			{
+				return retval;
+			}
 			cfi_add_byte(bank, current_word, byte);
 		}
 
@@ -1695,7 +1927,7 @@ int cfi_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
 
 	/* handle blocks of bus_size aligned bytes */
 	blk_count = count & ~(bank->bus_width - 1); /* round down, leave tail bytes */
-	switch(cfi_info->pri_id)
+	switch (cfi_info->pri_id)
 	{
 		/* try block writes (fails without working area) */
 		case 1:
@@ -1721,11 +1953,12 @@ int cfi_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
 	{
 		if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
 		{
-			u32 buffersize = 1UL << cfi_info->max_buf_write_size;
-			u32 buffermask = buffersize-1;
-			u32 bufferwsize;
+			//adjust buffersize for chip width
+			uint32_t buffersize = (1UL << cfi_info->max_buf_write_size) * (bank->bus_width / bank->chip_width);
+			uint32_t buffermask = buffersize-1;
+			uint32_t bufferwsize;
 
-			switch(bank->chip_width)
+			switch (bank->chip_width)
 			{
 			case 4 : bufferwsize = buffersize / 4; break;
 			case 2 : bufferwsize = buffersize / 2; break;
@@ -1735,13 +1968,15 @@ int cfi_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
 				return ERROR_FLASH_OPERATION_FAILED;
 			}
 
+			bufferwsize/=(bank->bus_width / bank->chip_width);
+
 			/* fall back to memory writes */
-			while (count >= bank->bus_width)
+			while (count >= (uint32_t)bank->bus_width)
 			{
 				int fallback;
 				if ((write_p & 0xff) == 0)
 				{
-					LOG_INFO("Programming at %08x, count %08x bytes remaining", write_p, count);
+					LOG_INFO("Programming at %08" PRIx32 ", count %08" PRIx32 " bytes remaining", write_p, count);
 				}
 				fallback = 1;
 				if ((bufferwsize > 0) && (count >= buffersize) && !(write_p & buffermask))
@@ -1752,7 +1987,7 @@ int cfi_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
 						buffer += buffersize;
 						write_p += buffersize;
 						count -= buffersize;
-						fallback=0;
+						fallback = 0;
 					}
 				}
 				/* try the slow way? */
@@ -1781,14 +2016,20 @@ int cfi_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
 
 	/* return to read array mode, so we can read from flash again for padding */
 	cfi_command(bank, 0xf0, current_word);
-	target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, current_word);
+	if ((retval = target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, current_word)) != ERROR_OK)
+	{
+		return retval;
+	}
 	cfi_command(bank, 0xff, current_word);
-	target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, current_word);
+	if ((retval = target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, current_word)) != ERROR_OK)
+	{
+		return retval;
+	}
 
 	/* handle unaligned tail bytes */
 	if (count > 0)
 	{
-		LOG_INFO("Fixup %d unaligned tail bytes", count );
+		LOG_INFO("Fixup %" PRId32 " unaligned tail bytes", count );
 
 		copy_p = write_p;
 		for (i = 0; i < bank->bus_width; i++)
@@ -1801,8 +2042,11 @@ int cfi_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
 		}
 		for (; i < bank->bus_width; ++i, ++copy_p)
 		{
-			u8 byte;
-			target->type->read_memory(target, copy_p, 1, 1, &byte);
+			uint8_t byte;
+			if ((retval = target_read_memory(target, copy_p, 1, 1, &byte)) != ERROR_OK)
+			{
+				return retval;
+			}
 			cfi_add_byte(bank, current_word, byte);
 		}
 		retval = cfi_write_word(bank, current_word, write_p);
@@ -1812,26 +2056,29 @@ int cfi_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
 
 	/* return to read array mode */
 	cfi_command(bank, 0xf0, current_word);
-	target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, current_word);
+	if ((retval = target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, current_word)) != ERROR_OK)
+	{
+		return retval;
+	}
 	cfi_command(bank, 0xff, current_word);
-	target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, current_word);
-
-	return ERROR_OK;
+	return target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, current_word);
 }
 
-void cfi_fixup_atmel_reversed_erase_regions(flash_bank_t *bank, void *param)
+static void cfi_fixup_atmel_reversed_erase_regions(flash_bank_t *bank, void *param)
 {
+	(void) param;
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 	cfi_spansion_pri_ext_t *pri_ext = cfi_info->pri_ext;
 
 	pri_ext->_reversed_geometry = 1;
 }
 
-void cfi_fixup_0002_erase_regions(flash_bank_t *bank, void *param)
+static void cfi_fixup_0002_erase_regions(flash_bank_t *bank, void *param)
 {
 	int i;
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 	cfi_spansion_pri_ext_t *pri_ext = cfi_info->pri_ext;
+	(void) param;
 
 	if ((pri_ext->_reversed_geometry) || (pri_ext->TopBottom == 3))
 	{
@@ -1840,7 +2087,7 @@ void cfi_fixup_0002_erase_regions(flash_bank_t *bank, void *param)
 		for (i = 0; i < cfi_info->num_erase_regions / 2; i++)
 		{
 			int j = (cfi_info->num_erase_regions - 1) - i;
-			u32 swap;
+			uint32_t swap;
 
 			swap = cfi_info->erase_region_info[i];
 			cfi_info->erase_region_info[i] = cfi_info->erase_region_info[j];
@@ -1849,7 +2096,7 @@ void cfi_fixup_0002_erase_regions(flash_bank_t *bank, void *param)
 	}
 }
 
-void cfi_fixup_0002_unlock_addresses(flash_bank_t *bank, void *param)
+static void cfi_fixup_0002_unlock_addresses(flash_bank_t *bank, void *param)
 {
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 	cfi_spansion_pri_ext_t *pri_ext = cfi_info->pri_ext;
@@ -1859,20 +2106,21 @@ void cfi_fixup_0002_unlock_addresses(flash_bank_t *bank, void *param)
 	pri_ext->_unlock2 = unlock_addresses->unlock2;
 }
 
-int cfi_probe(struct flash_bank_s *bank)
+static int cfi_probe(struct flash_bank_s *bank)
 {
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 	target_t *target = bank->target;
-	u8 command[8];
+	uint8_t command[8];
 	int num_sectors = 0;
 	int i;
 	int sector = 0;
-	u32 offset = 0;
-	u32 unlock1 = 0x555;
-	u32 unlock2 = 0x2aa;
+	uint32_t unlock1 = 0x555;
+	uint32_t unlock2 = 0x2aa;
+	int retval;
 
 	if (bank->target->state != TARGET_HALTED)
 	{
+		LOG_ERROR("Target not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
@@ -1889,33 +2137,62 @@ int cfi_probe(struct flash_bank_s *bank)
 
 	/* switch to read identifier codes mode ("AUTOSELECT") */
 	cfi_command(bank, 0xaa, command);
-	target->type->write_memory(target, flash_address(bank, 0, unlock1), bank->bus_width, 1, command);
+	if ((retval = target_write_memory(target, flash_address(bank, 0, unlock1), bank->bus_width, 1, command)) != ERROR_OK)
+	{
+		return retval;
+	}
 	cfi_command(bank, 0x55, command);
-	target->type->write_memory(target, flash_address(bank, 0, unlock2), bank->bus_width, 1, command);
+	if ((retval = target_write_memory(target, flash_address(bank, 0, unlock2), bank->bus_width, 1, command)) != ERROR_OK)
+	{
+		return retval;
+	}
 	cfi_command(bank, 0x90, command);
-	target->type->write_memory(target, flash_address(bank, 0, unlock1), bank->bus_width, 1, command);
+	if ((retval = target_write_memory(target, flash_address(bank, 0, unlock1), bank->bus_width, 1, command)) != ERROR_OK)
+	{
+		return retval;
+	}
 
 	if (bank->chip_width == 1)
 	{
-		u8 manufacturer, device_id;
-		target_read_u8(target, bank->base + 0x0, &manufacturer);
-		target_read_u8(target, bank->base + 0x1, &device_id);
+		uint8_t manufacturer, device_id;
+		if ((retval = target_read_u8(target, flash_address(bank, 0, 0x00), &manufacturer)) != ERROR_OK)
+		{
+			return retval;
+		}
+		if ((retval = target_read_u8(target, flash_address(bank, 0, 0x01), &device_id)) != ERROR_OK)
+		{
+			return retval;
+		}
 		cfi_info->manufacturer = manufacturer;
 		cfi_info->device_id = device_id;
 	}
 	else if (bank->chip_width == 2)
 	{
-		target_read_u16(target, bank->base + 0x0, &cfi_info->manufacturer);
-		target_read_u16(target, bank->base + 0x2, &cfi_info->device_id);
+		if ((retval = target_read_u16(target, flash_address(bank, 0, 0x00), &cfi_info->manufacturer)) != ERROR_OK)
+		{
+			return retval;
+		}
+		if ((retval = target_read_u16(target, flash_address(bank, 0, 0x02), &cfi_info->device_id)) != ERROR_OK)
+		{
+			return retval;
+		}
 	}
 
+	LOG_INFO("Flash Manufacturer/Device: 0x%04x 0x%04x", cfi_info->manufacturer, cfi_info->device_id);
 	/* switch back to read array mode */
 	cfi_command(bank, 0xf0, command);
-	target->type->write_memory(target, flash_address(bank, 0, 0x00), bank->bus_width, 1, command);
+	if ((retval = target_write_memory(target, flash_address(bank, 0, 0x00), bank->bus_width, 1, command)) != ERROR_OK)
+	{
+		return retval;
+	}
 	cfi_command(bank, 0xff, command);
-	target->type->write_memory(target, flash_address(bank, 0, 0x00), bank->bus_width, 1, command);
+	if ((retval = target_write_memory(target, flash_address(bank, 0, 0x00), bank->bus_width, 1, command)) != ERROR_OK)
+	{
+		return retval;
+	}
 
-	cfi_fixup(bank, cfi_jedec_fixups);
+	/* check device/manufacturer ID for known non-CFI flashes. */
+	cfi_fixup_non_cfi(bank);
 
 	/* query only if this is a CFI compatible flash,
 	 * otherwise the relevant info has already been filled in
@@ -1930,7 +2207,10 @@ int cfi_probe(struct flash_bank_s *bank)
 		 * SST flashes clearly violate this, and we will consider them incompatbile for now
 		 */
 		cfi_command(bank, 0x98, command);
-		target->type->write_memory(target, flash_address(bank, 0, 0x55), bank->bus_width, 1, command);
+		if ((retval = target_write_memory(target, flash_address(bank, 0, 0x55), bank->bus_width, 1, command)) != ERROR_OK)
+		{
+			return retval;
+		}
 
 		cfi_info->qry[0] = cfi_query_u8(bank, 0, 0x10);
 		cfi_info->qry[1] = cfi_query_u8(bank, 0, 0x11);
@@ -1941,10 +2221,16 @@ int cfi_probe(struct flash_bank_s *bank)
 		if ((cfi_info->qry[0] != 'Q') || (cfi_info->qry[1] != 'R') || (cfi_info->qry[2] != 'Y'))
 		{
 			cfi_command(bank, 0xf0, command);
-			target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+			if ((retval = target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+			{
+				return retval;
+			}
 			cfi_command(bank, 0xff, command);
-			target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
-			LOG_ERROR("Could not probe bank");
+			if ((retval = target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+			{
+				return retval;
+			}
+			LOG_ERROR("Could not probe bank: no QRY");
 			return ERROR_FLASH_BANK_INVALID;
 		}
 
@@ -1980,17 +2266,12 @@ int cfi_probe(struct flash_bank_s *bank)
 			(1 << cfi_info->block_erase_timeout_max) * (1 << cfi_info->block_erase_timeout_typ),
 			(1 << cfi_info->chip_erase_timeout_max) * (1 << cfi_info->chip_erase_timeout_typ));
 
-		cfi_info->dev_size = cfi_query_u8(bank, 0, 0x27);
+		cfi_info->dev_size = 1 << cfi_query_u8(bank, 0, 0x27);
 		cfi_info->interface_desc = cfi_query_u16(bank, 0, 0x28);
 		cfi_info->max_buf_write_size = cfi_query_u16(bank, 0, 0x2a);
 		cfi_info->num_erase_regions = cfi_query_u8(bank, 0, 0x2c);
 
-		LOG_DEBUG("size: 0x%x, interface desc: %i, max buffer write size: %x", 1 << cfi_info->dev_size, cfi_info->interface_desc, (1 << cfi_info->max_buf_write_size));
-
-		if (((1 << cfi_info->dev_size) * bank->bus_width / bank->chip_width) != bank->size)
-		{
-			LOG_WARNING("configuration specifies 0x%x size, but a 0x%x size flash was found", bank->size, 1 << cfi_info->dev_size);
-		}
+		LOG_DEBUG("size: 0x%" PRIx32 ", interface desc: %i, max buffer write size: %x", cfi_info->dev_size, cfi_info->interface_desc, (1 << cfi_info->max_buf_write_size));
 
 		if (cfi_info->num_erase_regions)
 		{
@@ -1998,7 +2279,10 @@ int cfi_probe(struct flash_bank_s *bank)
 			for (i = 0; i < cfi_info->num_erase_regions; i++)
 			{
 				cfi_info->erase_region_info[i] = cfi_query_u32(bank, 0, 0x2d + (4 * i));
-				LOG_DEBUG("erase region[%i]: %i blocks of size 0x%x", i, (cfi_info->erase_region_info[i] & 0xffff) + 1, (cfi_info->erase_region_info[i] >> 16) * 256);
+				LOG_DEBUG("erase region[%i]: %" PRIu32 " blocks of size 0x%" PRIx32 "", 
+					  i, 
+					  (cfi_info->erase_region_info[i] & 0xffff) + 1, 
+					  (cfi_info->erase_region_info[i] >> 16) * 256);
 			}
 		}
 		else
@@ -2009,7 +2293,7 @@ int cfi_probe(struct flash_bank_s *bank)
 		/* We need to read the primary algorithm extended query table before calculating
 		 * the sector layout to be able to apply fixups
 		 */
-		switch(cfi_info->pri_id)
+		switch (cfi_info->pri_id)
 		{
 			/* Intel command set (standard and extended) */
 			case 0x0001:
@@ -2018,6 +2302,7 @@ int cfi_probe(struct flash_bank_s *bank)
 				break;
 			/* AMD/Spansion, Atmel, ... command set */
 			case 0x0002:
+				cfi_info->status_poll_mask = CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7; /* default for all CFI flashs */
 				cfi_read_0002_pri_ext(bank);
 				break;
 			default:
@@ -2029,13 +2314,19 @@ int cfi_probe(struct flash_bank_s *bank)
 		 * we use both reset commands, as some Intel flashes fail to recognize the 0xF0 command
 		 */
 		cfi_command(bank, 0xf0, command);
-		target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+		if ((retval = target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+		{
+			return retval;
+		}
 		cfi_command(bank, 0xff, command);
-		target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
-	}
+		if ((retval = target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+		{
+			return retval;
+		}
+	} /* end CFI case */
 
 	/* apply fixups depending on the primary command set */
-	switch(cfi_info->pri_id)
+	switch (cfi_info->pri_id)
 	{
 		/* Intel command set (standard and extended) */
 		case 0x0001:
@@ -2051,6 +2342,11 @@ int cfi_probe(struct flash_bank_s *bank)
 			break;
 	}
 
+	if ((cfi_info->dev_size * bank->bus_width / bank->chip_width) != bank->size)
+	{
+		LOG_WARNING("configuration specifies 0x%" PRIx32 " size, but a 0x%" PRIx32 " size flash was found", bank->size, cfi_info->dev_size);
+	}
+
 	if (cfi_info->num_erase_regions == 0)
 	{
 		/* a device might have only one erase block, spanning the whole device */
@@ -2064,6 +2360,8 @@ int cfi_probe(struct flash_bank_s *bank)
 	}
 	else
 	{
+		uint32_t offset = 0;
+
 		for (i = 0; i < cfi_info->num_erase_regions; i++)
 		{
 			num_sectors += (cfi_info->erase_region_info[i] & 0xffff) + 1;
@@ -2074,7 +2372,7 @@ int cfi_probe(struct flash_bank_s *bank)
 
 		for (i = 0; i < cfi_info->num_erase_regions; i++)
 		{
-			int j;
+			uint32_t j;
 			for (j = 0; j < (cfi_info->erase_region_info[i] & 0xffff) + 1; j++)
 			{
 				bank->sectors[sector].offset = offset;
@@ -2085,14 +2383,18 @@ int cfi_probe(struct flash_bank_s *bank)
 				sector++;
 			}
 		}
+		if (offset != cfi_info->dev_size)
+		{
+			LOG_WARNING("CFI size is 0x%" PRIx32 ", but total sector size is 0x%" PRIx32 "", cfi_info->dev_size, offset);
+		}
 	}
-	
+
 	cfi_info->probed = 1;
 
 	return ERROR_OK;
 }
 
-int cfi_auto_probe(struct flash_bank_s *bank)
+static int cfi_auto_probe(struct flash_bank_s *bank)
 {
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 	if (cfi_info->probed)
@@ -2101,12 +2403,13 @@ int cfi_auto_probe(struct flash_bank_s *bank)
 }
 
 
-int cfi_intel_protect_check(struct flash_bank_s *bank)
+static int cfi_intel_protect_check(struct flash_bank_s *bank)
 {
+	int retval;
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 	cfi_intel_pri_ext_t *pri_ext = cfi_info->pri_ext;
 	target_t *target = bank->target;
-	u8 command[CFI_MAX_BUS_WIDTH];
+	uint8_t command[CFI_MAX_BUS_WIDTH];
 	int i;
 
 	/* check if block lock bits are supported on this device */
@@ -2114,11 +2417,14 @@ int cfi_intel_protect_check(struct flash_bank_s *bank)
 		return ERROR_FLASH_OPERATION_FAILED;
 
 	cfi_command(bank, 0x90, command);
-	target->type->write_memory(target, flash_address(bank, 0, 0x55), bank->bus_width, 1, command);
+	if ((retval = target_write_memory(target, flash_address(bank, 0, 0x55), bank->bus_width, 1, command)) != ERROR_OK)
+	{
+		return retval;
+	}
 
 	for (i = 0; i < bank->num_sectors; i++)
 	{
-		u8 block_status = cfi_get_u8(bank, i, 0x2);
+		uint8_t block_status = cfi_get_u8(bank, i, 0x2);
 
 		if (block_status & 1)
 			bank->sectors[i].is_protected = 1;
@@ -2127,31 +2433,39 @@ int cfi_intel_protect_check(struct flash_bank_s *bank)
 	}
 
 	cfi_command(bank, 0xff, command);
-	target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
-
-	return ERROR_OK;
+	return target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
 }
 
-int cfi_spansion_protect_check(struct flash_bank_s *bank)
+static int cfi_spansion_protect_check(struct flash_bank_s *bank)
 {
+	int retval;
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 	cfi_spansion_pri_ext_t *pri_ext = cfi_info->pri_ext;
 	target_t *target = bank->target;
-	u8 command[8];
+	uint8_t command[8];
 	int i;
 
 	cfi_command(bank, 0xaa, command);
-	target->type->write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command);
+	if ((retval = target_write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command)) != ERROR_OK)
+	{
+		return retval;
+	}
 
 	cfi_command(bank, 0x55, command);
-	target->type->write_memory(target, flash_address(bank, 0, pri_ext->_unlock2), bank->bus_width, 1, command);
+	if ((retval = target_write_memory(target, flash_address(bank, 0, pri_ext->_unlock2), bank->bus_width, 1, command)) != ERROR_OK)
+	{
+		return retval;
+	}
 
 	cfi_command(bank, 0x90, command);
-	target->type->write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command);
+	if ((retval = target_write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command)) != ERROR_OK)
+	{
+		return retval;
+	}
 
 	for (i = 0; i < bank->num_sectors; i++)
 	{
-		u8 block_status = cfi_get_u8(bank, i, 0x2);
+		uint8_t block_status = cfi_get_u8(bank, i, 0x2);
 
 		if (block_status & 1)
 			bank->sectors[i].is_protected = 1;
@@ -2160,24 +2474,23 @@ int cfi_spansion_protect_check(struct flash_bank_s *bank)
 	}
 
 	cfi_command(bank, 0xf0, command);
-	target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
-
-	return ERROR_OK;
+	return target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
 }
 
-int cfi_protect_check(struct flash_bank_s *bank)
+static int cfi_protect_check(struct flash_bank_s *bank)
 {
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
 
 	if (bank->target->state != TARGET_HALTED)
 	{
+		LOG_ERROR("Target not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
 	if (cfi_info->qry[0] != 'Q')
 		return ERROR_FLASH_BANK_NOT_PROBED;
 
-	switch(cfi_info->pri_id)
+	switch (cfi_info->pri_id)
 	{
 		case 1:
 		case 3:
@@ -2194,7 +2507,7 @@ int cfi_protect_check(struct flash_bank_s *bank)
 	return ERROR_OK;
 }
 
-int cfi_info(struct flash_bank_s *bank, char *buf, int buf_size)
+static int cfi_info(struct flash_bank_s *bank, char *buf, int buf_size)
 {
 	int printed;
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
@@ -2206,7 +2519,7 @@ int cfi_info(struct flash_bank_s *bank, char *buf, int buf_size)
 	}
 
 	if (cfi_info->not_cfi == 0)
-	printed = snprintf(buf, buf_size, "\ncfi information:\n");
+		printed = snprintf(buf, buf_size, "\ncfi information:\n");
 	else
 		printed = snprintf(buf, buf_size, "\nnon-cfi flash:\n");
 	buf += printed;
@@ -2247,14 +2560,14 @@ int cfi_info(struct flash_bank_s *bank, char *buf, int buf_size)
 	buf += printed;
 	buf_size -= printed;
 
-		printed = snprintf(buf, buf_size, "size: 0x%x, interface desc: %i, max buffer write size: %x\n",
-		                   1 << cfi_info->dev_size,
+		printed = snprintf(buf, buf_size, "size: 0x%" PRIx32 ", interface desc: %i, max buffer write size: %x\n",
+		                   cfi_info->dev_size,
 		                   cfi_info->interface_desc,
-		                   cfi_info->max_buf_write_size);
+		                   1 << cfi_info->max_buf_write_size);
 	buf += printed;
 	buf_size -= printed;
 
-	switch(cfi_info->pri_id)
+	switch (cfi_info->pri_id)
 	{
 		case 1:
 		case 3: