X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Fflash%2Fcfi.c;h=c0c2cb79ddbd4dde5484ce4b845c488fd2694a0a;hp=66b94c966fcfb826dc7f5314e91135e0b499fe5f;hb=15e8e4530866454c18c5d91ad9e867f339c2e82b;hpb=7b5ddb4a58f31cc63f626b7ffb41c0b958540026

diff --git a/src/flash/cfi.c b/src/flash/cfi.c
index 66b94c966f..c0c2cb79dd 100644
--- a/src/flash/cfi.c
+++ b/src/flash/cfi.c
@@ -29,40 +29,13 @@
 #include "binarybuffer.h"
 
 
-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);
-
-//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
 
 /* defines internal maximum size for code fragment in cfi_intel_write_block() */
 #define CFI_MAX_INTEL_CODESIZE 256
 
-flash_driver_t cfi_flash =
-{
-	.name = "cfi",
-	.register_commands = cfi_register_commands,
-	.flash_bank_command = cfi_flash_bank_command,
-	.erase = cfi_erase,
-	.protect = cfi_protect,
-	.write = cfi_write,
-	.probe = cfi_probe,
-	.auto_probe = cfi_auto_probe,
-	.erase_check = default_flash_blank_check,
-	.protect_check = cfi_protect_check,
-	.info = cfi_info
-};
-
-static cfi_unlock_addresses_t cfi_unlock_addresses[] =
+static struct cfi_unlock_addresses cfi_unlock_addresses[] =
 {
 	[CFI_UNLOCK_555_2AA] = { .unlock1 = 0x555, .unlock2 = 0x2aa },
 	[CFI_UNLOCK_5555_2AAA] = { .unlock1 = 0x5555, .unlock2 = 0x2aaa },
@@ -74,7 +47,7 @@ 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 */
-static const cfi_fixup_t cfi_0002_fixups[] = {
+static const struct cfi_fixup 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]},
@@ -90,14 +63,14 @@ static const cfi_fixup_t cfi_0002_fixups[] = {
 };
 
 /* fixup after reading cmdset 0001 primary query table */
