15 files changed, 12192 insertions, 12377 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;

+}

diff --git a/src/helper/interpreter.c b/src/helper/interpreter.c
index c5c38b83..cfda856a 100644
--- a/src/helper/interpreter.c
+++ b/src/helper/interpreter.c
@@ -1,243 +1,243 @@
-/***************************************************************************
- *   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 "interpreter.h"
-#include "configuration.h"
-
-#include "binarybuffer.h"
-#include <stdlib.h>
-#include <string.h>
-
-var_t *variables = NULL;
-
-int handle_var_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_field_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_script_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-int interpreter_register_commands(struct command_context_s *cmd_ctx)
-{
-	register_command(cmd_ctx, NULL, "var", handle_var_command,
-		COMMAND_ANY, "allocate, display or delete variable <name> [num_fields|'del'] [size1] ...");
-	register_command(cmd_ctx, NULL, "field", handle_field_command,
-		COMMAND_ANY, "display/modify variable field <var> <field> [value|'flip']");
-	register_command(cmd_ctx, NULL, "script", handle_script_command,
-		COMMAND_ANY, "execute commands from <file>");
-
-	return ERROR_OK;
-}
-
-var_t* get_var_by_num(int num)
-{
-	int count = 0;
-	var_t *var = variables;
-
-	if (var)	
-	{
-		if (num == count)
-			return var;
-		while (var->next)
-		{
-			var = var->next;
-			count++;
-			if (num == count)
-				return var;
-		}
-	}
-	return NULL;
-}
-
-var_t* get_var_by_name(char *name)
-{
-	var_t *var = variables;
-
-	if (var)	
-	{
-		if (strcmp(var->name, name) == 0)
-			return var;
-		while (var->next)
-		{
-			var = var->next;
-			if (strcmp(var->name, name) == 0)
-				return var;
-		}
-	}
-	return NULL;
-}
-
-var_t* get_var_by_namenum(char *namenum)
-{
-	if ((namenum[0] >= '0') && (namenum[0] <= '9'))
-		return get_var_by_num(strtol(namenum, NULL, 0));
-	else
-		return get_var_by_name(namenum);
-	
-}
-
-int field_le_to_host(u8 *buffer, void *priv, struct scan_field_s *dummy)
-{
-	var_field_t *field = priv;
-	field->value = buf_get_u32(buffer, 0, field->num_bits);
-
-	return ERROR_OK;
-}
-
-int handle_var_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	var_t **last_var_p = &variables;
-	int i;
-
-	if (argc >= 2)
-	{
-		while (*last_var_p)
-		{
-			if (strcmp((*last_var_p)->name, args[0]) == 0)
-			{
-				if (strcmp(args[1], "del") == 0)
-				{
-					var_t *next = (*last_var_p)->next;
-					free ((*last_var_p)->fields);
-					free (*last_var_p);
-					*last_var_p = next;
-					command_print(cmd_ctx, "variable %s deleted", args[0]);
-				}
-				else
-					command_print(cmd_ctx, "variable of that name already exists");
-				return ERROR_OK;
-			}
-			last_var_p = &((*last_var_p)->next);
-		}
-
-		if ((args[0][0] >= '0') && (args[0][0] <= '9'))
-		{
-			command_print(cmd_ctx, "invalid name specified (first character may not be a number)");
-			return ERROR_OK;
-		}
-
-		*last_var_p = malloc(sizeof(var_t));
-		(*last_var_p)->name = strdup(args[0]);
-		(*last_var_p)->num_fields = argc - 1;
-		(*last_var_p)->next = NULL;
-
-		(*last_var_p)->fields = malloc(sizeof(var_field_t) * (*last_var_p)->num_fields);
-		for (i = 0; i < (*last_var_p)->num_fields; i++)
-		{
-			(*last_var_p)->fields[i].num_bits = strtol(args[1+i], NULL, 0);
-			(*last_var_p)->fields[i].value = 0x0;
-		}
-		return ERROR_OK;
-	}
-
-	if (argc == 1)
-	{
-		var_t *var = get_var_by_namenum(args[0]);
-		if (var)
-		{
-			int i;
-			command_print(cmd_ctx, "%s (%i fields):", var->name, var->num_fields);
-			for (i = 0; i < (var->num_fields); i++)
-			{
-				command_print(cmd_ctx, "0x%x (/%i)", var->fields[i].value, var->fields[i].num_bits);
-			}
-		}
-		else
-		{
-			command_print(cmd_ctx, "variable %s doesn't exist", args[0]);
-		}
-	}
-
-	if (argc == 0)
-	{
-		var_t *var = variables;
-		int count = 0;
-		while (var)
-		{
-			command_print(cmd_ctx, "%i: %s (%i fields)", count, var->name, var->num_fields);
-			var = var->next;
-			count++;
-		}
-	}
-
-	return ERROR_OK;
-}
-
-int handle_field_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-
-	if (argc < 2)
-		command_print(cmd_ctx, "usage: field <var> <field> [value|'flip']");
-
-	if (argc >= 2)
-	{
-		var_t *var = get_var_by_namenum(args[0]);
-		int field_num = strtol(args[1], NULL, 0);
-		if (!var)
-		{
-			command_print(cmd_ctx, "variable %s doesn't exist", args[0]);
-			return ERROR_OK;
-		}
-		if (field_num >= var->num_fields)
-			command_print(cmd_ctx, "variable field %i is out of bounds (max. %i)", field_num, var->num_fields - 1);
-		if ((var) && (field_num < var->num_fields))
-		{
-			if (argc > 2)
-			{
-				if (strcmp(args[2], "flip") == 0)
-					var->fields[field_num].value = flip_u32(var->fields[field_num].value, var->fields[field_num].num_bits);
-				else
-					var->fields[field_num].value = strtoul(args[2], NULL, 0);
-			}
-
-			command_print(cmd_ctx, "%s(%i): 0x%x (/%i)", var->name, field_num, var->fields[field_num].value, var->fields[field_num].num_bits);
-		}
-	}
-
-	return ERROR_OK;
-}
-
-int handle_script_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	FILE *script_file;
-	int echo;
-
-	if (argc != 1)
-		command_print(cmd_ctx, "usage: script <file>");
-
-	script_file = open_file_from_path(cmd_ctx, args[0], "r");
-
-	if (!script_file)
-	{
-		command_print(cmd_ctx, "couldn't open script file %s", args[0]);
-		return ERROR_OK;
-	}
-
-	echo = cmd_ctx->echo;
-	cmd_ctx->echo = 1;
-	
-	command_run_file(cmd_ctx, script_file, cmd_ctx->mode);
-	
-	cmd_ctx->echo = echo;
-	
-	fclose(script_file);
-
-	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 "interpreter.h"

+#include "configuration.h"

+

+#include "binarybuffer.h"

+#include <stdlib.h>

+#include <string.h>

+

+var_t *variables = NULL;

+

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

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

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

+

+int interpreter_register_commands(struct command_context_s *cmd_ctx)

+{

+	register_command(cmd_ctx, NULL, "var", handle_var_command,

+		COMMAND_ANY, "allocate, display or delete variable <name> [num_fields|'del'] [size1] ...");

+	register_command(cmd_ctx, NULL, "field", handle_field_command,

+		COMMAND_ANY, "display/modify variable field <var> <field> [value|'flip']");

+	register_command(cmd_ctx, NULL, "script", handle_script_command,

+		COMMAND_ANY, "execute commands from <file>");

+

+	return ERROR_OK;

+}

+

+var_t* get_var_by_num(int num)

+{

+	int count = 0;

+	var_t *var = variables;

+

+	if (var)	

+	{

+		if (num == count)

+			return var;

+		while (var->next)

+		{

+			var = var->next;

+			count++;

+			if (num == count)

+				return var;

+		}

+	}

+	return NULL;

+}

+

+var_t* get_var_by_name(char *name)

+{

+	var_t *var = variables;

+

+	if (var)	

+	{

+		if (strcmp(var->name, name) == 0)

+			return var;

+		while (var->next)

+		{

+			var = var->next;

+			if (strcmp(var->name, name) == 0)

+				return var;

+		}

+	}

+	return NULL;

+}

+

+var_t* get_var_by_namenum(char *namenum)

+{

+	if ((namenum[0] >= '0') && (namenum[0] <= '9'))

+		return get_var_by_num(strtol(namenum, NULL, 0));

+	else

+		return get_var_by_name(namenum);

+	

+}

+

+int field_le_to_host(u8 *buffer, void *priv, struct scan_field_s *dummy)

+{

+	var_field_t *field = priv;

+	field->value = buf_get_u32(buffer, 0, field->num_bits);

+

+	return ERROR_OK;

+}

+

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

+{

+	var_t **last_var_p = &variables;

+	int i;

+

+	if (argc >= 2)

+	{

+		while (*last_var_p)

+		{

+			if (strcmp((*last_var_p)->name, args[0]) == 0)

+			{

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

+				{

+					var_t *next = (*last_var_p)->next;

+					free ((*last_var_p)->fields);

+					free (*last_var_p);

+					*last_var_p = next;

+					command_print(cmd_ctx, "variable %s deleted", args[0]);

+				}

+				else

+					command_print(cmd_ctx, "variable of that name already exists");

+				return ERROR_OK;

+			}

+			last_var_p = &((*last_var_p)->next);

+		}

+

+		if ((args[0][0] >= '0') && (args[0][0] <= '9'))

+		{

+			command_print(cmd_ctx, "invalid name specified (first character may not be a number)");

+			return ERROR_OK;

+		}

+

+		*last_var_p = malloc(sizeof(var_t));

+		(*last_var_p)->name = strdup(args[0]);

+		(*last_var_p)->num_fields = argc - 1;

+		(*last_var_p)->next = NULL;

+

+		(*last_var_p)->fields = malloc(sizeof(var_field_t) * (*last_var_p)->num_fields);

+		for (i = 0; i < (*last_var_p)->num_fields; i++)

+		{

+			(*last_var_p)->fields[i].num_bits = strtol(args[1+i], NULL, 0);

+			(*last_var_p)->fields[i].value = 0x0;

+		}

+		return ERROR_OK;

+	}

+

+	if (argc == 1)

+	{

+		var_t *var = get_var_by_namenum(args[0]);

+		if (var)

+		{

+			int i;

+			command_print(cmd_ctx, "%s (%i fields):", var->name, var->num_fields);

+			for (i = 0; i < (var->num_fields); i++)

+			{

+				command_print(cmd_ctx, "0x%x (/%i)", var->fields[i].value, var->fields[i].num_bits);

+			}

+		}

+		else

+		{

+			command_print(cmd_ctx, "variable %s doesn't exist", args[0]);

+		}

+	}

+

+	if (argc == 0)

+	{

+		var_t *var = variables;

+		int count = 0;

+		while (var)

+		{

+			command_print(cmd_ctx, "%i: %s (%i fields)", count, var->name, var->num_fields);

+			var = var->next;

+			count++;

+		}

+	}

+

+	return ERROR_OK;

+}

+

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

+{

+

+	if (argc < 2)

+		return ERROR_COMMAND_SYNTAX_ERROR;

+

+	if (argc >= 2)

+	{

+		var_t *var = get_var_by_namenum(args[0]);

+		int field_num = strtol(args[1], NULL, 0);

+		if (!var)

+		{

+			command_print(cmd_ctx, "variable %s doesn't exist", args[0]);

+			return ERROR_OK;

+		}

+		if (field_num >= var->num_fields)

+			command_print(cmd_ctx, "variable field %i is out of bounds (max. %i)", field_num, var->num_fields - 1);

+		if ((var) && (field_num < var->num_fields))

+		{

+			if (argc > 2)

+			{

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

+					var->fields[field_num].value = flip_u32(var->fields[field_num].value, var->fields[field_num].num_bits);

+				else

+					var->fields[field_num].value = strtoul(args[2], NULL, 0);

+			}

+

+			command_print(cmd_ctx, "%s(%i): 0x%x (/%i)", var->name, field_num, var->fields[field_num].value, var->fields[field_num].num_bits);

+		}

+	}

+

+	return ERROR_OK;

+}

+

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

+{

+	FILE *script_file;

+	int echo;

+

+	if (argc != 1)

+		return ERROR_COMMAND_SYNTAX_ERROR;

+

+	script_file = open_file_from_path(cmd_ctx, args[0], "r");

+

+	if (!script_file)

+	{

+		command_print(cmd_ctx, "couldn't open script file %s", args[0]);

+		return ERROR_OK;

+	}

+

+	echo = cmd_ctx->echo;

+	cmd_ctx->echo = 1;

+	

+	command_run_file(cmd_ctx, script_file, cmd_ctx->mode);

+	

+	cmd_ctx->echo = echo;

+	

+	fclose(script_file);

+

+	return ERROR_OK;

+}

diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index c2651e88..b4a79820 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -1,2114 +1,2107 @@
-/***************************************************************************
- *   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 "gdb_server.h"
-
-#include "server.h"
-#include "log.h"
-#include "binarybuffer.h"
-#include "jtag.h"
-#include "breakpoints.h"
-#include "flash.h"
-#include "target_request.h"
-#include "configuration.h"
-
-#include <string.h>
-#include <errno.h>
-#include <unistd.h>
-#include <stdlib.h>
-
-#if 0
-#define _DEBUG_GDB_IO_
-#endif
-
-static unsigned short gdb_port;
-static const char *DIGITS = "0123456789abcdef";
-
-static void gdb_log_callback(void *priv, const char *file, int line, 
-		const char *function, const char *format, va_list args);
-
-enum gdb_detach_mode
-{
-	GDB_DETACH_RESUME,
-	GDB_DETACH_RESET,
-	GDB_DETACH_HALT,
-	GDB_DETACH_NOTHING
-};
-
-/* target behaviour on gdb detach */
-enum gdb_detach_mode detach_mode = GDB_DETACH_RESUME;
-
-/* set if we are sending a memory map to gdb
- * via qXfer:memory-map:read packet */
-int gdb_use_memory_map = 0;
-int gdb_flash_program = 0;
-
-/* if set, data aborts cause an error to be reported in memory read packets
- * see the code in gdb_read_memory_packet() for further explanations */
-int gdb_report_data_abort = 0;
-
-int gdb_last_signal(target_t *target)
-{
-	switch (target->debug_reason)
-	{
-		case DBG_REASON_DBGRQ:
-			return 0x2; /* SIGINT */
-		case DBG_REASON_BREAKPOINT:
-		case DBG_REASON_WATCHPOINT:
-		case DBG_REASON_WPTANDBKPT:
-			return 0x05; /* SIGTRAP */
-		case DBG_REASON_SINGLESTEP:
-			return 0x05; /* SIGTRAP */
-		case DBG_REASON_NOTHALTED:
-			return 0x0; /* no signal... shouldn't happen */
-		default:
-			ERROR("BUG: undefined debug reason");
-			exit(-1);
-	}
-}
-
-int gdb_get_char(connection_t *connection, int* next_char)
-{
-	gdb_connection_t *gdb_con = connection->priv;
-
-#ifdef _DEBUG_GDB_IO_
-	char *debug_buffer;
-#endif
-
-	if (gdb_con->buf_cnt-- > 0)
-	{
-		*next_char = *(gdb_con->buf_p++);
-		if (gdb_con->buf_cnt > 0)
-			connection->input_pending = 1;
-		else
-			connection->input_pending = 0;
-
-#ifdef _DEBUG_GDB_IO_
-		DEBUG("returned char '%c' (0x%2.2x)", *next_char, *next_char);
-#endif
-
-		return ERROR_OK;
-	}
-
-	for (;;)
-	{
-#ifndef _WIN32
-		/* a non-blocking socket will block if there is 0 bytes available on the socket,
-		 * but return with as many bytes as are available immediately
-		 */
-		struct timeval tv;
-		fd_set read_fds;
-		
-		FD_ZERO(&read_fds);
-		FD_SET(connection->fd, &read_fds);
-		
-		tv.tv_sec = 1;
-		tv.tv_usec = 0;
-		if (select(connection->fd + 1, &read_fds, NULL, NULL, &tv) == 0)
-		{
-			/* This can typically be because a "monitor" command took too long
-			 * before printing any progress messages
-			 */
-			return ERROR_GDB_TIMEOUT; 
-		}
-#endif
-		gdb_con->buf_cnt = read_socket(connection->fd, gdb_con->buffer, GDB_BUFFER_SIZE);
-		if (gdb_con->buf_cnt > 0)
-		{
-			break;
-		}
-		if (gdb_con->buf_cnt == 0)
-		{
-			gdb_con->closed = 1;
-			return ERROR_SERVER_REMOTE_CLOSED;
-		}
-
-#ifdef _WIN32
-		errno = WSAGetLastError();
-
-		switch(errno)
-		{
-			case WSAEWOULDBLOCK:
-				usleep(1000);
-				break;
-			case WSAECONNABORTED:
-				return ERROR_SERVER_REMOTE_CLOSED;
-			case WSAECONNRESET:
-				return ERROR_SERVER_REMOTE_CLOSED;
-			default:
-				ERROR("read: %d", errno);
-				exit(-1);
-		}
-#else
-		switch(errno)
-		{
-			case EAGAIN:
-				usleep(1000);
-				break;
-			case ECONNABORTED:
-				return ERROR_SERVER_REMOTE_CLOSED;
-			case ECONNRESET:
-				return ERROR_SERVER_REMOTE_CLOSED;
-			default:
-				ERROR("read: %s", strerror(errno));
-				return ERROR_SERVER_REMOTE_CLOSED;
-		}
-#endif
-	}
-
-#ifdef _DEBUG_GDB_IO_
-	debug_buffer = malloc(gdb_con->buf_cnt + 1);
-	memcpy(debug_buffer, gdb_con->buffer, gdb_con->buf_cnt);
-	debug_buffer[gdb_con->buf_cnt] = 0;
-	DEBUG("received '%s'", debug_buffer);
-	free(debug_buffer);
-#endif
-
-	gdb_con->buf_p = gdb_con->buffer;
-	gdb_con->buf_cnt--;
-	*next_char = *(gdb_con->buf_p++);
-	if (gdb_con->buf_cnt > 0)
-		connection->input_pending = 1;
-	else
-		connection->input_pending = 0;	
-#ifdef _DEBUG_GDB_IO_
-	DEBUG("returned char '%c' (0x%2.2x)", *next_char, *next_char);
-#endif
-
-	return ERROR_OK;
-}
-
-int gdb_putback_char(connection_t *connection, int last_char)
-{
-	gdb_connection_t *gdb_con = connection->priv;
-
-	if (gdb_con->buf_p > gdb_con->buffer)
-	{
-		*(--gdb_con->buf_p) = last_char;
-		gdb_con->buf_cnt++;
-	}
-	else
-	{
-		ERROR("BUG: couldn't put character back"); 	
-	}
-
-	return ERROR_OK;
-}
-
-/* The only way we can detect that the socket is closed is the first time
- * we write to it, we will fail. Subsequent write operations will
- * succeed. Shudder! */
-int gdb_write(connection_t *connection, void *data, int len)
-{
-	gdb_connection_t *gdb_con = connection->priv;
-	if (gdb_con->closed)
-		return ERROR_SERVER_REMOTE_CLOSED;
-
-	if (write_socket(connection->fd, data, len) == len)
-	{
-		return ERROR_OK;
-	}
-	gdb_con->closed = 1;
-	return ERROR_SERVER_REMOTE_CLOSED;
-}
-
-int gdb_put_packet_inner(connection_t *connection, char *buffer, int len)
-{
-	int i;
-	unsigned char my_checksum = 0;
-#ifdef _DEBUG_GDB_IO_
-	char *debug_buffer;
-#endif
-	int reply;
-	int retval;
-	gdb_connection_t *gdb_con = connection->priv;
-
-	for (i = 0; i < len; i++)
-		my_checksum += buffer[i];
-
-	while (1)
-	{
-#ifdef _DEBUG_GDB_IO_
-		debug_buffer = malloc(len + 1);
-		memcpy(debug_buffer, buffer, len);
-		debug_buffer[len] = 0;
-		DEBUG("sending packet '$%s#%2.2x'", debug_buffer, my_checksum);
-		free(debug_buffer);
-#endif
-#if 0
-		char checksum[3];
-		gdb_write(connection, "$", 1);
-		if (len > 0)
-			gdb_write(connection, buffer, len);
-		gdb_write(connection, "#", 1);
-		
-		snprintf(checksum, 3, "%2.2x", my_checksum);
-		
-		gdb_write(connection, checksum, 2);
-#else
-		void *allocated = NULL;
-		char stackAlloc[1024];
-		char *t = stackAlloc;
-		int totalLen = 1 + len + 1 + 2;
-		if (totalLen > sizeof(stackAlloc))
-		{
-			allocated = malloc(totalLen);
-			t = allocated;
-			if (allocated == NULL)
-			{
-				ERROR("Ran out of memory trying to reply packet %d\n", totalLen);
-				exit(-1);
-			}
-		}
-		t[0] = '$';
-		memcpy(t + 1, buffer, len);
-		t[1 + len] = '#';
-		t[1 + len + 1] = DIGITS[(my_checksum >> 4) & 0xf];
-		t[1 + len + 2] = DIGITS[my_checksum & 0xf];
-		
-		gdb_write(connection, t, totalLen);
-		
-		if (allocated)
-		{
-			free(allocated);
-		}
-#endif
-		if ((retval = gdb_get_char(connection, &reply)) != ERROR_OK)
-			return retval;
-
-		if (reply == '+')
-			break;
-		else if (reply == '-')
-		{
-			/* Stop sending output packets for now */
-			log_setCallback(NULL, NULL);
-			WARNING("negative reply, retrying");
-		}
-		else if (reply == 0x3)
-		{
-			gdb_con->ctrl_c = 1;
-			if ((retval = gdb_get_char(connection, &reply)) != ERROR_OK)
-				return retval;
-			if (reply == '+')
-				break;
-			else if (reply == '-')
-			{
-				/* Stop sending output packets for now */
-				log_setCallback(NULL, NULL);
-				WARNING("negative reply, retrying");
-			}
-			else
-			{
-				ERROR("unknown character 0x%2.2x in reply, dropping connection", reply);
-				return ERROR_SERVER_REMOTE_CLOSED;
-			}
-		}
-		else
-		{
-			ERROR("unknown character 0x%2.2x in reply, dropping connection", reply);
-			return ERROR_SERVER_REMOTE_CLOSED;
-		}
-	}
-	if (gdb_con->closed)
-		return ERROR_SERVER_REMOTE_CLOSED;
-
-	return ERROR_OK;
-}
-
-int gdb_put_packet(connection_t *connection, char *buffer, int len)
-{
-	gdb_connection_t *gdb_con = connection->priv;
-	gdb_con->busy = 1;
-	int retval = gdb_put_packet_inner(connection, buffer, len);
-	gdb_con->busy = 0;
-	return retval;
-}
-
-int gdb_get_packet_inner(connection_t *connection, char *buffer, int *len)
-{
-	int character;
-	int count = 0;
-	int retval;
-	char checksum[3];
-	unsigned char my_checksum = 0;
-	gdb_connection_t *gdb_con = connection->priv;
-
-	while (1)
-	{
-		do
-		{
-			if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
-				return retval;
-
-#ifdef _DEBUG_GDB_IO_
-			DEBUG("character: '%c'", character);
-#endif
-
-			switch (character)
-			{
-				case '$':
-					break;
-				case '+':
-					WARNING("acknowledgment received, but no packet pending");
-					break;
-				case '-':
-					WARNING("negative acknowledgment, but no packet pending");
-					break;
-				case 0x3:
-					gdb_con->ctrl_c = 1;
-					*len = 0;
-					return ERROR_OK;
-				default:
-					WARNING("ignoring character 0x%x", character);
-					break;
-			}
-		} while (character != '$');
-
-		my_checksum = 0;
-		
-		count = 0;
-		gdb_connection_t *gdb_con = connection->priv;
-		for (;;)
-		{
-			/* The common case is that we have an entire packet with no escape chars.
-			 * We need to leave at least 2 bytes in the buffer to have
-			 * gdb_get_char() update various bits and bobs correctly. 
-			 */
-			if ((gdb_con->buf_cnt > 2) && ((gdb_con->buf_cnt+count) < *len))
-			{
-				/* The compiler will struggle a bit with constant propagation and
-				 * aliasing, so we help it by showing that these values do not
-				 * change inside the loop 
-				 */ 
-				int i;
-				char *buf = gdb_con->buf_p;
-				int run = gdb_con->buf_cnt - 2;
-				i = 0;
-				int done = 0;
-				while (i < run)
-				{
-					character = *buf++;
-					i++;
-					if (character == '#')
-					{
-						/* Danger! character can be '#' when esc is 
-						 * used so we need an explicit boolean for done here.
-						 */
-						done = 1;
-						break;
-					}
-					
-					if (character == '}')
-					{
-						/* data transmitted in binary mode (X packet)
-						 * uses 0x7d as escape character */
-						my_checksum += character & 0xff;
-						character = *buf++;
-						i++;
-						my_checksum += character & 0xff;
-						buffer[count++] = (character ^ 0x20) & 0xff;
-					} else
-					{
-						my_checksum += character & 0xff;
-						buffer[count++] = character & 0xff;
-					}
-				}
-				gdb_con->buf_p += i;
-				gdb_con->buf_cnt -= i;
-				if (done) 
-					break;
-			} 
-			if (count > *len)
-			{
-				ERROR("packet buffer too small");
-				return ERROR_GDB_BUFFER_TOO_SMALL;
-			}
-			
-			if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
-				return retval;
-
-			if (character == '#')
-				break;
-
-			if (character == '}')
-			{
-				/* data transmitted in binary mode (X packet)
-				 * uses 0x7d as escape character */
-				my_checksum += character & 0xff;
-				if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
-					return retval;
-				my_checksum += character & 0xff;
-				buffer[count++] = (character ^ 0x20) & 0xff;
-			}
-			else
-			{
-				my_checksum += character & 0xff;
-				buffer[count++] = character & 0xff;
-			}
-
-		}
-
-		*len = count;
-
-		if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
-			return retval;
-		checksum[0] = character;
-		if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
-			return retval;
-		checksum[1] = character;
-		checksum[2] = 0;
-
-		if (my_checksum == strtoul(checksum, NULL, 16))
-		{
-			gdb_write(connection, "+", 1);
-			break;
-		}
-
-		WARNING("checksum error, requesting retransmission");
-		gdb_write(connection, "-", 1);
-	}
-	if (gdb_con->closed)
-		return ERROR_SERVER_REMOTE_CLOSED;
-
-	return ERROR_OK;
-}
-
-int gdb_get_packet(connection_t *connection, char *buffer, int *len)
-{
-	gdb_connection_t *gdb_con = connection->priv;
-	gdb_con->busy = 1;
-	int retval = gdb_get_packet_inner(connection, buffer, len);
-	gdb_con->busy = 0;
-	return retval;
-}
-	
-int gdb_output_con(connection_t *connection, char* line)
-{
-	char *hex_buffer;
-	int i, bin_size;
-
-	bin_size = strlen(line);
-
-	hex_buffer = malloc(bin_size*2 + 4);
-
-	hex_buffer[0] = 'O';
-	for (i=0; i<bin_size; i++)
-		snprintf(hex_buffer + 1 + i*2, 3, "%2.2x", line[i]);
-	hex_buffer[bin_size*2+1] = '0';
-	hex_buffer[bin_size*2+2] = 'a';
-	hex_buffer[bin_size*2+3] = 0x0;
-
-	gdb_put_packet(connection, hex_buffer, bin_size*2 + 3);
-
-	free(hex_buffer);
-	return ERROR_OK;
-}
-
-int gdb_output(struct command_context_s *context, char* line)
-{
-	/* this will be dumped to the log and also sent as an O packet if possible */
-	USER(line); 
-	return ERROR_OK;
-}
-
-int gdb_program_handler(struct target_s *target, enum target_event event, void *priv)
-{
-	FILE *script;
-	struct command_context_s *cmd_ctx = priv;
-	
-	if (target->gdb_program_script)
-	{
-		script = open_file_from_path(cmd_ctx, target->gdb_program_script, "r");
-		if (!script)
-		{
-			ERROR("couldn't open script file %s", target->gdb_program_script);
-				return ERROR_OK;
-		}
-
-		INFO("executing gdb_program script '%s'", target->gdb_program_script);
-		command_run_file(cmd_ctx, script, COMMAND_EXEC);
-		fclose(script);
-		
-		jtag_execute_queue();
-	}
-	
-	return ERROR_OK;
-}
-
-int gdb_target_callback_event_handler(struct target_s *target, enum target_event event, void *priv)
-{
-	connection_t *connection = priv;
-	gdb_connection_t *gdb_connection = connection->priv;
-	char sig_reply[4];
-	int signal;
-
-	switch (event)
-	{
-		case TARGET_EVENT_HALTED:
-			/* In the GDB protocol when we are stepping or coninuing execution,
-			 * we have a lingering reply. Upon receiving a halted event 
-			 * when we have that lingering packet, we reply to the original
-			 * step or continue packet.
-			 * 
-			 * Executing monitor commands can bring the target in and
-			 * out of the running state so we'll see lots of TARGET_EVENT_XXX
-			 * that are to be ignored.
-			 */
-			if (gdb_connection->frontend_state == TARGET_RUNNING)
-			{
-				/* stop forwarding log packets! */
-				log_setCallback(NULL, NULL);
-				
-				if (gdb_connection->ctrl_c)
-				{
-					signal = 0x2;
-					gdb_connection->ctrl_c = 0;
-				}
-				else
-				{
-					signal = gdb_last_signal(target);
-				}
-
-				snprintf(sig_reply, 4, "T%2.2x", signal);
-				gdb_put_packet(connection, sig_reply, 3);
-				gdb_connection->frontend_state = TARGET_HALTED;
-			}
-			break;
-		case TARGET_EVENT_GDB_PROGRAM:
-			gdb_program_handler(target, event, connection->cmd_ctx);
-			break;
-		default:
-			break;
-	}
-
-	return ERROR_OK;
-}
-
-int gdb_new_connection(connection_t *connection)
-{
-	gdb_connection_t *gdb_connection = malloc(sizeof(gdb_connection_t));
-	gdb_service_t *gdb_service = connection->service->priv;
-	int retval;
-	int initial_ack;
-
-	connection->priv = gdb_connection;
-
-	/* initialize gdb connection information */
-	gdb_connection->buf_p = gdb_connection->buffer;
-	gdb_connection->buf_cnt = 0;
-	gdb_connection->ctrl_c = 0;
-	gdb_connection->frontend_state = TARGET_HALTED;
-	gdb_connection->vflash_image = NULL;
-	gdb_connection->closed = 0;
-	gdb_connection->busy = 0;
-	
-	/* output goes through gdb connection */
-	command_set_output_handler(connection->cmd_ctx, gdb_output, connection);
-
-	/* register callback to be informed about target events */
-	target_register_event_callback(gdb_target_callback_event_handler, connection);	
-
-	/* a gdb session just attached, put the target in halt mode */
-	if (((retval = gdb_service->target->type->halt(gdb_service->target)) != ERROR_OK) &&
-			(retval != ERROR_TARGET_ALREADY_HALTED))
-	{
-		ERROR("error(%d) when trying to halt target, falling back to \"reset halt\"", retval);
-		command_run_line(connection->cmd_ctx, "reset halt");
-	}
-
-	/* This will time out after 1 second */
-	command_run_line(connection->cmd_ctx, "wait_halt 1");
-
-	/* remove the initial ACK from the incoming buffer */
-	if ((retval = gdb_get_char(connection, &initial_ack)) != ERROR_OK)
-		return retval;
-
-	if (initial_ack != '+')
-		gdb_putback_char(connection, initial_ack);
-
-	return ERROR_OK;
-}
-
-int gdb_connection_closed(connection_t *connection)
-{
-	gdb_service_t *gdb_service = connection->service->priv;
-	gdb_connection_t *gdb_connection = connection->priv;
-
-	/* see if an image built with vFlash commands is left */
-	if (gdb_connection->vflash_image)
-	{
-		image_close(gdb_connection->vflash_image);
-		free(gdb_connection->vflash_image);
-		gdb_connection->vflash_image = NULL;
-	}
-
-	/* if this connection registered a debug-message receiver delete it */
-	delete_debug_msg_receiver(connection->cmd_ctx, gdb_service->target);
-	
-	if (connection->priv)
-	{
-		free(connection->priv);
-		connection->priv = NULL;
-	}
-	else
-	{
-		ERROR("BUG: connection->priv == NULL");
-	}
-
-	target_unregister_event_callback(gdb_target_callback_event_handler, connection);
-	log_setCallback(NULL, NULL);
-
-	return ERROR_OK;
-}
-
-void gdb_send_error(connection_t *connection, u8 the_error)
-{
-	char err[4];
-	snprintf(err, 4, "E%2.2X", the_error );
-	gdb_put_packet(connection, err, 3);
-}
-
-int gdb_last_signal_packet(connection_t *connection, target_t *target, char* packet, int packet_size)
-{
-	char sig_reply[4];
-	int signal;
-
-	signal = gdb_last_signal(target);
-
-	snprintf(sig_reply, 4, "S%2.2x", signal);
-	gdb_put_packet(connection, sig_reply, 3);
-
-	return ERROR_OK;
-}
-
-/* Convert register to string of bits. NB! The # of bits in the
- * register might be non-divisible by 8(a byte), in which
- * case an entire byte is shown. */
-void gdb_str_to_target(target_t *target, char *tstr, reg_t *reg)
-{
-	int i;
-
-	u8 *buf;
-	int buf_len;
-	buf = reg->value;
-	buf_len = CEIL(reg->size, 8); 
-
-	if (target->endianness == TARGET_LITTLE_ENDIAN)
-	{
-		for (i = 0; i < buf_len; i++)
-		{
-			tstr[i*2]   = DIGITS[(buf[i]>>4) & 0xf];
-			tstr[i*2+1] = DIGITS[buf[i]&0xf];
-		}
-	}
-	else
-	{
-		for (i = 0; i < buf_len; i++)
-		{
-			tstr[(buf_len-1-i)*2]   = DIGITS[(buf[i]>>4)&0xf];
-			tstr[(buf_len-1-i)*2+1] = DIGITS[buf[i]&0xf];
-		}
-	}	
-}
-
-void gdb_target_to_str(target_t *target, char *tstr, char *str)
-{
-	int str_len = strlen(tstr);
-	int i;
-
-	if (str_len % 2)
-	{
-		ERROR("BUG: gdb value with uneven number of characters encountered");
-		exit(-1);
-	}
-
-	if (target->endianness == TARGET_LITTLE_ENDIAN)
-	{
-		for (i = 0; i < str_len; i+=2)
-		{
-			str[str_len - i - 1] = tstr[i + 1];
-			str[str_len - i - 2] = tstr[i];
-		}
-	}
-	else
-	{
-		for (i = 0; i < str_len; i++)
-		{
-			str[i] = tstr[i];
-		}
-	}	
-}
-
-int gdb_get_registers_packet(connection_t *connection, target_t *target, char* packet, int packet_size)
-{
-	reg_t **reg_list;
-	int reg_list_size;
-	int retval;
-	int reg_packet_size = 0;
-	char *reg_packet;
-	char *reg_packet_p;
-	int i;
-
-#ifdef _DEBUG_GDB_IO_
-	DEBUG("-");
-#endif
-
-	if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
-	{
-		switch (retval)
-		{
-			case ERROR_TARGET_NOT_HALTED:
-				ERROR("gdb requested registers but we're not halted, dropping connection");
-				return ERROR_SERVER_REMOTE_CLOSED;
-			default:
-				/* this is a bug condition - get_gdb_reg_list() may not return any other error */
-				ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
-				exit(-1);
-		}
-	}
-
-	for (i = 0; i < reg_list_size; i++)
-	{
-		reg_packet_size += reg_list[i]->size;
-	}
-
-	reg_packet = malloc(CEIL(reg_packet_size, 8) * 2);
-	reg_packet_p = reg_packet;
-
-	for (i = 0; i < reg_list_size; i++)
-	{
-		gdb_str_to_target(target, reg_packet_p, reg_list[i]);
-		reg_packet_p += CEIL(reg_list[i]->size, 8) * 2;
-	}
-
-#ifdef _DEBUG_GDB_IO_
-	{
-		char *reg_packet_p;
-		reg_packet_p = strndup(reg_packet, CEIL(reg_packet_size, 8) * 2);
-		DEBUG("reg_packet: %s", reg_packet_p);
-		free(reg_packet_p);
-	}
-#endif
-
-	gdb_put_packet(connection, reg_packet, CEIL(reg_packet_size, 8) * 2);
-	free(reg_packet);
-
-	free(reg_list);
-
-	return ERROR_OK;
-}
-
-int gdb_set_registers_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
-{
-	int i;
-	reg_t **reg_list;
-	int reg_list_size;
-	int retval;
-	char *packet_p;
-
-#ifdef _DEBUG_GDB_IO_
-	DEBUG("-");
-#endif
-
-	/* skip command character */
-	packet++;
-	packet_size--;
-
-	if (packet_size % 2)
-	{
-		WARNING("GDB set_registers packet with uneven characters received, dropping connection");
-		return ERROR_SERVER_REMOTE_CLOSED;
-	}
-
-	if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
-	{
-		switch (retval)
-		{
-			case ERROR_TARGET_NOT_HALTED:
-				ERROR("gdb tried to registers but we're not halted, dropping connection");
-				return ERROR_SERVER_REMOTE_CLOSED;
-			default:
-				/* this is a bug condition - get_gdb_reg_list() may not return any other error */
-				ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
-				exit(-1);
-		}
-	}
-
-	packet_p = packet;
-	for (i = 0; i < reg_list_size; i++)
-	{
-		u8 *bin_buf;
-		char *hex_buf;
-		reg_arch_type_t *arch_type;
-
-		/* convert from GDB-string (target-endian) to hex-string (big-endian) */
-		hex_buf = malloc(CEIL(reg_list[i]->size, 8) * 2);
-		gdb_target_to_str(target, packet_p, hex_buf);
-
-		/* convert hex-string to binary buffer */
-		bin_buf = malloc(CEIL(reg_list[i]->size, 8));
-		str_to_buf(hex_buf, CEIL(reg_list[i]->size, 8) * 2, bin_buf, reg_list[i]->size, 16);
-
-		/* get register arch_type, and call set method */	
-		arch_type = register_get_arch_type(reg_list[i]->arch_type);
-		if (arch_type == NULL)
-		{
-			ERROR("BUG: encountered unregistered arch type");
-			exit(-1);
-		}
-		arch_type->set(reg_list[i], bin_buf);
-
-		/* advance packet pointer */		
-		packet_p += (CEIL(reg_list[i]->size, 8) * 2);
-
-		free(bin_buf);
-		free(hex_buf);
-	}
-
-	/* free reg_t *reg_list[] array allocated by get_gdb_reg_list */ 
-	free(reg_list);
-
-	gdb_put_packet(connection, "OK", 2);
-
-	return ERROR_OK;
-}
-
-int gdb_get_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
-{
-	char *reg_packet;
-	int reg_num = strtoul(packet + 1, NULL, 16);
-	reg_t **reg_list;
-	int reg_list_size;
-	int retval;
-
-#ifdef _DEBUG_GDB_IO_
-	DEBUG("-");
-#endif
-
-	if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
-	{
-		switch (retval)
-		{
-			case ERROR_TARGET_NOT_HALTED:
-				ERROR("gdb requested registers but we're not halted, dropping connection");
-				return ERROR_SERVER_REMOTE_CLOSED;
-			default:
-				/* this is a bug condition - get_gdb_reg_list() may not return any other error */
-				ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
-				exit(-1);
-		}
-	}
-
-	if (reg_list_size <= reg_num)
-	{
-		ERROR("gdb requested a non-existing register");
-		exit(-1);
-	}
-
-	reg_packet = malloc(CEIL(reg_list[reg_num]->size, 8) * 2);
-
-	gdb_str_to_target(target, reg_packet, reg_list[reg_num]);
-
-	gdb_put_packet(connection, reg_packet, CEIL(reg_list[reg_num]->size, 8) * 2);
-
-	free(reg_list);
-	free(reg_packet);
-
-	return ERROR_OK;
-}
-
-int gdb_set_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
-{
-	char *separator;
-	char *hex_buf;
-	u8 *bin_buf;
-	int reg_num = strtoul(packet + 1, &separator, 16);
-	reg_t **reg_list;
-	int reg_list_size;
-	int retval;
-	reg_arch_type_t *arch_type;
-
-	DEBUG("-");
-
-	if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)
-	{
-		switch (retval)
-		{
-			case ERROR_TARGET_NOT_HALTED:
-				ERROR("gdb tried to set a register but we're not halted, dropping connection");
-				return ERROR_SERVER_REMOTE_CLOSED;
-			default:
-				/* this is a bug condition - get_gdb_reg_list() may not return any other error */
-				ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
-				exit(-1);
-		}
-	}
-
-	if (reg_list_size < reg_num)
-	{
-		ERROR("gdb requested a non-existing register");
-		return ERROR_SERVER_REMOTE_CLOSED;
-	}
-
-	if (*separator != '=')
-	{
-		ERROR("GDB 'set register packet', but no '=' following the register number");
-		return ERROR_SERVER_REMOTE_CLOSED;
-	}
-
-	/* convert from GDB-string (target-endian) to hex-string (big-endian) */
-	hex_buf = malloc(CEIL(reg_list[reg_num]->size, 8) * 2);
-	gdb_target_to_str(target, separator + 1, hex_buf);
-
-	/* convert hex-string to binary buffer */
-	bin_buf = malloc(CEIL(reg_list[reg_num]->size, 8));
-	str_to_buf(hex_buf, CEIL(reg_list[reg_num]->size, 8) * 2, bin_buf, reg_list[reg_num]->size, 16);
-
-	/* get register arch_type, and call set method */	
-	arch_type = register_get_arch_type(reg_list[reg_num]->arch_type);
-	if (arch_type == NULL)
-	{
-		ERROR("BUG: encountered unregistered arch type");
-		exit(-1);
-	}
-	arch_type->set(reg_list[reg_num], bin_buf);
-
-	gdb_put_packet(connection, "OK", 2);
-
-	free(bin_buf);
-	free(hex_buf);
-	free(reg_list);
-
-	return ERROR_OK;
-}
-
-int gdb_memory_packet_error(connection_t *connection, int retval)
-{
-	switch (retval)
-	{
-		case ERROR_TARGET_NOT_HALTED:
-			ERROR("gdb tried to read memory but we're not halted, dropping connection");
-			return ERROR_SERVER_REMOTE_CLOSED;
-		case ERROR_TARGET_DATA_ABORT:
-			gdb_send_error(connection, EIO);
-			break;
-		case ERROR_TARGET_TRANSLATION_FAULT:
-			gdb_send_error(connection, EFAULT);
-			break;
-		case ERROR_TARGET_UNALIGNED_ACCESS:
-			gdb_send_error(connection, EFAULT);
-			break;
-		default:
-			/* This could be that the target reset itself. */
-			ERROR("unexpected error %i. Dropping connection.", retval);
-			return ERROR_SERVER_REMOTE_CLOSED;
-	}
-
-	return ERROR_OK;
-}
-
-/* We don't have to worry about the default 2 second timeout for GDB packets,
- * because GDB breaks up large memory reads into smaller reads.
- * 
- * 8191 bytes by the looks of it. Why 8191 bytes instead of 8192?????
- */
-int gdb_read_memory_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
-{
-	char *separator;
-	u32 addr = 0;
-	u32 len = 0;
-
-	u8 *buffer;
-	char *hex_buffer;
-
-	int retval = ERROR_OK;
-
-	/* skip command character */
-	packet++;
-
-	addr = strtoul(packet, &separator, 16);
-
-	if (*separator != ',')
-	{
-		ERROR("incomplete read memory packet received, dropping connection");
-		return ERROR_SERVER_REMOTE_CLOSED;
-	}
-
-	len = strtoul(separator+1, NULL, 16);
-
-	buffer = malloc(len);
-
-	DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
-
-	retval = target_read_buffer(target, addr, len, buffer);
-
-	if ((retval == ERROR_TARGET_DATA_ABORT) && (!gdb_report_data_abort))
-	{
-		/* TODO : Here we have to lie and send back all zero's lest stack traces won't work.
-		 * At some point this might be fixed in GDB, in which case this code can be removed.
-		 * 
-		 * OpenOCD developers are acutely aware of this problem, but there is nothing
-		 * gained by involving the user in this problem that hopefully will get resolved
-		 * eventually
-		 * 
-		 * http://sourceware.org/cgi-bin/gnatsweb.pl?cmd=view%20audit-trail&database=gdb&pr=2395
-		 *
-		 * For now, the default is to fix up things to make current GDB versions work.
-		 * This can be overwritten using the gdb_report_data_abort <'enable'|'disable'> command.
-		 */
-		memset(buffer, 0, len);
-		retval = ERROR_OK;
-	}
-
-	if (retval == ERROR_OK)
-	{
-		hex_buffer = malloc(len * 2 + 1);
-
-		int i;
-		for (i = 0; i < len; i++)
-		{
-			u8 t = buffer[i];
-			hex_buffer[2 * i] = DIGITS[(t >> 4) & 0xf];
-			hex_buffer[2 * i + 1] = DIGITS[t & 0xf];
-		}
-
-		gdb_put_packet(connection, hex_buffer, len * 2);
-
-		free(hex_buffer);
-	}
-	else
-	{
-		retval = gdb_memory_packet_error(connection, retval);
-	}
-
-	free(buffer);
-
-	return retval;
-}
-
-int gdb_write_memory_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
-{
-	char *separator;
-	u32 addr = 0;
-	u32 len = 0;
-
-	u8 *buffer;
-
-	int i;
-	int retval;
-
-	/* skip command character */
-	packet++;
-
-	addr = strtoul(packet, &separator, 16);
-
-	if (*separator != ',')
-	{
-		ERROR("incomplete write memory packet received, dropping connection");
-		return ERROR_SERVER_REMOTE_CLOSED;
-	}
-
-	len = strtoul(separator+1, &separator, 16);
-
-	if (*(separator++) != ':')
-	{
-		ERROR("incomplete write memory packet received, dropping connection");
-		return ERROR_SERVER_REMOTE_CLOSED;
-	}
-
-	buffer = malloc(len);
-
-	DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
-
-	for (i=0; i<len; i++)
-	{
-		u32 tmp;
-		sscanf(separator + 2*i, "%2x", &tmp);
-		buffer[i] = tmp;
-	}
-
-	retval = target_write_buffer(target, addr, len, buffer);
-
-	if (retval == ERROR_OK)
-	{
-		gdb_put_packet(connection, "OK", 2);
-	}
-	else
-	{
-		if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK)
-			return retval; 
-	}
-
-	free(buffer);
-
-	return ERROR_OK;
-}
-
-int gdb_write_memory_binary_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
-{
-	char *separator;
-	u32 addr = 0;
-	u32 len = 0;
-
-	int retval;
-
-	/* skip command character */
-	packet++;
-
-	addr = strtoul(packet, &separator, 16);
-
-	if (*separator != ',')
-	{
-		ERROR("incomplete write memory binary packet received, dropping connection");
-		return ERROR_SERVER_REMOTE_CLOSED;
-	}
-
-	len = strtoul(separator+1, &separator, 16);
-
-	if (*(separator++) != ':')
-	{
-		ERROR("incomplete write memory binary packet received, dropping connection");
-		return ERROR_SERVER_REMOTE_CLOSED;
-	}
-
-	retval = ERROR_OK;
-	if (len)
-	{
-		DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
-
-		retval = target_write_buffer(target, addr, len, (u8*)separator);
-	}
-
-	if (retval == ERROR_OK)
-	{
-		gdb_put_packet(connection, "OK", 2);
-	}
-	else
-	{
-		if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK)
-			return retval; 
-	}
-
-	return ERROR_OK;
-}
-
-void gdb_step_continue_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
-{
-	int current = 0;
-	u32 address = 0x0;
-
-	DEBUG("-");
-
-	if (packet_size > 1)
-	{
-		packet[packet_size] = 0;
-		address = strtoul(packet + 1, NULL, 16);
-	}
-	else
-	{
-		current = 1;
-	}
-
-	if (packet[0] == 'c')
-	{
-		DEBUG("continue");
-		target->type->resume(target, current, address, 0, 0); /* resume at current address, don't handle breakpoints, not debugging */
-	}
-	else if (packet[0] == 's')
-	{
-		DEBUG("step");
-		target->type->step(target, current, address, 0); /* step at current or address, don't handle breakpoints */
-	}
-}
-
-int gdb_bp_wp_packet_error(connection_t *connection, int retval)
-{
-	switch (retval)
-	{
-		case ERROR_TARGET_NOT_HALTED:
-			ERROR("gdb tried to set a breakpoint but we're not halted, dropping connection");
-			return ERROR_SERVER_REMOTE_CLOSED;
-			break;
-		case ERROR_TARGET_RESOURCE_NOT_AVAILABLE:
-			gdb_send_error(connection, EBUSY);
-			break;
-		default:
-			ERROR("BUG: unexpected error %i", retval);
-			exit(-1);
-	}
-
-	return ERROR_OK;
-}
-
-int gdb_breakpoint_watchpoint_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
-{
-	int type;
-	enum breakpoint_type bp_type = BKPT_SOFT /* dummy init to avoid warning */;
-	enum watchpoint_rw wp_type;
-	u32 address;
-	u32 size;
-	char *separator;
-	int retval;
-
-	DEBUG("-");
-
-	type = strtoul(packet + 1, &separator, 16);
-
-	if (type == 0)	/* memory breakpoint */
-		bp_type = BKPT_SOFT;
-	else if (type == 1) /* hardware breakpoint */
-		bp_type = BKPT_HARD;
-	else if (type == 2) /* write watchpoint */
-		wp_type = WPT_WRITE;
-	else if (type == 3) /* read watchpoint */
-		wp_type = WPT_READ;
-	else if (type == 4) /* access watchpoint */
-		wp_type = WPT_ACCESS;
-
-	if (*separator != ',')
-	{
-		ERROR("incomplete breakpoint/watchpoint packet received, dropping connection");
-		return ERROR_SERVER_REMOTE_CLOSED;
-	}
-
-	address = strtoul(separator+1, &separator, 16);
-
-	if (*separator != ',')
-	{
-		ERROR("incomplete breakpoint/watchpoint packet received, dropping connection");
-		return ERROR_SERVER_REMOTE_CLOSED;
-	}
-
-	size = strtoul(separator+1, &separator, 16);
-
-	switch (type)
-	{
-		case 0:
-		case 1:
-			if (packet[0] == 'Z')
-			{
-				if ((retval = breakpoint_add(target, address, size, bp_type)) != ERROR_OK)
-				{
-					if ((retval = gdb_bp_wp_packet_error(connection, retval)) != ERROR_OK)
-						return retval;
-				}
-				else
-				{
-					gdb_put_packet(connection, "OK", 2);
-				}
-			}
-			else
-			{
-				breakpoint_remove(target, address);
-				gdb_put_packet(connection, "OK", 2);
-			}
-			break;
-		case 2:
-		case 3:
-		case 4:
-		{
-			if (packet[0] == 'Z')
-			{
-				if ((retval = watchpoint_add(target, address, size, type-2, 0, 0xffffffffu)) != ERROR_OK)
-				{
-					if ((retval = gdb_bp_wp_packet_error(connection, retval)) != ERROR_OK)
-						return retval;
-				}
-				else
-				{
-					gdb_put_packet(connection, "OK", 2);
-				}
-			}
-			else
-			{
-				watchpoint_remove(target, address);
-				gdb_put_packet(connection, "OK", 2);
-			}
-			break;
-		}
-		default:
-			break;
-	}
-
-	return ERROR_OK;
-}
-
-/* print out a string and allocate more space as needed, mainly used for XML at this point */
-void xml_printf(int *retval, char **xml, int *pos, int *size, const char *fmt, ...)
-{
-	if (*retval != ERROR_OK)
-	{
-		return;
-	}
-	int first = 1;
-	
-	for (;;)
-	{
-		if ((*xml == NULL) || (!first))
-		{
-			/* start by 0 to exercise all the code paths.
-			 * Need minimum 2 bytes to fit 1 char and 0 terminator. */
-			 
-			*size = *size * 2 + 2;
-			char *t = *xml;
-			*xml = realloc(*xml, *size);
-			if (*xml == NULL)
-			{
-				if (t)
-					free(t);
-				*retval = ERROR_SERVER_REMOTE_CLOSED;
-				return;
-			}
-		}
-		
-	    va_list ap;
-	    int ret;
-	    va_start(ap, fmt);
-	    ret = vsnprintf(*xml + *pos, *size - *pos, fmt, ap);
-	    va_end(ap);
-	    if ((ret > 0) && ((ret + 1) < *size - *pos))
-	    {
-	    	*pos += ret;
-	    	return;
-	    }
-	    /* there was just enough or not enough space, allocate more. */
-	    first = 0;
-	}
-}
-
-static int decode_xfer_read(char *buf, char **annex, int *ofs, unsigned int *len)
-{
-	char *separator;
-	
-	/* Extract and NUL-terminate the annex. */
-	*annex = buf;
-	while (*buf && *buf != ':')
-		buf++;
-	if (*buf == '\0')
-		return -1;
-	*buf++ = 0;
-	
-	/* After the read marker and annex, qXfer looks like a
-	 * traditional 'm' packet. */
-	
-	*ofs = strtoul(buf, &separator, 16);
-
-	if (*separator != ',')
-		return -1;
-
-	*len = strtoul(separator+1, NULL, 16);
-	
-	return 0;
-}
-
-int gdb_calc_blocksize(flash_bank_t *bank)
-{
-	int i;
-	int block_size = 0xffffffff;
-	
-	/* loop through all sectors and return smallest sector size */
-	
-	for (i = 0; i < bank->num_sectors; i++)
-	{
-		if (bank->sectors[i].size < block_size)
-			block_size = bank->sectors[i].size;
-	}
-	
-	return block_size;
-}
-
-int gdb_query_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
-{
-	command_context_t *cmd_ctx = connection->cmd_ctx;
-	
-	if (strstr(packet, "qRcmd,"))
-	{
-		if (packet_size > 6)
-		{
-			char *cmd;
-			int i;
-			cmd = malloc((packet_size - 6)/2 + 1);
-			for (i=0; i < (packet_size - 6)/2; i++)
-			{
-				u32 tmp;
-				sscanf(packet + 6 + 2*i, "%2x", &tmp);
-				cmd[i] = tmp;
-			}
-			cmd[(packet_size - 6)/2] = 0x0;
-			
-			/* We want to print all debug output to GDB connection */
-			log_setCallback(gdb_log_callback, connection);
-			target_call_timer_callbacks();
-			command_run_line(cmd_ctx, cmd);
-			free(cmd);
-		}
-		gdb_put_packet(connection, "OK", 2);
-		return ERROR_OK;
-	}
-	else if (strstr(packet, "qCRC:"))
-	{
-		if (packet_size > 5)
-		{
-			int retval;
-			char gdb_reply[10];
-			char *separator;
-			u32 checksum;
-			u32 addr = 0;
-			u32 len = 0;
-			
-			/* skip command character */
-			packet += 5;
-			
-			addr = strtoul(packet, &separator, 16);
-			
-			if (*separator != ',')
-			{
-				ERROR("incomplete read memory packet received, dropping connection");
-				return ERROR_SERVER_REMOTE_CLOSED;
-			}
-			
-			len = strtoul(separator + 1, NULL, 16);
-			
-			retval = target_checksum_memory(target, addr, len, &checksum);
-			
-			if (retval == ERROR_OK)
-			{
-				snprintf(gdb_reply, 10, "C%8.8x", checksum);
-				gdb_put_packet(connection, gdb_reply, 9);
-			}
-			else
-			{
-				if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK)
-					return retval; 
-			}
-			
-			return ERROR_OK;
-		}
-	}
-	else if (strstr(packet, "qSupported"))
-	{
-		/* we currently support packet size and qXfer:memory-map:read (if enabled)
-		 * disable qXfer:features:read for the moment */
-		int retval = ERROR_OK;
-		char *buffer = NULL;
-		int pos = 0;
-		int size = 0;
-
-		xml_printf(&retval, &buffer, &pos, &size, 
-				"PacketSize=%x;qXfer:memory-map:read%c;qXfer:features:read-",
-				(GDB_BUFFER_SIZE - 1), gdb_use_memory_map == 1 ? '+' : '-');
-		
-		if (retval != ERROR_OK)
-		{
-			gdb_send_error(connection, 01);
-			return ERROR_OK;
-		}
-		
-		gdb_put_packet(connection, buffer, strlen(buffer));
-		free(buffer);
-		
-		return ERROR_OK;
-	}
-	else if (strstr(packet, "qXfer:memory-map:read::"))
-	{
-		/* We get away with only specifying flash here. Regions that are not
-		 * specified are treated as if we provided no memory map(if not we 
-		 * could detect the holes and mark them as RAM).
-		 * Normally we only execute this code once, but no big deal if we
-		 * have to regenerate it a couple of times. */
-		 
-		flash_bank_t *p;
-		char *xml = NULL;
-		int size = 0;
-		int pos = 0;
-		int retval = ERROR_OK;
-		
-		int offset;
-		int length;
-		char *separator;
-		int blocksize;
-		
-		/* skip command character */
-		packet += 23;
-		
-		offset = strtoul(packet, &separator, 16);
-		length = strtoul(separator + 1, &separator, 16);
-		
-		xml_printf(&retval, &xml, &pos, &size, "<memory-map>\n");
-		
-		int i = 0;
-		for (;;)
-		{
-			p = get_flash_bank_by_num(i);
-			if (p == NULL)
-				break;
-			
-			/* if device has uneven sector sizes, eg. str7, lpc
-			 * we pass the smallest sector size to gdb memory map */
-			blocksize = gdb_calc_blocksize(p);
-			
-			xml_printf(&retval, &xml, &pos, &size, "<memory type=\"flash\" start=\"0x%x\" length=\"0x%x\">\n" \
-				"<property name=\"blocksize\">0x%x</property>\n" \
-				"</memory>\n", \
-				p->base, p->size, blocksize);
-			i++;
-		}
-		
-		xml_printf(&retval, &xml, &pos, &size, "</memory-map>\n");
-
-		if (retval != ERROR_OK)
-		{
-			gdb_send_error(connection, retval);
-			return retval;
-		}
-				
-		if (offset + length > pos)
-		{
-			length = pos - offset;
-		}
-
-		char *t = malloc(length + 1);
-		t[0] = 'l';
-		memcpy(t + 1, xml + offset, length);
-		gdb_put_packet(connection, t, length + 1);
-		
-		free(t);
-		free(xml);
-		return ERROR_OK;
-	}
-	else if (strstr(packet, "qXfer:features:read:"))
-	{		 
-		char *xml = NULL;
-		int size = 0;
-		int pos = 0;
-		int retval = ERROR_OK;
-		
-		int offset;
-		unsigned int length;
-		char *annex;
-		
-		/* skip command character */
-		packet += 20;
-		
-		if (decode_xfer_read(packet, &annex, &offset, &length) < 0)
-		{
-			gdb_send_error(connection, 01);
-			return ERROR_OK;
-		}
-		
-		if (strcmp(annex, "target.xml") != 0)
-		{
-			gdb_send_error(connection, 01);
-			return ERROR_OK;
-		}
-				
-		xml_printf(&retval, &xml, &pos, &size, \
-			"l<target version=\"1.0\">\n<architecture>arm</architecture>\n</target>\n");
-					
-		if (retval != ERROR_OK)
-		{
-			gdb_send_error(connection, retval);
-			return retval;
-		}
-		
-		gdb_put_packet(connection, xml, strlen(xml) + 1);
-		
-		free(xml);
-		return ERROR_OK;
-	}
-	
-	gdb_put_packet(connection, "", 0);
-	return ERROR_OK;
-}
-
-int gdb_v_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
-{
-	gdb_connection_t *gdb_connection = connection->priv;
-	gdb_service_t *gdb_service = connection->service->priv;
-	int result;
-
-	/* if flash programming disabled - send a empty reply */
-	
-	if (gdb_flash_program == 0)
-	{
-		gdb_put_packet(connection, "", 0);
-		return ERROR_OK;
-	}
-	
-	if (strstr(packet, "vFlashErase:"))
-	{
-		unsigned long addr;
-		unsigned long length;
-	
-		char *parse = packet + 12;
-		if (*parse == '\0')
-		{
-			ERROR("incomplete vFlashErase packet received, dropping connection");
-			return ERROR_SERVER_REMOTE_CLOSED;
-		}
-
-		addr = strtoul(parse, &parse, 16);
-
-		if (*(parse++) != ',' || *parse == '\0')
-		{
-			ERROR("incomplete vFlashErase packet received, dropping connection");
-			return ERROR_SERVER_REMOTE_CLOSED;
-		}
-
-		length = strtoul(parse, &parse, 16);
-
-		if (*parse != '\0')
-		{
-			ERROR("incomplete vFlashErase packet received, dropping connection");
-			return ERROR_SERVER_REMOTE_CLOSED;
-		}
-		
-		/* assume all sectors need erasing - stops any problems
-		 * when flash_write is called multiple times */
-		flash_set_dirty();
-		
-		/* perform any target specific operations before the erase */
-		target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_PROGRAM);
-		
-		/* perform erase */
-		if ((result = flash_erase_address_range(gdb_service->target, addr, length)) != ERROR_OK)
-		{
-			/* GDB doesn't evaluate the actual error number returned,
-			 * treat a failed erase as an I/O error
-			 */
-			gdb_send_error(connection, EIO);
-			ERROR("flash_erase returned %i", result);
-		}
-		else
-			gdb_put_packet(connection, "OK", 2);
-		
-		return ERROR_OK;
-	}
-
-	if (strstr(packet, "vFlashWrite:"))
-	{
-		unsigned long addr;
-		unsigned long length;
-		char *parse = packet + 12;
-
-		if (*parse == '\0')
-		{
-			ERROR("incomplete vFlashErase packet received, dropping connection");
-			return ERROR_SERVER_REMOTE_CLOSED;
-		}
-		addr = strtoul(parse, &parse, 16);
-		if (*(parse++) != ':')
-		{
-			ERROR("incomplete vFlashErase packet received, dropping connection");
-			return ERROR_SERVER_REMOTE_CLOSED;
-		}
-		length = packet_size - (parse - packet);
-		
-		/* create a new image if there isn't already one */
-		if (gdb_connection->vflash_image == NULL)
-		{
-			gdb_connection->vflash_image = malloc(sizeof(image_t));
-			image_open(gdb_connection->vflash_image, "", "build");
-		}
-
-		/* create new section with content from packet buffer */
-		image_add_section(gdb_connection->vflash_image, addr, length, 0x0, (u8*)parse);
-
-		gdb_put_packet(connection, "OK", 2);
-
-		return ERROR_OK;
-	}
-
-	if (!strcmp(packet, "vFlashDone"))
-	{
-		u32 written;
-		char *error_str;
-
-		/* process the flashing buffer. No need to erase as GDB
-		 * always issues a vFlashErase first. */
-		if ((result = flash_write(gdb_service->target, gdb_connection->vflash_image, &written, &error_str, NULL, 0)) != ERROR_OK)
-		{
-			if (result == ERROR_FLASH_DST_OUT_OF_BANK)
-				gdb_put_packet(connection, "E.memtype", 9);
-			else
-				gdb_send_error(connection, EIO);
-			
-			if (error_str)
-			{
-				ERROR("flash writing failed: %s", error_str);
-				free(error_str);
-			}
-		}
-		else
-		{
-			DEBUG("wrote %u bytes from vFlash image to flash", written);
-			gdb_put_packet(connection, "OK", 2);
-		}
-		
-		image_close(gdb_connection->vflash_image);
-		free(gdb_connection->vflash_image);
-		gdb_connection->vflash_image = NULL;
-		
-		return ERROR_OK;
-	}
-
-	gdb_put_packet(connection, "", 0);
-	return ERROR_OK;
-}
-
-int gdb_detach(connection_t *connection, target_t *target)
-{
-	switch( detach_mode )
-	{
-		case GDB_DETACH_RESUME:
-			target->type->resume(target, 1, 0, 1, 0);
-			break;
-		
-		case GDB_DETACH_RESET:
-			target_process_reset(connection->cmd_ctx);
-			break;
-		
-		case GDB_DETACH_HALT:
-			target->type->halt(target);
-			break;
-		
-		case GDB_DETACH_NOTHING:
-			break;
-	}
-	
-	gdb_put_packet(connection, "OK", 2);
-	
-	return ERROR_OK;
-}
-
-static void gdb_log_callback(void *priv, const char *file, int line, 
-		const char *function, const char *format, va_list args)
-{
-	connection_t *connection = priv;
-	gdb_connection_t *gdb_con = connection->priv;
-	
-	if (gdb_con->busy)
-	{
-		/* do not reply this using the O packet */
-		return;
-	}
-
-	char *t = allocPrintf(format, args);
-	if (t == NULL)
-		return;
-	
-	gdb_output_con(connection, t); 
-	
-	free(t);
-}
-
-int gdb_input_inner(connection_t *connection)
-{
-	gdb_service_t *gdb_service = connection->service->priv;
-	target_t *target = gdb_service->target;
-	char packet[GDB_BUFFER_SIZE];
-	int packet_size;
-	int retval;
-	gdb_connection_t *gdb_con = connection->priv;
-	static int extended_protocol = 0;
-
-	/* drain input buffer */
-	do
-	{
-		packet_size = GDB_BUFFER_SIZE-1;
-		if ((retval = gdb_get_packet(connection, packet, &packet_size)) != ERROR_OK)
-		{
-			return retval;
-		}
-
-		/* terminate with zero */
-		packet[packet_size] = 0;
-
-		DEBUG("received packet: '%s'", packet);
-
-		if (packet_size > 0)
-		{
-			retval = ERROR_OK;
-			switch (packet[0])
-			{
-				case 'H':
-					/* Hct... -- set thread 
-					 * we don't have threads, send empty reply */
-					gdb_put_packet(connection, NULL, 0);
-					break;
-				case 'q':
-					retval = gdb_query_packet(connection, target, packet, packet_size);
-					break;
-				case 'g':
-					retval = gdb_get_registers_packet(connection, target, packet, packet_size);
-					break;
-				case 'G':
-					retval = gdb_set_registers_packet(connection, target, packet, packet_size);
-					break;
-				case 'p':
-					retval = gdb_get_register_packet(connection, target, packet, packet_size);
-					break;
-				case 'P':
-					retval = gdb_set_register_packet(connection, target, packet, packet_size);
-					break;
-				case 'm':
-					retval = gdb_read_memory_packet(connection, target, packet, packet_size);
-					break;
-				case 'M':
-					retval = gdb_write_memory_packet(connection, target, packet, packet_size);
-					break;
-				case 'z':
-				case 'Z':
-					retval = gdb_breakpoint_watchpoint_packet(connection, target, packet, packet_size);
-					break;
-				case '?':
-					gdb_last_signal_packet(connection, target, packet, packet_size);
-					break;
-				case 'c':
-				case 's':
-					{
-					/* We're running/stepping, in which case we can 
-					 * forward log output until the target is halted */
-						gdb_connection_t *gdb_con = connection->priv;
-						gdb_con->frontend_state = TARGET_RUNNING;
-						log_setCallback(gdb_log_callback, connection);
-						gdb_step_continue_packet(connection, target, packet, packet_size);
-					}
-					break;
-				case 'v':
-					retval = gdb_v_packet(connection, target, packet, packet_size);
-					break;
-				case 'D':
-					retval = gdb_detach(connection, target);
-					extended_protocol = 0;
-					break;
-				case 'X':
-					if ((retval = gdb_write_memory_binary_packet(connection, target, packet, packet_size)) != ERROR_OK)
-						return retval;
-					break;
-				case 'k':
-					if (extended_protocol != 0)
-						break;
-					gdb_put_packet(connection, "OK", 2);
-					return ERROR_SERVER_REMOTE_CLOSED;
-				case '!':
-					/* handle extended remote protocol */
-					extended_protocol = 1;
-					gdb_put_packet(connection, "OK", 2);
-					break;
-				case 'R':
-					/* handle extended restart packet */
-					target_process_reset(connection->cmd_ctx);
-					break;
-				default:
-					/* ignore unkown packets */
-					DEBUG("ignoring 0x%2.2x packet", packet[0]);
-					gdb_put_packet(connection, NULL, 0);
-					break;
-			}
-
-			/* if a packet handler returned an error, exit input loop */
-			if (retval != ERROR_OK)
-				return retval;
-		}
-
-		if (gdb_con->ctrl_c)
-		{
-			if (target->state == TARGET_RUNNING)
-			{
-				target->type->halt(target);
-				gdb_con->ctrl_c = 0;
-			}
-		}
-
-	} while (gdb_con->buf_cnt > 0);
-
-	return ERROR_OK;
-}
-
-int gdb_input(connection_t *connection)
-{
-	int retval = gdb_input_inner(connection);
-	if (retval == ERROR_SERVER_REMOTE_CLOSED)
-		return retval;
-	/* we'll recover from any other errors(e.g. temporary timeouts, etc.) */
-	return ERROR_OK;
-}
-
-int gdb_init()
-{
-	gdb_service_t *gdb_service;
-	target_t *target = targets;
-	int i = 0;
-
-	if (!target)
-	{
-		WARNING("no gdb ports allocated as no target has been specified");
-		return ERROR_OK;
-	}
-
-	if (gdb_port == 0)
-	{
-		WARNING("no gdb port specified, using default port 3333");
-		gdb_port = 3333;
-	}
-
-	while (target)
-	{
-		char service_name[8];
-
-		snprintf(service_name, 8, "gdb-%2.2i", i);
-
-		gdb_service = malloc(sizeof(gdb_service_t));
-		gdb_service->target = target;
-
-		add_service("gdb", CONNECTION_GDB, gdb_port + i, 1, gdb_new_connection, gdb_input, gdb_connection_closed, gdb_service);
-
-		DEBUG("gdb service for target %s at port %i", target->type->name, gdb_port + i);
-
-		i++;
-		target = target->next;
-	}
-
-	return ERROR_OK;
-}
-
-/* daemon configuration command gdb_port */
-int handle_gdb_port_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	if (argc == 0)
-		return ERROR_OK;
-
-	/* only if the port wasn't overwritten by cmdline */
-	if (gdb_port == 0)
-		gdb_port = strtoul(args[0], NULL, 0);
-
-	return ERROR_OK;
-}
-
-int handle_gdb_detach_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	if (argc == 1)
-	{
-		if (strcmp(args[0], "resume") == 0)
-		{
-			detach_mode = GDB_DETACH_RESUME;
-			return ERROR_OK;
-		}
-		else if (strcmp(args[0], "reset") == 0)
-		{
-			detach_mode = GDB_DETACH_RESET;
-			return ERROR_OK;
-		}
-		else if (strcmp(args[0], "halt") == 0)
-		{
-			detach_mode = GDB_DETACH_HALT;
-			return ERROR_OK;
-		}
-		else if (strcmp(args[0], "nothing") == 0)
-		{
-			detach_mode = GDB_DETACH_NOTHING;
-			return ERROR_OK;
-		}
-	}
-	
-	WARNING("invalid gdb_detach configuration directive: %s", args[0]);
-	return ERROR_OK;
-}
-
-int handle_gdb_memory_map_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	if (argc == 1)
-	{
-		if (strcmp(args[0], "enable") == 0)
-		{
-			gdb_use_memory_map = 1;
-			return ERROR_OK;
-		}
-		else if (strcmp(args[0], "disable") == 0)
-		{
-			gdb_use_memory_map = 0;
-			return ERROR_OK;
-		}
-	}
-	
-	WARNING("invalid gdb_memory_map configuration directive: %s", args[0]);
-	return ERROR_OK;
-}
-
-int handle_gdb_flash_program_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	if (argc == 1)
-	{
-		if (strcmp(args[0], "enable") == 0)
-		{
-			gdb_flash_program = 1;
-			return ERROR_OK;
-		}
-		else if (strcmp(args[0], "disable") == 0)
-		{
-			gdb_flash_program = 0;
-			return ERROR_OK;
-		}
-	}
-	
-	WARNING("invalid gdb_memory_map configuration directive: %s", args[0]);
-	return ERROR_OK;
-}
-
-int handle_gdb_report_data_abort_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	if (argc == 1)
-	{
-		if (strcmp(args[0], "enable") == 0)
-		{
-			gdb_report_data_abort = 1;
-			return ERROR_OK;
-		}
-		else if (strcmp(args[0], "disable") == 0)
-		{
-			gdb_report_data_abort = 0;
-			return ERROR_OK;
-		}
-	}
-	
-	WARNING("invalid gdb_report_data_abort configuration directive: %s", args[0]);
-	return ERROR_OK;
-}
-
-int gdb_register_commands(command_context_t *command_context)
-{
-	register_command(command_context, NULL, "gdb_port", handle_gdb_port_command,
-			COMMAND_CONFIG, "");
-	register_command(command_context, NULL, "gdb_detach", handle_gdb_detach_command,
-			COMMAND_CONFIG, "");
-	register_command(command_context, NULL, "gdb_memory_map", handle_gdb_memory_map_command,
-			COMMAND_CONFIG, "");
-	register_command(command_context, NULL, "gdb_flash_program", handle_gdb_flash_program_command,
-			COMMAND_CONFIG, "");
-	register_command(command_context, NULL, "gdb_report_data_abort", handle_gdb_report_data_abort_command,
-			COMMAND_CONFIG, "");
-	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 "gdb_server.h"

+

+#include "server.h"

+#include "log.h"

+#include "binarybuffer.h"

+#include "jtag.h"

+#include "breakpoints.h"

+#include "flash.h"

+#include "target_request.h"

+#include "configuration.h"

+

+#include <string.h>

+#include <errno.h>

+#include <unistd.h>

+#include <stdlib.h>

+

+#if 0

+#define _DEBUG_GDB_IO_

+#endif

+

+static unsigned short gdb_port;

+static const char *DIGITS = "0123456789abcdef";

+

+static void gdb_log_callback(void *priv, const char *file, int line, 

+		const char *function, const char *format, va_list args);

+

+enum gdb_detach_mode

+{

+	GDB_DETACH_RESUME,

+	GDB_DETACH_RESET,

+	GDB_DETACH_HALT,

+	GDB_DETACH_NOTHING

+};

+

+/* target behaviour on gdb detach */

+enum gdb_detach_mode detach_mode = GDB_DETACH_RESUME;

+

+/* set if we are sending a memory map to gdb

+ * via qXfer:memory-map:read packet */

+int gdb_use_memory_map = 0;

+int gdb_flash_program = 0;

+

+/* if set, data aborts cause an error to be reported in memory read packets

+ * see the code in gdb_read_memory_packet() for further explanations */

+int gdb_report_data_abort = 0;

+

+int gdb_last_signal(target_t *target)

+{

+	switch (target->debug_reason)

+	{

+		case DBG_REASON_DBGRQ:

+			return 0x2; /* SIGINT */

+		case DBG_REASON_BREAKPOINT:

+		case DBG_REASON_WATCHPOINT:

+		case DBG_REASON_WPTANDBKPT:

+			return 0x05; /* SIGTRAP */

+		case DBG_REASON_SINGLESTEP:

+			return 0x05; /* SIGTRAP */

+		case DBG_REASON_NOTHALTED:

+			return 0x0; /* no signal... shouldn't happen */

+		default:

+			ERROR("BUG: undefined debug reason");

+			exit(-1);

+	}

+}

+

+int gdb_get_char(connection_t *connection, int* next_char)

+{

+	gdb_connection_t *gdb_con = connection->priv;

+

+#ifdef _DEBUG_GDB_IO_

+	char *debug_buffer;

+#endif

+

+	if (gdb_con->buf_cnt-- > 0)

+	{

+		*next_char = *(gdb_con->buf_p++);

+		if (gdb_con->buf_cnt > 0)

+			connection->input_pending = 1;

+		else

+			connection->input_pending = 0;

+

+#ifdef _DEBUG_GDB_IO_

+		DEBUG("returned char '%c' (0x%2.2x)", *next_char, *next_char);

+#endif

+

+		return ERROR_OK;

+	}

+

+	for (;;)

+	{

+#ifndef _WIN32

+		/* a non-blocking socket will block if there is 0 bytes available on the socket,

+		 * but return with as many bytes as are available immediately

+		 */

+		struct timeval tv;

+		fd_set read_fds;

+		

+		FD_ZERO(&read_fds);

+		FD_SET(connection->fd, &read_fds);

+		

+		tv.tv_sec = 1;

+		tv.tv_usec = 0;

+		if (select(connection->fd + 1, &read_fds, NULL, NULL, &tv) == 0)

+		{

+			/* This can typically be because a "monitor" command took too long

+			 * before printing any progress messages

+			 */

+			return ERROR_GDB_TIMEOUT; 

+		}

+#endif

+		gdb_con->buf_cnt = read_socket(connection->fd, gdb_con->buffer, GDB_BUFFER_SIZE);

+		if (gdb_con->buf_cnt > 0)

+		{

+			break;

+		}

+		if (gdb_con->buf_cnt == 0)

+		{

+			gdb_con->closed = 1;

+			return ERROR_SERVER_REMOTE_CLOSED;

+		}

+

+#ifdef _WIN32

+		errno = WSAGetLastError();

+

+		switch(errno)

+		{

+			case WSAEWOULDBLOCK:

+				usleep(1000);

+				break;

+			case WSAECONNABORTED:

+				return ERROR_SERVER_REMOTE_CLOSED;

+			case WSAECONNRESET:

+				return ERROR_SERVER_REMOTE_CLOSED;

+			default:

+				ERROR("read: %d", errno);

+				exit(-1);

+		}

+#else

+		switch(errno)

+		{

+			case EAGAIN:

+				usleep(1000);

+				break;

+			case ECONNABORTED:

+				return ERROR_SERVER_REMOTE_CLOSED;

+			case ECONNRESET:

+				return ERROR_SERVER_REMOTE_CLOSED;

+			default:

+				ERROR("read: %s", strerror(errno));

+				return ERROR_SERVER_REMOTE_CLOSED;

+		}

+#endif

+	}

+

+#ifdef _DEBUG_GDB_IO_

+	debug_buffer = malloc(gdb_con->buf_cnt + 1);

+	memcpy(debug_buffer, gdb_con->buffer, gdb_con->buf_cnt);

+	debug_buffer[gdb_con->buf_cnt] = 0;

+	DEBUG("received '%s'", debug_buffer);

+	free(debug_buffer);

+#endif

+

+	gdb_con->buf_p = gdb_con->buffer;

+	gdb_con->buf_cnt--;

+	*next_char = *(gdb_con->buf_p++);

+	if (gdb_con->buf_cnt > 0)

+		connection->input_pending = 1;

+	else

+		connection->input_pending = 0;	

+#ifdef _DEBUG_GDB_IO_

+	DEBUG("returned char '%c' (0x%2.2x)", *next_char, *next_char);

+#endif

+

+	return ERROR_OK;

+}

+

+int gdb_putback_char(connection_t *connection, int last_char)

+{

+	gdb_connection_t *gdb_con = connection->priv;

+

+	if (gdb_con->buf_p > gdb_con->buffer)

+	{

+		*(--gdb_con->buf_p) = last_char;

+		gdb_con->buf_cnt++;

+	}

+	else

+	{

+		ERROR("BUG: couldn't put character back"); 	

+	}

+

+	return ERROR_OK;

+}

+

+/* The only way we can detect that the socket is closed is the first time

+ * we write to it, we will fail. Subsequent write operations will

+ * succeed. Shudder! */

+int gdb_write(connection_t *connection, void *data, int len)

+{

+	gdb_connection_t *gdb_con = connection->priv;

+	if (gdb_con->closed)

+		return ERROR_SERVER_REMOTE_CLOSED;

+

+	if (write_socket(connection->fd, data, len) == len)

+	{

+		return ERROR_OK;

+	}

+	gdb_con->closed = 1;

+	return ERROR_SERVER_REMOTE_CLOSED;

+}

+

+int gdb_put_packet_inner(connection_t *connection, char *buffer, int len)

+{

+	int i;

+	unsigned char my_checksum = 0;

+#ifdef _DEBUG_GDB_IO_

+	char *debug_buffer;

+#endif

+	int reply;

+	int retval;

+	gdb_connection_t *gdb_con = connection->priv;

+

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

+		my_checksum += buffer[i];

+

+	while (1)

+	{

+#ifdef _DEBUG_GDB_IO_

+		debug_buffer = malloc(len + 1);

+		memcpy(debug_buffer, buffer, len);

+		debug_buffer[len] = 0;

+		DEBUG("sending packet '$%s#%2.2x'", debug_buffer, my_checksum);

+		free(debug_buffer);

+#endif

+#if 0

+		char checksum[3];

+		gdb_write(connection, "$", 1);

+		if (len > 0)

+			gdb_write(connection, buffer, len);

+		gdb_write(connection, "#", 1);

+		

+		snprintf(checksum, 3, "%2.2x", my_checksum);

+		

+		gdb_write(connection, checksum, 2);

+#else

+		void *allocated = NULL;

+		char stackAlloc[1024];

+		char *t = stackAlloc;

+		int totalLen = 1 + len + 1 + 2;

+		if (totalLen > sizeof(stackAlloc))

+		{

+			allocated = malloc(totalLen);

+			t = allocated;

+			if (allocated == NULL)

+			{

+				ERROR("Ran out of memory trying to reply packet %d\n", totalLen);

+				exit(-1);

+			}

+		}

+		t[0] = '$';

+		memcpy(t + 1, buffer, len);

+		t[1 + len] = '#';

+		t[1 + len + 1] = DIGITS[(my_checksum >> 4) & 0xf];

+		t[1 + len + 2] = DIGITS[my_checksum & 0xf];

+		

+		gdb_write(connection, t, totalLen);

+		

+		if (allocated)

+		{

+			free(allocated);

+		}

+#endif

+		if ((retval = gdb_get_char(connection, &reply)) != ERROR_OK)

+			return retval;

+

+		if (reply == '+')

+			break;

+		else if (reply == '-')

+		{

+			/* Stop sending output packets for now */

+			log_setCallback(NULL, NULL);

+			WARNING("negative reply, retrying");

+		}

+		else if (reply == 0x3)

+		{

+			gdb_con->ctrl_c = 1;

+			if ((retval = gdb_get_char(connection, &reply)) != ERROR_OK)

+				return retval;

+			if (reply == '+')

+				break;

+			else if (reply == '-')

+			{

+				/* Stop sending output packets for now */

+				log_setCallback(NULL, NULL);

+				WARNING("negative reply, retrying");

+			}

+			else

+			{

+				ERROR("unknown character 0x%2.2x in reply, dropping connection", reply);

+				return ERROR_SERVER_REMOTE_CLOSED;

+			}

+		}

+		else

+		{

+			ERROR("unknown character 0x%2.2x in reply, dropping connection", reply);

+			return ERROR_SERVER_REMOTE_CLOSED;

+		}

+	}

+	if (gdb_con->closed)

+		return ERROR_SERVER_REMOTE_CLOSED;

+

+	return ERROR_OK;

+}

+

+int gdb_put_packet(connection_t *connection, char *buffer, int len)

+{

+	gdb_connection_t *gdb_con = connection->priv;

+	gdb_con->busy = 1;

+	int retval = gdb_put_packet_inner(connection, buffer, len);

+	gdb_con->busy = 0;

+	return retval;

+}

+

+int gdb_get_packet_inner(connection_t *connection, char *buffer, int *len)

+{

+	int character;

+	int count = 0;

+	int retval;

+	char checksum[3];

+	unsigned char my_checksum = 0;

+	gdb_connection_t *gdb_con = connection->priv;

+

+	while (1)

+	{

+		do

+		{

+			if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)

+				return retval;

+

+#ifdef _DEBUG_GDB_IO_

+			DEBUG("character: '%c'", character);

+#endif

+

+			switch (character)

+			{

+				case '$':

+					break;

+				case '+':

+					WARNING("acknowledgment received, but no packet pending");

+					break;

+				case '-':

+					WARNING("negative acknowledgment, but no packet pending");

+					break;

+				case 0x3:

+					gdb_con->ctrl_c = 1;

+					*len = 0;

+					return ERROR_OK;

+				default:

+					WARNING("ignoring character 0x%x", character);

+					break;

+			}

+		} while (character != '$');

+

+		my_checksum = 0;

+		

+		count = 0;

+		gdb_connection_t *gdb_con = connection->priv;

+		for (;;)

+		{

+			/* The common case is that we have an entire packet with no escape chars.

+			 * We need to leave at least 2 bytes in the buffer to have

+			 * gdb_get_char() update various bits and bobs correctly. 

+			 */

+			if ((gdb_con->buf_cnt > 2) && ((gdb_con->buf_cnt+count) < *len))

+			{

+				/* The compiler will struggle a bit with constant propagation and

+				 * aliasing, so we help it by showing that these values do not

+				 * change inside the loop 

+				 */ 

+				int i;

+				char *buf = gdb_con->buf_p;

+				int run = gdb_con->buf_cnt - 2;

+				i = 0;

+				int done = 0;

+				while (i < run)

+				{

+					character = *buf++;

+					i++;

+					if (character == '#')

+					{

+						/* Danger! character can be '#' when esc is 

+						 * used so we need an explicit boolean for done here.

+						 */

+						done = 1;

+						break;

+					}

+					

+					if (character == '}')

+					{

+						/* data transmitted in binary mode (X packet)

+						 * uses 0x7d as escape character */

+						my_checksum += character & 0xff;

+						character = *buf++;

+						i++;

+						my_checksum += character & 0xff;

+						buffer[count++] = (character ^ 0x20) & 0xff;

+					} else

+					{

+						my_checksum += character & 0xff;

+						buffer[count++] = character & 0xff;

+					}

+				}

+				gdb_con->buf_p += i;

+				gdb_con->buf_cnt -= i;

+				if (done) 

+					break;

+			} 

+			if (count > *len)

+			{

+				ERROR("packet buffer too small");

+				return ERROR_GDB_BUFFER_TOO_SMALL;

+			}

+			

+			if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)

+				return retval;

+

+			if (character == '#')

+				break;

+

+			if (character == '}')

+			{

+				/* data transmitted in binary mode (X packet)

+				 * uses 0x7d as escape character */

+				my_checksum += character & 0xff;

+				if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)

+					return retval;

+				my_checksum += character & 0xff;

+				buffer[count++] = (character ^ 0x20) & 0xff;

+			}

+			else

+			{

+				my_checksum += character & 0xff;

+				buffer[count++] = character & 0xff;

+			}

+

+		}

