move nor drivers to src/flash/nor

Moves NOR flash drivers to 'src/flash/nor/'.
Adds 'src/flash/nor/Makefile.am'.
Builds 'libocdflashnor.la'.

35 files changed, 20332 insertions, 0 deletions

diff --git a/src/flash/nor/Makefile.am b/src/flash/nor/Makefile.am
new file mode 100644
index 00000000..d2d99981
--- /dev/null
+++ b/src/flash/nor/Makefile.am
@@ -0,0 +1,46 @@
+AM_CPPFLAGS = \
+	-I$(top_srcdir)/src/helper \
+	-I$(top_srcdir)/src/jtag \
+	-I$(top_srcdir)/src/flash \
+	-I$(top_srcdir)/src/target
+
+noinst_LTLIBRARIES = libocdflashnor.la
+libocdflashnor_la_SOURCES = \
+	aduc702x.c \
+	at91sam3.c \
+	at91sam7.c \
+	avrf.c \
+	cfi.c \
+	ecos.c \
+	faux.c \
+	lpc2000.c \
+	lpc288x.c \
+	lpc2900.c \
+	non_cfi.c \
+	ocl.c \
+	pic32mx.c \
+	stellaris.c \
+	stm32x.c \
+	str7x.c \
+	str9x.c \
+	str9xpec.c \
+	tms470.c
+
+noinst_HEADERS = \
+	at91sam7.h \
+	at91sam3.h \
+	avrf.h \
+	cfi.h \
+	lpc2000.h \
+	lpc288x.h \
+	non_cfi.h \
+	ocl.h \
+	pic32mx.h \
+	stellaris.h \
+	stm32x.h \
+	str7x.h \
+	str9x.h \
+	str9xpec.h \
+	tms470.h
+
+MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
diff --git a/src/flash/nor/aduc702x.c b/src/flash/nor/aduc702x.c
new file mode 100644
index 00000000..643705ca
--- /dev/null
+++ b/src/flash/nor/aduc702x.c
@@ -0,0 +1,425 @@
+/***************************************************************************
+ *   Copyright (C) 2008 by Kevin McGuire                                   *
+ *   Copyright (C) 2008 by Marcel Wijlaars                                 *
+ *   Copyright (C) 2009 by Michael Ashton                                  *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "flash.h"
+#include "armv4_5.h"
+#include "binarybuffer.h"
+#include "time_support.h"
+#include "algorithm.h"
+
+
+static int aduc702x_build_sector_list(struct flash_bank *bank);
+static int aduc702x_check_flash_completion(struct target* target, unsigned int timeout_ms);
+static int aduc702x_set_write_enable(struct target *target, int enable);
+
+#define ADUC702x_FLASH				0xfffff800
+#define ADUC702x_FLASH_FEESTA		(0*4)
+#define ADUC702x_FLASH_FEEMOD		(1*4)
+#define ADUC702x_FLASH_FEECON		(2*4)
+#define ADUC702x_FLASH_FEEDAT		(3*4)
+#define ADUC702x_FLASH_FEEADR		(4*4)
+#define ADUC702x_FLASH_FEESIGN		(5*4)
+#define ADUC702x_FLASH_FEEPRO		(6*4)
+#define ADUC702x_FLASH_FEEHIDE		(7*4)
+
+struct aduc702x_flash_bank {
+	struct working_area *write_algorithm;
+};
+
+/* flash bank aduc702x 0 0 0 0 <target#>
+ * The ADC7019-28 devices all have the same flash layout */
+FLASH_BANK_COMMAND_HANDLER(aduc702x_flash_bank_command)
+{
+	struct aduc702x_flash_bank *nbank;
+
+	nbank = malloc(sizeof(struct aduc702x_flash_bank));
+
+        bank->base = 0x80000;
+        bank->size = 0xF800; // top 4k not accessible
+	bank->driver_priv = nbank;
+
+        aduc702x_build_sector_list(bank);
+
+        return ERROR_OK;
+}
+
+static int aduc702x_build_sector_list(struct flash_bank *bank)
+{
+	//aduc7026_struct flash_bank *aduc7026_info = bank->driver_priv;
+
+        int i = 0;
+        uint32_t offset = 0;
+
+        // sector size is 512
+        bank->num_sectors = bank->size / 512;
+        bank->sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
+        for (i = 0; i < bank->num_sectors; ++i)
+        {
+                bank->sectors[i].offset = offset;
+                bank->sectors[i].size = 512;
+                offset += bank->sectors[i].size;
+                bank->sectors[i].is_erased = -1;
+                bank->sectors[i].is_protected = 0;
+        }
+
+	return ERROR_OK;
+}
+
+static int aduc702x_protect_check(struct flash_bank *bank)
+{
+	printf("aduc702x_protect_check not implemented yet.\n");
+	return ERROR_OK;
+}
+
+static int aduc702x_erase(struct flash_bank *bank, int first, int last)
+{
+        //int res;
+	int x;
+	int count;
+	//uint32_t v;
+	struct target *target = bank->target;
+
+        aduc702x_set_write_enable(target, 1);
+
+	/* mass erase */
+	if (((first | last) == 0) || ((first == 0) && (last >= bank->num_sectors))) {
+		LOG_DEBUG("performing mass erase.\n");
+		target_write_u16(target, ADUC702x_FLASH + ADUC702x_FLASH_FEEDAT, 0x3cff);
+		target_write_u16(target, ADUC702x_FLASH + ADUC702x_FLASH_FEEADR, 0xffc3);
+		target_write_u8(target, ADUC702x_FLASH + ADUC702x_FLASH_FEECON, 0x06);
+
+                if (aduc702x_check_flash_completion(target, 3500) != ERROR_OK)
+		{
+			LOG_ERROR("mass erase failed\n");
+                        aduc702x_set_write_enable(target, 0);
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+
+		LOG_DEBUG("mass erase successful.\n");
+		return ERROR_OK;
+	} else {
+                unsigned long adr;
+
+                count = last - first + 1;
+                for (x = 0; x < count; ++x)
+                {
+                        adr = bank->base + ((first + x) * 512);
+
+                        target_write_u16(target, ADUC702x_FLASH + ADUC702x_FLASH_FEEADR, adr);
+                        target_write_u8(target, ADUC702x_FLASH + ADUC702x_FLASH_FEECON, 0x05);
+
+                        if (aduc702x_check_flash_completion(target, 50) != ERROR_OK)
+                        {
+                                LOG_ERROR("failed to erase sector at address 0x%08lX\n", adr);
+                                aduc702x_set_write_enable(target, 0);
+                                return ERROR_FLASH_SECTOR_NOT_ERASED;
+                        }
+
+                        LOG_DEBUG("erased sector at address 0x%08lX\n", adr);
+                }
+        }
+
+        aduc702x_set_write_enable(target, 0);
+
+	return ERROR_OK;
+}
+
+static int aduc702x_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	printf("aduc702x_protect not implemented yet.\n");
+	return ERROR_FLASH_OPERATION_FAILED;
+}
+
+/* If this fn returns ERROR_TARGET_RESOURCE_NOT_AVAILABLE, then the caller can fall
+ * back to another mechanism that does not require onboard RAM
+ *
+ * Caller should not check for other return values specifically
+ */
+static int aduc702x_write_block(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	struct aduc702x_flash_bank *aduc702x_info = bank->driver_priv;
+	struct target *target = bank->target;
+	uint32_t buffer_size = 7000;
+	struct working_area *source;
+	uint32_t address = bank->base + offset;
+	struct reg_param reg_params[6];
+	struct armv4_5_algorithm armv4_5_info;
+	int retval = ERROR_OK;
+
+	if (((count%2)!=0)||((offset%2)!=0))
+	{
+		LOG_ERROR("write block must be multiple of two bytes in offset & length");
+		return ERROR_FAIL;
+	}
+
+        /* parameters:
+
+        r0 - address of source data (absolute)
+        r1 - number of halfwords to be copied
+        r2 - start address in flash (offset from beginning of flash memory)
+        r3 - exit code
+        r4 - base address of flash controller (0xFFFFF800)
+
+        registers:
+
+        r5 - scratch
+        r6 - set to 2, used to write flash command
+
+        */
+        uint32_t aduc702x_flash_write_code[] = {
+        //<_start>:
+                0xe3a05008,	// mov	r5, #8	; 0x8
+                0xe5845004,	// str	r5, [r4, #4]
+                0xe3a06002,	// mov	r6, #2	; 0x2
+        //<next>:
+                0xe1c421b0,	// strh	r2, [r4, #16]
+                0xe0d050b2,	// ldrh	r5, [r0], #2
+                0xe1c450bc,	// strh	r5, [r4, #12]
+                0xe5c46008,	// strb	r6, [r4, #8]
+        //<wait_complete>:
+                0xe1d430b0,	// ldrh	r3, [r4]
+                0xe3130004,	// tst	r3, #4	; 0x4
+                0x1afffffc,	// bne	1001c <wait_complete>
+                0xe2822002,	// add	r2, r2, #2	; 0x2
+                0xe2511001,	// subs	r1, r1, #1	; 0x1
+                0x0a000001,	// beq	1003c <done>
+                0xe3130001,	// tst	r3, #1	; 0x1
+                0x1afffff3,	// bne	1000c <next>
+        //<done>:
+                0xeafffffe 	// b	1003c <done>
+	};
+
+	/* flash write code */
+	if (target_alloc_working_area(target, sizeof(aduc702x_flash_write_code),
+                &aduc702x_info->write_algorithm) != ERROR_OK)
+	{
+		LOG_WARNING("no working area available, can't do block memory writes");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	};
+
+	retval=target_write_buffer(target, aduc702x_info->write_algorithm->address,
+                sizeof(aduc702x_flash_write_code), (uint8_t*)aduc702x_flash_write_code);
+	if (retval!=ERROR_OK)
+	{
+		return retval;
+	}
+
+	/* memory buffer */
+	while (target_alloc_working_area(target, buffer_size, &source) != ERROR_OK)
+	{
+		buffer_size /= 2;
+		if (buffer_size <= 256)
+		{
+			/* if we already allocated the writing code, but failed to get a buffer, free the algorithm */
+			if (aduc702x_info->write_algorithm)
+				target_free_working_area(target, aduc702x_info->write_algorithm);
+
+			LOG_WARNING("no large enough working area available, can't do block memory writes");
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+	}
+
+	armv4_5_info.common_magic = ARMV4_5_COMMON_MAGIC;
+	armv4_5_info.core_mode = ARMV4_5_MODE_SVC;
+	armv4_5_info.core_state = ARMV4_5_STATE_ARM;
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
+	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
+	init_reg_param(&reg_params[3], "r3", 32, PARAM_IN);
+	init_reg_param(&reg_params[4], "r4", 32, PARAM_OUT);
+
+	while (count > 0)
+	{
+		uint32_t thisrun_count = (count > buffer_size) ? buffer_size : count;
+
+		retval=target_write_buffer(target, source->address, thisrun_count, buffer);
+		if (retval!=ERROR_OK)
+		{
+			break;
+		}
+
+		buf_set_u32(reg_params[0].value, 0, 32, source->address);
+		buf_set_u32(reg_params[1].value, 0, 32, thisrun_count/2);
+		buf_set_u32(reg_params[2].value, 0, 32, address);
+		buf_set_u32(reg_params[4].value, 0, 32, 0xFFFFF800);
+
+		if ((retval = target_run_algorithm(target, 0, NULL, 5,
+                        reg_params, aduc702x_info->write_algorithm->address,
+                        aduc702x_info->write_algorithm->address + sizeof(aduc702x_flash_write_code) - 4,
+                        10000, &armv4_5_info)) != ERROR_OK)
+		{
+			LOG_ERROR("error executing aduc702x flash write algorithm");
+			break;
+		}
+
+		if ((buf_get_u32(reg_params[3].value, 0, 32) & 1) != 1)
+		{
+			/* FIX!!!! what does this mean??? replace w/sensible error message */
+			LOG_ERROR("aduc702x detected error writing flash");
+			retval = ERROR_FAIL;
+			break;
+		}
+
+		buffer += thisrun_count;
+		address += thisrun_count;
+		count -= thisrun_count;
+	}
+
+	target_free_working_area(target, source);
+	target_free_working_area(target, aduc702x_info->write_algorithm);
+
+	destroy_reg_param(&reg_params[0]);
+	destroy_reg_param(&reg_params[1]);
+	destroy_reg_param(&reg_params[2]);
+	destroy_reg_param(&reg_params[3]);
+	destroy_reg_param(&reg_params[4]);
+
+	return retval;
+}
+
+/* All-JTAG, single-access method.  Very slow.  Used only if there is no
+ * working area available. */
+static int aduc702x_write_single(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	uint32_t x;
+        uint8_t b;
+	struct target *target = bank->target;
+
+        aduc702x_set_write_enable(target, 1);
+
+	for (x = 0; x < count; x += 2) {
+                // FEEADR = address
+		target_write_u16(target, ADUC702x_FLASH + ADUC702x_FLASH_FEEADR, offset + x);
+
+                // set up data
+		if ((x + 1) == count)
+                {
+                        // last byte
+                        target_read_u8(target, offset + x + 1, &b);
+                }
+                else
+                        b = buffer[x + 1];
+
+                target_write_u16(target, ADUC702x_FLASH + ADUC702x_FLASH_FEEDAT, buffer[x] | (b << 8));
+
+                // do single-write command
+		target_write_u8(target, ADUC702x_FLASH + ADUC702x_FLASH_FEECON, 0x02);
+
+                if (aduc702x_check_flash_completion(target, 1) != ERROR_OK)
+                {
+			LOG_ERROR("single write failed for address 0x%08lX\n", (unsigned long)(offset + x));
+                        aduc702x_set_write_enable(target, 0);
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+
+	}
+        LOG_DEBUG("wrote %d bytes at address 0x%08lX\n", (int)count, (unsigned long)(offset + x));
+
+        aduc702x_set_write_enable(target, 0);
+
+	return ERROR_OK;
+}
+
+int aduc702x_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	int retval;
+
+        /* try using a block write */
+        if ((retval = aduc702x_write_block(bank, buffer, offset, count)) != ERROR_OK)
+        {
+                if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
+                {
+                        /* if block write failed (no sufficient working area),
+                         * use normal (slow) JTAG method */
+                        LOG_WARNING("couldn't use block writes, falling back to single memory accesses");
+
+                        if ((retval = aduc702x_write_single(bank, buffer, offset, count)) != ERROR_OK)
+                        {
+                                LOG_ERROR("slow write failed");
+                                return ERROR_FLASH_OPERATION_FAILED;
+                        }
+                }
+        }
+
+        return retval;
+}
+
+static int aduc702x_probe(struct flash_bank *bank)
+{
+	return ERROR_OK;
+}
+
+static int aduc702x_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	snprintf(buf, buf_size, "aduc702x flash driver info");
+	return ERROR_OK;
+}
+
+/* sets FEEMOD bit 3
+ * enable = 1 enables writes & erases, 0 disables them */
+static int aduc702x_set_write_enable(struct target *target, int enable)
+{
+        // don't bother to preserve int enable bit here
+        target_write_u16(target, ADUC702x_FLASH + ADUC702x_FLASH_FEEMOD, enable ? 8 : 0);
+
+        return ERROR_OK;
+}
+
+/* wait up to timeout_ms for controller to not be busy,
+ * then check whether the command passed or failed.
+ *
+ * this function sleeps 1ms between checks (after the first one),
+ * so in some cases may slow things down without a usleep after the first read */
+static int aduc702x_check_flash_completion(struct target* target, unsigned int timeout_ms)
+{
+        uint8_t v = 4;
+
+        long long endtime = timeval_ms() + timeout_ms;
+        while (1) {
+                target_read_u8(target, ADUC702x_FLASH + ADUC702x_FLASH_FEESTA, &v);
+                if ((v & 4) == 0) break;
+                alive_sleep(1);
+                if (timeval_ms() >= endtime) break;
+        }
+
+        if (v & 2) return ERROR_FAIL;
+        // if a command is ignored, both the success and fail bits may be 0
+        else if ((v & 3) == 0) return ERROR_FAIL;
+        else return ERROR_OK;
+}
+
+struct flash_driver aduc702x_flash = {
+		.name = "aduc702x",
+		.flash_bank_command = &aduc702x_flash_bank_command,
+		.erase = &aduc702x_erase,
+		.protect = &aduc702x_protect,
+		.write = &aduc702x_write,
+		.probe = &aduc702x_probe,
+		.auto_probe = &aduc702x_probe,
+		.erase_check = &default_flash_blank_check,
+		.protect_check = &aduc702x_protect_check,
+		.info = &aduc702x_info
+	};
diff --git a/src/flash/nor/at91sam3.c b/src/flash/nor/at91sam3.c
new file mode 100644
index 00000000..be17a5f8
--- /dev/null
+++ b/src/flash/nor/at91sam3.c
@@ -0,0 +1,2516 @@
+/***************************************************************************
+ *   Copyright (C) 2009 by Duane Ellis                                     *
+ *   openocd@duaneellis.com                                                *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS for A PARTICULAR PURPOSE.  See the         *
+ *   GNU General public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+****************************************************************************/
+
+/* Some of the the lower level code was based on code supplied by
+ * ATMEL under this copyright. */
+
+/* BEGIN ATMEL COPYRIGHT */
+/* ----------------------------------------------------------------------------
+ *         ATMEL Microcontroller Software Support
+ * ----------------------------------------------------------------------------
+ * Copyright (c) 2009, Atmel Corporation
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * - Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the disclaimer below.
+ *
+ * Atmel's name may not be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * DISCLAIMER: THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE
+ * DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
+ * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ * ----------------------------------------------------------------------------
+ */
+/* END ATMEL COPYRIGHT */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+
+#include <stdio.h>
+#include <string.h>
+#include <stddef.h>
+#include "types.h"
+#include "flash.h"
+#include "membuf.h"
+#include "at91sam3.h"
+#include "time_support.h"
+
+#define REG_NAME_WIDTH  (12)
+
+
+#define FLASH_BANK0_BASE   0x00080000
+#define FLASH_BANK1_BASE   0x00100000
+
+#define 	AT91C_EFC_FCMD_GETD                 (0x0) // (EFC) Get Flash Descriptor
+#define 	AT91C_EFC_FCMD_WP                   (0x1) // (EFC) Write Page
+#define 	AT91C_EFC_FCMD_WPL                  (0x2) // (EFC) Write Page and Lock
+#define 	AT91C_EFC_FCMD_EWP                  (0x3) // (EFC) Erase Page and Write Page
+#define 	AT91C_EFC_FCMD_EWPL                 (0x4) // (EFC) Erase Page and Write Page then Lock
+#define 	AT91C_EFC_FCMD_EA                   (0x5) // (EFC) Erase All
+// cmd6 is not present int he at91sam3u4/2/1 data sheet table 17-2
+// #define 	AT91C_EFC_FCMD_EPL                  (0x6) // (EFC) Erase plane?
+// cmd7 is not present int he at91sam3u4/2/1 data sheet table 17-2
+// #define 	AT91C_EFC_FCMD_EPA                  (0x7) // (EFC) Erase pages?
+#define 	AT91C_EFC_FCMD_SLB                  (0x8) // (EFC) Set Lock Bit
+#define 	AT91C_EFC_FCMD_CLB                  (0x9) // (EFC) Clear Lock Bit
+#define 	AT91C_EFC_FCMD_GLB                  (0xA) // (EFC) Get Lock Bit
+#define 	AT91C_EFC_FCMD_SFB                  (0xB) // (EFC) Set Fuse Bit
+#define 	AT91C_EFC_FCMD_CFB                  (0xC) // (EFC) Clear Fuse Bit
+#define 	AT91C_EFC_FCMD_GFB                  (0xD) // (EFC) Get Fuse Bit
+#define 	AT91C_EFC_FCMD_STUI                 (0xE) // (EFC) Start Read Unique ID
+#define 	AT91C_EFC_FCMD_SPUI                 (0xF) // (EFC) Stop Read Unique ID
+
+#define  offset_EFC_FMR   0
+#define  offset_EFC_FCR   4
+#define  offset_EFC_FSR   8
+#define  offset_EFC_FRR   12
+
+
+static float
+_tomhz(uint32_t freq_hz)
+{
+	float f;
+
+	f = ((float)(freq_hz)) / 1000000.0;
+	return f;
+}
+
+// How the chip is configured.
+struct sam3_cfg {
+	uint32_t unique_id[4];
+
+	uint32_t slow_freq;
+	uint32_t rc_freq;
+	uint32_t mainosc_freq;
+	uint32_t plla_freq;
+	uint32_t mclk_freq;
+	uint32_t cpu_freq;
+	uint32_t fclk_freq;
+	uint32_t pclk0_freq;
+	uint32_t pclk1_freq;
+	uint32_t pclk2_freq;
+
+
+#define SAM3_CHIPID_CIDR          (0x400E0740)
+	uint32_t CHIPID_CIDR;
+#define SAM3_CHIPID_EXID          (0x400E0744)
+	uint32_t CHIPID_EXID;
+
+#define SAM3_SUPC_CR              (0x400E1210)
+	uint32_t SUPC_CR;
+
+#define SAM3_PMC_BASE             (0x400E0400)
+#define SAM3_PMC_SCSR             (SAM3_PMC_BASE + 0x0008)
+	uint32_t PMC_SCSR;
+#define SAM3_PMC_PCSR             (SAM3_PMC_BASE + 0x0018)
+	uint32_t PMC_PCSR;
+#define SAM3_CKGR_UCKR            (SAM3_PMC_BASE + 0x001c)
+	uint32_t CKGR_UCKR;
+#define SAM3_CKGR_MOR             (SAM3_PMC_BASE + 0x0020)
+	uint32_t CKGR_MOR;
+#define SAM3_CKGR_MCFR            (SAM3_PMC_BASE + 0x0024)
+	uint32_t CKGR_MCFR;
+#define SAM3_CKGR_PLLAR           (SAM3_PMC_BASE + 0x0028)
+	uint32_t CKGR_PLLAR;
+#define SAM3_PMC_MCKR             (SAM3_PMC_BASE + 0x0030)
+	uint32_t PMC_MCKR;
+#define SAM3_PMC_PCK0             (SAM3_PMC_BASE + 0x0040)
+	uint32_t PMC_PCK0;
+#define SAM3_PMC_PCK1             (SAM3_PMC_BASE + 0x0044)
+	uint32_t PMC_PCK1;
+#define SAM3_PMC_PCK2             (SAM3_PMC_BASE + 0x0048)
+	uint32_t PMC_PCK2;
+#define SAM3_PMC_SR               (SAM3_PMC_BASE + 0x0068)
+	uint32_t PMC_SR;
+#define SAM3_PMC_IMR              (SAM3_PMC_BASE + 0x006c)
+	uint32_t PMC_IMR;
+#define SAM3_PMC_FSMR             (SAM3_PMC_BASE + 0x0070)
+	uint32_t PMC_FSMR;
+#define SAM3_PMC_FSPR             (SAM3_PMC_BASE + 0x0074)
+	uint32_t PMC_FSPR;
+};
+
+
+struct sam3_bank_private {
+	int probed;
+	// DANGER: THERE ARE DRAGONS HERE..
+	// NOTE: If you add more 'ghost' pointers
+	// be aware that you must *manually* update
+	// these pointers in the function sam3_GetDetails()
+	// See the comment "Here there be dragons"
+
+	// so we can find the chip we belong to
+	struct sam3_chip *pChip;
+	// so we can find the orginal bank pointer
+	struct flash_bank *pBank;
+	unsigned bank_number;
+	uint32_t controller_address;
+	uint32_t base_address;
+	bool present;
+	unsigned size_bytes;
+	unsigned nsectors;
+	unsigned sector_size;
+	unsigned page_size;
+};
+
+struct sam3_chip_details {
+	// THERE ARE DRAGONS HERE..
+	// note: If you add pointers here
+	// becareful about them as they
+	// may need to be updated inside
+	// the function: "sam3_GetDetails()
+	// which copy/overwrites the
+	// 'runtime' copy of this structure
+	uint32_t chipid_cidr;
+	const char *name;
+
+	unsigned n_gpnvms;
+#define SAM3_N_NVM_BITS 3
+	unsigned  gpnvm[SAM3_N_NVM_BITS];
+	unsigned  total_flash_size;
+	unsigned  total_sram_size;
+	unsigned  n_banks;
+#define SAM3_MAX_FLASH_BANKS 2
+	// these are "initialized" from the global const data
+	struct sam3_bank_private bank[SAM3_MAX_FLASH_BANKS];
+};
+
+
+struct sam3_chip {
+	struct sam3_chip *next;
+	int    probed;
+
+	// this is "initialized" from the global const structure
+	struct sam3_chip_details details;
+	struct target *target;
+	struct sam3_cfg cfg;
+
+	struct membuf *mbuf;
+};
+
+
+struct sam3_reg_list {
+	uint32_t address;  size_t struct_offset; const char *name;
+	void (*explain_func)(struct sam3_chip *pInfo);
+};
+
+
+static struct sam3_chip *all_sam3_chips;
+
+static struct sam3_chip *
+get_current_sam3(struct command_context *cmd_ctx)
+{
+	struct target *t;
+	static struct sam3_chip *p;
+
+	t = get_current_target(cmd_ctx);
+	if (!t) {
+		command_print(cmd_ctx, "No current target?");
+		return NULL;
+	}
+
+	p = all_sam3_chips;
+	if (!p) {
+		// this should not happen
+		// the command is not registered until the chip is created?
+		command_print(cmd_ctx, "No SAM3 chips exist?");
+		return NULL;
+	}
+
+	while (p) {
+		if (p->target == t) {
+			return p;
+		}
+		p = p->next;
+	}
+	command_print(cmd_ctx, "Cannot find SAM3 chip?");
+	return NULL;
+}
+
+
+// these are used to *initialize* the "pChip->details" structure.
+static const struct sam3_chip_details all_sam3_details[] = {
+	{
+		.chipid_cidr    = 0x28100960,
+		.name           = "at91sam3u4e",
+		.total_flash_size     = 256 * 1024,
+		.total_sram_size      = 52 * 1024,
+		.n_gpnvms       = 3,
+		.n_banks        = 2,
+
+		// System boots at address 0x0
+		// gpnvm[1] = selects boot code
+		//     if gpnvm[1] == 0
+		//         boot is via "SAMBA" (rom)
+		//     else
+		//         boot is via FLASH
+		//         Selection is via gpnvm[2]
+		//     endif
+		//
+		// NOTE: banks 0 & 1 switch places
+		//     if gpnvm[2] == 0
+		//         Bank0 is the boot rom
+		//      else
+		//         Bank1 is the boot rom
+		//      endif
+//		.bank[0] = {
+		{
+		  {
+			.probed = 0,
+			.pChip  = NULL,
+			.pBank  = NULL,
+			.bank_number = 0,
+			.base_address = FLASH_BANK0_BASE,
+			.controller_address = 0x400e0800,
+			.present = 1,
+			.size_bytes = 128 * 1024,
+			.nsectors   = 16,
+			.sector_size = 8192,
+			.page_size   = 256,
+		  },
+
+//		.bank[1] = {
+		  {
+			.probed = 0,
+			.pChip  = NULL,
+			.pBank  = NULL,
+			.bank_number = 1,
+			.base_address = FLASH_BANK1_BASE,
+			.controller_address = 0x400e0a00,
+			.present = 1,
+			.size_bytes = 128 * 1024,
+			.nsectors   = 16,
+			.sector_size = 8192,
+			.page_size   = 256,
+		  },
+		},
+	},
+
+	{
+		.chipid_cidr    = 0x281a0760,
+		.name           = "at91sam3u2e",
+		.total_flash_size     = 128 * 1024,
+		.total_sram_size      =  36 * 1024,
+		.n_gpnvms       = 2,
+		.n_banks        = 1,
+
+		// System boots at address 0x0
+		// gpnvm[1] = selects boot code
+		//     if gpnvm[1] == 0
+		//         boot is via "SAMBA" (rom)
+		//     else
+		//         boot is via FLASH
+		//         Selection is via gpnvm[2]
+		//     endif
+//		.bank[0] = {
+		{
+		  {
+			.probed = 0,
+			.pChip  = NULL,
+			.pBank  = NULL,
+			.bank_number = 0,
+			.base_address = FLASH_BANK0_BASE,
+			.controller_address = 0x400e0800,
+			.present = 1,
+			.size_bytes = 128 * 1024,
+			.nsectors   = 16,
+			.sector_size = 8192,
+			.page_size   = 256,
+		  },
+//		  .bank[1] = {
+		  {
+			.present = 0,
+			.probed = 0,
+			.bank_number = 1,
+		  },
+		},
+	},
+	{
+		.chipid_cidr    = 0x28190560,
+		.name           = "at91sam3u1e",
+		.total_flash_size     = 64 * 1024,
+		.total_sram_size      = 20 * 1024,
+		.n_gpnvms       = 2,
+		.n_banks        = 1,
+
+		// System boots at address 0x0
+		// gpnvm[1] = selects boot code
+		//     if gpnvm[1] == 0
+		//         boot is via "SAMBA" (rom)
+		//     else
+		//         boot is via FLASH
+		//         Selection is via gpnvm[2]
+		//     endif
+		//
+
+//		.bank[0] = {
+		{
+		  {
+			.probed = 0,
+			.pChip  = NULL,
+			.pBank  = NULL,
+			.bank_number = 0,
+			.base_address = FLASH_BANK0_BASE,
+			.controller_address = 0x400e0800,
+			.present = 1,
+			.size_bytes =  64 * 1024,
+			.nsectors   =  8,
+			.sector_size = 8192,
+			.page_size   = 256,
+		  },
+
+//		.bank[1] = {
+		  {
+			.present = 0,
+			.probed = 0,
+			.bank_number = 1,
+		  },
+		},
+	},
+
+	{
+		.chipid_cidr    = 0x28000960,
+		.name           = "at91sam3u4c",
+		.total_flash_size     = 256 * 1024,
+		.total_sram_size      = 52 * 1024,
+		.n_gpnvms       = 3,
+		.n_banks        = 2,
+
+		// System boots at address 0x0
+		// gpnvm[1] = selects boot code
+		//     if gpnvm[1] == 0
+		//         boot is via "SAMBA" (rom)
+		//     else
+		//         boot is via FLASH
+		//         Selection is via gpnvm[2]
+		//     endif
+		//
+		// NOTE: banks 0 & 1 switch places
+		//     if gpnvm[2] == 0
+		//         Bank0 is the boot rom
+		//      else
+		//         Bank1 is the boot rom
+		//      endif
+		{
+		  {
+//		.bank[0] = {
+			.probed = 0,
+			.pChip  = NULL,
+			.pBank  = NULL,
+			.bank_number = 0,
+			.base_address = FLASH_BANK0_BASE,
+			.controller_address = 0x400e0800,
+			.present = 1,
+			.size_bytes = 128 * 1024,
+			.nsectors   = 16,
+			.sector_size = 8192,
+			.page_size   = 256,
+		  },
+//		.bank[1] = {
+		  {
+			.probed = 0,
+			.pChip  = NULL,
+			.pBank  = NULL,
+			.bank_number = 1,
+			.base_address = FLASH_BANK1_BASE,
+			.controller_address = 0x400e0a00,
+			.present = 1,
+			.size_bytes = 128 * 1024,
+			.nsectors   = 16,
+			.sector_size = 8192,
+			.page_size   = 256,
+		  },
+		},
+	},
+
+	{
+		.chipid_cidr    = 0x280a0760,
+		.name           = "at91sam3u2c",
+		.total_flash_size     = 128 * 1024,
+		.total_sram_size      = 36 * 1024,
+		.n_gpnvms       = 2,
+		.n_banks        = 1,
+
+		// System boots at address 0x0
+		// gpnvm[1] = selects boot code
+		//     if gpnvm[1] == 0
+		//         boot is via "SAMBA" (rom)
+		//     else
+		//         boot is via FLASH
+		//         Selection is via gpnvm[2]
+		//     endif
+		{
+//		.bank[0] = {
+		  {
+			.probed = 0,
+			.pChip  = NULL,
+			.pBank  = NULL,
+			.bank_number = 0,
+			.base_address = FLASH_BANK0_BASE,
+			.controller_address = 0x400e0800,
+			.present = 1,
+			.size_bytes = 128 * 1024,
+			.nsectors   = 16,
+			.sector_size = 8192,
+			.page_size   = 256,
+		  },
+//		.bank[1] = {
+		  {
+			.present = 0,
+			.probed = 0,
+			.bank_number = 1,
+		  },
+		},
+	},
+	{
+		.chipid_cidr    = 0x28090560,
+		.name           = "at91sam3u1c",
+		.total_flash_size     = 64 * 1024,
+		.total_sram_size      = 20 * 1024,
+		.n_gpnvms       = 2,
+		.n_banks        = 1,
+
+		// System boots at address 0x0
+		// gpnvm[1] = selects boot code
+		//     if gpnvm[1] == 0
+		//         boot is via "SAMBA" (rom)
+		//     else
+		//         boot is via FLASH
+		//         Selection is via gpnvm[2]
+		//     endif
+		//
+
+		{
+//		.bank[0] = {
+		  {
+			.probed = 0,
+			.pChip  = NULL,
+			.pBank  = NULL,
+			.bank_number = 0,
+			.base_address = FLASH_BANK0_BASE,
+			.controller_address = 0x400e0800,
+			.present = 1,
+			.size_bytes =  64 * 1024,
+			.nsectors   =  8,
+			.sector_size = 8192,
+			.page_size   = 256,
+		  },
+//		.bank[1] = {
+		  {
+			.present = 0,
+			.probed = 0,
+			.bank_number = 1,
+
+		  },
+		},
+	},
+
+	// terminate
+	{
+		.chipid_cidr	= 0,
+		.name			= NULL,
+	}
+};
+
+/* Globals above */
+/***********************************************************************
+ **********************************************************************
+ **********************************************************************
+ **********************************************************************
+ **********************************************************************
+ **********************************************************************/
+/* *ATMEL* style code - from the SAM3 driver code */
+
+/**
+ * Get the current status of the EEFC and
+ * the value of some status bits (LOCKE, PROGE).
+ * @param pPrivate - info about the bank
+ * @param v        - result goes here
+ */
+static int
+EFC_GetStatus(struct sam3_bank_private *pPrivate, uint32_t *v)
+{
+	int r;
+	r = target_read_u32(pPrivate->pChip->target, pPrivate->controller_address + offset_EFC_FSR, v);
+	LOG_DEBUG("Status: 0x%08x (lockerror: %d, cmderror: %d, ready: %d)",
+			  (unsigned int)(*v),
+			  ((unsigned int)((*v >> 2) & 1)),
+			  ((unsigned int)((*v >> 1) & 1)),
+			  ((unsigned int)((*v >> 0) & 1)));
+
+	return r;
+}
+
+/**
+ * Get the result of the last executed command.
+ * @param pPrivate - info about the bank
+ * @param v        - result goes here
+ */
+static int
+EFC_GetResult(struct sam3_bank_private *pPrivate, uint32_t *v)
+{
+	int r;
+	uint32_t rv;
+	r = target_read_u32(pPrivate->pChip->target, pPrivate->controller_address + offset_EFC_FRR, &rv);
+	if (v) {
+		*v = rv;
+	}
+	LOG_DEBUG("Result: 0x%08x", ((unsigned int)(rv)));
+	return r;
+}
+
+static int
+EFC_StartCommand(struct sam3_bank_private *pPrivate,
+				 unsigned command, unsigned argument)
+{
+	uint32_t n,v;
+	int r;
+	int retry;
+
+	retry = 0;
+ do_retry:
+
+    // Check command & argument
+    switch (command) {
+
+	case AT91C_EFC_FCMD_WP:
+	case AT91C_EFC_FCMD_WPL:
+	case AT91C_EFC_FCMD_EWP:
+	case AT91C_EFC_FCMD_EWPL:
+		// case AT91C_EFC_FCMD_EPL:
+		// case AT91C_EFC_FCMD_EPA:
+	case AT91C_EFC_FCMD_SLB:
+	case AT91C_EFC_FCMD_CLB:
+		n = (pPrivate->size_bytes / pPrivate->page_size);
+		if (argument >= n) {
+			LOG_ERROR("*BUG*: Embedded flash has only %u pages", (unsigned)(n));
+		}
+		break;
+
+	case AT91C_EFC_FCMD_SFB:
+	case AT91C_EFC_FCMD_CFB:
+		if (argument >= pPrivate->pChip->details.n_gpnvms) {
+			LOG_ERROR("*BUG*: Embedded flash has only %d GPNVMs",
+					  pPrivate->pChip->details.n_gpnvms);
+		}
+		break;
+
+	case AT91C_EFC_FCMD_GETD:
+	case AT91C_EFC_FCMD_EA:
+	case AT91C_EFC_FCMD_GLB:
+	case AT91C_EFC_FCMD_GFB:
+	case AT91C_EFC_FCMD_STUI:
+	case AT91C_EFC_FCMD_SPUI:
+		if (argument != 0) {
+			LOG_ERROR("Argument is meaningless for cmd: %d", command);
+		}
+		break;
+	default:
+		LOG_ERROR("Unknown command %d", command);
+		break;
+    }
+
+	if (command == AT91C_EFC_FCMD_SPUI) {
+		// this is a very special situation.
+		// Situation (1) - error/retry - see below
+		//      And we are being called recursively
+		// Situation (2) - normal, finished reading unique id
+	} else {
+		// it should be "ready"
+		EFC_GetStatus(pPrivate, &v);
+		if (v & 1) {
+			// then it is ready
+			// we go on
+		} else {
+			if (retry) {
+				// we have done this before
+				// the controller is not responding.
+				LOG_ERROR("flash controller(%d) is not ready! Error", pPrivate->bank_number);
+				return ERROR_FAIL;
+			} else {
+				retry++;
+				LOG_ERROR("Flash controller(%d) is not ready, attempting reset",
+						  pPrivate->bank_number);
+				// we do that by issuing the *STOP* command
+				EFC_StartCommand(pPrivate, AT91C_EFC_FCMD_SPUI, 0);
+				// above is recursive, and further recursion is blocked by
+				// if (command == AT91C_EFC_FCMD_SPUI) above
+				goto do_retry;
+			}
+		}
+	}
+
+	v = (0x5A << 24) | (argument << 8) | command;
+	LOG_DEBUG("Command: 0x%08x", ((unsigned int)(v)));
+	r = target_write_u32(pPrivate->pBank->target,
+						  pPrivate->controller_address + offset_EFC_FCR,
+						  v);
+	if (r != ERROR_OK) {
+		LOG_DEBUG("Error Write failed");
+	}
+	return r;
+}
+
+/**
+ * Performs the given command and wait until its completion (or an error).
+ * @param pPrivate - info about the bank
+ * @param command  - Command to perform.
+ * @param argument - Optional command argument.
+ * @param status   - put command status bits here
+ */
+static int
+EFC_PerformCommand(struct sam3_bank_private *pPrivate,
+					unsigned command,
+					unsigned argument,
+					uint32_t *status)
+{
+
+	int r;
+	uint32_t v;
+	long long ms_now, ms_end;
+
+	// default
+	if (status) {
+		*status = 0;
+	}
+
+	r = EFC_StartCommand(pPrivate, command, argument);
+	if (r != ERROR_OK) {
+		return r;
+	}
+
+	ms_end = 500 + timeval_ms();
+
+
+    do {
+		r = EFC_GetStatus(pPrivate, &v);
+		if (r != ERROR_OK) {
+			return r;
+		}
+		ms_now = timeval_ms();
+		if (ms_now > ms_end) {
+			// error
+			LOG_ERROR("Command timeout");
+			return ERROR_FAIL;
+		}
+    }
+    while ((v & 1) == 0)
+		;
+
+	// error bits..
+	if (status) {
+		*status = (v & 0x6);
+	}
+	return ERROR_OK;
+
+}
+
+
+
+
+
+/**
+ * Read the unique ID.
+ * @param pPrivate - info about the bank
+ * The unique ID is stored in the 'pPrivate' structure.
+ */
+static int
+FLASHD_ReadUniqueID (struct sam3_bank_private *pPrivate)
+{
+	int r;
+	uint32_t v;
+	int x;
+	// assume 0
+    pPrivate->pChip->cfg.unique_id[0] = 0;
+    pPrivate->pChip->cfg.unique_id[1] = 0;
+    pPrivate->pChip->cfg.unique_id[2] = 0;
+    pPrivate->pChip->cfg.unique_id[3] = 0;
+
+	LOG_DEBUG("Begin");
+	r = EFC_StartCommand(pPrivate, AT91C_EFC_FCMD_STUI, 0);
+	if (r < 0) {
+		return r;
+	}
+
+	for (x = 0 ; x < 4 ; x++) {
+		r = target_read_u32(pPrivate->pChip->target,
+							 pPrivate->pBank->base + (x * 4),
+							 &v);
+		if (r < 0) {
+			return r;
+		}
+		pPrivate->pChip->cfg.unique_id[x] = v;
+	}
+
+    r = EFC_PerformCommand(pPrivate, AT91C_EFC_FCMD_SPUI, 0, NULL);
+	LOG_DEBUG("End: R=%d, id = 0x%08x, 0x%08x, 0x%08x, 0x%08x",
+			  r,
+			  (unsigned int)(pPrivate->pChip->cfg.unique_id[0]),
+			  (unsigned int)(pPrivate->pChip->cfg.unique_id[1]),
+			  (unsigned int)(pPrivate->pChip->cfg.unique_id[2]),
+			  (unsigned int)(pPrivate->pChip->cfg.unique_id[3]));
+	return r;
+
+}
+
+/**
+ * Erases the entire flash.
+ * @param pPrivate - the info about the bank.
+ */
+static int
+FLASHD_EraseEntireBank(struct sam3_bank_private *pPrivate)
+{
+	LOG_DEBUG("Here");
+	return EFC_PerformCommand(pPrivate, AT91C_EFC_FCMD_EA, 0, NULL);
+}
+
+
+
+/**
+ * Gets current GPNVM state.
+ * @param pPrivate - info about the bank.
+ * @param gpnvm    -  GPNVM bit index.
+ * @param puthere  - result stored here.
+ */
+//------------------------------------------------------------------------------
+static int
+FLASHD_GetGPNVM(struct sam3_bank_private *pPrivate, unsigned gpnvm, unsigned *puthere)
+{
+	uint32_t v;
+	int r;
+
+	LOG_DEBUG("Here");
+	if (pPrivate->bank_number != 0) {
+		LOG_ERROR("GPNVM only works with Bank0");
+		return ERROR_FAIL;
+	}
+
+	if (gpnvm >= pPrivate->pChip->details.n_gpnvms) {
+		LOG_ERROR("Invalid GPNVM %d, max: %d, ignored",
+				  gpnvm,pPrivate->pChip->details.n_gpnvms);
+		return ERROR_FAIL;
+	}
+
+    // Get GPNVMs status
+	r = EFC_PerformCommand(pPrivate, AT91C_EFC_FCMD_GFB, 0, NULL);
+	if (r != ERROR_OK) {
+		LOG_ERROR("Failed");
+		return r;
+	}
+
+    r = EFC_GetResult(pPrivate, &v);
+
+	if (puthere) {
+		// Check if GPNVM is set
+		// get the bit and make it a 0/1
+		*puthere = (v >> gpnvm) & 1;
+	}
+
+	return r;
+}
+
+
+
+
+/**
+ * Clears the selected GPNVM bit.
+ * @param pPrivate info about the bank
+ * @param gpnvm GPNVM index.
+ * @returns 0 if successful; otherwise returns an error code.
+ */
+static int
+FLASHD_ClrGPNVM(struct sam3_bank_private *pPrivate, unsigned gpnvm)
+{
+	int r;
+	unsigned v;
+
+	LOG_DEBUG("Here");
+	if (pPrivate->bank_number != 0) {
+		LOG_ERROR("GPNVM only works with Bank0");
+		return ERROR_FAIL;
+	}
+
+	if (gpnvm >= pPrivate->pChip->details.n_gpnvms) {
+		LOG_ERROR("Invalid GPNVM %d, max: %d, ignored",
+				  gpnvm,pPrivate->pChip->details.n_gpnvms);
+		return ERROR_FAIL;
+	}
+
+	r = FLASHD_GetGPNVM(pPrivate, gpnvm, &v);
+	if (r != ERROR_OK) {
+		LOG_DEBUG("Failed: %d",r);
+		return r;
+	}
+	r = EFC_PerformCommand(pPrivate, AT91C_EFC_FCMD_CFB, gpnvm, NULL);
+	LOG_DEBUG("End: %d",r);
+	return r;
+}
+
+
+
+/**
+ * Sets the selected GPNVM bit.
+ * @param pPrivate info about the bank
+ * @param gpnvm GPNVM index.
+ */
+static int
+FLASHD_SetGPNVM(struct sam3_bank_private *pPrivate, unsigned gpnvm)
+{
+	int r;
+	unsigned v;
+
+	if (pPrivate->bank_number != 0) {
+		LOG_ERROR("GPNVM only works with Bank0");
+		return ERROR_FAIL;
+	}
+
+	if (gpnvm >= pPrivate->pChip->details.n_gpnvms) {
+		LOG_ERROR("Invalid GPNVM %d, max: %d, ignored",
+				  gpnvm,pPrivate->pChip->details.n_gpnvms);
+		return ERROR_FAIL;
+	}
+
+	r = FLASHD_GetGPNVM(pPrivate, gpnvm, &v);
+	if (r != ERROR_OK) {
+		return r;
+	}
+	if (v) {
+		// already set
+		r = ERROR_OK;
+	} else {
+		// set it
+		r = EFC_PerformCommand(pPrivate, AT91C_EFC_FCMD_SFB, gpnvm, NULL);
+	}
+	return r;
+}
+
+
+/**
+ * Returns a bit field (at most 64) of locked regions within a page.
+ * @param pPrivate info about the bank
+ * @param v where to store locked bits
+ */
+static int
+FLASHD_GetLockBits(struct sam3_bank_private *pPrivate, uint32_t *v)
+{
+	int r;
+	LOG_DEBUG("Here");
+    r = EFC_PerformCommand(pPrivate, AT91C_EFC_FCMD_GLB, 0, NULL);
+	if (r == ERROR_OK) {
+		r = EFC_GetResult(pPrivate, v);
+	}
+	LOG_DEBUG("End: %d",r);
+	return r;
+}
+
+
+/**
+ * Unlocks all the regions in the given address range.
+ * @param pPrivate info about the bank
+ * @param start_sector first sector to unlock
+ * @param end_sector last (inclusive) to unlock
+ */
+
+static int
+FLASHD_Unlock(struct sam3_bank_private *pPrivate,
+			   unsigned start_sector,
+			   unsigned end_sector)
+{
+	int r;
+	uint32_t status;
+	uint32_t pg;
+	uint32_t pages_per_sector;
+
+	pages_per_sector = pPrivate->sector_size / pPrivate->page_size;
+
+    /* Unlock all pages */
+    while (start_sector <= end_sector) {
+		pg = start_sector * pages_per_sector;
+
+        r = EFC_PerformCommand(pPrivate, AT91C_EFC_FCMD_CLB, pg, &status);
+        if (r != ERROR_OK) {
+            return r;
+        }
+        start_sector++;
+    }
+
+    return ERROR_OK;
+}
+
+
+/**
+ * Locks regions
+ * @param pPrivate - info about the bank
+ * @param start_sector - first sector to lock
+ * @param end_sector   - last sector (inclusive) to lock
+ */
+static int
+FLASHD_Lock(struct sam3_bank_private *pPrivate,
+			 unsigned start_sector,
+			 unsigned end_sector)
+{
+	uint32_t status;
+	uint32_t pg;
+	uint32_t pages_per_sector;
+	int r;
+
+	pages_per_sector = pPrivate->sector_size / pPrivate->page_size;
+
+    /* Lock all pages */
+    while (start_sector <= end_sector) {
+		pg = start_sector * pages_per_sector;
+
+        r = EFC_PerformCommand(pPrivate, AT91C_EFC_FCMD_SLB, pg, &status);
+        if (r != ERROR_OK) {
+            return r;
+        }
+        start_sector++;
+    }
+    return ERROR_OK;
+}
+
+
+/****** END SAM3 CODE ********/
+
+/* begin helpful debug code */
+
+static void
+sam3_sprintf(struct sam3_chip *pChip , const char *fmt, ...)
+{
+	va_list ap;
+	va_start(ap,fmt);
+	if (pChip->mbuf == NULL) {
+		return;
+	}
+
+	membuf_vsprintf(pChip->mbuf, fmt, ap);
+	va_end(ap);
+}
+
+// print the fieldname, the field value, in dec & hex, and return field value
+static uint32_t
+sam3_reg_fieldname(struct sam3_chip *pChip,
+					const char *regname,
+					uint32_t value,
+					unsigned shift,
+					unsigned width)
+{
+	uint32_t v;
+	int hwidth, dwidth;
+
+
+	// extract the field
+	v = value >> shift;
+	v = v & ((1 << width)-1);
+	if (width <= 16) {
+		hwidth = 4;
+		dwidth = 5;
+	} else {
+		hwidth = 8;
+		dwidth = 12;
+	}
+
+	// show the basics
+	sam3_sprintf(pChip, "\t%*s: %*d [0x%0*x] ",
+				  REG_NAME_WIDTH, regname,
+				  dwidth, v,
+				  hwidth, v);
+	return v;
+}
+
+
+static const char _unknown[] = "unknown";
+static const char * const eproc_names[] = {
+	_unknown,					// 0
+	"arm946es",					// 1
+	"arm7tdmi",					// 2
+	"cortex-m3",				// 3
+	"arm920t",					// 4
+	"arm926ejs",				// 5
+	_unknown,					// 6
+	_unknown,					// 7
+	_unknown,					// 8
+	_unknown,					// 9
+	_unknown,					// 10
+	_unknown,					// 11
+	_unknown,					// 12
+	_unknown,					// 13
+	_unknown,					// 14
+	_unknown,					// 15
+};
+
+#define nvpsize2 nvpsize		// these two tables are identical
+static const char * const nvpsize[] = {
+	"none",						//  0
+	"8K bytes",					//  1
+	"16K bytes",				//  2
+	"32K bytes",				//  3
+	_unknown,					//  4
+	"64K bytes",				//  5
+	_unknown,					//  6
+	"128K bytes",				//  7
+	_unknown,					//  8
+	"256K bytes",				//  9
+	"512K bytes",				// 10
+	_unknown,					// 11
+	"1024K bytes",				// 12
+	_unknown,					// 13
+	"2048K bytes",				// 14
+	_unknown,					// 15
+};
+
+
+static const char * const sramsize[] = {
+	"48K Bytes",				//  0
+	"1K Bytes",					//  1
+	"2K Bytes",					//  2
+	"6K Bytes",					//  3
+	"112K Bytes",				//  4
+	"4K Bytes",					//  5
+	"80K Bytes",				//  6
+	"160K Bytes",				//  7
+	"8K Bytes",					//  8
+	"16K Bytes",				//  9
+	"32K Bytes",				// 10
+	"64K Bytes",				// 11
+	"128K Bytes",				// 12
+	"256K Bytes",				// 13
+	"96K Bytes",				// 14
+	"512K Bytes",				// 15
+
+};
+
+static const struct archnames { unsigned value; const char *name; } archnames[] = {
+	{ 0x19,  "AT91SAM9xx Series"						},
+	{ 0x29,  "AT91SAM9XExx Series"						},
+	{ 0x34,  "AT91x34 Series"							},
+	{ 0x37,  "CAP7 Series"								},
+	{ 0x39,  "CAP9 Series"								},
+	{ 0x3B,  "CAP11 Series"								},
+	{ 0x40,  "AT91x40 Series"							},
+	{ 0x42,  "AT91x42 Series"							},
+	{ 0x55,  "AT91x55 Series"							},
+	{ 0x60,  "AT91SAM7Axx Series"						},
+	{ 0x61,  "AT91SAM7AQxx Series"						},
+	{ 0x63,  "AT91x63 Series"							},
+	{ 0x70,  "AT91SAM7Sxx Series"						},
+	{ 0x71,  "AT91SAM7XCxx Series"						},
+	{ 0x72,  "AT91SAM7SExx Series"						},
+	{ 0x73,  "AT91SAM7Lxx Series"						},
+	{ 0x75,  "AT91SAM7Xxx Series"						},
+	{ 0x76,  "AT91SAM7SLxx Series"						},
+	{ 0x80,  "ATSAM3UxC Series (100-pin version)"		},
+	{ 0x81,  "ATSAM3UxE Series (144-pin version)"		},
+	{ 0x83,  "ATSAM3AxC Series (100-pin version)"		},
+	{ 0x84,  "ATSAM3XxC Series (100-pin version)"		},
+	{ 0x85,  "ATSAM3XxE Series (144-pin version)"		},
+	{ 0x86,  "ATSAM3XxG Series (208/217-pin version)"	},
+	{ 0x88,  "ATSAM3SxA Series (48-pin version)"		},
+	{ 0x89,  "ATSAM3SxB Series (64-pin version)"		},
+	{ 0x8A,  "ATSAM3SxC Series (100-pin version)"		},
+	{ 0x92,  "AT91x92 Series"							},
+	{ 0xF0,  "AT75Cxx Series"							},
+	{ -1, NULL },
+
+};
+
+static const char * const nvptype[] = {
+	"rom", // 0
+	"romless or onchip flash", // 1
+	"embedded flash memory", // 2
+	"rom(nvpsiz) + embedded flash (nvpsiz2)", //3
+	"sram emulating flash", // 4
+	_unknown, // 5
+	_unknown, // 6
+	_unknown, // 7
+
+};
+
+static const char *_yes_or_no(uint32_t v)
+{
+	if (v) {
+		return "YES";
+	} else {
+		return "NO";
+	}
+}
+
+static const char * const _rc_freq[] = {
+	"4 MHz", "8 MHz", "12 MHz", "reserved"
+};
+
+static void
+sam3_explain_ckgr_mor(struct sam3_chip *pChip)
+{
+	uint32_t v;
+	uint32_t rcen;
+
+	v = sam3_reg_fieldname(pChip, "MOSCXTEN", pChip->cfg.CKGR_MOR, 0, 1);
+	sam3_sprintf(pChip, "(main xtal enabled: %s)\n",
+				  _yes_or_no(v));
+	v = sam3_reg_fieldname(pChip, "MOSCXTBY", pChip->cfg.CKGR_MOR, 1, 1);
+	sam3_sprintf(pChip, "(main osc bypass: %s)\n",
+				  _yes_or_no(v));
+	rcen = sam3_reg_fieldname(pChip, "MOSCRCEN", pChip->cfg.CKGR_MOR, 2, 1);
+	sam3_sprintf(pChip, "(onchip RC-OSC enabled: %s)\n",
+				  _yes_or_no(rcen));
+	v = sam3_reg_fieldname(pChip, "MOSCRCF", pChip->cfg.CKGR_MOR, 4, 3);
+	sam3_sprintf(pChip, "(onchip RC-OSC freq: %s)\n",
+				  _rc_freq[v]);
+
+	pChip->cfg.rc_freq = 0;
+	if (rcen) {
+		switch (v) {
+		default:
+			pChip->cfg.rc_freq = 0;
+		case 0:
+			pChip->cfg.rc_freq = 4 * 1000 * 1000;
+			break;
+		case 1:
+			pChip->cfg.rc_freq = 8 * 1000 * 1000;
+			break;
+		case 2:
+			pChip->cfg.rc_freq = 12* 1000 * 1000;
+			break;
+		}
+	}
+
+	v = sam3_reg_fieldname(pChip,"MOSCXTST", pChip->cfg.CKGR_MOR, 8, 8);
+	sam3_sprintf(pChip, "(startup clks, time= %f uSecs)\n",
+				  ((float)(v * 1000000)) / ((float)(pChip->cfg.slow_freq)));
+	v = sam3_reg_fieldname(pChip, "MOSCSEL", pChip->cfg.CKGR_MOR, 24, 1);
+	sam3_sprintf(pChip, "(mainosc source: %s)\n",
+				  v ? "external xtal" : "internal RC");
+
+	v = sam3_reg_fieldname(pChip,"CFDEN", pChip->cfg.CKGR_MOR, 25, 1);
+	sam3_sprintf(pChip, "(clock failure enabled: %s)\n",
+				 _yes_or_no(v));
+}
+
+
+
+static void
+sam3_explain_chipid_cidr(struct sam3_chip *pChip)
+{
+	int x;
+	uint32_t v;
+	const char *cp;
+
+	sam3_reg_fieldname(pChip, "Version", pChip->cfg.CHIPID_CIDR, 0, 5);
+	sam3_sprintf(pChip,"\n");
+
+	v = sam3_reg_fieldname(pChip, "EPROC", pChip->cfg.CHIPID_CIDR, 5, 3);
+	sam3_sprintf(pChip, "%s\n", eproc_names[v]);
+
+	v = sam3_reg_fieldname(pChip, "NVPSIZE", pChip->cfg.CHIPID_CIDR, 8, 4);
+	sam3_sprintf(pChip, "%s\n", nvpsize[v]);
+
+	v = sam3_reg_fieldname(pChip, "NVPSIZE2", pChip->cfg.CHIPID_CIDR, 12, 4);
+	sam3_sprintf(pChip, "%s\n", nvpsize2[v]);
+
+	v = sam3_reg_fieldname(pChip, "SRAMSIZE", pChip->cfg.CHIPID_CIDR, 16,4);
+	sam3_sprintf(pChip, "%s\n", sramsize[ v ]);
+
+	v = sam3_reg_fieldname(pChip, "ARCH", pChip->cfg.CHIPID_CIDR, 20, 8);
+	cp = _unknown;
+	for (x = 0 ; archnames[x].name ; x++) {
+		if (v == archnames[x].value) {
+			cp = archnames[x].name;
+			break;
+		}
+	}
+
+	sam3_sprintf(pChip, "%s\n", cp);
+
+	v = sam3_reg_fieldname(pChip, "NVPTYP", pChip->cfg.CHIPID_CIDR, 28, 3);
+	sam3_sprintf(pChip, "%s\n", nvptype[ v ]);
+
+	v = sam3_reg_fieldname(pChip, "EXTID", pChip->cfg.CHIPID_CIDR, 31, 1);
+	sam3_sprintf(pChip, "(exists: %s)\n", _yes_or_no(v));
+}
+
+static void
+sam3_explain_ckgr_mcfr(struct sam3_chip *pChip)
+{
+	uint32_t v;
+
+
+	v = sam3_reg_fieldname(pChip, "MAINFRDY", pChip->cfg.CKGR_MCFR, 16, 1);
+	sam3_sprintf(pChip, "(main ready: %s)\n", _yes_or_no(v));
+
+	v = sam3_reg_fieldname(pChip, "MAINF", pChip->cfg.CKGR_MCFR, 0, 16);
+
+	v = (v * pChip->cfg.slow_freq) / 16;
+	pChip->cfg.mainosc_freq = v;
+
+	sam3_sprintf(pChip, "(%3.03f Mhz (%d.%03dkhz slowclk)\n",
+				 _tomhz(v),
+				 pChip->cfg.slow_freq / 1000,
+				 pChip->cfg.slow_freq % 1000);
+
+}
+
+static void
+sam3_explain_ckgr_plla(struct sam3_chip *pChip)
+{
+	uint32_t mula,diva;
+
+	diva = sam3_reg_fieldname(pChip, "DIVA", pChip->cfg.CKGR_PLLAR, 0, 8);
+	sam3_sprintf(pChip,"\n");
+	mula = sam3_reg_fieldname(pChip, "MULA", pChip->cfg.CKGR_PLLAR, 16, 11);
+	sam3_sprintf(pChip,"\n");
+	pChip->cfg.plla_freq = 0;
+	if (mula == 0) {
+		sam3_sprintf(pChip,"\tPLLA Freq: (Disabled,mula = 0)\n");
+	} else if (diva == 0) {
+		sam3_sprintf(pChip,"\tPLLA Freq: (Disabled,diva = 0)\n");
+	} else if (diva == 1) {
+		pChip->cfg.plla_freq = (pChip->cfg.mainosc_freq * (mula + 1));
+		sam3_sprintf(pChip,"\tPLLA Freq: %3.03f MHz\n",
+					 _tomhz(pChip->cfg.plla_freq));
+	}
+}
+
+
+static void
+sam3_explain_mckr(struct sam3_chip *pChip)
+{
+	uint32_t css, pres, fin = 0;
+	int pdiv = 0;
+	const char *cp = NULL;
+
+	css = sam3_reg_fieldname(pChip, "CSS", pChip->cfg.PMC_MCKR, 0, 2);
+	switch (css & 3) {
+	case 0:
+		fin = pChip->cfg.slow_freq;
+		cp = "slowclk";
+		break;
+	case 1:
+		fin = pChip->cfg.mainosc_freq;
+		cp  = "mainosc";
+		break;
+	case 2:
+		fin = pChip->cfg.plla_freq;
+		cp  = "plla";
+		break;
+	case 3:
+		if (pChip->cfg.CKGR_UCKR & (1 << 16)) {
+			fin = 480 * 1000 * 1000;
+			cp = "upll";
+		} else {
+			fin = 0;
+			cp  = "upll (*ERROR* UPLL is disabled)";
+		}
+		break;
+	default:
+		assert(0);
+		break;
+	}
+
+	sam3_sprintf(pChip, "%s (%3.03f Mhz)\n",
+				  cp,
+				  _tomhz(fin));
+	pres = sam3_reg_fieldname(pChip, "PRES", pChip->cfg.PMC_MCKR, 4, 3);
+	switch (pres & 0x07) {
+	case 0:
+		pdiv = 1;
+		cp = "selected clock";
+	case 1:
+		pdiv = 2;
+		cp = "clock/2";
+		break;
+	case 2:
+		pdiv = 4;
+		cp = "clock/4";
+		break;
+	case 3:
+		pdiv = 8;
+		cp = "clock/8";
+		break;
+	case 4:
+		pdiv = 16;
+		cp = "clock/16";
+		break;
+	case 5:
+		pdiv = 32;
+		cp = "clock/32";
+		break;
+	case 6:
+		pdiv = 64;
+		cp = "clock/64";
+		break;
+	case 7:
+		pdiv = 6;
+		cp = "clock/6";
+		break;
+	default:
+		assert(0);
+		break;
+	}
+	sam3_sprintf(pChip, "(%s)\n", cp);
+	fin = fin / pdiv;
+	// sam3 has a *SINGLE* clock -
+	// other at91 series parts have divisors for these.
+	pChip->cfg.cpu_freq = fin;
+	pChip->cfg.mclk_freq = fin;
+	pChip->cfg.fclk_freq = fin;
+	sam3_sprintf(pChip, "\t\tResult CPU Freq: %3.03f\n",
+				  _tomhz(fin));
+}
+
+#if 0
+static struct sam3_chip *
+target2sam3(struct target *pTarget)
+{
+	struct sam3_chip *pChip;
+
+	if (pTarget == NULL) {
+		return NULL;
+	}
+
+	pChip = all_sam3_chips;
+	while (pChip) {
+		if (pChip->target == pTarget) {
+			break; // return below
+		} else {
+			pChip = pChip->next;
+		}
+	}
+	return pChip;
+}
+#endif
+
+static uint32_t *
+sam3_get_reg_ptr(struct sam3_cfg *pCfg, const struct sam3_reg_list *pList)
+{
+	// this function exists to help
+	// keep funky offsetof() errors
+	// and casting from causing bugs
+
+	// By using prototypes - we can detect what would
+	// be casting errors.
+
+	return ((uint32_t *)(((char *)(pCfg)) + pList->struct_offset));
+}
+
+
+#define SAM3_ENTRY(NAME, FUNC)  { .address = SAM3_ ## NAME, .struct_offset = offsetof(struct sam3_cfg, NAME), #NAME, FUNC }
+static const struct sam3_reg_list sam3_all_regs[] = {
+	SAM3_ENTRY(CKGR_MOR , sam3_explain_ckgr_mor),
+	SAM3_ENTRY(CKGR_MCFR , sam3_explain_ckgr_mcfr),
+	SAM3_ENTRY(CKGR_PLLAR , sam3_explain_ckgr_plla),
+	SAM3_ENTRY(CKGR_UCKR , NULL),
+	SAM3_ENTRY(PMC_FSMR , NULL),
+	SAM3_ENTRY(PMC_FSPR , NULL),
+	SAM3_ENTRY(PMC_IMR , NULL),
+	SAM3_ENTRY(PMC_MCKR , sam3_explain_mckr),
+	SAM3_ENTRY(PMC_PCK0 , NULL),
+	SAM3_ENTRY(PMC_PCK1 , NULL),
+	SAM3_ENTRY(PMC_PCK2 , NULL),
+	SAM3_ENTRY(PMC_PCSR , NULL),
+	SAM3_ENTRY(PMC_SCSR , NULL),
+	SAM3_ENTRY(PMC_SR , NULL),
+	SAM3_ENTRY(CHIPID_CIDR , sam3_explain_chipid_cidr),
+	SAM3_ENTRY(CHIPID_EXID , NULL),
+	SAM3_ENTRY(SUPC_CR, NULL),
+
+	// TERMINATE THE LIST
+	{ .name = NULL }
+};
+#undef SAM3_ENTRY
+
+
+
+
+static struct sam3_bank_private *
+get_sam3_bank_private(struct flash_bank *bank)
+{
+	return (struct sam3_bank_private *)(bank->driver_priv);
+}
+
+/**
+ * Given a pointer to where it goes in the structure,
+ * determine the register name, address from the all registers table.
+ */
+static const struct sam3_reg_list *
+sam3_GetReg(struct sam3_chip *pChip, uint32_t *goes_here)
+{
+	const struct sam3_reg_list *pReg;
+
+	pReg = &(sam3_all_regs[0]);
+	while (pReg->name) {
+		uint32_t *pPossible;
+
+		// calculate where this one go..
+		// it is "possibly" this register.
+
+		pPossible = ((uint32_t *)(((char *)(&(pChip->cfg))) + pReg->struct_offset));
+
+		// well? Is it this register
+		if (pPossible == goes_here) {
+			// Jump for joy!
+			return pReg;
+		}
+
+		// next...
+		pReg++;
+	}
+	// This is *TOTAL*PANIC* - we are totally screwed.
+	LOG_ERROR("INVALID SAM3 REGISTER");
+	return NULL;
+}
+
+
+static int
+sam3_ReadThisReg(struct sam3_chip *pChip, uint32_t *goes_here)
+{
+	const struct sam3_reg_list *pReg;
+	int r;
+
+	pReg = sam3_GetReg(pChip, goes_here);
+	if (!pReg) {
+		return ERROR_FAIL;
+	}
+
+	r = target_read_u32(pChip->target, pReg->address, goes_here);
+	if (r != ERROR_OK) {
+		LOG_ERROR("Cannot read SAM3 register: %s @ 0x%08x, Err: %d\n",
+				  pReg->name, (unsigned)(pReg->address), r);
+	}
+	return r;
+}
+
+
+
+static int
+sam3_ReadAllRegs(struct sam3_chip *pChip)
+{
+	int r;
+	const struct sam3_reg_list *pReg;
+
+	pReg = &(sam3_all_regs[0]);
+	while (pReg->name) {
+		r = sam3_ReadThisReg(pChip,
+								  sam3_get_reg_ptr(&(pChip->cfg), pReg));
+		if (r != ERROR_OK) {
+			LOG_ERROR("Cannot read SAM3 registere: %s @ 0x%08x, Error: %d\n",
+					  pReg->name, ((unsigned)(pReg->address)), r);
+			return r;
+		}
+
+		pReg++;
+	}
+
+	return ERROR_OK;
+}
+
+
+static int
+sam3_GetInfo(struct sam3_chip *pChip)
+{
+	const struct sam3_reg_list *pReg;
+	uint32_t regval;
+
+	membuf_reset(pChip->mbuf);
+
+
+	pReg = &(sam3_all_regs[0]);
+	while (pReg->name) {
+		// display all regs
+		LOG_DEBUG("Start: %s", pReg->name);
+		regval = *sam3_get_reg_ptr(&(pChip->cfg), pReg);
+		sam3_sprintf(pChip, "%*s: [0x%08x] -> 0x%08x\n",
+					 REG_NAME_WIDTH,
+					 pReg->name,
+					 pReg->address,
+					 regval);
+ 		if (pReg->explain_func) {
+			(*(pReg->explain_func))(pChip);
+		}
+		LOG_DEBUG("End: %s", pReg->name);
+		pReg++;
+	}
+	sam3_sprintf(pChip,"   rc-osc: %3.03f MHz\n", _tomhz(pChip->cfg.rc_freq));
+	sam3_sprintf(pChip,"  mainosc: %3.03f MHz\n", _tomhz(pChip->cfg.mainosc_freq));
+	sam3_sprintf(pChip,"     plla: %3.03f MHz\n", _tomhz(pChip->cfg.plla_freq));
+	sam3_sprintf(pChip," cpu-freq: %3.03f MHz\n", _tomhz(pChip->cfg.cpu_freq));
+	sam3_sprintf(pChip,"mclk-freq: %3.03f MHz\n", _tomhz(pChip->cfg.mclk_freq));
+
+
+	sam3_sprintf(pChip, " UniqueId: 0x%08x 0x%08x 0x%08x 0x%08x\n",
+				  pChip->cfg.unique_id[0],
+				  pChip->cfg.unique_id[1],
+				  pChip->cfg.unique_id[2],
+				  pChip->cfg.unique_id[3]);
+
+
+	return ERROR_OK;
+}
+
+
+static int
+sam3_erase_check(struct flash_bank *bank)
+{
+	int x;
+
+	LOG_DEBUG("Here");
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	if (0 == bank->num_sectors) {
+		LOG_ERROR("Target: not supported/not probed\n");
+		return ERROR_FAIL;
+	}
+
+	LOG_INFO("sam3 - supports auto-erase, erase_check ignored");
+	for (x = 0 ; x < bank->num_sectors ; x++) {
+		bank->sectors[x].is_erased = 1;
+	}
+
+	LOG_DEBUG("Done");
+	return ERROR_OK;
+}
+
+static int
+sam3_protect_check(struct flash_bank *bank)
+{
+	int r;
+	uint32_t v=0;
+	unsigned x;
+	struct sam3_bank_private *pPrivate;
+
+	LOG_DEBUG("Begin");
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	pPrivate = get_sam3_bank_private(bank);
+	if (!pPrivate) {
+		LOG_ERROR("no private for this bank?");
+		return ERROR_FAIL;
+	}
+	if (!(pPrivate->probed)) {
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	r = FLASHD_GetLockBits(pPrivate , &v);
+	if (r != ERROR_OK) {
+		LOG_DEBUG("Failed: %d",r);
+		return r;
+	}
+
+	for (x = 0 ; x < pPrivate->nsectors ; x++) {
+		bank->sectors[x].is_protected = (!!(v & (1 << x)));
+	}
+	LOG_DEBUG("Done");
+	return ERROR_OK;
+}
+
+FLASH_BANK_COMMAND_HANDLER(sam3_flash_bank_command)
+{
+	struct sam3_chip *pChip;
+
+	pChip = all_sam3_chips;
+
+	// is this an existing chip?
+	while (pChip) {
+		if (pChip->target == bank->target) {
+			break;
+		}
+		pChip = pChip->next;
+	}
+
+	if (!pChip) {
+		// this is a *NEW* chip
+		pChip = calloc(1, sizeof(struct sam3_chip));
+		if (!pChip) {
+			LOG_ERROR("NO RAM!");
+			return ERROR_FAIL;
+		}
+		pChip->target = bank->target;
+		// insert at head
+		pChip->next = all_sam3_chips;
+		all_sam3_chips = pChip;
+		pChip->target = bank->target;
+		// assumption is this runs at 32khz
+		pChip->cfg.slow_freq = 32768;
+		pChip->probed = 0;
+		pChip->mbuf = membuf_new();
+		if (!(pChip->mbuf)) {
+			LOG_ERROR("no memory");
+			return ERROR_FAIL;
+		}
+	}
+
+	switch (bank->base) {
+	default:
+		LOG_ERROR("Address 0x%08x invalid bank address (try 0x%08x or 0x%08x)",
+				  ((unsigned int)(bank->base)),
+				  ((unsigned int)(FLASH_BANK0_BASE)),
+				  ((unsigned int)(FLASH_BANK1_BASE)));
+		return ERROR_FAIL;
+		break;
+	case FLASH_BANK0_BASE:
+		bank->driver_priv = &(pChip->details.bank[0]);
+		bank->bank_number = 0;
+		pChip->details.bank[0].pChip = pChip;
+		pChip->details.bank[0].pBank = bank;
+		break;
+	case FLASH_BANK1_BASE:
+		bank->driver_priv = &(pChip->details.bank[1]);
+		bank->bank_number = 1;
+		pChip->details.bank[1].pChip = pChip;
+		pChip->details.bank[1].pBank = bank;
+		break;
+	}
+
+	// we initialize after probing.
+	return ERROR_OK;
+}
+
+static int
+sam3_GetDetails(struct sam3_bank_private *pPrivate)
+{
+	const struct sam3_chip_details *pDetails;
+	struct sam3_chip *pChip;
+	void *vp;
+	struct flash_bank *saved_banks[SAM3_MAX_FLASH_BANKS];
+
+	unsigned x;
+	const char *cp;
+
+	LOG_DEBUG("Begin");
+	pDetails = all_sam3_details;
+	while (pDetails->name) {
+		if (pDetails->chipid_cidr == pPrivate->pChip->cfg.CHIPID_CIDR) {
+			break;
+		} else {
+			pDetails++;
+		}
+	}
+	if (pDetails->name == NULL) {
+		LOG_ERROR("SAM3 ChipID 0x%08x not found in table (perhaps you can this chip?)",
+				  (unsigned int)(pPrivate->pChip->cfg.CHIPID_CIDR));
+		// Help the victim, print details about the chip
+		membuf_reset(pPrivate->pChip->mbuf);
+		membuf_sprintf(pPrivate->pChip->mbuf,
+						"SAM3 CHIPID_CIDR: 0x%08x decodes as follows\n",
+						pPrivate->pChip->cfg.CHIPID_CIDR);
+		sam3_explain_chipid_cidr(pPrivate->pChip);
+		cp = membuf_strtok(pPrivate->pChip->mbuf, "\n", &vp);
+		while (cp) {
+			LOG_INFO("%s", cp);
+			cp = membuf_strtok(NULL, "\n", &vp);
+		}
+		return ERROR_FAIL;
+	}
+
+	// DANGER: THERE ARE DRAGONS HERE
+
+	// get our pChip - it is going
+	// to be over-written shortly
+	pChip = pPrivate->pChip;
+
+	// Note that, in reality:
+	//
+	//     pPrivate = &(pChip->details.bank[0])
+	// or  pPrivate = &(pChip->details.bank[1])
+	//
+
+	// save the "bank" pointers
+	for (x = 0 ; x < SAM3_MAX_FLASH_BANKS ; x++) {
+		saved_banks[ x ] = pChip->details.bank[x].pBank;
+	}
+
+	// Overwrite the "details" structure.
+	memcpy(&(pPrivate->pChip->details),
+			pDetails,
+			sizeof(pPrivate->pChip->details));
+
+	// now fix the ghosted pointers
+	for (x = 0 ; x < SAM3_MAX_FLASH_BANKS ; x++) {
+		pChip->details.bank[x].pChip = pChip;
+		pChip->details.bank[x].pBank = saved_banks[x];
+	}
+
+	// update the *BANK*SIZE*
+
+	LOG_DEBUG("End");
+	return ERROR_OK;
+}
+
+
+
+static int
+_sam3_probe(struct flash_bank *bank, int noise)
+{
+	unsigned x;
+	int r;
+	struct sam3_bank_private *pPrivate;
+
+
+	LOG_DEBUG("Begin: Bank: %d, Noise: %d", bank->bank_number, noise);
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	pPrivate = get_sam3_bank_private(bank);
+	if (!pPrivate) {
+		LOG_ERROR("Invalid/unknown bank number\n");
+		return ERROR_FAIL;
+	}
+
+	r = sam3_ReadAllRegs(pPrivate->pChip);
+	if (r != ERROR_OK) {
+		return r;
+	}
+
+
+	LOG_DEBUG("Here");
+	r = sam3_GetInfo(pPrivate->pChip);
+	if (r != ERROR_OK) {
+		return r;
+	}
+	if (!(pPrivate->pChip->probed)) {
+		pPrivate->pChip->probed = 1;
+		LOG_DEBUG("Here");
+		r = sam3_GetDetails(pPrivate);
+		if (r != ERROR_OK) {
+			return r;
+		}
+	}
+
+	// update the flash bank size
+	for (x = 0 ; x < SAM3_MAX_FLASH_BANKS ; x++) {
+		if (bank->base == pPrivate->pChip->details.bank[0].base_address) {
+			bank->size =  pPrivate->pChip->details.bank[0].size_bytes;
+			break;
+		}
+	}
+
+	if (bank->sectors == NULL) {
+		bank->sectors     = calloc(pPrivate->nsectors, (sizeof((bank->sectors)[0])));
+		if (bank->sectors == NULL) {
+			LOG_ERROR("No memory!");
+			return ERROR_FAIL;
+		}
+		bank->num_sectors = pPrivate->nsectors;
+
+		for (x = 0 ; ((int)(x)) < bank->num_sectors ; x++) {
+			bank->sectors[x].size         = pPrivate->sector_size;
+			bank->sectors[x].offset       = x * (pPrivate->sector_size);
+			// mark as unknown
+			bank->sectors[x].is_erased    = -1;
+			bank->sectors[x].is_protected = -1;
+		}
+	}
+
+	pPrivate->probed = 1;
+
+	r = sam3_protect_check(bank);
+	if (r != ERROR_OK) {
+		return r;
+	}
+
+	LOG_DEBUG("Bank = %d, nbanks = %d",
+			  pPrivate->bank_number , pPrivate->pChip->details.n_banks);
+	if ((pPrivate->bank_number + 1) == pPrivate->pChip->details.n_banks) {
+		// read unique id,
+		// it appears to be associated with the *last* flash bank.
+		FLASHD_ReadUniqueID(pPrivate);
+	}
+
+	return r;
+}
+
+static int
+sam3_probe(struct flash_bank *bank)
+{
+	return _sam3_probe(bank, 1);
+}
+
+static int
+sam3_auto_probe(struct flash_bank *bank)
+{
+	return _sam3_probe(bank, 0);
+}
+
+
+
+static int
+sam3_erase(struct flash_bank *bank, int first, int last)
+{
+	struct sam3_bank_private *pPrivate;
+	int r;
+
+	LOG_DEBUG("Here");
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	r = sam3_auto_probe(bank);
+	if (r != ERROR_OK) {
+		LOG_DEBUG("Here,r=%d",r);
+		return r;
+	}
+
+	pPrivate = get_sam3_bank_private(bank);
+	if (!(pPrivate->probed)) {
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	if ((first == 0) && ((last + 1)== ((int)(pPrivate->nsectors)))) {
+		// whole chip
+		LOG_DEBUG("Here");
+		return FLASHD_EraseEntireBank(pPrivate);
+	}
+	LOG_INFO("sam3 auto-erases while programing (request ignored)");
+	return ERROR_OK;
+}
+
+static int
+sam3_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	struct sam3_bank_private *pPrivate;
+	int r;
+
+	LOG_DEBUG("Here");
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	pPrivate = get_sam3_bank_private(bank);
+	if (!(pPrivate->probed)) {
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	if (set) {
+		r = FLASHD_Lock(pPrivate, (unsigned)(first), (unsigned)(last));
+	} else {
+		r = FLASHD_Unlock(pPrivate, (unsigned)(first), (unsigned)(last));
+	}
+	LOG_DEBUG("End: r=%d",r);
+
+	return r;
+
+}
+
+
+static int
+sam3_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	buf[ 0 ] = 0;
+	return ERROR_OK;
+}
+
+static int
+sam3_page_read(struct sam3_bank_private *pPrivate, unsigned pagenum, uint8_t *buf)
+{
+	uint32_t adr;
+	int r;
+
+	adr = pagenum * pPrivate->page_size;
+	adr += adr + pPrivate->base_address;
+
+	r = target_read_memory(pPrivate->pChip->target,
+							adr,
+							4, /* THIS*MUST*BE* in 32bit values */
+							pPrivate->page_size / 4,
+							buf);
+	if (r != ERROR_OK) {
+		LOG_ERROR("SAM3: Flash program failed to read page phys address: 0x%08x", (unsigned int)(adr));
+	}
+	return r;
+}
+
+// The code below is basically this:
+// compiled with
+// arm-none-eabi-gcc -mthumb -mcpu = cortex-m3 -O9 -S ./foobar.c -o foobar.s
+//
+// Only the *CPU* can write to the flash buffer.
+// the DAP cannot... so - we download this 28byte thing
+// Run the algorithm - (below)
+// to program the device
+//
+// ========================================
+// #include <stdint.h>
+//
+// struct foo {
+//   uint32_t *dst;
+//   const uint32_t *src;
+//   int   n;
+//   volatile uint32_t *base;
+//   uint32_t   cmd;
+// };
+//
+//
+// uint32_t sam3_function(struct foo *p)
+// {
+//   volatile uint32_t *v;
+//   uint32_t *d;
+//   const uint32_t *s;
+//   int   n;
+//   uint32_t r;
+//
+//   d = p->dst;
+//   s = p->src;
+//   n = p->n;
+//
+//   do {
+//     *d++ = *s++;
+//   } while (--n)
+//     ;
+//
+//   v = p->base;
+//
+//   v[ 1 ] = p->cmd;
+//   do {
+//     r = v[8/4];
+//   } while (!(r&1))
+//     ;
+//   return r;
+// }
+// ========================================
+
+
+
+static const uint8_t
+sam3_page_write_opcodes[] = {
+	//  24 0000 0446     		mov	r4, r0
+	0x04,0x46,
+	//  25 0002 6168     		ldr	r1, [r4, #4]
+	0x61,0x68,
+	//  26 0004 0068     		ldr	r0, [r0, #0]
+	0x00,0x68,
+	//  27 0006 A268     		ldr	r2, [r4, #8]
+	0xa2,0x68,
+	//  28              		@ lr needed for prologue
+	//  29              	.L2:
+	//  30 0008 51F8043B 		ldr	r3, [r1], #4
+	0x51,0xf8,0x04,0x3b,
+	//  31 000c 12F1FF32 		adds	r2, r2, #-1
+	0x12,0xf1,0xff,0x32,
+	//  32 0010 40F8043B 		str	r3, [r0], #4
+	0x40,0xf8,0x04,0x3b,
+	//  33 0014 F8D1     		bne	.L2
+	0xf8,0xd1,
+	//  34 0016 E268     		ldr	r2, [r4, #12]
+	0xe2,0x68,
+	//  35 0018 2369     		ldr	r3, [r4, #16]
+	0x23,0x69,
+	//  36 001a 5360     		str	r3, [r2, #4]
+	0x53,0x60,
+	//  37 001c 0832     		adds	r2, r2, #8
+	0x08,0x32,
+	//  38              	.L4:
+	//  39 001e 1068     		ldr	r0, [r2, #0]
+	0x10,0x68,
+	//  40 0020 10F0010F 		tst	r0, #1
+	0x10,0xf0,0x01,0x0f,
+	//  41 0024 FBD0     		beq	.L4
+	0xfb,0xd0,
+	//  42              	.done:
+	//  43 0026 FEE7     		b	.done
+	0xfe,0xe7
+};
+
+
+static int
+sam3_page_write(struct sam3_bank_private *pPrivate, unsigned pagenum, uint8_t *buf)
+{
+	uint32_t adr;
+	uint32_t status;
+	int r;
+
+	adr = pagenum * pPrivate->page_size;
+	adr += (adr + pPrivate->base_address);
+
+	LOG_DEBUG("Wr Page %u @ phys address: 0x%08x", pagenum, (unsigned int)(adr));
+	r = target_write_memory(pPrivate->pChip->target,
+							 adr,
+							 4, /* THIS*MUST*BE* in 32bit values */
+							 pPrivate->page_size / 4,
+							 buf);
+	if (r != ERROR_OK) {
+		LOG_ERROR("SAM3: Failed to write (buffer) page at phys address 0x%08x", (unsigned int)(adr));
+		return r;
+	}
+
+	r = EFC_PerformCommand(pPrivate,
+							// send Erase & Write Page
+							AT91C_EFC_FCMD_EWP,
+							pagenum,
+							&status);
+
+	if (r != ERROR_OK) {
+		LOG_ERROR("SAM3: Error performing Erase & Write page @ phys address 0x%08x", (unsigned int)(adr));
+	}
+	if (status & (1 << 2)) {
+		LOG_ERROR("SAM3: Page @ Phys address 0x%08x is locked", (unsigned int)(adr));
+		return ERROR_FAIL;
+	}
+	if (status & (1 << 1)) {
+		LOG_ERROR("SAM3: Flash Command error @phys address 0x%08x", (unsigned int)(adr));
+		return ERROR_FAIL;
+	}
+	return ERROR_OK;
+}
+
+
+
+
+
+static int
+sam3_write(struct flash_bank *bank,
+		   uint8_t *buffer,
+		   uint32_t offset,
+		   uint32_t count)
+{
+	int n;
+	unsigned page_cur;
+	unsigned page_end;
+	int r;
+	unsigned page_offset;
+	struct sam3_bank_private *pPrivate;
+	uint8_t *pagebuffer;
+
+	// incase we bail further below, set this to null
+	pagebuffer = NULL;
+
+	// ignore dumb requests
+	if (count == 0) {
+		r = ERROR_OK;
+		goto done;
+	}
+
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		r = ERROR_TARGET_NOT_HALTED;
+		goto done;
+	}
+
+	pPrivate = get_sam3_bank_private(bank);
+	if (!(pPrivate->probed)) {
+		r = ERROR_FLASH_BANK_NOT_PROBED;
+		goto done;
+	}
+
+
+	if ((offset + count) > pPrivate->size_bytes) {
+		LOG_ERROR("Flash write error - past end of bank");
+		LOG_ERROR(" offset: 0x%08x, count 0x%08x, BankEnd: 0x%08x",
+				  (unsigned int)(offset),
+				  (unsigned int)(count),
+				  (unsigned int)(pPrivate->size_bytes));
+		r = ERROR_FAIL;
+		goto done;
+	}
+
+	pagebuffer = malloc(pPrivate->page_size);
+	if( !pagebuffer ){
+		LOG_ERROR("No memory for %d Byte page buffer", (int)(pPrivate->page_size));
+		r = ERROR_FAIL;
+		goto done;
+	}
+
+	// what page do we start & end in?
+	page_cur = offset / pPrivate->page_size;
+	page_end = (offset + count - 1) / pPrivate->page_size;
+
+	LOG_DEBUG("Offset: 0x%08x, Count: 0x%08x", (unsigned int)(offset), (unsigned int)(count));
+	LOG_DEBUG("Page start: %d, Page End: %d", (int)(page_cur), (int)(page_end));
+
+	// Special case: all one page
+	//
+	// Otherwise:
+	//    (1) non-aligned start
+	//    (2) body pages
+	//    (3) non-aligned end.
+
+	// Handle special case - all one page.
+	if (page_cur == page_end) {
+		LOG_DEBUG("Special case, all in one page");
+		r = sam3_page_read(pPrivate, page_cur, pagebuffer);
+		if (r != ERROR_OK) {
+			goto done;
+		}
+
+		page_offset = (offset & (pPrivate->page_size-1));
+		memcpy(pagebuffer + page_offset,
+				buffer,
+				count);
+
+		r = sam3_page_write(pPrivate, page_cur, pagebuffer);
+		if (r != ERROR_OK) {
+			goto done;
+		}
+		r = ERROR_OK;
+		goto done;
+	}
+
+	// non-aligned start
+	page_offset = offset & (pPrivate->page_size - 1);
+	if (page_offset) {
+		LOG_DEBUG("Not-Aligned start");
+		// read the partial
+		r = sam3_page_read(pPrivate, page_cur, pagebuffer);
+		if (r != ERROR_OK) {
+			goto done;
+		}
+
+		// over-write with new data
+		n = (pPrivate->page_size - page_offset);
+		memcpy(pagebuffer + page_offset,
+				buffer,
+				n);
+
+		r = sam3_page_write(pPrivate, page_cur, pagebuffer);
+		if (r != ERROR_OK) {
+			goto done;
+		}
+
+		count  -= n;
+		offset += n;
+		buffer += n;
+		page_cur++;
+	}
+
+	// intermediate large pages
+	// also - the final *terminal*
+	// if that terminal page is a full page
+	LOG_DEBUG("Full Page Loop: cur=%d, end=%d, count = 0x%08x",
+			  (int)page_cur, (int)page_end, (unsigned int)(count));
+
+	while ((page_cur < page_end) &&
+		   (count >= pPrivate->page_size)) {
+		r = sam3_page_write(pPrivate, page_cur, buffer);
+		if (r != ERROR_OK) {
+			goto done;
+		}
+		count    -= pPrivate->page_size;
+		buffer   += pPrivate->page_size;
+		page_cur += 1;
+	}
+
+	// terminal partial page?
+	if (count) {
+		LOG_DEBUG("Terminal partial page, count = 0x%08x", (unsigned int)(count));
+		// we have a partial page
+		r = sam3_page_read(pPrivate, page_cur, pagebuffer);
+		if (r != ERROR_OK) {
+			goto done;
+		}
+		// data goes at start
+		memcpy(pagebuffer, buffer, count);
+		r = sam3_page_write(pPrivate, page_cur, pagebuffer);
+		if (r != ERROR_OK) {
+			goto done;
+		}
+		buffer += count;
+		count  -= count;
+	}
+	LOG_DEBUG("Done!");
+	r = ERROR_OK;
+ done:
+	if( pagebuffer ){
+		free(pagebuffer);
+	}
+	return r;
+}
+
+COMMAND_HANDLER(sam3_handle_info_command)
+{
+	struct sam3_chip *pChip;
+	void *vp;
+	const char *cp;
+	unsigned x;
+	int r;
+
+	pChip = get_current_sam3(CMD_CTX);
+	if (!pChip) {
+		return ERROR_OK;
+	}
+
+	r = 0;
+
+	// bank0 must exist before we can do anything
+	if (pChip->details.bank[0].pBank == NULL) {
+		x = 0;
+	need_define:
+		command_print(CMD_CTX,
+					   "Please define bank %d via command: flash bank %s ... ",
+					   x,
+					   at91sam3_flash.name);
+		return ERROR_FAIL;
+	}
+
+	// if bank 0 is not probed, then probe it
+	if (!(pChip->details.bank[0].probed)) {
+		r = sam3_auto_probe(pChip->details.bank[0].pBank);
+		if (r != ERROR_OK) {
+			return ERROR_FAIL;
+		}
+	}
+	// above garentees the "chip details" structure is valid
+	// and thus, bank private areas are valid
+	// and we have a SAM3 chip, what a concept!
+
+
+	// auto-probe other banks, 0 done above
+    for (x = 1 ; x < SAM3_MAX_FLASH_BANKS ; x++) {
+		// skip banks not present
+		if (!(pChip->details.bank[x].present)) {
+			continue;
+		}
+
+		if (pChip->details.bank[x].pBank == NULL) {
+			goto need_define;
+		}
+
+		if (pChip->details.bank[x].probed) {
+			continue;
+		}
+
+		r = sam3_auto_probe(pChip->details.bank[x].pBank);
+		if (r != ERROR_OK) {
+			return r;
+		}
+	}
+
+
+	r = sam3_GetInfo(pChip);
+	if (r != ERROR_OK) {
+		LOG_DEBUG("Sam3Info, Failed %d\n",r);
+		return r;
+	}
+
+
+	// print results
+	cp = membuf_strtok(pChip->mbuf, "\n", &vp);
+	while (cp) {
+		command_print(CMD_CTX,"%s", cp);
+		cp = membuf_strtok(NULL, "\n", &vp);
+	}
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(sam3_handle_gpnvm_command)
+{
+	unsigned x,v;
+	int r,who;
+	struct sam3_chip *pChip;
+
+	pChip = get_current_sam3(CMD_CTX);
+	if (!pChip) {
+		return ERROR_OK;
+	}
+
+	if (pChip->target->state != TARGET_HALTED) {
+		LOG_ERROR("sam3 - target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+
+	if (pChip->details.bank[0].pBank == NULL) {
+		command_print(CMD_CTX, "Bank0 must be defined first via: flash bank %s ...",
+					   at91sam3_flash.name);
+		return ERROR_FAIL;
+	}
+	if (!pChip->details.bank[0].probed) {
+		r = sam3_auto_probe(pChip->details.bank[0].pBank);
+		if (r != ERROR_OK) {
+			return r;
+		}
+	}
+
+
+	switch (CMD_ARGC) {
+	default:
+		command_print(CMD_CTX,"Too many parameters\n");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+		break;
+	case 0:
+		who = -1;
+		goto showall;
+		break;
+	case 1:
+		who = -1;
+		break;
+	case 2:
+		if ((0 == strcmp(CMD_ARGV[0], "show")) && (0 == strcmp(CMD_ARGV[1], "all"))) {
+			who = -1;
+		} else {
+			uint32_t v32;
+			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], v32);
+			who = v32;
+		}
+		break;
+	}
+
+	if (0 == strcmp("show", CMD_ARGV[0])) {
+		if (who == -1) {
+		showall:
+			r = ERROR_OK;
+			for (x = 0 ; x < pChip->details.n_gpnvms ; x++) {
+				r = FLASHD_GetGPNVM(&(pChip->details.bank[0]), x, &v);
+				if (r != ERROR_OK) {
+					break;
+				}
+				command_print(CMD_CTX, "sam3-gpnvm%u: %u", x, v);
+			}
+			return r;
+		}
+		if ((who >= 0) && (((unsigned)(who)) < pChip->details.n_gpnvms)) {
+			r = FLASHD_GetGPNVM(&(pChip->details.bank[0]), who, &v);
+			command_print(CMD_CTX, "sam3-gpnvm%u: %u", who, v);
+			return r;
+		} else {
+			command_print(CMD_CTX, "sam3-gpnvm invalid GPNVM: %u", who);
+			return ERROR_COMMAND_SYNTAX_ERROR;
+		}
+	}
+
+	if (who == -1) {
+		command_print(CMD_CTX, "Missing GPNVM number");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	if (0 == strcmp("set", CMD_ARGV[0])) {
+		r = FLASHD_SetGPNVM(&(pChip->details.bank[0]), who);
+	} else if ((0 == strcmp("clr", CMD_ARGV[0])) ||
+			   (0 == strcmp("clear", CMD_ARGV[0]))) { // quietly accept both
+		r = FLASHD_ClrGPNVM(&(pChip->details.bank[0]), who);
+	} else {
+		command_print(CMD_CTX, "Unkown command: %s", CMD_ARGV[0]);
+		r = ERROR_COMMAND_SYNTAX_ERROR;
+	}
+	return r;
+}
+
+COMMAND_HANDLER(sam3_handle_slowclk_command)
+{
+	struct sam3_chip *pChip;
+
+	pChip = get_current_sam3(CMD_CTX);
+	if (!pChip) {
+		return ERROR_OK;
+	}
+
+
+	switch (CMD_ARGC) {
+	case 0:
+		// show
+		break;
+	case 1:
+	{
+		// set
+		uint32_t v;
+		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], v);
+		if (v > 200000) {
+			// absurd slow clock of 200Khz?
+			command_print(CMD_CTX,"Absurd/illegal slow clock freq: %d\n", (int)(v));
+			return ERROR_COMMAND_SYNTAX_ERROR;
+		}
+		pChip->cfg.slow_freq = v;
+		break;
+	}
+	default:
+		// error
+		command_print(CMD_CTX,"Too many parameters");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+		break;
+	}
+	command_print(CMD_CTX, "Slowclk freq: %d.%03dkhz",
+				   (int)(pChip->cfg.slow_freq/ 1000),
+				   (int)(pChip->cfg.slow_freq% 1000));
+	return ERROR_OK;
+}
+
+static const struct command_registration at91sam3_exec_command_handlers[] = {
+	{
+		.name = "gpnvm",
+		.handler = &sam3_handle_gpnvm_command,
+		.mode = COMMAND_EXEC,
+		.usage = "[(set|clear) [<bit_id>]]",
+		.help = "Without arguments, shows the gpnvm register; "
+			"otherwise, sets or clear the specified bit.",
+	},
+	{
+		.name = "info",
+		.handler = &sam3_handle_info_command,
+		.mode = COMMAND_EXEC,
+		.help = "print information about the current sam3 chip",
+	},
+	{
+		.name = "slowclk",
+		.handler = &sam3_handle_slowclk_command,
+		.mode = COMMAND_EXEC,
+		.usage = "<value>",
+		.help = "set the slowclock frequency (default 32768hz)",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+static const struct command_registration at91sam3_command_handlers[] = {
+	{
+		.name = "at91sam3",
+		.mode = COMMAND_ANY,
+		.help = "at91sam3 flash command group",
+		.chain = at91sam3_exec_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+struct flash_driver at91sam3_flash = {
+		.name = "at91sam3",
+		.commands = at91sam3_command_handlers,
+		.flash_bank_command = &sam3_flash_bank_command,
+		.erase = &sam3_erase,
+		.protect = &sam3_protect,
+		.write = &sam3_write,
+		.probe = &sam3_probe,
+		.auto_probe = &sam3_auto_probe,
+		.erase_check = &sam3_erase_check,
+		.protect_check = &sam3_protect_check,
+		.info = &sam3_info,
+	};
diff --git a/src/flash/nor/at91sam3.h b/src/flash/nor/at91sam3.h
new file mode 100644
index 00000000..4fa7f467
--- /dev/null
+++ b/src/flash/nor/at91sam3.h
@@ -0,0 +1,23 @@
+/***************************************************************************
+ *   Copyright (C) 2009 by Duane Ellis                                     *
+ *   openocd@duaneellis.com                                                *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+
+// nothing to do here other then export this.
+extern struct flash_driver at91sam3_flash;
diff --git a/src/flash/nor/at91sam7.c b/src/flash/nor/at91sam7.c
new file mode 100644
index 00000000..f9b87bab
--- /dev/null
+++ b/src/flash/nor/at91sam7.c
@@ -0,0 +1,1213 @@
+/***************************************************************************
+ *   Copyright (C) 2006 by Magnus Lundin                                   *
+ *   lundin@mlu.mine.nu                                                    *
+ *                                                                         *
+ *   Copyright (C) 2008 by Gheorghe Guran (atlas)                          *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS for A PARTICULAR PURPOSE.  See the         *
+ *   GNU General public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+****************************************************************************/
+
+/***************************************************************************
+*
+* New flash setup command:
+*
+* flash bank <driver> <base_addr> <size> <chip_width> <bus_width> <target_id>
+*	[<chip_type> <banks>
+*	 <sectors_per_bank> <pages_per_sector>
+*	 <page_size> <num_nvmbits>
+*	 <ext_freq_khz>]
+*
+*   <ext_freq_khz> - MUST be used if clock is from external source,
+*                    CAN be used if main oscillator frequency is known (recommended)
+* Examples:
+* ==== RECOMMENDED (covers clock speed) ============
+*  flash bank at91sam7 0x00100000 0 0 4 $_TARGETNAME AT91SAM7XC256 1 16 64 256 3 25000
+*			(if auto-detect fails; provides clock spec)
+*  flash bank at91sam7 0 0 0 0 $_TARGETNAME 0 0 0 0 0 0 25000
+*			(auto-detect everything except the clock)
+* ==== NOT RECOMMENDED !!! (clock speed is not configured) ====
+*  flash bank at91sam7 0x00100000 0 0 4 $_TARGETNAME AT91SAM7XC256 1 16 64 256 3 0
+*			(if auto-detect fails)
+*  flash bank at91sam7 0 0 0 0 $_TARGETNAME
+*			(old style, auto-detect everything)
+****************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "at91sam7.h"
+#include "binarybuffer.h"
+
+static int at91sam7_protect_check(struct flash_bank *bank);
+static int at91sam7_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count);
+
+static uint32_t at91sam7_get_flash_status(struct target *target, int bank_number);
+static void at91sam7_set_flash_mode(struct flash_bank *bank, int mode);
+static uint32_t at91sam7_wait_status_busy(struct flash_bank *bank, uint32_t waitbits, int timeout);
+static int at91sam7_flash_command(struct flash_bank *bank, uint8_t cmd, uint16_t pagen);
+
+static uint32_t MC_FMR[4] = { 0xFFFFFF60, 0xFFFFFF70, 0xFFFFFF80, 0xFFFFFF90 };
+static uint32_t MC_FCR[4] = { 0xFFFFFF64, 0xFFFFFF74, 0xFFFFFF84, 0xFFFFFF94 };
+static uint32_t MC_FSR[4] = { 0xFFFFFF68, 0xFFFFFF78, 0xFFFFFF88, 0xFFFFFF98 };
+
+static char * EPROC[8]= {"Unknown","ARM946-E","ARM7TDMI","Unknown","ARM920T","ARM926EJ-S","Unknown","Unknown"};
+
+#if 0
+static long SRAMSIZ[16] = {
+	-1,
+	0x0400,		/*  1K */
+	0x0800,		/*  2K */
+	-1,
+	0x1c000,	/* 112K */
+	0x1000,		/*   4K */
+	0x14000,	/*  80K */
+	0x28000,	/* 160K */
+	0x2000,		/*   8K */
+	0x4000,		/*  16K */
+	0x8000,		/*  32K */
+	0x10000,	/*  64K */
+	0x20000,	/* 128K */
+	0x40000,	/* 256K */
+	0x18000,	/*  96K */
+	0x80000,	/* 512K */
+};
+#endif
+
+
+static uint32_t at91sam7_get_flash_status(struct target *target, int bank_number)
+{
+	uint32_t fsr;
+	target_read_u32(target, MC_FSR[bank_number], &fsr);
+
+	return fsr;
+}
+
+/* Read clock configuration and set at91sam7_info->mck_freq */
+static void at91sam7_read_clock_info(struct flash_bank *bank)
+{
+	struct at91sam7_flash_bank *at91sam7_info = bank->driver_priv;
+	struct target *target = bank->target;
+	uint32_t mckr, mcfr, pllr, mor;
+	unsigned long tmp = 0, mainfreq;
+
+	/* Read Clock Generator Main Oscillator Register */
+	target_read_u32(target, CKGR_MOR, &mor);
+	/* Read Clock Generator Main Clock Frequency Register */
+	target_read_u32(target, CKGR_MCFR, &mcfr);
+	/* Read Master Clock Register*/
+	target_read_u32(target, PMC_MCKR, &mckr);
+	/* Read Clock Generator PLL Register  */
+	target_read_u32(target, CKGR_PLLR, &pllr);
+
+	at91sam7_info->mck_valid = 0;
+	at91sam7_info->mck_freq = 0;
+	switch (mckr & PMC_MCKR_CSS)
+	{
+		case 0:			/* Slow Clock */
+			at91sam7_info->mck_valid = 1;
+			tmp = RC_FREQ;
+			break;
+
+		case 1:			/* Main Clock */
+			if ((mcfr & CKGR_MCFR_MAINRDY) &&
+				(at91sam7_info->ext_freq == 0))
+			{
+				at91sam7_info->mck_valid = 1;
+				tmp = RC_FREQ / 16ul * (mcfr & 0xffff);
+			}
+			else if (at91sam7_info->ext_freq != 0)
+			{
+				at91sam7_info->mck_valid = 1;
+				tmp = at91sam7_info->ext_freq;
+			}
+			break;
+
+		case 2:			/* Reserved */
+			break;
+
+		case 3:			/* PLL Clock */
+			if ((mcfr & CKGR_MCFR_MAINRDY) &&
+				(at91sam7_info->ext_freq == 0))
+			{
+				target_read_u32(target, CKGR_PLLR, &pllr);
+				if (!(pllr & CKGR_PLLR_DIV))
+					break; /* 0 Hz */
+				at91sam7_info->mck_valid = 1;
+				mainfreq = RC_FREQ / 16ul * (mcfr & 0xffff);
+				/* Integer arithmetic should have sufficient precision
+				 * as long as PLL is properly configured. */
+				tmp = mainfreq / (pllr & CKGR_PLLR_DIV)*
+					(((pllr & CKGR_PLLR_MUL) >> 16) + 1);
+			}
+			else if ((at91sam7_info->ext_freq != 0) &&
+				((pllr&CKGR_PLLR_DIV) != 0))
+			{
+				at91sam7_info->mck_valid = 1;
+				tmp = at91sam7_info->ext_freq / (pllr&CKGR_PLLR_DIV)*
+					(((pllr & CKGR_PLLR_MUL) >> 16) + 1);
+			}
+			break;
+	}
+
+	/* Prescaler adjust */
+	if ((((mckr & PMC_MCKR_PRES) >> 2) == 7) || (tmp == 0))
+	{
+		at91sam7_info->mck_valid = 0;
+		at91sam7_info->mck_freq = 0;
+	}
+	else if (((mckr & PMC_MCKR_PRES) >> 2) != 0)
+		at91sam7_info->mck_freq = tmp >> ((mckr & PMC_MCKR_PRES) >> 2);
+	else
+		at91sam7_info->mck_freq = tmp;
+}
+
+/* Setup the timimg registers for nvbits or normal flash */
+static void at91sam7_set_flash_mode(struct flash_bank *bank, int mode)
+{
+	uint32_t fmr, fmcn = 0, fws = 0;
+	struct at91sam7_flash_bank *at91sam7_info = bank->driver_priv;
+	struct target *target = bank->target;
+
+	if (mode && (mode != at91sam7_info->flashmode))
+	{
+		/* Always round up (ceil) */
+		if (mode == FMR_TIMING_NVBITS)
+		{
+			if (at91sam7_info->cidr_arch == 0x60)
+			{
+				/* AT91SAM7A3 uses master clocks in 100 ns */
+				fmcn = (at91sam7_info->mck_freq/10000000ul) + 1;
+			}
+			else
+			{
+				/* master clocks in 1uS for ARCH 0x7 types */
+				fmcn = (at91sam7_info->mck_freq/1000000ul) + 1;
+			}
+		}
+		else if (mode == FMR_TIMING_FLASH)
+		{
+			/* main clocks in 1.5uS */
+			fmcn = (at91sam7_info->mck_freq/1000000ul)+
+				(at91sam7_info->mck_freq/2000000ul) + 1;
+		}
+
+		/* hard overclocking */
+		if (fmcn > 0xFF)
+			fmcn = 0xFF;
+
+		/* Only allow fmcn = 0 if clock period is > 30 us = 33kHz. */
+		if (at91sam7_info->mck_freq <= 33333ul)
+			fmcn = 0;
+		/* Only allow fws = 0 if clock frequency is < 30 MHz. */
+		if (at91sam7_info->mck_freq > 30000000ul)
+			fws = 1;
+
+		LOG_DEBUG("fmcn[%i]: %i", bank->bank_number, (int)(fmcn));
+		fmr = fmcn << 16 | fws << 8;
+		target_write_u32(target, MC_FMR[bank->bank_number], fmr);
+	}
+
+	at91sam7_info->flashmode = mode;
+}
+
+static uint32_t at91sam7_wait_status_busy(struct flash_bank *bank, uint32_t waitbits, int timeout)
+{
+	uint32_t status;
+
+	while ((!((status = at91sam7_get_flash_status(bank->target, bank->bank_number)) & waitbits)) && (timeout-- > 0))
+	{
+		LOG_DEBUG("status[%i]: 0x%" PRIx32 "", (int)bank->bank_number, status);
+		alive_sleep(1);
+	}
+
+	LOG_DEBUG("status[%i]: 0x%" PRIx32 "", bank->bank_number, status);
+
+	if (status & 0x0C)
+	{
+		LOG_ERROR("status register: 0x%" PRIx32 "", status);
+		if (status & 0x4)
+			LOG_ERROR("Lock Error Bit Detected, Operation Abort");
+		if (status & 0x8)
+			LOG_ERROR("Invalid command and/or bad keyword, Operation Abort");
+		if (status & 0x10)
+			LOG_ERROR("Security Bit Set, Operation Abort");
+	}
+
+	return status;
+}
+
+/* Send one command to the AT91SAM flash controller */
+static int at91sam7_flash_command(struct flash_bank *bank, uint8_t cmd, uint16_t pagen)
+{
+	uint32_t fcr;
+	struct at91sam7_flash_bank *at91sam7_info = bank->driver_priv;
+	struct target *target = bank->target;
+
+	fcr = (0x5A << 24) | ((pagen&0x3FF) << 8) | cmd;
+	target_write_u32(target, MC_FCR[bank->bank_number], fcr);
+	LOG_DEBUG("Flash command: 0x%" PRIx32 ", flash bank: %i, page number: %u", fcr, bank->bank_number + 1, pagen);
+
+	if ((at91sam7_info->cidr_arch == 0x60) && ((cmd == SLB) | (cmd == CLB)))
+	{
+		/* Lock bit manipulation on AT91SAM7A3 waits for FC_FSR bit 1, EOL */
+		if (at91sam7_wait_status_busy(bank, MC_FSR_EOL, 10)&0x0C)
+		{
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+		return ERROR_OK;
+	}
+
+	if (at91sam7_wait_status_busy(bank, MC_FSR_FRDY, 10)&0x0C)
+	{
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	return ERROR_OK;
+}
+
+/* Read device id register, main clock frequency register and fill in driver info structure */
+static int at91sam7_read_part_info(struct flash_bank *bank)
+{
+	struct flash_bank *t_bank = bank;
+	struct at91sam7_flash_bank *at91sam7_info;
+	struct target *target = t_bank->target;
+
+	uint16_t bnk, sec;
+	uint16_t arch;
+	uint32_t cidr;
+	uint8_t banks_num = 0;
+	uint16_t num_nvmbits = 0;
+	uint16_t sectors_num = 0;
+	uint16_t pages_per_sector = 0;
+	uint16_t page_size = 0;
+	uint32_t ext_freq;
+	uint32_t bank_size;
+	uint32_t base_address = 0;
+	char *target_name = "Unknown";
+
+	at91sam7_info = t_bank->driver_priv;
+
+	if (at91sam7_info->cidr != 0)
+	{
+		/* flash already configured, update clock and check for protected sectors */
+		struct flash_bank *fb = bank;
+		t_bank = fb;
+
+		while (t_bank)
+		{
+			/* re-calculate master clock frequency */
+			at91sam7_read_clock_info(t_bank);
+
+			/* no timming */
+			at91sam7_set_flash_mode(t_bank, FMR_TIMING_NONE);
+
+			/* check protect state */
+			at91sam7_protect_check(t_bank);
+
+			t_bank = fb->next;
+			fb = t_bank;
+		}
+
+		return ERROR_OK;
+	}
+
+	/* Read and parse chip identification register */
+	target_read_u32(target, DBGU_CIDR, &cidr);
+	if (cidr == 0)
+	{
+		LOG_WARNING("Cannot identify target as an AT91SAM");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	if (at91sam7_info->flash_autodetection == 0)
+	{
+		/* banks and sectors are already created, based on data from input file */
+		struct flash_bank *fb = bank;
+		t_bank = fb;
+		while (t_bank)
+		{
+			at91sam7_info = t_bank->driver_priv;
+
+			at91sam7_info->cidr = cidr;
+			at91sam7_info->cidr_ext = (cidr >> 31)&0x0001;
+			at91sam7_info->cidr_nvptyp = (cidr >> 28)&0x0007;
+			at91sam7_info->cidr_arch = (cidr >> 20)&0x00FF;
+			at91sam7_info->cidr_sramsiz = (cidr >> 16)&0x000F;
+			at91sam7_info->cidr_nvpsiz2 = (cidr >> 12)&0x000F;
+			at91sam7_info->cidr_nvpsiz = (cidr >> 8)&0x000F;
+			at91sam7_info->cidr_eproc = (cidr >> 5)&0x0007;
+			at91sam7_info->cidr_version = cidr&0x001F;
+
+			/* calculate master clock frequency */
+			at91sam7_read_clock_info(t_bank);
+
+			/* no timming */
+			at91sam7_set_flash_mode(t_bank, FMR_TIMING_NONE);
+
+			/* check protect state */
+			at91sam7_protect_check(t_bank);
+
+			t_bank = fb->next;
+			fb = t_bank;
+		}
+
+		return ERROR_OK;
+	}
+
+	arch = (cidr >> 20)&0x00FF;
+
+	/* check flash size */
+	switch ((cidr >> 8)&0x000F)
+	{
+		case FLASH_SIZE_8KB:
+			break;
+
+		case FLASH_SIZE_16KB:
+			banks_num = 1;
+			sectors_num = 8;
+			pages_per_sector = 32;
+			page_size  = 64;
+			base_address = 0x00100000;
+			if (arch == 0x70)
+			{
+				num_nvmbits = 2;
+				target_name = "AT91SAM7S161/16";
+			}
+			break;
+
+		case FLASH_SIZE_32KB:
+			banks_num = 1;
+			sectors_num = 8;
+			pages_per_sector = 32;
+			page_size  = 128;
+			base_address = 0x00100000;
+			if (arch == 0x70)
+			{
+				num_nvmbits = 2;
+				target_name = "AT91SAM7S321/32";
+			}
+			if (arch == 0x72)
+			{
+				num_nvmbits = 3;
+				target_name = "AT91SAM7SE32";
+			}
+			break;
+
+		case FLASH_SIZE_64KB:
+			banks_num = 1;
+			sectors_num = 16;
+			pages_per_sector = 32;
+			page_size  = 128;
+			base_address = 0x00100000;
+			if (arch == 0x70)
+			{
+				num_nvmbits = 2;
+				target_name = "AT91SAM7S64";
+			}
+			break;
+
+		case FLASH_SIZE_128KB:
+			banks_num = 1;
+			sectors_num = 8;
+			pages_per_sector = 64;
+			page_size  = 256;
+			base_address = 0x00100000;
+			if (arch == 0x70)
+			{
+				num_nvmbits = 2;
+				target_name = "AT91SAM7S128";
+			}
+			if (arch == 0x71)
+			{
+				num_nvmbits = 3;
+				target_name = "AT91SAM7XC128";
+			}
+			if (arch == 0x72)
+			{
+				num_nvmbits = 3;
+				target_name = "AT91SAM7SE128";
+			}
+			if (arch == 0x75)
+			{
+				num_nvmbits = 3;
+				target_name = "AT91SAM7X128";
+			}
+			break;
+
+		case FLASH_SIZE_256KB:
+			banks_num = 1;
+			sectors_num = 16;
+			pages_per_sector = 64;
+			page_size  = 256;
+			base_address = 0x00100000;
+			if (arch == 0x60)
+			{
+				num_nvmbits = 3;
+				target_name = "AT91SAM7A3";
+			}
+			if (arch == 0x70)
+			{
+				num_nvmbits = 2;
+				target_name = "AT91SAM7S256";
+			}
+			if (arch == 0x71)
+			{
+				num_nvmbits = 3;
+				target_name = "AT91SAM7XC256";
+			}
+			if (arch == 0x72)
+			{
+				num_nvmbits = 3;
+				target_name = "AT91SAM7SE256";
+			}
+			if (arch == 0x75)
+			{
+				num_nvmbits = 3;
+				target_name = "AT91SAM7X256";
+			}
+			break;
+
+		case FLASH_SIZE_512KB:
+			banks_num = 2;
+			sectors_num = 16;
+			pages_per_sector = 64;
+			page_size  = 256;
+			base_address = 0x00100000;
+			if (arch == 0x70)
+			{
+				num_nvmbits = 2;
+				target_name = "AT91SAM7S512";
+			}
+			if (arch == 0x71)
+			{
+				num_nvmbits = 3;
+				target_name = "AT91SAM7XC512";
+			}
+			if (arch == 0x72)
+			{
+				num_nvmbits = 3;
+				target_name = "AT91SAM7SE512";
+			}
+			if (arch == 0x75)
+			{
+				num_nvmbits = 3;
+				target_name = "AT91SAM7X512";
+			}
+			break;
+
+		case FLASH_SIZE_1024KB:
+			break;
+
+		case FLASH_SIZE_2048KB:
+			break;
+	}
+
+	if (strcmp(target_name, "Unknown") == 0)
+	{
+		LOG_ERROR("Target autodetection failed! Please specify target parameters in configuration file");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	ext_freq = at91sam7_info->ext_freq;
+
+	/* calculate bank size  */
+	bank_size = sectors_num * pages_per_sector * page_size;
+
+	for (bnk = 0; bnk < banks_num; bnk++)
+	{
+		if (bnk > 0)
+		{
+			/* create a new flash bank element */
+			struct flash_bank *fb = malloc(sizeof(struct flash_bank));
+			fb->target = target;
+			fb->driver = bank->driver;
+			fb->driver_priv = malloc(sizeof(struct at91sam7_flash_bank));
+			fb->next = NULL;
+
+			/* link created bank in 'flash_banks' list and redirect t_bank */
+			t_bank->next = fb;
+			t_bank = fb;
+		}
+
+		t_bank->bank_number = bnk;
+		t_bank->base = base_address + bnk * bank_size;
+		t_bank->size = bank_size;
+		t_bank->chip_width = 0;
+		t_bank->bus_width = 4;
+		t_bank->num_sectors = sectors_num;
+
+		/* allocate sectors */
+		t_bank->sectors = malloc(sectors_num * sizeof(struct flash_sector));
+		for (sec = 0; sec < sectors_num; sec++)
+		{
+			t_bank->sectors[sec].offset = sec * pages_per_sector * page_size;
+			t_bank->sectors[sec].size = pages_per_sector * page_size;
+			t_bank->sectors[sec].is_erased = -1;
+			t_bank->sectors[sec].is_protected = -1;
+		}
+
+		at91sam7_info = t_bank->driver_priv;
+
+		at91sam7_info->cidr = cidr;
+		at91sam7_info->cidr_ext = (cidr >> 31)&0x0001;
+		at91sam7_info->cidr_nvptyp = (cidr >> 28)&0x0007;
+		at91sam7_info->cidr_arch = (cidr >> 20)&0x00FF;
+		at91sam7_info->cidr_sramsiz = (cidr >> 16)&0x000F;
+		at91sam7_info->cidr_nvpsiz2 = (cidr >> 12)&0x000F;
+		at91sam7_info->cidr_nvpsiz = (cidr >> 8)&0x000F;
+		at91sam7_info->cidr_eproc = (cidr >> 5)&0x0007;
+		at91sam7_info->cidr_version = cidr&0x001F;
+
+		at91sam7_info->target_name  = target_name;
+		at91sam7_info->flashmode = 0;
+		at91sam7_info->ext_freq = ext_freq;
+		at91sam7_info->num_nvmbits = num_nvmbits;
+		at91sam7_info->num_nvmbits_on = 0;
+		at91sam7_info->pagesize = page_size;
+		at91sam7_info->pages_per_sector = pages_per_sector;
+
+		/* calculate master clock frequency */
+		at91sam7_read_clock_info(t_bank);
+
+		/* no timming */
+		at91sam7_set_flash_mode(t_bank, FMR_TIMING_NONE);
+
+		/* check protect state */
+		at91sam7_protect_check(t_bank);
+	}
+
+	LOG_DEBUG("nvptyp: 0x%3.3x, arch: 0x%4.4x", at91sam7_info->cidr_nvptyp, at91sam7_info->cidr_arch);
+
+	return ERROR_OK;
+}
+
+static int at91sam7_erase_check(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	uint16_t retval;
+	uint32_t blank;
+	uint16_t fast_check;
+	uint8_t *buffer;
+	uint16_t nSector;
+	uint16_t nByte;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* Configure the flash controller timing */
+	at91sam7_read_clock_info(bank);
+	at91sam7_set_flash_mode(bank, FMR_TIMING_FLASH);
+
+	fast_check = 1;
+	for (nSector = 0; nSector < bank->num_sectors; nSector++)
+	{
+		retval = target_blank_check_memory(target, bank->base + bank->sectors[nSector].offset,
+			bank->sectors[nSector].size, &blank);
+		if (retval != ERROR_OK)
+		{
+			fast_check = 0;
+			break;
+		}
+		if (blank == 0xFF)
+			bank->sectors[nSector].is_erased = 1;
+		else
+			bank->sectors[nSector].is_erased = 0;
+	}
+
+	if (fast_check)
+	{
+		return ERROR_OK;
+	}
+
+	LOG_USER("Running slow fallback erase check - add working memory");
+
+	buffer = malloc(bank->sectors[0].size);
+	for (nSector = 0; nSector < bank->num_sectors; nSector++)
+	{
+		bank->sectors[nSector].is_erased = 1;
+		retval = target_read_memory(target, bank->base + bank->sectors[nSector].offset, 4,
+			bank->sectors[nSector].size/4, buffer);
+		if (retval != ERROR_OK)
+			return retval;
+
+		for (nByte = 0; nByte < bank->sectors[nSector].size; nByte++)
+		{
+			if (buffer[nByte] != 0xFF)
+			{
+				bank->sectors[nSector].is_erased = 0;
+				break;
+			}
+		}
+	}
+	free(buffer);
+
+	return ERROR_OK;
+}
+
+static int at91sam7_protect_check(struct flash_bank *bank)
+{
+	uint8_t lock_pos, gpnvm_pos;
+	uint32_t status;
+
+	struct at91sam7_flash_bank *at91sam7_info = bank->driver_priv;
+
+	if (at91sam7_info->cidr == 0)
+	{
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	status = at91sam7_get_flash_status(bank->target, bank->bank_number);
+	at91sam7_info->lockbits = (status >> 16);
+
+	at91sam7_info->num_lockbits_on = 0;
+	for (lock_pos = 0; lock_pos < bank->num_sectors; lock_pos++)
+	{
+		if (((status >> (16 + lock_pos))&(0x0001)) == 1)
+		{
+			at91sam7_info->num_lockbits_on++;
+			bank->sectors[lock_pos].is_protected = 1;
+		}
+		else
+			bank->sectors[lock_pos].is_protected = 0;
+	}
+
+	/* GPNVM and SECURITY bits apply only for MC_FSR of EFC0 */
+	status = at91sam7_get_flash_status(bank->target, 0);
+
+	at91sam7_info->securitybit = (status >> 4)&0x01;
+	at91sam7_info->nvmbits = (status >> 8)&0xFF;
+
+	at91sam7_info->num_nvmbits_on = 0;
+	for (gpnvm_pos = 0; gpnvm_pos < at91sam7_info->num_nvmbits; gpnvm_pos++)
+	{
+		if (((status >> (8 + gpnvm_pos))&(0x01)) == 1)
+		{
+			at91sam7_info->num_nvmbits_on++;
+		}
+	}
+
+	return ERROR_OK;
+}
+
+FLASH_BANK_COMMAND_HANDLER(at91sam7_flash_bank_command)
+{
+	struct flash_bank *t_bank = bank;
+	struct at91sam7_flash_bank *at91sam7_info;
+	struct target *target = t_bank->target;
+
+	uint32_t base_address;
+	uint32_t bank_size;
+	uint32_t ext_freq = 0;
+
+	int chip_width;
+	int bus_width;
+	int banks_num;
+	int num_sectors;
+
+	uint16_t pages_per_sector;
+	uint16_t page_size;
+	uint16_t num_nvmbits;
+
+	char *target_name;
+
+	int bnk, sec;
+
+	at91sam7_info = malloc(sizeof(struct at91sam7_flash_bank));
+	t_bank->driver_priv = at91sam7_info;
+
+	/* part wasn't probed for info yet */
+	at91sam7_info->cidr = 0;
+	at91sam7_info->flashmode = 0;
+	at91sam7_info->ext_freq = 0;
+	at91sam7_info->flash_autodetection = 0;
+
+	if (CMD_ARGC < 13)
+	{
+		at91sam7_info->flash_autodetection = 1;
+		return ERROR_OK;
+	}
+
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], base_address);
+
+	COMMAND_PARSE_NUMBER(int, CMD_ARGV[3], chip_width);
+	COMMAND_PARSE_NUMBER(int, CMD_ARGV[4], bus_width);
+
+	COMMAND_PARSE_NUMBER(int, CMD_ARGV[8], banks_num);
+	COMMAND_PARSE_NUMBER(int, CMD_ARGV[9], num_sectors);
+	COMMAND_PARSE_NUMBER(u16, CMD_ARGV[10], pages_per_sector);
+	COMMAND_PARSE_NUMBER(u16, CMD_ARGV[11], page_size);
+	COMMAND_PARSE_NUMBER(u16, CMD_ARGV[12], num_nvmbits);
+
+	if (CMD_ARGC == 14) {
+		unsigned long freq;
+		COMMAND_PARSE_NUMBER(ulong, CMD_ARGV[13], freq);
+		ext_freq = freq * 1000;
+		at91sam7_info->ext_freq = ext_freq;
+	}
+
+	if ((bus_width == 0) || (banks_num == 0) || (num_sectors == 0) ||
+		(pages_per_sector == 0) || (page_size == 0) || (num_nvmbits == 0))
+	{
+		at91sam7_info->flash_autodetection = 1;
+		return ERROR_OK;
+	}
+
+	target_name = calloc(strlen(CMD_ARGV[7]) + 1, sizeof(char));
+	strcpy(target_name, CMD_ARGV[7]);
+
+	/* calculate bank size  */
+	bank_size = num_sectors * pages_per_sector * page_size;
+
+	for (bnk = 0; bnk < banks_num; bnk++)
+	{
+		if (bnk > 0)
+		{
+			/* create a new bank element */
+			struct flash_bank *fb = malloc(sizeof(struct flash_bank));
+			fb->target = target;
+			fb->driver = bank->driver;
+			fb->driver_priv = malloc(sizeof(struct at91sam7_flash_bank));
+			fb->next = NULL;
+
+			/* link created bank in 'flash_banks' list and redirect t_bank */
+			t_bank->next = fb;
+			t_bank = fb;
+		}
+
+		t_bank->bank_number = bnk;
+		t_bank->base = base_address + bnk * bank_size;
+		t_bank->size = bank_size;
+		t_bank->chip_width = chip_width;
+		t_bank->bus_width = bus_width;
+		t_bank->num_sectors = num_sectors;
+
+		/* allocate sectors */
+		t_bank->sectors = malloc(num_sectors * sizeof(struct flash_sector));
+		for (sec = 0; sec < num_sectors; sec++)
+		{
+			t_bank->sectors[sec].offset = sec * pages_per_sector * page_size;
+			t_bank->sectors[sec].size = pages_per_sector * page_size;
+			t_bank->sectors[sec].is_erased = -1;
+			t_bank->sectors[sec].is_protected = -1;
+		}
+
+		at91sam7_info = t_bank->driver_priv;
+
+		at91sam7_info->target_name  = target_name;
+		at91sam7_info->flashmode = 0;
+		at91sam7_info->ext_freq  = ext_freq;
+		at91sam7_info->num_nvmbits = num_nvmbits;
+		at91sam7_info->num_nvmbits_on = 0;
+		at91sam7_info->pagesize = page_size;
+		at91sam7_info->pages_per_sector = pages_per_sector;
+	}
+
+	return ERROR_OK;
+}
+
+static int at91sam7_erase(struct flash_bank *bank, int first, int last)
+{
+	struct at91sam7_flash_bank *at91sam7_info = bank->driver_priv;
+	int sec;
+	uint32_t nbytes, pos;
+	uint8_t *buffer;
+	uint8_t erase_all;
+
+	if (at91sam7_info->cidr == 0)
+	{
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if ((first < 0) || (last < first) || (last >= bank->num_sectors))
+	{
+		return ERROR_FLASH_SECTOR_INVALID;
+	}
+
+	erase_all = 0;
+	if ((first == 0) && (last == (bank->num_sectors-1)))
+	{
+		erase_all = 1;
+	}
+
+	/* Configure the flash controller timing */
+	at91sam7_read_clock_info(bank);
+	at91sam7_set_flash_mode(bank, FMR_TIMING_FLASH);
+
+	if (erase_all)
+	{
+		if (at91sam7_flash_command(bank, EA, 0) != ERROR_OK)
+		{
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+	}
+	else
+	{
+		/* allocate and clean buffer  */
+		nbytes = (last - first + 1) * bank->sectors[first].size;
+		buffer = malloc(nbytes * sizeof(uint8_t));
+		for (pos = 0; pos < nbytes; pos++)
+		{
+			buffer[pos] = 0xFF;
+		}
+
+		if (at91sam7_write(bank, buffer, bank->sectors[first].offset, nbytes) != ERROR_OK)
+		{
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+
+		free(buffer);
+	}
+
+	/* mark erased sectors */
+	for (sec = first; sec <= last; sec++)
+	{
+		bank->sectors[sec].is_erased = 1;
+	}
+
+	return ERROR_OK;
+}
+
+static int at91sam7_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	uint32_t cmd;
+	int sector;
+	uint32_t pagen;
+
+	struct at91sam7_flash_bank *at91sam7_info = bank->driver_priv;
+
+	if (at91sam7_info->cidr == 0)
+	{
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if ((first < 0) || (last < first) || (last >= bank->num_sectors))
+	{
+		return ERROR_FLASH_SECTOR_INVALID;
+	}
+
+	/* Configure the flash controller timing */
+	at91sam7_read_clock_info(bank);
+	at91sam7_set_flash_mode(bank, FMR_TIMING_NVBITS);
+
+	for (sector = first; sector <= last; sector++)
+	{
+		if (set)
+			cmd = SLB;
+		else
+			cmd = CLB;
+
+		/* if we lock a page from one sector then entire sector will be locked, also,
+		 * if we unlock a page from a locked sector, entire sector will be unlocked   */
+		pagen = sector * at91sam7_info->pages_per_sector;
+
+		if (at91sam7_flash_command(bank, cmd, pagen) != ERROR_OK)
+		{
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+	}
+
+	at91sam7_protect_check(bank);
+
+	return ERROR_OK;
+}
+
+static int at91sam7_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	int retval;
+	struct at91sam7_flash_bank *at91sam7_info = bank->driver_priv;
+	struct target *target = bank->target;
+	uint32_t dst_min_alignment, wcount, bytes_remaining = count;
+	uint32_t first_page, last_page, pagen, buffer_pos;
+
+	if (at91sam7_info->cidr == 0)
+	{
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	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;
+
+	dst_min_alignment = at91sam7_info->pagesize;
+
+	if (offset % dst_min_alignment)
+	{
+		LOG_WARNING("offset 0x%" PRIx32 " breaks required alignment 0x%" PRIx32 "", offset, dst_min_alignment);
+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+	}
+
+	if (at91sam7_info->cidr_arch == 0)
+		return ERROR_FLASH_BANK_NOT_PROBED;
+
+	first_page = offset/dst_min_alignment;
+	last_page = DIV_ROUND_UP(offset + count, dst_min_alignment);
+
+	LOG_DEBUG("first_page: %i, last_page: %i, count %i", (int)first_page, (int)last_page, (int)count);
+
+	/* Configure the flash controller timing */
+	at91sam7_read_clock_info(bank);
+	at91sam7_set_flash_mode(bank, FMR_TIMING_FLASH);
+
+	for (pagen = first_page; pagen < last_page; pagen++)
+	{
+		if (bytes_remaining < dst_min_alignment)
+			count = bytes_remaining;
+		else
+			count = dst_min_alignment;
+		bytes_remaining -= count;
+
+		/* Write one block to the PageWriteBuffer */
+		buffer_pos = (pagen-first_page)*dst_min_alignment;
+		wcount = DIV_ROUND_UP(count,4);
+		if ((retval = target_write_memory(target, bank->base + pagen*dst_min_alignment, 4, wcount, buffer + buffer_pos)) != ERROR_OK)
+		{
+			return retval;
+		}
+
+		/* Send Write Page command to Flash Controller */
+		if (at91sam7_flash_command(bank, WP, pagen) != ERROR_OK)
+		{
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+		LOG_DEBUG("Write flash bank:%i page number:%" PRIi32 "", bank->bank_number, pagen);
+	}
+
+	return ERROR_OK;
+}
+
+static int at91sam7_probe(struct flash_bank *bank)
+{
+	/* we can't probe on an at91sam7
+	 * if this is an at91sam7, it has the configured flash */
+	int retval;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	retval = at91sam7_read_part_info(bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return ERROR_OK;
+}
+
+static int at91sam7_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	int printed;
+	struct at91sam7_flash_bank *at91sam7_info = bank->driver_priv;
+
+	if (at91sam7_info->cidr == 0)
+	{
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	printed = snprintf(buf, buf_size,
+		"\n at91sam7 driver information: Chip is %s\n",
+		at91sam7_info->target_name);
+
+	buf += printed;
+	buf_size -= printed;
+
+	printed = snprintf(buf,
+			   buf_size,
+			   " Cidr: 0x%8.8" PRIx32 " | Arch: 0x%4.4x | Eproc: %s | Version: 0x%3.3x | Flashsize: 0x%8.8" PRIx32 "\n",
+			   at91sam7_info->cidr,
+			   at91sam7_info->cidr_arch,
+			   EPROC[at91sam7_info->cidr_eproc],
+			   at91sam7_info->cidr_version,
+			   bank->size);
+
+	buf += printed;
+	buf_size -= printed;
+
+	printed = snprintf(buf, buf_size,
+		" Master clock (estimated): %u KHz | External clock: %u KHz\n",
+		(unsigned)(at91sam7_info->mck_freq / 1000), (unsigned)(at91sam7_info->ext_freq / 1000));
+
+	buf += printed;
+	buf_size -= printed;
+
+	printed = snprintf(buf, buf_size,
+		" Pagesize: %i bytes | Lockbits(%i): %i 0x%4.4x | Pages in lock region: %i \n",
+		at91sam7_info->pagesize, bank->num_sectors, at91sam7_info->num_lockbits_on,
+		at91sam7_info->lockbits, at91sam7_info->pages_per_sector*at91sam7_info->num_lockbits_on);
+
+	buf += printed;
+	buf_size -= printed;
+
+	printed = snprintf(buf, buf_size,
+		" Securitybit: %i | Nvmbits(%i): %i 0x%1.1x\n",
+		at91sam7_info->securitybit, at91sam7_info->num_nvmbits,
+		at91sam7_info->num_nvmbits_on, at91sam7_info->nvmbits);
+
+	buf += printed;
+	buf_size -= printed;
+
+	return ERROR_OK;
+}
+
+/*
+* On AT91SAM7S: When the gpnvm bits are set with
+* > at91sam7 gpnvm bitnr set
+* the changes are not visible in the flash controller status register MC_FSR
+* until the processor has been reset.
+* On the Olimex board this requires a power cycle.
+* Note that the AT91SAM7S has the following errata (doc6175.pdf sec 14.1.3):
+*   The maximum number of write/erase cycles for Non volatile Memory bits is 100. this includes
+*   Lock Bits (LOCKx), General Purpose NVM bits (GPNVMx) and the Security Bit.
+*/
+COMMAND_HANDLER(at91sam7_handle_gpnvm_command)
+{
+	struct flash_bank *bank;
+	int bit;
+	uint8_t  flashcmd;
+	uint32_t status;
+	struct at91sam7_flash_bank *at91sam7_info;
+	int retval;
+
+	if (CMD_ARGC != 2)
+	{
+		command_print(CMD_CTX, "at91sam7 gpnvm <bit> <set | clear>");
+		return ERROR_OK;
+	}
+
+	bank = get_flash_bank_by_num_noprobe(0);
+	if (bank ==  NULL)
+	{
+		return ERROR_FLASH_BANK_INVALID;
+	}
+	if (strcmp(bank->driver->name, "at91sam7"))
+	{
+		command_print(CMD_CTX, "not an at91sam7 flash bank '%s'", CMD_ARGV[0]);
+		return ERROR_FLASH_BANK_INVALID;
+	}
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("target has to be halted to perform flash operation");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (strcmp(CMD_ARGV[1], "set") == 0)
+	{
+		flashcmd = SGPB;
+	}
+	else if (strcmp(CMD_ARGV[1], "clear") == 0)
+	{
+		flashcmd = CGPB;
+	}
+	else
+	{
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	at91sam7_info = bank->driver_priv;
+	if (at91sam7_info->cidr == 0)
+	{
+		retval = at91sam7_read_part_info(bank);
+		if (retval != ERROR_OK)
+		{
+			return retval;
+		}
+	}
+
+	COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], bit);
+	if ((bit < 0) || (bit >= at91sam7_info->num_nvmbits))
+	{
+		command_print(CMD_CTX, "gpnvm bit '#%s' is out of bounds for target %s", CMD_ARGV[0], at91sam7_info->target_name);
+		return ERROR_OK;
+	}
+
+	/* Configure the flash controller timing */
+	at91sam7_read_clock_info(bank);
+	at91sam7_set_flash_mode(bank, FMR_TIMING_NVBITS);
+
+	if (at91sam7_flash_command(bank, flashcmd, bit) != ERROR_OK)
+	{
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	/* GPNVM and SECURITY bits apply only for MC_FSR of EFC0 */
+	status = at91sam7_get_flash_status(bank->target, 0);
+	LOG_DEBUG("at91sam7_handle_gpnvm_command: cmd 0x%x, value %d, status 0x%" PRIx32 " \n", flashcmd, bit, status);
+
+	/* check protect state */
+	at91sam7_protect_check(bank);
+
+	return ERROR_OK;
+}
+
+static const struct command_registration at91sam7_exec_command_handlers[] = {
+	{
+		.name = "gpnvm",
+		.handler = &at91sam7_handle_gpnvm_command,
+		.mode = COMMAND_EXEC,
+		.usage = "gpnvm <bit> set | clear, "
+			"set or clear one gpnvm bit",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+static const struct command_registration at91sam7_command_handlers[] = {
+	{
+		.name = "at91sam7",
+		.mode = COMMAND_ANY,
+		.help = "at91sam7 flash command group",
+		.chain = at91sam7_exec_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+struct flash_driver at91sam7_flash = {
+		.name = "at91sam7",
+		.commands = at91sam7_command_handlers,
+		.flash_bank_command = &at91sam7_flash_bank_command,
+		.erase = &at91sam7_erase,
+		.protect = &at91sam7_protect,
+		.write = &at91sam7_write,
+		.probe = &at91sam7_probe,
+		.auto_probe = &at91sam7_probe,
+		.erase_check = &at91sam7_erase_check,
+		.protect_check = &at91sam7_protect_check,
+		.info = &at91sam7_info,
+	};
diff --git a/src/flash/nor/at91sam7.h b/src/flash/nor/at91sam7.h
new file mode 100644
index 00000000..45106869
--- /dev/null
+++ b/src/flash/nor/at91sam7.h
@@ -0,0 +1,118 @@
+/***************************************************************************
+ *   Copyright (C) 2006 by Magnus Lundin                                   *
+ *   lundin@mlu.mine.nu                                                    *
+ *                                                                         *
+ *   Copyright (C) 2006 by Gheorghe Guran (atlas)                          *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+#ifndef AT91SAM7_H
+#define AT91SAM7_H
+
+#include "flash.h"
+
+struct at91sam7_flash_bank
+{
+	/* chip id register */
+	uint32_t cidr;
+	uint16_t cidr_ext;
+	uint16_t cidr_nvptyp;
+	uint16_t cidr_arch;
+	uint16_t cidr_sramsiz;
+	uint16_t cidr_nvpsiz;
+	uint16_t cidr_nvpsiz2;
+	uint16_t cidr_eproc;
+	uint16_t cidr_version;
+	char *target_name;
+
+	/* flash auto-detection */
+	uint8_t  flash_autodetection;
+
+	/* flash geometry */
+	uint16_t pages_per_sector;
+	uint16_t pagesize;
+	uint16_t pages_in_lockregion;
+
+	/* nv memory bits */
+	uint16_t num_lockbits_on;
+	uint16_t lockbits;
+	uint16_t num_nvmbits;
+	uint16_t num_nvmbits_on;
+	uint16_t nvmbits;
+	uint8_t  securitybit;
+
+	/* 0: not init
+	 * 1: fmcn for nvbits (1uS)
+	 * 2: fmcn for flash (1.5uS) */
+	uint8_t  flashmode;
+
+	/* main clock status */
+	uint8_t  mck_valid;
+	uint32_t mck_freq;
+
+	/* external clock frequency */
+	uint32_t ext_freq;
+
+};
+
+
+/* AT91SAM7 control registers */
+#define DBGU_CIDR			0xFFFFF240
+#define CKGR_MCFR			0xFFFFFC24
+#define CKGR_MOR			0xFFFFFC20
+#define CKGR_MCFR_MAINRDY	0x10000
+#define CKGR_PLLR			0xFFFFFC2c
+#define CKGR_PLLR_DIV		0xff
+#define CKGR_PLLR_MUL		0x07ff0000
+#define PMC_MCKR			0xFFFFFC30
+#define PMC_MCKR_CSS		0x03
+#define PMC_MCKR_PRES		0x1c
+
+/* Flash Controller Commands */
+#define WP		0x01
+#define SLB		0x02
+#define WPL		0x03
+#define CLB		0x04
+#define EA		0x08
+#define SGPB	0x0B
+#define CGPB	0x0D
+#define SSB		0x0F
+
+/* MC_FSR bit definitions */
+#define MC_FSR_FRDY			1
+#define MC_FSR_EOL			2
+
+/* AT91SAM7 constants */
+#define RC_FREQ				32000
+
+/* Flash timing modes */
+#define FMR_TIMING_NONE		0
+#define FMR_TIMING_NVBITS	1
+#define FMR_TIMING_FLASH	2
+
+/* Flash size constants */
+#define FLASH_SIZE_8KB		1
+#define FLASH_SIZE_16KB		2
+#define FLASH_SIZE_32KB		3
+#define FLASH_SIZE_64KB		5
+#define FLASH_SIZE_128KB	7
+#define FLASH_SIZE_256KB	9
+#define FLASH_SIZE_512KB	10
+#define FLASH_SIZE_1024KB	12
+#define FLASH_SIZE_2048KB	14
+
+#endif /* AT91SAM7_H */
diff --git a/src/flash/nor/avrf.c b/src/flash/nor/avrf.c
new file mode 100644
index 00000000..687dd4b5
--- /dev/null
+++ b/src/flash/nor/avrf.c
@@ -0,0 +1,483 @@
+/***************************************************************************
+ *   Copyright (C) 2009 by Simon Qian                                      *
+ *   SimonQian@SimonQian.com                                               *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "avrf.h"
+#include "avrt.h"
+#include "flash.h"
+
+
+/* AVR_JTAG_Instructions */
+#define AVR_JTAG_INS_LEN							4
+// Public Instructions:
+#define AVR_JTAG_INS_EXTEST							0x00
+#define AVR_JTAG_INS_IDCODE							0x01
+#define AVR_JTAG_INS_SAMPLE_PRELOAD					0x02
+#define AVR_JTAG_INS_BYPASS							0x0F
+// AVR Specified Public Instructions:
+#define AVR_JTAG_INS_AVR_RESET						0x0C
+#define AVR_JTAG_INS_PROG_ENABLE					0x04
+#define AVR_JTAG_INS_PROG_COMMANDS					0x05
+#define AVR_JTAG_INS_PROG_PAGELOAD					0x06
+#define AVR_JTAG_INS_PROG_PAGEREAD					0x07
+
+// Data Registers:
+#define AVR_JTAG_REG_Bypass_Len						1
+#define AVR_JTAG_REG_DeviceID_Len					32
+
+#define AVR_JTAG_REG_Reset_Len						1
+#define AVR_JTAG_REG_JTAGID_Len						32
+#define AVR_JTAG_REG_ProgrammingEnable_Len			16
+#define AVR_JTAG_REG_ProgrammingCommand_Len			15
+#define AVR_JTAG_REG_FlashDataByte_Len				16
+
+struct avrf_type avft_chips_info[] =
+{
+//	 name,			chip_id,	flash_page_size,	flash_page_num,	eeprom_page_size,	eeprom_page_num
+	{"atmega128",	0x9702,		256,				512,			8,					512},
+};
+
+int avr_jtag_sendinstr(struct jtag_tap *tap, uint8_t *ir_in, uint8_t ir_out);
+int avr_jtag_senddat(struct jtag_tap *tap, uint32_t *dr_in, uint32_t dr_out, int len);
+
+int mcu_write_ir(struct jtag_tap *tap, uint8_t *ir_in, uint8_t *ir_out, int ir_len, int rti);
+int mcu_write_dr(struct jtag_tap *tap, uint8_t *ir_in, uint8_t *ir_out, int dr_len, int rti);
+int mcu_write_ir_u8(struct jtag_tap *tap, uint8_t *ir_in, uint8_t ir_out, int ir_len, int rti);
+int mcu_write_dr_u8(struct jtag_tap *tap, uint8_t *ir_in, uint8_t ir_out, int dr_len, int rti);
+int mcu_write_ir_u16(struct jtag_tap *tap, uint16_t *ir_in, uint16_t ir_out, int ir_len, int rti);
+int mcu_write_dr_u16(struct jtag_tap *tap, uint16_t *ir_in, uint16_t ir_out, int dr_len, int rti);
+int mcu_write_ir_u32(struct jtag_tap *tap, uint32_t *ir_in, uint32_t ir_out, int ir_len, int rti);
+int mcu_write_dr_u32(struct jtag_tap *tap, uint32_t *ir_in, uint32_t ir_out, int dr_len, int rti);
+int mcu_execute_queue(void);
+
+/* avr program functions */
+static int avr_jtag_reset(struct avr_common *avr, uint32_t reset)
+{
+	avr_jtag_sendinstr(avr->jtag_info.tap, NULL, AVR_JTAG_INS_AVR_RESET);
+	avr_jtag_senddat(avr->jtag_info.tap, NULL, reset ,AVR_JTAG_REG_Reset_Len);
+
+	return ERROR_OK;
+}
+
+static int avr_jtag_read_jtagid(struct avr_common *avr, uint32_t *id)
+{
+	avr_jtag_sendinstr(avr->jtag_info.tap, NULL, AVR_JTAG_INS_IDCODE);
+	avr_jtag_senddat(avr->jtag_info.tap, id, 0, AVR_JTAG_REG_JTAGID_Len);
+
+	return ERROR_OK;
+}
+
+static int avr_jtagprg_enterprogmode(struct avr_common *avr)
+{
+	avr_jtag_reset(avr, 1);
+
+	avr_jtag_sendinstr(avr->jtag_info.tap, NULL, AVR_JTAG_INS_PROG_ENABLE);
+	avr_jtag_senddat(avr->jtag_info.tap, NULL, 0xA370, AVR_JTAG_REG_ProgrammingEnable_Len);
+
+	return ERROR_OK;
+}
+
+static int avr_jtagprg_leaveprogmode(struct avr_common *avr)
+{
+	avr_jtag_sendinstr(avr->jtag_info.tap, NULL, AVR_JTAG_INS_PROG_COMMANDS);
+	avr_jtag_senddat(avr->jtag_info.tap, NULL, 0x2300, AVR_JTAG_REG_ProgrammingCommand_Len);
+	avr_jtag_senddat(avr->jtag_info.tap, NULL, 0x3300, AVR_JTAG_REG_ProgrammingCommand_Len);
+
+	avr_jtag_sendinstr(avr->jtag_info.tap, NULL, AVR_JTAG_INS_PROG_ENABLE);
+	avr_jtag_senddat(avr->jtag_info.tap, NULL, 0, AVR_JTAG_REG_ProgrammingEnable_Len);
+
+	avr_jtag_reset(avr, 0);
+
+	return ERROR_OK;
+}
+
+static int avr_jtagprg_chiperase(struct avr_common *avr)
+{
+	uint32_t poll_value;
+
+	avr_jtag_sendinstr(avr->jtag_info.tap, NULL, AVR_JTAG_INS_PROG_COMMANDS);
+	avr_jtag_senddat(avr->jtag_info.tap, NULL, 0x2380, AVR_JTAG_REG_ProgrammingCommand_Len);
+	avr_jtag_senddat(avr->jtag_info.tap, NULL, 0x3180, AVR_JTAG_REG_ProgrammingCommand_Len);
+	avr_jtag_senddat(avr->jtag_info.tap, NULL, 0x3380, AVR_JTAG_REG_ProgrammingCommand_Len);
+	avr_jtag_senddat(avr->jtag_info.tap, NULL, 0x3380, AVR_JTAG_REG_ProgrammingCommand_Len);
+
+	do {
+		poll_value = 0;
+		avr_jtag_senddat(avr->jtag_info.tap, &poll_value, 0x3380, AVR_JTAG_REG_ProgrammingCommand_Len);
+		if (ERROR_OK != mcu_execute_queue())
+		{
+			return ERROR_FAIL;
+		}
+		LOG_DEBUG("poll_value = 0x%04" PRIx32 "", poll_value);
+	} while (!(poll_value & 0x0200));
+
+	return ERROR_OK;
+}
+
+static int avr_jtagprg_writeflashpage(struct avr_common *avr, uint8_t *page_buf, uint32_t buf_size, uint32_t addr, uint32_t page_size)
+{
+	uint32_t i, poll_value;
+
+	avr_jtag_sendinstr(avr->jtag_info.tap, NULL, AVR_JTAG_INS_PROG_COMMANDS);
+	avr_jtag_senddat(avr->jtag_info.tap, NULL, 0x2310, AVR_JTAG_REG_ProgrammingCommand_Len);
+
+	// load addr high byte
+	avr_jtag_senddat(avr->jtag_info.tap, NULL, 0x0700 | ((addr >> 9) & 0xFF), AVR_JTAG_REG_ProgrammingCommand_Len);
+
+	// load addr low byte
+	avr_jtag_senddat(avr->jtag_info.tap, NULL, 0x0300 | ((addr >> 1) & 0xFF), AVR_JTAG_REG_ProgrammingCommand_Len);
+
+	avr_jtag_sendinstr(avr->jtag_info.tap, NULL, AVR_JTAG_INS_PROG_PAGELOAD);
+
+	for (i = 0; i < page_size; i++)
+	{
+		if (i < buf_size)
+		{
+			avr_jtag_senddat(avr->jtag_info.tap, NULL, page_buf[i], 8);
+		}
+		else
+		{
+			avr_jtag_senddat(avr->jtag_info.tap, NULL, 0xFF, 8);
+		}
+	}
+
+	avr_jtag_sendinstr(avr->jtag_info.tap, NULL, AVR_JTAG_INS_PROG_COMMANDS);
+
+	avr_jtag_senddat(avr->jtag_info.tap, NULL, 0x3700, AVR_JTAG_REG_ProgrammingCommand_Len);
+	avr_jtag_senddat(avr->jtag_info.tap, NULL, 0x3500, AVR_JTAG_REG_ProgrammingCommand_Len);
+	avr_jtag_senddat(avr->jtag_info.tap, NULL, 0x3700, AVR_JTAG_REG_ProgrammingCommand_Len);
+	avr_jtag_senddat(avr->jtag_info.tap, NULL, 0x3700, AVR_JTAG_REG_ProgrammingCommand_Len);
+
+	do {
+		poll_value = 0;
+		avr_jtag_senddat(avr->jtag_info.tap, &poll_value, 0x3700, AVR_JTAG_REG_ProgrammingCommand_Len);
+		if (ERROR_OK != mcu_execute_queue())
+		{
+			return ERROR_FAIL;
+		}
+		LOG_DEBUG("poll_value = 0x%04" PRIx32 "", poll_value);
+	} while (!(poll_value & 0x0200));
+
+	return ERROR_OK;
+}
+
+FLASH_BANK_COMMAND_HANDLER(avrf_flash_bank_command)
+{
+	struct avrf_flash_bank *avrf_info;
+
+	if (CMD_ARGC < 6)
+	{
+		LOG_WARNING("incomplete flash_bank avr configuration");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	avrf_info = malloc(sizeof(struct avrf_flash_bank));
+	bank->driver_priv = avrf_info;
+
+	avrf_info->probed = 0;
+
+	return ERROR_OK;
+}
+
+static int avrf_erase(struct flash_bank *bank, int first, int last)
+{
+	LOG_INFO("%s", __FUNCTION__);
+	return ERROR_OK;
+}
+
+static int avrf_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	LOG_INFO("%s", __FUNCTION__);
+	return ERROR_OK;
+}
+
+static int avrf_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	struct avr_common *avr = target->arch_info;
+	uint32_t cur_size, cur_buffer_size, page_size;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	page_size = bank->sectors[0].size;
+	if ((offset % page_size) != 0)
+	{
+		LOG_WARNING("offset 0x%" PRIx32 " breaks required %" PRIu32 "-byte alignment", offset, page_size);
+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+	}
+
+	LOG_DEBUG("offset is 0x%08" PRIx32 "", offset);
+	LOG_DEBUG("count is %" PRId32 "", count);
+
+	if (ERROR_OK != avr_jtagprg_enterprogmode(avr))
+	{
+		return ERROR_FAIL;
+	}
+
+	cur_size = 0;
+	while (count > 0)
+	{
+		if (count > page_size)
+		{
+			cur_buffer_size = page_size;
+		}
+		else
+		{
+			cur_buffer_size = count;
+		}
+		avr_jtagprg_writeflashpage(avr, buffer + cur_size, cur_buffer_size, offset + cur_size, page_size);
+		count -= cur_buffer_size;
+		cur_size += cur_buffer_size;
+
+		keep_alive();
+	}
+
+	return avr_jtagprg_leaveprogmode(avr);
+}
+
+#define EXTRACT_MFG(X)  (((X) & 0xffe) >> 1)
+#define EXTRACT_PART(X) (((X) & 0xffff000) >> 12)
+#define EXTRACT_VER(X)  (((X) & 0xf0000000) >> 28)
+static int avrf_probe(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct avrf_flash_bank *avrf_info = bank->driver_priv;
+	struct avr_common *avr = target->arch_info;
+	struct avrf_type *avr_info = NULL;
+	int i;
+	uint32_t device_id;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	avrf_info->probed = 0;
+
+	avr_jtag_read_jtagid(avr, &device_id);
+	if (ERROR_OK != mcu_execute_queue())
+	{
+		return ERROR_FAIL;
+	}
+
+	LOG_INFO("device id = 0x%08" PRIx32 "", device_id);
+	if (EXTRACT_MFG(device_id) != 0x1F)
+	{
+		LOG_ERROR("0x%" PRIx32 " is invalid Manufacturer for avr, 0x%X is expected", EXTRACT_MFG(device_id), 0x1F);
+	}
+
+	for (i = 0; i < (int)ARRAY_SIZE(avft_chips_info); i++)
+	{
+		if (avft_chips_info[i].chip_id == EXTRACT_PART(device_id))
+		{
+			avr_info = &avft_chips_info[i];
+			LOG_INFO("target device is %s", avr_info->name);
+			break;
+		}
+	}
+
+	if (avr_info != NULL)
+	{
+		// chip found
+		bank->base = 0x00000000;
+		bank->size = (avr_info->flash_page_size * avr_info->flash_page_num);
+		bank->num_sectors = avr_info->flash_page_num;
+		bank->sectors = malloc(sizeof(struct flash_sector) * avr_info->flash_page_num);
+
+		for (i = 0; i < avr_info->flash_page_num; i++)
+		{
+			bank->sectors[i].offset = i * avr_info->flash_page_size;
+			bank->sectors[i].size = avr_info->flash_page_size;
+			bank->sectors[i].is_erased = -1;
+			bank->sectors[i].is_protected = 1;
+		}
+
+		avrf_info->probed = 1;
+		return ERROR_OK;
+	}
+	else
+	{
+		// chip not supported
+		LOG_ERROR("0x%" PRIx32 " is not support for avr", EXTRACT_PART(device_id));
+
+		avrf_info->probed = 1;
+		return ERROR_FAIL;
+	}
+}
+
+static int avrf_auto_probe(struct flash_bank *bank)
+{
+	struct avrf_flash_bank *avrf_info = bank->driver_priv;
+	if (avrf_info->probed)
+		return ERROR_OK;
+	return avrf_probe(bank);
+}
+
+static int avrf_protect_check(struct flash_bank *bank)
+{
+	LOG_INFO("%s", __FUNCTION__);
+	return ERROR_OK;
+}
+
+static int avrf_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	struct target *target = bank->target;
+	struct avr_common *avr = target->arch_info;
+	struct avrf_type *avr_info = NULL;
+	int i;
+	uint32_t device_id;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	avr_jtag_read_jtagid(avr, &device_id);
+	if (ERROR_OK != mcu_execute_queue())
+	{
+		return ERROR_FAIL;
+	}
+
+	LOG_INFO("device id = 0x%08" PRIx32 "", device_id);
+	if (EXTRACT_MFG(device_id) != 0x1F)
+	{
+		LOG_ERROR("0x%" PRIx32 " is invalid Manufacturer for avr, 0x%X is expected", EXTRACT_MFG(device_id), 0x1F);
+	}
+
+	for (i = 0; i < (int)ARRAY_SIZE(avft_chips_info); i++)
+	{
+		if (avft_chips_info[i].chip_id == EXTRACT_PART(device_id))
+		{
+			avr_info = &avft_chips_info[i];
+			LOG_INFO("target device is %s", avr_info->name);
+
+			break;
+		}
+	}
+
+	if (avr_info != NULL)
+	{
+		// chip found
+		snprintf(buf, buf_size, "%s - Rev: 0x%" PRIx32 "", avr_info->name, EXTRACT_VER(device_id));
+		return ERROR_OK;
+	}
+	else
+	{
+		// chip not supported
+		snprintf(buf, buf_size, "Cannot identify target as a avr\n");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+}
+
+static int avrf_mass_erase(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct avr_common *avr = target->arch_info;
+
+	if (target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if ((ERROR_OK != avr_jtagprg_enterprogmode(avr))
+		|| (ERROR_OK != avr_jtagprg_chiperase(avr))
+		|| (ERROR_OK != avr_jtagprg_leaveprogmode(avr)))
+	{
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(avrf_handle_mass_erase_command)
+{
+	int i;
+
+	if (CMD_ARGC < 1)
+	{
+		command_print(CMD_CTX, "avr mass_erase <bank>");
+		return ERROR_OK;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	if (avrf_mass_erase(bank) == ERROR_OK)
+	{
+		/* set all sectors as erased */
+		for (i = 0; i < bank->num_sectors; i++)
+		{
+			bank->sectors[i].is_erased = 1;
+		}
+
+		command_print(CMD_CTX, "avr mass erase complete");
+	}
+	else
+	{
+		command_print(CMD_CTX, "avr mass erase failed");
+	}
+
+	LOG_DEBUG("%s", __FUNCTION__);
+	return ERROR_OK;
+}
+
+static const struct command_registration avrf_exec_command_handlers[] = {
+	{
+		.name = "mass_erase",
+		.handler = &avrf_handle_mass_erase_command,
+		.mode = COMMAND_EXEC,
+		.help = "erase entire device",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+static const struct command_registration avrf_command_handlers[] = {
+	{
+		.name = "avrf",
+		.mode = COMMAND_ANY,
+		.help = "AVR flash command group",
+		.chain = avrf_exec_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+struct flash_driver avr_flash = {
+		.name = "avr",
+		.commands = avrf_command_handlers,
+		.flash_bank_command = &avrf_flash_bank_command,
+		.erase = &avrf_erase,
+		.protect = &avrf_protect,
+		.write = &avrf_write,
+		.probe = &avrf_probe,
+		.auto_probe = &avrf_auto_probe,
+		.erase_check = &default_flash_mem_blank_check,
+		.protect_check = &avrf_protect_check,
+		.info = &avrf_info,
+	};
diff --git a/src/flash/nor/avrf.h b/src/flash/nor/avrf.h
new file mode 100644
index 00000000..e75d9d7e
--- /dev/null
+++ b/src/flash/nor/avrf.h
@@ -0,0 +1,41 @@
+/***************************************************************************
+ *   Copyright (C) 2009 by Simon Qian                                      *
+ *   SimonQian@SimonQian.com                                               *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifndef AVRF_H
+#define AVRF_H
+
+#include "types.h"
+
+struct avrf_type
+{
+	char name[15];
+	uint16_t chip_id;
+	int flash_page_size;
+	int flash_page_num;
+	int eeprom_page_size;
+	int eeprom_page_num;
+};
+
+struct avrf_flash_bank
+{
+	int ppage_size;
+	int probed;
+};
+
+#endif /* AVRF_H */
diff --git a/src/flash/nor/cfi.c b/src/flash/nor/cfi.c
new file mode 100644
index 00000000..6dbffb9e
--- /dev/null
+++ b/src/flash/nor/cfi.c
@@ -0,0 +1,2630 @@
+/***************************************************************************
+ *   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  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "cfi.h"
+#include "non_cfi.h"
+#include "armv4_5.h"
+#include "binarybuffer.h"
+#include "algorithm.h"
+
+
+#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
+
+static struct cfi_unlock_addresses cfi_unlock_addresses[] =
+{
+	[CFI_UNLOCK_555_2AA] = { .unlock1 = 0x555, .unlock2 = 0x2aa },
+	[CFI_UNLOCK_5555_2AAA] = { .unlock1 = 0x5555, .unlock2 = 0x2aaa },
+};
+
+/* CFI fixups foward declarations */
+static void cfi_fixup_0002_erase_regions(struct flash_bank *flash, void *param);
+static void cfi_fixup_0002_unlock_addresses(struct flash_bank *flash, void *param);
+static void cfi_fixup_atmel_reversed_erase_regions(struct flash_bank *flash, void *param);
+
+/* fixup after reading cmdset 0002 primary query table */
+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]},
+	{CFI_MFR_SST, 0x00D7, cfi_fixup_0002_unlock_addresses, &cfi_unlock_addresses[CFI_UNLOCK_5555_2AAA]},
+	{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 */
+static const struct cfi_fixup cfi_0001_fixups[] = {
+	{0, 0, NULL, NULL}
+};
+
+static void cfi_fixup(struct flash_bank *bank, const struct cfi_fixup *fixups)
+{
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	const struct cfi_fixup *f;
+
+	for (f = fixups; f->fixup; f++)
+	{
+		if (((f->mfr == CFI_MFR_ANY) || (f->mfr == cfi_info->manufacturer)) &&
+			((f->id  == CFI_ID_ANY)  || (f->id  == cfi_info->device_id)))
+		{
+			f->fixup(bank, f->param);
+		}
+	}
+}
+
+/* inline uint32_t flash_address(struct flash_bank *bank, int sector, uint32_t offset) */
+static __inline__ uint32_t flash_address(struct flash_bank *bank, int sector, uint32_t offset)
+{
+	struct cfi_flash_bank *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;
+	else
+	{
+		if (!bank->sectors)
+		{
+			LOG_ERROR("BUG: sector list not yet built");
+			exit(-1);
+		}
+		return bank->base + bank->sectors[sector].offset + offset * bank->bus_width;
+	}
+
+}
+
+static void cfi_command(struct flash_bank *bank, uint8_t cmd, uint8_t *cmd_buf)
+{
+	int i;
+
+	/* clear whole buffer, to ensure bits that exceed the bus_width
+	 * are set to zero
+	 */
+	for (i = 0; i < CFI_MAX_BUS_WIDTH; i++)
+		cmd_buf[i] = 0;
+
+	if (bank->target->endianness == TARGET_LITTLE_ENDIAN)
+	{
+		for (i = bank->bus_width; i > 0; i--)
+		{
+			*cmd_buf++ = (i & (bank->chip_width - 1)) ? 0x0 : cmd;
+		}
+	}
+	else
+	{
+		for (i = 1; i <= bank->bus_width; i++)
+		{
+			*cmd_buf++ = (i & (bank->chip_width - 1)) ? 0x0 : cmd;
+		}
+	}
+}
+
+/* read unsigned 8-bit value from the bank
+ * flash banks are expected to be made of similar chips
+ * the query result should be the same for all
+ */
+static uint8_t cfi_query_u8(struct flash_bank *bank, int sector, uint32_t offset)
+{
+	struct target *target = bank->target;
+	uint8_t data[CFI_MAX_BUS_WIDTH];
+
+	target_read_memory(target, flash_address(bank, sector, offset), bank->bus_width, 1, data);
+
+	if (bank->target->endianness == TARGET_LITTLE_ENDIAN)
+		return data[0];
+	else
+		return data[bank->bus_width - 1];
+}
+
+/* read unsigned 8-bit value from the bank
+ * in case of a bank made of multiple chips,
+ * the individual values are ORed
+ */
+static uint8_t cfi_get_u8(struct flash_bank *bank, int sector, uint32_t offset)
+{
+	struct target *target = bank->target;
+	uint8_t data[CFI_MAX_BUS_WIDTH];
+	int i;
+
+	target_read_memory(target, flash_address(bank, sector, offset), bank->bus_width, 1, data);
+
+	if (bank->target->endianness == TARGET_LITTLE_ENDIAN)
+	{
+		for (i = 0; i < bank->bus_width / bank->chip_width; i++)
+			data[0] |= data[i];
+
+		return data[0];
+	}
+	else
+	{
+		uint8_t value = 0;
+		for (i = 0; i < bank->bus_width / bank->chip_width; i++)
+			value |= data[bank->bus_width - 1 - i];
+
+		return value;
+	}
+}
+
+static uint16_t cfi_query_u16(struct flash_bank *bank, int sector, uint32_t offset)
+{
+	struct target *target = bank->target;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	uint8_t data[CFI_MAX_BUS_WIDTH * 2];
+
+	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;
+	else
+		return data[bank->bus_width - 1] | data[(2 * bank->bus_width) - 1] << 8;
+}
+
+static uint32_t cfi_query_u32(struct flash_bank *bank, int sector, uint32_t offset)
+{
+	struct target *target = bank->target;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	uint8_t data[CFI_MAX_BUS_WIDTH * 4];
+
+	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;
+	else
+		return data[bank->bus_width - 1] | data[(2* bank->bus_width) - 1] << 8 |
+				data[(3 * bank->bus_width) - 1] << 16 | data[(4 * bank->bus_width) - 1] << 24;
+}
+
+static void cfi_intel_clear_status_register(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	uint8_t command[8];
+
+	if (target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("BUG: attempted to clear status register while target wasn't halted");
+		exit(-1);
+	}
+
+	cfi_command(bank, 0x50, command);
+	target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+}
+
+uint8_t cfi_intel_wait_status_busy(struct flash_bank *bank, int timeout)
+{
+	uint8_t status;
+
+	while ((!((status = cfi_get_u8(bank, 0, 0x0)) & 0x80)) && (timeout-- > 0))
+	{
+		LOG_DEBUG("status: 0x%x", status);
+		alive_sleep(1);
+	}
+
+	/* mask out bit 0 (reserved) */
+	status = status & 0xfe;
+
+	LOG_DEBUG("status: 0x%x", status);
+
+	if ((status & 0x80) != 0x80)
+	{
+		LOG_ERROR("timeout while waiting for WSM to become ready");
+	}
+	else if (status != 0x80)
+	{
+		LOG_ERROR("status register: 0x%x", status);
+		if (status & 0x2)
+			LOG_ERROR("Block Lock-Bit Detected, Operation Abort");
+		if (status & 0x4)
+			LOG_ERROR("Program suspended");
+		if (status & 0x8)
+			LOG_ERROR("Low Programming Voltage Detected, Operation Aborted");
+		if (status & 0x10)
+			LOG_ERROR("Program Error / Error in Setting Lock-Bit");
+		if (status & 0x20)
+			LOG_ERROR("Error in Block Erasure or Clear Lock-Bits");
+		if (status & 0x40)
+			LOG_ERROR("Block Erase Suspended");
+
+		cfi_intel_clear_status_register(bank);
+	}
+
+	return status;
+}
+
+int cfi_spansion_wait_status_busy(struct flash_bank *bank, int timeout)
+{
+	uint8_t status, oldstatus;
+	struct cfi_flash_bank *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 & cfi_info->status_poll_mask & 0x20) {
+				oldstatus = cfi_get_u8(bank, 0, 0x0);
+				status = cfi_get_u8(bank, 0, 0x0);
+				if ((status ^ oldstatus) & 0x40) {
+					LOG_ERROR("dq5 timeout, status: 0x%x", status);
+					return(ERROR_FLASH_OPERATION_FAILED);
+				} else {
+					LOG_DEBUG("status: 0x%x", status);
+					return(ERROR_OK);
+				}
+			}
+		} else { /* no toggle: finished, OK */
+			LOG_DEBUG("status: 0x%x", status);
+			return(ERROR_OK);
+		}
+
+		oldstatus = status;
+		alive_sleep(1);
+	} while (timeout-- > 0);
+
+	LOG_ERROR("timeout, status: 0x%x", status);
+
+	return(ERROR_FLASH_BUSY);
+}
+
+static int cfi_read_intel_pri_ext(struct flash_bank *bank)
+{
+	int retval;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct cfi_intel_pri_ext *pri_ext = malloc(sizeof(struct cfi_intel_pri_ext));
+	struct target *target = bank->target;
+	uint8_t command[8];
+
+	cfi_info->pri_ext = pri_ext;
+
+	pri_ext->pri[0] = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0);
+	pri_ext->pri[1] = cfi_query_u8(bank, 0, cfi_info->pri_addr + 1);
+	pri_ext->pri[2] = cfi_query_u8(bank, 0, cfi_info->pri_addr + 2);
+
+	if ((pri_ext->pri[0] != 'P') || (pri_ext->pri[1] != 'R') || (pri_ext->pri[2] != 'I'))
+	{
+		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 read bank flash bank information");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	pri_ext->major_version = cfi_query_u8(bank, 0, cfi_info->pri_addr + 3);
+	pri_ext->minor_version = cfi_query_u8(bank, 0, cfi_info->pri_addr + 4);
+
+	LOG_DEBUG("pri: '%c%c%c', version: %c.%c", pri_ext->pri[0], pri_ext->pri[1], pri_ext->pri[2], pri_ext->major_version, pri_ext->minor_version);
+
+	pri_ext->feature_support = cfi_query_u32(bank, 0, cfi_info->pri_addr + 5);
+	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%" 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);
+
+	LOG_DEBUG("Vcc opt: %1.1x.%1.1x, Vpp opt: %1.1x.%1.1x",
+		  (pri_ext->vcc_optimal & 0xf0) >> 4, pri_ext->vcc_optimal & 0x0f,
+		  (pri_ext->vpp_optimal & 0xf0) >> 4, pri_ext->vpp_optimal & 0x0f);
+
+	pri_ext->num_protection_fields = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0xe);
+	if (pri_ext->num_protection_fields != 1)
+	{
+		LOG_WARNING("expected one protection register field, but found %i", pri_ext->num_protection_fields);
+	}
+
+	pri_ext->prot_reg_addr = cfi_query_u16(bank, 0, cfi_info->pri_addr + 0xf);
+	pri_ext->fact_prot_reg_size = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0x11);
+	pri_ext->user_prot_reg_size = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0x12);
+
+	LOG_DEBUG("protection_fields: %i, prot_reg_addr: 0x%x, factory pre-programmed: %i, user programmable: %i", pri_ext->num_protection_fields, pri_ext->prot_reg_addr, 1 << pri_ext->fact_prot_reg_size, 1 << pri_ext->user_prot_reg_size);
+
+	return ERROR_OK;
+}
+
+static int cfi_read_spansion_pri_ext(struct flash_bank *bank)
+{
+	int retval;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct cfi_spansion_pri_ext *pri_ext = malloc(sizeof(struct cfi_spansion_pri_ext));
+	struct target *target = bank->target;
+	uint8_t command[8];
+
+	cfi_info->pri_ext = pri_ext;
+
+	pri_ext->pri[0] = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0);
+	pri_ext->pri[1] = cfi_query_u8(bank, 0, cfi_info->pri_addr + 1);
+	pri_ext->pri[2] = cfi_query_u8(bank, 0, cfi_info->pri_addr + 2);
+
+	if ((pri_ext->pri[0] != 'P') || (pri_ext->pri[1] != 'R') || (pri_ext->pri[2] != 'I'))
+	{
+		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("Could not read spansion bank information");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	pri_ext->major_version = cfi_query_u8(bank, 0, cfi_info->pri_addr + 3);
+	pri_ext->minor_version = cfi_query_u8(bank, 0, cfi_info->pri_addr + 4);
+
+	LOG_DEBUG("pri: '%c%c%c', version: %c.%c", pri_ext->pri[0], pri_ext->pri[1], pri_ext->pri[2], pri_ext->major_version, pri_ext->minor_version);
+
+	pri_ext->SiliconRevision = cfi_query_u8(bank, 0, cfi_info->pri_addr + 5);
+	pri_ext->EraseSuspend    = cfi_query_u8(bank, 0, cfi_info->pri_addr + 6);
+	pri_ext->BlkProt         = cfi_query_u8(bank, 0, cfi_info->pri_addr + 7);
+	pri_ext->TmpBlkUnprotect = cfi_query_u8(bank, 0, cfi_info->pri_addr + 8);
+	pri_ext->BlkProtUnprot   = cfi_query_u8(bank, 0, cfi_info->pri_addr + 9);
+	pri_ext->SimultaneousOps = cfi_query_u8(bank, 0, cfi_info->pri_addr + 10);
+	pri_ext->BurstMode       = cfi_query_u8(bank, 0, cfi_info->pri_addr + 11);
+	pri_ext->PageMode        = cfi_query_u8(bank, 0, cfi_info->pri_addr + 12);
+	pri_ext->VppMin          = cfi_query_u8(bank, 0, cfi_info->pri_addr + 13);
+	pri_ext->VppMax          = cfi_query_u8(bank, 0, cfi_info->pri_addr + 14);
+	pri_ext->TopBottom       = cfi_query_u8(bank, 0, cfi_info->pri_addr + 15);
+
+	LOG_DEBUG("Silicon Revision: 0x%x, Erase Suspend: 0x%x, Block protect: 0x%x", pri_ext->SiliconRevision,
+	      pri_ext->EraseSuspend, pri_ext->BlkProt);
+
+	LOG_DEBUG("Temporary Unprotect: 0x%x, Block Protect Scheme: 0x%x, Simultaneous Ops: 0x%x", pri_ext->TmpBlkUnprotect,
+	      pri_ext->BlkProtUnprot, pri_ext->SimultaneousOps);
+
+	LOG_DEBUG("Burst Mode: 0x%x, Page Mode: 0x%x, ", pri_ext->BurstMode, pri_ext->PageMode);
+
+
+	LOG_DEBUG("Vpp min: %2.2d.%1.1d, Vpp max: %2.2d.%1.1x",
+		  (pri_ext->VppMin & 0xf0) >> 4, pri_ext->VppMin & 0x0f,
+		  (pri_ext->VppMax & 0xf0) >> 4, pri_ext->VppMax & 0x0f);
+
+	LOG_DEBUG("WP# protection 0x%x", pri_ext->TopBottom);
+
+	/* default values for implementation specific workarounds */
+	pri_ext->_unlock1 = cfi_unlock_addresses[CFI_UNLOCK_555_2AA].unlock1;
+	pri_ext->_unlock2 = cfi_unlock_addresses[CFI_UNLOCK_555_2AA].unlock2;
+	pri_ext->_reversed_geometry = 0;
+
+	return ERROR_OK;
+}
+
+static int cfi_read_atmel_pri_ext(struct flash_bank *bank)
+{
+	int retval;
+	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));
+	struct target *target = bank->target;
+	uint8_t command[8];
+
+	/* ATMEL devices use the same CFI primary command set (0x2) as AMD/Spansion,
+	 * but a different primary extended query table.
+	 * We read the atmel table, and prepare a valid AMD/Spansion query table.
+	 */
+
+	memset(pri_ext, 0, sizeof(struct cfi_spansion_pri_ext));
+
+	cfi_info->pri_ext = pri_ext;
+
+	atmel_pri_ext.pri[0] = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0);
+	atmel_pri_ext.pri[1] = cfi_query_u8(bank, 0, cfi_info->pri_addr + 1);
+	atmel_pri_ext.pri[2] = cfi_query_u8(bank, 0, cfi_info->pri_addr + 2);
+
+	if ((atmel_pri_ext.pri[0] != 'P') || (atmel_pri_ext.pri[1] != 'R') || (atmel_pri_ext.pri[2] != 'I'))
+	{
+		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("Could not read atmel bank information");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	pri_ext->pri[0] = atmel_pri_ext.pri[0];
+	pri_ext->pri[1] = atmel_pri_ext.pri[1];
+	pri_ext->pri[2] = atmel_pri_ext.pri[2];
+
+	atmel_pri_ext.major_version = cfi_query_u8(bank, 0, cfi_info->pri_addr + 3);
+	atmel_pri_ext.minor_version = cfi_query_u8(bank, 0, cfi_info->pri_addr + 4);
+
+	LOG_DEBUG("pri: '%c%c%c', version: %c.%c", atmel_pri_ext.pri[0], atmel_pri_ext.pri[1], atmel_pri_ext.pri[2], atmel_pri_ext.major_version, atmel_pri_ext.minor_version);
+
+	pri_ext->major_version = atmel_pri_ext.major_version;
+	pri_ext->minor_version = atmel_pri_ext.minor_version;
+
+	atmel_pri_ext.features = cfi_query_u8(bank, 0, cfi_info->pri_addr + 5);
+	atmel_pri_ext.bottom_boot = cfi_query_u8(bank, 0, cfi_info->pri_addr + 6);
+	atmel_pri_ext.burst_mode = cfi_query_u8(bank, 0, cfi_info->pri_addr + 7);
+	atmel_pri_ext.page_mode = cfi_query_u8(bank, 0, cfi_info->pri_addr + 8);
+
+	LOG_DEBUG("features: 0x%2.2x, bottom_boot: 0x%2.2x, burst_mode: 0x%2.2x, page_mode: 0x%2.2x",
+		atmel_pri_ext.features, atmel_pri_ext.bottom_boot, atmel_pri_ext.burst_mode, atmel_pri_ext.page_mode);
+
+	if (atmel_pri_ext.features & 0x02)
+		pri_ext->EraseSuspend = 2;
+
+	if (atmel_pri_ext.bottom_boot)
+		pri_ext->TopBottom = 2;
+	else
+		pri_ext->TopBottom = 3;
+
+	pri_ext->_unlock1 = cfi_unlock_addresses[CFI_UNLOCK_555_2AA].unlock1;
+	pri_ext->_unlock2 = cfi_unlock_addresses[CFI_UNLOCK_555_2AA].unlock2;
+
+	return ERROR_OK;
+}
+
+static int cfi_read_0002_pri_ext(struct flash_bank *bank)
+{
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+
+	if (cfi_info->manufacturer == CFI_MFR_ATMEL)
+	{
+		return cfi_read_atmel_pri_ext(bank);
+	}
+	else
+	{
+		return cfi_read_spansion_pri_ext(bank);
+	}
+}
+
+static int cfi_spansion_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	int printed;
+	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;
+	buf_size -= printed;
+
+	printed = snprintf(buf, buf_size, "pri: '%c%c%c', version: %c.%c\n", pri_ext->pri[0],
+			   pri_ext->pri[1], pri_ext->pri[2],
+			   pri_ext->major_version, pri_ext->minor_version);
+	buf += printed;
+	buf_size -= printed;
+
+	printed = snprintf(buf, buf_size, "Silicon Rev.: 0x%x, Address Sensitive unlock: 0x%x\n",
+			   (pri_ext->SiliconRevision) >> 2,
+			   (pri_ext->SiliconRevision) & 0x03);
+	buf += printed;
+	buf_size -= printed;
+
+	printed = snprintf(buf, buf_size, "Erase Suspend: 0x%x, Sector Protect: 0x%x\n",
+			   pri_ext->EraseSuspend,
+			   pri_ext->BlkProt);
+	buf += printed;
+	buf_size -= printed;
+
+	printed = snprintf(buf, buf_size, "VppMin: %2.2d.%1.1x, VppMax: %2.2d.%1.1x\n",
+		(pri_ext->VppMin & 0xf0) >> 4, pri_ext->VppMin & 0x0f,
+		(pri_ext->VppMax & 0xf0) >> 4, pri_ext->VppMax & 0x0f);
+
+	return ERROR_OK;
+}
+
+static int cfi_intel_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	int printed;
+	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;
+	buf_size -= printed;
+
+	printed = snprintf(buf, buf_size, "pri: '%c%c%c', version: %c.%c\n", pri_ext->pri[0], pri_ext->pri[1], pri_ext->pri[2], pri_ext->major_version, pri_ext->minor_version);
+	buf += printed;
+	buf_size -= printed;
+
+	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;
+
+	printed = snprintf(buf, buf_size, "Vcc opt: %1.1x.%1.1x, Vpp opt: %1.1x.%1.1x\n",
+		(pri_ext->vcc_optimal & 0xf0) >> 4, pri_ext->vcc_optimal & 0x0f,
+		(pri_ext->vpp_optimal & 0xf0) >> 4, pri_ext->vpp_optimal & 0x0f);
+	buf += printed;
+	buf_size -= printed;
+
+	printed = snprintf(buf, buf_size, "protection_fields: %i, prot_reg_addr: 0x%x, factory pre-programmed: %i, user programmable: %i\n", pri_ext->num_protection_fields, pri_ext->prot_reg_addr, 1 << pri_ext->fact_prot_reg_size, 1 << pri_ext->user_prot_reg_size);
+
+	return ERROR_OK;
+}
+
+/* flash_bank cfi <base> <size> <chip_width> <bus_width> <target#> [options]
+ */
+FLASH_BANK_COMMAND_HANDLER(cfi_flash_bank_command)
+{
+	struct cfi_flash_bank *cfi_info;
+
+	if (CMD_ARGC < 6)
+	{
+		LOG_WARNING("incomplete flash_bank cfi configuration");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	uint16_t chip_width, bus_width;
+	COMMAND_PARSE_NUMBER(u16, CMD_ARGV[3], bus_width);
+	COMMAND_PARSE_NUMBER(u16, CMD_ARGV[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(struct cfi_flash_bank));
+	cfi_info->probed = 0;
+	bank->driver_priv = cfi_info;
+
+	cfi_info->write_algorithm = NULL;
+
+	cfi_info->x16_as_x8 = 0;
+	cfi_info->jedec_probe = 0;
+	cfi_info->not_cfi = 0;
+
+	for (unsigned i = 6; i < CMD_ARGC; i++)
+	{
+		if (strcmp(CMD_ARGV[i], "x16_as_x8") == 0)
+		{
+			cfi_info->x16_as_x8 = 1;
+		}
+		else if (strcmp(CMD_ARGV[i], "jedec_probe") == 0)
+		{
+			cfi_info->jedec_probe = 1;
+		}
+	}
+
+	cfi_info->write_algorithm = NULL;
+
+	/* bank wasn't probed yet */
+	cfi_info->qry[0] = -1;
+
+	return ERROR_OK;
+}
+
+static int cfi_intel_erase(struct flash_bank *bank, int first, int last)
+{
+	int retval;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct target *target = bank->target;
+	uint8_t command[8];
+	int i;
+
+	cfi_intel_clear_status_register(bank);
+
+	for (i = first; i <= last; i++)
+	{
+		cfi_command(bank, 0x20, 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);
+		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);
+			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%" PRIx32 , i, bank->base);
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+	}
+
+	cfi_command(bank, 0xff, command);
+	return target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+
+}
+
+static int cfi_spansion_erase(struct flash_bank *bank, int first, int last)
+{
+	int retval;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
+	struct target *target = bank->target;
+	uint8_t command[8];
+	int i;
+
+	for (i = first; i <= last; i++)
+	{
+		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;
+		}
+
+		cfi_command(bank, 0x80, 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);
+		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;
+		}
+
+		cfi_command(bank, 0x30, 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);
+			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%" PRIx32, i, bank->base);
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+	}
+
+	cfi_command(bank, 0xf0, command);
+	return target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+}
+
+static int cfi_erase(struct flash_bank *bank, int first, int last)
+{
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if ((first < 0) || (last < first) || (last >= bank->num_sectors))
+	{
+		return ERROR_FLASH_SECTOR_INVALID;
+	}
+
+	if (cfi_info->qry[0] != 'Q')
+		return ERROR_FLASH_BANK_NOT_PROBED;
+
+	switch (cfi_info->pri_id)
+	{
+		case 1:
+		case 3:
+			return cfi_intel_erase(bank, first, last);
+			break;
+		case 2:
+			return cfi_spansion_erase(bank, first, last);
+			break;
+		default:
+			LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
+			break;
+	}
+
+	return ERROR_OK;
+}
+
+static int cfi_intel_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	int retval;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct cfi_intel_pri_ext *pri_ext = cfi_info->pri_ext;
+	struct target *target = bank->target;
+	uint8_t command[8];
+	int retry = 0;
+	int i;
+
+	/* if the device supports neither legacy lock/unlock (bit 3) nor
+	 * instant individual block locking (bit 5).
+	 */
+	if (!(pri_ext->feature_support & 0x28))
+		return ERROR_FLASH_OPERATION_FAILED;
+
+	cfi_intel_clear_status_register(bank);
+
+	for (i = first; i <= last; i++)
+	{
+		cfi_command(bank, 0x60, 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.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.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;
+		}
+
+		/* instant individual block locking doesn't require reading of the status register */
+		if (!(pri_ext->feature_support & 0x20))
+		{
+			/* Clear lock bits operation may take up to 1.4s */
+			cfi_intel_wait_status_busy(bank, 1400);
+		}
+		else
+		{
+			uint8_t block_status;
+			/* read block lock bit, to verify status */
+			cfi_command(bank, 0x90, 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);
+				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)
+					return ERROR_FLASH_OPERATION_FAILED;
+				else
+				{
+					i--;
+					retry++;
+				}
+			}
+		}
+	}
+
+	/* if the device doesn't support individual block lock bits set/clear,
+	 * all blocks have been unlocked in parallel, so we set those that should be protected
+	 */
+	if ((!set) && (!(pri_ext->feature_support & 0x20)))
+	{
+		for (i = 0; i < bank->num_sectors; i++)
+		{
+			if (bank->sectors[i].is_protected == 1)
+			{
+				cfi_intel_clear_status_register(bank);
+
+				cfi_command(bank, 0x60, 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);
+				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);
+			}
+		}
+	}
+
+	cfi_command(bank, 0xff, command);
+	return target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+}
+
+static int cfi_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if ((first < 0) || (last < first) || (last >= bank->num_sectors))
+	{
+		return ERROR_FLASH_SECTOR_INVALID;
+	}
+
+	if (cfi_info->qry[0] != 'Q')
+		return ERROR_FLASH_BANK_NOT_PROBED;
+
+	switch (cfi_info->pri_id)
+	{
+		case 1:
+		case 3:
+			cfi_intel_protect(bank, set, first, last);
+			break;
+		default:
+			LOG_ERROR("protect: cfi primary command set %i unsupported", cfi_info->pri_id);
+			break;
+	}
+
+	return ERROR_OK;
+}
+
+/* FIXME Replace this by a simple memcpy() - still unsure about sideeffects */
+static void cfi_add_byte(struct flash_bank *bank, uint8_t *word, uint8_t byte)
+{
+	/* struct target *target = bank->target; */
+
+	int i;
+
+	/* 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 uint16_t or uint32_t
+	 * word (data seems to be in the upper or lower byte lane for uint16_t accesses).
+	 */
+
+#if 0
+	if (target->endianness == TARGET_LITTLE_ENDIAN)
+	{
+#endif
+		/* shift bytes */
+		for (i = 0; i < bank->bus_width - 1; i++)
+			word[i] = word[i + 1];
+		word[bank->bus_width - 1] = byte;
+#if 0
+	}
+	else
+	{
+		/* shift bytes */
+		for (i = bank->bus_width - 1; i > 0; i--)
+			word[i] = word[i - 1];
+		word[0] = byte;
+	}
+#endif
+}
+
+/* Convert code image to target endian */
+/* FIXME create general block conversion fcts in target.c?) */
+static void cfi_fix_code_endian(struct target *target, uint8_t *dest, const uint32_t *src, uint32_t count)
+{
+	uint32_t i;
+	for (i = 0; i< count; i++)
+	{
+		target_buffer_set_u32(target, dest, *src);
+		dest += 4;
+		src++;
+	}
+}
+
+static uint32_t cfi_command_val(struct flash_bank *bank, uint8_t cmd)
+{
+	struct target *target = bank->target;
+
+	uint8_t buf[CFI_MAX_BUS_WIDTH];
+	cfi_command(bank, cmd, buf);
+	switch (bank->bus_width)
+	{
+	case 1 :
+		return buf[0];
+		break;
+	case 2 :
+		return target_buffer_get_u16(target, buf);
+		break;
+	case 4 :
+		return target_buffer_get_u32(target, buf);
+		break;
+	default :
+		LOG_ERROR("Unsupported bank buswidth %d, can't do block memory writes", bank->bus_width);
+		return 0;
+	}
+}
+
+static int cfi_intel_write_block(struct flash_bank *bank, uint8_t *buffer, uint32_t address, uint32_t count)
+{
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct target *target = bank->target;
+	struct reg_param reg_params[7];
+	struct armv4_5_algorithm armv4_5_info;
+	struct working_area *source;
+	uint32_t buffer_size = 32768;
+	uint32_t write_command_val, busy_pattern_val, error_pattern_val;
+
+	/* algorithm register usage:
+	 * r0: source address (in RAM)
+	 * r1: target address (in Flash)
+	 * r2: count
+	 * r3: flash write command
+	 * r4: status byte (returned to host)
+	 * r5: busy test pattern
+	 * r6: error test pattern
+	 */
+
+	static const uint32_t word_32_code[] = {
+		0xe4904004,   /* loop:	ldr r4, [r0], #4 */
+		0xe5813000,   /* 		str r3, [r1] */
+		0xe5814000,   /* 		str r4, [r1] */
+		0xe5914000,   /* busy:  ldr r4, [r1] */
+		0xe0047005,   /*		and r7, r4, r5 */
+		0xe1570005,   /*		cmp r7, r5 */
+		0x1afffffb,   /*		bne busy */
+		0xe1140006,   /* 		tst r4, r6 */
+		0x1a000003,   /*		bne done */
+		0xe2522001,   /*		subs r2, r2, #1 */
+		0x0a000001,   /* 		beq done */
+		0xe2811004,   /*		add r1, r1 #4 */
+		0xeafffff2,   /* 		b loop */
+		0xeafffffe    /* done:	b -2 */
+	};
+
+	static const uint32_t word_16_code[] = {
+		0xe0d040b2,   /* loop:	ldrh r4, [r0], #2 */
+		0xe1c130b0,   /* 		strh r3, [r1] */
+		0xe1c140b0,   /* 		strh r4, [r1] */
+		0xe1d140b0,   /* busy	ldrh r4, [r1] */
+		0xe0047005,   /*		and r7, r4, r5 */
+		0xe1570005,   /*		cmp r7, r5 */
+		0x1afffffb,   /*		bne busy */
+		0xe1140006,   /* 		tst r4, r6 */
+		0x1a000003,   /*		bne done */
+		0xe2522001,   /*		subs r2, r2, #1 */
+		0x0a000001,   /* 		beq done */
+		0xe2811002,   /*		add r1, r1 #2 */
+		0xeafffff2,   /* 		b loop */
+		0xeafffffe    /* done:	b -2 */
+	};
+
+	static const uint32_t word_8_code[] = {
+		0xe4d04001,   /* loop:	ldrb r4, [r0], #1 */
+		0xe5c13000,   /* 		strb r3, [r1] */
+		0xe5c14000,   /* 		strb r4, [r1] */
+		0xe5d14000,   /* busy	ldrb r4, [r1] */
+		0xe0047005,   /*		and r7, r4, r5 */
+		0xe1570005,   /*		cmp r7, r5 */
+		0x1afffffb,   /*		bne busy */
+		0xe1140006,   /* 		tst r4, r6 */
+		0x1a000003,   /*		bne done */
+		0xe2522001,   /*		subs r2, r2, #1 */
+		0x0a000001,   /* 		beq done */
+		0xe2811001,   /*		add r1, r1 #1 */
+		0xeafffff2,   /* 		b loop */
+		0xeafffffe    /* done:	b -2 */
+	};
+	uint8_t target_code[4*CFI_MAX_INTEL_CODESIZE];
+	const uint32_t *target_code_src;
+	uint32_t target_code_size;
+	int retval = ERROR_OK;
+
+
+	cfi_intel_clear_status_register(bank);
+
+	armv4_5_info.common_magic = ARMV4_5_COMMON_MAGIC;
+	armv4_5_info.core_mode = ARMV4_5_MODE_SVC;
+	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 */
+
+	/* prepare algorithm code for target endian */
+	switch (bank->bus_width)
+	{
+	case 1 :
+		target_code_src = word_8_code;
+		target_code_size = sizeof(word_8_code);
+		break;
+	case 2 :
+		target_code_src = word_16_code;
+		target_code_size = sizeof(word_16_code);
+		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)
+	{
+		if (target_code_size > sizeof(target_code))
+		{
+			LOG_WARNING("Internal error - target code buffer to small. Increase CFI_MAX_INTEL_CODESIZE and recompile.");
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+		cfi_fix_code_endian(target, target_code, target_code_src, target_code_size / 4);
+
+		/* Get memory for block write handler */
+		retval = target_alloc_working_area(target, target_code_size, &cfi_info->write_algorithm);
+		if (retval != ERROR_OK)
+		{
+			LOG_WARNING("No working area available, can't do block memory writes");
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		};
+
+		/* write algorithm code to working area */
+		retval = target_write_buffer(target, cfi_info->write_algorithm->address, target_code_size, target_code);
+		if (retval != ERROR_OK)
+		{
+			LOG_ERROR("Unable to write block write code to target");
+			goto cleanup;
+		}
+	}
+
+	/* Get a workspace buffer for the data to flash starting with 32k size.
+	   Half size until buffer would be smaller 256 Bytem then fail back */
+	/* FIXME Why 256 bytes, why not 32 bytes (smallest flash write page */
+	while (target_alloc_working_area(target, buffer_size, &source) != ERROR_OK)
+	{
+		buffer_size /= 2;
+		if (buffer_size <= 256)
+		{
+			LOG_WARNING("no large enough working area available, can't do block memory writes");
+			retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+			goto cleanup;
+		}
+	};
+
+	/* setup algo registers */
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
+	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
+	init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);
+	init_reg_param(&reg_params[4], "r4", 32, PARAM_IN);
+	init_reg_param(&reg_params[5], "r5", 32, PARAM_OUT);
+	init_reg_param(&reg_params[6], "r6", 32, PARAM_OUT);
+
+	/* prepare command and status register patterns */
+	write_command_val = cfi_command_val(bank, 0x40);
+	busy_pattern_val  = cfi_command_val(bank, 0x80);
+	error_pattern_val = cfi_command_val(bank, 0x7e);
+
+	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)
+	{
+		uint32_t thisrun_count = (count > buffer_size) ? buffer_size : count;
+		uint32_t wsm_error;
+
+		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);
+		buf_set_u32(reg_params[2].value, 0, 32, thisrun_count / bank->bus_width);
+
+		buf_set_u32(reg_params[3].value, 0, 32, write_command_val);
+		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%04" PRIx32 " bytes to flash at 0x%08" PRIx32 , thisrun_count, address);
+
+		/* Execute algorithm, assume breakpoint for last instruction */
+		retval = target_run_algorithm(target, 0, NULL, 7, reg_params,
+			cfi_info->write_algorithm->address,
+			cfi_info->write_algorithm->address + target_code_size - sizeof(uint32_t),
+			10000, /* 10s should be enough for max. 32k of data */
+			&armv4_5_info);
+
+		/* On failure try a fall back to direct word writes */
+		if (retval != ERROR_OK)
+		{
+			cfi_intel_clear_status_register(bank);
+			LOG_ERROR("Execution of flash algorythm failed. Can't fall back. Please report.");
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			/* retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE; */
+			/* FIXME To allow fall back or recovery, we must save the actual status
+			   somewhere, so that a higher level code can start recovery. */
+			goto cleanup;
+		}
+
+		/* Check return value from algo code */
+		wsm_error = buf_get_u32(reg_params[4].value, 0, 32) & error_pattern_val;
+		if (wsm_error)
+		{
+			/* read status register (outputs debug inforation) */
+			cfi_intel_wait_status_busy(bank, 100);
+			cfi_intel_clear_status_register(bank);
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			goto cleanup;
+		}
+
+		buffer += thisrun_count;
+		address += thisrun_count;
+		count -= thisrun_count;
+	}
+
+	/* free up resources */
+cleanup:
+	if (source)
+		target_free_working_area(target, source);
+
+	if (cfi_info->write_algorithm)
+	{
+		target_free_working_area(target, cfi_info->write_algorithm);
+		cfi_info->write_algorithm = NULL;
+	}
+
+	destroy_reg_param(&reg_params[0]);
+	destroy_reg_param(&reg_params[1]);
+	destroy_reg_param(&reg_params[2]);
+	destroy_reg_param(&reg_params[3]);
+	destroy_reg_param(&reg_params[4]);
+	destroy_reg_param(&reg_params[5]);
+	destroy_reg_param(&reg_params[6]);
+
+	return retval;
+}
+
+static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer, uint32_t address, uint32_t count)
+{
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
+	struct target *target = bank->target;
+	struct reg_param reg_params[10];
+	struct armv4_5_algorithm armv4_5_info;
+	struct working_area *source;
+	uint32_t buffer_size = 32768;
+	uint32_t status;
+	int retval, retvaltemp;
+	int exit_code = ERROR_OK;
+
+	/* input parameters - */
+	/*	R0 = source address */
+	/*	R1 = destination address */
+	/*	R2 = number of writes */
+	/*	R3 = flash write command */
+	/*	R4 = constant to mask DQ7 bits (also used for Dq5 with shift) */
+	/* output parameters - */
+	/*	R5 = 0x80 ok 0x00 bad */
+	/* temp registers - */
+	/*	R6 = value read from flash to test status */
+	/*	R7 = holding register */
+	/* unlock registers - */
+	/*  R8 = unlock1_addr */
+	/*  R9 = unlock1_cmd */
+	/*  R10 = unlock2_addr */
+	/*  R11 = unlock2_cmd */
+
+	static const uint32_t word_32_code[] = {
+						/* 00008100 <sp_32_code>:		*/
+		0xe4905004,		/* ldr	r5, [r0], #4			*/
+		0xe5889000, 	/* str	r9, [r8]				*/
+		0xe58ab000, 	/* str	r11, [r10]				*/
+		0xe5883000, 	/* str	r3, [r8]				*/
+		0xe5815000, 	/* str	r5, [r1]				*/
+		0xe1a00000, 	/* nop							*/
+						/*								*/
+						/* 00008110 <sp_32_busy>:		*/
+		0xe5916000, 	/* ldr	r6, [r1]				*/
+		0xe0257006, 	/* eor	r7, r5, r6				*/
+		0xe0147007, 	/* ands	r7, r4, r7				*/
+		0x0a000007, 	/* beq	8140 <sp_32_cont> ; b if DQ7 == Data7 */
+		0xe0166124, 	/* ands	r6, r6, r4, lsr #2		*/
+		0x0afffff9, 	/* beq	8110 <sp_32_busy> ;	b if DQ5 low */
+		0xe5916000, 	/* ldr	r6, [r1]				*/
+		0xe0257006, 	/* eor	r7, r5, r6				*/
+		0xe0147007, 	/* ands	r7, r4, r7				*/
+		0x0a000001, 	/* beq	8140 <sp_32_cont> ; b if DQ7 == Data7 */
+		0xe3a05000, 	/* mov	r5, #0	; 0x0 - return 0x00, error */
+		0x1a000004, 	/* bne	8154 <sp_32_done>		*/
+						/*								*/
+				/* 00008140 <sp_32_cont>:				*/
+		0xe2522001, 	/* subs	r2, r2, #1	; 0x1		*/
+		0x03a05080, 	/* moveq	r5, #128	; 0x80	*/
+		0x0a000001, 	/* beq	8154 <sp_32_done>		*/
+		0xe2811004, 	/* add	r1, r1, #4	; 0x4		*/
+		0xeaffffe8, 	/* b	8100 <sp_32_code>		*/
+						/*								*/
+						/* 00008154 <sp_32_done>:		*/
+		0xeafffffe		/* b	8154 <sp_32_done>		*/
+		};
+
+		static const uint32_t word_16_code[] = {
+				/* 00008158 <sp_16_code>:              */
+		0xe0d050b2, 	/* ldrh	r5, [r0], #2		   */
+		0xe1c890b0, 	/* strh	r9, [r8]				*/
+		0xe1cab0b0, 	/* strh	r11, [r10]				*/
+		0xe1c830b0, 	/* strh	r3, [r8]				*/
+		0xe1c150b0, 	/* strh	r5, [r1]		       */
+		0xe1a00000, 	/* nop			(mov r0,r0)    */
+				/* 				       */
+				/* 00008168 <sp_16_busy>:	       */
+		0xe1d160b0, 	/* ldrh	r6, [r1]		       */
+		0xe0257006, 	/* eor	r7, r5, r6		       */
+		0xe0147007, 	/* ands	r7, r4, r7		       */
+		0x0a000007, 	/* beq	8198 <sp_16_cont>	       */
+		0xe0166124, 	/* ands	r6, r6, r4, lsr #2	       */
+		0x0afffff9, 	/* beq	8168 <sp_16_busy>	       */
+		0xe1d160b0, 	/* ldrh	r6, [r1]		       */
+		0xe0257006, 	/* eor	r7, r5, r6		       */
+		0xe0147007, 	/* ands	r7, r4, r7		       */
+		0x0a000001, 	/* beq	8198 <sp_16_cont>	       */
+		0xe3a05000, 	/* mov	r5, #0	; 0x0		       */
+		0x1a000004, 	/* bne	81ac <sp_16_done>	       */
+				/* 				       */
+				/* 00008198 <sp_16_cont>:	       */
+		0xe2522001, 	/* subs	r2, r2, #1	; 0x1	       */
+		0x03a05080, 	/* moveq	r5, #128	; 0x80 */
+		0x0a000001, 	/* beq	81ac <sp_16_done>	       */
+		0xe2811002, 	/* add	r1, r1, #2	; 0x2	       */
+		0xeaffffe8, 	/* b	8158 <sp_16_code>	       */
+				/* 				       */
+				/* 000081ac <sp_16_done>:	       */
+		0xeafffffe 	/* b	81ac <sp_16_done>              */
+		};
+
+		static const uint32_t word_16_code_dq7only[] = {
+				/* <sp_16_code>:                       */
+		0xe0d050b2, 	/* ldrh r5, [r0], #2                   */
+		0xe1c890b0, 	/* strh r9, [r8]                       */
+		0xe1cab0b0, 	/* strh	r11, [r10]				*/
+		0xe1c830b0, 	/* strh	r3, [r8]				*/
+		0xe1c150b0, 	/* strh	r5, [r1]		       */
+		0xe1a00000, 	/* nop			(mov r0,r0)    */
+				/* 				       */
+				/* <sp_16_busy>:                       */
+		0xe1d160b0, 	/* ldrh	r6, [r1]		       */
+		0xe0257006, 	/* eor	r7, r5, r6		       */
+		0xe2177080, 	/* ands	r7, #0x80                      */
+		0x1afffffb, 	/* bne	8168 <sp_16_busy>	       */
+				/* 				       */
+		0xe2522001, 	/* subs	r2, r2, #1	; 0x1	       */
+		0x03a05080, 	/* moveq	r5, #128	; 0x80 */
+		0x0a000001, 	/* beq	81ac <sp_16_done>	       */
+		0xe2811002, 	/* add	r1, r1, #2	; 0x2	       */
+		0xeafffff0, 	/* b	8158 <sp_16_code>	       */
+				/* 				       */
+				/* 000081ac <sp_16_done>:	       */
+		0xeafffffe 	/* b	81ac <sp_16_done>              */
+		};
+
+		static const uint32_t word_8_code[] = {
+				/* 000081b0 <sp_16_code_end>:          */
+		0xe4d05001, 	/* ldrb	r5, [r0], #1		       */
+		0xe5c89000, 	/* strb	r9, [r8]				*/
+		0xe5cab000, 	/* strb	r11, [r10]				*/
+		0xe5c83000, 	/* strb	r3, [r8]				*/
+		0xe5c15000, 	/* strb	r5, [r1]		       */
+		0xe1a00000, 	/* nop			(mov r0,r0)    */
+				/* 				       */
+				/* 000081c0 <sp_8_busy>:	       */
+		0xe5d16000, 	/* ldrb	r6, [r1]		       */
+		0xe0257006, 	/* eor	r7, r5, r6		       */
+		0xe0147007, 	/* ands	r7, r4, r7		       */
+		0x0a000007, 	/* beq	81f0 <sp_8_cont>	       */
+		0xe0166124, 	/* ands	r6, r6, r4, lsr #2	       */
+		0x0afffff9, 	/* beq	81c0 <sp_8_busy>	       */
+		0xe5d16000, 	/* ldrb	r6, [r1]		       */
+		0xe0257006, 	/* eor	r7, r5, r6		       */
+		0xe0147007, 	/* ands	r7, r4, r7		       */
+		0x0a000001, 	/* beq	81f0 <sp_8_cont>	       */
+		0xe3a05000, 	/* mov	r5, #0	; 0x0		       */
+		0x1a000004, 	/* bne	8204 <sp_8_done>	       */
+				/* 				       */
+				/* 000081f0 <sp_8_cont>:	       */
+		0xe2522001, 	/* subs	r2, r2, #1	; 0x1	       */
+		0x03a05080, 	/* moveq	r5, #128	; 0x80 */
+		0x0a000001, 	/* beq	8204 <sp_8_done>	       */
+		0xe2811001, 	/* add	r1, r1, #1	; 0x1	       */
+		0xeaffffe8, 	/* b	81b0 <sp_16_code_end>	       */
+				/* 				       */
+				/* 00008204 <sp_8_done>:	       */
+		0xeafffffe 	/* b	8204 <sp_8_done>               */
+	};
+
+	armv4_5_info.common_magic = ARMV4_5_COMMON_MAGIC;
+	armv4_5_info.core_mode = ARMV4_5_MODE_SVC;
+	armv4_5_info.core_state = ARMV4_5_STATE_ARM;
+
+	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;
+
+		/* convert bus-width dependent algorithm code to correct endiannes */
+		target_code = malloc(target_code_size);
+		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,
+				&cfi_info->write_algorithm);
+		if (retval != ERROR_OK)
+		{
+			free(target_code);
+			return retval;
+		}
+
+		/* write algorithm code to working area */
+		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);
+	}
+	/* the following code still assumes target code is fixed 24*4 bytes */
+
+	while (target_alloc_working_area(target, buffer_size, &source) != ERROR_OK)
+	{
+		buffer_size /= 2;
+		if (buffer_size <= 256)
+		{
+			/* if we already allocated the writing code, but failed to get a buffer, free the algorithm */
+			if (cfi_info->write_algorithm)
+				target_free_working_area(target, cfi_info->write_algorithm);
+
+			LOG_WARNING("not enough working area available, can't do block memory writes");
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+	};
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
+	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
+	init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);
+	init_reg_param(&reg_params[4], "r4", 32, PARAM_OUT);
+	init_reg_param(&reg_params[5], "r5", 32, PARAM_IN);
+	init_reg_param(&reg_params[6], "r8", 32, PARAM_OUT);
+	init_reg_param(&reg_params[7], "r9", 32, PARAM_OUT);
+	init_reg_param(&reg_params[8], "r10", 32, PARAM_OUT);
+	init_reg_param(&reg_params[9], "r11", 32, PARAM_OUT);
+
+	while (count > 0)
+	{
+		uint32_t thisrun_count = (count > buffer_size) ? buffer_size : count;
+
+		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);
+		buf_set_u32(reg_params[2].value, 0, 32, thisrun_count / bank->bus_width);
+		buf_set_u32(reg_params[3].value, 0, 32, cfi_command_val(bank, 0xA0));
+		buf_set_u32(reg_params[4].value, 0, 32, cfi_command_val(bank, 0x80));
+		buf_set_u32(reg_params[6].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock1));
+		buf_set_u32(reg_params[7].value, 0, 32, 0xaaaaaaaa);
+		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_run_algorithm(target, 0, NULL, 10, reg_params,
+						     cfi_info->write_algorithm->address,
+						     cfi_info->write_algorithm->address + ((target_code_size) - 4),
+						     10000, &armv4_5_info);
+
+		status = buf_get_u32(reg_params[5].value, 0, 32);
+
+		if ((retval != ERROR_OK) || (retvaltemp != ERROR_OK) || status != 0x80)
+		{
+			LOG_DEBUG("status: 0x%" PRIx32 , status);
+			exit_code = ERROR_FLASH_OPERATION_FAILED;
+			break;
+		}
+
+		buffer += thisrun_count;
+		address += thisrun_count;
+		count -= thisrun_count;
+	}
+
+	target_free_all_working_areas(target);
+
+	destroy_reg_param(&reg_params[0]);
+	destroy_reg_param(&reg_params[1]);
+	destroy_reg_param(&reg_params[2]);
+	destroy_reg_param(&reg_params[3]);
+	destroy_reg_param(&reg_params[4]);
+	destroy_reg_param(&reg_params[5]);
+	destroy_reg_param(&reg_params[6]);
+	destroy_reg_param(&reg_params[7]);
+	destroy_reg_param(&reg_params[8]);
+	destroy_reg_param(&reg_params[9]);
+
+	return exit_code;
+}
+
+static int cfi_intel_write_word(struct flash_bank *bank, uint8_t *word, uint32_t address)
+{
+	int retval;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct target *target = bank->target;
+	uint8_t command[8];
+
+	cfi_intel_clear_status_register(bank);
+	cfi_command(bank, 0x40, 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, 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);
+		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%" PRIx32 ", address %" PRIx32 , bank->base, address);
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	return ERROR_OK;
+}
+
+static int cfi_intel_write_words(struct flash_bank *bank, uint8_t *word, uint32_t wordcount, uint32_t address)
+{
+	int retval;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct target *target = bank->target;
+	uint8_t command[8];
+
+	/* 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;
+	}
+
+	/* Write to flash buffer */
+	cfi_intel_clear_status_register(bank);
+
+	/* Initiate buffer operation _*/
+	cfi_command(bank, 0xE8, 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);
+		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%" PRIx32 ", address %" PRIx32 , bank->base, address);
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	/* 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, 0xd0, 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);
+		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%" PRIx32 ", address %" PRIx32 " failed.", bank->base, address);
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	return ERROR_OK;
+}
+
+static int cfi_spansion_write_word(struct flash_bank *bank, uint8_t *word, uint32_t address)
+{
+	int retval;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
+	struct target *target = bank->target;
+	uint8_t command[8];
+
+	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;
+	}
+
+	cfi_command(bank, 0xa0, command);
+	if ((retval = target_write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command)) != ERROR_OK)
+	{
+		return retval;
+	}
+
+	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);
+		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%" PRIx32 ", address %" PRIx32 , bank->base, address);
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	return ERROR_OK;
+}
+
+static int cfi_spansion_write_words(struct flash_bank *bank, uint8_t *word, uint32_t wordcount, uint32_t address)
+{
+	int retval;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct target *target = bank->target;
+	uint8_t command[8];
+	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);
+	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 *bank, uint8_t *word, uint32_t address)
+{
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+
+	switch (cfi_info->pri_id)
+	{
+		case 1:
+		case 3:
+			return cfi_intel_write_word(bank, word, address);
+			break;
+		case 2:
+			return cfi_spansion_write_word(bank, word, address);
+			break;
+		default:
+			LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
+			break;
+	}
+
+	return ERROR_FLASH_OPERATION_FAILED;
+}
+
+static int cfi_write_words(struct flash_bank *bank, uint8_t *word, uint32_t wordcount, uint32_t address)
+{
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+
+	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, wordcount, address);
+			break;
+		default:
+			LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
+			break;
+	}
+
+	return ERROR_FLASH_OPERATION_FAILED;
+}
+
+int cfi_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct target *target = bank->target;
+	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 */
+	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;
+
+	if (cfi_info->qry[0] != 'Q')
+		return ERROR_FLASH_BANK_NOT_PROBED;
+
+	/* start at the first byte of the first word (bus_width size) */
+	write_p = address & ~(bank->bus_width - 1);
+	if ((align = address - write_p) != 0)
+	{
+		LOG_INFO("Fixup %d unaligned head bytes", align);
+
+		for (i = 0; i < bank->bus_width; i++)
+			current_word[i] = 0;
+		copy_p = write_p;
+
+		/* copy bytes before the first write address */
+		for (i = 0; i < align; ++i, ++copy_p)
+		{
+			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);
+		}
+
+		/* add bytes from the buffer */
+		for (; (i < bank->bus_width) && (count > 0); i++)
+		{
+			cfi_add_byte(bank, current_word, *buffer++);
+			count--;
+			copy_p++;
+		}
+
+		/* if the buffer is already finished, copy bytes after the last write address */
+		for (; (count == 0) && (i < bank->bus_width); ++i, ++copy_p)
+		{
+			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);
+		if (retval != ERROR_OK)
+			return retval;
+		write_p = copy_p;
+	}
+
+	/* handle blocks of bus_size aligned bytes */
+	blk_count = count & ~(bank->bus_width - 1); /* round down, leave tail bytes */
+	switch (cfi_info->pri_id)
+	{
+		/* try block writes (fails without working area) */
+		case 1:
+		case 3:
+			retval = cfi_intel_write_block(bank, buffer, write_p, blk_count);
+			break;
+		case 2:
+			retval = cfi_spansion_write_block(bank, buffer, write_p, blk_count);
+			break;
+		default:
+			LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			break;
+	}
+	if (retval == ERROR_OK)
+	{
+		/* Increment pointers and decrease count on succesful block write */
+		buffer += blk_count;
+		write_p += blk_count;
+		count -= blk_count;
+	}
+	else
+	{
+		if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
+		{
+			//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)
+			{
+			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);
+
+			/* fall back to memory writes */
+			while (count >= (uint32_t)bank->bus_width)
+			{
+				int fallback;
+				if ((write_p & 0xff) == 0)
+				{
+					LOG_INFO("Programming at %08" PRIx32 ", count %08" PRIx32 " bytes remaining", write_p, count);
+				}
+				fallback = 1;
+				if ((bufferwsize > 0) && (count >= buffersize) && !(write_p & buffermask))
+				{
+					retval = cfi_write_words(bank, buffer, bufferwsize, write_p);
+					if (retval == ERROR_OK)
+					{
+						buffer += buffersize;
+						write_p += buffersize;
+						count -= buffersize;
+						fallback = 0;
+					}
+				}
+				/* try the slow way? */
+				if (fallback)
+				{
+					for (i = 0; i < bank->bus_width; i++)
+						current_word[i] = 0;
+
+					for (i = 0; i < bank->bus_width; i++)
+					{
+						cfi_add_byte(bank, current_word, *buffer++);
+					}
+
+					retval = cfi_write_word(bank, current_word, write_p);
+					if (retval != ERROR_OK)
+						return retval;
+
+					write_p += bank->bus_width;
+					count -= bank->bus_width;
+				}
+			}
+		}
+		else
+			return retval;
+	}
+
+	/* return to read array mode, so we can read from flash again for padding */
+	cfi_command(bank, 0xf0, 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);
+	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 %" PRId32 " unaligned tail bytes", count);
+
+		copy_p = write_p;
+		for (i = 0; i < bank->bus_width; i++)
+			current_word[i] = 0;
+
+		for (i = 0; (i < bank->bus_width) && (count > 0); ++i, ++copy_p)
+		{
+			cfi_add_byte(bank, current_word, *buffer++);
+			count--;
+		}
+		for (; i < bank->bus_width; ++i, ++copy_p)
+		{
+			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);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	/* return to read array mode */
+	cfi_command(bank, 0xf0, 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);
+	return target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, current_word);
+}
+
+static void cfi_fixup_atmel_reversed_erase_regions(struct flash_bank *bank, void *param)
+{
+	(void) param;
+	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;
+}
+
+static void cfi_fixup_0002_erase_regions(struct flash_bank *bank, void *param)
+{
+	int i;
+	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))
+	{
+		LOG_DEBUG("swapping reversed erase region information on cmdset 0002 device");
+
+		for (i = 0; i < cfi_info->num_erase_regions / 2; i++)
+		{
+			int j = (cfi_info->num_erase_regions - 1) - i;
+			uint32_t swap;
+
+			swap = cfi_info->erase_region_info[i];
+			cfi_info->erase_region_info[i] = cfi_info->erase_region_info[j];
+			cfi_info->erase_region_info[j] = swap;
+		}
+	}
+}
+
+static void cfi_fixup_0002_unlock_addresses(struct flash_bank *bank, void *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 *bank, int address)
+{
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct target *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 *bank)
+{
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct target *target = bank->target;
+	uint8_t command[8];
+	int num_sectors = 0;
+	int i;
+	int sector = 0;
+	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;
+	}
+
+	cfi_info->probed = 0;
+
+	/* JEDEC standard JESD21C uses 0x5555 and 0x2aaa as unlock addresses,
+	 * while CFI compatible AMD/Spansion flashes use 0x555 and 0x2aa
+	 */
+	if (cfi_info->jedec_probe)
+	{
+		unlock1 = 0x5555;
+		unlock2 = 0x2aaa;
+	}
+
+	/* switch to read identifier codes mode ("AUTOSELECT") */
+	cfi_command(bank, 0xaa, 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);
+	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);
+	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)
+	{
+		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)
+	{
+		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, 0x01), &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);
+	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);
+	if ((retval = target_write_memory(target, flash_address(bank, 0, 0x00), bank->bus_width, 1, command)) != ERROR_OK)
+	{
+		return retval;
+	}
+
+	/* 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
+	 */
+	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
+		 * the device into CFI query mode.
+		 *
+		 * SST flashes clearly violate this, and we will consider them incompatbile for now
+		 */
+
+		retval = cfi_query_string(bank, 0x55);
+		if (retval != ERROR_OK)
+		{
+			/*
+			 * 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);
+		cfi_info->alt_id = cfi_query_u16(bank, 0, 0x17);
+		cfi_info->alt_addr = cfi_query_u16(bank, 0, 0x19);
+
+		LOG_DEBUG("qry: '%c%c%c', pri_id: 0x%4.4x, pri_addr: 0x%4.4x, alt_id: 0x%4.4x, alt_addr: 0x%4.4x", cfi_info->qry[0], cfi_info->qry[1], cfi_info->qry[2], cfi_info->pri_id, cfi_info->pri_addr, cfi_info->alt_id, cfi_info->alt_addr);
+
+		cfi_info->vcc_min = cfi_query_u8(bank, 0, 0x1b);
+		cfi_info->vcc_max = cfi_query_u8(bank, 0, 0x1c);
+		cfi_info->vpp_min = cfi_query_u8(bank, 0, 0x1d);
+		cfi_info->vpp_max = cfi_query_u8(bank, 0, 0x1e);
+		cfi_info->word_write_timeout_typ = cfi_query_u8(bank, 0, 0x1f);
+		cfi_info->buf_write_timeout_typ = cfi_query_u8(bank, 0, 0x20);
+		cfi_info->block_erase_timeout_typ = cfi_query_u8(bank, 0, 0x21);
+		cfi_info->chip_erase_timeout_typ = cfi_query_u8(bank, 0, 0x22);
+		cfi_info->word_write_timeout_max = cfi_query_u8(bank, 0, 0x23);
+		cfi_info->buf_write_timeout_max = cfi_query_u8(bank, 0, 0x24);
+		cfi_info->block_erase_timeout_max = cfi_query_u8(bank, 0, 0x25);
+		cfi_info->chip_erase_timeout_max = cfi_query_u8(bank, 0, 0x26);
+
+		LOG_DEBUG("Vcc min: %1.1x.%1.1x, Vcc max: %1.1x.%1.1x, Vpp min: %1.1x.%1.1x, Vpp max: %1.1x.%1.1x",
+			(cfi_info->vcc_min & 0xf0) >> 4, cfi_info->vcc_min & 0x0f,
+			(cfi_info->vcc_max & 0xf0) >> 4, cfi_info->vcc_max & 0x0f,
+			(cfi_info->vpp_min & 0xf0) >> 4, cfi_info->vpp_min & 0x0f,
+			(cfi_info->vpp_max & 0xf0) >> 4, cfi_info->vpp_max & 0x0f);
+		LOG_DEBUG("typ. word write timeout: %u, typ. buf write timeout: %u, typ. block erase timeout: %u, typ. chip erase timeout: %u", 1 << cfi_info->word_write_timeout_typ, 1 << cfi_info->buf_write_timeout_typ,
+			1 << cfi_info->block_erase_timeout_typ, 1 << cfi_info->chip_erase_timeout_typ);
+		LOG_DEBUG("max. word write timeout: %u, max. buf write timeout: %u, max. block erase timeout: %u, max. chip erase timeout: %u", (1 << cfi_info->word_write_timeout_max) * (1 << cfi_info->word_write_timeout_typ),
+			(1 << cfi_info->buf_write_timeout_max) * (1 << cfi_info->buf_write_timeout_typ),
+			(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 = 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%" 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)
+		{
+			cfi_info->erase_region_info = malloc(4 * cfi_info->num_erase_regions);
+			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]: %" PRIu32 " blocks of size 0x%" PRIx32 "",
+					  i,
+					  (cfi_info->erase_region_info[i] & 0xffff) + 1,
+					  (cfi_info->erase_region_info[i] >> 16) * 256);
+			}
+		}
+		else
+		{
+			cfi_info->erase_region_info = NULL;
+		}
+
+		/* 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)
+		{
+			/* Intel command set (standard and extended) */
+			case 0x0001:
+			case 0x0003:
+				cfi_read_intel_pri_ext(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:
+				LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
+				break;
+		}
+
+		/* return to read array mode
+		 * we use both reset commands, as some Intel flashes fail to recognize the 0xF0 command
+		 */
+		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;
+		}
+	} /* end CFI case */
+
+	/* apply fixups depending on the primary command set */
+	switch (cfi_info->pri_id)
+	{
+		/* Intel command set (standard and extended) */
+		case 0x0001:
+		case 0x0003:
+			cfi_fixup(bank, cfi_0001_fixups);
+			break;
+		/* AMD/Spansion, Atmel, ... command set */
+		case 0x0002:
+			cfi_fixup(bank, cfi_0002_fixups);
+			break;
+		default:
+			LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
+			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 */
+		bank->num_sectors = 1;
+		bank->sectors = malloc(sizeof(struct flash_sector));
+
+		bank->sectors[sector].offset = 0x0;
+		bank->sectors[sector].size = bank->size;
+		bank->sectors[sector].is_erased = -1;
+		bank->sectors[sector].is_protected = -1;
+	}
+	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;
+		}
+
+		bank->num_sectors = num_sectors;
+		bank->sectors = malloc(sizeof(struct flash_sector) * num_sectors);
+
+		for (i = 0; i < cfi_info->num_erase_regions; i++)
+		{
+			uint32_t j;
+			for (j = 0; j < (cfi_info->erase_region_info[i] & 0xffff) + 1; j++)
+			{
+				bank->sectors[sector].offset = offset;
+				bank->sectors[sector].size = ((cfi_info->erase_region_info[i] >> 16) * 256) * bank->bus_width / bank->chip_width;
+				offset += bank->sectors[sector].size;
+				bank->sectors[sector].is_erased = -1;
+				bank->sectors[sector].is_protected = -1;
+				sector++;
+			}
+		}
+		if (offset != (cfi_info->dev_size * bank->bus_width / bank->chip_width))
+		{
+			LOG_WARNING("CFI size is 0x%" PRIx32 ", but total sector size is 0x%" PRIx32 "", \
+				(cfi_info->dev_size * bank->bus_width / bank->chip_width), offset);
+		}
+	}
+
+	cfi_info->probed = 1;
+
+	return ERROR_OK;
+}
+
+static int cfi_auto_probe(struct flash_bank *bank)
+{
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	if (cfi_info->probed)
+		return ERROR_OK;
+	return cfi_probe(bank);
+}
+
+
+static int cfi_intel_protect_check(struct flash_bank *bank)
+{
+	int retval;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct cfi_intel_pri_ext *pri_ext = cfi_info->pri_ext;
+	struct target *target = bank->target;
+	uint8_t command[CFI_MAX_BUS_WIDTH];
+	int i;
+
+	/* check if block lock bits are supported on this device */
+	if (!(pri_ext->blk_status_reg_mask & 0x1))
+		return ERROR_FLASH_OPERATION_FAILED;
+
+	cfi_command(bank, 0x90, 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++)
+	{
+		uint8_t block_status = cfi_get_u8(bank, i, 0x2);
+
+		if (block_status & 1)
+			bank->sectors[i].is_protected = 1;
+		else
+			bank->sectors[i].is_protected = 0;
+	}
+
+	cfi_command(bank, 0xff, command);
+	return target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+}
+
+static int cfi_spansion_protect_check(struct flash_bank *bank)
+{
+	int retval;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
+	struct target *target = bank->target;
+	uint8_t command[8];
+	int i;
+
+	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;
+	}
+
+	cfi_command(bank, 0x90, 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++)
+	{
+		uint8_t block_status = cfi_get_u8(bank, i, 0x2);
+
+		if (block_status & 1)
+			bank->sectors[i].is_protected = 1;
+		else
+			bank->sectors[i].is_protected = 0;
+	}
+
+	cfi_command(bank, 0xf0, command);
+	return target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+}
+
+static int cfi_protect_check(struct flash_bank *bank)
+{
+	struct cfi_flash_bank *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)
+	{
+		case 1:
+		case 3:
+			return cfi_intel_protect_check(bank);
+			break;
+		case 2:
+			return cfi_spansion_protect_check(bank);
+			break;
+		default:
+			LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
+			break;
+	}
+
+	return ERROR_OK;
+}
+
+static int cfi_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	int printed;
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+
+	if (cfi_info->qry[0] == (char)-1)
+	{
+		printed = snprintf(buf, buf_size, "\ncfi flash bank not probed yet\n");
+		return ERROR_OK;
+	}
+
+	if (cfi_info->not_cfi == 0)
+		printed = snprintf(buf, buf_size, "\ncfi information:\n");
+	else
+		printed = snprintf(buf, buf_size, "\nnon-cfi flash:\n");
+	buf += printed;
+	buf_size -= printed;
+
+	printed = snprintf(buf, buf_size, "\nmfr: 0x%4.4x, id:0x%4.4x\n",
+		cfi_info->manufacturer, cfi_info->device_id);
+	buf += printed;
+	buf_size -= printed;
+
+	if (cfi_info->not_cfi == 0)
+	{
+	printed = snprintf(buf, buf_size, "qry: '%c%c%c', pri_id: 0x%4.4x, pri_addr: 0x%4.4x, alt_id: 0x%4.4x, alt_addr: 0x%4.4x\n", cfi_info->qry[0], cfi_info->qry[1], cfi_info->qry[2], cfi_info->pri_id, cfi_info->pri_addr, cfi_info->alt_id, cfi_info->alt_addr);
+	buf += printed;
+	buf_size -= printed;
+
+		printed = snprintf(buf, buf_size, "Vcc min: %1.1x.%1.1x, Vcc max: %1.1x.%1.1x, Vpp min: %1.1x.%1.1x, Vpp max: %1.1x.%1.1x\n",
+		                   (cfi_info->vcc_min & 0xf0) >> 4, cfi_info->vcc_min & 0x0f,
+	(cfi_info->vcc_max & 0xf0) >> 4, cfi_info->vcc_max & 0x0f,
+	(cfi_info->vpp_min & 0xf0) >> 4, cfi_info->vpp_min & 0x0f,
+	(cfi_info->vpp_max & 0xf0) >> 4, cfi_info->vpp_max & 0x0f);
+	buf += printed;
+	buf_size -= printed;
+
+		printed = snprintf(buf, buf_size, "typ. word write timeout: %u, typ. buf write timeout: %u, typ. block erase timeout: %u, typ. chip erase timeout: %u\n",
+		                   1 << cfi_info->word_write_timeout_typ,
+		                   1 << cfi_info->buf_write_timeout_typ,
+		                   1 << cfi_info->block_erase_timeout_typ,
+		                   1 << cfi_info->chip_erase_timeout_typ);
+	buf += printed;
+	buf_size -= printed;
+
+		printed = snprintf(buf, buf_size, "max. word write timeout: %u, max. buf write timeout: %u, max. block erase timeout: %u, max. chip erase timeout: %u\n",
+		                   (1 << cfi_info->word_write_timeout_max) * (1 << cfi_info->word_write_timeout_typ),
+		  (1 << cfi_info->buf_write_timeout_max) * (1 << cfi_info->buf_write_timeout_typ),
+		  (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));
+	buf += printed;
+	buf_size -= printed;
+
+		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,
+		                   1 << cfi_info->max_buf_write_size);
+	buf += printed;
+	buf_size -= printed;
+
+	switch (cfi_info->pri_id)
+	{
+		case 1:
+		case 3:
+			cfi_intel_info(bank, buf, buf_size);
+			break;
+		case 2:
+			cfi_spansion_info(bank, buf, buf_size);
+			break;
+		default:
+			LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
+			break;
+	}
+	}
+
+	return ERROR_OK;
+}
+
+struct flash_driver cfi_flash = {
+		.name = "cfi",
+		.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,
+	};
diff --git a/src/flash/nor/cfi.h b/src/flash/nor/cfi.h
new file mode 100644
index 00000000..d55fd34e
--- /dev/null
+++ b/src/flash/nor/cfi.h
@@ -0,0 +1,164 @@
+/***************************************************************************
+ *   Copyright (C) 2005 by Dominic Rath                                    *
+ *   Dominic.Rath@gmx.de                                                   *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifndef CFI_H
+#define CFI_H
+
+#include "flash.h"
+
+#define CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7 0xE0 /* DQ5..DQ7 */
+#define CFI_STATUS_POLL_MASK_DQ6_DQ7     0xC0 /* DQ6..DQ7 */
+
+struct cfi_flash_bank
+{
+	struct working_area *write_algorithm;
+
+	int x16_as_x8;
+	int jedec_probe;
+	int not_cfi;
+	int probed;
+
+	uint16_t manufacturer;
+	uint16_t device_id;
+
+	char qry[3];
+
+	/* identification string */
+	uint16_t pri_id;
+	uint16_t pri_addr;
+	uint16_t alt_id;
+	uint16_t alt_addr;
+
+	/* device-system interface */
+	uint8_t vcc_min;
+	uint8_t vcc_max;
+	uint8_t vpp_min;
+	uint8_t vpp_max;
+	uint8_t word_write_timeout_typ;
+	uint8_t buf_write_timeout_typ;
+	uint8_t block_erase_timeout_typ;
+	uint8_t chip_erase_timeout_typ;
+	uint8_t word_write_timeout_max;
+	uint8_t buf_write_timeout_max;
+	uint8_t block_erase_timeout_max;
+	uint8_t chip_erase_timeout_max;
+
+	uint8_t status_poll_mask;
+
+	/* flash geometry */
+	uint32_t dev_size;
+	uint16_t interface_desc;
+	uint16_t max_buf_write_size;
+	uint8_t num_erase_regions;
+	uint32_t *erase_region_info;
+
+	void *pri_ext;
+	void *alt_ext;
+};
+
+/* Intel primary extended query table
+ * as defined for the Advanced+ Boot Block Flash Memory (C3)
+ * and used by the linux kernel cfi driver (as of 2.6.14)
+ */
+struct cfi_intel_pri_ext
+{
+	char pri[3];
+	uint8_t major_version;
+	uint8_t minor_version;
+	uint32_t feature_support;
+	uint8_t suspend_cmd_support;
+	uint16_t blk_status_reg_mask;
+	uint8_t vcc_optimal;
+	uint8_t vpp_optimal;
+	uint8_t num_protection_fields;
+	uint16_t prot_reg_addr;
+	uint8_t fact_prot_reg_size;
+	uint8_t user_prot_reg_size;
+	uint8_t extra[0];
+};
+
+/* Spansion primary extended query table as defined for and used by
+ * the linux kernel cfi driver (as of 2.6.15)
+ */
+struct cfi_spansion_pri_ext
+{
+	uint8_t  pri[3];
+	uint8_t  major_version;
+	uint8_t  minor_version;
+	uint8_t  SiliconRevision; /* bits 1-0: Address Sensitive Unlock */
+	uint8_t  EraseSuspend;
+	uint8_t  BlkProt;
+	uint8_t  TmpBlkUnprotect;
+	uint8_t  BlkProtUnprot;
+	uint8_t  SimultaneousOps;
+	uint8_t  BurstMode;
+	uint8_t  PageMode;
+	uint8_t  VppMin;
+	uint8_t  VppMax;
+	uint8_t  TopBottom;
+	int _reversed_geometry;
+	uint32_t _unlock1;
+	uint32_t _unlock2;
+};
+
+/* Atmel primary extended query table as defined for and used by
+ * the linux kernel cfi driver (as of 2.6.20+)
+ */
+struct cfi_atmel_pri_ext
+{
+	uint8_t pri[3];
+	uint8_t major_version;
+	uint8_t minor_version;
+	uint8_t features;
+	uint8_t bottom_boot;
+	uint8_t burst_mode;
+	uint8_t page_mode;
+};
+
+enum {
+	CFI_UNLOCK_555_2AA,
+	CFI_UNLOCK_5555_2AAA,
+};
+
+struct cfi_unlock_addresses
+{
+	uint32_t unlock1;
+	uint32_t unlock2;
+};
+
+struct cfi_fixup
+{
+	uint16_t mfr;
+	uint16_t id;
+	void (*fixup)(struct flash_bank *flash, void *param);
+	void *param;
+};
+
+#define CFI_MFR_AMD		0x0001
+#define CFI_MFR_FUJITSU	0x0004
+#define CFI_MFR_ATMEL	0x001F
+#define CFI_MFR_ST		0x0020	/* STMicroelectronics */
+#define CFI_MFR_AMIC	0x0037
+#define CFI_MFR_SST		0x00BF
+#define CFI_MFR_MX		0x00C2
+
+#define CFI_MFR_ANY		0xffff
+#define CFI_ID_ANY		0xffff
+
+#endif /* CFI_H */
diff --git a/src/flash/nor/ecos.c b/src/flash/nor/ecos.c
new file mode 100644
index 00000000..7a0b26f3
--- /dev/null
+++ b/src/flash/nor/ecos.c
@@ -0,0 +1,444 @@
+/***************************************************************************
+ *   Copyright (C) 2007,2008 Øyvind Harboe                                 *
+ *   oyvind.harboe@zylin.com                                               *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "flash.h"
+#include "embeddedice.h"
+#include "image.h"
+#include "algorithm.h"
+
+
+#if 0
+static uint32_t ecosflash_get_flash_status(struct flash_bank *bank);
+static void ecosflash_set_flash_mode(struct flash_bank *bank,int mode);
+static uint32_t ecosflash_wait_status_busy(struct flash_bank *bank, uint32_t waitbits, int timeout);
+static int ecosflash_handle_gpnvm_command(struct command_context *cmd_ctx, char *cmd, char **args, int argc);
+#endif
+
+struct ecosflash_flash_bank
+{
+	struct target *target;
+	struct working_area *write_algorithm;
+	struct working_area *erase_check_algorithm;
+	char *driverPath;
+	uint32_t start_address;
+};
+
+static const int sectorSize = 0x10000;
+
+char *
+flash_errmsg(int err);
+
+#ifndef __ECOS
+#define FLASH_ERR_OK              0x00  /* No error - operation complete */
+#define FLASH_ERR_INVALID         0x01  /* Invalid FLASH address */
+#define FLASH_ERR_ERASE           0x02  /* Error trying to erase */
+#define FLASH_ERR_LOCK            0x03  /* Error trying to lock/unlock */
+#define FLASH_ERR_PROGRAM         0x04  /* Error trying to program */
+#define FLASH_ERR_PROTOCOL        0x05  /* Generic error */
+#define FLASH_ERR_PROTECT         0x06  /* Device/region is write-protected */
+#define FLASH_ERR_NOT_INIT        0x07  /* FLASH info not yet initialized */
+#define FLASH_ERR_HWR             0x08  /* Hardware (configuration?) problem */
+#define FLASH_ERR_ERASE_SUSPEND   0x09  /* Device is in erase suspend mode */
+#define FLASH_ERR_PROGRAM_SUSPEND 0x0a  /* Device is in in program suspend mode */
+#define FLASH_ERR_DRV_VERIFY      0x0b  /* Driver failed to verify data */
+#define FLASH_ERR_DRV_TIMEOUT     0x0c  /* Driver timed out waiting for device */
+#define FLASH_ERR_DRV_WRONG_PART  0x0d  /* Driver does not support device */
+#define FLASH_ERR_LOW_VOLTAGE     0x0e  /* Not enough juice to complete job */
+
+char *
+flash_errmsg(int err)
+{
+	switch (err) {
+	case FLASH_ERR_OK:
+		return "No error - operation complete";
+	case FLASH_ERR_ERASE_SUSPEND:
+		return "Device is in erase suspend state";
+	case FLASH_ERR_PROGRAM_SUSPEND:
+		return "Device is in program suspend state";
+	case FLASH_ERR_INVALID:
+		return "Invalid FLASH address";
+	case FLASH_ERR_ERASE:
+		return "Error trying to erase";
+	case FLASH_ERR_LOCK:
+		return "Error trying to lock/unlock";
+	case FLASH_ERR_PROGRAM:
+		return "Error trying to program";
+	case FLASH_ERR_PROTOCOL:
+		return "Generic error";
+	case FLASH_ERR_PROTECT:
+		return "Device/region is write-protected";
+	case FLASH_ERR_NOT_INIT:
+		return "FLASH sub-system not initialized";
+	case FLASH_ERR_DRV_VERIFY:
+		return "Data verify failed after operation";
+	case FLASH_ERR_DRV_TIMEOUT:
+		return "Driver timed out waiting for device";
+	case FLASH_ERR_DRV_WRONG_PART:
+		return "Driver does not support device";
+	case FLASH_ERR_LOW_VOLTAGE:
+		return "Device reports low voltage";
+	default:
+		return "Unknown error";
+	}
+}
+#endif
+
+/* flash bank ecosflash <base> <size> <chip_width> <bus_width> <target#> <driverPath>
+ */
+FLASH_BANK_COMMAND_HANDLER(ecosflash_flash_bank_command)
+{
+	struct ecosflash_flash_bank *info;
+
+	if (CMD_ARGC < 7)
+	{
+		LOG_WARNING("incomplete flash_bank ecosflash configuration");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	info = malloc(sizeof(struct ecosflash_flash_bank));
+	if (info == NULL)
+	{
+		LOG_ERROR("no memory for flash bank info");
+		exit(-1);
+	}
+	bank->driver_priv = info;
+	info->driverPath = strdup(CMD_ARGV[6]);
+
+	/* eCos flash sector sizes are not exposed to OpenOCD, use 0x10000 as
+	 * a way to improve impedance match between OpenOCD and eCos flash
+	 * driver.
+	 */
+	int i = 0;
+	uint32_t offset = 0;
+	bank->num_sectors = bank->size/sectorSize;
+	bank->sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
+	for (i = 0; i < bank->num_sectors; i++)
+	{
+		bank->sectors[i].offset = offset;
+		bank->sectors[i].size = sectorSize;
+		offset += bank->sectors[i].size;
+		bank->sectors[i].is_erased = -1;
+		bank->sectors[i].is_protected = 0;
+	}
+
+	info->target = get_target(CMD_ARGV[5]);
+	if (info->target == NULL)
+	{
+		LOG_ERROR("target '%s' not defined", CMD_ARGV[5]);
+		return ERROR_FAIL;
+	}
+	return ERROR_OK;
+}
+
+static int loadDriver(struct ecosflash_flash_bank *info)
+{
+	size_t buf_cnt;
+	size_t image_size;
+	struct image image;
+
+	image.base_address_set = 0;
+	image.start_address_set = 0;
+	struct target *target = info->target;
+	int retval;
+
+	if ((retval = image_open(&image, info->driverPath, NULL)) != ERROR_OK)
+	{
+		return retval;
+	}
+
+	info->start_address = image.start_address;
+
+	image_size = 0x0;
+	int i;
+	for (i = 0; i < image.num_sections; i++)
+	{
+		void *buffer = malloc(image.sections[i].size);
+		int retval;
+		if ((retval = image_read_section(&image, i, 0x0, image.sections[i].size, buffer, &buf_cnt)) != ERROR_OK)
+		{
+			free(buffer);
+			image_close(&image);
+			return retval;
+		}
+		target_write_buffer(target, image.sections[i].base_address, buf_cnt, buffer);
+		image_size += buf_cnt;
+		LOG_DEBUG("%zu bytes written at address 0x%8.8" PRIx32 "",
+				buf_cnt, image.sections[i].base_address);
+
+		free(buffer);
+	}
+
+	image_close(&image);
+
+	return ERROR_OK;
+}
+
+static int const OFFSET_ERASE = 0x0;
+static int const OFFSET_ERASE_SIZE = 0x8;
+static int const OFFSET_FLASH = 0xc;
+static int const OFFSET_FLASH_SIZE = 0x8;
+static int const OFFSET_GET_WORKAREA = 0x18;
+static int const OFFSET_GET_WORKAREA_SIZE = 0x4;
+
+static int runCode(struct ecosflash_flash_bank *info,
+		uint32_t codeStart, uint32_t codeStop, uint32_t r0, uint32_t r1, uint32_t r2,
+		uint32_t *result,
+		/* timeout in ms */
+		int timeout)
+{
+	struct target *target = info->target;
+
+	struct reg_param reg_params[3];
+	struct armv4_5_algorithm armv4_5_info;
+	armv4_5_info.common_magic = ARMV4_5_COMMON_MAGIC;
+	armv4_5_info.core_mode = ARMV4_5_MODE_SVC;
+	armv4_5_info.core_state = ARMV4_5_STATE_ARM;
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT);
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
+	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
+
+	buf_set_u32(reg_params[0].value, 0, 32, r0);
+	buf_set_u32(reg_params[1].value, 0, 32, r1);
+	buf_set_u32(reg_params[2].value, 0, 32, r2);
+
+	int retval;
+	if ((retval = target_run_algorithm(target, 0, NULL, 3, reg_params,
+			codeStart,
+			codeStop, timeout,
+			&armv4_5_info)) != ERROR_OK)
+	{
+		LOG_ERROR("error executing eCos flash algorithm");
+		return retval;
+	}
+
+	*result = buf_get_u32(reg_params[0].value, 0, 32);
+
+	destroy_reg_param(&reg_params[0]);
+	destroy_reg_param(&reg_params[1]);
+	destroy_reg_param(&reg_params[2]);
+
+	return ERROR_OK;
+}
+
+static int eCosBoard_erase(struct ecosflash_flash_bank *info, uint32_t address, uint32_t len)
+{
+	int retval;
+	int timeout = (len / 20480 + 1) * 1000; /*asume 20 KB/s*/
+
+	retval = loadDriver(info);
+	if (retval != ERROR_OK)
+		return retval;
+
+	uint32_t flashErr;
+	retval = runCode(info,
+			info->start_address + OFFSET_ERASE,
+			info->start_address + OFFSET_ERASE + OFFSET_ERASE_SIZE,
+			address,
+			len,
+			0,
+			&flashErr,
+			timeout
+);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (flashErr != 0x0)
+	{
+		LOG_ERROR("Flash erase failed with %d (%s)\n", (int)flashErr, flash_errmsg(flashErr));
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+static int eCosBoard_flash(struct ecosflash_flash_bank *info, void *data, uint32_t address, uint32_t len)
+{
+	struct target *target = info->target;
+	const int chunk = 8192;
+	int retval = ERROR_OK;
+	int timeout = (chunk / 20480 + 1) * 1000; /*asume 20 KB/s + 1 second*/
+
+	retval = loadDriver(info);
+	if (retval != ERROR_OK)
+		return retval;
+
+	uint32_t buffer;
+	retval = runCode(info,
+			info->start_address + OFFSET_GET_WORKAREA,
+			info->start_address + OFFSET_GET_WORKAREA + OFFSET_GET_WORKAREA_SIZE,
+			0,
+			0,
+			0,
+			&buffer,
+			1000);
+	if (retval != ERROR_OK)
+		return retval;
+
+
+	uint32_t i;
+	for (i = 0; i < len; i += chunk)
+	{
+		int t = len-i;
+		if (t > chunk)
+		{
+			t = chunk;
+		}
+
+		int retval;
+		retval = target_write_buffer(target, buffer, t, ((uint8_t *)data) + i);
+		if (retval != ERROR_OK)
+			return retval;
+
+		uint32_t flashErr;
+		retval = runCode(info,
+				info->start_address + OFFSET_FLASH,
+				info->start_address + OFFSET_FLASH + OFFSET_FLASH_SIZE,
+				buffer,
+				address + i,
+				t,
+				&flashErr,
+				timeout);
+		if (retval != ERROR_OK)
+			return retval;
+
+		if (flashErr != 0x0)
+		{
+			LOG_ERROR("Flash prog failed with %d (%s)\n", (int)flashErr, flash_errmsg(flashErr));
+			return ERROR_FAIL;
+		}
+	}
+	return ERROR_OK;
+}
+
+static int ecosflash_probe(struct flash_bank *bank)
+{
+	return ERROR_OK;
+}
+
+#if 0
+static void command(struct flash_bank *bank, uint8_t cmd, uint8_t *cmd_buf)
+{
+	struct ecosflash_flash_bank *info = bank->driver_priv;
+	int i;
+
+	if (info->target->endianness == TARGET_LITTLE_ENDIAN)
+	{
+		for (i = bank->bus_width; i > 0; i--)
+		{
+			*cmd_buf++ = (i & (bank->chip_width - 1)) ? 0x0 : cmd;
+		}
+	}
+	else
+	{
+		for (i = 1; i <= bank->bus_width; i++)
+		{
+			*cmd_buf++ = (i & (bank->chip_width - 1)) ? 0x0 : cmd;
+		}
+	}
+}
+#endif
+
+#if 0
+static uint32_t ecosflash_address(struct flash_bank *bank, uint32_t address)
+{
+	uint32_t retval = 0;
+	switch (bank->bus_width)
+	{
+		case 4:
+			retval = address & 0xfffffffc;
+		case 2:
+			retval = address & 0xfffffffe;
+		case 1:
+			retval = address;
+	}
+
+	return retval + bank->base;
+}
+#endif
+
+static int ecosflash_erase(struct flash_bank *bank, int first, int last)
+{
+	struct flash_bank *c = bank;
+	struct ecosflash_flash_bank *info = bank->driver_priv;
+	return eCosBoard_erase(info, c->base + first*sectorSize, sectorSize*(last-first + 1));
+}
+
+static int ecosflash_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	return ERROR_OK;
+}
+
+static int ecosflash_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	struct ecosflash_flash_bank *info = bank->driver_priv;
+	struct flash_bank *c = bank;
+	return eCosBoard_flash(info, buffer, c->base + offset, count);
+}
+
+static int ecosflash_protect_check(struct flash_bank *bank)
+{
+	return ERROR_OK;
+}
+
+static int ecosflash_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	struct ecosflash_flash_bank *info = bank->driver_priv;
+	snprintf(buf, buf_size, "eCos flash driver: %s", info->driverPath);
+	return ERROR_OK;
+}
+
+#if 0
+static uint32_t ecosflash_get_flash_status(struct flash_bank *bank)
+{
+	return ERROR_OK;
+}
+
+static void ecosflash_set_flash_mode(struct flash_bank *bank,int mode)
+{
+
+}
+
+static uint32_t ecosflash_wait_status_busy(struct flash_bank *bank, uint32_t waitbits, int timeout)
+{
+	return ERROR_OK;
+}
+
+static int ecosflash_handle_gpnvm_command(struct command_context *cmd_ctx, char *cmd, char **args, int argc)
+{
+	return ERROR_OK;
+}
+#endif
+
+struct flash_driver ecosflash_flash = {
+		.name = "ecosflash",
+		.flash_bank_command = &ecosflash_flash_bank_command,
+		.erase = &ecosflash_erase,
+		.protect = &ecosflash_protect,
+		.write = &ecosflash_write,
+		.probe = &ecosflash_probe,
+		.auto_probe = &ecosflash_probe,
+		.erase_check = &default_flash_blank_check,
+		.protect_check = &ecosflash_protect_check,
+		.info = &ecosflash_info
+	};
diff --git a/src/flash/nor/faux.c b/src/flash/nor/faux.c
new file mode 100644
index 00000000..caec2c79
--- /dev/null
+++ b/src/flash/nor/faux.c
@@ -0,0 +1,149 @@
+/***************************************************************************
+ *   Copyright (C) 2009 Øyvind Harboe                                      *
+ *   oyvind.harboe@zylin.com                                               *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "flash.h"
+#include "image.h"
+#include "../hello.h"
+
+
+struct faux_flash_bank
+{
+	struct target *target;
+	uint8_t *memory;
+	uint32_t start_address;
+};
+
+static const int sectorSize = 0x10000;
+
+
+/* flash bank faux <base> <size> <chip_width> <bus_width> <target#> <driverPath>
+ */
+FLASH_BANK_COMMAND_HANDLER(faux_flash_bank_command)
+{
+	struct faux_flash_bank *info;
+
+	if (CMD_ARGC < 6)
+	{
+		LOG_WARNING("incomplete flash_bank faux configuration");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	info = malloc(sizeof(struct faux_flash_bank));
+	if (info == NULL)
+	{
+		LOG_ERROR("no memory for flash bank info");
+		return ERROR_FAIL;
+	}
+	info->memory = malloc(bank->size);
+	if (info == NULL)
+	{
+		free(info);
+		LOG_ERROR("no memory for flash bank info");
+		return ERROR_FAIL;
+	}
+	bank->driver_priv = info;
+
+	/* Use 0x10000 as a fixed sector size. */
+	int i = 0;
+	uint32_t offset = 0;
+	bank->num_sectors = bank->size/sectorSize;
+	bank->sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
+	for (i = 0; i < bank->num_sectors; i++)
+	{
+		bank->sectors[i].offset = offset;
+		bank->sectors[i].size = sectorSize;
+		offset += bank->sectors[i].size;
+		bank->sectors[i].is_erased = -1;
+		bank->sectors[i].is_protected = 0;
+	}
+
+	info->target = get_target(CMD_ARGV[5]);
+	if (info->target == NULL)
+	{
+		LOG_ERROR("target '%s' not defined", CMD_ARGV[5]);
+		free(info->memory);
+		free(info);
+		return ERROR_FAIL;
+	}
+	return ERROR_OK;
+}
+
+static int faux_erase(struct flash_bank *bank, int first, int last)
+{
+	struct faux_flash_bank *info = bank->driver_priv;
+	memset(info->memory + first*sectorSize, 0xff, sectorSize*(last-first + 1));
+	return ERROR_OK;
+}
+
+static int faux_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	LOG_USER("set protection sector %d to %d to %s", first, last, set?"on":"off");
+	return ERROR_OK;
+}
+
+static int faux_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	struct faux_flash_bank *info = bank->driver_priv;
+	memcpy(info->memory + offset, buffer, count);
+	return ERROR_OK;
+}
+
+static int faux_protect_check(struct flash_bank *bank)
+{
+	return ERROR_OK;
+}
+
+static int faux_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	snprintf(buf, buf_size, "faux flash driver");
+	return ERROR_OK;
+}
+
+static int faux_probe(struct flash_bank *bank)
+{
+	return ERROR_OK;
+}
+
+static const struct command_registration faux_command_handlers[] = {
+	{
+		.name = "faux",
+		.mode = COMMAND_ANY,
+		.help = "faux flash command group",
+		.chain = hello_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+struct flash_driver faux_flash = {
+		.name = "faux",
+		.commands = faux_command_handlers,
+		.flash_bank_command = &faux_flash_bank_command,
+		.erase = &faux_erase,
+		.protect = &faux_protect,
+		.write = &faux_write,
+		.probe = &faux_probe,
+		.auto_probe = &faux_probe,
+		.erase_check = &default_flash_blank_check,
+		.protect_check = &faux_protect_check,
+		.info = &faux_info
+	};
diff --git a/src/flash/nor/lpc2000.c b/src/flash/nor/lpc2000.c
new file mode 100644
index 00000000..418b5b03
--- /dev/null
+++ b/src/flash/nor/lpc2000.c
@@ -0,0 +1,812 @@
+/***************************************************************************
+ *   Copyright (C) 2005 by Dominic Rath                                    *
+ *   Dominic.Rath@gmx.de                                                   *
+ *                                                                         *
+ *   LPC1700 support Copyright (C) 2009 by Audrius Urmanavicius            *
+ *   didele.deze@gmail.com                                                 *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "lpc2000.h"
+#include "armv7m.h"
+#include "binarybuffer.h"
+#include "algorithm.h"
+
+
+/* flash programming support for NXP LPC17xx and LPC2xxx devices
+ * currently supported devices:
+ * variant 1 (lpc2000_v1):
+ * - 2104 | 5 | 6
+ * - 2114 | 9
+ * - 2124 | 9
+ * - 2194
+ * - 2212 | 4
+ * - 2292 | 4
+ *
+ * variant 2 (lpc2000_v2):
+ * - 213x
+ * - 214x
+ * - 2101 | 2 | 3
+ * - 2364 | 6 | 8
+ * - 2378
+ *
+ * lpc1700:
+ * - 175x
+ * - 176x (tested with LPC1768)
+ */
+
+static int lpc2000_build_sector_list(struct flash_bank *bank)
+{
+	struct lpc2000_flash_bank *lpc2000_info = bank->driver_priv;
+	int i;
+	uint32_t offset = 0;
+
+	/* default to a 4096 write buffer */
+	lpc2000_info->cmd51_max_buffer = 4096;
+
+	if (lpc2000_info->variant == lpc2000_v1)
+	{
+		/* variant 1 has different layout for 128kb and 256kb flashes */
+		if (bank->size == 128 * 1024)
+		{
+			bank->num_sectors = 16;
+			bank->sectors = malloc(sizeof(struct flash_sector) * 16);
+			for (i = 0; i < 16; i++)
+			{
+				bank->sectors[i].offset = offset;
+				bank->sectors[i].size = 8 * 1024;
+				offset += bank->sectors[i].size;
+				bank->sectors[i].is_erased = -1;
+				bank->sectors[i].is_protected = 1;
+			}
+		}
+		else if (bank->size == 256 * 1024)
+		{
+			bank->num_sectors = 18;
+			bank->sectors = malloc(sizeof(struct flash_sector) * 18);
+
+			for (i = 0; i < 8; i++)
+			{
+				bank->sectors[i].offset = offset;
+				bank->sectors[i].size = 8 * 1024;
+				offset += bank->sectors[i].size;
+				bank->sectors[i].is_erased = -1;
+				bank->sectors[i].is_protected = 1;
+			}
+			for (i = 8; i < 10; i++)
+			{
+				bank->sectors[i].offset = offset;
+				bank->sectors[i].size = 64 * 1024;
+				offset += bank->sectors[i].size;
+				bank->sectors[i].is_erased = -1;
+				bank->sectors[i].is_protected = 1;
+			}
+			for (i = 10; i < 18; i++)
+			{
+				bank->sectors[i].offset = offset;
+				bank->sectors[i].size = 8 * 1024;
+				offset += bank->sectors[i].size;
+				bank->sectors[i].is_erased = -1;
+				bank->sectors[i].is_protected = 1;
+			}
+		}
+		else
+		{
+			LOG_ERROR("BUG: unknown bank->size encountered");
+			exit(-1);
+		}
+	}
+	else if (lpc2000_info->variant == lpc2000_v2)
+	{
+		/* variant 2 has a uniform layout, only number of sectors differs */
+		switch (bank->size)
+		{
+			case 4 * 1024:
+				lpc2000_info->cmd51_max_buffer = 1024;
+				bank->num_sectors = 1;
+				break;
+			case 8 * 1024:
+				lpc2000_info->cmd51_max_buffer = 1024;
+				bank->num_sectors = 2;
+				break;
+			case 16 * 1024:
+				bank->num_sectors = 4;
+				break;
+			case 32 * 1024:
+				bank->num_sectors = 8;
+				break;
+			case 64 * 1024:
+				bank->num_sectors = 9;
+				break;
+			case 128 * 1024:
+				bank->num_sectors = 11;
+				break;
+			case 256 * 1024:
+				bank->num_sectors = 15;
+				break;
+			case 512 * 1024:
+			case 500 * 1024:
+				bank->num_sectors = 27;
+				break;
+			default:
+				LOG_ERROR("BUG: unknown bank->size encountered");
+				exit(-1);
+				break;
+		}
+
+		bank->sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
+
+		for (i = 0; i < bank->num_sectors; i++)
+		{
+			if ((i >= 0) && (i < 8))
+			{
+				bank->sectors[i].offset = offset;
+				bank->sectors[i].size = 4 * 1024;
+				offset += bank->sectors[i].size;
+				bank->sectors[i].is_erased = -1;
+				bank->sectors[i].is_protected = 1;
+			}
+			if ((i >= 8) && (i < 22))
+			{
+				bank->sectors[i].offset = offset;
+				bank->sectors[i].size = 32 * 1024;
+				offset += bank->sectors[i].size;
+				bank->sectors[i].is_erased = -1;
+				bank->sectors[i].is_protected = 1;
+			}
+			if ((i >= 22) && (i < 27))
+			{
+				bank->sectors[i].offset = offset;
+				bank->sectors[i].size = 4 * 1024;
+				offset += bank->sectors[i].size;
+				bank->sectors[i].is_erased = -1;
+				bank->sectors[i].is_protected = 1;
+			}
+		}
+	}
+	else if (lpc2000_info->variant == lpc1700)
+	{
+		switch(bank->size)
+		{
+			case 32 * 1024:
+				bank->num_sectors = 8;
+				break;
+			case 64 * 1024:
+				bank->num_sectors = 16;
+				break;
+			case 128 * 1024:
+				bank->num_sectors = 18;
+				break;
+			case 256 * 1024:
+				bank->num_sectors = 22;
+				break;
+			case 512 * 1024:
+				bank->num_sectors = 30;
+				break;
+			default:
+				LOG_ERROR("BUG: unknown bank->size encountered");
+				exit(-1);
+		}
+
+		bank->sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
+
+		for(i = 0; i < bank->num_sectors; i++)
+		{
+			bank->sectors[i].offset = offset;
+			/* sectors 0-15 are 4kB-sized, 16 and above are 32kB-sized for LPC17xx devices */
+			bank->sectors[i].size = (i < 16)? 4 * 1024 : 32 * 1024;
+			offset += bank->sectors[i].size;
+			bank->sectors[i].is_erased = -1;
+			bank->sectors[i].is_protected = 1;
+		}
+	}
+	else
+	{
+		LOG_ERROR("BUG: unknown lpc2000_info->variant encountered");
+		exit(-1);
+	}
+
+	return ERROR_OK;
+}
+
+/* call LPC1700/LPC2000 IAP function
+ * uses 180 bytes working area
+ * 0x0 to 0x7: jump gate (BX to thumb state, b -2 to wait)
+ * 0x8 to 0x1f: command parameter table (1+5 words)
+ * 0x20 to 0x33: command result table (1+4 words)
+ * 0x34 to 0xb3: stack (only 128b needed)
+ */
+static int lpc2000_iap_call(struct flash_bank *bank, int code, uint32_t param_table[5], uint32_t result_table[4])
+{
+	int retval;
+	struct lpc2000_flash_bank *lpc2000_info = bank->driver_priv;
+	struct target *target = bank->target;
+	struct mem_param mem_params[2];
+	struct reg_param reg_params[5];
+	struct armv4_5_algorithm armv4_5_info; /* for LPC2000 */
+	struct armv7m_algorithm armv7m_info;   /* for LPC1700 */
+ 	uint32_t status_code;
+	uint32_t iap_entry_point = 0; /* to make compiler happier */
+
+	/* regrab previously allocated working_area, or allocate a new one */
+	if (!lpc2000_info->iap_working_area)
+	{
+		uint8_t jump_gate[8];
+
+		/* make sure we have a working area */
+		if (target_alloc_working_area(target, 180, &lpc2000_info->iap_working_area) != ERROR_OK)
+		{
+			LOG_ERROR("no working area specified, can't write LPC2000 internal flash");
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+
+		/* write IAP code to working area */
+		switch(lpc2000_info->variant)
+		{
+			case lpc1700:
+				target_buffer_set_u32(target, jump_gate, ARMV7M_T_BX(12));
+				target_buffer_set_u32(target, jump_gate + 4, ARMV7M_T_B(0xfffffe));
+				break;
+			case lpc2000_v1:
+			case lpc2000_v2:
+				target_buffer_set_u32(target, jump_gate, ARMV4_5_BX(12));
+				target_buffer_set_u32(target, jump_gate + 4, ARMV4_5_B(0xfffffe, 0));
+				break;
+			default:
+				LOG_ERROR("BUG: unknown bank->size encountered");
+				exit(-1);
+		}
+
+		if ((retval = target_write_memory(target, lpc2000_info->iap_working_area->address, 4, 2, jump_gate)) != ERROR_OK)
+		{
+			LOG_ERROR("Write memory at address 0x%8.8" PRIx32 " failed (check work_area definition)", lpc2000_info->iap_working_area->address);
+			return retval;
+		}
+	}
+
+	switch(lpc2000_info->variant)
+	{
+		case lpc1700:
+			armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
+			armv7m_info.core_mode = ARMV7M_MODE_ANY;
+			iap_entry_point = 0x1fff1ff1;
+			break;
+		case lpc2000_v1:
+		case lpc2000_v2:
+			armv4_5_info.common_magic = ARMV4_5_COMMON_MAGIC;
+			armv4_5_info.core_mode = ARMV4_5_MODE_SVC;
+			armv4_5_info.core_state = ARMV4_5_STATE_ARM;
+			iap_entry_point = 0x7ffffff1;
+			break;
+		default:
+			LOG_ERROR("BUG: unknown lpc2000->variant encountered");
+			exit(-1);
+	}
+
+	/* command parameter table */
+	init_mem_param(&mem_params[0], lpc2000_info->iap_working_area->address + 8, 6 * 4, PARAM_OUT);
+	target_buffer_set_u32(target, mem_params[0].value, code);
+	target_buffer_set_u32(target, mem_params[0].value + 0x04, param_table[0]);
+	target_buffer_set_u32(target, mem_params[0].value + 0x08, param_table[1]);
+	target_buffer_set_u32(target, mem_params[0].value + 0x0c, param_table[2]);
+	target_buffer_set_u32(target, mem_params[0].value + 0x10, param_table[3]);
+	target_buffer_set_u32(target, mem_params[0].value + 0x14, param_table[4]);
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
+	buf_set_u32(reg_params[0].value, 0, 32, lpc2000_info->iap_working_area->address + 0x08);
+
+	/* command result table */
+	init_mem_param(&mem_params[1], lpc2000_info->iap_working_area->address + 0x20, 5 * 4, PARAM_IN);
+
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
+	buf_set_u32(reg_params[1].value, 0, 32, lpc2000_info->iap_working_area->address + 0x20);
+
+	/* IAP entry point */
+	init_reg_param(&reg_params[2], "r12", 32, PARAM_OUT);
+	buf_set_u32(reg_params[2].value, 0, 32, iap_entry_point);
+
+	switch(lpc2000_info->variant)
+	{
+		case lpc1700:
+			/* IAP stack */
+			init_reg_param(&reg_params[3], "sp", 32, PARAM_OUT);
+			buf_set_u32(reg_params[3].value, 0, 32, lpc2000_info->iap_working_area->address + 0xb4);
+
+			/* return address */
+			init_reg_param(&reg_params[4], "lr", 32, PARAM_OUT);
+			buf_set_u32(reg_params[4].value, 0, 32, (lpc2000_info->iap_working_area->address + 0x04) | 1); /* bit0 of LR = 1 to return in Thumb mode */
+
+			target_run_algorithm(target, 2, mem_params, 5, reg_params, lpc2000_info->iap_working_area->address, lpc2000_info->iap_working_area->address + 0x4, 10000, &armv7m_info);
+			break;
+		case lpc2000_v1:
+		case lpc2000_v2:
+			/* IAP stack */
+			init_reg_param(&reg_params[3], "r13_svc", 32, PARAM_OUT);
+			buf_set_u32(reg_params[3].value, 0, 32, lpc2000_info->iap_working_area->address + 0xb4);
+
+			/* return address */
+			init_reg_param(&reg_params[4], "lr_svc", 32, PARAM_OUT);
+			buf_set_u32(reg_params[4].value, 0, 32, lpc2000_info->iap_working_area->address + 0x04);
+
+			target_run_algorithm(target, 2, mem_params, 5, reg_params, lpc2000_info->iap_working_area->address, lpc2000_info->iap_working_area->address + 0x4, 10000, &armv4_5_info);
+			break;
+		default:
+			LOG_ERROR("BUG: unknown lpc2000->variant encountered");
+			exit(-1);
+	}
+
+
+	status_code     = target_buffer_get_u32(target, mem_params[1].value);
+	result_table[0] = target_buffer_get_u32(target, mem_params[1].value + 0x04);
+	result_table[1] = target_buffer_get_u32(target, mem_params[1].value + 0x08);
+	result_table[2] = target_buffer_get_u32(target, mem_params[1].value + 0x0c);
+	result_table[3] = target_buffer_get_u32(target, mem_params[1].value + 0x10);
+
+	LOG_DEBUG("IAP command = %i (0x%8.8" PRIx32", 0x%8.8" PRIx32", 0x%8.8" PRIx32", 0x%8.8" PRIx32", 0x%8.8" PRIx32") completed with result = %8.8" PRIx32,
+			  code, param_table[0], param_table[1], param_table[2], param_table[3], param_table[4], status_code);
+
+	destroy_mem_param(&mem_params[0]);
+	destroy_mem_param(&mem_params[1]);
+
+	destroy_reg_param(&reg_params[0]);
+	destroy_reg_param(&reg_params[1]);
+	destroy_reg_param(&reg_params[2]);
+	destroy_reg_param(&reg_params[3]);
+	destroy_reg_param(&reg_params[4]);
+
+	return status_code;
+}
+
+static int lpc2000_iap_blank_check(struct flash_bank *bank, int first, int last)
+{
+	uint32_t param_table[5];
+	uint32_t result_table[4];
+	int status_code;
+	int i;
+
+	if ((first < 0) || (last >= bank->num_sectors))
+		return ERROR_FLASH_SECTOR_INVALID;
+
+	for (i = first; i <= last; i++)
+	{
+		/* check single sector */
+		param_table[0] = param_table[1] = i;
+		status_code = lpc2000_iap_call(bank, 53, param_table, result_table);
+
+		switch (status_code)
+		{
+			case ERROR_FLASH_OPERATION_FAILED:
+				return ERROR_FLASH_OPERATION_FAILED;
+			case LPC2000_CMD_SUCCESS:
+				bank->sectors[i].is_erased = 1;
+				break;
+			case LPC2000_SECTOR_NOT_BLANK:
+				bank->sectors[i].is_erased = 0;
+				break;
+			case LPC2000_INVALID_SECTOR:
+				bank->sectors[i].is_erased = 0;
+				break;
+			case LPC2000_BUSY:
+				return ERROR_FLASH_BUSY;
+				break;
+			default:
+				LOG_ERROR("BUG: unknown LPC2000 status code %i", status_code);
+				exit(-1);
+		}
+	}
+
+	return ERROR_OK;
+}
+
+/*
+ * flash bank lpc2000 <base> <size> 0 0 <target#> <lpc_variant> <cclk> [calc_checksum]
+ */
+FLASH_BANK_COMMAND_HANDLER(lpc2000_flash_bank_command)
+{
+	struct lpc2000_flash_bank *lpc2000_info;
+
+	if (CMD_ARGC < 8)
+	{
+		LOG_WARNING("incomplete flash_bank lpc2000 configuration");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	lpc2000_info = malloc(sizeof(struct lpc2000_flash_bank));
+	bank->driver_priv = lpc2000_info;
+
+	if (strcmp(CMD_ARGV[6], "lpc2000_v1") == 0)
+	{
+		lpc2000_info->variant = lpc2000_v1;
+		lpc2000_info->cmd51_dst_boundary = 512;
+		lpc2000_info->cmd51_can_256b = 0;
+		lpc2000_info->cmd51_can_8192b = 1;
+		lpc2000_info->checksum_vector = 5;
+	}
+	else if (strcmp(CMD_ARGV[6], "lpc2000_v2") == 0)
+	{
+		lpc2000_info->variant = lpc2000_v2;
+		lpc2000_info->cmd51_dst_boundary = 256;
+		lpc2000_info->cmd51_can_256b = 1;
+		lpc2000_info->cmd51_can_8192b = 0;
+		lpc2000_info->checksum_vector = 5;
+	}
+	else if (strcmp(CMD_ARGV[6], "lpc1700") == 0)
+	{
+		lpc2000_info->variant = lpc1700;
+		lpc2000_info->cmd51_dst_boundary = 256;
+		lpc2000_info->cmd51_can_256b = 1;
+		lpc2000_info->cmd51_can_8192b = 0;
+		lpc2000_info->checksum_vector = 7;
+	}
+	else
+	{
+		LOG_ERROR("unknown LPC2000 variant: %s", CMD_ARGV[6]);
+		free(lpc2000_info);
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	lpc2000_info->iap_working_area = NULL;
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[7], lpc2000_info->cclk);
+	lpc2000_info->calc_checksum = 0;
+	lpc2000_build_sector_list(bank);
+
+	if (CMD_ARGC >= 9)
+	{
+		if (strcmp(CMD_ARGV[8], "calc_checksum") == 0)
+			lpc2000_info->calc_checksum = 1;
+	}
+
+	return ERROR_OK;
+}
+
+static int lpc2000_erase(struct flash_bank *bank, int first, int last)
+{
+	struct lpc2000_flash_bank *lpc2000_info = bank->driver_priv;
+	uint32_t param_table[5];
+	uint32_t result_table[4];
+	int status_code;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	param_table[0] = first;
+	param_table[1] = last;
+	param_table[2] = lpc2000_info->cclk;
+
+	/* Prepare sectors */
+	status_code = lpc2000_iap_call(bank, 50, param_table, result_table);
+	switch (status_code)
+	{
+		case ERROR_FLASH_OPERATION_FAILED:
+			return ERROR_FLASH_OPERATION_FAILED;
+		case LPC2000_CMD_SUCCESS:
+			break;
+		case LPC2000_INVALID_SECTOR:
+			return ERROR_FLASH_SECTOR_INVALID;
+			break;
+		default:
+			LOG_WARNING("lpc2000 prepare sectors returned %i", status_code);
+			return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	/* Erase sectors */
+	status_code = lpc2000_iap_call(bank, 52, param_table, result_table);
+	switch (status_code)
+	{
+		case ERROR_FLASH_OPERATION_FAILED:
+			return ERROR_FLASH_OPERATION_FAILED;
+		case LPC2000_CMD_SUCCESS:
+			break;
+		case LPC2000_INVALID_SECTOR:
+			return ERROR_FLASH_SECTOR_INVALID;
+			break;
+		default:
+			LOG_WARNING("lpc2000 erase sectors returned %i", status_code);
+			return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	return ERROR_OK;
+}
+
+static int lpc2000_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	/* can't protect/unprotect on the lpc2000 */
+	return ERROR_OK;
+}
+
+static int lpc2000_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	struct lpc2000_flash_bank *lpc2000_info = bank->driver_priv;
+	struct target *target = bank->target;
+	uint32_t dst_min_alignment;
+	uint32_t bytes_remaining = count;
+	uint32_t bytes_written = 0;
+	int first_sector = 0;
+	int last_sector = 0;
+	uint32_t param_table[5];
+	uint32_t result_table[4];
+	int status_code;
+	int i;
+	struct working_area *download_area;
+	int retval = ERROR_OK;
+
+	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;
+
+	dst_min_alignment = lpc2000_info->cmd51_dst_boundary;
+
+	if (offset % dst_min_alignment)
+	{
+		LOG_WARNING("offset 0x%" PRIx32 " breaks required alignment 0x%" PRIx32, offset, dst_min_alignment);
+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+	}
+
+	for (i = 0; i < bank->num_sectors; i++)
+	{
+		if (offset >= bank->sectors[i].offset)
+			first_sector = i;
+		if (offset + DIV_ROUND_UP(count, dst_min_alignment) * dst_min_alignment > bank->sectors[i].offset)
+			last_sector = i;
+	}
+
+	LOG_DEBUG("first_sector: %i, last_sector: %i", first_sector, last_sector);
+
+	/* check if exception vectors should be flashed */
+	if ((offset == 0) && (count >= 0x20) && lpc2000_info->calc_checksum)
+	{
+		uint32_t checksum = 0;
+		int i;
+		for (i = 0; i < 8; i++)
+		{
+			LOG_DEBUG("Vector 0x%2.2x: 0x%8.8" PRIx32, i * 4, buf_get_u32(buffer + (i * 4), 0, 32));
+			if (i != lpc2000_info->checksum_vector)
+				checksum += buf_get_u32(buffer + (i * 4), 0, 32);
+		}
+		checksum = 0 - checksum;
+		LOG_DEBUG("checksum: 0x%8.8" PRIx32, checksum);
+
+		uint32_t original_value = buf_get_u32(buffer + (lpc2000_info->checksum_vector * 4), 0, 32);
+		if (original_value != checksum)
+		{
+			LOG_WARNING("Verification will fail since checksum in image (0x%8.8" PRIx32 ") to be written to flash is different from calculated vector checksum (0x%8.8" PRIx32 ").",
+					original_value, checksum);
+			LOG_WARNING("To remove this warning modify build tools on developer PC to inject correct LPC vector checksum.");
+		}
+
+		buf_set_u32(buffer + (lpc2000_info->checksum_vector * 4), 0, 32, checksum);
+	}
+
+	/* allocate a working area */
+	if (target_alloc_working_area(target, lpc2000_info->cmd51_max_buffer, &download_area) != ERROR_OK)
+	{
+		LOG_ERROR("no working area specified, can't write LPC2000 internal flash");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	while (bytes_remaining > 0)
+	{
+		uint32_t thisrun_bytes;
+		if (bytes_remaining >= lpc2000_info->cmd51_max_buffer)
+			thisrun_bytes = lpc2000_info->cmd51_max_buffer;
+		else if (bytes_remaining >= 1024)
+			thisrun_bytes = 1024;
+		else if ((bytes_remaining >= 512) || (!lpc2000_info->cmd51_can_256b))
+			thisrun_bytes = 512;
+		else
+			thisrun_bytes = 256;
+
+		/* Prepare sectors */
+		param_table[0] = first_sector;
+		param_table[1] = last_sector;
+		status_code = lpc2000_iap_call(bank, 50, param_table, result_table);
+		switch (status_code)
+		{
+			case ERROR_FLASH_OPERATION_FAILED:
+				retval = ERROR_FLASH_OPERATION_FAILED;
+				break;
+			case LPC2000_CMD_SUCCESS:
+				break;
+			case LPC2000_INVALID_SECTOR:
+				retval = ERROR_FLASH_SECTOR_INVALID;
+				break;
+			default:
+				LOG_WARNING("lpc2000 prepare sectors returned %i", status_code);
+				retval = ERROR_FLASH_OPERATION_FAILED;
+				break;
+		}
+
+		/* Exit if error occured */
+		if (retval != ERROR_OK)
+			break;
+
+		if (bytes_remaining >= thisrun_bytes)
+		{
+			if ((retval = target_write_buffer(bank->target, download_area->address, thisrun_bytes, buffer + bytes_written)) != ERROR_OK)
+			{
+				retval = ERROR_FLASH_OPERATION_FAILED;
+				break;
+			}
+		}
+		else
+		{
+			uint8_t *last_buffer = malloc(thisrun_bytes);
+			memcpy(last_buffer, buffer + bytes_written, bytes_remaining);
+			memset(last_buffer + bytes_remaining, 0xff, thisrun_bytes - bytes_remaining);
+			target_write_buffer(bank->target, download_area->address, thisrun_bytes, last_buffer);
+			free(last_buffer);
+		}
+
+		LOG_DEBUG("writing 0x%" PRIx32 " bytes to address 0x%" PRIx32 , thisrun_bytes, bank->base + offset + bytes_written);
+
+		/* Write data */
+		param_table[0] = bank->base + offset + bytes_written;
+		param_table[1] = download_area->address;
+		param_table[2] = thisrun_bytes;
+		param_table[3] = lpc2000_info->cclk;
+		status_code = lpc2000_iap_call(bank, 51, param_table, result_table);
+		switch (status_code)
+		{
+			case ERROR_FLASH_OPERATION_FAILED:
+				retval = ERROR_FLASH_OPERATION_FAILED;
+				break;
+			case LPC2000_CMD_SUCCESS:
+				break;
+			case LPC2000_INVALID_SECTOR:
+				retval = ERROR_FLASH_SECTOR_INVALID;
+				break;
+			default:
+				LOG_WARNING("lpc2000 returned %i", status_code);
+				retval = ERROR_FLASH_OPERATION_FAILED;
+				break;
+		}
+
+		/* Exit if error occured */
+		if (retval != ERROR_OK)
+			break;
+
+		if (bytes_remaining > thisrun_bytes)
+			bytes_remaining -= thisrun_bytes;
+		else
+			bytes_remaining = 0;
+		bytes_written += thisrun_bytes;
+	}
+
+	target_free_working_area(target, download_area);
+
+	return retval;
+}
+
+static int lpc2000_probe(struct flash_bank *bank)
+{
+	/* we can't probe on an lpc2000
+	 * if this is an lpc2xxx, it has the configured flash
+	 */
+	return ERROR_OK;
+}
+
+static int lpc2000_erase_check(struct flash_bank *bank)
+{
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	return lpc2000_iap_blank_check(bank, 0, bank->num_sectors - 1);
+}
+
+static int lpc2000_protect_check(struct flash_bank *bank)
+{
+	/* sectors are always protected	*/
+	return ERROR_OK;
+}
+
+static int lpc2000_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	struct lpc2000_flash_bank *lpc2000_info = bank->driver_priv;
+
+	snprintf(buf, buf_size, "lpc2000 flash driver variant: %i, clk: %" PRIi32 "kHz" , lpc2000_info->variant, lpc2000_info->cclk);
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(lpc2000_handle_part_id_command)
+{
+	uint32_t param_table[5];
+	uint32_t result_table[4];
+	int status_code;
+
+	if (CMD_ARGC < 1)
+	{
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if ((status_code = lpc2000_iap_call(bank, 54, param_table, result_table)) != 0x0)
+	{
+		if (status_code == ERROR_FLASH_OPERATION_FAILED)
+		{
+			command_print(CMD_CTX, "no sufficient working area specified, can't access LPC2000 IAP interface");
+			return ERROR_OK;
+		}
+		command_print(CMD_CTX, "lpc2000 IAP returned status code %i", status_code);
+	}
+	else
+	{
+		command_print(CMD_CTX, "lpc2000 part id: 0x%8.8" PRIx32 , result_table[0]);
+	}
+
+	return ERROR_OK;
+}
+
+static const struct command_registration lpc2000_exec_command_handlers[] = {
+	{
+		.name = "part_id",
+		.handler = &lpc2000_handle_part_id_command,
+		.mode = COMMAND_EXEC,
+		.help = "print part id of lpc2000 flash bank <num>",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+static const struct command_registration lpc2000_command_handlers[] = {
+	{
+		.name = "lpc2000",
+		.mode = COMMAND_ANY,
+		.help = "lpc2000 flash command group",
+		.chain = lpc2000_exec_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+struct flash_driver lpc2000_flash = {
+		.name = "lpc2000",
+		.commands = lpc2000_command_handlers,
+		.flash_bank_command = &lpc2000_flash_bank_command,
+		.erase = &lpc2000_erase,
+		.protect = &lpc2000_protect,
+		.write = &lpc2000_write,
+		.probe = &lpc2000_probe,
+		.auto_probe = &lpc2000_probe,
+		.erase_check = &lpc2000_erase_check,
+		.protect_check = &lpc2000_protect_check,
+		.info = &lpc2000_info,
+	};
+
+
diff --git a/src/flash/nor/lpc2000.h b/src/flash/nor/lpc2000.h
new file mode 100644
index 00000000..08e278a3
--- /dev/null
+++ b/src/flash/nor/lpc2000.h
@@ -0,0 +1,73 @@
+/***************************************************************************
+ *   Copyright (C) 2005 by Dominic Rath                                    *
+ *   Dominic.Rath@gmx.de                                                   *
+ *                                                                         *
+ *   LPC1700 support Copyright (C) 2009 by Audrius Urmanavicius            *
+ *   didele.deze@gmail.com                                                 *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifndef LPC2000_H
+#define LPC2000_H
+
+#include "flash.h"
+
+typedef enum
+{
+	lpc2000_v1,
+	lpc2000_v2,
+	lpc1700
+} lpc2000_variant;
+
+struct lpc2000_flash_bank
+{
+	lpc2000_variant variant;
+	struct working_area *iap_working_area;
+	uint32_t cclk;
+	int cmd51_dst_boundary;
+	int cmd51_can_256b;
+	int cmd51_can_8192b;
+	int calc_checksum;
+	uint32_t cmd51_max_buffer;
+	int checksum_vector;
+};
+
+enum lpc2000_status_codes
+{
+	LPC2000_CMD_SUCCESS = 0,
+	LPC2000_INVALID_COMMAND = 1,
+	LPC2000_SRC_ADDR_ERROR = 2,
+	LPC2000_DST_ADDR_ERROR = 3,
+	LPC2000_SRC_ADDR_NOT_MAPPED = 4,
+	LPC2000_DST_ADDR_NOT_MAPPED = 5,
+	LPC2000_COUNT_ERROR = 6,
+	LPC2000_INVALID_SECTOR = 7,
+	LPC2000_SECTOR_NOT_BLANK = 8,
+	LPC2000_SECTOR_NOT_PREPARED = 9,
+	LPC2000_COMPARE_ERROR = 10,
+	LPC2000_BUSY = 11,
+	LPC2000_PARAM_ERROR = 12,
+	LPC2000_ADDR_ERROR = 13,
+	LPC2000_ADDR_NOT_MAPPED = 14,
+	LPC2000_CMD_NOT_LOCKED = 15,
+	LPC2000_INVALID_CODE = 16,
+	LPC2000_INVALID_BAUD_RATE = 17,
+	LPC2000_INVALID_STOP_BIT = 18,
+	LPC2000_CRP_ENABLED = 19
+
+};
+
+#endif /* LPC2000_H */
diff --git a/src/flash/nor/lpc288x.c b/src/flash/nor/lpc288x.c
new file mode 100644
index 00000000..446fc9da
--- /dev/null
+++ b/src/flash/nor/lpc288x.c
@@ -0,0 +1,485 @@
+/***************************************************************************
+ *   Copyright (C) 2008 by			                                       *
+ *   Karl RobinSod <karl.robinsod@gmail.com>                               *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+/***************************************************************************
+* There are some things to notice
+*
+* You need to unprotect flash sectors each time you connect the OpenOCD
+* Dumping 1MB takes about 60 Seconds
+* Full erase (sectors 0-22 inclusive) takes 2-4 seconds
+* Writing 1MB takes 88 seconds
+*
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "lpc288x.h"
+#include "binarybuffer.h"
+
+
+#define LOAD_TIMER_ERASE	0
+#define LOAD_TIMER_WRITE	1
+
+#define FLASH_PAGE_SIZE		512
+
+/* LPC288X control registers */
+#define DBGU_CIDR		0x8000507C
+/* LPC288X flash registers */
+#define F_CTRL			0x80102000	/* Flash control register R/W 0x5 */
+#define F_STAT			0x80102004	/* Flash status register RO 0x45 */
+#define F_PROG_TIME		0x80102008	/* Flash program time register R/W 0 */
+#define F_WAIT			0x80102010	/* Flash read wait state register R/W 0xC004 */
+#define F_CLK_TIME		0x8010201C	/* Flash clock divider for 66 kHz generation R/W 0 */
+#define F_INTEN_CLR		0x80102FD8	/* Clear interrupt enable bits WO - */
+#define F_INTEN_SET		0x80102FDC	/* Set interrupt enable bits WO - */
+#define F_INT_STAT		0x80102FE0	/* Interrupt status bits RO 0 */
+#define F_INTEN			0x80102FE4	/* Interrupt enable bits RO 0 */
+#define F_INT_CLR		0x80102FE8	/* Clear interrupt status bits WO */
+#define F_INT_SET		0x80102FEC	/* Set interrupt status bits WO - */
+#define FLASH_PD		0x80005030	/* Allows turning off the Flash memory for power savings. R/W 1*/
+#define FLASH_INIT		0x80005034	/* Monitors Flash readiness, such as recovery from Power Down mode. R/W -*/
+
+/* F_CTRL bits */
+#define FC_CS			0x0001
+#define FC_FUNC			0x0002
+#define FC_WEN			0x0004
+#define FC_RD_LATCH		0x0020
+#define FC_PROTECT		0x0080
+#define FC_SET_DATA		0x0400
+#define FC_RSSL			0x0800
+#define FC_PROG_REQ		0x1000
+#define FC_CLR_BUF		0x4000
+#define FC_LOAD_REQ		0x8000
+/* F_STAT bits */
+#define FS_DONE			0x0001
+#define FS_PROGGNT		0x0002
+#define FS_RDY			0x0004
+#define FS_ERR			0x0020
+/* F_PROG_TIME */
+#define FPT_TIME_MASK	0x7FFF
+
+#define FPT_ENABLE		0x8000
+/* F_WAIT */
+#define FW_WAIT_STATES_MASK		0x00FF
+#define FW_SET_MASK				0xC000
+
+/* F_CLK_TIME */
+#define FCT_CLK_DIV_MASK    0x0FFF
+
+static uint32_t lpc288x_wait_status_busy(struct flash_bank *bank, int timeout);
+static void lpc288x_load_timer(int erase, struct target *target);
+static void lpc288x_set_flash_clk(struct flash_bank *bank);
+static uint32_t lpc288x_system_ready(struct flash_bank *bank);
+
+static uint32_t lpc288x_wait_status_busy(struct flash_bank *bank, int timeout)
+{
+	uint32_t status;
+	struct target *target = bank->target;
+	do
+	{
+		alive_sleep(1);
+		timeout--;
+		target_read_u32(target, F_STAT, &status);
+	} while (((status & FS_DONE) == 0) && timeout);
+
+	if (timeout == 0)
+	{
+		LOG_DEBUG("Timedout!");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+	return ERROR_OK;
+}
+
+/* Read device id register and fill in driver info structure */
+static int lpc288x_read_part_info(struct flash_bank *bank)
+{
+	struct lpc288x_flash_bank *lpc288x_info = bank->driver_priv;
+	struct target *target = bank->target;
+	uint32_t cidr;
+
+	int i = 0;
+	uint32_t offset;
+
+	if (lpc288x_info->cidr == 0x0102100A)
+		return ERROR_OK; /* already probed, multiple probes may cause memory leak, not allowed */
+
+	/* Read and parse chip identification register */
+	target_read_u32(target, DBGU_CIDR, &cidr);
+
+	if (cidr != 0x0102100A)
+	{
+		LOG_WARNING("Cannot identify target as an LPC288X (%08" PRIx32 ")",cidr);
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	lpc288x_info->cidr = cidr;
+	lpc288x_info->sector_size_break = 0x000F0000;
+	lpc288x_info->target_name = "LPC288x";
+
+	/* setup the sector info... */
+	offset = bank->base;
+	bank->num_sectors = 23;
+	bank->sectors = malloc(sizeof(struct flash_sector) * 23);
+
+	for (i = 0; i < 15; i++)
+	{
+		bank->sectors[i].offset = offset;
+		bank->sectors[i].size = 64 * 1024;
+		offset += bank->sectors[i].size;
+		bank->sectors[i].is_erased = -1;
+		bank->sectors[i].is_protected = 1;
+	}
+	for (i = 15; i < 23; i++)
+	{
+		bank->sectors[i].offset = offset;
+		bank->sectors[i].size = 8 * 1024;
+		offset += bank->sectors[i].size;
+		bank->sectors[i].is_erased = -1;
+		bank->sectors[i].is_protected = 1;
+	}
+
+	return ERROR_OK;
+}
+
+static int lpc288x_protect_check(struct flash_bank *bank)
+{
+	return ERROR_OK;
+}
+
+/* flash_bank LPC288x 0 0 0 0 <target#> <cclk> */
+FLASH_BANK_COMMAND_HANDLER(lpc288x_flash_bank_command)
+{
+	struct lpc288x_flash_bank *lpc288x_info;
+
+	if (CMD_ARGC < 6)
+	{
+		LOG_WARNING("incomplete flash_bank LPC288x configuration");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	lpc288x_info = malloc(sizeof(struct lpc288x_flash_bank));
+	bank->driver_priv = lpc288x_info;
+
+	/* part wasn't probed for info yet */
+	lpc288x_info->cidr = 0;
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[6], lpc288x_info->cclk);
+
+	return ERROR_OK;
+}
+
+/* The frequency is the AHB clock frequency divided by (CLK_DIV ×3) + 1.
+ * This must be programmed such that the Flash Programming clock frequency is 66 kHz ± 20%.
+ * AHB = 12 MHz ?
+ * 12000000/66000 = 182
+ * CLK_DIV = 60 ? */
+static void lpc288x_set_flash_clk(struct flash_bank *bank)
+{
+	uint32_t clk_time;
+	struct lpc288x_flash_bank *lpc288x_info = bank->driver_priv;
+	clk_time = (lpc288x_info->cclk / 66000) / 3;
+	target_write_u32(bank->target, F_CTRL, FC_CS | FC_WEN);
+	target_write_u32(bank->target, F_CLK_TIME, clk_time);
+}
+
+/* AHB tcyc (in ns) 83 ns
+ * LOAD_TIMER_ERASE		FPT_TIME	= ((400,000,000 / AHB tcyc (in ns)) - 2) / 512
+ *									= 9412 (9500) (AN10548 9375)
+ * LOAD_TIMER_WRITE		FPT_TIME	= ((1,000,000 / AHB tcyc (in ns)) - 2) / 512
+ *									= 23 (75) (AN10548 72 - is this wrong?)
+ * TODO: Sort out timing calcs ;) */
+static void lpc288x_load_timer(int erase, struct target *target)
+{
+	if (erase == LOAD_TIMER_ERASE)
+	{
+		target_write_u32(target, F_PROG_TIME, FPT_ENABLE | 9500);
+	}
+	else
+	{
+		target_write_u32(target, F_PROG_TIME, FPT_ENABLE | 75);
+	}
+}
+
+static uint32_t lpc288x_system_ready(struct flash_bank *bank)
+{
+	struct lpc288x_flash_bank *lpc288x_info = bank->driver_priv;
+	if (lpc288x_info->cidr == 0)
+	{
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	return ERROR_OK;
+}
+
+static int lpc288x_erase_check(struct flash_bank *bank)
+{
+	uint32_t status = lpc288x_system_ready(bank);	/* probed? halted? */
+	if (status != ERROR_OK)
+	{
+		LOG_INFO("Processor not halted/not probed");
+		return status;
+	}
+
+	return ERROR_OK;
+}
+
+static int lpc288x_erase(struct flash_bank *bank, int first, int last)
+{
+	uint32_t status;
+	int sector;
+	struct target *target = bank->target;
+
+	status = lpc288x_system_ready(bank);    /* probed? halted? */
+	if (status != ERROR_OK)
+	{
+		return status;
+	}
+
+	if ((first < 0) || (last < first) || (last >= bank->num_sectors))
+	{
+		LOG_INFO("Bad sector range");
+		return ERROR_FLASH_SECTOR_INVALID;
+	}
+
+	/* Configure the flash controller timing */
+	lpc288x_set_flash_clk(bank);
+
+	for (sector = first; sector <= last; sector++)
+	{
+		if (lpc288x_wait_status_busy(bank, 1000) != ERROR_OK)
+		{
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+
+		lpc288x_load_timer(LOAD_TIMER_ERASE,target);
+
+		target_write_u32(target, bank->sectors[sector].offset, 0x00);
+
+		target_write_u32(target, F_CTRL, FC_PROG_REQ | FC_PROTECT | FC_CS);
+	}
+	if (lpc288x_wait_status_busy(bank, 1000) != ERROR_OK)
+	{
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+	return ERROR_OK;
+}
+
+static int lpc288x_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	uint8_t page_buffer[FLASH_PAGE_SIZE];
+	uint32_t status, source_offset,dest_offset;
+	struct target *target = bank->target;
+	uint32_t bytes_remaining = count;
+	uint32_t first_sector, last_sector, sector, page;
+	int i;
+
+	/* probed? halted? */
+	status = lpc288x_system_ready(bank);
+	if (status != ERROR_OK)
+	{
+		return status;
+	}
+
+	/* Initialise search indices */
+	first_sector = last_sector = 0xffffffff;
+
+	/* validate the write range... */
+	for (i = 0; i < bank->num_sectors; i++)
+	{
+		if ((offset >= bank->sectors[i].offset) &&
+			(offset < (bank->sectors[i].offset + bank->sectors[i].size)) &&
+			(first_sector == 0xffffffff))
+		{
+			first_sector = i;
+			/* all writes must start on a sector boundary... */
+			if (offset % bank->sectors[i].size)
+			{
+				LOG_INFO("offset 0x%" PRIx32 " breaks required alignment 0x%" PRIx32 "", offset, bank->sectors[i].size);
+				return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+			}
+		}
+		if (((offset + count) > bank->sectors[i].offset) &&
+			((offset + count) <= (bank->sectors[i].offset + bank->sectors[i].size)) &&
+			(last_sector == 0xffffffff))
+		{
+			last_sector = i;
+		}
+	}
+
+	/* Range check... */
+	if (first_sector == 0xffffffff || last_sector == 0xffffffff)
+	{
+		LOG_INFO("Range check failed %" PRIx32 " %" PRIx32 "", offset, count);
+		return ERROR_FLASH_DST_OUT_OF_BANK;
+	}
+
+	/* Configure the flash controller timing */
+	lpc288x_set_flash_clk(bank);
+
+	/* initialise the offsets */
+	source_offset = 0;
+	dest_offset = 0;
+
+	for (sector = first_sector; sector <= last_sector; sector++)
+	{
+		for (page = 0; page < bank->sectors[sector].size / FLASH_PAGE_SIZE; page++)
+		{
+			if (bytes_remaining == 0)
+			{
+				count = 0;
+				memset(page_buffer, 0xFF, FLASH_PAGE_SIZE);
+			}
+			else if (bytes_remaining < FLASH_PAGE_SIZE)
+			{
+				count = bytes_remaining;
+				memset(page_buffer, 0xFF, FLASH_PAGE_SIZE);
+				memcpy(page_buffer, &buffer[source_offset], count);
+			}
+			else
+			{
+				count = FLASH_PAGE_SIZE;
+				memcpy(page_buffer, &buffer[source_offset], count);
+			}
+
+			/* Wait for flash to become ready */
+			if (lpc288x_wait_status_busy(bank, 1000) != ERROR_OK)
+			{
+				return ERROR_FLASH_OPERATION_FAILED;
+			}
+
+			/* fill flash data latches with 1's */
+			target_write_u32(target, F_CTRL, FC_CS | FC_SET_DATA | FC_WEN | FC_FUNC);
+
+			target_write_u32(target, F_CTRL, FC_CS | FC_WEN | FC_FUNC);
+			/*would be better to use the clean target_write_buffer() interface but
+			 * it seems not to be a LOT slower....
+			 * bulk_write_memory() is no quicker :(*/
+#if 1
+			if (target_write_memory(target, offset + dest_offset, 4, 128, page_buffer) != ERROR_OK)
+			{
+				LOG_ERROR("Write failed s %" PRIx32 " p %" PRIx32 "", sector, page);
+				return ERROR_FLASH_OPERATION_FAILED;
+			}
+#else
+			if (target_write_buffer(target, offset + dest_offset, FLASH_PAGE_SIZE, page_buffer) != ERROR_OK)
+			{
+				LOG_INFO("Write to flash buffer failed");
+				return ERROR_FLASH_OPERATION_FAILED;
+			}
+#endif
+			dest_offset += FLASH_PAGE_SIZE;
+			source_offset += count;
+			bytes_remaining -= count;
+
+			lpc288x_load_timer(LOAD_TIMER_WRITE, target);
+
+			target_write_u32(target, F_CTRL, FC_PROG_REQ | FC_PROTECT | FC_FUNC | FC_CS);
+		}
+	}
+
+	return ERROR_OK;
+}
+
+static int lpc288x_probe(struct flash_bank *bank)
+{
+	/* we only deal with LPC2888 so flash config is fixed */
+	struct lpc288x_flash_bank *lpc288x_info = bank->driver_priv;
+	int retval;
+
+	if (lpc288x_info->cidr != 0)
+	{
+		return ERROR_OK; /* already probed */
+	}
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	retval = lpc288x_read_part_info(bank);
+	if (retval != ERROR_OK)
+		return retval;
+	return ERROR_OK;
+}
+
+static int lpc288x_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	snprintf(buf, buf_size, "lpc288x flash driver");
+	return ERROR_OK;
+}
+
+static int lpc288x_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	int lockregion, status;
+	uint32_t value;
+	struct target *target = bank->target;
+
+	/* probed? halted? */
+	status = lpc288x_system_ready(bank);
+	if (status != ERROR_OK)
+	{
+		return status;
+	}
+
+	if ((first < 0) || (last < first) || (last >= bank->num_sectors))
+	{
+		return ERROR_FLASH_SECTOR_INVALID;
+	}
+
+	/* Configure the flash controller timing */
+	lpc288x_set_flash_clk(bank);
+
+	for (lockregion = first; lockregion <= last; lockregion++)
+	{
+		if (set)
+		{
+			/* write an odd value to base addy to protect... */
+			value = 0x01;
+		}
+		else
+		{
+			/* write an even value to base addy to unprotect... */
+			value = 0x00;
+		}
+		target_write_u32(target, bank->sectors[lockregion].offset, value);
+		target_write_u32(target, F_CTRL, FC_LOAD_REQ | FC_PROTECT | FC_WEN | FC_FUNC | FC_CS);
+	}
+
+	return ERROR_OK;
+}
+
+struct flash_driver lpc288x_flash = {
+		.name = "lpc288x",
+		.flash_bank_command = &lpc288x_flash_bank_command,
+		.erase = &lpc288x_erase,
+		.protect = &lpc288x_protect,
+		.write = &lpc288x_write,
+		.probe = &lpc288x_probe,
+		.auto_probe = &lpc288x_probe,
+		.erase_check = &lpc288x_erase_check,
+		.protect_check = &lpc288x_protect_check,
+		.info = &lpc288x_info,
+	};
diff --git a/src/flash/nor/lpc288x.h b/src/flash/nor/lpc288x.h
new file mode 100644
index 00000000..5a71ee08
--- /dev/null
+++ b/src/flash/nor/lpc288x.h
@@ -0,0 +1,39 @@
+/***************************************************************************
+ *   Copyright (C) 2008 by			                                       *
+ *   Karl RobinSod <karl.robinsod@gmail.com>                               *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+#ifndef lpc288x_H
+#define lpc288x_H
+
+#include "flash.h"
+
+struct lpc288x_flash_bank
+{
+	uint32_t working_area;
+	uint32_t working_area_size;
+
+	/* chip id register */
+	uint32_t cidr;
+	char * target_name;
+	uint32_t cclk;
+
+	uint32_t sector_size_break;
+};
+
+#endif /* lpc288x_H */
diff --git a/src/flash/nor/lpc2900.c b/src/flash/nor/lpc2900.c
new file mode 100644
index 00000000..81e2def4
--- /dev/null
+++ b/src/flash/nor/lpc2900.c
@@ -0,0 +1,1834 @@
+/***************************************************************************
+ *   Copyright (C) 2009 by                                                 *
+ *   Rolf Meeser <rolfm_9dq@yahoo.de>                                      *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+
+#include "image.h"
+#include "flash.h"
+#include "binarybuffer.h"
+#include "armv4_5.h"
+#include "algorithm.h"
+
+
+/* 1024 bytes */
+#define KiB                 1024
+
+/* Some flash constants */
+#define FLASH_PAGE_SIZE     512     /* bytes */
+#define FLASH_ERASE_TIME    100000  /* microseconds */
+#define FLASH_PROGRAM_TIME  1000    /* microseconds */
+
+/* Chip ID / Feature Registers */
+#define CHIPID          0xE0000000  /* Chip ID */
+#define FEAT0           0xE0000100  /* Chip feature 0 */
+#define FEAT1           0xE0000104  /* Chip feature 1 */
+#define FEAT2           0xE0000108  /* Chip feature 2 (contains flash size indicator) */
+#define FEAT3           0xE000010C  /* Chip feature 3 */
+
+#define EXPECTED_CHIPID 0x209CE02B  /* Chip ID of all LPC2900 devices */
+
+/* Flash/EEPROM Control Registers */
+#define FCTR            0x20200000  /* Flash control */
+#define FPTR            0x20200008  /* Flash program-time */
+#define FTCTR           0x2020000C  /* Flash test control */
+#define FBWST           0x20200010  /* Flash bridge wait-state */
+#define FCRA            0x2020001C  /* Flash clock divider */
+#define FMSSTART        0x20200020  /* Flash Built-In Selft Test start address */
+#define FMSSTOP         0x20200024  /* Flash Built-In Selft Test stop address */
+#define FMS16           0x20200028  /* Flash 16-bit signature */
+#define FMSW0           0x2020002C  /* Flash 128-bit signature Word 0 */
+#define FMSW1           0x20200030  /* Flash 128-bit signature Word 1 */
+#define FMSW2           0x20200034  /* Flash 128-bit signature Word 2 */
+#define FMSW3           0x20200038  /* Flash 128-bit signature Word 3 */
+
+#define EECMD           0x20200080  /* EEPROM command */
+#define EEADDR          0x20200084  /* EEPROM address */
+#define EEWDATA         0x20200088  /* EEPROM write data */
+#define EERDATA         0x2020008C  /* EEPROM read data */
+#define EEWSTATE        0x20200090  /* EEPROM wait state */
+#define EECLKDIV        0x20200094  /* EEPROM clock divider */
+#define EEPWRDWN        0x20200098  /* EEPROM power-down/start */
+#define EEMSSTART       0x2020009C  /* EEPROM BIST start address */
+#define EEMSSTOP        0x202000A0  /* EEPROM BIST stop address */
+#define EEMSSIG         0x202000A4  /* EEPROM 24-bit BIST signature */
+
+#define INT_CLR_ENABLE  0x20200FD8  /* Flash/EEPROM interrupt clear enable */
+#define INT_SET_ENABLE  0x20200FDC  /* Flash/EEPROM interrupt set enable */
+#define INT_STATUS      0x20200FE0  /* Flash/EEPROM interrupt status */
+#define INT_ENABLE      0x20200FE4  /* Flash/EEPROM interrupt enable */
+#define INT_CLR_STATUS  0x20200FE8  /* Flash/EEPROM interrupt clear status */
+#define INT_SET_STATUS  0x20200FEC  /* Flash/EEPROM interrupt set status */
+
+/* Interrupt sources */
+#define INTSRC_END_OF_PROG    (1 << 28)
+#define INTSRC_END_OF_BIST    (1 << 27)
+#define INTSRC_END_OF_RDWR    (1 << 26)
+#define INTSRC_END_OF_MISR    (1 << 2)
+#define INTSRC_END_OF_BURN    (1 << 1)
+#define INTSRC_END_OF_ERASE   (1 << 0)
+
+
+/* FCTR bits */
+#define FCTR_FS_LOADREQ       (1 << 15)
+#define FCTR_FS_CACHECLR      (1 << 14)
+#define FCTR_FS_CACHEBYP      (1 << 13)
+#define FCTR_FS_PROGREQ       (1 << 12)
+#define FCTR_FS_RLS           (1 << 11)
+#define FCTR_FS_PDL           (1 << 10)
+#define FCTR_FS_PD            (1 << 9)
+#define FCTR_FS_WPB           (1 << 7)
+#define FCTR_FS_ISS           (1 << 6)
+#define FCTR_FS_RLD           (1 << 5)
+#define FCTR_FS_DCR           (1 << 4)
+#define FCTR_FS_WEB           (1 << 2)
+#define FCTR_FS_WRE           (1 << 1)
+#define FCTR_FS_CS            (1 << 0)
+/* FPTR bits */
+#define FPTR_EN_T             (1 << 15)
+/* FTCTR bits */
+#define FTCTR_FS_BYPASS_R     (1 << 29)
+#define FTCTR_FS_BYPASS_W     (1 << 28)
+/* FMSSTOP bits */
+#define FMSSTOP_MISR_START    (1 << 17)
+/* EEMSSTOP bits */
+#define EEMSSTOP_STRTBIST     (1 << 31)
+
+/* Index sector */
+#define ISS_CUSTOMER_START1   (0x830)
+#define ISS_CUSTOMER_END1     (0xA00)
+#define ISS_CUSTOMER_SIZE1    (ISS_CUSTOMER_END1 - ISS_CUSTOMER_START1)
+#define ISS_CUSTOMER_NWORDS1  (ISS_CUSTOMER_SIZE1 / 4)
+#define ISS_CUSTOMER_START2   (0xA40)
+#define ISS_CUSTOMER_END2     (0xC00)
+#define ISS_CUSTOMER_SIZE2    (ISS_CUSTOMER_END2 - ISS_CUSTOMER_START2)
+#define ISS_CUSTOMER_NWORDS2  (ISS_CUSTOMER_SIZE2 / 4)
+#define ISS_CUSTOMER_SIZE     (ISS_CUSTOMER_SIZE1 + ISS_CUSTOMER_SIZE2)
+
+
+
+/**
+ * Private data for \c lpc2900 flash driver.
+ */
+struct lpc2900_flash_bank
+{
+	/**
+	 * Holds the value read from CHIPID register.
+	 * The driver will not load if the chipid doesn't match the expected
+	 * value of 0x209CE02B of the LPC2900 family. A probe will only be done
+	 * if the chipid does not yet contain the expected value.
+	 */
+	uint32_t chipid;
+
+	/**
+	 * String holding device name.
+	 * This string is set by the probe function to the type number of the
+	 * device. It takes the form "LPC29xx".
+	 */
+	char * target_name;
+
+	/**
+	 * System clock frequency.
+	 * Holds the clock frequency in Hz, as passed by the configuration file
+	 * to the <tt>flash bank</tt> command.
+	 */
+	uint32_t clk_sys_fmc;
+
+	/**
+	 * Flag to indicate that dangerous operations are possible.
+	 * This flag can be set by passing the correct password to the
+	 * <tt>lpc2900 password</tt> command. If set, other dangerous commands,
+	 * which operate on the index sector, can be executed.
+	 */
+	uint32_t risky;
+
+	/**
+	 * Maximum contiguous block of internal SRAM (bytes).
+	 * Autodetected by the driver. Not the total amount of SRAM, only the
+	 * the largest \em contiguous block!
+	 */
+	uint32_t max_ram_block;
+
+};
+
+
+static uint32_t lpc2900_wait_status(struct flash_bank *bank, uint32_t mask, int timeout);
+static void lpc2900_setup(struct flash_bank *bank);
+static uint32_t lpc2900_is_ready(struct flash_bank *bank);
+static uint32_t lpc2900_read_security_status(struct flash_bank *bank);
+static uint32_t lpc2900_run_bist128(struct flash_bank *bank,
+                                    uint32_t addr_from, uint32_t addr_to,
+                                    uint32_t (*signature)[4] );
+static uint32_t lpc2900_address2sector(struct flash_bank *bank, uint32_t offset);
+static uint32_t lpc2900_calc_tr( uint32_t clock, uint32_t time );
+
+
+/***********************  Helper functions  **************************/
+
+
+/**
+ * Wait for an event in mask to occur in INT_STATUS.
+ *
+ * Return when an event occurs, or after a timeout.
+ *
+ * @param[in] bank Pointer to the flash bank descriptor
+ * @param[in] mask Mask to be used for INT_STATUS
+ * @param[in] timeout Timeout in ms
+ */
+static uint32_t lpc2900_wait_status( struct flash_bank *bank,
+                                     uint32_t mask,
+                                     int timeout )
+{
+	uint32_t int_status;
+	struct target *target = bank->target;
+
+
+	do
+	{
+		alive_sleep(1);
+		timeout--;
+		target_read_u32(target, INT_STATUS, &int_status);
+	}
+	while( ((int_status & mask) == 0) && (timeout != 0) );
+
+	if (timeout == 0)
+	{
+		LOG_DEBUG("Timeout!");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	return ERROR_OK;
+}
+
+
+
+/**
+ * Set up the flash for erase/program operations.
+ *
+ * Enable the flash, and set the correct CRA clock of 66 kHz.
+ *
+ * @param bank Pointer to the flash bank descriptor
+ */
+static void lpc2900_setup( struct flash_bank *bank )
+{
+	uint32_t fcra;
+	struct lpc2900_flash_bank *lpc2900_info = bank->driver_priv;
+
+
+	/* Power up the flash block */
+	target_write_u32( bank->target, FCTR, FCTR_FS_WEB | FCTR_FS_CS );
+
+
+	fcra = (lpc2900_info->clk_sys_fmc / (3 * 66000)) - 1;
+	target_write_u32( bank->target, FCRA, fcra );
+}
+
+
+
+/**
+ * Check if device is ready.
+ *
+ * Check if device is ready for flash operation:
+ * Must have been successfully probed.
+ * Must be halted.
+ */
+static uint32_t lpc2900_is_ready( struct flash_bank *bank )
+{
+	struct lpc2900_flash_bank *lpc2900_info = bank->driver_priv;
+
+	if( lpc2900_info->chipid != EXPECTED_CHIPID )
+	{
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	if( bank->target->state != TARGET_HALTED )
+	{
+		LOG_ERROR( "Target not halted" );
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	return ERROR_OK;
+}
+
+
+/**
+ * Read the status of sector security from the index sector.
+ *
+ * @param bank Pointer to the flash bank descriptor
+ */
+static uint32_t lpc2900_read_security_status( struct flash_bank *bank )
+{
+	uint32_t status;
+	if( (status = lpc2900_is_ready( bank )) != ERROR_OK )
+	{
+		return status;
+	}
+
+	struct target *target = bank->target;
+
+	/* Enable ISS access */
+	target_write_u32(target, FCTR, FCTR_FS_CS | FCTR_FS_WEB | FCTR_FS_ISS);
+
+	/* Read the relevant block of memory from the ISS sector */
+	uint32_t iss_secured_field[ 0x230/16 ][ 4 ];
+	target_read_memory(target, bank->base + 0xC00, 4, 0x230/4,
+	                           (uint8_t *)iss_secured_field);
+
+	/* Disable ISS access */
+	target_write_u32(target, FCTR, FCTR_FS_CS | FCTR_FS_WEB);
+
+	/* Check status of each sector. Note that the sector numbering in the LPC2900
+	 * is different from the logical sector numbers used in OpenOCD!
+	 * Refer to the user manual for details.
+	 *
+	 * All zeros (16x 0x00) are treated as a secured sector (is_protected = 1)
+	 * All ones (16x 0xFF) are treated as a non-secured sector (is_protected = 0)
+	 * Anything else is undefined (is_protected = -1). This is treated as
+	 * a protected sector!
+	 */
+	int sector;
+	int index;
+	for( sector = 0; sector < bank->num_sectors; sector++ )
+	{
+		/* Convert logical sector number to physical sector number */
+		if( sector <= 4 )
+		{
+			index = sector + 11;
+		}
+		else if( sector <= 7 )
+		{
+			index = sector + 27;
+		}
+		else
+		{
+			index = sector - 8;
+		}
+
+		bank->sectors[sector].is_protected = -1;
+
+		if (
+		    (iss_secured_field[index][0] == 0x00000000) &&
+		    (iss_secured_field[index][1] == 0x00000000) &&
+		    (iss_secured_field[index][2] == 0x00000000) &&
+		    (iss_secured_field[index][3] == 0x00000000) )
+		{
+			bank->sectors[sector].is_protected = 1;
+		}
+
+		if (
+		    (iss_secured_field[index][0] == 0xFFFFFFFF) &&
+		    (iss_secured_field[index][1] == 0xFFFFFFFF) &&
+		    (iss_secured_field[index][2] == 0xFFFFFFFF) &&
+		    (iss_secured_field[index][3] == 0xFFFFFFFF) )
+		{
+			bank->sectors[sector].is_protected = 0;
+		}
+	}
+
+	return ERROR_OK;
+}
+
+
+/**
+ * Use BIST to calculate a 128-bit hash value over a range of flash.
+ *
+ * @param bank Pointer to the flash bank descriptor
+ * @param addr_from
+ * @param addr_to
+ * @param signature
+ */
+static uint32_t lpc2900_run_bist128(struct flash_bank *bank,
+                                    uint32_t addr_from,
+                                    uint32_t addr_to,
+                                    uint32_t (*signature)[4] )
+{
+	struct target *target = bank->target;
+
+	/* Clear END_OF_MISR interrupt status */
+	target_write_u32( target, INT_CLR_STATUS, INTSRC_END_OF_MISR );
+
+	/* Start address */
+	target_write_u32( target, FMSSTART, addr_from >> 4);
+	/* End address, and issue start command */
+	target_write_u32( target, FMSSTOP, (addr_to >> 4) | FMSSTOP_MISR_START );
+
+	/* Poll for end of operation. Calculate a reasonable timeout. */
+	if( lpc2900_wait_status( bank, INTSRC_END_OF_MISR, 1000 ) != ERROR_OK )
+	{
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	/* Return the signature */
+	target_read_memory( target, FMSW0, 4, 4, (uint8_t *)signature );
+
+	return ERROR_OK;
+}
+
+
+/**
+ * Return sector number for given address.
+ *
+ * Return the (logical) sector number for a given relative address.
+ * No sanity check is done. It assumed that the address is valid.
+ *
+ * @param bank Pointer to the flash bank descriptor
+ * @param offset Offset address relative to bank start
+ */
+static uint32_t lpc2900_address2sector( struct flash_bank *bank,
+                                        uint32_t offset )
+{
+	uint32_t address = bank->base + offset;
+
+
+	/* Run through all sectors of this bank */
+	int sector;
+	for( sector = 0; sector < bank->num_sectors; sector++ )
+	{
+		/* Return immediately if address is within the current sector */
+		if( address < (bank->sectors[sector].offset + bank->sectors[sector].size) )
+		{
+			return sector;
+		}
+	}
+
+	/* We should never come here. If we do, return an arbitrary sector number. */
+	return 0;
+}
+
+
+
+
+/**
+ * Write one page to the index sector.
+ *
+ * @param bank Pointer to the flash bank descriptor
+ * @param pagenum Page number (0...7)
+ * @param page Page array (FLASH_PAGE_SIZE bytes)
+ */
+static int lpc2900_write_index_page( struct flash_bank *bank,
+                                     int pagenum,
+                                     uint8_t (*page)[FLASH_PAGE_SIZE] )
+{
+	/* Only pages 4...7 are user writable */
+	if ((pagenum < 4) || (pagenum > 7))
+	{
+		LOG_ERROR("Refuse to burn index sector page %d", pagenum);
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+	}
+
+	/* Get target, and check if it's halted */
+	struct target *target = bank->target;
+	if( target->state != TARGET_HALTED )
+	{
+		LOG_ERROR( "Target not halted" );
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* Private info */
+	struct lpc2900_flash_bank *lpc2900_info = bank->driver_priv;
+
+	/* Enable flash block and set the correct CRA clock of 66 kHz */
+	lpc2900_setup( bank );
+
+	/* Un-protect the index sector */
+	target_write_u32( target, bank->base, 0 );
+	target_write_u32( target, FCTR,
+	                  FCTR_FS_LOADREQ | FCTR_FS_WPB | FCTR_FS_ISS |
+	                  FCTR_FS_WEB | FCTR_FS_WRE | FCTR_FS_CS );
+
+	/* Set latch load mode */
+	target_write_u32( target, FCTR,
+	                  FCTR_FS_ISS | FCTR_FS_WEB | FCTR_FS_WRE | FCTR_FS_CS );
+
+	/* Write whole page to flash data latches */
+	if( target_write_memory( target,
+	                         bank->base + pagenum * FLASH_PAGE_SIZE,
+	                         4, FLASH_PAGE_SIZE / 4, (uint8_t *)page) != ERROR_OK )
+	{
+		LOG_ERROR("Index sector write failed @ page %d", pagenum);
+		target_write_u32( target, FCTR, FCTR_FS_CS | FCTR_FS_WEB );
+
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	/* Clear END_OF_BURN interrupt status */
+	target_write_u32( target, INT_CLR_STATUS, INTSRC_END_OF_BURN );
+
+	/* Set the program/erase time to FLASH_PROGRAM_TIME */
+	target_write_u32(target, FPTR,
+	                 FPTR_EN_T | lpc2900_calc_tr( lpc2900_info->clk_sys_fmc,
+	                                              FLASH_PROGRAM_TIME ));
+
+	/* Trigger flash write */
+	target_write_u32( target, FCTR,
+	                  FCTR_FS_PROGREQ | FCTR_FS_ISS |
+	                  FCTR_FS_WPB | FCTR_FS_WRE | FCTR_FS_CS );
+
+	/* Wait for the end of the write operation. If it's not over after one
+	 * second, something went dreadfully wrong... :-(
+	 */
+	if (lpc2900_wait_status(bank, INTSRC_END_OF_BURN, 1000) != ERROR_OK)
+	{
+		LOG_ERROR("Index sector write failed @ page %d", pagenum);
+		target_write_u32(target, FCTR, FCTR_FS_CS | FCTR_FS_WEB);
+
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	target_write_u32( target, FCTR, FCTR_FS_CS | FCTR_FS_WEB );
+
+	return ERROR_OK;
+}
+
+
+
+/**
+ * Calculate FPTR.TR register value for desired program/erase time.
+ *
+ * @param clock System clock in Hz
+ * @param time Program/erase time in µs
+ */
+static uint32_t lpc2900_calc_tr( uint32_t clock, uint32_t time )
+{
+	/*           ((time[µs]/1e6) * f[Hz]) + 511
+	 * FPTR.TR = -------------------------------
+	 *                         512
+	 *
+	 * The result is the
+	 */
+
+	uint32_t tr_val = (uint32_t)((((time / 1e6) * clock) + 511.0) / 512.0);
+
+	return tr_val;
+}
+
+
+/***********************  Private flash commands  **************************/
+
+
+/**
+ * Command to determine the signature of the whole flash.
+ *
+ * Uses the Built-In-Self-Test (BIST) to generate a 128-bit hash value
+ * of the flash content.
+ */
+COMMAND_HANDLER(lpc2900_handle_signature_command)
+{
+	uint32_t status;
+	uint32_t signature[4];
+
+
+	if( CMD_ARGC < 1 )
+	{
+		LOG_WARNING( "Too few arguments. Call: lpc2900 signature <bank#>" );
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	if( bank->target->state != TARGET_HALTED )
+	{
+		LOG_ERROR( "Target not halted" );
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* Run BIST over whole flash range */
+	if( (status = lpc2900_run_bist128( bank,
+	                                   bank->base,
+	                                   bank->base + (bank->size - 1),
+	                                   &signature)
+	                                 ) != ERROR_OK )
+	{
+		return status;
+	}
+
+	command_print( CMD_CTX, "signature: 0x%8.8" PRIx32
+	                                  ":0x%8.8" PRIx32
+	                                  ":0x%8.8" PRIx32
+	                                  ":0x%8.8" PRIx32,
+	              signature[3], signature[2], signature[1], signature[0] );
+
+	return ERROR_OK;
+}
+
+
+
+/**
+ * Store customer info in file.
+ *
+ * Read customer info from index sector, and store that block of data into
+ * a disk file. The format is binary.
+ */
+COMMAND_HANDLER(lpc2900_handle_read_custom_command)
+{
+	if( CMD_ARGC < 2 )
+	{
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	struct lpc2900_flash_bank *lpc2900_info = bank->driver_priv;
+	lpc2900_info->risky = 0;
+
+	/* Get target, and check if it's halted */
+	struct target *target = bank->target;
+	if( target->state != TARGET_HALTED )
+	{
+		LOG_ERROR( "Target not halted" );
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* Storage for customer info. Read in two parts */
+	uint32_t customer[ ISS_CUSTOMER_NWORDS1 + ISS_CUSTOMER_NWORDS2 ];
+
+	/* Enable access to index sector */
+	target_write_u32( target, FCTR, FCTR_FS_CS | FCTR_FS_WEB | FCTR_FS_ISS );
+
+	/* Read two parts */
+	target_read_memory( target, bank->base+ISS_CUSTOMER_START1, 4,
+	                            ISS_CUSTOMER_NWORDS1,
+	                            (uint8_t *)&customer[0] );
+	target_read_memory( target, bank->base+ISS_CUSTOMER_START2, 4,
+	                            ISS_CUSTOMER_NWORDS2,
+	                            (uint8_t *)&customer[ISS_CUSTOMER_NWORDS1] );
+
+	/* Deactivate access to index sector */
+	target_write_u32( target, FCTR, FCTR_FS_CS | FCTR_FS_WEB );
+
+	/* Try and open the file */
+	struct fileio fileio;
+	const char *filename = CMD_ARGV[1];
+	int ret = fileio_open( &fileio, filename, FILEIO_WRITE, FILEIO_BINARY );
+	if( ret != ERROR_OK )
+	{
+		LOG_WARNING( "Could not open file %s", filename );
+		return ret;
+	}
+
+	size_t nwritten;
+	ret = fileio_write( &fileio, sizeof(customer),
+                        (const uint8_t *)customer, &nwritten );
+	if( ret != ERROR_OK )
+	{
+		LOG_ERROR( "Write operation to file %s failed", filename );
+		fileio_close( &fileio );
+		return ret;
+	}
+
+	fileio_close( &fileio );
+
+	return ERROR_OK;
+}
+
+
+
+
+/**
+ * Enter password to enable potentially dangerous options.
+ */
+COMMAND_HANDLER(lpc2900_handle_password_command)
+{
+	if (CMD_ARGC < 2)
+	{
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	struct lpc2900_flash_bank *lpc2900_info = bank->driver_priv;
+
+#define ISS_PASSWORD "I_know_what_I_am_doing"
+
+	lpc2900_info->risky = !strcmp( CMD_ARGV[1], ISS_PASSWORD );
+
+	if( !lpc2900_info->risky )
+	{
+		command_print(CMD_CTX, "Wrong password (use '%s')", ISS_PASSWORD);
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+	}
+
+	command_print(CMD_CTX,
+                  "Potentially dangerous operation allowed in next command!");
+
+	return ERROR_OK;
+}
+
+
+
+/**
+ * Write customer info from file to the index sector.
+ */
+COMMAND_HANDLER(lpc2900_handle_write_custom_command)
+{
+	if (CMD_ARGC < 2)
+	{
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	struct lpc2900_flash_bank *lpc2900_info = bank->driver_priv;
+
+	/* Check if command execution is allowed. */
+	if( !lpc2900_info->risky )
+	{
+		command_print( CMD_CTX, "Command execution not allowed!" );
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+	}
+	lpc2900_info->risky = 0;
+
+	/* Get target, and check if it's halted */
+	struct target *target = bank->target;
+	if (target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* The image will always start at offset 0 */
+	struct image image;
+	image.base_address_set = 1;
+	image.base_address = 0;
+	image.start_address_set = 0;
+
+	const char *filename = CMD_ARGV[1];
+	const char *type = (CMD_ARGC >= 3) ? CMD_ARGV[2] : NULL;
+	retval = image_open(&image, filename, type);
+	if (retval != ERROR_OK)
+	{
+		return retval;
+	}
+
+	/* Do a sanity check: The image must be exactly the size of the customer
+	   programmable area. Any other size is rejected. */
+	if( image.num_sections != 1 )
+	{
+		LOG_ERROR("Only one section allowed in image file.");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+	if( (image.sections[0].base_address != 0) ||
+        (image.sections[0].size != ISS_CUSTOMER_SIZE) )
+	{
+		LOG_ERROR("Incorrect image file size. Expected %d, "
+			"got %" PRIu32,
+                   ISS_CUSTOMER_SIZE, image.sections[0].size);
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	/* Well boys, I reckon this is it... */
+
+	/* Customer info is split into two blocks in pages 4 and 5. */
+	uint8_t page[FLASH_PAGE_SIZE];
+
+	/* Page 4 */
+	uint32_t offset = ISS_CUSTOMER_START1 % FLASH_PAGE_SIZE;
+	memset( page, 0xff, FLASH_PAGE_SIZE );
+	size_t size_read;
+	retval = image_read_section( &image, 0, 0,
+	                             ISS_CUSTOMER_SIZE1, &page[offset], &size_read);
+	if( retval != ERROR_OK )
+	{
+		LOG_ERROR("couldn't read from file '%s'", filename);
+		image_close(&image);
+		return retval;
+	}
+	if( (retval = lpc2900_write_index_page( bank, 4, &page )) != ERROR_OK )
+	{
+		image_close(&image);
+		return retval;
+	}
+
+	/* Page 5 */
+	offset = ISS_CUSTOMER_START2 % FLASH_PAGE_SIZE;
+	memset( page, 0xff, FLASH_PAGE_SIZE );
+	retval = image_read_section( &image, 0, ISS_CUSTOMER_SIZE1,
+	                             ISS_CUSTOMER_SIZE2, &page[offset], &size_read);
+	if( retval != ERROR_OK )
+	{
+		LOG_ERROR("couldn't read from file '%s'", filename);
+		image_close(&image);
+		return retval;
+	}
+	if( (retval = lpc2900_write_index_page( bank, 5, &page )) != ERROR_OK )
+	{
+		image_close(&image);
+		return retval;
+	}
+
+	image_close(&image);
+
+	return ERROR_OK;
+}
+
+
+
+/**
+ * Activate 'sector security' for a range of sectors.
+ */
+COMMAND_HANDLER(lpc2900_handle_secure_sector_command)
+{
+	if (CMD_ARGC < 3)
+	{
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	/* Get the bank descriptor */
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	struct lpc2900_flash_bank *lpc2900_info = bank->driver_priv;
+
+	/* Check if command execution is allowed. */
+	if( !lpc2900_info->risky )
+	{
+		command_print( CMD_CTX, "Command execution not allowed! "
+		"(use 'password' command first)");
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+	}
+	lpc2900_info->risky = 0;
+
+	/* Read sector range, and do a sanity check. */
+	int first, last;
+	COMMAND_PARSE_NUMBER(int, CMD_ARGV[1], first);
+	COMMAND_PARSE_NUMBER(int, CMD_ARGV[2], last);
+	if( (first >= bank->num_sectors) ||
+	    (last >= bank->num_sectors) ||
+	    (first > last) )
+	{
+		command_print( CMD_CTX, "Illegal sector range" );
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+	}
+
+	uint8_t page[FLASH_PAGE_SIZE];
+	int sector;
+
+	/* Sectors in page 6 */
+	if( (first <= 4) || (last >= 8) )
+	{
+		memset( &page, 0xff, FLASH_PAGE_SIZE );
+		for( sector = first; sector <= last; sector++ )
+		{
+			if( sector <= 4 )
+			{
+				memset( &page[0xB0 + 16*sector], 0, 16 );
+			}
+			else if( sector >= 8 )
+			{
+				memset( &page[0x00 + 16*(sector - 8)], 0, 16 );
+			}
+		}
+
+		if( (retval = lpc2900_write_index_page( bank, 6, &page )) != ERROR_OK )
+		{
+			LOG_ERROR("failed to update index sector page 6");
+			return retval;
+		}
+	}
+
+	/* Sectors in page 7 */
+	if( (first <= 7) && (last >= 5) )
+	{
+		memset( &page, 0xff, FLASH_PAGE_SIZE );
+		for( sector = first; sector <= last; sector++ )
+		{
+			if( (sector >= 5) && (sector <= 7) )
+			{
+				memset( &page[0x00 + 16*(sector - 5)], 0, 16 );
+			}
+		}
+
+		if( (retval = lpc2900_write_index_page( bank, 7, &page )) != ERROR_OK )
+		{
+			LOG_ERROR("failed to update index sector page 7");
+			return retval;
+		}
+	}
+
+	command_print( CMD_CTX,
+	        "Sectors security will become effective after next power cycle");
+
+	/* Update the sector security status */
+	if ( lpc2900_read_security_status(bank) != ERROR_OK )
+	{
+		LOG_ERROR( "Cannot determine sector security status" );
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	return ERROR_OK;
+}
+
+
+
+/**
+ * Activate JTAG protection.
+ */
+COMMAND_HANDLER(lpc2900_handle_secure_jtag_command)
+{
+	if (CMD_ARGC < 1)
+	{
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	/* Get the bank descriptor */
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	struct lpc2900_flash_bank *lpc2900_info = bank->driver_priv;
+
+	/* Check if command execution is allowed. */
+	if( !lpc2900_info->risky )
+	{
+		command_print( CMD_CTX, "Command execution not allowed! "
+		                        "(use 'password' command first)");
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+	}
+	lpc2900_info->risky = 0;
+
+	/* Prepare page */
+	uint8_t page[FLASH_PAGE_SIZE];
+	memset( &page, 0xff, FLASH_PAGE_SIZE );
+
+
+	/* Insert "soft" protection word */
+	page[0x30 + 15] = 0x7F;
+	page[0x30 + 11] = 0x7F;
+	page[0x30 +  7] = 0x7F;
+	page[0x30 +  3] = 0x7F;
+
+	/* Write to page 5 */
+	if( (retval = lpc2900_write_index_page( bank, 5, &page ))
+			!= ERROR_OK )
+	{
+		LOG_ERROR("failed to update index sector page 5");
+		return retval;
+	}
+
+	LOG_INFO("JTAG security set. Good bye!");
+
+	return ERROR_OK;
+}
+
+
+
+/***********************  Flash interface functions  **************************/
+
+static const struct command_registration lpc2900_exec_command_handlers[] = {
+	{
+		.name = "signature",
+		.handler = &lpc2900_handle_signature_command,
+		.mode = COMMAND_EXEC,
+		.usage = "<bank>",
+		.help = "print device signature of flash bank",
+	},
+	{
+		.name = "read_custom",
+		.handler = &lpc2900_handle_read_custom_command,
+		.mode = COMMAND_EXEC,
+		.usage = "<bank> <filename>",
+		.help = "read customer information from index sector to file",
+	},
+	{
+		.name = "password",
+		.handler = &lpc2900_handle_password_command,
+		.mode = COMMAND_EXEC,
+		.usage = "<bank> <password>",
+		.help = "enter password to enable 'dangerous' options",
+	},
+	{
+		.name = "write_custom",
+		.handler = &lpc2900_handle_write_custom_command,
+		.mode = COMMAND_EXEC,
+		.usage = "<bank> <filename> [<type>]",
+		.help = "write customer info from file to index sector",
+	},
+	{
+		.name = "secure_sector",
+		.handler = &lpc2900_handle_secure_sector_command,
+		.mode = COMMAND_EXEC,
+		.usage = "<bank> <first> <last>",
+		.help = "activate sector security for a range of sectors",
+	},
+	{
+		.name = "secure_jtag",
+		.handler = &lpc2900_handle_secure_jtag_command,
+		.mode = COMMAND_EXEC,
+		.usage = "<bank> <level>",
+		.help = "activate JTAG security",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+static const struct command_registration lpc2900_command_handlers[] = {
+	{
+		.name = "lpc2900",
+		.mode = COMMAND_ANY,
+		.help = "LPC2900 flash command group",
+		.chain = lpc2900_exec_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+/// Evaluate flash bank command.
+FLASH_BANK_COMMAND_HANDLER(lpc2900_flash_bank_command)
+{
+	struct lpc2900_flash_bank *lpc2900_info;
+
+	if (CMD_ARGC < 6)
+	{
+		LOG_WARNING("incomplete flash_bank LPC2900 configuration");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	lpc2900_info = malloc(sizeof(struct lpc2900_flash_bank));
+	bank->driver_priv = lpc2900_info;
+
+	/* Get flash clock.
+	 * Reject it if we can't meet the requirements for program time
+	 * (if clock too slow), or for erase time (clock too fast).
+	 */
+	uint32_t clk_sys_fmc;
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[6], clk_sys_fmc);
+	lpc2900_info->clk_sys_fmc = clk_sys_fmc * 1000;
+
+	uint32_t clock_limit;
+	/* Check program time limit */
+	clock_limit = 512000000l / FLASH_PROGRAM_TIME;
+	if (lpc2900_info->clk_sys_fmc < clock_limit)
+	{
+		LOG_WARNING("flash clock must be at least %" PRIu32 " kHz",
+                    (clock_limit / 1000));
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	/* Check erase time limit */
+	clock_limit = (uint32_t)((32767.0 * 512.0 * 1e6) / FLASH_ERASE_TIME);
+	if (lpc2900_info->clk_sys_fmc > clock_limit)
+	{
+		LOG_WARNING("flash clock must be a maximum of %" PRIu32" kHz",
+                    (clock_limit / 1000));
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	/* Chip ID will be obtained by probing the device later */
+	lpc2900_info->chipid = 0;
+
+	return ERROR_OK;
+}
+
+
+/**
+ * Erase sector(s).
+ *
+ * @param bank Pointer to the flash bank descriptor
+ * @param first First sector to be erased
+ * @param last Last sector (including) to be erased
+ */
+static int lpc2900_erase(struct flash_bank *bank, int first, int last)
+{
+	uint32_t status;
+	int sector;
+	int last_unsecured_sector;
+	struct target *target = bank->target;
+	struct lpc2900_flash_bank *lpc2900_info = bank->driver_priv;
+
+
+	status = lpc2900_is_ready(bank);
+	if (status != ERROR_OK)
+	{
+		return status;
+	}
+
+	/* Sanity check on sector range */
+	if ((first < 0) || (last < first) || (last >= bank->num_sectors))
+	{
+		LOG_INFO("Bad sector range");
+		return ERROR_FLASH_SECTOR_INVALID;
+	}
+
+	/* Update the info about secured sectors */
+	lpc2900_read_security_status( bank );
+
+	/* The selected sector range might include secured sectors. An attempt
+	 * to erase such a sector will cause the erase to fail also for unsecured
+	 * sectors. It is necessary to determine the last unsecured sector now,
+	 * because we have to treat the last relevant sector in the list in
+	 * a special way.
+	 */
+	last_unsecured_sector = -1;
+	for (sector = first; sector <= last; sector++)
+	{
+		if ( !bank->sectors[sector].is_protected )
+		{
+			last_unsecured_sector = sector;
+		}
+	}
+
+	/* Exit now, in case of the rare constellation where all sectors in range
+	 * are secured. This is regarded a success, since erasing/programming of
+	 * secured sectors shall be handled transparently.
+	 */
+	if ( last_unsecured_sector == -1 )
+	{
+		return ERROR_OK;
+	}
+
+	/* Enable flash block and set the correct CRA clock of 66 kHz */
+	lpc2900_setup(bank);
+
+	/* Clear END_OF_ERASE interrupt status */
+	target_write_u32(target, INT_CLR_STATUS, INTSRC_END_OF_ERASE);
+
+	/* Set the program/erase timer to FLASH_ERASE_TIME */
+	target_write_u32(target, FPTR,
+	                 FPTR_EN_T | lpc2900_calc_tr( lpc2900_info->clk_sys_fmc,
+	                                              FLASH_ERASE_TIME ));
+
+	/* Sectors are marked for erasure, then erased all together */
+	for (sector = first; sector <= last_unsecured_sector; sector++)
+	{
+		/* Only mark sectors that aren't secured. Any attempt to erase a group
+		 * of sectors will fail if any single one of them is secured!
+		 */
+		if ( !bank->sectors[sector].is_protected )
+		{
+			/* Unprotect the sector */
+			target_write_u32(target, bank->sectors[sector].offset, 0);
+			target_write_u32(target, FCTR,
+			                 FCTR_FS_LOADREQ | FCTR_FS_WPB |
+			                 FCTR_FS_WEB | FCTR_FS_WRE | FCTR_FS_CS);
+
+			/* Mark the sector for erasure. The last sector in the list
+			   triggers the erasure. */
+			target_write_u32(target, bank->sectors[sector].offset, 0);
+			if ( sector == last_unsecured_sector )
+			{
+				target_write_u32(target, FCTR,
+				                 FCTR_FS_PROGREQ | FCTR_FS_WPB | FCTR_FS_CS);
+			}
+			else
+			{
+				target_write_u32(target, FCTR,
+				                 FCTR_FS_LOADREQ | FCTR_FS_WPB |
+				                 FCTR_FS_WEB | FCTR_FS_CS);
+			}
+		}
+	}
+
+	/* Wait for the end of the erase operation. If it's not over after two seconds,
+	 * something went dreadfully wrong... :-(
+	 */
+	if( lpc2900_wait_status(bank, INTSRC_END_OF_ERASE, 2000) != ERROR_OK )
+	{
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	/* Normal flash operating mode */
+	target_write_u32(target, FCTR, FCTR_FS_CS | FCTR_FS_WEB);
+
+	return ERROR_OK;
+}
+
+
+
+static int lpc2900_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	/* This command is not supported.
+     * "Protection" in LPC2900 terms is handled transparently. Sectors will
+     * automatically be unprotected as needed.
+     * Instead we use the concept of sector security. A secured sector is shown
+     * as "protected" in OpenOCD. Sector security is a permanent feature, and
+     * cannot be disabled once activated.
+     */
+
+	return ERROR_OK;
+}
+
+
+/**
+ * Write data to flash.
+ *
+ * @param bank Pointer to the flash bank descriptor
+ * @param buffer Buffer with data
+ * @param offset Start address (relative to bank start)
+ * @param count Number of bytes to be programmed
+ */
+static int lpc2900_write(struct flash_bank *bank, uint8_t *buffer,
+                         uint32_t offset, uint32_t count)
+{
+	uint8_t page[FLASH_PAGE_SIZE];
+	uint32_t status;
+	uint32_t num_bytes;
+	struct target *target = bank->target;
+	struct lpc2900_flash_bank *lpc2900_info = bank->driver_priv;
+	int sector;
+	int retval;
+
+	static const uint32_t write_target_code[] = {
+		/* Set auto latch mode: FCTR=CS|WRE|WEB */
+		0xe3a0a007,   /* loop       mov r10, #0x007 */
+		0xe583a000,   /*            str r10,[r3,#0] */
+
+		/* Load complete page into latches */
+		0xe3a06020,   /*            mov r6,#(512/16) */
+		0xe8b00f00,   /* next       ldmia r0!,{r8-r11} */
+		0xe8a10f00,   /*            stmia r1!,{r8-r11} */
+		0xe2566001,   /*            subs r6,#1 */
+		0x1afffffb,   /*            bne next */
+
+		/* Clear END_OF_BURN interrupt status */
+		0xe3a0a002,   /*            mov r10,#(1 << 1) */
+		0xe583afe8,   /*            str r10,[r3,#0xfe8] */
+
+		/* Set the erase time to FLASH_PROGRAM_TIME */
+		0xe5834008,   /*            str r4,[r3,#8] */
+
+		/* Trigger flash write
+			FCTR = CS | WRE | WPB | PROGREQ */
+		0xe3a0a083,   /*            mov r10,#0x83 */
+		0xe38aaa01,   /*            orr r10,#0x1000 */
+		0xe583a000,   /*            str r10,[r3,#0] */
+
+		/* Wait for end of burn */
+		0xe593afe0,   /* wait       ldr r10,[r3,#0xfe0] */
+		0xe21aa002,   /*            ands r10,#(1 << 1) */
+		0x0afffffc,   /*            beq wait */
+
+		/* End? */
+		0xe2522001,   /*            subs r2,#1 */
+		0x1affffed,   /*            bne loop */
+
+		0xeafffffe    /* done       b done */
+	};
+
+
+	status = lpc2900_is_ready(bank);
+	if (status != ERROR_OK)
+	{
+		return status;
+	}
+
+	/* Enable flash block and set the correct CRA clock of 66 kHz */
+	lpc2900_setup(bank);
+
+	/* Update the info about secured sectors */
+	lpc2900_read_security_status( bank );
+
+	/* Unprotect all involved sectors */
+	for (sector = 0; sector < bank->num_sectors; sector++)
+	{
+		/* Start address in or before this sector? */
+		/* End address in or behind this sector? */
+		if ( ((bank->base + offset) <
+		          (bank->sectors[sector].offset + bank->sectors[sector].size)) &&
+		     ((bank->base + (offset + count - 1)) >= bank->sectors[sector].offset) )
+		{
+			/* This sector is involved and needs to be unprotected.
+				* Don't do it for secured sectors.
+				*/
+			if ( !bank->sectors[sector].is_protected )
+			{
+				target_write_u32(target, bank->sectors[sector].offset, 0);
+				target_write_u32(target, FCTR,
+				                 FCTR_FS_LOADREQ | FCTR_FS_WPB |
+				                 FCTR_FS_WEB | FCTR_FS_WRE | FCTR_FS_CS);
+			}
+		}
+	}
+
+	/* Set the program/erase time to FLASH_PROGRAM_TIME */
+	uint32_t prog_time = FPTR_EN_T | lpc2900_calc_tr( lpc2900_info->clk_sys_fmc,
+	                                                  FLASH_PROGRAM_TIME );
+
+	/* If there is a working area of reasonable size, use it to program via
+	   a target algorithm. If not, fall back to host programming. */
+
+	/* We need some room for target code. */
+	uint32_t target_code_size = sizeof(write_target_code);
+
+	/* Try working area allocation. Start with a large buffer, and try with
+	   reduced size if that fails. */
+	struct working_area *warea;
+	uint32_t buffer_size = lpc2900_info->max_ram_block - 1 * KiB;
+	while( (retval = target_alloc_working_area(target,
+	                                           buffer_size + target_code_size,
+	                                           &warea)) != ERROR_OK )
+	{
+		/* Try a smaller buffer now, and stop if it's too small. */
+		buffer_size -= 1 * KiB;
+		if (buffer_size < 2 * KiB)
+		{
+			LOG_INFO( "no (large enough) working area"
+			          ", falling back to host mode" );
+			warea = NULL;
+			break;
+		}
+	};
+
+	if( warea )
+	{
+		struct reg_param reg_params[5];
+		struct armv4_5_algorithm armv4_5_info;
+
+		/* We can use target mode. Download the algorithm. */
+		retval = target_write_buffer( target,
+		                              (warea->address)+buffer_size,
+		                              target_code_size,
+		                              (uint8_t *)write_target_code);
+		if (retval != ERROR_OK)
+		{
+			LOG_ERROR("Unable to write block write code to target");
+			target_free_all_working_areas(target);
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+
+		init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
+		init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
+		init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
+		init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);
+		init_reg_param(&reg_params[4], "r4", 32, PARAM_OUT);
+
+		/* Write to flash in large blocks */
+		while ( count != 0 )
+		{
+			uint32_t this_npages;
+			uint8_t *this_buffer;
+			int start_sector = lpc2900_address2sector( bank, offset );
+
+			/* First page / last page / rest */
+			if( offset % FLASH_PAGE_SIZE )
+			{
+				/* Block doesn't start on page boundary.
+				   Burn first partial page separately. */
+				memset( &page, 0xff, sizeof(page) );
+				memcpy( &page[offset % FLASH_PAGE_SIZE],
+				        buffer,
+				        FLASH_PAGE_SIZE - (offset % FLASH_PAGE_SIZE) );
+				this_npages = 1;
+				this_buffer = &page[0];
+				count = count + (offset % FLASH_PAGE_SIZE);
+				offset = offset - (offset % FLASH_PAGE_SIZE);
+			}
+			else if( count < FLASH_PAGE_SIZE )
+			{
+				/* Download last incomplete page separately. */
+				memset( &page, 0xff, sizeof(page) );
+				memcpy( &page, buffer, count );
+				this_npages = 1;
+				this_buffer = &page[0];
+				count = FLASH_PAGE_SIZE;
+			}
+			else
+			{
+				/* Download as many full pages as possible */
+				this_npages = (count < buffer_size) ?
+				               count / FLASH_PAGE_SIZE :
+				               buffer_size / FLASH_PAGE_SIZE;
+				this_buffer = buffer;
+
+				/* Make sure we stop at the next secured sector */
+				int sector = start_sector + 1;
+				while( sector < bank->num_sectors )
+				{
+					/* Secured? */
+					if( bank->sectors[sector].is_protected )
+					{
+						/* Is that next sector within the current block? */
+						if( (bank->sectors[sector].offset - bank->base) <
+							(offset + (this_npages * FLASH_PAGE_SIZE)) )
+						{
+							/* Yes! Split the block */
+							this_npages =
+							  (bank->sectors[sector].offset - bank->base - offset)
+							      / FLASH_PAGE_SIZE;
+							break;
+						}
+					}
+
+					sector++;
+				}
+			}
+
+			/* Skip the current sector if it is secured */
+			if (bank->sectors[start_sector].is_protected)
+			{
+				LOG_DEBUG("Skip secured sector %d",
+						start_sector);
+
+				/* Stop if this is the last sector */
+				if (start_sector == bank->num_sectors - 1)
+				{
+					break;
+				}
+
+				/* Skip */
+				uint32_t nskip = bank->sectors[start_sector].size -
+				                 (offset % bank->sectors[start_sector].size);
+				offset += nskip;
+				buffer += nskip;
+				count = (count >= nskip) ? (count - nskip) : 0;
+				continue;
+			}
+
+			/* Execute buffer download */
+			if ((retval = target_write_buffer(target,
+			                                  warea->address,
+			                                  this_npages * FLASH_PAGE_SIZE,
+			                                  this_buffer)) != ERROR_OK)
+			{
+				LOG_ERROR("Unable to write data to target");
+				target_free_all_working_areas(target);
+				return ERROR_FLASH_OPERATION_FAILED;
+			}
+
+			/* Prepare registers */
+			buf_set_u32(reg_params[0].value, 0, 32, warea->address);
+			buf_set_u32(reg_params[1].value, 0, 32, offset);
+			buf_set_u32(reg_params[2].value, 0, 32, this_npages);
+			buf_set_u32(reg_params[3].value, 0, 32, FCTR);
+			buf_set_u32(reg_params[4].value, 0, 32, FPTR_EN_T | prog_time);
+
+			/* Execute algorithm, assume breakpoint for last instruction */
+			armv4_5_info.common_magic = ARMV4_5_COMMON_MAGIC;
+			armv4_5_info.core_mode = ARMV4_5_MODE_SVC;
+			armv4_5_info.core_state = ARMV4_5_STATE_ARM;
+
+			retval = target_run_algorithm(target, 0, NULL, 5, reg_params,
+				(warea->address) + buffer_size,
+				(warea->address) + buffer_size + target_code_size - 4,
+				10000, /* 10s should be enough for max. 16 KiB of data */
+				&armv4_5_info);
+
+			if (retval != ERROR_OK)
+			{
+				LOG_ERROR("Execution of flash algorithm failed.");
+				target_free_all_working_areas(target);
+				retval = ERROR_FLASH_OPERATION_FAILED;
+				break;
+			}
+
+			count -= this_npages * FLASH_PAGE_SIZE;
+			buffer += this_npages * FLASH_PAGE_SIZE;
+			offset += this_npages * FLASH_PAGE_SIZE;
+		}
+
+		/* Free all resources */
+		destroy_reg_param(&reg_params[0]);
+		destroy_reg_param(&reg_params[1]);
+		destroy_reg_param(&reg_params[2]);
+		destroy_reg_param(&reg_params[3]);
+		destroy_reg_param(&reg_params[4]);
+		target_free_all_working_areas(target);
+	}
+	else
+	{
+		/* Write to flash memory page-wise */
+		while ( count != 0 )
+		{
+			/* How many bytes do we copy this time? */
+			num_bytes = (count >= FLASH_PAGE_SIZE) ?
+			            FLASH_PAGE_SIZE - (offset % FLASH_PAGE_SIZE) :
+			            count;
+
+			/* Don't do anything with it if the page is in a secured sector. */
+			if ( !bank->sectors[lpc2900_address2sector(bank, offset)].is_protected )
+			{
+				/* Set latch load mode */
+				target_write_u32(target, FCTR,
+				                 FCTR_FS_CS | FCTR_FS_WRE | FCTR_FS_WEB);
+
+				/* Always clear the buffer (a little overhead, but who cares) */
+				memset(page, 0xFF, FLASH_PAGE_SIZE);
+
+				/* Copy them to the buffer */
+				memcpy( &page[offset % FLASH_PAGE_SIZE],
+				        &buffer[offset % FLASH_PAGE_SIZE],
+				        num_bytes );
+
+				/* Write whole page to flash data latches */
+				if (target_write_memory(
+				                 target,
+				                 bank->base + (offset - (offset % FLASH_PAGE_SIZE)),
+				                 4, FLASH_PAGE_SIZE / 4, page) != ERROR_OK)
+				{
+					LOG_ERROR("Write failed @ 0x%8.8" PRIx32, offset);
+					target_write_u32(target, FCTR, FCTR_FS_CS | FCTR_FS_WEB);
+
+					return ERROR_FLASH_OPERATION_FAILED;
+				}
+
+				/* Clear END_OF_BURN interrupt status */
+				target_write_u32(target, INT_CLR_STATUS, INTSRC_END_OF_BURN);
+
+				/* Set the programming time */
+				target_write_u32(target, FPTR, FPTR_EN_T | prog_time);
+
+				/* Trigger flash write */
+				target_write_u32(target, FCTR,
+				    FCTR_FS_CS | FCTR_FS_WRE | FCTR_FS_WPB | FCTR_FS_PROGREQ);
+
+				/* Wait for the end of the write operation. If it's not over
+				 * after one second, something went dreadfully wrong... :-(
+				 */
+				if (lpc2900_wait_status(bank, INTSRC_END_OF_BURN, 1000) != ERROR_OK)
+				{
+					LOG_ERROR("Write failed @ 0x%8.8" PRIx32, offset);
+					target_write_u32(target, FCTR, FCTR_FS_CS | FCTR_FS_WEB);
+
+					return ERROR_FLASH_OPERATION_FAILED;
+				}
+			}
+
+			/* Update pointers and counters */
+			offset += num_bytes;
+			buffer += num_bytes;
+			count -= num_bytes;
+		}
+
+		retval = ERROR_OK;
+	}
+
+	/* Normal flash operating mode */
+	target_write_u32(target, FCTR, FCTR_FS_CS | FCTR_FS_WEB);
+
+	return retval;
+}
+
+
+/**
+ * Try and identify the device.
+ *
+ * Determine type number and its memory layout.
+ *
+ * @param bank Pointer to the flash bank descriptor
+ */
+static int lpc2900_probe(struct flash_bank *bank)
+{
+	struct lpc2900_flash_bank *lpc2900_info = bank->driver_priv;
+	struct target *target = bank->target;
+	int i = 0;
+	uint32_t offset;
+
+
+	if (target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* We want to do this only once. Check if we already have a valid CHIPID,
+	 * because then we will have already successfully probed the device.
+	 */
+	if (lpc2900_info->chipid == EXPECTED_CHIPID)
+	{
+		return ERROR_OK;
+	}
+
+	/* Probing starts with reading the CHIPID register. We will continue only
+	 * if this identifies as an LPC2900 device.
+	 */
+	target_read_u32(target, CHIPID, &lpc2900_info->chipid);
+
+	if (lpc2900_info->chipid != EXPECTED_CHIPID)
+	{
+		LOG_WARNING("Device is not an LPC29xx");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	/* It's an LPC29xx device. Now read the feature register FEAT0...FEAT3. */
+	uint32_t feat0, feat1, feat2, feat3;
+	target_read_u32(target, FEAT0, &feat0);
+	target_read_u32(target, FEAT1, &feat1);
+	target_read_u32(target, FEAT2, &feat2);
+	target_read_u32(target, FEAT3, &feat3);
+
+	/* Base address */
+	bank->base = 0x20000000;
+
+	/* Determine flash layout from FEAT2 register */
+	uint32_t num_64k_sectors = (feat2 >> 16) & 0xFF;
+	uint32_t num_8k_sectors = (feat2 >> 0) & 0xFF;
+	bank->num_sectors = num_64k_sectors + num_8k_sectors;
+	bank->size = KiB * (64 * num_64k_sectors + 8 * num_8k_sectors);
+
+	/* Determine maximum contiguous RAM block */
+	lpc2900_info->max_ram_block = 16 * KiB;
+	if( (feat1 & 0x30) == 0x30 )
+	{
+		lpc2900_info->max_ram_block = 32 * KiB;
+		if( (feat1 & 0x0C) == 0x0C )
+		{
+			lpc2900_info->max_ram_block = 48 * KiB;
+		}
+	}
+
+	/* Determine package code and ITCM size */
+	uint32_t package_code = feat0 & 0x0F;
+	uint32_t itcm_code = (feat1 >> 16) & 0x1F;
+
+	/* Determine the exact type number. */
+	uint32_t found = 1;
+	if ( (package_code == 4) && (itcm_code == 5) )
+	{
+		/* Old LPC2917 or LPC2919 (non-/01 devices) */
+		lpc2900_info->target_name = (bank->size == 768*KiB) ? "LPC2919" : "LPC2917";
+	}
+	else
+	{
+		if ( package_code == 2 )
+		{
+			/* 100-pin package */
+			if ( bank->size == 128*KiB )
+			{
+				lpc2900_info->target_name = "LPC2921";
+			}
+			else if ( bank->size == 256*KiB )
+			{
+				lpc2900_info->target_name = "LPC2923";
+			}
+			else if ( bank->size == 512*KiB )
+			{
+				lpc2900_info->target_name = "LPC2925";
+			}
+			else
+			{
+				found = 0;
+			}
+		}
+		else if ( package_code == 4 )
+		{
+			/* 144-pin package */
+			if ( (bank->size == 512*KiB) && (feat3 == 0xFFFFFCF0) )
+			{
+				lpc2900_info->target_name = "LPC2917/01";
+			}
+			else if ( (bank->size == 512*KiB) && (feat3 == 0xFFFFFFF1) )
+			{
+				lpc2900_info->target_name = "LPC2927";
+			}
+			else if ( (bank->size == 768*KiB) && (feat3 == 0xFFFFFCF8) )
+			{
+				lpc2900_info->target_name = "LPC2919/01";
+			}
+			else if ( (bank->size == 768*KiB) && (feat3 == 0xFFFFFFF9) )
+			{
+				lpc2900_info->target_name = "LPC2929";
+			}
+			else
+			{
+				found = 0;
+			}
+		}
+		else if ( package_code == 5 )
+		{
+			/* 208-pin package */
+			lpc2900_info->target_name = (bank->size == 0) ? "LPC2930" : "LPC2939";
+		}
+		else
+		{
+			found = 0;
+		}
+	}
+
+	if ( !found )
+	{
+		LOG_WARNING("Unknown LPC29xx derivative");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	/* Show detected device */
+	LOG_INFO("Flash bank %d"
+	         ": Device %s, %" PRIu32
+	         " KiB in %d sectors",
+	         bank->bank_number,
+	         lpc2900_info->target_name, bank->size / KiB,
+	         bank->num_sectors);
+
+	/* Flashless devices cannot be handled */
+	if ( bank->num_sectors == 0 )
+	{
+		LOG_WARNING("Flashless device cannot be handled");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	/* Sector layout.
+	 * These are logical sector numbers. When doing real flash operations,
+	 * the logical flash number are translated into the physical flash numbers
+	 * of the device.
+	 */
+	bank->sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
+
+	offset = 0;
+	for (i = 0; i < bank->num_sectors; i++)
+	{
+		bank->sectors[i].offset = offset;
+		bank->sectors[i].is_erased = -1;
+		bank->sectors[i].is_protected = -1;
+
+		if ( i <= 7 )
+		{
+			bank->sectors[i].size = 8 * KiB;
+		}
+		else if ( i <= 18 )
+		{
+			bank->sectors[i].size = 64 * KiB;
+		}
+		else
+		{
+			/* We shouldn't come here. But there might be a new part out there
+			 * that has more than 19 sectors. Politely ask for a fix then.
+			 */
+			bank->sectors[i].size = 0;
+			LOG_ERROR("Never heard about sector %d", i);
+		}
+
+		offset += bank->sectors[i].size;
+	}
+
+	/* Read sector security status */
+	if ( lpc2900_read_security_status(bank) != ERROR_OK )
+	{
+		LOG_ERROR("Cannot determine sector security status");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	return ERROR_OK;
+}
+
+
+/**
+ * Run a blank check for each sector.
+ *
+ * For speed reasons, the device isn't read word by word.
+ * A hash value is calculated by the hardware ("BIST") for each sector.
+ * This value is then compared against the known hash of an empty sector.
+ *
+ * @param bank Pointer to the flash bank descriptor
+ */
+static int lpc2900_erase_check(struct flash_bank *bank)
+{
+	uint32_t status = lpc2900_is_ready(bank);
+	if (status != ERROR_OK)
+	{
+		LOG_INFO("Processor not halted/not probed");
+		return status;
+	}
+
+	/* Use the BIST (Built-In Selft Test) to generate a signature of each flash
+	 * sector. Compare against the expected signature of an empty sector.
+	 */
+	int sector;
+	for ( sector = 0; sector < bank->num_sectors; sector++ )
+	{
+		uint32_t signature[4];
+		if ( (status = lpc2900_run_bist128( bank,
+		                                    bank->sectors[sector].offset,
+		                                    bank->sectors[sector].offset +
+		                                       (bank->sectors[sector].size - 1),
+		                                    &signature)) != ERROR_OK )
+		{
+			return status;
+		}
+
+		/* The expected signatures for an empty sector are different
+		 * for 8 KiB and 64 KiB sectors.
+		 */
+		if ( bank->sectors[sector].size == 8*KiB )
+		{
+			bank->sectors[sector].is_erased =
+			    (signature[3] == 0x01ABAAAA) &&
+			    (signature[2] == 0xAAAAAAAA) &&
+			    (signature[1] == 0xAAAAAAAA) &&
+			    (signature[0] == 0xAAA00AAA);
+		}
+		if ( bank->sectors[sector].size == 64*KiB )
+		{
+			bank->sectors[sector].is_erased =
+			    (signature[3] == 0x11801222) &&
+			    (signature[2] == 0xB88844FF) &&
+			    (signature[1] == 0x11A22008) &&
+			    (signature[0] == 0x2B1BFE44);
+		}
+	}
+
+	return ERROR_OK;
+}
+
+
+/**
+ * Get protection (sector security) status.
+ *
+ * Determine the status of "sector security" for each sector.
+ * A secured sector is one that can never be erased/programmed again.
+ *
+ * @param bank Pointer to the flash bank descriptor
+ */
+static int lpc2900_protect_check(struct flash_bank *bank)
+{
+	return lpc2900_read_security_status(bank);
+}
+
+
+/**
+ * Print info about the driver (not the device).
+ *
+ * @param bank Pointer to the flash bank descriptor
+ * @param buf Buffer to take the string
+ * @param buf_size Maximum number of characters that the buffer can take
+ */
+static int lpc2900_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	snprintf(buf, buf_size, "lpc2900 flash driver");
+
+	return ERROR_OK;
+}
+
+
+struct flash_driver lpc2900_flash =
+{
+	.name               = "lpc2900",
+	.commands           = lpc2900_command_handlers,
+	.flash_bank_command = lpc2900_flash_bank_command,
+	.erase              = lpc2900_erase,
+	.protect            = lpc2900_protect,
+	.write              = lpc2900_write,
+	.probe              = lpc2900_probe,
+	.auto_probe         = lpc2900_probe,
+	.erase_check        = lpc2900_erase_check,
+	.protect_check      = lpc2900_protect_check,
+	.info               = lpc2900_info
+};
diff --git a/src/flash/nor/non_cfi.c b/src/flash/nor/non_cfi.c
new file mode 100644
index 00000000..f98b1080
--- /dev/null
+++ b/src/flash/nor/non_cfi.c
@@ -0,0 +1,491 @@
+/***************************************************************************
+ *   Copyright (C) 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  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "non_cfi.h"
+#include "cfi.h"
+
+
+#define KB 1024
+#define MB (1024*1024)
+#define ERASE_REGION(num, size) (((size/256) << 16) | (num-1))
+
+/* non-CFI compatible flashes */
+static struct non_cfi non_cfi_flashes[] = {
+	{
+		.mfr = CFI_MFR_SST,
+		.id = 0xd4,
+		.pri_id = 0x02,
+		.dev_size = 64*KB,
+		.interface_desc = 0x0,		/* x8 only device */
+		.max_buf_write_size = 0x0,
+		.status_poll_mask = CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7,
+		.num_erase_regions = 1,
+		.erase_region_info =
+		{
+			ERASE_REGION(16, 4*KB)
+		}
+	},
+	{
+		.mfr = CFI_MFR_SST,
+		.id = 0xd5,
+		.pri_id = 0x02,
+		.dev_size = 128*KB,
+		.interface_desc = 0x0,		/* x8 only device */
+		.max_buf_write_size = 0x0,
+		.status_poll_mask = CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7,
+		.num_erase_regions = 1,
+		.erase_region_info =
+		{
+			ERASE_REGION(32, 4*KB)
+		}
+	},
+	{
+		.mfr = CFI_MFR_SST,
+		.id = 0xd6,
+		.pri_id = 0x02,
+		.dev_size = 256*KB,
+		.interface_desc = 0x0,		/* x8 only device */
+		.max_buf_write_size = 0x0,
+		.status_poll_mask = CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7,
+		.num_erase_regions = 1,
+		.erase_region_info =
+		{
+			ERASE_REGION(64, 4*KB)
+		}
+	},
+	{
+		.mfr = CFI_MFR_SST,
+		.id = 0xd7,
+		.pri_id = 0x02,
+		.dev_size = 512*KB,
+		.interface_desc = 0x0,		/* x8 only device */
+		.max_buf_write_size = 0x0,
+		.status_poll_mask = CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7,
+		.num_erase_regions = 1,
+		.erase_region_info =
+		{
+			ERASE_REGION(128, 4*KB)
+		}
+	},
+	{
+		.mfr = CFI_MFR_SST,
+		.id = 0x2780,
+		.pri_id = 0x02,
+		.dev_size = 512*KB,
+		.interface_desc = 0x2,		/* x8 or x16 device */
+		.max_buf_write_size = 0x0,
+		.status_poll_mask = CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7,
+		.num_erase_regions = 1,
+		.erase_region_info =
+		{
+			ERASE_REGION(128, 4*KB)
+		}
+	},
+	{
+		.mfr = CFI_MFR_ST,
+		.id = 0xd6,					/* ST29F400BB */
+		.pri_id = 0x02,
+		.dev_size = 512*KB,
+		.interface_desc = 0x2,		/* x8 or x16 device with nBYTE */
+		.max_buf_write_size = 0x0,
+		.status_poll_mask = CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7,
+		.num_erase_regions = 4,
+		.erase_region_info =
+		{
+			ERASE_REGION(1, 16*KB),
+			ERASE_REGION(2,  8*KB),
+			ERASE_REGION(1, 32*KB),
+			ERASE_REGION(7, 64*KB)
+		}
+	},
+	{
+		.mfr = CFI_MFR_ST,
+		.id = 0xd5,					/* ST29F400BT */
+		.pri_id = 0x02,
+		.dev_size = 512*KB,
+		.interface_desc = 0x2,		/* x8 or x16 device with nBYTE */
+		.max_buf_write_size = 0x0,
+		.status_poll_mask = CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7,
+		.num_erase_regions = 4,
+		.erase_region_info =
+		{
+			ERASE_REGION(7, 64*KB),
+			ERASE_REGION(1, 32*KB),
+			ERASE_REGION(2,  8*KB),
+			ERASE_REGION(1, 16*KB)
+		}
+	},
+
+	/* SST 39VF* do not support DQ5 status polling - this currently is
+	   only supported by the host algorithm, not by the target code using
+	   the work area.
+           Only true for 8-bit and 32-bit wide memories. 16-bit wide memories
+           without DQ5 status polling are supported by the target code.
+        */
+	{
+		.mfr = CFI_MFR_SST,
+		.id = 0x2782,				/* SST39xF160 */
+		.pri_id = 0x02,
+		.dev_size = 2*MB,
+		.interface_desc = 0x2,		/* x8 or x16 device with nBYTE */
+		.max_buf_write_size = 0x0,
+		.status_poll_mask = CFI_STATUS_POLL_MASK_DQ6_DQ7,
+		.num_erase_regions = 1,
+		.erase_region_info =
+		{
+			ERASE_REGION(512, 4*KB)
+		}
+	},
+	{
+		.mfr = CFI_MFR_SST,
+		.id = 0x2783,				/* SST39VF320 */
+		.pri_id = 0x02,
+		.dev_size = 4*MB,
+		.interface_desc = 0x2,		/* x8 or x16 device with nBYTE */
+		.max_buf_write_size = 0x0,
+		.status_poll_mask = CFI_STATUS_POLL_MASK_DQ6_DQ7,
+		.num_erase_regions = 1,
+		.erase_region_info =
+		{
+			ERASE_REGION(1024, 4*KB)
+		}
+	},
+	{
+		.mfr = CFI_MFR_SST,
+		.id = 0x234b,				/* SST39VF1601 */
+		.pri_id = 0x02,
+		.dev_size = 2*MB,
+		.interface_desc = 0x2,		/* x8 or x16 device with nBYTE */
+		.max_buf_write_size = 0x0,
+		.status_poll_mask = CFI_STATUS_POLL_MASK_DQ6_DQ7,
+		.num_erase_regions = 1,
+		.erase_region_info =
+		{
+			ERASE_REGION(512, 4*KB)
+		}
+	},
+	{
+		.mfr = CFI_MFR_SST,
+		.id = 0x234a,				/* SST39VF1602 */
+		.pri_id = 0x02,
+		.dev_size = 2*MB,
+		.interface_desc = 0x2,		/* x8 or x16 device with nBYTE */
+		.max_buf_write_size = 0x0,
+		.status_poll_mask = CFI_STATUS_POLL_MASK_DQ6_DQ7,
+		.num_erase_regions = 1,
+		.erase_region_info =
+		{
+			ERASE_REGION(512, 4*KB)
+		}
+	},
+	{
+		.mfr = CFI_MFR_SST,
+		.id = 0x235b,				/* SST39VF3201 */
+		.pri_id = 0x02,
+		.dev_size = 4*MB,
+		.interface_desc = 0x2,		/* x8 or x16 device with nBYTE */
+		.max_buf_write_size = 0x0,
+		.status_poll_mask = CFI_STATUS_POLL_MASK_DQ6_DQ7,
+		.num_erase_regions = 1,
+		.erase_region_info =
+		{
+			ERASE_REGION(1024, 4*KB)
+		}
+	},
+	{
+		.mfr = CFI_MFR_SST,
+		.id = 0x235a,				/* SST39VF3202 */
+		.pri_id = 0x02,
+		.dev_size = 4*MB,
+		.interface_desc = 0x2,		/* x8 or x16 device with nBYTE */
+		.max_buf_write_size = 0x0,
+		.status_poll_mask = CFI_STATUS_POLL_MASK_DQ6_DQ7,
+		.num_erase_regions = 1,
+		.erase_region_info =
+		{
+			ERASE_REGION(1024, 4*KB)
+		}
+	},
+	{
+		.mfr = CFI_MFR_AMD,
+		.id = 0x22ab,				/* AM29F400BB */
+		.pri_id = 0x02,
+		.dev_size = 512*KB,
+		.interface_desc = 0x2,		/* x8 or x16 device with nBYTE */
+		.max_buf_write_size = 0x0,
+		.status_poll_mask = CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7,
+		.num_erase_regions = 4,
+		.erase_region_info =
+		{
+			ERASE_REGION(1, 16*KB),
+			ERASE_REGION(2,  8*KB),
+			ERASE_REGION(1, 32*KB),
+			ERASE_REGION(7, 64*KB)
+		}
+	},
+	{
+		.mfr = CFI_MFR_AMD,
+		.id = 0x2223,				/* AM29F400BT */
+		.pri_id = 0x02,
+		.dev_size = 512*KB,
+		.interface_desc = 0x2,		/* x8 or x16 device with nBYTE */
+		.max_buf_write_size = 0x0,
+		.status_poll_mask = CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7,
+		.num_erase_regions = 4,
+		.erase_region_info =
+		{
+			ERASE_REGION(7, 64*KB),
+			ERASE_REGION(1, 32*KB),
+			ERASE_REGION(2,  8*KB),
+			ERASE_REGION(1, 16*KB)
+		}
+	},
+	{
+		.mfr = CFI_MFR_FUJITSU,
+		.id = 0x226b,				/* AM29SL800DB */
+		.pri_id = 0x02,
+		.dev_size = 1*MB,
+		.interface_desc = 0x2,		/* x8 or x16 device with nBYTE */
+		.max_buf_write_size = 0x0,
+		.status_poll_mask = CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7,
+		.num_erase_regions = 4,
+		.erase_region_info =
+		{
+			ERASE_REGION(1, 16*KB),
+			ERASE_REGION(2,  8*KB),
+			ERASE_REGION(1, 32*KB),
+			ERASE_REGION(15, 64*KB)
+		}
+	},
+	{
+		.mfr = CFI_MFR_AMIC,
+		.id = 0xb31a,				/* A29L800A */
+		.pri_id = 0x02,
+		.dev_size = 1*MB,
+		.interface_desc = 0x2,
+		.max_buf_write_size = 0x0,
+		.status_poll_mask = CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7,
+		.num_erase_regions = 4,
+		.erase_region_info =
+		{
+			ERASE_REGION(1, 16*KB),
+			ERASE_REGION(2,  8*KB),
+			ERASE_REGION(1, 32*KB),
+			ERASE_REGION(15, 64*KB)
+		}
+	},
+	{
+		.mfr = CFI_MFR_MX,
+		.id = 0x225b,				/* MX29LV800B */
+		.pri_id = 0x02,
+		.dev_size = 1*MB,
+		.interface_desc = 0x2,		/* x8 or x16 device with nBYTE */
+		.max_buf_write_size = 0x0,
+		.status_poll_mask = CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7,
+		.num_erase_regions = 4,
+		.erase_region_info =
+		{
+			ERASE_REGION(1, 16*KB),
+			ERASE_REGION(2, 8*KB),
+			ERASE_REGION(1, 32*KB),
+			ERASE_REGION(15, 64*KB)
+		}
+	},
+
+	{
+		.mfr = CFI_MFR_MX,
+		.id = 0x2249,				/* MX29LV160AB: 2MB */
+		.pri_id = 0x02,
+		.dev_size = 2*MB,
+		.interface_desc = 0x2,		/* x8 or x16 device with nBYTE */
+		.max_buf_write_size = 0x0,
+		.status_poll_mask = CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7,
+		.num_erase_regions = 4,
+		.erase_region_info =
+		{
+			ERASE_REGION(1, 16*KB),
+			ERASE_REGION(2, 8*KB),
+			ERASE_REGION(1, 32*KB),
+			ERASE_REGION(31, 64*KB)
+		}
+	},
+	{
+		.mfr = CFI_MFR_MX,
+		.id = 0x22C4,				/* MX29LV160AT: 2MB */
+		.pri_id = 0x02,
+		.dev_size = 2*MB,
+		.interface_desc = 0x2,		/* x8 or x16 device with nBYTE */
+		.max_buf_write_size = 0x0,
+		.status_poll_mask = CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7,
+		.num_erase_regions = 4,
+		.erase_region_info =
+		{
+			ERASE_REGION(31, 64*KB),
+			ERASE_REGION(1, 32*KB),
+			ERASE_REGION(2, 8*KB),
+			ERASE_REGION(1, 16*KB)
+		}
+	},
+	{
+		.mfr = CFI_MFR_ATMEL,
+		.id = 0x00c0,				/* Atmel 49BV1614 */
+		.pri_id = 0x02,
+		.dev_size = 2*MB,
+		.interface_desc = 0x2,		/* x8 or x16 device with nBYTE */
+		.max_buf_write_size = 0x0,
+		.status_poll_mask = CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7,
+		.num_erase_regions = 3,
+		.erase_region_info =
+		{
+			ERASE_REGION(8,  8*KB),
+			ERASE_REGION(2, 32*KB),
+			ERASE_REGION(30, 64*KB)
+		}
+	},
+	{
+		.mfr = CFI_MFR_ATMEL,
+		.id = 0xC2,					/* Atmel 49BV1614T */
+		.pri_id = 0x02,
+		.dev_size = 2*MB,
+		.interface_desc = 0x2,		/* x8 or x16 device with nBYTE */
+		.max_buf_write_size = 0x0,
+		.status_poll_mask = CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7,
+		.num_erase_regions = 3,
+		.erase_region_info =
+		{
+			ERASE_REGION(30, 64*KB),
+			ERASE_REGION(2, 32*KB),
+			ERASE_REGION(8,  8*KB)
+		}
+	},
+	{
+		.mfr = CFI_MFR_AMD,
+		.id = 0x225b,				/* S29AL008D */
+		.pri_id = 0x02,
+		.dev_size = 1*MB,
+		.interface_desc = 0x2,		/* x8 or x16 device with nBYTE */
+		.max_buf_write_size = 0x0,
+		.status_poll_mask = CFI_STATUS_POLL_MASK_DQ5_DQ6_DQ7,
+		.num_erase_regions = 4,
+		.erase_region_info =
+		{
+			ERASE_REGION(1, 16*KB),
+			ERASE_REGION(2, 8*KB),
+			ERASE_REGION(1, 32*KB),
+			ERASE_REGION(15, 64*KB)
+		}
+	},
+	{
+		.mfr = 0,
+		.id = 0,
+	}
+};
+
+void cfi_fixup_non_cfi(struct flash_bank *bank)
+{
+	struct cfi_flash_bank *cfi_info = bank->driver_priv;
+	struct non_cfi *non_cfi = non_cfi_flashes;
+
+	for (non_cfi = non_cfi_flashes; non_cfi->mfr; non_cfi++)
+	{
+		if ((cfi_info->manufacturer == non_cfi->mfr)
+			&& (cfi_info->device_id == non_cfi->id))
+		{
+			break;
+		}
+	}
+
+	/* only fixup jedec flashs found in table */
+	if (!non_cfi->mfr)
+		return;
+
+	cfi_info->not_cfi = 1;
+
+	/* fill in defaults for non-critical data */
+	cfi_info->vcc_min = 0x0;
+	cfi_info->vcc_max = 0x0;
+	cfi_info->vpp_min = 0x0;
+	cfi_info->vpp_max = 0x0;
+	cfi_info->word_write_timeout_typ = 0x0;
+	cfi_info->buf_write_timeout_typ = 0x0;
+	cfi_info->block_erase_timeout_typ = 0x0;
+	cfi_info->chip_erase_timeout_typ = 0x0;
+	cfi_info->word_write_timeout_max = 0x0;
+	cfi_info->buf_write_timeout_max = 0x0;
+	cfi_info->block_erase_timeout_max = 0x0;
+	cfi_info->chip_erase_timeout_max = 0x0;
+
+	cfi_info->qry[0] = 'Q';
+	cfi_info->qry[1] = 'R';
+	cfi_info->qry[2] = 'Y';
+
+	cfi_info->pri_id = non_cfi->pri_id;
+	cfi_info->pri_addr = 0x0;
+	cfi_info->alt_id = 0x0;
+	cfi_info->alt_addr = 0x0;
+	cfi_info->alt_ext = NULL;
+
+	cfi_info->interface_desc = non_cfi->interface_desc;
+	cfi_info->max_buf_write_size = non_cfi->max_buf_write_size;
+	cfi_info->status_poll_mask = non_cfi->status_poll_mask;
+	cfi_info->num_erase_regions = non_cfi->num_erase_regions;
+	cfi_info->erase_region_info = non_cfi->erase_region_info;
+	cfi_info->dev_size = non_cfi->dev_size;
+
+	if (cfi_info->pri_id == 0x2)
+	{
+		struct cfi_spansion_pri_ext *pri_ext = malloc(sizeof(struct cfi_spansion_pri_ext));
+
+		pri_ext->pri[0] = 'P';
+		pri_ext->pri[1] = 'R';
+		pri_ext->pri[2] = 'I';
+
+		pri_ext->major_version = '1';
+		pri_ext->minor_version = '0';
+
+		pri_ext->SiliconRevision = 0x0;
+		pri_ext->EraseSuspend = 0x0;
+		pri_ext->EraseSuspend = 0x0;
+		pri_ext->BlkProt = 0x0;
+		pri_ext->TmpBlkUnprotect = 0x0;
+		pri_ext->BlkProtUnprot = 0x0;
+		pri_ext->SimultaneousOps = 0x0;
+		pri_ext->BurstMode = 0x0;
+		pri_ext->PageMode = 0x0;
+		pri_ext->VppMin = 0x0;
+		pri_ext->VppMax = 0x0;
+		pri_ext->TopBottom = 0x0;
+
+		pri_ext->_unlock1 = 0x5555;
+		pri_ext->_unlock2 = 0x2AAA;
+		pri_ext->_reversed_geometry = 0;
+
+		cfi_info->pri_ext = pri_ext;
+	} else if ((cfi_info->pri_id == 0x1) || (cfi_info->pri_id == 0x3))
+	{
+		LOG_ERROR("BUG: non-CFI flashes using the Intel commandset are not yet supported");
+		exit(-1);
+	}
+}
diff --git a/src/flash/nor/non_cfi.h b/src/flash/nor/non_cfi.h
new file mode 100644
index 00000000..44c92db4
--- /dev/null
+++ b/src/flash/nor/non_cfi.h
@@ -0,0 +1,40 @@
+/***************************************************************************
+ *   Copyright (C) 2007 by Dominic Rath                                    *
+ *   Dominic.Rath@gmx.de                                                   *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifndef NON_CFI_H
+#define NON_CFI_H
+
+#include "flash.h"
+
+struct non_cfi
+{
+	uint16_t mfr;
+	uint16_t id;
+	uint16_t pri_id;
+	uint32_t dev_size;
+	uint16_t interface_desc;
+	uint16_t max_buf_write_size;
+	uint8_t num_erase_regions;
+	uint32_t erase_region_info[6];
+	uint8_t  status_poll_mask;
+};
+
+void cfi_fixup_non_cfi(struct flash_bank *bank);
+
+#endif /* NON_CFI_H */
diff --git a/src/flash/nor/ocl.c b/src/flash/nor/ocl.c
new file mode 100644
index 00000000..388395f0
--- /dev/null
+++ b/src/flash/nor/ocl.c
@@ -0,0 +1,361 @@
+/***************************************************************************
+ *   Copyright (C) 2007 by Pavel Chromy                                    *
+ *   chromy@asix.cz                                                        *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "ocl.h"
+#include "flash.h"
+#include "embeddedice.h"
+
+
+struct ocl_priv
+{
+	struct arm_jtag *jtag_info;
+	unsigned int buflen;
+	unsigned int bufalign;
+};
+
+static int ocl_erase_check(struct flash_bank *bank)
+{
+	return ERROR_OK;
+}
+
+static int ocl_protect_check(struct flash_bank *bank)
+{
+	return ERROR_OK;
+}
+
+/* flash_bank ocl 0 0 0 0 <target#> */
+FLASH_BANK_COMMAND_HANDLER(ocl_flash_bank_command)
+{
+	struct arm7_9_common *arm7_9;
+	struct ocl_priv *ocl;
+
+	if (CMD_ARGC < 6)
+	{
+		LOG_WARNING("incomplete flash_bank ocl configuration");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	arm7_9 = target_to_arm7_9(bank->target);
+	if (!is_arm7_9(arm7_9))
+		return ERROR_TARGET_INVALID;
+
+	ocl = bank->driver_priv = malloc(sizeof(struct ocl_priv));
+	ocl->jtag_info = &arm7_9->jtag_info;
+	ocl->buflen = 0;
+	ocl->bufalign = 1;
+
+	return ERROR_OK;
+}
+
+static int ocl_erase(struct flash_bank *bank, int first, int last)
+{
+	struct ocl_priv *ocl = bank->driver_priv;
+	int retval;
+	uint32_t dcc_buffer[3];
+
+	/* check preconditions */
+	if (bank->num_sectors == 0)
+		return ERROR_FLASH_BANK_NOT_PROBED;
+
+	if (bank->target->state != TARGET_RUNNING)
+	{
+		LOG_ERROR("target has to be running to communicate with the loader");
+		return ERROR_TARGET_NOT_RUNNING;
+	}
+
+	if ((first == 0) && (last == bank->num_sectors - 1))
+	{
+		dcc_buffer[0] = OCL_ERASE_ALL;
+		if ((retval = embeddedice_send(ocl->jtag_info, dcc_buffer, 1) != ERROR_OK))
+			return retval;
+	}
+	else
+	{
+		dcc_buffer[0] = OCL_ERASE_BLOCK;
+		dcc_buffer[1] = first;
+		dcc_buffer[2] = last;
+		if ((retval = embeddedice_send(ocl->jtag_info, dcc_buffer, 3) != ERROR_OK))
+			return retval;
+	}
+
+	/* wait for response, fixed timeout of 1 s */
+	if ((retval = embeddedice_handshake(ocl->jtag_info, EICE_COMM_CTRL_WBIT, 1000) != ERROR_OK))
+	{
+		if (retval == ERROR_TARGET_TIMEOUT)
+			LOG_ERROR("loader not responding");
+		return retval;
+	}
+
+	/* receive response */
+	if ((retval = embeddedice_receive(ocl->jtag_info, dcc_buffer + 1, 1) != ERROR_OK))
+		return retval;
+
+	if (dcc_buffer[1] != OCL_CMD_DONE)
+	{
+		if (dcc_buffer[0] == OCL_ERASE_ALL)
+			LOG_ERROR("loader response to OCL_ERASE_ALL 0x%08" PRIx32 "", dcc_buffer[1]);
+		else
+			LOG_ERROR("loader response to OCL_ERASE_BLOCK 0x%08" PRIx32 "", dcc_buffer[1]);
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	return ERROR_OK;
+}
+
+static int ocl_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	return ERROR_OK;
+}
+
+static int ocl_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	struct ocl_priv *ocl = bank->driver_priv;
+	int retval;
+	uint32_t *dcc_buffer;
+	uint32_t *dcc_bufptr;
+	int byteofs;
+	int runlen;
+	uint32_t chksum;
+
+	int i;
+
+	/* check preconditions */
+	if (ocl->buflen == 0 || ocl->bufalign == 0)
+		return ERROR_FLASH_BANK_NOT_PROBED;
+
+	if (bank->target->state != TARGET_RUNNING)
+	{
+		LOG_ERROR("target has to be running to communicate with the loader");
+		return ERROR_TARGET_NOT_RUNNING;
+	}
+
+	/* allocate buffer for max. ocl buffer + overhead */
+	dcc_buffer = malloc(sizeof(uint32_t)*(ocl->buflen/4 + 3));
+
+	while (count)
+	{
+		if (count + (offset % ocl->bufalign) > ocl->buflen)
+			runlen = ocl->buflen - (offset % ocl->bufalign);
+		else
+			runlen = count;
+
+		dcc_buffer[0] = OCL_FLASH_BLOCK | runlen;
+		dcc_buffer[1] = offset;
+		dcc_bufptr = &dcc_buffer[2];
+
+		*dcc_bufptr = 0xffffffff;
+		byteofs = (offset % ocl->bufalign) % 4;
+		chksum = OCL_CHKS_INIT;
+
+		/* copy data to DCC buffer in proper byte order and properly aligned */
+		for (i = 0; i < runlen; i++)
+		{
+			switch (byteofs++)
+			{
+				case 0:
+					*dcc_bufptr &= *(buffer++) | 0xffffff00;
+					break;
+				case 1:
+					*dcc_bufptr &= ((*(buffer++)) << 8) | 0xffff00ff;
+					break;
+				case 2:
+					*dcc_bufptr &= ((*(buffer++)) << 16) | 0xff00ffff;
+					break;
+				case 3:
+					*dcc_bufptr &= ((*(buffer++)) << 24) | 0x00ffffff;
+					chksum ^= *(dcc_bufptr++);
+					*dcc_bufptr = 0xffffffff;
+					byteofs = 0;
+					break;
+			}
+		}
+
+		/* add the remaining word to checksum */
+		if (byteofs)
+			chksum ^= *(dcc_bufptr++);
+
+		*(dcc_bufptr++) = chksum;
+
+		/* send the data */
+		if ((retval = embeddedice_send(ocl->jtag_info, dcc_buffer, dcc_bufptr-dcc_buffer)) != ERROR_OK)
+		{
+			free(dcc_buffer);
+		  return retval;
+		}
+
+		/* wait for response, fixed timeout of 1 s */
+		if ((retval = embeddedice_handshake(ocl->jtag_info, EICE_COMM_CTRL_WBIT, 1000) != ERROR_OK))
+		{
+			if (retval == ERROR_TARGET_TIMEOUT)
+				LOG_ERROR("loader not responding");
+			free(dcc_buffer);
+			return retval;
+		}
+
+		/* receive response */
+		if ((retval = embeddedice_receive(ocl->jtag_info, dcc_buffer, 1) != ERROR_OK))
+		{
+			free(dcc_buffer);
+			return retval;
+		}
+
+		if (dcc_buffer[0] != OCL_CMD_DONE)
+		{
+			LOG_ERROR("loader response to OCL_FLASH_BLOCK 0x%08" PRIx32 "", dcc_buffer[0]);
+			free(dcc_buffer);
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+
+		count -= runlen;
+		offset += runlen;
+	}
+
+	free(dcc_buffer);
+	return ERROR_OK;
+}
+
+static int ocl_probe(struct flash_bank *bank)
+{
+	struct ocl_priv *ocl = bank->driver_priv;
+	int retval;
+	uint32_t dcc_buffer[1];
+	int sectsize;
+	int i;
+
+	/* purge pending data in DCC */
+	embeddedice_receive(ocl->jtag_info, dcc_buffer, 1);
+
+	dcc_buffer[0] = OCL_PROBE;
+	if ((retval = embeddedice_send(ocl->jtag_info, dcc_buffer, 1) != ERROR_OK))
+		return retval;
+
+	/* wait for response, fixed timeout of 1 s */
+	if ((retval = embeddedice_handshake(ocl->jtag_info, EICE_COMM_CTRL_WBIT, 1000) != ERROR_OK))
+	{
+		if (retval == ERROR_TARGET_TIMEOUT)
+			LOG_ERROR("loader not responding");
+		return retval;
+	}
+
+	/* receive response */
+	if ((retval = embeddedice_receive(ocl->jtag_info, dcc_buffer, 1) != ERROR_OK))
+		return retval;
+
+	if (dcc_buffer[0] != OCL_CMD_DONE)
+	{
+		LOG_ERROR("loader response to OCL_PROBE 0x%08" PRIx32 "", dcc_buffer[0]);
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	/* receive and fill in parameters, detection of loader is important, receive it one by one */
+	if ((retval = embeddedice_handshake(ocl->jtag_info, EICE_COMM_CTRL_WBIT, 0) != ERROR_OK)
+		|| (retval = embeddedice_receive(ocl->jtag_info, dcc_buffer, 1) != ERROR_OK))
+		return retval;
+	bank->base = dcc_buffer[0];
+
+	if ((retval = embeddedice_handshake(ocl->jtag_info, EICE_COMM_CTRL_WBIT, 0) != ERROR_OK)
+		|| (retval = embeddedice_receive(ocl->jtag_info, dcc_buffer, 1) != ERROR_OK))
+		return retval;
+	bank->size = dcc_buffer[0];
+
+	if ((retval = embeddedice_handshake(ocl->jtag_info, EICE_COMM_CTRL_WBIT, 0) != ERROR_OK)
+		|| (retval = embeddedice_receive(ocl->jtag_info, dcc_buffer, 1) != ERROR_OK))
+		return retval;
+	bank->num_sectors = dcc_buffer[0];
+
+	if ((retval = embeddedice_handshake(ocl->jtag_info, EICE_COMM_CTRL_WBIT, 0) != ERROR_OK)
+		|| (retval = embeddedice_receive(ocl->jtag_info, dcc_buffer, 1) != ERROR_OK))
+		return retval;
+	ocl->buflen = dcc_buffer[0] & 0xffff;
+	ocl->bufalign = dcc_buffer[0] >> 16;
+
+	bank->sectors = realloc(bank->sectors, sizeof(struct flash_sector)*bank->num_sectors);
+	if (bank->num_sectors == 0)
+	{
+		LOG_ERROR("number of sectors shall be non zero value");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+	if (bank->size % bank->num_sectors) {
+		LOG_ERROR("bank size not divisible by number of sectors");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+	sectsize = bank->size / bank->num_sectors;
+	for (i = 0; i < bank->num_sectors; i++)
+	{
+		bank->sectors[i].offset = i * sectsize;
+		bank->sectors[i].size = sectsize;
+		bank->sectors[i].is_erased = -1;
+		bank->sectors[i].is_protected = -1;
+	}
+
+	if (ocl->bufalign == 0)
+		ocl->bufalign = 1;
+
+	if (ocl->buflen == 0)
+	{
+		LOG_ERROR("buflen shall be non zero value");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	if ((ocl->bufalign > ocl->buflen) || (ocl->buflen % ocl->bufalign))
+	{
+		LOG_ERROR("buflen is not multiple of bufalign");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	if (ocl->buflen % 4)
+	{
+		LOG_ERROR("buflen shall be divisible by 4");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	return ERROR_OK;
+}
+
+static int ocl_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	return ERROR_OK;
+}
+
+static int ocl_auto_probe(struct flash_bank *bank)
+{
+	struct ocl_priv *ocl = bank->driver_priv;
+
+	if (ocl->buflen == 0 || ocl->bufalign == 0)
+		return ERROR_FLASH_BANK_NOT_PROBED;
+
+	return ERROR_OK;
+}
+
+struct flash_driver ocl_flash = {
+		.name = "ocl",
+		.flash_bank_command = &ocl_flash_bank_command,
+		.erase = &ocl_erase,
+		.protect = &ocl_protect,
+		.write = &ocl_write,
+		.probe = &ocl_probe,
+		.erase_check = &ocl_erase_check,
+		.protect_check = &ocl_protect_check,
+		.info = &ocl_info,
+		.auto_probe = &ocl_auto_probe,
+	};
diff --git a/src/flash/nor/ocl.h b/src/flash/nor/ocl.h
new file mode 100644
index 00000000..d5c430be
--- /dev/null
+++ b/src/flash/nor/ocl.h
@@ -0,0 +1,40 @@
+/***************************************************************************
+ *   Copyright (C) 2007 by Pavel Chromy                                    *
+ *   chromy@asix.cz                                                        *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifndef OCL_H
+#define OCL_H
+
+/* command/response mask */
+#define OCL_CMD_MASK 0xFFFF0000L
+
+/* commads */
+#define OCL_FLASH_BLOCK 0x0CFB0000L
+#define OCL_ERASE_BLOCK 0x0CEB0000L
+#define OCL_ERASE_ALL 0x0CEA0000L
+#define OCL_PROBE 0x0CBE0000L
+
+/* responses */
+#define OCL_CMD_DONE 0x0ACD0000L
+#define OCL_CMD_ERR 0x0ACE0000L
+#define OCL_CHKS_FAIL 0x0ACF0000L
+#define OCL_BUFF_OVER 0x0AB00000L
+
+#define OCL_CHKS_INIT 0xC100CD0CL
+
+#endif /* OCL_H */
diff --git a/src/flash/nor/pic32mx.c b/src/flash/nor/pic32mx.c
new file mode 100644
index 00000000..9bb6c97e
--- /dev/null
+++ b/src/flash/nor/pic32mx.c
@@ -0,0 +1,922 @@
+/***************************************************************************
+ *   Copyright (C) 2005 by Dominic Rath                                    *
+ *   Dominic.Rath@gmx.de                                                   *
+ *                                                                         *
+ *   Copyright (C) 2008 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
+ *   Copyright (C) 2008 by John McCarthy                                   *
+ *   jgmcc@magma.ca                                                        *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "pic32mx.h"
+#include "mips32.h"
+
+
+static
+struct pic32mx_devs_s {
+	uint8_t	devid;
+	char	*name;
+	uint32_t	pfm_size;
+} pic32mx_devs[] = {
+	{ 0x78, "460F512L USB", 512 },
+	{ 0x74, "460F256L USB", 256 },
+	{ 0x6D, "440F128L USB", 128 },
+	{ 0x56, "440F512H USB", 512 },
+	{ 0x52, "440F256H USB", 256 },
+	{ 0x4D, "440F128H USB", 128 },
+	{ 0x42, "420F032H USB",  32 },
+	{ 0x38, "360F512L",     512 },
+	{ 0x34, "360F256L",     256 },
+	{ 0x2D, "340F128L",     128 },
+	{ 0x2A, "320F128L",     128 },
+	{ 0x16, "340F512H",     512 },
+	{ 0x12, "340F256H",     256 },
+	{ 0x0D, "340F128H",     128 },
+	{ 0x0A, "320F128H",     128 },
+	{ 0x06, "320F064H",      64 },
+	{ 0x02, "320F032H",      32 },
+	{ 0x00, NULL, 0 }
+};
+
+static int pic32mx_write_row(struct flash_bank *bank, uint32_t address, uint32_t srcaddr);
+static int pic32mx_write_word(struct flash_bank *bank, uint32_t address, uint32_t word);
+
+/* flash bank pic32mx <base> <size> 0 0 <target#>
+ */
+FLASH_BANK_COMMAND_HANDLER(pic32mx_flash_bank_command)
+{
+	struct pic32mx_flash_bank *pic32mx_info;
+
+	if (CMD_ARGC < 6)
+	{
+		LOG_WARNING("incomplete flash_bank pic32mx configuration");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	pic32mx_info = malloc(sizeof(struct pic32mx_flash_bank));
+	bank->driver_priv = pic32mx_info;
+
+	pic32mx_info->write_algorithm = NULL;
+	pic32mx_info->probed = 0;
+
+	return ERROR_OK;
+}
+
+static uint32_t pic32mx_get_flash_status(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	uint32_t status;
+
+	target_read_u32(target, PIC32MX_NVMCON, &status);
+
+	return status;
+}
+
+static uint32_t pic32mx_wait_status_busy(struct flash_bank *bank, int timeout)
+{
+	uint32_t status;
+
+	/* wait for busy to clear */
+	while (((status = pic32mx_get_flash_status(bank)) & NVMCON_NVMWR) && (timeout-- > 0))
+	{
+		LOG_DEBUG("status: 0x%" PRIx32, status);
+		alive_sleep(1);
+	}
+	if (timeout <= 0)
+		LOG_DEBUG("timeout: status: 0x%" PRIx32, status);
+
+	return status;
+}
+
+static int pic32mx_nvm_exec(struct flash_bank *bank, uint32_t op, uint32_t timeout)
+{
+	struct target *target = bank->target;
+	uint32_t status;
+
+	target_write_u32(target, PIC32MX_NVMCON, NVMCON_NVMWREN | op);
+
+	/* unlock flash registers */
+	target_write_u32(target, PIC32MX_NVMKEY, NVMKEY1);
+	target_write_u32(target, PIC32MX_NVMKEY, NVMKEY2);
+
+	/* start operation */
+	target_write_u32(target, PIC32MX_NVMCONSET, NVMCON_NVMWR);
+
+	status = pic32mx_wait_status_busy(bank, timeout);
+
+	/* lock flash registers */
+	target_write_u32(target, PIC32MX_NVMCONCLR, NVMCON_NVMWREN);
+
+	return status;
+}
+
+static int pic32mx_protect_check(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+
+	uint32_t devcfg0;
+	int s;
+	int num_pages;
+
+	if (target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	target_read_u32(target, PIC32MX_DEVCFG0, &devcfg0);
+	if ((devcfg0 & (1 << 28)) == 0) /* code protect bit */
+		num_pages = 0xffff;  /* All pages protected */
+	else if (bank->base == PIC32MX_KSEG1_BOOT_FLASH)
+	{
+		if (devcfg0 & (1 << 24))
+			num_pages = 0;       /* All pages unprotected */
+		else
+			num_pages = 0xffff;  /* All pages protected */
+	}
+	else /* pgm flash */
+		num_pages = (~devcfg0 >> 12) & 0xff;
+	for (s = 0; s < bank->num_sectors && s < num_pages; s++)
+		bank->sectors[s].is_protected = 1;
+	for (; s < bank->num_sectors; s++)
+		bank->sectors[s].is_protected = 0;
+
+	return ERROR_OK;
+}
+
+static int pic32mx_erase(struct flash_bank *bank, int first, int last)
+{
+	struct target *target = bank->target;
+	int i;
+	uint32_t status;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if ((first == 0) && (last == (bank->num_sectors - 1)) && (bank->base == PIC32MX_KSEG0_PGM_FLASH || bank->base == PIC32MX_KSEG1_PGM_FLASH))
+	{
+		LOG_DEBUG("Erasing entire program flash");
+		status = pic32mx_nvm_exec(bank, NVMCON_OP_PFM_ERASE, 50);
+		if (status & NVMCON_NVMERR)
+			return ERROR_FLASH_OPERATION_FAILED;
+		if (status & NVMCON_LVDERR)
+			return ERROR_FLASH_OPERATION_FAILED;
+		return ERROR_OK;
+	}
+
+	for (i = first; i <= last; i++)
+	{
+		if (bank->base >= PIC32MX_KSEG1_PGM_FLASH)
+			target_write_u32(target, PIC32MX_NVMADDR, KS1Virt2Phys(bank->base + bank->sectors[i].offset));
+		else
+			target_write_u32(target, PIC32MX_NVMADDR, KS0Virt2Phys(bank->base + bank->sectors[i].offset));
+
+		status = pic32mx_nvm_exec(bank, NVMCON_OP_PAGE_ERASE, 10);
+
+		if (status & NVMCON_NVMERR)
+			return ERROR_FLASH_OPERATION_FAILED;
+		if (status & NVMCON_LVDERR)
+			return ERROR_FLASH_OPERATION_FAILED;
+		bank->sectors[i].is_erased = 1;
+	}
+
+	return ERROR_OK;
+}
+
+static int pic32mx_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	struct pic32mx_flash_bank *pic32mx_info = NULL;
+	struct target *target = bank->target;
+#if 0
+	uint16_t prot_reg[4] = {0xFFFF, 0xFFFF, 0xFFFF, 0xFFFF};
+	int i, reg, bit;
+	int status;
+	uint32_t protection;
+#endif
+
+	pic32mx_info = bank->driver_priv;
+
+	if (target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+#if 0
+	if ((first && (first % pic32mx_info->ppage_size)) || ((last + 1) && (last + 1) % pic32mx_info->ppage_size))
+	{
+		LOG_WARNING("sector start/end incorrect - stm32 has %dK sector protection", pic32mx_info->ppage_size);
+		return ERROR_FLASH_SECTOR_INVALID;
+	}
+
+	/* medium density - each bit refers to a 4bank protection
+	 * high density - each bit refers to a 2bank protection */
+	target_read_u32(target, PIC32MX_FLASH_WRPR, &protection);
+
+	prot_reg[0] = (uint16_t)protection;
+	prot_reg[1] = (uint16_t)(protection >> 8);
+	prot_reg[2] = (uint16_t)(protection >> 16);
+	prot_reg[3] = (uint16_t)(protection >> 24);
+
+	if (pic32mx_info->ppage_size == 2)
+	{
+		/* high density flash */
+
+		/* bit 7 controls sector 62 - 255 protection */
+		if (last > 61)
+		{
+			if (set)
+				prot_reg[3] &= ~(1 << 7);
+			else
+				prot_reg[3] |= (1 << 7);
+		}
+
+		if (first > 61)
+			first = 62;
+		if (last > 61)
+			last = 61;
+
+		for (i = first; i <= last; i++)
+		{
+			reg = (i / pic32mx_info->ppage_size) / 8;
+			bit = (i / pic32mx_info->ppage_size) - (reg * 8);
+
+			if (set)
+				prot_reg[reg] &= ~(1 << bit);
+			else
+				prot_reg[reg] |= (1 << bit);
+		}
+	}
+	else
+	{
+		/* medium density flash */
+		for (i = first; i <= last; i++)
+		{
+			reg = (i / pic32mx_info->ppage_size) / 8;
+			bit = (i / pic32mx_info->ppage_size) - (reg * 8);
+
+			if (set)
+				prot_reg[reg] &= ~(1 << bit);
+			else
+				prot_reg[reg] |= (1 << bit);
+		}
+	}
+
+	if ((status = pic32mx_erase_options(bank)) != ERROR_OK)
+		return status;
+
+	pic32mx_info->option_bytes.protection[0] = prot_reg[0];
+	pic32mx_info->option_bytes.protection[1] = prot_reg[1];
+	pic32mx_info->option_bytes.protection[2] = prot_reg[2];
+	pic32mx_info->option_bytes.protection[3] = prot_reg[3];
+
+	return pic32mx_write_options(bank);
+#else
+	return ERROR_OK;
+#endif
+}
+
+static int pic32mx_write_block(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	uint32_t buffer_size = 512;
+	struct working_area *source;
+	uint32_t address = bank->base + offset;
+	int retval = ERROR_OK;
+#if 0
+	struct pic32mx_flash_bank *pic32mx_info = bank->driver_priv;
+	struct armv7m_algorithm armv7m_info;
+
+	uint8_t pic32mx_flash_write_code[] = {
+									/* write: */
+		0xDF, 0xF8, 0x24, 0x40,		/* ldr	r4, PIC32MX_FLASH_CR */
+		0x09, 0x4D,					/* ldr	r5, PIC32MX_FLASH_SR */
+		0x4F, 0xF0, 0x01, 0x03,		/* mov	r3, #1 */
+		0x23, 0x60,					/* str	r3, [r4, #0] */
+		0x30, 0xF8, 0x02, 0x3B,		/* ldrh r3, [r0], #2 */
+		0x21, 0xF8, 0x02, 0x3B,		/* strh r3, [r1], #2 */
+									/* busy: */
+		0x2B, 0x68,					/* ldr 	r3, [r5, #0] */
+		0x13, 0xF0, 0x01, 0x0F,		/* tst 	r3, #0x01 */
+		0xFB, 0xD0,					/* beq 	busy */
+		0x13, 0xF0, 0x14, 0x0F,		/* tst	r3, #0x14 */
+		0x01, 0xD1,					/* bne	exit */
+		0x01, 0x3A,					/* subs	r2, r2, #1 */
+		0xED, 0xD1,					/* bne	write */
+									/* exit: */
+		0xFE, 0xE7,					/* b exit */
+		0x10, 0x20, 0x02, 0x40,		/* PIC32MX_FLASH_CR:	.word 0x40022010 */
+		0x0C, 0x20, 0x02, 0x40		/* PIC32MX_FLASH_SR:	.word 0x4002200C */
+	};
+
+	/* flash write code */
+	if (target_alloc_working_area(target, sizeof(pic32mx_flash_write_code), &pic32mx_info->write_algorithm) != ERROR_OK)
+	{
+		LOG_WARNING("no working area available, can't do block memory writes");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	};
+
+	if ((retval = target_write_buffer(target, pic32mx_info->write_algorithm->address, sizeof(pic32mx_flash_write_code), pic32mx_flash_write_code)) != ERROR_OK)
+		return retval;
+#endif
+
+	/* memory buffer */
+	if (target_alloc_working_area(target, buffer_size, &source) != ERROR_OK)
+	{
+#if 0
+		/* if we already allocated the writing code, but failed to get a buffer, free the algorithm */
+		if (pic32mx_info->write_algorithm)
+			target_free_working_area(target, pic32mx_info->write_algorithm);
+#endif
+
+		LOG_WARNING("no large enough working area available, can't do block memory writes");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	while (count >= buffer_size/4)
+	{
+		uint32_t status;
+
+		if ((retval = target_write_buffer(target, source->address, buffer_size, buffer)) != ERROR_OK) {
+			LOG_ERROR("Failed to write row buffer (%d words) to RAM", (int)(buffer_size/4));
+			break;
+		}
+
+#if 0
+		buf_set_u32(reg_params[0].value, 0, 32, source->address);
+		buf_set_u32(reg_params[1].value, 0, 32, address);
+		buf_set_u32(reg_params[2].value, 0, 32, buffer_size/4);
+
+		if ((retval = target_run_algorithm(target, 0, NULL, 4, reg_params, pic32mx_info->write_algorithm->address, \
+				pic32mx_info->write_algorithm->address + (sizeof(pic32mx_flash_write_code) - 10), 10000, &armv7m_info)) != ERROR_OK)
+		{
+			LOG_ERROR("error executing pic32mx flash write algorithm");
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			break;
+		}
+
+		if (buf_get_u32(reg_params[3].value, 0, 32) & 0x14)
+		{
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			break;
+		}
+#endif
+		status = pic32mx_write_row(bank, address, source->address);
+		if (status & NVMCON_NVMERR) {
+			LOG_ERROR("Flash write error NVMERR (status = 0x%08" PRIx32 ")", status);
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			break;
+		}
+		if (status & NVMCON_LVDERR) {
+			LOG_ERROR("Flash write error LVDERR (status = 0x%08" PRIx32 ")", status);
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			break;
+		}
+
+		buffer  += buffer_size;
+		address += buffer_size;
+		count   -= buffer_size/4;
+	}
+
+	target_free_working_area(target, source);
+
+	while (count > 0)
+	{
+		uint32_t value;
+		memcpy(&value, buffer, sizeof(uint32_t));
+
+		uint32_t status = pic32mx_write_word(bank, address, value);
+		if (status & NVMCON_NVMERR) {
+			LOG_ERROR("Flash write error NVMERR (status = 0x%08" PRIx32 ")", status);
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			break;
+		}
+		if (status & NVMCON_LVDERR) {
+			LOG_ERROR("Flash write error LVDERR (status = 0x%08" PRIx32 ")", status);
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			break;
+		}
+
+		buffer  += 4;
+		address += 4;
+		count--;
+	}
+
+	return retval;
+}
+
+static int pic32mx_write_word(struct flash_bank *bank, uint32_t address, uint32_t word)
+{
+	struct target *target = bank->target;
+
+	if (bank->base >= PIC32MX_KSEG1_PGM_FLASH)
+		target_write_u32(target, PIC32MX_NVMADDR, KS1Virt2Phys(address));
+	else
+		target_write_u32(target, PIC32MX_NVMADDR, KS0Virt2Phys(address));
+	target_write_u32(target, PIC32MX_NVMDATA, word);
+
+	return pic32mx_nvm_exec(bank, NVMCON_OP_WORD_PROG, 5);
+}
+
+/*
+ * Write a 128 word (512 byte) row to flash address from RAM srcaddr.
+ */
+static int pic32mx_write_row(struct flash_bank *bank, uint32_t address, uint32_t srcaddr)
+{
+	struct target *target = bank->target;
+
+	LOG_DEBUG("addr: 0x%08" PRIx32 " srcaddr: 0x%08" PRIx32 "", address, srcaddr);
+
+	if (address >= PIC32MX_KSEG1_PGM_FLASH)
+		target_write_u32(target, PIC32MX_NVMADDR,    KS1Virt2Phys(address));
+	else
+		target_write_u32(target, PIC32MX_NVMADDR,    KS0Virt2Phys(address));
+	if (srcaddr >= PIC32MX_KSEG1_RAM)
+		target_write_u32(target, PIC32MX_NVMSRCADDR, KS1Virt2Phys(srcaddr));
+	else
+		target_write_u32(target, PIC32MX_NVMSRCADDR, KS0Virt2Phys(srcaddr));
+
+	return pic32mx_nvm_exec(bank, NVMCON_OP_ROW_PROG, 100);
+}
+
+static int pic32mx_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	uint32_t words_remaining = (count / 4);
+	uint32_t bytes_remaining = (count & 0x00000003);
+	uint32_t address = bank->base + offset;
+	uint32_t bytes_written = 0;
+	uint32_t status;
+	int retval;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (offset & 0x3)
+	{
+		LOG_WARNING("offset 0x%" PRIx32 "breaks required 4-byte alignment", offset);
+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+	}
+
+	/* multiple words (4-byte) to be programmed? */
+	if (words_remaining > 0)
+	{
+		/* try using a block write */
+		if ((retval = pic32mx_write_block(bank, buffer, offset, words_remaining)) != ERROR_OK)
+		{
+			if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
+			{
+				/* if block write failed (no sufficient working area),
+				 * we use normal (slow) single dword accesses */
+				LOG_WARNING("couldn't use block writes, falling back to single memory accesses");
+			}
+			else if (retval == ERROR_FLASH_OPERATION_FAILED)
+			{
+				LOG_ERROR("flash writing failed with error code: 0x%x", retval);
+				return ERROR_FLASH_OPERATION_FAILED;
+			}
+		}
+		else
+		{
+			buffer += words_remaining * 4;
+			address += words_remaining * 4;
+			words_remaining = 0;
+		}
+	}
+
+	while (words_remaining > 0)
+	{
+		uint32_t value;
+		memcpy(&value, buffer + bytes_written, sizeof(uint32_t));
+
+		status = pic32mx_write_word(bank, address, value);
+		if (status & NVMCON_NVMERR)
+			return ERROR_FLASH_OPERATION_FAILED;
+		if (status & NVMCON_LVDERR)
+			return ERROR_FLASH_OPERATION_FAILED;
+
+		bytes_written += 4;
+		words_remaining--;
+		address += 4;
+	}
+
+	if (bytes_remaining)
+	{
+		uint32_t value = 0xffffffff;
+		memcpy(&value, buffer + bytes_written, bytes_remaining);
+
+		status = pic32mx_write_word(bank, address, value);
+		if (status & NVMCON_NVMERR)
+			return ERROR_FLASH_OPERATION_FAILED;
+		if (status & NVMCON_LVDERR)
+			return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	return ERROR_OK;
+}
+
+static int pic32mx_probe(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct pic32mx_flash_bank *pic32mx_info = bank->driver_priv;
+	struct mips32_common *mips32 = target->arch_info;
+	struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
+	int i;
+	uint16_t num_pages = 0;
+	uint32_t device_id;
+	int page_size;
+
+	pic32mx_info->probed = 0;
+
+	device_id = ejtag_info->idcode;
+	LOG_INFO("device id = 0x%08" PRIx32 " (manuf 0x%03x dev 0x%02x, ver 0x%03x)",
+			  device_id,
+			  (unsigned)((device_id >> 1)&0x7ff),
+			  (unsigned)((device_id >> 12)&0xff),
+			  (unsigned)((device_id >> 20)&0xfff));
+
+	if (((device_id >> 1)&0x7ff) != PIC32MX_MANUF_ID) {
+		LOG_WARNING("Cannot identify target as a PIC32MX family.");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	page_size = 4096;
+	if (bank->base == PIC32MX_KSEG1_BOOT_FLASH || bank->base == 1) {
+		/* 0xBFC00000: Boot flash size fixed at 12k */
+		num_pages = 12;
+	} else {
+		/* 0xBD000000: Program flash size varies with device */
+		for (i = 0; pic32mx_devs[i].name != NULL; i++)
+			if (pic32mx_devs[i].devid == ((device_id >> 12) & 0xff)) {
+				num_pages = pic32mx_devs[i].pfm_size;
+				break;
+			}
+		if (pic32mx_devs[i].name == NULL) {
+			LOG_WARNING("Cannot identify target as a PIC32MX family.");
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+	}
+
+#if 0
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* get flash size from target */
+	if (target_read_u16(target, 0x1FFFF7E0, &num_pages) != ERROR_OK)
+	{
+		/* failed reading flash size, default to max target family */
+		num_pages = 0xffff;
+	}
+#endif
+
+	LOG_INFO("flash size = %dkbytes", num_pages);
+
+	/* calculate numbers of pages */
+	num_pages /= (page_size / 1024);
+
+	if (bank->base == 0) bank->base = PIC32MX_KSEG1_PGM_FLASH;
+	if (bank->base == 1) bank->base = PIC32MX_KSEG1_BOOT_FLASH;
+	bank->size = (num_pages * page_size);
+	bank->num_sectors = num_pages;
+	bank->chip_width = 4;
+	bank->bus_width  = 4;
+	bank->sectors = malloc(sizeof(struct flash_sector) * num_pages);
+
+	for (i = 0; i < num_pages; i++)
+	{
+		bank->sectors[i].offset = i * page_size;
+		bank->sectors[i].size = page_size;
+		bank->sectors[i].is_erased = -1;
+		bank->sectors[i].is_protected = 1;
+	}
+
+	pic32mx_info->probed = 1;
+
+	return ERROR_OK;
+}
+
+static int pic32mx_auto_probe(struct flash_bank *bank)
+{
+	struct pic32mx_flash_bank *pic32mx_info = bank->driver_priv;
+	if (pic32mx_info->probed)
+		return ERROR_OK;
+	return pic32mx_probe(bank);
+}
+
+#if 0
+COMMAND_HANDLER(pic32mx_handle_part_id_command)
+{
+	return ERROR_OK;
+}
+#endif
+
+static int pic32mx_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	struct target *target = bank->target;
+	struct mips32_common *mips32 = target->arch_info;
+	struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
+	uint32_t device_id;
+	int printed = 0, i;
+
+	device_id = ejtag_info->idcode;
+
+	if (((device_id >> 1)&0x7ff) != PIC32MX_MANUF_ID) {
+		snprintf(buf, buf_size,
+				 "Cannot identify target as a PIC32MX family (manufacturer 0x%03d != 0x%03d)\n",
+				 (unsigned)((device_id >> 1)&0x7ff),
+				 PIC32MX_MANUF_ID);
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+	for (i = 0; pic32mx_devs[i].name != NULL; i++)
+		if (pic32mx_devs[i].devid == ((device_id >> 12) & 0xff)) {
+			printed = snprintf(buf, buf_size, "PIC32MX%s", pic32mx_devs[i].name);
+			break;
+		}
+	if (pic32mx_devs[i].name == NULL) {
+		snprintf(buf, buf_size, "Cannot identify target as a PIC32MX family\n");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+	buf += printed;
+	buf_size -= printed;
+	printed = snprintf(buf, buf_size, "  Ver: 0x%03x",
+					   (unsigned)((device_id >> 20)&0xfff));
+
+	return ERROR_OK;
+}
+
+#if 0
+COMMAND_HANDLER(pic32mx_handle_lock_command)
+{
+	struct target *target = NULL;
+	struct pic32mx_flash_bank *pic32mx_info = NULL;
+
+	if (CMD_ARGC < 1)
+	{
+		command_print(CMD_CTX, "pic32mx lock <bank>");
+		return ERROR_OK;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	pic32mx_info = bank->driver_priv;
+
+	target = bank->target;
+
+	if (target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (pic32mx_erase_options(bank) != ERROR_OK)
+	{
+		command_print(CMD_CTX, "pic32mx failed to erase options");
+		return ERROR_OK;
+	}
+
+	/* set readout protection */
+	pic32mx_info->option_bytes.RDP = 0;
+
+	if (pic32mx_write_options(bank) != ERROR_OK)
+	{
+		command_print(CMD_CTX, "pic32mx failed to lock device");
+		return ERROR_OK;
+	}
+
+	command_print(CMD_CTX, "pic32mx locked");
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(pic32mx_handle_unlock_command)
+{
+	struct target *target = NULL;
+	struct pic32mx_flash_bank *pic32mx_info = NULL;
+
+	if (CMD_ARGC < 1)
+	{
+		command_print(CMD_CTX, "pic32mx unlock <bank>");
+		return ERROR_OK;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	pic32mx_info = bank->driver_priv;
+
+	target = bank->target;
+
+	if (target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (pic32mx_erase_options(bank) != ERROR_OK)
+	{
+		command_print(CMD_CTX, "pic32mx failed to unlock device");
+		return ERROR_OK;
+	}
+
+	if (pic32mx_write_options(bank) != ERROR_OK)
+	{
+		command_print(CMD_CTX, "pic32mx failed to lock device");
+		return ERROR_OK;
+	}
+
+	command_print(CMD_CTX, "pic32mx unlocked");
+
+	return ERROR_OK;
+}
+#endif
+
+#if 0
+static int pic32mx_chip_erase(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+#if 0
+	uint32_t status;
+#endif
+
+	if (target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	LOG_INFO("PIC32MX chip erase called");
+
+#if 0
+	/* unlock option flash registers */
+	target_write_u32(target, PIC32MX_FLASH_KEYR, KEY1);
+	target_write_u32(target, PIC32MX_FLASH_KEYR, KEY2);
+
+	/* chip erase flash memory */
+	target_write_u32(target, PIC32MX_FLASH_CR, FLASH_MER);
+	target_write_u32(target, PIC32MX_FLASH_CR, FLASH_MER | FLASH_STRT);
+
+	status = pic32mx_wait_status_busy(bank, 10);
+
+	target_write_u32(target, PIC32MX_FLASH_CR, FLASH_LOCK);
+
+	if (status & FLASH_WRPRTERR)
+	{
+		LOG_ERROR("pic32mx device protected");
+		return ERROR_OK;
+	}
+
+	if (status & FLASH_PGERR)
+	{
+		LOG_ERROR("pic32mx device programming failed");
+		return ERROR_OK;
+	}
+#endif
+
+	return ERROR_OK;
+}
+#endif
+
+COMMAND_HANDLER(pic32mx_handle_chip_erase_command)
+{
+#if 0
+	int i;
+
+	if (CMD_ARGC != 0)
+	{
+		command_print(CMD_CTX, "pic32mx chip_erase");
+		return ERROR_OK;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	if (pic32mx_chip_erase(bank) == ERROR_OK)
+	{
+		/* set all sectors as erased */
+		for (i = 0; i < bank->num_sectors; i++)
+		{
+			bank->sectors[i].is_erased = 1;
+		}
+
+		command_print(CMD_CTX, "pic32mx chip erase complete");
+	}
+	else
+	{
+		command_print(CMD_CTX, "pic32mx chip erase failed");
+	}
+#endif
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(pic32mx_handle_pgm_word_command)
+{
+	uint32_t address, value;
+	int status, res;
+
+	if (CMD_ARGC != 3)
+	{
+		command_print(CMD_CTX, "pic32mx pgm_word <addr> <value> <bank>");
+		return ERROR_OK;
+	}
+
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], value);
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 2, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	if (address < bank->base || address >= (bank->base + bank->size))
+	{
+		command_print(CMD_CTX, "flash address '%s' is out of bounds", CMD_ARGV[0]);
+		return ERROR_OK;
+	}
+
+	res = ERROR_OK;
+	status = pic32mx_write_word(bank, address, value);
+	if (status & NVMCON_NVMERR)
+		res = ERROR_FLASH_OPERATION_FAILED;
+	if (status & NVMCON_LVDERR)
+		res = ERROR_FLASH_OPERATION_FAILED;
+
+	if (res == ERROR_OK)
+		command_print(CMD_CTX, "pic32mx pgm word complete");
+	else
+		command_print(CMD_CTX, "pic32mx pgm word failed (status = 0x%x)", status);
+
+	return ERROR_OK;
+}
+static const struct command_registration pic32mx_exec_command_handlers[] = {
+	{
+		.name = "chip_erase",
+		.handler = &pic32mx_handle_chip_erase_command,
+		.mode = COMMAND_EXEC,
+		.help = "erase device",
+	},
+	{
+		.name = "pgm_word",
+		.handler = &pic32mx_handle_pgm_word_command,
+		.mode = COMMAND_EXEC,
+		.help = "program a word",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+static const struct command_registration pic32mx_command_handlers[] = {
+	{
+		.name = "pic32mx",
+		.mode = COMMAND_ANY,
+		.help = "pic32mx flash command group",
+		.chain = pic32mx_exec_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+struct flash_driver pic32mx_flash = {
+		.name = "pic32mx",
+		.commands = pic32mx_command_handlers,
+		.flash_bank_command = &pic32mx_flash_bank_command,
+		.erase = &pic32mx_erase,
+		.protect = &pic32mx_protect,
+		.write = &pic32mx_write,
+		.probe = &pic32mx_probe,
+		.auto_probe = &pic32mx_auto_probe,
+		.erase_check = &default_flash_mem_blank_check,
+		.protect_check = &pic32mx_protect_check,
+		.info = &pic32mx_info,
+	};
diff --git a/src/flash/nor/pic32mx.h b/src/flash/nor/pic32mx.h
new file mode 100644
index 00000000..92f40c2e
--- /dev/null
+++ b/src/flash/nor/pic32mx.h
@@ -0,0 +1,113 @@
+/***************************************************************************
+ *   Copyright (C) 2005 by Dominic Rath                                    *
+ *   Dominic.Rath@gmx.de                                                   *
+ *                                                                         *
+ *   Copyright (C) 2008 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
+ *   Copyright (C) 2008 by John McCarthy                                   *
+ *   jgmcc@magma.ca                                                        *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifndef PIC32MX_H
+#define PIC32MX_H
+
+#include "flash.h"
+
+struct pic32mx_flash_bank
+{
+	struct working_area *write_algorithm;
+	int devid;
+	int ppage_size;
+	int probed;
+};
+
+#define PIC32MX_MANUF_ID	0x029
+
+/* pic32mx memory locations */
+
+#define PIC32MX_KUSEG_PGM_FLASH		0x7D000000
+#define PIC32MX_KUSEG_RAM			0x7F000000
+
+#define PIC32MX_KSEG0_RAM			0x80000000
+#define PIC32MX_KSEG0_PGM_FLASH		0x9D000000
+#define PIC32MX_KSEG0_BOOT_FLASH	0x9FC00000
+
+#define PIC32MX_KSEG1_RAM			0xA0000000
+#define PIC32MX_KSEG1_PGM_FLASH		0xBD000000
+#define PIC32MX_KSEG1_PERIPHERAL	0xBF800000
+#define PIC32MX_KSEG1_BOOT_FLASH	0xBFC00000
+
+#define PIC32MX_PHYS_RAM			0x00000000
+#define PIC32MX_PHYS_PGM_FLASH		0x1D000000
+#define PIC32MX_PHYS_PERIPHERALS	0x1F800000
+#define PIC32MX_PHYS_BOOT_FLASH		0x1FC00000
+
+/*
+ * Translate Virtual and Physical addresses.
+ * Note: These macros only work for KSEG0/KSEG1 addresses.
+ */
+#define KS1Virt2Phys(vaddr)			((vaddr)-0xA0000000)
+#define Phys2KS1Virt(paddr)			((paddr) + 0xA0000000)
+#define KS0Virt2Phys(vaddr)			((vaddr)-0x80000000)
+#define Phys2KS0Virt(paddr)			((paddr) + 0x80000000)
+
+/* pic32mx configuration register locations */
+
+#define PIC32MX_DEVCFG0		0xBFC02FFC
+#define PIC32MX_DEVCFG1		0xBFC02FF8
+#define PIC32MX_DEVCFG2		0xBFC02FF4
+#define PIC32MX_DEVCFG3		0XBFC02FF0
+#define PIC32MX_DEVID		0xBF80F220
+
+/* pic32mx flash controller register locations */
+
+#define PIC32MX_NVMCON		0xBF80F400
+#define PIC32MX_NVMCONCLR	0xBF80F404
+#define PIC32MX_NVMCONSET	0xBF80F408
+#define PIC32MX_NVMCONINV	0xBF80F40C
+#define NVMCON_NVMWR		(1 << 15)
+#define NVMCON_NVMWREN		(1 << 14)
+#define NVMCON_NVMERR		(1 << 13)
+#define NVMCON_LVDERR		(1 << 12)
+#define NVMCON_LVDSTAT		(1 << 11)
+#define NVMCON_OP_PFM_ERASE		0x5
+#define NVMCON_OP_PAGE_ERASE	0x4
+#define NVMCON_OP_ROW_PROG		0x3
+#define NVMCON_OP_WORD_PROG		0x1
+#define NVMCON_OP_NOP			0x0
+
+#define PIC32MX_NVMKEY		0xBF80F410
+#define PIC32MX_NVMADDR		0xBF80F420
+#define PIC32MX_NVMADDRCLR	0xBF80F424
+#define PIC32MX_NVMADDRSET	0xBF80F428
+#define PIC32MX_NVMADDRINV	0xBF80F42C
+#define PIC32MX_NVMDATA		0xBF80F430
+#define PIC32MX_NVMSRCADDR	0xBF80F440
+
+/* flash unlock keys */
+
+#define NVMKEY1			0xAA996655
+#define NVMKEY2			0x556699AA
+
+struct pic32mx_mem_layout {
+	uint32_t sector_start;
+	uint32_t sector_size;
+};
+
+#endif /* PIC32MX_H */
+
diff --git a/src/flash/nor/stellaris.c b/src/flash/nor/stellaris.c
new file mode 100644
index 00000000..771f0a71
--- /dev/null
+++ b/src/flash/nor/stellaris.c
@@ -0,0 +1,1195 @@
+/***************************************************************************
+ *   Copyright (C) 2006 by Magnus Lundin                                   *
+ *   lundin@mlu.mine.nu                                                    *
+ *                                                                         *
+ *   Copyright (C) 2008 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+/***************************************************************************
+* STELLARIS is tested on LM3S811, LM3S6965
+***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "stellaris.h"
+#include "armv7m.h"
+#include "binarybuffer.h"
+#include "algorithm.h"
+
+
+#define DID0_VER(did0) ((did0 >> 28)&0x07)
+
+static int stellaris_read_part_info(struct flash_bank *bank);
+static uint32_t stellaris_get_flash_status(struct flash_bank *bank);
+static void stellaris_set_flash_mode(struct flash_bank *bank,int mode);
+//static uint32_t stellaris_wait_status_busy(struct flash_bank *bank, uint32_t waitbits, int timeout);
+
+static int stellaris_mass_erase(struct flash_bank *bank);
+
+static struct {
+	uint32_t partno;
+	char *partname;
+}	StellarisParts[] =
+{
+	{0x01,"LM3S101"},
+	{0x02,"LM3S102"},
+	{0x03,"LM3S1625"},
+	{0x04,"LM3S1626"},
+	{0x05,"LM3S1627"},
+	{0x06,"LM3S1607"},
+	{0x10,"LM3S1776"},
+	{0x19,"LM3S300"},
+	{0x11,"LM3S301"},
+	{0x12,"LM3S310"},
+	{0x1A,"LM3S308"},
+	{0x13,"LM3S315"},
+	{0x14,"LM3S316"},
+	{0x17,"LM3S317"},
+	{0x18,"LM3S318"},
+	{0x15,"LM3S328"},
+	{0x2A,"LM3S600"},
+	{0x21,"LM3S601"},
+	{0x2B,"LM3S608"},
+	{0x22,"LM3S610"},
+	{0x23,"LM3S611"},
+	{0x24,"LM3S612"},
+	{0x25,"LM3S613"},
+	{0x26,"LM3S615"},
+	{0x28,"LM3S617"},
+	{0x29,"LM3S618"},
+	{0x27,"LM3S628"},
+	{0x38,"LM3S800"},
+	{0x31,"LM3S801"},
+	{0x39,"LM3S808"},
+	{0x32,"LM3S811"},
+	{0x33,"LM3S812"},
+	/*{0x33,"LM3S2616"},*/
+	{0x34,"LM3S815"},
+	{0x36,"LM3S817"},
+	{0x37,"LM3S818"},
+	{0x35,"LM3S828"},
+	{0x39,"LM3S2276"},
+	{0x3A,"LM3S2776"},
+	{0x43,"LM3S3651"},
+	{0x44,"LM3S3739"},
+	{0x45,"LM3S3749"},
+	{0x46,"LM3S3759"},
+	{0x48,"LM3S3768"},
+	{0x49,"LM3S3748"},
+	{0x50,"LM3S2678"},
+	{0x51,"LM3S2110"},
+	{0x52,"LM3S2739"},
+	{0x53,"LM3S2651"},
+	{0x54,"LM3S2939"},
+	{0x55,"LM3S2965"},
+	{0x56,"LM3S2432"},
+	{0x57,"LM3S2620"},
+	{0x58,"LM3S2950"},
+	{0x59,"LM3S2412"},
+	{0x5A,"LM3S2533"},
+	{0x61,"LM3S8630"},
+	{0x62,"LM3S8970"},
+	{0x63,"LM3S8730"},
+	{0x64,"LM3S8530"},
+	{0x65,"LM3S8930"},
+	{0x71,"LM3S6610"},
+	{0x72,"LM3S6950"},
+	{0x73,"LM3S6965"},
+	{0x74,"LM3S6110"},
+	{0x75,"LM3S6432"},
+	{0x76,"LM3S6537"},
+	{0x77,"LM3S6753"},
+	{0x78,"LM3S6952"},
+	{0x80,"LM3S2671"},
+	{0x81,"LM3S5632"},
+	{0x82,"LM3S6422"},
+	{0x83,"LM3S6633"},
+	{0x84,"LM3S2139"},
+	{0x85,"LM3S2637"},
+	{0x86,"LM3S8738"},
+	{0x88,"LM3S8938"},
+	{0x89,"LM3S6938"},
+	{0x8A,"LM3S5652"},
+	{0x8B,"LM3S6637"},
+	{0x8C,"LM3S8933"},
+	{0x8D,"LM3S8733"},
+	{0x8E,"LM3S8538"},
+	{0x8F,"LM3S2948"},
+	{0x91,"LM3S5662"},
+	{0x96,"LM3S5732"},
+	{0x97,"LM3S5737"},
+	{0x99,"LM3S5747"},
+	{0x9A,"LM3S5752"},
+	{0x9B,"LM3S5757"},
+	{0x9C,"LM3S5762"},
+	{0x9D,"LM3S5767"},
+	{0xA0,"LM3S5739"},
+	{0xA1,"LM3S6100"},
+	{0xA2,"LM3S2410"},
+	{0xA3,"LM3S6730"},
+	{0xA4,"LM3S2730"},
+	{0xA5,"LM3S6420"},
+	{0xA6,"LM3S8962"},
+	{0xA7,"LM3S5749"},
+	{0xA8,"LM3S5769"},
+	{0xA9,"LM3S5768"},
+	{0xB3,"LM3S1635"},
+	{0xB4,"LM3S1850"},
+	{0xB5,"LM3S1960"},
+	{0xB7,"LM3S1937"},
+	{0xB8,"LM3S1968"},
+	{0xB9,"LM3S1751"},
+	{0xBA,"LM3S1439"},
+	{0xBB,"LM3S1512"},
+	{0xBC,"LM3S1435"},
+	{0xBD,"LM3S1637"},
+	{0xBE,"LM3S1958"},
+	{0xBF,"LM3S1110"},
+	{0xC0,"LM3S1620"},
+	{0xC1,"LM3S1150"},
+	{0xC2,"LM3S1165"},
+	{0xC3,"LM3S1133"},
+	{0xC4,"LM3S1162"},
+	{0xC5,"LM3S1138"},
+	{0xC6,"LM3S1332"},
+	{0xC7,"LM3S1538"},
+	{0xD0,"LM3S6815"},
+	{0xD1,"LM3S6816"},
+	{0xD2,"LM3S6915"},
+	{0xD3,"LM3S6916"},
+	{0xD4,"LM3S2016"},
+	{0xD5,"LM3S1615"},
+	{0xD6,"LM3S1616"},
+	{0xD7,"LM3S8971"},
+	{0xD8,"LM3S1108"},
+	{0xD9,"LM3S1101"},
+	{0xDA,"LM3S1608"},
+	{0xDB,"LM3S1601"},
+	{0xDC,"LM3S1918"},
+	{0xDD,"LM3S1911"},
+	{0xDE,"LM3S2108"},
+	{0xDF,"LM3S2101"},
+	{0xE0,"LM3S2608"},
+	{0xE1,"LM3S2601"},
+	{0xE2,"LM3S2918"},
+	{0xE3,"LM3S2911"},
+	{0xE4,"LM3S6118"},
+	{0xE5,"LM3S6111"},
+	{0xE6,"LM3S6618"},
+	{0xE7,"LM3S6611"},
+	{0xE8,"LM3S6918"},
+	{0xE9,"LM3S6911"},
+	{0,"Unknown part"}
+};
+
+static char * StellarisClassname[5] =
+{
+	"Sandstorm",
+	"Fury",
+	"Unknown",
+	"DustDevil",
+	"Tempest"
+};
+
+/***************************************************************************
+*	openocd command interface                                              *
+***************************************************************************/
+
+/* flash_bank stellaris <base> <size> 0 0 <target#>
+ */
+FLASH_BANK_COMMAND_HANDLER(stellaris_flash_bank_command)
+{
+	struct stellaris_flash_bank *stellaris_info;
+
+	if (CMD_ARGC < 6)
+	{
+		LOG_WARNING("incomplete flash_bank stellaris configuration");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	stellaris_info = calloc(sizeof(struct stellaris_flash_bank), 1);
+	bank->base = 0x0;
+	bank->driver_priv = stellaris_info;
+
+	stellaris_info->target_name = "Unknown target";
+
+	/* part wasn't probed for info yet */
+	stellaris_info->did1 = 0;
+
+	/* TODO Specify the main crystal speed in kHz using an optional
+	 * argument; ditto, the speed of an external oscillator used
+	 * instead of a crystal.  Avoid programming flash using IOSC.
+	 */
+	return ERROR_OK;
+}
+
+static int stellaris_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	int printed, device_class;
+	struct stellaris_flash_bank *stellaris_info = bank->driver_priv;
+
+	stellaris_read_part_info(bank);
+
+	if (stellaris_info->did1 == 0)
+	{
+		printed = snprintf(buf, buf_size, "Cannot identify target as a Stellaris\n");
+		buf += printed;
+		buf_size -= printed;
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	if (DID0_VER(stellaris_info->did0) > 0)
+	{
+		device_class = (stellaris_info->did0 >> 16) & 0xFF;
+	}
+	else
+	{
+		device_class = 0;
+	}
+	printed = snprintf(buf,
+			   buf_size,
+			   "\nTI/LMI Stellaris information: Chip is "
+			   "class %i (%s) %s rev %c%i\n",
+			   device_class,
+			   StellarisClassname[device_class],
+			   stellaris_info->target_name,
+			   (int)('A' + ((stellaris_info->did0 >> 8) & 0xFF)),
+			   (int)((stellaris_info->did0) & 0xFF));
+	buf += printed;
+	buf_size -= printed;
+
+	printed = snprintf(buf,
+			   buf_size,
+			   "did1: 0x%8.8" PRIx32 ", arch: 0x%4.4" PRIx32
+			   ", eproc: %s, ramsize: %ik, flashsize: %ik\n",
+			   stellaris_info->did1,
+			   stellaris_info->did1,
+			   "ARMv7M",
+			   (int)((1 + ((stellaris_info->dc0 >> 16) & 0xFFFF))/4),
+			   (int)((1 + (stellaris_info->dc0 & 0xFFFF))*2));
+	buf += printed;
+	buf_size -= printed;
+
+	printed = snprintf(buf,
+			   buf_size,
+			   "master clock: %ikHz%s, "
+			   "rcc is 0x%" PRIx32 ", rcc2 is 0x%" PRIx32 "\n",
+			   (int)(stellaris_info->mck_freq / 1000),
+			   stellaris_info->mck_desc,
+			   stellaris_info->rcc,
+			   stellaris_info->rcc2);
+	buf += printed;
+	buf_size -= printed;
+
+	if (stellaris_info->num_lockbits > 0)
+	{
+		printed = snprintf(buf,
+				   buf_size,
+				   "pagesize: %" PRIi32 ", lockbits: %i 0x%4.4" PRIx32 ", pages in lock region: %i \n",
+				   stellaris_info->pagesize,
+				   stellaris_info->num_lockbits,
+				   stellaris_info->lockbits,
+				   (int)(stellaris_info->num_pages/stellaris_info->num_lockbits));
+		buf += printed;
+		buf_size -= printed;
+	}
+	return ERROR_OK;
+}
+
+/***************************************************************************
+*	chip identification and status                                         *
+***************************************************************************/
+
+static uint32_t stellaris_get_flash_status(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	uint32_t fmc;
+
+	target_read_u32(target, FLASH_CONTROL_BASE | FLASH_FMC, &fmc);
+
+	return fmc;
+}
+
+/** Read clock configuration and set stellaris_info->usec_clocks*/
+
+static const unsigned rcc_xtal[32] = {
+	[0x00] = 1000000,		/* no pll */
+	[0x01] = 1843200,		/* no pll */
+	[0x02] = 2000000,		/* no pll */
+	[0x03] = 2457600,		/* no pll */
+
+	[0x04] = 3579545,
+	[0x05] = 3686400,
+	[0x06] = 4000000,		/* usb */
+	[0x07] = 4096000,
+
+	[0x08] = 4915200,
+	[0x09] = 5000000,		/* usb */
+	[0x0a] = 5120000,
+	[0x0b] = 6000000,		/* (reset) usb */
+
+	[0x0c] = 6144000,
+	[0x0d] = 7372800,
+	[0x0e] = 8000000,		/* usb */
+	[0x0f] = 8192000,
+
+	/* parts before DustDevil use just 4 bits for xtal spec */
+
+	[0x10] = 10000000,		/* usb */
+	[0x11] = 12000000,		/* usb */
+	[0x12] = 12288000,
+	[0x13] = 13560000,
+
+	[0x14] = 14318180,
+	[0x15] = 16000000,		/* usb */
+	[0x16] = 16384000,
+};
+
+static void stellaris_read_clock_info(struct flash_bank *bank)
+{
+	struct stellaris_flash_bank *stellaris_info = bank->driver_priv;
+	struct target *target = bank->target;
+	uint32_t rcc, rcc2, pllcfg, sysdiv, usesysdiv, bypass, oscsrc;
+	unsigned xtal;
+	unsigned long mainfreq;
+
+	target_read_u32(target, SCB_BASE | RCC, &rcc);
+	LOG_DEBUG("Stellaris RCC %" PRIx32 "", rcc);
+
+	target_read_u32(target, SCB_BASE | RCC2, &rcc2);
+	LOG_DEBUG("Stellaris RCC2 %" PRIx32 "", rcc);
+
+	target_read_u32(target, SCB_BASE | PLLCFG, &pllcfg);
+	LOG_DEBUG("Stellaris PLLCFG %" PRIx32 "", pllcfg);
+
+	stellaris_info->rcc = rcc;
+	stellaris_info->rcc = rcc2;
+
+	sysdiv = (rcc >> 23) & 0xF;
+	usesysdiv = (rcc >> 22) & 0x1;
+	bypass = (rcc >> 11) & 0x1;
+	oscsrc = (rcc >> 4) & 0x3;
+	xtal = (rcc >> 6) & stellaris_info->xtal_mask;
+
+	/* NOTE: post-Sandstorm parts have RCC2 which may override
+	 * parts of RCC ... with more sysdiv options, option for
+	 * 32768 Hz mainfreq, PLL controls.  On Sandstorm it reads
+	 * as zero, so the "use RCC2" flag is always clear.
+	 */
+	if (rcc2 & (1 << 31)) {
+		sysdiv = (rcc2 >> 23) & 0x3F;
+		bypass = (rcc2 >> 11) & 0x1;
+		oscsrc = (rcc2 >> 4) & 0x7;
+
+		/* FIXME Tempest parts have an additional lsb for
+		 * fractional sysdiv (200 MHz / 2.5 == 80 MHz)
+		 */
+	}
+
+	stellaris_info->mck_desc = "";
+
+	switch (oscsrc)
+	{
+		case 0:				/* MOSC */
+			mainfreq = rcc_xtal[xtal];
+			break;
+		case 1:				/* IOSC */
+			mainfreq = stellaris_info->iosc_freq;
+			stellaris_info->mck_desc = stellaris_info->iosc_desc;
+			break;
+		case 2:				/* IOSC/4 */
+			mainfreq = stellaris_info->iosc_freq / 4;
+			stellaris_info->mck_desc = stellaris_info->iosc_desc;
+			break;
+		case 3:				/* lowspeed */
+			/* Sandstorm doesn't have this 30K +/- 30% osc */
+			mainfreq = 30000;
+			stellaris_info->mck_desc = " (±30%)";
+			break;
+		case 8:				/* hibernation osc */
+			/* not all parts support hibernation */
+			mainfreq = 32768;
+			break;
+
+		default: /* NOTREACHED */
+			mainfreq = 0;
+			break;
+	}
+
+	/* PLL is used if it's not bypassed; its output is 200 MHz
+	 * even when it runs at 400 MHz (adds divide-by-two stage).
+	 */
+	if (!bypass)
+		mainfreq = 200000000;
+
+	if (usesysdiv)
+		stellaris_info->mck_freq = mainfreq/(1 + sysdiv);
+	else
+		stellaris_info->mck_freq = mainfreq;
+
+	/* Forget old flash timing */
+	stellaris_set_flash_mode(bank, 0);
+}
+
+/* Setup the timimg registers */
+static void stellaris_set_flash_mode(struct flash_bank *bank,int mode)
+{
+	struct stellaris_flash_bank *stellaris_info = bank->driver_priv;
+	struct target *target = bank->target;
+
+	uint32_t usecrl = (stellaris_info->mck_freq/1000000ul-1);
+	LOG_DEBUG("usecrl = %i",(int)(usecrl));
+	target_write_u32(target, SCB_BASE | USECRL, usecrl);
+}
+
+#if 0
+static uint32_t stellaris_wait_status_busy(struct flash_bank *bank, uint32_t waitbits, int timeout)
+{
+	uint32_t status;
+
+	/* Stellaris waits for cmdbit to clear */
+	while (((status = stellaris_get_flash_status(bank)) & waitbits) && (timeout-- > 0))
+	{
+		LOG_DEBUG("status: 0x%x", status);
+		alive_sleep(1);
+	}
+
+	/* Flash errors are reflected in the FLASH_CRIS register */
+
+	return status;
+}
+
+/* Send one command to the flash controller */
+static int stellaris_flash_command(struct flash_bank *bank,uint8_t cmd,uint16_t pagen)
+{
+	uint32_t fmc;
+	struct target *target = bank->target;
+
+	fmc = FMC_WRKEY | cmd;
+	target_write_u32(target, FLASH_CONTROL_BASE | FLASH_FMC, fmc);
+	LOG_DEBUG("Flash command: 0x%x", fmc);
+
+	if (stellaris_wait_status_busy(bank, cmd, 100))
+	{
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	return ERROR_OK;
+}
+#endif
+
+/* Read device id register, main clock frequency register and fill in driver info structure */
+static int stellaris_read_part_info(struct flash_bank *bank)
+{
+	struct stellaris_flash_bank *stellaris_info = bank->driver_priv;
+	struct target *target = bank->target;
+	uint32_t did0, did1, ver, fam, status;
+	int i;
+
+	/* Read and parse chip identification register */
+	target_read_u32(target, SCB_BASE | DID0, &did0);
+	target_read_u32(target, SCB_BASE | DID1, &did1);
+	target_read_u32(target, SCB_BASE | DC0, &stellaris_info->dc0);
+	target_read_u32(target, SCB_BASE | DC1, &stellaris_info->dc1);
+	LOG_DEBUG("did0 0x%" PRIx32 ", did1 0x%" PRIx32 ", dc0 0x%" PRIx32 ", dc1 0x%" PRIx32 "",
+		  did0, did1, stellaris_info->dc0, stellaris_info->dc1);
+
+	ver = did0 >> 28;
+	if ((ver != 0) && (ver != 1))
+	{
+		LOG_WARNING("Unknown did0 version, cannot identify target");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	if (did1 == 0)
+	{
+		LOG_WARNING("Cannot identify target as a Stellaris");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	ver = did1 >> 28;
+	fam = (did1 >> 24) & 0xF;
+	if (((ver != 0) && (ver != 1)) || (fam != 0))
+	{
+		LOG_WARNING("Unknown did1 version/family, cannot positively identify target as a Stellaris");
+	}
+
+	/* For Sandstorm, Fury, DustDevil:  current data sheets say IOSC
+	 * is 12 MHz, but some older parts have 15 MHz.  A few data sheets
+	 * even give _both_ numbers!  We'll use current numbers; IOSC is
+	 * always approximate.
+	 *
+	 * For Tempest:  IOSC is calibrated, 16 MHz
+	 */
+	stellaris_info->iosc_freq = 12000000;
+	stellaris_info->iosc_desc = " (±30%)";
+	stellaris_info->xtal_mask = 0x0f;
+
+	switch ((did0 >> 28) & 0x7) {
+	case 0:				/* Sandstorm */
+		/*
+		 * Current (2009-August) parts seem to be rev C2 and use 12 MHz.
+		 * Parts before rev C0 used 15 MHz; some C0 parts use 15 MHz
+		 * (LM3S618), but some other C0 parts are 12 MHz (LM3S811).
+		 */
+		if (((did0 >> 8) & 0xff) < 2) {
+			stellaris_info->iosc_freq = 15000000;
+			stellaris_info->iosc_desc = " (±50%)";
+		}
+		break;
+	case 1:
+		switch ((did0 >> 16) & 0xff) {
+		case 1:			/* Fury */
+			break;
+		case 4:			/* Tempest */
+			stellaris_info->iosc_freq = 16000000;	/* +/- 1% */
+			stellaris_info->iosc_desc = " (±1%)";
+			/* FALL THROUGH */
+		case 3:			/* DustDevil */
+			stellaris_info->xtal_mask = 0x1f;
+			break;
+		default:
+			LOG_WARNING("Unknown did0 class");
+		}
+	default:
+		break;
+		LOG_WARNING("Unknown did0 version");
+	}
+
+	for (i = 0; StellarisParts[i].partno; i++)
+	{
+		if (StellarisParts[i].partno == ((did1 >> 16) & 0xFF))
+			break;
+	}
+
+	stellaris_info->target_name = StellarisParts[i].partname;
+
+	stellaris_info->did0 = did0;
+	stellaris_info->did1 = did1;
+
+	stellaris_info->num_lockbits = 1 + (stellaris_info->dc0 & 0xFFFF);
+	stellaris_info->num_pages = 2 *(1 + (stellaris_info->dc0 & 0xFFFF));
+	stellaris_info->pagesize = 1024;
+	bank->size = 1024 * stellaris_info->num_pages;
+	stellaris_info->pages_in_lockregion = 2;
+	target_read_u32(target, SCB_BASE | FMPPE, &stellaris_info->lockbits);
+
+	/* provide this for the benefit of the higher flash driver layers */
+	bank->num_sectors = stellaris_info->num_pages;
+	bank->sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
+	for (i = 0; i < bank->num_sectors; i++)
+	{
+		bank->sectors[i].offset = i * stellaris_info->pagesize;
+		bank->sectors[i].size = stellaris_info->pagesize;
+		bank->sectors[i].is_erased = -1;
+		bank->sectors[i].is_protected = -1;
+	}
+
+	/* Read main and master clock freqency register */
+	stellaris_read_clock_info(bank);
+
+	status = stellaris_get_flash_status(bank);
+
+	return ERROR_OK;
+}
+
+/***************************************************************************
+*	flash operations                                                       *
+***************************************************************************/
+
+static int stellaris_protect_check(struct flash_bank *bank)
+{
+	uint32_t status;
+
+	struct stellaris_flash_bank *stellaris_info = bank->driver_priv;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (stellaris_info->did1 == 0)
+	{
+		stellaris_read_part_info(bank);
+	}
+
+	if (stellaris_info->did1 == 0)
+	{
+		LOG_WARNING("Cannot identify target as Stellaris");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	status = stellaris_get_flash_status(bank);
+	stellaris_info->lockbits = status >> 16;
+
+	return ERROR_OK;
+}
+
+static int stellaris_erase(struct flash_bank *bank, int first, int last)
+{
+	int banknr;
+	uint32_t flash_fmc, flash_cris;
+	struct stellaris_flash_bank *stellaris_info = bank->driver_priv;
+	struct target *target = bank->target;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (stellaris_info->did1 == 0)
+	{
+		stellaris_read_part_info(bank);
+	}
+
+	if (stellaris_info->did1 == 0)
+	{
+		LOG_WARNING("Cannot identify target as Stellaris");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	if ((first < 0) || (last < first) || (last >= (int)stellaris_info->num_pages))
+	{
+		return ERROR_FLASH_SECTOR_INVALID;
+	}
+
+	if ((first == 0) && (last == ((int)stellaris_info->num_pages-1)))
+	{
+		return stellaris_mass_erase(bank);
+	}
+
+	/* Configure the flash controller timing */
+	stellaris_read_clock_info(bank);
+	stellaris_set_flash_mode(bank,0);
+
+	/* Clear and disable flash programming interrupts */
+	target_write_u32(target, FLASH_CIM, 0);
+	target_write_u32(target, FLASH_MISC, PMISC | AMISC);
+
+	for (banknr = first; banknr <= last; banknr++)
+	{
+		/* Address is first word in page */
+		target_write_u32(target, FLASH_FMA, banknr * stellaris_info->pagesize);
+		/* Write erase command */
+		target_write_u32(target, FLASH_FMC, FMC_WRKEY | FMC_ERASE);
+		/* Wait until erase complete */
+		do
+		{
+			target_read_u32(target, FLASH_FMC, &flash_fmc);
+		}
+		while (flash_fmc & FMC_ERASE);
+
+		/* Check acess violations */
+		target_read_u32(target, FLASH_CRIS, &flash_cris);
+		if (flash_cris & (AMASK))
+		{
+			LOG_WARNING("Error erasing flash page %i,  flash_cris 0x%" PRIx32 "", banknr, flash_cris);
+			target_write_u32(target, FLASH_CRIS, 0);
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+
+		bank->sectors[banknr].is_erased = 1;
+	}
+
+	return ERROR_OK;
+}
+
+static int stellaris_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	uint32_t fmppe, flash_fmc, flash_cris;
+	int lockregion;
+
+	struct stellaris_flash_bank *stellaris_info = bank->driver_priv;
+	struct target *target = bank->target;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if ((first < 0) || (last < first) || (last >= stellaris_info->num_lockbits))
+	{
+		return ERROR_FLASH_SECTOR_INVALID;
+	}
+
+	if (stellaris_info->did1 == 0)
+	{
+		stellaris_read_part_info(bank);
+	}
+
+	if (stellaris_info->did1 == 0)
+	{
+		LOG_WARNING("Cannot identify target as an Stellaris MCU");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	/* Configure the flash controller timing */
+	stellaris_read_clock_info(bank);
+	stellaris_set_flash_mode(bank, 0);
+
+	fmppe = stellaris_info->lockbits;
+	for (lockregion = first; lockregion <= last; lockregion++)
+	{
+		if (set)
+			fmppe &= ~(1 << lockregion);
+		else
+			fmppe |= (1 << lockregion);
+	}
+
+	/* Clear and disable flash programming interrupts */
+	target_write_u32(target, FLASH_CIM, 0);
+	target_write_u32(target, FLASH_MISC, PMISC | AMISC);
+
+	LOG_DEBUG("fmppe 0x%" PRIx32 "",fmppe);
+	target_write_u32(target, SCB_BASE | FMPPE, fmppe);
+	/* Commit FMPPE */
+	target_write_u32(target, FLASH_FMA, 1);
+	/* Write commit command */
+	/* TODO safety check, sice this cannot be undone */
+	LOG_WARNING("Flash protection cannot be removed once commited, commit is NOT executed !");
+	/* target_write_u32(target, FLASH_FMC, FMC_WRKEY | FMC_COMT); */
+	/* Wait until erase complete */
+	do
+	{
+		target_read_u32(target, FLASH_FMC, &flash_fmc);
+	}
+	while (flash_fmc & FMC_COMT);
+
+	/* Check acess violations */
+	target_read_u32(target, FLASH_CRIS, &flash_cris);
+	if (flash_cris & (AMASK))
+	{
+		LOG_WARNING("Error setting flash page protection,  flash_cris 0x%" PRIx32 "", flash_cris);
+		target_write_u32(target, FLASH_CRIS, 0);
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	target_read_u32(target, SCB_BASE | FMPPE, &stellaris_info->lockbits);
+
+	return ERROR_OK;
+}
+
+static uint8_t stellaris_write_code[] =
+{
+/*
+	Call with :
+	r0 = buffer address
+	r1 = destination address
+	r2 = bytecount (in) - endaddr (work)
+
+	Used registers:
+	r3 = pFLASH_CTRL_BASE
+	r4 = FLASHWRITECMD
+	r5 = #1
+	r6 = bytes written
+	r7 = temp reg
+*/
+	0x07,0x4B,			/* ldr r3,pFLASH_CTRL_BASE */
+	0x08,0x4C,			/* ldr r4,FLASHWRITECMD */
+	0x01,0x25,			/* movs r5, 1 */
+	0x00,0x26,			/* movs r6, #0 */
+/* mainloop: */
+	0x19,0x60,			/* str	r1, [r3, #0] */
+	0x87,0x59,			/* ldr	r7, [r0, r6] */
+	0x5F,0x60,			/* str	r7, [r3, #4] */
+	0x9C,0x60,			/* str	r4, [r3, #8] */
+/* waitloop: */
+	0x9F,0x68,			/* ldr	r7, [r3, #8] */
+	0x2F,0x42,			/* tst	r7, r5 */
+	0xFC,0xD1,			/* bne	waitloop */
+	0x04,0x31,			/* adds	r1, r1, #4 */
+	0x04,0x36,			/* adds	r6, r6, #4 */
+	0x96,0x42,			/* cmp	r6, r2 */
+	0xF4,0xD1,			/* bne	mainloop */
+						/* exit: */
+	0xFE,0xE7,			/* b exit */
+/* pFLASH_CTRL_BASE: */
+	0x00,0xD0,0x0F,0x40,	/* .word	0x400FD000 */
+/* FLASHWRITECMD: */
+	0x01,0x00,0x42,0xA4 	/* .word	0xA4420001 */
+};
+
+static int stellaris_write_block(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t wcount)
+{
+	struct target *target = bank->target;
+	uint32_t buffer_size = 8192;
+	struct working_area *source;
+	struct working_area *write_algorithm;
+	uint32_t address = bank->base + offset;
+	struct reg_param reg_params[3];
+	struct armv7m_algorithm armv7m_info;
+	int retval = ERROR_OK;
+
+	LOG_DEBUG("(bank=%p buffer=%p offset=%08" PRIx32 " wcount=%08" PRIx32 "",
+			bank, buffer, offset, wcount);
+
+	/* flash write code */
+	if (target_alloc_working_area(target, sizeof(stellaris_write_code), &write_algorithm) != ERROR_OK)
+	{
+		LOG_WARNING("no working area available, can't do block memory writes");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	};
+
+	target_write_buffer(target, write_algorithm->address, sizeof(stellaris_write_code), stellaris_write_code);
+
+	/* memory buffer */
+	while (target_alloc_working_area(target, buffer_size, &source) != ERROR_OK)
+	{
+		LOG_DEBUG("called target_alloc_working_area(target=%p buffer_size=%08" PRIx32 " source=%p)",
+				target, buffer_size, source);
+		buffer_size /= 2;
+		if (buffer_size <= 256)
+		{
+			/* if we already allocated the writing code, but failed to get a buffer, free the algorithm */
+			if (write_algorithm)
+				target_free_working_area(target, write_algorithm);
+
+			LOG_WARNING("no large enough working area available, can't do block memory writes");
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+	};
+
+	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
+	armv7m_info.core_mode = ARMV7M_MODE_ANY;
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
+	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
+
+	while (wcount > 0)
+	{
+		uint32_t thisrun_count = (wcount > (buffer_size / 4)) ? (buffer_size / 4) : wcount;
+
+		target_write_buffer(target, source->address, thisrun_count * 4, buffer);
+
+		buf_set_u32(reg_params[0].value, 0, 32, source->address);
+		buf_set_u32(reg_params[1].value, 0, 32, address);
+		buf_set_u32(reg_params[2].value, 0, 32, 4*thisrun_count);
+		LOG_INFO("Algorithm flash write %" PRIi32 " words to 0x%" PRIx32 ", %" PRIi32 " remaining", thisrun_count, address, (wcount - thisrun_count));
+		LOG_DEBUG("Algorithm flash write %" PRIi32 " words to 0x%" PRIx32 ", %" PRIi32 " remaining", thisrun_count, address, (wcount - thisrun_count));
+		if ((retval = target_run_algorithm(target, 0, NULL, 3, reg_params, write_algorithm->address, write_algorithm->address + sizeof(stellaris_write_code)-10, 10000, &armv7m_info)) != ERROR_OK)
+		{
+			LOG_ERROR("error executing stellaris flash write algorithm");
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			break;
+		}
+
+		buffer += thisrun_count * 4;
+		address += thisrun_count * 4;
+		wcount -= thisrun_count;
+	}
+
+	target_free_working_area(target, write_algorithm);
+	target_free_working_area(target, source);
+
+	destroy_reg_param(&reg_params[0]);
+	destroy_reg_param(&reg_params[1]);
+	destroy_reg_param(&reg_params[2]);
+
+	return retval;
+}
+
+static int stellaris_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	struct stellaris_flash_bank *stellaris_info = bank->driver_priv;
+	struct target *target = bank->target;
+	uint32_t address = offset;
+	uint32_t flash_cris, flash_fmc;
+	uint32_t words_remaining = (count / 4);
+	uint32_t bytes_remaining = (count & 0x00000003);
+	uint32_t bytes_written = 0;
+	int retval;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	LOG_DEBUG("(bank=%p buffer=%p offset=%08" PRIx32 " count=%08" PRIx32 "",
+			bank, buffer, offset, count);
+
+	if (stellaris_info->did1 == 0)
+	{
+		stellaris_read_part_info(bank);
+	}
+
+	if (stellaris_info->did1 == 0)
+	{
+		LOG_WARNING("Cannot identify target as a Stellaris processor");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	if (offset & 0x3)
+	{
+		LOG_WARNING("offset size must be word aligned");
+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+	}
+
+	if (offset + count > bank->size)
+		return ERROR_FLASH_DST_OUT_OF_BANK;
+
+	/* Configure the flash controller timing */
+	stellaris_read_clock_info(bank);
+	stellaris_set_flash_mode(bank, 0);
+
+	/* Clear and disable flash programming interrupts */
+	target_write_u32(target, FLASH_CIM, 0);
+	target_write_u32(target, FLASH_MISC, PMISC | AMISC);
+
+	/* multiple words to be programmed? */
+	if (words_remaining > 0)
+	{
+		/* try using a block write */
+		if ((retval = stellaris_write_block(bank, buffer, offset, words_remaining)) != ERROR_OK)
+		{
+			if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
+			{
+				/* if block write failed (no sufficient working area),
+				 * we use normal (slow) single dword accesses */
+				LOG_WARNING("couldn't use block writes, falling back to single memory accesses");
+			}
+			else if (retval == ERROR_FLASH_OPERATION_FAILED)
+			{
+				/* if an error occured, we examine the reason, and quit */
+				target_read_u32(target, FLASH_CRIS, &flash_cris);
+
+				LOG_ERROR("flash writing failed with CRIS: 0x%" PRIx32 "", flash_cris);
+				return ERROR_FLASH_OPERATION_FAILED;
+			}
+		}
+		else
+		{
+			buffer += words_remaining * 4;
+			address += words_remaining * 4;
+			words_remaining = 0;
+		}
+	}
+
+	while (words_remaining > 0)
+	{
+		if (!(address & 0xff))
+			LOG_DEBUG("0x%" PRIx32 "", address);
+
+		/* Program one word */
+		target_write_u32(target, FLASH_FMA, address);
+		target_write_buffer(target, FLASH_FMD, 4, buffer);
+		target_write_u32(target, FLASH_FMC, FMC_WRKEY | FMC_WRITE);
+		/* LOG_DEBUG("0x%x 0x%x 0x%x",address,buf_get_u32(buffer, 0, 32),FMC_WRKEY | FMC_WRITE); */
+		/* Wait until write complete */
+		do
+		{
+			target_read_u32(target, FLASH_FMC, &flash_fmc);
+		} while (flash_fmc & FMC_WRITE);
+
+		buffer += 4;
+		address += 4;
+		words_remaining--;
+	}
+
+	if (bytes_remaining)
+	{
+		uint8_t last_word[4] = {0xff, 0xff, 0xff, 0xff};
+		int i = 0;
+
+		while (bytes_remaining > 0)
+		{
+			last_word[i++] = *(buffer + bytes_written);
+			bytes_remaining--;
+			bytes_written++;
+		}
+
+		if (!(address & 0xff))
+			LOG_DEBUG("0x%" PRIx32 "", address);
+
+		/* Program one word */
+		target_write_u32(target, FLASH_FMA, address);
+		target_write_buffer(target, FLASH_FMD, 4, last_word);
+		target_write_u32(target, FLASH_FMC, FMC_WRKEY | FMC_WRITE);
+		/* LOG_DEBUG("0x%x 0x%x 0x%x",address,buf_get_u32(buffer, 0, 32),FMC_WRKEY | FMC_WRITE); */
+		/* Wait until write complete */
+		do
+		{
+			target_read_u32(target, FLASH_FMC, &flash_fmc);
+		} while (flash_fmc & FMC_WRITE);
+	}
+
+	/* Check access violations */
+	target_read_u32(target, FLASH_CRIS, &flash_cris);
+	if (flash_cris & (AMASK))
+	{
+		LOG_DEBUG("flash_cris 0x%" PRIx32 "", flash_cris);
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+	return ERROR_OK;
+}
+
+static int stellaris_probe(struct flash_bank *bank)
+{
+	/* we can't probe on an stellaris
+	 * if this is an stellaris, it has the configured flash
+	 */
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* stellaris_read_part_info() already takes care about error checking and reporting */
+	return stellaris_read_part_info(bank);
+}
+
+static int stellaris_auto_probe(struct flash_bank *bank)
+{
+	struct stellaris_flash_bank *stellaris_info = bank->driver_priv;
+	if (stellaris_info->did1)
+		return ERROR_OK;
+	return stellaris_probe(bank);
+}
+
+static int stellaris_mass_erase(struct flash_bank *bank)
+{
+	struct target *target = NULL;
+	struct stellaris_flash_bank *stellaris_info = NULL;
+	uint32_t flash_fmc;
+
+	stellaris_info = bank->driver_priv;
+	target = bank->target;
+
+	if (target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (stellaris_info->did1 == 0)
+	{
+		stellaris_read_part_info(bank);
+	}
+
+	if (stellaris_info->did1 == 0)
+	{
+		LOG_WARNING("Cannot identify target as Stellaris");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	/* Configure the flash controller timing */
+	stellaris_read_clock_info(bank);
+	stellaris_set_flash_mode(bank, 0);
+
+	/* Clear and disable flash programming interrupts */
+	target_write_u32(target, FLASH_CIM, 0);
+	target_write_u32(target, FLASH_MISC, PMISC | AMISC);
+
+	target_write_u32(target, FLASH_FMA, 0);
+	target_write_u32(target, FLASH_FMC, FMC_WRKEY | FMC_MERASE);
+	/* Wait until erase complete */
+	do
+	{
+		target_read_u32(target, FLASH_FMC, &flash_fmc);
+	}
+	while (flash_fmc & FMC_MERASE);
+
+	/* if device has > 128k, then second erase cycle is needed
+	 * this is only valid for older devices, but will not hurt */
+	if (stellaris_info->num_pages * stellaris_info->pagesize > 0x20000)
+	{
+		target_write_u32(target, FLASH_FMA, 0x20000);
+		target_write_u32(target, FLASH_FMC, FMC_WRKEY | FMC_MERASE);
+		/* Wait until erase complete */
+		do
+		{
+			target_read_u32(target, FLASH_FMC, &flash_fmc);
+		}
+		while (flash_fmc & FMC_MERASE);
+	}
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(stellaris_handle_mass_erase_command)
+{
+	int i;
+
+	if (CMD_ARGC < 1)
+	{
+		command_print(CMD_CTX, "stellaris mass_erase <bank>");
+		return ERROR_OK;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	if (stellaris_mass_erase(bank) == ERROR_OK)
+	{
+		/* set all sectors as erased */
+		for (i = 0; i < bank->num_sectors; i++)
+		{
+			bank->sectors[i].is_erased = 1;
+		}
+
+		command_print(CMD_CTX, "stellaris mass erase complete");
+	}
+	else
+	{
+		command_print(CMD_CTX, "stellaris mass erase failed");
+	}
+
+	return ERROR_OK;
+}
+
+static const struct command_registration stellaris_exec_command_handlers[] = {
+	{
+		.name = "mass_erase",
+		.handler = &stellaris_handle_mass_erase_command,
+		.mode = COMMAND_EXEC,
+		.help = "erase entire device",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+static const struct command_registration stellaris_command_handlers[] = {
+	{
+		.name = "stellaris",
+		.mode = COMMAND_ANY,
+		.help = "Stellaris flash command group",
+		.chain = stellaris_exec_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+struct flash_driver stellaris_flash = {
+		.name = "stellaris",
+		.commands = stellaris_command_handlers,
+		.flash_bank_command = &stellaris_flash_bank_command,
+		.erase = &stellaris_erase,
+		.protect = &stellaris_protect,
+		.write = &stellaris_write,
+		.probe = &stellaris_probe,
+		.auto_probe = &stellaris_auto_probe,
+		.erase_check = &default_flash_mem_blank_check,
+		.protect_check = &stellaris_protect_check,
+		.info = &stellaris_info,
+	};
diff --git a/src/flash/nor/stellaris.h b/src/flash/nor/stellaris.h
new file mode 100644
index 00000000..949a346d
--- /dev/null
+++ b/src/flash/nor/stellaris.h
@@ -0,0 +1,99 @@
+/***************************************************************************
+ *   Copyright (C) 2006 by Magnus Lundin                                   *
+ *   lundin@mlu.mine.nu                                                    *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifndef STELLARIS_FLASH_H
+#define STELLARIS_FLASH_H
+
+#include "flash.h"
+
+struct stellaris_flash_bank
+{
+	/* chip id register */
+	uint32_t did0;
+	uint32_t did1;
+	uint32_t dc0;
+	uint32_t dc1;
+
+	char * target_name;
+
+	uint32_t sramsiz;
+	uint32_t flshsz;
+	/* flash geometry */
+	uint32_t num_pages;
+	uint32_t pagesize;
+	uint32_t pages_in_lockregion;
+
+	/* nv memory bits */
+	uint16_t num_lockbits;
+	uint32_t lockbits;
+
+	/* main clock status */
+	uint32_t rcc;
+	uint32_t rcc2;
+	uint8_t  mck_valid;
+	uint8_t  xtal_mask;
+	uint32_t iosc_freq;
+	uint32_t mck_freq;
+	const char *iosc_desc;
+	const char *mck_desc;
+};
+
+/* STELLARIS control registers */
+#define SCB_BASE	0x400FE000
+#define	DID0		0x000
+#define	DID1		0x004
+#define	DC0			0x008
+#define	DC1			0x010
+#define	DC2			0x014
+#define	DC3			0x018
+#define	DC4			0x01C
+
+#define	RIS			0x050
+#define	RCC			0x060
+#define	PLLCFG		0x064
+#define	RCC2		0x070
+
+#define FMPRE		0x130
+#define FMPPE		0x134
+#define USECRL		0x140
+
+#define FLASH_CONTROL_BASE	0x400FD000
+#define FLASH_FMA	(FLASH_CONTROL_BASE | 0x000)
+#define FLASH_FMD	(FLASH_CONTROL_BASE | 0x004)
+#define FLASH_FMC	(FLASH_CONTROL_BASE | 0x008)
+#define FLASH_CRIS	(FLASH_CONTROL_BASE | 0x00C)
+#define FLASH_CIM	(FLASH_CONTROL_BASE | 0x010)
+#define FLASH_MISC	(FLASH_CONTROL_BASE | 0x014)
+
+#define AMISC	1
+#define PMISC	2
+
+#define AMASK	1
+#define PMASK	2
+
+/* Flash Controller Command bits */
+#define FMC_WRKEY	(0xA442 << 16)
+#define FMC_COMT	(1 << 3)
+#define FMC_MERASE	(1 << 2)
+#define FMC_ERASE	(1 << 1)
+#define FMC_WRITE	(1 << 0)
+
+/* STELLARIS constants */
+
+#endif /* STELLARIS_H */
diff --git a/src/flash/nor/stm32x.c b/src/flash/nor/stm32x.c
new file mode 100644
index 00000000..2f51aa55
--- /dev/null
+++ b/src/flash/nor/stm32x.c
@@ -0,0 +1,1240 @@
+/***************************************************************************
+ *   Copyright (C) 2005 by Dominic Rath                                    *
+ *   Dominic.Rath@gmx.de                                                   *
+ *                                                                         *
+ *   Copyright (C) 2008 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "stm32x.h"
+#include "armv7m.h"
+#include "binarybuffer.h"
+#include "algorithm.h"
+
+
+static int stm32x_mass_erase(struct flash_bank *bank);
+
+/* flash bank stm32x <base> <size> 0 0 <target#>
+ */
+FLASH_BANK_COMMAND_HANDLER(stm32x_flash_bank_command)
+{
+	struct stm32x_flash_bank *stm32x_info;
+
+	if (CMD_ARGC < 6)
+	{
+		LOG_WARNING("incomplete flash_bank stm32x configuration");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	stm32x_info = malloc(sizeof(struct stm32x_flash_bank));
+	bank->driver_priv = stm32x_info;
+
+	stm32x_info->write_algorithm = NULL;
+	stm32x_info->probed = 0;
+
+	return ERROR_OK;
+}
+
+static uint32_t stm32x_get_flash_status(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	uint32_t status;
+
+	target_read_u32(target, STM32_FLASH_SR, &status);
+
+	return status;
+}
+
+static uint32_t stm32x_wait_status_busy(struct flash_bank *bank, int timeout)
+{
+	struct target *target = bank->target;
+	uint32_t status;
+
+	/* wait for busy to clear */
+	while (((status = stm32x_get_flash_status(bank)) & FLASH_BSY) && (timeout-- > 0))
+	{
+		LOG_DEBUG("status: 0x%" PRIx32 "", status);
+		alive_sleep(1);
+	}
+	/* Clear but report errors */
+	if (status & (FLASH_WRPRTERR | FLASH_PGERR))
+	{
+		target_write_u32(target, STM32_FLASH_SR, FLASH_WRPRTERR | FLASH_PGERR);
+	}
+	return status;
+}
+
+static int stm32x_read_options(struct flash_bank *bank)
+{
+	uint32_t optiondata;
+	struct stm32x_flash_bank *stm32x_info = NULL;
+	struct target *target = bank->target;
+
+	stm32x_info = bank->driver_priv;
+
+	/* read current option bytes */
+	target_read_u32(target, STM32_FLASH_OBR, &optiondata);
+
+	stm32x_info->option_bytes.user_options = (uint16_t)0xFFF8 | ((optiondata >> 2) & 0x07);
+	stm32x_info->option_bytes.RDP = (optiondata & (1 << OPT_READOUT)) ? 0xFFFF : 0x5AA5;
+
+	if (optiondata & (1 << OPT_READOUT))
+		LOG_INFO("Device Security Bit Set");
+
+	/* each bit refers to a 4bank protection */
+	target_read_u32(target, STM32_FLASH_WRPR, &optiondata);
+
+	stm32x_info->option_bytes.protection[0] = (uint16_t)optiondata;
+	stm32x_info->option_bytes.protection[1] = (uint16_t)(optiondata >> 8);
+	stm32x_info->option_bytes.protection[2] = (uint16_t)(optiondata >> 16);
+	stm32x_info->option_bytes.protection[3] = (uint16_t)(optiondata >> 24);
+
+	return ERROR_OK;
+}
+
+static int stm32x_erase_options(struct flash_bank *bank)
+{
+	struct stm32x_flash_bank *stm32x_info = NULL;
+	struct target *target = bank->target;
+	uint32_t status;
+
+	stm32x_info = bank->driver_priv;
+
+	/* read current options */
+	stm32x_read_options(bank);
+
+	/* unlock flash registers */
+	target_write_u32(target, STM32_FLASH_KEYR, KEY1);
+	target_write_u32(target, STM32_FLASH_KEYR, KEY2);
+
+	/* unlock option flash registers */
+	target_write_u32(target, STM32_FLASH_OPTKEYR, KEY1);
+	target_write_u32(target, STM32_FLASH_OPTKEYR, KEY2);
+
+	/* erase option bytes */
+	target_write_u32(target, STM32_FLASH_CR, FLASH_OPTER | FLASH_OPTWRE);
+	target_write_u32(target, STM32_FLASH_CR, FLASH_OPTER | FLASH_STRT | FLASH_OPTWRE);
+
+	status = stm32x_wait_status_busy(bank, 10);
+
+	if (status & FLASH_WRPRTERR)
+		return ERROR_FLASH_OPERATION_FAILED;
+	if (status & FLASH_PGERR)
+		return ERROR_FLASH_OPERATION_FAILED;
+
+	/* clear readout protection and complementary option bytes
+	 * this will also force a device unlock if set */
+	stm32x_info->option_bytes.RDP = 0x5AA5;
+
+	return ERROR_OK;
+}
+
+static int stm32x_write_options(struct flash_bank *bank)
+{
+	struct stm32x_flash_bank *stm32x_info = NULL;
+	struct target *target = bank->target;
+	uint32_t status;
+
+	stm32x_info = bank->driver_priv;
+
+	/* unlock flash registers */
+	target_write_u32(target, STM32_FLASH_KEYR, KEY1);
+	target_write_u32(target, STM32_FLASH_KEYR, KEY2);
+
+	/* unlock option flash registers */
+	target_write_u32(target, STM32_FLASH_OPTKEYR, KEY1);
+	target_write_u32(target, STM32_FLASH_OPTKEYR, KEY2);
+
+	/* program option bytes */
+	target_write_u32(target, STM32_FLASH_CR, FLASH_OPTPG | FLASH_OPTWRE);
+
+	/* write user option byte */
+	target_write_u16(target, STM32_OB_USER, stm32x_info->option_bytes.user_options);
+
+	status = stm32x_wait_status_busy(bank, 10);
+
+	if (status & FLASH_WRPRTERR)
+		return ERROR_FLASH_OPERATION_FAILED;
+	if (status & FLASH_PGERR)
+		return ERROR_FLASH_OPERATION_FAILED;
+
+	/* write protection byte 1 */
+	target_write_u16(target, STM32_OB_WRP0, stm32x_info->option_bytes.protection[0]);
+
+	status = stm32x_wait_status_busy(bank, 10);
+
+	if (status & FLASH_WRPRTERR)
+		return ERROR_FLASH_OPERATION_FAILED;
+	if (status & FLASH_PGERR)
+		return ERROR_FLASH_OPERATION_FAILED;
+
+	/* write protection byte 2 */
+	target_write_u16(target, STM32_OB_WRP1, stm32x_info->option_bytes.protection[1]);
+
+	status = stm32x_wait_status_busy(bank, 10);
+
+	if (status & FLASH_WRPRTERR)
+		return ERROR_FLASH_OPERATION_FAILED;
+	if (status & FLASH_PGERR)
+		return ERROR_FLASH_OPERATION_FAILED;
+
+	/* write protection byte 3 */
+	target_write_u16(target, STM32_OB_WRP2, stm32x_info->option_bytes.protection[2]);
+
+	status = stm32x_wait_status_busy(bank, 10);
+
+	if (status & FLASH_WRPRTERR)
+		return ERROR_FLASH_OPERATION_FAILED;
+	if (status & FLASH_PGERR)
+		return ERROR_FLASH_OPERATION_FAILED;
+
+	/* write protection byte 4 */
+	target_write_u16(target, STM32_OB_WRP3, stm32x_info->option_bytes.protection[3]);
+
+	status = stm32x_wait_status_busy(bank, 10);
+
+	if (status & FLASH_WRPRTERR)
+		return ERROR_FLASH_OPERATION_FAILED;
+	if (status & FLASH_PGERR)
+		return ERROR_FLASH_OPERATION_FAILED;
+
+	/* write readout protection bit */
+	target_write_u16(target, STM32_OB_RDP, stm32x_info->option_bytes.RDP);
+
+	status = stm32x_wait_status_busy(bank, 10);
+
+	if (status & FLASH_WRPRTERR)
+		return ERROR_FLASH_OPERATION_FAILED;
+	if (status & FLASH_PGERR)
+		return ERROR_FLASH_OPERATION_FAILED;
+
+	target_write_u32(target, STM32_FLASH_CR, FLASH_LOCK);
+
+	return ERROR_OK;
+}
+
+static int stm32x_protect_check(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct stm32x_flash_bank *stm32x_info = bank->driver_priv;
+
+	uint32_t protection;
+	int i, s;
+	int num_bits;
+	int set;
+
+	if (target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* medium density - each bit refers to a 4bank protection
+	 * high density - each bit refers to a 2bank protection */
+	target_read_u32(target, STM32_FLASH_WRPR, &protection);
+
+	/* medium density - each protection bit is for 4 * 1K pages
+	 * high density - each protection bit is for 2 * 2K pages */
+	num_bits = (bank->num_sectors / stm32x_info->ppage_size);
+
+	if (stm32x_info->ppage_size == 2)
+	{
+		/* high density flash/connectivity line protection */
+
+		set = 1;
+
+		if (protection & (1 << 31))
+			set = 0;
+
+		/* bit 31 controls sector 62 - 255 protection for high density
+		 * bit 31 controls sector 62 - 127 protection for connectivity line */
+		for (s = 62; s < bank->num_sectors; s++)
+		{
+			bank->sectors[s].is_protected = set;
+		}
+
+		if (bank->num_sectors > 61)
+			num_bits = 31;
+
+		for (i = 0; i < num_bits; i++)
+		{
+			set = 1;
+
+			if (protection & (1 << i))
+				set = 0;
+
+			for (s = 0; s < stm32x_info->ppage_size; s++)
+				bank->sectors[(i * stm32x_info->ppage_size) + s].is_protected = set;
+		}
+	}
+	else
+	{
+		/* low/medium density flash protection */
+		for (i = 0; i < num_bits; i++)
+		{
+			set = 1;
+
+			if (protection & (1 << i))
+				set = 0;
+
+			for (s = 0; s < stm32x_info->ppage_size; s++)
+				bank->sectors[(i * stm32x_info->ppage_size) + s].is_protected = set;
+		}
+	}
+
+	return ERROR_OK;
+}
+
+static int stm32x_erase(struct flash_bank *bank, int first, int last)
+{
+	struct target *target = bank->target;
+	int i;
+	uint32_t status;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if ((first == 0) && (last == (bank->num_sectors - 1)))
+	{
+		return stm32x_mass_erase(bank);
+	}
+
+	/* unlock flash registers */
+	target_write_u32(target, STM32_FLASH_KEYR, KEY1);
+	target_write_u32(target, STM32_FLASH_KEYR, KEY2);
+
+	for (i = first; i <= last; i++)
+	{
+		target_write_u32(target, STM32_FLASH_CR, FLASH_PER);
+		target_write_u32(target, STM32_FLASH_AR, bank->base + bank->sectors[i].offset);
+		target_write_u32(target, STM32_FLASH_CR, FLASH_PER | FLASH_STRT);
+
+		status = stm32x_wait_status_busy(bank, 10);
+
+		if (status & FLASH_WRPRTERR)
+			return ERROR_FLASH_OPERATION_FAILED;
+		if (status & FLASH_PGERR)
+			return ERROR_FLASH_OPERATION_FAILED;
+		bank->sectors[i].is_erased = 1;
+	}
+
+	target_write_u32(target, STM32_FLASH_CR, FLASH_LOCK);
+
+	return ERROR_OK;
+}
+
+static int stm32x_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	struct stm32x_flash_bank *stm32x_info = NULL;
+	struct target *target = bank->target;
+	uint16_t prot_reg[4] = {0xFFFF, 0xFFFF, 0xFFFF, 0xFFFF};
+	int i, reg, bit;
+	int status;
+	uint32_t protection;
+
+	stm32x_info = bank->driver_priv;
+
+	if (target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if ((first && (first % stm32x_info->ppage_size)) || ((last + 1) && (last + 1) % stm32x_info->ppage_size))
+	{
+		LOG_WARNING("Error: start and end sectors must be on a %d sector boundary", stm32x_info->ppage_size);
+		return ERROR_FLASH_SECTOR_INVALID;
+	}
+
+	/* medium density - each bit refers to a 4bank protection
+	 * high density - each bit refers to a 2bank protection */
+	target_read_u32(target, STM32_FLASH_WRPR, &protection);
+
+	prot_reg[0] = (uint16_t)protection;
+	prot_reg[1] = (uint16_t)(protection >> 8);
+	prot_reg[2] = (uint16_t)(protection >> 16);
+	prot_reg[3] = (uint16_t)(protection >> 24);
+
+	if (stm32x_info->ppage_size == 2)
+	{
+		/* high density flash */
+
+		/* bit 7 controls sector 62 - 255 protection */
+		if (last > 61)
+		{
+			if (set)
+				prot_reg[3] &= ~(1 << 7);
+			else
+				prot_reg[3] |= (1 << 7);
+		}
+
+		if (first > 61)
+			first = 62;
+		if (last > 61)
+			last = 61;
+
+		for (i = first; i <= last; i++)
+		{
+			reg = (i / stm32x_info->ppage_size) / 8;
+			bit = (i / stm32x_info->ppage_size) - (reg * 8);
+
+			if (set)
+				prot_reg[reg] &= ~(1 << bit);
+			else
+				prot_reg[reg] |= (1 << bit);
+		}
+	}
+	else
+	{
+		/* medium density flash */
+		for (i = first; i <= last; i++)
+		{
+			reg = (i / stm32x_info->ppage_size) / 8;
+			bit = (i / stm32x_info->ppage_size) - (reg * 8);
+
+			if (set)
+				prot_reg[reg] &= ~(1 << bit);
+			else
+				prot_reg[reg] |= (1 << bit);
+		}
+	}
+
+	if ((status = stm32x_erase_options(bank)) != ERROR_OK)
+		return status;
+
+	stm32x_info->option_bytes.protection[0] = prot_reg[0];
+	stm32x_info->option_bytes.protection[1] = prot_reg[1];
+	stm32x_info->option_bytes.protection[2] = prot_reg[2];
+	stm32x_info->option_bytes.protection[3] = prot_reg[3];
+
+	return stm32x_write_options(bank);
+}
+
+static int stm32x_write_block(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	struct stm32x_flash_bank *stm32x_info = bank->driver_priv;
+	struct target *target = bank->target;
+	uint32_t buffer_size = 16384;
+	struct working_area *source;
+	uint32_t address = bank->base + offset;
+	struct reg_param reg_params[4];
+	struct armv7m_algorithm armv7m_info;
+	int retval = ERROR_OK;
+
+	uint8_t stm32x_flash_write_code[] = {
+									/* write: */
+		0xDF, 0xF8, 0x24, 0x40,		/* ldr	r4, STM32_FLASH_CR */
+		0x09, 0x4D,					/* ldr	r5, STM32_FLASH_SR */
+		0x4F, 0xF0, 0x01, 0x03,		/* mov	r3, #1 */
+		0x23, 0x60,					/* str	r3, [r4, #0] */
+		0x30, 0xF8, 0x02, 0x3B,		/* ldrh r3, [r0], #2 */
+		0x21, 0xF8, 0x02, 0x3B,		/* strh r3, [r1], #2 */
+									/* busy: */
+		0x2B, 0x68,					/* ldr 	r3, [r5, #0] */
+		0x13, 0xF0, 0x01, 0x0F,		/* tst 	r3, #0x01 */
+		0xFB, 0xD0,					/* beq 	busy */
+		0x13, 0xF0, 0x14, 0x0F,		/* tst	r3, #0x14 */
+		0x01, 0xD1,					/* bne	exit */
+		0x01, 0x3A,					/* subs	r2, r2, #1 */
+		0xED, 0xD1,					/* bne	write */
+									/* exit: */
+		0xFE, 0xE7,					/* b exit */
+		0x10, 0x20, 0x02, 0x40,		/* STM32_FLASH_CR:	.word 0x40022010 */
+		0x0C, 0x20, 0x02, 0x40		/* STM32_FLASH_SR:	.word 0x4002200C */
+	};
+
+	/* flash write code */
+	if (target_alloc_working_area(target, sizeof(stm32x_flash_write_code), &stm32x_info->write_algorithm) != ERROR_OK)
+	{
+		LOG_WARNING("no working area available, can't do block memory writes");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	};
+
+	if ((retval = target_write_buffer(target, stm32x_info->write_algorithm->address, sizeof(stm32x_flash_write_code), stm32x_flash_write_code)) != ERROR_OK)
+		return retval;
+
+	/* memory buffer */
+	while (target_alloc_working_area(target, buffer_size, &source) != ERROR_OK)
+	{
+		buffer_size /= 2;
+		if (buffer_size <= 256)
+		{
+			/* if we already allocated the writing code, but failed to get a buffer, free the algorithm */
+			if (stm32x_info->write_algorithm)
+				target_free_working_area(target, stm32x_info->write_algorithm);
+
+			LOG_WARNING("no large enough working area available, can't do block memory writes");
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+	};
+
+	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
+	armv7m_info.core_mode = ARMV7M_MODE_ANY;
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
+	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
+	init_reg_param(&reg_params[3], "r3", 32, PARAM_IN);
+
+	while (count > 0)
+	{
+		uint32_t thisrun_count = (count > (buffer_size / 2)) ? (buffer_size / 2) : count;
+
+		if ((retval = target_write_buffer(target, source->address, thisrun_count * 2, buffer)) != ERROR_OK)
+			break;
+
+		buf_set_u32(reg_params[0].value, 0, 32, source->address);
+		buf_set_u32(reg_params[1].value, 0, 32, address);
+		buf_set_u32(reg_params[2].value, 0, 32, thisrun_count);
+
+		if ((retval = target_run_algorithm(target, 0, NULL, 4, reg_params, stm32x_info->write_algorithm->address, \
+				stm32x_info->write_algorithm->address + (sizeof(stm32x_flash_write_code) - 10), 10000, &armv7m_info)) != ERROR_OK)
+		{
+			LOG_ERROR("error executing stm32x flash write algorithm");
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			break;
+		}
+
+		if (buf_get_u32(reg_params[3].value, 0, 32) & FLASH_PGERR)
+		{
+			LOG_ERROR("flash memory not erased before writing");
+			/* Clear but report errors */
+			target_write_u32(target, STM32_FLASH_SR, FLASH_PGERR);
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			break;
+		}
+
+		if (buf_get_u32(reg_params[3].value, 0, 32) & FLASH_WRPRTERR)
+		{
+			LOG_ERROR("flash memory write protected");
+			/* Clear but report errors */
+			target_write_u32(target, STM32_FLASH_SR, FLASH_WRPRTERR);
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			break;
+		}
+
+		buffer += thisrun_count * 2;
+		address += thisrun_count * 2;
+		count -= thisrun_count;
+	}
+
+	target_free_working_area(target, source);
+	target_free_working_area(target, stm32x_info->write_algorithm);
+
+	destroy_reg_param(&reg_params[0]);
+	destroy_reg_param(&reg_params[1]);
+	destroy_reg_param(&reg_params[2]);
+	destroy_reg_param(&reg_params[3]);
+
+	return retval;
+}
+
+static int stm32x_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	uint32_t words_remaining = (count / 2);
+	uint32_t bytes_remaining = (count & 0x00000001);
+	uint32_t address = bank->base + offset;
+	uint32_t bytes_written = 0;
+	uint8_t status;
+	int retval;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (offset & 0x1)
+	{
+		LOG_WARNING("offset 0x%" PRIx32 " breaks required 2-byte alignment", offset);
+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+	}
+
+	/* unlock flash registers */
+	target_write_u32(target, STM32_FLASH_KEYR, KEY1);
+	target_write_u32(target, STM32_FLASH_KEYR, KEY2);
+
+	/* multiple half words (2-byte) to be programmed? */
+	if (words_remaining > 0)
+	{
+		/* try using a block write */
+		if ((retval = stm32x_write_block(bank, buffer, offset, words_remaining)) != ERROR_OK)
+		{
+			if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
+			{
+				/* if block write failed (no sufficient working area),
+				 * we use normal (slow) single dword accesses */
+				LOG_WARNING("couldn't use block writes, falling back to single memory accesses");
+			}
+			else if (retval == ERROR_FLASH_OPERATION_FAILED)
+			{
+				LOG_ERROR("flash writing failed with error code: 0x%x", retval);
+				return ERROR_FLASH_OPERATION_FAILED;
+			}
+		}
+		else
+		{
+			buffer += words_remaining * 2;
+			address += words_remaining * 2;
+			words_remaining = 0;
+		}
+	}
+
+	while (words_remaining > 0)
+	{
+		uint16_t value;
+		memcpy(&value, buffer + bytes_written, sizeof(uint16_t));
+
+		target_write_u32(target, STM32_FLASH_CR, FLASH_PG);
+		target_write_u16(target, address, value);
+
+		status = stm32x_wait_status_busy(bank, 5);
+
+		if (status & FLASH_WRPRTERR)
+		{
+			LOG_ERROR("flash memory not erased before writing");
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+		if (status & FLASH_PGERR)
+		{
+			LOG_ERROR("flash memory write protected");
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+
+		bytes_written += 2;
+		words_remaining--;
+		address += 2;
+	}
+
+	if (bytes_remaining)
+	{
+		uint16_t value = 0xffff;
+		memcpy(&value, buffer + bytes_written, bytes_remaining);
+
+		target_write_u32(target, STM32_FLASH_CR, FLASH_PG);
+		target_write_u16(target, address, value);
+
+		status = stm32x_wait_status_busy(bank, 5);
+
+		if (status & FLASH_WRPRTERR)
+		{
+			LOG_ERROR("flash memory not erased before writing");
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+		if (status & FLASH_PGERR)
+		{
+			LOG_ERROR("flash memory write protected");
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+	}
+
+	target_write_u32(target, STM32_FLASH_CR, FLASH_LOCK);
+
+	return ERROR_OK;
+}
+
+static int stm32x_probe(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct stm32x_flash_bank *stm32x_info = bank->driver_priv;
+	int i;
+	uint16_t num_pages;
+	uint32_t device_id;
+	int page_size;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	stm32x_info->probed = 0;
+
+	/* read stm32 device id register */
+	target_read_u32(target, 0xE0042000, &device_id);
+	LOG_INFO("device id = 0x%08" PRIx32 "", device_id);
+
+	/* get flash size from target */
+	if (target_read_u16(target, 0x1FFFF7E0, &num_pages) != ERROR_OK)
+	{
+		/* failed reading flash size, default to max target family */
+		num_pages = 0xffff;
+	}
+
+	if ((device_id & 0x7ff) == 0x410)
+	{
+		/* medium density - we have 1k pages
+		 * 4 pages for a protection area */
+		page_size = 1024;
+		stm32x_info->ppage_size = 4;
+
+		/* check for early silicon */
+		if (num_pages == 0xffff)
+		{
+			/* number of sectors incorrect on revA */
+			LOG_WARNING("STM32 flash size failed, probe inaccurate - assuming 128k flash");
+			num_pages = 128;
+		}
+	}
+	else if ((device_id & 0x7ff) == 0x412)
+	{
+		/* low density - we have 1k pages
+		 * 4 pages for a protection area */
+		page_size = 1024;
+		stm32x_info->ppage_size = 4;
+
+		/* check for early silicon */
+		if (num_pages == 0xffff)
+		{
+			/* number of sectors incorrect on revA */
+			LOG_WARNING("STM32 flash size failed, probe inaccurate - assuming 32k flash");
+			num_pages = 32;
+		}
+	}
+	else if ((device_id & 0x7ff) == 0x414)
+	{
+		/* high density - we have 2k pages
+		 * 2 pages for a protection area */
+		page_size = 2048;
+		stm32x_info->ppage_size = 2;
+
+		/* check for early silicon */
+		if (num_pages == 0xffff)
+		{
+			/* number of sectors incorrect on revZ */
+			LOG_WARNING("STM32 flash size failed, probe inaccurate - assuming 512k flash");
+			num_pages = 512;
+		}
+	}
+	else if ((device_id & 0x7ff) == 0x418)
+	{
+		/* connectivity line density - we have 2k pages
+		 * 2 pages for a protection area */
+		page_size = 2048;
+		stm32x_info->ppage_size = 2;
+
+		/* check for early silicon */
+		if (num_pages == 0xffff)
+		{
+			/* number of sectors incorrect on revZ */
+			LOG_WARNING("STM32 flash size failed, probe inaccurate - assuming 256k flash");
+			num_pages = 256;
+		}
+	}
+	else
+	{
+		LOG_WARNING("Cannot identify target as a STM32 family.");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	LOG_INFO("flash size = %dkbytes", num_pages);
+
+	/* calculate numbers of pages */
+	num_pages /= (page_size / 1024);
+
+	bank->base = 0x08000000;
+	bank->size = (num_pages * page_size);
+	bank->num_sectors = num_pages;
+	bank->sectors = malloc(sizeof(struct flash_sector) * num_pages);
+
+	for (i = 0; i < num_pages; i++)
+	{
+		bank->sectors[i].offset = i * page_size;
+		bank->sectors[i].size = page_size;
+		bank->sectors[i].is_erased = -1;
+		bank->sectors[i].is_protected = 1;
+	}
+
+	stm32x_info->probed = 1;
+
+	return ERROR_OK;
+}
+
+static int stm32x_auto_probe(struct flash_bank *bank)
+{
+	struct stm32x_flash_bank *stm32x_info = bank->driver_priv;
+	if (stm32x_info->probed)
+		return ERROR_OK;
+	return stm32x_probe(bank);
+}
+
+#if 0
+COMMAND_HANDLER(stm32x_handle_part_id_command)
+{
+	return ERROR_OK;
+}
+#endif
+
+static int stm32x_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	struct target *target = bank->target;
+	uint32_t device_id;
+	int printed;
+
+	/* read stm32 device id register */
+	target_read_u32(target, 0xE0042000, &device_id);
+
+	if ((device_id & 0x7ff) == 0x410)
+	{
+		printed = snprintf(buf, buf_size, "stm32x (Medium Density) - Rev: ");
+		buf += printed;
+		buf_size -= printed;
+
+		switch (device_id >> 16)
+		{
+			case 0x0000:
+				snprintf(buf, buf_size, "A");
+				break;
+
+			case 0x2000:
+				snprintf(buf, buf_size, "B");
+				break;
+
+			case 0x2001:
+				snprintf(buf, buf_size, "Z");
+				break;
+
+			case 0x2003:
+				snprintf(buf, buf_size, "Y");
+				break;
+
+			default:
+				snprintf(buf, buf_size, "unknown");
+				break;
+		}
+	}
+	else if ((device_id & 0x7ff) == 0x412)
+	{
+		printed = snprintf(buf, buf_size, "stm32x (Low Density) - Rev: ");
+		buf += printed;
+		buf_size -= printed;
+
+		switch (device_id >> 16)
+		{
+			case 0x1000:
+				snprintf(buf, buf_size, "A");
+				break;
+
+			default:
+				snprintf(buf, buf_size, "unknown");
+				break;
+		}
+	}
+	else if ((device_id & 0x7ff) == 0x414)
+	{
+		printed = snprintf(buf, buf_size, "stm32x (High Density) - Rev: ");
+		buf += printed;
+		buf_size -= printed;
+
+		switch (device_id >> 16)
+		{
+			case 0x1000:
+				snprintf(buf, buf_size, "A");
+				break;
+
+			case 0x1001:
+				snprintf(buf, buf_size, "Z");
+				break;
+
+			default:
+				snprintf(buf, buf_size, "unknown");
+				break;
+		}
+	}
+	else if ((device_id & 0x7ff) == 0x418)
+	{
+		printed = snprintf(buf, buf_size, "stm32x (Connectivity) - Rev: ");
+		buf += printed;
+		buf_size -= printed;
+
+		switch (device_id >> 16)
+		{
+			case 0x1000:
+				snprintf(buf, buf_size, "A");
+				break;
+
+			case 0x1001:
+				snprintf(buf, buf_size, "Z");
+				break;
+
+			default:
+				snprintf(buf, buf_size, "unknown");
+				break;
+		}
+	}
+	else
+	{
+		snprintf(buf, buf_size, "Cannot identify target as a stm32x\n");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(stm32x_handle_lock_command)
+{
+	struct target *target = NULL;
+	struct stm32x_flash_bank *stm32x_info = NULL;
+
+	if (CMD_ARGC < 1)
+	{
+		command_print(CMD_CTX, "stm32x lock <bank>");
+		return ERROR_OK;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	stm32x_info = bank->driver_priv;
+
+	target = bank->target;
+
+	if (target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (stm32x_erase_options(bank) != ERROR_OK)
+	{
+		command_print(CMD_CTX, "stm32x failed to erase options");
+		return ERROR_OK;
+	}
+
+	/* set readout protection */
+	stm32x_info->option_bytes.RDP = 0;
+
+	if (stm32x_write_options(bank) != ERROR_OK)
+	{
+		command_print(CMD_CTX, "stm32x failed to lock device");
+		return ERROR_OK;
+	}
+
+	command_print(CMD_CTX, "stm32x locked");
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(stm32x_handle_unlock_command)
+{
+	struct target *target = NULL;
+	struct stm32x_flash_bank *stm32x_info = NULL;
+
+	if (CMD_ARGC < 1)
+	{
+		command_print(CMD_CTX, "stm32x unlock <bank>");
+		return ERROR_OK;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	stm32x_info = bank->driver_priv;
+
+	target = bank->target;
+
+	if (target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (stm32x_erase_options(bank) != ERROR_OK)
+	{
+		command_print(CMD_CTX, "stm32x failed to unlock device");
+		return ERROR_OK;
+	}
+
+	if (stm32x_write_options(bank) != ERROR_OK)
+	{
+		command_print(CMD_CTX, "stm32x failed to lock device");
+		return ERROR_OK;
+	}
+
+	command_print(CMD_CTX, "stm32x unlocked");
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(stm32x_handle_options_read_command)
+{
+	uint32_t optionbyte;
+	struct target *target = NULL;
+	struct stm32x_flash_bank *stm32x_info = NULL;
+
+	if (CMD_ARGC < 1)
+	{
+		command_print(CMD_CTX, "stm32x options_read <bank>");
+		return ERROR_OK;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	stm32x_info = bank->driver_priv;
+
+	target = bank->target;
+
+	if (target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	target_read_u32(target, STM32_FLASH_OBR, &optionbyte);
+	command_print(CMD_CTX, "Option Byte: 0x%" PRIx32 "", optionbyte);
+
+	if (buf_get_u32((uint8_t*)&optionbyte, OPT_ERROR, 1))
+		command_print(CMD_CTX, "Option Byte Complement Error");
+
+	if (buf_get_u32((uint8_t*)&optionbyte, OPT_READOUT, 1))
+		command_print(CMD_CTX, "Readout Protection On");
+	else
+		command_print(CMD_CTX, "Readout Protection Off");
+
+	if (buf_get_u32((uint8_t*)&optionbyte, OPT_RDWDGSW, 1))
+		command_print(CMD_CTX, "Software Watchdog");
+	else
+		command_print(CMD_CTX, "Hardware Watchdog");
+
+	if (buf_get_u32((uint8_t*)&optionbyte, OPT_RDRSTSTOP, 1))
+		command_print(CMD_CTX, "Stop: No reset generated");
+	else
+		command_print(CMD_CTX, "Stop: Reset generated");
+
+	if (buf_get_u32((uint8_t*)&optionbyte, OPT_RDRSTSTDBY, 1))
+		command_print(CMD_CTX, "Standby: No reset generated");
+	else
+		command_print(CMD_CTX, "Standby: Reset generated");
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(stm32x_handle_options_write_command)
+{
+	struct target *target = NULL;
+	struct stm32x_flash_bank *stm32x_info = NULL;
+	uint16_t optionbyte = 0xF8;
+
+	if (CMD_ARGC < 4)
+	{
+		command_print(CMD_CTX, "stm32x options_write <bank> <SWWDG | HWWDG> <RSTSTNDBY | NORSTSTNDBY> <RSTSTOP | NORSTSTOP>");
+		return ERROR_OK;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	stm32x_info = bank->driver_priv;
+
+	target = bank->target;
+
+	if (target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (strcmp(CMD_ARGV[1], "SWWDG") == 0)
+	{
+		optionbyte |= (1 << 0);
+	}
+	else
+	{
+		optionbyte &= ~(1 << 0);
+	}
+
+	if (strcmp(CMD_ARGV[2], "NORSTSTNDBY") == 0)
+	{
+		optionbyte |= (1 << 1);
+	}
+	else
+	{
+		optionbyte &= ~(1 << 1);
+	}
+
+	if (strcmp(CMD_ARGV[3], "NORSTSTOP") == 0)
+	{
+		optionbyte |= (1 << 2);
+	}
+	else
+	{
+		optionbyte &= ~(1 << 2);
+	}
+
+	if (stm32x_erase_options(bank) != ERROR_OK)
+	{
+		command_print(CMD_CTX, "stm32x failed to erase options");
+		return ERROR_OK;
+	}
+
+	stm32x_info->option_bytes.user_options = optionbyte;
+
+	if (stm32x_write_options(bank) != ERROR_OK)
+	{
+		command_print(CMD_CTX, "stm32x failed to write options");
+		return ERROR_OK;
+	}
+
+	command_print(CMD_CTX, "stm32x write options complete");
+
+	return ERROR_OK;
+}
+
+static int stm32x_mass_erase(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	uint32_t status;
+
+	if (target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* unlock option flash registers */
+	target_write_u32(target, STM32_FLASH_KEYR, KEY1);
+	target_write_u32(target, STM32_FLASH_KEYR, KEY2);
+
+	/* mass erase flash memory */
+	target_write_u32(target, STM32_FLASH_CR, FLASH_MER);
+	target_write_u32(target, STM32_FLASH_CR, FLASH_MER | FLASH_STRT);
+
+	status = stm32x_wait_status_busy(bank, 10);
+
+	target_write_u32(target, STM32_FLASH_CR, FLASH_LOCK);
+
+	if (status & FLASH_WRPRTERR)
+	{
+		LOG_ERROR("stm32x device protected");
+		return ERROR_OK;
+	}
+
+	if (status & FLASH_PGERR)
+	{
+		LOG_ERROR("stm32x device programming failed");
+		return ERROR_OK;
+	}
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(stm32x_handle_mass_erase_command)
+{
+	int i;
+
+	if (CMD_ARGC < 1)
+	{
+		command_print(CMD_CTX, "stm32x mass_erase <bank>");
+		return ERROR_OK;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	if (stm32x_mass_erase(bank) == ERROR_OK)
+	{
+		/* set all sectors as erased */
+		for (i = 0; i < bank->num_sectors; i++)
+		{
+			bank->sectors[i].is_erased = 1;
+		}
+
+		command_print(CMD_CTX, "stm32x mass erase complete");
+	}
+	else
+	{
+		command_print(CMD_CTX, "stm32x mass erase failed");
+	}
+
+	return ERROR_OK;
+}
+
+static const struct command_registration stm32x_exec_command_handlers[] = {
+	{
+		.name = "lock",
+		.handler = &stm32x_handle_lock_command,
+		.mode = COMMAND_EXEC,
+		.help = "lock device",
+	},
+	{
+		.name = "unlock",
+		.handler = &stm32x_handle_unlock_command,
+		.mode = COMMAND_EXEC,
+		.help = "unlock protected device",
+	},
+	{
+		.name = "mass_erase",
+		.handler = &stm32x_handle_mass_erase_command,
+		.mode = COMMAND_EXEC,
+		.help = "mass erase device",
+	},
+	{
+		.name = "options_read",
+		.handler = &stm32x_handle_options_read_command,
+		.mode = COMMAND_EXEC,
+		.help = "read device option bytes",
+	},
+	{
+		.name = "options_write",
+		.handler = &stm32x_handle_options_write_command,
+		.mode = COMMAND_EXEC,
+		.help = "write device option bytes",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+static const struct command_registration stm32x_command_handlers[] = {
+	{
+		.name = "stm32x",
+		.mode = COMMAND_ANY,
+		.help = "stm32x flash command group",
+		.chain = stm32x_exec_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+struct flash_driver stm32x_flash = {
+		.name = "stm32x",
+		.commands = stm32x_command_handlers,
+		.flash_bank_command = &stm32x_flash_bank_command,
+		.erase = &stm32x_erase,
+		.protect = &stm32x_protect,
+		.write = &stm32x_write,
+		.probe = &stm32x_probe,
+		.auto_probe = &stm32x_auto_probe,
+		.erase_check = &default_flash_mem_blank_check,
+		.protect_check = &stm32x_protect_check,
+		.info = &stm32x_info,
+	};
diff --git a/src/flash/nor/stm32x.h b/src/flash/nor/stm32x.h
new file mode 100644
index 00000000..6cd047e1
--- /dev/null
+++ b/src/flash/nor/stm32x.h
@@ -0,0 +1,101 @@
+/***************************************************************************
+ *   Copyright (C) 2005 by Dominic Rath                                    *
+ *   Dominic.Rath@gmx.de                                                   *
+ *                                                                         *
+ *   Copyright (C) 2008 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifndef STM32X_H
+#define STM32X_H
+
+#include "flash.h"
+
+struct stm32x_options
+{
+	uint16_t RDP;
+	uint16_t user_options;
+	uint16_t protection[4];
+};
+
+struct stm32x_flash_bank
+{
+	struct stm32x_options option_bytes;
+	struct working_area *write_algorithm;
+	int ppage_size;
+	int probed;
+};
+
+/* stm32x register locations */
+
+#define STM32_FLASH_ACR		0x40022000
+#define STM32_FLASH_KEYR	0x40022004
+#define STM32_FLASH_OPTKEYR	0x40022008
+#define STM32_FLASH_SR		0x4002200C
+#define STM32_FLASH_CR		0x40022010
+#define STM32_FLASH_AR		0x40022014
+#define STM32_FLASH_OBR		0x4002201C
+#define STM32_FLASH_WRPR	0x40022020
+
+/* option byte location */
+
+#define STM32_OB_RDP		0x1FFFF800
+#define STM32_OB_USER		0x1FFFF802
+#define STM32_OB_DATA0		0x1FFFF804
+#define STM32_OB_DATA1		0x1FFFF806
+#define STM32_OB_WRP0		0x1FFFF808
+#define STM32_OB_WRP1		0x1FFFF80A
+#define STM32_OB_WRP2		0x1FFFF80C
+#define STM32_OB_WRP3		0x1FFFF80E
+
+/* FLASH_CR register bits */
+
+#define FLASH_PG		(1 << 0)
+#define FLASH_PER		(1 << 1)
+#define FLASH_MER		(1 << 2)
+#define FLASH_OPTPG		(1 << 4)
+#define FLASH_OPTER		(1 << 5)
+#define FLASH_STRT		(1 << 6)
+#define FLASH_LOCK		(1 << 7)
+#define FLASH_OPTWRE	(1 << 9)
+
+/* FLASH_SR register bits */
+
+#define FLASH_BSY		(1 << 0)
+#define FLASH_PGERR		(1 << 2)
+#define FLASH_WRPRTERR	(1 << 4)
+#define FLASH_EOP		(1 << 5)
+
+/* STM32_FLASH_OBR bit definitions (reading) */
+
+#define OPT_ERROR		0
+#define OPT_READOUT		1
+#define OPT_RDWDGSW		2
+#define OPT_RDRSTSTOP	3
+#define OPT_RDRSTSTDBY	4
+
+/* register unlock keys */
+
+#define KEY1			0x45670123
+#define KEY2			0xCDEF89AB
+
+struct stm32x_mem_layout {
+	uint32_t sector_start;
+	uint32_t sector_size;
+};
+
+#endif /* STM32X_H */
diff --git a/src/flash/nor/str7x.c b/src/flash/nor/str7x.c
new file mode 100644
index 00000000..7edffac9
--- /dev/null
+++ b/src/flash/nor/str7x.c
@@ -0,0 +1,706 @@
+/***************************************************************************
+ *   Copyright (C) 2005 by Dominic Rath                                    *
+ *   Dominic.Rath@gmx.de                                                   *
+ *                                                                         *
+ *   Copyright (C) 2008 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "str7x.h"
+#include "armv4_5.h"
+#include "binarybuffer.h"
+#include "algorithm.h"
+
+
+struct str7x_mem_layout mem_layout_str7bank0[] = {
+	{0x00000000, 0x02000, 0x01},
+	{0x00002000, 0x02000, 0x02},
+	{0x00004000, 0x02000, 0x04},
+	{0x00006000, 0x02000, 0x08},
+	{0x00008000, 0x08000, 0x10},
+	{0x00010000, 0x10000, 0x20},
+	{0x00020000, 0x10000, 0x40},
+	{0x00030000, 0x10000, 0x80}
+};
+
+struct str7x_mem_layout mem_layout_str7bank1[] = {
+	{0x00000000, 0x02000, 0x10000},
+	{0x00002000, 0x02000, 0x20000}
+};
+
+static int str7x_get_flash_adr(struct flash_bank *bank, uint32_t reg)
+{
+	struct str7x_flash_bank *str7x_info = bank->driver_priv;
+	return (str7x_info->register_base | reg);
+}
+
+static int str7x_build_block_list(struct flash_bank *bank)
+{
+	struct str7x_flash_bank *str7x_info = bank->driver_priv;
+
+	int i;
+	int num_sectors;
+	int b0_sectors = 0, b1_sectors = 0;
+
+	switch (bank->size)
+	{
+		case 16 * 1024:
+			b1_sectors = 2;
+			break;
+		case 64 * 1024:
+			b0_sectors = 5;
+			break;
+		case 128 * 1024:
+			b0_sectors = 6;
+			break;
+		case 256 * 1024:
+			b0_sectors = 8;
+			break;
+		default:
+			LOG_ERROR("BUG: unknown bank->size encountered");
+			exit(-1);
+	}
+
+	num_sectors = b0_sectors + b1_sectors;
+
+	bank->num_sectors = num_sectors;
+	bank->sectors = malloc(sizeof(struct flash_sector) * num_sectors);
+	str7x_info->sector_bits = malloc(sizeof(uint32_t) * num_sectors);
+
+	num_sectors = 0;
+
+	for (i = 0; i < b0_sectors; i++)
+	{
+		bank->sectors[num_sectors].offset = mem_layout_str7bank0[i].sector_start;
+		bank->sectors[num_sectors].size = mem_layout_str7bank0[i].sector_size;
+		bank->sectors[num_sectors].is_erased = -1;
+		bank->sectors[num_sectors].is_protected = 1;
+		str7x_info->sector_bits[num_sectors++] = mem_layout_str7bank0[i].sector_bit;
+	}
+
+	for (i = 0; i < b1_sectors; i++)
+	{
+		bank->sectors[num_sectors].offset = mem_layout_str7bank1[i].sector_start;
+		bank->sectors[num_sectors].size = mem_layout_str7bank1[i].sector_size;
+		bank->sectors[num_sectors].is_erased = -1;
+		bank->sectors[num_sectors].is_protected = 1;
+		str7x_info->sector_bits[num_sectors++] = mem_layout_str7bank1[i].sector_bit;
+	}
+
+	return ERROR_OK;
+}
+
+/* flash bank str7x <base> <size> 0 0 <target#> <str71_variant>
+ */
+FLASH_BANK_COMMAND_HANDLER(str7x_flash_bank_command)
+{
+	struct str7x_flash_bank *str7x_info;
+
+	if (CMD_ARGC < 7)
+	{
+		LOG_WARNING("incomplete flash_bank str7x configuration");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	str7x_info = malloc(sizeof(struct str7x_flash_bank));
+	bank->driver_priv = str7x_info;
+
+	/* set default bits for str71x flash */
+	str7x_info->busy_bits = (FLASH_LOCK | FLASH_BSYA1 | FLASH_BSYA0);
+	str7x_info->disable_bit = (1 << 1);
+
+	if (strcmp(CMD_ARGV[6], "STR71x") == 0)
+	{
+		str7x_info->register_base = 0x40100000;
+	}
+	else if (strcmp(CMD_ARGV[6], "STR73x") == 0)
+	{
+		str7x_info->register_base = 0x80100000;
+		str7x_info->busy_bits = (FLASH_LOCK | FLASH_BSYA0);
+	}
+	else if (strcmp(CMD_ARGV[6], "STR75x") == 0)
+	{
+		str7x_info->register_base = 0x20100000;
+		str7x_info->disable_bit = (1 << 0);
+	}
+	else
+	{
+		LOG_ERROR("unknown STR7x variant: '%s'", CMD_ARGV[6]);
+		free(str7x_info);
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	str7x_build_block_list(bank);
+
+	str7x_info->write_algorithm = NULL;
+
+	return ERROR_OK;
+}
+
+static uint32_t str7x_status(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	uint32_t retval;
+
+	target_read_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), &retval);
+
+	return retval;
+}
+
+static uint32_t str7x_result(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	uint32_t retval;
+
+	target_read_u32(target, str7x_get_flash_adr(bank, FLASH_ER), &retval);
+
+	return retval;
+}
+
+static int str7x_protect_check(struct flash_bank *bank)
+{
+	struct str7x_flash_bank *str7x_info = bank->driver_priv;
+	struct target *target = bank->target;
+
+	int i;
+	uint32_t retval;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	target_read_u32(target, str7x_get_flash_adr(bank, FLASH_NVWPAR), &retval);
+
+	for (i = 0; i < bank->num_sectors; i++)
+	{
+		if (retval & str7x_info->sector_bits[i])
+			bank->sectors[i].is_protected = 0;
+		else
+			bank->sectors[i].is_protected = 1;
+	}
+
+	return ERROR_OK;
+}
+
+static int str7x_erase(struct flash_bank *bank, int first, int last)
+{
+	struct str7x_flash_bank *str7x_info = bank->driver_priv;
+	struct target *target = bank->target;
+
+	int i;
+	uint32_t cmd;
+	uint32_t retval;
+	uint32_t sectors = 0;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	for (i = first; i <= last; i++)
+	{
+		sectors |= str7x_info->sector_bits[i];
+	}
+
+	LOG_DEBUG("sectors: 0x%" PRIx32 "", sectors);
+
+	/* clear FLASH_ER register */
+	target_write_u32(target, str7x_get_flash_adr(bank, FLASH_ER), 0x0);
+
+	cmd = FLASH_SER;
+	target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);
+
+	cmd = sectors;
+	target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR1), cmd);
+
+	cmd = FLASH_SER | FLASH_WMS;
+	target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);
+
+	while (((retval = str7x_status(bank)) & str7x_info->busy_bits)) {
+		alive_sleep(1);
+	}
+
+	retval = str7x_result(bank);
+
+	if (retval)
+	{
+		LOG_ERROR("error erasing flash bank, FLASH_ER: 0x%" PRIx32 "", retval);
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	for (i = first; i <= last; i++)
+		bank->sectors[i].is_erased = 1;
+
+	return ERROR_OK;
+}
+
+static int str7x_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	struct str7x_flash_bank *str7x_info = bank->driver_priv;
+	struct target *target = bank->target;
+	int i;
+	uint32_t cmd;
+	uint32_t retval;
+	uint32_t protect_blocks;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	protect_blocks = 0xFFFFFFFF;
+
+	if (set)
+	{
+		for (i = first; i <= last; i++)
+			protect_blocks &= ~(str7x_info->sector_bits[i]);
+	}
+
+	/* clear FLASH_ER register */
+	target_write_u32(target, str7x_get_flash_adr(bank, FLASH_ER), 0x0);
+
+	cmd = FLASH_SPR;
+	target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);
+
+	cmd = str7x_get_flash_adr(bank, FLASH_NVWPAR);
+	target_write_u32(target, str7x_get_flash_adr(bank, FLASH_AR), cmd);
+
+	cmd = protect_blocks;
+	target_write_u32(target, str7x_get_flash_adr(bank, FLASH_DR0), cmd);
+
+	cmd = FLASH_SPR | FLASH_WMS;
+	target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);
+
+	while (((retval = str7x_status(bank)) & str7x_info->busy_bits)) {
+		alive_sleep(1);
+	}
+
+	retval = str7x_result(bank);
+
+	LOG_DEBUG("retval: 0x%8.8" PRIx32 "", retval);
+
+	if (retval & FLASH_ERER)
+		return ERROR_FLASH_SECTOR_NOT_ERASED;
+	else if (retval & FLASH_WPF)
+		return ERROR_FLASH_OPERATION_FAILED;
+
+	return ERROR_OK;
+}
+
+static int str7x_write_block(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	struct str7x_flash_bank *str7x_info = bank->driver_priv;
+	struct target *target = bank->target;
+	uint32_t buffer_size = 8192;
+	struct working_area *source;
+	uint32_t address = bank->base + offset;
+	struct reg_param reg_params[6];
+	struct armv4_5_algorithm armv4_5_info;
+	int retval = ERROR_OK;
+
+	uint32_t str7x_flash_write_code[] = {
+					/* write:				*/
+		0xe3a04201, /*	mov r4, #0x10000000	*/
+		0xe5824000, /*	str r4, [r2, #0x0]	*/
+		0xe5821010, /*	str r1, [r2, #0x10]	*/
+		0xe4904004, /*	ldr r4, [r0], #4	*/
+		0xe5824008, /*	str r4, [r2, #0x8]	*/
+		0xe4904004, /*	ldr r4, [r0], #4	*/
+		0xe582400c, /*	str r4, [r2, #0xc]	*/
+		0xe3a04209, /*	mov r4, #0x90000000	*/
+		0xe5824000, /*	str r4, [r2, #0x0]	*/
+		            /* busy:				*/
+		0xe5924000, /*	ldr r4, [r2, #0x0]	*/
+		0xe1140005,	/*	tst r4, r5			*/
+		0x1afffffc, /*	bne busy			*/
+		0xe5924014, /*	ldr r4, [r2, #0x14]	*/
+		0xe31400ff, /*	tst r4, #0xff		*/
+		0x03140c01, /*	tsteq r4, #0x100	*/
+		0x1a000002, /*	bne exit			*/
+		0xe2811008, /*	add r1, r1, #0x8	*/
+		0xe2533001, /*	subs r3, r3, #1		*/
+		0x1affffec, /*	bne write			*/
+					/* exit:				*/
+		0xeafffffe, /*	b exit				*/
+	};
+
+	/* flash write code */
+	if (target_alloc_working_area(target, 4 * 20, &str7x_info->write_algorithm) != ERROR_OK)
+	{
+		LOG_WARNING("no working area available, can't do block memory writes");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	};
+
+	target_write_buffer(target, str7x_info->write_algorithm->address, 20 * 4, (uint8_t*)str7x_flash_write_code);
+
+	/* memory buffer */
+	while (target_alloc_working_area(target, buffer_size, &source) != ERROR_OK)
+	{
+		buffer_size /= 2;
+		if (buffer_size <= 256)
+		{
+			/* if we already allocated the writing code, but failed to get a buffer, free the algorithm */
+			if (str7x_info->write_algorithm)
+				target_free_working_area(target, str7x_info->write_algorithm);
+
+			LOG_WARNING("no large enough working area available, can't do block memory writes");
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+	}
+
+	armv4_5_info.common_magic = ARMV4_5_COMMON_MAGIC;
+	armv4_5_info.core_mode = ARMV4_5_MODE_SVC;
+	armv4_5_info.core_state = ARMV4_5_STATE_ARM;
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
+	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
+	init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);
+	init_reg_param(&reg_params[4], "r4", 32, PARAM_IN);
+	init_reg_param(&reg_params[5], "r5", 32, PARAM_OUT);
+
+	while (count > 0)
+	{
+		uint32_t thisrun_count = (count > (buffer_size / 8)) ? (buffer_size / 8) : count;
+
+		target_write_buffer(target, source->address, thisrun_count * 8, buffer);
+
+		buf_set_u32(reg_params[0].value, 0, 32, source->address);
+		buf_set_u32(reg_params[1].value, 0, 32, address);
+		buf_set_u32(reg_params[2].value, 0, 32, str7x_get_flash_adr(bank, FLASH_CR0));
+		buf_set_u32(reg_params[3].value, 0, 32, thisrun_count);
+		buf_set_u32(reg_params[5].value, 0, 32, str7x_info->busy_bits);
+
+		if ((retval = target_run_algorithm(target, 0, NULL, 6, reg_params, str7x_info->write_algorithm->address, str7x_info->write_algorithm->address + (19 * 4), 10000, &armv4_5_info)) != ERROR_OK)
+		{
+			LOG_ERROR("error executing str7x flash write algorithm");
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			break;
+		}
+
+		if (buf_get_u32(reg_params[4].value, 0, 32) != 0x00)
+		{
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			break;
+		}
+
+		buffer += thisrun_count * 8;
+		address += thisrun_count * 8;
+		count -= thisrun_count;
+	}
+
+	target_free_working_area(target, source);
+	target_free_working_area(target, str7x_info->write_algorithm);
+
+	destroy_reg_param(&reg_params[0]);
+	destroy_reg_param(&reg_params[1]);
+	destroy_reg_param(&reg_params[2]);
+	destroy_reg_param(&reg_params[3]);
+	destroy_reg_param(&reg_params[4]);
+	destroy_reg_param(&reg_params[5]);
+
+	return retval;
+}
+
+static int str7x_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	struct str7x_flash_bank *str7x_info = bank->driver_priv;
+	uint32_t dwords_remaining = (count / 8);
+	uint32_t bytes_remaining = (count & 0x00000007);
+	uint32_t address = bank->base + offset;
+	uint32_t bytes_written = 0;
+	uint32_t cmd;
+	int retval;
+	uint32_t check_address = offset;
+	int i;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (offset & 0x7)
+	{
+		LOG_WARNING("offset 0x%" PRIx32 " breaks required 8-byte alignment", offset);
+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+	}
+
+	for (i = 0; i < bank->num_sectors; i++)
+	{
+		uint32_t sec_start = bank->sectors[i].offset;
+		uint32_t sec_end = sec_start + bank->sectors[i].size;
+
+		/* check if destination falls within the current sector */
+		if ((check_address >= sec_start) && (check_address < sec_end))
+		{
+			/* check if destination ends in the current sector */
+			if (offset + count < sec_end)
+				check_address = offset + count;
+			else
+				check_address = sec_end;
+		}
+	}
+
+	if (check_address != offset + count)
+		return ERROR_FLASH_DST_OUT_OF_BANK;
+
+	/* clear FLASH_ER register */
+	target_write_u32(target, str7x_get_flash_adr(bank, FLASH_ER), 0x0);
+
+	/* multiple dwords (8-byte) to be programmed? */
+	if (dwords_remaining > 0)
+	{
+		/* try using a block write */
+		if ((retval = str7x_write_block(bank, buffer, offset, dwords_remaining)) != ERROR_OK)
+		{
+			if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
+			{
+				/* if block write failed (no sufficient working area),
+				 * we use normal (slow) single dword accesses */
+				LOG_WARNING("couldn't use block writes, falling back to single memory accesses");
+			}
+			else if (retval == ERROR_FLASH_OPERATION_FAILED)
+			{
+				/* if an error occured, we examine the reason, and quit */
+				retval = str7x_result(bank);
+
+				LOG_ERROR("flash writing failed with error code: 0x%x", retval);
+				return ERROR_FLASH_OPERATION_FAILED;
+			}
+		}
+		else
+		{
+			buffer += dwords_remaining * 8;
+			address += dwords_remaining * 8;
+			dwords_remaining = 0;
+		}
+	}
+
+	while (dwords_remaining > 0)
+	{
+		/* command */
+		cmd = FLASH_DWPG;
+		target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);
+
+		/* address */
+		target_write_u32(target, str7x_get_flash_adr(bank, FLASH_AR), address);
+
+		/* data word 1 */
+		target_write_memory(target, str7x_get_flash_adr(bank, FLASH_DR0), 4, 1, buffer + bytes_written);
+		bytes_written += 4;
+
+		/* data word 2 */
+		target_write_memory(target, str7x_get_flash_adr(bank, FLASH_DR1), 4, 1, buffer + bytes_written);
+		bytes_written += 4;
+
+		/* start programming cycle */
+		cmd = FLASH_DWPG | FLASH_WMS;
+		target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);
+
+		while (((retval = str7x_status(bank)) & str7x_info->busy_bits))
+		{
+			alive_sleep(1);
+		}
+
+		retval = str7x_result(bank);
+
+		if (retval & FLASH_PGER)
+			return ERROR_FLASH_OPERATION_FAILED;
+		else if (retval & FLASH_WPF)
+			return ERROR_FLASH_OPERATION_FAILED;
+
+		dwords_remaining--;
+		address += 8;
+	}
+
+	if (bytes_remaining)
+	{
+		uint8_t last_dword[8] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+		int i = 0;
+
+		while (bytes_remaining > 0)
+		{
+			last_dword[i++] = *(buffer + bytes_written);
+			bytes_remaining--;
+			bytes_written++;
+		}
+
+		/* command */
+		cmd = FLASH_DWPG;
+		target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);
+
+		/* address */
+		target_write_u32(target, str7x_get_flash_adr(bank, FLASH_AR), address);
+
+		/* data word 1 */
+		target_write_memory(target, str7x_get_flash_adr(bank, FLASH_DR0), 4, 1, last_dword);
+		bytes_written += 4;
+
+		/* data word 2 */
+		target_write_memory(target, str7x_get_flash_adr(bank, FLASH_DR1), 4, 1, last_dword + 4);
+		bytes_written += 4;
+
+		/* start programming cycle */
+		cmd = FLASH_DWPG | FLASH_WMS;
+		target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);
+
+		while (((retval = str7x_status(bank)) & str7x_info->busy_bits))
+		{
+			alive_sleep(1);
+		}
+
+		retval = str7x_result(bank);
+
+		if (retval & FLASH_PGER)
+			return ERROR_FLASH_OPERATION_FAILED;
+		else if (retval & FLASH_WPF)
+			return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	return ERROR_OK;
+}
+
+static int str7x_probe(struct flash_bank *bank)
+{
+	return ERROR_OK;
+}
+
+#if 0
+COMMAND_HANDLER(str7x_handle_part_id_command)
+{
+	return ERROR_OK;
+}
+#endif
+
+static int str7x_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	snprintf(buf, buf_size, "str7x flash driver info");
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(str7x_handle_disable_jtag_command)
+{
+	struct target *target = NULL;
+	struct str7x_flash_bank *str7x_info = NULL;
+
+	uint32_t flash_cmd;
+	uint16_t ProtectionLevel = 0;
+	uint16_t ProtectionRegs;
+
+	if (CMD_ARGC < 1)
+	{
+		command_print(CMD_CTX, "str7x disable_jtag <bank>");
+		return ERROR_OK;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	str7x_info = bank->driver_priv;
+
+	target = bank->target;
+
+	if (target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* first we get protection status */
+	uint32_t reg;
+	target_read_u32(target, str7x_get_flash_adr(bank, FLASH_NVAPR0), &reg);
+
+	if (!(reg & str7x_info->disable_bit))
+	{
+		ProtectionLevel = 1;
+	}
+
+	target_read_u32(target, str7x_get_flash_adr(bank, FLASH_NVAPR1), &reg);
+	ProtectionRegs = ~(reg >> 16);
+
+	while (((ProtectionRegs) != 0) && (ProtectionLevel < 16))
+	{
+		ProtectionRegs >>= 1;
+		ProtectionLevel++;
+	}
+
+	if (ProtectionLevel == 0)
+	{
+		flash_cmd = FLASH_SPR;
+		target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), flash_cmd);
+		target_write_u32(target, str7x_get_flash_adr(bank, FLASH_AR), 0x4010DFB8);
+		target_write_u32(target, str7x_get_flash_adr(bank, FLASH_DR0), 0xFFFFFFFD);
+		flash_cmd = FLASH_SPR | FLASH_WMS;
+		target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), flash_cmd);
+	}
+	else
+	{
+		flash_cmd = FLASH_SPR;
+		target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), flash_cmd);
+		target_write_u32(target, str7x_get_flash_adr(bank, FLASH_AR), 0x4010DFBC);
+		target_write_u32(target, str7x_get_flash_adr(bank, FLASH_DR0), ~(1 << (15 + ProtectionLevel)));
+		flash_cmd = FLASH_SPR | FLASH_WMS;
+		target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), flash_cmd);
+	}
+
+	return ERROR_OK;
+}
+
+static const struct command_registration str7x_exec_command_handlers[] = {
+	{
+		.name = "disable_jtag",
+		.handler = &str7x_handle_disable_jtag_command,
+		.mode = COMMAND_EXEC,
+		.help = "disable jtag access",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+static const struct command_registration str7x_command_handlers[] = {
+	{
+		.name = "str7x",
+		.mode = COMMAND_ANY,
+		.help = "str7x flash command group",
+		.chain = str7x_exec_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+struct flash_driver str7x_flash = {
+		.name = "str7x",
+		.commands = str7x_command_handlers,
+		.flash_bank_command = &str7x_flash_bank_command,
+		.erase = &str7x_erase,
+		.protect = &str7x_protect,
+		.write = &str7x_write,
+		.probe = &str7x_probe,
+		.auto_probe = &str7x_probe,
+		.erase_check = &default_flash_blank_check,
+		.protect_check = &str7x_protect_check,
+		.info = &str7x_info,
+	};
diff --git a/src/flash/nor/str7x.h b/src/flash/nor/str7x.h
new file mode 100644
index 00000000..81af0f1e
--- /dev/null
+++ b/src/flash/nor/str7x.h
@@ -0,0 +1,110 @@
+/***************************************************************************
+ *   Copyright (C) 2005 by Dominic Rath                                    *
+ *   Dominic.Rath@gmx.de                                                   *
+ *                                                                         *
+ *   Copyright (C) 2008 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifndef STR7X_H
+#define STR7X_H
+
+#include "flash.h"
+
+struct str7x_flash_bank
+{
+	uint32_t *sector_bits;
+	uint32_t disable_bit;
+	uint32_t busy_bits;
+	uint32_t register_base;
+	struct working_area *write_algorithm;
+};
+
+enum str7x_status_codes
+{
+	STR7X_CMD_SUCCESS = 0,
+	STR7X_INVALID_COMMAND = 1,
+	STR7X_SRC_ADDR_ERROR = 2,
+	STR7X_DST_ADDR_ERROR = 3,
+	STR7X_SRC_ADDR_NOT_MAPPED = 4,
+	STR7X_DST_ADDR_NOT_MAPPED = 5,
+	STR7X_COUNT_ERROR = 6,
+	STR7X_INVALID_SECTOR = 7,
+	STR7X_SECTOR_NOT_BLANK = 8,
+	STR7X_SECTOR_NOT_PREPARED = 9,
+	STR7X_COMPARE_ERROR = 10,
+	STR7X_BUSY = 11
+};
+
+/*  Flash registers */
+
+#define FLASH_CR0		0x00000000
+#define FLASH_CR1		0x00000004
+#define FLASH_DR0		0x00000008
+#define FLASH_DR1		0x0000000C
+#define FLASH_AR		0x00000010
+#define FLASH_ER		0x00000014
+#define FLASH_NVWPAR	0x0000DFB0
+#define FLASH_NVAPR0	0x0000DFB8
+#define FLASH_NVAPR1	0x0000DFBC
+
+/* FLASH_CR0 register bits */
+
+#define FLASH_WMS		0x80000000
+#define FLASH_SUSP		0x40000000
+#define FLASH_WPG		0x20000000
+#define FLASH_DWPG		0x10000000
+#define FLASH_SER		0x08000000
+#define FLASH_SPR		0x01000000
+#define FLASH_BER		0x04000000
+#define FLASH_MER		0x02000000
+#define FLASH_LOCK		0x00000010
+#define FLASH_BSYA1		0x00000004
+#define FLASH_BSYA0		0x00000002
+
+/* FLASH_CR1 register bits */
+
+#define FLASH_B1S		0x02000000
+#define FLASH_B0S		0x01000000
+#define FLASH_B1F1		0x00020000
+#define FLASH_B1F0		0x00010000
+#define FLASH_B0F7		0x00000080
+#define FLASH_B0F6		0x00000040
+#define FLASH_B0F5		0x00000020
+#define FLASH_B0F4		0x00000010
+#define FLASH_B0F3		0x00000008
+#define FLASH_B0F2		0x00000004
+#define FLASH_B0F1		0x00000002
+#define FLASH_B0F0		0x00000001
+
+/* FLASH_ER register bits */
+
+#define FLASH_WPF		0x00000100
+#define FLASH_RESER		0x00000080
+#define FLASH_SEQER		0x00000040
+#define FLASH_10ER		0x00000008
+#define FLASH_PGER		0x00000004
+#define FLASH_ERER		0x00000002
+#define FLASH_ERR		0x00000001
+
+struct str7x_mem_layout {
+	uint32_t sector_start;
+	uint32_t sector_size;
+	uint32_t sector_bit;
+};
+
+#endif /* STR7X_H */
diff --git a/src/flash/nor/str9x.c b/src/flash/nor/str9x.c
new file mode 100644
index 00000000..98f15e75
--- /dev/null
+++ b/src/flash/nor/str9x.c
@@ -0,0 +1,711 @@
+/***************************************************************************
+ *   Copyright (C) 2005 by Dominic Rath                                    *
+ *   Dominic.Rath@gmx.de                                                   *
+ *                                                                         *
+ *   Copyright (C) 2008 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *
+ *   Copyright (C) 2008 by Oyvind Harboe                                   *
+ *   oyvind.harboe@zylin.com                                               *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "str9x.h"
+#include "arm966e.h"
+#include "algorithm.h"
+
+
+static uint32_t bank1start = 0x00080000;
+
+static int str9x_build_block_list(struct flash_bank *bank)
+{
+	struct str9x_flash_bank *str9x_info = bank->driver_priv;
+
+	int i;
+	int num_sectors;
+	int b0_sectors = 0, b1_sectors = 0;
+	uint32_t offset = 0;
+
+	/* set if we have large flash str9 */
+	str9x_info->variant = 0;
+	str9x_info->bank1 = 0;
+
+	switch (bank->size)
+	{
+		case (256 * 1024):
+			b0_sectors = 4;
+			break;
+		case (512 * 1024):
+			b0_sectors = 8;
+			break;
+		case (1024 * 1024):
+			bank1start = 0x00100000;
+			str9x_info->variant = 1;
+			b0_sectors = 16;
+			break;
+		case (2048 * 1024):
+			bank1start = 0x00200000;
+			str9x_info->variant = 1;
+			b0_sectors = 32;
+			break;
+		case (128 * 1024):
+			str9x_info->variant = 1;
+			str9x_info->bank1 = 1;
+			b1_sectors = 8;
+			bank1start = bank->base;
+			break;
+		case (32 * 1024):
+			str9x_info->bank1 = 1;
+			b1_sectors = 4;
+			bank1start = bank->base;
+			break;
+		default:
+			LOG_ERROR("BUG: unknown bank->size encountered");
+			exit(-1);
+	}
+
+	num_sectors = b0_sectors + b1_sectors;
+
+	bank->num_sectors = num_sectors;
+	bank->sectors = malloc(sizeof(struct flash_sector) * num_sectors);
+	str9x_info->sector_bits = malloc(sizeof(uint32_t) * num_sectors);
+
+	num_sectors = 0;
+
+	for (i = 0; i < b0_sectors; i++)
+	{
+		bank->sectors[num_sectors].offset = offset;
+		bank->sectors[num_sectors].size = 0x10000;
+		offset += bank->sectors[i].size;
+		bank->sectors[num_sectors].is_erased = -1;
+		bank->sectors[num_sectors].is_protected = 1;
+		str9x_info->sector_bits[num_sectors++] = (1 << i);
+	}
+
+	for (i = 0; i < b1_sectors; i++)
+	{
+		bank->sectors[num_sectors].offset = offset;
+		bank->sectors[num_sectors].size = str9x_info->variant == 0 ? 0x2000 : 0x4000;
+		offset += bank->sectors[i].size;
+		bank->sectors[num_sectors].is_erased = -1;
+		bank->sectors[num_sectors].is_protected = 1;
+		if (str9x_info->variant)
+			str9x_info->sector_bits[num_sectors++] = (1 << i);
+		else
+			str9x_info->sector_bits[num_sectors++] = (1 << (i + 8));
+	}
+
+	return ERROR_OK;
+}
+
+/* flash bank str9x <base> <size> 0 0 <target#>
+ */
+FLASH_BANK_COMMAND_HANDLER(str9x_flash_bank_command)
+{
+	struct str9x_flash_bank *str9x_info;
+
+	if (CMD_ARGC < 6)
+	{
+		LOG_WARNING("incomplete flash_bank str9x configuration");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	str9x_info = malloc(sizeof(struct str9x_flash_bank));
+	bank->driver_priv = str9x_info;
+
+	str9x_build_block_list(bank);
+
+	str9x_info->write_algorithm = NULL;
+
+	return ERROR_OK;
+}
+
+static int str9x_protect_check(struct flash_bank *bank)
+{
+	int retval;
+	struct str9x_flash_bank *str9x_info = bank->driver_priv;
+	struct target *target = bank->target;
+
+	int i;
+	uint32_t adr;
+	uint32_t status = 0;
+	uint16_t hstatus = 0;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* read level one protection */
+
+	if (str9x_info->variant)
+	{
+		if (str9x_info->bank1)
+		{
+			adr = bank1start + 0x18;
+			if ((retval = target_write_u16(target, adr, 0x90)) != ERROR_OK)
+			{
+				return retval;
+			}
+			if ((retval = target_read_u16(target, adr, &hstatus)) != ERROR_OK)
+			{
+				return retval;
+			}
+			status = hstatus;
+		}
+		else
+		{
+			adr = bank1start + 0x14;
+			if ((retval = target_write_u16(target, adr, 0x90)) != ERROR_OK)
+			{
+				return retval;
+			}
+			if ((retval = target_read_u32(target, adr, &status)) != ERROR_OK)
+			{
+				return retval;
+			}
+		}
+	}
+	else
+	{
+		adr = bank1start + 0x10;
+		if ((retval = target_write_u16(target, adr, 0x90)) != ERROR_OK)
+		{
+			return retval;
+		}
+		if ((retval = target_read_u16(target, adr, &hstatus)) != ERROR_OK)
+		{
+			return retval;
+		}
+		status = hstatus;
+	}
+
+	/* read array command */
+	if ((retval = target_write_u16(target, adr, 0xFF)) != ERROR_OK)
+	{
+		return retval;
+	}
+
+	for (i = 0; i < bank->num_sectors; i++)
+	{
+		if (status & str9x_info->sector_bits[i])
+			bank->sectors[i].is_protected = 1;
+		else
+			bank->sectors[i].is_protected = 0;
+	}
+
+	return ERROR_OK;
+}
+
+static int str9x_erase(struct flash_bank *bank, int first, int last)
+{
+	struct target *target = bank->target;
+	int i;
+	uint32_t adr;
+	uint8_t status;
+	uint8_t erase_cmd;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* Check if we erase whole bank */
+	if ((first == 0) && (last == (bank->num_sectors - 1)))
+	{
+		/* Optimize to run erase bank command instead of sector */
+		erase_cmd = 0x80;
+	}
+	else
+	{
+		/* Erase sector command */
+		erase_cmd = 0x20;
+	}
+
+	for (i = first; i <= last; i++)
+	{
+		int retval;
+		adr = bank->base + bank->sectors[i].offset;
+
+		/* erase sectors */
+		if ((retval = target_write_u16(target, adr, erase_cmd)) != ERROR_OK)
+		{
+			return retval;
+		}
+		if ((retval = target_write_u16(target, adr, 0xD0)) != ERROR_OK)
+		{
+			return retval;
+		}
+
+		/* get status */
+		if ((retval = target_write_u16(target, adr, 0x70)) != ERROR_OK)
+		{
+			return retval;
+		}
+
+		int timeout;
+		for (timeout = 0; timeout < 1000; timeout++) {
+			if ((retval = target_read_u8(target, adr, &status)) != ERROR_OK)
+			{
+				return retval;
+			}
+			if (status & 0x80)
+				break;
+			alive_sleep(1);
+		}
+		if (timeout == 1000)
+		{
+			LOG_ERROR("erase timed out");
+			return ERROR_FAIL;
+		}
+
+		/* clear status, also clear read array */
+		if ((retval = target_write_u16(target, adr, 0x50)) != ERROR_OK)
+		{
+			return retval;
+		}
+
+		/* read array command */
+		if ((retval = target_write_u16(target, adr, 0xFF)) != ERROR_OK)
+		{
+			return retval;
+		}
+
+		if (status & 0x22)
+		{
+			LOG_ERROR("error erasing flash bank, status: 0x%x", status);
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+
+		/* If we ran erase bank command, we are finished */
+		if (erase_cmd == 0x80)
+			break;
+	}
+
+	for (i = first; i <= last; i++)
+		bank->sectors[i].is_erased = 1;
+
+	return ERROR_OK;
+}
+
+static int str9x_protect(struct flash_bank *bank,
+		int set, int first, int last)
+{
+	struct target *target = bank->target;
+	int i;
+	uint32_t adr;
+	uint8_t status;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	for (i = first; i <= last; i++)
+	{
+		/* Level One Protection */
+
+		adr = bank->base + bank->sectors[i].offset;
+
+		target_write_u16(target, adr, 0x60);
+		if (set)
+			target_write_u16(target, adr, 0x01);
+		else
+			target_write_u16(target, adr, 0xD0);
+
+		/* query status */
+		target_read_u8(target, adr, &status);
+
+		/* clear status, also clear read array */
+		target_write_u16(target, adr, 0x50);
+
+		/* read array command */
+		target_write_u16(target, adr, 0xFF);
+	}
+
+	return ERROR_OK;
+}
+
+static int str9x_write_block(struct flash_bank *bank,
+		uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	struct str9x_flash_bank *str9x_info = bank->driver_priv;
+	struct target *target = bank->target;
+	uint32_t buffer_size = 8192;
+	struct working_area *source;
+	uint32_t address = bank->base + offset;
+	struct reg_param reg_params[4];
+	struct armv4_5_algorithm armv4_5_info;
+	int retval = ERROR_OK;
+
+	uint32_t str9x_flash_write_code[] = {
+					/* write:				*/
+		0xe3c14003,	/*	bic	r4, r1, #3		*/
+		0xe3a03040,	/*	mov	r3, #0x40		*/
+		0xe1c430b0,	/*	strh r3, [r4, #0]	*/
+		0xe0d030b2,	/*	ldrh r3, [r0], #2	*/
+		0xe0c130b2,	/*	strh r3, [r1], #2	*/
+		0xe3a03070,	/*	mov r3, #0x70		*/
+		0xe1c430b0,	/*	strh r3, [r4, #0]	*/
+					/* busy:				*/
+		0xe5d43000,	/*	ldrb r3, [r4, #0]	*/
+		0xe3130080,	/*	tst r3, #0x80		*/
+		0x0afffffc,	/*	beq busy			*/
+		0xe3a05050,	/*	mov	r5, #0x50		*/
+		0xe1c450b0,	/*	strh r5, [r4, #0]	*/
+		0xe3a050ff,	/*	mov	r5, #0xFF		*/
+		0xe1c450b0,	/*	strh r5, [r4, #0]	*/
+		0xe3130012,	/*	tst	r3, #0x12		*/
+		0x1a000001,	/*	bne exit			*/
+		0xe2522001,	/*	subs r2, r2, #1		*/
+		0x1affffed,	/*	bne write			*/
+					/* exit:				*/
+		0xeafffffe,	/*	b exit				*/
+	};
+
+	/* flash write code */
+	if (target_alloc_working_area(target, 4 * 19, &str9x_info->write_algorithm) != ERROR_OK)
+	{
+		LOG_WARNING("no working area available, can't do block memory writes");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	};
+
+	target_write_buffer(target, str9x_info->write_algorithm->address, 19 * 4, (uint8_t*)str9x_flash_write_code);
+
+	/* memory buffer */
+	while (target_alloc_working_area(target, buffer_size, &source) != ERROR_OK)
+	{
+		buffer_size /= 2;
+		if (buffer_size <= 256)
+		{
+			/* if we already allocated the writing code, but failed to get a buffer, free the algorithm */
+			if (str9x_info->write_algorithm)
+				target_free_working_area(target, str9x_info->write_algorithm);
+
+			LOG_WARNING("no large enough working area available, can't do block memory writes");
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+	}
+
+	armv4_5_info.common_magic = ARMV4_5_COMMON_MAGIC;
+	armv4_5_info.core_mode = ARMV4_5_MODE_SVC;
+	armv4_5_info.core_state = ARMV4_5_STATE_ARM;
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
+	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
+	init_reg_param(&reg_params[3], "r3", 32, PARAM_IN);
+
+	while (count > 0)
+	{
+		uint32_t thisrun_count = (count > (buffer_size / 2)) ? (buffer_size / 2) : count;
+
+		target_write_buffer(target, source->address, thisrun_count * 2, buffer);
+
+		buf_set_u32(reg_params[0].value, 0, 32, source->address);
+		buf_set_u32(reg_params[1].value, 0, 32, address);
+		buf_set_u32(reg_params[2].value, 0, 32, thisrun_count);
+
+		if ((retval = target_run_algorithm(target, 0, NULL, 4, reg_params, str9x_info->write_algorithm->address, str9x_info->write_algorithm->address + (18 * 4), 10000, &armv4_5_info)) != ERROR_OK)
+		{
+			LOG_ERROR("error executing str9x flash write algorithm");
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			break;
+		}
+
+		if (buf_get_u32(reg_params[3].value, 0, 32) != 0x80)
+		{
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			break;
+		}
+
+		buffer += thisrun_count * 2;
+		address += thisrun_count * 2;
+		count -= thisrun_count;
+	}
+
+	target_free_working_area(target, source);
+	target_free_working_area(target, str9x_info->write_algorithm);
+
+	destroy_reg_param(&reg_params[0]);
+	destroy_reg_param(&reg_params[1]);
+	destroy_reg_param(&reg_params[2]);
+	destroy_reg_param(&reg_params[3]);
+
+	return retval;
+}
+
+static int str9x_write(struct flash_bank *bank,
+		uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	uint32_t words_remaining = (count / 2);
+	uint32_t bytes_remaining = (count & 0x00000001);
+	uint32_t address = bank->base + offset;
+	uint32_t bytes_written = 0;
+	uint8_t status;
+	int retval;
+	uint32_t check_address = offset;
+	uint32_t bank_adr;
+	int i;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (offset & 0x1)
+	{
+		LOG_WARNING("offset 0x%" PRIx32 " breaks required 2-byte alignment", offset);
+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+	}
+
+	for (i = 0; i < bank->num_sectors; i++)
+	{
+		uint32_t sec_start = bank->sectors[i].offset;
+		uint32_t sec_end = sec_start + bank->sectors[i].size;
+
+		/* check if destination falls within the current sector */
+		if ((check_address >= sec_start) && (check_address < sec_end))
+		{
+			/* check if destination ends in the current sector */
+			if (offset + count < sec_end)
+				check_address = offset + count;
+			else
+				check_address = sec_end;
+		}
+	}
+
+	if (check_address != offset + count)
+		return ERROR_FLASH_DST_OUT_OF_BANK;
+
+	/* multiple half words (2-byte) to be programmed? */
+	if (words_remaining > 0)
+	{
+		/* try using a block write */
+		if ((retval = str9x_write_block(bank, buffer, offset, words_remaining)) != ERROR_OK)
+		{
+			if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
+			{
+				/* if block write failed (no sufficient working area),
+				 * we use normal (slow) single dword accesses */
+				LOG_WARNING("couldn't use block writes, falling back to single memory accesses");
+			}
+			else if (retval == ERROR_FLASH_OPERATION_FAILED)
+			{
+				LOG_ERROR("flash writing failed with error code: 0x%x", retval);
+				return ERROR_FLASH_OPERATION_FAILED;
+			}
+		}
+		else
+		{
+			buffer += words_remaining * 2;
+			address += words_remaining * 2;
+			words_remaining = 0;
+		}
+	}
+
+	while (words_remaining > 0)
+	{
+		bank_adr = address & ~0x03;
+
+		/* write data command */
+		target_write_u16(target, bank_adr, 0x40);
+		target_write_memory(target, address, 2, 1, buffer + bytes_written);
+
+		/* get status command */
+		target_write_u16(target, bank_adr, 0x70);
+
+		int timeout;
+		for (timeout = 0; timeout < 1000; timeout++)
+		{
+			target_read_u8(target, bank_adr, &status);
+			if (status & 0x80)
+				break;
+			alive_sleep(1);
+		}
+		if (timeout == 1000)
+		{
+			LOG_ERROR("write timed out");
+			return ERROR_FAIL;
+		}
+
+		/* clear status reg and read array */
+		target_write_u16(target, bank_adr, 0x50);
+		target_write_u16(target, bank_adr, 0xFF);
+
+		if (status & 0x10)
+			return ERROR_FLASH_OPERATION_FAILED;
+		else if (status & 0x02)
+			return ERROR_FLASH_OPERATION_FAILED;
+
+		bytes_written += 2;
+		words_remaining--;
+		address += 2;
+	}
+
+	if (bytes_remaining)
+	{
+		uint8_t last_halfword[2] = {0xff, 0xff};
+		int i = 0;
+
+		while (bytes_remaining > 0)
+		{
+			last_halfword[i++] = *(buffer + bytes_written);
+			bytes_remaining--;
+			bytes_written++;
+		}
+
+		bank_adr = address & ~0x03;
+
+		/* write data command */
+		target_write_u16(target, bank_adr, 0x40);
+		target_write_memory(target, address, 2, 1, last_halfword);
+
+		/* query status command */
+		target_write_u16(target, bank_adr, 0x70);
+
+		int timeout;
+		for (timeout = 0; timeout < 1000; timeout++)
+		{
+			target_read_u8(target, bank_adr, &status);
+			if (status & 0x80)
+				break;
+			alive_sleep(1);
+		}
+		if (timeout == 1000)
+		{
+			LOG_ERROR("write timed out");
+			return ERROR_FAIL;
+		}
+
+		/* clear status reg and read array */
+		target_write_u16(target, bank_adr, 0x50);
+		target_write_u16(target, bank_adr, 0xFF);
+
+		if (status & 0x10)
+			return ERROR_FLASH_OPERATION_FAILED;
+		else if (status & 0x02)
+			return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	return ERROR_OK;
+}
+
+static int str9x_probe(struct flash_bank *bank)
+{
+	return ERROR_OK;
+}
+
+#if 0
+COMMAND_HANDLER(str9x_handle_part_id_command)
+{
+	return ERROR_OK;
+}
+#endif
+
+static int str9x_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	snprintf(buf, buf_size, "str9x flash driver info");
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(str9x_handle_flash_config_command)
+{
+	struct str9x_flash_bank *str9x_info;
+	struct target *target = NULL;
+
+	if (CMD_ARGC < 5)
+	{
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	uint32_t bbsr, nbbsr, bbadr, nbbadr;
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], bbsr);
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], nbbsr);
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[3], bbadr);
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[4], nbbadr);
+
+	str9x_info = bank->driver_priv;
+
+	target = bank->target;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* config flash controller */
+	target_write_u32(target, FLASH_BBSR, bbsr);
+	target_write_u32(target, FLASH_NBBSR, nbbsr);
+	target_write_u32(target, FLASH_BBADR, bbadr >> 2);
+	target_write_u32(target, FLASH_NBBADR, nbbadr >> 2);
+
+	/* set bit 18 instruction TCM order as per flash programming manual */
+	arm966e_write_cp15(target, 62, 0x40000);
+
+	/* enable flash bank 1 */
+	target_write_u32(target, FLASH_CR, 0x18);
+	return ERROR_OK;
+}
+
+static const struct command_registration str9x_config_command_handlers[] = {
+	{
+		.name = "disable_jtag",
+		.handler = &str9x_handle_flash_config_command,
+		.mode = COMMAND_EXEC,
+		.help = "configure str9x flash controller",
+		.usage = "<bank_id> <BBSR> <NBBSR> <BBADR> <NBBADR>",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+static const struct command_registration str9x_command_handlers[] = {
+	{
+		.name = "str9x",
+		.mode = COMMAND_ANY,
+		.help = "str9x flash command group",
+		.chain = str9x_config_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+struct flash_driver str9x_flash = {
+		.name = "str9x",
+		.commands = str9x_command_handlers,
+		.flash_bank_command = &str9x_flash_bank_command,
+		.erase = &str9x_erase,
+		.protect = &str9x_protect,
+		.write = &str9x_write,
+		.probe = &str9x_probe,
+		.auto_probe = &str9x_probe,
+		.erase_check = &default_flash_blank_check,
+		.protect_check = &str9x_protect_check,
+		.info = &str9x_info,
+	};
diff --git a/src/flash/nor/str9x.h b/src/flash/nor/str9x.h
new file mode 100644
index 00000000..c9d5152f
--- /dev/null
+++ b/src/flash/nor/str9x.h
@@ -0,0 +1,62 @@
+/***************************************************************************
+ *   Copyright (C) 2005 by Dominic Rath                                    *
+ *   Dominic.Rath@gmx.de                                                   *
+ *                                                                         *
+ *   Copyright (C) 2008 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifndef STR9X_H
+#define STR9X_H
+
+#include "flash.h"
+
+struct str9x_flash_bank
+{
+	uint32_t *sector_bits;
+	int variant;
+	int bank1;
+	struct working_area *write_algorithm;
+};
+
+enum str9x_status_codes
+{
+	STR9X_CMD_SUCCESS = 0,
+	STR9X_INVALID_COMMAND = 1,
+	STR9X_SRC_ADDR_ERROR = 2,
+	STR9X_DST_ADDR_ERROR = 3,
+	STR9X_SRC_ADDR_NOT_MAPPED = 4,
+	STR9X_DST_ADDR_NOT_MAPPED = 5,
+	STR9X_COUNT_ERROR = 6,
+	STR9X_INVALID_SECTOR = 7,
+	STR9X_SECTOR_NOT_BLANK = 8,
+	STR9X_SECTOR_NOT_PREPARED = 9,
+	STR9X_COMPARE_ERROR = 10,
+	STR9X_BUSY = 11
+};
+
+/* Flash registers */
+
+#define FLASH_BBSR		0x54000000		/* Boot Bank Size Register                */
+#define FLASH_NBBSR		0x54000004		/* Non-Boot Bank Size Register            */
+#define FLASH_BBADR		0x5400000C		/* Boot Bank Base Address Register        */
+#define FLASH_NBBADR	0x54000010		/* Non-Boot Bank Base Address Register    */
+#define FLASH_CR		0x54000018		/* Control Register                       */
+#define FLASH_SR		0x5400001C		/* Status Register                        */
+#define FLASH_BCE5ADDR	0x54000020		/* BC Fifth Entry Target Address Register */
+
+#endif /* STR9X_H */
diff --git a/src/flash/nor/str9xpec.c b/src/flash/nor/str9xpec.c
new file mode 100644
index 00000000..96e12596
--- /dev/null
+++ b/src/flash/nor/str9xpec.c
@@ -0,0 +1,1257 @@
+/***************************************************************************
+ *   Copyright (C) 2005 by Dominic Rath                                    *
+ *   Dominic.Rath@gmx.de                                                   *
+ *                                                                         *
+ *   Copyright (C) 2008 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "str9xpec.h"
+#include "arm7_9_common.h"
+
+
+static int str9xpec_erase_area(struct flash_bank *bank, int first, int last);
+static int str9xpec_set_address(struct flash_bank *bank, uint8_t sector);
+static int str9xpec_write_options(struct flash_bank *bank);
+
+int str9xpec_set_instr(struct jtag_tap *tap, uint32_t new_instr, tap_state_t end_state)
+{
+	if (tap == NULL) {
+		return ERROR_TARGET_INVALID;
+	}
+
+	if (buf_get_u32(tap->cur_instr, 0, tap->ir_length) != new_instr)
+	{
+		struct scan_field field;
+
+		field.tap = tap;
+		field.num_bits = tap->ir_length;
+		field.out_value = calloc(DIV_ROUND_UP(field.num_bits, 8), 1);
+		buf_set_u32(field.out_value, 0, field.num_bits, new_instr);
+		field.in_value = NULL;
+
+		jtag_add_ir_scan(1, &field, end_state);
+
+		free(field.out_value);
+	}
+
+	return ERROR_OK;
+}
+
+static uint8_t str9xpec_isc_status(struct jtag_tap *tap)
+{
+	struct scan_field field;
+	uint8_t status;
+
+	if (str9xpec_set_instr(tap, ISC_NOOP, TAP_IRPAUSE) != ERROR_OK)
+		return ISC_STATUS_ERROR;
+
+	field.tap = tap;
+	field.num_bits = 8;
+	field.out_value = NULL;
+	field.in_value = &status;
+
+
+	jtag_add_dr_scan(1, &field, jtag_set_end_state(TAP_IDLE));
+	jtag_execute_queue();
+
+	LOG_DEBUG("status: 0x%2.2x", status);
+
+	if (status & ISC_STATUS_SECURITY)
+		LOG_INFO("Device Security Bit Set");
+
+	return status;
+}
+
+static int str9xpec_isc_enable(struct flash_bank *bank)
+{
+	uint8_t status;
+	struct jtag_tap *tap;
+	struct str9xpec_flash_controller *str9xpec_info = bank->driver_priv;
+
+	tap = str9xpec_info->tap;
+
+	if (str9xpec_info->isc_enable)
+		return ERROR_OK;
+
+	/* enter isc mode */
+	if (str9xpec_set_instr(tap, ISC_ENABLE, TAP_IDLE) != ERROR_OK)
+		return ERROR_TARGET_INVALID;
+
+	/* check ISC status */
+	status = str9xpec_isc_status(tap);
+	if (status & ISC_STATUS_MODE)
+	{
+		/* we have entered isc mode */
+		str9xpec_info->isc_enable = 1;
+		LOG_DEBUG("ISC_MODE Enabled");
+	}
+
+	return ERROR_OK;
+}
+
+static int str9xpec_isc_disable(struct flash_bank *bank)
+{
+	uint8_t status;
+	struct jtag_tap *tap;
+	struct str9xpec_flash_controller *str9xpec_info = bank->driver_priv;
+
+	tap = str9xpec_info->tap;
+
+	if (!str9xpec_info->isc_enable)
+		return ERROR_OK;
+
+	if (str9xpec_set_instr(tap, ISC_DISABLE, TAP_IDLE) != ERROR_OK)
+		return ERROR_TARGET_INVALID;
+
+	/* delay to handle aborts */
+	jtag_add_sleep(50);
+
+	/* check ISC status */
+	status = str9xpec_isc_status(tap);
+	if (!(status & ISC_STATUS_MODE))
+	{
+		/* we have left isc mode */
+		str9xpec_info->isc_enable = 0;
+		LOG_DEBUG("ISC_MODE Disabled");
+	}
+
+	return ERROR_OK;
+}
+
+static int str9xpec_read_config(struct flash_bank *bank)
+{
+	struct scan_field field;
+	uint8_t status;
+	struct jtag_tap *tap;
+
+	struct str9xpec_flash_controller *str9xpec_info = bank->driver_priv;
+
+	tap = str9xpec_info->tap;
+
+	LOG_DEBUG("ISC_CONFIGURATION");
+
+	/* execute ISC_CONFIGURATION command */
+	str9xpec_set_instr(tap, ISC_CONFIGURATION, TAP_IRPAUSE);
+
+	field.tap = tap;
+	field.num_bits = 64;
+	field.out_value = NULL;
+	field.in_value = str9xpec_info->options;
+
+
+	jtag_add_dr_scan(1, &field, jtag_set_end_state(TAP_IDLE));
+	jtag_execute_queue();
+
+	status = str9xpec_isc_status(tap);
+
+	return status;
+}
+
+static int str9xpec_build_block_list(struct flash_bank *bank)
+{
+	struct str9xpec_flash_controller *str9xpec_info = bank->driver_priv;
+
+	int i;
+	int num_sectors;
+	int b0_sectors = 0, b1_sectors = 0;
+	uint32_t offset = 0;
+	int b1_size = 0x2000;
+
+	switch (bank->size)
+	{
+		case (256 * 1024):
+			b0_sectors = 4;
+			break;
+		case (512 * 1024):
+			b0_sectors = 8;
+			break;
+		case (1024 * 1024):
+			b0_sectors = 16;
+			break;
+		case (2048 * 1024):
+			b0_sectors = 32;
+			break;
+		case (128 * 1024):
+			b1_size = 0x4000;
+			b1_sectors = 8;
+			break;
+		case (32 * 1024):
+			b1_sectors = 4;
+			break;
+		default:
+			LOG_ERROR("BUG: unknown bank->size encountered");
+			exit(-1);
+	}
+
+	num_sectors = b0_sectors + b1_sectors;
+
+	bank->num_sectors = num_sectors;
+	bank->sectors = malloc(sizeof(struct flash_sector) * num_sectors);
+	str9xpec_info->sector_bits = malloc(sizeof(uint32_t) * num_sectors);
+
+	num_sectors = 0;
+
+	for (i = 0; i < b0_sectors; i++)
+	{
+		bank->sectors[num_sectors].offset = offset;
+		bank->sectors[num_sectors].size = 0x10000;
+		offset += bank->sectors[i].size;
+		bank->sectors[num_sectors].is_erased = -1;
+		bank->sectors[num_sectors].is_protected = 1;
+		str9xpec_info->sector_bits[num_sectors++] = i;
+	}
+
+	for (i = 0; i < b1_sectors; i++)
+	{
+		bank->sectors[num_sectors].offset = offset;
+		bank->sectors[num_sectors].size = b1_size;
+		offset += bank->sectors[i].size;
+		bank->sectors[num_sectors].is_erased = -1;
+		bank->sectors[num_sectors].is_protected = 1;
+		str9xpec_info->sector_bits[num_sectors++] = i + 32;
+	}
+
+	return ERROR_OK;
+}
+
+/* flash bank str9x <base> <size> 0 0 <target#>
+ */
+FLASH_BANK_COMMAND_HANDLER(str9xpec_flash_bank_command)
+{
+	struct str9xpec_flash_controller *str9xpec_info;
+	struct arm *armv4_5 = NULL;
+	struct arm7_9_common *arm7_9 = NULL;
+	struct arm_jtag *jtag_info = NULL;
+
+	if (CMD_ARGC < 6)
+	{
+		LOG_WARNING("incomplete flash_bank str9x configuration");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+
+	str9xpec_info = malloc(sizeof(struct str9xpec_flash_controller));
+	bank->driver_priv = str9xpec_info;
+
+	/* REVISIT verify that the jtag position of flash controller is
+	 * right after *THIS* core, which must be a STR9xx core ...
+	 */
+	armv4_5 = bank->target->arch_info;
+	arm7_9 = armv4_5->arch_info;
+	jtag_info = &arm7_9->jtag_info;
+
+	str9xpec_info->tap = bank->target->tap;
+	str9xpec_info->isc_enable = 0;
+
+	str9xpec_build_block_list(bank);
+
+	/* clear option byte register */
+	buf_set_u32(str9xpec_info->options, 0, 64, 0);
+
+	return ERROR_OK;
+}
+
+static int str9xpec_blank_check(struct flash_bank *bank, int first, int last)
+{
+	struct scan_field field;
+	uint8_t status;
+	struct jtag_tap *tap;
+	int i;
+	uint8_t *buffer = NULL;
+
+	struct str9xpec_flash_controller *str9xpec_info = bank->driver_priv;
+
+	tap = str9xpec_info->tap;
+
+	if (!str9xpec_info->isc_enable) {
+		str9xpec_isc_enable(bank);
+	}
+
+	if (!str9xpec_info->isc_enable) {
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	buffer = calloc(DIV_ROUND_UP(64, 8), 1);
+
+	LOG_DEBUG("blank check: first_bank: %i, last_bank: %i", first, last);
+
+	for (i = first; i <= last; i++) {
+		buf_set_u32(buffer, str9xpec_info->sector_bits[i], 1, 1);
+	}
+
+	/* execute ISC_BLANK_CHECK command */
+	str9xpec_set_instr(tap, ISC_BLANK_CHECK, TAP_IRPAUSE);
+
+	field.tap = tap;
+	field.num_bits = 64;
+	field.out_value = buffer;
+	field.in_value = NULL;
+
+	jtag_add_dr_scan(1, &field, jtag_set_end_state(TAP_IDLE));
+	jtag_add_sleep(40000);
+
+	/* read blank check result */
+	field.tap = tap;
+	field.num_bits = 64;
+	field.out_value = NULL;
+	field.in_value = buffer;
+
+	jtag_add_dr_scan(1, &field, TAP_IRPAUSE);
+	jtag_execute_queue();
+
+	status = str9xpec_isc_status(tap);
+
+	for (i = first; i <= last; i++)
+	{
+		if (buf_get_u32(buffer, str9xpec_info->sector_bits[i], 1))
+			bank->sectors[i].is_erased = 0;
+		else
+			bank->sectors[i].is_erased = 1;
+	}
+
+	free(buffer);
+
+	str9xpec_isc_disable(bank);
+
+	if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
+		return ERROR_FLASH_OPERATION_FAILED;
+	return ERROR_OK;
+}
+
+static int str9xpec_protect_check(struct flash_bank *bank)
+{
+	uint8_t status;
+	int i;
+
+	struct str9xpec_flash_controller *str9xpec_info = bank->driver_priv;
+
+	status = str9xpec_read_config(bank);
+
+	for (i = 0; i < bank->num_sectors; i++)
+	{
+		if (buf_get_u32(str9xpec_info->options, str9xpec_info->sector_bits[i], 1))
+			bank->sectors[i].is_protected = 1;
+		else
+			bank->sectors[i].is_protected = 0;
+	}
+
+	if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
+		return ERROR_FLASH_OPERATION_FAILED;
+	return ERROR_OK;
+}
+
+static int str9xpec_erase_area(struct flash_bank *bank, int first, int last)
+{
+	struct scan_field field;
+	uint8_t status;
+	struct jtag_tap *tap;
+	int i;
+	uint8_t *buffer = NULL;
+
+	struct str9xpec_flash_controller *str9xpec_info = bank->driver_priv;
+
+	tap = str9xpec_info->tap;
+
+	if (!str9xpec_info->isc_enable) {
+		str9xpec_isc_enable(bank);
+	}
+
+	if (!str9xpec_info->isc_enable) {
+		return ISC_STATUS_ERROR;
+	}
+
+	buffer = calloc(DIV_ROUND_UP(64, 8), 1);
+
+	LOG_DEBUG("erase: first_bank: %i, last_bank: %i", first, last);
+
+	/* last bank: 0xFF signals a full erase (unlock complete device) */
+	/* last bank: 0xFE signals a option byte erase */
+	if (last == 0xFF)
+	{
+		for (i = 0; i < 64; i++) {
+			buf_set_u32(buffer, i, 1, 1);
+		}
+	}
+	else if (last == 0xFE)
+	{
+		buf_set_u32(buffer, 49, 1, 1);
+	}
+	else
+	{
+		for (i = first; i <= last; i++) {
+			buf_set_u32(buffer, str9xpec_info->sector_bits[i], 1, 1);
+		}
+	}
+
+	LOG_DEBUG("ISC_ERASE");
+
+	/* execute ISC_ERASE command */
+	str9xpec_set_instr(tap, ISC_ERASE, TAP_IRPAUSE);
+
+	field.tap = tap;
+	field.num_bits = 64;
+	field.out_value = buffer;
+	field.in_value = NULL;
+
+	jtag_add_dr_scan(1, &field, jtag_set_end_state(TAP_IDLE));
+	jtag_execute_queue();
+
+	jtag_add_sleep(10);
+
+	/* wait for erase completion */
+	while (!((status = str9xpec_isc_status(tap)) & ISC_STATUS_BUSY)) {
+		alive_sleep(1);
+	}
+
+	free(buffer);
+
+	str9xpec_isc_disable(bank);
+
+	return status;
+}
+
+static int str9xpec_erase(struct flash_bank *bank, int first, int last)
+{
+	int status;
+
+	status = str9xpec_erase_area(bank, first, last);
+
+	if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
+		return ERROR_FLASH_OPERATION_FAILED;
+
+	return ERROR_OK;
+}
+
+static int str9xpec_lock_device(struct flash_bank *bank)
+{
+	struct scan_field field;
+	uint8_t status;
+	struct jtag_tap *tap;
+	struct str9xpec_flash_controller *str9xpec_info = NULL;
+
+	str9xpec_info = bank->driver_priv;
+	tap = str9xpec_info->tap;
+
+	if (!str9xpec_info->isc_enable) {
+		str9xpec_isc_enable(bank);
+	}
+
+	if (!str9xpec_info->isc_enable) {
+		return ISC_STATUS_ERROR;
+	}
+
+	/* set security address */
+	str9xpec_set_address(bank, 0x80);
+
+	/* execute ISC_PROGRAM command */
+	str9xpec_set_instr(tap, ISC_PROGRAM_SECURITY, TAP_IDLE);
+
+	str9xpec_set_instr(tap, ISC_NOOP, TAP_IRPAUSE);
+
+	do {
+		field.tap = tap;
+		field.num_bits = 8;
+		field.out_value = NULL;
+		field.in_value = &status;
+
+		jtag_add_dr_scan(1, &field, jtag_get_end_state());
+		jtag_execute_queue();
+
+	} while (!(status & ISC_STATUS_BUSY));
+
+	str9xpec_isc_disable(bank);
+
+	return status;
+}
+
+static int str9xpec_unlock_device(struct flash_bank *bank)
+{
+	uint8_t status;
+
+	status = str9xpec_erase_area(bank, 0, 255);
+
+	return status;
+}
+
+static int str9xpec_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	uint8_t status;
+	int i;
+
+	struct str9xpec_flash_controller *str9xpec_info = bank->driver_priv;
+
+	status = str9xpec_read_config(bank);
+
+	if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
+		return ERROR_FLASH_OPERATION_FAILED;
+
+	LOG_DEBUG("protect: first_bank: %i, last_bank: %i", first, last);
+
+	/* last bank: 0xFF signals a full device protect */
+	if (last == 0xFF)
+	{
+		if (set)
+		{
+			status = str9xpec_lock_device(bank);
+		}
+		else
+		{
+			/* perform full erase to unlock device */
+			status = str9xpec_unlock_device(bank);
+		}
+	}
+	else
+	{
+		for (i = first; i <= last; i++)
+		{
+			if (set)
+				buf_set_u32(str9xpec_info->options, str9xpec_info->sector_bits[i], 1, 1);
+			else
+				buf_set_u32(str9xpec_info->options, str9xpec_info->sector_bits[i], 1, 0);
+		}
+
+		status = str9xpec_write_options(bank);
+	}
+
+	if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
+		return ERROR_FLASH_OPERATION_FAILED;
+
+	return ERROR_OK;
+}
+
+static int str9xpec_set_address(struct flash_bank *bank, uint8_t sector)
+{
+	struct jtag_tap *tap;
+	struct scan_field field;
+	struct str9xpec_flash_controller *str9xpec_info = bank->driver_priv;
+
+	tap = str9xpec_info->tap;
+
+	/* set flash controller address */
+	str9xpec_set_instr(tap, ISC_ADDRESS_SHIFT, TAP_IRPAUSE);
+
+	field.tap = tap;
+	field.num_bits = 8;
+	field.out_value = &sector;
+	field.in_value = NULL;
+
+	jtag_add_dr_scan(1, &field, jtag_get_end_state());
+
+	return ERROR_OK;
+}
+
+static int str9xpec_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	struct str9xpec_flash_controller *str9xpec_info = bank->driver_priv;
+	uint32_t dwords_remaining = (count / 8);
+	uint32_t bytes_remaining = (count & 0x00000007);
+	uint32_t bytes_written = 0;
+	uint8_t status;
+	uint32_t check_address = offset;
+	struct jtag_tap *tap;
+	struct scan_field field;
+	uint8_t *scanbuf;
+	int i;
+	int first_sector = 0;
+	int last_sector = 0;
+
+	tap = str9xpec_info->tap;
+
+	if (!str9xpec_info->isc_enable) {
+		str9xpec_isc_enable(bank);
+	}
+
+	if (!str9xpec_info->isc_enable) {
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	if (offset & 0x7)
+	{
+		LOG_WARNING("offset 0x%" PRIx32 " breaks required 8-byte alignment", offset);
+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+	}
+
+	for (i = 0; i < bank->num_sectors; i++)
+	{
+		uint32_t sec_start = bank->sectors[i].offset;
+		uint32_t sec_end = sec_start + bank->sectors[i].size;
+
+		/* check if destination falls within the current sector */
+		if ((check_address >= sec_start) && (check_address < sec_end))
+		{
+			/* check if destination ends in the current sector */
+			if (offset + count < sec_end)
+				check_address = offset + count;
+			else
+				check_address = sec_end;
+		}
+
+		if ((offset >= sec_start) && (offset < sec_end)) {
+			first_sector = i;
+		}
+
+		if ((offset + count >= sec_start) && (offset + count < sec_end)) {
+			last_sector = i;
+		}
+	}
+
+	if (check_address != offset + count)
+		return ERROR_FLASH_DST_OUT_OF_BANK;
+
+	LOG_DEBUG("first_sector: %i, last_sector: %i", first_sector, last_sector);
+
+	scanbuf = calloc(DIV_ROUND_UP(64, 8), 1);
+
+	LOG_DEBUG("ISC_PROGRAM");
+
+	for (i = first_sector; i <= last_sector; i++)
+	{
+		str9xpec_set_address(bank, str9xpec_info->sector_bits[i]);
+
+		dwords_remaining = dwords_remaining < (bank->sectors[i].size/8) ? dwords_remaining : (bank->sectors[i].size/8);
+
+		while (dwords_remaining > 0)
+		{
+			str9xpec_set_instr(tap, ISC_PROGRAM, TAP_IRPAUSE);
+
+			field.tap = tap;
+			field.num_bits = 64;
+			field.out_value = (buffer + bytes_written);
+			field.in_value = NULL;
+
+			jtag_add_dr_scan(1, &field, jtag_set_end_state(TAP_IDLE));
+
+			/* small delay before polling */
+			jtag_add_sleep(50);
+
+			str9xpec_set_instr(tap, ISC_NOOP, TAP_IRPAUSE);
+
+			do {
+				field.tap = tap;
+				field.num_bits = 8;
+				field.out_value = NULL;
+				field.in_value = scanbuf;
+
+				jtag_add_dr_scan(1, &field, jtag_get_end_state());
+				jtag_execute_queue();
+
+				status = buf_get_u32(scanbuf, 0, 8);
+
+			} while (!(status & ISC_STATUS_BUSY));
+
+			if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
+				return ERROR_FLASH_OPERATION_FAILED;
+
+			/* if ((status & ISC_STATUS_INT_ERROR) != STR9XPEC_ISC_INTFAIL)
+				return ERROR_FLASH_OPERATION_FAILED; */
+
+			dwords_remaining--;
+			bytes_written += 8;
+		}
+	}
+
+	if (bytes_remaining)
+	{
+		uint8_t last_dword[8] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+		int i = 0;
+
+		while (bytes_remaining > 0)
+		{
+			last_dword[i++] = *(buffer + bytes_written);
+			bytes_remaining--;
+			bytes_written++;
+		}
+
+		str9xpec_set_instr(tap, ISC_PROGRAM, TAP_IRPAUSE);
+
+		field.tap = tap;
+		field.num_bits = 64;
+		field.out_value = last_dword;
+		field.in_value = NULL;
+
+		jtag_add_dr_scan(1, &field, jtag_set_end_state(TAP_IDLE));
+
+		/* small delay before polling */
+		jtag_add_sleep(50);
+
+		str9xpec_set_instr(tap, ISC_NOOP, TAP_IRPAUSE);
+
+		do {
+			field.tap = tap;
+			field.num_bits = 8;
+			field.out_value = NULL;
+			field.in_value = scanbuf;
+
+			jtag_add_dr_scan(1, &field, jtag_get_end_state());
+			jtag_execute_queue();
+
+			status = buf_get_u32(scanbuf, 0, 8);
+
+		} while (!(status & ISC_STATUS_BUSY));
+
+		if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
+			return ERROR_FLASH_OPERATION_FAILED;
+
+		/* if ((status & ISC_STATUS_INT_ERROR) != STR9XPEC_ISC_INTFAIL)
+			return ERROR_FLASH_OPERATION_FAILED; */
+	}
+
+	free(scanbuf);
+
+	str9xpec_isc_disable(bank);
+
+	return ERROR_OK;
+}
+
+static int str9xpec_probe(struct flash_bank *bank)
+{
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(str9xpec_handle_part_id_command)
+{
+	struct scan_field field;
+	uint8_t *buffer = NULL;
+	struct jtag_tap *tap;
+	uint32_t idcode;
+	struct str9xpec_flash_controller *str9xpec_info = NULL;
+
+	if (CMD_ARGC < 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	str9xpec_info = bank->driver_priv;
+	tap = str9xpec_info->tap;
+
+	buffer = calloc(DIV_ROUND_UP(32, 8), 1);
+
+	str9xpec_set_instr(tap, ISC_IDCODE, TAP_IRPAUSE);
+
+	field.tap = tap;
+	field.num_bits = 32;
+	field.out_value = NULL;
+	field.in_value = buffer;
+
+	jtag_add_dr_scan(1, &field, jtag_set_end_state(TAP_IDLE));
+	jtag_execute_queue();
+
+	idcode = buf_get_u32(buffer, 0, 32);
+
+	command_print(CMD_CTX, "str9xpec part id: 0x%8.8" PRIx32 "", idcode);
+
+	free(buffer);
+
+	return ERROR_OK;
+}
+
+static int str9xpec_erase_check(struct flash_bank *bank)
+{
+	return str9xpec_blank_check(bank, 0, bank->num_sectors - 1);
+}
+
+static int str9xpec_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	snprintf(buf, buf_size, "str9xpec flash driver info");
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(str9xpec_handle_flash_options_read_command)
+{
+	uint8_t status;
+	struct str9xpec_flash_controller *str9xpec_info = NULL;
+
+	if (CMD_ARGC < 1)
+	{
+		command_print(CMD_CTX, "str9xpec options_read <bank>");
+		return ERROR_OK;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	str9xpec_info = bank->driver_priv;
+
+	status = str9xpec_read_config(bank);
+
+	if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
+		return ERROR_FLASH_OPERATION_FAILED;
+
+	/* boot bank */
+	if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1))
+		command_print(CMD_CTX, "CS Map: bank1");
+	else
+		command_print(CMD_CTX, "CS Map: bank0");
+
+	/* OTP lock */
+	if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_OTPBIT, 1))
+		command_print(CMD_CTX, "OTP Lock: OTP Locked");
+	else
+		command_print(CMD_CTX, "OTP Lock: OTP Unlocked");
+
+	/* LVD Threshold */
+	if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1))
+		command_print(CMD_CTX, "LVD Threshold: 2.7v");
+	else
+		command_print(CMD_CTX, "LVD Threshold: 2.4v");
+
+	/* LVD reset warning */
+	if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1))
+		command_print(CMD_CTX, "LVD Reset Warning: VDD or VDDQ Inputs");
+	else
+		command_print(CMD_CTX, "LVD Reset Warning: VDD Input Only");
+
+	/* LVD reset select */
+	if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1))
+		command_print(CMD_CTX, "LVD Reset Selection: VDD or VDDQ Inputs");
+	else
+		command_print(CMD_CTX, "LVD Reset Selection: VDD Input Only");
+
+	return ERROR_OK;
+}
+
+static int str9xpec_write_options(struct flash_bank *bank)
+{
+	struct scan_field field;
+	uint8_t status;
+	struct jtag_tap *tap;
+	struct str9xpec_flash_controller *str9xpec_info = NULL;
+
+	str9xpec_info = bank->driver_priv;
+	tap = str9xpec_info->tap;
+
+	/* erase config options first */
+	status = str9xpec_erase_area(bank, 0xFE, 0xFE);
+
+	if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
+		return status;
+
+	if (!str9xpec_info->isc_enable) {
+		str9xpec_isc_enable(bank);
+	}
+
+	if (!str9xpec_info->isc_enable) {
+		return ISC_STATUS_ERROR;
+	}
+
+	/* according to data 64th bit has to be set */
+	buf_set_u32(str9xpec_info->options, 63, 1, 1);
+
+	/* set option byte address */
+	str9xpec_set_address(bank, 0x50);
+
+	/* execute ISC_PROGRAM command */
+	str9xpec_set_instr(tap, ISC_PROGRAM, TAP_IRPAUSE);
+
+	field.tap = tap;
+	field.num_bits = 64;
+	field.out_value = str9xpec_info->options;
+	field.in_value = NULL;
+
+	jtag_add_dr_scan(1, &field, jtag_set_end_state(TAP_IDLE));
+
+	/* small delay before polling */
+	jtag_add_sleep(50);
+
+	str9xpec_set_instr(tap, ISC_NOOP, TAP_IRPAUSE);
+
+	do {
+		field.tap = tap;
+		field.num_bits = 8;
+		field.out_value = NULL;
+		field.in_value = &status;
+
+		jtag_add_dr_scan(1, &field, jtag_get_end_state());
+		jtag_execute_queue();
+
+	} while (!(status & ISC_STATUS_BUSY));
+
+	str9xpec_isc_disable(bank);
+
+	return status;
+}
+
+COMMAND_HANDLER(str9xpec_handle_flash_options_write_command)
+{
+	uint8_t status;
+
+	if (CMD_ARGC < 1)
+	{
+		command_print(CMD_CTX, "str9xpec options_write <bank>");
+		return ERROR_OK;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	status = str9xpec_write_options(bank);
+
+	if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
+		return ERROR_FLASH_OPERATION_FAILED;
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(str9xpec_handle_flash_options_cmap_command)
+{
+	struct str9xpec_flash_controller *str9xpec_info = NULL;
+
+	if (CMD_ARGC < 2)
+	{
+		command_print(CMD_CTX, "str9xpec options_cmap <bank> <bank0 | bank1>");
+		return ERROR_OK;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	str9xpec_info = bank->driver_priv;
+
+	if (strcmp(CMD_ARGV[1], "bank1") == 0)
+	{
+		buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1, 1);
+	}
+	else
+	{
+		buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1, 0);
+	}
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(str9xpec_handle_flash_options_lvdthd_command)
+{
+	struct str9xpec_flash_controller *str9xpec_info = NULL;
+
+	if (CMD_ARGC < 2)
+	{
+		command_print(CMD_CTX, "str9xpec options_lvdthd <bank> <2.4v | 2.7v>");
+		return ERROR_OK;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	str9xpec_info = bank->driver_priv;
+
+	if (strcmp(CMD_ARGV[1], "2.7v") == 0)
+	{
+		buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1, 1);
+	}
+	else
+	{
+		buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1, 0);
+	}
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(str9xpec_handle_flash_options_lvdsel_command)
+{
+	struct str9xpec_flash_controller *str9xpec_info = NULL;
+
+	if (CMD_ARGC < 2)
+	{
+		command_print(CMD_CTX, "str9xpec options_lvdsel <bank> <vdd | vdd_vddq>");
+		return ERROR_OK;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	str9xpec_info = bank->driver_priv;
+
+	if (strcmp(CMD_ARGV[1], "vdd_vddq") == 0)
+	{
+		buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1, 1);
+	}
+	else
+	{
+		buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1, 0);
+	}
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(str9xpec_handle_flash_options_lvdwarn_command)
+{
+	struct str9xpec_flash_controller *str9xpec_info = NULL;
+
+	if (CMD_ARGC < 2)
+	{
+		command_print(CMD_CTX, "str9xpec options_lvdwarn <bank> <vdd | vdd_vddq>");
+		return ERROR_OK;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	str9xpec_info = bank->driver_priv;
+
+	if (strcmp(CMD_ARGV[1], "vdd_vddq") == 0)
+	{
+		buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1, 1);
+	}
+	else
+	{
+		buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1, 0);
+	}
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(str9xpec_handle_flash_lock_command)
+{
+	uint8_t status;
+
+	if (CMD_ARGC < 1)
+	{
+		command_print(CMD_CTX, "str9xpec lock <bank>");
+		return ERROR_OK;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	status = str9xpec_lock_device(bank);
+
+	if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
+		return ERROR_FLASH_OPERATION_FAILED;
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(str9xpec_handle_flash_unlock_command)
+{
+	uint8_t status;
+
+	if (CMD_ARGC < 1)
+	{
+		command_print(CMD_CTX, "str9xpec unlock <bank>");
+		return ERROR_OK;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	status = str9xpec_unlock_device(bank);
+
+	if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
+		return ERROR_FLASH_OPERATION_FAILED;
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(str9xpec_handle_flash_enable_turbo_command)
+{
+	struct jtag_tap *tap0;
+	struct jtag_tap *tap1;
+	struct jtag_tap *tap2;
+	struct str9xpec_flash_controller *str9xpec_info = NULL;
+
+	if (CMD_ARGC < 1)
+	{
+		command_print(CMD_CTX, "str9xpec enable_turbo <bank>");
+		return ERROR_OK;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	str9xpec_info = bank->driver_priv;
+
+	tap0 = str9xpec_info->tap;
+
+	/* remove arm core from chain - enter turbo mode */
+	tap1 = tap0->next_tap;
+	if (tap1 == NULL)
+	{
+		/* things are *WRONG* */
+		command_print(CMD_CTX,"**STR9FLASH** (tap1) invalid chain?");
+		return ERROR_OK;
+	}
+	tap2 = tap1->next_tap;
+	if (tap2 == NULL)
+	{
+		/* things are *WRONG* */
+		command_print(CMD_CTX,"**STR9FLASH** (tap2) invalid chain?");
+		return ERROR_OK;
+	}
+
+	/* enable turbo mode - TURBO-PROG-ENABLE */
+	str9xpec_set_instr(tap2, 0xD, TAP_IDLE);
+	if ((retval = jtag_execute_queue()) != ERROR_OK)
+		return retval;
+
+	/* modify scan chain - str9 core has been removed */
+	tap1->enabled = 0;
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(str9xpec_handle_flash_disable_turbo_command)
+{
+	struct jtag_tap *tap;
+	struct str9xpec_flash_controller *str9xpec_info = NULL;
+
+	if (CMD_ARGC < 1)
+	{
+		command_print(CMD_CTX, "str9xpec disable_turbo <bank>");
+		return ERROR_OK;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	str9xpec_info = bank->driver_priv;
+	tap = str9xpec_info->tap;
+
+	if (tap == NULL)
+		return ERROR_FAIL;
+
+	/* exit turbo mode via RESET */
+	str9xpec_set_instr(tap, ISC_NOOP, TAP_IDLE);
+	jtag_add_tlr();
+	jtag_execute_queue();
+
+	/* restore previous scan chain */
+	if (tap->next_tap) {
+		tap->next_tap->enabled = 1;
+	}
+
+	return ERROR_OK;
+}
+
+static const struct command_registration str9xpec_config_command_handlers[] = {
+	{
+		.name = "enable_turbo",
+		.handler = str9xpec_handle_flash_enable_turbo_command,
+		.mode = COMMAND_EXEC,
+		.help = "enable str9xpec turbo mode",
+	},
+	{
+		.name = "disable_turbo",
+		.handler = str9xpec_handle_flash_disable_turbo_command,
+		.mode = COMMAND_EXEC,
+		.help = "disable str9xpec turbo mode",
+	},
+	{
+		.name = "options_cmap",
+		.handler = str9xpec_handle_flash_options_cmap_command,
+		.mode = COMMAND_EXEC,
+		.help = "configure str9xpec boot sector",
+	},
+	{
+		.name = "options_lvdthd",
+		.handler = str9xpec_handle_flash_options_lvdthd_command,
+		.mode = COMMAND_EXEC,
+		.help = "configure str9xpec lvd threshold",
+	},
+	{
+		.name = "options_lvdsel",
+		.handler = str9xpec_handle_flash_options_lvdsel_command,
+		.mode = COMMAND_EXEC,
+		.help = "configure str9xpec lvd selection",
+	},
+	{
+		.name = "options_lvdwarn",
+		.handler = str9xpec_handle_flash_options_lvdwarn_command,
+		.mode = COMMAND_EXEC,
+		.help = "configure str9xpec lvd warning",
+	},
+	{
+		.name = "options_read",
+		.handler = str9xpec_handle_flash_options_read_command,
+		.mode = COMMAND_EXEC,
+		.help = "read str9xpec options",
+	},
+	{
+		.name = "options_write",
+		.handler = str9xpec_handle_flash_options_write_command,
+		.mode = COMMAND_EXEC,
+		.help = "write str9xpec options",
+	},
+	{
+		.name = "lock",
+		.handler = str9xpec_handle_flash_lock_command,
+		.mode = COMMAND_EXEC,
+		.help = "lock str9xpec device",
+	},
+	{
+		.name = "unlock",
+		.handler = str9xpec_handle_flash_unlock_command,
+		.mode = COMMAND_EXEC,
+		.help = "unlock str9xpec device",
+	},
+	{
+		.name = "part_id",
+		.handler = str9xpec_handle_part_id_command,
+		.mode = COMMAND_EXEC,
+		.help = "print part id of str9xpec flash bank <num>",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+static const struct command_registration str9xpec_command_handlers[] = {
+	{
+		.name = "str9xpec",
+		.mode = COMMAND_ANY,
+		.help = "str9xpec flash command group",
+		.chain = str9xpec_config_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+struct flash_driver str9xpec_flash = {
+		.name = "str9xpec",
+		.commands = str9xpec_command_handlers,
+		.flash_bank_command = &str9xpec_flash_bank_command,
+		.erase = &str9xpec_erase,
+		.protect = &str9xpec_protect,
+		.write = &str9xpec_write,
+		.probe = &str9xpec_probe,
+		.auto_probe = &str9xpec_probe,
+		.erase_check = &str9xpec_erase_check,
+		.protect_check = &str9xpec_protect_check,
+		.info = &str9xpec_info,
+	};
diff --git a/src/flash/nor/str9xpec.h b/src/flash/nor/str9xpec.h
new file mode 100644
index 00000000..1c8d41b1
--- /dev/null
+++ b/src/flash/nor/str9xpec.h
@@ -0,0 +1,79 @@
+/***************************************************************************
+ *   Copyright (C) 2005 by Dominic Rath                                    *
+ *   Dominic.Rath@gmx.de                                                   *
+ *                                                                         *
+ *   Copyright (C) 2008 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifndef STR9XPEC_H
+#define STR9XPEC_H
+
+#include "flash.h"
+#include "jtag.h"
+
+struct str9xpec_flash_controller
+{
+	struct jtag_tap *tap;
+	uint32_t *sector_bits;
+	int chain_pos;
+	int isc_enable;
+	uint8_t options[8];
+};
+
+enum str9xpec_status_codes
+{
+	STR9XPEC_INVALID_COMMAND = 1,
+	STR9XPEC_ISC_SUCCESS = 2,
+	STR9XPEC_ISC_DISABLED = 3,
+	STR9XPEC_ISC_INTFAIL = 32,
+};
+
+/* ISC commands */
+
+#define ISC_IDCODE				0xFE
+#define ISC_MFG_READ			0x4C
+#define ISC_CONFIGURATION		0x07
+#define ISC_ENABLE				0x0C
+#define ISC_DISABLE				0x0F
+#define ISC_NOOP				0x10
+#define ISC_ADDRESS_SHIFT		0x11
+#define ISC_CLR_STATUS			0x13
+#define ISC_PROGRAM				0x20
+#define ISC_PROGRAM_SECURITY	0x22
+#define ISC_PROGRAM_UC			0x23
+#define ISC_ERASE				0x30
+#define ISC_READ				0x50
+#define ISC_BLANK_CHECK			0x60
+
+/* ISC_DEFAULT bit definitions */
+
+#define ISC_STATUS_SECURITY		0x40
+#define ISC_STATUS_INT_ERROR	0x30
+#define ISC_STATUS_MODE			0x08
+#define ISC_STATUS_BUSY			0x04
+#define ISC_STATUS_ERROR		0x03
+
+/* Option bytes definitions */
+
+#define STR9XPEC_OPT_CSMAPBIT		48
+#define STR9XPEC_OPT_LVDTHRESBIT	49
+#define STR9XPEC_OPT_LVDSELBIT		50
+#define STR9XPEC_OPT_LVDWARNBIT		51
+#define STR9XPEC_OPT_OTPBIT			63
+
+#endif /* STR9XPEC_H */
diff --git a/src/flash/nor/tms470.c b/src/flash/nor/tms470.c
new file mode 100644
index 00000000..59659346
--- /dev/null
+++ b/src/flash/nor/tms470.c
@@ -0,0 +1,1271 @@
+/***************************************************************************
+ *   Copyright (C) 2007,2008 by Christopher Kilgour                        *
+ *   techie |_at_| whiterocker |_dot_| com                                 *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "tms470.h"
+
+
+/* ----------------------------------------------------------------------
+                      Internal Support, Helpers
+   ---------------------------------------------------------------------- */
+
+const struct flash_sector TMS470R1A256_SECTORS[] = {
+	{0x00000000, 0x00002000, -1, -1},
+	{0x00002000, 0x00002000, -1, -1},
+	{0x00004000, 0x00002000, -1, -1},
+	{0x00006000, 0x00002000, -1, -1},
+	{0x00008000, 0x00008000, -1, -1},
+	{0x00010000, 0x00008000, -1, -1},
+	{0x00018000, 0x00008000, -1, -1},
+	{0x00020000, 0x00008000, -1, -1},
+	{0x00028000, 0x00008000, -1, -1},
+	{0x00030000, 0x00008000, -1, -1},
+	{0x00038000, 0x00002000, -1, -1},
+	{0x0003A000, 0x00002000, -1, -1},
+	{0x0003C000, 0x00002000, -1, -1},
+	{0x0003E000, 0x00002000, -1, -1},
+};
+
+#define TMS470R1A256_NUM_SECTORS \
+	ARRAY_SIZE(TMS470R1A256_SECTORS)
+
+const struct flash_sector TMS470R1A288_BANK0_SECTORS[] = {
+	{0x00000000, 0x00002000, -1, -1},
+	{0x00002000, 0x00002000, -1, -1},
+	{0x00004000, 0x00002000, -1, -1},
+	{0x00006000, 0x00002000, -1, -1},
+};
+
+#define TMS470R1A288_BANK0_NUM_SECTORS \
+	ARRAY_SIZE(TMS470R1A288_BANK0_SECTORS)
+
+const struct flash_sector TMS470R1A288_BANK1_SECTORS[] = {
+	{0x00040000, 0x00010000, -1, -1},
+	{0x00050000, 0x00010000, -1, -1},
+	{0x00060000, 0x00010000, -1, -1},
+	{0x00070000, 0x00010000, -1, -1},
+};
+
+#define TMS470R1A288_BANK1_NUM_SECTORS \
+	ARRAY_SIZE(TMS470R1A288_BANK1_SECTORS)
+
+const struct flash_sector TMS470R1A384_BANK0_SECTORS[] = {
+	{0x00000000, 0x00002000, -1, -1},
+	{0x00002000, 0x00002000, -1, -1},
+	{0x00004000, 0x00004000, -1, -1},
+	{0x00008000, 0x00004000, -1, -1},
+	{0x0000C000, 0x00004000, -1, -1},
+	{0x00010000, 0x00004000, -1, -1},
+	{0x00014000, 0x00004000, -1, -1},
+	{0x00018000, 0x00002000, -1, -1},
+	{0x0001C000, 0x00002000, -1, -1},
+	{0x0001E000, 0x00002000, -1, -1},
+};
+
+#define TMS470R1A384_BANK0_NUM_SECTORS \
+	ARRAY_SIZE(TMS470R1A384_BANK0_SECTORS)
+
+const struct flash_sector TMS470R1A384_BANK1_SECTORS[] = {
+	{0x00020000, 0x00008000, -1, -1},
+	{0x00028000, 0x00008000, -1, -1},
+	{0x00030000, 0x00008000, -1, -1},
+	{0x00038000, 0x00008000, -1, -1},
+};
+
+#define TMS470R1A384_BANK1_NUM_SECTORS \
+	ARRAY_SIZE(TMS470R1A384_BANK1_SECTORS)
+
+const struct flash_sector TMS470R1A384_BANK2_SECTORS[] = {
+	{0x00040000, 0x00008000, -1, -1},
+	{0x00048000, 0x00008000, -1, -1},
+	{0x00050000, 0x00008000, -1, -1},
+	{0x00058000, 0x00008000, -1, -1},
+};
+
+#define TMS470R1A384_BANK2_NUM_SECTORS \
+	ARRAY_SIZE(TMS470R1A384_BANK2_SECTORS)
+
+/* ---------------------------------------------------------------------- */
+
+static int tms470_read_part_info(struct flash_bank *bank)
+{
+	struct tms470_flash_bank *tms470_info = bank->driver_priv;
+	struct target *target = bank->target;
+	uint32_t device_ident_reg;
+	uint32_t silicon_version;
+	uint32_t technology_family;
+	uint32_t rom_flash;
+	uint32_t part_number;
+	char *part_name;
+
+	/* we shall not rely on the caller in this test, this function allocates memory,
+	   thus and executing the code more than once may cause memory leak */
+	if (tms470_info->device_ident_reg)
+	  return ERROR_OK;
+
+	/* read and parse the device identification register */
+	target_read_u32(target, 0xFFFFFFF0, &device_ident_reg);
+
+	LOG_INFO("device_ident_reg = 0x%08" PRIx32 "", device_ident_reg);
+
+	if ((device_ident_reg & 7) == 0)
+	{
+		LOG_WARNING("Cannot identify target as a TMS470 family.");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	silicon_version = (device_ident_reg >> 12) & 0xF;
+	technology_family = (device_ident_reg >> 11) & 1;
+	rom_flash = (device_ident_reg >> 10) & 1;
+	part_number = (device_ident_reg >> 3) & 0x7f;
+
+	/*
+	 * If the part number is known, determine if the flash bank is valid
+	 * based on the base address being within the known flash bank
+	 * ranges.  Then fixup/complete the remaining fields of the flash
+	 * bank structure.
+	 */
+	switch (part_number)
+	{
+	case 0x0a:
+		part_name = "TMS470R1A256";
+
+		if (bank->base >= 0x00040000)
+		{
+			LOG_ERROR("No %s flash bank contains base address 0x%08" PRIx32 ".", part_name, bank->base);
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+		tms470_info->ordinal = 0;
+		bank->base = 0x00000000;
+		bank->size = 256 * 1024;
+		bank->num_sectors = TMS470R1A256_NUM_SECTORS;
+		bank->sectors = malloc(sizeof(TMS470R1A256_SECTORS));
+		if (!bank->sectors)
+		{
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+		(void)memcpy(bank->sectors, TMS470R1A256_SECTORS, sizeof(TMS470R1A256_SECTORS));
+		break;
+
+	case 0x2b:
+		part_name = "TMS470R1A288";
+
+		if (bank->base < 0x00008000)
+		{
+			tms470_info->ordinal = 0;
+			bank->base = 0x00000000;
+			bank->size = 32 * 1024;
+			bank->num_sectors = TMS470R1A288_BANK0_NUM_SECTORS;
+			bank->sectors = malloc(sizeof(TMS470R1A288_BANK0_SECTORS));
+			if (!bank->sectors)
+			{
+				return ERROR_FLASH_OPERATION_FAILED;
+			}
+			(void)memcpy(bank->sectors, TMS470R1A288_BANK0_SECTORS, sizeof(TMS470R1A288_BANK0_SECTORS));
+		}
+		else if ((bank->base >= 0x00040000) && (bank->base < 0x00080000))
+		{
+			tms470_info->ordinal = 1;
+			bank->base = 0x00040000;
+			bank->size = 256 * 1024;
+			bank->num_sectors = TMS470R1A288_BANK1_NUM_SECTORS;
+			bank->sectors = malloc(sizeof(TMS470R1A288_BANK1_SECTORS));
+			if (!bank->sectors)
+			{
+				return ERROR_FLASH_OPERATION_FAILED;
+			}
+			(void)memcpy(bank->sectors, TMS470R1A288_BANK1_SECTORS, sizeof(TMS470R1A288_BANK1_SECTORS));
+		}
+		else
+		{
+			LOG_ERROR("No %s flash bank contains base address 0x%08" PRIx32 ".", part_name, bank->base);
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+		break;
+
+	case 0x2d:
+		part_name = "TMS470R1A384";
+
+		if (bank->base < 0x00020000)
+		{
+			tms470_info->ordinal = 0;
+			bank->base = 0x00000000;
+			bank->size = 128 * 1024;
+			bank->num_sectors = TMS470R1A384_BANK0_NUM_SECTORS;
+			bank->sectors = malloc(sizeof(TMS470R1A384_BANK0_SECTORS));
+			if (!bank->sectors)
+			{
+				return ERROR_FLASH_OPERATION_FAILED;
+			}
+			(void)memcpy(bank->sectors, TMS470R1A384_BANK0_SECTORS, sizeof(TMS470R1A384_BANK0_SECTORS));
+		}
+		else if ((bank->base >= 0x00020000) && (bank->base < 0x00040000))
+		{
+			tms470_info->ordinal = 1;
+			bank->base = 0x00020000;
+			bank->size = 128 * 1024;
+			bank->num_sectors = TMS470R1A384_BANK1_NUM_SECTORS;
+			bank->sectors = malloc(sizeof(TMS470R1A384_BANK1_SECTORS));
+			if (!bank->sectors)
+			{
+				return ERROR_FLASH_OPERATION_FAILED;
+			}
+			(void)memcpy(bank->sectors, TMS470R1A384_BANK1_SECTORS, sizeof(TMS470R1A384_BANK1_SECTORS));
+		}
+		else if ((bank->base >= 0x00040000) && (bank->base < 0x00060000))
+		{
+			tms470_info->ordinal = 2;
+			bank->base = 0x00040000;
+			bank->size = 128 * 1024;
+			bank->num_sectors = TMS470R1A384_BANK2_NUM_SECTORS;
+			bank->sectors = malloc(sizeof(TMS470R1A384_BANK2_SECTORS));
+			if (!bank->sectors)
+			{
+				return ERROR_FLASH_OPERATION_FAILED;
+			}
+			(void)memcpy(bank->sectors, TMS470R1A384_BANK2_SECTORS, sizeof(TMS470R1A384_BANK2_SECTORS));
+		}
+		else
+		{
+			LOG_ERROR("No %s flash bank contains base address 0x%08" PRIx32 ".", part_name, bank->base);
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+		break;
+
+	default:
+		LOG_WARNING("Could not identify part 0x%02x as a member of the TMS470 family.", (unsigned)part_number);
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	/* turn off memory selects */
+	target_write_u32(target, 0xFFFFFFE4, 0x00000000);
+	target_write_u32(target, 0xFFFFFFE0, 0x00000000);
+
+	bank->chip_width = 32;
+	bank->bus_width = 32;
+
+	LOG_INFO("Identified %s, ver=%d, core=%s, nvmem=%s.",
+		 part_name,
+		 (int)(silicon_version),
+		 (technology_family ? "1.8v" : "3.3v"),
+		 (rom_flash ? "rom" : "flash"));
+
+	tms470_info->device_ident_reg = device_ident_reg;
+	tms470_info->silicon_version = silicon_version;
+	tms470_info->technology_family = technology_family;
+	tms470_info->rom_flash = rom_flash;
+	tms470_info->part_number = part_number;
+	tms470_info->part_name = part_name;
+
+	/*
+	 * Disable reset on address access violation.
+	 */
+	target_write_u32(target, 0xFFFFFFE0, 0x00004007);
+
+	return ERROR_OK;
+}
+
+/* ---------------------------------------------------------------------- */
+
+static uint32_t keysSet = 0;
+static uint32_t flashKeys[4];
+
+COMMAND_HANDLER(tms470_handle_flash_keyset_command)
+{
+	if (CMD_ARGC > 4)
+	{
+		command_print(CMD_CTX, "tms470 flash_keyset <key0> <key1> <key2> <key3>");
+		return ERROR_INVALID_ARGUMENTS;
+	}
+	else if (CMD_ARGC == 4)
+	{
+		int i;
+
+		for (i = 0; i < 4; i++)
+		{
+			int start = (0 == strncmp(CMD_ARGV[i], "0x", 2)) ? 2 : 0;
+
+			if (1 != sscanf(&CMD_ARGV[i][start], "%" SCNx32 "", &flashKeys[i]))
+			{
+				command_print(CMD_CTX, "could not process flash key %s", CMD_ARGV[i]);
+				LOG_ERROR("could not process flash key %s", CMD_ARGV[i]);
+				return ERROR_INVALID_ARGUMENTS;
+			}
+		}
+
+		keysSet = 1;
+	}
+	else if (CMD_ARGC != 0)
+	{
+		command_print(CMD_CTX, "tms470 flash_keyset <key0> <key1> <key2> <key3>");
+		return ERROR_INVALID_ARGUMENTS;
+	}
+
+	if (keysSet)
+	{
+		command_print(CMD_CTX, "using flash keys 0x%08" PRIx32 ", 0x%08" PRIx32 ", 0x%08" PRIx32 ", 0x%08" PRIx32 "",
+			      flashKeys[0], flashKeys[1], flashKeys[2], flashKeys[3]);
+	}
+	else
+	{
+		command_print(CMD_CTX, "flash keys not set");
+	}
+
+	return ERROR_OK;
+}
+
+static const uint32_t FLASH_KEYS_ALL_ONES[] = { 0xFFFFFFFF, 0xFFFFFFFF,
+	0xFFFFFFFF, 0xFFFFFFFF,
+};
+
+static const uint32_t FLASH_KEYS_ALL_ZEROS[] = { 0x00000000, 0x00000000,
+	0x00000000, 0x00000000,
+};
+
+static const uint32_t FLASH_KEYS_MIX1[] = { 0xf0fff0ff, 0xf0fff0ff,
+	0xf0fff0ff, 0xf0fff0ff
+};
+
+static const uint32_t FLASH_KEYS_MIX2[] = { 0x0000ffff, 0x0000ffff,
+	0x0000ffff, 0x0000ffff
+};
+
+/* ---------------------------------------------------------------------- */
+
+static int oscMHz = 12;
+
+COMMAND_HANDLER(tms470_handle_osc_megahertz_command)
+{
+	if (CMD_ARGC > 1)
+	{
+		command_print(CMD_CTX, "tms470 osc_megahertz <MHz>");
+		return ERROR_INVALID_ARGUMENTS;
+	}
+	else if (CMD_ARGC == 1)
+	{
+		sscanf(CMD_ARGV[0], "%d", &oscMHz);
+	}
+
+	if (oscMHz <= 0)
+	{
+		LOG_ERROR("osc_megahertz must be positive and non-zero!");
+		command_print(CMD_CTX, "osc_megahertz must be positive and non-zero!");
+		oscMHz = 12;
+		return ERROR_INVALID_ARGUMENTS;
+	}
+
+	command_print(CMD_CTX, "osc_megahertz=%d", oscMHz);
+
+	return ERROR_OK;
+}
+
+/* ---------------------------------------------------------------------- */
+
+static int plldis = 0;
+
+COMMAND_HANDLER(tms470_handle_plldis_command)
+{
+	if (CMD_ARGC > 1)
+	{
+		command_print(CMD_CTX, "tms470 plldis <0 | 1>");
+		return ERROR_INVALID_ARGUMENTS;
+	}
+	else if (CMD_ARGC == 1)
+	{
+		sscanf(CMD_ARGV[0], "%d", &plldis);
+		plldis = plldis ? 1 : 0;
+	}
+
+	command_print(CMD_CTX, "plldis=%d", plldis);
+
+	return ERROR_OK;
+}
+
+/* ---------------------------------------------------------------------- */
+
+static int tms470_check_flash_unlocked(struct target * target)
+{
+	uint32_t fmbbusy;
+
+	target_read_u32(target, 0xFFE89C08, &fmbbusy);
+	LOG_INFO("tms470 fmbbusy = 0x%08" PRIx32 " -> %s", fmbbusy, fmbbusy & 0x8000 ? "unlocked" : "LOCKED");
+	return fmbbusy & 0x8000 ? ERROR_OK : ERROR_FLASH_OPERATION_FAILED;
+}
+
+/* ---------------------------------------------------------------------- */
+
+static int tms470_try_flash_keys(struct target * target, const uint32_t * key_set)
+{
+	uint32_t glbctrl, fmmstat;
+	int retval = ERROR_FLASH_OPERATION_FAILED;
+
+	/* set GLBCTRL.4  */
+	target_read_u32(target, 0xFFFFFFDC, &glbctrl);
+	target_write_u32(target, 0xFFFFFFDC, glbctrl | 0x10);
+
+	/* only perform the key match when 3VSTAT is clear */
+	target_read_u32(target, 0xFFE8BC0C, &fmmstat);
+	if (!(fmmstat & 0x08))
+	{
+		unsigned i;
+		uint32_t fmbptr, fmbac2, orig_fmregopt;
+
+		target_write_u32(target, 0xFFE8BC04, fmmstat & ~0x07);
+
+		/* wait for pump ready */
+		do
+		{
+			target_read_u32(target, 0xFFE8A814, &fmbptr);
+			alive_sleep(1);
+		}
+		while (!(fmbptr & 0x0200));
+
+		/* force max wait states */
+		target_read_u32(target, 0xFFE88004, &fmbac2);
+		target_write_u32(target, 0xFFE88004, fmbac2 | 0xff);
+
+		/* save current access mode, force normal read mode */
+		target_read_u32(target, 0xFFE89C00, &orig_fmregopt);
+		target_write_u32(target, 0xFFE89C00, 0x00);
+
+		for (i = 0; i < 4; i++)
+		{
+			uint32_t tmp;
+
+			/* There is no point displaying the value of tmp, it is
+			 * filtered by the chip.  The purpose of this read is to
+			 * prime the unlocking logic rather than read out the value.
+			 */
+			target_read_u32(target, 0x00001FF0 + 4 * i, &tmp);
+
+			LOG_INFO("tms470 writing fmpkey = 0x%08" PRIx32 "", key_set[i]);
+			target_write_u32(target, 0xFFE89C0C, key_set[i]);
+		}
+
+		if (ERROR_OK == tms470_check_flash_unlocked(target))
+		{
+			/*
+			 * There seems to be a side-effect of reading the FMPKEY
+			 * register in that it re-enables the protection.  So we
+			 * re-enable it.
+			 */
+			for (i = 0; i < 4; i++)
+			{
+				uint32_t tmp;
+
+				target_read_u32(target, 0x00001FF0 + 4 * i, &tmp);
+				target_write_u32(target, 0xFFE89C0C, key_set[i]);
+			}
+			retval = ERROR_OK;
+		}
+
+		/* restore settings */
+		target_write_u32(target, 0xFFE89C00, orig_fmregopt);
+		target_write_u32(target, 0xFFE88004, fmbac2);
+	}
+
+	/* clear config bit */
+	target_write_u32(target, 0xFFFFFFDC, glbctrl);
+
+	return retval;
+}
+
+/* ---------------------------------------------------------------------- */
+
+static int tms470_unlock_flash(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	const uint32_t *p_key_sets[5];
+	unsigned i, key_set_count;
+
+	if (keysSet)
+	{
+		key_set_count = 5;
+		p_key_sets[0] = flashKeys;
+		p_key_sets[1] = FLASH_KEYS_ALL_ONES;
+		p_key_sets[2] = FLASH_KEYS_ALL_ZEROS;
+		p_key_sets[3] = FLASH_KEYS_MIX1;
+		p_key_sets[4] = FLASH_KEYS_MIX2;
+	}
+	else
+	{
+		key_set_count = 4;
+		p_key_sets[0] = FLASH_KEYS_ALL_ONES;
+		p_key_sets[1] = FLASH_KEYS_ALL_ZEROS;
+		p_key_sets[2] = FLASH_KEYS_MIX1;
+		p_key_sets[3] = FLASH_KEYS_MIX2;
+	}
+
+	for (i = 0; i < key_set_count; i++)
+	{
+		if (tms470_try_flash_keys(target, p_key_sets[i]) == ERROR_OK)
+		{
+			LOG_INFO("tms470 flash is unlocked");
+			return ERROR_OK;
+		}
+	}
+
+	LOG_WARNING("tms470 could not unlock flash memory protection level 2");
+	return ERROR_FLASH_OPERATION_FAILED;
+}
+
+/* ---------------------------------------------------------------------- */
+
+static int tms470_flash_initialize_internal_state_machine(struct flash_bank *bank)
+{
+	uint32_t fmmac2, fmmac1, fmmaxep, k, delay, glbctrl, sysclk;
+	struct target *target = bank->target;
+	struct tms470_flash_bank *tms470_info = bank->driver_priv;
+	int result = ERROR_OK;
+
+	/*
+	 * Select the desired bank to be programmed by writing BANK[2:0] of
+	 * FMMAC2.
+	 */
+	target_read_u32(target, 0xFFE8BC04, &fmmac2);
+	fmmac2 &= ~0x0007;
+	fmmac2 |= (tms470_info->ordinal & 7);
+	target_write_u32(target, 0xFFE8BC04, fmmac2);
+	LOG_DEBUG("set fmmac2 = 0x%04" PRIx32 "", fmmac2);
+
+	/*
+	 * Disable level 1 sector protection by setting bit 15 of FMMAC1.
+	 */
+	target_read_u32(target, 0xFFE8BC00, &fmmac1);
+	fmmac1 |= 0x8000;
+	target_write_u32(target, 0xFFE8BC00, fmmac1);
+	LOG_DEBUG("set fmmac1 = 0x%04" PRIx32 "", fmmac1);
+
+	/*
+	 * FMTCREG = 0x2fc0;
+	 */
+	target_write_u32(target, 0xFFE8BC10, 0x2fc0);
+	LOG_DEBUG("set fmtcreg = 0x2fc0");
+
+	/*
+	 * MAXPP = 50
+	 */
+	target_write_u32(target, 0xFFE8A07C, 50);
+	LOG_DEBUG("set fmmaxpp = 50");
+
+	/*
+	 * MAXCP = 0xf000 + 2000
+	 */
+	target_write_u32(target, 0xFFE8A084, 0xf000 + 2000);
+	LOG_DEBUG("set fmmaxcp = 0x%04x", 0xf000 + 2000);
+
+	/*
+	 * configure VHV
+	 */
+	target_read_u32(target, 0xFFE8A080, &fmmaxep);
+	if (fmmaxep == 0xf000)
+	{
+		fmmaxep = 0xf000 + 4095;
+		target_write_u32(target, 0xFFE8A80C, 0x9964);
+		LOG_DEBUG("set fmptr3 = 0x9964");
+	}
+	else
+	{
+		fmmaxep = 0xa000 + 4095;
+		target_write_u32(target, 0xFFE8A80C, 0x9b64);
+		LOG_DEBUG("set fmptr3 = 0x9b64");
+	}
+	target_write_u32(target, 0xFFE8A080, fmmaxep);
+	LOG_DEBUG("set fmmaxep = 0x%04" PRIx32 "", fmmaxep);
+
+	/*
+	 * FMPTR4 = 0xa000
+	 */
+	target_write_u32(target, 0xFFE8A810, 0xa000);
+	LOG_DEBUG("set fmptr4 = 0xa000");
+
+	/*
+	 * FMPESETUP, delay parameter selected based on clock frequency.
+	 *
+	 * According to the TI App Note SPNU257 and flashing code, delay is
+	 * int((sysclk(MHz) + 1) / 2), with a minimum of 5.  The system
+	 * clock is usually derived from the ZPLL module, and selected by
+	 * the plldis global.
+	 */
+	target_read_u32(target, 0xFFFFFFDC, &glbctrl);
+	sysclk = (plldis ? 1 : (glbctrl & 0x08) ? 4 : 8) * oscMHz / (1 + (glbctrl & 7));
+	delay = (sysclk > 10) ? (sysclk + 1) / 2 : 5;
+	target_write_u32(target, 0xFFE8A018, (delay << 4) | (delay << 8));
+	LOG_DEBUG("set fmpsetup = 0x%04" PRIx32 "", (delay << 4) | (delay << 8));
+
+	/*
+	 * FMPVEVACCESS, based on delay.
+	 */
+	k = delay | (delay << 8);
+	target_write_u32(target, 0xFFE8A05C, k);
+	LOG_DEBUG("set fmpvevaccess = 0x%04" PRIx32 "", k);
+
+	/*
+	 * FMPCHOLD, FMPVEVHOLD, FMPVEVSETUP, based on delay.
+	 */
+	k <<= 1;
+	target_write_u32(target, 0xFFE8A034, k);
+	LOG_DEBUG("set fmpchold = 0x%04" PRIx32 "", k);
+	target_write_u32(target, 0xFFE8A040, k);
+	LOG_DEBUG("set fmpvevhold = 0x%04" PRIx32 "", k);
+	target_write_u32(target, 0xFFE8A024, k);
+	LOG_DEBUG("set fmpvevsetup = 0x%04" PRIx32 "", k);
+
+	/*
+	 * FMCVACCESS, based on delay.
+	 */
+	k = delay * 16;
+	target_write_u32(target, 0xFFE8A060, k);
+	LOG_DEBUG("set fmcvaccess = 0x%04" PRIx32 "", k);
+
+	/*
+	 * FMCSETUP, based on delay.
+	 */
+	k = 0x3000 | delay * 20;
+	target_write_u32(target, 0xFFE8A020, k);
+	LOG_DEBUG("set fmcsetup = 0x%04" PRIx32 "", k);
+
+	/*
+	 * FMEHOLD, based on delay.
+	 */
+	k = (delay * 20) << 2;
+	target_write_u32(target, 0xFFE8A038, k);
+	LOG_DEBUG("set fmehold = 0x%04" PRIx32 "", k);
+
+	/*
+	 * PWIDTH, CWIDTH, EWIDTH, based on delay.
+	 */
+	target_write_u32(target, 0xFFE8A050, delay * 8);
+	LOG_DEBUG("set fmpwidth = 0x%04" PRIx32 "", delay * 8);
+	target_write_u32(target, 0xFFE8A058, delay * 1000);
+	LOG_DEBUG("set fmcwidth = 0x%04" PRIx32 "", delay * 1000);
+	target_write_u32(target, 0xFFE8A054, delay * 5400);
+	LOG_DEBUG("set fmewidth = 0x%04" PRIx32 "", delay * 5400);
+
+	return result;
+}
+
+/* ---------------------------------------------------------------------- */
+
+int tms470_flash_status(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	int result = ERROR_OK;
+	uint32_t fmmstat;
+
+	target_read_u32(target, 0xFFE8BC0C, &fmmstat);
+	LOG_DEBUG("set fmmstat = 0x%04" PRIx32 "", fmmstat);
+
+	if (fmmstat & 0x0080)
+	{
+		LOG_WARNING("tms470 flash command: erase still active after busy clear.");
+		result = ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	if (fmmstat & 0x0040)
+	{
+		LOG_WARNING("tms470 flash command: program still active after busy clear.");
+		result = ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	if (fmmstat & 0x0020)
+	{
+		LOG_WARNING("tms470 flash command: invalid data command.");
+		result = ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	if (fmmstat & 0x0010)
+	{
+		LOG_WARNING("tms470 flash command: program, erase or validate sector failed.");
+		result = ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	if (fmmstat & 0x0008)
+	{
+		LOG_WARNING("tms470 flash command: voltage instability detected.");
+		result = ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	if (fmmstat & 0x0006)
+	{
+		LOG_WARNING("tms470 flash command: command suspend detected.");
+		result = ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	if (fmmstat & 0x0001)
+	{
+		LOG_WARNING("tms470 flash command: sector was locked.");
+		result = ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	return result;
+}
+
+/* ---------------------------------------------------------------------- */
+
+static int tms470_erase_sector(struct flash_bank *bank, int sector)
+{
+	uint32_t glbctrl, orig_fmregopt, fmbsea, fmbseb, fmmstat;
+	struct target *target = bank->target;
+	uint32_t flashAddr = bank->base + bank->sectors[sector].offset;
+	int result = ERROR_OK;
+
+	/*
+	 * Set the bit GLBCTRL4 of the GLBCTRL register (in the System
+	 * module) to enable writing to the flash registers }.
+	 */
+	target_read_u32(target, 0xFFFFFFDC, &glbctrl);
+	target_write_u32(target, 0xFFFFFFDC, glbctrl | 0x10);
+	LOG_DEBUG("set glbctrl = 0x%08" PRIx32 "", glbctrl | 0x10);
+
+	/* Force normal read mode. */
+	target_read_u32(target, 0xFFE89C00, &orig_fmregopt);
+	target_write_u32(target, 0xFFE89C00, 0);
+	LOG_DEBUG("set fmregopt = 0x%08x", 0);
+
+	(void)tms470_flash_initialize_internal_state_machine(bank);
+
+	/*
+	 * Select one or more bits in FMBSEA or FMBSEB to disable Level 1
+	 * protection for the particular sector to be erased/written.
+	 */
+	if (sector < 16)
+	{
+		target_read_u32(target, 0xFFE88008, &fmbsea);
+		target_write_u32(target, 0xFFE88008, fmbsea | (1 << sector));
+		LOG_DEBUG("set fmbsea = 0x%04" PRIx32 "", fmbsea | (1 << sector));
+	}
+	else
+	{
+		target_read_u32(target, 0xFFE8800C, &fmbseb);
+		target_write_u32(target, 0xFFE8800C, fmbseb | (1 << (sector - 16)));
+		LOG_DEBUG("set fmbseb = 0x%04" PRIx32 "", fmbseb | (1 << (sector - 16)));
+	}
+	bank->sectors[sector].is_protected = 0;
+
+	/*
+	 * clear status regiser, sent erase command, kickoff erase
+	 */
+	target_write_u16(target, flashAddr, 0x0040);
+	LOG_DEBUG("write *(uint16_t *)0x%08" PRIx32 "=0x0040", flashAddr);
+	target_write_u16(target, flashAddr, 0x0020);
+	LOG_DEBUG("write *(uint16_t *)0x%08" PRIx32 "=0x0020", flashAddr);
+	target_write_u16(target, flashAddr, 0xffff);
+	LOG_DEBUG("write *(uint16_t *)0x%08" PRIx32 "=0xffff", flashAddr);
+
+	/*
+	 * Monitor FMMSTAT, busy until clear, then check and other flags for
+	 * ultimate result of the operation.
+	 */
+	do
+	{
+		target_read_u32(target, 0xFFE8BC0C, &fmmstat);
+		if (fmmstat & 0x0100)
+		{
+			alive_sleep(1);
+		}
+	}
+	while (fmmstat & 0x0100);
+
+	result = tms470_flash_status(bank);
+
+	if (sector < 16)
+	{
+		target_write_u32(target, 0xFFE88008, fmbsea);
+		LOG_DEBUG("set fmbsea = 0x%04" PRIx32 "", fmbsea);
+		bank->sectors[sector].is_protected = fmbsea & (1 << sector) ? 0 : 1;
+	}
+	else
+	{
+		target_write_u32(target, 0xFFE8800C, fmbseb);
+		LOG_DEBUG("set fmbseb = 0x%04" PRIx32 "", fmbseb);
+		bank->sectors[sector].is_protected = fmbseb & (1 << (sector - 16)) ? 0 : 1;
+	}
+	target_write_u32(target, 0xFFE89C00, orig_fmregopt);
+	LOG_DEBUG("set fmregopt = 0x%08" PRIx32 "", orig_fmregopt);
+	target_write_u32(target, 0xFFFFFFDC, glbctrl);
+	LOG_DEBUG("set glbctrl = 0x%08" PRIx32 "", glbctrl);
+
+	if (result == ERROR_OK)
+	{
+		bank->sectors[sector].is_erased = 1;
+	}
+
+	return result;
+}
+
+/* ----------------------------------------------------------------------
+              Implementation of Flash Driver Interfaces
+   ---------------------------------------------------------------------- */
+
+static const struct command_registration tms470_any_command_handlers[] = {
+	{
+		.name = "flash_keyset",
+		.handler = &tms470_handle_flash_keyset_command,
+		.mode = COMMAND_ANY,
+		.help = "tms470 flash_keyset <key0> <key1> <key2> <key3>",
+	},
+	{
+		.name = "osc_megahertz",
+		.handler = &tms470_handle_osc_megahertz_command,
+		.mode = COMMAND_ANY,
+		.help = "tms470 osc_megahertz <MHz>",
+	},
+	{
+		.name = "plldis",
+		.handler = &tms470_handle_plldis_command,
+		.mode = COMMAND_ANY,
+		.help = "tms470 plldis <0/1>",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+static const struct command_registration tms470_command_handlers[] = {
+	{
+		.name = "tms470",
+		.mode = COMMAND_ANY,
+		.help = "TI tms470 flash command group",
+		.chain = tms470_any_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+/* ---------------------------------------------------------------------- */
+
+static int tms470_erase(struct flash_bank *bank, int first, int last)
+{
+	struct tms470_flash_bank *tms470_info = bank->driver_priv;
+	int sector, result = ERROR_OK;
+
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	tms470_read_part_info(bank);
+
+	if ((first < 0) || (first >= bank->num_sectors) || (last < 0) || (last >= bank->num_sectors) || (first > last))
+	{
+		LOG_ERROR("Sector range %d to %d invalid.", first, last);
+		return ERROR_FLASH_SECTOR_INVALID;
+	}
+
+	result = tms470_unlock_flash(bank);
+	if (result != ERROR_OK)
+	{
+		return result;
+	}
+
+	for (sector = first; sector <= last; sector++)
+	{
+		LOG_INFO("Erasing tms470 bank %d sector %d...", tms470_info->ordinal, sector);
+
+		result = tms470_erase_sector(bank, sector);
+
+		if (result != ERROR_OK)
+		{
+			LOG_ERROR("tms470 could not erase flash sector.");
+			break;
+		}
+		else
+		{
+			LOG_INFO("sector erased successfully.");
+		}
+	}
+
+	return result;
+}
+
+/* ---------------------------------------------------------------------- */
+
+static int tms470_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	struct tms470_flash_bank *tms470_info = bank->driver_priv;
+	struct target *target = bank->target;
+	uint32_t fmmac2, fmbsea, fmbseb;
+	int sector;
+
+	if (target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	tms470_read_part_info(bank);
+
+	if ((first < 0) || (first >= bank->num_sectors) || (last < 0) || (last >= bank->num_sectors) || (first > last))
+	{
+		LOG_ERROR("Sector range %d to %d invalid.", first, last);
+		return ERROR_FLASH_SECTOR_INVALID;
+	}
+
+	/* enable the appropriate bank */
+	target_read_u32(target, 0xFFE8BC04, &fmmac2);
+	target_write_u32(target, 0xFFE8BC04, (fmmac2 & ~7) | tms470_info->ordinal);
+
+	/* get the original sector proection flags for this bank */
+	target_read_u32(target, 0xFFE88008, &fmbsea);
+	target_read_u32(target, 0xFFE8800C, &fmbseb);
+
+	for (sector = 0; sector < bank->num_sectors; sector++)
+	{
+		if (sector < 16)
+		{
+			fmbsea = set ? fmbsea & ~(1 << sector) : fmbsea | (1 << sector);
+			bank->sectors[sector].is_protected = set ? 1 : 0;
+		}
+		else
+		{
+			fmbseb = set ? fmbseb & ~(1 << (sector - 16)) : fmbseb | (1 << (sector - 16));
+			bank->sectors[sector].is_protected = set ? 1 : 0;
+		}
+	}
+
+	/* update the protection bits */
+	target_write_u32(target, 0xFFE88008, fmbsea);
+	target_write_u32(target, 0xFFE8800C, fmbseb);
+
+	return ERROR_OK;
+}
+
+/* ---------------------------------------------------------------------- */
+
+static int tms470_write(struct flash_bank *bank, uint8_t * buffer, uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	uint32_t glbctrl, fmbac2, orig_fmregopt, fmbsea, fmbseb, fmmaxpp, fmmstat;
+	int result = ERROR_OK;
+	uint32_t i;
+
+	if (target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	tms470_read_part_info(bank);
+
+	LOG_INFO("Writing %" PRId32 " bytes starting at 0x%08" PRIx32 "", count, bank->base + offset);
+
+	/* set GLBCTRL.4  */
+	target_read_u32(target, 0xFFFFFFDC, &glbctrl);
+	target_write_u32(target, 0xFFFFFFDC, glbctrl | 0x10);
+
+	(void)tms470_flash_initialize_internal_state_machine(bank);
+
+	/* force max wait states */
+	target_read_u32(target, 0xFFE88004, &fmbac2);
+	target_write_u32(target, 0xFFE88004, fmbac2 | 0xff);
+
+	/* save current access mode, force normal read mode */
+	target_read_u32(target, 0xFFE89C00, &orig_fmregopt);
+	target_write_u32(target, 0xFFE89C00, 0x00);
+
+	/*
+	 * Disable Level 1 protection for all sectors to be erased/written.
+	 */
+	target_read_u32(target, 0xFFE88008, &fmbsea);
+	target_write_u32(target, 0xFFE88008, 0xffff);
+	target_read_u32(target, 0xFFE8800C, &fmbseb);
+	target_write_u32(target, 0xFFE8800C, 0xffff);
+
+	/* read MAXPP */
+	target_read_u32(target, 0xFFE8A07C, &fmmaxpp);
+
+	for (i = 0; i < count; i += 2)
+	{
+		uint32_t addr = bank->base + offset + i;
+		uint16_t word = (((uint16_t) buffer[i]) << 8) | (uint16_t) buffer[i + 1];
+
+		if (word != 0xffff)
+		{
+			LOG_INFO("writing 0x%04x at 0x%08" PRIx32 "", word, addr);
+
+			/* clear status register */
+			target_write_u16(target, addr, 0x0040);
+			/* program flash command */
+			target_write_u16(target, addr, 0x0010);
+			/* burn the 16-bit word (big-endian) */
+			target_write_u16(target, addr, word);
+
+			/*
+			 * Monitor FMMSTAT, busy until clear, then check and other flags
+			 * for ultimate result of the operation.
+			 */
+			do
+			{
+				target_read_u32(target, 0xFFE8BC0C, &fmmstat);
+				if (fmmstat & 0x0100)
+				{
+					alive_sleep(1);
+				}
+			}
+			while (fmmstat & 0x0100);
+
+			if (fmmstat & 0x3ff)
+			{
+				LOG_ERROR("fmstat = 0x%04" PRIx32 "", fmmstat);
+				LOG_ERROR("Could not program word 0x%04x at address 0x%08" PRIx32 ".", word, addr);
+				result = ERROR_FLASH_OPERATION_FAILED;
+				break;
+			}
+		}
+		else
+		{
+			LOG_INFO("skipping 0xffff at 0x%08" PRIx32 "", addr);
+		}
+	}
+
+	/* restore */
+	target_write_u32(target, 0xFFE88008, fmbsea);
+	target_write_u32(target, 0xFFE8800C, fmbseb);
+	target_write_u32(target, 0xFFE88004, fmbac2);
+	target_write_u32(target, 0xFFE89C00, orig_fmregopt);
+	target_write_u32(target, 0xFFFFFFDC, glbctrl);
+
+	return result;
+}
+
+/* ---------------------------------------------------------------------- */
+
+static int tms470_probe(struct flash_bank *bank)
+{
+	if (bank->target->state != TARGET_HALTED)
+	{
+		LOG_WARNING("Cannot communicate... target not halted.");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	return tms470_read_part_info(bank);
+}
+
+static int tms470_auto_probe(struct flash_bank *bank)
+{
+	struct tms470_flash_bank *tms470_info = bank->driver_priv;
+
+	if (tms470_info->device_ident_reg)
+		return ERROR_OK;
+	return tms470_probe(bank);
+}
+
+/* ---------------------------------------------------------------------- */
+
+static int tms470_erase_check(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct tms470_flash_bank *tms470_info = bank->driver_priv;
+	int sector, result = ERROR_OK;
+	uint32_t fmmac2, fmbac2, glbctrl, orig_fmregopt;
+	static uint8_t buffer[64 * 1024];
+
+	if (target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (!tms470_info->device_ident_reg)
+	{
+		tms470_read_part_info(bank);
+	}
+
+	/* set GLBCTRL.4  */
+	target_read_u32(target, 0xFFFFFFDC, &glbctrl);
+	target_write_u32(target, 0xFFFFFFDC, glbctrl | 0x10);
+
+	/* save current access mode, force normal read mode */
+	target_read_u32(target, 0xFFE89C00, &orig_fmregopt);
+	target_write_u32(target, 0xFFE89C00, 0x00);
+
+	/* enable the appropriate bank */
+	target_read_u32(target, 0xFFE8BC04, &fmmac2);
+	target_write_u32(target, 0xFFE8BC04, (fmmac2 & ~7) | tms470_info->ordinal);
+
+	/* TCR = 0 */
+	target_write_u32(target, 0xFFE8BC10, 0x2fc0);
+
+	/* clear TEZ in fmbrdy */
+	target_write_u32(target, 0xFFE88010, 0x0b);
+
+	/* save current wait states, force max */
+	target_read_u32(target, 0xFFE88004, &fmbac2);
+	target_write_u32(target, 0xFFE88004, fmbac2 | 0xff);
+
+	/*
+	 * The TI primitives inspect the flash memory by reading one 32-bit
+	 * word at a time.  Here we read an entire sector and inspect it in
+	 * an attempt to reduce the JTAG overhead.
+	 */
+	for (sector = 0; sector < bank->num_sectors; sector++)
+	{
+		if (bank->sectors[sector].is_erased != 1)
+		{
+			uint32_t i, addr = bank->base + bank->sectors[sector].offset;
+
+			LOG_INFO("checking flash bank %d sector %d", tms470_info->ordinal, sector);
+
+			target_read_buffer(target, addr, bank->sectors[sector].size, buffer);
+
+			bank->sectors[sector].is_erased = 1;
+			for (i = 0; i < bank->sectors[sector].size; i++)
+			{
+				if (buffer[i] != 0xff)
+				{
+					LOG_WARNING("tms470 bank %d, sector %d, not erased.", tms470_info->ordinal, sector);
+					LOG_WARNING("at location 0x%08" PRIx32 ": flash data is 0x%02x.", addr + i, buffer[i]);
+
+					bank->sectors[sector].is_erased = 0;
+					break;
+				}
+			}
+		}
+		if (bank->sectors[sector].is_erased != 1)
+		{
+			result = ERROR_FLASH_SECTOR_NOT_ERASED;
+			break;
+		}
+		else
+		{
+			LOG_INFO("sector erased");
+		}
+	}
+
+	/* reset TEZ, wait states, read mode, GLBCTRL.4 */
+	target_write_u32(target, 0xFFE88010, 0x0f);
+	target_write_u32(target, 0xFFE88004, fmbac2);
+	target_write_u32(target, 0xFFE89C00, orig_fmregopt);
+	target_write_u32(target, 0xFFFFFFDC, glbctrl);
+
+	return result;
+}
+
+/* ---------------------------------------------------------------------- */
+
+static int tms470_protect_check(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct tms470_flash_bank *tms470_info = bank->driver_priv;
+	int sector, result = ERROR_OK;
+	uint32_t fmmac2, fmbsea, fmbseb;
+
+	if (target->state != TARGET_HALTED)
+	{
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (!tms470_info->device_ident_reg)
+	{
+		tms470_read_part_info(bank);
+	}
+
+	/* enable the appropriate bank */
+	target_read_u32(target, 0xFFE8BC04, &fmmac2);
+	target_write_u32(target, 0xFFE8BC04, (fmmac2 & ~7) | tms470_info->ordinal);
+
+	target_read_u32(target, 0xFFE88008, &fmbsea);
+	target_read_u32(target, 0xFFE8800C, &fmbseb);
+
+	for (sector = 0; sector < bank->num_sectors; sector++)
+	{
+		int protected;
+
+		if (sector < 16)
+		{
+			protected = fmbsea & (1 << sector) ? 0 : 1;
+			bank->sectors[sector].is_protected = protected;
+		}
+		else
+		{
+			protected = fmbseb & (1 << (sector - 16)) ? 0 : 1;
+			bank->sectors[sector].is_protected = protected;
+		}
+
+		LOG_DEBUG("bank %d sector %d is %s", tms470_info->ordinal, sector, protected ? "protected" : "not protected");
+	}
+
+	return result;
+}
+
+/* ---------------------------------------------------------------------- */
+
+static int tms470_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	int used = 0;
+	struct tms470_flash_bank *tms470_info = bank->driver_priv;
+
+	if (!tms470_info->device_ident_reg)
+	{
+		tms470_read_part_info(bank);
+	}
+
+	if (!tms470_info->device_ident_reg)
+	{
+		(void)snprintf(buf, buf_size, "Cannot identify target as a TMS470\n");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	used += snprintf(buf, buf_size, "\ntms470 information: Chip is %s\n", tms470_info->part_name);
+	buf += used;
+	buf_size -= used;
+
+	used += snprintf(buf, buf_size, "Flash protection level 2 is %s\n", tms470_check_flash_unlocked(bank->target) == ERROR_OK ? "disabled" : "enabled");
+	buf += used;
+	buf_size -= used;
+
+	return ERROR_OK;
+}
+
+/* ---------------------------------------------------------------------- */
+
+/*
+ * flash bank tms470 <base> <size> <chip_width> <bus_width> <target>
+ * [options...]
+ */
+
+FLASH_BANK_COMMAND_HANDLER(tms470_flash_bank_command)
+{
+	bank->driver_priv = malloc(sizeof(struct tms470_flash_bank));
+
+	if (!bank->driver_priv)
+	{
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	(void)memset(bank->driver_priv, 0, sizeof(struct tms470_flash_bank));
+
+	return ERROR_OK;
+}
+
+struct flash_driver tms470_flash = {
+		.name = "tms470",
+		.commands = tms470_command_handlers,
+		.flash_bank_command = &tms470_flash_bank_command,
+		.erase = &tms470_erase,
+		.protect = &tms470_protect,
+		.write = &tms470_write,
+		.probe = &tms470_probe,
+		.auto_probe = &tms470_auto_probe,
+		.erase_check = &tms470_erase_check,
+		.protect_check = &tms470_protect_check,
+		.info = &tms470_info,
+	};
diff --git a/src/flash/nor/tms470.h b/src/flash/nor/tms470.h
new file mode 100644
index 00000000..f275e510
--- /dev/null
+++ b/src/flash/nor/tms470.h
@@ -0,0 +1,39 @@
+/***************************************************************************
+ *   Copyright (C) 2007,2008 by Christopher Kilgour                        *
+ *   techie |_at_| whiterocker |_dot_| com                                 *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifndef TMS470_DOT_H
+#define TMS470_DOT_H
+
+#include "flash.h"
+
+struct tms470_flash_bank
+{
+	unsigned ordinal;
+
+	/* device identification register */
+	uint32_t device_ident_reg;
+	uint32_t silicon_version;
+	uint32_t technology_family;
+	uint32_t rom_flash;
+	uint32_t part_number;
+	char * part_name;
+
+};
+
+#endif /* TMS470_DOT_H */