-static const cfi_fixup_t cfi_0001_fixups[] = {
+static const struct cfi_fixup cfi_0001_fixups[] = {
 	{0, 0, NULL, NULL}
 };
 
-static void cfi_fixup(flash_bank_t *bank, const cfi_fixup_t *fixups)
+static void cfi_fixup(flash_bank_t *bank, const struct cfi_fixup *fixups)
 {
-	cfi_flash_bank_t *cfi_info = bank->driver_priv;
-	const cfi_fixup_t *f;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	const struct cfi_fixup *f;
 
 	for (f = fixups; f->fixup; f++)
 	{
@@ -112,7 +85,7 @@ static void cfi_fixup(flash_bank_t *bank, const cfi_fixup_t *fixups)
 /* 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;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
 
 	if (cfi_info->x16_as_x8) offset *= 2;
 
@@ -206,7 +179,7 @@ static uint8_t cfi_get_u8(flash_bank_t *bank, int sector, uint32_t offset)
 static uint16_t cfi_query_u16(flash_bank_t *bank, int sector, uint32_t offset)
 {
 	target_t *target = bank->target;
-	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
 	uint8_t data[CFI_MAX_BUS_WIDTH * 2];
 
 	if (cfi_info->x16_as_x8)
@@ -228,7 +201,7 @@ static uint16_t cfi_query_u16(flash_bank_t *bank, int sector, uint32_t offset)
 static uint32_t cfi_query_u32(flash_bank_t *bank, int sector, uint32_t offset)
 {
 	target_t *target = bank->target;
-	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
 	uint8_t data[CFI_MAX_BUS_WIDTH * 4];
 
 	if (cfi_info->x16_as_x8)
@@ -307,7 +280,7 @@ uint8_t cfi_intel_wait_status_busy(flash_bank_t *bank, int timeout)
 int cfi_spansion_wait_status_busy(flash_bank_t *bank, int timeout)
 {
 	uint8_t status, oldstatus;
-	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
 
 	oldstatus = cfi_get_u8(bank, 0, 0x0);
 
@@ -342,8 +315,8 @@ int cfi_spansion_wait_status_busy(flash_bank_t *bank, int timeout)
 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));
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct cfi_intel_pri_ext *pri_ext = malloc(sizeof(struct cfi_intel_pri_ext));
 	target_t *target = bank->target;
 	uint8_t command[8];
 
@@ -408,8 +381,8 @@ static int cfi_read_intel_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));
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct cfi_spansion_pri_ext *pri_ext = malloc(sizeof(struct cfi_spansion_pri_ext));
 	target_t *target = bank->target;
 	uint8_t command[8];
 
@@ -473,9 +446,9 @@ static int cfi_read_spansion_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));
+	struct cfi_atmel_pri_ext atmel_pri_ext;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct cfi_spansion_pri_ext *pri_ext = malloc(sizeof(struct cfi_spansion_pri_ext));
 	target_t *target = bank->target;
 	uint8_t command[8];
 
@@ -484,7 +457,7 @@ static int cfi_read_atmel_pri_ext(flash_bank_t *bank)
 	 * We read the atmel table, and prepare a valid AMD/Spansion query table.
 	 */
 
-	memset(pri_ext, 0, sizeof(cfi_spansion_pri_ext_t));
+	memset(pri_ext, 0, sizeof(struct cfi_spansion_pri_ext));
 
 	cfi_info->pri_ext = pri_ext;
 
@@ -539,7 +512,7 @@ static int cfi_read_atmel_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;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
 
 	if (cfi_info->manufacturer == CFI_MFR_ATMEL)
 	{
@@ -554,8 +527,8 @@ static int cfi_read_0002_pri_ext(flash_bank_t *bank)
 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;
-	cfi_spansion_pri_ext_t *pri_ext = cfi_info->pri_ext;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
 
 	printed = snprintf(buf, buf_size, "\nSpansion primary algorithm extend information:\n");
 	buf += printed;
@@ -589,8 +562,8 @@ static int cfi_spansion_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;
-	cfi_intel_pri_ext_t *pri_ext = cfi_info->pri_ext;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct cfi_intel_pri_ext *pri_ext = cfi_info->pri_ext;
 
 	printed = snprintf(buf, buf_size, "\nintel primary algorithm extend information:\n");
 	buf += printed;
@@ -628,12 +601,9 @@ static int cfi_register_commands(struct command_context_s *cmd_ctx)
 
 /* flash_bank cfi <base> <size> <chip_width> <bus_width> <target#> [options]
  */
-static int cfi_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)
+FLASH_BANK_COMMAND_HANDLER(cfi_flash_bank_command)
 {
-	cfi_flash_bank_t *cfi_info;
-	int i;
-	(void) cmd_ctx;
-	(void) cmd;
+	struct cfi_flash_bank *cfi_info;
 
 	if (argc < 6)
 	{
@@ -641,14 +611,18 @@ static int cfi_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd,
 		return ERROR_FLASH_BANK_INVALID;
 	}
 
-	if ((strtoul(args[4], NULL, 0) > CFI_MAX_CHIP_WIDTH)
-		|| (strtoul(args[3], NULL, 0) > CFI_MAX_BUS_WIDTH))
+	uint16_t chip_width, bus_width;
+	COMMAND_PARSE_NUMBER(u16, args[3], bus_width);
+	COMMAND_PARSE_NUMBER(u16, args[4], chip_width);
+
+	if ((chip_width > CFI_MAX_CHIP_WIDTH)
+			|| (bus_width > CFI_MAX_BUS_WIDTH))
 	{
 		LOG_ERROR("chip and bus width have to specified in bytes");
 		return ERROR_FLASH_BANK_INVALID;
 	}
 
-	cfi_info = malloc(sizeof(cfi_flash_bank_t));
+	cfi_info = malloc(sizeof(struct cfi_flash_bank));
 	cfi_info->probed = 0;
 	bank->driver_priv = cfi_info;
 
@@ -658,7 +632,7 @@ static int cfi_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd,
 	cfi_info->jedec_probe = 0;
 	cfi_info->not_cfi = 0;
 
-	for (i = 6; i < argc; i++)
+	for (unsigned i = 6; i < argc; i++)
 	{
 		if (strcmp(args[i], "x16_as_x8") == 0)
 		{
@@ -681,7 +655,7 @@ static int cfi_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd,
 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;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
 	target_t *target = bank->target;
 	uint8_t command[8];
 	int i;
@@ -725,8 +699,8 @@ static int cfi_intel_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;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
 	target_t *target = bank->target;
 	uint8_t command[8];
 	int i;
@@ -790,7 +764,7 @@ static int cfi_spansion_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;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
 
 	if (bank->target->state != TARGET_HALTED)
 	{
@@ -826,8 +800,8 @@ static int cfi_erase(struct flash_bank_s *bank, 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;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct cfi_intel_pri_ext *pri_ext = cfi_info->pri_ext;
 	target_t *target = bank->target;
 	uint8_t command[8];
 	int retry = 0;
@@ -942,7 +916,7 @@ static int cfi_intel_protect(struct flash_bank_s *bank, int set, int first, int
 
 static int cfi_protect(struct flash_bank_s *bank, int set, int first, int last)
 {
-	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
 
 	if (bank->target->state != TARGET_HALTED)
 	{
@@ -1044,10 +1018,10 @@ static uint32_t cfi_command_val(flash_bank_t *bank, uint8_t cmd)
 
 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;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
 	target_t *target = bank->target;
-	reg_param_t reg_params[7];
-	armv4_5_algorithm_t armv4_5_info;
+	struct reg_param reg_params[7];
+	struct armv4_5_algorithm armv4_5_info;
 	working_area_t *source;
 	uint32_t buffer_size = 32768;
 	uint32_t write_command_val, busy_pattern_val, error_pattern_val;
@@ -1125,11 +1099,11 @@ static int cfi_intel_write_block(struct flash_bank_s *bank, uint8_t *buffer, uin
 	armv4_5_info.core_state = ARMV4_5_STATE_ARM;
 
 	/* If we are setting up the write_algorith, we need target_code_src */
-	/* if not we only need target_code_size.														*/
-	/* 																																	*/
-	/* However, we don't want to create multiple code paths, so we			*/
-	/* do the unecessary evaluation of target_code_src, which the 			*/
-	/* compiler will probably nicely optimize away if not needed				*/
+	/* if not we only need target_code_size. */
+
+	/* However, we don't want to create multiple code paths, so we */
+	/* do the unecessary evaluation of target_code_src, which the */
+	/* compiler will probably nicely optimize away if not needed */
 
 	/* prepare algorithm code for target endian */
 	switch (bank->bus_width)
@@ -1288,11 +1262,11 @@ cleanup:
 
 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;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
 	target_t *target = bank->target;
-	reg_param_t reg_params[10];
-	armv4_5_algorithm_t armv4_5_info;
+	struct reg_param reg_params[10];
+	struct armv4_5_algorithm armv4_5_info;
 	working_area_t *source;
 	uint32_t buffer_size = 32768;
 	uint32_t status;
@@ -1447,44 +1421,46 @@ static int cfi_spansion_write_block(struct flash_bank_s *bank, uint8_t *buffer,
 	armv4_5_info.core_mode = ARMV4_5_MODE_SVC;
 	armv4_5_info.core_state = ARMV4_5_STATE_ARM;
 
-	/* flash write code */
 	int target_code_size;
+	const uint32_t *target_code_src;
+
+	switch (bank->bus_width)
+	{
+	case 1 :
+		target_code_src = word_8_code;
+		target_code_size = sizeof(word_8_code);
+		break;
+	case 2 :
+		/* Check for DQ5 support */
+		if( cfi_info->status_poll_mask & (1 << 5) )
+		{
+			target_code_src = word_16_code;
+			target_code_size = sizeof(word_16_code);
+		}
+		else
+		{
+			/* No DQ5 support. Use DQ7 DATA# polling only. */
+			target_code_src = word_16_code_dq7only;
+			target_code_size = sizeof(word_16_code_dq7only);
+		}
+		break;
+	case 4 :
+		target_code_src = word_32_code;
+		target_code_size = sizeof(word_32_code);
+		break;
+	default:
+		LOG_ERROR("Unsupported bank buswidth %d, can't do block memory writes", bank->bus_width);
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	/* flash write code */
 	if (!cfi_info->write_algorithm)
 	{
 		uint8_t *target_code;
-		const uint32_t *src;
 
 		/* convert bus-width dependent algorithm code to correct endiannes */
-		switch (bank->bus_width)
-		{
-		case 1:
-			src = word_8_code;
-			target_code_size = sizeof(word_8_code);
-			break;
-		case 2:
-			/* Check for DQ5 support */
-			if( cfi_info->status_poll_mask & (1 << 5) )
-			{
-				src = word_16_code;
-				target_code_size = sizeof(word_16_code);
-			}
-			else
-			{
-				/* No DQ5 support. Use DQ7 DATA# polling only. */
-				src = word_16_code_dq7only;
-				target_code_size = sizeof(word_16_code_dq7only);
-			}
-			break;
-		case 4:
-			src = word_32_code;
-			target_code_size = sizeof(word_32_code);
-			break;
-		default:
-			LOG_ERROR("Unsupported bank buswidth %d, can't do block memory writes", bank->bus_width);
-			return ERROR_FLASH_OPERATION_FAILED;
-		}
 		target_code = malloc(target_code_size);
-		cfi_fix_code_endian(target, target_code, src, target_code_size / 4);
+		cfi_fix_code_endian(target, target_code, target_code_src, target_code_size / 4);
 
 		/* allocate working area */
 		retval = target_alloc_working_area(target, target_code_size,
@@ -1586,7 +1562,7 @@ static int cfi_spansion_write_block(struct flash_bank_s *bank, uint8_t *buffer,
 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;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
 	target_t *target = bank->target;
 	uint8_t command[8];
 
@@ -1620,7 +1596,7 @@ static int cfi_intel_write_word(struct flash_bank_s *bank, uint8_t *word, uint32
 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;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
 	target_t *target = bank->target;
 	uint8_t command[8];
 
@@ -1713,8 +1689,8 @@ static int cfi_intel_write_words(struct flash_bank_s *bank, uint8_t *word, uint3
 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;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
 	target_t *target = bank->target;
 	uint8_t command[8];
 
@@ -1759,10 +1735,10 @@ static int cfi_spansion_write_word(struct flash_bank_s *bank, uint8_t *word, uin
 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;
+	struct cfi_flash_bank *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;
+	struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
 
 	/* Calculate buffer size and boundary mask */
 	uint32_t buffersize = (1UL << cfi_info->max_buf_write_size) * (bank->bus_width / bank->chip_width);
@@ -1850,7 +1826,7 @@ static int cfi_spansion_write_words(struct flash_bank_s *bank, uint8_t *word, ui
 
 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;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
 
 	switch (cfi_info->pri_id)
 	{
@@ -1871,7 +1847,7 @@ static int cfi_write_word(struct flash_bank_s *bank, uint8_t *word, uint32_t add
 
 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;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
 
 	switch (cfi_info->pri_id)
 	{
@@ -1892,7 +1868,7 @@ static int cfi_write_words(struct flash_bank_s *bank, uint8_t *word, uint32_t wo
 
 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;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
 	target_t *target = bank->target;
 	uint32_t address = bank->base + offset;	/* address of first byte to be programmed */
 	uint32_t write_p, copy_p;
@@ -2102,8 +2078,8 @@ int cfi_write(struct flash_bank_s *bank, uint8_t *buffer, uint32_t offset, uint3
 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;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
 
 	pri_ext->_reversed_geometry = 1;
 }
@@ -2111,8 +2087,8 @@ static void cfi_fixup_atmel_reversed_erase_regions(flash_bank_t *bank, void *par
 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;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
 	(void) param;
 
 	if ((pri_ext->_reversed_geometry) || (pri_ext->TopBottom == 3))
@@ -2133,17 +2109,56 @@ static void cfi_fixup_0002_erase_regions(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;
-	cfi_unlock_addresses_t *unlock_addresses = param;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
+	struct cfi_unlock_addresses *unlock_addresses = param;
 
 	pri_ext->_unlock1 = unlock_addresses->unlock1;
 	pri_ext->_unlock2 = unlock_addresses->unlock2;
 }
 
+
+static int cfi_query_string(struct flash_bank_s *bank, int address)
+{
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	target_t *target = bank->target;
+	int retval;
+	uint8_t command[8];
+
+	cfi_command(bank, 0x98, command);
+	if ((retval = target_write_memory(target, flash_address(bank, 0, address), 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);
+	cfi_info->qry[2] = cfi_query_u8(bank, 0, 0x12);
+
+	LOG_DEBUG("CFI qry returned: 0x%2.2x 0x%2.2x 0x%2.2x", cfi_info->qry[0], cfi_info->qry[1], cfi_info->qry[2]);
+
+	if ((cfi_info->qry[0] != 'Q') || (cfi_info->qry[1] != 'R') || (cfi_info->qry[2] != 'Y'))
+	{
+		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;
+		}
+		cfi_command(bank, 0xff, 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 probe bank: no QRY");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	return ERROR_OK;
+}
+
 static int cfi_probe(struct flash_bank_s *bank)
 {
-	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
 	target_t *target = bank->target;
 	uint8_t command[8];
 	int num_sectors = 0;
@@ -2234,6 +2249,8 @@ static int cfi_probe(struct flash_bank_s *bank)
 	 */
 	if (cfi_info->not_cfi == 0)
 	{
+		int retval;
+
 		/* enter CFI query mode
 		 * according to JEDEC Standard No. 68.01,
 		 * a single bus sequence with address = 0x55, data = 0x98 should put
@@ -2241,33 +2258,21 @@ static 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);
-		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);
-		cfi_info->qry[2] = cfi_query_u8(bank, 0, 0x12);
-
-		LOG_DEBUG("CFI qry returned: 0x%2.2x 0x%2.2x 0x%2.2x", cfi_info->qry[0], cfi_info->qry[1], cfi_info->qry[2]);
 
-		if ((cfi_info->qry[0] != 'Q') || (cfi_info->qry[1] != 'R') || (cfi_info->qry[2] != 'Y'))
+		retval = cfi_query_string(bank, 0x55);
+		if (retval != 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;
-			}
-			cfi_command(bank, 0xff, 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 probe bank: no QRY");
-			return ERROR_FLASH_BANK_INVALID;
+			/*
+			 * Spansion S29WS-N CFI query fix is to try 0x555 if 0x55 fails. Should
+			 * be harmless enough:
+			 *
+			 * http://www.infradead.org/pipermail/linux-mtd/2005-September/013618.html
+			 */
+			LOG_USER("Try workaround w/0x555 instead of 0x55 to get QRY.");
+			retval = cfi_query_string(bank, 0x555);
 		}
+		if (retval != ERROR_OK)
+			return retval;
 
 		cfi_info->pri_id = cfi_query_u16(bank, 0, 0x13);
 		cfi_info->pri_addr = cfi_query_u16(bank, 0, 0x15);
@@ -2386,7 +2391,7 @@ static int cfi_probe(struct flash_bank_s *bank)
 	{
 		/* a device might have only one erase block, spanning the whole device */
 		bank->num_sectors = 1;
-		bank->sectors = malloc(sizeof(flash_sector_t));
+		bank->sectors = malloc(sizeof(struct flash_sector));
 
 		bank->sectors[sector].offset = 0x0;
 		bank->sectors[sector].size = bank->size;
@@ -2403,7 +2408,7 @@ static int cfi_probe(struct flash_bank_s *bank)
 		}
 
 		bank->num_sectors = num_sectors;
-		bank->sectors = malloc(sizeof(flash_sector_t) * num_sectors);
+		bank->sectors = malloc(sizeof(struct flash_sector) * num_sectors);
 
 		for (i = 0; i < cfi_info->num_erase_regions; i++)
 		{
@@ -2432,7 +2437,7 @@ static int cfi_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;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
 	if (cfi_info->probed)
 		return ERROR_OK;
 	return cfi_probe(bank);
@@ -2442,8 +2447,8 @@ static int cfi_auto_probe(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;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct cfi_intel_pri_ext *pri_ext = cfi_info->pri_ext;
 	target_t *target = bank->target;
 	uint8_t command[CFI_MAX_BUS_WIDTH];
 	int i;
@@ -2475,8 +2480,8 @@ static int cfi_intel_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;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
 	target_t *target = bank->target;
 	uint8_t command[8];
 	int i;
@@ -2515,7 +2520,7 @@ static int cfi_spansion_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;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
 
 	if (bank->target->state != TARGET_HALTED)
 	{
@@ -2546,7 +2551,7 @@ 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 printed;
-	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
 
 	if (cfi_info->qry[0] == (char)-1)
 	{
@@ -2620,3 +2625,17 @@ static int cfi_info(struct flash_bank_s *bank, char *buf, int buf_size)
 
 	return ERROR_OK;
 }
+
+struct flash_driver cfi_flash = {
+		.name = "cfi",
+		.register_commands = &cfi_register_commands,
+		.flash_bank_command = &cfi_flash_bank_command,
+		.erase = &cfi_erase,
+		.protect = &cfi_protect,
+		.write = &cfi_write,
+		.probe = &cfi_probe,
+		.auto_probe = &cfi_auto_probe,
+		.erase_check = &default_flash_blank_check,
+		.protect_check = &cfi_protect_check,
+		.info = &cfi_info,
+	};