+

+		*len = count;

+

+		if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)

+			return retval;

+		checksum[0] = character;

+		if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)

+			return retval;

+		checksum[1] = character;

+		checksum[2] = 0;

+

+		if (my_checksum == strtoul(checksum, NULL, 16))

+		{

+			gdb_write(connection, "+", 1);

+			break;

+		}

+

+		WARNING("checksum error, requesting retransmission");

+		gdb_write(connection, "-", 1);

+	}

+	if (gdb_con->closed)

+		return ERROR_SERVER_REMOTE_CLOSED;

+

+	return ERROR_OK;

+}

+

+int gdb_get_packet(connection_t *connection, char *buffer, int *len)

+{

+	gdb_connection_t *gdb_con = connection->priv;

+	gdb_con->busy = 1;

+	int retval = gdb_get_packet_inner(connection, buffer, len);

+	gdb_con->busy = 0;

+	return retval;

+}

+	

+int gdb_output_con(connection_t *connection, char* line)

+{

+	char *hex_buffer;

+	int i, bin_size;

+

+	bin_size = strlen(line);

+

+	hex_buffer = malloc(bin_size*2 + 4);

+

+	hex_buffer[0] = 'O';

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

+		snprintf(hex_buffer + 1 + i*2, 3, "%2.2x", line[i]);

+	hex_buffer[bin_size*2+1] = '0';

+	hex_buffer[bin_size*2+2] = 'a';

+	hex_buffer[bin_size*2+3] = 0x0;

+

+	gdb_put_packet(connection, hex_buffer, bin_size*2 + 3);

+

+	free(hex_buffer);

+	return ERROR_OK;

+}

