- "flash write_binary" is now "flash write_bank" to clarify the focus of the

 command and reduce confusion with "flash write_image".
- retired deprecated "flash erase" & "flash write".
- added flash_driver_protect/write/erase() that are wafer thin frontend
 functions to low level driver functions. They implement checks
 that were inconsistently handled by the drivers, e.g. check for
 target halted was done in a spotty fashion.
- use return ERROR_COMMAND_SYNTAX_ERROR to print out
 syntax of command instead of having lots of inlined replicas of
 the command line syntax(some of which were wrong).
- use logging instead of dubious translation of error values to
 human understandable explanations of why things failed.
 The lower levels log the precise reason and the higher
 levels can ammend context as the error propagates up
 the call stack.
- simplified flash API slightly with logging instead of
 allocating and returning information that the caller then
 has to translate into print statements.



git-svn-id: svn://svn.berlios.de/openocd/trunk@337 b42882b7-edfa-0310-969c-e2dbd0fdcd60

12 files changed, 9839 insertions, 10020 deletions

diff --git a/src/flash/at91sam7.c b/src/flash/at91sam7.c
index f552900d..3e920e3d 100644
--- a/src/flash/at91sam7.c
+++ b/src/flash/at91sam7.c
@@ -1,981 +1,976 @@
-/***************************************************************************
- *   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.             *
- ***************************************************************************/
-
-/***************************************************************************
-There are some things to notice
-
-* AT91SAM7S64 is tested
-* All AT91SAM7Sxx  and  AT91SAM7Xxx should work but is not tested
-* All parameters are identified from onchip configuartion registers 
-*
-* The flash controller handles erases automatically on a page (128/265 byte) basis
-* Only an EraseAll command is supported by the controller
-* Partial erases can be implemented in software by writing one 0xFFFFFFFF word to 
-* some location in every page in the region to be erased
-*  
-* Lock regions (sectors) are 32 or 64 pages
-*
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include "replacements.h"
-
-#include "at91sam7.h"
-
-#include "flash.h"
-#include "target.h"
-#include "log.h"
-#include "binarybuffer.h"
-#include "types.h"
-
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-
-int at91sam7_register_commands(struct command_context_s *cmd_ctx);
-int at91sam7_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);
-int at91sam7_erase(struct flash_bank_s *bank, int first, int last);
-int at91sam7_protect(struct flash_bank_s *bank, int set, int first, int last);
-int at91sam7_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);
-int at91sam7_probe(struct flash_bank_s *bank);
-int at91sam7_auto_probe(struct flash_bank_s *bank);
-int at91sam7_erase_check(struct flash_bank_s *bank);
-int at91sam7_protect_check(struct flash_bank_s *bank);
-int at91sam7_info(struct flash_bank_s *bank, char *buf, int buf_size);
-
-u32 at91sam7_get_flash_status(flash_bank_t *bank, u8 flashplane);
-void at91sam7_set_flash_mode(flash_bank_t *bank, u8 flashplane, int mode);
-u32 at91sam7_wait_status_busy(flash_bank_t *bank, u8 flashplane, u32 waitbits, int timeout);
-int at91sam7_flash_command(struct flash_bank_s *bank, u8 flashplane, u8 cmd, u16 pagen); 
-int at91sam7_handle_gpnvm_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-flash_driver_t at91sam7_flash =
-{
-	.name = "at91sam7",
-	.register_commands = at91sam7_register_commands,
-	.flash_bank_command = at91sam7_flash_bank_command,
-	.erase = at91sam7_erase,
-	.protect = at91sam7_protect,
-	.write = at91sam7_write,
-	.probe = at91sam7_probe,
-	.auto_probe = at91sam7_auto_probe,
-	.erase_check = at91sam7_erase_check,
-	.protect_check = at91sam7_protect_check,
-	.info = at91sam7_info
-};
-
-u32 MC_FMR[4] =	{ 0xFFFFFF60, 0xFFFFFF70, 0xFFFFFF80, 0xFFFFFF90 };
-u32 MC_FCR[4] =	{ 0xFFFFFF64, 0xFFFFFF74, 0xFFFFFF84, 0xFFFFFF94 };
-u32 MC_FSR[4] =	{ 0xFFFFFF68, 0xFFFFFF78, 0xFFFFFF88, 0xFFFFFF98 };
-
-char * EPROC[8]= {"Unknown","ARM946-E","ARM7TDMI","Unknown","ARM920T","ARM926EJ-S","Unknown","Unknown"};
-long NVPSIZ[16] = {
-   0,
-   0x2000, /*  8K */
-   0x4000, /* 16K */ 
-   0x8000, /* 32K */
-   -1,
-   0x10000, /* 64K */
-   -1,
-   0x20000, /* 128K */
-   -1,
-   0x40000, /* 256K */
-   0x80000, /* 512K */
-   -1,
-   0x100000, /* 1024K */
-   -1,
-   0x200000, /* 2048K */
-   -1
-};
-
-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 */
-};
-
-int at91sam7_register_commands(struct command_context_s *cmd_ctx)
-{
-	command_t *at91sam7_cmd = register_command(cmd_ctx, NULL, "at91sam7", NULL, COMMAND_ANY, NULL);
-	register_command(cmd_ctx, at91sam7_cmd, "gpnvm", at91sam7_handle_gpnvm_command, COMMAND_EXEC,
-			"at91sam7 gpnvm <num> <bit> set|clear, set or clear at91sam7 gpnvm bit");
-
-	return ERROR_OK;
-}
-
-u32 at91sam7_get_flash_status(flash_bank_t *bank, u8 flashplane)
-{
-	target_t *target = bank->target;
-	u32 fsr;
-	
-	target_read_u32(target, MC_FSR[flashplane], &fsr);
-	
-	return fsr;
-}
-
-/** Read clock configuration and set at91sam7_info->usec_clocks*/ 
-void at91sam7_read_clock_info(flash_bank_t *bank)
-{
-	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;
-	target_t *target = bank->target;
-	u32 mckr, mcfr, pllr;
-	unsigned long tmp = 0, mainfreq;
-	int flashplane;
-
-	/* Read main clock freqency 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;
-	switch (mckr & PMC_MCKR_CSS) 
-	{
-		case 0:			/* Slow Clock */
-			at91sam7_info->mck_valid = 1;
-			mainfreq = RC_FREQ / 16ul * (mcfr & 0xffff);
-			tmp = mainfreq;
-			break;
-		case 1:			/* Main Clock */
-			if (mcfr & CKGR_MCFR_MAINRDY) 
-			{
-				at91sam7_info->mck_valid = 1;
-				mainfreq = RC_FREQ / 16ul * (mcfr & 0xffff);
-				tmp = mainfreq;
-			}
-			break;
-
-		case 2:			/* Reserved */
-			break;
-		case 3:		/* PLL Clock */
-			if (mcfr & CKGR_MCFR_MAINRDY) 
-			{
-				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);
-			}
-			break;
-	}
-	
-	/* Prescaler adjust */
-	if (((mckr & PMC_MCKR_PRES) >> 2) == 7)
-		at91sam7_info->mck_valid = 0;
-	else
-		at91sam7_info->mck_freq = tmp >> ((mckr & PMC_MCKR_PRES) >> 2);
-
-	/* Forget old flash timing */
-	for (flashplane = 0; flashplane<at91sam7_info->num_planes; flashplane++)
-	{
-		at91sam7_set_flash_mode(bank, flashplane, FMR_TIMING_NONE);
-	}
-}
-
-/* Setup the timimg registers for nvbits or normal flash */
-void at91sam7_set_flash_mode(flash_bank_t *bank, u8 flashplane, int mode)
-{
-	u32 fmr, fmcn = 0, fws = 0;
-	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;
-	target_t *target = bank->target;
-	
-	if (mode && (mode != at91sam7_info->flashmode[flashplane]))
-	{
-		/* 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/666666ul)+1;
-
-		/* 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;
-
-		DEBUG("fmcn[%i]: %i", flashplane, fmcn); 
-		fmr = fmcn << 16 | fws << 8;
-		target_write_u32(target, MC_FMR[flashplane], fmr);
-	}
-	
-	at91sam7_info->flashmode[flashplane] = mode;		
-}
-
-u32 at91sam7_wait_status_busy(flash_bank_t *bank, u8 flashplane, u32 waitbits, int timeout)
-{
-	u32 status;
-	
-	while ((!((status = at91sam7_get_flash_status(bank,flashplane)) & waitbits)) && (timeout-- > 0))
-	{
-		DEBUG("status[%i]: 0x%x", flashplane, status);
-		usleep(1000);
-	}
-	
-	DEBUG("status[%i]: 0x%x", flashplane, status);
-
-	if (status & 0x0C)
-	{
-		ERROR("status register: 0x%x", status);
-		if (status & 0x4)
-			ERROR("Lock Error Bit Detected, Operation Abort");
-		if (status & 0x8)
-			ERROR("Invalid command and/or bad keyword, Operation Abort");
-		if (status & 0x10)
-			ERROR("Security Bit Set, Operation Abort");
-	}
-	
-	return status;
-}
-
-
-/* Send one command to the AT91SAM flash controller */
-int at91sam7_flash_command(struct flash_bank_s *bank, u8 flashplane, u8 cmd, u16 pagen) 
-{
-	u32 fcr;
-	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;
-	target_t *target = bank->target;
-
-	fcr = (0x5A<<24) | ((pagen&0x3FF)<<8) | cmd; 
-	target_write_u32(target, MC_FCR[flashplane], fcr);
-	DEBUG("Flash command: 0x%x, flashplane: %i, pagenumber:%u", fcr, flashplane, 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, flashplane, MC_FSR_EOL, 10)&0x0C) 
-		{
-			return ERROR_FLASH_OPERATION_FAILED;
-		}
-		return ERROR_OK;
-	}
-
-	if (at91sam7_wait_status_busy(bank, flashplane, 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 */
-int at91sam7_read_part_info(struct flash_bank_s *bank)
-{
-	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;
-	target_t *target = bank->target;
-	u32 cidr, status;
-	int sectornum;
-	
-	if (bank->target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	
-	/* Read and parse chip identification register */
-	target_read_u32(target, DBGU_CIDR, &cidr);
-	
-	if (cidr == 0)
-	{
-		WARNING("Cannot identify target as an AT91SAM");
-		return ERROR_FLASH_OPERATION_FAILED;
-	}
-	
-	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;
-	bank->size = NVPSIZ[at91sam7_info->cidr_nvpsiz];
-	at91sam7_info->target_name = "Unknown";
-
-	/* Support just for bulk erase of a single flash plane, whole device if flash size <= 256k */
-	if (NVPSIZ[at91sam7_info->cidr_nvpsiz]<0x80000)  /* Flash size less than 512K, one flash plane */
-	{
-		bank->num_sectors = 1;
-		bank->sectors = malloc(sizeof(flash_sector_t));
-		bank->sectors[0].offset = 0;
-		bank->sectors[0].size = bank->size;
-		bank->sectors[0].is_erased = -1;
-		bank->sectors[0].is_protected = -1;
-	}
-	else	/* Flash size 512K or larger, several flash planes */
-	{
-		bank->num_sectors = NVPSIZ[at91sam7_info->cidr_nvpsiz]/0x40000;
-		bank->sectors = malloc(bank->num_sectors*sizeof(flash_sector_t));
-		for (sectornum=0; sectornum<bank->num_sectors; sectornum++)
-		{
-			bank->sectors[sectornum].offset = sectornum*0x40000;
-			bank->sectors[sectornum].size = 0x40000;
-			bank->sectors[sectornum].is_erased = -1;
-			bank->sectors[sectornum].is_protected = -1;
-		}
-	}
-		
-	
-
-	DEBUG("nvptyp: 0x%3.3x, arch: 0x%4.4x", at91sam7_info->cidr_nvptyp, at91sam7_info->cidr_arch );
-
-	/* Read main and master clock freqency register */
-	at91sam7_read_clock_info(bank);
-	
-	at91sam7_info->num_planes = 1;
-	status = at91sam7_get_flash_status(bank, 0);
-	at91sam7_info->securitybit = (status>>4)&0x01;
-	at91sam7_protect_check(bank);   /* TODO Check the protect check */
-	
-	if (at91sam7_info->cidr_arch == 0x70 )
-	{
-		at91sam7_info->num_nvmbits = 2;
-		at91sam7_info->nvmbits = (status>>8)&0x03;
-		bank->base = 0x100000;
-		bank->bus_width = 4;
-		if (bank->size==0x80000)  /* AT91SAM7S512 */
-		{
-			at91sam7_info->target_name = "AT91SAM7S512";
-			at91sam7_info->num_planes = 2;
-			if (at91sam7_info->num_planes != bank->num_sectors)
-				WARNING("Internal error: Number of flash planes and erase sectors does not match, please report");;
-			at91sam7_info->num_lockbits = 2*16;
-			at91sam7_info->pagesize = 256;
-			at91sam7_info->pages_in_lockregion = 64;
-			at91sam7_info->num_pages = 2*16*64;
-		}
-		if (bank->size==0x40000)  /* AT91SAM7S256 */
-		{
-			at91sam7_info->target_name = "AT91SAM7S256";
-			at91sam7_info->num_lockbits = 16;
-			at91sam7_info->pagesize = 256;
-			at91sam7_info->pages_in_lockregion = 64;
-			at91sam7_info->num_pages = 16*64;
-		}
-		if (bank->size==0x20000)  /* AT91SAM7S128 */
-		{
-			at91sam7_info->target_name = "AT91SAM7S128";
-			at91sam7_info->num_lockbits = 8;
-			at91sam7_info->pagesize = 256;
-			at91sam7_info->pages_in_lockregion = 64;
-			at91sam7_info->num_pages = 8*64;
-		}
-		if (bank->size==0x10000)  /* AT91SAM7S64 */
-		{
-			at91sam7_info->target_name = "AT91SAM7S64";
-			at91sam7_info->num_lockbits = 16;
-			at91sam7_info->pagesize = 128;
-			at91sam7_info->pages_in_lockregion = 32;
-			at91sam7_info->num_pages = 16*32;
-		}
-		if (bank->size==0x08000)  /* AT91SAM7S321/32 */
-		{
-			at91sam7_info->target_name = "AT91SAM7S321/32";
-			at91sam7_info->num_lockbits = 8;
-			at91sam7_info->pagesize = 128;
-			at91sam7_info->pages_in_lockregion = 32;
-			at91sam7_info->num_pages = 8*32;
-		}
-		
-		return ERROR_OK;
-	}
-
-	if (at91sam7_info->cidr_arch == 0x71 )
-	{
-		at91sam7_info->num_nvmbits = 3;
-		at91sam7_info->nvmbits = (status>>8)&0x07;
-		bank->base = 0x100000;
-		bank->bus_width = 4;
-		if (bank->size==0x80000)  /* AT91SAM7XC512 */
-		{
-			at91sam7_info->target_name = "AT91SAM7XC512";
-			at91sam7_info->num_planes = 2;
-			if (at91sam7_info->num_planes != bank->num_sectors)
-				WARNING("Internal error: Number of flash planes and erase sectors does not match, please report");;
-			at91sam7_info->num_lockbits = 2*16;
-			at91sam7_info->pagesize = 256;
-			at91sam7_info->pages_in_lockregion = 64;
-			at91sam7_info->num_pages = 2*16*64;
-		}
-		if (bank->size==0x40000)  /* AT91SAM7XC256 */
-		{
-			at91sam7_info->target_name = "AT91SAM7XC256";
-			at91sam7_info->num_lockbits = 16;
-			at91sam7_info->pagesize = 256;
-			at91sam7_info->pages_in_lockregion = 64;
-			at91sam7_info->num_pages = 16*64;
-		}
-		if (bank->size==0x20000)  /* AT91SAM7XC128 */
-		{
-			at91sam7_info->target_name = "AT91SAM7XC128";
-			at91sam7_info->num_lockbits = 8;
-			at91sam7_info->pagesize = 256;
-			at91sam7_info->pages_in_lockregion = 64;
-			at91sam7_info->num_pages = 8*64;
-		}
-		
-		return ERROR_OK;
-	}
-	
-	if (at91sam7_info->cidr_arch == 0x72 )
-	{
-		at91sam7_info->num_nvmbits = 3;
-		at91sam7_info->nvmbits = (status>>8)&0x07;
-		bank->base = 0x100000;
-		bank->bus_width = 4;
-		if (bank->size==0x80000) /* AT91SAM7SE512 */
-		{
-			at91sam7_info->target_name = "AT91SAM7SE512";
-			at91sam7_info->num_planes = 2;
-			if (at91sam7_info->num_planes != bank->num_sectors)
-				WARNING("Internal error: Number of flash planes and erase sectors does not match, please report");;
-			at91sam7_info->num_lockbits = 32;
-			at91sam7_info->pagesize = 256;
-			at91sam7_info->pages_in_lockregion = 64;
-			at91sam7_info->num_pages = 32*64;
-		}
-		if (bank->size==0x40000)
-		{
-			at91sam7_info->target_name = "AT91SAM7SE256";
-			at91sam7_info->num_lockbits = 16;
-			at91sam7_info->pagesize = 256;
-			at91sam7_info->pages_in_lockregion = 64;
-			at91sam7_info->num_pages = 16*64;
-		}
-		if (bank->size==0x08000)
-		{
-			at91sam7_info->target_name = "AT91SAM7SE32";
-			at91sam7_info->num_lockbits = 8;
-			at91sam7_info->pagesize = 128;
-			at91sam7_info->pages_in_lockregion = 32;
-			at91sam7_info->num_pages = 8*32;
-		}
-		
-		return ERROR_OK;
-	}
-	
-	if (at91sam7_info->cidr_arch == 0x75 )
-	{
-		at91sam7_info->num_nvmbits = 3;
-		at91sam7_info->nvmbits = (status>>8)&0x07;
-		bank->base = 0x100000;
-		bank->bus_width = 4;
-		if (bank->size==0x80000)  /* AT91SAM7X512 */
-		{
-			at91sam7_info->target_name = "AT91SAM7X512";
-			at91sam7_info->num_planes = 2;
-			if (at91sam7_info->num_planes != bank->num_sectors)
-				WARNING("Internal error: Number of flash planes and erase sectors does not match, please report");;
-			at91sam7_info->num_lockbits = 32;
-			at91sam7_info->pagesize = 256;
-			at91sam7_info->pages_in_lockregion = 64;
-			at91sam7_info->num_pages = 2*16*64;
-			DEBUG("Support for AT91SAM7X512 is experimental in this version!");
-		}
-		if (bank->size==0x40000)  /* AT91SAM7X256 */
-		{
-			at91sam7_info->target_name = "AT91SAM7X256";
-			at91sam7_info->num_lockbits = 16;
-			at91sam7_info->pagesize = 256;
-			at91sam7_info->pages_in_lockregion = 64;
-			at91sam7_info->num_pages = 16*64;
-		}
-		if (bank->size==0x20000)  /* AT91SAM7X128 */
-		{
-			at91sam7_info->target_name = "AT91SAM7X128";
-			at91sam7_info->num_lockbits = 8;
-			at91sam7_info->pagesize = 256;
-			at91sam7_info->pages_in_lockregion = 64;
-			at91sam7_info->num_pages = 8*64;
-		}
-	
-		return ERROR_OK;
-	}
-	
-	if (at91sam7_info->cidr_arch == 0x60 )
-	{
-		at91sam7_info->num_nvmbits = 3;
-		at91sam7_info->nvmbits = (status>>8)&0x07;
-		bank->base = 0x100000;
-		bank->bus_width = 4;
-		
-		if (bank->size == 0x40000)  /* AT91SAM7A3 */
-		{
-			at91sam7_info->target_name = "AT91SAM7A3";
-			at91sam7_info->num_lockbits = 16;
-			at91sam7_info->pagesize = 256;
-			at91sam7_info->pages_in_lockregion = 16;
-			at91sam7_info->num_pages = 16*64;
-		}
-		return ERROR_OK;
-	}
-	
-   WARNING("at91sam7 flash only tested for AT91SAM7Sxx series");
-	
-   return ERROR_OK;
-}
-
-int at91sam7_erase_check(struct flash_bank_s *bank)
-{
-	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;
-	
-	if (!at91sam7_info->working_area_size)
-	{
-	}
-	else
-	{	
-	}
-	
-	return ERROR_OK;
-}
-
-int at91sam7_protect_check(struct flash_bank_s *bank)
-{
-	u32 status;
-	int flashplane;
-	
-	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;
-
-	if (at91sam7_info->cidr == 0)
-	{
-		at91sam7_read_part_info(bank);
-	}
-
-	if (at91sam7_info->cidr == 0)
-	{
-		WARNING("Cannot identify target as an AT91SAM");
-		return ERROR_FLASH_OPERATION_FAILED;
-	}
-
-	for (flashplane=0;flashplane<at91sam7_info->num_planes;flashplane++)
-	{
-		status = at91sam7_get_flash_status(bank, flashplane);
-		at91sam7_info->lockbits[flashplane] = (status >> 16);
-	}
-	
-	return ERROR_OK;
-}
-
-/* flash_bank at91sam7 0 0 0 0 <target#>
- */
-int at91sam7_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)
-{
-	at91sam7_flash_bank_t *at91sam7_info;
-	int i;
-	
-	if (argc < 6)
-	{
-		WARNING("incomplete flash_bank at91sam7 configuration");
-		return ERROR_FLASH_BANK_INVALID;
-	}
-	
-	at91sam7_info = malloc(sizeof(at91sam7_flash_bank_t));
-	bank->driver_priv = at91sam7_info;
-	at91sam7_info->probed = 0;
-	
-	/* part wasn't probed for info yet */
-	at91sam7_info->cidr = 0;
-	for (i=0;i<4;i++)
-		at91sam7_info->flashmode[i]=0;
-	
-	return ERROR_OK;
-}
-
-int at91sam7_erase(struct flash_bank_s *bank, int first, int last)
-{
-	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;
-	u8 flashplane;
-
-	if (bank->target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	
-	if (at91sam7_info->cidr == 0)
-	{
-		at91sam7_read_part_info(bank);
-	}
-
-	if (at91sam7_info->cidr == 0)
-	{
-		WARNING("Cannot identify target as an AT91SAM");
-		return ERROR_FLASH_OPERATION_FAILED;
-	}	
-	
-	if ((first < 0) || (last < first) || (last >= bank->num_sectors))
-	{
-		if ((first == 0) && (last == (at91sam7_info->num_lockbits-1)))
-		{
-			WARNING("Sector numbers based on lockbit count, probably a deprecated script");
-			last = bank->num_sectors-1;
-		}
-		else return ERROR_FLASH_SECTOR_INVALID;
-	}
-
-	/* Configure the flash controller timing */
-	at91sam7_read_clock_info(bank);	
-	for (flashplane = first; flashplane<=last; flashplane++)
-	{
-		/* Configure the flash controller timing */
-		at91sam7_set_flash_mode(bank, flashplane, FMR_TIMING_FLASH);
-		if (at91sam7_flash_command(bank, flashplane, EA, 0) != ERROR_OK) 
-		{
-			return ERROR_FLASH_OPERATION_FAILED;
-		}	
-	}
-	return ERROR_OK;
-
-}
-
-int at91sam7_protect(struct flash_bank_s *bank, int set, int first, int last)
-{
-	u32 cmd, pagen;
-	u8 flashplane;
-	int lockregion;
-	
-	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;
-	
-	if (bank->target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	
-	if ((first < 0) || (last < first) || (last >= at91sam7_info->num_lockbits))
-	{
-		return ERROR_FLASH_SECTOR_INVALID;
-	}
-	
-	if (at91sam7_info->cidr == 0)
-	{
-		at91sam7_read_part_info(bank);
-	}
-
-	if (at91sam7_info->cidr == 0)
-	{
-		WARNING("Cannot identify target as an AT91SAM");
-		return ERROR_FLASH_OPERATION_FAILED;
-	}
-	
-	at91sam7_read_clock_info(bank);	
-	
-	for (lockregion=first;lockregion<=last;lockregion++) 
-	{
-		pagen = lockregion*at91sam7_info->pages_in_lockregion;
-		flashplane = (pagen>>10)&0x03;
-		/* Configure the flash controller timing */
-		at91sam7_set_flash_mode(bank, flashplane, FMR_TIMING_NVBITS);
-		
-		if (set)
-			 cmd = SLB; 
-		else
-			 cmd = CLB; 		
-
-		if (at91sam7_flash_command(bank, flashplane, cmd, pagen) != ERROR_OK) 
-		{
-			return ERROR_FLASH_OPERATION_FAILED;
-		}	
-	}
-	
-	at91sam7_protect_check(bank);
-		
-	return ERROR_OK;
-}
-
-
-int at91sam7_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
-{
-	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;
-	target_t *target = bank->target;
-	u32 dst_min_alignment, wcount, bytes_remaining = count;
-	u32 first_page, last_page, pagen, buffer_pos;
-	u8 flashplane;
-	
-	if (bank->target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	
-	if (at91sam7_info->cidr == 0)
-	{
-		at91sam7_read_part_info(bank);
-	}
-
-	if (at91sam7_info->cidr == 0)
-	{
-		WARNING("Cannot identify target as an AT91SAM");
-		return ERROR_FLASH_OPERATION_FAILED;
-	}
-	
-	if (offset + count > bank->size)
-		return ERROR_FLASH_DST_OUT_OF_BANK;
-	
-	dst_min_alignment = at91sam7_info->pagesize;
-
-	if (offset % dst_min_alignment)
-	{
-		WARNING("offset 0x%x breaks required alignment 0x%x", 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 = CEIL(offset + count, dst_min_alignment);
-	
-	DEBUG("first_page: %i, last_page: %i, count %i", first_page, last_page, count);
-	
-	at91sam7_read_clock_info(bank);	
-
-	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 = CEIL(count,4);
-		target->type->write_memory(target, bank->base+pagen*dst_min_alignment, 4, wcount, buffer+buffer_pos);
-		flashplane = (pagen>>10)&0x3;
-		
-		/* Configure the flash controller timing */	
-		at91sam7_set_flash_mode(bank, flashplane, FMR_TIMING_FLASH);
-		/* Send Write Page command to Flash Controller */
-		if (at91sam7_flash_command(bank, flashplane, WP, pagen) != ERROR_OK) 
-		{
-				return ERROR_FLASH_OPERATION_FAILED;
-		}
-		DEBUG("Write flash plane:%i page number:%i", flashplane, pagen);
-	}
-	
-	return ERROR_OK;
-}
-
-
-int at91sam7_probe(struct flash_bank_s *bank)
-{
-	/* we can't probe on an at91sam7
-	 * if this is an at91sam7, it has the configured flash
-	 */
-	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;
-	at91sam7_info->probed = 0;
-	
-	if (at91sam7_info->cidr == 0)
-	{
-		at91sam7_read_part_info(bank);
-	}
-
-	if (at91sam7_info->cidr == 0)
-	{
-		WARNING("Cannot identify target as an AT91SAM");
-		return ERROR_FLASH_OPERATION_FAILED;
-	}
-	
-	at91sam7_info->probed = 1;
-	
-	return ERROR_OK;
-}
-
-
-int at91sam7_auto_probe(struct flash_bank_s *bank)
-{
-	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;
-	if (at91sam7_info->probed)
-		return ERROR_OK;
-	return at91sam7_probe(bank);
-}
-
-int at91sam7_info(struct flash_bank_s *bank, char *buf, int buf_size)
-{
-	int printed, flashplane;
-	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;
-	
-	at91sam7_read_part_info(bank);
-
-	if (at91sam7_info->cidr == 0)
-	{
-		printed = snprintf(buf, buf_size, "Cannot identify target as an AT91SAM\n");
-		buf += printed;
-		buf_size -= printed;
-		return ERROR_FLASH_OPERATION_FAILED;
-	}
-	
-	printed = snprintf(buf, buf_size, "\nat91sam7 information: Chip is %s\n",at91sam7_info->target_name);
-	buf += printed;
-	buf_size -= printed;
-	
-	printed = snprintf(buf, buf_size, "cidr: 0x%8.8x, arch: 0x%4.4x, eproc: %s, version:0x%3.3x,  flashsize: 0x%8.8x\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): %ikHz \n", at91sam7_info->mck_freq / 1000);
-	buf += printed;
-	buf_size -= printed;
-	
-	if (at91sam7_info->num_planes>1) {		
-		printed = snprintf(buf, buf_size, "flashplanes: %i, pagesize: %i, lock regions: %i, pages in lock region: %i \n", 
-			   at91sam7_info->num_planes, at91sam7_info->pagesize, at91sam7_info->num_lockbits, at91sam7_info->num_pages/at91sam7_info->num_lockbits);
-		buf += printed;
-		buf_size -= printed;
-		for (flashplane=0; flashplane<at91sam7_info->num_planes; flashplane++)
-		{
-			printed = snprintf(buf, buf_size, "lockbits[%i]: 0x%4.4x,  ", flashplane, at91sam7_info->lockbits[flashplane]);
-			buf += printed;
-			buf_size -= printed;
-		}
-	}
-	else
-	if (at91sam7_info->num_lockbits>0) {		
-		printed = snprintf(buf, buf_size, "pagesize: %i, lockbits: %i 0x%4.4x, pages in lock region: %i \n", 
-			   at91sam7_info->pagesize, at91sam7_info->num_lockbits, at91sam7_info->lockbits[0], at91sam7_info->num_pages/at91sam7_info->num_lockbits);
-		buf += printed;
-		buf_size -= printed;
-	}
-			
-	printed = snprintf(buf, buf_size, "securitybit: %i, nvmbits: 0x%1.1x\n", at91sam7_info->securitybit, at91sam7_info->nvmbits);
-	buf += printed;
-	buf_size -= printed;
-
-	return ERROR_OK;
-}
-
-/* 
-* On AT91SAM7S: When the gpnvm bits are set with 
-* > at91sam7 gpnvm 0 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.
-*/
-int at91sam7_handle_gpnvm_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	flash_bank_t *bank;
-	int bit;
-	u8  flashcmd;
-	u32 status;
-	char *value;
-	at91sam7_flash_bank_t *at91sam7_info;
-
-	if (argc < 3)
-	{
-		command_print(cmd_ctx, "at91sam7 gpnvm <num> <bit> <set|clear>");
-		return ERROR_OK;
-	}
-	
-	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-	bit = atoi(args[1]);
-	value = args[2];
-
-	if (!bank)
-	{
-		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-		return ERROR_OK;
-	}
-
-	at91sam7_info = bank->driver_priv;
-
-	if (bank->target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	
-	if (at91sam7_info->cidr == 0)
-	{
-		at91sam7_read_part_info(bank);
-	}
-
-	if (at91sam7_info->cidr == 0)
-	{
-		WARNING("Cannot identify target as an AT91SAM");
-		return ERROR_FLASH_OPERATION_FAILED;
-	}
-
-	if ((bit<0) || (at91sam7_info->num_nvmbits <= bit))
-	{ 
-		command_print(cmd_ctx, "gpnvm bit '#%s' is out of bounds for target %s", args[1],at91sam7_info->target_name);
-		return ERROR_OK;
-	}
-
-	if (strcmp(value, "set") == 0)
-	{
-		flashcmd = SGPB;
-	}
-	else if (strcmp(value, "clear") == 0)
-	{
-		flashcmd = CGPB;
-	}
-	else
-	{
-		command_print(cmd_ctx, "usage: at91sam7 gpnvm <num> <bit> <set|clear>");
-		return ERROR_OK;
-	}
-
-	/* Configure the flash controller timing */
-	at91sam7_read_clock_info(bank);	
-	at91sam7_set_flash_mode(bank, 0, FMR_TIMING_NVBITS);
-	
-	if (at91sam7_flash_command(bank, 0, flashcmd, (u16)(bit)) != ERROR_OK) 
-	{
-		return ERROR_FLASH_OPERATION_FAILED;
-	}	
-
-	status = at91sam7_get_flash_status(bank, 0);
-	DEBUG("at91sam7_handle_gpnvm_command: cmd 0x%x, value 0x%x, status 0x%x \n",flashcmd,bit,status);
-	at91sam7_info->nvmbits = (status>>8)&((1<<at91sam7_info->num_nvmbits)-1);
-
-	return ERROR_OK;
-}
+/***************************************************************************

+ *   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.             *

+ ***************************************************************************/

+

+/***************************************************************************

+There are some things to notice

+

+* AT91SAM7S64 is tested

+* All AT91SAM7Sxx  and  AT91SAM7Xxx should work but is not tested

+* All parameters are identified from onchip configuartion registers 

+*

+* The flash controller handles erases automatically on a page (128/265 byte) basis

+* Only an EraseAll command is supported by the controller

+* Partial erases can be implemented in software by writing one 0xFFFFFFFF word to 

+* some location in every page in the region to be erased

+*  

+* Lock regions (sectors) are 32 or 64 pages

+*

+ ***************************************************************************/

+#ifdef HAVE_CONFIG_H

+#include "config.h"

+#endif

+

+#include "replacements.h"

+

+#include "at91sam7.h"

+

+#include "flash.h"

+#include "target.h"

+#include "log.h"

+#include "binarybuffer.h"

+#include "types.h"

+

+#include <stdlib.h>

+#include <string.h>

+#include <unistd.h>

+

+int at91sam7_register_commands(struct command_context_s *cmd_ctx);

+int at91sam7_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);

+int at91sam7_erase(struct flash_bank_s *bank, int first, int last);

+int at91sam7_protect(struct flash_bank_s *bank, int set, int first, int last);

+int at91sam7_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);

+int at91sam7_probe(struct flash_bank_s *bank);

+int at91sam7_auto_probe(struct flash_bank_s *bank);

+int at91sam7_erase_check(struct flash_bank_s *bank);

+int at91sam7_protect_check(struct flash_bank_s *bank);

+int at91sam7_info(struct flash_bank_s *bank, char *buf, int buf_size);

+

+u32 at91sam7_get_flash_status(flash_bank_t *bank, u8 flashplane);

+void at91sam7_set_flash_mode(flash_bank_t *bank, u8 flashplane, int mode);

+u32 at91sam7_wait_status_busy(flash_bank_t *bank, u8 flashplane, u32 waitbits, int timeout);

+int at91sam7_flash_command(struct flash_bank_s *bank, u8 flashplane, u8 cmd, u16 pagen); 

+int at91sam7_handle_gpnvm_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+

+flash_driver_t at91sam7_flash =

+{

+	.name = "at91sam7",

+	.register_commands = at91sam7_register_commands,

+	.flash_bank_command = at91sam7_flash_bank_command,

+	.erase = at91sam7_erase,

+	.protect = at91sam7_protect,

+	.write = at91sam7_write,

+	.probe = at91sam7_probe,

+	.auto_probe = at91sam7_auto_probe,

+	.erase_check = at91sam7_erase_check,

+	.protect_check = at91sam7_protect_check,

+	.info = at91sam7_info

+};

+

+u32 MC_FMR[4] =	{ 0xFFFFFF60, 0xFFFFFF70, 0xFFFFFF80, 0xFFFFFF90 };

+u32 MC_FCR[4] =	{ 0xFFFFFF64, 0xFFFFFF74, 0xFFFFFF84, 0xFFFFFF94 };

+u32 MC_FSR[4] =	{ 0xFFFFFF68, 0xFFFFFF78, 0xFFFFFF88, 0xFFFFFF98 };

+

+char * EPROC[8]= {"Unknown","ARM946-E","ARM7TDMI","Unknown","ARM920T","ARM926EJ-S","Unknown","Unknown"};

+long NVPSIZ[16] = {

+   0,

+   0x2000, /*  8K */

+   0x4000, /* 16K */ 

+   0x8000, /* 32K */

+   -1,

+   0x10000, /* 64K */

+   -1,

+   0x20000, /* 128K */

+   -1,

+   0x40000, /* 256K */

+   0x80000, /* 512K */

+   -1,

+   0x100000, /* 1024K */

+   -1,

+   0x200000, /* 2048K */

+   -1

+};

+

+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 */

+};

+

+int at91sam7_register_commands(struct command_context_s *cmd_ctx)

+{

+	command_t *at91sam7_cmd = register_command(cmd_ctx, NULL, "at91sam7", NULL, COMMAND_ANY, NULL);

+	register_command(cmd_ctx, at91sam7_cmd, "gpnvm", at91sam7_handle_gpnvm_command, COMMAND_EXEC,

+			"at91sam7 gpnvm <num> <bit> set|clear, set or clear at91sam7 gpnvm bit");

+

+	return ERROR_OK;

+}

+

+u32 at91sam7_get_flash_status(flash_bank_t *bank, u8 flashplane)

+{

+	target_t *target = bank->target;

+	u32 fsr;

+	

+	target_read_u32(target, MC_FSR[flashplane], &fsr);

+	

+	return fsr;

+}

+

+/** Read clock configuration and set at91sam7_info->usec_clocks*/ 

+void at91sam7_read_clock_info(flash_bank_t *bank)

+{

+	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;

+	target_t *target = bank->target;

+	u32 mckr, mcfr, pllr;

+	unsigned long tmp = 0, mainfreq;

+	int flashplane;

+

+	/* Read main clock freqency 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;

+	switch (mckr & PMC_MCKR_CSS) 

+	{

+		case 0:			/* Slow Clock */

+			at91sam7_info->mck_valid = 1;

+			mainfreq = RC_FREQ / 16ul * (mcfr & 0xffff);

+			tmp = mainfreq;

+			break;

+		case 1:			/* Main Clock */

+			if (mcfr & CKGR_MCFR_MAINRDY) 

+			{

+				at91sam7_info->mck_valid = 1;

+				mainfreq = RC_FREQ / 16ul * (mcfr & 0xffff);

+				tmp = mainfreq;

+			}

+			break;

+

+		case 2:			/* Reserved */

+			break;

+		case 3:		/* PLL Clock */

+			if (mcfr & CKGR_MCFR_MAINRDY) 

+			{

+				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);

+			}

+			break;

+	}

+	

+	/* Prescaler adjust */

+	if (((mckr & PMC_MCKR_PRES) >> 2) == 7)

+		at91sam7_info->mck_valid = 0;

+	else

+		at91sam7_info->mck_freq = tmp >> ((mckr & PMC_MCKR_PRES) >> 2);

+

+	/* Forget old flash timing */

+	for (flashplane = 0; flashplane<at91sam7_info->num_planes; flashplane++)

+	{

+		at91sam7_set_flash_mode(bank, flashplane, FMR_TIMING_NONE);

+	}

+}

+

+/* Setup the timimg registers for nvbits or normal flash */

+void at91sam7_set_flash_mode(flash_bank_t *bank, u8 flashplane, int mode)

+{

+	u32 fmr, fmcn = 0, fws = 0;

+	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;

+	target_t *target = bank->target;

+	

+	if (mode && (mode != at91sam7_info->flashmode[flashplane]))

+	{

+		/* 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/666666ul)+1;

+

+		/* 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;

+

+		DEBUG("fmcn[%i]: %i", flashplane, fmcn); 

+		fmr = fmcn << 16 | fws << 8;

+		target_write_u32(target, MC_FMR[flashplane], fmr);

+	}

+	

+	at91sam7_info->flashmode[flashplane] = mode;		

+}

+

+u32 at91sam7_wait_status_busy(flash_bank_t *bank, u8 flashplane, u32 waitbits, int timeout)

+{

+	u32 status;

+	

+	while ((!((status = at91sam7_get_flash_status(bank,flashplane)) & waitbits)) && (timeout-- > 0))

+	{

+		DEBUG("status[%i]: 0x%x", flashplane, status);

+		usleep(1000);

+	}

+	

+	DEBUG("status[%i]: 0x%x", flashplane, status);

+

+	if (status & 0x0C)

+	{

+		ERROR("status register: 0x%x", status);

+		if (status & 0x4)

+			ERROR("Lock Error Bit Detected, Operation Abort");

+		if (status & 0x8)

+			ERROR("Invalid command and/or bad keyword, Operation Abort");

+		if (status & 0x10)

+			ERROR("Security Bit Set, Operation Abort");

+	}

+	

+	return status;

+}

+

+

+/* Send one command to the AT91SAM flash controller */

+int at91sam7_flash_command(struct flash_bank_s *bank, u8 flashplane, u8 cmd, u16 pagen) 

+{

+	u32 fcr;

+	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;

+	target_t *target = bank->target;

+

+	fcr = (0x5A<<24) | ((pagen&0x3FF)<<8) | cmd; 

+	target_write_u32(target, MC_FCR[flashplane], fcr);

+	DEBUG("Flash command: 0x%x, flashplane: %i, pagenumber:%u", fcr, flashplane, 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, flashplane, MC_FSR_EOL, 10)&0x0C) 

+		{

+			return ERROR_FLASH_OPERATION_FAILED;

+		}

+		return ERROR_OK;

+	}

+

+	if (at91sam7_wait_status_busy(bank, flashplane, 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 */

+int at91sam7_read_part_info(struct flash_bank_s *bank)

+{

+	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;

+	target_t *target = bank->target;

+	u32 cidr, status;

+	int sectornum;

+	

+	if (bank->target->state != TARGET_HALTED)

+	{

+		return ERROR_TARGET_NOT_HALTED;

+	}

+	

+	/* Read and parse chip identification register */

+	target_read_u32(target, DBGU_CIDR, &cidr);

+	

+	if (cidr == 0)

+	{

+		WARNING("Cannot identify target as an AT91SAM");

+		return ERROR_FLASH_OPERATION_FAILED;

+	}

+	

+	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;

+	bank->size = NVPSIZ[at91sam7_info->cidr_nvpsiz];

+	at91sam7_info->target_name = "Unknown";

+

+	/* Support just for bulk erase of a single flash plane, whole device if flash size <= 256k */

+	if (NVPSIZ[at91sam7_info->cidr_nvpsiz]<0x80000)  /* Flash size less than 512K, one flash plane */

+	{

+		bank->num_sectors = 1;

+		bank->sectors = malloc(sizeof(flash_sector_t));

+		bank->sectors[0].offset = 0;

+		bank->sectors[0].size = bank->size;

+		bank->sectors[0].is_erased = -1;

+		bank->sectors[0].is_protected = -1;

+	}

+	else	/* Flash size 512K or larger, several flash planes */

+	{

+		bank->num_sectors = NVPSIZ[at91sam7_info->cidr_nvpsiz]/0x40000;

+		bank->sectors = malloc(bank->num_sectors*sizeof(flash_sector_t));

+		for (sectornum=0; sectornum<bank->num_sectors; sectornum++)

+		{

+			bank->sectors[sectornum].offset = sectornum*0x40000;

+			bank->sectors[sectornum].size = 0x40000;

+			bank->sectors[sectornum].is_erased = -1;

+			bank->sectors[sectornum].is_protected = -1;

+		}

+	}

+		

+	

+

+	DEBUG("nvptyp: 0x%3.3x, arch: 0x%4.4x", at91sam7_info->cidr_nvptyp, at91sam7_info->cidr_arch );

+

+	/* Read main and master clock freqency register */

+	at91sam7_read_clock_info(bank);

+	

+	at91sam7_info->num_planes = 1;

+	status = at91sam7_get_flash_status(bank, 0);

+	at91sam7_info->securitybit = (status>>4)&0x01;

+	at91sam7_protect_check(bank);   /* TODO Check the protect check */

+	

+	if (at91sam7_info->cidr_arch == 0x70 )

+	{

+		at91sam7_info->num_nvmbits = 2;

+		at91sam7_info->nvmbits = (status>>8)&0x03;

+		bank->base = 0x100000;

+		bank->bus_width = 4;

+		if (bank->size==0x80000)  /* AT91SAM7S512 */

+		{

+			at91sam7_info->target_name = "AT91SAM7S512";

+			at91sam7_info->num_planes = 2;

+			if (at91sam7_info->num_planes != bank->num_sectors)

+				WARNING("Internal error: Number of flash planes and erase sectors does not match, please report");;

+			at91sam7_info->num_lockbits = 2*16;

+			at91sam7_info->pagesize = 256;

+			at91sam7_info->pages_in_lockregion = 64;

+			at91sam7_info->num_pages = 2*16*64;

+		}

+		if (bank->size==0x40000)  /* AT91SAM7S256 */

+		{

+			at91sam7_info->target_name = "AT91SAM7S256";

+			at91sam7_info->num_lockbits = 16;

+			at91sam7_info->pagesize = 256;

+			at91sam7_info->pages_in_lockregion = 64;

+			at91sam7_info->num_pages = 16*64;

+		}

+		if (bank->size==0x20000)  /* AT91SAM7S128 */

+		{

+			at91sam7_info->target_name = "AT91SAM7S128";

+			at91sam7_info->num_lockbits = 8;

+			at91sam7_info->pagesize = 256;

+			at91sam7_info->pages_in_lockregion = 64;

+			at91sam7_info->num_pages = 8*64;

+		}

+		if (bank->size==0x10000)  /* AT91SAM7S64 */

+		{

+			at91sam7_info->target_name = "AT91SAM7S64";

+			at91sam7_info->num_lockbits = 16;

+			at91sam7_info->pagesize = 128;

+			at91sam7_info->pages_in_lockregion = 32;

+			at91sam7_info->num_pages = 16*32;

+		}

+		if (bank->size==0x08000)  /* AT91SAM7S321/32 */

+		{

+			at91sam7_info->target_name = "AT91SAM7S321/32";

+			at91sam7_info->num_lockbits = 8;

+			at91sam7_info->pagesize = 128;

+			at91sam7_info->pages_in_lockregion = 32;

+			at91sam7_info->num_pages = 8*32;

+		}

+		

+		return ERROR_OK;

+	}

+

+	if (at91sam7_info->cidr_arch == 0x71 )

+	{

+		at91sam7_info->num_nvmbits = 3;

+		at91sam7_info->nvmbits = (status>>8)&0x07;

+		bank->base = 0x100000;

+		bank->bus_width = 4;

+		if (bank->size==0x80000)  /* AT91SAM7XC512 */

+		{

+			at91sam7_info->target_name = "AT91SAM7XC512";

+			at91sam7_info->num_planes = 2;

+			if (at91sam7_info->num_planes != bank->num_sectors)

+				WARNING("Internal error: Number of flash planes and erase sectors does not match, please report");;

+			at91sam7_info->num_lockbits = 2*16;

+			at91sam7_info->pagesize = 256;

+			at91sam7_info->pages_in_lockregion = 64;

+			at91sam7_info->num_pages = 2*16*64;

+		}

+		if (bank->size==0x40000)  /* AT91SAM7XC256 */

+		{

+			at91sam7_info->target_name = "AT91SAM7XC256";

+			at91sam7_info->num_lockbits = 16;

+			at91sam7_info->pagesize = 256;

+			at91sam7_info->pages_in_lockregion = 64;

+			at91sam7_info->num_pages = 16*64;

+		}

+		if (bank->size==0x20000)  /* AT91SAM7XC128 */

+		{

+			at91sam7_info->target_name = "AT91SAM7XC128";

+			at91sam7_info->num_lockbits = 8;

+			at91sam7_info->pagesize = 256;

+			at91sam7_info->pages_in_lockregion = 64;

+			at91sam7_info->num_pages = 8*64;

+		}

+		

+		return ERROR_OK;

+	}

+	

+	if (at91sam7_info->cidr_arch == 0x72 )

+	{

+		at91sam7_info->num_nvmbits = 3;

+		at91sam7_info->nvmbits = (status>>8)&0x07;

+		bank->base = 0x100000;

+		bank->bus_width = 4;

+		if (bank->size==0x80000) /* AT91SAM7SE512 */

+		{

+			at91sam7_info->target_name = "AT91SAM7SE512";

+			at91sam7_info->num_planes = 2;

+			if (at91sam7_info->num_planes != bank->num_sectors)

+				WARNING("Internal error: Number of flash planes and erase sectors does not match, please report");;

+			at91sam7_info->num_lockbits = 32;

+			at91sam7_info->pagesize = 256;

+			at91sam7_info->pages_in_lockregion = 64;

+			at91sam7_info->num_pages = 32*64;

+		}

+		if (bank->size==0x40000)

+		{

+			at91sam7_info->target_name = "AT91SAM7SE256";

+			at91sam7_info->num_lockbits = 16;

+			at91sam7_info->pagesize = 256;

+			at91sam7_info->pages_in_lockregion = 64;

+			at91sam7_info->num_pages = 16*64;

+		}

+		if (bank->size==0x08000)

+		{

+			at91sam7_info->target_name = "AT91SAM7SE32";

+			at91sam7_info->num_lockbits = 8;

+			at91sam7_info->pagesize = 128;

+			at91sam7_info->pages_in_lockregion = 32;

+			at91sam7_info->num_pages = 8*32;

+		}

+		

+		return ERROR_OK;

+	}

+	

+	if (at91sam7_info->cidr_arch == 0x75 )

+	{

+		at91sam7_info->num_nvmbits = 3;

+		at91sam7_info->nvmbits = (status>>8)&0x07;

+		bank->base = 0x100000;

+		bank->bus_width = 4;

+		if (bank->size==0x80000)  /* AT91SAM7X512 */

+		{

+			at91sam7_info->target_name = "AT91SAM7X512";

+			at91sam7_info->num_planes = 2;

+			if (at91sam7_info->num_planes != bank->num_sectors)

+				WARNING("Internal error: Number of flash planes and erase sectors does not match, please report");;

+			at91sam7_info->num_lockbits = 32;

+			at91sam7_info->pagesize = 256;

+			at91sam7_info->pages_in_lockregion = 64;

+			at91sam7_info->num_pages = 2*16*64;

+			DEBUG("Support for AT91SAM7X512 is experimental in this version!");

+		}

+		if (bank->size==0x40000)  /* AT91SAM7X256 */

+		{

+			at91sam7_info->target_name = "AT91SAM7X256";

+			at91sam7_info->num_lockbits = 16;

+			at91sam7_info->pagesize = 256;

+			at91sam7_info->pages_in_lockregion = 64;

+			at91sam7_info->num_pages = 16*64;

+		}

+		if (bank->size==0x20000)  /* AT91SAM7X128 */

+		{

+			at91sam7_info->target_name = "AT91SAM7X128";

+			at91sam7_info->num_lockbits = 8;

+			at91sam7_info->pagesize = 256;

+			at91sam7_info->pages_in_lockregion = 64;

+			at91sam7_info->num_pages = 8*64;

+		}

+	

+		return ERROR_OK;

+	}

+	

+	if (at91sam7_info->cidr_arch == 0x60 )

+	{

+		at91sam7_info->num_nvmbits = 3;

+		at91sam7_info->nvmbits = (status>>8)&0x07;

+		bank->base = 0x100000;

+		bank->bus_width = 4;

+		

+		if (bank->size == 0x40000)  /* AT91SAM7A3 */

+		{

+			at91sam7_info->target_name = "AT91SAM7A3";

+			at91sam7_info->num_lockbits = 16;

+			at91sam7_info->pagesize = 256;

+			at91sam7_info->pages_in_lockregion = 16;

+			at91sam7_info->num_pages = 16*64;

+		}

+		return ERROR_OK;

+	}

+	

+   WARNING("at91sam7 flash only tested for AT91SAM7Sxx series");

+	

+   return ERROR_OK;

+}

+

+int at91sam7_erase_check(struct flash_bank_s *bank)

+{

+	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;

+	

+	if (!at91sam7_info->working_area_size)

+	{

+	}

+	else

+	{	

+	}

+	

+	return ERROR_OK;

+}

+

+int at91sam7_protect_check(struct flash_bank_s *bank)

+{

+	u32 status;

+	int flashplane;

+	

+	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;

+

+	if (at91sam7_info->cidr == 0)

+	{

+		at91sam7_read_part_info(bank);

+	}

+

+	if (at91sam7_info->cidr == 0)

+	{

+		WARNING("Cannot identify target as an AT91SAM");

+		return ERROR_FLASH_OPERATION_FAILED;

+	}

+

+	for (flashplane=0;flashplane<at91sam7_info->num_planes;flashplane++)

+	{

+		status = at91sam7_get_flash_status(bank, flashplane);

+		at91sam7_info->lockbits[flashplane] = (status >> 16);

+	}

+	

+	return ERROR_OK;

+}

+

+/* flash_bank at91sam7 0 0 0 0 <target#>

+ */

+int at91sam7_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)

+{

+	at91sam7_flash_bank_t *at91sam7_info;

+	int i;

+	

+	if (argc < 6)

+	{

+		WARNING("incomplete flash_bank at91sam7 configuration");

+		return ERROR_FLASH_BANK_INVALID;

+	}

+	

+	at91sam7_info = malloc(sizeof(at91sam7_flash_bank_t));

+	bank->driver_priv = at91sam7_info;

+	at91sam7_info->probed = 0;

+	

+	/* part wasn't probed for info yet */

+	at91sam7_info->cidr = 0;

+	for (i=0;i<4;i++)

+		at91sam7_info->flashmode[i]=0;

+	

+	return ERROR_OK;

+}

+

+int at91sam7_erase(struct flash_bank_s *bank, int first, int last)

+{

+	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;

+	u8 flashplane;

+

+	if (bank->target->state != TARGET_HALTED)

+	{

+		return ERROR_TARGET_NOT_HALTED;

+	}

+	

+	if (at91sam7_info->cidr == 0)

+	{

+		at91sam7_read_part_info(bank);

+	}

+

+	if (at91sam7_info->cidr == 0)

+	{

+		WARNING("Cannot identify target as an AT91SAM");

+		return ERROR_FLASH_OPERATION_FAILED;

+	}	

+	

+	if ((first < 0) || (last < first) || (last >= bank->num_sectors))

+	{

+		if ((first == 0) && (last == (at91sam7_info->num_lockbits-1)))

+		{

+			WARNING("Sector numbers based on lockbit count, probably a deprecated script");

+			last = bank->num_sectors-1;

+		}

+		else return ERROR_FLASH_SECTOR_INVALID;

+	}

+

+	/* Configure the flash controller timing */

+	at91sam7_read_clock_info(bank);	

+	for (flashplane = first; flashplane<=last; flashplane++)

+	{

+		/* Configure the flash controller timing */

+		at91sam7_set_flash_mode(bank, flashplane, FMR_TIMING_FLASH);

+		if (at91sam7_flash_command(bank, flashplane, EA, 0) != ERROR_OK) 

+		{

+			return ERROR_FLASH_OPERATION_FAILED;

+		}	

+	}

+	return ERROR_OK;

+

+}

+

+int at91sam7_protect(struct flash_bank_s *bank, int set, int first, int last)

+{

+	u32 cmd, pagen;

+	u8 flashplane;

+	int lockregion;

+	

+	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;

+	

+	if (bank->target->state != TARGET_HALTED)

+	{

+		return ERROR_TARGET_NOT_HALTED;

+	}

+	

+	if ((first < 0) || (last < first) || (last >= at91sam7_info->num_lockbits))

+	{

+		return ERROR_FLASH_SECTOR_INVALID;

+	}

+	

+	if (at91sam7_info->cidr == 0)

+	{

+		at91sam7_read_part_info(bank);

+	}

+

+	if (at91sam7_info->cidr == 0)

+	{

+		WARNING("Cannot identify target as an AT91SAM");

+		return ERROR_FLASH_OPERATION_FAILED;

+	}

+	

+	at91sam7_read_clock_info(bank);	

+	

+	for (lockregion=first;lockregion<=last;lockregion++) 

+	{

+		pagen = lockregion*at91sam7_info->pages_in_lockregion;

+		flashplane = (pagen>>10)&0x03;

+		/* Configure the flash controller timing */

+		at91sam7_set_flash_mode(bank, flashplane, FMR_TIMING_NVBITS);

+		

+		if (set)

+			 cmd = SLB; 

+		else

+			 cmd = CLB; 		

+

+		if (at91sam7_flash_command(bank, flashplane, cmd, pagen) != ERROR_OK) 

+		{

+			return ERROR_FLASH_OPERATION_FAILED;

+		}	

+	}

+	

+	at91sam7_protect_check(bank);

+		

+	return ERROR_OK;

+}

+

+

+int at91sam7_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)

+{

+	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;

+	target_t *target = bank->target;

+	u32 dst_min_alignment, wcount, bytes_remaining = count;

+	u32 first_page, last_page, pagen, buffer_pos;

+	u8 flashplane;

+	

+	if (at91sam7_info->cidr == 0)

+	{

+		at91sam7_read_part_info(bank);

+	}

+

+	if (at91sam7_info->cidr == 0)

+	{

+		WARNING("Cannot identify target as an AT91SAM");

+		return ERROR_FLASH_OPERATION_FAILED;

+	}

+	

+	if (offset + count > bank->size)

+		return ERROR_FLASH_DST_OUT_OF_BANK;

+	

+	dst_min_alignment = at91sam7_info->pagesize;

+

+	if (offset % dst_min_alignment)

+	{

+		WARNING("offset 0x%x breaks required alignment 0x%x", 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 = CEIL(offset + count, dst_min_alignment);

+	

+	DEBUG("first_page: %i, last_page: %i, count %i", first_page, last_page, count);

+	

+	at91sam7_read_clock_info(bank);	

+

+	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 = CEIL(count,4);

+		target->type->write_memory(target, bank->base+pagen*dst_min_alignment, 4, wcount, buffer+buffer_pos);

+		flashplane = (pagen>>10)&0x3;

+		

+		/* Configure the flash controller timing */	

+		at91sam7_set_flash_mode(bank, flashplane, FMR_TIMING_FLASH);

+		/* Send Write Page command to Flash Controller */

+		if (at91sam7_flash_command(bank, flashplane, WP, pagen) != ERROR_OK) 

+		{

+				return ERROR_FLASH_OPERATION_FAILED;

+		}

+		DEBUG("Write flash plane:%i page number:%i", flashplane, pagen);

+	}

+	

+	return ERROR_OK;

+}

+

+

+int at91sam7_probe(struct flash_bank_s *bank)

+{

+	/* we can't probe on an at91sam7

+	 * if this is an at91sam7, it has the configured flash

+	 */

+	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;

+	at91sam7_info->probed = 0;

+	

+	if (at91sam7_info->cidr == 0)

+	{

+		at91sam7_read_part_info(bank);

+	}

+

+	if (at91sam7_info->cidr == 0)

+	{

+		WARNING("Cannot identify target as an AT91SAM");

+		return ERROR_FLASH_OPERATION_FAILED;

+	}

+	

+	at91sam7_info->probed = 1;

+	

+	return ERROR_OK;

+}

+

+

+int at91sam7_auto_probe(struct flash_bank_s *bank)

+{

+	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;

+	if (at91sam7_info->probed)

+		return ERROR_OK;

+	return at91sam7_probe(bank);

+}

+

+int at91sam7_info(struct flash_bank_s *bank, char *buf, int buf_size)

+{

+	int printed, flashplane;

+	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;

+	

+	at91sam7_read_part_info(bank);

+

+	if (at91sam7_info->cidr == 0)

+	{

+		printed = snprintf(buf, buf_size, "Cannot identify target as an AT91SAM\n");

+		buf += printed;

+		buf_size -= printed;

+		return ERROR_FLASH_OPERATION_FAILED;

+	}

+	

+	printed = snprintf(buf, buf_size, "\nat91sam7 information: Chip is %s\n",at91sam7_info->target_name);

+	buf += printed;

+	buf_size -= printed;

+	

+	printed = snprintf(buf, buf_size, "cidr: 0x%8.8x, arch: 0x%4.4x, eproc: %s, version:0x%3.3x,  flashsize: 0x%8.8x\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): %ikHz \n", at91sam7_info->mck_freq / 1000);

+	buf += printed;

+	buf_size -= printed;

+	

+	if (at91sam7_info->num_planes>1) {		

+		printed = snprintf(buf, buf_size, "flashplanes: %i, pagesize: %i, lock regions: %i, pages in lock region: %i \n", 

+			   at91sam7_info->num_planes, at91sam7_info->pagesize, at91sam7_info->num_lockbits, at91sam7_info->num_pages/at91sam7_info->num_lockbits);

+		buf += printed;

+		buf_size -= printed;

+		for (flashplane=0; flashplane<at91sam7_info->num_planes; flashplane++)

+		{

+			printed = snprintf(buf, buf_size, "lockbits[%i]: 0x%4.4x,  ", flashplane, at91sam7_info->lockbits[flashplane]);

+			buf += printed;

+			buf_size -= printed;

+		}

+	}

+	else

+	if (at91sam7_info->num_lockbits>0) {		

+		printed = snprintf(buf, buf_size, "pagesize: %i, lockbits: %i 0x%4.4x, pages in lock region: %i \n", 

+			   at91sam7_info->pagesize, at91sam7_info->num_lockbits, at91sam7_info->lockbits[0], at91sam7_info->num_pages/at91sam7_info->num_lockbits);

+		buf += printed;

+		buf_size -= printed;

+	}

+			

+	printed = snprintf(buf, buf_size, "securitybit: %i, nvmbits: 0x%1.1x\n", at91sam7_info->securitybit, at91sam7_info->nvmbits);

+	buf += printed;

+	buf_size -= printed;

+

+	return ERROR_OK;

+}

+

+/* 

+* On AT91SAM7S: When the gpnvm bits are set with 

+* > at91sam7 gpnvm 0 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.

+*/

+int at91sam7_handle_gpnvm_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	flash_bank_t *bank;

+	int bit;

+	u8  flashcmd;

+	u32 status;

+	char *value;

+	at91sam7_flash_bank_t *at91sam7_info;

+

+	if (argc < 3)

+	{

+		command_print(cmd_ctx, "at91sam7 gpnvm <num> <bit> <set|clear>");

+		return ERROR_OK;

+	}

+	

+	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+	bit = atoi(args[1]);

+	value = args[2];

+

+	if (!bank)

+	{

+		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);

+		return ERROR_OK;

+	}

+

+	at91sam7_info = bank->driver_priv;

+

+	if (bank->target->state != TARGET_HALTED)

+	{

+		return ERROR_TARGET_NOT_HALTED;

+	}

+	

+	if (at91sam7_info->cidr == 0)

+	{

+		at91sam7_read_part_info(bank);

+	}

+

+	if (at91sam7_info->cidr == 0)

+	{

+		WARNING("Cannot identify target as an AT91SAM");

+		return ERROR_FLASH_OPERATION_FAILED;

+	}

+

+	if ((bit<0) || (at91sam7_info->num_nvmbits <= bit))

+	{ 

+		command_print(cmd_ctx, "gpnvm bit '#%s' is out of bounds for target %s", args[1],at91sam7_info->target_name);

+		return ERROR_OK;

+	}

+

+	if (strcmp(value, "set") == 0)

+	{

+		flashcmd = SGPB;

+	}

+	else if (strcmp(value, "clear") == 0)

+	{

+		flashcmd = CGPB;

+	}

+	else

+	{

+		command_print(cmd_ctx, "usage: at91sam7 gpnvm <num> <bit> <set|clear>");

+		return ERROR_OK;

+	}

+

+	/* Configure the flash controller timing */

+	at91sam7_read_clock_info(bank);	

+	at91sam7_set_flash_mode(bank, 0, FMR_TIMING_NVBITS);

+	

+	if (at91sam7_flash_command(bank, 0, flashcmd, (u16)(bit)) != ERROR_OK) 

+	{

+		return ERROR_FLASH_OPERATION_FAILED;

+	}	

+

+	status = at91sam7_get_flash_status(bank, 0);

+	DEBUG("at91sam7_handle_gpnvm_command: cmd 0x%x, value 0x%x, status 0x%x \n",flashcmd,bit,status);

+	at91sam7_info->nvmbits = (status>>8)&((1<<at91sam7_info->num_nvmbits)-1);

+

+	return ERROR_OK;

+}

diff --git a/src/flash/cfi.c b/src/flash/cfi.c
index 66a1c656..82120d8d 100644
--- a/src/flash/cfi.c
+++ b/src/flash/cfi.c
@@ -1375,11 +1375,11 @@ int cfi_spansion_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address,
 		}

 

 		/* allocate working area */

-		if (target_alloc_working_area(target, 24 * 4,

-			&cfi_info->write_algorithm) != ERROR_OK)

+		retval=target_alloc_working_area(target, 24 * 4,

+				&cfi_info->write_algorithm);

+		if (retval != ERROR_OK)

 		{

-			WARNING("no working area available, can't do block memory writes");

-			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;

+			return retval;

 		}

 

 		/* write algorithm code to working area */

@@ -1645,11 +1645,6 @@ int cfi_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
 	int i;

 	int retval;

 

-	if (bank->target->state != TARGET_HALTED)

-	{

-		return ERROR_TARGET_NOT_HALTED;

-	}

-

 	if (offset + count > bank->size)

 		return ERROR_FLASH_DST_OUT_OF_BANK;

 

diff --git a/src/flash/flash.c b/src/flash/flash.c
index dc87f5dd..416cac44 100644
--- a/src/flash/flash.c
+++ b/src/flash/flash.c
@@ -1,1071 +1,945 @@
-/***************************************************************************
- *   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.             *
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include "flash.h"
-#include "command.h"
-#include "target.h"
-#include "time_support.h"
-#include "fileio.h"
-#include "image.h"
-#include "log.h"
-
-#include <string.h>
-#include <unistd.h>
-#include <stdlib.h>
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <errno.h>
-#include <inttypes.h>
-
-/* command handlers */
-int handle_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_flash_banks_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_flash_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_flash_probe_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_flash_erase_check_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_flash_erase_address_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_flash_protect_check_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_flash_erase_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_flash_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_flash_write_binary_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_flash_write_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_flash_protect_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_flash_auto_erase_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-flash_bank_t *get_flash_bank_by_addr(target_t *target, u32 addr);
-
-/* flash drivers
- */
-extern flash_driver_t lpc2000_flash;
-extern flash_driver_t cfi_flash;
-extern flash_driver_t at91sam7_flash;
-extern flash_driver_t str7x_flash;
-extern flash_driver_t str9x_flash;
-extern flash_driver_t stellaris_flash;
-extern flash_driver_t str9xpec_flash;
-extern flash_driver_t stm32x_flash;
-extern flash_driver_t tms470_flash;
-
-flash_driver_t *flash_drivers[] =
-{
-	&lpc2000_flash,
-	&cfi_flash,
-	&at91sam7_flash,
-	&str7x_flash,
-	&str9x_flash,
-	&stellaris_flash,
-	&str9xpec_flash,
-	&stm32x_flash,
-	&tms470_flash,
-	NULL,
-};
-
-flash_bank_t *flash_banks;
-static 	command_t *flash_cmd;
-static int auto_erase = 0;
-
-int flash_register_commands(struct command_context_s *cmd_ctx)
-{
-	flash_cmd = register_command(cmd_ctx, NULL, "flash", NULL, COMMAND_ANY, NULL);
-	
-	register_command(cmd_ctx, flash_cmd, "bank", handle_flash_bank_command, COMMAND_CONFIG, "flash_bank <driver> <base> <size> <chip_width> <bus_width> <target> [driver_options ...]");
-	register_command(cmd_ctx, flash_cmd, "auto_erase", handle_flash_auto_erase_command, COMMAND_ANY,
-						 "auto erase flash sectors <on|off>");
-	return ERROR_OK;
-}
-
-int flash_init_drivers(struct command_context_s *cmd_ctx)
-{
-	if (flash_banks)
-	{
-		register_command(cmd_ctx, flash_cmd, "banks", handle_flash_banks_command, COMMAND_EXEC,
-						 "list configured flash banks ");
-		register_command(cmd_ctx, flash_cmd, "info", handle_flash_info_command, COMMAND_EXEC,
-						 "print info about flash bank <num>");
-		register_command(cmd_ctx, flash_cmd, "probe", handle_flash_probe_command, COMMAND_EXEC,
-						 "identify flash bank <num>");
-		register_command(cmd_ctx, flash_cmd, "erase_check", handle_flash_erase_check_command, COMMAND_EXEC,
-						 "check erase state of sectors in flash bank <num>");
-		register_command(cmd_ctx, flash_cmd, "protect_check", handle_flash_protect_check_command, COMMAND_EXEC,
-						 "check protection state of sectors in flash bank <num>");
-		register_command(cmd_ctx, flash_cmd, "erase", handle_flash_erase_command, COMMAND_EXEC,
-						 "DEPRECATED, use 'erase_sector' instead");
-		register_command(cmd_ctx, flash_cmd, "erase_sector", handle_flash_erase_command, COMMAND_EXEC,
-						 "erase sectors at <bank> <first> <last>");
-		register_command(cmd_ctx, flash_cmd, "erase_address", handle_flash_erase_address_command, COMMAND_EXEC,
-						 "erase address range <address> <length>");
-		register_command(cmd_ctx, flash_cmd, "write", handle_flash_write_binary_command, COMMAND_EXEC,
-						 "DEPRECATED, use 'write_binary' instead");
-		register_command(cmd_ctx, flash_cmd, "write_binary", handle_flash_write_binary_command, COMMAND_EXEC,
-						 "write binary <bank> <file> <offset>");
-		register_command(cmd_ctx, flash_cmd, "write_image", handle_flash_write_image_command, COMMAND_EXEC,
-						 "write_image <file> [offset] [type]");
-		register_command(cmd_ctx, flash_cmd, "protect", handle_flash_protect_command, COMMAND_EXEC,
-						 "set protection of sectors at <bank> <first> <last> <on|off>");
-	}
-	
-	return ERROR_OK;
-}
-
-flash_bank_t *get_flash_bank_by_num_noprobe(int num)
-{
-	flash_bank_t *p;
-	int i = 0;
-
-	for (p = flash_banks; p; p = p->next)
-	{
-		if (i++ == num)
-		{
-			return p;
-		}
-	}
-	
-	return NULL;
-}
-
-flash_bank_t *get_flash_bank_by_num(int num)
-{
-	flash_bank_t *p = get_flash_bank_by_num_noprobe(num);
-	int retval;
-	
-	if (p == NULL)
-		return NULL;
-	
-	retval = p->driver->auto_probe(p);
-	
-	if (retval != ERROR_OK)
-	{
-		ERROR("auto_probe failed %d\n", retval);
-		return NULL;
-	}
-	return p;
-}
-
-int handle_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	int i;
-	int found = 0;
-	target_t *target;
-		
-	if (argc < 6)
-	{
-		return ERROR_COMMAND_SYNTAX_ERROR;
-	}
-	
-	if ((target = get_target_by_num(strtoul(args[5], NULL, 0))) == NULL)
-	{
-		ERROR("target %lu not defined", strtoul(args[5], NULL, 0));
-		return ERROR_OK;
-	}
-	
-	for (i = 0; flash_drivers[i]; i++)
-	{
-		if (strcmp(args[0], flash_drivers[i]->name) == 0)
-		{
-			flash_bank_t *p, *c;
-			
-			/* register flash specific commands */
-			if (flash_drivers[i]->register_commands(cmd_ctx) != ERROR_OK)
-			{
-				ERROR("couldn't register '%s' commands", args[0]);
-				exit(-1);
-			}
-			
-			c = malloc(sizeof(flash_bank_t));
-			c->target = target;
-			c->driver = flash_drivers[i];
-			c->driver_priv = NULL;
-			c->base = strtoul(args[1], NULL, 0);
-			c->size = strtoul(args[2], NULL, 0);
-			c->chip_width = strtoul(args[3], NULL, 0);
-			c->bus_width = strtoul(args[4], NULL, 0);
-			c->num_sectors = 0;
-			c->sectors = NULL;
-			c->next = NULL;
-			
-			if (flash_drivers[i]->flash_bank_command(cmd_ctx, cmd, args, argc, c) != ERROR_OK)
-			{
-				ERROR("'%s' driver rejected flash bank at 0x%8.8x", args[0], c->base);
-				free(c);
-				return ERROR_OK;
-			}
-			
-			/* put flash bank in linked list */
-			if (flash_banks)
-			{
-				/* find last flash bank */
-				for (p = flash_banks; p && p->next; p = p->next);
-				if (p)
-					p->next = c;
-			}
-			else
-			{
-				flash_banks = c;
-			}
-			
-			found = 1;
-		}
-	}
-		
-	/* no matching flash driver found */
-	if (!found)
-	{
-		ERROR("flash driver '%s' not found", args[0]);
-		exit(-1);
-	}
-	
-	return ERROR_OK;
-}
-
-int handle_flash_banks_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	flash_bank_t *p;
-	int i = 0;
-	
-	if (!flash_banks)
-	{
-		command_print(cmd_ctx, "no flash banks configured");
-		return ERROR_OK;
-	}
-	
-	for (p = flash_banks; p; p = p->next)
-	{
-		command_print(cmd_ctx, "#%i: %s at 0x%8.8x, size 0x%8.8x, buswidth %i, chipwidth %i",
-					  i++, p->driver->name, p->base, p->size, p->bus_width, p->chip_width);
-	}
-	
-	return ERROR_OK;
-}
-
-int handle_flash_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	flash_bank_t *p;
-	int i = 0;
-	int j = 0;
-		
-	if (argc != 1)
-	{
-		command_print(cmd_ctx, "usage: flash info <num>");
-		return ERROR_OK;
-	}
-	
-	for (p = flash_banks; p; p = p->next, i++)
-	{
-		if (i == strtoul(args[0], NULL, 0))
-		{
-			char buf[1024];
-			
-			/* attempt auto probe */
-			p->driver->auto_probe(p);
-			
-			command_print(cmd_ctx, "#%i: %s at 0x%8.8x, size 0x%8.8x, buswidth %i, chipwidth %i",
-						i, p->driver->name, p->base, p->size, p->bus_width, p->chip_width);
-			for (j = 0; j < p->num_sectors; j++)
-			{
-				char *erase_state, *protect_state;
-				
-				if (p->sectors[j].is_erased == 0)
-					erase_state = "not erased";
-				else if (p->sectors[j].is_erased == 1)
-					erase_state = "erased";
-				else
-					erase_state = "erase state unknown";
-				
-				if (p->sectors[j].is_protected == 0)
-					protect_state = "not protected";
-				else if (p->sectors[j].is_protected == 1)
-					protect_state = "protected";
-				else
-					protect_state = "protection state unknown";
-
-				command_print(cmd_ctx, "\t#%i: 0x%8.8x (0x%x %ikB) %s, %s",
-							j, p->sectors[j].offset, p->sectors[j].size, p->sectors[j].size>>10,
-							erase_state, protect_state);
-			}
-			
-			p->driver->info(p, buf, 1024);
-			command_print(cmd_ctx, "%s", buf);
-		}
-	}
-	
-	return ERROR_OK;
-}
-
-int handle_flash_probe_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	flash_bank_t *p;
-	int retval;
-		
-	if (argc != 1)
-	{
-		command_print(cmd_ctx, "usage: flash probe <num>");
-		return ERROR_OK;
-	}
-	
-	p = get_flash_bank_by_num_noprobe(strtoul(args[0], NULL, 0));
-	if (p)
-	{
-		if ((retval = p->driver->probe(p)) == ERROR_OK)
-		{
-			command_print(cmd_ctx, "flash '%s' found at 0x%8.8x", p->driver->name, p->base);
-		}
-		else if (retval == ERROR_FLASH_BANK_INVALID)
-		{
-			command_print(cmd_ctx, "probing failed for flash bank '#%s' at 0x%8.8x",
-						  args[0], p->base);
-		}
-		else
-		{
-			command_print(cmd_ctx, "unknown error when probing flash bank '#%s' at 0x%8.8x",
-						  args[0], p->base);
-		}
-	}
-	else
-	{
-		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-	}
-	
-	return ERROR_OK;
-}
-
-int handle_flash_erase_check_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	flash_bank_t *p;
-	int retval;
-		
-	if (argc != 1)
-	{
-		command_print(cmd_ctx, "usage: flash erase_check <num>");
-		return ERROR_OK;
-	}
-	
-	p = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-	if (p)
-	{
-		if ((retval = p->driver->erase_check(p)) == ERROR_OK)
-		{
-			command_print(cmd_ctx, "successfully checked erase state", p->driver->name, p->base);
-		}
-		else
-		{
-			command_print(cmd_ctx, "unknown error when checking erase state of flash bank #%s at 0x%8.8x",
-				args[0], p->base);
-		}
-	}
-	else
-	{
-		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-	}
-	
-	return ERROR_OK;
-}
-
-int handle_flash_erase_address_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	flash_bank_t *p;
-	int retval;
-	int address;
-	int length;
-	duration_t duration;
-	char *duration_text;
-	
-	target_t *target = get_current_target(cmd_ctx);
-
-	if (argc != 2)
-	{
-		command_print(cmd_ctx, "usage: flash erase_address <address> <length>");
-		return ERROR_OK;
-	}
-	
-	address = strtoul(args[0], NULL, 0);
-	length = strtoul(args[1], NULL, 0);
-	if (length <= 0)
-	{
-		command_print(cmd_ctx, "Length must be >0");
-		return ERROR_INVALID_ARGUMENTS;
-	}
-
-	p = get_flash_bank_by_addr(target, address);
-	if (p == NULL)
-	{
-		command_print(cmd_ctx, "No flash at that address");
-		return ERROR_INVALID_ARGUMENTS;
-	}
-	
-	/* We can't know if we did a resume + halt, in which case we no longer know the erased state */
-	flash_set_dirty();
-	
-	duration_start_measure(&duration);
-	
-	if ((retval = flash_erase_address_range(target, address, length)) != ERROR_OK)
-	{
-		switch (retval)
-		{
-			case ERROR_TARGET_NOT_HALTED:
-				command_print(cmd_ctx, "can't work with this flash while target is running");
-				break;
-			case ERROR_INVALID_ARGUMENTS:
-				command_print(cmd_ctx, "usage: flash erase_address <address> <length>");
-				break;
-			case ERROR_FLASH_BANK_INVALID:
-				command_print(cmd_ctx, "no '%s' flash found at 0x%8.8x", p->driver->name, p->base);
-				break;
-			case ERROR_FLASH_OPERATION_FAILED:
-				command_print(cmd_ctx, "flash erase error");
-				break;
-			case ERROR_FLASH_SECTOR_INVALID:
-				command_print(cmd_ctx, "sector number(s) invalid");
-				break;
-			default:
-				command_print(cmd_ctx, "unknown error");
-		}
-	}
-	else
-	{
-		duration_stop_measure(&duration, &duration_text);	
-		command_print(cmd_ctx, "erased address 0x%8.8x length %i in %s", address, length, duration_text);
-		free(duration_text);
-	}
-	
-	return retval;
-}
-
-int handle_flash_protect_check_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	flash_bank_t *p;
-	int retval;
-		
-	if (argc != 1)
-	{
-		command_print(cmd_ctx, "usage: flash protect_check <num>");
-		return ERROR_OK;
-	}
-	
-	p = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-	if (p)
-	{
-		if ((retval = p->driver->protect_check(p)) == ERROR_OK)
-		{
-			command_print(cmd_ctx, "successfully checked protect state");
-		}
-		else if (retval == ERROR_FLASH_OPERATION_FAILED)
-		{
-			command_print(cmd_ctx, "checking protection state failed (possibly unsupported) by flash #%s at 0x%8.8x", args[0], p->base);
-		}
-		else
-		{
-			command_print(cmd_ctx, "unknown error when checking protection state of flash bank '#%s' at 0x%8.8x", args[0], p->base);
-		}
-	}
-	else
-	{
-		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-	}
-	
-	return ERROR_OK;
-}
-
-int handle_flash_erase_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	if (argc > 2)
-	{
-		int first = strtoul(args[1], NULL, 0);
-		int last = strtoul(args[2], NULL, 0);
-		int retval;
-		flash_bank_t *p = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-		duration_t duration;
-		char *duration_text;
-	
-		duration_start_measure(&duration);
-	
-		if (!p)
-		{
-			command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-			return ERROR_OK;
-		}
-		
-		if ((retval = p->driver->erase(p, first, last)) != ERROR_OK)
-		{
-			switch (retval)
-			{
-				case ERROR_TARGET_NOT_HALTED:
-					command_print(cmd_ctx, "can't work with this flash while target is running");
-					break;
-				case ERROR_INVALID_ARGUMENTS:
-					command_print(cmd_ctx, "usage: flash_erase <bank> <first> <last>");
-					break;
-				case ERROR_FLASH_BANK_INVALID:
-					command_print(cmd_ctx, "no '%s' flash found at 0x%8.8x", p->driver->name, p->base);
-					break;
-				case ERROR_FLASH_OPERATION_FAILED:
-					command_print(cmd_ctx, "flash erase error");
-					break;
-				case ERROR_FLASH_SECTOR_INVALID:
-					command_print(cmd_ctx, "sector number(s) invalid");
-					break;
-				case ERROR_OK:
-					command_print(cmd_ctx, "erased flash sectors %i to %i", first, last);
-					break;
-				default:
-					command_print(cmd_ctx, "unknown error");
-			}
-		}
-		else
-		{
-			duration_stop_measure(&duration, &duration_text);	
-			
-			command_print(cmd_ctx, "erased sectors %i through %i on flash bank %i in %s", first, last, strtoul(args[0], 0, 0), duration_text);
-			free(duration_text);
-		}
-	}
-	else
-	{
-		command_print(cmd_ctx, "usage: flash erase <bank> <first> <last>");
-	}
-
-	return ERROR_OK;
-}
-
-int handle_flash_protect_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	if (argc > 3)
-	{
-		int first = strtoul(args[1], NULL, 0);
-		int last = strtoul(args[2], NULL, 0);
-		int set;
-		int retval;
-		flash_bank_t *p = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-		if (!p)
-		{
-			command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-			return ERROR_OK;
-		}
-		
-		if (strcmp(args[3], "on") == 0)
-			set = 1;
-		else if (strcmp(args[3], "off") == 0)
-			set = 0;
-		else
-		{
-			command_print(cmd_ctx, "usage: flash protect <bank> <first> <last> <on|off>");
-			return ERROR_OK;
-		}
-		
-		if ((retval = p->driver->protect(p, set, first, last)) != ERROR_OK)
-		{
-			switch (retval)
-			{
-				case ERROR_TARGET_NOT_HALTED:
-					command_print(cmd_ctx, "can't work with this flash while target is running");
-					break;
-				case ERROR_INVALID_ARGUMENTS:
-					command_print(cmd_ctx, "usage: flash protect <bank> <first> <last> <on|off>");
-					break;
-				case ERROR_FLASH_BANK_INVALID:
-					command_print(cmd_ctx, "no '%s' flash found at 0x%8.8x", p->driver->name, p->base);
-					break;
-				case ERROR_FLASH_OPERATION_FAILED:
-					command_print(cmd_ctx, "flash program error");
-					break;
-				case ERROR_FLASH_SECTOR_INVALID:
-					command_print(cmd_ctx, "sector number(s) invalid");
-					break;
-				case ERROR_OK:
-					command_print(cmd_ctx, "protection of flash sectors %i to %i turned %s", first, last, args[3]);
-					break;
-				default:
-					command_print(cmd_ctx, "unknown error");
-			}
-		}
-		else
-		{
-			command_print(cmd_ctx, "%s protection for sectors %i through %i on flash bank %i", (set) ? "set" : "cleared", first, last, strtoul(args[0], 0, 0));
-		}
-	}
-	else
-	{
-		command_print(cmd_ctx, "usage: flash protect <bank> <first> <last> <on|off>");
-	}
-
-	return ERROR_OK;
-}
-
-int handle_flash_write_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	target_t *target = get_current_target(cmd_ctx);
-	
-	image_t image;
-	u32 written;
-	char *error_str;
-	int *failed;
-	
-	int i;
-	
-	duration_t duration;
-	char *duration_text;
-	
-	int retval;
-
-	if (argc < 1)
-	{
-		command_print(cmd_ctx, "usage: flash %s <file> [offset] [type]", cmd);
-		return ERROR_INVALID_ARGUMENTS;
-	}
-	
-	if (!target)
-	{
-		ERROR("no target selected");
-		return ERROR_OK;
-	}
-	
-	duration_start_measure(&duration);
-	
-	if (argc >= 2)
-	{
-		image.base_address_set = 1;
-		image.base_address = strtoul(args[1], NULL, 0);
-	}
-	else
-	{
-		image.base_address_set = 0;
-		image.base_address = 0x0;
-	}
-	
-	image.start_address_set = 0;
-
-	retval = image_open(&image, args[0], (argc == 3) ? args[2] : NULL);
-	if (retval != ERROR_OK)
-	{
-		command_print(cmd_ctx, "image_open error: %s", image.error_str);
-		return retval;
-	}
-	
-	failed = malloc(sizeof(int) * image.num_sections);
-
-	error_str = NULL;
-		
-	retval = flash_write(target, &image, &written, &error_str, failed, auto_erase);
-	
-	if (retval != ERROR_OK)
-	{
-		if (error_str)
-		{
-			command_print(cmd_ctx, "failed writing image %s: %s", args[0], error_str);
-			free(error_str);
-		}
-		image_close(&image);
-		free(failed);
-		return retval;
-	}
-	
-	for (i = 0; i < image.num_sections; i++)
-	{
-		if (failed[i])
-		{
-			command_print(cmd_ctx, "didn't write section at 0x%8.8x, size 0x%8.8x",
-					image.sections[i].base_address, image.sections[i].size);
-		}
-	}
-	
-	duration_stop_measure(&duration, &duration_text);
-	command_print(cmd_ctx, "wrote %u byte from file %s in %s (%f kb/s)",
-		written, args[0], duration_text,
-		(float)written / 1024.0 / ((float)duration.duration.tv_sec + ((float)duration.duration.tv_usec / 1000000.0)));
-	free(duration_text);
-	free(failed);
-
-	image_close(&image);
-	
-	return ERROR_OK;
-}
-
-int handle_flash_write_binary_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	u32 offset;
-	u8 *buffer;
-	u32 buf_cnt;
-
-	fileio_t fileio;
-	
-	duration_t duration;
-	char *duration_text;
-	
-	int retval;
-	flash_bank_t *p;
-
-	if (argc < 3)
-	{
-		command_print(cmd_ctx, "usage: flash write_binary <bank> <file> <offset>");
-		return ERROR_OK;
-	}
-	
-	duration_start_measure(&duration);
-	
-	offset = strtoul(args[2], NULL, 0);
-	p = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-	if (!p)
-	{
-		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-		return ERROR_OK;
-	}
-	
-	if (fileio_open(&fileio, args[1], FILEIO_READ, FILEIO_BINARY) != ERROR_OK)
-	{
-		command_print(cmd_ctx, "flash write_binary error: %s", fileio.error_str);
-		return ERROR_OK;
-	}
-	
-	buffer = malloc(fileio.size);
-	if (fileio_read(&fileio, fileio.size, buffer, &buf_cnt) != ERROR_OK)
-	{
-		command_print(cmd_ctx, "flash write_binary error: %s", fileio.error_str);
-		return ERROR_OK;
-	}
-	
-	if ((retval = p->driver->write(p, buffer, offset, buf_cnt)) != ERROR_OK)
-	{
-		command_print(cmd_ctx, "failed writing file %s to flash bank %i at offset 0x%8.8x",
-			args[1], strtoul(args[0], NULL, 0), strtoul(args[2], NULL, 0));
-		
-		switch (retval)
-		{
-			case ERROR_TARGET_NOT_HALTED:
-				command_print(cmd_ctx, "can't work with this flash while target is running");
-				break;
-			case ERROR_INVALID_ARGUMENTS:
-				command_print(cmd_ctx, "usage: flash write <bank> <file> <offset>");
-				break;
-			case ERROR_FLASH_BANK_INVALID:
-				command_print(cmd_ctx, "no '%s' flash found at 0x%8.8x", p->driver->name, p->base);
-				break;
-			case ERROR_FLASH_OPERATION_FAILED:
-				command_print(cmd_ctx, "flash program error");
-				break;
-			case ERROR_FLASH_DST_BREAKS_ALIGNMENT:
-				command_print(cmd_ctx, "offset breaks required alignment");
-				break;
-			case ERROR_FLASH_DST_OUT_OF_BANK:
-				command_print(cmd_ctx, "destination is out of flash bank (offset and/or file too large)");
-				break;
-			case ERROR_FLASH_SECTOR_NOT_ERASED:
-				command_print(cmd_ctx, "destination sector(s) not erased");
-				break;
-			default:
-				command_print(cmd_ctx, "unknown error");
-		}
-	}
-
-	free(buffer);
-	
-	duration_stop_measure(&duration, &duration_text);
-	command_print(cmd_ctx, "wrote  %"PRIi64" byte from file %s to flash bank %i at offset 0x%8.8x in %s (%f kb/s)",
-		fileio.size, args[1], strtoul(args[0], NULL, 0), offset, duration_text,
-		(float)fileio.size / 1024.0 / ((float)duration.duration.tv_sec + ((float)duration.duration.tv_usec / 1000000.0)));
-	free(duration_text);
-
-	fileio_close(&fileio);
-	
-	return ERROR_OK;
-}
-
-void flash_set_dirty(void)
-{
-	flash_bank_t *c;
-	int i;
-	
-	/* set all flash to require erasing */
-	for (c = flash_banks; c; c = c->next)
-	{
-		for (i = 0; i < c->num_sectors; i++)
-		{
-			c->sectors[i].is_erased = 0; 
-		}
-	}
-}
-
-/* lookup flash bank by address */
-flash_bank_t *get_flash_bank_by_addr(target_t *target, u32 addr)
-{
-	flash_bank_t *c;
-
-	/* cycle through bank list */
-	for (c = flash_banks; c; c = c->next)
-	{
-		int retval;
-		retval = c->driver->auto_probe(c);
-		
-		if (retval != ERROR_OK)
-		{
-			ERROR("auto_probe failed %d\n", retval);
-			return NULL;
-		}
-		/* check whether address belongs to this flash bank */
-		if ((addr >= c->base) && (addr < c->base + c->size) && target == c->target)
-			return c;
-	}
-
-	return NULL;
-}
-
-/* erase given flash region, selects proper bank according to target and address */
-int flash_erase_address_range(target_t *target, u32 addr, u32 length)
-{
-	flash_bank_t *c;
-	int first = -1;
-	int last = -1;
-	int i;
-	
-	if ((c = get_flash_bank_by_addr(target, addr)) == NULL)
-		return ERROR_FLASH_DST_OUT_OF_BANK; /* no corresponding bank found */
-
-	if (c->size == 0 || c->num_sectors == 0)
-		return ERROR_FLASH_BANK_INVALID;
-	
-	if (length == 0)
-	{
-		/* special case, erase whole bank when length is zero */
-		if (addr != c->base)
-			return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
-		
-		return c->driver->erase(c, 0, c->num_sectors - 1);
-	}
-
-	/* check whether it fits */
-	if (addr + length > c->base + c->size)
-		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
-	
-	addr -= c->base;
-	
-	for (i = 0; i < c->num_sectors; i++)
-	{		
-		/* check whether sector overlaps with the given range and is not yet erased */
-		if (addr < c->sectors[i].offset + c->sectors[i].size && addr + length > c->sectors[i].offset && c->sectors[i].is_erased != 1) {
-			/* if first is not set yet then this is the first sector */
-			if (first == -1)
-				first = i;
-			last = i; /* and it is the last one so far in any case */
-		}
-	}
-	
-	if( first == -1 || last == -1 )
-		return ERROR_OK;
-	
-	return c->driver->erase(c, first, last);
-}
-
-/* write (optional verify) an image to flash memory of the given target */
-int flash_write(target_t *target, image_t *image, u32 *written, char **error_str, int *failed, int erase)
-{
-	int retval;
-	int i;
-
-	int section;
-	u32 section_offset;
-	flash_bank_t *c;
-	
-	section = 0;
-	section_offset = 0;
-
-	if (written)
-		*written = 0;
-	
-	if (failed != NULL)
-		for (i = 0; i < image->num_sections; i++)
-			failed[i] = 0;
-	
-	if (erase)
-	{
-		/* assume all sectors need erasing - stops any problems
-		 * when flash_write is called multiple times */
-		
-		flash_set_dirty();
-	}
-	
-	/* loop until we reach end of the image */
-	while (section < image->num_sections)
-	{
-		u32 buffer_size;
-		u8 *buffer;
-		int section_first;
-		int section_last;
-		u32 run_address = image->sections[section].base_address + section_offset;
-		u32 run_size = image->sections[section].size - section_offset;
-
-		if (image->sections[section].size ==  0)
-		{
-			WARNING("empty section %d", section);
-			section++;
-			section_offset = 0;
-			continue;
-		}
-
-		/* find the corresponding flash bank */
-		if ((c = get_flash_bank_by_addr(target, run_address)) == NULL)
-		{
-			if (failed == NULL)
-			{
-				if (error_str == NULL)
-					return ERROR_FLASH_DST_OUT_OF_BANK; /* abort operation */
-				*error_str = malloc(FLASH_MAX_ERROR_STR);
-				snprintf(*error_str, FLASH_MAX_ERROR_STR, "no flash mapped at requested address");
-				return ERROR_FLASH_DST_OUT_OF_BANK; /* abort operation */
-			}
-			failed[section] = ERROR_FLASH_DST_OUT_OF_BANK; /* mark the section as failed */
-			section++; /* and skip it */
-			section_offset = 0;
-			continue;
-		}
-
-		/* collect consecutive sections which fall into the same bank */
-		section_first = section;
-		section_last = section;
-		while ((run_address + run_size < c->base + c->size)
-				&& (section_last + 1 < image->num_sections))
-		{
-			if (image->sections[section_last + 1].base_address < (run_address + run_size))
-			{
-				DEBUG("section %d out of order(very slightly surprising, but supported)", section_last + 1);
-				break;
-			}
-			if (image->sections[section_last + 1].base_address != (run_address + run_size))
-				break;
-			run_size += image->sections[++section_last].size;
-		}
-
-		/* fit the run into bank constraints */
-		if (run_address + run_size > c->base + c->size)
-			run_size = c->base + c->size - run_address;
-
-		/* allocate buffer */
-		buffer = malloc(run_size);
-		buffer_size = 0;
-
-		/* read sections to the buffer */
-		while (buffer_size < run_size)
-		{
-			u32 size_read;
-			
-			if (buffer_size - run_size <= image->sections[section].size - section_offset)
-				size_read = buffer_size - run_size;
-			else
-				size_read = image->sections[section].size - section_offset;
-			
-			if ((retval = image_read_section(image, section, section_offset,
-					size_read, buffer + buffer_size, &size_read)) != ERROR_OK || size_read == 0)
-			{
-				free(buffer);
-				
-				if (error_str == NULL)
-					return ERROR_IMAGE_TEMPORARILY_UNAVAILABLE;
-				
-				*error_str = malloc(FLASH_MAX_ERROR_STR);
-				
-				/* if image_read_section returned an error there's an error string we can pass on */
-				if (retval != ERROR_OK)
-					snprintf(*error_str, FLASH_MAX_ERROR_STR, "error reading from image: %s", image->error_str);
-				else
-					snprintf(*error_str, FLASH_MAX_ERROR_STR, "error reading from image");
-				
-				return ERROR_IMAGE_TEMPORARILY_UNAVAILABLE;
-			}
-
-			buffer_size += size_read;
-			section_offset += size_read;
-
-			if (section_offset >= image->sections[section].size)
-			{
-				section++;
-				section_offset = 0;
-			}
-		}
-
-		retval = ERROR_OK;
-		
-		if (erase)
-		{
-			/* calculate and erase sectors */
-			retval = flash_erase_address_range( target, run_address, run_size );
-		}
-		
-		if (retval == ERROR_OK)
-		{
-			/* write flash sectors */
-			retval = c->driver->write(c, buffer, run_address - c->base, run_size);
-		}
-		
-		free(buffer);
-
-		if (retval != ERROR_OK)
-		{
-			if (error_str == NULL)
-				return retval; /* abort operation */
-
-			*error_str = malloc(FLASH_MAX_ERROR_STR);
-			switch (retval)
-			{
-				case ERROR_TARGET_NOT_HALTED:
-					snprintf(*error_str, FLASH_MAX_ERROR_STR, "can't flash image while target is running");
-					break;
-				case ERROR_INVALID_ARGUMENTS:
-					snprintf(*error_str, FLASH_MAX_ERROR_STR, "flash driver can't fulfill request");
-					break;
-				case ERROR_FLASH_OPERATION_FAILED:
-					snprintf(*error_str, FLASH_MAX_ERROR_STR, "flash program error");
-					break;
-				case ERROR_FLASH_DST_BREAKS_ALIGNMENT:
-					snprintf(*error_str, FLASH_MAX_ERROR_STR, "offset breaks required alignment");
-					break;
-				case ERROR_FLASH_DST_OUT_OF_BANK:
-					snprintf(*error_str, FLASH_MAX_ERROR_STR, "no flash mapped at requested address");
-					break;
-				case ERROR_FLASH_SECTOR_NOT_ERASED:
-					snprintf(*error_str, FLASH_MAX_ERROR_STR, "destination sector(s) not erased");
-					break;
-				default:
-					snprintf(*error_str, FLASH_MAX_ERROR_STR, "unknown error: %i", retval);
-			}
-
-			return retval; /* abort operation */
-		}
-
-		if (written != NULL)
-			*written += run_size; /* add run size to total written counter */
-	}
-
-	return ERROR_OK;
-}
-
-int handle_flash_auto_erase_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	if (argc != 1)
-	{
-		command_print(cmd_ctx, "usage: flash auto_erase <on|off>");
-		return ERROR_OK;
-	}
-	
-	if (strcmp(args[0], "on") == 0)
-		auto_erase = 1;
-	else if (strcmp(args[0], "off") == 0)
-		auto_erase = 0;
-	
-	return ERROR_OK;
-}
+/***************************************************************************

+ *   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.             *

+ ***************************************************************************/

+#ifdef HAVE_CONFIG_H

+#include "config.h"

+#endif

+

+#include "flash.h"

+#include "command.h"

+#include "target.h"

+#include "time_support.h"

+#include "fileio.h"

+#include "image.h"

+#include "log.h"

+

+#include <string.h>

+#include <unistd.h>

+#include <stdlib.h>

+#include <sys/types.h>

+#include <sys/stat.h>

+#include <errno.h>

+#include <inttypes.h>

+

+/* command handlers */

+int handle_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int handle_flash_banks_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int handle_flash_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int handle_flash_probe_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int handle_flash_erase_check_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int handle_flash_erase_address_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int handle_flash_protect_check_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int handle_flash_erase_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int handle_flash_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int handle_flash_write_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int handle_flash_write_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int handle_flash_protect_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int handle_flash_auto_erase_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+flash_bank_t *get_flash_bank_by_addr(target_t *target, u32 addr);

+

+/* flash drivers

+ */

+extern flash_driver_t lpc2000_flash;

+extern flash_driver_t cfi_flash;

+extern flash_driver_t at91sam7_flash;

+extern flash_driver_t str7x_flash;

+extern flash_driver_t str9x_flash;

+extern flash_driver_t stellaris_flash;

+extern flash_driver_t str9xpec_flash;

+extern flash_driver_t stm32x_flash;

+extern flash_driver_t tms470_flash;

+

+flash_driver_t *flash_drivers[] =

+{

+	&lpc2000_flash,

+	&cfi_flash,

+	&at91sam7_flash,

+	&str7x_flash,

+	&str9x_flash,

+	&stellaris_flash,

+	&str9xpec_flash,

+	&stm32x_flash,

+	&tms470_flash,

+	NULL,

+};

+

+flash_bank_t *flash_banks;

+static 	command_t *flash_cmd;

+static int auto_erase = 0;

+

+/* wafer thin wrapper for invoking the flash driver */

+static int flash_driver_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)

+{

+	int retval=ERROR_OK;

+	if (bank->target->state != TARGET_HALTED)

+	{

+		ERROR("target not halted - aborting flash write");

+		retval=ERROR_TARGET_NOT_HALTED;

+	} else

+	{

+		retval=bank->driver->write(bank, buffer, offset, count);

+	}

+	if (retval!=ERROR_OK)

+	{

+		ERROR("Writing to flash bank at address 0x%08x at offset 0x%8.8x", bank->base, offset);

+	}

+	return retval;

+}

+

+static int flash_driver_erase(struct flash_bank_s *bank, int first, int last)

+{

+	int retval=ERROR_OK;

+	if (bank->target->state != TARGET_HALTED)

+	{

+		ERROR("target not halted - aborting flash erase");

+		retval=ERROR_TARGET_NOT_HALTED;

+	} else if ((first < 0) || (last < first) || (last >= bank->num_sectors))

+	{

+		ERROR("invalid flash sector");

+		retval=ERROR_FLASH_SECTOR_INVALID;

+	} else		

+	{

+		retval=bank->driver->erase(bank, first, last);

+	}

+	if (retval!=ERROR_OK)

+	{

+		ERROR("Failed erasing banks %d to %d", first, last);

+	}

+	return retval;

+}

+

+int flash_driver_protect(struct flash_bank_s *bank, int set, int first, int last)

+{

+	int retval;

+	if (bank->target->state != TARGET_HALTED)

+	{

+		ERROR("target not halted - aborting flash erase");

+		retval=ERROR_TARGET_NOT_HALTED;

+	} else if ((first < 0) || (last < first) || (last >= bank->num_sectors))

+	{

+		ERROR("invalid flash sector");

+		retval=ERROR_FLASH_SECTOR_INVALID;

+	} else		

+	{

+		retval=bank->driver->protect(bank, set, first, last);

+	}

+	if (retval!=ERROR_OK)

+	{

+		ERROR("Failed protecting banks %d to %d", first, last);

+	}

+	return retval;

+}

+

+

+int flash_register_commands(struct command_context_s *cmd_ctx)

+{

+	flash_cmd = register_command(cmd_ctx, NULL, "flash", NULL, COMMAND_ANY, NULL);

+	

+	register_command(cmd_ctx, flash_cmd, "bank", handle_flash_bank_command, COMMAND_CONFIG, "flash_bank <driver> <base> <size> <chip_width> <bus_width> <target> [driver_options ...]");

+	register_command(cmd_ctx, flash_cmd, "auto_erase", handle_flash_auto_erase_command, COMMAND_ANY,

+						 "auto erase flash sectors <on|off>");

+	return ERROR_OK;

+}

+

+int flash_init_drivers(struct command_context_s *cmd_ctx)

+{

+	if (flash_banks)

+	{

+		register_command(cmd_ctx, flash_cmd, "banks", handle_flash_banks_command, COMMAND_EXEC,

+						 "list configured flash banks ");

+		register_command(cmd_ctx, flash_cmd, "info", handle_flash_info_command, COMMAND_EXEC,

+						 "print info about flash bank <num>");

+		register_command(cmd_ctx, flash_cmd, "probe", handle_flash_probe_command, COMMAND_EXEC,

+						 "identify flash bank <num>");

+		register_command(cmd_ctx, flash_cmd, "erase_check", handle_flash_erase_check_command, COMMAND_EXEC,

+						 "check erase state of sectors in flash bank <num>");

+		register_command(cmd_ctx, flash_cmd, "protect_check", handle_flash_protect_check_command, COMMAND_EXEC,

+						 "check protection state of sectors in flash bank <num>");

+		register_command(cmd_ctx, flash_cmd, "erase_sector", handle_flash_erase_command, COMMAND_EXEC,

+						 "erase sectors at <bank> <first> <last>");

+		register_command(cmd_ctx, flash_cmd, "erase_address", handle_flash_erase_address_command, COMMAND_EXEC,

+						 "erase address range <address> <length>");

+		register_command(cmd_ctx, flash_cmd, "write_bank", handle_flash_write_bank_command, COMMAND_EXEC,

+						 "write binary data to <bank> <file> <offset>");

+		register_command(cmd_ctx, flash_cmd, "write_image", handle_flash_write_image_command, COMMAND_EXEC,

+						 "write_image <file> [offset] [type]");

+		register_command(cmd_ctx, flash_cmd, "protect", handle_flash_protect_command, COMMAND_EXEC,

+						 "set protection of sectors at <bank> <first> <last> <on|off>");

+	}

+	

+	return ERROR_OK;

+}

+

+flash_bank_t *get_flash_bank_by_num_noprobe(int num)

+{

+	flash_bank_t *p;

+	int i = 0;

+

+	for (p = flash_banks; p; p = p->next)

+	{

+		if (i++ == num)

+		{

+			return p;

+		}

+	}

+	ERROR("Flash bank %d does not exist", num);

+	return NULL;

+}

+

+flash_bank_t *get_flash_bank_by_num(int num)

+{

+	flash_bank_t *p = get_flash_bank_by_num_noprobe(num);

+	int retval;

+	

+	if (p == NULL)

+		return NULL;

+	

+	retval = p->driver->auto_probe(p);

+	

+	if (retval != ERROR_OK)

+	{

+		ERROR("auto_probe failed %d\n", retval);

+		return NULL;

+	}

+	return p;

+}

+

+int handle_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	int i;

+	int found = 0;

+	target_t *target;

+		

+	if (argc < 6)

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+	}

+	

+	if ((target = get_target_by_num(strtoul(args[5], NULL, 0))) == NULL)

+	{

+		ERROR("target %lu not defined", strtoul(args[5], NULL, 0));

+		return ERROR_OK;

+	}

+	

+	for (i = 0; flash_drivers[i]; i++)

+	{

+		if (strcmp(args[0], flash_drivers[i]->name) == 0)

+		{

+			flash_bank_t *p, *c;

+			

+			/* register flash specific commands */

+			if (flash_drivers[i]->register_commands(cmd_ctx) != ERROR_OK)

+			{

+				ERROR("couldn't register '%s' commands", args[0]);

+				exit(-1);

+			}

+			

+			c = malloc(sizeof(flash_bank_t));

+			c->target = target;

+			c->driver = flash_drivers[i];

+			c->driver_priv = NULL;

+			c->base = strtoul(args[1], NULL, 0);

+			c->size = strtoul(args[2], NULL, 0);

+			c->chip_width = strtoul(args[3], NULL, 0);

+			c->bus_width = strtoul(args[4], NULL, 0);

+			c->num_sectors = 0;

+			c->sectors = NULL;

+			c->next = NULL;

+			

+			if (flash_drivers[i]->flash_bank_command(cmd_ctx, cmd, args, argc, c) != ERROR_OK)

+			{

+				ERROR("'%s' driver rejected flash bank at 0x%8.8x", args[0], c->base);

+				free(c);

+				return ERROR_OK;

+			}

+			

+			/* put flash bank in linked list */

+			if (flash_banks)

+			{

+				/* find last flash bank */

+				for (p = flash_banks; p && p->next; p = p->next);

+				if (p)

+					p->next = c;

+			}

+			else

+			{

+				flash_banks = c;

+			}

+			

+			found = 1;

+		}

+	}

+		

+	/* no matching flash driver found */

+	if (!found)

+	{

+		ERROR("flash driver '%s' not found", args[0]);

+		exit(-1);

+	}

+	

+	return ERROR_OK;

+}

+

+int handle_flash_banks_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	flash_bank_t *p;

+	int i = 0;

+	

+	if (!flash_banks)

+	{

+		command_print(cmd_ctx, "no flash banks configured");

+		return ERROR_OK;

+	}

+	

+	for (p = flash_banks; p; p = p->next)

+	{

+		command_print(cmd_ctx, "#%i: %s at 0x%8.8x, size 0x%8.8x, buswidth %i, chipwidth %i",

+					  i++, p->driver->name, p->base, p->size, p->bus_width, p->chip_width);

+	}

+	

+	return ERROR_OK;

+}

+

+int handle_flash_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	flash_bank_t *p;

+	int i = 0;

+	int j = 0;

+		

+	if (argc != 1)

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+	}

+	

+	for (p = flash_banks; p; p = p->next, i++)

+	{

+		if (i == strtoul(args[0], NULL, 0))

+		{

+			char buf[1024];

+			

+			/* attempt auto probe */

+			p->driver->auto_probe(p);

+			

+			command_print(cmd_ctx, "#%i: %s at 0x%8.8x, size 0x%8.8x, buswidth %i, chipwidth %i",

+						i, p->driver->name, p->base, p->size, p->bus_width, p->chip_width);

+			for (j = 0; j < p->num_sectors; j++)

+			{

+				char *erase_state, *protect_state;

+				

+				if (p->sectors[j].is_erased == 0)

+					erase_state = "not erased";

+				else if (p->sectors[j].is_erased == 1)

+					erase_state = "erased";

+				else

+					erase_state = "erase state unknown";

+				

+				if (p->sectors[j].is_protected == 0)

+					protect_state = "not protected";

+				else if (p->sectors[j].is_protected == 1)

+					protect_state = "protected";

+				else

+					protect_state = "protection state unknown";

+

+				command_print(cmd_ctx, "\t#%i: 0x%8.8x (0x%x %ikB) %s, %s",

+							j, p->sectors[j].offset, p->sectors[j].size, p->sectors[j].size>>10,

+							erase_state, protect_state);

+			}

+			

+			p->driver->info(p, buf, 1024);

+			command_print(cmd_ctx, "%s", buf);

+		}

+	}

+	

+	return ERROR_OK;

+}

+

+int handle_flash_probe_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	flash_bank_t *p;

+	int retval;

+		

+	if (argc != 1)

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+	}

+	

+	p = get_flash_bank_by_num_noprobe(strtoul(args[0], NULL, 0));

+	if (p)

+	{

+		if ((retval = p->driver->probe(p)) == ERROR_OK)

+		{

+			command_print(cmd_ctx, "flash '%s' found at 0x%8.8x", p->driver->name, p->base);

+		}

+		else if (retval == ERROR_FLASH_BANK_INVALID)

+		{

+			command_print(cmd_ctx, "probing failed for flash bank '#%s' at 0x%8.8x",

+						  args[0], p->base);

+		}

+		else

+		{

+			command_print(cmd_ctx, "unknown error when probing flash bank '#%s' at 0x%8.8x",

+						  args[0], p->base);

+		}

+	}

+	else

+	{

+		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);

+	}

+	

+	return ERROR_OK;

+}

+

+int handle_flash_erase_check_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	flash_bank_t *p;

+	int retval;

+		

+	if (argc != 1)

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+	}

+	

+	p = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+	if (p)

+	{

+		if ((retval = p->driver->erase_check(p)) == ERROR_OK)

+		{

+			command_print(cmd_ctx, "successfully checked erase state", p->driver->name, p->base);

+		}

+		else

+		{

+			command_print(cmd_ctx, "unknown error when checking erase state of flash bank #%s at 0x%8.8x",

+				args[0], p->base);

+		}

+	}

+	

+	return ERROR_OK;

+}

+

+int handle_flash_erase_address_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	flash_bank_t *p;

+	int retval;

+	int address;

+	int length;

+	duration_t duration;

+	char *duration_text;

+	

+	target_t *target = get_current_target(cmd_ctx);

+

+	if (argc != 2)

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+	}

+	

+	address = strtoul(args[0], NULL, 0);

+	length = strtoul(args[1], NULL, 0);

+	if (length <= 0)

+	{

+		command_print(cmd_ctx, "Length must be >0");

+		return ERROR_COMMAND_SYNTAX_ERROR;

+	}

+

+	p = get_flash_bank_by_addr(target, address);

+	if (p == NULL)

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+	}

+	

+	/* We can't know if we did a resume + halt, in which case we no longer know the erased state */

+	flash_set_dirty();

+	

+	duration_start_measure(&duration);

+	

+	if ((retval = flash_erase_address_range(target, address, length)) == ERROR_OK)

+	{

+		duration_stop_measure(&duration, &duration_text);	

+		command_print(cmd_ctx, "erased address 0x%8.8x length %i in %s", address, length, duration_text);

+		free(duration_text);

+	}

+	

+	return retval;

+}

+

+int handle_flash_protect_check_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	flash_bank_t *p;

+	int retval;

+		

+	if (argc != 1)

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+	}

+	

+	p = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+	if (p)

+	{

+		if ((retval = p->driver->protect_check(p)) == ERROR_OK)

+		{

+			command_print(cmd_ctx, "successfully checked protect state");

+		}

+		else if (retval == ERROR_FLASH_OPERATION_FAILED)

+		{

+			command_print(cmd_ctx, "checking protection state failed (possibly unsupported) by flash #%s at 0x%8.8x", args[0], p->base);

+		}

+		else

+		{

+			command_print(cmd_ctx, "unknown error when checking protection state of flash bank '#%s' at 0x%8.8x", args[0], p->base);

+		}

+	}

+	else

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+	}

+	

+	return ERROR_OK;

+}

+

+int handle_flash_erase_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	if (argc > 2)

+	{

+		int first = strtoul(args[1], NULL, 0);

+		int last = strtoul(args[2], NULL, 0);

+		int retval;

+		flash_bank_t *p = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+		duration_t duration;

+		char *duration_text;

+	

+		duration_start_measure(&duration);

+	

+		if (!p)

+		{

+			return ERROR_COMMAND_SYNTAX_ERROR;

+		}

+		

+		if ((retval = flash_driver_erase(p, first, last)) == ERROR_OK)

+		{

+			duration_stop_measure(&duration, &duration_text);	

+			

+			command_print(cmd_ctx, "erased sectors %i through %i on flash bank %i in %s", first, last, strtoul(args[0], 0, 0), duration_text);

+			free(duration_text);

+		}

+	}

+	else

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+	}

+

+	return ERROR_OK;

+}

+

+int handle_flash_protect_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	if (argc > 3)

+	{

+		int first = strtoul(args[1], NULL, 0);

+		int last = strtoul(args[2], NULL, 0);

+		int set;

+		int retval;

+		flash_bank_t *p = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+		if (!p)

+		{

+			command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);

+			return ERROR_OK;

+		}

+		

+		if (strcmp(args[3], "on") == 0)

+			set = 1;

+		else if (strcmp(args[3], "off") == 0)

+			set = 0;

+		else

+		{

+			return ERROR_COMMAND_SYNTAX_ERROR;

+		}

+		

+		retval = flash_driver_protect(p, set, first, last);

+		if (retval == ERROR_OK)

+		{

+			command_print(cmd_ctx, "%s protection for sectors %i through %i on flash bank %i", (set) ? "set" : "cleared", first, last, strtoul(args[0], 0, 0));

+		}

+	}

+	else

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+

+	}

+

+	return ERROR_OK;

+}

+

+int handle_flash_write_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	target_t *target = get_current_target(cmd_ctx);

+	

+	image_t image;

+	u32 written;

+	

+	duration_t duration;

+	char *duration_text;

+	

+	int retval;

+

+	if (argc < 1)

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+

+	}

+	

+	if (!target)

+	{

+		ERROR("no target selected");

+		return ERROR_OK;

+	}

+	

+	duration_start_measure(&duration);

+	

+	if (argc >= 2)

+	{

+		image.base_address_set = 1;

+		image.base_address = strtoul(args[1], NULL, 0);

+	}

+	else

+	{

+		image.base_address_set = 0;

+		image.base_address = 0x0;

+	}

+	

+	image.start_address_set = 0;

+

+	retval = image_open(&image, args[0], (argc == 3) ? args[2] : NULL);

+	if (retval != ERROR_OK)

+	{

+		command_print(cmd_ctx, "image_open error: %s", image.error_str);

+		return retval;

+	}

+	

+	retval = flash_write(target, &image, &written, auto_erase);

+

+	if (retval != ERROR_OK)

+	{

+		image_close(&image);

+		return retval;

+	}

+	

+	duration_stop_measure(&duration, &duration_text);

+	if (retval == ERROR_OK)

+	{

+	command_print(cmd_ctx, "wrote %u byte from file %s in %s (%f kb/s)",

+		written, args[0], duration_text,

+		(float)written / 1024.0 / ((float)duration.duration.tv_sec + ((float)duration.duration.tv_usec / 1000000.0)));

+	}

+	free(duration_text);

+

+	image_close(&image);

+	

+	return retval;

+}

+

+int handle_flash_write_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	u32 offset;

+	u8 *buffer;

+	u32 buf_cnt;

+

+	fileio_t fileio;

+	

+	duration_t duration;

+	char *duration_text;

+	

+	int retval;

+	flash_bank_t *p;

+

+	if (argc != 3)

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+	}

+	

+	duration_start_measure(&duration);

+	

+	offset = strtoul(args[2], NULL, 0);

+	p = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+	if (!p)

+	{

+		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);

+		return ERROR_OK;

+	}

+	

+	if (fileio_open(&fileio, args[1], FILEIO_READ, FILEIO_BINARY) != ERROR_OK)

+	{

+		command_print(cmd_ctx, "flash write_binary error: %s", fileio.error_str);

+		return ERROR_OK;

+	}

+	

+	buffer = malloc(fileio.size);

+	if (fileio_read(&fileio, fileio.size, buffer, &buf_cnt) != ERROR_OK)

+	{

+		command_print(cmd_ctx, "flash write_binary error: %s", fileio.error_str);

+		return ERROR_OK;

+	}

+	

+	retval = flash_driver_write(p, buffer, offset, buf_cnt);

+		

+	free(buffer);

+	

+	duration_stop_measure(&duration, &duration_text);

+	if (retval!=ERROR_OK)

+	{

+	command_print(cmd_ctx, "wrote  %"PRIi64" byte from file %s to flash bank %i at offset 0x%8.8x in %s (%f kb/s)",

+		fileio.size, args[1], strtoul(args[0], NULL, 0), offset, duration_text,

+		(float)fileio.size / 1024.0 / ((float)duration.duration.tv_sec + ((float)duration.duration.tv_usec / 1000000.0)));

+	}

+	free(duration_text);

+

+	fileio_close(&fileio);

+	

+	return retval;

+}

+

+void flash_set_dirty(void)

+{

+	flash_bank_t *c;

+	int i;

+	

+	/* set all flash to require erasing */

+	for (c = flash_banks; c; c = c->next)

+	{

+		for (i = 0; i < c->num_sectors; i++)

+		{

+			c->sectors[i].is_erased = 0; 

+		}

+	}

+}

+

+/* lookup flash bank by address */

+flash_bank_t *get_flash_bank_by_addr(target_t *target, u32 addr)

+{

+	flash_bank_t *c;

+

+	/* cycle through bank list */

+	for (c = flash_banks; c; c = c->next)

+	{

+		int retval;

+		retval = c->driver->auto_probe(c);

+		

+		if (retval != ERROR_OK)

+		{

+			ERROR("auto_probe failed %d\n", retval);

+			return NULL;

+		}

+		/* check whether address belongs to this flash bank */

+		if ((addr >= c->base) && (addr < c->base + c->size) && target == c->target)

+			return c;

+	}

+	ERROR("No flash at address 0x%08x\n", addr);

+	return NULL;

+}

+

+/* erase given flash region, selects proper bank according to target and address */

+int flash_erase_address_range(target_t *target, u32 addr, u32 length)

+{

+	flash_bank_t *c;

+	int first = -1;

+	int last = -1;

+	int i;

+	

+	if ((c = get_flash_bank_by_addr(target, addr)) == NULL)

+		return ERROR_FLASH_DST_OUT_OF_BANK; /* no corresponding bank found */

+

+	if (c->size == 0 || c->num_sectors == 0)

+		return ERROR_FLASH_BANK_INVALID;

+	

+	if (length == 0)

+	{

+		/* special case, erase whole bank when length is zero */

+		if (addr != c->base)

+			return ERROR_FLASH_DST_BREAKS_ALIGNMENT;

+		

+		return flash_driver_erase(c, 0, c->num_sectors - 1);

+	}

+

+	/* check whether it fits */

+	if (addr + length > c->base + c->size)

+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;

+	

+	addr -= c->base;

+	

+	for (i = 0; i < c->num_sectors; i++)

+	{		

+		/* check whether sector overlaps with the given range and is not yet erased */

+		if (addr < c->sectors[i].offset + c->sectors[i].size && addr + length > c->sectors[i].offset && c->sectors[i].is_erased != 1) {

+			/* if first is not set yet then this is the first sector */

+			if (first == -1)

+				first = i;

+			last = i; /* and it is the last one so far in any case */

+		}

+	}

+	

+	if( first == -1 || last == -1 )

+		return ERROR_OK;

+	

+	return flash_driver_erase(c, first, last);

+}

+

+/* write (optional verify) an image to flash memory of the given target */

+int flash_write(target_t *target, image_t *image, u32 *written, int erase)

+{

+	int retval;

+

+	int section;

+	u32 section_offset;

+	flash_bank_t *c;

+	

+	section = 0;

+	section_offset = 0;

+

+	if (written)

+		*written = 0;

+	

+	if (erase)

+	{

+		/* assume all sectors need erasing - stops any problems

+		 * when flash_write is called multiple times */

+		

+		flash_set_dirty();

+	}

+	

+	/* loop until we reach end of the image */

+	while (section < image->num_sections)

+	{

+		u32 buffer_size;

+		u8 *buffer;

+		int section_first;

+		int section_last;

+		u32 run_address = image->sections[section].base_address + section_offset;

+		u32 run_size = image->sections[section].size - section_offset;

+

+		if (image->sections[section].size ==  0)

+		{

+			WARNING("empty section %d", section);

+			section++;

+			section_offset = 0;

+			continue;

+		}

+

+		/* find the corresponding flash bank */

+		if ((c = get_flash_bank_by_addr(target, run_address)) == NULL)

+		{

+			section++; /* and skip it */

+			section_offset = 0;

+			continue;

+		}

+

+		/* collect consecutive sections which fall into the same bank */

+		section_first = section;

+		section_last = section;

+		while ((run_address + run_size < c->base + c->size)

+				&& (section_last + 1 < image->num_sections))

+		{

+			if (image->sections[section_last + 1].base_address < (run_address + run_size))

+			{

+				DEBUG("section %d out of order(very slightly surprising, but supported)", section_last + 1);

+				break;

+			}

+			if (image->sections[section_last + 1].base_address != (run_address + run_size))

+				break;

+			run_size += image->sections[++section_last].size;

+		}

+

+		/* fit the run into bank constraints */

+		if (run_address + run_size > c->base + c->size)

+			run_size = c->base + c->size - run_address;

+

+		/* allocate buffer */

+		buffer = malloc(run_size);

+		buffer_size = 0;

+

+		/* read sections to the buffer */

+		while (buffer_size < run_size)

+		{

+			u32 size_read;

+			

+			if (buffer_size - run_size <= image->sections[section].size - section_offset)

+				size_read = buffer_size - run_size;

+			else

+				size_read = image->sections[section].size - section_offset;

+			

+			if ((retval = image_read_section(image, section, section_offset,

+					size_read, buffer + buffer_size, &size_read)) != ERROR_OK || size_read == 0)

+			{

+				free(buffer);

+				

+				return retval;

+			}

+

+			buffer_size += size_read;

+			section_offset += size_read;

+

+			if (section_offset >= image->sections[section].size)

+			{

+				section++;

+				section_offset = 0;

+			}

+		}

+

+		retval = ERROR_OK;

+		

+		if (erase)

+		{

+			/* calculate and erase sectors */

+			retval = flash_erase_address_range( target, run_address, run_size );

+		}

+		

+		if (retval == ERROR_OK)

+		{

+			/* write flash sectors */

+			retval = flash_driver_write(c, buffer, run_address - c->base, run_size);

+		}

+		

+		free(buffer);

+

+		if (retval != ERROR_OK)

+		{

+				return retval; /* abort operation */

+			}

+

+		if (written != NULL)

+			*written += run_size; /* add run size to total written counter */

+	}

+

+	return ERROR_OK;

+}

+

+int handle_flash_auto_erase_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	if (argc != 1)

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+

+	}

+	

+	if (strcmp(args[0], "on") == 0)

+		auto_erase = 1;

+	else if (strcmp(args[0], "off") == 0)

+		auto_erase = 0;

+	else 

+		return ERROR_COMMAND_SYNTAX_ERROR;

+	

+	return ERROR_OK;

+}

diff --git a/src/flash/flash.h b/src/flash/flash.h
index 1ef74c89..e8262efa 100644
--- a/src/flash/flash.h
+++ b/src/flash/flash.h
@@ -1,86 +1,97 @@
-/***************************************************************************
- *   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 FLASH_H
-#define FLASH_H
-
-#include "target.h"
-#include "image.h"
-
-#define FLASH_MAX_ERROR_STR	(128)
-
-typedef struct flash_sector_s
-{
-	u32 offset;
-	u32 size;
-	int is_erased;
-	int is_protected;
-} flash_sector_t;
-
-struct flash_bank_s;
-
-typedef struct flash_driver_s
-{
-	char *name;
-	int (*register_commands)(struct command_context_s *cmd_ctx);
-	int (*flash_bank_command)(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);
-	int (*erase)(struct flash_bank_s *bank, int first, int last);
-	int (*protect)(struct flash_bank_s *bank, int set, int first, int last);
-	int (*write)(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);
-	int (*probe)(struct flash_bank_s *bank);
-	int (*erase_check)(struct flash_bank_s *bank);
-	int (*protect_check)(struct flash_bank_s *bank);
-	int (*info)(struct flash_bank_s *bank, char *buf, int buf_size);
-	int (*auto_probe)(struct flash_bank_s *bank);
-} flash_driver_t;
-
-typedef struct flash_bank_s
-{
-	target_t *target;
-	flash_driver_t *driver;
-	void *driver_priv;
-	u32 base;
-	u32 size;
-	int chip_width;
-	int bus_width;
-	int num_sectors;
-	flash_sector_t *sectors;
-	struct flash_bank_s *next;
-} flash_bank_t;
-
-extern int flash_register_commands(struct command_context_s *cmd_ctx);
-extern int flash_init_drivers(struct command_context_s *cmd_ctx);
-
-extern int flash_erase_address_range(target_t *target, u32 addr, u32 length);
-extern int flash_write(target_t *target, image_t *image, u32 *written, char **error, int *failed, int erase);
-extern void flash_set_dirty(void);
-
-extern flash_bank_t *get_flash_bank_by_num(int num);
-extern flash_bank_t *get_flash_bank_by_addr(target_t *target, u32 addr);
-
-#define		ERROR_FLASH_BANK_INVALID		(-900)
-#define		ERROR_FLASH_SECTOR_INVALID		(-901)
-#define		ERROR_FLASH_OPERATION_FAILED	(-902)
-#define		ERROR_FLASH_DST_OUT_OF_BANK		(-903)
-#define		ERROR_FLASH_DST_BREAKS_ALIGNMENT (-904)
-#define		ERROR_FLASH_BUSY				(-905)
-#define		ERROR_FLASH_SECTOR_NOT_ERASED	(-906)
-#define		ERROR_FLASH_BANK_NOT_PROBED		(-907)
-
-#endif /* FLASH_H */
+/***************************************************************************

+ *   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 FLASH_H

+#define FLASH_H

+

+#include "target.h"

+#include "image.h"

+

+#define FLASH_MAX_ERROR_STR	(128)

+

+typedef struct flash_sector_s

+{

+	u32 offset;

+	u32 size;

+	int is_erased;

+	int is_protected;

+} flash_sector_t;

+

+struct flash_bank_s;

+

+typedef struct flash_driver_s

+{

+	char *name;

+	int (*register_commands)(struct command_context_s *cmd_ctx);

+	int (*flash_bank_command)(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);

+	/* low level flash erase. Only invoke from flash_driver_erase()

+	 * 

+	 * Will only be invoked when target is halted. 

+	 */

+	int (*erase)(struct flash_bank_s *bank, int first, int last);

+	/* invoked only from flash_driver_protect().

+	 *  

+	 * Only invoked if target is halted

+	 */

+	int (*protect)(struct flash_bank_s *bank, int set, int first, int last);

+	/* low level flash write. Will only be invoked if the target is halted.

+	 * use the flash_driver_write() wrapper to invoke.

+	 */

+	int (*write)(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);

+	int (*probe)(struct flash_bank_s *bank);

+	int (*erase_check)(struct flash_bank_s *bank);

+	int (*protect_check)(struct flash_bank_s *bank);

+	int (*info)(struct flash_bank_s *bank, char *buf, int buf_size);

+	int (*auto_probe)(struct flash_bank_s *bank);

+} flash_driver_t;

+

+typedef struct flash_bank_s

+{

+	target_t *target;

+	flash_driver_t *driver;

+	void *driver_priv;

+	u32 base;

+	u32 size;

+	int chip_width;

+	int bus_width;

+	int num_sectors;

+	flash_sector_t *sectors;

+	struct flash_bank_s *next;

+} flash_bank_t;

+

+extern int flash_register_commands(struct command_context_s *cmd_ctx);

+extern int flash_init_drivers(struct command_context_s *cmd_ctx);

+

+extern int flash_erase_address_range(target_t *target, u32 addr, u32 length);

+extern int flash_write(target_t *target, image_t *image, u32 *written, int erase);

+extern void flash_set_dirty(void);

+

+extern flash_bank_t *get_flash_bank_by_num(int num);

+extern flash_bank_t *get_flash_bank_by_addr(target_t *target, u32 addr);

+

+#define		ERROR_FLASH_BANK_INVALID		(-900)

+#define		ERROR_FLASH_SECTOR_INVALID		(-901)

+#define		ERROR_FLASH_OPERATION_FAILED	(-902)

+#define		ERROR_FLASH_DST_OUT_OF_BANK		(-903)

+#define		ERROR_FLASH_DST_BREAKS_ALIGNMENT (-904)

+#define		ERROR_FLASH_BUSY				(-905)

+#define		ERROR_FLASH_SECTOR_NOT_ERASED	(-906)

+#define		ERROR_FLASH_BANK_NOT_PROBED		(-907)

+

+#endif /* FLASH_H */

diff --git a/src/flash/lpc2000.c b/src/flash/lpc2000.c
index 1f689cc5..da79ea73 100644
--- a/src/flash/lpc2000.c
+++ b/src/flash/lpc2000.c
@@ -1,699 +1,689 @@
-/***************************************************************************
- *   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.             *
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include "lpc2000.h"
-
-#include "flash.h"
-#include "target.h"
-#include "log.h"
-#include "armv4_5.h"
-#include "algorithm.h"
-#include "binarybuffer.h"
-
-#include <stdlib.h>
-#include <string.h>
-
-/* flash programming support for Philips 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
- */
-
-int lpc2000_register_commands(struct command_context_s *cmd_ctx);
-int lpc2000_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);
-int lpc2000_erase(struct flash_bank_s *bank, int first, int last);
-int lpc2000_protect(struct flash_bank_s *bank, int set, int first, int last);
-int lpc2000_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);
-int lpc2000_probe(struct flash_bank_s *bank);
-int lpc2000_erase_check(struct flash_bank_s *bank);
-int lpc2000_protect_check(struct flash_bank_s *bank);
-int lpc2000_info(struct flash_bank_s *bank, char *buf, int buf_size);
-	
-int lpc2000_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-flash_driver_t lpc2000_flash =
-{
-	.name = "lpc2000",
-	.register_commands = lpc2000_register_commands,
-	.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
-};
-
-int lpc2000_register_commands(struct command_context_s *cmd_ctx)
-{
-	command_t *lpc2000_cmd = register_command(cmd_ctx, NULL, "lpc2000", NULL, COMMAND_ANY, NULL);
-	
-	register_command(cmd_ctx, lpc2000_cmd, "part_id", lpc2000_handle_part_id_command, COMMAND_EXEC,
-					 "print part id of lpc2000 flash bank <num>");
-	
-	return ERROR_OK;
-}
-
-int lpc2000_build_sector_list(struct flash_bank_s *bank)
-{
-	lpc2000_flash_bank_t *lpc2000_info = bank->driver_priv;
-	
-	/* default to a 4096 write buffer */
-	lpc2000_info->cmd51_max_buffer = 4096;
-	
-	if (lpc2000_info->variant == 1)
-	{
-		int i = 0;
-		u32 offset = 0;
-		
-		/* variant 1 has different layout for 128kb and 256kb flashes */
-		if (bank->size == 128 * 1024)
-		{
-			bank->num_sectors = 16;
-			bank->sectors = malloc(sizeof(flash_sector_t) * 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(flash_sector_t) * 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
-		{
-			ERROR("BUG: unknown bank->size encountered");
-			exit(-1);
-		}
-	}
-	else if (lpc2000_info->variant == 2)
-	{
-		int num_sectors;
-		int i;
-		u32 offset = 0;
-	
-		/* variant 2 has a uniform layout, only number of sectors differs */
-		switch (bank->size)
-		{
-			case 4 * 1024:
-				lpc2000_info->cmd51_max_buffer = 1024;
-				num_sectors = 1;
-				break;
-			case 8 * 1024:
-				lpc2000_info->cmd51_max_buffer = 1024;
-				num_sectors = 2;
-				break;
-			case 16 * 1024:
-				num_sectors = 4;
-				break;
-			case 32 * 1024:
-				num_sectors = 8;
-				break;
-			case 64 * 1024:
-				num_sectors = 9;
-				break;
-			case 128 * 1024:
-				num_sectors = 11;
-				break;
-			case 256 * 1024:
-				num_sectors = 15;
-				break;
-			case 512 * 1024:
-			case 500 * 1024:
-				num_sectors = 27;
-				break;
-			default:
-				ERROR("BUG: unknown bank->size encountered");
-				exit(-1);
-				break;
-		}
-		
-		bank->num_sectors = num_sectors;
-		bank->sectors = malloc(sizeof(flash_sector_t) * num_sectors);
-		
-		for (i = 0; i < 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
-	{
-		ERROR("BUG: unknown lpc2000_info->variant encountered");
-		exit(-1);
-	}
-	
-	return ERROR_OK;
-}
-
-/* call LPC2000 IAP function
- * uses 172 bytes working area
- * 0x0 to 0x7: jump gate (BX to thumb state, b -2 to wait)
- * 0x8 to 0x1f: command parameter table
- * 0x20 to 0x2b: command result table
- * 0x2c to 0xac: stack (only 128b needed)
- */
-int lpc2000_iap_call(flash_bank_t *bank, int code, u32 param_table[5], u32 result_table[2])
-{
-	lpc2000_flash_bank_t *lpc2000_info = bank->driver_priv;
-	target_t *target = bank->target;
-	mem_param_t mem_params[2];
-	reg_param_t reg_params[5];
-	armv4_5_algorithm_t armv4_5_info;
-	u32 status_code;
-	
-	/* regrab previously allocated working_area, or allocate a new one */
-	if (!lpc2000_info->iap_working_area)
-	{
-		u8 jump_gate[8];
-		
-		/* make sure we have a working area */
-		if (target_alloc_working_area(target, 172, &lpc2000_info->iap_working_area) != ERROR_OK)
-		{
-			ERROR("no working area specified, can't write LPC2000 internal flash");
-			return ERROR_FLASH_OPERATION_FAILED;
-		}
-		
-		/* write IAP code to working area */
-		target_buffer_set_u32(target, jump_gate, ARMV4_5_BX(12));
-		target_buffer_set_u32(target, jump_gate + 4, ARMV4_5_B(0xfffffe, 0));
-		target->type->write_memory(target, lpc2000_info->iap_working_area->address, 4, 2, jump_gate);
-	}
-	
-	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;
-	
-	/* command parameter table */
-	init_mem_param(&mem_params[0], lpc2000_info->iap_working_area->address + 8, 4 * 6, PARAM_OUT);
-	target_buffer_set_u32(target, mem_params[0].value, code);
-	target_buffer_set_u32(target, mem_params[0].value + 0x4, param_table[0]);
-	target_buffer_set_u32(target, mem_params[0].value + 0x8, param_table[1]);
-	target_buffer_set_u32(target, mem_params[0].value + 0xc, 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 + 0x8);
-	
-	/* command result table */
-	init_mem_param(&mem_params[1], lpc2000_info->iap_working_area->address + 0x20, 4 * 3, 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, 0x7ffffff1);
-	
-	/* 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 + 0xac);
-
-	/* 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 + 0x4);
-	
-	target->type->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);
-	
-	status_code = buf_get_u32(mem_params[1].value, 0, 32);
-	result_table[0] = target_buffer_get_u32(target, mem_params[1].value);
-	result_table[1] = target_buffer_get_u32(target, mem_params[1].value + 4);
-	
-	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;
-}
-
-int lpc2000_iap_blank_check(struct flash_bank_s *bank, int first, int last)
-{
-	u32 param_table[5];
-	u32 result_table[2];
-	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:
-				ERROR("BUG: unknown LPC2000 status code");
-				exit(-1);
-		}
-	}
-	
-	return ERROR_OK;
-}
-
-/* flash bank lpc2000 <base> <size> 0 0 <target#> <lpc_variant> <cclk> [calc_checksum]
- */
-int lpc2000_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)
-{
-	lpc2000_flash_bank_t *lpc2000_info;
-	
-	if (argc < 8)
-	{
-		WARNING("incomplete flash_bank lpc2000 configuration");
-		return ERROR_FLASH_BANK_INVALID;
-	}
-	
-	lpc2000_info = malloc(sizeof(lpc2000_flash_bank_t));
-	bank->driver_priv = lpc2000_info;
-	
-	if (strcmp(args[6], "lpc2000_v1") == 0)
-	{
-		lpc2000_info->variant = 1;
-		lpc2000_info->cmd51_dst_boundary = 512;
-		lpc2000_info->cmd51_can_256b = 0;
-		lpc2000_info->cmd51_can_8192b = 1;
-	}
-	else if (strcmp(args[6], "lpc2000_v2") == 0)
-	{
-		lpc2000_info->variant = 2;
-		lpc2000_info->cmd51_dst_boundary = 256;
-		lpc2000_info->cmd51_can_256b = 1;
-		lpc2000_info->cmd51_can_8192b = 0;
-	}
-	else
-	{
-		ERROR("unknown LPC2000 variant");
-		free(lpc2000_info);
-		return ERROR_FLASH_BANK_INVALID;
-	}
-	
-	lpc2000_info->iap_working_area = NULL;
-	lpc2000_info->cclk = strtoul(args[7], NULL, 0);
-	lpc2000_info->calc_checksum = 0;
-	lpc2000_build_sector_list(bank);
-		
-	if (argc >= 9)
-	{
-		if (strcmp(args[8], "calc_checksum") == 0)
-			lpc2000_info->calc_checksum = 1;
-	}
-	
-	return ERROR_OK;
-}
-
-int lpc2000_erase(struct flash_bank_s *bank, int first, int last)
-{
-	lpc2000_flash_bank_t *lpc2000_info = bank->driver_priv;
-	u32 param_table[5];
-	u32 result_table[2];
-	int status_code;
-	
-	if (bank->target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	
-	if ((first < 0) || (last < first) || (last >= bank->num_sectors))
-	{
-		return ERROR_FLASH_SECTOR_INVALID;
-	}
-	
-	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:
-			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:
-			WARNING("lpc2000 erase sectors returned %i", status_code);
-			return ERROR_FLASH_OPERATION_FAILED;
-	}
-	
-	return ERROR_OK;
-}
-
-int lpc2000_protect(struct flash_bank_s *bank, int set, int first, int last)
-{
-	/* can't protect/unprotect on the lpc2000 */
-	return ERROR_OK;
-}
-
-int lpc2000_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
-{
-	lpc2000_flash_bank_t *lpc2000_info = bank->driver_priv;
-	target_t *target = bank->target;
-	u32 dst_min_alignment;
-	u32 bytes_remaining = count;
-	u32 bytes_written = 0;
-	int first_sector = 0;
-	int last_sector = 0;
-	u32 param_table[5];
-	u32 result_table[2];
-	int status_code;
-	int i;
-	working_area_t *download_area;
-		 
-	if (bank->target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	
-	/* allocate a working area */
-	if (target_alloc_working_area(target, lpc2000_info->cmd51_max_buffer, &download_area) != ERROR_OK)
-	{
-		ERROR("no working area specified, can't write LPC2000 internal flash");
-		return ERROR_FLASH_OPERATION_FAILED;
-	}
-	
-	if (offset + count > bank->size)
-		return ERROR_FLASH_DST_OUT_OF_BANK;
-	
-	if (lpc2000_info->cmd51_can_256b)
-		dst_min_alignment = 256;
-	else
-		dst_min_alignment = 512;
-	
-	if (offset % dst_min_alignment)
-	{
-		WARNING("offset 0x%x breaks required alignment 0x%x", 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 + CEIL(count, dst_min_alignment) * dst_min_alignment > bank->sectors[i].offset)
-			last_sector = i;
-	}
-	
-	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)
-	{
-		u32 checksum = 0;
-		int i = 0;
-		for (i = 0; i < 8; i++)
-		{
-			DEBUG("0x%2.2x: 0x%8.8x", i * 4, buf_get_u32(buffer + (i * 4), 0, 32));
-			if (i != 5)
-				checksum += buf_get_u32(buffer + (i * 4), 0, 32);
-		}
-		checksum = 0 - checksum;
-		DEBUG("checksum: 0x%8.8x", checksum);
-		buf_set_u32(buffer + 0x14, 0, 32, checksum);
-	}
-	
-	while (bytes_remaining > 0)
-	{
-		u32 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:
-				return ERROR_FLASH_OPERATION_FAILED;
-			case LPC2000_CMD_SUCCESS:
-				break;
-			case LPC2000_INVALID_SECTOR:
-				return ERROR_FLASH_SECTOR_INVALID;
-				break;
-			default:
-				WARNING("lpc2000 prepare sectors returned %i", status_code);
-				return ERROR_FLASH_OPERATION_FAILED;
-		}
-		
-		if (bytes_remaining >= thisrun_bytes)
-		{
-			if (target_write_buffer(bank->target, download_area->address, thisrun_bytes, buffer + bytes_written) != ERROR_OK)
-			{
-				target_free_working_area(target, download_area);
-				return ERROR_FLASH_OPERATION_FAILED;
-			}
-		}
-		else
-		{
-			u8 *last_buffer = malloc(thisrun_bytes);
-			int i;
-			memcpy(last_buffer, buffer + bytes_written, bytes_remaining);
-			for (i = bytes_remaining; i < thisrun_bytes; i++)
-				last_buffer[i] = 0xff;
-			target_write_buffer(bank->target, download_area->address, thisrun_bytes, last_buffer);
-			free(last_buffer);
-		}
-		
-		DEBUG("writing 0x%x bytes to address 0x%x", 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:
-				return ERROR_FLASH_OPERATION_FAILED;
-			case LPC2000_CMD_SUCCESS:
-				break;
-			case LPC2000_INVALID_SECTOR:
-				return ERROR_FLASH_SECTOR_INVALID;
-				break;
-			default:
-				WARNING("lpc2000 returned %i", status_code);
-				return ERROR_FLASH_OPERATION_FAILED;
-		}
-		
-		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 ERROR_OK;
-}
-
-int lpc2000_probe(struct flash_bank_s *bank)
-{
-	/* we can't probe on an lpc2000 
-	 * if this is an lpc2xxx, it has the configured flash
-	 */
-	return ERROR_OK;
-}
-
-int lpc2000_erase_check(struct flash_bank_s *bank)
-{
-	if (bank->target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	
-	return lpc2000_iap_blank_check(bank, 0, bank->num_sectors - 1);
-}
-
-int lpc2000_protect_check(struct flash_bank_s *bank)
-{
-	/* sectors are always protected	*/
-	return ERROR_OK;
-}
-
-int lpc2000_info(struct flash_bank_s *bank, char *buf, int buf_size)
-{
-	lpc2000_flash_bank_t *lpc2000_info = bank->driver_priv;
-
-	snprintf(buf, buf_size, "lpc2000 flash driver variant: %i, clk: %i", lpc2000_info->variant, lpc2000_info->cclk);
-	
-	return ERROR_OK;
-}
-
-int lpc2000_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	flash_bank_t *bank;
-	u32 param_table[5];
-	u32 result_table[2];
-	int status_code;
-	lpc2000_flash_bank_t *lpc2000_info;
-
-	if (argc < 1)
-	{
-		command_print(cmd_ctx, "usage: lpc2000 part_id <num>");
-		return ERROR_OK;
-	}
-	
-	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-	if (!bank)
-	{
-		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-		return ERROR_OK;
-	}
-
-	lpc2000_info = bank->driver_priv;
-	if (bank->target->state != TARGET_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.8x", result_table[0]);
-	}
-	
-	return ERROR_OK;
-}
+/***************************************************************************

+ *   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.             *

+ ***************************************************************************/

+#ifdef HAVE_CONFIG_H

+#include "config.h"

+#endif

+

+#include "lpc2000.h"

+

+#include "flash.h"

+#include "target.h"

+#include "log.h"

+#include "armv4_5.h"

+#include "algorithm.h"

+#include "binarybuffer.h"

+

+#include <stdlib.h>

+#include <string.h>

+

+/* flash programming support for Philips 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

+ */

+

+int lpc2000_register_commands(struct command_context_s *cmd_ctx);

+int lpc2000_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);

+int lpc2000_erase(struct flash_bank_s *bank, int first, int last);

+int lpc2000_protect(struct flash_bank_s *bank, int set, int first, int last);

+int lpc2000_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);

+int lpc2000_probe(struct flash_bank_s *bank);

+int lpc2000_erase_check(struct flash_bank_s *bank);

+int lpc2000_protect_check(struct flash_bank_s *bank);

+int lpc2000_info(struct flash_bank_s *bank, char *buf, int buf_size);

+	

+int lpc2000_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+

+flash_driver_t lpc2000_flash =

+{

+	.name = "lpc2000",

+	.register_commands = lpc2000_register_commands,

+	.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

+};

+

+int lpc2000_register_commands(struct command_context_s *cmd_ctx)

+{

+	command_t *lpc2000_cmd = register_command(cmd_ctx, NULL, "lpc2000", NULL, COMMAND_ANY, NULL);

+	

+	register_command(cmd_ctx, lpc2000_cmd, "part_id", lpc2000_handle_part_id_command, COMMAND_EXEC,

+					 "print part id of lpc2000 flash bank <num>");

+	

+	return ERROR_OK;

+}

+

+int lpc2000_build_sector_list(struct flash_bank_s *bank)

+{

+	lpc2000_flash_bank_t *lpc2000_info = bank->driver_priv;

+	

+	/* default to a 4096 write buffer */

+	lpc2000_info->cmd51_max_buffer = 4096;

+	

+	if (lpc2000_info->variant == 1)

+	{

+		int i = 0;

+		u32 offset = 0;

+		

+		/* variant 1 has different layout for 128kb and 256kb flashes */

+		if (bank->size == 128 * 1024)

+		{

+			bank->num_sectors = 16;

+			bank->sectors = malloc(sizeof(flash_sector_t) * 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(flash_sector_t) * 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

+		{

+			ERROR("BUG: unknown bank->size encountered");

+			exit(-1);

+		}

+	}

+	else if (lpc2000_info->variant == 2)

+	{

+		int num_sectors;

+		int i;

+		u32 offset = 0;

+	

+		/* variant 2 has a uniform layout, only number of sectors differs */

+		switch (bank->size)

+		{

+			case 4 * 1024:

+				lpc2000_info->cmd51_max_buffer = 1024;

+				num_sectors = 1;

+				break;

+			case 8 * 1024:

+				lpc2000_info->cmd51_max_buffer = 1024;

+				num_sectors = 2;

+				break;

+			case 16 * 1024:

+				num_sectors = 4;

+				break;

+			case 32 * 1024:

+				num_sectors = 8;

+				break;

+			case 64 * 1024:

+				num_sectors = 9;

+				break;

+			case 128 * 1024:

+				num_sectors = 11;

+				break;

+			case 256 * 1024:

+				num_sectors = 15;

+				break;

+			case 512 * 1024:

+			case 500 * 1024:

+				num_sectors = 27;

+				break;

+			default:

+				ERROR("BUG: unknown bank->size encountered");

+				exit(-1);

+				break;

+		}

+		

+		bank->num_sectors = num_sectors;

+		bank->sectors = malloc(sizeof(flash_sector_t) * num_sectors);

+		

+		for (i = 0; i < 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

+	{

+		ERROR("BUG: unknown lpc2000_info->variant encountered");

+		exit(-1);

+	}

+	

+	return ERROR_OK;

+}

+

+/* call LPC2000 IAP function

+ * uses 172 bytes working area

+ * 0x0 to 0x7: jump gate (BX to thumb state, b -2 to wait)

+ * 0x8 to 0x1f: command parameter table

+ * 0x20 to 0x2b: command result table

+ * 0x2c to 0xac: stack (only 128b needed)

+ */

+int lpc2000_iap_call(flash_bank_t *bank, int code, u32 param_table[5], u32 result_table[2])

+{

+	lpc2000_flash_bank_t *lpc2000_info = bank->driver_priv;

+	target_t *target = bank->target;

+	mem_param_t mem_params[2];

+	reg_param_t reg_params[5];

+	armv4_5_algorithm_t armv4_5_info;

+	u32 status_code;

+	

+	/* regrab previously allocated working_area, or allocate a new one */

+	if (!lpc2000_info->iap_working_area)

+	{

+		u8 jump_gate[8];

+		

+		/* make sure we have a working area */

+		if (target_alloc_working_area(target, 172, &lpc2000_info->iap_working_area) != ERROR_OK)

+		{

+			ERROR("no working area specified, can't write LPC2000 internal flash");

+			return ERROR_FLASH_OPERATION_FAILED;

+		}

+		

+		/* write IAP code to working area */

+		target_buffer_set_u32(target, jump_gate, ARMV4_5_BX(12));

+		target_buffer_set_u32(target, jump_gate + 4, ARMV4_5_B(0xfffffe, 0));

+		target->type->write_memory(target, lpc2000_info->iap_working_area->address, 4, 2, jump_gate);

+	}

+	

+	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;

+	

+	/* command parameter table */

+	init_mem_param(&mem_params[0], lpc2000_info->iap_working_area->address + 8, 4 * 6, PARAM_OUT);

+	target_buffer_set_u32(target, mem_params[0].value, code);

+	target_buffer_set_u32(target, mem_params[0].value + 0x4, param_table[0]);

+	target_buffer_set_u32(target, mem_params[0].value + 0x8, param_table[1]);

+	target_buffer_set_u32(target, mem_params[0].value + 0xc, 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 + 0x8);

+	

+	/* command result table */

+	init_mem_param(&mem_params[1], lpc2000_info->iap_working_area->address + 0x20, 4 * 3, 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, 0x7ffffff1);

+	

+	/* 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 + 0xac);

+

+	/* 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 + 0x4);

+	

+	target->type->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);

+	

+	status_code = buf_get_u32(mem_params[1].value, 0, 32);

+	result_table[0] = target_buffer_get_u32(target, mem_params[1].value);

+	result_table[1] = target_buffer_get_u32(target, mem_params[1].value + 4);

+	

+	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;

+}

+

+int lpc2000_iap_blank_check(struct flash_bank_s *bank, int first, int last)

+{

+	u32 param_table[5];

+	u32 result_table[2];

+	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:

+				ERROR("BUG: unknown LPC2000 status code");

+				exit(-1);

+		}

+	}

+	

+	return ERROR_OK;

+}

+

+/* flash bank lpc2000 <base> <size> 0 0 <target#> <lpc_variant> <cclk> [calc_checksum]

+ */

+int lpc2000_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)

+{

+	lpc2000_flash_bank_t *lpc2000_info;

+	

+	if (argc < 8)

+	{

+		WARNING("incomplete flash_bank lpc2000 configuration");

+		return ERROR_FLASH_BANK_INVALID;

+	}

+	

+	lpc2000_info = malloc(sizeof(lpc2000_flash_bank_t));

+	bank->driver_priv = lpc2000_info;

+	

+	if (strcmp(args[6], "lpc2000_v1") == 0)

+	{

+		lpc2000_info->variant = 1;

+		lpc2000_info->cmd51_dst_boundary = 512;

+		lpc2000_info->cmd51_can_256b = 0;

+		lpc2000_info->cmd51_can_8192b = 1;

+	}

+	else if (strcmp(args[6], "lpc2000_v2") == 0)

+	{

+		lpc2000_info->variant = 2;

+		lpc2000_info->cmd51_dst_boundary = 256;

+		lpc2000_info->cmd51_can_256b = 1;

+		lpc2000_info->cmd51_can_8192b = 0;

+	}

+	else

+	{

+		ERROR("unknown LPC2000 variant");

+		free(lpc2000_info);

+		return ERROR_FLASH_BANK_INVALID;

+	}

+	

+	lpc2000_info->iap_working_area = NULL;

+	lpc2000_info->cclk = strtoul(args[7], NULL, 0);

+	lpc2000_info->calc_checksum = 0;

+	lpc2000_build_sector_list(bank);

+		

+	if (argc >= 9)

+	{

+		if (strcmp(args[8], "calc_checksum") == 0)

+			lpc2000_info->calc_checksum = 1;

+	}

+	

+	return ERROR_OK;

+}

+

+int lpc2000_erase(struct flash_bank_s *bank, int first, int last)

+{

+	lpc2000_flash_bank_t *lpc2000_info = bank->driver_priv;

+	u32 param_table[5];

+	u32 result_table[2];

+	int status_code;

+	

+	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:

+			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:

+			WARNING("lpc2000 erase sectors returned %i", status_code);

+			return ERROR_FLASH_OPERATION_FAILED;

+	}

+	

+	return ERROR_OK;

+}

+

+int lpc2000_protect(struct flash_bank_s *bank, int set, int first, int last)

+{

+	/* can't protect/unprotect on the lpc2000 */

+	return ERROR_OK;

+}

+

+int lpc2000_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)

+{

+	lpc2000_flash_bank_t *lpc2000_info = bank->driver_priv;

+	target_t *target = bank->target;

+	u32 dst_min_alignment;

+	u32 bytes_remaining = count;

+	u32 bytes_written = 0;

+	int first_sector = 0;

+	int last_sector = 0;

+	u32 param_table[5];

+	u32 result_table[2];

+	int status_code;

+	int i;

+	working_area_t *download_area;

+		 

+	if (bank->target->state != TARGET_HALTED)

+	{

+		return ERROR_TARGET_NOT_HALTED;

+	}

+	

+	/* allocate a working area */

+	if (target_alloc_working_area(target, lpc2000_info->cmd51_max_buffer, &download_area) != ERROR_OK)

+	{

+		ERROR("no working area specified, can't write LPC2000 internal flash");

+		return ERROR_FLASH_OPERATION_FAILED;

+	}

+	

+	if (offset + count > bank->size)

+		return ERROR_FLASH_DST_OUT_OF_BANK;

+	

+	if (lpc2000_info->cmd51_can_256b)

+		dst_min_alignment = 256;

+	else

+		dst_min_alignment = 512;

+	

+	if (offset % dst_min_alignment)

+	{

+		WARNING("offset 0x%x breaks required alignment 0x%x", 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 + CEIL(count, dst_min_alignment) * dst_min_alignment > bank->sectors[i].offset)

+			last_sector = i;

+	}

+	

+	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)

+	{

+		u32 checksum = 0;

+		int i = 0;

+		for (i = 0; i < 8; i++)

+		{

+			DEBUG("0x%2.2x: 0x%8.8x", i * 4, buf_get_u32(buffer + (i * 4), 0, 32));

+			if (i != 5)

+				checksum += buf_get_u32(buffer + (i * 4), 0, 32);

+		}

+		checksum = 0 - checksum;

+		DEBUG("checksum: 0x%8.8x", checksum);

+		buf_set_u32(buffer + 0x14, 0, 32, checksum);

+	}

+	

+	while (bytes_remaining > 0)

+	{

+		u32 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:

+				return ERROR_FLASH_OPERATION_FAILED;

+			case LPC2000_CMD_SUCCESS:

+				break;

+			case LPC2000_INVALID_SECTOR:

+				return ERROR_FLASH_SECTOR_INVALID;

+				break;

+			default:

+				WARNING("lpc2000 prepare sectors returned %i", status_code);

+				return ERROR_FLASH_OPERATION_FAILED;

+		}

+		

+		if (bytes_remaining >= thisrun_bytes)

+		{

+			if (target_write_buffer(bank->target, download_area->address, thisrun_bytes, buffer + bytes_written) != ERROR_OK)

+			{

+				target_free_working_area(target, download_area);

+				return ERROR_FLASH_OPERATION_FAILED;

+			}

+		}

+		else

+		{

+			u8 *last_buffer = malloc(thisrun_bytes);

+			int i;

+			memcpy(last_buffer, buffer + bytes_written, bytes_remaining);

+			for (i = bytes_remaining; i < thisrun_bytes; i++)

+				last_buffer[i] = 0xff;

+			target_write_buffer(bank->target, download_area->address, thisrun_bytes, last_buffer);

+			free(last_buffer);

+		}

+		

+		DEBUG("writing 0x%x bytes to address 0x%x", 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:

+				return ERROR_FLASH_OPERATION_FAILED;

+			case LPC2000_CMD_SUCCESS:

+				break;

+			case LPC2000_INVALID_SECTOR:

+				return ERROR_FLASH_SECTOR_INVALID;

+				break;

+			default:

+				WARNING("lpc2000 returned %i", status_code);

+				return ERROR_FLASH_OPERATION_FAILED;

+		}

+		

+		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 ERROR_OK;

+}

+

+int lpc2000_probe(struct flash_bank_s *bank)

+{

+	/* we can't probe on an lpc2000 

+	 * if this is an lpc2xxx, it has the configured flash

+	 */

+	return ERROR_OK;

+}

+

+int lpc2000_erase_check(struct flash_bank_s *bank)

+{

+	if (bank->target->state != TARGET_HALTED)

+	{

+		return ERROR_TARGET_NOT_HALTED;

+	}

+	

+	return lpc2000_iap_blank_check(bank, 0, bank->num_sectors - 1);

+}

+

+int lpc2000_protect_check(struct flash_bank_s *bank)

+{

+	/* sectors are always protected	*/

+	return ERROR_OK;

+}

+

+int lpc2000_info(struct flash_bank_s *bank, char *buf, int buf_size)

+{

+	lpc2000_flash_bank_t *lpc2000_info = bank->driver_priv;

+

+	snprintf(buf, buf_size, "lpc2000 flash driver variant: %i, clk: %i", lpc2000_info->variant, lpc2000_info->cclk);

+	

+	return ERROR_OK;

+}

+

+int lpc2000_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	flash_bank_t *bank;

+	u32 param_table[5];

+	u32 result_table[2];

+	int status_code;

+	lpc2000_flash_bank_t *lpc2000_info;

+

+	if (argc < 1)

+	{

+		command_print(cmd_ctx, "usage: lpc2000 part_id <num>");

+		return ERROR_OK;

+	}

+	

+	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+	if (!bank)

+	{

+		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);

+		return ERROR_OK;

+	}

+

+	lpc2000_info = bank->driver_priv;

+	if (bank->target->state != TARGET_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.8x", result_table[0]);

+	}

+	

+	return ERROR_OK;

+}

diff --git a/src/flash/lpc3180_nand_controller.c b/src/flash/lpc3180_nand_controller.c
index 2ef6f40b..4e1fd2e7 100644
--- a/src/flash/lpc3180_nand_controller.c
+++ b/src/flash/lpc3180_nand_controller.c
@@ -1,916 +1,915 @@
-/***************************************************************************
- *   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.             *
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include "lpc3180_nand_controller.h"
-
-#include "replacements.h"
-#include "log.h"
-
-#include <stdlib.h>
-#include <string.h>
-
-#include "nand.h"
-#include "target.h"
-
-int lpc3180_nand_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct nand_device_s *device);
-int lpc3180_register_commands(struct command_context_s *cmd_ctx);
-int lpc3180_init(struct nand_device_s *device);
-int lpc3180_reset(struct nand_device_s *device);
-int lpc3180_command(struct nand_device_s *device, u8 command);
-int lpc3180_address(struct nand_device_s *device, u8 address);
-int lpc3180_write_data(struct nand_device_s *device, u16 data);
-int lpc3180_read_data(struct nand_device_s *device, void *data);
-int lpc3180_write_page(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size);
-int lpc3180_read_page(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size);
-int lpc3180_controller_ready(struct nand_device_s *device, int timeout);
-int lpc3180_nand_ready(struct nand_device_s *device, int timeout);
-
-int handle_lpc3180_select_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-nand_flash_controller_t lpc3180_nand_controller =
-{
-	.name = "lpc3180",
-	.nand_device_command = lpc3180_nand_device_command,
-	.register_commands = lpc3180_register_commands,
-	.init = lpc3180_init,
-	.reset = lpc3180_reset,
-	.command = lpc3180_command,
-	.address = lpc3180_address,
-	.write_data = lpc3180_write_data,
-	.read_data = lpc3180_read_data,
-	.write_page = lpc3180_write_page,
-	.read_page = lpc3180_read_page,
-	.controller_ready = lpc3180_controller_ready,
-	.nand_ready = lpc3180_nand_ready,
-};
-
-/* nand device lpc3180 <target#> <oscillator_frequency>
- */
-int lpc3180_nand_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct nand_device_s *device)
-{
-	lpc3180_nand_controller_t *lpc3180_info;
-		
-	if (argc < 3)
-	{
-		WARNING("incomplete 'lpc3180' nand flash configuration");
-		return ERROR_FLASH_BANK_INVALID;
-	}
-	
-	lpc3180_info = malloc(sizeof(lpc3180_nand_controller_t));
-	device->controller_priv = lpc3180_info;
-
-	lpc3180_info->target = get_target_by_num(strtoul(args[1], NULL, 0));
-	if (!lpc3180_info->target)
-	{
-		ERROR("no target '%s' configured", args[1]);
-		return ERROR_NAND_DEVICE_INVALID;
-	}
-
-	lpc3180_info->osc_freq = strtoul(args[2], NULL, 0);
-	if ((lpc3180_info->osc_freq < 1000) || (lpc3180_info->osc_freq > 20000))
-	{
-		WARNING("LPC3180 oscillator frequency should be between 1000 and 20000 kHz, was %i", lpc3180_info->osc_freq); 
-	}
-	lpc3180_info->selected_controller = LPC3180_NO_CONTROLLER;
-	lpc3180_info->sw_write_protection = 0;
-	lpc3180_info->sw_wp_lower_bound = 0x0;
-	lpc3180_info->sw_wp_upper_bound = 0x0;
-		
-	return ERROR_OK;
-}
-
-int lpc3180_register_commands(struct command_context_s *cmd_ctx)
-{
-	command_t *lpc3180_cmd = register_command(cmd_ctx, NULL, "lpc3180", NULL, COMMAND_ANY, "commands specific to the LPC3180 NAND flash controllers");
-	
-	register_command(cmd_ctx, lpc3180_cmd, "select", handle_lpc3180_select_command, COMMAND_EXEC, "select <'mlc'|'slc'> controller (default is mlc)");
-	
-	return ERROR_OK;
-}
-
-int lpc3180_pll(int fclkin, u32 pll_ctrl)
-{
-	int bypass = (pll_ctrl & 0x8000) >> 15;
-	int direct = (pll_ctrl & 0x4000) >> 14;
-	int feedback = (pll_ctrl & 0x2000) >> 13;
-	int p = (1 << ((pll_ctrl & 0x1800) >> 11) * 2);
-	int n = ((pll_ctrl & 0x0600) >> 9) + 1;
-	int m = ((pll_ctrl & 0x01fe) >> 1) + 1;
-	int lock = (pll_ctrl & 0x1);
-
-	if (!lock)
-		WARNING("PLL is not locked");
-	
-	if (!bypass && direct)	/* direct mode */
-		return (m * fclkin) / n;
-	
-	if (bypass && !direct)	/* bypass mode */
-		return fclkin / (2 * p);
-		
-	if (bypass & direct)	/* direct bypass mode */
-		return fclkin;
-	
-	if (feedback)			/* integer mode */
-		return m * (fclkin / n);
-	else					/* non-integer mode */
-		return (m / (2 * p)) * (fclkin / n); 
-}
-
-float lpc3180_cycle_time(lpc3180_nand_controller_t *lpc3180_info)
-{
-	target_t *target = lpc3180_info->target;
-	u32 sysclk_ctrl, pwr_ctrl, hclkdiv_ctrl, hclkpll_ctrl;
-	int sysclk;
-	int hclk;
-	int hclk_pll;
-	float cycle;
-	
-	/* calculate timings */
-	
-	/* determine current SYSCLK (13'MHz or main oscillator) */ 
-	target_read_u32(target, 0x40004050, &sysclk_ctrl);
-	
-	if ((sysclk_ctrl & 1) == 0)
-		sysclk = lpc3180_info->osc_freq;
-	else
-		sysclk = 13000;
-	
-	/* determine selected HCLK source */
-	target_read_u32(target, 0x40004044, &pwr_ctrl);
-	
-	if ((pwr_ctrl & (1 << 2)) == 0) /* DIRECT RUN mode */
-	{
-		hclk = sysclk;
-	}
-	else
-	{
-		target_read_u32(target, 0x40004058, &hclkpll_ctrl);
-		hclk_pll = lpc3180_pll(sysclk, hclkpll_ctrl);
-
-		target_read_u32(target, 0x40004040, &hclkdiv_ctrl);
-		
-		if (pwr_ctrl & (1 << 10)) /* ARM_CLK and HCLK use PERIPH_CLK */
-		{
-			hclk = hclk_pll / (((hclkdiv_ctrl & 0x7c) >> 2) + 1);
-		}
-		else /* HCLK uses HCLK_PLL */
-		{
-			hclk = hclk_pll / (1 << (hclkdiv_ctrl & 0x3)); 
-		}
-	}
-	
-	DEBUG("LPC3180 HCLK currently clocked at %i kHz", hclk);
-	
-	cycle = (1.0 / hclk) * 1000000.0;
-	
-	return cycle;
-}
-
-int lpc3180_init(struct nand_device_s *device)
-{
-	lpc3180_nand_controller_t *lpc3180_info = device->controller_priv;
-	target_t *target = lpc3180_info->target;
-	int bus_width = (device->bus_width) ? (device->bus_width) : 8;
-	int address_cycles = (device->address_cycles) ? (device->address_cycles) : 3;
-	int page_size = (device->page_size) ? (device->page_size) : 512;
-		
-	if (target->state != TARGET_HALTED)
-	{
-		ERROR("target must be halted to use LPC3180 NAND flash controller");
-		return ERROR_NAND_OPERATION_FAILED;
-	}
-	
-	/* sanitize arguments */
-	if ((bus_width != 8) && (bus_width != 16))
-	{
-		ERROR("LPC3180 only supports 8 or 16 bit bus width, not %i", bus_width);
-		return ERROR_NAND_OPERATION_NOT_SUPPORTED;
-	}
-	
-	/* The LPC3180 only brings out 8 bit NAND data bus, but the controller
-	 * would support 16 bit, too, so we just warn about this for now
-	 */
-	if (bus_width == 16)
-	{
-		WARNING("LPC3180 only supports 8 bit bus width");
-	}
-	
-	/* inform calling code about selected bus width */
-	device->bus_width = bus_width;
-	
-	if ((address_cycles != 3) && (address_cycles != 4))
-	{
-		ERROR("LPC3180 only supports 3 or 4 address cycles, not %i", address_cycles);
-		return ERROR_NAND_OPERATION_NOT_SUPPORTED;
-	}
-	
-	if ((page_size != 512) && (page_size != 2048))
-	{
-		ERROR("LPC3180 only supports 512 or 2048 byte pages, not %i", page_size);
-		return ERROR_NAND_OPERATION_NOT_SUPPORTED;
-	}
-	
-	/* select MLC controller if none is currently selected */
-	if (lpc3180_info->selected_controller == LPC3180_NO_CONTROLLER)
-	{
-		DEBUG("no LPC3180 NAND flash controller selected, using default 'mlc'");
-		lpc3180_info->selected_controller = LPC3180_MLC_CONTROLLER;
-	}
-	
-	if (lpc3180_info->selected_controller == LPC3180_MLC_CONTROLLER)
-	{
-		u32 mlc_icr_value = 0x0;
-		float cycle;
-		int twp, twh, trp, treh, trhz, trbwb, tcea;
-		
-		/* FLASHCLK_CTRL = 0x22 (enable clock for MLC flash controller) */
-		target_write_u32(target, 0x400040c8, 0x22);
-		
-		/* MLC_CEH = 0x0 (Force nCE assert) */
-		target_write_u32(target, 0x200b804c, 0x0);
-		
-		/* MLC_LOCK = 0xa25e (unlock protected registers) */
-		target_write_u32(target, 0x200b8044, 0xa25e);
-		
-		/* MLC_ICR = configuration */
-		if (lpc3180_info->sw_write_protection)
-			mlc_icr_value |= 0x8;
-		if (page_size == 2048)
-			mlc_icr_value |= 0x4;
-		if (address_cycles == 4)
-			mlc_icr_value |= 0x2;
-		if (bus_width == 16)
-			mlc_icr_value |= 0x1;
-		target_write_u32(target, 0x200b8030, mlc_icr_value);
-		
-		/* calculate NAND controller timings */
-		cycle = lpc3180_cycle_time(lpc3180_info);
-		
-		twp = ((40 / cycle) + 1);
-		twh = ((20 / cycle) + 1);
-		trp = ((30 / cycle) + 1);
-		treh = ((15 / cycle) + 1);
-		trhz = ((30 / cycle) + 1);
-		trbwb = ((100 / cycle) + 1);
-		tcea = ((45 / cycle) + 1);
-				
-		/* MLC_LOCK = 0xa25e (unlock protected registers) */
-		target_write_u32(target, 0x200b8044, 0xa25e);
-	
-		/* MLC_TIME_REG */
-		target_write_u32(target, 0x200b8034, (twp & 0xf) | ((twh & 0xf) << 4) | 
-			((trp & 0xf) << 8) | ((treh & 0xf) << 12) | ((trhz & 0x7) << 16) | 
-			((trbwb & 0x1f) << 19) | ((tcea & 0x3) << 24)); 
-
-		lpc3180_reset(device);
-	}
-	else if (lpc3180_info->selected_controller == LPC3180_SLC_CONTROLLER)
-	{
-		float cycle;
-		int r_setup, r_hold, r_width, r_rdy;
-		int w_setup, w_hold, w_width, w_rdy;
-		
-		/* FLASHCLK_CTRL = 0x05 (enable clock for SLC flash controller) */
-		target_write_u32(target, 0x400040c8, 0x05);
-		
-		/* SLC_CFG = 0x (Force nCE assert, ECC enabled, WIDTH = bus_width) */
-		target_write_u32(target, 0x20020014, 0x28 | (bus_width == 16) ? 1 : 0);
-		
-		/* calculate NAND controller timings */
-		cycle = lpc3180_cycle_time(lpc3180_info);
-		
-		r_setup = w_setup = 0;
-		r_hold = w_hold = 10 / cycle;
-		r_width = 30 / cycle;
-		w_width = 40 / cycle;
-		r_rdy = w_rdy = 100 / cycle;
-		
-		/* SLC_TAC: SLC timing arcs register */
-		target_write_u32(target, 0x2002002c, (r_setup & 0xf) | ((r_hold & 0xf) << 4) |
-			((r_width & 0xf) << 8) | ((r_rdy & 0xf) << 12) |  ((w_setup & 0xf) << 16) |
-			((w_hold & 0xf) << 20) | ((w_width & 0xf) << 24) | ((w_rdy & 0xf) << 28)); 
-		
-		lpc3180_reset(device);
-	}
-	
-	return ERROR_OK;
-}
-
-int lpc3180_reset(struct nand_device_s *device)
-{
-	lpc3180_nand_controller_t *lpc3180_info = device->controller_priv;
-	target_t *target = lpc3180_info->target;
-	
-	if (target->state != TARGET_HALTED)
-	{
-		ERROR("target must be halted to use LPC3180 NAND flash controller");
-		return ERROR_NAND_OPERATION_FAILED;
-	}
-	
-	if (lpc3180_info->selected_controller == LPC3180_NO_CONTROLLER)
-	{
-		ERROR("BUG: no LPC3180 NAND flash controller selected");
-		return ERROR_NAND_OPERATION_FAILED;
-	}
-	else if (lpc3180_info->selected_controller == LPC3180_MLC_CONTROLLER)
-	{
-		/* MLC_CMD = 0xff (reset controller and NAND device) */
-		target_write_u32(target, 0x200b8000, 0xff);
-
-		if (!lpc3180_controller_ready(device, 100))
-		{
-			ERROR("LPC3180 NAND controller timed out after reset");
-			return ERROR_NAND_OPERATION_TIMEOUT;
-		}
-	}
-	else if (lpc3180_info->selected_controller == LPC3180_SLC_CONTROLLER)
-	{
-		/* SLC_CTRL = 0x6 (ECC_CLEAR, SW_RESET) */
-		target_write_u32(target, 0x20020010, 0x6);
-		
-		if (!lpc3180_controller_ready(device, 100))
-		{
-			ERROR("LPC3180 NAND controller timed out after reset");
-			return ERROR_NAND_OPERATION_TIMEOUT;
-		}
-	}
-	
-	return ERROR_OK;
-}
-
-int lpc3180_command(struct nand_device_s *device, u8 command)
-{
-	lpc3180_nand_controller_t *lpc3180_info = device->controller_priv;
-	target_t *target = lpc3180_info->target;
-	
-	if (target->state != TARGET_HALTED)
-	{
-		ERROR("target must be halted to use LPC3180 NAND flash controller");
-		return ERROR_NAND_OPERATION_FAILED;
-	}
-	
-	if (lpc3180_info->selected_controller == LPC3180_NO_CONTROLLER)
-	{
-		ERROR("BUG: no LPC3180 NAND flash controller selected");
-		return ERROR_NAND_OPERATION_FAILED;
-	}
-	else if (lpc3180_info->selected_controller == LPC3180_MLC_CONTROLLER)
-	{
-		/* MLC_CMD = command */
-		target_write_u32(target, 0x200b8000, command);
-	}
-	else if (lpc3180_info->selected_controller == LPC3180_SLC_CONTROLLER)
-	{
-		/* SLC_CMD = command */
-		target_write_u32(target, 0x20020008, command);
-	}	
-	
-	return ERROR_OK;
-}
-
-int lpc3180_address(struct nand_device_s *device, u8 address)
-{
-	lpc3180_nand_controller_t *lpc3180_info = device->controller_priv;
-	target_t *target = lpc3180_info->target;
-	
-	if (target->state != TARGET_HALTED)
-	{
-		ERROR("target must be halted to use LPC3180 NAND flash controller");
-		return ERROR_NAND_OPERATION_FAILED;
-	}
-	
-	if (lpc3180_info->selected_controller == LPC3180_NO_CONTROLLER)
-	{
-		ERROR("BUG: no LPC3180 NAND flash controller selected");
-		return ERROR_NAND_OPERATION_FAILED;
-	}
-	else if (lpc3180_info->selected_controller == LPC3180_MLC_CONTROLLER)
-	{
-		/* MLC_ADDR = address */
-		target_write_u32(target, 0x200b8004, address);
-	}
-	else if (lpc3180_info->selected_controller == LPC3180_SLC_CONTROLLER)
-	{
-		/* SLC_ADDR = address */
-		target_write_u32(target, 0x20020004, address);
-	}
-		
-	return ERROR_OK;
-}
-
-int lpc3180_write_data(struct nand_device_s *device, u16 data)
-{
-	lpc3180_nand_controller_t *lpc3180_info = device->controller_priv;
-	target_t *target = lpc3180_info->target;
-	
-	if (target->state != TARGET_HALTED)
-	{
-		ERROR("target must be halted to use LPC3180 NAND flash controller");
-		return ERROR_NAND_OPERATION_FAILED;
-	}
-	
-	if (lpc3180_info->selected_controller == LPC3180_NO_CONTROLLER)
-	{
-		ERROR("BUG: no LPC3180 NAND flash controller selected");
-		return ERROR_NAND_OPERATION_FAILED;
-	}
-	else if (lpc3180_info->selected_controller == LPC3180_MLC_CONTROLLER)
-	{
-		/* MLC_DATA = data */
-		target_write_u32(target, 0x200b0000, data);
-	}
-	else if (lpc3180_info->selected_controller == LPC3180_SLC_CONTROLLER)
-	{
-		/* SLC_DATA = data */
-		target_write_u32(target, 0x20020000, data);
-	}
-	
-	return ERROR_OK;
-}
-
-int lpc3180_read_data(struct nand_device_s *device, void *data)
-{
-	lpc3180_nand_controller_t *lpc3180_info = device->controller_priv;
-	target_t *target = lpc3180_info->target;
-	
-	if (target->state != TARGET_HALTED)
-	{
-		ERROR("target must be halted to use LPC3180 NAND flash controller");
-		return ERROR_NAND_OPERATION_FAILED;
-	}
-	
-	if (lpc3180_info->selected_controller == LPC3180_NO_CONTROLLER)
-	{
-		ERROR("BUG: no LPC3180 NAND flash controller selected");
-		return ERROR_NAND_OPERATION_FAILED;
-	}
-	else if (lpc3180_info->selected_controller == LPC3180_MLC_CONTROLLER)
-	{
-		/* data = MLC_DATA, use sized access */
-		if (device->bus_width == 8)
-		{
-			u8 *data8 = data;
-			target_read_u8(target, 0x200b0000, data8);
-		}
-		else if (device->bus_width == 16)
-		{
-			u16 *data16 = data;
-			target_read_u16(target, 0x200b0000, data16);
-		}
-		else
-		{
-			ERROR("BUG: bus_width neither 8 nor 16 bit");
-			return ERROR_NAND_OPERATION_FAILED;
-		}
-	}
-	else if (lpc3180_info->selected_controller == LPC3180_SLC_CONTROLLER)
-	{
-		u32 data32;
-
-		/* data = SLC_DATA, must use 32-bit access */
-		target_read_u32(target, 0x20020000, &data32);
-		
-		if (device->bus_width == 8)
-		{
-			u8 *data8 = data;
-			*data8 = data32 & 0xff;
-		}
-		else if (device->bus_width == 16)
-		{
-			u16 *data16 = data;
-			*data16 = data32 & 0xffff;
-		}
-		else
-		{
-			ERROR("BUG: bus_width neither 8 nor 16 bit");
-			return ERROR_NAND_OPERATION_FAILED;
-		}
-	}	
-	
-	return ERROR_OK;
-}
-
-int lpc3180_write_page(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size)
-{
-	lpc3180_nand_controller_t *lpc3180_info = device->controller_priv;
-	target_t *target = lpc3180_info->target;
-	int retval;
-	u8 status;
-	
-	if (target->state != TARGET_HALTED)
-	{
-		ERROR("target must be halted to use LPC3180 NAND flash controller");
-		return ERROR_NAND_OPERATION_FAILED;
-	}
-	
-	if (lpc3180_info->selected_controller == LPC3180_NO_CONTROLLER)
-	{
-		ERROR("BUG: no LPC3180 NAND flash controller selected");
-		return ERROR_NAND_OPERATION_FAILED;
-	}
-	else if (lpc3180_info->selected_controller == LPC3180_MLC_CONTROLLER)
-	{
-		u8 *page_buffer;
-		u8 *oob_buffer;
-		int quarter, num_quarters;
-		
-		if (!data && oob)
-		{
-			ERROR("LPC3180 MLC controller can't write OOB data only");
-			return ERROR_NAND_OPERATION_NOT_SUPPORTED;
-		}
-		
-		if (oob && (oob_size > 6))
-		{
-			ERROR("LPC3180 MLC controller can't write more than 6 bytes of OOB data");
-			return ERROR_NAND_OPERATION_NOT_SUPPORTED;
-		}
-		
-		if (data_size > device->page_size)
-		{
-			ERROR("data size exceeds page size");
-			return ERROR_NAND_OPERATION_NOT_SUPPORTED;
-		}
-		
-		/* MLC_CMD = sequential input */
-		target_write_u32(target, 0x200b8000, NAND_CMD_SEQIN);
-
-		page_buffer = malloc(512);
-		oob_buffer = malloc(6);		
-
-		if (device->page_size == 512)
-		{
-			/* MLC_ADDR = 0x0 (one column cycle) */
-			target_write_u32(target, 0x200b8004, 0x0);
-
-			/* MLC_ADDR = row */
-			target_write_u32(target, 0x200b8004, page & 0xff);
-			target_write_u32(target, 0x200b8004, (page >> 8) & 0xff);
-			
-			if (device->address_cycles == 4)
-				target_write_u32(target, 0x200b8004, (page >> 16) & 0xff);
-		}
-		else
-		{
-			/* MLC_ADDR = 0x0 (two column cycles) */
-			target_write_u32(target, 0x200b8004, 0x0);
-			target_write_u32(target, 0x200b8004, 0x0);
-
-			/* MLC_ADDR = row */
-			target_write_u32(target, 0x200b8004, page & 0xff);
-			target_write_u32(target, 0x200b8004, (page >> 8) & 0xff);
-		}
-		
-		/* when using the MLC controller, we have to treat a large page device
-		 * as being made out of four quarters, each the size of a small page device
-		 */
-		num_quarters = (device->page_size == 2048) ? 4 : 1;
-		 
-		for (quarter = 0; quarter < num_quarters; quarter++)
-		{
-			int thisrun_data_size = (data_size > 512) ? 512 : data_size;
-			int thisrun_oob_size = (oob_size > 6) ? 6 : oob_size;
-			
-			memset(page_buffer, 0xff, 512);
-			if (data)
-			{
-				memcpy(page_buffer, data, thisrun_data_size);
-				data_size -= thisrun_data_size;
-				data += thisrun_data_size;
-			}
-			
-			memset(oob_buffer, 0xff, (device->page_size == 512) ? 6 : 24);
-			if (oob)
-			{
-				memcpy(page_buffer, oob, thisrun_oob_size);
-				oob_size -= thisrun_oob_size;
-				oob += thisrun_oob_size;
-			}
-			
-			/* write MLC_ECC_ENC_REG to start encode cycle */
-			target_write_u32(target, 0x200b8008, 0x0);
-			
-			target->type->write_memory(target, 0x200a8000, 4, 128, page_buffer + (quarter * 512));
-			target->type->write_memory(target, 0x200a8000, 1, 6, oob_buffer + (quarter * 6));
-			
-			/* write MLC_ECC_AUTO_ENC_REG to start auto encode */
-			target_write_u32(target, 0x200b8010, 0x0);
-			
-			if (!lpc3180_controller_ready(device, 1000))
-			{
-				ERROR("timeout while waiting for completion of auto encode cycle");
-				return ERROR_NAND_OPERATION_FAILED;
-			}
-		}
-		
-		/* MLC_CMD = auto program command */
-		target_write_u32(target, 0x200b8000, NAND_CMD_PAGEPROG);
-		
-		if ((retval = nand_read_status(device, &status)) != ERROR_OK)
-		{
-			ERROR("couldn't read status");
-			return ERROR_NAND_OPERATION_FAILED;
-		}
-			
-		if (status & NAND_STATUS_FAIL)
-		{
-			ERROR("write operation didn't pass, status: 0x%2.2x", status);
-			return ERROR_NAND_OPERATION_FAILED;
-		}
-	
-		free(page_buffer);
-		free(oob_buffer);
-	}
-	else if (lpc3180_info->selected_controller == LPC3180_SLC_CONTROLLER)
-	{
-		return nand_write_page_raw(device, page, data, data_size, oob, oob_size);
-	}
-	
-	return ERROR_OK;
-}
-
-int lpc3180_read_page(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size)
-{
-	lpc3180_nand_controller_t *lpc3180_info = device->controller_priv;
-	target_t *target = lpc3180_info->target;
-	
-	if (target->state != TARGET_HALTED)
-	{
-		ERROR("target must be halted to use LPC3180 NAND flash controller");
-		return ERROR_NAND_OPERATION_FAILED;
-	}
-	
-	if (lpc3180_info->selected_controller == LPC3180_NO_CONTROLLER)
-	{
-		ERROR("BUG: no LPC3180 NAND flash controller selected");
-		return ERROR_NAND_OPERATION_FAILED;
-	}
-	else if (lpc3180_info->selected_controller == LPC3180_MLC_CONTROLLER)
-	{
-		u8 *page_buffer;
-		u8 *oob_buffer;
-		u32 page_bytes_done = 0;
-		u32 oob_bytes_done = 0;
-		u32 mlc_isr;
-
-#if 0
-		if (oob && (oob_size > 6))
-		{
-			ERROR("LPC3180 MLC controller can't read more than 6 bytes of OOB data");
-			return ERROR_NAND_OPERATION_NOT_SUPPORTED;
-		}
-#endif
-		
-		if (data_size > device->page_size)
-		{
-			ERROR("data size exceeds page size");
-			return ERROR_NAND_OPERATION_NOT_SUPPORTED;
-		}
-		
-		if (device->page_size == 2048)
-		{
-			page_buffer = malloc(2048);
-			oob_buffer = malloc(64);
-		}
-		else
-		{
-			page_buffer = malloc(512);
-			oob_buffer = malloc(16);
-		}
-		
-		if (!data && oob)
-		{
-			/* MLC_CMD = Read OOB 
-			 * we can use the READOOB command on both small and large page devices,
-			 * as the controller translates the 0x50 command to a 0x0 with appropriate
-			 * positioning of the serial buffer read pointer
-			 */
-			target_write_u32(target, 0x200b8000, NAND_CMD_READOOB);
-		}
-		else
-		{
-			/* MLC_CMD = Read0 */
-			target_write_u32(target, 0x200b8000, NAND_CMD_READ0);
-		}
-		
-		if (device->page_size == 512)
-		{
-			/* small page device */
-			/* MLC_ADDR = 0x0 (one column cycle) */
-			target_write_u32(target, 0x200b8004, 0x0);
-
-			/* MLC_ADDR = row */
-			target_write_u32(target, 0x200b8004, page & 0xff);
-			target_write_u32(target, 0x200b8004, (page >> 8) & 0xff);
-			
-			if (device->address_cycles == 4)
-				target_write_u32(target, 0x200b8004, (page >> 16) & 0xff);
-		}
-		else
-		{
-			/* large page device */
-			/* MLC_ADDR = 0x0 (two column cycles) */
-			target_write_u32(target, 0x200b8004, 0x0);
-			target_write_u32(target, 0x200b8004, 0x0);
-
-			/* MLC_ADDR = row */
-			target_write_u32(target, 0x200b8004, page & 0xff);
-			target_write_u32(target, 0x200b8004, (page >> 8) & 0xff);
-			
-			/* MLC_CMD = Read Start */
-			target_write_u32(target, 0x200b8000, NAND_CMD_READSTART);
-		}
-		
-		while (page_bytes_done < device->page_size)
-		{
-			/* MLC_ECC_AUTO_DEC_REG = dummy */
-			target_write_u32(target, 0x200b8014, 0xaa55aa55);
-			
-			if (!lpc3180_controller_ready(device, 1000))
-			{
-				ERROR("timeout while waiting for completion of auto decode cycle");
-				return ERROR_NAND_OPERATION_FAILED;
-			}
-		
-			target_read_u32(target, 0x200b8048, &mlc_isr);
-			
-			if (mlc_isr & 0x8)
-			{
-				if (mlc_isr & 0x40)
-				{
-					ERROR("uncorrectable error detected: 0x%2.2x", mlc_isr);
-					return ERROR_NAND_OPERATION_FAILED;
-				}
-				
-				WARNING("%i symbol error detected and corrected", ((mlc_isr & 0x30) >> 4) + 1);
-			}
-			
-			if (data)
-			{
-				target->type->read_memory(target, 0x200a8000, 4, 128, page_buffer + page_bytes_done);
-			}
-			
-			if (oob)
-			{
-				target->type->read_memory(target, 0x200a8000, 4, 4, oob_buffer + oob_bytes_done);
-			}
-
-			page_bytes_done += 512;
-			oob_bytes_done += 16;
-		}
-		
-		if (data)
-			memcpy(data, page_buffer, data_size);
-		
-		if (oob)
-			memcpy(oob, oob_buffer, oob_size);
-		
-		free(page_buffer);
-		free(oob_buffer);
-	}
-	else if (lpc3180_info->selected_controller == LPC3180_SLC_CONTROLLER)
-	{
-		return nand_read_page_raw(device, page, data, data_size, oob, oob_size);
-	}
-	
-	return ERROR_OK;
-}
-
-int lpc3180_controller_ready(struct nand_device_s *device, int timeout)
-{
-	lpc3180_nand_controller_t *lpc3180_info = device->controller_priv;
-	target_t *target = lpc3180_info->target;
-	u8 status = 0x0;
-	
-	if (target->state != TARGET_HALTED)
-	{
-		ERROR("target must be halted to use LPC3180 NAND flash controller");
-		return ERROR_NAND_OPERATION_FAILED;
-	}
-			
-	do
-	{
-		if (lpc3180_info->selected_controller == LPC3180_MLC_CONTROLLER)
-		{
-			/* Read MLC_ISR, wait for controller to become ready */
-			target_read_u8(target, 0x200b8048, &status);
-			
-			if (status & 2)
-				return 1;
-		}
-		else if (lpc3180_info->selected_controller == LPC3180_SLC_CONTROLLER)
-		{
-			/* we pretend that the SLC controller is always ready */
-			return 1;
-		}
-
-		usleep(1000);
-	} while (timeout-- > 0);
-	
-	return 0;
-}
-
-int lpc3180_nand_ready(struct nand_device_s *device, int timeout)
-{
-	lpc3180_nand_controller_t *lpc3180_info = device->controller_priv;
-	target_t *target = lpc3180_info->target;
-	
-	if (target->state != TARGET_HALTED)
-	{
-		ERROR("target must be halted to use LPC3180 NAND flash controller");
-		return ERROR_NAND_OPERATION_FAILED;
-	}
-			
-	do
-	{
-		if (lpc3180_info->selected_controller == LPC3180_MLC_CONTROLLER)
-		{	
-			u8 status = 0x0;
-			
-			/* Read MLC_ISR, wait for NAND flash device to become ready */
-			target_read_u8(target, 0x200b8048, &status);
-			
-			if (status & 1)
-				return 1;
-		}
-		else if (lpc3180_info->selected_controller == LPC3180_SLC_CONTROLLER)
-		{
-			u32 status = 0x0;
-			
-			/* Read SLC_STAT and check READY bit */
-			target_read_u32(target, 0x20020018, &status);
-			
-			if (status & 1)
-				return 1;
-		}
-		
-		usleep(1000);
-	} while (timeout-- > 0);
-	
-	return 0;	
-}
-
-int handle_lpc3180_select_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	nand_device_t *device = NULL;
-	lpc3180_nand_controller_t *lpc3180_info = NULL;
-	char *selected[] = 
-	{
-		"no", "mlc", "slc"
-	};
-	
-	if ((argc < 1) || (argc > 2))
-	{
-		command_print(cmd_ctx, "usage: lpc3180 select <num> <'mlc'|'slc'>");
-		return ERROR_OK;
-	}
-	
-	device = get_nand_device_by_num(strtoul(args[0], NULL, 0));
-	if (!device)
-	{
-		command_print(cmd_ctx, "nand device '#%s' is out of bounds", args[0]);
-		return ERROR_OK;
-	}
-	
-	lpc3180_info = device->controller_priv;
-	
-	if (argc == 2)
-	{
-		if (strcmp(args[1], "mlc") == 0)
-		{
-			lpc3180_info->selected_controller = LPC3180_MLC_CONTROLLER;
-		}
-		else if (strcmp(args[1], "slc") == 0)
-		{
-			lpc3180_info->selected_controller = LPC3180_SLC_CONTROLLER;
-		}
-		else
-		{
-			command_print(cmd_ctx, "usage: lpc3180 select <'mlc'|'slc'>");
-		}
-	}
-	
-	command_print(cmd_ctx, "%s controller selected", selected[lpc3180_info->selected_controller]);
-	
-	return ERROR_OK;
-}
+/***************************************************************************

+ *   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.             *

+ ***************************************************************************/

+#ifdef HAVE_CONFIG_H

+#include "config.h"

+#endif

+

+#include "lpc3180_nand_controller.h"

+

+#include "replacements.h"

+#include "log.h"

+

+#include <stdlib.h>

+#include <string.h>

+

+#include "nand.h"

+#include "target.h"

+

+int lpc3180_nand_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct nand_device_s *device);

+int lpc3180_register_commands(struct command_context_s *cmd_ctx);

+int lpc3180_init(struct nand_device_s *device);

+int lpc3180_reset(struct nand_device_s *device);

+int lpc3180_command(struct nand_device_s *device, u8 command);

+int lpc3180_address(struct nand_device_s *device, u8 address);

+int lpc3180_write_data(struct nand_device_s *device, u16 data);

+int lpc3180_read_data(struct nand_device_s *device, void *data);

+int lpc3180_write_page(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size);

+int lpc3180_read_page(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size);

+int lpc3180_controller_ready(struct nand_device_s *device, int timeout);

+int lpc3180_nand_ready(struct nand_device_s *device, int timeout);

+

+int handle_lpc3180_select_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+

+nand_flash_controller_t lpc3180_nand_controller =

+{

+	.name = "lpc3180",

+	.nand_device_command = lpc3180_nand_device_command,

+	.register_commands = lpc3180_register_commands,

+	.init = lpc3180_init,

+	.reset = lpc3180_reset,

+	.command = lpc3180_command,

+	.address = lpc3180_address,

+	.write_data = lpc3180_write_data,

+	.read_data = lpc3180_read_data,

+	.write_page = lpc3180_write_page,

+	.read_page = lpc3180_read_page,

+	.controller_ready = lpc3180_controller_ready,

+	.nand_ready = lpc3180_nand_ready,

+};

+

+/* nand device lpc3180 <target#> <oscillator_frequency>

+ */

+int lpc3180_nand_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct nand_device_s *device)

+{

+	lpc3180_nand_controller_t *lpc3180_info;

+		

+	if (argc < 3)

+	{

+		WARNING("incomplete 'lpc3180' nand flash configuration");

+		return ERROR_FLASH_BANK_INVALID;

+	}

+	

+	lpc3180_info = malloc(sizeof(lpc3180_nand_controller_t));

+	device->controller_priv = lpc3180_info;

+

+	lpc3180_info->target = get_target_by_num(strtoul(args[1], NULL, 0));

+	if (!lpc3180_info->target)

+	{

+		ERROR("no target '%s' configured", args[1]);

+		return ERROR_NAND_DEVICE_INVALID;

+	}

+

+	lpc3180_info->osc_freq = strtoul(args[2], NULL, 0);

+	if ((lpc3180_info->osc_freq < 1000) || (lpc3180_info->osc_freq > 20000))

+	{

+		WARNING("LPC3180 oscillator frequency should be between 1000 and 20000 kHz, was %i", lpc3180_info->osc_freq); 

+	}

+	lpc3180_info->selected_controller = LPC3180_NO_CONTROLLER;

+	lpc3180_info->sw_write_protection = 0;

+	lpc3180_info->sw_wp_lower_bound = 0x0;

+	lpc3180_info->sw_wp_upper_bound = 0x0;

+		

+	return ERROR_OK;

+}

+

+int lpc3180_register_commands(struct command_context_s *cmd_ctx)

+{

+	command_t *lpc3180_cmd = register_command(cmd_ctx, NULL, "lpc3180", NULL, COMMAND_ANY, "commands specific to the LPC3180 NAND flash controllers");

+	

+	register_command(cmd_ctx, lpc3180_cmd, "select", handle_lpc3180_select_command, COMMAND_EXEC, "select <'mlc'|'slc'> controller (default is mlc)");

+	

+	return ERROR_OK;

+}

+

+int lpc3180_pll(int fclkin, u32 pll_ctrl)

+{

+	int bypass = (pll_ctrl & 0x8000) >> 15;

+	int direct = (pll_ctrl & 0x4000) >> 14;

+	int feedback = (pll_ctrl & 0x2000) >> 13;

+	int p = (1 << ((pll_ctrl & 0x1800) >> 11) * 2);

+	int n = ((pll_ctrl & 0x0600) >> 9) + 1;

+	int m = ((pll_ctrl & 0x01fe) >> 1) + 1;

+	int lock = (pll_ctrl & 0x1);

+

+	if (!lock)

+		WARNING("PLL is not locked");

+	

+	if (!bypass && direct)	/* direct mode */

+		return (m * fclkin) / n;

+	

+	if (bypass && !direct)	/* bypass mode */

+		return fclkin / (2 * p);

+		

+	if (bypass & direct)	/* direct bypass mode */

+		return fclkin;

+	

+	if (feedback)			/* integer mode */

+		return m * (fclkin / n);

+	else					/* non-integer mode */

+		return (m / (2 * p)) * (fclkin / n); 

+}

+

+float lpc3180_cycle_time(lpc3180_nand_controller_t *lpc3180_info)

+{

+	target_t *target = lpc3180_info->target;

+	u32 sysclk_ctrl, pwr_ctrl, hclkdiv_ctrl, hclkpll_ctrl;

+	int sysclk;

+	int hclk;

+	int hclk_pll;

+	float cycle;

+	

+	/* calculate timings */

+	

+	/* determine current SYSCLK (13'MHz or main oscillator) */ 

+	target_read_u32(target, 0x40004050, &sysclk_ctrl);

+	

+	if ((sysclk_ctrl & 1) == 0)

+		sysclk = lpc3180_info->osc_freq;

+	else

+		sysclk = 13000;

+	

+	/* determine selected HCLK source */

+	target_read_u32(target, 0x40004044, &pwr_ctrl);

+	

+	if ((pwr_ctrl & (1 << 2)) == 0) /* DIRECT RUN mode */

+	{

+		hclk = sysclk;

+	}

+	else

+	{

+		target_read_u32(target, 0x40004058, &hclkpll_ctrl);

+		hclk_pll = lpc3180_pll(sysclk, hclkpll_ctrl);

+

+		target_read_u32(target, 0x40004040, &hclkdiv_ctrl);

+		

+		if (pwr_ctrl & (1 << 10)) /* ARM_CLK and HCLK use PERIPH_CLK */

+		{

+			hclk = hclk_pll / (((hclkdiv_ctrl & 0x7c) >> 2) + 1);

+		}

+		else /* HCLK uses HCLK_PLL */

+		{

+			hclk = hclk_pll / (1 << (hclkdiv_ctrl & 0x3)); 

+		}

+	}

+	

+	DEBUG("LPC3180 HCLK currently clocked at %i kHz", hclk);

+	

+	cycle = (1.0 / hclk) * 1000000.0;

+	

+	return cycle;

+}

+

+int lpc3180_init(struct nand_device_s *device)

+{

+	lpc3180_nand_controller_t *lpc3180_info = device->controller_priv;

+	target_t *target = lpc3180_info->target;

+	int bus_width = (device->bus_width) ? (device->bus_width) : 8;

+	int address_cycles = (device->address_cycles) ? (device->address_cycles) : 3;

+	int page_size = (device->page_size) ? (device->page_size) : 512;

+		

+	if (target->state != TARGET_HALTED)

+	{

+		ERROR("target must be halted to use LPC3180 NAND flash controller");

+		return ERROR_NAND_OPERATION_FAILED;

+	}

+	

+	/* sanitize arguments */

+	if ((bus_width != 8) && (bus_width != 16))

+	{

+		ERROR("LPC3180 only supports 8 or 16 bit bus width, not %i", bus_width);

+		return ERROR_NAND_OPERATION_NOT_SUPPORTED;

+	}

+	

+	/* The LPC3180 only brings out 8 bit NAND data bus, but the controller

+	 * would support 16 bit, too, so we just warn about this for now

+	 */

+	if (bus_width == 16)

+	{

+		WARNING("LPC3180 only supports 8 bit bus width");

+	}

+	

+	/* inform calling code about selected bus width */

+	device->bus_width = bus_width;

+	

+	if ((address_cycles != 3) && (address_cycles != 4))

+	{

+		ERROR("LPC3180 only supports 3 or 4 address cycles, not %i", address_cycles);

+		return ERROR_NAND_OPERATION_NOT_SUPPORTED;

+	}

+	

+	if ((page_size != 512) && (page_size != 2048))

+	{

+		ERROR("LPC3180 only supports 512 or 2048 byte pages, not %i", page_size);

+		return ERROR_NAND_OPERATION_NOT_SUPPORTED;

+	}

+	

+	/* select MLC controller if none is currently selected */

+	if (lpc3180_info->selected_controller == LPC3180_NO_CONTROLLER)

+	{

+		DEBUG("no LPC3180 NAND flash controller selected, using default 'mlc'");

+		lpc3180_info->selected_controller = LPC3180_MLC_CONTROLLER;

+	}

+	

+	if (lpc3180_info->selected_controller == LPC3180_MLC_CONTROLLER)

+	{

+		u32 mlc_icr_value = 0x0;

+		float cycle;

+		int twp, twh, trp, treh, trhz, trbwb, tcea;

+		

+		/* FLASHCLK_CTRL = 0x22 (enable clock for MLC flash controller) */

+		target_write_u32(target, 0x400040c8, 0x22);

+		

+		/* MLC_CEH = 0x0 (Force nCE assert) */

+		target_write_u32(target, 0x200b804c, 0x0);

+		

+		/* MLC_LOCK = 0xa25e (unlock protected registers) */

+		target_write_u32(target, 0x200b8044, 0xa25e);

+		

+		/* MLC_ICR = configuration */

+		if (lpc3180_info->sw_write_protection)

+			mlc_icr_value |= 0x8;

+		if (page_size == 2048)

+			mlc_icr_value |= 0x4;

+		if (address_cycles == 4)

+			mlc_icr_value |= 0x2;

+		if (bus_width == 16)

+			mlc_icr_value |= 0x1;

+		target_write_u32(target, 0x200b8030, mlc_icr_value);

+		

+		/* calculate NAND controller timings */

+		cycle = lpc3180_cycle_time(lpc3180_info);

+		

+		twp = ((40 / cycle) + 1);

+		twh = ((20 / cycle) + 1);

+		trp = ((30 / cycle) + 1);

+		treh = ((15 / cycle) + 1);

+		trhz = ((30 / cycle) + 1);

+		trbwb = ((100 / cycle) + 1);

+		tcea = ((45 / cycle) + 1);

+				

+		/* MLC_LOCK = 0xa25e (unlock protected registers) */

+		target_write_u32(target, 0x200b8044, 0xa25e);

+	

+		/* MLC_TIME_REG */

+		target_write_u32(target, 0x200b8034, (twp & 0xf) | ((twh & 0xf) << 4) | 

+			((trp & 0xf) << 8) | ((treh & 0xf) << 12) | ((trhz & 0x7) << 16) | 

+			((trbwb & 0x1f) << 19) | ((tcea & 0x3) << 24)); 

+

+		lpc3180_reset(device);

+	}

+	else if (lpc3180_info->selected_controller == LPC3180_SLC_CONTROLLER)

+	{

+		float cycle;

+		int r_setup, r_hold, r_width, r_rdy;

+		int w_setup, w_hold, w_width, w_rdy;

+		

+		/* FLASHCLK_CTRL = 0x05 (enable clock for SLC flash controller) */

+		target_write_u32(target, 0x400040c8, 0x05);

+		

+		/* SLC_CFG = 0x (Force nCE assert, ECC enabled, WIDTH = bus_width) */

+		target_write_u32(target, 0x20020014, 0x28 | (bus_width == 16) ? 1 : 0);

+		

+		/* calculate NAND controller timings */

+		cycle = lpc3180_cycle_time(lpc3180_info);

+		

+		r_setup = w_setup = 0;

+		r_hold = w_hold = 10 / cycle;

+		r_width = 30 / cycle;

+		w_width = 40 / cycle;

+		r_rdy = w_rdy = 100 / cycle;

+		

+		/* SLC_TAC: SLC timing arcs register */

+		target_write_u32(target, 0x2002002c, (r_setup & 0xf) | ((r_hold & 0xf) << 4) |

+			((r_width & 0xf) << 8) | ((r_rdy & 0xf) << 12) |  ((w_setup & 0xf) << 16) |

+			((w_hold & 0xf) << 20) | ((w_width & 0xf) << 24) | ((w_rdy & 0xf) << 28)); 

+		

+		lpc3180_reset(device);

+	}

+	

+	return ERROR_OK;

+}

+

+int lpc3180_reset(struct nand_device_s *device)

+{

+	lpc3180_nand_controller_t *lpc3180_info = device->controller_priv;

+	target_t *target = lpc3180_info->target;

+	

+	if (target->state != TARGET_HALTED)

+	{

+		ERROR("target must be halted to use LPC3180 NAND flash controller");

+		return ERROR_NAND_OPERATION_FAILED;

+	}

+	

+	if (lpc3180_info->selected_controller == LPC3180_NO_CONTROLLER)

+	{

+		ERROR("BUG: no LPC3180 NAND flash controller selected");

+		return ERROR_NAND_OPERATION_FAILED;

+	}

+	else if (lpc3180_info->selected_controller == LPC3180_MLC_CONTROLLER)

+	{

+		/* MLC_CMD = 0xff (reset controller and NAND device) */

+		target_write_u32(target, 0x200b8000, 0xff);

+

+		if (!lpc3180_controller_ready(device, 100))

+		{

+			ERROR("LPC3180 NAND controller timed out after reset");

+			return ERROR_NAND_OPERATION_TIMEOUT;

+		}

+	}

+	else if (lpc3180_info->selected_controller == LPC3180_SLC_CONTROLLER)

+	{

+		/* SLC_CTRL = 0x6 (ECC_CLEAR, SW_RESET) */

+		target_write_u32(target, 0x20020010, 0x6);

+		

+		if (!lpc3180_controller_ready(device, 100))

+		{

+			ERROR("LPC3180 NAND controller timed out after reset");

+			return ERROR_NAND_OPERATION_TIMEOUT;

+		}

+	}

+	

+	return ERROR_OK;

+}

+

+int lpc3180_command(struct nand_device_s *device, u8 command)

+{

+	lpc3180_nand_controller_t *lpc3180_info = device->controller_priv;

+	target_t *target = lpc3180_info->target;

+	

+	if (target->state != TARGET_HALTED)

+	{

+		ERROR("target must be halted to use LPC3180 NAND flash controller");

+		return ERROR_NAND_OPERATION_FAILED;

+	}

+	

+	if (lpc3180_info->selected_controller == LPC3180_NO_CONTROLLER)

+	{

+		ERROR("BUG: no LPC3180 NAND flash controller selected");

+		return ERROR_NAND_OPERATION_FAILED;

+	}

+	else if (lpc3180_info->selected_controller == LPC3180_MLC_CONTROLLER)

+	{

+		/* MLC_CMD = command */

+		target_write_u32(target, 0x200b8000, command);

+	}

+	else if (lpc3180_info->selected_controller == LPC3180_SLC_CONTROLLER)

+	{

+		/* SLC_CMD = command */

+		target_write_u32(target, 0x20020008, command);

+	}	

+	

+	return ERROR_OK;

+}

+

+int lpc3180_address(struct nand_device_s *device, u8 address)

+{

+	lpc3180_nand_controller_t *lpc3180_info = device->controller_priv;

+	target_t *target = lpc3180_info->target;

+	

+	if (target->state != TARGET_HALTED)

+	{

+		ERROR("target must be halted to use LPC3180 NAND flash controller");

+		return ERROR_NAND_OPERATION_FAILED;

+	}

+	

+	if (lpc3180_info->selected_controller == LPC3180_NO_CONTROLLER)

+	{

+		ERROR("BUG: no LPC3180 NAND flash controller selected");

+		return ERROR_NAND_OPERATION_FAILED;

+	}

+	else if (lpc3180_info->selected_controller == LPC3180_MLC_CONTROLLER)

+	{

+		/* MLC_ADDR = address */

+		target_write_u32(target, 0x200b8004, address);

+	}

+	else if (lpc3180_info->selected_controller == LPC3180_SLC_CONTROLLER)

+	{

+		/* SLC_ADDR = address */

+		target_write_u32(target, 0x20020004, address);

+	}

+		

+	return ERROR_OK;

+}

+

+int lpc3180_write_data(struct nand_device_s *device, u16 data)

+{

+	lpc3180_nand_controller_t *lpc3180_info = device->controller_priv;

+	target_t *target = lpc3180_info->target;

+	

+	if (target->state != TARGET_HALTED)

+	{

+		ERROR("target must be halted to use LPC3180 NAND flash controller");

+		return ERROR_NAND_OPERATION_FAILED;

+	}

+	

+	if (lpc3180_info->selected_controller == LPC3180_NO_CONTROLLER)

+	{

+		ERROR("BUG: no LPC3180 NAND flash controller selected");

+		return ERROR_NAND_OPERATION_FAILED;

+	}

+	else if (lpc3180_info->selected_controller == LPC3180_MLC_CONTROLLER)

+	{

+		/* MLC_DATA = data */

+		target_write_u32(target, 0x200b0000, data);

+	}

+	else if (lpc3180_info->selected_controller == LPC3180_SLC_CONTROLLER)

+	{

+		/* SLC_DATA = data */

+		target_write_u32(target, 0x20020000, data);

+	}

+	

+	return ERROR_OK;

+}

+

+int lpc3180_read_data(struct nand_device_s *device, void *data)

+{

+	lpc3180_nand_controller_t *lpc3180_info = device->controller_priv;

+	target_t *target = lpc3180_info->target;

+	

+	if (target->state != TARGET_HALTED)

+	{

+		ERROR("target must be halted to use LPC3180 NAND flash controller");

+		return ERROR_NAND_OPERATION_FAILED;

+	}

+	

+	if (lpc3180_info->selected_controller == LPC3180_NO_CONTROLLER)

+	{

+		ERROR("BUG: no LPC3180 NAND flash controller selected");

+		return ERROR_NAND_OPERATION_FAILED;

+	}

+	else if (lpc3180_info->selected_controller == LPC3180_MLC_CONTROLLER)

+	{

+		/* data = MLC_DATA, use sized access */

+		if (device->bus_width == 8)

+		{

+			u8 *data8 = data;

+			target_read_u8(target, 0x200b0000, data8);

+		}

+		else if (device->bus_width == 16)

+		{

+			u16 *data16 = data;

+			target_read_u16(target, 0x200b0000, data16);

+		}

+		else

+		{

+			ERROR("BUG: bus_width neither 8 nor 16 bit");

+			return ERROR_NAND_OPERATION_FAILED;

+		}

+	}

+	else if (lpc3180_info->selected_controller == LPC3180_SLC_CONTROLLER)

+	{

+		u32 data32;

+

+		/* data = SLC_DATA, must use 32-bit access */

+		target_read_u32(target, 0x20020000, &data32);

+		

+		if (device->bus_width == 8)

+		{

+			u8 *data8 = data;

+			*data8 = data32 & 0xff;

+		}

+		else if (device->bus_width == 16)

+		{

+			u16 *data16 = data;

+			*data16 = data32 & 0xffff;

+		}

+		else

+		{

+			ERROR("BUG: bus_width neither 8 nor 16 bit");

+			return ERROR_NAND_OPERATION_FAILED;

+		}

+	}	

+	

+	return ERROR_OK;

+}

+

+int lpc3180_write_page(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size)

+{

+	lpc3180_nand_controller_t *lpc3180_info = device->controller_priv;

+	target_t *target = lpc3180_info->target;

+	int retval;

+	u8 status;

+	

+	if (target->state != TARGET_HALTED)

+	{

+		ERROR("target must be halted to use LPC3180 NAND flash controller");

+		return ERROR_NAND_OPERATION_FAILED;

+	}

+	

+	if (lpc3180_info->selected_controller == LPC3180_NO_CONTROLLER)

+	{

+		ERROR("BUG: no LPC3180 NAND flash controller selected");

+		return ERROR_NAND_OPERATION_FAILED;

+	}

+	else if (lpc3180_info->selected_controller == LPC3180_MLC_CONTROLLER)

+	{

+		u8 *page_buffer;

+		u8 *oob_buffer;

+		int quarter, num_quarters;

+		

+		if (!data && oob)

+		{

+			ERROR("LPC3180 MLC controller can't write OOB data only");

+			return ERROR_NAND_OPERATION_NOT_SUPPORTED;

+		}

+		

+		if (oob && (oob_size > 6))

+		{

+			ERROR("LPC3180 MLC controller can't write more than 6 bytes of OOB data");

+			return ERROR_NAND_OPERATION_NOT_SUPPORTED;

+		}

+		

+		if (data_size > device->page_size)

+		{

+			ERROR("data size exceeds page size");

+			return ERROR_NAND_OPERATION_NOT_SUPPORTED;

+		}

+		

+		/* MLC_CMD = sequential input */

+		target_write_u32(target, 0x200b8000, NAND_CMD_SEQIN);

+

+		page_buffer = malloc(512);

+		oob_buffer = malloc(6);		

+

+		if (device->page_size == 512)

+		{

+			/* MLC_ADDR = 0x0 (one column cycle) */

+			target_write_u32(target, 0x200b8004, 0x0);

+

+			/* MLC_ADDR = row */

+			target_write_u32(target, 0x200b8004, page & 0xff);

+			target_write_u32(target, 0x200b8004, (page >> 8) & 0xff);

+			

+			if (device->address_cycles == 4)

+				target_write_u32(target, 0x200b8004, (page >> 16) & 0xff);

+		}

+		else

+		{

+			/* MLC_ADDR = 0x0 (two column cycles) */

+			target_write_u32(target, 0x200b8004, 0x0);

+			target_write_u32(target, 0x200b8004, 0x0);

+

+			/* MLC_ADDR = row */

+			target_write_u32(target, 0x200b8004, page & 0xff);

+			target_write_u32(target, 0x200b8004, (page >> 8) & 0xff);

+		}

+		

+		/* when using the MLC controller, we have to treat a large page device

+		 * as being made out of four quarters, each the size of a small page device

+		 */

+		num_quarters = (device->page_size == 2048) ? 4 : 1;

+		 

+		for (quarter = 0; quarter < num_quarters; quarter++)

+		{

+			int thisrun_data_size = (data_size > 512) ? 512 : data_size;

+			int thisrun_oob_size = (oob_size > 6) ? 6 : oob_size;

+			

+			memset(page_buffer, 0xff, 512);

+			if (data)

+			{

+				memcpy(page_buffer, data, thisrun_data_size);

+				data_size -= thisrun_data_size;

+				data += thisrun_data_size;

+			}

+			

+			memset(oob_buffer, 0xff, (device->page_size == 512) ? 6 : 24);

+			if (oob)

+			{

+				memcpy(page_buffer, oob, thisrun_oob_size);

+				oob_size -= thisrun_oob_size;

+				oob += thisrun_oob_size;

+			}

+			

+			/* write MLC_ECC_ENC_REG to start encode cycle */

+			target_write_u32(target, 0x200b8008, 0x0);

+			

+			target->type->write_memory(target, 0x200a8000, 4, 128, page_buffer + (quarter * 512));

+			target->type->write_memory(target, 0x200a8000, 1, 6, oob_buffer + (quarter * 6));

+			

+			/* write MLC_ECC_AUTO_ENC_REG to start auto encode */

+			target_write_u32(target, 0x200b8010, 0x0);

+			

+			if (!lpc3180_controller_ready(device, 1000))

+			{

+				ERROR("timeout while waiting for completion of auto encode cycle");

+				return ERROR_NAND_OPERATION_FAILED;

+			}

+		}

+		

+		/* MLC_CMD = auto program command */

+		target_write_u32(target, 0x200b8000, NAND_CMD_PAGEPROG);

+		

+		if ((retval = nand_read_status(device, &status)) != ERROR_OK)

+		{

+			ERROR("couldn't read status");

+			return ERROR_NAND_OPERATION_FAILED;

+		}

+			

+		if (status & NAND_STATUS_FAIL)

+		{

+			ERROR("write operation didn't pass, status: 0x%2.2x", status);

+			return ERROR_NAND_OPERATION_FAILED;

+		}

+	

+		free(page_buffer);

+		free(oob_buffer);

+	}

+	else if (lpc3180_info->selected_controller == LPC3180_SLC_CONTROLLER)

+	{

+		return nand_write_page_raw(device, page, data, data_size, oob, oob_size);

+	}

+	

+	return ERROR_OK;

+}

+

+int lpc3180_read_page(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size)

+{

+	lpc3180_nand_controller_t *lpc3180_info = device->controller_priv;

+	target_t *target = lpc3180_info->target;

+	

+	if (target->state != TARGET_HALTED)

+	{

+		ERROR("target must be halted to use LPC3180 NAND flash controller");

+		return ERROR_NAND_OPERATION_FAILED;

+	}

+	

+	if (lpc3180_info->selected_controller == LPC3180_NO_CONTROLLER)

+	{

+		ERROR("BUG: no LPC3180 NAND flash controller selected");

+		return ERROR_NAND_OPERATION_FAILED;

+	}

+	else if (lpc3180_info->selected_controller == LPC3180_MLC_CONTROLLER)

+	{

+		u8 *page_buffer;

+		u8 *oob_buffer;

+		u32 page_bytes_done = 0;

+		u32 oob_bytes_done = 0;

+		u32 mlc_isr;

+

+#if 0

+		if (oob && (oob_size > 6))

+		{

+			ERROR("LPC3180 MLC controller can't read more than 6 bytes of OOB data");

+			return ERROR_NAND_OPERATION_NOT_SUPPORTED;

+		}

+#endif

+		

+		if (data_size > device->page_size)

+		{

+			ERROR("data size exceeds page size");

+			return ERROR_NAND_OPERATION_NOT_SUPPORTED;

+		}

+		

+		if (device->page_size == 2048)

+		{

+			page_buffer = malloc(2048);

+			oob_buffer = malloc(64);

+		}

+		else

+		{

+			page_buffer = malloc(512);

+			oob_buffer = malloc(16);

+		}

+		

+		if (!data && oob)

+		{

+			/* MLC_CMD = Read OOB 

+			 * we can use the READOOB command on both small and large page devices,

+			 * as the controller translates the 0x50 command to a 0x0 with appropriate

+			 * positioning of the serial buffer read pointer

+			 */

+			target_write_u32(target, 0x200b8000, NAND_CMD_READOOB);

+		}

+		else

+		{

+			/* MLC_CMD = Read0 */

+			target_write_u32(target, 0x200b8000, NAND_CMD_READ0);

+		}

+		

+		if (device->page_size == 512)

+		{

+			/* small page device */

+			/* MLC_ADDR = 0x0 (one column cycle) */

+			target_write_u32(target, 0x200b8004, 0x0);

+

+			/* MLC_ADDR = row */

+			target_write_u32(target, 0x200b8004, page & 0xff);

+			target_write_u32(target, 0x200b8004, (page >> 8) & 0xff);

+			

+			if (device->address_cycles == 4)

+				target_write_u32(target, 0x200b8004, (page >> 16) & 0xff);

+		}

+		else

+		{

+			/* large page device */

+			/* MLC_ADDR = 0x0 (two column cycles) */

+			target_write_u32(target, 0x200b8004, 0x0);

+			target_write_u32(target, 0x200b8004, 0x0);

+

+			/* MLC_ADDR = row */

+			target_write_u32(target, 0x200b8004, page & 0xff);

+			target_write_u32(target, 0x200b8004, (page >> 8) & 0xff);

+			

+			/* MLC_CMD = Read Start */

+			target_write_u32(target, 0x200b8000, NAND_CMD_READSTART);

+		}

+		

+		while (page_bytes_done < device->page_size)

+		{

+			/* MLC_ECC_AUTO_DEC_REG = dummy */

+			target_write_u32(target, 0x200b8014, 0xaa55aa55);

+			

+			if (!lpc3180_controller_ready(device, 1000))

+			{

+				ERROR("timeout while waiting for completion of auto decode cycle");

+				return ERROR_NAND_OPERATION_FAILED;

+			}

+		

+			target_read_u32(target, 0x200b8048, &mlc_isr);

+			

+			if (mlc_isr & 0x8)

+			{

+				if (mlc_isr & 0x40)

+				{

+					ERROR("uncorrectable error detected: 0x%2.2x", mlc_isr);

+					return ERROR_NAND_OPERATION_FAILED;

+				}

+				

+				WARNING("%i symbol error detected and corrected", ((mlc_isr & 0x30) >> 4) + 1);

+			}

+			

+			if (data)

+			{

+				target->type->read_memory(target, 0x200a8000, 4, 128, page_buffer + page_bytes_done);

+			}

+			

+			if (oob)

+			{

+				target->type->read_memory(target, 0x200a8000, 4, 4, oob_buffer + oob_bytes_done);

+			}

+

+			page_bytes_done += 512;

+			oob_bytes_done += 16;

+		}

+		

+		if (data)

+			memcpy(data, page_buffer, data_size);

+		

+		if (oob)

+			memcpy(oob, oob_buffer, oob_size);

+		

+		free(page_buffer);

+		free(oob_buffer);

+	}

+	else if (lpc3180_info->selected_controller == LPC3180_SLC_CONTROLLER)

+	{

+		return nand_read_page_raw(device, page, data, data_size, oob, oob_size);

+	}

+	

+	return ERROR_OK;

+}

+

+int lpc3180_controller_ready(struct nand_device_s *device, int timeout)

+{

+	lpc3180_nand_controller_t *lpc3180_info = device->controller_priv;

+	target_t *target = lpc3180_info->target;

+	u8 status = 0x0;

+	

+	if (target->state != TARGET_HALTED)

+	{

+		ERROR("target must be halted to use LPC3180 NAND flash controller");

+		return ERROR_NAND_OPERATION_FAILED;

+	}

+			

+	do

+	{

+		if (lpc3180_info->selected_controller == LPC3180_MLC_CONTROLLER)

+		{

+			/* Read MLC_ISR, wait for controller to become ready */

+			target_read_u8(target, 0x200b8048, &status);

+			

+			if (status & 2)

+				return 1;

+		}

+		else if (lpc3180_info->selected_controller == LPC3180_SLC_CONTROLLER)

+		{

+			/* we pretend that the SLC controller is always ready */

+			return 1;

+		}

+

+		usleep(1000);

+	} while (timeout-- > 0);

+	

+	return 0;

+}

+

+int lpc3180_nand_ready(struct nand_device_s *device, int timeout)

+{

+	lpc3180_nand_controller_t *lpc3180_info = device->controller_priv;

+	target_t *target = lpc3180_info->target;

+	

+	if (target->state != TARGET_HALTED)

+	{

+		ERROR("target must be halted to use LPC3180 NAND flash controller");

+		return ERROR_NAND_OPERATION_FAILED;

+	}

+			

+	do

+	{

+		if (lpc3180_info->selected_controller == LPC3180_MLC_CONTROLLER)

+		{	

+			u8 status = 0x0;

+			

+			/* Read MLC_ISR, wait for NAND flash device to become ready */

+			target_read_u8(target, 0x200b8048, &status);

+			

+			if (status & 1)

+				return 1;

+		}

+		else if (lpc3180_info->selected_controller == LPC3180_SLC_CONTROLLER)

+		{

+			u32 status = 0x0;

+			

+			/* Read SLC_STAT and check READY bit */

+			target_read_u32(target, 0x20020018, &status);

+			

+			if (status & 1)

+				return 1;

+		}

+		

+		usleep(1000);

+	} while (timeout-- > 0);

+	

+	return 0;	

+}

+

+int handle_lpc3180_select_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	nand_device_t *device = NULL;

+	lpc3180_nand_controller_t *lpc3180_info = NULL;

+	char *selected[] = 

+	{

+		"no", "mlc", "slc"

+	};

+	

+	if ((argc < 1) || (argc > 2))

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+	}

+	

+	device = get_nand_device_by_num(strtoul(args[0], NULL, 0));

+	if (!device)

+	{

+		command_print(cmd_ctx, "nand device '#%s' is out of bounds", args[0]);

+		return ERROR_OK;

+	}

+	

+	lpc3180_info = device->controller_priv;

+	

+	if (argc == 2)

+	{

+		if (strcmp(args[1], "mlc") == 0)

+		{

+			lpc3180_info->selected_controller = LPC3180_MLC_CONTROLLER;

+		}

+		else if (strcmp(args[1], "slc") == 0)

+		{

+			lpc3180_info->selected_controller = LPC3180_SLC_CONTROLLER;

+		}

+		else

+		{

+			return ERROR_COMMAND_SYNTAX_ERROR;

+		}

+	}

+	

+	command_print(cmd_ctx, "%s controller selected", selected[lpc3180_info->selected_controller]);

+	

+	return ERROR_OK;

+}

diff --git a/src/flash/nand.c b/src/flash/nand.c
index 92c7bc7a..b3394428 100644
--- a/src/flash/nand.c
+++ b/src/flash/nand.c
@@ -1,1517 +1,1514 @@
-/***************************************************************************
- *   Copyright (C) 2007 by Dominic Rath                                    *
- *   Dominic.Rath@gmx.de                                                   *
- *                                                                         *
- *   partially based on                                                    *
- * 	 drivers/mtd/nand_ids.c                                            *
- *                                                                         *
- *   Copyright (C) 2002 Thomas Gleixner (tglx@linutronix.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 "replacements.h"
-#include "log.h"
-
-#include <stdlib.h>
-#include <string.h>
-#include <inttypes.h>
-
-#include <errno.h>
-
-#include "nand.h"
-#include "flash.h"
-#include "time_support.h"
-#include "fileio.h"
-#include "image.h"
-
-int nand_register_commands(struct command_context_s *cmd_ctx);
-int handle_nand_list_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_nand_probe_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_nand_check_bad_blocks_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_nand_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_nand_copy_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_nand_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_nand_dump_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_nand_erase_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-int handle_nand_raw_access_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-int nand_read_page_raw(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size);
-int nand_read_page(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size);
-int nand_read_plain(struct nand_device_s *device, u32 address, u8 *data, u32 data_size);
-
-int nand_write_page_raw(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size);
-int nand_write_page(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size);
-
-/* NAND flash controller
- */
-extern nand_flash_controller_t lpc3180_nand_controller;
-extern nand_flash_controller_t s3c2410_nand_controller;
-extern nand_flash_controller_t s3c2412_nand_controller;
-extern nand_flash_controller_t s3c2440_nand_controller;
-extern nand_flash_controller_t s3c2443_nand_controller;
-
-/* extern nand_flash_controller_t boundary_scan_nand_controller; */
-
-nand_flash_controller_t *nand_flash_controllers[] =
-{
-	&lpc3180_nand_controller,
-	&s3c2410_nand_controller,
-	&s3c2412_nand_controller,
-	&s3c2440_nand_controller,
-	&s3c2443_nand_controller,
-/*	&boundary_scan_nand_controller, */
-	NULL
-};
-
-/* configured NAND devices and NAND Flash command handler */
-nand_device_t *nand_devices = NULL;
-static command_t *nand_cmd;
-
-/*	Chip ID list
- *
- *	Name, ID code, pagesize, chipsize in MegaByte, eraseblock size,
- *	options
- *
- *	Pagesize; 0, 256, 512
- *	0	get this information from the extended chip ID
- *	256	256 Byte page size
- *	512	512 Byte page size
- */
-nand_info_t nand_flash_ids[] =
-{
-	{"NAND 1MiB 5V 8-bit",		0x6e, 256, 1, 0x1000, 0},
-	{"NAND 2MiB 5V 8-bit",		0x64, 256, 2, 0x1000, 0},
-	{"NAND 4MiB 5V 8-bit",		0x6b, 512, 4, 0x2000, 0},
-	{"NAND 1MiB 3,3V 8-bit",	0xe8, 256, 1, 0x1000, 0},
-	{"NAND 1MiB 3,3V 8-bit",	0xec, 256, 1, 0x1000, 0},
-	{"NAND 2MiB 3,3V 8-bit",	0xea, 256, 2, 0x1000, 0},
-	{"NAND 4MiB 3,3V 8-bit",	0xd5, 512, 4, 0x2000, 0},
-	{"NAND 4MiB 3,3V 8-bit",	0xe3, 512, 4, 0x2000, 0},
-	{"NAND 4MiB 3,3V 8-bit",	0xe5, 512, 4, 0x2000, 0},
-	{"NAND 8MiB 3,3V 8-bit",	0xd6, 512, 8, 0x2000, 0},
-
-	{"NAND 8MiB 1,8V 8-bit",	0x39, 512, 8, 0x2000, 0},
-	{"NAND 8MiB 3,3V 8-bit",	0xe6, 512, 8, 0x2000, 0},
-	{"NAND 8MiB 1,8V 16-bit",	0x49, 512, 8, 0x2000, NAND_BUSWIDTH_16},
-	{"NAND 8MiB 3,3V 16-bit",	0x59, 512, 8, 0x2000, NAND_BUSWIDTH_16},
-
-	{"NAND 16MiB 1,8V 8-bit",	0x33, 512, 16, 0x4000, 0},
-	{"NAND 16MiB 3,3V 8-bit",	0x73, 512, 16, 0x4000, 0},
-	{"NAND 16MiB 1,8V 16-bit",	0x43, 512, 16, 0x4000, NAND_BUSWIDTH_16},
-	{"NAND 16MiB 3,3V 16-bit",	0x53, 512, 16, 0x4000, NAND_BUSWIDTH_16},
-
-	{"NAND 32MiB 1,8V 8-bit",	0x35, 512, 32, 0x4000, 0},
-	{"NAND 32MiB 3,3V 8-bit",	0x75, 512, 32, 0x4000, 0},
-	{"NAND 32MiB 1,8V 16-bit",	0x45, 512, 32, 0x4000, NAND_BUSWIDTH_16},
-	{"NAND 32MiB 3,3V 16-bit",	0x55, 512, 32, 0x4000, NAND_BUSWIDTH_16},
-
-	{"NAND 64MiB 1,8V 8-bit",	0x36, 512, 64, 0x4000, 0},
-	{"NAND 64MiB 3,3V 8-bit",	0x76, 512, 64, 0x4000, 0},
-	{"NAND 64MiB 1,8V 16-bit",	0x46, 512, 64, 0x4000, NAND_BUSWIDTH_16},
-	{"NAND 64MiB 3,3V 16-bit",	0x56, 512, 64, 0x4000, NAND_BUSWIDTH_16},
-
-	{"NAND 128MiB 1,8V 8-bit",	0x78, 512, 128, 0x4000, 0},
-	{"NAND 128MiB 1,8V 8-bit",	0x39, 512, 128, 0x4000, 0},
-	{"NAND 128MiB 3,3V 8-bit",	0x79, 512, 128, 0x4000, 0},
-	{"NAND 128MiB 1,8V 16-bit",	0x72, 512, 128, 0x4000, NAND_BUSWIDTH_16},
-	{"NAND 128MiB 1,8V 16-bit",	0x49, 512, 128, 0x4000, NAND_BUSWIDTH_16},
-	{"NAND 128MiB 3,3V 16-bit",	0x74, 512, 128, 0x4000, NAND_BUSWIDTH_16},
-	{"NAND 128MiB 3,3V 16-bit",	0x59, 512, 128, 0x4000, NAND_BUSWIDTH_16},
-
-	{"NAND 256MiB 3,3V 8-bit",	0x71, 512, 256, 0x4000, 0},
-
-	{"NAND 64MiB 1,8V 8-bit",	0xA2, 0,  64, 0, LP_OPTIONS},
-	{"NAND 64MiB 3,3V 8-bit",	0xF2, 0,  64, 0, LP_OPTIONS},
-	{"NAND 64MiB 1,8V 16-bit",	0xB2, 0,  64, 0, LP_OPTIONS16},
-	{"NAND 64MiB 3,3V 16-bit",	0xC2, 0,  64, 0, LP_OPTIONS16},
-
-	{"NAND 128MiB 1,8V 8-bit",	0xA1, 0, 128, 0, LP_OPTIONS},
-	{"NAND 128MiB 3,3V 8-bit",	0xF1, 0, 128, 0, LP_OPTIONS},
-	{"NAND 128MiB 1,8V 16-bit",	0xB1, 0, 128, 0, LP_OPTIONS16},
-	{"NAND 128MiB 3,3V 16-bit",	0xC1, 0, 128, 0, LP_OPTIONS16},
-
-	{"NAND 256MiB 1,8V 8-bit",	0xAA, 0, 256, 0, LP_OPTIONS},
-	{"NAND 256MiB 3,3V 8-bit",	0xDA, 0, 256, 0, LP_OPTIONS},
-	{"NAND 256MiB 1,8V 16-bit",	0xBA, 0, 256, 0, LP_OPTIONS16},
-	{"NAND 256MiB 3,3V 16-bit",	0xCA, 0, 256, 0, LP_OPTIONS16},
-
-	{"NAND 512MiB 1,8V 8-bit",	0xAC, 0, 512, 0, LP_OPTIONS},
-	{"NAND 512MiB 3,3V 8-bit",	0xDC, 0, 512, 0, LP_OPTIONS},
-	{"NAND 512MiB 1,8V 16-bit",	0xBC, 0, 512, 0, LP_OPTIONS16},
-	{"NAND 512MiB 3,3V 16-bit",	0xCC, 0, 512, 0, LP_OPTIONS16},
-
-	{"NAND 1GiB 1,8V 8-bit",	0xA3, 0, 1024, 0, LP_OPTIONS},
-	{"NAND 1GiB 3,3V 8-bit",	0xD3, 0, 1024, 0, LP_OPTIONS},
-	{"NAND 1GiB 1,8V 16-bit",	0xB3, 0, 1024, 0, LP_OPTIONS16},
-	{"NAND 1GiB 3,3V 16-bit",	0xC3, 0, 1024, 0, LP_OPTIONS16},
-
-	{"NAND 2GiB 1,8V 8-bit",	0xA5, 0, 2048, 0, LP_OPTIONS},
-	{"NAND 2GiB 3,3V 8-bit",	0xD5, 0, 2048, 0, LP_OPTIONS},
-	{"NAND 2GiB 1,8V 16-bit",	0xB5, 0, 2048, 0, LP_OPTIONS16},
-	{"NAND 2GiB 3,3V 16-bit",	0xC5, 0, 2048, 0, LP_OPTIONS16},
-
-	{NULL, 0,}
-};
-
-/* Manufacturer ID list
- */
-nand_manufacturer_t nand_manuf_ids[] =
-{
-	{0x0, "unknown"},
-	{NAND_MFR_TOSHIBA, "Toshiba"},
-	{NAND_MFR_SAMSUNG, "Samsung"},
-	{NAND_MFR_FUJITSU, "Fujitsu"},
-	{NAND_MFR_NATIONAL, "National"},
-	{NAND_MFR_RENESAS, "Renesas"},
-	{NAND_MFR_STMICRO, "ST Micro"},
-	{NAND_MFR_HYNIX, "Hynix"},
-	{0x0, NULL},
-};
-
-/* nand device <nand_controller> [controller options]
- */
-int handle_nand_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	int i;
-	int retval;
-		
-	if (argc < 1)
-	{
-		WARNING("incomplete flash device nand configuration");
-		return ERROR_FLASH_BANK_INVALID;
-	}
-	
-	for (i = 0; nand_flash_controllers[i]; i++)
-	{
-		nand_device_t *p, *c;
-		
-		if (strcmp(args[0], nand_flash_controllers[i]->name) == 0)
-		{
-			/* register flash specific commands */
-			if (nand_flash_controllers[i]->register_commands(cmd_ctx) != ERROR_OK)
-			{
-				ERROR("couldn't register '%s' commands", args[0]);
-				exit(-1);
-			}
-	
-			c = malloc(sizeof(nand_device_t));
-
-			c->controller = nand_flash_controllers[i];
-			c->controller_priv = NULL;
-			c->manufacturer = NULL;
-			c->device = NULL;
-			c->bus_width = 0;
-			c->address_cycles = 0;
-			c->page_size = 0;
-			c->use_raw = 0;
-			c->next = NULL;
-
-			if ((retval = nand_flash_controllers[i]->nand_device_command(cmd_ctx, cmd, args, argc, c)) != ERROR_OK)
-			{
-				ERROR("'%s' driver rejected nand flash", c->controller->name);
-				free(c);
-				return ERROR_OK;
-			}
-			
-			/* put NAND device in linked list */
-			if (nand_devices)
-			{
-				/* find last flash device */
-				for (p = nand_devices; p && p->next; p = p->next);
-				if (p)
-					p->next = c;
-			}
-			else
-			{
-				nand_devices = c;
-			}
-			
-			return ERROR_OK;
-		}
-	}
-
-	/* no valid NAND controller was found (i.e. the configuration option,
-	 * didn't match one of the compiled-in controllers)
-	 */
-	ERROR("No valid NAND flash controller found (%s)", args[0]);
-	ERROR("compiled-in NAND flash controllers:");
-	for (i = 0; nand_flash_controllers[i]; i++)
-	{
-		ERROR("%i: %s", i, nand_flash_controllers[i]->name);
-	}
-	
-	return ERROR_OK;
-}
-
-int nand_register_commands(struct command_context_s *cmd_ctx)
-{
-	nand_cmd = register_command(cmd_ctx, NULL, "nand", NULL, COMMAND_ANY, "NAND specific commands");
-	
-	register_command(cmd_ctx, nand_cmd, "device", handle_nand_device_command, COMMAND_CONFIG, NULL);
-	
-	return ERROR_OK;
-}
-
-int nand_init(struct command_context_s *cmd_ctx)
-{
-	if (nand_devices)
-	{
-		register_command(cmd_ctx, nand_cmd, "list", handle_nand_list_command, COMMAND_EXEC,
-						 "list configured NAND flash devices");
-		register_command(cmd_ctx, nand_cmd, "info", handle_nand_info_command, COMMAND_EXEC,
-						 "print info about NAND flash device <num>");
-		register_command(cmd_ctx, nand_cmd, "probe", handle_nand_probe_command, COMMAND_EXEC,
-						 "identify NAND flash device <num>");
-		register_command(cmd_ctx, nand_cmd, "check_bad_blocks", handle_nand_check_bad_blocks_command, COMMAND_EXEC,
-						 "check NAND flash device <num> for bad blocks [<first> <last>]");
-		register_command(cmd_ctx, nand_cmd, "erase", handle_nand_erase_command, COMMAND_EXEC,
-						 "erase blocks on NAND flash device <num> <first> <last>");
-		register_command(cmd_ctx, nand_cmd, "copy", handle_nand_copy_command, COMMAND_EXEC,
-						 "copy from NAND flash device <num> <offset> <length> <ram-address>");
-		register_command(cmd_ctx, nand_cmd, "dump", handle_nand_dump_command, COMMAND_EXEC,
-						 "dump from NAND flash device <num> <filename> <offset> <size> [options]");
-		register_command(cmd_ctx, nand_cmd, "write", handle_nand_write_command, COMMAND_EXEC,
-						 "write to NAND flash device <num> <filename> <offset> [options]");
-		register_command(cmd_ctx, nand_cmd, "raw_access", handle_nand_raw_access_command, COMMAND_EXEC,
-						 "raw access to NAND flash device <num> ['enable'|'disable']");
-	}
-	
-	return ERROR_OK;
-}
-
-nand_device_t *get_nand_device_by_num(int num)
-{
-	nand_device_t *p;
-	int i = 0;
-
-	for (p = nand_devices; p; p = p->next)
-	{
-		if (i++ == num)
-		{
-			return p;
-		}
-	}
-	
-	return NULL;
-}
-
-int nand_build_bbt(struct nand_device_s *device, int first, int last)
-{
-	u32 page = 0x0;
-	int i;
-	u8 *oob;
-	
-	oob = malloc(6);
-	
-	if ((first < 0) || (first >= device->num_blocks))
-		first = 0;
-	
-	if ((last >= device->num_blocks) || (last == -1))
-		last = device->num_blocks - 1;
-	
-	for (i = first; i < last; i++)
-	{
-		nand_read_page(device, page, NULL, 0, oob, 6);
-		
-		if (((device->device->options & NAND_BUSWIDTH_16) && ((oob[0] & oob[1]) != 0xff))
-			|| (((device->page_size == 512) && (oob[5] != 0xff)) ||
-				((device->page_size == 2048) && (oob[0] != 0xff))))
-		{
-			WARNING("invalid block: %i", i);
-			device->blocks[i].is_bad = 1;
-		}
-		else
-		{
-			device->blocks[i].is_bad = 0;
-		}
-		
-		page += (device->erase_size / device->page_size);
-	}
-	
-	return ERROR_OK;
-}
-
-int nand_read_status(struct nand_device_s *device, u8 *status)
-{
-	if (!device->device)
-		return ERROR_NAND_DEVICE_NOT_PROBED;
-		
-	/* Send read status command */
-	device->controller->command(device, NAND_CMD_STATUS);
-	
-	usleep(1000);
-	
-	/* read status */
-	if (device->device->options & NAND_BUSWIDTH_16)
-	{
-		u16 data;
-		device->controller->read_data(device, &data);
-		*status = data & 0xff;
-	}
-	else
-	{
-		device->controller->read_data(device, status);
-	}
-			
-	return ERROR_OK;
-}
-
-int nand_probe(struct nand_device_s *device)
-{
-	u8 manufacturer_id, device_id;
-	u8 id_buff[5];
-	int retval;
-	int i;
-
-	/* clear device data */
-	device->device = NULL;
-	device->manufacturer = NULL;
-	
-	/* clear device parameters */
-	device->bus_width = 0;
-	device->address_cycles = 0;
-	device->page_size = 0;
-	device->erase_size = 0;
-	
-	/* initialize controller (device parameters are zero, use controller default) */
-	if ((retval = device->controller->init(device) != ERROR_OK))
-	{
-		switch (retval)
-		{
-			case ERROR_NAND_OPERATION_FAILED:
-				DEBUG("controller initialization failed");
-				return ERROR_NAND_OPERATION_FAILED;
-			case ERROR_NAND_OPERATION_NOT_SUPPORTED:
-				ERROR("BUG: controller reported that it doesn't support default parameters");
-				return ERROR_NAND_OPERATION_FAILED;
-			default:
-				ERROR("BUG: unknown controller initialization failure");
-				return ERROR_NAND_OPERATION_FAILED;
-		}
-	}
-	
-	device->controller->command(device, NAND_CMD_RESET);
-	device->controller->reset(device);
-
-	device->controller->command(device, NAND_CMD_READID);
-	device->controller->address(device, 0x0);
-	
-	if (device->bus_width == 8)
-	{
-		device->controller->read_data(device, &manufacturer_id);
-		device->controller->read_data(device, &device_id);
-	}
-	else
-	{
-		u16 data_buf;
-		device->controller->read_data(device, &data_buf);
-		manufacturer_id = data_buf & 0xff;
-		device->controller->read_data(device, &data_buf);
-		device_id = data_buf & 0xff;
-	}
-		
-	for (i = 0; nand_flash_ids[i].name; i++)
-	{
-		if (nand_flash_ids[i].id == device_id)
-		{
-			device->device = &nand_flash_ids[i];
-			break;
-		}
-	}
-	
-	for (i = 0; nand_manuf_ids[i].name; i++)
-	{
-		if (nand_manuf_ids[i].id == manufacturer_id)
-		{
-			device->manufacturer = &nand_manuf_ids[i];
-			break;
-		}
-	}
-	
-	if (!device->manufacturer)
-	{
-		device->manufacturer = &nand_manuf_ids[0];
-		device->manufacturer->id = manufacturer_id;
-	}
-	
-	if (!device->device)
-	{
-		ERROR("unknown NAND flash device found, manufacturer id: 0x%2.2x device id: 0x%2.2x",
-			manufacturer_id, device_id);
-		return ERROR_NAND_OPERATION_FAILED;
-	}
-	
-	DEBUG("found %s (%s)", device->device->name, device->manufacturer->name);
-	
-	/* initialize device parameters */
-	
-	/* bus width */ 
-	if (device->device->options & NAND_BUSWIDTH_16)
-		device->bus_width = 16;
-	else
-		device->bus_width = 8;
-
-	/* Do we need extended device probe information? */
-	if (device->device->page_size == 0 ||
-	    device->device->erase_size == 0)
-	{
-		if (device->bus_width == 8)
-		{
-			device->controller->read_data(device, id_buff+3);
-			device->controller->read_data(device, id_buff+4);
-			device->controller->read_data(device, id_buff+5);
-		}
-		else
-		{
-			u16 data_buf;
-
-			device->controller->read_data(device, &data_buf);
-			id_buff[3] = data_buf;
-
-			device->controller->read_data(device, &data_buf);
-			id_buff[4] = data_buf;
-
-			device->controller->read_data(device, &data_buf);
-			id_buff[5] = data_buf >> 8;
-		}
-	}
-		
-	/* page size */
-	if (device->device->page_size == 0)
-	{
-		device->page_size = 1 << (10 + (id_buff[4] & 3));
-	}
-	else if (device->device->page_size == 256)
-	{
-		ERROR("NAND flashes with 256 byte pagesize are not supported");
-		return ERROR_NAND_OPERATION_FAILED;
-	}
-	else
-	{
-		device->page_size = device->device->page_size;
-	}
-	
-	/* number of address cycles */
-	if (device->page_size <= 512)
-	{
-		/* small page devices */
-		if (device->device->chip_size <= 32)
-			device->address_cycles = 3;
-		else if (device->device->chip_size <= 8*1024)
-			device->address_cycles = 4;
-		else
-		{
-			ERROR("BUG: small page NAND device with more than 8 GiB encountered");
-			device->address_cycles = 5;
-		}
-	}
-	else
-	{
-		/* large page devices */
-		if (device->device->chip_size <= 128)
-			device->address_cycles = 4;
-		else if (device->device->chip_size <= 32*1024)
-			device->address_cycles = 5;
-		else
-		{
-			ERROR("BUG: small page NAND device with more than 32 GiB encountered");
-			device->address_cycles = 6;
-		}
-	}
-	
-	/* erase size */
-	if (device->device->erase_size == 0)
-	{
-		switch ((id_buff[4] >> 4) & 3) {
-		case 0:
-			device->erase_size = 64 << 10;
-			break;
-		case 1:
-			device->erase_size = 128 << 10;
-			break;
-		case 2:
-			device->erase_size = 256 << 10;
-			break;
-		case 3:
-			device->erase_size =512 << 10;
-			break;
-		}
-	}
-	else
-	{
-		device->erase_size = device->device->erase_size;
-	}
-	
-	/* initialize controller, but leave parameters at the controllers default */
-	if ((retval = device->controller->init(device) != ERROR_OK))
-	{
-		switch (retval)
-		{
-			case ERROR_NAND_OPERATION_FAILED:
-				DEBUG("controller initialization failed");
-				return ERROR_NAND_OPERATION_FAILED;
-			case ERROR_NAND_OPERATION_NOT_SUPPORTED:
-				ERROR("controller doesn't support requested parameters (buswidth: %i, address cycles: %i, page size: %i)",
-					device->bus_width, device->address_cycles, device->page_size);
-				return ERROR_NAND_OPERATION_FAILED;
-			default:
-				ERROR("BUG: unknown controller initialization failure");
-				return ERROR_NAND_OPERATION_FAILED;
-		}
-	}
-	
-	device->num_blocks = (device->device->chip_size * 1024) / (device->erase_size / 1024);
-	device->blocks = malloc(sizeof(nand_block_t) * device->num_blocks);
-	
-	for (i = 0; i < device->num_blocks; i++)
-	{
-		device->blocks[i].size = device->erase_size;
-		device->blocks[i].offset = i * device->erase_size;
-		device->blocks[i].is_erased = -1;
-		device->blocks[i].is_bad = -1;
-	}
-	
-	return ERROR_OK;
-}
-
-int nand_erase(struct nand_device_s *device, int first_block, int last_block)
-{
-	int i;
-	u32 page;
-	u8 status;
-	int retval;
-	
-	if (!device->device)
-		return ERROR_NAND_DEVICE_NOT_PROBED;
-	
-	if ((first_block < 0) || (last_block > device->num_blocks))
-		return ERROR_INVALID_ARGUMENTS;
-	
-	/* make sure we know if a block is bad before erasing it */
-	for (i = first_block; i <= last_block; i++)
-	{
-		if (device->blocks[i].is_bad == -1)
-		{
-			nand_build_bbt(device, i, last_block);
-			break;
-		}
-	}
-	
-	for (i = first_block; i <= last_block; i++)
-	{
-		/* Send erase setup command */
-		device->controller->command(device, NAND_CMD_ERASE1);
-		
-		page = i * (device->erase_size / device->page_size);
-		
-		/* Send page address */
-		if (device->page_size <= 512)
-		{
-			/* row */
-			device->controller->address(device, page & 0xff);
-			device->controller->address(device, (page >> 8) & 0xff);
-			
-			/* 3rd cycle only on devices with more than 32 MiB */
-			if (device->address_cycles >= 4)
-				device->controller->address(device, (page >> 16) & 0xff);
-	
-			/* 4th cycle only on devices with more than 8 GiB */
-			if (device->address_cycles >= 5)
-				device->controller->address(device, (page >> 24) & 0xff);
-		}
-		else
-		{
-			/* row */
-			device->controller->address(device, page & 0xff);
-			device->controller->address(device, (page >> 8) & 0xff);
-	
-			/* 3rd cycle only on devices with more than 128 MiB */
-			if (device->address_cycles >= 5)
-				device->controller->address(device, (page >> 16) & 0xff);
-		}
-		
-		/* Send erase confirm command */
-		device->controller->command(device, NAND_CMD_ERASE2);
-		
-		if (!device->controller->nand_ready(device, 1000))
-		{
-			ERROR("timeout waiting for NAND flash block erase to complete");
-			return ERROR_NAND_OPERATION_TIMEOUT;
-		}
-		
-		if ((retval = nand_read_status(device, &status)) != ERROR_OK)
-		{
-			ERROR("couldn't read status");
-			return ERROR_NAND_OPERATION_FAILED;
-		}
-		
-		if (status & 0x1)
-		{
-			ERROR("erase operation didn't pass, status: 0x%2.2x", status);
-			return ERROR_NAND_OPERATION_FAILED;
-		}
-	}
-	
-	return ERROR_OK;
-}
-
-int nand_read_plain(struct nand_device_s *device, u32 address, u8 *data, u32 data_size)
-{
-	u8 *page;
-	
-	if (!device->device)
-		return ERROR_NAND_DEVICE_NOT_PROBED;
-		
-	if (address % device->page_size)
-	{
-		ERROR("reads need to be page aligned");
-		return ERROR_NAND_OPERATION_FAILED;
-	}
-	
-	page = malloc(device->page_size);
-	
-	while (data_size > 0 )
-	{
-		u32 thisrun_size = (data_size > device->page_size) ? device->page_size : data_size;
-		u32 page_address;
-		
-		
-		page_address = address / device->page_size;
-		
-		nand_read_page(device, page_address, page, device->page_size, NULL, 0);
-
-		memcpy(data, page, thisrun_size);
-		
-		address += thisrun_size;
-		data += thisrun_size;
-		data_size -= thisrun_size;
-	}
-	
-	free(page);
-	
-	return ERROR_OK;
-}
-
-int nand_write_plain(struct nand_device_s *device, u32 address, u8 *data, u32 data_size)
-{
-	u8 *page;
-	
-	if (!device->device)
-		return ERROR_NAND_DEVICE_NOT_PROBED;
-		
-	if (address % device->page_size)
-	{
-		ERROR("writes need to be page aligned");
-		return ERROR_NAND_OPERATION_FAILED;
-	}
-	
-	page = malloc(device->page_size);
-	
-	while (data_size > 0 )
-	{
-		u32 thisrun_size = (data_size > device->page_size) ? device->page_size : data_size;
-		u32 page_address;
-		
-		memset(page, 0xff, device->page_size);
-		memcpy(page, data, thisrun_size);
-		
-		page_address = address / device->page_size;
-		
-		nand_write_page(device, page_address, page, device->page_size, NULL, 0);
-		
-		address += thisrun_size;
-		data += thisrun_size;
-		data_size -= thisrun_size;
-	}
-	
-	free(page);
-	
-	return ERROR_OK;
-}
-
-int nand_write_page(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size)
-{
-	if (!device->device)
-		return ERROR_NAND_DEVICE_NOT_PROBED;
-		
-	if (device->use_raw || device->controller->write_page == NULL)
-		return nand_write_page_raw(device, page, data, data_size, oob, oob_size);
-	else
-		return device->controller->write_page(device, page, data, data_size, oob, oob_size);
-}
-
-int nand_read_page(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size)
-{
-	if (!device->device)
-		return ERROR_NAND_DEVICE_NOT_PROBED;
-		
-	if (device->use_raw || device->controller->read_page == NULL)
-		return nand_read_page_raw(device, page, data, data_size, oob, oob_size);
-	else
-		return device->controller->read_page(device, page, data, data_size, oob, oob_size);
-}
-
-int nand_read_page_raw(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size)
-{
-	int i;
-	
-	if (!device->device)
-		return ERROR_NAND_DEVICE_NOT_PROBED;
-
-	if (device->page_size <= 512)
-	{
-		/* small page device */
-		if (data)
-			device->controller->command(device, NAND_CMD_READ0);
-		else
-			device->controller->command(device, NAND_CMD_READOOB);
-		
-		/* column (always 0, we start at the beginning of a page/OOB area) */
-		device->controller->address(device, 0x0);
-		
-		/* row */
-		device->controller->address(device, page & 0xff);
-		device->controller->address(device, (page >> 8) & 0xff);
-		
-		/* 4th cycle only on devices with more than 32 MiB */
-		if (device->address_cycles >= 4)
-			device->controller->address(device, (page >> 16) & 0xff);
-
-		/* 5th cycle only on devices with more than 8 GiB */
-		if (device->address_cycles >= 5)
-			device->controller->address(device, (page >> 24) & 0xff);
-	}
-	else
-	{
-		/* large page device */
-		device->controller->command(device, NAND_CMD_READ0);
-		
-		/* column (0 when we start at the beginning of a page,
-		 * or 2048 for the beginning of OOB area)
-		 */
-		device->controller->address(device, 0x0);
-		device->controller->address(device, 0x8);
-		
-		/* row */
-		device->controller->address(device, page & 0xff);
-		device->controller->address(device, (page >> 8) & 0xff);
-
-		/* 5th cycle only on devices with more than 128 MiB */
-		if (device->address_cycles >= 5)
-			device->controller->address(device, (page >> 16) & 0xff);
-
-		/* large page devices need a start command */
-		device->controller->command(device, NAND_CMD_READSTART);
-	}
-	
-	if (!device->controller->nand_ready(device, 100))
-		return ERROR_NAND_OPERATION_TIMEOUT;
-	
-	if (data)
-	{
-		if (device->controller->read_block_data != NULL)
-			(device->controller->read_block_data)(device, data, data_size);
-		else
-		{
-			for (i = 0; i < data_size;)
-			{
-				if (device->device->options & NAND_BUSWIDTH_16)
-				{
-					device->controller->read_data(device, data);
-					data += 2;
-					i += 2;
-				}
-				else
-				{
-					device->controller->read_data(device, data);
-					data += 1;
-					i += 1;
-				}
-			}
-		}
-	}
-	
-	if (oob)
-	{
-		if (device->controller->read_block_data != NULL)
-			(device->controller->read_block_data)(device, oob, oob_size);
-		else
-		{
-			for (i = 0; i < oob_size;)
-			{
-				if (device->device->options & NAND_BUSWIDTH_16)
-				{
-					device->controller->read_data(device, oob);
-					oob += 2;
-					i += 2;
-				}
-				else
-				{
-					device->controller->read_data(device, oob);
-					oob += 1;
-					i += 1;
-				}
-			}
-		}
-	}
-	
-	return ERROR_OK;	
-}
-
-int nand_write_page_raw(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size)
-{
-	int i;
-	int retval;
-	u8 status;
-	
-	if (!device->device)
-		return ERROR_NAND_DEVICE_NOT_PROBED;
-
-	device->controller->command(device, NAND_CMD_SEQIN);
-	
-	if (device->page_size <= 512)
-	{
-		/* column (always 0, we start at the beginning of a page/OOB area) */
-		device->controller->address(device, 0x0);
-		
-		/* row */
-		device->controller->address(device, page & 0xff);
-		device->controller->address(device, (page >> 8) & 0xff);
-		
-		/* 4th cycle only on devices with more than 32 MiB */
-		if (device->address_cycles >= 4)
-			device->controller->address(device, (page >> 16) & 0xff);
-
-		/* 5th cycle only on devices with more than 8 GiB */
-		if (device->address_cycles >= 5)
-			device->controller->address(device, (page >> 24) & 0xff);
-	}
-	else
-	{
-		/* column (0 when we start at the beginning of a page,
-		 * or 2048 for the beginning of OOB area)
-		 */
-		device->controller->address(device, 0x0);
-		device->controller->address(device, 0x8);
-		
-		/* row */
-		device->controller->address(device, page & 0xff);
-		device->controller->address(device, (page >> 8) & 0xff);
-
-		/* 5th cycle only on devices with more than 128 MiB */
-		if (device->address_cycles >= 5)
-			device->controller->address(device, (page >> 16) & 0xff);
-	}
-	
-	if (data)
-	{
-		if (device->controller->write_block_data != NULL)
-			(device->controller->write_block_data)(device, data, data_size);
-		else
-		{
-			for (i = 0; i < data_size;)
-			{
-				if (device->device->options & NAND_BUSWIDTH_16)
-				{
-					u16 data_buf = le_to_h_u16(data);
-					device->controller->write_data(device, data_buf);
-					data += 2;
-					i += 2;
-				}
-				else
-				{
-					device->controller->write_data(device, *data);
-					data += 1;
-					i += 1;
-				}
-			}
-		}
-	}
-	
-	if (oob)
-	{
-		if (device->controller->write_block_data != NULL)
-			(device->controller->write_block_data)(device, oob, oob_size);
-		else
-		{
-			for (i = 0; i < oob_size;)
-			{
-				if (device->device->options & NAND_BUSWIDTH_16)
-				{
-					u16 oob_buf = le_to_h_u16(data);
-					device->controller->write_data(device, oob_buf);
-					oob += 2;
-					i += 2;
-				}
-				else
-				{
-					device->controller->write_data(device, *oob);
-					oob += 1;
-					i += 1;
-				}
-			}
-		}
-	}
-	
-	device->controller->command(device, NAND_CMD_PAGEPROG);
-	
-	if (!device->controller->nand_ready(device, 100))
-		return ERROR_NAND_OPERATION_TIMEOUT;
-	
-	if ((retval = nand_read_status(device, &status)) != ERROR_OK)
-	{
-		ERROR("couldn't read status");
-		return ERROR_NAND_OPERATION_FAILED;
-	}
-		
-	if (status & NAND_STATUS_FAIL)
-	{
-		ERROR("write operation didn't pass, status: 0x%2.2x", status);
-		return ERROR_NAND_OPERATION_FAILED;
-	}
-	
-	return ERROR_OK;	
-}
-
-int handle_nand_list_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	nand_device_t *p;
-	int i = 0;
-	
-	if (!nand_devices)
-	{
-		command_print(cmd_ctx, "no NAND flash devices configured");
-		return ERROR_OK;
-	}
-	
-	for (p = nand_devices; p; p = p->next)
-	{
-		if (p->device)
-			command_print(cmd_ctx, "#%i: %s (%s) pagesize: %i, buswidth: %i, erasesize: %i",
-				i++, p->device->name, p->manufacturer->name, p->page_size, p->bus_width, p->erase_size);
-		else
-			command_print(cmd_ctx, "#%i: not probed");
-	}
-	
-	return ERROR_OK;
-}
-
-int handle_nand_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	nand_device_t *p;
-	int i = 0;
-	int j = 0;
-	int first = -1;
-	int last = -1;
-		
-	if ((argc < 1) || (argc > 3))
-	{
-		command_print(cmd_ctx, "usage: nand info <num> [<first> <last>]");
-		return ERROR_OK;
-	}
-	
-	if (argc == 2)
-	{
-		first = last = strtoul(args[1], NULL, 0);
-	}
-	else if (argc == 3)
-	{
-		first = strtoul(args[1], NULL, 0);
-		last = strtoul(args[2], NULL, 0);
-	}
-		
-	p = get_nand_device_by_num(strtoul(args[0], NULL, 0));
-	if (p)
-	{
-		if (p->device)
-		{
-			if (first >= p->num_blocks)
-				first = p->num_blocks - 1;
-			
-			if (last >= p->num_blocks)
-				last = p->num_blocks - 1;
-			
-			command_print(cmd_ctx, "#%i: %s (%s) pagesize: %i, buswidth: %i, erasesize: %i",
-				i++, p->device->name, p->manufacturer->name, p->page_size, p->bus_width, p->erase_size);
-			
-			for (j = first; j <= last; j++)
-			{
-				char *erase_state, *bad_state;
-				
-				if (p->blocks[j].is_erased == 0)
-					erase_state = "not erased";
-				else if (p->blocks[j].is_erased == 1)
-					erase_state = "erased";
-				else
-					erase_state = "erase state unknown";
-				
-				if (p->blocks[j].is_bad == 0)
-					bad_state = "";
-				else if (p->blocks[j].is_bad == 1)
-					bad_state = " (marked bad)";
-				else
-					bad_state = " (block condition unknown)";
-
-				command_print(cmd_ctx, "\t#%i: 0x%8.8x (0x%xkB) %s%s",
-							j, p->blocks[j].offset, p->blocks[j].size / 1024,
-							erase_state, bad_state);
-			}
-		}
-		else
-		{
-			command_print(cmd_ctx, "#%i: not probed");
-		}
-	}
-	
-	return ERROR_OK;
-}
-
-int handle_nand_probe_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	nand_device_t *p;
-	int retval;
-		
-	if (argc != 1)
-	{
-		command_print(cmd_ctx, "usage: nand probe <num>");
-		return ERROR_OK;
-	}
-	
-	p = get_nand_device_by_num(strtoul(args[0], NULL, 0));
-	if (p)
-	{
-		if ((retval = nand_probe(p)) == ERROR_OK)
-		{
-			command_print(cmd_ctx, "NAND flash device '%s' found", p->device->name);
-		}
-		else if (retval == ERROR_NAND_OPERATION_FAILED)
-		{
-			command_print(cmd_ctx, "probing failed for NAND flash device");
-		}
-		else
-		{
-			command_print(cmd_ctx, "unknown error when probing NAND flash device");
-		}
-	}
-	else
-	{
-		command_print(cmd_ctx, "NAND flash device '#%s' is out of bounds", args[0]);
-	}
-	
-	return ERROR_OK;
-}
-
-int handle_nand_erase_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	nand_device_t *p;
-	int retval;
-		
-	if (argc != 3)
-	{
-		command_print(cmd_ctx, "usage: nand erase <num> <first> <last>");
-		return ERROR_OK;
-	}
-	
-	p = get_nand_device_by_num(strtoul(args[0], NULL, 0));
-	if (p)
-	{
-		int first = strtoul(args[1], NULL, 0);
-		int last = strtoul(args[2], NULL, 0);
-		
-		if ((retval = nand_erase(p, first, last)) == ERROR_OK)
-		{
-			command_print(cmd_ctx, "successfully erased blocks %i to %i on NAND flash device '%s'", first, last, p->device->name);
-		}
-		else if (retval == ERROR_NAND_OPERATION_FAILED)
-		{
-			command_print(cmd_ctx, "erase failed");
-		}
-		else
-		{
-			command_print(cmd_ctx, "unknown error when erasing NAND flash device");
-		}
-	}
-	else
-	{
-		command_print(cmd_ctx, "NAND flash device '#%s' is out of bounds", args[0]);
-	}
-	
-	return ERROR_OK;
-}
-
-int handle_nand_check_bad_blocks_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	nand_device_t *p;
-	int retval;
-	int first = -1;
-	int last = -1;
-		
-	if ((argc < 1) || (argc > 3) || (argc == 2))
-	{
-		command_print(cmd_ctx, "usage: nand check_bad_blocks <num> [<first> <last>]");
-		return ERROR_OK;
-	}
-	
-	if (argc == 3)
-	{
-		first = strtoul(args[1], NULL, 0);
-		last = strtoul(args[2], NULL, 0);
-	}
-	
-	p = get_nand_device_by_num(strtoul(args[0], NULL, 0));
-	if (p)
-	{
-		if ((retval = nand_build_bbt(p, first, last)) == ERROR_OK)
-		{
-			command_print(cmd_ctx, "checked NAND flash device for bad blocks, use \"nand info\" command to list blocks", p->device->name);
-		}
-		else if (retval == ERROR_NAND_OPERATION_FAILED)
-		{
-			command_print(cmd_ctx, "error when checking for bad blocks on NAND flash device");
-		}
-		else
-		{
-			command_print(cmd_ctx, "unknown error when checking for bad blocks on NAND flash device");
-		}
-	}
-	else
-	{
-		command_print(cmd_ctx, "NAND flash device '#%s' is out of bounds", args[0]);
-	}
-	
-	return ERROR_OK;
-}
-
-int handle_nand_copy_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	nand_device_t *p;
-		
-	if (argc != 4)
-	{
-		command_print(cmd_ctx, "usage: nand copy <num> <offset> <length> <ram-address>");
-		return ERROR_OK;
-	}
-	
-	p = get_nand_device_by_num(strtoul(args[0], NULL, 0));
-	if (p)
-	{
-
-	}
-	else
-	{
-		command_print(cmd_ctx, "NAND flash device '#%s' is out of bounds", args[0]);
-	}
-	
-	return ERROR_OK;
-}
-
-int handle_nand_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	u32 offset;
-	u32 binary_size;
-	u32 buf_cnt;
-	enum oob_formats oob_format = NAND_OOB_NONE;
-	
-	fileio_t fileio;
-	
-	duration_t duration;
-	char *duration_text;
-	
-	nand_device_t *p;
-		
-	if (argc < 3)
-	{
-		command_print(cmd_ctx, "usage: nand write <num> <file> <offset> [options]");
-		return ERROR_OK;
-	}
-	
-	p = get_nand_device_by_num(strtoul(args[0], NULL, 0));
-	if (p)
-	{
-		u8 *page = NULL;
-		u32 page_size = 0;
-		u8 *oob = NULL;
-		u32 oob_size = 0;
-			
-		duration_start_measure(&duration);
-		offset = strtoul(args[2], NULL, 0);
-		
-		if (argc > 3)
-		{
-			int i;
-			for (i = 3; i < argc; i++)
-			{
-				if (!strcmp(args[i], "oob_raw"))
-					oob_format |= NAND_OOB_RAW;
-				else if (!strcmp(args[i], "oob_only"))
-					oob_format |= NAND_OOB_RAW | NAND_OOB_ONLY;
-				else
-				{
-					command_print(cmd_ctx, "unknown option: %s", args[i]);
-				}
-			}
-		}
-		
-		if (fileio_open(&fileio, args[1], FILEIO_READ, FILEIO_BINARY) != ERROR_OK)
-		{
-			command_print(cmd_ctx, "file open error: %s", fileio.error_str);
-			return ERROR_OK;
-		}
-	
-		buf_cnt = binary_size = fileio.size;
-		
-		if (!(oob_format & NAND_OOB_ONLY))
-		{
-			page_size = p->page_size;
-			page = malloc(p->page_size);
-		}
-
-		if (oob_format & NAND_OOB_RAW)
-		{
-			if (p->page_size == 512)
-				oob_size = 16;
-			else if (p->page_size == 2048)
-				oob_size = 64;
-			oob = malloc(oob_size);
-		}
-		
-		if (offset % p->page_size)
-		{
-			command_print(cmd_ctx, "only page size aligned offsets and sizes are supported");
-			return ERROR_OK;
-		}
-		
-		while (buf_cnt > 0)
-		{
-			u32 size_read;
-			
-			if (page)
-			{
-				fileio_read(&fileio, page_size, page, &size_read);
-				buf_cnt -= size_read;
-				if (size_read < page_size)
-				{
-					memset(page + size_read, 0xff, page_size - size_read);
-				}
-			}
-				
-			if (oob)
-			{
-				fileio_read(&fileio, oob_size, oob, &size_read);
-				buf_cnt -= size_read;
-				if (size_read < oob_size)
-				{
-					memset(oob + size_read, 0xff, oob_size - size_read);
-				}
-			}
-			
-			if (nand_write_page(p, offset / p->page_size, page, page_size, oob, oob_size) != ERROR_OK)
-			{
-				command_print(cmd_ctx, "failed writing file %s to NAND flash %s at offset 0x%8.8x",
-					args[1], args[0], offset);
-				return ERROR_OK;
-			}
-			offset += page_size;
-		}
-
-		fileio_close(&fileio);
-		
-		duration_stop_measure(&duration, &duration_text);
-		command_print(cmd_ctx, "wrote file %s to NAND flash %s at offset 0x%8.8x in %s",
-			args[1], args[0], offset, duration_text);
-		free(duration_text);
-	}
-	else
-	{
-		command_print(cmd_ctx, "NAND flash device '#%s' is out of bounds", args[0]);
-	}
-	
-	return ERROR_OK;
-}
-
-int handle_nand_dump_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	nand_device_t *p;
-			
-	if (argc < 4)
-	{
-		command_print(cmd_ctx, "usage: nand dump <num> <filename> <address> <size> [options]");
-		return ERROR_OK;
-	}
-	
-	p = get_nand_device_by_num(strtoul(args[0], NULL, 0));
-	if (p)
-	{
-		if (p->device)
-		{
-			fileio_t fileio;
-			duration_t duration;
-			char *duration_text;
-			int retval;
-			
-			u8 *page = NULL;
-			u32 page_size = 0;
-			u8 *oob = NULL;
-			u32 oob_size = 0;
-			u32 address = strtoul(args[2], NULL, 0);
-			u32 size = strtoul(args[3], NULL, 0);
-			u32 bytes_done = 0;
-			enum oob_formats oob_format = NAND_OOB_NONE;
-			
-			if (argc > 4)
-			{
-				int i;
-				for (i = 4; i < argc; i++)
-				{
-					if (!strcmp(args[i], "oob_raw"))
-						oob_format |= NAND_OOB_RAW;
-					else if (!strcmp(args[i], "oob_only"))
-						oob_format |= NAND_OOB_RAW | NAND_OOB_ONLY;
-					else
-						command_print(cmd_ctx, "unknown option: '%s'", args[i]); 
-				}
-			}
-			
-			if ((address % p->page_size) || (size % p->page_size))
-			{
-				command_print(cmd_ctx, "only page size aligned addresses and sizes are supported");
-				return ERROR_OK;
-			}
-		
-			if (!(oob_format & NAND_OOB_ONLY))
-			{
-				page_size = p->page_size;
-				page = malloc(p->page_size);
-			}
-
-			if (oob_format & NAND_OOB_RAW)
-			{
-				if (p->page_size == 512)
-					oob_size = 16;
-				else if (p->page_size == 2048)
-					oob_size = 64;
-				oob = malloc(oob_size);
-			}
-			
-			if (fileio_open(&fileio, args[1], FILEIO_WRITE, FILEIO_BINARY) != ERROR_OK)
-			{
-				command_print(cmd_ctx, "dump_image error: %s", fileio.error_str);
-				return ERROR_OK;
-			}
-	
-			duration_start_measure(&duration);
-			
-			while (size > 0)
-			{
-				u32 size_written;
-				if ((retval = nand_read_page(p, address / p->page_size, page, page_size, oob, oob_size)) != ERROR_OK)
-				{
-					command_print(cmd_ctx, "reading NAND flash page failed");
-					return ERROR_OK;
-				}
-				
-				if (page)
-				{
-					fileio_write(&fileio, page_size, page, &size_written);
-					bytes_done += page_size;
-				}
-					
-				if (oob)
-				{
-					fileio_write(&fileio, oob_size, oob, &size_written);
-					bytes_done += oob_size;
-				}
-					
-				size -= p->page_size;
-				address += p->page_size;
-			}
-			
-			if (page)
-				free(page);
-				
-			if (oob)
-				free(oob);
-			
-			fileio_close(&fileio);
-
-			duration_stop_measure(&duration, &duration_text);
-			command_print(cmd_ctx, "dumped %"PRIi64" byte in %s", fileio.size, duration_text);
-			free(duration_text);
-		}
-		else
-		{
-			command_print(cmd_ctx, "#%i: not probed");
-		}
-	}
-	else
-	{
-		command_print(cmd_ctx, "NAND flash device '#%s' is out of bounds", args[0]);
-	}
-	
-	return ERROR_OK;
-}
-
-int handle_nand_raw_access_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	nand_device_t *p;
-		
-	if ((argc < 1) || (argc > 2))
-	{
-		command_print(cmd_ctx, "usage: nand raw_access <num> ['enable'|'disable']");
-		return ERROR_OK;
-	}
-	
-	p = get_nand_device_by_num(strtoul(args[0], NULL, 0));
-	if (p)
-	{
-		if (p->device)
-		{
-			if (argc == 2)
-			{
-				if (strcmp("enable", args[1]) == 0)
-				{
-					p->use_raw = 1;
-				}
-				else if (strcmp("disable", args[1]) == 0)
-				{
-					p->use_raw = 0;
-				}
-				else
-				{
-					command_print(cmd_ctx, "usage: nand raw_access ['enable'|disable']");
-				}
-			}
-	
-			command_print(cmd_ctx, "raw access is %s", (p->use_raw) ? "enabled" : "disabled");
-		}
-		else
-		{
-			command_print(cmd_ctx, "#%i: not probed");
-		}
-	}
-	else
-	{
-		command_print(cmd_ctx, "NAND flash device '#%s' is out of bounds", args[0]);
-	}
-	
-	return ERROR_OK;
-}
-
+/***************************************************************************

+ *   Copyright (C) 2007 by Dominic Rath                                    *

+ *   Dominic.Rath@gmx.de                                                   *

+ *                                                                         *

+ *   partially based on                                                    *

+ * 	 drivers/mtd/nand_ids.c                                            *

+ *                                                                         *

+ *   Copyright (C) 2002 Thomas Gleixner (tglx@linutronix.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 "replacements.h"

+#include "log.h"

+

+#include <stdlib.h>

+#include <string.h>

+#include <inttypes.h>

+

+#include <errno.h>

+

+#include "nand.h"

+#include "flash.h"

+#include "time_support.h"

+#include "fileio.h"

+#include "image.h"

+

+int nand_register_commands(struct command_context_s *cmd_ctx);

+int handle_nand_list_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int handle_nand_probe_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int handle_nand_check_bad_blocks_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int handle_nand_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int handle_nand_copy_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int handle_nand_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int handle_nand_dump_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int handle_nand_erase_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+

+int handle_nand_raw_access_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+

+int nand_read_page_raw(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size);

+int nand_read_page(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size);

+int nand_read_plain(struct nand_device_s *device, u32 address, u8 *data, u32 data_size);

+

+int nand_write_page_raw(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size);

+int nand_write_page(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size);

+

+/* NAND flash controller

+ */

+extern nand_flash_controller_t lpc3180_nand_controller;

+extern nand_flash_controller_t s3c2410_nand_controller;

+extern nand_flash_controller_t s3c2412_nand_controller;

+extern nand_flash_controller_t s3c2440_nand_controller;

+extern nand_flash_controller_t s3c2443_nand_controller;

+

+/* extern nand_flash_controller_t boundary_scan_nand_controller; */

+

+nand_flash_controller_t *nand_flash_controllers[] =

+{

+	&lpc3180_nand_controller,

+	&s3c2410_nand_controller,

+	&s3c2412_nand_controller,

+	&s3c2440_nand_controller,

+	&s3c2443_nand_controller,

+/*	&boundary_scan_nand_controller, */

+	NULL

+};

+

+/* configured NAND devices and NAND Flash command handler */

+nand_device_t *nand_devices = NULL;

+static command_t *nand_cmd;

+

+/*	Chip ID list

+ *

+ *	Name, ID code, pagesize, chipsize in MegaByte, eraseblock size,

+ *	options

+ *

+ *	Pagesize; 0, 256, 512

+ *	0	get this information from the extended chip ID

+ *	256	256 Byte page size

+ *	512	512 Byte page size

+ */

+nand_info_t nand_flash_ids[] =

+{

+	{"NAND 1MiB 5V 8-bit",		0x6e, 256, 1, 0x1000, 0},

+	{"NAND 2MiB 5V 8-bit",		0x64, 256, 2, 0x1000, 0},

+	{"NAND 4MiB 5V 8-bit",		0x6b, 512, 4, 0x2000, 0},

+	{"NAND 1MiB 3,3V 8-bit",	0xe8, 256, 1, 0x1000, 0},

+	{"NAND 1MiB 3,3V 8-bit",	0xec, 256, 1, 0x1000, 0},

+	{"NAND 2MiB 3,3V 8-bit",	0xea, 256, 2, 0x1000, 0},

+	{"NAND 4MiB 3,3V 8-bit",	0xd5, 512, 4, 0x2000, 0},

+	{"NAND 4MiB 3,3V 8-bit",	0xe3, 512, 4, 0x2000, 0},

+	{"NAND 4MiB 3,3V 8-bit",	0xe5, 512, 4, 0x2000, 0},

+	{"NAND 8MiB 3,3V 8-bit",	0xd6, 512, 8, 0x2000, 0},

+

+	{"NAND 8MiB 1,8V 8-bit",	0x39, 512, 8, 0x2000, 0},

+	{"NAND 8MiB 3,3V 8-bit",	0xe6, 512, 8, 0x2000, 0},

+	{"NAND 8MiB 1,8V 16-bit",	0x49, 512, 8, 0x2000, NAND_BUSWIDTH_16},

+	{"NAND 8MiB 3,3V 16-bit",	0x59, 512, 8, 0x2000, NAND_BUSWIDTH_16},

+

+	{"NAND 16MiB 1,8V 8-bit",	0x33, 512, 16, 0x4000, 0},

+	{"NAND 16MiB 3,3V 8-bit",	0x73, 512, 16, 0x4000, 0},

+	{"NAND 16MiB 1,8V 16-bit",	0x43, 512, 16, 0x4000, NAND_BUSWIDTH_16},

+	{"NAND 16MiB 3,3V 16-bit",	0x53, 512, 16, 0x4000, NAND_BUSWIDTH_16},

+

+	{"NAND 32MiB 1,8V 8-bit",	0x35, 512, 32, 0x4000, 0},

+	{"NAND 32MiB 3,3V 8-bit",	0x75, 512, 32, 0x4000, 0},

+	{"NAND 32MiB 1,8V 16-bit",	0x45, 512, 32, 0x4000, NAND_BUSWIDTH_16},

+	{"NAND 32MiB 3,3V 16-bit",	0x55, 512, 32, 0x4000, NAND_BUSWIDTH_16},

+

+	{"NAND 64MiB 1,8V 8-bit",	0x36, 512, 64, 0x4000, 0},

+	{"NAND 64MiB 3,3V 8-bit",	0x76, 512, 64, 0x4000, 0},

+	{"NAND 64MiB 1,8V 16-bit",	0x46, 512, 64, 0x4000, NAND_BUSWIDTH_16},

+	{"NAND 64MiB 3,3V 16-bit",	0x56, 512, 64, 0x4000, NAND_BUSWIDTH_16},

+

+	{"NAND 128MiB 1,8V 8-bit",	0x78, 512, 128, 0x4000, 0},

+	{"NAND 128MiB 1,8V 8-bit",	0x39, 512, 128, 0x4000, 0},

+	{"NAND 128MiB 3,3V 8-bit",	0x79, 512, 128, 0x4000, 0},

+	{"NAND 128MiB 1,8V 16-bit",	0x72, 512, 128, 0x4000, NAND_BUSWIDTH_16},

+	{"NAND 128MiB 1,8V 16-bit",	0x49, 512, 128, 0x4000, NAND_BUSWIDTH_16},

+	{"NAND 128MiB 3,3V 16-bit",	0x74, 512, 128, 0x4000, NAND_BUSWIDTH_16},

+	{"NAND 128MiB 3,3V 16-bit",	0x59, 512, 128, 0x4000, NAND_BUSWIDTH_16},

+

+	{"NAND 256MiB 3,3V 8-bit",	0x71, 512, 256, 0x4000, 0},

+

+	{"NAND 64MiB 1,8V 8-bit",	0xA2, 0,  64, 0, LP_OPTIONS},

+	{"NAND 64MiB 3,3V 8-bit",	0xF2, 0,  64, 0, LP_OPTIONS},

+	{"NAND 64MiB 1,8V 16-bit",	0xB2, 0,  64, 0, LP_OPTIONS16},

+	{"NAND 64MiB 3,3V 16-bit",	0xC2, 0,  64, 0, LP_OPTIONS16},

+

+	{"NAND 128MiB 1,8V 8-bit",	0xA1, 0, 128, 0, LP_OPTIONS},

+	{"NAND 128MiB 3,3V 8-bit",	0xF1, 0, 128, 0, LP_OPTIONS},

+	{"NAND 128MiB 1,8V 16-bit",	0xB1, 0, 128, 0, LP_OPTIONS16},

+	{"NAND 128MiB 3,3V 16-bit",	0xC1, 0, 128, 0, LP_OPTIONS16},

+

+	{"NAND 256MiB 1,8V 8-bit",	0xAA, 0, 256, 0, LP_OPTIONS},

+	{"NAND 256MiB 3,3V 8-bit",	0xDA, 0, 256, 0, LP_OPTIONS},

+	{"NAND 256MiB 1,8V 16-bit",	0xBA, 0, 256, 0, LP_OPTIONS16},

+	{"NAND 256MiB 3,3V 16-bit",	0xCA, 0, 256, 0, LP_OPTIONS16},

+

+	{"NAND 512MiB 1,8V 8-bit",	0xAC, 0, 512, 0, LP_OPTIONS},

+	{"NAND 512MiB 3,3V 8-bit",	0xDC, 0, 512, 0, LP_OPTIONS},

+	{"NAND 512MiB 1,8V 16-bit",	0xBC, 0, 512, 0, LP_OPTIONS16},

+	{"NAND 512MiB 3,3V 16-bit",	0xCC, 0, 512, 0, LP_OPTIONS16},

+

+	{"NAND 1GiB 1,8V 8-bit",	0xA3, 0, 1024, 0, LP_OPTIONS},

+	{"NAND 1GiB 3,3V 8-bit",	0xD3, 0, 1024, 0, LP_OPTIONS},

+	{"NAND 1GiB 1,8V 16-bit",	0xB3, 0, 1024, 0, LP_OPTIONS16},

+	{"NAND 1GiB 3,3V 16-bit",	0xC3, 0, 1024, 0, LP_OPTIONS16},

+

+	{"NAND 2GiB 1,8V 8-bit",	0xA5, 0, 2048, 0, LP_OPTIONS},

+	{"NAND 2GiB 3,3V 8-bit",	0xD5, 0, 2048, 0, LP_OPTIONS},

+	{"NAND 2GiB 1,8V 16-bit",	0xB5, 0, 2048, 0, LP_OPTIONS16},

+	{"NAND 2GiB 3,3V 16-bit",	0xC5, 0, 2048, 0, LP_OPTIONS16},

+

+	{NULL, 0,}

+};

+

+/* Manufacturer ID list

+ */

+nand_manufacturer_t nand_manuf_ids[] =

+{

+	{0x0, "unknown"},

+	{NAND_MFR_TOSHIBA, "Toshiba"},

+	{NAND_MFR_SAMSUNG, "Samsung"},

+	{NAND_MFR_FUJITSU, "Fujitsu"},

+	{NAND_MFR_NATIONAL, "National"},

+	{NAND_MFR_RENESAS, "Renesas"},

+	{NAND_MFR_STMICRO, "ST Micro"},

+	{NAND_MFR_HYNIX, "Hynix"},

+	{0x0, NULL},

+};

+

+/* nand device <nand_controller> [controller options]

+ */

+int handle_nand_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	int i;

+	int retval;

+		

+	if (argc < 1)

+	{

+		WARNING("incomplete flash device nand configuration");

+		return ERROR_FLASH_BANK_INVALID;

+	}

+	

+	for (i = 0; nand_flash_controllers[i]; i++)

+	{

+		nand_device_t *p, *c;

+		

+		if (strcmp(args[0], nand_flash_controllers[i]->name) == 0)

+		{

+			/* register flash specific commands */

+			if (nand_flash_controllers[i]->register_commands(cmd_ctx) != ERROR_OK)

+			{

+				ERROR("couldn't register '%s' commands", args[0]);

+				exit(-1);

+			}

+	

+			c = malloc(sizeof(nand_device_t));

+

+			c->controller = nand_flash_controllers[i];

+			c->controller_priv = NULL;

+			c->manufacturer = NULL;

+			c->device = NULL;

+			c->bus_width = 0;

+			c->address_cycles = 0;

+			c->page_size = 0;

+			c->use_raw = 0;

+			c->next = NULL;

+

+			if ((retval = nand_flash_controllers[i]->nand_device_command(cmd_ctx, cmd, args, argc, c)) != ERROR_OK)

+			{

+				ERROR("'%s' driver rejected nand flash", c->controller->name);

+				free(c);

+				return ERROR_OK;

+			}

+			

+			/* put NAND device in linked list */

+			if (nand_devices)

+			{

+				/* find last flash device */

+				for (p = nand_devices; p && p->next; p = p->next);

+				if (p)

+					p->next = c;

+			}

+			else

+			{

+				nand_devices = c;

+			}

+			

+			return ERROR_OK;

+		}

+	}

+

+	/* no valid NAND controller was found (i.e. the configuration option,

+	 * didn't match one of the compiled-in controllers)

+	 */

+	ERROR("No valid NAND flash controller found (%s)", args[0]);

+	ERROR("compiled-in NAND flash controllers:");

+	for (i = 0; nand_flash_controllers[i]; i++)

+	{

+		ERROR("%i: %s", i, nand_flash_controllers[i]->name);

+	}

+	

+	return ERROR_OK;

+}

+

+int nand_register_commands(struct command_context_s *cmd_ctx)

+{

+	nand_cmd = register_command(cmd_ctx, NULL, "nand", NULL, COMMAND_ANY, "NAND specific commands");

+	

+	register_command(cmd_ctx, nand_cmd, "device", handle_nand_device_command, COMMAND_CONFIG, NULL);

+	

+	return ERROR_OK;

+}

+

+int nand_init(struct command_context_s *cmd_ctx)

+{

+	if (nand_devices)

+	{

+		register_command(cmd_ctx, nand_cmd, "list", handle_nand_list_command, COMMAND_EXEC,

+						 "list configured NAND flash devices");

+		register_command(cmd_ctx, nand_cmd, "info", handle_nand_info_command, COMMAND_EXEC,

+						 "print info about NAND flash device <num>");

+		register_command(cmd_ctx, nand_cmd, "probe", handle_nand_probe_command, COMMAND_EXEC,

+						 "identify NAND flash device <num>");

+		register_command(cmd_ctx, nand_cmd, "check_bad_blocks", handle_nand_check_bad_blocks_command, COMMAND_EXEC,

+						 "check NAND flash device <num> for bad blocks [<first> <last>]");

+		register_command(cmd_ctx, nand_cmd, "erase", handle_nand_erase_command, COMMAND_EXEC,

+						 "erase blocks on NAND flash device <num> <first> <last>");

+		register_command(cmd_ctx, nand_cmd, "copy", handle_nand_copy_command, COMMAND_EXEC,

+						 "copy from NAND flash device <num> <offset> <length> <ram-address>");

+		register_command(cmd_ctx, nand_cmd, "dump", handle_nand_dump_command, COMMAND_EXEC,

+						 "dump from NAND flash device <num> <filename> <offset> <size> [options]");

+		register_command(cmd_ctx, nand_cmd, "write", handle_nand_write_command, COMMAND_EXEC,

+						 "write to NAND flash device <num> <filename> <offset> [options]");

+		register_command(cmd_ctx, nand_cmd, "raw_access", handle_nand_raw_access_command, COMMAND_EXEC,

+						 "raw access to NAND flash device <num> ['enable'|'disable']");

+	}

+	

+	return ERROR_OK;

+}

+

+nand_device_t *get_nand_device_by_num(int num)

+{

+	nand_device_t *p;

+	int i = 0;

+

+	for (p = nand_devices; p; p = p->next)

+	{

+		if (i++ == num)

+		{

+			return p;

+		}

+	}

+	

+	return NULL;

+}

+

+int nand_build_bbt(struct nand_device_s *device, int first, int last)

+{

+	u32 page = 0x0;

+	int i;

+	u8 *oob;

+	

+	oob = malloc(6);

+	

+	if ((first < 0) || (first >= device->num_blocks))

+		first = 0;

+	

+	if ((last >= device->num_blocks) || (last == -1))

+		last = device->num_blocks - 1;

+	

+	for (i = first; i < last; i++)

+	{

+		nand_read_page(device, page, NULL, 0, oob, 6);

+		

+		if (((device->device->options & NAND_BUSWIDTH_16) && ((oob[0] & oob[1]) != 0xff))

+			|| (((device->page_size == 512) && (oob[5] != 0xff)) ||

+				((device->page_size == 2048) && (oob[0] != 0xff))))

+		{

+			WARNING("invalid block: %i", i);

+			device->blocks[i].is_bad = 1;

+		}

+		else

+		{

+			device->blocks[i].is_bad = 0;

+		}

+		

+		page += (device->erase_size / device->page_size);

+	}

+	

+	return ERROR_OK;

+}

+

+int nand_read_status(struct nand_device_s *device, u8 *status)

+{

+	if (!device->device)

+		return ERROR_NAND_DEVICE_NOT_PROBED;

+		

+	/* Send read status command */

+	device->controller->command(device, NAND_CMD_STATUS);

+	

+	usleep(1000);

+	

+	/* read status */

+	if (device->device->options & NAND_BUSWIDTH_16)

+	{

+		u16 data;

+		device->controller->read_data(device, &data);

+		*status = data & 0xff;

+	}

+	else

+	{

+		device->controller->read_data(device, status);

+	}

+			

+	return ERROR_OK;

+}

+

+int nand_probe(struct nand_device_s *device)

+{

+	u8 manufacturer_id, device_id;

+	u8 id_buff[5];

+	int retval;

+	int i;

+

+	/* clear device data */

+	device->device = NULL;

+	device->manufacturer = NULL;

+	

+	/* clear device parameters */

+	device->bus_width = 0;

+	device->address_cycles = 0;

+	device->page_size = 0;

+	device->erase_size = 0;

+	

+	/* initialize controller (device parameters are zero, use controller default) */

+	if ((retval = device->controller->init(device) != ERROR_OK))

+	{

+		switch (retval)

+		{

+			case ERROR_NAND_OPERATION_FAILED:

+				DEBUG("controller initialization failed");

+				return ERROR_NAND_OPERATION_FAILED;

+			case ERROR_NAND_OPERATION_NOT_SUPPORTED:

+				ERROR("BUG: controller reported that it doesn't support default parameters");

+				return ERROR_NAND_OPERATION_FAILED;

+			default:

+				ERROR("BUG: unknown controller initialization failure");

+				return ERROR_NAND_OPERATION_FAILED;

+		}

+	}

+	

+	device->controller->command(device, NAND_CMD_RESET);

+	device->controller->reset(device);

+

+	device->controller->command(device, NAND_CMD_READID);

+	device->controller->address(device, 0x0);

+	

+	if (device->bus_width == 8)

+	{

+		device->controller->read_data(device, &manufacturer_id);

+		device->controller->read_data(device, &device_id);

+	}

+	else

+	{

+		u16 data_buf;

+		device->controller->read_data(device, &data_buf);

+		manufacturer_id = data_buf & 0xff;

+		device->controller->read_data(device, &data_buf);

+		device_id = data_buf & 0xff;

+	}

+		

+	for (i = 0; nand_flash_ids[i].name; i++)

+	{

+		if (nand_flash_ids[i].id == device_id)

+		{

+			device->device = &nand_flash_ids[i];

+			break;

+		}

+	}

+	

+	for (i = 0; nand_manuf_ids[i].name; i++)

+	{

+		if (nand_manuf_ids[i].id == manufacturer_id)

+		{

+			device->manufacturer = &nand_manuf_ids[i];

+			break;

+		}

+	}

+	

+	if (!device->manufacturer)

+	{

+		device->manufacturer = &nand_manuf_ids[0];

+		device->manufacturer->id = manufacturer_id;

+	}

+	

+	if (!device->device)

+	{

+		ERROR("unknown NAND flash device found, manufacturer id: 0x%2.2x device id: 0x%2.2x",

+			manufacturer_id, device_id);

+		return ERROR_NAND_OPERATION_FAILED;

+	}

+	

+	DEBUG("found %s (%s)", device->device->name, device->manufacturer->name);

+	

+	/* initialize device parameters */

+	

+	/* bus width */ 

+	if (device->device->options & NAND_BUSWIDTH_16)

+		device->bus_width = 16;

+	else

+		device->bus_width = 8;

+

+	/* Do we need extended device probe information? */

+	if (device->device->page_size == 0 ||

+	    device->device->erase_size == 0)

+	{

+		if (device->bus_width == 8)

+		{

+			device->controller->read_data(device, id_buff+3);

+			device->controller->read_data(device, id_buff+4);

+			device->controller->read_data(device, id_buff+5);

+		}

+		else

+		{

+			u16 data_buf;

+

+			device->controller->read_data(device, &data_buf);

+			id_buff[3] = data_buf;

+

+			device->controller->read_data(device, &data_buf);

+			id_buff[4] = data_buf;

+

+			device->controller->read_data(device, &data_buf);

+			id_buff[5] = data_buf >> 8;

+		}

+	}

+		

+	/* page size */

+	if (device->device->page_size == 0)

+	{

+		device->page_size = 1 << (10 + (id_buff[4] & 3));

+	}

+	else if (device->device->page_size == 256)

+	{

+		ERROR("NAND flashes with 256 byte pagesize are not supported");

+		return ERROR_NAND_OPERATION_FAILED;

+	}

+	else

+	{

+		device->page_size = device->device->page_size;

+	}

+	

+	/* number of address cycles */

+	if (device->page_size <= 512)

+	{

+		/* small page devices */

+		if (device->device->chip_size <= 32)

+			device->address_cycles = 3;

+		else if (device->device->chip_size <= 8*1024)

+			device->address_cycles = 4;

+		else

+		{

+			ERROR("BUG: small page NAND device with more than 8 GiB encountered");

+			device->address_cycles = 5;

+		}

+	}

+	else

+	{

+		/* large page devices */

+		if (device->device->chip_size <= 128)

+			device->address_cycles = 4;

+		else if (device->device->chip_size <= 32*1024)

+			device->address_cycles = 5;

+		else

+		{

+			ERROR("BUG: small page NAND device with more than 32 GiB encountered");

+			device->address_cycles = 6;

+		}

+	}

+	

+	/* erase size */

+	if (device->device->erase_size == 0)

+	{

+		switch ((id_buff[4] >> 4) & 3) {

+		case 0:

+			device->erase_size = 64 << 10;

+			break;

+		case 1:

+			device->erase_size = 128 << 10;

+			break;

+		case 2:

+			device->erase_size = 256 << 10;

+			break;

+		case 3:

+			device->erase_size =512 << 10;

+			break;

+		}

+	}

+	else

+	{

+		device->erase_size = device->device->erase_size;

+	}

+	

+	/* initialize controller, but leave parameters at the controllers default */

+	if ((retval = device->controller->init(device) != ERROR_OK))

+	{

+		switch (retval)

+		{

+			case ERROR_NAND_OPERATION_FAILED:

+				DEBUG("controller initialization failed");

+				return ERROR_NAND_OPERATION_FAILED;

+			case ERROR_NAND_OPERATION_NOT_SUPPORTED:

+				ERROR("controller doesn't support requested parameters (buswidth: %i, address cycles: %i, page size: %i)",

+					device->bus_width, device->address_cycles, device->page_size);

+				return ERROR_NAND_OPERATION_FAILED;

+			default:

+				ERROR("BUG: unknown controller initialization failure");

+				return ERROR_NAND_OPERATION_FAILED;

+		}

+	}

+	

+	device->num_blocks = (device->device->chip_size * 1024) / (device->erase_size / 1024);

+	device->blocks = malloc(sizeof(nand_block_t) * device->num_blocks);

+	

+	for (i = 0; i < device->num_blocks; i++)

+	{

+		device->blocks[i].size = device->erase_size;

+		device->blocks[i].offset = i * device->erase_size;

+		device->blocks[i].is_erased = -1;

+		device->blocks[i].is_bad = -1;

+	}

+	

+	return ERROR_OK;

+}

+

+int nand_erase(struct nand_device_s *device, int first_block, int last_block)

+{

+	int i;

+	u32 page;

+	u8 status;

+	int retval;

+	

+	if (!device->device)

+		return ERROR_NAND_DEVICE_NOT_PROBED;

+	

+	if ((first_block < 0) || (last_block > device->num_blocks))

+		return ERROR_INVALID_ARGUMENTS;

+	

+	/* make sure we know if a block is bad before erasing it */

+	for (i = first_block; i <= last_block; i++)

+	{

+		if (device->blocks[i].is_bad == -1)

+		{

+			nand_build_bbt(device, i, last_block);

+			break;

+		}

+	}

+	

+	for (i = first_block; i <= last_block; i++)

+	{

+		/* Send erase setup command */

+		device->controller->command(device, NAND_CMD_ERASE1);

+		

+		page = i * (device->erase_size / device->page_size);

+		

+		/* Send page address */

+		if (device->page_size <= 512)

+		{

+			/* row */

+			device->controller->address(device, page & 0xff);

+			device->controller->address(device, (page >> 8) & 0xff);

+			

+			/* 3rd cycle only on devices with more than 32 MiB */

+			if (device->address_cycles >= 4)

+				device->controller->address(device, (page >> 16) & 0xff);

+	

+			/* 4th cycle only on devices with more than 8 GiB */

+			if (device->address_cycles >= 5)

+				device->controller->address(device, (page >> 24) & 0xff);

+		}

+		else

+		{

+			/* row */

+			device->controller->address(device, page & 0xff);

+			device->controller->address(device, (page >> 8) & 0xff);

+	

+			/* 3rd cycle only on devices with more than 128 MiB */

+			if (device->address_cycles >= 5)

+				device->controller->address(device, (page >> 16) & 0xff);

+		}

+		

+		/* Send erase confirm command */

+		device->controller->command(device, NAND_CMD_ERASE2);

+		

+		if (!device->controller->nand_ready(device, 1000))

+		{

+			ERROR("timeout waiting for NAND flash block erase to complete");

+			return ERROR_NAND_OPERATION_TIMEOUT;

+		}

+		

+		if ((retval = nand_read_status(device, &status)) != ERROR_OK)

+		{

+			ERROR("couldn't read status");

+			return ERROR_NAND_OPERATION_FAILED;

+		}

+		

+		if (status & 0x1)

+		{

+			ERROR("erase operation didn't pass, status: 0x%2.2x", status);

+			return ERROR_NAND_OPERATION_FAILED;

+		}

+	}

+	

+	return ERROR_OK;

+}

+

+int nand_read_plain(struct nand_device_s *device, u32 address, u8 *data, u32 data_size)

+{

+	u8 *page;

+	

+	if (!device->device)

+		return ERROR_NAND_DEVICE_NOT_PROBED;

+		

+	if (address % device->page_size)

+	{

+		ERROR("reads need to be page aligned");

+		return ERROR_NAND_OPERATION_FAILED;

+	}

+	

+	page = malloc(device->page_size);

+	

+	while (data_size > 0 )

+	{

+		u32 thisrun_size = (data_size > device->page_size) ? device->page_size : data_size;

+		u32 page_address;

+		

+		

+		page_address = address / device->page_size;

+		

+		nand_read_page(device, page_address, page, device->page_size, NULL, 0);

+

+		memcpy(data, page, thisrun_size);

+		

+		address += thisrun_size;

+		data += thisrun_size;

+		data_size -= thisrun_size;

+	}

+	

+	free(page);

+	

+	return ERROR_OK;

+}

+

+int nand_write_plain(struct nand_device_s *device, u32 address, u8 *data, u32 data_size)

+{

+	u8 *page;

+	

+	if (!device->device)

+		return ERROR_NAND_DEVICE_NOT_PROBED;

+		

+	if (address % device->page_size)

+	{

+		ERROR("writes need to be page aligned");

+		return ERROR_NAND_OPERATION_FAILED;

+	}

+	

+	page = malloc(device->page_size);

+	

+	while (data_size > 0 )

+	{

+		u32 thisrun_size = (data_size > device->page_size) ? device->page_size : data_size;

+		u32 page_address;

+		

+		memset(page, 0xff, device->page_size);

+		memcpy(page, data, thisrun_size);

+		

+		page_address = address / device->page_size;

+		

+		nand_write_page(device, page_address, page, device->page_size, NULL, 0);

+		

+		address += thisrun_size;

+		data += thisrun_size;

+		data_size -= thisrun_size;

+	}

+	

+	free(page);

+	

+	return ERROR_OK;

+}

+

+int nand_write_page(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size)

+{

+	if (!device->device)

+		return ERROR_NAND_DEVICE_NOT_PROBED;

+		

+	if (device->use_raw || device->controller->write_page == NULL)

+		return nand_write_page_raw(device, page, data, data_size, oob, oob_size);

+	else

+		return device->controller->write_page(device, page, data, data_size, oob, oob_size);

+}

+

+int nand_read_page(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size)

+{

+	if (!device->device)

+		return ERROR_NAND_DEVICE_NOT_PROBED;

+		

+	if (device->use_raw || device->controller->read_page == NULL)

+		return nand_read_page_raw(device, page, data, data_size, oob, oob_size);

+	else

+		return device->controller->read_page(device, page, data, data_size, oob, oob_size);

+}

+

+int nand_read_page_raw(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size)

+{

+	int i;

+	

+	if (!device->device)

+		return ERROR_NAND_DEVICE_NOT_PROBED;

+

+	if (device->page_size <= 512)

+	{

+		/* small page device */

+		if (data)

+			device->controller->command(device, NAND_CMD_READ0);

+		else

+			device->controller->command(device, NAND_CMD_READOOB);

+		

+		/* column (always 0, we start at the beginning of a page/OOB area) */

+		device->controller->address(device, 0x0);

+		

+		/* row */

+		device->controller->address(device, page & 0xff);

+		device->controller->address(device, (page >> 8) & 0xff);

+		

+		/* 4th cycle only on devices with more than 32 MiB */

+		if (device->address_cycles >= 4)

+			device->controller->address(device, (page >> 16) & 0xff);

+

+		/* 5th cycle only on devices with more than 8 GiB */

+		if (device->address_cycles >= 5)

+			device->controller->address(device, (page >> 24) & 0xff);

+	}

+	else

+	{

+		/* large page device */

+		device->controller->command(device, NAND_CMD_READ0);

+		

+		/* column (0 when we start at the beginning of a page,

+		 * or 2048 for the beginning of OOB area)

+		 */

+		device->controller->address(device, 0x0);

+		device->controller->address(device, 0x8);

+		

+		/* row */

+		device->controller->address(device, page & 0xff);

+		device->controller->address(device, (page >> 8) & 0xff);

+

+		/* 5th cycle only on devices with more than 128 MiB */

+		if (device->address_cycles >= 5)

+			device->controller->address(device, (page >> 16) & 0xff);

+

+		/* large page devices need a start command */

+		device->controller->command(device, NAND_CMD_READSTART);

+	}

+	

+	if (!device->controller->nand_ready(device, 100))

+		return ERROR_NAND_OPERATION_TIMEOUT;

+	

+	if (data)

+	{

+		if (device->controller->read_block_data != NULL)

+			(device->controller->read_block_data)(device, data, data_size);

+		else

+		{

+			for (i = 0; i < data_size;)

+			{

+				if (device->device->options & NAND_BUSWIDTH_16)

+				{

+					device->controller->read_data(device, data);

+					data += 2;

+					i += 2;

+				}

+				else

+				{

+					device->controller->read_data(device, data);

+					data += 1;

+					i += 1;

+				}

+			}

+		}

+	}

+	

+	if (oob)

+	{

+		if (device->controller->read_block_data != NULL)

+			(device->controller->read_block_data)(device, oob, oob_size);

+		else

+		{

+			for (i = 0; i < oob_size;)

+			{

+				if (device->device->options & NAND_BUSWIDTH_16)

+				{

+					device->controller->read_data(device, oob);

+					oob += 2;

+					i += 2;

+				}

+				else

+				{

+					device->controller->read_data(device, oob);

+					oob += 1;

+					i += 1;

+				}

+			}

+		}

+	}

+	

+	return ERROR_OK;	

+}

+

+int nand_write_page_raw(struct nand_device_s *device, u32 page, u8 *data, u32 data_size, u8 *oob, u32 oob_size)

+{

+	int i;

+	int retval;

+	u8 status;

+	

+	if (!device->device)

+		return ERROR_NAND_DEVICE_NOT_PROBED;

+

+	device->controller->command(device, NAND_CMD_SEQIN);

+	

+	if (device->page_size <= 512)

+	{

+		/* column (always 0, we start at the beginning of a page/OOB area) */

+		device->controller->address(device, 0x0);

+		

+		/* row */

+		device->controller->address(device, page & 0xff);

+		device->controller->address(device, (page >> 8) & 0xff);

+		

+		/* 4th cycle only on devices with more than 32 MiB */

+		if (device->address_cycles >= 4)

+			device->controller->address(device, (page >> 16) & 0xff);

+

+		/* 5th cycle only on devices with more than 8 GiB */

+		if (device->address_cycles >= 5)

+			device->controller->address(device, (page >> 24) & 0xff);

+	}

+	else

+	{

+		/* column (0 when we start at the beginning of a page,

+		 * or 2048 for the beginning of OOB area)

+		 */

+		device->controller->address(device, 0x0);

+		device->controller->address(device, 0x8);

+		

+		/* row */

+		device->controller->address(device, page & 0xff);

+		device->controller->address(device, (page >> 8) & 0xff);

+

+		/* 5th cycle only on devices with more than 128 MiB */

+		if (device->address_cycles >= 5)

+			device->controller->address(device, (page >> 16) & 0xff);

+	}

+	

+	if (data)

+	{

+		if (device->controller->write_block_data != NULL)

+			(device->controller->write_block_data)(device, data, data_size);

+		else

+		{

+			for (i = 0; i < data_size;)

+			{

+				if (device->device->options & NAND_BUSWIDTH_16)

+				{

+					u16 data_buf = le_to_h_u16(data);

+					device->controller->write_data(device, data_buf);

+					data += 2;

+					i += 2;

+				}

+				else

+				{

+					device->controller->write_data(device, *data);

+					data += 1;

+					i += 1;

+				}

+			}

+		}

+	}

+	

+	if (oob)

+	{

+		if (device->controller->write_block_data != NULL)

+			(device->controller->write_block_data)(device, oob, oob_size);

+		else

+		{

+			for (i = 0; i < oob_size;)

+			{

+				if (device->device->options & NAND_BUSWIDTH_16)

+				{

+					u16 oob_buf = le_to_h_u16(data);

+					device->controller->write_data(device, oob_buf);

+					oob += 2;

+					i += 2;

+				}

+				else

+				{

+					device->controller->write_data(device, *oob);

+					oob += 1;

+					i += 1;

+				}

+			}

+		}

+	}

+	

+	device->controller->command(device, NAND_CMD_PAGEPROG);

+	

+	if (!device->controller->nand_ready(device, 100))

+		return ERROR_NAND_OPERATION_TIMEOUT;

+	

+	if ((retval = nand_read_status(device, &status)) != ERROR_OK)

+	{

+		ERROR("couldn't read status");

+		return ERROR_NAND_OPERATION_FAILED;

+	}

+		

+	if (status & NAND_STATUS_FAIL)

+	{

+		ERROR("write operation didn't pass, status: 0x%2.2x", status);

+		return ERROR_NAND_OPERATION_FAILED;

+	}

+	

+	return ERROR_OK;	

+}

+

+int handle_nand_list_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	nand_device_t *p;

+	int i = 0;

+	

+	if (!nand_devices)

+	{

+		command_print(cmd_ctx, "no NAND flash devices configured");

+		return ERROR_OK;

+	}

+	

+	for (p = nand_devices; p; p = p->next)

+	{

+		if (p->device)

+			command_print(cmd_ctx, "#%i: %s (%s) pagesize: %i, buswidth: %i, erasesize: %i",

+				i++, p->device->name, p->manufacturer->name, p->page_size, p->bus_width, p->erase_size);

+		else

+			command_print(cmd_ctx, "#%i: not probed");

+	}

+	

+	return ERROR_OK;

+}

+

+int handle_nand_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	nand_device_t *p;

+	int i = 0;

+	int j = 0;

+	int first = -1;

+	int last = -1;

+		

+	if ((argc < 1) || (argc > 3))

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+

+	}

+	

+	if (argc == 2)

+	{

+		first = last = strtoul(args[1], NULL, 0);

+	}

+	else if (argc == 3)

+	{

+		first = strtoul(args[1], NULL, 0);

+		last = strtoul(args[2], NULL, 0);

+	}

+		

+	p = get_nand_device_by_num(strtoul(args[0], NULL, 0));

+	if (p)

+	{

+		if (p->device)

+		{

+			if (first >= p->num_blocks)

+				first = p->num_blocks - 1;

+			

+			if (last >= p->num_blocks)

+				last = p->num_blocks - 1;

+			

+			command_print(cmd_ctx, "#%i: %s (%s) pagesize: %i, buswidth: %i, erasesize: %i",

+				i++, p->device->name, p->manufacturer->name, p->page_size, p->bus_width, p->erase_size);

+			

+			for (j = first; j <= last; j++)

+			{

+				char *erase_state, *bad_state;

+				

+				if (p->blocks[j].is_erased == 0)

+					erase_state = "not erased";

+				else if (p->blocks[j].is_erased == 1)

+					erase_state = "erased";

+				else

+					erase_state = "erase state unknown";

+				

+				if (p->blocks[j].is_bad == 0)

+					bad_state = "";

+				else if (p->blocks[j].is_bad == 1)

+					bad_state = " (marked bad)";

+				else

+					bad_state = " (block condition unknown)";

+

+				command_print(cmd_ctx, "\t#%i: 0x%8.8x (0x%xkB) %s%s",

+							j, p->blocks[j].offset, p->blocks[j].size / 1024,

+							erase_state, bad_state);

+			}

+		}

+		else

+		{

+			command_print(cmd_ctx, "#%i: not probed");

+		}

+	}

+	

+	return ERROR_OK;

+}

+

+int handle_nand_probe_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	nand_device_t *p;

+	int retval;

+		

+	if (argc != 1)

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+	}

+	

+	p = get_nand_device_by_num(strtoul(args[0], NULL, 0));

+	if (p)

+	{

+		if ((retval = nand_probe(p)) == ERROR_OK)

+		{

+			command_print(cmd_ctx, "NAND flash device '%s' found", p->device->name);

+		}

+		else if (retval == ERROR_NAND_OPERATION_FAILED)

+		{

+			command_print(cmd_ctx, "probing failed for NAND flash device");

+		}

+		else

+		{

+			command_print(cmd_ctx, "unknown error when probing NAND flash device");

+		}

+	}

+	else

+	{

+		command_print(cmd_ctx, "NAND flash device '#%s' is out of bounds", args[0]);

+	}

+	

+	return ERROR_OK;

+}

+

+int handle_nand_erase_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	nand_device_t *p;

+	int retval;

+		

+	if (argc != 3)

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+

+	}

+	

+	p = get_nand_device_by_num(strtoul(args[0], NULL, 0));

+	if (p)

+	{

+		int first = strtoul(args[1], NULL, 0);

+		int last = strtoul(args[2], NULL, 0);

+		

+		if ((retval = nand_erase(p, first, last)) == ERROR_OK)

+		{

+			command_print(cmd_ctx, "successfully erased blocks %i to %i on NAND flash device '%s'", first, last, p->device->name);

+		}

+		else if (retval == ERROR_NAND_OPERATION_FAILED)

+		{

+			command_print(cmd_ctx, "erase failed");

+		}

+		else

+		{

+			command_print(cmd_ctx, "unknown error when erasing NAND flash device");

+		}

+	}

+	else

+	{

+		command_print(cmd_ctx, "NAND flash device '#%s' is out of bounds", args[0]);

+	}

+	

+	return ERROR_OK;

+}

+

+int handle_nand_check_bad_blocks_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	nand_device_t *p;

+	int retval;

+	int first = -1;

+	int last = -1;

+		

+	if ((argc < 1) || (argc > 3) || (argc == 2))

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+

+	}

+	

+	if (argc == 3)

+	{

+		first = strtoul(args[1], NULL, 0);

+		last = strtoul(args[2], NULL, 0);

+	}

+	

+	p = get_nand_device_by_num(strtoul(args[0], NULL, 0));

+	if (p)

+	{

+		if ((retval = nand_build_bbt(p, first, last)) == ERROR_OK)

+		{

+			command_print(cmd_ctx, "checked NAND flash device for bad blocks, use \"nand info\" command to list blocks", p->device->name);

+		}

+		else if (retval == ERROR_NAND_OPERATION_FAILED)

+		{

+			command_print(cmd_ctx, "error when checking for bad blocks on NAND flash device");

+		}

+		else

+		{

+			command_print(cmd_ctx, "unknown error when checking for bad blocks on NAND flash device");

+		}

+	}

+	else

+	{

+		command_print(cmd_ctx, "NAND flash device '#%s' is out of bounds", args[0]);

+	}

+	

+	return ERROR_OK;

+}

+

+int handle_nand_copy_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	nand_device_t *p;

+		

+	if (argc != 4)

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+

+	}

+	

+	p = get_nand_device_by_num(strtoul(args[0], NULL, 0));

+	if (p)

+	{

+

+	}

+	else

+	{

+		command_print(cmd_ctx, "NAND flash device '#%s' is out of bounds", args[0]);

+	}

+	

+	return ERROR_OK;

+}

+

+int handle_nand_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	u32 offset;

+	u32 binary_size;

+	u32 buf_cnt;

+	enum oob_formats oob_format = NAND_OOB_NONE;

+	

+	fileio_t fileio;

+	

+	duration_t duration;

+	char *duration_text;

+	

+	nand_device_t *p;

+		

+	if (argc < 3)

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+

+	}

+	

+	p = get_nand_device_by_num(strtoul(args[0], NULL, 0));

+	if (p)

+	{

+		u8 *page = NULL;

+		u32 page_size = 0;

+		u8 *oob = NULL;

+		u32 oob_size = 0;

+			

+		duration_start_measure(&duration);

+		offset = strtoul(args[2], NULL, 0);

+		

+		if (argc > 3)

+		{

+			int i;

+			for (i = 3; i < argc; i++)

+			{

+				if (!strcmp(args[i], "oob_raw"))

+					oob_format |= NAND_OOB_RAW;

+				else if (!strcmp(args[i], "oob_only"))

+					oob_format |= NAND_OOB_RAW | NAND_OOB_ONLY;

+				else

+				{

+					command_print(cmd_ctx, "unknown option: %s", args[i]);

+				}

+			}

+		}

+		

+		if (fileio_open(&fileio, args[1], FILEIO_READ, FILEIO_BINARY) != ERROR_OK)

+		{

+			command_print(cmd_ctx, "file open error: %s", fileio.error_str);

+			return ERROR_OK;

+		}

+	

+		buf_cnt = binary_size = fileio.size;

+		

+		if (!(oob_format & NAND_OOB_ONLY))

+		{

+			page_size = p->page_size;

+			page = malloc(p->page_size);

+		}

+

+		if (oob_format & NAND_OOB_RAW)

+		{

+			if (p->page_size == 512)

+				oob_size = 16;

+			else if (p->page_size == 2048)

+				oob_size = 64;

+			oob = malloc(oob_size);

+		}

+		

+		if (offset % p->page_size)

+		{

+			command_print(cmd_ctx, "only page size aligned offsets and sizes are supported");

+			return ERROR_OK;

+		}

+		

+		while (buf_cnt > 0)

+		{

+			u32 size_read;

+			

+			if (page)

+			{

+				fileio_read(&fileio, page_size, page, &size_read);

+				buf_cnt -= size_read;

+				if (size_read < page_size)

+				{

+					memset(page + size_read, 0xff, page_size - size_read);

+				}

+			}

+				

+			if (oob)

+			{

+				fileio_read(&fileio, oob_size, oob, &size_read);

+				buf_cnt -= size_read;

+				if (size_read < oob_size)

+				{

+					memset(oob + size_read, 0xff, oob_size - size_read);

+				}

+			}

+			

+			if (nand_write_page(p, offset / p->page_size, page, page_size, oob, oob_size) != ERROR_OK)

+			{

+				command_print(cmd_ctx, "failed writing file %s to NAND flash %s at offset 0x%8.8x",

+					args[1], args[0], offset);

+				return ERROR_OK;

+			}

+			offset += page_size;

+		}

+

+		fileio_close(&fileio);

+		

+		duration_stop_measure(&duration, &duration_text);

+		command_print(cmd_ctx, "wrote file %s to NAND flash %s at offset 0x%8.8x in %s",

+			args[1], args[0], offset, duration_text);

+		free(duration_text);

+	}

+	else

+	{

+		command_print(cmd_ctx, "NAND flash device '#%s' is out of bounds", args[0]);

+	}

+	

+	return ERROR_OK;

+}

+

+int handle_nand_dump_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	nand_device_t *p;

+			

+	if (argc < 4)

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+	}

+	

+	p = get_nand_device_by_num(strtoul(args[0], NULL, 0));

+	if (p)

+	{

+		if (p->device)

+		{

+			fileio_t fileio;

+			duration_t duration;

+			char *duration_text;

+			int retval;

+			

+			u8 *page = NULL;

+			u32 page_size = 0;

+			u8 *oob = NULL;

+			u32 oob_size = 0;

+			u32 address = strtoul(args[2], NULL, 0);

+			u32 size = strtoul(args[3], NULL, 0);

+			u32 bytes_done = 0;

+			enum oob_formats oob_format = NAND_OOB_NONE;

+			

+			if (argc > 4)

+			{

+				int i;

+				for (i = 4; i < argc; i++)

+				{

+					if (!strcmp(args[i], "oob_raw"))

+						oob_format |= NAND_OOB_RAW;

+					else if (!strcmp(args[i], "oob_only"))

+						oob_format |= NAND_OOB_RAW | NAND_OOB_ONLY;

+					else

+						command_print(cmd_ctx, "unknown option: '%s'", args[i]); 

+				}

+			}

+			

+			if ((address % p->page_size) || (size % p->page_size))

+			{

+				command_print(cmd_ctx, "only page size aligned addresses and sizes are supported");

+				return ERROR_OK;

+			}

+		

+			if (!(oob_format & NAND_OOB_ONLY))

+			{

+				page_size = p->page_size;

+				page = malloc(p->page_size);

+			}

+

+			if (oob_format & NAND_OOB_RAW)

+			{

+				if (p->page_size == 512)

+					oob_size = 16;

+				else if (p->page_size == 2048)

+					oob_size = 64;

+				oob = malloc(oob_size);

+			}

+			

+			if (fileio_open(&fileio, args[1], FILEIO_WRITE, FILEIO_BINARY) != ERROR_OK)

+			{

+				command_print(cmd_ctx, "dump_image error: %s", fileio.error_str);

+				return ERROR_OK;

+			}

+	

+			duration_start_measure(&duration);

+			

+			while (size > 0)

+			{

+				u32 size_written;

+				if ((retval = nand_read_page(p, address / p->page_size, page, page_size, oob, oob_size)) != ERROR_OK)

+				{

+					command_print(cmd_ctx, "reading NAND flash page failed");

+					return ERROR_OK;

+				}

+				

+				if (page)

+				{

+					fileio_write(&fileio, page_size, page, &size_written);

+					bytes_done += page_size;

+				}

+					

+				if (oob)

+				{

+					fileio_write(&fileio, oob_size, oob, &size_written);

+					bytes_done += oob_size;

+				}

+					

+				size -= p->page_size;

+				address += p->page_size;

+			}

+			

+			if (page)

+				free(page);

+				

+			if (oob)

+				free(oob);

+			

+			fileio_close(&fileio);

+

+			duration_stop_measure(&duration, &duration_text);

+			command_print(cmd_ctx, "dumped %"PRIi64" byte in %s", fileio.size, duration_text);

+			free(duration_text);

+		}

+		else

+		{

+			command_print(cmd_ctx, "#%i: not probed");

+		}

+	}

+	else

+	{

+		command_print(cmd_ctx, "NAND flash device '#%s' is out of bounds", args[0]);

+	}

+	

+	return ERROR_OK;

+}

+

+int handle_nand_raw_access_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	nand_device_t *p;

+		

+	if ((argc < 1) || (argc > 2))

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+	}

+	

+	p = get_nand_device_by_num(strtoul(args[0], NULL, 0));

+	if (p)

+	{

+		if (p->device)

+		{

+			if (argc == 2)

+			{

+				if (strcmp("enable", args[1]) == 0)

+				{

+					p->use_raw = 1;

+				}

+				else if (strcmp("disable", args[1]) == 0)

+				{

+					p->use_raw = 0;

+				}

+				else

+				{

+					return ERROR_COMMAND_SYNTAX_ERROR;

+				}

+			}

+	

+			command_print(cmd_ctx, "raw access is %s", (p->use_raw) ? "enabled" : "disabled");

+		}

+		else

+		{

+			command_print(cmd_ctx, "#%i: not probed");

+		}

+	}

+	else

+	{

+		command_print(cmd_ctx, "NAND flash device '#%s' is out of bounds", args[0]);

+	}

+	

+	return ERROR_OK;

+}

+

diff --git a/src/flash/stellaris.c b/src/flash/stellaris.c
index b944c34e..196730b2 100644
--- a/src/flash/stellaris.c
+++ b/src/flash/stellaris.c
@@ -1,945 +1,935 @@
-/***************************************************************************
- *   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.             *
- ***************************************************************************/
-
-/***************************************************************************
-* STELLARIS is tested on LM3S811
-* 
-*
-*
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include "replacements.h"
-
-#include "stellaris.h"
-#include "cortex_m3.h"
-
-#include "flash.h"
-#include "target.h"
-#include "log.h"
-#include "binarybuffer.h"
-#include "types.h"
-
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-
-#define DID0_VER(did0) ((did0>>28)&0x07)
-int stellaris_register_commands(struct command_context_s *cmd_ctx);
-int stellaris_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);
-int stellaris_erase(struct flash_bank_s *bank, int first, int last);
-int stellaris_protect(struct flash_bank_s *bank, int set, int first, int last);
-int stellaris_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);
-int stellaris_auto_probe(struct flash_bank_s *bank);
-int stellaris_probe(struct flash_bank_s *bank);
-int stellaris_erase_check(struct flash_bank_s *bank);
-int stellaris_protect_check(struct flash_bank_s *bank);
-int stellaris_info(struct flash_bank_s *bank, char *buf, int buf_size);
-
-int stellaris_read_part_info(struct flash_bank_s *bank);
-u32 stellaris_get_flash_status(flash_bank_t *bank);
-void stellaris_set_flash_mode(flash_bank_t *bank,int mode);
-u32 stellaris_wait_status_busy(flash_bank_t *bank, u32 waitbits, int timeout);
-
-int stellaris_read_part_info(struct flash_bank_s *bank);
-
-flash_driver_t stellaris_flash =
-{
-	.name = "stellaris",
-	.register_commands = stellaris_register_commands,
-	.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 = stellaris_erase_check,
-	.protect_check = stellaris_protect_check,
-	.info = stellaris_info
-};
-
-
-struct {
-	u32 partno;
-    char *partname;
-}	StellarisParts[] =
-{
-	{0x01,"LM3S101"},
-	{0x02,"LM3S102"},
-	{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"},
-	{0x34,"LM3S815"},
-	{0x36,"LM3S817"},
-	{0x37,"LM3S818"},
-	{0x35,"LM3S828"},
-	{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"},
-	{0x82,"LM3S6422"},
-	{0x83,"LM3S6633"},
-	{0x84,"LM3S2139"},
-	{0x85,"LM3S2637"},
-	{0x86,"LM3S8738"},
-	{0x88,"LM3S8938"},
-	{0x89,"LM3S6938"},
-	{0x8B,"LM3S6637"},
-	{0x8C,"LM3S8933"},
-	{0x8D,"LM3S8733"},
-	{0x8E,"LM3S8538"},
-	{0x8F,"LM3S2948"},
-	{0xA1,"LM3S6100"},
-	{0xA2,"LM3S2410"},
-	{0xA3,"LM3S6730"},
-	{0xA4,"LM3S2730"},
-	{0xA5,"LM3S6420"},
-	{0xA6,"LM3S8962"},
-	{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"}
-};
-
-char * StellarisClassname[2] =
-{
-	"Sandstorm",
-	"Fury"
-};
-
-/***************************************************************************
-*	openocd command interface                                              *
-***************************************************************************/
-
-/* flash_bank stellaris <base> <size> 0 0 <target#>
- */
-int stellaris_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)
-{
-	stellaris_flash_bank_t *stellaris_info;
-	
-	if (argc < 6)
-	{
-		WARNING("incomplete flash_bank stellaris configuration");
-		return ERROR_FLASH_BANK_INVALID;
-	}
-	
-	stellaris_info = calloc(sizeof(stellaris_flash_bank_t),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 Use an optional main oscillator clock rate in kHz from arg[6] */ 
-	return ERROR_OK;
-}
-
-int stellaris_register_commands(struct command_context_s *cmd_ctx)
-{
-/*
-	command_t *stellaris_cmd = register_command(cmd_ctx, NULL, "stellaris", NULL, COMMAND_ANY, NULL);
-	register_command(cmd_ctx, stellaris_cmd, "gpnvm", stellaris_handle_gpnvm_command, COMMAND_EXEC,
-			"stellaris gpnvm <num> <bit> set|clear, set or clear stellaris gpnvm bit");
-*/
-	return ERROR_OK;
-}
-
-int stellaris_info(struct flash_bank_s *bank, char *buf, int buf_size)
-{
-	int printed, device_class;
-	stellaris_flash_bank_t *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, "\nLMI Stellaris information: Chip is class %i(%s) %s v%c.%i\n",
-         device_class, StellarisClassname[device_class], stellaris_info->target_name,
-         'A' + (stellaris_info->did0>>8)&0xFF, (stellaris_info->did0)&0xFF);
-	buf += printed;
-	buf_size -= printed;
-
-	printed = snprintf(buf, buf_size, "did1: 0x%8.8x, arch: 0x%4.4x, eproc: %s, ramsize:%ik,  flashsize: %ik\n", 
-	 stellaris_info->did1, stellaris_info->did1, "ARMV7M", (1+(stellaris_info->dc0>>16)&0xFFFF)/4, (1+stellaris_info->dc0&0xFFFF)*2);
-	buf += printed;
-	buf_size -= printed;
-
-	printed = snprintf(buf, buf_size, "master clock(estimated): %ikHz,  rcc is 0x%x \n", stellaris_info->mck_freq / 1000, stellaris_info->rcc);
-	buf += printed;
-	buf_size -= printed;
-
-	if (stellaris_info->num_lockbits>0) {		
-		printed = snprintf(buf, buf_size, "pagesize: %i, lockbits: %i 0x%4.4x, pages in lock region: %i \n", stellaris_info->pagesize, stellaris_info->num_lockbits, stellaris_info->lockbits,stellaris_info->num_pages/stellaris_info->num_lockbits);
-		buf += printed;
-		buf_size -= printed;
-	}
-	return ERROR_OK;
-}
-
-/***************************************************************************
-*	chip identification and status                                         *
-***************************************************************************/
-
-u32 stellaris_get_flash_status(flash_bank_t *bank)
-{
-	target_t *target = bank->target;
-	u32 fmc;
-	
-	target_read_u32(target, FLASH_CONTROL_BASE|FLASH_FMC, &fmc);
-	
-	return fmc;
-}
-
-/** Read clock configuration and set stellaris_info->usec_clocks*/
- 
-void stellaris_read_clock_info(flash_bank_t *bank)
-{
-	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;
-	target_t *target = bank->target;
-	u32 rcc, pllcfg, sysdiv, usesysdiv, bypass, oscsrc;
-	unsigned long mainfreq;
-
-	target_read_u32(target, SCB_BASE|RCC, &rcc);
-	DEBUG("Stellaris RCC %x",rcc);
-	target_read_u32(target, SCB_BASE|PLLCFG, &pllcfg);
-	DEBUG("Stellaris PLLCFG %x",pllcfg);
-	stellaris_info->rcc = rcc;
-	
-	sysdiv = (rcc>>23)&0xF;
-	usesysdiv = (rcc>>22)&0x1;
-	bypass = (rcc>>11)&0x1;
-	oscsrc = (rcc>>4)&0x3;
-	/* xtal = (rcc>>6)&0xF; */
-	switch (oscsrc)
-	{
-		case 0:
-			mainfreq = 6000000;  /* Default xtal */
-			break;
-		case 1:
-			mainfreq = 22500000; /* Internal osc. 15 MHz +- 50% */
-			break;
-		case 2:
-			mainfreq = 5625000;  /* Internal osc. / 4 */
-			break;
-		case 3:
-			WARNING("Invalid oscsrc (3) in rcc register");
-			mainfreq = 6000000;
-			break;
-	}
-	
-	if (!bypass)
-		mainfreq = 200000000; /* PLL out frec */
-		
-	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 */
-void stellaris_set_flash_mode(flash_bank_t *bank,int mode)
-{
-	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;
-	target_t *target = bank->target;
-
-	u32 usecrl = (stellaris_info->mck_freq/1000000ul-1);
-	DEBUG("usecrl = %i",usecrl);	
-	target_write_u32(target, SCB_BASE|USECRL , usecrl);
-	
-}
-
-u32 stellaris_wait_status_busy(flash_bank_t *bank, u32 waitbits, int timeout)
-{
-	u32 status;
-	
-	/* Stellaris waits for cmdbit to clear */
-	while (((status = stellaris_get_flash_status(bank)) & waitbits) && (timeout-- > 0))
-	{
-		DEBUG("status: 0x%x", status);
-		usleep(1000);
-	}
-	
-	/* Flash errors are reflected in the FLASH_CRIS register */
-
-	return status;
-}
-
-
-/* Send one command to the flash controller */
-int stellaris_flash_command(struct flash_bank_s *bank,u8 cmd,u16 pagen) 
-{
-	u32 fmc;
-//	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;
-	target_t *target = bank->target;
-
-	fmc = FMC_WRKEY | cmd; 
-	target_write_u32(target, FLASH_CONTROL_BASE|FLASH_FMC, fmc);
-	DEBUG("Flash command: 0x%x", fmc);
-
-	if (stellaris_wait_status_busy(bank, cmd, 100)) 
-	{
-		return ERROR_FLASH_OPERATION_FAILED;
-	}		
-
-	return ERROR_OK;
-}
-
-/* Read device id register, main clock frequency register and fill in driver info structure */
-int stellaris_read_part_info(struct flash_bank_s *bank)
-{
-	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;
-	target_t *target = bank->target;
-    u32 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);
-	DEBUG("did0 0x%x, did1 0x%x, dc0 0x%x, dc1 0x%x",did0, did1, stellaris_info->dc0,stellaris_info->dc1);
-
-    ver = did0 >> 28;
-    if((ver != 0) && (ver != 1))
-	{
-        WARNING("Unknown did0 version, cannot identify target");
-		return ERROR_FLASH_OPERATION_FAILED;	
-	}
-
-    ver = did1 >> 28;
-    fam = (did1 >> 24) & 0xF;
-    if(((ver != 0) && (ver != 1)) || (fam != 0))
-	{
-        WARNING("Unknown did1 version/family, cannot positively identify target as a Stellaris");
-	}
-
-	if (did1 == 0)
-	{
-		WARNING("Cannot identify target as a Stellaris");
-		return ERROR_FLASH_OPERATION_FAILED;
-	}
-	
-	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);
-
-	// Read main and master clock freqency register 
-	stellaris_read_clock_info(bank);
-	
-	status = stellaris_get_flash_status(bank);
-	
-	return ERROR_OK;
-}
-
-/***************************************************************************
-*	flash operations                                         *
-***************************************************************************/
-
-int stellaris_erase_check(struct flash_bank_s *bank)
-{
-	/* 
-	
-	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;
-	target_t *target = bank->target;
-	int i;
-	
-	*/
-	
-	return ERROR_OK;
-}
-
-int stellaris_protect_check(struct flash_bank_s *bank)
-{
-	u32 status;
-	
-	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;
-
-	if (stellaris_info->did1 == 0)
-	{
-		stellaris_read_part_info(bank);
-	}
-
-	if (stellaris_info->did1 == 0)
-	{
-		WARNING("Cannot identify target as an AT91SAM");
-		return ERROR_FLASH_OPERATION_FAILED;
-	}
-		
-	status = stellaris_get_flash_status(bank);
-	stellaris_info->lockbits = status >> 16;
-	
-	return ERROR_OK;
-}
-
-int stellaris_erase(struct flash_bank_s *bank, int first, int last)
-{
-	int banknr;
-	u32 flash_fmc, flash_cris;
-	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;
-	target_t *target = bank->target;
-	
-	if (bank->target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	
-	if (stellaris_info->did1 == 0)
-	{
-		stellaris_read_part_info(bank);
-	}
-
-	if (stellaris_info->did1 == 0)
-	{
-        WARNING("Cannot identify target as Stellaris");
-		return ERROR_FLASH_OPERATION_FAILED;
-	}	
-	
-	if ((first < 0) || (last < first) || (last >= stellaris_info->num_pages))
-	{
-		return ERROR_FLASH_SECTOR_INVALID;
-	}
-
-	/* 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);
-
-	if ((first == 0) && (last == (stellaris_info->num_pages-1)))
-	{
-        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 */
-        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;
-	}
-
-	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))
-		{
-			WARNING("Error erasing flash page %i,  flash_cris 0x%x", banknr, flash_cris);
-			target_write_u32(target, FLASH_CRIS, 0);
-			return ERROR_FLASH_OPERATION_FAILED;
-		}
-	}
-
-	return ERROR_OK;
-}
-
-int stellaris_protect(struct flash_bank_s *bank, int set, int first, int last)
-{
-	u32 fmppe, flash_fmc, flash_cris;
-	int lockregion;
-	
-	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;
-	target_t *target = bank->target;
-	
-	if (bank->target->state != TARGET_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)
-	{
-		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);
-	
-	DEBUG("fmppe 0x%x",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 */
-	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))
-	{
-		WARNING("Error setting flash page protection,  flash_cris 0x%x", 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;
-}
-
-u8 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 */
-	0x00,0xBE,     		/* bkpt #0 */
-/* pFLASH_CTRL_BASE: */
-	0x00,0xD0,0x0F,0x40, 	/* .word	0x400FD000 */
-/* FLASHWRITECMD: */
-	0x01,0x00,0x42,0xA4 	/* .word	0xA4420001 */
-};
-
-int stellaris_write_block(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 wcount)
-{
-//	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;
-	target_t *target = bank->target;
-	u32 buffer_size = 8192;
-	working_area_t *source;
-	working_area_t *write_algorithm;
-	u32 address = bank->base + offset;
-	reg_param_t reg_params[8];
-	armv7m_algorithm_t armv7m_info;
-	int retval;
-	
-	DEBUG("(bank=%08X buffer=%08X offset=%08X wcount=%08X)",
-			(unsigned int)bank, (unsigned int)buffer, offset, wcount);
-
-	/* flash write code */
-	if (target_alloc_working_area(target, sizeof(stellaris_write_code), &write_algorithm) != ERROR_OK)
-		{
-			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)
-	{
-		DEBUG("called target_alloc_working_area(target=%08X buffer_size=%08X source=%08X)",
-				(unsigned int)target, buffer_size, (unsigned int)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);
-			
-			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;
-	armv7m_info.core_state = ARMV7M_STATE_THUMB;
-	
-	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_OUT);
-	init_reg_param(&reg_params[6], "r6", 32, PARAM_OUT);
-	init_reg_param(&reg_params[7], "r7", 32, PARAM_OUT);
-
-	while (wcount > 0)
-	{
-		u32 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);
-		WARNING("Algorithm flash write  %i words to 0x%x, %i remaining",thisrun_count,address, wcount);
-		DEBUG("Algorithm flash write  %i words to 0x%x, %i remaining",thisrun_count,address, wcount);
-		if ((retval = target->type->run_algorithm(target, 0, NULL, 3, reg_params, write_algorithm->address, write_algorithm->address + sizeof(stellaris_write_code)-10, 10000, &armv7m_info)) != ERROR_OK)
-		{
-			ERROR("error executing stellaris flash 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 ERROR_FLASH_OPERATION_FAILED;
-		}
-	
-		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]);
-	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]);
-	
-	return ERROR_OK;
-}
-
-int stellaris_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
-{
-	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;
-	target_t *target = bank->target;
-	u32 address = offset;
-	u32 flash_cris,flash_fmc;
-	u32 retval;
-	
-	DEBUG("(bank=%08X buffer=%08X offset=%08X count=%08X)",
-			(unsigned int)bank, (unsigned int)buffer, offset, count);
-
-	if (bank->target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	
-	if (stellaris_info->did1 == 0)
-	{
-		stellaris_read_part_info(bank);
-	}
-
-	if (stellaris_info->did1 == 0)
-	{
-		WARNING("Cannot identify target as a Stellaris processor");
-		return ERROR_FLASH_OPERATION_FAILED;
-	}
-	
-	if((offset & 3) || (count & 3))
-	{
-		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 (count > 0) 
-	{
-		/* try using a block write */
-		if ((retval = stellaris_write_block(bank, buffer, offset, count/4)) != ERROR_OK)
-		{
-			if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
-			{
-				/* if block write failed (no sufficient working area),
-				 * we use normal (slow) single dword accesses */ 
-				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);
-				
-				ERROR("flash writing failed with CRIS: 0x%x", flash_cris);
-				return ERROR_FLASH_OPERATION_FAILED;
-			}
-		}
-		else
-		{
-			buffer += count * 4;
-			address += count * 4;
-			count = 0;
-		}
-	}
-
-
-
-	while(count>0)
-	{
-		if (!(address&0xff)) DEBUG("0x%x",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);
-		//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;
-		count -= 4;
-	}
-	/* Check acess violations */
-	target_read_u32(target, FLASH_CRIS, &flash_cris);
-	if(flash_cris & (AMASK))
-	{
-		DEBUG("flash_cris 0x%x", flash_cris);
-		return ERROR_FLASH_OPERATION_FAILED;
-	}
-	return ERROR_OK;
-}
-
-
-int stellaris_probe(struct flash_bank_s *bank)
-{
-	/* we can't probe on an stellaris
-	 * if this is an stellaris, it has the configured flash
-	 */
-	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;
-	
-	stellaris_info->probed = 0;
-	
-	if (stellaris_info->did1 == 0)
-	{
-		stellaris_read_part_info(bank);
-	}
-
-	if (stellaris_info->did1 == 0)
-	{
-		WARNING("Cannot identify target as a LMI Stellaris");
-		return ERROR_FLASH_OPERATION_FAILED;
-	}
-	
-	stellaris_info->probed = 1;
-	
-	return ERROR_OK;
-}
-
-int stellaris_auto_probe(struct flash_bank_s *bank)
-{
-	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;
-	if (stellaris_info->probed)
-		return ERROR_OK;
-	return stellaris_probe(bank);
-}
+/***************************************************************************

+ *   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.             *

+ ***************************************************************************/

+

+/***************************************************************************

+* STELLARIS is tested on LM3S811

+* 

+*

+*

+ ***************************************************************************/

+#ifdef HAVE_CONFIG_H

+#include "config.h"

+#endif

+

+#include "replacements.h"

+

+#include "stellaris.h"

+#include "cortex_m3.h"

+

+#include "flash.h"

+#include "target.h"

+#include "log.h"

+#include "binarybuffer.h"

+#include "types.h"

+

+#include <stdlib.h>

+#include <string.h>

+#include <unistd.h>

+

+#define DID0_VER(did0) ((did0>>28)&0x07)

+int stellaris_register_commands(struct command_context_s *cmd_ctx);

+int stellaris_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);

+int stellaris_erase(struct flash_bank_s *bank, int first, int last);

+int stellaris_protect(struct flash_bank_s *bank, int set, int first, int last);

+int stellaris_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);

+int stellaris_auto_probe(struct flash_bank_s *bank);

+int stellaris_probe(struct flash_bank_s *bank);

+int stellaris_erase_check(struct flash_bank_s *bank);

+int stellaris_protect_check(struct flash_bank_s *bank);

+int stellaris_info(struct flash_bank_s *bank, char *buf, int buf_size);

+

+int stellaris_read_part_info(struct flash_bank_s *bank);

+u32 stellaris_get_flash_status(flash_bank_t *bank);

+void stellaris_set_flash_mode(flash_bank_t *bank,int mode);

+u32 stellaris_wait_status_busy(flash_bank_t *bank, u32 waitbits, int timeout);

+

+int stellaris_read_part_info(struct flash_bank_s *bank);

+

+flash_driver_t stellaris_flash =

+{

+	.name = "stellaris",

+	.register_commands = stellaris_register_commands,

+	.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 = stellaris_erase_check,

+	.protect_check = stellaris_protect_check,

+	.info = stellaris_info

+};

+

+

+struct {

+	u32 partno;

+    char *partname;

+}	StellarisParts[] =

+{

+	{0x01,"LM3S101"},

+	{0x02,"LM3S102"},

+	{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"},

+	{0x34,"LM3S815"},

+	{0x36,"LM3S817"},

+	{0x37,"LM3S818"},

+	{0x35,"LM3S828"},

+	{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"},

+	{0x82,"LM3S6422"},

+	{0x83,"LM3S6633"},

+	{0x84,"LM3S2139"},

+	{0x85,"LM3S2637"},

+	{0x86,"LM3S8738"},

+	{0x88,"LM3S8938"},

+	{0x89,"LM3S6938"},

+	{0x8B,"LM3S6637"},

+	{0x8C,"LM3S8933"},

+	{0x8D,"LM3S8733"},

+	{0x8E,"LM3S8538"},

+	{0x8F,"LM3S2948"},

+	{0xA1,"LM3S6100"},

+	{0xA2,"LM3S2410"},

+	{0xA3,"LM3S6730"},

+	{0xA4,"LM3S2730"},

+	{0xA5,"LM3S6420"},

+	{0xA6,"LM3S8962"},

+	{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"}

+};

+

+char * StellarisClassname[2] =

+{

+	"Sandstorm",

+	"Fury"

+};

+

+/***************************************************************************

+*	openocd command interface                                              *

+***************************************************************************/

+

+/* flash_bank stellaris <base> <size> 0 0 <target#>

+ */

+int stellaris_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)

+{

+	stellaris_flash_bank_t *stellaris_info;

+	

+	if (argc < 6)

+	{

+		WARNING("incomplete flash_bank stellaris configuration");

+		return ERROR_FLASH_BANK_INVALID;

+	}

+	

+	stellaris_info = calloc(sizeof(stellaris_flash_bank_t),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 Use an optional main oscillator clock rate in kHz from arg[6] */ 

+	return ERROR_OK;

+}

+

+int stellaris_register_commands(struct command_context_s *cmd_ctx)

+{

+/*

+	command_t *stellaris_cmd = register_command(cmd_ctx, NULL, "stellaris", NULL, COMMAND_ANY, NULL);

+	register_command(cmd_ctx, stellaris_cmd, "gpnvm", stellaris_handle_gpnvm_command, COMMAND_EXEC,

+			"stellaris gpnvm <num> <bit> set|clear, set or clear stellaris gpnvm bit");

+*/

+	return ERROR_OK;

+}

+

+int stellaris_info(struct flash_bank_s *bank, char *buf, int buf_size)

+{

+	int printed, device_class;

+	stellaris_flash_bank_t *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, "\nLMI Stellaris information: Chip is class %i(%s) %s v%c.%i\n",

+         device_class, StellarisClassname[device_class], stellaris_info->target_name,

+         'A' + (stellaris_info->did0>>8)&0xFF, (stellaris_info->did0)&0xFF);

+	buf += printed;

+	buf_size -= printed;

+

+	printed = snprintf(buf, buf_size, "did1: 0x%8.8x, arch: 0x%4.4x, eproc: %s, ramsize:%ik,  flashsize: %ik\n", 

+	 stellaris_info->did1, stellaris_info->did1, "ARMV7M", (1+(stellaris_info->dc0>>16)&0xFFFF)/4, (1+stellaris_info->dc0&0xFFFF)*2);

+	buf += printed;

+	buf_size -= printed;

+

+	printed = snprintf(buf, buf_size, "master clock(estimated): %ikHz,  rcc is 0x%x \n", stellaris_info->mck_freq / 1000, stellaris_info->rcc);

+	buf += printed;

+	buf_size -= printed;

+

+	if (stellaris_info->num_lockbits>0) {		

+		printed = snprintf(buf, buf_size, "pagesize: %i, lockbits: %i 0x%4.4x, pages in lock region: %i \n", stellaris_info->pagesize, stellaris_info->num_lockbits, stellaris_info->lockbits,stellaris_info->num_pages/stellaris_info->num_lockbits);

+		buf += printed;

+		buf_size -= printed;

+	}

+	return ERROR_OK;

+}

+

+/***************************************************************************

+*	chip identification and status                                         *

+***************************************************************************/

+

+u32 stellaris_get_flash_status(flash_bank_t *bank)

+{

+	target_t *target = bank->target;

+	u32 fmc;

+	

+	target_read_u32(target, FLASH_CONTROL_BASE|FLASH_FMC, &fmc);

+	

+	return fmc;

+}

+

+/** Read clock configuration and set stellaris_info->usec_clocks*/

+ 

+void stellaris_read_clock_info(flash_bank_t *bank)

+{

+	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;

+	target_t *target = bank->target;

+	u32 rcc, pllcfg, sysdiv, usesysdiv, bypass, oscsrc;

+	unsigned long mainfreq;

+

+	target_read_u32(target, SCB_BASE|RCC, &rcc);

+	DEBUG("Stellaris RCC %x",rcc);

+	target_read_u32(target, SCB_BASE|PLLCFG, &pllcfg);

+	DEBUG("Stellaris PLLCFG %x",pllcfg);

+	stellaris_info->rcc = rcc;

+	

+	sysdiv = (rcc>>23)&0xF;

+	usesysdiv = (rcc>>22)&0x1;

+	bypass = (rcc>>11)&0x1;

+	oscsrc = (rcc>>4)&0x3;

+	/* xtal = (rcc>>6)&0xF; */

+	switch (oscsrc)

+	{

+		case 0:

+			mainfreq = 6000000;  /* Default xtal */

+			break;

+		case 1:

+			mainfreq = 22500000; /* Internal osc. 15 MHz +- 50% */

+			break;

+		case 2:

+			mainfreq = 5625000;  /* Internal osc. / 4 */

+			break;

+		case 3:

+			WARNING("Invalid oscsrc (3) in rcc register");

+			mainfreq = 6000000;

+			break;

+	}

+	

+	if (!bypass)

+		mainfreq = 200000000; /* PLL out frec */

+		

+	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 */

+void stellaris_set_flash_mode(flash_bank_t *bank,int mode)

+{

+	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;

+	target_t *target = bank->target;

+

+	u32 usecrl = (stellaris_info->mck_freq/1000000ul-1);

+	DEBUG("usecrl = %i",usecrl);	

+	target_write_u32(target, SCB_BASE|USECRL , usecrl);

+	

+}

+

+u32 stellaris_wait_status_busy(flash_bank_t *bank, u32 waitbits, int timeout)

+{

+	u32 status;

+	

+	/* Stellaris waits for cmdbit to clear */

+	while (((status = stellaris_get_flash_status(bank)) & waitbits) && (timeout-- > 0))

+	{

+		DEBUG("status: 0x%x", status);

+		usleep(1000);

+	}

+	

+	/* Flash errors are reflected in the FLASH_CRIS register */

+

+	return status;

+}

+

+

+/* Send one command to the flash controller */

+int stellaris_flash_command(struct flash_bank_s *bank,u8 cmd,u16 pagen) 

+{

+	u32 fmc;

+//	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;

+	target_t *target = bank->target;

+

+	fmc = FMC_WRKEY | cmd; 

+	target_write_u32(target, FLASH_CONTROL_BASE|FLASH_FMC, fmc);

+	DEBUG("Flash command: 0x%x", fmc);

+

+	if (stellaris_wait_status_busy(bank, cmd, 100)) 

+	{

+		return ERROR_FLASH_OPERATION_FAILED;

+	}		

+

+	return ERROR_OK;

+}

+

+/* Read device id register, main clock frequency register and fill in driver info structure */

+int stellaris_read_part_info(struct flash_bank_s *bank)

+{

+	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;

+	target_t *target = bank->target;

+    u32 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);

+	DEBUG("did0 0x%x, did1 0x%x, dc0 0x%x, dc1 0x%x",did0, did1, stellaris_info->dc0,stellaris_info->dc1);

+

+    ver = did0 >> 28;

+    if((ver != 0) && (ver != 1))

+	{

+        WARNING("Unknown did0 version, cannot identify target");

+		return ERROR_FLASH_OPERATION_FAILED;	

+	}

+

+    ver = did1 >> 28;

+    fam = (did1 >> 24) & 0xF;

+    if(((ver != 0) && (ver != 1)) || (fam != 0))

+	{

+        WARNING("Unknown did1 version/family, cannot positively identify target as a Stellaris");

+	}

+

+	if (did1 == 0)

+	{

+		WARNING("Cannot identify target as a Stellaris");

+		return ERROR_FLASH_OPERATION_FAILED;

+	}

+	

+	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);

+

+	// Read main and master clock freqency register 

+	stellaris_read_clock_info(bank);

+	

+	status = stellaris_get_flash_status(bank);

+	

+	return ERROR_OK;

+}

+

+/***************************************************************************

+*	flash operations                                         *

+***************************************************************************/

+

+int stellaris_erase_check(struct flash_bank_s *bank)

+{

+	/* 

+	

+	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;

+	target_t *target = bank->target;

+	int i;

+	

+	*/

+	

+	return ERROR_OK;

+}

+

+int stellaris_protect_check(struct flash_bank_s *bank)

+{

+	u32 status;

+	

+	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;

+

+	if (stellaris_info->did1 == 0)

+	{

+		stellaris_read_part_info(bank);

+	}

+

+	if (stellaris_info->did1 == 0)

+	{

+		WARNING("Cannot identify target as an AT91SAM");

+		return ERROR_FLASH_OPERATION_FAILED;

+	}

+		

+	status = stellaris_get_flash_status(bank);

+	stellaris_info->lockbits = status >> 16;

+	

+	return ERROR_OK;

+}

+

+int stellaris_erase(struct flash_bank_s *bank, int first, int last)

+{

+	int banknr;

+	u32 flash_fmc, flash_cris;

+	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;

+	target_t *target = bank->target;

+	

+	if (stellaris_info->did1 == 0)

+	{

+		stellaris_read_part_info(bank);

+	}

+

+	if (stellaris_info->did1 == 0)

+	{

+        WARNING("Cannot identify target as Stellaris");

+		return ERROR_FLASH_OPERATION_FAILED;

+	}	

+	

+	if ((first < 0) || (last < first) || (last >= stellaris_info->num_pages))

+	{

+		return ERROR_FLASH_SECTOR_INVALID;

+	}

+

+	/* 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);

+

+	if ((first == 0) && (last == (stellaris_info->num_pages-1)))

+	{

+        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 */

+        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;

+	}

+

+	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))

+		{

+			WARNING("Error erasing flash page %i,  flash_cris 0x%x", banknr, flash_cris);

+			target_write_u32(target, FLASH_CRIS, 0);

+			return ERROR_FLASH_OPERATION_FAILED;

+		}

+	}

+

+	return ERROR_OK;

+}

+

+int stellaris_protect(struct flash_bank_s *bank, int set, int first, int last)

+{

+	u32 fmppe, flash_fmc, flash_cris;

+	int lockregion;

+	

+	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;

+	target_t *target = bank->target;

+	

+	if (bank->target->state != TARGET_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)

+	{

+		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);

+	

+	DEBUG("fmppe 0x%x",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 */

+	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))

+	{

+		WARNING("Error setting flash page protection,  flash_cris 0x%x", 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;

+}

+

+u8 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 */

+	0x00,0xBE,     		/* bkpt #0 */

+/* pFLASH_CTRL_BASE: */

+	0x00,0xD0,0x0F,0x40, 	/* .word	0x400FD000 */

+/* FLASHWRITECMD: */

+	0x01,0x00,0x42,0xA4 	/* .word	0xA4420001 */

+};

+

+int stellaris_write_block(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 wcount)

+{

+//	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;

+	target_t *target = bank->target;

+	u32 buffer_size = 8192;

+	working_area_t *source;

+	working_area_t *write_algorithm;

+	u32 address = bank->base + offset;

+	reg_param_t reg_params[8];

+	armv7m_algorithm_t armv7m_info;

+	int retval;

+	

+	DEBUG("(bank=%08X buffer=%08X offset=%08X wcount=%08X)",

+			(unsigned int)bank, (unsigned int)buffer, offset, wcount);

+

+	/* flash write code */

+	if (target_alloc_working_area(target, sizeof(stellaris_write_code), &write_algorithm) != ERROR_OK)

+		{

+			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)

+	{

+		DEBUG("called target_alloc_working_area(target=%08X buffer_size=%08X source=%08X)",

+				(unsigned int)target, buffer_size, (unsigned int)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);

+			

+			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;

+	armv7m_info.core_state = ARMV7M_STATE_THUMB;

+	

+	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_OUT);

+	init_reg_param(&reg_params[6], "r6", 32, PARAM_OUT);

+	init_reg_param(&reg_params[7], "r7", 32, PARAM_OUT);

+

+	while (wcount > 0)

+	{

+		u32 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);

+		WARNING("Algorithm flash write  %i words to 0x%x, %i remaining",thisrun_count,address, wcount);

+		DEBUG("Algorithm flash write  %i words to 0x%x, %i remaining",thisrun_count,address, wcount);

+		if ((retval = target->type->run_algorithm(target, 0, NULL, 3, reg_params, write_algorithm->address, write_algorithm->address + sizeof(stellaris_write_code)-10, 10000, &armv7m_info)) != ERROR_OK)

+		{

+			ERROR("error executing stellaris flash 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 ERROR_FLASH_OPERATION_FAILED;

+		}

+	

+		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]);

+	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]);

+	

+	return ERROR_OK;

+}

+

+int stellaris_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)

+{

+	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;

+	target_t *target = bank->target;

+	u32 address = offset;

+	u32 flash_cris,flash_fmc;

+	u32 retval;

+	

+	DEBUG("(bank=%08X buffer=%08X offset=%08X count=%08X)",

+			(unsigned int)bank, (unsigned int)buffer, offset, count);

+

+	if (stellaris_info->did1 == 0)

+	{

+		stellaris_read_part_info(bank);

+	}

+

+	if (stellaris_info->did1 == 0)

+	{

+		WARNING("Cannot identify target as a Stellaris processor");

+		return ERROR_FLASH_OPERATION_FAILED;

+	}

+	

+	if((offset & 3) || (count & 3))

+	{

+		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 (count > 0) 

+	{

+		/* try using a block write */

+		if ((retval = stellaris_write_block(bank, buffer, offset, count/4)) != ERROR_OK)

+		{

+			if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)

+			{

+				/* if block write failed (no sufficient working area),

+				 * we use normal (slow) single dword accesses */ 

+				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);

+				

+				ERROR("flash writing failed with CRIS: 0x%x", flash_cris);

+				return ERROR_FLASH_OPERATION_FAILED;

+			}

+		}

+		else

+		{

+			buffer += count * 4;

+			address += count * 4;

+			count = 0;

+		}

+	}

+

+

+

+	while(count>0)

+	{

+		if (!(address&0xff)) DEBUG("0x%x",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);

+		//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;

+		count -= 4;

+	}

+	/* Check acess violations */

+	target_read_u32(target, FLASH_CRIS, &flash_cris);

+	if(flash_cris & (AMASK))

+	{

+		DEBUG("flash_cris 0x%x", flash_cris);

+		return ERROR_FLASH_OPERATION_FAILED;

+	}

+	return ERROR_OK;

+}

+

+

+int stellaris_probe(struct flash_bank_s *bank)

+{

+	/* we can't probe on an stellaris

+	 * if this is an stellaris, it has the configured flash

+	 */

+	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;

+	

+	stellaris_info->probed = 0;

+	

+	if (stellaris_info->did1 == 0)

+	{

+		stellaris_read_part_info(bank);

+	}

+

+	if (stellaris_info->did1 == 0)

+	{

+		WARNING("Cannot identify target as a LMI Stellaris");

+		return ERROR_FLASH_OPERATION_FAILED;

+	}

+	

+	stellaris_info->probed = 1;

+	

+	return ERROR_OK;

+}

+

+int stellaris_auto_probe(struct flash_bank_s *bank)

+{

+	stellaris_flash_bank_t *stellaris_info = bank->driver_priv;

+	if (stellaris_info->probed)

+		return ERROR_OK;

+	return stellaris_probe(bank);

+}

diff --git a/src/flash/stm32x.c b/src/flash/stm32x.c
index 8363039f..0935c2c7 100644
--- a/src/flash/stm32x.c
+++ b/src/flash/stm32x.c
@@ -1,998 +1,988 @@
-/***************************************************************************
- *   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.             *
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include "replacements.h"
-
-#include "stm32x.h"
-#include "flash.h"
-#include "target.h"
-#include "log.h"
-#include "armv7m.h"
-#include "algorithm.h"
-#include "binarybuffer.h"
-
-#include <stdlib.h>
-#include <string.h>
-
-int stm32x_register_commands(struct command_context_s *cmd_ctx);
-int stm32x_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);
-int stm32x_erase(struct flash_bank_s *bank, int first, int last);
-int stm32x_protect(struct flash_bank_s *bank, int set, int first, int last);
-int stm32x_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);
-int stm32x_probe(struct flash_bank_s *bank);
-int stm32x_auto_probe(struct flash_bank_s *bank);
-int stm32x_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int stm32x_protect_check(struct flash_bank_s *bank);
-int stm32x_erase_check(struct flash_bank_s *bank);
-int stm32x_info(struct flash_bank_s *bank, char *buf, int buf_size);
-
-int stm32x_handle_lock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int stm32x_handle_unlock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int stm32x_handle_options_read_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int stm32x_handle_options_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int stm32x_handle_mass_erase_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-flash_driver_t stm32x_flash =
-{
-	.name = "stm32x",
-	.register_commands = stm32x_register_commands,
-	.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 = stm32x_erase_check,
-	.protect_check = stm32x_protect_check,
-	.info = stm32x_info
-};
-
-int stm32x_register_commands(struct command_context_s *cmd_ctx)
-{
-	command_t *stm32x_cmd = register_command(cmd_ctx, NULL, "stm32x", NULL, COMMAND_ANY, "stm32x flash specific commands");
-	
-	register_command(cmd_ctx, stm32x_cmd, "lock", stm32x_handle_lock_command, COMMAND_EXEC,
-					 "lock device");
-	register_command(cmd_ctx, stm32x_cmd, "unlock", stm32x_handle_unlock_command, COMMAND_EXEC,
-					 "unlock protected device");
-	register_command(cmd_ctx, stm32x_cmd, "mass_erase", stm32x_handle_mass_erase_command, COMMAND_EXEC,
-					 "mass erase device");
-	register_command(cmd_ctx, stm32x_cmd, "options_read", stm32x_handle_options_read_command, COMMAND_EXEC,
-					 "read device option bytes");
-	register_command(cmd_ctx, stm32x_cmd, "options_write", stm32x_handle_options_write_command, COMMAND_EXEC,
-					 "write device option bytes");
-	return ERROR_OK;
-}
-
-/* flash bank stm32x <base> <size> 0 0 <target#>
- */
-int stm32x_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)
-{
-	stm32x_flash_bank_t *stm32x_info;
-	
-	if (argc < 6)
-	{
-		WARNING("incomplete flash_bank stm32x configuration");
-		return ERROR_FLASH_BANK_INVALID;
-	}
-	
-	stm32x_info = malloc(sizeof(stm32x_flash_bank_t));
-	bank->driver_priv = stm32x_info;
-	
-	stm32x_info->write_algorithm = NULL;
-	stm32x_info->probed = 0;
-	
-	return ERROR_OK;
-}
-
-u32 stm32x_get_flash_status(flash_bank_t *bank)
-{
-	target_t *target = bank->target;
-	u32 status;
-	
-	target_read_u32(target, STM32_FLASH_SR, &status);
-	
-	return status;
-}
-
-u32 stm32x_wait_status_busy(flash_bank_t *bank, int timeout)
-{
-	u32 status;
-	
-	/* wait for busy to clear */
-	while (((status = stm32x_get_flash_status(bank)) & FLASH_BSY) && (timeout-- > 0))
-	{
-		DEBUG("status: 0x%x", status);
-		usleep(1000);
-	}
-	
-	return status;
-}
-
-int stm32x_read_options(struct flash_bank_s *bank)
-{
-	u32 optiondata;
-	stm32x_flash_bank_t *stm32x_info = NULL;
-	target_t *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 = (u16)0xFFF8|((optiondata >> 2) & 0x07);
-	stm32x_info->option_bytes.RDP = (optiondata & (1 << OPT_READOUT)) ? 0xFFFF : 0x5AA5;
-	
-	if (optiondata & (1 << OPT_READOUT))
-		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] = (u16)optiondata;
-	stm32x_info->option_bytes.protection[1] = (u16)(optiondata >> 8);
-	stm32x_info->option_bytes.protection[2] = (u16)(optiondata >> 16);
-	stm32x_info->option_bytes.protection[3] = (u16)(optiondata >> 24);
-		
-	return ERROR_OK;
-}
-
-int stm32x_erase_options(struct flash_bank_s *bank)
-{
-	stm32x_flash_bank_t *stm32x_info = NULL;
-	target_t *target = bank->target;
-	u32 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;
-}
-
-int stm32x_write_options(struct flash_bank_s *bank)
-{
-	stm32x_flash_bank_t *stm32x_info = NULL;
-	target_t *target = bank->target;
-	u32 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;
-}
-
-int stm32x_blank_check(struct flash_bank_s *bank, int first, int last)
-{
-	target_t *target = bank->target;
-	u8 *buffer;
-	int i;
-	int nBytes;
-	
-	if ((first < 0) || (last > bank->num_sectors))
-		return ERROR_FLASH_SECTOR_INVALID;
-
-	if (target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	
-	buffer = malloc(256);
-	
-	for (i = first; i <= last; i++)
-	{
-		bank->sectors[i].is_erased = 1;
-
-		target->type->read_memory(target, bank->base + bank->sectors[i].offset, 4, 256/4, buffer);
-		
-		for (nBytes = 0; nBytes < 256; nBytes++)
-		{
-			if (buffer[nBytes] != 0xFF)
-			{
-				bank->sectors[i].is_erased = 0;
-				break;
-			}
-		}	
-	}
-	
-	free(buffer);
-
-	return ERROR_OK;
-}
-
-int stm32x_protect_check(struct flash_bank_s *bank)
-{
-	target_t *target = bank->target;
-	
-	u32 protection;
-	int i, s;
-	int num_bits;
-
-	if (target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-
-	/* each bit refers to a 4bank protection */
-	target_read_u32(target, STM32_FLASH_WRPR, &protection);
-	
-	/* each protection bit is for 4 1K pages */
-	num_bits = (bank->num_sectors / 4);
-	
-	for (i = 0; i < num_bits; i++)
-	{
-		int set = 1;
-		
-		if( protection & (1 << i))
-			set = 0;
-		
-		for (s = 0; s < 4; s++)
-			bank->sectors[(i * 4) + s].is_protected = set;
-	}
-
-	return ERROR_OK;
-}
-
-int stm32x_erase(struct flash_bank_s *bank, int first, int last)
-{
-	target_t *target = bank->target;
-	
-	int i;
-	u32 status;
-	
-	if (target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	
-	/* 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;
-}
-
-int stm32x_protect(struct flash_bank_s *bank, int set, int first, int last)
-{
-	stm32x_flash_bank_t *stm32x_info = NULL;
-	target_t *target = bank->target;
-	u16 prot_reg[4] = {0xFFFF, 0xFFFF, 0xFFFF, 0xFFFF};
-	int i, reg, bit;
-	int status;
-	u32 protection;
-	
-	stm32x_info = bank->driver_priv;
-	
-	if (target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	
-	if ((first && (first % 4)) || ((last + 1) && (last + 1) % 4))
-	{
-		WARNING("sector start/end incorrect - stm32 has 4K sector protection");
-		return ERROR_FLASH_SECTOR_INVALID;
-	}
-	
-	/* each bit refers to a 4bank protection */
-	target_read_u32(target, STM32_FLASH_WRPR, &protection);
-	
-	prot_reg[0] = (u16)protection;
-	prot_reg[1] = (u16)(protection >> 8);
-	prot_reg[2] = (u16)(protection >> 16);
-	prot_reg[3] = (u16)(protection >> 24);
-	
-	for (i = first; i <= last; i++)
-	{
-		reg = (i / 4) / 8;
-		bit = (i / 4) - (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);
-}
-
-int stm32x_write_block(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
-{
-	stm32x_flash_bank_t *stm32x_info = bank->driver_priv;
-	target_t *target = bank->target;
-	u32 buffer_size = 8192;
-	working_area_t *source;
-	u32 address = bank->base + offset;
-	reg_param_t reg_params[4];
-	armv7m_algorithm_t armv7m_info;
-	int retval = ERROR_OK;
-	
-	u8 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)
-	{
-		WARNING("no working area available, can't do block memory writes");
-		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
-	};
-	
-	target_write_buffer(target, stm32x_info->write_algorithm->address, sizeof(stm32x_flash_write_code), stm32x_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 (stm32x_info->write_algorithm)
-				target_free_working_area(target, stm32x_info->write_algorithm);
-			
-			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;
-	armv7m_info.core_state = ARMV7M_STATE_THUMB;
-	
-	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)
-	{
-		u32 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->type->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)
-		{
-			ERROR("error executing str7x flash write algorithm");
-			break;
-		}
-		
-		if (buf_get_u32(reg_params[3].value, 0, 32) & 0x14)
-		{
-			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;
-}
-
-int stm32x_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
-{
-	target_t *target = bank->target;
-	u32 words_remaining = (count / 2);
-	u32 bytes_remaining = (count & 0x00000001);
-	u32 address = bank->base + offset;
-	u32 bytes_written = 0;
-	u8 status;
-	u32 retval;
-	
-	if (target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	
-	if (offset & 0x1)
-	{
-		WARNING("offset 0x%x 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 */ 
-				WARNING("couldn't use block writes, falling back to single memory accesses");
-			}
-			else if (retval == ERROR_FLASH_OPERATION_FAILED)
-			{
-				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)
-	{
-		target_write_u32(target, STM32_FLASH_CR, FLASH_PG);
-		target_write_u16(target, address, *(u16*)(buffer + bytes_written));
-		
-		status = stm32x_wait_status_busy(bank, 5);
-		
-		if( status & FLASH_WRPRTERR )
-			return ERROR_FLASH_OPERATION_FAILED;
-		if( status & FLASH_PGERR )
-			return ERROR_FLASH_OPERATION_FAILED;
-
-		bytes_written += 2;
-		words_remaining--;
-		address += 2;
-	}
-	
-	if (bytes_remaining)
-	{
-		u8 last_halfword[2] = {0xff, 0xff};
-		int i = 0;
-				
-		while(bytes_remaining > 0)
-		{
-			last_halfword[i++] = *(buffer + bytes_written); 
-			bytes_remaining--;
-			bytes_written++;
-		}
-		
-		target_write_u32(target, STM32_FLASH_CR, FLASH_PG);
-		target_write_u16(target, address, *(u16*)last_halfword);
-		
-		status = stm32x_wait_status_busy(bank, 5);
-		
-		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;
-}
-
-int stm32x_probe(struct flash_bank_s *bank)
-{
-	target_t *target = bank->target;
-	stm32x_flash_bank_t *stm32x_info = bank->driver_priv;
-	int i;
-	u16 num_sectors;
-	u32 device_id;
-	
-	stm32x_info->probed = 0;
-	
-	/* read stm32 device id register */
-	target_read_u32(target, 0xE0042000, &device_id);
-	INFO( "device id = 0x%08x", device_id );
-	
-	if (!(device_id & 0x410))
-    {
-		WARNING( "Cannot identify target as a STM32 family." );
-		return ERROR_FLASH_OPERATION_FAILED;
-    }
-    
-	/* get flash size from target */
-	target_read_u16(target, 0x1FFFF7E0, &num_sectors);
-	
-	/* check for early silicon rev A */
-	if ((device_id >> 16) == 0 )
-	{
-		/* number of sectors incorrect on revA */
-		WARNING( "STM32 Rev A Silicon detected, probe inaccurate - assuming 128k flash" );
-		num_sectors = 128;
-	}
-	
-	INFO( "flash size = %dkbytes", num_sectors );
-	
-	bank->base = 0x08000000;
-	bank->size = num_sectors * 1024;
-	bank->num_sectors = num_sectors;
-	bank->sectors = malloc(sizeof(flash_sector_t) * num_sectors);
-	
-	for (i = 0; i < num_sectors; i++)
-	{
-		bank->sectors[i].offset = i * 1024;
-		bank->sectors[i].size = 1024;
-		bank->sectors[i].is_erased = -1;
-		bank->sectors[i].is_protected = 1;
-	}
-	
-	stm32x_info->probed = 1;
-	
-	return ERROR_OK;
-}
-
-int stm32x_auto_probe(struct flash_bank_s *bank)
-{
-	stm32x_flash_bank_t *stm32x_info = bank->driver_priv;
-	if (stm32x_info->probed)
-		return ERROR_OK;
-	return stm32x_probe(bank);
-}
-
-int stm32x_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	return ERROR_OK;
-}
-
-int stm32x_erase_check(struct flash_bank_s *bank)
-{
-	return stm32x_blank_check(bank, 0, bank->num_sectors - 1);
-}
-
-int stm32x_info(struct flash_bank_s *bank, char *buf, int buf_size)
-{
-	snprintf(buf, buf_size, "stm32x flash driver info" );
-	return ERROR_OK;
-}
-
-int stm32x_handle_lock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	flash_bank_t *bank;
-	target_t *target = NULL;
-	stm32x_flash_bank_t *stm32x_info = NULL;
-	
-	if (argc < 1)
-	{
-		command_print(cmd_ctx, "stm32x lock <bank>");
-		return ERROR_OK;	
-	}
-	
-	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-	if (!bank)
-	{
-		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-		return ERROR_OK;
-	}
-	
-	stm32x_info = bank->driver_priv;
-	
-	target = bank->target;
-	
-	if (target->state != TARGET_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;
-}
-
-int stm32x_handle_unlock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	flash_bank_t *bank;
-	target_t *target = NULL;
-	stm32x_flash_bank_t *stm32x_info = NULL;
-	
-	if (argc < 1)
-	{
-		command_print(cmd_ctx, "stm32x unlock <bank>");
-		return ERROR_OK;	
-	}
-	
-	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-	if (!bank)
-	{
-		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-		return ERROR_OK;
-	}
-	
-	stm32x_info = bank->driver_priv;
-	
-	target = bank->target;
-	
-	if (target->state != TARGET_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;
-}
-
-int stm32x_handle_options_read_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	flash_bank_t *bank;
-	u32 optionbyte;
-	target_t *target = NULL;
-	stm32x_flash_bank_t *stm32x_info = NULL;
-	
-	if (argc < 1)
-	{
-		command_print(cmd_ctx, "stm32x options_read <bank>");
-		return ERROR_OK;	
-	}
-	
-	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-	if (!bank)
-	{
-		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-		return ERROR_OK;
-	}
-	
-	stm32x_info = bank->driver_priv;
-	
-	target = bank->target;
-	
-	if (target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	
-	target_read_u32(target, STM32_FLASH_OBR, &optionbyte);
-	command_print(cmd_ctx, "Option Byte: 0x%x", optionbyte);
-	
-	if (buf_get_u32((u8*)&optionbyte, OPT_ERROR, 1))
-		command_print(cmd_ctx, "Option Byte Complement Error");
-	
-	if (buf_get_u32((u8*)&optionbyte, OPT_READOUT, 1))
-		command_print(cmd_ctx, "Readout Protection On");
-	else
-		command_print(cmd_ctx, "Readout Protection Off");
-	
-	if (buf_get_u32((u8*)&optionbyte, OPT_RDWDGSW, 1))
-		command_print(cmd_ctx, "Software Watchdog");
-	else
-		command_print(cmd_ctx, "Hardware Watchdog");
-	
-	if (buf_get_u32((u8*)&optionbyte, OPT_RDRSTSTOP, 1))
-		command_print(cmd_ctx, "Stop: No reset generated");
-	else
-		command_print(cmd_ctx, "Stop: Reset generated");
-	
-	if (buf_get_u32((u8*)&optionbyte, OPT_RDRSTSTDBY, 1))
-		command_print(cmd_ctx, "Standby: No reset generated");
-	else
-		command_print(cmd_ctx, "Standby: Reset generated");
-	
-	return ERROR_OK;
-}
-
-int stm32x_handle_options_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	flash_bank_t *bank;
-	target_t *target = NULL;
-	stm32x_flash_bank_t *stm32x_info = NULL;
-	u16 optionbyte = 0xF8;
-	
-	if (argc < 4)
-	{
-		command_print(cmd_ctx, "stm32x options_write <bank> <SWWDG|HWWDG> <RSTSTNDBY|NORSTSTNDBY> <RSTSTOP|NORSTSTOP>");
-		return ERROR_OK;	
-	}
-	
-	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-	if (!bank)
-	{
-		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-		return ERROR_OK;
-	}
-	
-	stm32x_info = bank->driver_priv;
-	
-	target = bank->target;
-	
-	if (target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	
-	if (strcmp(args[1], "SWWDG") == 0)
-	{
-		optionbyte |= (1<<0);
-	}
-	else
-	{
-		optionbyte &= ~(1<<0);
-	}
-	
-	if (strcmp(args[2], "NORSTSTNDBY") == 0)
-	{
-		optionbyte |= (1<<1);
-	}
-	else
-	{
-		optionbyte &= ~(1<<1);
-	}
-	
-	if (strcmp(args[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;
-}
-
-int stm32x_handle_mass_erase_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	target_t *target = NULL;
-	stm32x_flash_bank_t *stm32x_info = NULL;
-	flash_bank_t *bank;
-	u32 status;
-	
-	if (argc < 1)
-	{
-		command_print(cmd_ctx, "stm32x mass_erase <bank>");
-		return ERROR_OK;	
-	}
-	
-	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-	if (!bank)
-	{
-		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-		return ERROR_OK;
-	}
-	
-	stm32x_info = bank->driver_priv;
-	
-	target = bank->target;
-	
-	if (target->state != TARGET_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 )
-	{
-		command_print(cmd_ctx, "stm32x device protected");
-		return ERROR_OK;
-	}
-	
-	if( status & FLASH_PGERR )
-	{
-		command_print(cmd_ctx, "stm32x device programming failed");
-		return ERROR_OK;
-	}
-	
-	command_print(cmd_ctx, "stm32x mass erase complete");
-	
-	return ERROR_OK;
-}
+/***************************************************************************

+ *   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.             *

+ ***************************************************************************/

+#ifdef HAVE_CONFIG_H

+#include "config.h"

+#endif

+

+#include "replacements.h"

+

+#include "stm32x.h"

+#include "flash.h"

+#include "target.h"

+#include "log.h"

+#include "armv7m.h"

+#include "algorithm.h"

+#include "binarybuffer.h"

+

+#include <stdlib.h>

+#include <string.h>

+

+int stm32x_register_commands(struct command_context_s *cmd_ctx);

+int stm32x_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);

+int stm32x_erase(struct flash_bank_s *bank, int first, int last);

+int stm32x_protect(struct flash_bank_s *bank, int set, int first, int last);

+int stm32x_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);

+int stm32x_probe(struct flash_bank_s *bank);

+int stm32x_auto_probe(struct flash_bank_s *bank);

+int stm32x_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int stm32x_protect_check(struct flash_bank_s *bank);

+int stm32x_erase_check(struct flash_bank_s *bank);

+int stm32x_info(struct flash_bank_s *bank, char *buf, int buf_size);

+

+int stm32x_handle_lock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int stm32x_handle_unlock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int stm32x_handle_options_read_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int stm32x_handle_options_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int stm32x_handle_mass_erase_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+

+flash_driver_t stm32x_flash =

+{

+	.name = "stm32x",

+	.register_commands = stm32x_register_commands,

+	.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 = stm32x_erase_check,

+	.protect_check = stm32x_protect_check,

+	.info = stm32x_info

+};

+

+int stm32x_register_commands(struct command_context_s *cmd_ctx)

+{

+	command_t *stm32x_cmd = register_command(cmd_ctx, NULL, "stm32x", NULL, COMMAND_ANY, "stm32x flash specific commands");

+	

+	register_command(cmd_ctx, stm32x_cmd, "lock", stm32x_handle_lock_command, COMMAND_EXEC,

+					 "lock device");

+	register_command(cmd_ctx, stm32x_cmd, "unlock", stm32x_handle_unlock_command, COMMAND_EXEC,

+					 "unlock protected device");

+	register_command(cmd_ctx, stm32x_cmd, "mass_erase", stm32x_handle_mass_erase_command, COMMAND_EXEC,

+					 "mass erase device");

+	register_command(cmd_ctx, stm32x_cmd, "options_read", stm32x_handle_options_read_command, COMMAND_EXEC,

+					 "read device option bytes");

+	register_command(cmd_ctx, stm32x_cmd, "options_write", stm32x_handle_options_write_command, COMMAND_EXEC,

+					 "write device option bytes");

+	return ERROR_OK;

+}

+

+/* flash bank stm32x <base> <size> 0 0 <target#>

+ */

+int stm32x_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)

+{

+	stm32x_flash_bank_t *stm32x_info;

+	

+	if (argc < 6)

+	{

+		WARNING("incomplete flash_bank stm32x configuration");

+		return ERROR_FLASH_BANK_INVALID;

+	}

+	

+	stm32x_info = malloc(sizeof(stm32x_flash_bank_t));

+	bank->driver_priv = stm32x_info;

+	

+	stm32x_info->write_algorithm = NULL;

+	stm32x_info->probed = 0;

+	

+	return ERROR_OK;

+}

+

+u32 stm32x_get_flash_status(flash_bank_t *bank)

+{

+	target_t *target = bank->target;

+	u32 status;

+	

+	target_read_u32(target, STM32_FLASH_SR, &status);

+	

+	return status;

+}

+

+u32 stm32x_wait_status_busy(flash_bank_t *bank, int timeout)

+{

+	u32 status;

+	

+	/* wait for busy to clear */

+	while (((status = stm32x_get_flash_status(bank)) & FLASH_BSY) && (timeout-- > 0))

+	{

+		DEBUG("status: 0x%x", status);

+		usleep(1000);

+	}

+	

+	return status;

+}

+

+int stm32x_read_options(struct flash_bank_s *bank)

+{

+	u32 optiondata;

+	stm32x_flash_bank_t *stm32x_info = NULL;

+	target_t *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 = (u16)0xFFF8|((optiondata >> 2) & 0x07);

+	stm32x_info->option_bytes.RDP = (optiondata & (1 << OPT_READOUT)) ? 0xFFFF : 0x5AA5;

+	

+	if (optiondata & (1 << OPT_READOUT))

+		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] = (u16)optiondata;

+	stm32x_info->option_bytes.protection[1] = (u16)(optiondata >> 8);

+	stm32x_info->option_bytes.protection[2] = (u16)(optiondata >> 16);

+	stm32x_info->option_bytes.protection[3] = (u16)(optiondata >> 24);

+		

+	return ERROR_OK;

+}

+

+int stm32x_erase_options(struct flash_bank_s *bank)

+{

+	stm32x_flash_bank_t *stm32x_info = NULL;

+	target_t *target = bank->target;

+	u32 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;

+}

+

+int stm32x_write_options(struct flash_bank_s *bank)

+{

+	stm32x_flash_bank_t *stm32x_info = NULL;

+	target_t *target = bank->target;

+	u32 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;

+}

+

+int stm32x_blank_check(struct flash_bank_s *bank, int first, int last)

+{

+	target_t *target = bank->target;

+	u8 *buffer;

+	int i;

+	int nBytes;

+	

+	if ((first < 0) || (last > bank->num_sectors))

+		return ERROR_FLASH_SECTOR_INVALID;

+

+	if (target->state != TARGET_HALTED)

+	{

+		return ERROR_TARGET_NOT_HALTED;

+	}

+	

+	buffer = malloc(256);

+	

+	for (i = first; i <= last; i++)

+	{

+		bank->sectors[i].is_erased = 1;

+

+		target->type->read_memory(target, bank->base + bank->sectors[i].offset, 4, 256/4, buffer);

+		

+		for (nBytes = 0; nBytes < 256; nBytes++)

+		{

+			if (buffer[nBytes] != 0xFF)

+			{

+				bank->sectors[i].is_erased = 0;

+				break;

+			}

+		}	

+	}

+	

+	free(buffer);

+

+	return ERROR_OK;

+}

+

+int stm32x_protect_check(struct flash_bank_s *bank)

+{

+	target_t *target = bank->target;

+	

+	u32 protection;

+	int i, s;

+	int num_bits;

+

+	if (target->state != TARGET_HALTED)

+	{

+		return ERROR_TARGET_NOT_HALTED;

+	}

+

+	/* each bit refers to a 4bank protection */

+	target_read_u32(target, STM32_FLASH_WRPR, &protection);

+	

+	/* each protection bit is for 4 1K pages */

+	num_bits = (bank->num_sectors / 4);

+	

+	for (i = 0; i < num_bits; i++)

+	{

+		int set = 1;

+		

+		if( protection & (1 << i))

+			set = 0;

+		

+		for (s = 0; s < 4; s++)

+			bank->sectors[(i * 4) + s].is_protected = set;

+	}

+

+	return ERROR_OK;

+}

+

+int stm32x_erase(struct flash_bank_s *bank, int first, int last)

+{

+	target_t *target = bank->target;

+	

+	int i;

+	u32 status;

+	

+	/* 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;

+}

+

+int stm32x_protect(struct flash_bank_s *bank, int set, int first, int last)

+{

+	stm32x_flash_bank_t *stm32x_info = NULL;

+	target_t *target = bank->target;

+	u16 prot_reg[4] = {0xFFFF, 0xFFFF, 0xFFFF, 0xFFFF};

+	int i, reg, bit;

+	int status;

+	u32 protection;

+	

+	stm32x_info = bank->driver_priv;

+	

+	if (target->state != TARGET_HALTED)

+	{

+		return ERROR_TARGET_NOT_HALTED;

+	}

+	

+	if ((first && (first % 4)) || ((last + 1) && (last + 1) % 4))

+	{

+		WARNING("sector start/end incorrect - stm32 has 4K sector protection");

+		return ERROR_FLASH_SECTOR_INVALID;

+	}

+	

+	/* each bit refers to a 4bank protection */

+	target_read_u32(target, STM32_FLASH_WRPR, &protection);

+	

+	prot_reg[0] = (u16)protection;

+	prot_reg[1] = (u16)(protection >> 8);

+	prot_reg[2] = (u16)(protection >> 16);

+	prot_reg[3] = (u16)(protection >> 24);

+	

+	for (i = first; i <= last; i++)

+	{

+		reg = (i / 4) / 8;

+		bit = (i / 4) - (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);

+}

+

+int stm32x_write_block(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)

+{

+	stm32x_flash_bank_t *stm32x_info = bank->driver_priv;

+	target_t *target = bank->target;

+	u32 buffer_size = 8192;

+	working_area_t *source;

+	u32 address = bank->base + offset;

+	reg_param_t reg_params[4];

+	armv7m_algorithm_t armv7m_info;

+	int retval = ERROR_OK;

+	

+	u8 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)

+	{

+		WARNING("no working area available, can't do block memory writes");

+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;

+	};

+	

+	target_write_buffer(target, stm32x_info->write_algorithm->address, sizeof(stm32x_flash_write_code), stm32x_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 (stm32x_info->write_algorithm)

+				target_free_working_area(target, stm32x_info->write_algorithm);

+			

+			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;

+	armv7m_info.core_state = ARMV7M_STATE_THUMB;

+	

+	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)

+	{

+		u32 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->type->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)

+		{

+			ERROR("error executing str7x flash write algorithm");

+			break;

+		}

+		

+		if (buf_get_u32(reg_params[3].value, 0, 32) & 0x14)

+		{

+			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;

+}

+

+int stm32x_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)

+{

+	target_t *target = bank->target;

+	u32 words_remaining = (count / 2);

+	u32 bytes_remaining = (count & 0x00000001);

+	u32 address = bank->base + offset;

+	u32 bytes_written = 0;

+	u8 status;

+	u32 retval;

+	

+	if (offset & 0x1)

+	{

+		WARNING("offset 0x%x 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 */ 

+				WARNING("couldn't use block writes, falling back to single memory accesses");

+			}

+			else if (retval == ERROR_FLASH_OPERATION_FAILED)

+			{

+				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)

+	{

+		target_write_u32(target, STM32_FLASH_CR, FLASH_PG);

+		target_write_u16(target, address, *(u16*)(buffer + bytes_written));

+		

+		status = stm32x_wait_status_busy(bank, 5);

+		

+		if( status & FLASH_WRPRTERR )

+			return ERROR_FLASH_OPERATION_FAILED;

+		if( status & FLASH_PGERR )

+			return ERROR_FLASH_OPERATION_FAILED;

+

+		bytes_written += 2;

+		words_remaining--;

+		address += 2;

+	}

+	

+	if (bytes_remaining)

+	{

+		u8 last_halfword[2] = {0xff, 0xff};

+		int i = 0;

+				

+		while(bytes_remaining > 0)

+		{

+			last_halfword[i++] = *(buffer + bytes_written); 

+			bytes_remaining--;

+			bytes_written++;

+		}

+		

+		target_write_u32(target, STM32_FLASH_CR, FLASH_PG);

+		target_write_u16(target, address, *(u16*)last_halfword);

+		

+		status = stm32x_wait_status_busy(bank, 5);

+		

+		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;

+}

+

+int stm32x_probe(struct flash_bank_s *bank)

+{

+	target_t *target = bank->target;

+	stm32x_flash_bank_t *stm32x_info = bank->driver_priv;

+	int i;

+	u16 num_sectors;

+	u32 device_id;

+	

+	stm32x_info->probed = 0;

+	

+	/* read stm32 device id register */

+	target_read_u32(target, 0xE0042000, &device_id);

+	INFO( "device id = 0x%08x", device_id );

+	

+	if (!(device_id & 0x410))

+    {

+		WARNING( "Cannot identify target as a STM32 family." );

+		return ERROR_FLASH_OPERATION_FAILED;

+    }

+    

+	/* get flash size from target */

+	target_read_u16(target, 0x1FFFF7E0, &num_sectors);

+	

+	/* check for early silicon rev A */

+	if ((device_id >> 16) == 0 )

+	{

+		/* number of sectors incorrect on revA */

+		WARNING( "STM32 Rev A Silicon detected, probe inaccurate - assuming 128k flash" );

+		num_sectors = 128;

+	}

+	

+	INFO( "flash size = %dkbytes", num_sectors );

+	

+	bank->base = 0x08000000;

+	bank->size = num_sectors * 1024;

+	bank->num_sectors = num_sectors;

+	bank->sectors = malloc(sizeof(flash_sector_t) * num_sectors);

+	

+	for (i = 0; i < num_sectors; i++)

+	{

+		bank->sectors[i].offset = i * 1024;

+		bank->sectors[i].size = 1024;

+		bank->sectors[i].is_erased = -1;

+		bank->sectors[i].is_protected = 1;

+	}

+	

+	stm32x_info->probed = 1;

+	

+	return ERROR_OK;

+}

+

+int stm32x_auto_probe(struct flash_bank_s *bank)

+{

+	stm32x_flash_bank_t *stm32x_info = bank->driver_priv;

+	if (stm32x_info->probed)

+		return ERROR_OK;

+	return stm32x_probe(bank);

+}

+

+int stm32x_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	return ERROR_OK;

+}

+

+int stm32x_erase_check(struct flash_bank_s *bank)

+{

+	return stm32x_blank_check(bank, 0, bank->num_sectors - 1);

+}

+

+int stm32x_info(struct flash_bank_s *bank, char *buf, int buf_size)

+{

+	snprintf(buf, buf_size, "stm32x flash driver info" );

+	return ERROR_OK;

+}

+

+int stm32x_handle_lock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	flash_bank_t *bank;

+	target_t *target = NULL;

+	stm32x_flash_bank_t *stm32x_info = NULL;

+	

+	if (argc < 1)

+	{

+		command_print(cmd_ctx, "stm32x lock <bank>");

+		return ERROR_OK;	

+	}

+	

+	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+	if (!bank)

+	{

+		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);

+		return ERROR_OK;

+	}

+	

+	stm32x_info = bank->driver_priv;

+	

+	target = bank->target;

+	

+	if (target->state != TARGET_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;

+}

+

+int stm32x_handle_unlock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	flash_bank_t *bank;

+	target_t *target = NULL;

+	stm32x_flash_bank_t *stm32x_info = NULL;

+	

+	if (argc < 1)

+	{

+		command_print(cmd_ctx, "stm32x unlock <bank>");

+		return ERROR_OK;	

+	}

+	

+	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+	if (!bank)

+	{

+		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);

+		return ERROR_OK;

+	}

+	

+	stm32x_info = bank->driver_priv;

+	

+	target = bank->target;

+	

+	if (target->state != TARGET_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;

+}

+

+int stm32x_handle_options_read_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	flash_bank_t *bank;

+	u32 optionbyte;

+	target_t *target = NULL;

+	stm32x_flash_bank_t *stm32x_info = NULL;

+	

+	if (argc < 1)

+	{

+		command_print(cmd_ctx, "stm32x options_read <bank>");

+		return ERROR_OK;	

+	}

+	

+	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+	if (!bank)

+	{

+		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);

+		return ERROR_OK;

+	}

+	

+	stm32x_info = bank->driver_priv;

+	

+	target = bank->target;

+	

+	if (target->state != TARGET_HALTED)

+	{

+		return ERROR_TARGET_NOT_HALTED;

+	}

+	

+	target_read_u32(target, STM32_FLASH_OBR, &optionbyte);

+	command_print(cmd_ctx, "Option Byte: 0x%x", optionbyte);

+	

+	if (buf_get_u32((u8*)&optionbyte, OPT_ERROR, 1))

+		command_print(cmd_ctx, "Option Byte Complement Error");

+	

+	if (buf_get_u32((u8*)&optionbyte, OPT_READOUT, 1))

+		command_print(cmd_ctx, "Readout Protection On");

+	else

+		command_print(cmd_ctx, "Readout Protection Off");

+	

+	if (buf_get_u32((u8*)&optionbyte, OPT_RDWDGSW, 1))

+		command_print(cmd_ctx, "Software Watchdog");

+	else

+		command_print(cmd_ctx, "Hardware Watchdog");

+	

+	if (buf_get_u32((u8*)&optionbyte, OPT_RDRSTSTOP, 1))

+		command_print(cmd_ctx, "Stop: No reset generated");

+	else

+		command_print(cmd_ctx, "Stop: Reset generated");

+	

+	if (buf_get_u32((u8*)&optionbyte, OPT_RDRSTSTDBY, 1))

+		command_print(cmd_ctx, "Standby: No reset generated");

+	else

+		command_print(cmd_ctx, "Standby: Reset generated");

+	

+	return ERROR_OK;

+}

+

+int stm32x_handle_options_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	flash_bank_t *bank;

+	target_t *target = NULL;

+	stm32x_flash_bank_t *stm32x_info = NULL;

+	u16 optionbyte = 0xF8;

+	

+	if (argc < 4)

+	{

+		command_print(cmd_ctx, "stm32x options_write <bank> <SWWDG|HWWDG> <RSTSTNDBY|NORSTSTNDBY> <RSTSTOP|NORSTSTOP>");

+		return ERROR_OK;	

+	}

+	

+	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+	if (!bank)

+	{

+		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);

+		return ERROR_OK;

+	}

+	

+	stm32x_info = bank->driver_priv;

+	

+	target = bank->target;

+	

+	if (target->state != TARGET_HALTED)

+	{

+		return ERROR_TARGET_NOT_HALTED;

+	}

+	

+	if (strcmp(args[1], "SWWDG") == 0)

+	{

+		optionbyte |= (1<<0);

+	}

+	else

+	{

+		optionbyte &= ~(1<<0);

+	}

+	

+	if (strcmp(args[2], "NORSTSTNDBY") == 0)

+	{

+		optionbyte |= (1<<1);

+	}

+	else

+	{

+		optionbyte &= ~(1<<1);

+	}

+	

+	if (strcmp(args[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;

+}

+

+int stm32x_handle_mass_erase_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	target_t *target = NULL;

+	stm32x_flash_bank_t *stm32x_info = NULL;

+	flash_bank_t *bank;

+	u32 status;

+	

+	if (argc < 1)

+	{

+		command_print(cmd_ctx, "stm32x mass_erase <bank>");

+		return ERROR_OK;	

+	}

+	

+	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+	if (!bank)

+	{

+		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);

+		return ERROR_OK;

+	}

+	

+	stm32x_info = bank->driver_priv;

+	

+	target = bank->target;

+	

+	if (target->state != TARGET_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 )

+	{

+		command_print(cmd_ctx, "stm32x device protected");

+		return ERROR_OK;

+	}

+	

+	if( status & FLASH_PGERR )

+	{

+		command_print(cmd_ctx, "stm32x device programming failed");

+		return ERROR_OK;

+	}

+	

+	command_print(cmd_ctx, "stm32x mass erase complete");

+	

+	return ERROR_OK;

+}

diff --git a/src/flash/str7x.c b/src/flash/str7x.c
index dc94ae0a..21122d0d 100644
--- a/src/flash/str7x.c
+++ b/src/flash/str7x.c
@@ -1,814 +1,804 @@
-/***************************************************************************
- *   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.             *
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include "replacements.h"
-
-#include "str7x.h"
-#include "flash.h"
-#include "target.h"
-#include "log.h"
-#include "armv4_5.h"
-#include "algorithm.h"
-#include "binarybuffer.h"
-
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-
-str7x_mem_layout_t mem_layout[] = {
-	{0x00000000, 0x02000, 0x01},
-	{0x00002000, 0x02000, 0x02},
-	{0x00004000, 0x02000, 0x04},
-	{0x00006000, 0x02000, 0x08},
-	{0x00008000, 0x08000, 0x10},
-	{0x00010000, 0x10000, 0x20},
-	{0x00020000, 0x10000, 0x40},
-	{0x00030000, 0x10000, 0x80},
-	{0x000C0000, 0x02000, 0x10000},
-	{0x000C2000, 0x02000, 0x20000},
-};
-
-int str7x_register_commands(struct command_context_s *cmd_ctx);
-int str7x_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);
-int str7x_erase(struct flash_bank_s *bank, int first, int last);
-int str7x_protect(struct flash_bank_s *bank, int set, int first, int last);
-int str7x_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);
-int str7x_probe(struct flash_bank_s *bank);
-int str7x_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int str7x_protect_check(struct flash_bank_s *bank);
-int str7x_erase_check(struct flash_bank_s *bank);
-int str7x_info(struct flash_bank_s *bank, char *buf, int buf_size);
-
-int str7x_handle_disable_jtag_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-flash_driver_t str7x_flash =
-{
-	.name = "str7x",
-	.register_commands = str7x_register_commands,
-	.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 = str7x_erase_check,
-	.protect_check = str7x_protect_check,
-	.info = str7x_info
-};
-
-int str7x_register_commands(struct command_context_s *cmd_ctx)
-{
-	command_t *str7x_cmd = register_command(cmd_ctx, NULL, "str7x", NULL, COMMAND_ANY, NULL);
-	
-	register_command(cmd_ctx, str7x_cmd, "disable_jtag", str7x_handle_disable_jtag_command, COMMAND_EXEC,
-					 "disable jtag access");
-					 
-	return ERROR_OK;
-}
-
-int str7x_get_flash_adr(struct flash_bank_s *bank, u32 reg)
-{
-	return (bank->base | reg);
-}
-
-int str7x_build_block_list(struct flash_bank_s *bank)
-{
-	str7x_flash_bank_t *str7x_info = bank->driver_priv;
-
-	int i;
-	int num_sectors = 0, b0_sectors = 0, b1_sectors = 0;
-		
-	switch (bank->size)
-	{
-		case 16 * 1024:
-			b0_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:
-			ERROR("BUG: unknown bank->size encountered");
-			exit(-1);
-	}
-	
-	if( str7x_info->bank1 == 1 )
-	{
-		b1_sectors += 2;
-	}
-	
-	num_sectors = b0_sectors + b1_sectors;
-	
-	bank->num_sectors = num_sectors;
-	bank->sectors = malloc(sizeof(flash_sector_t) * num_sectors);
-	str7x_info->sector_bits = malloc(sizeof(u32) * num_sectors);
-	str7x_info->sector_bank = malloc(sizeof(u32) * num_sectors);
-	
-	num_sectors = 0;
-	
-	for (i = 0; i < b0_sectors; i++)
-	{
-		bank->sectors[num_sectors].offset = mem_layout[i].sector_start;
-		bank->sectors[num_sectors].size = mem_layout[i].sector_size;
-		bank->sectors[num_sectors].is_erased = -1;
-		bank->sectors[num_sectors].is_protected = 1;
-		str7x_info->sector_bank[num_sectors] = 0;
-		str7x_info->sector_bits[num_sectors++] = mem_layout[i].sector_bit;
-	}
-	
-	if (b1_sectors)
-	{
-		for (i = 8; i < 10; i++)
-		{
-			bank->sectors[num_sectors].offset = mem_layout[i].sector_start;
-			bank->sectors[num_sectors].size = mem_layout[i].sector_size;
-			bank->sectors[num_sectors].is_erased = -1;
-			bank->sectors[num_sectors].is_protected = 1;
-			str7x_info->sector_bank[num_sectors] = 1;
-			str7x_info->sector_bits[num_sectors++] = mem_layout[i].sector_bit;
-		}
-	}
-	
-	return ERROR_OK;
-}
-
-/* flash bank str7x <base> <size> 0 0 <target#> <str71_variant>
- */
-int str7x_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)
-{
-	str7x_flash_bank_t *str7x_info;
-	
-	if (argc < 7)
-	{
-		WARNING("incomplete flash_bank str7x configuration");
-		return ERROR_FLASH_BANK_INVALID;
-	}
-	
-	str7x_info = malloc(sizeof(str7x_flash_bank_t));
-	bank->driver_priv = str7x_info;
-	
-	/* set default bits for str71x flash */
-	str7x_info->bank1 = 1;
-	str7x_info->busy_bits = (FLASH_LOCK|FLASH_BSYA1|FLASH_BSYA0);
-	str7x_info->disable_bit = (1<<1);
-	
-	if (strcmp(args[6], "STR71x") == 0)
-	{
-		if (bank->base != 0x40000000)
-		{
-			WARNING("overriding flash base address for STR71x device with 0x40000000");
-			bank->base = 0x40000000;
-		}
-	}
-	else if (strcmp(args[6], "STR73x") == 0)
-	{
-		str7x_info->bank1 = 0;
-		str7x_info->busy_bits = (FLASH_LOCK|FLASH_BSYA0);
-		
-		if (bank->base != 0x80000000)
-		{
-			WARNING("overriding flash base address for STR73x device with 0x80000000");
-			bank->base = 0x80000000;
-		}
-	}
-	else if (strcmp(args[6], "STR75x") == 0)
-	{
-		str7x_info->disable_bit = (1<<0);
-		
-		if (bank->base != 0x20000000)
-		{
-			WARNING("overriding flash base address for STR75x device with 0x20000000");
-			bank->base = 0x20000000;
-		}
-	}
-	else
-	{
-		ERROR("unknown STR7x variant: '%s'", args[6]);
-		free(str7x_info);
-		return ERROR_FLASH_BANK_INVALID;
-	}
-
-	str7x_build_block_list(bank);
-	
-	str7x_info->write_algorithm = NULL;
-	
-	return ERROR_OK;
-}
-
-u32 str7x_status(struct flash_bank_s *bank)
-{
-	target_t *target = bank->target;
-	u32 retval;
-
-	target_read_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), &retval);
-
-	return retval;
-}
-
-u32 str7x_result(struct flash_bank_s *bank)
-{
-	target_t *target = bank->target;
-	u32 retval;
-
-	target_read_u32(target, str7x_get_flash_adr(bank, FLASH_ER), &retval);
-	
-	return retval;
-}
-
-int str7x_blank_check(struct flash_bank_s *bank, int first, int last)
-{
-	target_t *target = bank->target;
-	u8 *buffer;
-	int i;
-	int nBytes;
-	
-	if ((first < 0) || (last > bank->num_sectors))
-		return ERROR_FLASH_SECTOR_INVALID;
-
-	if (bank->target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	
-	buffer = malloc(256);
-	
-	for (i = first; i <= last; i++)
-	{
-		bank->sectors[i].is_erased = 1;
-
-		target->type->read_memory(target, bank->base + bank->sectors[i].offset, 4, 256/4, buffer);
-		
-		for (nBytes = 0; nBytes < 256; nBytes++)
-		{
-			if (buffer[nBytes] != 0xFF)
-			{
-				bank->sectors[i].is_erased = 0;
-				break;
-			}
-		}	
-	}
-	
-	free(buffer);
-
-	return ERROR_OK;
-}
-
-int str7x_protect_check(struct flash_bank_s *bank)
-{
-	str7x_flash_bank_t *str7x_info = bank->driver_priv;
-	target_t *target = bank->target;
-	
-	int i;
-	u32 retval;
-
-	if (bank->target->state != TARGET_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;
-}
-
-int str7x_erase(struct flash_bank_s *bank, int first, int last)
-{
-	str7x_flash_bank_t *str7x_info = bank->driver_priv;
-	target_t *target = bank->target;
-	
-	int i;
-	u32 cmd;
-	u32 retval;
-	u32 b0_sectors = 0, b1_sectors = 0;
-	
-	if (bank->target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	
-	for (i = first; i <= last; i++)
-	{
-		if (str7x_info->sector_bank[i] == 0)
-			b0_sectors |= str7x_info->sector_bits[i];
-		else if (str7x_info->sector_bank[i] == 1)
-			b1_sectors |= str7x_info->sector_bits[i];
-		else
-			ERROR("BUG: str7x_info->sector_bank[i] neither 0 nor 1 (%i)", str7x_info->sector_bank[i]);
-	}
-	
-	if (b0_sectors)
-	{
-		DEBUG("b0_sectors: 0x%x", b0_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 = b0_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)){
-			usleep(1000);
-		}
-		
-		retval = str7x_result(bank);
-		
-		if (retval)
-		{
-			ERROR("error erasing flash bank, FLASH_ER: 0x%x", retval);
-			return ERROR_FLASH_OPERATION_FAILED;
-		}
-	}
-	
-	if (b1_sectors)
-	{
-		DEBUG("b1_sectors: 0x%x", b1_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 = b1_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)){
-			usleep(1000);
-		}
-		
-		retval = str7x_result(bank);
-		
-		if (retval)
-		{
-			ERROR("error erasing flash bank, FLASH_ER: 0x%x", retval);
-			return ERROR_FLASH_OPERATION_FAILED;
-		}
-	}
-	
-	for (i = first; i <= last; i++)
-		bank->sectors[i].is_erased = 1;
-
-	return ERROR_OK;
-}
-
-int str7x_protect(struct flash_bank_s *bank, int set, int first, int last)
-{
-	str7x_flash_bank_t *str7x_info = bank->driver_priv;
-	target_t *target = bank->target;
-	int i;
-	u32 cmd;
-	u32 retval;
-	u32 protect_blocks;
-	
-	if (bank->target->state != TARGET_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)){
-		usleep(1000);
-	}
-	
-	retval = str7x_result(bank);
-	
-	DEBUG("retval: 0x%8.8x", retval);
-	
-	if (retval & FLASH_ERER)
-		return ERROR_FLASH_SECTOR_NOT_ERASED;
-	else if (retval & FLASH_WPF)
-		return ERROR_FLASH_OPERATION_FAILED;
-
-	return ERROR_OK;
-}
-
-int str7x_write_block(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
-{
-	str7x_flash_bank_t *str7x_info = bank->driver_priv;
-	target_t *target = bank->target;
-	u32 buffer_size = 8192;
-	working_area_t *source;
-	u32 address = bank->base + offset;
-	reg_param_t reg_params[6];
-	armv4_5_algorithm_t armv4_5_info;
-	int retval = ERROR_OK;
-	
-	u32 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)
-	{
-		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, (u8*)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);
-			
-			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)
-	{
-		u32 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->type->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)
-		{
-			ERROR("error executing str7x flash write algorithm");
-			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;
-}
-
-int str7x_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
-{
-	target_t *target = bank->target;
-	str7x_flash_bank_t *str7x_info = bank->driver_priv;
-	u32 dwords_remaining = (count / 8);
-	u32 bytes_remaining = (count & 0x00000007);
-	u32 address = bank->base + offset;
-	u32 bytes_written = 0;
-	u32 cmd;
-	u32 retval;
-	u32 check_address = offset;
-	int i;
-	
-	if (bank->target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	
-	if (offset & 0x7)
-	{
-		WARNING("offset 0x%x breaks required 8-byte alignment", offset);
-		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
-	}
-	
-	for (i = 0; i < bank->num_sectors; i++)
-	{
-		u32 sec_start = bank->sectors[i].offset;
-		u32 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 */ 
-				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);
-				
-				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->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_DR0), 4, 1, buffer + bytes_written);
-		bytes_written += 4;
-		
-		// data word 2
-		target->type->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))
-		{
-			usleep(1000);
-		}
-		
-		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)
-	{
-		u8 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->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_DR0), 4, 1, last_dword);
-		bytes_written += 4;
-		
-		// data word 2
-		target->type->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))
-		{
-			usleep(1000);
-		}
-		
-		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;
-}
-
-int str7x_probe(struct flash_bank_s *bank)
-{
-	return ERROR_OK;
-}
-
-int str7x_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	return ERROR_OK;
-}
-
-int str7x_erase_check(struct flash_bank_s *bank)
-{
-	return str7x_blank_check(bank, 0, bank->num_sectors - 1);
-}
-
-int str7x_info(struct flash_bank_s *bank, char *buf, int buf_size)
-{
-	snprintf(buf, buf_size, "str7x flash driver info" );
-	return ERROR_OK;
-}
-
-int str7x_handle_disable_jtag_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	flash_bank_t *bank;
-	target_t *target = NULL;
-	str7x_flash_bank_t *str7x_info = NULL;
-	
-	u32 flash_cmd;
-	u32 retval;
-	u16 ProtectionLevel = 0;
-	u16 ProtectionRegs;
-	
-	if (argc < 1)
-	{
-		command_print(cmd_ctx, "str7x disable_jtag <bank>");
-		return ERROR_OK;	
-	}
-	
-	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-	if (!bank)
-	{
-		command_print(cmd_ctx, "str7x disable_jtag <bank> ok");
-		return ERROR_OK;
-	}
-	
-	str7x_info = bank->driver_priv;
-	
-	target = bank->target;
-	
-	if (target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	
-	/* first we get protection status */
-	target_read_u32(target, str7x_get_flash_adr(bank, FLASH_NVAPR0), &retval);
-
-	if (!(retval & str7x_info->disable_bit))
-	{
-		ProtectionLevel = 1;
-	}
-	
-	target_read_u32(target, str7x_get_flash_adr(bank, FLASH_NVAPR1), &retval);
-	ProtectionRegs = ~(retval >> 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;
-}
-
+/***************************************************************************

+ *   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.             *

+ ***************************************************************************/

+#ifdef HAVE_CONFIG_H

+#include "config.h"

+#endif

+

+#include "replacements.h"

+

+#include "str7x.h"

+#include "flash.h"

+#include "target.h"

+#include "log.h"

+#include "armv4_5.h"

+#include "algorithm.h"

+#include "binarybuffer.h"

+

+#include <stdlib.h>

+#include <string.h>

+#include <unistd.h>

+

+str7x_mem_layout_t mem_layout[] = {

+	{0x00000000, 0x02000, 0x01},

+	{0x00002000, 0x02000, 0x02},

+	{0x00004000, 0x02000, 0x04},

+	{0x00006000, 0x02000, 0x08},

+	{0x00008000, 0x08000, 0x10},

+	{0x00010000, 0x10000, 0x20},

+	{0x00020000, 0x10000, 0x40},

+	{0x00030000, 0x10000, 0x80},

+	{0x000C0000, 0x02000, 0x10000},

+	{0x000C2000, 0x02000, 0x20000},

+};

+

+int str7x_register_commands(struct command_context_s *cmd_ctx);

+int str7x_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);

+int str7x_erase(struct flash_bank_s *bank, int first, int last);

+int str7x_protect(struct flash_bank_s *bank, int set, int first, int last);

+int str7x_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);

+int str7x_probe(struct flash_bank_s *bank);

+int str7x_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int str7x_protect_check(struct flash_bank_s *bank);

+int str7x_erase_check(struct flash_bank_s *bank);

+int str7x_info(struct flash_bank_s *bank, char *buf, int buf_size);

+

+int str7x_handle_disable_jtag_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+

+flash_driver_t str7x_flash =

+{

+	.name = "str7x",

+	.register_commands = str7x_register_commands,

+	.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 = str7x_erase_check,

+	.protect_check = str7x_protect_check,

+	.info = str7x_info

+};

+

+int str7x_register_commands(struct command_context_s *cmd_ctx)

+{

+	command_t *str7x_cmd = register_command(cmd_ctx, NULL, "str7x", NULL, COMMAND_ANY, NULL);

+	

+	register_command(cmd_ctx, str7x_cmd, "disable_jtag", str7x_handle_disable_jtag_command, COMMAND_EXEC,

+					 "disable jtag access");

+					 

+	return ERROR_OK;

+}

+

+int str7x_get_flash_adr(struct flash_bank_s *bank, u32 reg)

+{

+	return (bank->base | reg);

+}

+

+int str7x_build_block_list(struct flash_bank_s *bank)

+{

+	str7x_flash_bank_t *str7x_info = bank->driver_priv;

+

+	int i;

+	int num_sectors = 0, b0_sectors = 0, b1_sectors = 0;

+		

+	switch (bank->size)

+	{

+		case 16 * 1024:

+			b0_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:

+			ERROR("BUG: unknown bank->size encountered");

+			exit(-1);

+	}

+	

+	if( str7x_info->bank1 == 1 )

+	{

+		b1_sectors += 2;

+	}

+	

+	num_sectors = b0_sectors + b1_sectors;

+	

+	bank->num_sectors = num_sectors;

+	bank->sectors = malloc(sizeof(flash_sector_t) * num_sectors);

+	str7x_info->sector_bits = malloc(sizeof(u32) * num_sectors);

+	str7x_info->sector_bank = malloc(sizeof(u32) * num_sectors);

+	

+	num_sectors = 0;

+	

+	for (i = 0; i < b0_sectors; i++)

+	{

+		bank->sectors[num_sectors].offset = mem_layout[i].sector_start;

+		bank->sectors[num_sectors].size = mem_layout[i].sector_size;

+		bank->sectors[num_sectors].is_erased = -1;

+		bank->sectors[num_sectors].is_protected = 1;

+		str7x_info->sector_bank[num_sectors] = 0;

+		str7x_info->sector_bits[num_sectors++] = mem_layout[i].sector_bit;

+	}

+	

+	if (b1_sectors)

+	{

+		for (i = 8; i < 10; i++)

+		{

+			bank->sectors[num_sectors].offset = mem_layout[i].sector_start;

+			bank->sectors[num_sectors].size = mem_layout[i].sector_size;

+			bank->sectors[num_sectors].is_erased = -1;

+			bank->sectors[num_sectors].is_protected = 1;

+			str7x_info->sector_bank[num_sectors] = 1;

+			str7x_info->sector_bits[num_sectors++] = mem_layout[i].sector_bit;

+		}

+	}

+	

+	return ERROR_OK;

+}

+

+/* flash bank str7x <base> <size> 0 0 <target#> <str71_variant>

+ */

+int str7x_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)

+{

+	str7x_flash_bank_t *str7x_info;

+	

+	if (argc < 7)

+	{

+		WARNING("incomplete flash_bank str7x configuration");

+		return ERROR_FLASH_BANK_INVALID;

+	}

+	

+	str7x_info = malloc(sizeof(str7x_flash_bank_t));

+	bank->driver_priv = str7x_info;

+	

+	/* set default bits for str71x flash */

+	str7x_info->bank1 = 1;

+	str7x_info->busy_bits = (FLASH_LOCK|FLASH_BSYA1|FLASH_BSYA0);

+	str7x_info->disable_bit = (1<<1);

+	

+	if (strcmp(args[6], "STR71x") == 0)

+	{

+		if (bank->base != 0x40000000)

+		{

+			WARNING("overriding flash base address for STR71x device with 0x40000000");

+			bank->base = 0x40000000;

+		}

+	}

+	else if (strcmp(args[6], "STR73x") == 0)

+	{

+		str7x_info->bank1 = 0;

+		str7x_info->busy_bits = (FLASH_LOCK|FLASH_BSYA0);

+		

+		if (bank->base != 0x80000000)

+		{

+			WARNING("overriding flash base address for STR73x device with 0x80000000");

+			bank->base = 0x80000000;

+		}

+	}

+	else if (strcmp(args[6], "STR75x") == 0)

+	{

+		str7x_info->disable_bit = (1<<0);

+		

+		if (bank->base != 0x20000000)

+		{

+			WARNING("overriding flash base address for STR75x device with 0x20000000");

+			bank->base = 0x20000000;

+		}

+	}

+	else

+	{

+		ERROR("unknown STR7x variant: '%s'", args[6]);

+		free(str7x_info);

+		return ERROR_FLASH_BANK_INVALID;

+	}

+

+	str7x_build_block_list(bank);

+	

+	str7x_info->write_algorithm = NULL;

+	

+	return ERROR_OK;

+}

+

+u32 str7x_status(struct flash_bank_s *bank)

+{

+	target_t *target = bank->target;

+	u32 retval;

+

+	target_read_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), &retval);

+

+	return retval;

+}

+

+u32 str7x_result(struct flash_bank_s *bank)

+{

+	target_t *target = bank->target;

+	u32 retval;

+

+	target_read_u32(target, str7x_get_flash_adr(bank, FLASH_ER), &retval);

+	

+	return retval;

+}

+

+int str7x_blank_check(struct flash_bank_s *bank, int first, int last)

+{

+	target_t *target = bank->target;

+	u8 *buffer;

+	int i;

+	int nBytes;

+	

+	if ((first < 0) || (last > bank->num_sectors))

+		return ERROR_FLASH_SECTOR_INVALID;

+

+	if (bank->target->state != TARGET_HALTED)

+	{

+		return ERROR_TARGET_NOT_HALTED;

+	}

+	

+	buffer = malloc(256);

+	

+	for (i = first; i <= last; i++)

+	{

+		bank->sectors[i].is_erased = 1;

+

+		target->type->read_memory(target, bank->base + bank->sectors[i].offset, 4, 256/4, buffer);

+		

+		for (nBytes = 0; nBytes < 256; nBytes++)

+		{

+			if (buffer[nBytes] != 0xFF)

+			{

+				bank->sectors[i].is_erased = 0;

+				break;

+			}

+		}	

+	}

+	

+	free(buffer);

+

+	return ERROR_OK;

+}

+

+int str7x_protect_check(struct flash_bank_s *bank)

+{

+	str7x_flash_bank_t *str7x_info = bank->driver_priv;

+	target_t *target = bank->target;

+	

+	int i;

+	u32 retval;

+

+	if (bank->target->state != TARGET_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;

+}

+

+int str7x_erase(struct flash_bank_s *bank, int first, int last)

+{

+	str7x_flash_bank_t *str7x_info = bank->driver_priv;

+	target_t *target = bank->target;

+	

+	int i;

+	u32 cmd;

+	u32 retval;

+	u32 b0_sectors = 0, b1_sectors = 0;

+	

+	for (i = first; i <= last; i++)

+	{

+		if (str7x_info->sector_bank[i] == 0)

+			b0_sectors |= str7x_info->sector_bits[i];

+		else if (str7x_info->sector_bank[i] == 1)

+			b1_sectors |= str7x_info->sector_bits[i];

+		else

+			ERROR("BUG: str7x_info->sector_bank[i] neither 0 nor 1 (%i)", str7x_info->sector_bank[i]);

+	}

+	

+	if (b0_sectors)

+	{

+		DEBUG("b0_sectors: 0x%x", b0_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 = b0_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)){

+			usleep(1000);

+		}

+		

+		retval = str7x_result(bank);

+		

+		if (retval)

+		{

+			ERROR("error erasing flash bank, FLASH_ER: 0x%x", retval);

+			return ERROR_FLASH_OPERATION_FAILED;

+		}

+	}

+	

+	if (b1_sectors)

+	{

+		DEBUG("b1_sectors: 0x%x", b1_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 = b1_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)){

+			usleep(1000);

+		}

+		

+		retval = str7x_result(bank);

+		

+		if (retval)

+		{

+			ERROR("error erasing flash bank, FLASH_ER: 0x%x", retval);

+			return ERROR_FLASH_OPERATION_FAILED;

+		}

+	}

+	

+	for (i = first; i <= last; i++)

+		bank->sectors[i].is_erased = 1;

+

+	return ERROR_OK;

+}

+

+int str7x_protect(struct flash_bank_s *bank, int set, int first, int last)

+{

+	str7x_flash_bank_t *str7x_info = bank->driver_priv;

+	target_t *target = bank->target;

+	int i;

+	u32 cmd;

+	u32 retval;

+	u32 protect_blocks;

+	

+	if (bank->target->state != TARGET_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)){

+		usleep(1000);

+	}

+	

+	retval = str7x_result(bank);

+	

+	DEBUG("retval: 0x%8.8x", retval);

+	

+	if (retval & FLASH_ERER)

+		return ERROR_FLASH_SECTOR_NOT_ERASED;

+	else if (retval & FLASH_WPF)

+		return ERROR_FLASH_OPERATION_FAILED;

+

+	return ERROR_OK;

+}

+

+int str7x_write_block(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)

+{

+	str7x_flash_bank_t *str7x_info = bank->driver_priv;

+	target_t *target = bank->target;

+	u32 buffer_size = 8192;

+	working_area_t *source;

+	u32 address = bank->base + offset;

+	reg_param_t reg_params[6];

+	armv4_5_algorithm_t armv4_5_info;

+	int retval = ERROR_OK;

+	

+	u32 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)

+	{

+		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, (u8*)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);

+			

+			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)

+	{

+		u32 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->type->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)

+		{

+			ERROR("error executing str7x flash write algorithm");

+			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;

+}

+

+int str7x_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)

+{

+	target_t *target = bank->target;

+	str7x_flash_bank_t *str7x_info = bank->driver_priv;

+	u32 dwords_remaining = (count / 8);

+	u32 bytes_remaining = (count & 0x00000007);

+	u32 address = bank->base + offset;

+	u32 bytes_written = 0;

+	u32 cmd;

+	u32 retval;

+	u32 check_address = offset;

+	int i;

+	

+	if (offset & 0x7)

+	{

+		WARNING("offset 0x%x breaks required 8-byte alignment", offset);

+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;

+	}

+	

+	for (i = 0; i < bank->num_sectors; i++)

+	{

+		u32 sec_start = bank->sectors[i].offset;

+		u32 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 */ 

+				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);

+				

+				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->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_DR0), 4, 1, buffer + bytes_written);

+		bytes_written += 4;

+		

+		// data word 2

+		target->type->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))

+		{

+			usleep(1000);

+		}

+		

+		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)

+	{

+		u8 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->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_DR0), 4, 1, last_dword);

+		bytes_written += 4;

+		

+		// data word 2

+		target->type->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))

+		{

+			usleep(1000);

+		}

+		

+		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;

+}

+

+int str7x_probe(struct flash_bank_s *bank)

+{

+	return ERROR_OK;

+}

+

+int str7x_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	return ERROR_OK;

+}

+

+int str7x_erase_check(struct flash_bank_s *bank)

+{

+	return str7x_blank_check(bank, 0, bank->num_sectors - 1);

+}

+

+int str7x_info(struct flash_bank_s *bank, char *buf, int buf_size)

+{

+	snprintf(buf, buf_size, "str7x flash driver info" );

+	return ERROR_OK;

+}

+

+int str7x_handle_disable_jtag_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	flash_bank_t *bank;

+	target_t *target = NULL;

+	str7x_flash_bank_t *str7x_info = NULL;

+	

+	u32 flash_cmd;

+	u32 retval;

+	u16 ProtectionLevel = 0;

+	u16 ProtectionRegs;

+	

+	if (argc < 1)

+	{

+		command_print(cmd_ctx, "str7x disable_jtag <bank>");

+		return ERROR_OK;	

+	}

+	

+	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+	if (!bank)

+	{

+		command_print(cmd_ctx, "str7x disable_jtag <bank> ok");

+		return ERROR_OK;

+	}

+	

+	str7x_info = bank->driver_priv;

+	

+	target = bank->target;

+	

+	if (target->state != TARGET_HALTED)

+	{

+		return ERROR_TARGET_NOT_HALTED;

+	}

+	

+	/* first we get protection status */

+	target_read_u32(target, str7x_get_flash_adr(bank, FLASH_NVAPR0), &retval);

+

+	if (!(retval & str7x_info->disable_bit))

+	{

+		ProtectionLevel = 1;

+	}

+	

+	target_read_u32(target, str7x_get_flash_adr(bank, FLASH_NVAPR1), &retval);

+	ProtectionRegs = ~(retval >> 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;

+}

+

diff --git a/src/flash/str9x.c b/src/flash/str9x.c
index d79e984f..f5fe2e96 100644
--- a/src/flash/str9x.c
+++ b/src/flash/str9x.c
@@ -1,635 +1,624 @@
-/***************************************************************************
- *   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.             *
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include "replacements.h"
-
-#include "str9x.h"
-#include "flash.h"
-#include "target.h"
-#include "log.h"
-#include "armv4_5.h"
-#include "arm966e.h"
-#include "algorithm.h"
-#include "binarybuffer.h"
-
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-
-str9x_mem_layout_t mem_layout_str9bank0[] = {
-	{0x00000000, 0x10000, 0x01},
-	{0x00010000, 0x10000, 0x02},
-	{0x00020000, 0x10000, 0x04},
-	{0x00030000, 0x10000, 0x08},
-	{0x00040000, 0x10000, 0x10},
-	{0x00050000, 0x10000, 0x20},
-	{0x00060000, 0x10000, 0x40},
-	{0x00070000, 0x10000, 0x80},
-};
-
-str9x_mem_layout_t mem_layout_str9bank1[] = {
-	{0x00000000, 0x02000, 0x100},
-	{0x00002000, 0x02000, 0x200},
-	{0x00004000, 0x02000, 0x400},
-	{0x00006000, 0x02000, 0x800}
-};
-
-static u32 bank1start = 0x00080000;
-
-int str9x_register_commands(struct command_context_s *cmd_ctx);
-int str9x_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);
-int str9x_erase(struct flash_bank_s *bank, int first, int last);
-int str9x_protect(struct flash_bank_s *bank, int set, int first, int last);
-int str9x_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);
-int str9x_probe(struct flash_bank_s *bank);
-int str9x_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int str9x_protect_check(struct flash_bank_s *bank);
-int str9x_erase_check(struct flash_bank_s *bank);
-int str9x_info(struct flash_bank_s *bank, char *buf, int buf_size);
-
-int str9x_handle_flash_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-flash_driver_t str9x_flash =
-{
-	.name = "str9x",
-	.register_commands = str9x_register_commands,
-	.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 = str9x_erase_check,
-	.protect_check = str9x_protect_check,
-	.info = str9x_info
-};
-
-int str9x_register_commands(struct command_context_s *cmd_ctx)
-{
-	command_t *str9x_cmd = register_command(cmd_ctx, NULL, "str9x", NULL, COMMAND_ANY, NULL);
-	
-	register_command(cmd_ctx, str9x_cmd, "flash_config", str9x_handle_flash_config_command, COMMAND_EXEC,
-					 "configure str9 flash controller");
-					 
-	return ERROR_OK;
-}
-
-int str9x_build_block_list(struct flash_bank_s *bank)
-{
-	str9x_flash_bank_t *str9x_info = bank->driver_priv;
-	
-	int i;
-	int num_sectors = 0;
-	int b0_sectors = 0, b1_sectors = 0;
-		
-	switch (bank->size)
-	{
-		case (256 * 1024):
-			b0_sectors = 4;
-			break;
-		case (512 * 1024):
-			b0_sectors = 8;
-			break;
-		case (32 * 1024):
-			b1_sectors = 4;
-			bank1start = bank->base;
-			break;
-		default:
-			ERROR("BUG: unknown bank->size encountered");
-			exit(-1);
-	}
-		
-	num_sectors = b0_sectors + b1_sectors;
-	
-	bank->num_sectors = num_sectors;
-	bank->sectors = malloc(sizeof(flash_sector_t) * num_sectors);
-	str9x_info->sector_bits = malloc(sizeof(u32) * num_sectors);
-	
-	num_sectors = 0;
-	
-	for (i = 0; i < b0_sectors; i++)
-	{
-		bank->sectors[num_sectors].offset = mem_layout_str9bank0[i].sector_start;
-		bank->sectors[num_sectors].size = mem_layout_str9bank0[i].sector_size;
-		bank->sectors[num_sectors].is_erased = -1;
-		bank->sectors[num_sectors].is_protected = 1;
-		str9x_info->sector_bits[num_sectors++] = mem_layout_str9bank0[i].sector_bit;
-	}
-
-	for (i = 0; i < b1_sectors; i++)
-	{
-		bank->sectors[num_sectors].offset = mem_layout_str9bank1[i].sector_start;
-		bank->sectors[num_sectors].size = mem_layout_str9bank1[i].sector_size;
-		bank->sectors[num_sectors].is_erased = -1;
-		bank->sectors[num_sectors].is_protected = 1;
-		str9x_info->sector_bits[num_sectors++] = mem_layout_str9bank1[i].sector_bit;
-	}
-	
-	return ERROR_OK;
-}
-
-/* flash bank str9x <base> <size> 0 0 <target#>
- */
-int str9x_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)
-{
-	str9x_flash_bank_t *str9x_info;
-	
-	if (argc < 6)
-	{
-		WARNING("incomplete flash_bank str9x configuration");
-		return ERROR_FLASH_BANK_INVALID;
-	}
-	
-	str9x_info = malloc(sizeof(str9x_flash_bank_t));
-	bank->driver_priv = str9x_info;
-	
-	str9x_build_block_list(bank);
-	
-	str9x_info->write_algorithm = NULL;
-	
-	return ERROR_OK;
-}
-
-int str9x_blank_check(struct flash_bank_s *bank, int first, int last)
-{
-	target_t *target = bank->target;
-	u8 *buffer;
-	int i;
-	int nBytes;
-	
-	if ((first < 0) || (last > bank->num_sectors))
-		return ERROR_FLASH_SECTOR_INVALID;
-
-	if (bank->target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	
-	buffer = malloc(256);
-	
-	for (i = first; i <= last; i++)
-	{
-		bank->sectors[i].is_erased = 1;
-
-		target->type->read_memory(target, bank->base + bank->sectors[i].offset, 4, 256/4, buffer);
-		
-		for (nBytes = 0; nBytes < 256; nBytes++)
-		{
-			if (buffer[nBytes] != 0xFF)
-			{
-				bank->sectors[i].is_erased = 0;
-				break;
-			}
-		}	
-	}
-	
-	free(buffer);
-
-	return ERROR_OK;
-}
-
-int str9x_protect_check(struct flash_bank_s *bank)
-{
-	str9x_flash_bank_t *str9x_info = bank->driver_priv;
-	target_t *target = bank->target;
-	
-	int i;
-	u32 adr;
-	u16 status;
-
-	if (bank->target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-
-	/* read level one protection */
-	
-	adr = bank1start + 0x10;
-	
-	target_write_u16(target, adr, 0x90);
-	target_read_u16(target, adr, &status);
-	target_write_u16(target, adr, 0xFF);
-	
-	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;
-}
-
-int str9x_erase(struct flash_bank_s *bank, int first, int last)
-{
-	target_t *target = bank->target;
-	int i;
-	u32 adr;
-	u8 status;
-	
-	if (bank->target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-    
-	for (i = first; i <= last; i++)
-	{
-		adr = bank->base + bank->sectors[i].offset;
-		
-    	/* erase sectors */
-		target_write_u16(target, adr, 0x20);
-		target_write_u16(target, adr, 0xD0);
-		
-		/* get status */
-		target_write_u16(target, adr, 0x70);
-		
-		while (1) {
-			target_read_u8(target, adr, &status);
-			if( status & 0x80 )
-				break;
-			usleep(1000);
-		}
-		
-		/* clear status, also clear read array */
-		target_write_u16(target, adr, 0x50);
-		
-		/* read array command */
-		target_write_u16(target, adr, 0xFF);
-		
-		if( status & 0x22 )
-		{
-			ERROR("error erasing flash bank, status: 0x%x", status);
-			return ERROR_FLASH_OPERATION_FAILED;
-		}
-	}
-	
-	for (i = first; i <= last; i++)
-		bank->sectors[i].is_erased = 1;
-
-	return ERROR_OK;
-}
-
-int str9x_protect(struct flash_bank_s *bank, int set, int first, int last)
-{
-	target_t *target = bank->target;
-	int i;
-	u32 adr;
-	u8 status;
-	
-	if (bank->target->state != TARGET_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);
-	}
-	
-	return ERROR_OK;
-}
-
-int str9x_write_block(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
-{
-	str9x_flash_bank_t *str9x_info = bank->driver_priv;
-	target_t *target = bank->target;
-	u32 buffer_size = 8192;
-	working_area_t *source;
-	u32 address = bank->base + offset;
-	reg_param_t reg_params[4];
-	armv4_5_algorithm_t armv4_5_info;
-	int retval;
-	
-	u32 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)
-	{
-		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, (u8*)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);
-			
-			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)
-	{
-		u32 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->type->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)
-		{
-			target_free_working_area(target, source);
-			target_free_working_area(target, str9x_info->write_algorithm);
-			ERROR("error executing str9x flash write algorithm");
-			return ERROR_FLASH_OPERATION_FAILED;
-		}
-	
-		if (buf_get_u32(reg_params[3].value, 0, 32) != 0x80)
-		{
-			return ERROR_FLASH_OPERATION_FAILED;
-		}
-		
-		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 ERROR_OK;
-}
-
-int str9x_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
-{
-	target_t *target = bank->target;
-	u32 words_remaining = (count / 2);
-	u32 bytes_remaining = (count & 0x00000001);
-	u32 address = bank->base + offset;
-	u32 bytes_written = 0;
-	u8 status;
-	u32 retval;
-	u32 check_address = offset;
-	u32 bank_adr;
-	int i;
-	
-	if (bank->target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	
-	if (offset & 0x1)
-	{
-		WARNING("offset 0x%x breaks required 2-byte alignment", offset);
-		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
-	}
-	
-	for (i = 0; i < bank->num_sectors; i++)
-	{
-		u32 sec_start = bank->sectors[i].offset;
-		u32 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 */ 
-				WARNING("couldn't use block writes, falling back to single memory accesses");
-			}
-			else if (retval == ERROR_FLASH_OPERATION_FAILED)
-			{
-				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->type->write_memory(target, address, 2, 1, buffer + bytes_written);
-		
-		/* get status command */
-		target_write_u16(target, bank_adr, 0x70);
-		
-		while (1) {
-			target_read_u8(target, bank_adr, &status);
-			if( status & 0x80 )
-				break;
-			usleep(1000);
-		}
-		
-		/* 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)
-	{
-		u8 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 comamnd */
-		target_write_u16(target, bank_adr, 0x40);
-		target->type->write_memory(target, address, 2, 1, last_halfword);
-		
-		/* query status command */
-		target_write_u16(target, bank_adr, 0x70);
-		
-		while (1) {
-			target_read_u8(target, bank_adr, &status);
-			if( status & 0x80 )
-				break;
-			usleep(1000);
-		}
-		
-		/* 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;
-}
-
-int str9x_probe(struct flash_bank_s *bank)
-{
-	return ERROR_OK;
-}
-
-int str9x_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	return ERROR_OK;
-}
-
-int str9x_erase_check(struct flash_bank_s *bank)
-{
-	return str9x_blank_check(bank, 0, bank->num_sectors - 1);
-}
-
-int str9x_info(struct flash_bank_s *bank, char *buf, int buf_size)
-{
-	snprintf(buf, buf_size, "str9x flash driver info" );
-	return ERROR_OK;
-}
-
-int str9x_handle_flash_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	str9x_flash_bank_t *str9x_info;
-	flash_bank_t *bank;
-	target_t *target = NULL;
-	
-	if (argc < 5)
-	{
-		command_print(cmd_ctx, "usage: str9x flash_config <bank> <bbsize> <nbsize> <bbstart> <nbstart>");
-		return ERROR_OK;
-	}
-	
-	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-	if (!bank)
-	{
-		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-		return ERROR_OK;
-	}
-	
-	str9x_info = bank->driver_priv;
-	
-	target = bank->target;
-	
-	if (bank->target->state != TARGET_HALTED)
-	{
-		return ERROR_TARGET_NOT_HALTED;
-	}
-	
-	/* config flash controller */
-	target_write_u32(target, FLASH_BBSR, strtoul(args[1], NULL, 0));
-	target_write_u32(target, FLASH_NBBSR, strtoul(args[2], NULL, 0));
-	target_write_u32(target, FLASH_BBADR, (strtoul(args[3], NULL, 0) >> 2));
-	target_write_u32(target, FLASH_NBBADR, (strtoul(args[4], NULL, 0) >> 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;
-}
+/***************************************************************************

+ *   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.             *

+ ***************************************************************************/

+#ifdef HAVE_CONFIG_H

+#include "config.h"

+#endif

+

+#include "replacements.h"

+

+#include "str9x.h"

+#include "flash.h"

+#include "target.h"

+#include "log.h"

+#include "armv4_5.h"

+#include "arm966e.h"

+#include "algorithm.h"

+#include "binarybuffer.h"

+

+#include <stdlib.h>

+#include <string.h>

+#include <unistd.h>

+

+str9x_mem_layout_t mem_layout_str9bank0[] = {

+	{0x00000000, 0x10000, 0x01},

+	{0x00010000, 0x10000, 0x02},

+	{0x00020000, 0x10000, 0x04},

+	{0x00030000, 0x10000, 0x08},

+	{0x00040000, 0x10000, 0x10},

+	{0x00050000, 0x10000, 0x20},

+	{0x00060000, 0x10000, 0x40},

+	{0x00070000, 0x10000, 0x80},

+};

+

+str9x_mem_layout_t mem_layout_str9bank1[] = {

+	{0x00000000, 0x02000, 0x100},

+	{0x00002000, 0x02000, 0x200},

+	{0x00004000, 0x02000, 0x400},

+	{0x00006000, 0x02000, 0x800}

+};

+

+static u32 bank1start = 0x00080000;

+

+int str9x_register_commands(struct command_context_s *cmd_ctx);

+int str9x_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);

+int str9x_erase(struct flash_bank_s *bank, int first, int last);

+int str9x_protect(struct flash_bank_s *bank, int set, int first, int last);

+int str9x_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);

+int str9x_probe(struct flash_bank_s *bank);

+int str9x_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int str9x_protect_check(struct flash_bank_s *bank);

+int str9x_erase_check(struct flash_bank_s *bank);

+int str9x_info(struct flash_bank_s *bank, char *buf, int buf_size);

+

+int str9x_handle_flash_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+

+flash_driver_t str9x_flash =

+{

+	.name = "str9x",

+	.register_commands = str9x_register_commands,

+	.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 = str9x_erase_check,

+	.protect_check = str9x_protect_check,

+	.info = str9x_info

+};

+

+int str9x_register_commands(struct command_context_s *cmd_ctx)

+{

+	command_t *str9x_cmd = register_command(cmd_ctx, NULL, "str9x", NULL, COMMAND_ANY, NULL);

+	

+	register_command(cmd_ctx, str9x_cmd, "flash_config", str9x_handle_flash_config_command, COMMAND_EXEC,

+					 "configure str9 flash controller");

+					 

+	return ERROR_OK;

+}

+

+int str9x_build_block_list(struct flash_bank_s *bank)

+{

+	str9x_flash_bank_t *str9x_info = bank->driver_priv;

+	

+	int i;

+	int num_sectors = 0;

+	int b0_sectors = 0, b1_sectors = 0;

+		

+	switch (bank->size)

+	{

+		case (256 * 1024):

+			b0_sectors = 4;

+			break;

+		case (512 * 1024):

+			b0_sectors = 8;

+			break;

+		case (32 * 1024):

+			b1_sectors = 4;

+			bank1start = bank->base;

+			break;

+		default:

+			ERROR("BUG: unknown bank->size encountered");

+			exit(-1);

+	}

+		

+	num_sectors = b0_sectors + b1_sectors;

+	

+	bank->num_sectors = num_sectors;

+	bank->sectors = malloc(sizeof(flash_sector_t) * num_sectors);

+	str9x_info->sector_bits = malloc(sizeof(u32) * num_sectors);

+	

+	num_sectors = 0;

+	

+	for (i = 0; i < b0_sectors; i++)

+	{

+		bank->sectors[num_sectors].offset = mem_layout_str9bank0[i].sector_start;

+		bank->sectors[num_sectors].size = mem_layout_str9bank0[i].sector_size;

+		bank->sectors[num_sectors].is_erased = -1;

+		bank->sectors[num_sectors].is_protected = 1;

+		str9x_info->sector_bits[num_sectors++] = mem_layout_str9bank0[i].sector_bit;

+	}

+

+	for (i = 0; i < b1_sectors; i++)

+	{

+		bank->sectors[num_sectors].offset = mem_layout_str9bank1[i].sector_start;

+		bank->sectors[num_sectors].size = mem_layout_str9bank1[i].sector_size;

+		bank->sectors[num_sectors].is_erased = -1;

+		bank->sectors[num_sectors].is_protected = 1;

+		str9x_info->sector_bits[num_sectors++] = mem_layout_str9bank1[i].sector_bit;

+	}

+	

+	return ERROR_OK;

+}

+

+/* flash bank str9x <base> <size> 0 0 <target#>

+ */

+int str9x_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)

+{

+	str9x_flash_bank_t *str9x_info;

+	

+	if (argc < 6)

+	{

+		WARNING("incomplete flash_bank str9x configuration");

+		return ERROR_FLASH_BANK_INVALID;

+	}

+	

+	str9x_info = malloc(sizeof(str9x_flash_bank_t));

+	bank->driver_priv = str9x_info;

+	

+	str9x_build_block_list(bank);

+	

+	str9x_info->write_algorithm = NULL;

+	

+	return ERROR_OK;

+}

+

+int str9x_blank_check(struct flash_bank_s *bank, int first, int last)

+{

+	target_t *target = bank->target;

+	u8 *buffer;

+	int i;

+	int nBytes;

+	

+	if ((first < 0) || (last > bank->num_sectors))

+		return ERROR_FLASH_SECTOR_INVALID;

+

+	if (bank->target->state != TARGET_HALTED)

+	{

+		return ERROR_TARGET_NOT_HALTED;

+	}

+	

+	buffer = malloc(256);

+	

+	for (i = first; i <= last; i++)

+	{

+		bank->sectors[i].is_erased = 1;

+

+		target->type->read_memory(target, bank->base + bank->sectors[i].offset, 4, 256/4, buffer);

+		

+		for (nBytes = 0; nBytes < 256; nBytes++)

+		{

+			if (buffer[nBytes] != 0xFF)

+			{

+				bank->sectors[i].is_erased = 0;

+				break;

+			}

+		}	

+	}

+	

+	free(buffer);

+

+	return ERROR_OK;

+}

+

+int str9x_protect_check(struct flash_bank_s *bank)

+{

+	str9x_flash_bank_t *str9x_info = bank->driver_priv;

+	target_t *target = bank->target;

+	

+	int i;

+	u32 adr;

+	u16 status;

+

+	if (bank->target->state != TARGET_HALTED)

+	{

+		return ERROR_TARGET_NOT_HALTED;

+	}

+

+	/* read level one protection */

+	

+	adr = bank1start + 0x10;

+	

+	target_write_u16(target, adr, 0x90);

+	target_read_u16(target, adr, &status);

+	target_write_u16(target, adr, 0xFF);

+	

+	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;

+}

+

+int str9x_erase(struct flash_bank_s *bank, int first, int last)

+{

+	target_t *target = bank->target;

+	int i;

+	u32 adr;

+	u8 status;

+	

+	for (i = first; i <= last; i++)

+	{

+		adr = bank->base + bank->sectors[i].offset;

+		

+    	/* erase sectors */

+		target_write_u16(target, adr, 0x20);

+		target_write_u16(target, adr, 0xD0);

+		

+		/* get status */

+		target_write_u16(target, adr, 0x70);

+		

+		while (1) {

+			target_read_u8(target, adr, &status);

+			if( status & 0x80 )

+				break;

+			usleep(1000);

+		}

+		

+		/* clear status, also clear read array */

+		target_write_u16(target, adr, 0x50);

+		

+		/* read array command */

+		target_write_u16(target, adr, 0xFF);

+		

+		if( status & 0x22 )

+		{

+			ERROR("error erasing flash bank, status: 0x%x", status);

+			return ERROR_FLASH_OPERATION_FAILED;

+		}

+	}

+	

+	for (i = first; i <= last; i++)

+		bank->sectors[i].is_erased = 1;

+

+	return ERROR_OK;

+}

+

+int str9x_protect(struct flash_bank_s *bank, int set, int first, int last)

+{

+	target_t *target = bank->target;

+	int i;

+	u32 adr;

+	u8 status;

+	

+	if (bank->target->state != TARGET_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);

+	}

+	

+	return ERROR_OK;

+}

+

+int str9x_write_block(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)

+{

+	str9x_flash_bank_t *str9x_info = bank->driver_priv;

+	target_t *target = bank->target;

+	u32 buffer_size = 8192;

+	working_area_t *source;

+	u32 address = bank->base + offset;

+	reg_param_t reg_params[4];

+	armv4_5_algorithm_t armv4_5_info;

+	int retval;

+	

+	u32 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)

+	{

+		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, (u8*)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);

+			

+			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)

+	{

+		u32 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->type->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)

+		{

+			target_free_working_area(target, source);

+			target_free_working_area(target, str9x_info->write_algorithm);

+			ERROR("error executing str9x flash write algorithm");

+			return ERROR_FLASH_OPERATION_FAILED;

+		}

+	

+		if (buf_get_u32(reg_params[3].value, 0, 32) != 0x80)

+		{

+			return ERROR_FLASH_OPERATION_FAILED;

+		}

+		

+		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 ERROR_OK;

+}

+

+int str9x_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)

+{

+	target_t *target = bank->target;

+	u32 words_remaining = (count / 2);

+	u32 bytes_remaining = (count & 0x00000001);

+	u32 address = bank->base + offset;

+	u32 bytes_written = 0;

+	u8 status;

+	u32 retval;

+	u32 check_address = offset;

+	u32 bank_adr;

+	int i;

+	

+	if (offset & 0x1)

+	{

+		WARNING("offset 0x%x breaks required 2-byte alignment", offset);

+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;

+	}

+	

+	for (i = 0; i < bank->num_sectors; i++)

+	{

+		u32 sec_start = bank->sectors[i].offset;

+		u32 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 */ 

+				WARNING("couldn't use block writes, falling back to single memory accesses");

+			}

+			else if (retval == ERROR_FLASH_OPERATION_FAILED)

+			{

+				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->type->write_memory(target, address, 2, 1, buffer + bytes_written);

+		

+		/* get status command */

+		target_write_u16(target, bank_adr, 0x70);

+		

+		while (1) {

+			target_read_u8(target, bank_adr, &status);

+			if( status & 0x80 )

+				break;

+			usleep(1000);

+		}

+		

+		/* 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)

+	{

+		u8 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 comamnd */

+		target_write_u16(target, bank_adr, 0x40);

+		target->type->write_memory(target, address, 2, 1, last_halfword);

+		

+		/* query status command */

+		target_write_u16(target, bank_adr, 0x70);

+		

+		while (1) {

+			target_read_u8(target, bank_adr, &status);

+			if( status & 0x80 )

+				break;

+			usleep(1000);

+		}

+		

+		/* 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;

+}

+

+int str9x_probe(struct flash_bank_s *bank)

+{

+	return ERROR_OK;

+}

+

+int str9x_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	return ERROR_OK;

+}

+

+int str9x_erase_check(struct flash_bank_s *bank)

+{

+	return str9x_blank_check(bank, 0, bank->num_sectors - 1);

+}

+

+int str9x_info(struct flash_bank_s *bank, char *buf, int buf_size)

+{

+	snprintf(buf, buf_size, "str9x flash driver info" );

+	return ERROR_OK;

+}

+

+int str9x_handle_flash_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	str9x_flash_bank_t *str9x_info;

+	flash_bank_t *bank;

+	target_t *target = NULL;

+	

+	if (argc < 5)

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+	}

+	

+	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+	if (!bank)

+	{

+		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);

+		return ERROR_OK;

+	}

+	

+	str9x_info = bank->driver_priv;

+	

+	target = bank->target;

+	

+	if (bank->target->state != TARGET_HALTED)

+	{

+		return ERROR_TARGET_NOT_HALTED;

+	}

+	

+	/* config flash controller */

+	target_write_u32(target, FLASH_BBSR, strtoul(args[1], NULL, 0));

+	target_write_u32(target, FLASH_NBBSR, strtoul(args[2], NULL, 0));

+	target_write_u32(target, FLASH_BBADR, (strtoul(args[3], NULL, 0) >> 2));

+	target_write_u32(target, FLASH_NBBADR, (strtoul(args[4], NULL, 0) >> 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;

+}

diff --git a/src/flash/str9xpec.c b/src/flash/str9xpec.c
index a9a24bfb..4d3093ee 100644
--- a/src/flash/str9xpec.c
+++ b/src/flash/str9xpec.c
@@ -1,1349 +1,1348 @@
-/***************************************************************************
- *   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.             *
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include "replacements.h"
-
-#include "str9xpec.h"
-#include "flash.h"
-#include "target.h"
-#include "log.h"
-#include "armv4_5.h"
-#include "arm7_9_common.h"
-#include "jtag.h"
-#include "binarybuffer.h"
-
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-#include <getopt.h>
-
-str9xpec_mem_layout_t mem_layout_str9pec[] = {
-	{0x00000000, 0x10000, 0},
-	{0x00010000, 0x10000, 1},
-	{0x00020000, 0x10000, 2},
-	{0x00030000, 0x10000, 3},
-	{0x00040000, 0x10000, 4},
-	{0x00050000, 0x10000, 5},
-	{0x00060000, 0x10000, 6},
-	{0x00070000, 0x10000, 7},
-	{0x00080000, 0x02000, 32},
-	{0x00082000, 0x02000, 33},
-	{0x00084000, 0x02000, 34},
-	{0x00086000, 0x02000, 35}
-};
-
-int str9xpec_register_commands(struct command_context_s *cmd_ctx);
-int str9xpec_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);
-int str9xpec_erase(struct flash_bank_s *bank, int first, int last);
-int str9xpec_protect(struct flash_bank_s *bank, int set, int first, int last);
-int str9xpec_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);
-int str9xpec_probe(struct flash_bank_s *bank);
-int str9xpec_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int str9xpec_protect_check(struct flash_bank_s *bank);
-int str9xpec_erase_check(struct flash_bank_s *bank);
-int str9xpec_info(struct flash_bank_s *bank, char *buf, int buf_size);
-
-int str9xpec_erase_area(struct flash_bank_s *bank, int first, int last);
-int str9xpec_set_address(struct flash_bank_s *bank, u8 sector);
-int str9xpec_write_options(struct flash_bank_s *bank);
-
-int str9xpec_handle_flash_options_cmap_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int str9xpec_handle_flash_options_lvdthd_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int str9xpec_handle_flash_options_lvdsel_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int str9xpec_handle_flash_options_lvdwarn_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int str9xpec_handle_flash_options_read_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int str9xpec_handle_flash_options_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int str9xpec_handle_flash_lock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int str9xpec_handle_flash_unlock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int str9xpec_handle_flash_enable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int str9xpec_handle_flash_disable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-flash_driver_t str9xpec_flash =
-{
-	.name = "str9xpec",
-	.register_commands = str9xpec_register_commands,
-	.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
-};
-
-int str9xpec_register_commands(struct command_context_s *cmd_ctx)
-{
-	command_t *str9xpec_cmd = register_command(cmd_ctx, NULL, "str9xpec", NULL, COMMAND_ANY, "str9xpec flash specific commands");
-	
-	register_command(cmd_ctx, str9xpec_cmd, "enable_turbo", str9xpec_handle_flash_enable_turbo_command, COMMAND_EXEC,
-					 "enable str9xpec turbo mode");
-	register_command(cmd_ctx, str9xpec_cmd, "disable_turbo", str9xpec_handle_flash_disable_turbo_command, COMMAND_EXEC,
-					 "disable str9xpec turbo mode");
-	register_command(cmd_ctx, str9xpec_cmd, "options_cmap", str9xpec_handle_flash_options_cmap_command, COMMAND_EXEC,
-					 "configure str9xpec boot sector");
-	register_command(cmd_ctx, str9xpec_cmd, "options_lvdthd", str9xpec_handle_flash_options_lvdthd_command, COMMAND_EXEC,
-					 "configure str9xpec lvd threshold");
-	register_command(cmd_ctx, str9xpec_cmd, "options_lvdsel", str9xpec_handle_flash_options_lvdsel_command, COMMAND_EXEC,
-					 "configure str9xpec lvd selection");
-	register_command(cmd_ctx, str9xpec_cmd, "options_lvdwarn", str9xpec_handle_flash_options_lvdwarn_command, COMMAND_EXEC,
-					 "configure str9xpec lvd warning");
-	register_command(cmd_ctx, str9xpec_cmd, "options_read", str9xpec_handle_flash_options_read_command, COMMAND_EXEC,
-					 "read str9xpec options");
-	register_command(cmd_ctx, str9xpec_cmd, "options_write", str9xpec_handle_flash_options_write_command, COMMAND_EXEC,
-					 "write str9xpec options");
-	register_command(cmd_ctx, str9xpec_cmd, "lock", str9xpec_handle_flash_lock_command, COMMAND_EXEC,
-					 "lock str9xpec device");
-	register_command(cmd_ctx, str9xpec_cmd, "unlock", str9xpec_handle_flash_unlock_command, COMMAND_EXEC,
-					 "unlock str9xpec device");
-	register_command(cmd_ctx, str9xpec_cmd, "part_id", str9xpec_handle_part_id_command, COMMAND_EXEC,
-					 "print part id of str9xpec flash bank <num>");
-	
-	return ERROR_OK;
-}
-
-int str9xpec_set_instr(int chain_pos, u32 new_instr, enum tap_state end_state)
-{
-	jtag_device_t *device = jtag_get_device(chain_pos);
-	
-	if (device == NULL)
-	{
-		DEBUG("Invalid Target");
-		return ERROR_TARGET_INVALID;
-	}
-		
-	if (buf_get_u32(device->cur_instr, 0, device->ir_length) != new_instr)
-	{
-		scan_field_t field;
-				
-		field.device = chain_pos;
-		field.num_bits = device->ir_length;
-		field.out_value = calloc(CEIL(field.num_bits, 8), 1);
-		buf_set_u32(field.out_value, 0, field.num_bits, new_instr);
-		field.out_mask = NULL;
-		field.in_value = NULL;
-		field.in_check_value = NULL;
-		field.in_check_mask = NULL;
-		field.in_handler = NULL;
-		field.in_handler_priv = NULL;
-		
-		jtag_add_ir_scan(1, &field, end_state, NULL);
-		
-		free(field.out_value);
-	}
-	
-	return ERROR_OK;
-}
-
-u8 str9xpec_isc_status(int chain_pos)
-{
-	scan_field_t field;
-	u8 status;
-	
-	if (str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI) != ERROR_OK)
-		return ISC_STATUS_ERROR;
-	
-	field.device = chain_pos;
-	field.num_bits = 8;
-	field.out_value = NULL;
-	field.out_mask = NULL;
-	field.in_value = &status;
-	field.in_check_value = NULL;
-	field.in_check_mask = NULL;
-	field.in_handler = NULL;
-	field.in_handler_priv = NULL;
-	
-	jtag_add_dr_scan(1, &field, TAP_RTI, NULL);
-	jtag_execute_queue();
-	
-	DEBUG("status: 0x%2.2x", status);
-	
-	if (status & ISC_STATUS_SECURITY)
-		INFO("Device Security Bit Set");
-	
-	return status;
-}
-
-int str9xpec_isc_enable(struct flash_bank_s *bank)
-{
-	u8 status;
-	u32 chain_pos;
-	str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
-	
-	chain_pos = str9xpec_info->chain_pos;
-	
-	if (str9xpec_info->isc_enable)
-		return ERROR_OK;
-	
-	/* enter isc mode */
-	if (str9xpec_set_instr(chain_pos, ISC_ENABLE, TAP_RTI) != ERROR_OK)
-		return ERROR_TARGET_INVALID;
-	
-	/* check ISC status */
-	status = str9xpec_isc_status(chain_pos);
-	if (status & ISC_STATUS_MODE)
-	{
-		/* we have entered isc mode */
-		str9xpec_info->isc_enable = 1;
-		DEBUG("ISC_MODE Enabled");
-	}
-	
-	return ERROR_OK;
-}
-
-int str9xpec_isc_disable(struct flash_bank_s *bank)
-{
-	u8 status;
-	u32 chain_pos;
-	str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
-	
-	chain_pos = str9xpec_info->chain_pos;
-	
-	if (!str9xpec_info->isc_enable)
-		return ERROR_OK;
-	
-	if (str9xpec_set_instr(chain_pos, ISC_DISABLE, TAP_RTI) != ERROR_OK)
-		return ERROR_TARGET_INVALID;
-	
-	/* delay to handle aborts */
-	jtag_add_sleep(50);
-	
-	/* check ISC status */
-	status = str9xpec_isc_status(chain_pos);
-	if (!(status & ISC_STATUS_MODE))
-	{
-		/* we have left isc mode */
-		str9xpec_info->isc_enable = 0;
-		DEBUG("ISC_MODE Disabled");
-	}
-	
-	return ERROR_OK;
-}
-
-int str9xpec_read_config(struct flash_bank_s *bank)
-{
-	scan_field_t field;
-	u8 status;
-	u32 chain_pos;
-		
-	str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
-	
-	chain_pos = str9xpec_info->chain_pos;
-	
-	DEBUG("ISC_CONFIGURATION");
-	
-	/* execute ISC_CONFIGURATION command */
-	str9xpec_set_instr(chain_pos, ISC_CONFIGURATION, TAP_PI);
-	
-	field.device = chain_pos;
-	field.num_bits = 64;
-	field.out_value = NULL;
-	field.out_mask = NULL;
-	field.in_value = str9xpec_info->options;
-	field.in_check_value = NULL;
-	field.in_check_mask = NULL;
-	field.in_handler = NULL;
-	field.in_handler_priv = NULL;
-	
-	jtag_add_dr_scan(1, &field, TAP_RTI, NULL);
-	jtag_execute_queue();
-	
-	status = str9xpec_isc_status(chain_pos);
-	
-	return status;
-}
-
-int str9xpec_build_block_list(struct flash_bank_s *bank)
-{
-	str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
-	
-	int i;
-	int num_sectors = 0, b0_sectors = 0;
-		
-	switch (bank->size)
-	{
-		case (256 * 1024):
-			b0_sectors = 4;
-			break;
-		case (512 * 1024):
-			b0_sectors = 8;
-			break;
-		default:
-			ERROR("BUG: unknown bank->size encountered");
-			exit(-1);
-	}
-	
-	/* include bank 1 sectors */
-	num_sectors = b0_sectors + 4;
-	bank->size += (32 * 1024);
-	
-	bank->num_sectors = num_sectors;
-	bank->sectors = malloc(sizeof(flash_sector_t) * num_sectors);
-	str9xpec_info->sector_bits = malloc(sizeof(u32) * num_sectors);
-	
-	num_sectors = 0;
-	
-	for (i = 0; i < b0_sectors; i++)
-	{
-		bank->sectors[num_sectors].offset = mem_layout_str9pec[i].sector_start;
-		bank->sectors[num_sectors].size = mem_layout_str9pec[i].sector_size;
-		bank->sectors[num_sectors].is_erased = -1;
-		bank->sectors[num_sectors].is_protected = 1;
-		str9xpec_info->sector_bits[num_sectors++] = mem_layout_str9pec[i].sector_bit;
-	}
-	
-	for (i = 8; i < 12; i++)
-	{
-		bank->sectors[num_sectors].offset = mem_layout_str9pec[i].sector_start;
-		bank->sectors[num_sectors].size = mem_layout_str9pec[i].sector_size;
-		bank->sectors[num_sectors].is_erased = -1;
-		bank->sectors[num_sectors].is_protected = 1;
-		str9xpec_info->sector_bits[num_sectors++] = mem_layout_str9pec[i].sector_bit;
-	}
-	
-	return ERROR_OK;
-}
-
-/* flash bank str9x <base> <size> 0 0 <target#>
- */
-int str9xpec_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)
-{
-	str9xpec_flash_controller_t *str9xpec_info;
-	armv4_5_common_t *armv4_5 = NULL;
-	arm7_9_common_t *arm7_9 = NULL;
-	arm_jtag_t *jtag_info = NULL;
-	
-	if (argc < 6)
-	{
-		WARNING("incomplete flash_bank str9x configuration");
-		return ERROR_FLASH_BANK_INVALID;
-	}
-	
-	str9xpec_info = malloc(sizeof(str9xpec_flash_controller_t));
-	bank->driver_priv = str9xpec_info;
-	
-	if (bank->base != 0x00000000)
-	{
-		WARNING("overriding flash base address for STR91x device with 0x00000000");
-		bank->base = 0x00000000;
-	}
-
-	/* find out jtag position of flash controller
-	 * it is always after the arm966 core */
-	
-	armv4_5 = bank->target->arch_info;
-	arm7_9 = armv4_5->arch_info;
-	jtag_info = &arm7_9->jtag_info;
-	
-	str9xpec_info->chain_pos = (jtag_info->chain_pos - 1);
-	str9xpec_info->isc_enable = 0;
-	str9xpec_info->devarm = NULL;
-	
-	str9xpec_build_block_list(bank);
-	
-	/* clear option byte register */
-	buf_set_u32(str9xpec_info->options, 0, 64, 0);
-	
-	return ERROR_OK;
-}
-
-int str9xpec_blank_check(struct flash_bank_s *bank, int first, int last)
-{
-	scan_field_t field;
-	u8 status;
-	u32 chain_pos;
-	int i;
-	u8 *buffer = NULL;
-		
-	str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
-	
-	chain_pos = str9xpec_info->chain_pos;
-	
-	if (!str9xpec_info->isc_enable) {
-		str9xpec_isc_enable( bank );
-	}
-	
-	if (!str9xpec_info->isc_enable) {
-		return ERROR_FLASH_OPERATION_FAILED;
-	}
-	
-	buffer = calloc(CEIL(64, 8), 1);
-
-	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(chain_pos, ISC_BLANK_CHECK, TAP_PI);
-	
-	field.device = chain_pos;
-	field.num_bits = 64;
-	field.out_value = buffer;
-	field.out_mask = NULL;
-	field.in_value = NULL;
-	field.in_check_value = NULL;
-	field.in_check_mask = NULL;
-	field.in_handler = NULL;
-	field.in_handler_priv = NULL;
-	
-	jtag_add_dr_scan(1, &field, TAP_RTI, NULL);
-	jtag_add_sleep(40000);
-	
-	/* read blank check result */
-	field.device = chain_pos;
-	field.num_bits = 64;
-	field.out_value = NULL;
-	field.out_mask = NULL;
-	field.in_value = buffer;
-	field.in_check_value = NULL;
-	field.in_check_mask = NULL;
-	field.in_handler = NULL;
-	field.in_handler_priv = NULL;
-	
-	jtag_add_dr_scan(1, &field, TAP_PI, NULL);
-	jtag_execute_queue();
-	
-	status = str9xpec_isc_status(chain_pos);
-	
-	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;
-}
-
-int str9xpec_protect_check(struct flash_bank_s *bank)
-{
-	u8 status;
-	int i;
-		
-	str9xpec_flash_controller_t *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;
-}
-
-int str9xpec_erase_area(struct flash_bank_s *bank, int first, int last)
-{
-	scan_field_t field;
-	u8 status;
-	u32 chain_pos;
-	int i;
-	u8 *buffer = NULL;
-	
-	str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
-	
-	chain_pos = str9xpec_info->chain_pos;
-	
-	if (!str9xpec_info->isc_enable) {
-		str9xpec_isc_enable( bank );
-	}
-	
-	if (!str9xpec_info->isc_enable) {
-		return ISC_STATUS_ERROR;
-	}
-	
-	buffer = calloc(CEIL(64, 8), 1);
-	
-	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);
-		}
-	}
-	
-	DEBUG("ISC_ERASE");
-	
-	/* execute ISC_ERASE command */
-	str9xpec_set_instr(chain_pos, ISC_ERASE, TAP_PI);
-	
-	field.device = chain_pos;
-	field.num_bits = 64;
-	field.out_value = buffer;
-	field.out_mask = NULL;
-	field.in_value = NULL;
-	field.in_check_value = NULL;
-	field.in_check_mask = NULL;
-	field.in_handler = NULL;
-	field.in_handler_priv = NULL;
-	
-	jtag_add_dr_scan(1, &field, TAP_RTI, NULL);
-	jtag_execute_queue();
-	
-	jtag_add_sleep(10);
-	
-	/* wait for erase completion */
-	while (!((status = str9xpec_isc_status(chain_pos)) & ISC_STATUS_BUSY)) {
-		usleep(1000);
-	}
-	
-	free(buffer);
-	
-	str9xpec_isc_disable(bank);
-	
-	return status;
-}
-
-int str9xpec_erase(struct flash_bank_s *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;
-}
-
-int str9xpec_lock_device(struct flash_bank_s *bank)
-{
-	scan_field_t field;
-	u8 status;
-	u32 chain_pos;
-	str9xpec_flash_controller_t *str9xpec_info = NULL;
-	
-	str9xpec_info = bank->driver_priv;
-	chain_pos = str9xpec_info->chain_pos;
-	
-	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(chain_pos, ISC_PROGRAM_SECURITY, TAP_RTI);
-	
-	str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI);
-	
-	do {
-		field.device = chain_pos;
-		field.num_bits = 8;
-		field.out_value = NULL;
-		field.out_mask = NULL;
-		field.in_value = &status;
-		field.in_check_value = NULL;
-		field.in_check_mask = NULL;
-		field.in_handler = NULL;
-		field.in_handler_priv = NULL;
-		
-		jtag_add_dr_scan(1, &field, -1, NULL);
-		jtag_execute_queue();
-		
-	} while(!(status & ISC_STATUS_BUSY));
-	
-	str9xpec_isc_disable(bank);
-	
-	return status;
-}
-
-int str9xpec_unlock_device(struct flash_bank_s *bank)
-{
-	u8 status;
-	
-	status = str9xpec_erase_area(bank, 0, 255);
-	
-	return status;
-}
-
-int str9xpec_protect(struct flash_bank_s *bank, int set, int first, int last)
-{
-	u8 status;
-	int i;
-	
-	str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
-	
-	status = str9xpec_read_config(bank);
-	
-	if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
-		return ERROR_FLASH_OPERATION_FAILED;
-
-	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;
-}
-
-int str9xpec_set_address(struct flash_bank_s *bank, u8 sector)
-{
-	u32 chain_pos;
-	scan_field_t field;
-	str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
-	
-	chain_pos = str9xpec_info->chain_pos;
-	
-	/* set flash controller address */
-	str9xpec_set_instr(chain_pos, ISC_ADDRESS_SHIFT, TAP_PI);
-	
-	field.device = chain_pos;
-	field.num_bits = 8;
-	field.out_value = &sector;
-	field.out_mask = NULL;
-	field.in_value = NULL;
-	field.in_check_value = NULL;
-	field.in_check_mask = NULL;
-	field.in_handler = NULL;
-	field.in_handler_priv = NULL;
-	
-	jtag_add_dr_scan(1, &field, -1, NULL);
-		
-	return ERROR_OK;
-}
-
-int str9xpec_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
-{
-	str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
-	u32 dwords_remaining = (count / 8);
-	u32 bytes_remaining = (count & 0x00000007);
-	u32 bytes_written = 0;
-	u8 status;
-	u32 check_address = offset;
-	u32 chain_pos;
-	scan_field_t field;
-	u8 *scanbuf;
-	int i;
-	u32 first_sector = 0;
-	u32 last_sector = 0;
-	
-	chain_pos = str9xpec_info->chain_pos;
-	
-	if (!str9xpec_info->isc_enable) {
-		str9xpec_isc_enable(bank);
-	}
-	
-	if (!str9xpec_info->isc_enable) {
-		return ERROR_FLASH_OPERATION_FAILED;
-	}
-	
-	if (offset & 0x7)
-	{
-		WARNING("offset 0x%x breaks required 8-byte alignment", offset);
-		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
-	}
-	
-	for (i = 0; i < bank->num_sectors; i++)
-	{
-		u32 sec_start = bank->sectors[i].offset;
-		u32 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;
-
-	DEBUG("first_sector: %i, last_sector: %i", first_sector, last_sector);
-	
-	scanbuf = calloc(CEIL(64, 8), 1);
-	
-	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(chain_pos, ISC_PROGRAM, TAP_PI);
-			
-			field.device = chain_pos;
-			field.num_bits = 64;
-			field.out_value = (buffer + bytes_written);
-			field.out_mask = NULL;
-			field.in_value = NULL;
-			field.in_check_value = NULL;
-			field.in_check_mask = NULL;
-			field.in_handler = NULL;
-			field.in_handler_priv = NULL;
-			
-			jtag_add_dr_scan(1, &field, TAP_RTI, NULL);
-			
-			/* small delay before polling */
-			jtag_add_sleep(50);
-			
-			str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI);
-			
-			do {
-				field.device = chain_pos;
-				field.num_bits = 8;
-				field.out_value = NULL;
-				field.out_mask = NULL;
-				field.in_value = scanbuf;
-				field.in_check_value = NULL;
-				field.in_check_mask = NULL;
-				field.in_handler = NULL;
-				field.in_handler_priv = NULL;
-				
-				jtag_add_dr_scan(1, &field, -1, NULL);
-				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)
-	{
-		u8 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(chain_pos, ISC_PROGRAM, TAP_PI);
-		
-		field.device = chain_pos;
-		field.num_bits = 64;
-		field.out_value = last_dword;
-		field.out_mask = NULL;
-		field.in_value = NULL;
-		field.in_check_value = NULL;
-		field.in_check_mask = NULL;
-		field.in_handler = NULL;
-		field.in_handler_priv = NULL;
-		
-		jtag_add_dr_scan(1, &field, TAP_RTI, NULL);
-		
-		/* small delay before polling */
-		jtag_add_sleep(50);
-		
-		str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI);
-		
-		do {
-			field.device = chain_pos;
-			field.num_bits = 8;
-			field.out_value = NULL;
-			field.out_mask = NULL;
-			field.in_value = scanbuf;
-			field.in_check_value = NULL;
-			field.in_check_mask = NULL;
-			field.in_handler = NULL;
-			field.in_handler_priv = NULL;
-			
-			jtag_add_dr_scan(1, &field, -1, NULL);
-			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;
-}
-
-int str9xpec_probe(struct flash_bank_s *bank)
-{
-	return ERROR_OK;
-}
-
-int str9xpec_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	flash_bank_t *bank;
-	scan_field_t field;
-	u8 *buffer = NULL;
-	u32 chain_pos;
-	u32 idcode;
-	str9xpec_flash_controller_t *str9xpec_info = NULL;
-
-	if (argc < 1)
-	{
-		command_print(cmd_ctx, "usage: str9xpec part_id <num>");
-		return ERROR_OK;
-	}
-	
-	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-	if (!bank)
-	{
-		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-		return ERROR_OK;
-	}
-	
-	str9xpec_info = bank->driver_priv;
-	chain_pos = str9xpec_info->chain_pos;
-	
-	buffer = calloc(CEIL(32, 8), 1);
-	
-	str9xpec_set_instr(chain_pos, ISC_IDCODE, TAP_PI);
-	
-	field.device = chain_pos;
-	field.num_bits = 32;
-	field.out_value = NULL;
-	field.out_mask = NULL;
-	field.in_value = buffer;
-	field.in_check_value = NULL;
-	field.in_check_mask = NULL;
-	field.in_handler = NULL;
-	field.in_handler_priv = NULL;
-	
-	jtag_add_dr_scan(1, &field, TAP_RTI, NULL);
-	jtag_execute_queue();
-	
-	idcode = buf_get_u32(buffer, 0, 32);
-	
-	command_print(cmd_ctx, "str9xpec part id: 0x%8.8x", idcode);
-	
-	free(buffer);
-	
-	return ERROR_OK;
-}
-
-int str9xpec_erase_check(struct flash_bank_s *bank)
-{
-	return str9xpec_blank_check(bank, 0, bank->num_sectors - 1);
-}
-
-int str9xpec_info(struct flash_bank_s *bank, char *buf, int buf_size)
-{
-	snprintf(buf, buf_size, "str9xpec flash driver info" );
-	return ERROR_OK;
-}
-
-int str9xpec_handle_flash_options_read_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	flash_bank_t *bank;
-	u8 status;
-	str9xpec_flash_controller_t *str9xpec_info = NULL;
-	
-	if (argc < 1)
-	{
-		command_print(cmd_ctx, "str9xpec options_read <bank>");
-		return ERROR_OK;	
-	}
-	
-	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-	if (!bank)
-	{
-		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-		return ERROR_OK;
-	}
-	
-	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;
-}
-
-int str9xpec_write_options(struct flash_bank_s *bank)
-{
-	scan_field_t field;
-	u8 status;
-	u32 chain_pos;
-	str9xpec_flash_controller_t *str9xpec_info = NULL;
-	
-	str9xpec_info = bank->driver_priv;
-	chain_pos = str9xpec_info->chain_pos;
-	
-	/* 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(chain_pos, ISC_PROGRAM, TAP_PI);
-		
-	field.device = chain_pos;
-	field.num_bits = 64;
-	field.out_value = str9xpec_info->options;
-	field.out_mask = NULL;
-	field.in_value = NULL;
-	field.in_check_value = NULL;
-	field.in_check_mask = NULL;
-	field.in_handler = NULL;
-	field.in_handler_priv = NULL;
-	
-	jtag_add_dr_scan(1, &field, TAP_RTI, NULL);
-	
-	/* small delay before polling */
-	jtag_add_sleep(50);
-	
-	str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI);
-	
-	do {
-		field.device = chain_pos;
-		field.num_bits = 8;
-		field.out_value = NULL;
-		field.out_mask = NULL;
-		field.in_value = &status;
-		field.in_check_value = NULL;
-		field.in_check_mask = NULL;
-		field.in_handler = NULL;
-		field.in_handler_priv = NULL;
-		
-		jtag_add_dr_scan(1, &field, -1, NULL);
-		jtag_execute_queue();
-		
-	} while(!(status & ISC_STATUS_BUSY));
-	
-	str9xpec_isc_disable(bank);
-	
-	return status;
-}
-
-int str9xpec_handle_flash_options_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	flash_bank_t *bank;
-	u8 status;
-	
-	if (argc < 1)
-	{
-		command_print(cmd_ctx, "str9xpec options_write <bank>");
-		return ERROR_OK;	
-	}
-	
-	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-	if (!bank)
-	{
-		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-		return ERROR_OK;
-	}
-	
-	status = str9xpec_write_options(bank);
-	
-	if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
-		return ERROR_FLASH_OPERATION_FAILED;
-	
-	return ERROR_OK;
-}
-
-int str9xpec_handle_flash_options_cmap_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	flash_bank_t *bank;
-	str9xpec_flash_controller_t *str9xpec_info = NULL;
-	
-	if (argc < 2)
-	{
-		command_print(cmd_ctx, "str9xpec options_cmap <bank> <bank0|bank1>");
-		return ERROR_OK;	
-	}
-	
-	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-	if (!bank)
-	{
-		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-		return ERROR_OK;
-	}
-	
-	str9xpec_info = bank->driver_priv;
-	
-	if (strcmp(args[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;
-}
-
-int str9xpec_handle_flash_options_lvdthd_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	flash_bank_t *bank;
-	str9xpec_flash_controller_t *str9xpec_info = NULL;
-	
-	if (argc < 2)
-	{
-		command_print(cmd_ctx, "str9xpec options_lvdthd <bank> <2.4v|2.7v>");
-		return ERROR_OK;	
-	}
-	
-	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-	if (!bank)
-	{
-		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-		return ERROR_OK;
-	}
-	
-	str9xpec_info = bank->driver_priv;
-	
-	if (strcmp(args[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;
-}
-
-int str9xpec_handle_flash_options_lvdsel_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	flash_bank_t *bank;
-	str9xpec_flash_controller_t *str9xpec_info = NULL;
-	
-	if (argc < 2)
-	{
-		command_print(cmd_ctx, "str9xpec options_lvdsel <bank> <vdd|vdd_vddq>");
-		return ERROR_OK;	
-	}
-	
-	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-	if (!bank)
-	{
-		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-		return ERROR_OK;
-	}
-	
-	str9xpec_info = bank->driver_priv;
-	
-	if (strcmp(args[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;
-}
-
-int str9xpec_handle_flash_options_lvdwarn_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	flash_bank_t *bank;
-	str9xpec_flash_controller_t *str9xpec_info = NULL;
-	
-	if (argc < 2)
-	{
-		command_print(cmd_ctx, "str9xpec options_lvdwarn <bank> <vdd|vdd_vddq>");
-		return ERROR_OK;	
-	}
-	
-	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-	if (!bank)
-	{
-		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-		return ERROR_OK;
-	}
-	
-	str9xpec_info = bank->driver_priv;
-	
-	if (strcmp(args[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;
-}
-
-int str9xpec_handle_flash_lock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	u8 status;
-	flash_bank_t *bank;
-	
-	if (argc < 1)
-	{
-		command_print(cmd_ctx, "str9xpec lock <bank>");
-		return ERROR_OK;	
-	}
-	
-	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-	if (!bank)
-	{
-		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-		return ERROR_OK;
-	}
-	
-	status = str9xpec_lock_device(bank);
-	
-	if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
-		return ERROR_FLASH_OPERATION_FAILED;
-	
-	return ERROR_OK;
-}
-
-int str9xpec_handle_flash_unlock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	u8 status;
-	flash_bank_t *bank;
-	
-	if (argc < 1)
-	{
-		command_print(cmd_ctx, "str9xpec unlock <bank>");
-		return ERROR_OK;	
-	}
-	
-	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-	if (!bank)
-	{
-		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-		return ERROR_OK;
-	}
-	
-	status = str9xpec_unlock_device(bank);
-	
-	if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
-		return ERROR_FLASH_OPERATION_FAILED;
-	
-	return ERROR_OK;
-}
-
-int str9xpec_handle_flash_enable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	flash_bank_t *bank;
-	u32 chain_pos;
-	jtag_device_t* dev0;
-	jtag_device_t* dev2;
-	str9xpec_flash_controller_t *str9xpec_info = NULL;
-	
-	if (argc < 1)
-	{
-		command_print(cmd_ctx, "str9xpec enable_turbo <bank>");
-		return ERROR_OK;	
-	}
-	
-	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-	if (!bank)
-	{
-		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-		return ERROR_OK;
-	}
-	
-	str9xpec_info = bank->driver_priv;
-	
-	chain_pos = str9xpec_info->chain_pos;
-	
-	/* remove arm core from chain - enter turbo mode */
-	
-	str9xpec_set_instr(chain_pos+2, 0xD, TAP_RTI);
-	jtag_execute_queue();
-	
-	/* modify scan chain - str9 core has been removed */
-	dev0 = jtag_get_device(chain_pos);
-	str9xpec_info->devarm = jtag_get_device(chain_pos+1);
-	dev2 = jtag_get_device(chain_pos+2);
-	dev0->next = dev2;
-	jtag_num_devices--;
-	
-	return ERROR_OK;
-}
-
-int str9xpec_handle_flash_disable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	flash_bank_t *bank;
-	u32 chain_pos;
-	jtag_device_t* dev0;
-	str9xpec_flash_controller_t *str9xpec_info = NULL;
-	
-	if (argc < 1)
-	{
-		command_print(cmd_ctx, "str9xpec disable_turbo <bank>");
-		return ERROR_OK;	
-	}
-	
-	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-	if (!bank)
-	{
-		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-		return ERROR_OK;
-	}
-	
-	str9xpec_info = bank->driver_priv;
-	
-	chain_pos = str9xpec_info->chain_pos;
-	
-	dev0 = jtag_get_device(chain_pos);
-	
-	/* exit turbo mode via TLR */
-	str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_TLR);
-	jtag_execute_queue();
-	
-	/* restore previous scan chain */
-	if( str9xpec_info->devarm ) {
-		dev0->next = str9xpec_info->devarm;
-		jtag_num_devices++;
-		str9xpec_info->devarm = NULL;
-	}
-	
-	return ERROR_OK;
-}
+/***************************************************************************

+ *   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.             *

+ ***************************************************************************/

+#ifdef HAVE_CONFIG_H

+#include "config.h"

+#endif

+

+#include "replacements.h"

+

+#include "str9xpec.h"

+#include "flash.h"

+#include "target.h"

+#include "log.h"

+#include "armv4_5.h"

+#include "arm7_9_common.h"

+#include "jtag.h"

+#include "binarybuffer.h"

+

+#include <stdlib.h>

+#include <string.h>

+#include <unistd.h>

+#include <getopt.h>

+

+str9xpec_mem_layout_t mem_layout_str9pec[] = {

+	{0x00000000, 0x10000, 0},

+	{0x00010000, 0x10000, 1},

+	{0x00020000, 0x10000, 2},

+	{0x00030000, 0x10000, 3},

+	{0x00040000, 0x10000, 4},

+	{0x00050000, 0x10000, 5},

+	{0x00060000, 0x10000, 6},

+	{0x00070000, 0x10000, 7},

+	{0x00080000, 0x02000, 32},

+	{0x00082000, 0x02000, 33},

+	{0x00084000, 0x02000, 34},

+	{0x00086000, 0x02000, 35}

+};

+

+int str9xpec_register_commands(struct command_context_s *cmd_ctx);

+int str9xpec_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);

+int str9xpec_erase(struct flash_bank_s *bank, int first, int last);

+int str9xpec_protect(struct flash_bank_s *bank, int set, int first, int last);

+int str9xpec_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);

+int str9xpec_probe(struct flash_bank_s *bank);

+int str9xpec_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int str9xpec_protect_check(struct flash_bank_s *bank);

+int str9xpec_erase_check(struct flash_bank_s *bank);

+int str9xpec_info(struct flash_bank_s *bank, char *buf, int buf_size);

+

+int str9xpec_erase_area(struct flash_bank_s *bank, int first, int last);

+int str9xpec_set_address(struct flash_bank_s *bank, u8 sector);

+int str9xpec_write_options(struct flash_bank_s *bank);

+

+int str9xpec_handle_flash_options_cmap_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int str9xpec_handle_flash_options_lvdthd_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int str9xpec_handle_flash_options_lvdsel_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int str9xpec_handle_flash_options_lvdwarn_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int str9xpec_handle_flash_options_read_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int str9xpec_handle_flash_options_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int str9xpec_handle_flash_lock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int str9xpec_handle_flash_unlock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int str9xpec_handle_flash_enable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+int str9xpec_handle_flash_disable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

+

+flash_driver_t str9xpec_flash =

+{

+	.name = "str9xpec",

+	.register_commands = str9xpec_register_commands,

+	.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

+};

+

+int str9xpec_register_commands(struct command_context_s *cmd_ctx)

+{

+	command_t *str9xpec_cmd = register_command(cmd_ctx, NULL, "str9xpec", NULL, COMMAND_ANY, "str9xpec flash specific commands");

+	

+	register_command(cmd_ctx, str9xpec_cmd, "enable_turbo", str9xpec_handle_flash_enable_turbo_command, COMMAND_EXEC,

+					 "enable str9xpec turbo mode");

+	register_command(cmd_ctx, str9xpec_cmd, "disable_turbo", str9xpec_handle_flash_disable_turbo_command, COMMAND_EXEC,

+					 "disable str9xpec turbo mode");

+	register_command(cmd_ctx, str9xpec_cmd, "options_cmap", str9xpec_handle_flash_options_cmap_command, COMMAND_EXEC,

+					 "configure str9xpec boot sector");

+	register_command(cmd_ctx, str9xpec_cmd, "options_lvdthd", str9xpec_handle_flash_options_lvdthd_command, COMMAND_EXEC,

+					 "configure str9xpec lvd threshold");

+	register_command(cmd_ctx, str9xpec_cmd, "options_lvdsel", str9xpec_handle_flash_options_lvdsel_command, COMMAND_EXEC,

+					 "configure str9xpec lvd selection");

+	register_command(cmd_ctx, str9xpec_cmd, "options_lvdwarn", str9xpec_handle_flash_options_lvdwarn_command, COMMAND_EXEC,

+					 "configure str9xpec lvd warning");

+	register_command(cmd_ctx, str9xpec_cmd, "options_read", str9xpec_handle_flash_options_read_command, COMMAND_EXEC,

+					 "read str9xpec options");

+	register_command(cmd_ctx, str9xpec_cmd, "options_write", str9xpec_handle_flash_options_write_command, COMMAND_EXEC,

+					 "write str9xpec options");

+	register_command(cmd_ctx, str9xpec_cmd, "lock", str9xpec_handle_flash_lock_command, COMMAND_EXEC,

+					 "lock str9xpec device");

+	register_command(cmd_ctx, str9xpec_cmd, "unlock", str9xpec_handle_flash_unlock_command, COMMAND_EXEC,

+					 "unlock str9xpec device");

+	register_command(cmd_ctx, str9xpec_cmd, "part_id", str9xpec_handle_part_id_command, COMMAND_EXEC,

+					 "print part id of str9xpec flash bank <num>");

+	

+	return ERROR_OK;

+}

+

+int str9xpec_set_instr(int chain_pos, u32 new_instr, enum tap_state end_state)

+{

+	jtag_device_t *device = jtag_get_device(chain_pos);

+	

+	if (device == NULL)

+	{

+		DEBUG("Invalid Target");

+		return ERROR_TARGET_INVALID;

+	}

+		

+	if (buf_get_u32(device->cur_instr, 0, device->ir_length) != new_instr)

+	{

+		scan_field_t field;

+				

+		field.device = chain_pos;

+		field.num_bits = device->ir_length;

+		field.out_value = calloc(CEIL(field.num_bits, 8), 1);

+		buf_set_u32(field.out_value, 0, field.num_bits, new_instr);

+		field.out_mask = NULL;

+		field.in_value = NULL;

+		field.in_check_value = NULL;

+		field.in_check_mask = NULL;

+		field.in_handler = NULL;

+		field.in_handler_priv = NULL;

+		

+		jtag_add_ir_scan(1, &field, end_state, NULL);

+		

+		free(field.out_value);

+	}

+	

+	return ERROR_OK;

+}

+

+u8 str9xpec_isc_status(int chain_pos)

+{

+	scan_field_t field;

+	u8 status;

+	

+	if (str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI) != ERROR_OK)

+		return ISC_STATUS_ERROR;

+	

+	field.device = chain_pos;

+	field.num_bits = 8;

+	field.out_value = NULL;

+	field.out_mask = NULL;

+	field.in_value = &status;

+	field.in_check_value = NULL;

+	field.in_check_mask = NULL;

+	field.in_handler = NULL;

+	field.in_handler_priv = NULL;

+	

+	jtag_add_dr_scan(1, &field, TAP_RTI, NULL);

+	jtag_execute_queue();

+	

+	DEBUG("status: 0x%2.2x", status);

+	

+	if (status & ISC_STATUS_SECURITY)

+		INFO("Device Security Bit Set");

+	

+	return status;

+}

+

+int str9xpec_isc_enable(struct flash_bank_s *bank)

+{

+	u8 status;

+	u32 chain_pos;

+	str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;

+	

+	chain_pos = str9xpec_info->chain_pos;

+	

+	if (str9xpec_info->isc_enable)

+		return ERROR_OK;

+	

+	/* enter isc mode */

+	if (str9xpec_set_instr(chain_pos, ISC_ENABLE, TAP_RTI) != ERROR_OK)

+		return ERROR_TARGET_INVALID;

+	

+	/* check ISC status */

+	status = str9xpec_isc_status(chain_pos);

+	if (status & ISC_STATUS_MODE)

+	{

+		/* we have entered isc mode */

+		str9xpec_info->isc_enable = 1;

+		DEBUG("ISC_MODE Enabled");

+	}

+	

+	return ERROR_OK;

+}

+

+int str9xpec_isc_disable(struct flash_bank_s *bank)

+{

+	u8 status;

+	u32 chain_pos;

+	str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;

+	

+	chain_pos = str9xpec_info->chain_pos;

+	

+	if (!str9xpec_info->isc_enable)

+		return ERROR_OK;

+	

+	if (str9xpec_set_instr(chain_pos, ISC_DISABLE, TAP_RTI) != ERROR_OK)

+		return ERROR_TARGET_INVALID;

+	

+	/* delay to handle aborts */

+	jtag_add_sleep(50);

+	

+	/* check ISC status */

+	status = str9xpec_isc_status(chain_pos);

+	if (!(status & ISC_STATUS_MODE))

+	{

+		/* we have left isc mode */

+		str9xpec_info->isc_enable = 0;

+		DEBUG("ISC_MODE Disabled");

+	}

+	

+	return ERROR_OK;

+}

+

+int str9xpec_read_config(struct flash_bank_s *bank)

+{

+	scan_field_t field;

+	u8 status;

+	u32 chain_pos;

+		

+	str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;

+	

+	chain_pos = str9xpec_info->chain_pos;

+	

+	DEBUG("ISC_CONFIGURATION");

+	

+	/* execute ISC_CONFIGURATION command */

+	str9xpec_set_instr(chain_pos, ISC_CONFIGURATION, TAP_PI);

+	

+	field.device = chain_pos;

+	field.num_bits = 64;

+	field.out_value = NULL;

+	field.out_mask = NULL;

+	field.in_value = str9xpec_info->options;

+	field.in_check_value = NULL;

+	field.in_check_mask = NULL;

+	field.in_handler = NULL;

+	field.in_handler_priv = NULL;

+	

+	jtag_add_dr_scan(1, &field, TAP_RTI, NULL);

+	jtag_execute_queue();

+	

+	status = str9xpec_isc_status(chain_pos);

+	

+	return status;

+}

+

+int str9xpec_build_block_list(struct flash_bank_s *bank)

+{

+	str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;

+	

+	int i;

+	int num_sectors = 0, b0_sectors = 0;

+		

+	switch (bank->size)

+	{

+		case (256 * 1024):

+			b0_sectors = 4;

+			break;

+		case (512 * 1024):

+			b0_sectors = 8;

+			break;

+		default:

+			ERROR("BUG: unknown bank->size encountered");

+			exit(-1);

+	}

+	

+	/* include bank 1 sectors */

+	num_sectors = b0_sectors + 4;

+	bank->size += (32 * 1024);

+	

+	bank->num_sectors = num_sectors;

+	bank->sectors = malloc(sizeof(flash_sector_t) * num_sectors);

+	str9xpec_info->sector_bits = malloc(sizeof(u32) * num_sectors);

+	

+	num_sectors = 0;

+	

+	for (i = 0; i < b0_sectors; i++)

+	{

+		bank->sectors[num_sectors].offset = mem_layout_str9pec[i].sector_start;

+		bank->sectors[num_sectors].size = mem_layout_str9pec[i].sector_size;

+		bank->sectors[num_sectors].is_erased = -1;

+		bank->sectors[num_sectors].is_protected = 1;

+		str9xpec_info->sector_bits[num_sectors++] = mem_layout_str9pec[i].sector_bit;

+	}

+	

+	for (i = 8; i < 12; i++)

+	{

+		bank->sectors[num_sectors].offset = mem_layout_str9pec[i].sector_start;

+		bank->sectors[num_sectors].size = mem_layout_str9pec[i].sector_size;

+		bank->sectors[num_sectors].is_erased = -1;

+		bank->sectors[num_sectors].is_protected = 1;

+		str9xpec_info->sector_bits[num_sectors++] = mem_layout_str9pec[i].sector_bit;

+	}

+	

+	return ERROR_OK;

+}

+

+/* flash bank str9x <base> <size> 0 0 <target#>

+ */

+int str9xpec_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)

+{

+	str9xpec_flash_controller_t *str9xpec_info;

+	armv4_5_common_t *armv4_5 = NULL;

+	arm7_9_common_t *arm7_9 = NULL;

+	arm_jtag_t *jtag_info = NULL;

+	

+	if (argc < 6)

+	{

+		WARNING("incomplete flash_bank str9x configuration");

+		return ERROR_FLASH_BANK_INVALID;

+	}

+	

+	str9xpec_info = malloc(sizeof(str9xpec_flash_controller_t));

+	bank->driver_priv = str9xpec_info;

+	

+	if (bank->base != 0x00000000)

+	{

+		WARNING("overriding flash base address for STR91x device with 0x00000000");

+		bank->base = 0x00000000;

+	}

+

+	/* find out jtag position of flash controller

+	 * it is always after the arm966 core */

+	

+	armv4_5 = bank->target->arch_info;

+	arm7_9 = armv4_5->arch_info;

+	jtag_info = &arm7_9->jtag_info;

+	

+	str9xpec_info->chain_pos = (jtag_info->chain_pos - 1);

+	str9xpec_info->isc_enable = 0;

+	str9xpec_info->devarm = NULL;

+	

+	str9xpec_build_block_list(bank);

+	

+	/* clear option byte register */

+	buf_set_u32(str9xpec_info->options, 0, 64, 0);

+	

+	return ERROR_OK;

+}

+

+int str9xpec_blank_check(struct flash_bank_s *bank, int first, int last)

+{

+	scan_field_t field;

+	u8 status;

+	u32 chain_pos;

+	int i;

+	u8 *buffer = NULL;

+		

+	str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;

+	

+	chain_pos = str9xpec_info->chain_pos;

+	

+	if (!str9xpec_info->isc_enable) {

+		str9xpec_isc_enable( bank );

+	}

+	

+	if (!str9xpec_info->isc_enable) {

+		return ERROR_FLASH_OPERATION_FAILED;

+	}

+	

+	buffer = calloc(CEIL(64, 8), 1);

+

+	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(chain_pos, ISC_BLANK_CHECK, TAP_PI);

+	

+	field.device = chain_pos;

+	field.num_bits = 64;

+	field.out_value = buffer;

+	field.out_mask = NULL;

+	field.in_value = NULL;

+	field.in_check_value = NULL;

+	field.in_check_mask = NULL;

+	field.in_handler = NULL;

+	field.in_handler_priv = NULL;

+	

+	jtag_add_dr_scan(1, &field, TAP_RTI, NULL);

+	jtag_add_sleep(40000);

+	

+	/* read blank check result */

+	field.device = chain_pos;

+	field.num_bits = 64;

+	field.out_value = NULL;

+	field.out_mask = NULL;

+	field.in_value = buffer;

+	field.in_check_value = NULL;

+	field.in_check_mask = NULL;

+	field.in_handler = NULL;

+	field.in_handler_priv = NULL;

+	

+	jtag_add_dr_scan(1, &field, TAP_PI, NULL);

+	jtag_execute_queue();

+	

+	status = str9xpec_isc_status(chain_pos);

+	

+	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;

+}

+

+int str9xpec_protect_check(struct flash_bank_s *bank)

+{

+	u8 status;

+	int i;

+		

+	str9xpec_flash_controller_t *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;

+}

+

+int str9xpec_erase_area(struct flash_bank_s *bank, int first, int last)

+{

+	scan_field_t field;

+	u8 status;

+	u32 chain_pos;

+	int i;

+	u8 *buffer = NULL;

+	

+	str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;

+	

+	chain_pos = str9xpec_info->chain_pos;

+	

+	if (!str9xpec_info->isc_enable) {

+		str9xpec_isc_enable( bank );

+	}

+	

+	if (!str9xpec_info->isc_enable) {

+		return ISC_STATUS_ERROR;

+	}

+	

+	buffer = calloc(CEIL(64, 8), 1);

+	

+	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);

+		}

+	}

+	

+	DEBUG("ISC_ERASE");

+	

+	/* execute ISC_ERASE command */

+	str9xpec_set_instr(chain_pos, ISC_ERASE, TAP_PI);

+	

+	field.device = chain_pos;

+	field.num_bits = 64;

+	field.out_value = buffer;

+	field.out_mask = NULL;

+	field.in_value = NULL;

+	field.in_check_value = NULL;

+	field.in_check_mask = NULL;

+	field.in_handler = NULL;

+	field.in_handler_priv = NULL;

+	

+	jtag_add_dr_scan(1, &field, TAP_RTI, NULL);

+	jtag_execute_queue();

+	

+	jtag_add_sleep(10);

+	

+	/* wait for erase completion */

+	while (!((status = str9xpec_isc_status(chain_pos)) & ISC_STATUS_BUSY)) {

+		usleep(1000);

+	}

+	

+	free(buffer);

+	

+	str9xpec_isc_disable(bank);

+	

+	return status;

+}

+

+int str9xpec_erase(struct flash_bank_s *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;

+}

+

+int str9xpec_lock_device(struct flash_bank_s *bank)

+{

+	scan_field_t field;

+	u8 status;

+	u32 chain_pos;

+	str9xpec_flash_controller_t *str9xpec_info = NULL;

+	

+	str9xpec_info = bank->driver_priv;

+	chain_pos = str9xpec_info->chain_pos;

+	

+	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(chain_pos, ISC_PROGRAM_SECURITY, TAP_RTI);

+	

+	str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI);

+	

+	do {

+		field.device = chain_pos;

+		field.num_bits = 8;

+		field.out_value = NULL;

+		field.out_mask = NULL;

+		field.in_value = &status;

+		field.in_check_value = NULL;

+		field.in_check_mask = NULL;

+		field.in_handler = NULL;

+		field.in_handler_priv = NULL;

+		

+		jtag_add_dr_scan(1, &field, -1, NULL);

+		jtag_execute_queue();

+		

+	} while(!(status & ISC_STATUS_BUSY));

+	

+	str9xpec_isc_disable(bank);

+	

+	return status;

+}

+

+int str9xpec_unlock_device(struct flash_bank_s *bank)

+{

+	u8 status;

+	

+	status = str9xpec_erase_area(bank, 0, 255);

+	

+	return status;

+}

+

+int str9xpec_protect(struct flash_bank_s *bank, int set, int first, int last)

+{

+	u8 status;

+	int i;

+	

+	str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;

+	

+	status = str9xpec_read_config(bank);

+	

+	if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)

+		return ERROR_FLASH_OPERATION_FAILED;

+

+	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;

+}

+

+int str9xpec_set_address(struct flash_bank_s *bank, u8 sector)

+{

+	u32 chain_pos;

+	scan_field_t field;

+	str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;

+	

+	chain_pos = str9xpec_info->chain_pos;

+	

+	/* set flash controller address */

+	str9xpec_set_instr(chain_pos, ISC_ADDRESS_SHIFT, TAP_PI);

+	

+	field.device = chain_pos;

+	field.num_bits = 8;

+	field.out_value = &sector;

+	field.out_mask = NULL;

+	field.in_value = NULL;

+	field.in_check_value = NULL;

+	field.in_check_mask = NULL;

+	field.in_handler = NULL;

+	field.in_handler_priv = NULL;

+	

+	jtag_add_dr_scan(1, &field, -1, NULL);

+		

+	return ERROR_OK;

+}

+

+int str9xpec_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)

+{

+	str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;

+	u32 dwords_remaining = (count / 8);

+	u32 bytes_remaining = (count & 0x00000007);

+	u32 bytes_written = 0;

+	u8 status;

+	u32 check_address = offset;

+	u32 chain_pos;

+	scan_field_t field;

+	u8 *scanbuf;

+	int i;

+	u32 first_sector = 0;

+	u32 last_sector = 0;

+	

+	chain_pos = str9xpec_info->chain_pos;

+	

+	if (!str9xpec_info->isc_enable) {

+		str9xpec_isc_enable(bank);

+	}

+	

+	if (!str9xpec_info->isc_enable) {

+		return ERROR_FLASH_OPERATION_FAILED;

+	}

+	

+	if (offset & 0x7)

+	{

+		WARNING("offset 0x%x breaks required 8-byte alignment", offset);

+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;

+	}

+	

+	for (i = 0; i < bank->num_sectors; i++)

+	{

+		u32 sec_start = bank->sectors[i].offset;

+		u32 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;

+

+	DEBUG("first_sector: %i, last_sector: %i", first_sector, last_sector);

+	

+	scanbuf = calloc(CEIL(64, 8), 1);

+	

+	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(chain_pos, ISC_PROGRAM, TAP_PI);

+			

+			field.device = chain_pos;

+			field.num_bits = 64;

+			field.out_value = (buffer + bytes_written);

+			field.out_mask = NULL;

+			field.in_value = NULL;

+			field.in_check_value = NULL;

+			field.in_check_mask = NULL;

+			field.in_handler = NULL;

+			field.in_handler_priv = NULL;

+			

+			jtag_add_dr_scan(1, &field, TAP_RTI, NULL);

+			

+			/* small delay before polling */

+			jtag_add_sleep(50);

+			

+			str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI);

+			

+			do {

+				field.device = chain_pos;

+				field.num_bits = 8;

+				field.out_value = NULL;

+				field.out_mask = NULL;

+				field.in_value = scanbuf;

+				field.in_check_value = NULL;

+				field.in_check_mask = NULL;

+				field.in_handler = NULL;

+				field.in_handler_priv = NULL;

+				

+				jtag_add_dr_scan(1, &field, -1, NULL);

+				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)

+	{

+		u8 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(chain_pos, ISC_PROGRAM, TAP_PI);

+		

+		field.device = chain_pos;

+		field.num_bits = 64;

+		field.out_value = last_dword;

+		field.out_mask = NULL;

+		field.in_value = NULL;

+		field.in_check_value = NULL;

+		field.in_check_mask = NULL;

+		field.in_handler = NULL;

+		field.in_handler_priv = NULL;

+		

+		jtag_add_dr_scan(1, &field, TAP_RTI, NULL);

+		

+		/* small delay before polling */

+		jtag_add_sleep(50);

+		

+		str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI);

+		

+		do {

+			field.device = chain_pos;

+			field.num_bits = 8;

+			field.out_value = NULL;

+			field.out_mask = NULL;

+			field.in_value = scanbuf;

+			field.in_check_value = NULL;

+			field.in_check_mask = NULL;

+			field.in_handler = NULL;

+			field.in_handler_priv = NULL;

+			

+			jtag_add_dr_scan(1, &field, -1, NULL);

+			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;

+}

+

+int str9xpec_probe(struct flash_bank_s *bank)

+{

+	return ERROR_OK;

+}

+

+int str9xpec_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	flash_bank_t *bank;

+	scan_field_t field;

+	u8 *buffer = NULL;

+	u32 chain_pos;

+	u32 idcode;

+	str9xpec_flash_controller_t *str9xpec_info = NULL;

+

+	if (argc < 1)

+	{

+		return ERROR_COMMAND_SYNTAX_ERROR;

+	}

+	

+	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+	if (!bank)

+	{

+		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);

+		return ERROR_OK;

+	}

+	

+	str9xpec_info = bank->driver_priv;

+	chain_pos = str9xpec_info->chain_pos;

+	

+	buffer = calloc(CEIL(32, 8), 1);

+	

+	str9xpec_set_instr(chain_pos, ISC_IDCODE, TAP_PI);

+	

+	field.device = chain_pos;

+	field.num_bits = 32;

+	field.out_value = NULL;

+	field.out_mask = NULL;

+	field.in_value = buffer;

+	field.in_check_value = NULL;

+	field.in_check_mask = NULL;

+	field.in_handler = NULL;

+	field.in_handler_priv = NULL;

+	

+	jtag_add_dr_scan(1, &field, TAP_RTI, NULL);

+	jtag_execute_queue();

+	

+	idcode = buf_get_u32(buffer, 0, 32);

+	

+	command_print(cmd_ctx, "str9xpec part id: 0x%8.8x", idcode);

+	

+	free(buffer);

+	

+	return ERROR_OK;

+}

+

+int str9xpec_erase_check(struct flash_bank_s *bank)

+{

+	return str9xpec_blank_check(bank, 0, bank->num_sectors - 1);

+}

+

+int str9xpec_info(struct flash_bank_s *bank, char *buf, int buf_size)

+{

+	snprintf(buf, buf_size, "str9xpec flash driver info" );

+	return ERROR_OK;

+}

+

+int str9xpec_handle_flash_options_read_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	flash_bank_t *bank;

+	u8 status;

+	str9xpec_flash_controller_t *str9xpec_info = NULL;

+	

+	if (argc < 1)

+	{

+		command_print(cmd_ctx, "str9xpec options_read <bank>");

+		return ERROR_OK;	

+	}

+	

+	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+	if (!bank)

+	{

+		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);

+		return ERROR_OK;

+	}

+	

+	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;

+}

+

+int str9xpec_write_options(struct flash_bank_s *bank)

+{

+	scan_field_t field;

+	u8 status;

+	u32 chain_pos;

+	str9xpec_flash_controller_t *str9xpec_info = NULL;

+	

+	str9xpec_info = bank->driver_priv;

+	chain_pos = str9xpec_info->chain_pos;

+	

+	/* 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(chain_pos, ISC_PROGRAM, TAP_PI);

+		

+	field.device = chain_pos;

+	field.num_bits = 64;

+	field.out_value = str9xpec_info->options;

+	field.out_mask = NULL;

+	field.in_value = NULL;

+	field.in_check_value = NULL;

+	field.in_check_mask = NULL;

+	field.in_handler = NULL;

+	field.in_handler_priv = NULL;

+	

+	jtag_add_dr_scan(1, &field, TAP_RTI, NULL);

+	

+	/* small delay before polling */

+	jtag_add_sleep(50);

+	

+	str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI);

+	

+	do {

+		field.device = chain_pos;

+		field.num_bits = 8;

+		field.out_value = NULL;

+		field.out_mask = NULL;

+		field.in_value = &status;

+		field.in_check_value = NULL;

+		field.in_check_mask = NULL;

+		field.in_handler = NULL;

+		field.in_handler_priv = NULL;

+		

+		jtag_add_dr_scan(1, &field, -1, NULL);

+		jtag_execute_queue();

+		

+	} while(!(status & ISC_STATUS_BUSY));

+	

+	str9xpec_isc_disable(bank);

+	

+	return status;

+}

+

+int str9xpec_handle_flash_options_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	flash_bank_t *bank;

+	u8 status;

+	

+	if (argc < 1)

+	{

+		command_print(cmd_ctx, "str9xpec options_write <bank>");

+		return ERROR_OK;	

+	}

+	

+	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+	if (!bank)

+	{

+		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);

+		return ERROR_OK;

+	}

+	

+	status = str9xpec_write_options(bank);

+	

+	if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)

+		return ERROR_FLASH_OPERATION_FAILED;

+	

+	return ERROR_OK;

+}

+

+int str9xpec_handle_flash_options_cmap_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	flash_bank_t *bank;

+	str9xpec_flash_controller_t *str9xpec_info = NULL;

+	

+	if (argc < 2)

+	{

+		command_print(cmd_ctx, "str9xpec options_cmap <bank> <bank0|bank1>");

+		return ERROR_OK;	

+	}

+	

+	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+	if (!bank)

+	{

+		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);

+		return ERROR_OK;

+	}

+	

+	str9xpec_info = bank->driver_priv;

+	

+	if (strcmp(args[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;

+}

+

+int str9xpec_handle_flash_options_lvdthd_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	flash_bank_t *bank;

+	str9xpec_flash_controller_t *str9xpec_info = NULL;

+	

+	if (argc < 2)

+	{

+		command_print(cmd_ctx, "str9xpec options_lvdthd <bank> <2.4v|2.7v>");

+		return ERROR_OK;	

+	}

+	

+	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+	if (!bank)

+	{

+		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);

+		return ERROR_OK;

+	}

+	

+	str9xpec_info = bank->driver_priv;

+	

+	if (strcmp(args[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;

+}

+

+int str9xpec_handle_flash_options_lvdsel_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	flash_bank_t *bank;

+	str9xpec_flash_controller_t *str9xpec_info = NULL;

+	

+	if (argc < 2)

+	{

+		command_print(cmd_ctx, "str9xpec options_lvdsel <bank> <vdd|vdd_vddq>");

+		return ERROR_OK;	

+	}

+	

+	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+	if (!bank)

+	{

+		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);

+		return ERROR_OK;

+	}

+	

+	str9xpec_info = bank->driver_priv;

+	

+	if (strcmp(args[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;

+}

+

+int str9xpec_handle_flash_options_lvdwarn_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	flash_bank_t *bank;

+	str9xpec_flash_controller_t *str9xpec_info = NULL;

+	

+	if (argc < 2)

+	{

+		command_print(cmd_ctx, "str9xpec options_lvdwarn <bank> <vdd|vdd_vddq>");

+		return ERROR_OK;	

+	}

+	

+	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+	if (!bank)

+	{

+		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);

+		return ERROR_OK;

+	}

+	

+	str9xpec_info = bank->driver_priv;

+	

+	if (strcmp(args[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;

+}

+

+int str9xpec_handle_flash_lock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	u8 status;

+	flash_bank_t *bank;

+	

+	if (argc < 1)

+	{

+		command_print(cmd_ctx, "str9xpec lock <bank>");

+		return ERROR_OK;	

+	}

+	

+	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+	if (!bank)

+	{

+		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);

+		return ERROR_OK;

+	}

+	

+	status = str9xpec_lock_device(bank);

+	

+	if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)

+		return ERROR_FLASH_OPERATION_FAILED;

+	

+	return ERROR_OK;

+}

+

+int str9xpec_handle_flash_unlock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	u8 status;

+	flash_bank_t *bank;

+	

+	if (argc < 1)

+	{

+		command_print(cmd_ctx, "str9xpec unlock <bank>");

+		return ERROR_OK;	

+	}

+	

+	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+	if (!bank)

+	{

+		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);

+		return ERROR_OK;

+	}

+	

+	status = str9xpec_unlock_device(bank);

+	

+	if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)

+		return ERROR_FLASH_OPERATION_FAILED;

+	

+	return ERROR_OK;

+}

+

+int str9xpec_handle_flash_enable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	flash_bank_t *bank;

+	u32 chain_pos;

+	jtag_device_t* dev0;

+	jtag_device_t* dev2;

+	str9xpec_flash_controller_t *str9xpec_info = NULL;

+	

+	if (argc < 1)

+	{

+		command_print(cmd_ctx, "str9xpec enable_turbo <bank>");

+		return ERROR_OK;	

+	}

+	

+	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+	if (!bank)

+	{

+		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);

+		return ERROR_OK;

+	}

+	

+	str9xpec_info = bank->driver_priv;

+	

+	chain_pos = str9xpec_info->chain_pos;

+	

+	/* remove arm core from chain - enter turbo mode */

+	

+	str9xpec_set_instr(chain_pos+2, 0xD, TAP_RTI);

+	jtag_execute_queue();

+	

+	/* modify scan chain - str9 core has been removed */

+	dev0 = jtag_get_device(chain_pos);

+	str9xpec_info->devarm = jtag_get_device(chain_pos+1);

+	dev2 = jtag_get_device(chain_pos+2);

+	dev0->next = dev2;

+	jtag_num_devices--;

+	

+	return ERROR_OK;

+}

+

+int str9xpec_handle_flash_disable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)

+{

+	flash_bank_t *bank;

+	u32 chain_pos;

+	jtag_device_t* dev0;

+	str9xpec_flash_controller_t *str9xpec_info = NULL;

+	

+	if (argc < 1)

+	{

+		command_print(cmd_ctx, "str9xpec disable_turbo <bank>");

+		return ERROR_OK;	

+	}

+	

+	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));

+	if (!bank)

+	{

+		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);

+		return ERROR_OK;

+	}

+	

+	str9xpec_info = bank->driver_priv;

+	

+	chain_pos = str9xpec_info->chain_pos;

+	

+	dev0 = jtag_get_device(chain_pos);

+	

+	/* exit turbo mode via TLR */

+	str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_TLR);

+	jtag_execute_queue();

+	

+	/* restore previous scan chain */

+	if( str9xpec_info->devarm ) {

+		dev0->next = str9xpec_info->devarm;

+		jtag_num_devices++;

+		str9xpec_info->devarm = NULL;

+	}

+	

+	return ERROR_OK;

+}