+

+int gdb_output(struct command_context_s *context, char* line)

+{

+	/* this will be dumped to the log and also sent as an O packet if possible */

+	USER(line); 

+	return ERROR_OK;

+}

+

+int gdb_program_handler(struct target_s *target, enum target_event event, void *priv)

+{

+	FILE *script;

+	struct command_context_s *cmd_ctx = priv;

+	

+	if (target->gdb_program_script)

+	{

+		script = open_file_from_path(cmd_ctx, target->gdb_program_script, "r");

+		if (!script)

+		{

+			ERROR("couldn't open script file %s", target->gdb_program_script);

+				return ERROR_OK;

+		}

+

+		INFO("executing gdb_program script '%s'", target->gdb_program_script);

+		command_run_file(cmd_ctx, script, COMMAND_EXEC);

+		fclose(script);

+		

+		jtag_execute_queue();

+	}

+	

+	return ERROR_OK;

+}

+

+int gdb_target_callback_event_handler(struct target_s *target, enum target_event event, void *priv)

+{

+	connection_t *connection = priv;

+	gdb_connection_t *gdb_connection = connection->priv;

+	char sig_reply[4];

+	int signal;

+

+	switch (event)

+	{

+		case TARGET_EVENT_HALTED:

+			/* In the GDB protocol when we are stepping or coninuing execution,

+			 * we have a lingering reply. Upon receiving a halted event 

+			 * when we have that lingering packet, we reply to the original

+			 * step or continue packet.

+			 * 

+			 * Executing monitor commands can bring the target in and

+			 * out of the running state so we'll see lots of TARGET_EVENT_XXX

+			 * that are to be ignored.

+			 */

+			if (gdb_connection->frontend_state == TARGET_RUNNING)

+			{

+				/* stop forwarding log packets! */

+				log_setCallback(NULL, NULL);

+				

+				if (gdb_connection->ctrl_c)

+				{

+					signal = 0x2;

+					gdb_connection->ctrl_c = 0;

+				}

+				else

+				{

+					signal = gdb_last_signal(target);

+				}

+

+				snprintf(sig_reply, 4, "T%2.2x", signal);

+				gdb_put_packet(connection, sig_reply, 3);

+				gdb_connection->frontend_state = TARGET_HALTED;

+			}

+			break;

+		case TARGET_EVENT_GDB_PROGRAM:

+			gdb_program_handler(target, event, connection->cmd_ctx);

+			break;

+		default:

+			break;

+	}

+

+	return ERROR_OK;

+}

+

+int gdb_new_connection(connection_t *connection)

+{

+	gdb_connection_t *gdb_connection = malloc(sizeof(gdb_connection_t));

+	gdb_service_t *gdb_service = connection->service->priv;

+	int retval;

+	int initial_ack;

+

+	connection->priv = gdb_connection;

+

+	/* initialize gdb connection information */

+	gdb_connection->buf_p = gdb_connection->buffer;

+	gdb_connection->buf_cnt = 0;

+	gdb_connection->ctrl_c = 0;

+	gdb_connection->frontend_state = TARGET_HALTED;

+	gdb_connection->vflash_image = NULL;

+	gdb_connection->closed = 0;

+	gdb_connection->busy = 0;

+	

+	/* output goes through gdb connection */

+	command_set_output_handler(connection->cmd_ctx, gdb_output, connection);

+

+	/* register callback to be informed about target events */

+	target_register_event_callback(gdb_target_callback_event_handler, connection);	

+

+	/* a gdb session just attached, put the target in halt mode */

+	if (((retval = gdb_service->target->type->halt(gdb_service->target)) != ERROR_OK) &&

+			(retval != ERROR_TARGET_ALREADY_HALTED))

+	{

+		ERROR("error(%d) when trying to halt target, falling back to \"reset halt\"", retval);

+		command_run_line(connection->cmd_ctx, "reset halt");

+	}

+

+	/* This will time out after 1 second */

+	command_run_line(connection->cmd_ctx, "wait_halt 1");

+

+	/* remove the initial ACK from the incoming buffer */

+	if ((retval = gdb_get_char(connection, &initial_ack)) != ERROR_OK)

+		return retval;

+

+	if (initial_ack != '+')

+		gdb_putback_char(connection, initial_ack);

+

+	return ERROR_OK;

+}

+

+int gdb_connection_closed(connection_t *connection)

+{

+	gdb_service_t *gdb_service = connection->service->priv;

+	gdb_connection_t *gdb_connection = connection->priv;

+

+	/* see if an image built with vFlash commands is left */

+	if (gdb_connection->vflash_image)

+	{

+		image_close(gdb_connection->vflash_image);

+		free(gdb_connection->vflash_image);

+		gdb_connection->vflash_image = NULL;

+	}

+

+	/* if this connection registered a debug-message receiver delete it */

+	delete_debug_msg_receiver(connection->cmd_ctx, gdb_service->target);

+	

+	if (connection->priv)

+	{

+		free(connection->priv);

+		connection->priv = NULL;

+	}

+	else

+	{

+		ERROR("BUG: connection->priv == NULL");

+	}

+

+	target_unregister_event_callback(gdb_target_callback_event_handler, connection);

+	log_setCallback(NULL, NULL);

+

+	return ERROR_OK;

+}

+

+void gdb_send_error(connection_t *connection, u8 the_error)

+{

+	char err[4];

+	snprintf(err, 4, "E%2.2X", the_error );

+	gdb_put_packet(connection, err, 3);

+}

+

+int gdb_last_signal_packet(connection_t *connection, target_t *target, char* packet, int packet_size)

+{

+	char sig_reply[4];

+	int signal;

+

+	signal = gdb_last_signal(target);

+

+	snprintf(sig_reply, 4, "S%2.2x", signal);

+	gdb_put_packet(connection, sig_reply, 3);

+

+	return ERROR_OK;

+}

+

+/* Convert register to string of bits. NB! The # of bits in the

+ * register might be non-divisible by 8(a byte), in which

+ * case an entire byte is shown. */

+void gdb_str_to_target(target_t *target, char *tstr, reg_t *reg)

+{

+	int i;

+

+	u8 *buf;

+	int buf_len;

+	buf = reg->value;

+	buf_len = CEIL(reg->size, 8); 

+

+	if (target->endianness == TARGET_LITTLE_ENDIAN)

+	{

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

+		{

+			tstr[i*2]   = DIGITS[(buf[i]>>4) & 0xf];

+			tstr[i*2+1] = DIGITS[buf[i]&0xf];

+		}

+	}

+	else

+	{

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

+		{

+			tstr[(buf_len-1-i)*2]   = DIGITS[(buf[i]>>4)&0xf];

+			tstr[(buf_len-1-i)*2+1] = DIGITS[buf[i]&0xf];

+		}

+	}	

+}

+

+void gdb_target_to_str(target_t *target, char *tstr, char *str)

+{

+	int str_len = strlen(tstr);

+	int i;

+

+	if (str_len % 2)

+	{

+		ERROR("BUG: gdb value with uneven number of characters encountered");

+		exit(-1);

+	}

+

+	if (target->endianness == TARGET_LITTLE_ENDIAN)

+	{

+		for (i = 0; i < str_len; i+=2)

+		{

+			str[str_len - i - 1] = tstr[i + 1];

+			str[str_len - i - 2] = tstr[i];

+		}

+	}

+	else

+	{

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

+		{

+			str[i] = tstr[i];

+		}

+	}	

+}

+

+int gdb_get_registers_packet(connection_t *connection, target_t *target, char* packet, int packet_size)

+{

+	reg_t **reg_list;

+	int reg_list_size;

+	int retval;

+	int reg_packet_size = 0;

+	char *reg_packet;

+	char *reg_packet_p;

+	int i;

+

+#ifdef _DEBUG_GDB_IO_

+	DEBUG("-");

+#endif

+

+	if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)

+	{

+		switch (retval)

+		{

+			case ERROR_TARGET_NOT_HALTED:

+				ERROR("gdb requested registers but we're not halted, dropping connection");

+				return ERROR_SERVER_REMOTE_CLOSED;

+			default:

+				/* this is a bug condition - get_gdb_reg_list() may not return any other error */

+				ERROR("BUG: unexpected error returned by get_gdb_reg_list()");

+				exit(-1);

+		}

+	}

+

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

+	{

+		reg_packet_size += reg_list[i]->size;

+	}

+

+	reg_packet = malloc(CEIL(reg_packet_size, 8) * 2);

+	reg_packet_p = reg_packet;

+

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

+	{

+		gdb_str_to_target(target, reg_packet_p, reg_list[i]);

+		reg_packet_p += CEIL(reg_list[i]->size, 8) * 2;

+	}

+

+#ifdef _DEBUG_GDB_IO_

+	{

+		char *reg_packet_p;

+		reg_packet_p = strndup(reg_packet, CEIL(reg_packet_size, 8) * 2);

+		DEBUG("reg_packet: %s", reg_packet_p);

+		free(reg_packet_p);

+	}

+#endif

+

+	gdb_put_packet(connection, reg_packet, CEIL(reg_packet_size, 8) * 2);

+	free(reg_packet);

+

+	free(reg_list);

+

+	return ERROR_OK;

+}

+

+int gdb_set_registers_packet(connection_t *connection, target_t *target, char *packet, int packet_size)

+{

+	int i;

+	reg_t **reg_list;

+	int reg_list_size;

+	int retval;

+	char *packet_p;

+

+#ifdef _DEBUG_GDB_IO_

+	DEBUG("-");

+#endif

+

+	/* skip command character */

+	packet++;

+	packet_size--;

+

+	if (packet_size % 2)

+	{

+		WARNING("GDB set_registers packet with uneven characters received, dropping connection");

+		return ERROR_SERVER_REMOTE_CLOSED;

+	}

+

+	if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)

+	{

+		switch (retval)

+		{

+			case ERROR_TARGET_NOT_HALTED:

+				ERROR("gdb tried to registers but we're not halted, dropping connection");

+				return ERROR_SERVER_REMOTE_CLOSED;

+			default:

+				/* this is a bug condition - get_gdb_reg_list() may not return any other error */

+				ERROR("BUG: unexpected error returned by get_gdb_reg_list()");

+				exit(-1);

+		}

+	}

+

+	packet_p = packet;

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

+	{

+		u8 *bin_buf;

+		char *hex_buf;

+		reg_arch_type_t *arch_type;

+

+		/* convert from GDB-string (target-endian) to hex-string (big-endian) */

+		hex_buf = malloc(CEIL(reg_list[i]->size, 8) * 2);

+		gdb_target_to_str(target, packet_p, hex_buf);

+

+		/* convert hex-string to binary buffer */

+		bin_buf = malloc(CEIL(reg_list[i]->size, 8));

+		str_to_buf(hex_buf, CEIL(reg_list[i]->size, 8) * 2, bin_buf, reg_list[i]->size, 16);

+

+		/* get register arch_type, and call set method */	

+		arch_type = register_get_arch_type(reg_list[i]->arch_type);

+		if (arch_type == NULL)

+		{

+			ERROR("BUG: encountered unregistered arch type");

+			exit(-1);

+		}

+		arch_type->set(reg_list[i], bin_buf);

+

+		/* advance packet pointer */		

+		packet_p += (CEIL(reg_list[i]->size, 8) * 2);

+

+		free(bin_buf);

+		free(hex_buf);

+	}

+

+	/* free reg_t *reg_list[] array allocated by get_gdb_reg_list */ 

+	free(reg_list);

+

+	gdb_put_packet(connection, "OK", 2);

+

+	return ERROR_OK;

+}

+

+int gdb_get_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size)

+{

+	char *reg_packet;

+	int reg_num = strtoul(packet + 1, NULL, 16);

+	reg_t **reg_list;

+	int reg_list_size;

+	int retval;

+

+#ifdef _DEBUG_GDB_IO_

+	DEBUG("-");

+#endif

+

+	if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)

+	{

+		switch (retval)

+		{

+			case ERROR_TARGET_NOT_HALTED:

+				ERROR("gdb requested registers but we're not halted, dropping connection");

+				return ERROR_SERVER_REMOTE_CLOSED;

+			default:

+				/* this is a bug condition - get_gdb_reg_list() may not return any other error */

+				ERROR("BUG: unexpected error returned by get_gdb_reg_list()");

+				exit(-1);

+		}

+	}

+

+	if (reg_list_size <= reg_num)

+	{

+		ERROR("gdb requested a non-existing register");

+		exit(-1);

+	}

+

+	reg_packet = malloc(CEIL(reg_list[reg_num]->size, 8) * 2);

+

+	gdb_str_to_target(target, reg_packet, reg_list[reg_num]);

+

+	gdb_put_packet(connection, reg_packet, CEIL(reg_list[reg_num]->size, 8) * 2);

+

+	free(reg_list);

+	free(reg_packet);

+

+	return ERROR_OK;

+}

+

+int gdb_set_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size)

+{

+	char *separator;

+	char *hex_buf;

+	u8 *bin_buf;

+	int reg_num = strtoul(packet + 1, &separator, 16);

+	reg_t **reg_list;

+	int reg_list_size;

+	int retval;

+	reg_arch_type_t *arch_type;

+

+	DEBUG("-");

+

+	if ((retval = target->type->get_gdb_reg_list(target, &reg_list, &reg_list_size)) != ERROR_OK)

+	{

+		switch (retval)

+		{

+			case ERROR_TARGET_NOT_HALTED:

+				ERROR("gdb tried to set a register but we're not halted, dropping connection");

+				return ERROR_SERVER_REMOTE_CLOSED;

+			default:

+				/* this is a bug condition - get_gdb_reg_list() may not return any other error */

+				ERROR("BUG: unexpected error returned by get_gdb_reg_list()");

+				exit(-1);

+		}

+	}

+

+	if (reg_list_size < reg_num)

+	{

+		ERROR("gdb requested a non-existing register");

+		return ERROR_SERVER_REMOTE_CLOSED;

+	}

+

+	if (*separator != '=')

+	{

+		ERROR("GDB 'set register packet', but no '=' following the register number");

+		return ERROR_SERVER_REMOTE_CLOSED;

+	}

+

+	/* convert from GDB-string (target-endian) to hex-string (big-endian) */

+	hex_buf = malloc(CEIL(reg_list[reg_num]->size, 8) * 2);

+	gdb_target_to_str(target, separator + 1, hex_buf);

+

+	/* convert hex-string to binary buffer */

+	bin_buf = malloc(CEIL(reg_list[reg_num]->size, 8));

+	str_to_buf(hex_buf, CEIL(reg_list[reg_num]->size, 8) * 2, bin_buf, reg_list[reg_num]->size, 16);

+

+	/* get register arch_type, and call set method */	

+	arch_type = register_get_arch_type(reg_list[reg_num]->arch_type);

+	if (arch_type == NULL)

+	{

+		ERROR("BUG: encountered unregistered arch type");

+		exit(-1);

+	}

+	arch_type->set(reg_list[reg_num], bin_buf);

+

+	gdb_put_packet(connection, "OK", 2);

+

+	free(bin_buf);

+	free(hex_buf);

+	free(reg_list);

+

+	return ERROR_OK;

+}

+

+int gdb_memory_packet_error(connection_t *connection, int retval)

+{

+	switch (retval)

+	{

+		case ERROR_TARGET_NOT_HALTED:

+			ERROR("gdb tried to read memory but we're not halted, dropping connection");

+			return ERROR_SERVER_REMOTE_CLOSED;

+		case ERROR_TARGET_DATA_ABORT:

+			gdb_send_error(connection, EIO);

+			break;

+		case ERROR_TARGET_TRANSLATION_FAULT:

+			gdb_send_error(connection, EFAULT);

+			break;

+		case ERROR_TARGET_UNALIGNED_ACCESS:

+			gdb_send_error(connection, EFAULT);

+			break;

+		default:

+			/* This could be that the target reset itself. */

+			ERROR("unexpected error %i. Dropping connection.", retval);

+			return ERROR_SERVER_REMOTE_CLOSED;

+	}

+

+	return ERROR_OK;

+}

+

+/* We don't have to worry about the default 2 second timeout for GDB packets,

+ * because GDB breaks up large memory reads into smaller reads.

+ * 

+ * 8191 bytes by the looks of it. Why 8191 bytes instead of 8192?????

+ */

+int gdb_read_memory_packet(connection_t *connection, target_t *target, char *packet, int packet_size)

+{

+	char *separator;

+	u32 addr = 0;

+	u32 len = 0;

+

+	u8 *buffer;

+	char *hex_buffer;

+

+	int retval = ERROR_OK;

+

+	/* skip command character */

+	packet++;

+

+	addr = strtoul(packet, &separator, 16);

+

+	if (*separator != ',')

+	{

+		ERROR("incomplete read memory packet received, dropping connection");

+		return ERROR_SERVER_REMOTE_CLOSED;

+	}

+

+	len = strtoul(separator+1, NULL, 16);

+

+	buffer = malloc(len);

+

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

+

+	retval = target_read_buffer(target, addr, len, buffer);

+

+	if ((retval == ERROR_TARGET_DATA_ABORT) && (!gdb_report_data_abort))

+	{

+		/* TODO : Here we have to lie and send back all zero's lest stack traces won't work.

+		 * At some point this might be fixed in GDB, in which case this code can be removed.

+		 * 

+		 * OpenOCD developers are acutely aware of this problem, but there is nothing

+		 * gained by involving the user in this problem that hopefully will get resolved

+		 * eventually

+		 * 

+		 * http://sourceware.org/cgi-bin/gnatsweb.pl?cmd=view%20audit-trail&database=gdb&pr=2395

+		 *

+		 * For now, the default is to fix up things to make current GDB versions work.

+		 * This can be overwritten using the gdb_report_data_abort <'enable'|'disable'> command.

+		 */

+		memset(buffer, 0, len);

+		retval = ERROR_OK;

+	}

+

+	if (retval == ERROR_OK)

+	{

+		hex_buffer = malloc(len * 2 + 1);

+

+		int i;

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

+		{

+			u8 t = buffer[i];

+			hex_buffer[2 * i] = DIGITS[(t >> 4) & 0xf];

+			hex_buffer[2 * i + 1] = DIGITS[t & 0xf];

+		}

+

+		gdb_put_packet(connection, hex_buffer, len * 2);

+

+		free(hex_buffer);

+	}

+	else

+	{

+		retval = gdb_memory_packet_error(connection, retval);

+	}

+

+	free(buffer);

+

+	return retval;

+}

+

+int gdb_write_memory_packet(connection_t *connection, target_t *target, char *packet, int packet_size)

+{

+	char *separator;

+	u32 addr = 0;

+	u32 len = 0;

+

+	u8 *buffer;

+

+	int i;

+	int retval;

+

+	/* skip command character */

+	packet++;

+

+	addr = strtoul(packet, &separator, 16);

+

+	if (*separator != ',')

+	{

+		ERROR("incomplete write memory packet received, dropping connection");

+		return ERROR_SERVER_REMOTE_CLOSED;

+	}

+

+	len = strtoul(separator+1, &separator, 16);

+

+	if (*(separator++) != ':')

+	{

+		ERROR("incomplete write memory packet received, dropping connection");

+		return ERROR_SERVER_REMOTE_CLOSED;

+	}

+

+	buffer = malloc(len);

+

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

+

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

+	{

+		u32 tmp;

+		sscanf(separator + 2*i, "%2x", &tmp);

+		buffer[i] = tmp;

+	}

+

+	retval = target_write_buffer(target, addr, len, buffer);

+

+	if (retval == ERROR_OK)

+	{

+		gdb_put_packet(connection, "OK", 2);

+	}

+	else

+	{

+		if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK)

+			return retval; 

+	}

+

+	free(buffer);

+

+	return ERROR_OK;

+}

+

+int gdb_write_memory_binary_packet(connection_t *connection, target_t *target, char *packet, int packet_size)

+{

+	char *separator;

+	u32 addr = 0;

+	u32 len = 0;

+

+	int retval;

+

+	/* skip command character */

+	packet++;

+

+	addr = strtoul(packet, &separator, 16);

+

+	if (*separator != ',')

+	{

+		ERROR("incomplete write memory binary packet received, dropping connection");

+		return ERROR_SERVER_REMOTE_CLOSED;

+	}

+

+	len = strtoul(separator+1, &separator, 16);

+

+	if (*(separator++) != ':')

+	{

+		ERROR("incomplete write memory binary packet received, dropping connection");

+		return ERROR_SERVER_REMOTE_CLOSED;

+	}

+

+	retval = ERROR_OK;

+	if (len)

+	{

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

+

+		retval = target_write_buffer(target, addr, len, (u8*)separator);

+	}

+

+	if (retval == ERROR_OK)

+	{

+		gdb_put_packet(connection, "OK", 2);

+	}

+	else

+	{

+		if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK)

+			return retval; 

+	}

+

+	return ERROR_OK;

+}

+

+void gdb_step_continue_packet(connection_t *connection, target_t *target, char *packet, int packet_size)

+{

+	int current = 0;

+	u32 address = 0x0;

+

+	DEBUG("-");

+

+	if (packet_size > 1)

+	{

+		packet[packet_size] = 0;

+		address = strtoul(packet + 1, NULL, 16);

+	}

+	else

+	{

+		current = 1;

+	}

+

+	if (packet[0] == 'c')

+	{

+		DEBUG("continue");

+		target->type->resume(target, current, address, 0, 0); /* resume at current address, don't handle breakpoints, not debugging */

+	}

+	else if (packet[0] == 's')

+	{

+		DEBUG("step");

+		target->type->step(target, current, address, 0); /* step at current or address, don't handle breakpoints */

+	}

+}

+

+int gdb_bp_wp_packet_error(connection_t *connection, int retval)

+{

+	switch (retval)

+	{

+		case ERROR_TARGET_NOT_HALTED:

+			ERROR("gdb tried to set a breakpoint but we're not halted, dropping connection");

+			return ERROR_SERVER_REMOTE_CLOSED;

+			break;

+		case ERROR_TARGET_RESOURCE_NOT_AVAILABLE:

+			gdb_send_error(connection, EBUSY);

+			break;

+		default:

+			ERROR("BUG: unexpected error %i", retval);

+			exit(-1);

+	}

+

+	return ERROR_OK;

+}

+

+int gdb_breakpoint_watchpoint_packet(connection_t *connection, target_t *target, char *packet, int packet_size)

+{

+	int type;

+	enum breakpoint_type bp_type = BKPT_SOFT /* dummy init to avoid warning */;

+	enum watchpoint_rw wp_type;

+	u32 address;

+	u32 size;

+	char *separator;

+	int retval;

+

+	DEBUG("-");

+

+	type = strtoul(packet + 1, &separator, 16);

+

+	if (type == 0)	/* memory breakpoint */

+		bp_type = BKPT_SOFT;

+	else if (type == 1) /* hardware breakpoint */

+		bp_type = BKPT_HARD;

+	else if (type == 2) /* write watchpoint */

+		wp_type = WPT_WRITE;

+	else if (type == 3) /* read watchpoint */

+		wp_type = WPT_READ;

+	else if (type == 4) /* access watchpoint */

+		wp_type = WPT_ACCESS;

+

+	if (*separator != ',')

+	{

+		ERROR("incomplete breakpoint/watchpoint packet received, dropping connection");

+		return ERROR_SERVER_REMOTE_CLOSED;

+	}

+

+	address = strtoul(separator+1, &separator, 16);

+

+	if (*separator != ',')

+	{

+		ERROR("incomplete breakpoint/watchpoint packet received, dropping connection");

+		return ERROR_SERVER_REMOTE_CLOSED;

+	}

+

+	size = strtoul(separator+1, &separator, 16);

+

+	switch (type)

+	{

+		case 0:

+		case 1:

+			if (packet[0] == 'Z')

+			{

+				if ((retval = breakpoint_add(target, address, size, bp_type)) != ERROR_OK)

+				{

+					if ((retval = gdb_bp_wp_packet_error(connection, retval)) != ERROR_OK)

+						return retval;

+				}

+				else

+				{

+					gdb_put_packet(connection, "OK", 2);

+				}

+			}

+			else

+			{

+				breakpoint_remove(target, address);

+				gdb_put_packet(connection, "OK", 2);

+			}

+			break;

+		case 2:

+		case 3:

+		case 4:

+		{

+			if (packet[0] == 'Z')

+			{

+				if ((retval = watchpoint_add(target, address, size, type-2, 0, 0xffffffffu)) != ERROR_OK)

+				{

+					if ((retval = gdb_bp_wp_packet_error(connection, retval)) != ERROR_OK)

+						return retval;

+				}

+				else

+				{

+					gdb_put_packet(connection, "OK", 2);

+				}

+			}

+			else

+			{

+				watchpoint_remove(target, address);

+				gdb_put_packet(connection, "OK", 2);

+			}

+			break;

+		}

+		default:

+			break;

+	}

+

+	return ERROR_OK;

+}

+

+/* print out a string and allocate more space as needed, mainly used for XML at this point */

+void xml_printf(int *retval, char **xml, int *pos, int *size, const char *fmt, ...)

+{

+	if (*retval != ERROR_OK)

+	{

+		return;

+	}

+	int first = 1;

+	

+	for (;;)

+	{

+		if ((*xml == NULL) || (!first))

+		{

+			/* start by 0 to exercise all the code paths.

+			 * Need minimum 2 bytes to fit 1 char and 0 terminator. */

+			 

+			*size = *size * 2 + 2;

+			char *t = *xml;

+			*xml = realloc(*xml, *size);

+			if (*xml == NULL)

+			{

+				if (t)

+					free(t);

+				*retval = ERROR_SERVER_REMOTE_CLOSED;

+				return;

+			}

+		}

+		

+	    va_list ap;

+	    int ret;

+	    va_start(ap, fmt);

+	    ret = vsnprintf(*xml + *pos, *size - *pos, fmt, ap);

+	    va_end(ap);

+	    if ((ret > 0) && ((ret + 1) < *size - *pos))

+	    {

+	    	*pos += ret;

+	    	return;

+	    }

+	    /* there was just enough or not enough space, allocate more. */

+	    first = 0;

+	}

+}

+

+static int decode_xfer_read(char *buf, char **annex, int *ofs, unsigned int *len)

+{

+	char *separator;

+	

+	/* Extract and NUL-terminate the annex. */

+	*annex = buf;

+	while (*buf && *buf != ':')

+		buf++;

+	if (*buf == '\0')

+		return -1;

+	*buf++ = 0;

+	

+	/* After the read marker and annex, qXfer looks like a

+	 * traditional 'm' packet. */

+	

+	*ofs = strtoul(buf, &separator, 16);

+

+	if (*separator != ',')

+		return -1;

+

+	*len = strtoul(separator+1, NULL, 16);

+	

+	return 0;

+}

+

+int gdb_calc_blocksize(flash_bank_t *bank)

+{

+	int i;

+	int block_size = 0xffffffff;

+	

+	/* loop through all sectors and return smallest sector size */

+	

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

+	{

+		if (bank->sectors[i].size < block_size)

+			block_size = bank->sectors[i].size;

+	}

+	

+	return block_size;

+}

+

+int gdb_query_packet(connection_t *connection, target_t *target, char *packet, int packet_size)

+{

+	command_context_t *cmd_ctx = connection->cmd_ctx;

+	

+	if (strstr(packet, "qRcmd,"))

+	{

+		if (packet_size > 6)

+		{

+			char *cmd;

+			int i;

+			cmd = malloc((packet_size - 6)/2 + 1);

+			for (i=0; i < (packet_size - 6)/2; i++)

+			{

+				u32 tmp;

+				sscanf(packet + 6 + 2*i, "%2x", &tmp);

+				cmd[i] = tmp;

+			}

+			cmd[(packet_size - 6)/2] = 0x0;

+			

+			/* We want to print all debug output to GDB connection */

+			log_setCallback(gdb_log_callback, connection);

+			target_call_timer_callbacks();

+			command_run_line(cmd_ctx, cmd);

+			free(cmd);

+		}

+		gdb_put_packet(connection, "OK", 2);

+		return ERROR_OK;

+	}

+	else if (strstr(packet, "qCRC:"))

+	{

+		if (packet_size > 5)

+		{

+			int retval;

+			char gdb_reply[10];

+			char *separator;

+			u32 checksum;

+			u32 addr = 0;

+			u32 len = 0;

+			

+			/* skip command character */

+			packet += 5;

+			

+			addr = strtoul(packet, &separator, 16);

+			

+			if (*separator != ',')

+			{

+				ERROR("incomplete read memory packet received, dropping connection");

+				return ERROR_SERVER_REMOTE_CLOSED;

+			}

+			

+			len = strtoul(separator + 1, NULL, 16);

+			

+			retval = target_checksum_memory(target, addr, len, &checksum);

+			

+			if (retval == ERROR_OK)

+			{

+				snprintf(gdb_reply, 10, "C%8.8x", checksum);

+				gdb_put_packet(connection, gdb_reply, 9);

+			}

+			else

+			{

+				if ((retval = gdb_memory_packet_error(connection, retval)) != ERROR_OK)

+					return retval; 

+			}

+			

+			return ERROR_OK;

+		}

+	}

+	else if (strstr(packet, "qSupported"))

+	{

+		/* we currently support packet size and qXfer:memory-map:read (if enabled)

+		 * disable qXfer:features:read for the moment */

+		int retval = ERROR_OK;

+		char *buffer = NULL;

+		int pos = 0;

+		int size = 0;

+

+		xml_printf(&retval, &buffer, &pos, &size, 

+				"PacketSize=%x;qXfer:memory-map:read%c;qXfer:features:read-",

+				(GDB_BUFFER_SIZE - 1), gdb_use_memory_map == 1 ? '+' : '-');

+		

+		if (retval != ERROR_OK)

+		{

+			gdb_send_error(connection, 01);

+			return ERROR_OK;

+		}

+		

+		gdb_put_packet(connection, buffer, strlen(buffer));

+		free(buffer);

+		

+		return ERROR_OK;

+	}

+	else if (strstr(packet, "qXfer:memory-map:read::"))

+	{

+		/* We get away with only specifying flash here. Regions that are not

+		 * specified are treated as if we provided no memory map(if not we 

+		 * could detect the holes and mark them as RAM).

+		 * Normally we only execute this code once, but no big deal if we

+		 * have to regenerate it a couple of times. */

+		 

+		flash_bank_t *p;

+		char *xml = NULL;

+		int size = 0;

+		int pos = 0;

+		int retval = ERROR_OK;

+		

+		int offset;

+		int length;

+		char *separator;

+		int blocksize;

+		

+		/* skip command character */

+		packet += 23;

+		

+		offset = strtoul(packet, &separator, 16);

+		length = strtoul(separator + 1, &separator, 16);

+		

+		xml_printf(&retval, &xml, &pos, &size, "<memory-map>\n");

+		

+		int i = 0;

+		for (;;)

+		{

+			p = get_flash_bank_by_num(i);

+			if (p == NULL)

+				break;

+			

+			/* if device has uneven sector sizes, eg. str7, lpc

+			 * we pass the smallest sector size to gdb memory map */

+			blocksize = gdb_calc_blocksize(p);

+			

+			xml_printf(&retval, &xml, &pos, &size, "<memory type=\"flash\" start=\"0x%x\" length=\"0x%x\">\n" \

+				"<property name=\"blocksize\">0x%x</property>\n" \

+				"</memory>\n", \

+				p->base, p->size, blocksize);

+			i++;

+		}

+		

+		xml_printf(&retval, &xml, &pos, &size, "</memory-map>\n");

+

+		if (retval != ERROR_OK)

+		{

+			gdb_send_error(connection, retval);

+			return retval;

+		}

+				

+		if (offset + length > pos)

+		{

+			length = pos - offset;

+		}

+

+		char *t = malloc(length + 1);

+		t[0] = 'l';

+		memcpy(t + 1, xml + offset, length);

+		gdb_put_packet(connection, t, length + 1);

+		

+		free(t);

+		free(xml);

+		return ERROR_OK;

+	}

+	else if (strstr(packet, "qXfer:features:read:"))

+	{		 

+		char *xml = NULL;

+		int size = 0;

+		int pos = 0;

+		int retval = ERROR_OK;

+		

+		int offset;

+		unsigned int length;

+		char *annex;

+		

+		/* skip command character */

+		packet += 20;

+		

+		if (decode_xfer_read(packet, &annex, &offset, &length) < 0)

+		{

+			gdb_send_error(connection, 01);

+			return ERROR_OK;

+		}

+		

+		if (strcmp(annex, "target.xml") != 0)

+		{

+			gdb_send_error(connection, 01);

+			return ERROR_OK;

+		}

+				

+		xml_printf(&retval, &xml, &pos, &size, \

+			"l<target version=\"1.0\">\n<architecture>arm</architecture>\n</target>\n");

+					

+		if (retval != ERROR_OK)

+		{

+			gdb_send_error(connection, retval);

+			return retval;

+		}

+		

+		gdb_put_packet(connection, xml, strlen(xml) + 1);

+		

+		free(xml);

+		return ERROR_OK;

+	}

+	

+	gdb_put_packet(connection, "", 0);

+	return ERROR_OK;

+}

+

+int gdb_v_packet(connection_t *connection, target_t *target, char *packet, int packet_size)

+{

+	gdb_connection_t *gdb_connection = connection->priv;

+	gdb_service_t *gdb_service = connection->service->priv;

+	int result;

+

+	/* if flash programming disabled - send a empty reply */

+	

+	if (gdb_flash_program == 0)

+	{

+		gdb_put_packet(connection, "", 0);

+		return ERROR_OK;

+	}

+	

+	if (strstr(packet, "vFlashErase:"))

+	{

+		unsigned long addr;

+		unsigned long length;

+	

+		char *parse = packet + 12;

+		if (*parse == '\0')

+		{

+			ERROR("incomplete vFlashErase packet received, dropping connection");

+			return ERROR_SERVER_REMOTE_CLOSED;

+		}

+

+		addr = strtoul(parse, &parse, 16);

+

+		if (*(parse++) != ',' || *parse == '\0')

+		{

+			ERROR("incomplete vFlashErase packet received, dropping connection");

+			return ERROR_SERVER_REMOTE_CLOSED;

+		}

+

+		length = strtoul(parse, &parse, 16);

+

+		if (*parse != '\0')

+		{

+			ERROR("incomplete vFlashErase packet received, dropping connection");

+			return ERROR_SERVER_REMOTE_CLOSED;

+		}

+		

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

+		 * when flash_write is called multiple times */

+		flash_set_dirty();

+		

+		/* perform any target specific operations before the erase */

+		target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_PROGRAM);

+		

+		/* perform erase */

+		if ((result = flash_erase_address_range(gdb_service->target, addr, length)) != ERROR_OK)

+		{

+			/* GDB doesn't evaluate the actual error number returned,

+			 * treat a failed erase as an I/O error

+			 */

+			gdb_send_error(connection, EIO);

+			ERROR("flash_erase returned %i", result);

+		}

+		else

+			gdb_put_packet(connection, "OK", 2);

+		

+		return ERROR_OK;

+	}

+

+	if (strstr(packet, "vFlashWrite:"))

+	{

+		unsigned long addr;

+		unsigned long length;

+		char *parse = packet + 12;

+

+		if (*parse == '\0')

+		{

+			ERROR("incomplete vFlashErase packet received, dropping connection");

+			return ERROR_SERVER_REMOTE_CLOSED;

+		}

+		addr = strtoul(parse, &parse, 16);

+		if (*(parse++) != ':')

+		{

+			ERROR("incomplete vFlashErase packet received, dropping connection");

+			return ERROR_SERVER_REMOTE_CLOSED;

+		}

+		length = packet_size - (parse - packet);

+		

+		/* create a new image if there isn't already one */

+		if (gdb_connection->vflash_image == NULL)

+		{

+			gdb_connection->vflash_image = malloc(sizeof(image_t));

+			image_open(gdb_connection->vflash_image, "", "build");

+		}

+

+		/* create new section with content from packet buffer */

+		image_add_section(gdb_connection->vflash_image, addr, length, 0x0, (u8*)parse);

+

+		gdb_put_packet(connection, "OK", 2);

+

+		return ERROR_OK;

+	}

+

+	if (!strcmp(packet, "vFlashDone"))

+	{

+		u32 written;

+

+		/* process the flashing buffer. No need to erase as GDB

+		 * always issues a vFlashErase first. */

+		if ((result = flash_write(gdb_service->target, gdb_connection->vflash_image, &written, 0)) != ERROR_OK)

+		{

+			if (result == ERROR_FLASH_DST_OUT_OF_BANK)

+				gdb_put_packet(connection, "E.memtype", 9);

+			else

+				gdb_send_error(connection, EIO);

+			}

+		else

+		{

+			DEBUG("wrote %u bytes from vFlash image to flash", written);

+			gdb_put_packet(connection, "OK", 2);

+		}

+		

+		image_close(gdb_connection->vflash_image);

+		free(gdb_connection->vflash_image);

+		gdb_connection->vflash_image = NULL;

+		

+		return ERROR_OK;

+	}

+

+	gdb_put_packet(connection, "", 0);

+	return ERROR_OK;

+}

+

+int gdb_detach(connection_t *connection, target_t *target)

+{

+	switch( detach_mode )

+	{

+		case GDB_DETACH_RESUME:

+			target->type->resume(target, 1, 0, 1, 0);

+			break;

+		

+		case GDB_DETACH_RESET:

+			target_process_reset(connection->cmd_ctx);

+			break;

+		

+		case GDB_DETACH_HALT:

+			target->type->halt(target);

+			break;

+		

+		case GDB_DETACH_NOTHING:

+			break;

+	}

+	

+	gdb_put_packet(connection, "OK", 2);

+	

+	return ERROR_OK;

+}

+

+static void gdb_log_callback(void *priv, const char *file, int line, 

+		const char *function, const char *format, va_list args)

+{

+	connection_t *connection = priv;

+	gdb_connection_t *gdb_con = connection->priv;

+	

+	if (gdb_con->busy)

+	{

+		/* do not reply this using the O packet */

+		return;

+	}

+

+	char *t = allocPrintf(format, args);

+	if (t == NULL)

+		return;

+	

+	gdb_output_con(connection, t); 

+	

+	free(t);

+}

+

+int gdb_input_inner(connection_t *connection)

+{

+	gdb_service_t *gdb_service = connection->service->priv;

+	target_t *target = gdb_service->target;

+	char packet[GDB_BUFFER_SIZE];

+	int packet_size;

+	int retval;

+	gdb_connection_t *gdb_con = connection->priv;

+	static int extended_protocol = 0;

+

+	/* drain input buffer */

+	do

+	{

+		packet_size = GDB_BUFFER_SIZE-1;

+		if ((retval = gdb_get_packet(connection, packet, &packet_size)) != ERROR_OK)

+		{

+			return retval;

+		}

+

+		/* terminate with zero */

+		packet[packet_size] = 0;

+

+		DEBUG("received packet: '%s'", packet);

+

+		if (packet_size > 0)

+		{

+			retval = ERROR_OK;

+			switch (packet[0])

+			{

+				case 'H':

+					/* Hct... -- set thread 

+					 * we don't have threads, send empty reply */

+					gdb_put_packet(connection, NULL, 0);

+					break;

+				case 'q':

+					retval = gdb_query_packet(connection, target, packet, packet_size);

+					break;

+				case 'g':

+					retval = gdb_get_registers_packet(connection, target, packet, packet_size);

+					break;

+				case 'G':

+					retval = gdb_set_registers_packet(connection, target, packet, packet_size);

+					break;

+				case 'p':

+					retval = gdb_get_register_packet(connection, target, packet, packet_size);

+					break;

+				case 'P':

+					retval = gdb_set_register_packet(connection, target, packet, packet_size);

+					break;

+				case 'm':

+					retval = gdb_read_memory_packet(connection, target, packet, packet_size);

+					break;

+				case 'M':

+					retval = gdb_write_memory_packet(connection, target, packet, packet_size);

+					break;

+				case 'z':

+				case 'Z':

+					retval = gdb_breakpoint_watchpoint_packet(connection, target, packet, packet_size);

+					break;

+				case '?':

+					gdb_last_signal_packet(connection, target, packet, packet_size);

+					break;

+				case 'c':

+				case 's':

+					{

+					/* We're running/stepping, in which case we can 

+					 * forward log output until the target is halted */

+						gdb_connection_t *gdb_con = connection->priv;

+						gdb_con->frontend_state = TARGET_RUNNING;

+						log_setCallback(gdb_log_callback, connection);

+						gdb_step_continue_packet(connection, target, packet, packet_size);

+					}

+					break;

+				case 'v':

+					retval = gdb_v_packet(connection, target, packet, packet_size);

+					break;

+				case 'D':

+					retval = gdb_detach(connection, target);

+					extended_protocol = 0;

+					break;

+				case 'X':

+					if ((retval = gdb_write_memory_binary_packet(connection, target, packet, packet_size)) != ERROR_OK)

+						return retval;

+					break;

+				case 'k':

+					if (extended_protocol != 0)

+						break;

+					gdb_put_packet(connection, "OK", 2);

+					return ERROR_SERVER_REMOTE_CLOSED;

+				case '!':

+					/* handle extended remote protocol */

+					extended_protocol = 1;

+					gdb_put_packet(connection, "OK", 2);

+					break;

+				case 'R':

+					/* handle extended restart packet */

+					target_process_reset(connection->cmd_ctx);

+					break;

+				default:

+					/* ignore unkown packets */

+					DEBUG("ignoring 0x%2.2x packet", packet[0]);

+					gdb_put_packet(connection, NULL, 0);

+					break;

+			}

+

+			/* if a packet handler returned an error, exit input loop */

+			if (retval != ERROR_OK)

+				return retval;

+		}

+

+		if (gdb_con->ctrl_c)

+		{

+			if (target->state == TARGET_RUNNING)

+			{

+				target->type->halt(target);

+				gdb_con->ctrl_c = 0;

+			}

+		}

+

+	} while (gdb_con->buf_cnt > 0);

+

+	return ERROR_OK;

+}

+

+int gdb_input(connection_t *connection)

+{

+	int retval = gdb_input_inner(connection);

+	if (retval == ERROR_SERVER_REMOTE_CLOSED)

+		return retval;

+	/* we'll recover from any other errors(e.g. temporary timeouts, etc.) */

+	return ERROR_OK;

+}

+

+int gdb_init()

+{

+	gdb_service_t *gdb_service;

+	target_t *target = targets;

+	int i = 0;

+

+	if (!target)

+	{

+		WARNING("no gdb ports allocated as no target has been specified");

+		return ERROR_OK;

+	}

+

+	if (gdb_port == 0)

+	{

+		WARNING("no gdb port specified, using default port 3333");

+		gdb_port = 3333;

+	}

+

+	while (target)

+	{

+		char service_name[8];

+

+		snprintf(service_name, 8, "gdb-%2.2i", i);

+

+		gdb_service = malloc(sizeof(gdb_service_t));

+		gdb_service->target = target;

+

+		add_service("gdb", CONNECTION_GDB, gdb_port + i, 1, gdb_new_connection, gdb_input, gdb_connection_closed, gdb_service);

+

+		DEBUG("gdb service for target %s at port %i", target->type->name, gdb_port + i);

+

+		i++;

+		target = target->next;

+	}

+

+	return ERROR_OK;

+}

+

+/* daemon configuration command gdb_port */

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

+{

+	if (argc == 0)

+		return ERROR_OK;

+

+	/* only if the port wasn't overwritten by cmdline */

+	if (gdb_port == 0)

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

+

+	return ERROR_OK;

+}

+

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

+{

+	if (argc == 1)

+	{

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

+		{

+			detach_mode = GDB_DETACH_RESUME;

+			return ERROR_OK;

+		}

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

+		{

+			detach_mode = GDB_DETACH_RESET;

+			return ERROR_OK;

+		}

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

+		{

+			detach_mode = GDB_DETACH_HALT;

+			return ERROR_OK;

+		}

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

+		{

+			detach_mode = GDB_DETACH_NOTHING;

+			return ERROR_OK;

+		}

+	}

+	

+	WARNING("invalid gdb_detach configuration directive: %s", args[0]);

+	return ERROR_OK;

+}

+

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

+{

+	if (argc == 1)

+	{

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

+		{

+			gdb_use_memory_map = 1;

+			return ERROR_OK;

+		}

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

+		{

+			gdb_use_memory_map = 0;

+			return ERROR_OK;

+		}

+	}

+	

+	WARNING("invalid gdb_memory_map configuration directive: %s", args[0]);

+	return ERROR_OK;

+}

+

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

+{

+	if (argc == 1)

+	{

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

+		{

+			gdb_flash_program = 1;

+			return ERROR_OK;

+		}

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

+		{

+			gdb_flash_program = 0;

+			return ERROR_OK;

+		}

+	}

+	

+	WARNING("invalid gdb_memory_map configuration directive: %s", args[0]);

+	return ERROR_OK;

+}

+

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

+{

+	if (argc == 1)

+	{

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

+		{

+			gdb_report_data_abort = 1;

+			return ERROR_OK;

+		}

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

+		{

+			gdb_report_data_abort = 0;

+			return ERROR_OK;

+		}

+	}

+	

+	WARNING("invalid gdb_report_data_abort configuration directive: %s", args[0]);

+	return ERROR_OK;

+}

+

+int gdb_register_commands(command_context_t *command_context)

+{

+	register_command(command_context, NULL, "gdb_port", handle_gdb_port_command,

+			COMMAND_CONFIG, "");

+	register_command(command_context, NULL, "gdb_detach", handle_gdb_detach_command,

+			COMMAND_CONFIG, "");

+	register_command(command_context, NULL, "gdb_memory_map", handle_gdb_memory_map_command,

+			COMMAND_CONFIG, "");

+	register_command(command_context, NULL, "gdb_flash_program", handle_gdb_flash_program_command,

+			COMMAND_CONFIG, "");

+	register_command(command_context, NULL, "gdb_report_data_abort", handle_gdb_report_data_abort_command,

+			COMMAND_CONFIG, "");

+	return ERROR_OK;

+}

diff --git a/src/target/target.h b/src/target/target.h
index 7c07288b..086b28bc 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -254,6 +254,9 @@ int target_write_u32(struct target_s *target, u32 address, u32 value);
 int target_write_u16(struct target_s *target, u32 address, u16 value);
 int target_write_u8(struct target_s *target, u32 address, u8 value);
 
+/* Issues USER() statements with target state information */
+int target_arch_state(struct target_s *target);
+
 #define ERROR_TARGET_INVALID	(-300)
 #define ERROR_TARGET_INIT_FAILED (-301)
 #define ERROR_TARGET_TIMEOUT	(-302)