- prepare OpenOCD for branching, created ./trunk/

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

78 files changed, 24274 insertions, 0 deletions

diff --git a/src/Makefile.am b/src/Makefile.am
new file mode 100644
index 00000000..e1973827
--- /dev/null
+++ b/src/Makefile.am
@@ -0,0 +1,40 @@
+bin_PROGRAMS = openocd
+openocd_SOURCES = openocd.c
+
+# set the include path found by configure
+INCLUDES = -I$(top_srcdir)/src/helper \
+	-I$(top_srcdir)/src/jtag -I$(top_srcdir)/src/target -I$(top_srcdir)/src/xsvf -I$(top_srcdir)/src/server \
+	-I$(top_srcdir)/src/flash $(all_includes)
+
+# the library search path.
+openocd_LDFLAGS = $(all_libraries) 
+SUBDIRS = helper jtag xsvf target server flash
+
+if FTDI2232
+FTDI2232LIB = -lftdi
+else
+FTDI2232LIB =
+endif
+
+if IS_CYGWIN
+if FTD2XXDIR
+FTD2XXLDADD = @WITH_FTD2XX@/FTD2XX.lib
+else
+FTD2XXLDADD = -lftd2xx
+endif
+else
+FTD2XXLDADD = -lftd2xx 
+endif
+  
+if FTD2XX
+FTD2XXLIB = $(FTD2XXLDADD)
+else
+FTD2XXLIB =
+endif
+
+openocd_LDADD = $(top_builddir)/src/xsvf/libxsvf.a \
+	$(top_builddir)/src/target/libtarget.a $(top_builddir)/src/jtag/libjtag.a \
+	$(top_builddir)/src/helper/libhelper.a \
+	$(top_builddir)/src/server/libserver.a $(top_builddir)/src/helper/libhelper.a \
+	$(top_builddir)/src/flash/libflash.a $(top_builddir)/src/target/libtarget.a \
+	$(FTDI2232LIB) $(FTD2XXLIB) 
diff --git a/src/flash/Makefile.am b/src/flash/Makefile.am
new file mode 100644
index 00000000..61e363c9
--- /dev/null
+++ b/src/flash/Makefile.am
@@ -0,0 +1,5 @@
+INCLUDES = -I$(top_srcdir)/src/helper -I$(top_srcdir)/src/jtag -I$(top_srcdir)/src/target $(all_includes)
+METASOURCES = AUTO
+noinst_LIBRARIES = libflash.a
+libflash_a_SOURCES = flash.c lpc2000.c cfi.c at91sam7.c str7x.c
+noinst_HEADERS = flash.h lpc2000.h cfi.h at91sam7.h str7x.h
diff --git a/src/flash/at91sam7.c b/src/flash/at91sam7.c
new file mode 100644
index 00000000..8a602a3f
--- /dev/null
+++ b/src/flash/at91sam7.c
@@ -0,0 +1,632 @@
+/***************************************************************************
+ *   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
+*
+ ***************************************************************************/
+
+#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_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);
+void at91sam7_set_flash_mode(flash_bank_t *bank,int mode);
+u8 at91sam7_wait_status_busy(flash_bank_t *bank, int timeout);
+int at91sam7_handle_part_id_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,
+	.erase_check = at91sam7_erase_check,
+	.protect_check = at91sam7_protect_check,
+	.info = at91sam7_info
+};
+
+
+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 */
+};
+
+u32 at91sam7_get_flash_status(flash_bank_t *bank)
+{
+	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;
+	target_t *target = at91sam7_info->target;
+	long fsr;
+	
+	target->type->read_memory(target, MC_FSR, 4, 1, (u8 *)&fsr);
+	
+	return fsr;
+}
+
+/* Setup the timimg registers for nvbits or normal flash */
+void at91sam7_set_flash_mode(flash_bank_t *bank,int mode)
+{
+	u32 fmcn, fmr;
+	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;
+	target_t *target = at91sam7_info->target;
+	
+	if (mode != at91sam7_info->flashmode) {
+		/* mainf contains the number of main clocks in approx 500uS */
+		if (mode==1)
+			/* main clocks in 1uS */
+			fmcn = (at91sam7_info->mainf>>9)+1;
+		else
+			/* main clocks in 1.5uS */
+			fmcn = (at91sam7_info->mainf>>9)+(at91sam7_info->mainf>>10)+1;
+		DEBUG("fmcn: %i", fmcn); 
+		fmr = fmcn<<16;
+		target->type->write_memory(target, MC_FSR, 4, 1, (u8 *)&fmr);
+		at91sam7_info->flashmode = mode;		
+	}
+}
+
+u8 at91sam7_wait_status_busy(flash_bank_t *bank, int timeout)
+{
+	u32 status;
+	
+	while ((!((status = at91sam7_get_flash_status(bank)) & 0x01)) && (timeout-- > 0))
+	{
+		DEBUG("status: 0x%x", status);
+		usleep(1000);
+	}
+	
+	DEBUG("status: 0x%x", 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;
+}
+
+int at91sam7_flash_command(struct flash_bank_s *bank,u8 cmd,u16 pagen) 
+{
+	u32 fcr;
+	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;
+	target_t *target = at91sam7_info->target;
+
+	fcr = (0x5A<<24) | (pagen<<8) | cmd; 
+	target->type->write_memory(target, MC_FCR, 4, 1, (u8 *)&fcr);
+	DEBUG("Flash command: 0x%x, pagenumber:", fcr, pagen);
+
+	if (at91sam7_wait_status_busy(bank, 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 = at91sam7_info->target;
+	unsigned long cidr, mcfr, status;
+	
+	if (at91sam7_info->target->state != TARGET_HALTED)
+	{
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	
+	/* Read and parse chip identification register */
+	target->type->read_memory(target, DBGU_CIDR, 4, 1, (u8 *)&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];
+	
+	DEBUG("nvptyp: 0x%3.3x, arch: 0x%4.4x, alt_id: 0x%4.4x, alt_addr: 0x%4.4x", at91sam7_info->cidr_nvptyp, at91sam7_info->cidr_arch );
+
+	/* Read main clock freqency register */
+	target->type->read_memory(target, CKGR_MCFR, 4, 1, (u8 *)&mcfr);
+	if (mcfr&0x10000)
+	{
+		at91sam7_info->mainrdy = 1;
+		at91sam7_info->mainf = mcfr&0xFFFF;
+		at91sam7_info->usec_clocks = mcfr>>9;
+	}
+	else 
+	{
+		at91sam7_info->mainrdy = 0;
+		at91sam7_info->mainf = 0;
+		at91sam7_info->usec_clocks = 0;
+	}
+	
+	status = at91sam7_get_flash_status(bank);
+	at91sam7_info->lockbits = status>>16;
+	at91sam7_info->securitybit = (status>>4)&0x01;
+	
+	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==0x40000)  /* 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->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->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->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 = 2;
+		at91sam7_info->nvmbits = (status>>8)&0x03;
+		bank->base = 0x100000;
+		bank->bus_width = 4;
+		if (bank->size==0x40000)  /* 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->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 == 0x75 ) {
+		at91sam7_info->num_nvmbits = 3;
+		at91sam7_info->nvmbits = (status>>8)&0x07;
+		bank->base = 0x100000;
+		bank->bus_width = 4;
+		if (bank->size==0x40000)  /* 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->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 != 0x70 ) 
+	{
+		   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;
+	target_t *target = at91sam7_info->target;
+	int i;
+	
+	if (!at91sam7_info->working_area_size)
+	{
+	}
+	else
+	{	
+	}
+	
+	return ERROR_OK;
+}
+
+int at91sam7_protect_check(struct flash_bank_s *bank)
+{
+	u32 status;
+	
+	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;
+	target_t *target = at91sam7_info->target;
+
+	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;
+	}
+		
+	status = at91sam7_get_flash_status(bank);
+	at91sam7_info->lockbits = status>>16;
+	
+	return ERROR_OK;
+}
+
+
+int at91sam7_register_commands(struct command_context_s *cmd_ctx)
+{
+	command_t *at91sam7_cmd = register_command(cmd_ctx, NULL, "cfi", NULL, COMMAND_ANY, NULL);
+
+	return ERROR_OK;
+}
+
+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;
+	
+	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->target = get_target_by_num(strtoul(args[5], NULL, 0));
+	if (!at91sam7_info->target)
+	{
+		ERROR("no target '%i' configured", args[5]);
+		exit(-1);
+	}
+	
+	
+	/* part wasn't probed for info yet */
+	at91sam7_info->cidr = 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;
+	
+	if (at91sam7_info->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 >= at91sam7_info->num_lockbits))
+	{
+		return ERROR_FLASH_SECTOR_INVALID;
+	}
+
+        if ((first == 0) && (last == (at91sam7_info->num_lockbits-1)))
+        {
+        	return at91sam7_flash_command(bank, EA, 0);
+        }
+        
+	WARNING("Can only erase the whole flash area, pages are autoerased on write");
+	return ERROR_FLASH_OPERATION_FAILED;
+}
+
+int at91sam7_protect(struct flash_bank_s *bank, int set, int first, int last)
+{
+	u32 cmd, pagen, status;
+	int lockregion;
+	
+	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;
+	target_t *target = at91sam7_info->target;
+	
+	if (at91sam7_info->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;
+	}
+	
+	/* Configure the flash controller timing */	
+	at91sam7_set_flash_mode(bank,1);
+	
+	for (lockregion=first;lockregion<=last;lockregion++) 
+	{
+		pagen = lockregion*at91sam7_info->pages_in_lockregion;	
+		if (set)
+			 cmd = SLB; 
+		else
+			 cmd = CLB; 		
+		if (at91sam7_flash_command(bank, cmd, pagen) != ERROR_OK) 
+		{
+			return ERROR_FLASH_OPERATION_FAILED;
+		}	
+	}
+	
+	status = at91sam7_get_flash_status(bank);
+	at91sam7_info->lockbits = status>>16;
+		
+	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 = at91sam7_info->target;
+	u32 dst_min_alignment, wcount, bytes_remaining = count;
+	u32 first_page, last_page, pagen, buffer_pos;
+	u32 fcr;
+	
+	if (at91sam7_info->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 (offset + count > bank->size)
+		return ERROR_FLASH_DST_OUT_OF_BANK;
+	
+	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);
+	
+	/* Configure the flash controller timing */	
+	at91sam7_set_flash_mode(bank,2);
+
+	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, 4, wcount, buffer+buffer_pos);
+		
+		/* Send Write Page command to Flash Controller */
+		if (at91sam7_flash_command(bank, WP, pagen) != ERROR_OK) 
+		{
+			return ERROR_FLASH_OPERATION_FAILED;
+		}	
+		DEBUG("Flash command: 0x%x, pagenumber:", fcr, 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;
+	
+	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;
+	}
+	return ERROR_OK;
+}
+
+int at91sam7_info(struct flash_bank_s *bank, char *buf, int buf_size)
+{
+	int printed;
+	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;
+	
+	if (at91sam7_info->cidr == 0)
+	{
+		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:\n");
+	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, "main clock(estimated): %ikHz \n", at91sam7_info->mainf*2);
+	buf += printed;
+	buf_size -= printed;
+	
+	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,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;
+}
diff --git a/src/flash/at91sam7.h b/src/flash/at91sam7.h
new file mode 100644
index 00000000..8f9e3db7
--- /dev/null
+++ b/src/flash/at91sam7.h
@@ -0,0 +1,83 @@
+/***************************************************************************
+ *   Copyright (C) 2006 by Magnus Lundin                                   *
+ *   lundinªmlu.mine.nu                                                    *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifndef AT91SAM7_H
+#define AT91SAM7_H
+
+#include "flash.h"
+#include "target.h"
+
+typedef struct at91sam7_flash_bank_s
+{
+	struct target_s *target;
+	u32 working_area;
+	u32 working_area_size;
+		
+	/* chip id register */
+	u32 cidr;
+	u16 cidr_ext;
+	u16 cidr_nvptyp;
+	u16 cidr_arch;
+	u16 cidr_sramsiz;
+	u16 cidr_nvpsiz;
+	u16 cidr_nvpsiz2;
+	u16 cidr_eproc;
+	u16 cidr_version;
+		
+	/* flash geometry */
+	u16 num_pages;
+	u16 pagesize;
+	u16 pages_in_lockregion;
+	u8 num_erase_regions;
+	u32 *erase_region_info;
+
+        /* nv memory bits */
+	u16 num_lockbits;
+	u16 lockbits;
+	u16 num_nvmbits;
+	u16 nvmbits;
+	u8  securitybit;
+	u8  flashmode;         /* 0: not init, 1: fmcn for nvbits (1uS), 2: fmcn for flash (1.5uS) */
+	
+	/* main clock status */
+	u8  mainrdy;
+	u16 mainf;
+	u16 usec_clocks;
+	
+} at91sam7_flash_bank_t;
+
+/* AT91SAM7 control registers */
+#define DBGU_CIDR 0xFFFFF240
+#define CKGR_MCFR 0xFFFFFC24
+#define MC_FMR	0xFFFFFF60
+#define MC_FCR	0xFFFFFF64
+#define MC_FSR	0xFFFFFF68
+
+/* Flash Controller Commands */
+#define  WP   0x01
+#define  SLB  0x02
+#define  WPL  0x03
+#define  CLB  0x04
+#define  EA   0x08
+#define  SGPB 0x0B
+#define  CGPB 0x0D
+#define  SSB  0x0F
+
+
+#endif /* AT91SAM7_H */
diff --git a/src/flash/cfi.c b/src/flash/cfi.c
new file mode 100644
index 00000000..5e943676
--- /dev/null
+++ b/src/flash/cfi.c
@@ -0,0 +1,1194 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "cfi.h"
+
+#include "flash.h"
+#include "target.h"
+#include "log.h"
+#include "armv4_5.h"
+#include "algorithm.h"
+#include "binarybuffer.h"
+#include "types.h"
+
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+int cfi_register_commands(struct command_context_s *cmd_ctx);
+int cfi_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);
+int cfi_erase(struct flash_bank_s *bank, int first, int last);
+int cfi_protect(struct flash_bank_s *bank, int set, int first, int last);
+int cfi_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);
+int cfi_probe(struct flash_bank_s *bank);
+int cfi_erase_check(struct flash_bank_s *bank);
+int cfi_protect_check(struct flash_bank_s *bank);
+int cfi_info(struct flash_bank_s *bank, char *buf, int buf_size);
+
+int cfi_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+#define CFI_MAX_BUS_WIDTH	4
+
+flash_driver_t cfi_flash =
+{
+	.name = "cfi",
+	.register_commands = cfi_register_commands,
+	.flash_bank_command = cfi_flash_bank_command,
+	.erase = cfi_erase,
+	.protect = cfi_protect,
+	.write = cfi_write,
+	.probe = cfi_probe,
+	.erase_check = cfi_erase_check,
+	.protect_check = cfi_protect_check,
+	.info = cfi_info
+};
+
+inline u32 flash_address(flash_bank_t *bank, int sector, u32 offset)
+{
+	/* while the sector list isn't built, only accesses to sector 0 work */
+	if (sector == 0)
+		return bank->base + offset * bank->bus_width;
+	else
+	{
+		if (!bank->sectors)
+		{
+			ERROR("BUG: sector list not yet built");
+			exit(-1);
+		}
+		return bank->base + bank->sectors[sector].offset + offset * bank->bus_width;
+	}
+
+}
+
+void cfi_command(flash_bank_t *bank, u8 cmd, u8 *cmd_buf)
+{
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	int i;
+	
+	if (cfi_info->target->endianness == TARGET_LITTLE_ENDIAN)
+	{
+		for (i = bank->bus_width; i > 0; i--)
+		{
+			*cmd_buf++ = (i & (bank->chip_width - 1)) ? 0x0 : cmd;
+		}
+	}
+	else
+	{
+		for (i = 1; i <= bank->bus_width; i++)
+		{
+			*cmd_buf++ = (i & (bank->chip_width - 1)) ? 0x0 : cmd;
+		}
+	}
+}
+
+/* read unsigned 8-bit value from the bank
+ * flash banks are expected to be made of similar chips
+ * the query result should be the same for all
+ */
+u8 cfi_query_u8(flash_bank_t *bank, int sector, u32 offset)
+{
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	target_t *target = cfi_info->target;
+	u8 data[CFI_MAX_BUS_WIDTH];
+		
+	target->type->read_memory(target, flash_address(bank, sector, offset), bank->bus_width, 1, data);
+	
+	if (cfi_info->target->endianness == TARGET_LITTLE_ENDIAN)
+		return data[0];
+	else
+		return data[bank->bus_width - 1];
+}
+
+/* read unsigned 8-bit value from the bank
+ * in case of a bank made of multiple chips,
+ * the individual values are ORed
+ */
+u8 cfi_get_u8(flash_bank_t *bank, int sector, u32 offset)
+{
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	target_t *target = cfi_info->target;
+	u8 data[CFI_MAX_BUS_WIDTH];
+	int i;
+	
+	target->type->read_memory(target, flash_address(bank, sector, offset), bank->bus_width, 1, data);
+	
+	if (cfi_info->target->endianness == TARGET_LITTLE_ENDIAN)
+	{
+		for (i = 0; i < bank->bus_width / bank->chip_width; i++)
+			data[0] |= data[i];
+		
+		return data[0];
+	}
+	else
+	{
+		u8 value = 0;
+		for (i = 0; i < bank->bus_width / bank->chip_width; i++)
+			value |= data[bank->bus_width - 1 - i];
+		
+		return value;
+	}
+}
+
+u16 cfi_query_u16(flash_bank_t *bank, int sector, u32 offset)
+{
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	target_t *target = cfi_info->target;
+	u8 data[CFI_MAX_BUS_WIDTH * 2];
+	
+	target->type->read_memory(target, flash_address(bank, sector, offset), bank->bus_width, 2, data);
+
+	if (cfi_info->target->endianness == TARGET_LITTLE_ENDIAN)
+		return data[0] | data[bank->bus_width] << 8;
+	else
+		return data[bank->bus_width - 1] | data[(2 * bank->bus_width) - 1] << 8;
+}
+
+u32 cfi_query_u32(flash_bank_t *bank, int sector, u32 offset)
+{
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	target_t *target = cfi_info->target;
+	u8 data[CFI_MAX_BUS_WIDTH * 4];
+	
+	target->type->read_memory(target, flash_address(bank, sector, offset), bank->bus_width, 4, data);
+	
+	if (cfi_info->target->endianness == TARGET_LITTLE_ENDIAN)
+		return data[0] | data[bank->bus_width] << 8 | data[bank->bus_width * 2] << 16 | data[bank->bus_width * 3] << 24;
+	else
+		return data[bank->bus_width - 1] | data[(2* bank->bus_width) - 1] << 8 | 
+				data[(3 * bank->bus_width) - 1] << 16 | data[(4 * bank->bus_width) - 1] << 24;
+}
+
+void cfi_intel_clear_status_register(flash_bank_t *bank)
+{
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	target_t *target = cfi_info->target;
+	u8 command[8];
+	
+	if (target->state != TARGET_HALTED)
+	{
+		ERROR("BUG: attempted to clear status register while target wasn't halted");
+		exit(-1);
+	}
+	
+	cfi_command(bank, 0x50, command);
+	target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+}
+
+u8 cfi_intel_wait_status_busy(flash_bank_t *bank, int timeout)
+{
+	u8 status;
+	
+	while ((!((status = cfi_get_u8(bank, 0, 0x0)) & 0x80)) && (timeout-- > 0))
+	{
+		DEBUG("status: 0x%x", status);
+		usleep(1000);
+	}
+	
+	DEBUG("status: 0x%x", status);
+
+	if (status != 0x80)
+	{
+		ERROR("status register: 0x%x", status);
+		if (status & 0x2)
+			ERROR("Block Lock-Bit Detected, Operation Abort");
+		if (status & 0x4)
+			ERROR("Program suspended");
+		if (status & 0x8)
+			ERROR("Low Programming Voltage Detected, Operation Aborted");
+		if (status & 0x10)
+			ERROR("Program Error / Error in Setting Lock-Bit");
+		if (status & 0x20)
+			ERROR("Error in Block Erasure or Clear Lock-Bits");
+		if (status & 0x40)
+			ERROR("Block Erase Suspended");
+		
+		cfi_intel_clear_status_register(bank);
+	}
+	
+	return status;
+}
+int cfi_read_intel_pri_ext(flash_bank_t *bank)
+{
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	cfi_intel_pri_ext_t *pri_ext = malloc(sizeof(cfi_intel_pri_ext_t));
+	target_t *target = cfi_info->target;
+	u8 command[8];
+
+	cfi_info->pri_ext = pri_ext;
+	
+	pri_ext->pri[0] = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0);
+	pri_ext->pri[1] = cfi_query_u8(bank, 0, cfi_info->pri_addr + 1);
+	pri_ext->pri[2] = cfi_query_u8(bank, 0, cfi_info->pri_addr + 2);
+	
+	if ((pri_ext->pri[0] != 'P') || (pri_ext->pri[1] != 'R') || (pri_ext->pri[2] != 'I'))
+	{
+		cfi_command(bank, 0xf0, command);
+		target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+		cfi_command(bank, 0xff, command);
+		target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+		return ERROR_FLASH_BANK_INVALID;
+	}
+	
+	pri_ext->major_version = cfi_query_u8(bank, 0, cfi_info->pri_addr + 3);
+	pri_ext->minor_version = cfi_query_u8(bank, 0, cfi_info->pri_addr + 4);
+	
+	DEBUG("pri: '%c%c%c', version: %c.%c", pri_ext->pri[0], pri_ext->pri[1], pri_ext->pri[2], pri_ext->major_version, pri_ext->minor_version);
+	
+	pri_ext->feature_support = cfi_query_u32(bank, 0, cfi_info->pri_addr + 5);
+	pri_ext->suspend_cmd_support = cfi_query_u8(bank, 0, cfi_info->pri_addr + 9);
+	pri_ext->blk_status_reg_mask = cfi_query_u16(bank, 0, cfi_info->pri_addr + 0xa);
+	
+	DEBUG("feature_support: 0x%x, suspend_cmd_support: 0x%x, blk_status_reg_mask: 0x%x", pri_ext->feature_support, pri_ext->suspend_cmd_support, pri_ext->blk_status_reg_mask);
+	
+	pri_ext->vcc_optimal = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0xc);
+	pri_ext->vpp_optimal = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0xd);
+	
+	DEBUG("Vcc opt: %1.1x.%1.1x, Vpp opt: %1.1x.%1.1x",
+		  (pri_ext->vcc_optimal & 0xf0) >> 4, pri_ext->vcc_optimal & 0x0f,
+		  (pri_ext->vpp_optimal & 0xf0) >> 4, pri_ext->vpp_optimal & 0x0f);
+	
+	pri_ext->num_protection_fields = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0xe);
+	if (pri_ext->num_protection_fields != 1)
+	{
+		WARNING("expected one protection register field, but found %i", pri_ext->num_protection_fields);
+	}
+	
+	pri_ext->prot_reg_addr = cfi_query_u16(bank, 0, cfi_info->pri_addr + 0xf);
+	pri_ext->fact_prot_reg_size = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0x11);
+	pri_ext->user_prot_reg_size = cfi_query_u8(bank, 0, cfi_info->pri_addr + 0x12);
+
+	DEBUG("protection_fields: %i, prot_reg_addr: 0x%x, factory pre-programmed: %i, user programmable: %i", pri_ext->num_protection_fields, pri_ext->prot_reg_addr, 1 << pri_ext->fact_prot_reg_size, 1 << pri_ext->user_prot_reg_size);
+	
+	return ERROR_OK;
+}
+
+int cfi_intel_info(struct flash_bank_s *bank, char *buf, int buf_size)
+{
+	int printed;
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	cfi_intel_pri_ext_t *pri_ext = cfi_info->pri_ext;
+		
+	printed = snprintf(buf, buf_size, "\nintel primary algorithm extend information:\n");
+	buf += printed;
+	buf_size -= printed;
+	
+	printed = snprintf(buf, buf_size, "pri: '%c%c%c', version: %c.%c\n", pri_ext->pri[0], pri_ext->pri[1], pri_ext->pri[2], pri_ext->major_version, pri_ext->minor_version);
+	buf += printed;
+	buf_size -= printed;
+	
+	printed = snprintf(buf, buf_size, "feature_support: 0x%x, suspend_cmd_support: 0x%x, blk_status_reg_mask: 0x%x\n", pri_ext->feature_support, pri_ext->suspend_cmd_support, pri_ext->blk_status_reg_mask);
+	buf += printed;
+	buf_size -= printed;
+	
+	printed = snprintf(buf, buf_size, "Vcc opt: %1.1x.%1.1x, Vpp opt: %1.1x.%1.1x\n",
+		(pri_ext->vcc_optimal & 0xf0) >> 4, pri_ext->vcc_optimal & 0x0f,
+		(pri_ext->vpp_optimal & 0xf0) >> 4, pri_ext->vpp_optimal & 0x0f);
+	buf += printed;
+	buf_size -= printed;
+	
+	printed = snprintf(buf, buf_size, "protection_fields: %i, prot_reg_addr: 0x%x, factory pre-programmed: %i, user programmable: %i\n", pri_ext->num_protection_fields, pri_ext->prot_reg_addr, 1 << pri_ext->fact_prot_reg_size, 1 << pri_ext->user_prot_reg_size);
+	
+	return ERROR_OK;
+}
+
+int cfi_register_commands(struct command_context_s *cmd_ctx)
+{
+	command_t *cfi_cmd = register_command(cmd_ctx, NULL, "cfi", NULL, COMMAND_ANY, NULL);
+	/*	
+	register_command(cmd_ctx, cfi_cmd, "part_id", cfi_handle_part_id_command, COMMAND_EXEC,
+					 "print part id of cfi flash bank <num>");
+	*/
+	return ERROR_OK;
+}
+
+/* flash_bank cfi <base> <size> <chip_width> <bus_width> <target#>
+ */
+int cfi_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)
+{
+	cfi_flash_bank_t *cfi_info;
+	
+	if (argc < 6)
+	{
+		WARNING("incomplete flash_bank cfi configuration");
+		return ERROR_FLASH_BANK_INVALID;
+	}
+	
+	cfi_info = malloc(sizeof(cfi_flash_bank_t));
+	bank->driver_priv = cfi_info;
+	
+	cfi_info->target = get_target_by_num(strtoul(args[5], NULL, 0));
+	if (!cfi_info->target)
+	{
+		ERROR("no target '%i' configured", args[5]);
+		exit(-1);
+	}
+
+	/* bank wasn't probed yet */
+	cfi_info->qry[0] = -1;
+	
+	return ERROR_OK;
+}
+
+int cfi_intel_erase(struct flash_bank_s *bank, int first, int last)
+{
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	cfi_intel_pri_ext_t *pri_ext = cfi_info->pri_ext;
+	target_t *target = cfi_info->target;
+	u8 command[8];
+	int i;
+	
+	cfi_intel_clear_status_register(bank);
+
+	for (i = first; i <= last; i++)
+	{
+		cfi_command(bank, 0x20, command);
+		target->type->write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command);
+			
+		cfi_command(bank, 0xd0, command);
+		target->type->write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command);
+		
+		if (cfi_intel_wait_status_busy(bank, 1000 * (1 << cfi_info->block_erase_timeout_typ)) == 0x80)
+			bank->sectors[i].is_erased = 1;
+		else
+		{
+			cfi_command(bank, 0xff, command);
+			target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+			
+			ERROR("couldn't erase block %i of flash bank at base 0x%x", i, bank->base);
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+	}
+	
+	cfi_command(bank, 0xff, command);
+	target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+	
+	return ERROR_OK;
+}
+	
+int cfi_erase(struct flash_bank_s *bank, int first, int last)
+{
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	
+	if (cfi_info->target->state != TARGET_HALTED)
+	{
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	
+	if ((first < 0) || (last < first) || (last >= bank->num_sectors))
+	{
+		return ERROR_FLASH_SECTOR_INVALID;
+	}
+
+	if (cfi_info->qry[0] != 'Q')
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	
+	switch(cfi_info->pri_id)
+	{
+		case 1:
+		case 3:
+			return cfi_intel_erase(bank, first, last);
+			break;
+		default:
+			ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
+			break;
+	}
+	
+	return ERROR_OK;
+}
+
+int cfi_intel_protect(struct flash_bank_s *bank, int set, int first, int last)
+{
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	cfi_intel_pri_ext_t *pri_ext = cfi_info->pri_ext;
+	target_t *target = cfi_info->target;
+	u8 command[8];
+	int i;
+	
+	if (!(pri_ext->feature_support & 0x28))
+		return ERROR_FLASH_OPERATION_FAILED;
+	
+	cfi_intel_clear_status_register(bank);
+
+	for (i = first; i <= last; i++)
+	{
+		cfi_command(bank, 0x60, command);
+		target->type->write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command);
+		if (set)
+		{
+			cfi_command(bank, 0x01, command);
+			target->type->write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command);
+			bank->sectors[i].is_protected = 1;
+		}
+		else
+		{
+			cfi_command(bank, 0xd0, command);
+			target->type->write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command);
+			bank->sectors[i].is_protected = 0;
+		}
+		
+		cfi_intel_wait_status_busy(bank, 100);
+	}
+	
+	/* if the device doesn't support individual block lock bits set/clear,
+	 * all blocks have been unlocked in parallel, so we set those that should be protected
+	 */
+	if ((!set) && (!(pri_ext->feature_support & 0x20)))
+	{
+		for (i = 0; i < bank->num_sectors; i++)
+		{
+			cfi_intel_clear_status_register(bank);
+			cfi_command(bank, 0x60, command);
+			target->type->write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command);
+			if (bank->sectors[i].is_protected == 1)
+			{
+				cfi_command(bank, 0x01, command);
+				target->type->write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command);
+			}
+			
+			cfi_intel_wait_status_busy(bank, 100);
+		}
+	}
+	
+	cfi_command(bank, 0xff, command);
+	target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+	
+	return ERROR_OK;
+}
+
+int cfi_protect(struct flash_bank_s *bank, int set, int first, int last)
+{
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	
+	if (cfi_info->target->state != TARGET_HALTED)
+	{
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	
+	if ((first < 0) || (last < first) || (last >= bank->num_sectors))
+	{
+		return ERROR_FLASH_SECTOR_INVALID;
+	}
+	
+	if (cfi_info->qry[0] != 'Q')
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	
+	switch(cfi_info->pri_id)
+	{
+		case 1:
+		case 3:
+			cfi_intel_protect(bank, set, first, last);
+			break;
+		default:
+			ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
+			break;
+	}
+	
+	return ERROR_OK;
+}
+
+void cfi_add_byte(struct flash_bank_s *bank, u8 *word, u8 byte)
+{
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	target_t *target = cfi_info->target;
+
+	int i;
+	
+	if (target->endianness == TARGET_LITTLE_ENDIAN)
+	{
+		/* shift bytes */
+		for (i = 0; i < bank->bus_width - 1; i++)
+			word[i] = word[i + 1];
+		word[bank->bus_width - 1] = byte;
+	}
+	else
+	{
+		/* shift bytes */
+		for (i = bank->bus_width - 1; i > 0; i--)
+			word[i] = word[i - 1];
+		word[0] = byte;
+	}
+}
+
+int cfi_intel_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address, u32 count)
+{
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	cfi_intel_pri_ext_t *pri_ext = cfi_info->pri_ext;
+	target_t *target = cfi_info->target;
+	reg_param_t reg_params[5];
+	armv4_5_algorithm_t armv4_5_info;
+	working_area_t *source;
+	u32 buffer_size = 32768;
+	u8 write_command[CFI_MAX_BUS_WIDTH];
+	int i;
+	int retval;
+	
+	u32 word_32_code[] = {
+		0xe4904004,   /* loop:	ldr r4, [r0], #4 */
+		0xe5813000,   /* 		str r3, [r1] */
+		0xe5814000,   /* 		str r4, [r1] */
+		0xe5914000,   /* busy	ldr r4, [r1] */
+		0xe3140080,   /*		tst r4, #0x80 */
+		0x0afffffc,   /*		beq busy */
+		0xe314007f,   /* 		tst r4, #0x7f */
+		0x1a000003,   /*		bne done */
+		0xe2522001,   /*		subs r2, r2, #1 */
+		0x0a000001,   /* 		beq done */
+		0xe2811004,   /*		add r1, r1 #4 */
+		0xeafffff3,   /* 		b loop */
+		0xeafffffe,   /* done:	b -2 */
+	};
+	
+	u32 word_16_code[] = {
+		0xe0d040b2,   /* loop:	ldrh r4, [r0], #2 */
+		0xe1c130b0,   /* 		strh r3, [r1] */
+		0xe1c140b0,   /* 		strh r4, [r1] */
+		0xe1d140b0,   /* busy	ldrh r4, [r1] */
+		0xe3140080,   /*		tst r4, #0x80 */
+		0x0afffffc,   /*		beq busy */
+		0xe314007f,   /* 		tst r4, #0x7f */
+		0x1a000003,   /*		bne done */
+		0xe2522001,   /*		subs r2, r2, #1 */
+		0x0a000001,   /* 		beq done */
+		0xe2811002,   /*		add r1, r1 #2 */
+		0xeafffff3,   /* 		b loop */
+		0xeafffffe,   /* done:	b -2 */
+	};
+	
+	u32 word_8_code[] = {
+		0xe4d04001,   /* loop:	ldrb r4, [r0], #1 */
+		0xe5c13000,   /* 		strb r3, [r1] */
+		0xe5c14000,   /* 		strb r4, [r1] */
+		0xe5d14000,   /* busy	ldrb r4, [r1] */
+		0xe3140080,   /*		tst r4, #0x80 */
+		0x0afffffc,   /*		beq busy */
+		0xe314007f,   /* 		tst r4, #0x7f */
+		0x1a000003,   /*		bne done */
+		0xe2522001,   /*		subs r2, r2, #1 */
+		0x0a000001,   /* 		beq done */
+		0xe2811001,   /*		add r1, r1 #1 */
+		0xeafffff3,   /* 		b loop */
+		0xeafffffe,   /* done:	b -2 */
+	};
+	
+	cfi_intel_clear_status_register(bank);
+		
+	armv4_5_info.common_magic = ARMV4_5_COMMON_MAGIC;
+	armv4_5_info.core_mode = ARMV4_5_MODE_SVC;
+	armv4_5_info.core_state = ARMV4_5_STATE_ARM;
+			
+	/* flash write code */
+	if (!cfi_info->write_algorithm)
+	{
+		if (target_alloc_working_area(target, 4 * 13, &cfi_info->write_algorithm) != ERROR_OK)
+		{
+			WARNING("no working area available, can't do block memory writes");
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		};
+			
+		/* write algorithm code to working area */
+		if (bank->bus_width == 1)
+		{
+			target_write_buffer(target, cfi_info->write_algorithm->address, 13 * 4, (u8*)word_8_code);
+		}
+		else if (bank->bus_width == 2)
+		{
+			target_write_buffer(target, cfi_info->write_algorithm->address, 13 * 4, (u8*)word_16_code);
+		}	
+		else if (bank->bus_width == 4)
+		{
+			target_write_buffer(target, cfi_info->write_algorithm->address, 13 * 4, (u8*)word_32_code);
+		}
+		else
+		{
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+	}
+	
+	while (target_alloc_working_area(target, buffer_size, &source) != ERROR_OK)
+	{
+		buffer_size /= 2;
+		if (buffer_size <= 256)
+		{
+			/* if we already allocated the writing code, but failed to get a buffer, free the algorithm */
+			if (cfi_info->write_algorithm)
+				target_free_working_area(target, cfi_info->write_algorithm);
+			
+			WARNING("no large enough working area available, can't do block memory writes");
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+	};
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
+	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
+	init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);
+	init_reg_param(&reg_params[4], "r4", 32, PARAM_IN);
+
+	while (count > 0)
+	{
+		u32 thisrun_count = (count > buffer_size) ? buffer_size : count;
+		
+		target_write_buffer(target, source->address, thisrun_count, buffer);
+		
+		buf_set_u32(reg_params[0].value, 0, 32, source->address);
+		buf_set_u32(reg_params[1].value, 0, 32, address);
+		buf_set_u32(reg_params[2].value, 0, 32, thisrun_count / bank->bus_width);
+		cfi_command(bank, 0x40, write_command);
+		buf_set_u32(reg_params[3].value, 0, 32, buf_get_u32(write_command, 0, 32));
+	
+		if ((retval = target->type->run_algorithm(target, 0, NULL, 5, reg_params, cfi_info->write_algorithm->address, cfi_info->write_algorithm->address + (12 * 4), 10000, &armv4_5_info)) != ERROR_OK)
+		{
+			cfi_intel_clear_status_register(bank);
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+	
+		if (buf_get_u32(reg_params[4].value, 0, 32) != 0x80)
+		{
+			/* read status register (outputs debug inforation) */
+			cfi_intel_wait_status_busy(bank, 100);
+			cfi_intel_clear_status_register(bank);
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+		
+		buffer += thisrun_count;
+		address += thisrun_count;
+		count -= thisrun_count;
+	}
+	
+	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 ERROR_OK;
+}
+
+int cfi_intel_write_word(struct flash_bank_s *bank, u8 *word, u32 address)
+{
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	cfi_intel_pri_ext_t *pri_ext = cfi_info->pri_ext;
+	target_t *target = cfi_info->target;
+	u8 command[8];
+	
+	cfi_intel_clear_status_register(bank);
+	cfi_command(bank, 0x40, command);
+	target->type->write_memory(target, address, bank->bus_width, 1, command);
+	
+	target->type->write_memory(target, address, bank->bus_width, 1, word);
+	
+	if (cfi_intel_wait_status_busy(bank, 1000 * (1 << cfi_info->word_write_timeout_max)) != 0x80)
+	{
+		cfi_command(bank, 0xff, command);
+		target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+		
+		ERROR("couldn't write word at base 0x%x, address %x", bank->base, address);
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+	
+	return ERROR_OK;
+}
+
+int cfi_write_word(struct flash_bank_s *bank, u8 *word, u32 address)
+{
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	target_t *target = cfi_info->target;
+	
+	switch(cfi_info->pri_id)
+	{
+		case 1:
+		case 3:
+			return cfi_intel_write_word(bank, word, address);
+			break;
+		default:
+			ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
+			break;
+	}
+	
+	return ERROR_FLASH_OPERATION_FAILED;
+}
+
+int cfi_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
+{
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	target_t *target = cfi_info->target;
+	u32 address = bank->base + offset;	/* address of first byte to be programmed */
+	u32 write_p, copy_p;
+	int align;	/* number of unaligned bytes */
+	u8 current_word[CFI_MAX_BUS_WIDTH * 4];	/* word (bus_width size) currently being programmed */
+	int i;
+	int retval;
+	
+	if (cfi_info->target->state != TARGET_HALTED)
+	{
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	
+	if (offset + count > bank->size)
+		return ERROR_FLASH_DST_OUT_OF_BANK;
+	
+	if (cfi_info->qry[0] != 'Q')
+		return ERROR_FLASH_BANK_NOT_PROBED;
+
+	/* start at the first byte of the first word (bus_width size) */
+	write_p = address & ~(bank->bus_width - 1);
+	if ((align = address - write_p) != 0)
+	{
+		for (i = 0; i < bank->bus_width; i++)
+			current_word[i] = 0;
+		copy_p = write_p;
+
+		/* copy bytes before the first write address */
+		for (i = 0; i < align; ++i, ++copy_p)
+		{
+			u8 byte;
+			target->type->read_memory(target, copy_p, 1, 1, &byte);
+			cfi_add_byte(bank, current_word, byte);
+		}
+
+		/* add bytes from the buffer */
+		for (; (i < bank->bus_width) && (count > 0); i++)
+		{
+			cfi_add_byte(bank, current_word, *buffer++);
+			count--;
+			copy_p++;
+		}
+
+		/* if the buffer is already finished, copy bytes after the last write address */
+		for (; (count == 0) && (i < bank->bus_width); ++i, ++copy_p)
+		{
+			u8 byte;
+			target->type->read_memory(target, copy_p, 1, 1, &byte);
+			cfi_add_byte(bank, current_word, byte);
+		}
+		
+		retval = cfi_write_word(bank, current_word, write_p);
+		if (retval != ERROR_OK)
+			return retval;
+		write_p = copy_p;
+	}
+	
+	/* handle blocks of bus_size aligned bytes */
+	switch(cfi_info->pri_id)
+	{
+		/* try block writes (fails without working area) */
+		case 1:
+		case 3:
+			retval = cfi_intel_write_block(bank, buffer, write_p, count);
+			break;
+		default:
+			ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
+			break;
+	}
+	if (retval != ERROR_OK)
+	{
+		if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
+		{
+			/* fall back to memory writes */
+			while (count > bank->bus_width)
+			{
+				for (i = 0; i < bank->bus_width; i++)
+					current_word[i] = 0;
+			
+				for (i = 0; i < bank->bus_width; i++)
+				{
+					cfi_add_byte(bank, current_word, *buffer++);
+				}
+			
+				retval = cfi_write_word(bank, current_word, write_p);
+				if (retval != ERROR_OK)
+					return retval;
+				write_p += bank->bus_width;
+				count -= bank->bus_width;
+			}
+		}
+		else
+			return retval;
+	}
+	
+	/* handle unaligned tail bytes */
+	if (count > 0)
+	{
+		copy_p = write_p;
+		for (i = 0; i < bank->bus_width; i++)
+			current_word[i] = 0;
+		
+		for (i = 0; (i < bank->bus_width) && (count > 0); ++i, ++copy_p)
+		{
+			cfi_add_byte(bank, current_word, *buffer++);
+			count--;
+		}
+		for (; i < bank->bus_width; ++i, ++copy_p)
+		{
+			u8 byte;
+			target->type->read_memory(target, copy_p, 1, 1, &byte);
+			cfi_add_byte(bank, current_word, byte);
+		}
+		retval = cfi_write_word(bank, current_word, write_p);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+	
+	/* return to read array mode */
+	cfi_command(bank, 0xf0, current_word);
+	target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, current_word);
+	cfi_command(bank, 0xff, current_word);
+	target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, current_word);
+	
+	return ERROR_OK;
+}
+
+int cfi_probe(struct flash_bank_s *bank)
+{
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	target_t *target = cfi_info->target;
+	u8 command[8];
+	
+
+	cfi_command(bank, 0x98, command);
+	target->type->write_memory(target, flash_address(bank, 0, 0x55), bank->bus_width, 1, command);
+	
+	cfi_info->qry[0] = cfi_query_u8(bank, 0, 0x10);
+	cfi_info->qry[1] = cfi_query_u8(bank, 0, 0x11);
+	cfi_info->qry[2] = cfi_query_u8(bank, 0, 0x12);
+	
+	if ((cfi_info->qry[0] != 'Q') || (cfi_info->qry[1] != 'R') || (cfi_info->qry[2] != 'Y'))
+	{
+		cfi_command(bank, 0xf0, command);
+		target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+		cfi_command(bank, 0xff, command);
+		target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+		return ERROR_FLASH_BANK_INVALID;
+	}
+	
+	cfi_info->pri_id = cfi_query_u16(bank, 0, 0x13);
+	cfi_info->pri_addr = cfi_query_u16(bank, 0, 0x15);
+	cfi_info->alt_id = cfi_query_u16(bank, 0, 0x17);
+	cfi_info->alt_addr = cfi_query_u16(bank, 0, 0x19);
+	
+	DEBUG("qry: '%c%c%c', pri_id: 0x%4.4x, pri_addr: 0x%4.4x, alt_id: 0x%4.4x, alt_addr: 0x%4.4x", cfi_info->qry[0], cfi_info->qry[1], cfi_info->qry[2], cfi_info->pri_id, cfi_info->pri_addr, cfi_info->alt_id, cfi_info->alt_addr);
+	
+	cfi_info->vcc_min = cfi_query_u8(bank, 0, 0x1b);
+	cfi_info->vcc_max = cfi_query_u8(bank, 0, 0x1c);
+	cfi_info->vpp_min = cfi_query_u8(bank, 0, 0x1d);
+	cfi_info->vpp_max = cfi_query_u8(bank, 0, 0x1e);
+	cfi_info->word_write_timeout_typ = cfi_query_u8(bank, 0, 0x1f);
+	cfi_info->buf_write_timeout_typ = cfi_query_u8(bank, 0, 0x20);
+	cfi_info->block_erase_timeout_typ = cfi_query_u8(bank, 0, 0x21);
+	cfi_info->chip_erase_timeout_typ = cfi_query_u8(bank, 0, 0x22);
+	cfi_info->word_write_timeout_max = cfi_query_u8(bank, 0, 0x23);
+	cfi_info->buf_write_timeout_max = cfi_query_u8(bank, 0, 0x24);
+	cfi_info->block_erase_timeout_max = cfi_query_u8(bank, 0, 0x25);
+	cfi_info->chip_erase_timeout_max = cfi_query_u8(bank, 0, 0x26);
+	
+	DEBUG("Vcc min: %1.1x.%1.1x, Vcc max: %1.1x.%1.1x, Vpp min: %1.1x.%1.1x, Vpp max: %1.1x.%1.1x",
+		(cfi_info->vcc_min & 0xf0) >> 4, cfi_info->vcc_min & 0x0f,
+		(cfi_info->vcc_max & 0xf0) >> 4, cfi_info->vcc_max & 0x0f,
+		(cfi_info->vpp_min & 0xf0) >> 4, cfi_info->vpp_min & 0x0f,
+		(cfi_info->vpp_max & 0xf0) >> 4, cfi_info->vpp_max & 0x0f);
+	DEBUG("typ. word write timeout: %u, typ. buf write timeout: %u, typ. block erase timeout: %u, typ. chip erase timeout: %u", 1 << cfi_info->word_write_timeout_typ, 1 << cfi_info->buf_write_timeout_typ,
+		1 << cfi_info->block_erase_timeout_typ, 1 << cfi_info->chip_erase_timeout_typ);
+	DEBUG("max. word write timeout: %u, max. buf write timeout: %u, max. block erase timeout: %u, max. chip erase timeout: %u", (1 << cfi_info->word_write_timeout_max) * (1 << cfi_info->word_write_timeout_typ),
+		(1 << cfi_info->buf_write_timeout_max) * (1 << cfi_info->buf_write_timeout_typ),
+		(1 << cfi_info->block_erase_timeout_max) * (1 << cfi_info->block_erase_timeout_typ),
+		(1 << cfi_info->chip_erase_timeout_max) * (1 << cfi_info->chip_erase_timeout_typ));
+	
+	cfi_info->dev_size = cfi_query_u8(bank, 0, 0x27);
+	cfi_info->interface_desc = cfi_query_u16(bank, 0, 0x28);
+	cfi_info->max_buf_write_size = cfi_query_u16(bank, 0, 0x2a);
+	cfi_info->num_erase_regions = cfi_query_u8(bank, 0, 0x2c);
+	
+	DEBUG("size: 0x%x, interface desc: %i, max buffer write size: %x", 1 << cfi_info->dev_size, cfi_info->interface_desc, cfi_info->max_buf_write_size);
+	
+	if (1 << cfi_info->dev_size != bank->size)
+	{
+		WARNING("configuration specifies 0x%x size, but a 0x%x size flash was found", bank->size, 1 << cfi_info->dev_size);
+	}
+	
+	if (cfi_info->num_erase_regions)
+	{
+		int i;
+		int num_sectors = 0;
+		int sector = 0;
+		u32 offset = 0;
+		cfi_info->erase_region_info = malloc(4 * cfi_info->num_erase_regions);
+		
+		for (i = 0; i < cfi_info->num_erase_regions; i++)
+		{
+			cfi_info->erase_region_info[i] = cfi_query_u32(bank, 0, 0x2d + (4 * i));
+			DEBUG("erase region[%i]: %i blocks of size 0x%x", i, (cfi_info->erase_region_info[i] & 0xffff) + 1, (cfi_info->erase_region_info[i] >> 16) * 256);
+			
+			num_sectors += (cfi_info->erase_region_info[i] & 0xffff) + 1;
+		}
+		
+		bank->num_sectors = num_sectors;
+		bank->sectors = malloc(sizeof(flash_sector_t) * num_sectors);
+		for (i = 0; i < cfi_info->num_erase_regions; i++)
+		{
+			int j;
+			for (j = 0; j < (cfi_info->erase_region_info[i] & 0xffff) + 1; j++)
+			{
+				bank->sectors[sector].offset = offset;
+				bank->sectors[sector].size = (cfi_info->erase_region_info[i] >> 16) * 256;
+				offset += bank->sectors[sector].size;
+				bank->sectors[sector].is_erased = -1;
+				bank->sectors[sector].is_protected = -1;
+				sector++;
+			}
+		}
+	}
+	else
+	{
+		cfi_info->erase_region_info = NULL;
+	}
+	
+	switch(cfi_info->pri_id)
+	{
+		case 1:
+		case 3:
+			cfi_read_intel_pri_ext(bank);
+			break;
+		default:
+			ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
+			break;
+	}
+	
+	/* return to read array mode */
+	cfi_command(bank, 0xf0, command);
+	target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+	cfi_command(bank, 0xff, command);
+	target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+	
+	return ERROR_OK;
+}
+
+int cfi_erase_check(struct flash_bank_s *bank)
+{
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	target_t *target = cfi_info->target;
+	int i;
+	int retval;
+	
+	if (!cfi_info->erase_check_algorithm)
+	{
+		u32 erase_check_code[] =
+		{
+			0xe4d03001,
+			0xe0022003,
+			0xe2511001,
+			0x1afffffb,
+			0xeafffffe
+		};
+		
+		/* make sure we have a working area */
+		if (target_alloc_working_area(target, 20, &cfi_info->erase_check_algorithm) != ERROR_OK)
+		{
+			WARNING("no working area available, falling back to slow memory reads");
+		}
+		else
+		{
+			/* write algorithm code to working area */
+			target->type->write_memory(target, cfi_info->erase_check_algorithm->address, 4, 5, (u8*)erase_check_code);
+		}
+	}
+	
+	if (!cfi_info->erase_check_algorithm)
+	{
+		u32 *buffer = malloc(4096);
+		
+		for (i = 0; i < bank->num_sectors; i++)
+		{
+			u32 address = bank->base + bank->sectors[i].offset;
+			u32 size = bank->sectors[i].size;
+			u32 check = 0xffffffffU;
+			int erased = 1;
+			
+			while (size > 0)
+			{
+				u32 thisrun_size = (size > 4096) ? 4096 : size;
+				int j;
+				
+				target->type->read_memory(target, address, 4, thisrun_size / 4, (u8*)buffer);
+				
+				for (j = 0; j < thisrun_size / 4; j++)
+					check &= buffer[j];
+				
+				if (check != 0xffffffff)
+				{
+					erased = 0;
+					break;
+				}
+				
+				size -= thisrun_size;
+				address += thisrun_size;
+			}
+			
+			bank->sectors[i].is_erased = erased;
+		}
+		
+		free(buffer);
+	}
+	else
+	{	
+		for (i = 0; i < bank->num_sectors; i++)
+		{
+			u32 address = bank->base + bank->sectors[i].offset;
+			u32 size = bank->sectors[i].size;
+
+			reg_param_t reg_params[3];
+			armv4_5_algorithm_t armv4_5_info;
+			
+			armv4_5_info.common_magic = ARMV4_5_COMMON_MAGIC;
+			armv4_5_info.core_mode = ARMV4_5_MODE_SVC;
+			armv4_5_info.core_state = ARMV4_5_STATE_ARM;
+			
+			init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
+			buf_set_u32(reg_params[0].value, 0, 32, address);
+			
+			init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
+			buf_set_u32(reg_params[1].value, 0, 32, size);
+			
+			init_reg_param(&reg_params[2], "r2", 32, PARAM_IN_OUT);
+			buf_set_u32(reg_params[2].value, 0, 32, 0xff);
+			
+			if ((retval = target->type->run_algorithm(target, 0, NULL, 3, reg_params, cfi_info->erase_check_algorithm->address, cfi_info->erase_check_algorithm->address + 0x10, 10000, &armv4_5_info)) != ERROR_OK)
+				return ERROR_FLASH_OPERATION_FAILED;
+			
+			if (buf_get_u32(reg_params[2].value, 0, 32) == 0xff)
+				bank->sectors[i].is_erased = 1;
+			else
+				bank->sectors[i].is_erased = 0;
+			
+			destroy_reg_param(&reg_params[0]);
+			destroy_reg_param(&reg_params[1]);
+			destroy_reg_param(&reg_params[2]);
+		}
+	}
+	
+	return ERROR_OK;
+}
+
+int cfi_intel_protect_check(struct flash_bank_s *bank)
+{
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	cfi_intel_pri_ext_t *pri_ext = cfi_info->pri_ext;
+	target_t *target = cfi_info->target;
+	u8 command[8];
+	int i;
+	
+	/* check if block lock bits are supported on this device */
+	if (!(pri_ext->blk_status_reg_mask & 0x1))
+		return ERROR_FLASH_OPERATION_FAILED;
+
+	cfi_command(bank, 0x90, command);
+	target->type->write_memory(target, flash_address(bank, 0, 0x55), bank->bus_width, 1, command);
+
+	for (i = 0; i < bank->num_sectors; i++)
+	{
+		u8 block_status = cfi_get_u8(bank, i, 0x2);
+		
+		if (block_status & 1)
+			bank->sectors[i].is_protected = 1;
+		else
+			bank->sectors[i].is_protected = 0;
+	}
+
+	cfi_command(bank, 0xff, command);
+	target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+
+	return ERROR_OK;
+}
+
+int cfi_protect_check(struct flash_bank_s *bank)
+{
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	target_t *target = cfi_info->target;
+
+	if (cfi_info->qry[0] != 'Q')
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	
+	switch(cfi_info->pri_id)
+	{
+		case 1:
+		case 3:
+			return cfi_intel_protect_check(bank);
+			break;
+		default:
+			ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
+			break;
+	}
+	
+	return ERROR_OK;
+}
+
+int cfi_info(struct flash_bank_s *bank, char *buf, int buf_size)
+{
+	int printed;
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	
+	if (cfi_info->qry[0] == -1)
+	{
+		printed = snprintf(buf, buf_size, "\ncfi flash bank not probed yet\n");
+		return ERROR_OK;
+	}
+	
+	printed = snprintf(buf, buf_size, "\ncfi information:\n");
+	buf += printed;
+	buf_size -= printed;
+	
+	printed = snprintf(buf, buf_size, "qry: '%c%c%c', pri_id: 0x%4.4x, pri_addr: 0x%4.4x, alt_id: 0x%4.4x, alt_addr: 0x%4.4x\n", cfi_info->qry[0], cfi_info->qry[1], cfi_info->qry[2], cfi_info->pri_id, cfi_info->pri_addr, cfi_info->alt_id, cfi_info->alt_addr);
+	buf += printed;
+	buf_size -= printed;
+	
+	printed = snprintf(buf, buf_size, "Vcc min: %1.1x.%1.1x, Vcc max: %1.1x.%1.1x, Vpp min: %1.1x.%1.1x, Vpp max: %1.1x.%1.1x\n", (cfi_info->vcc_min & 0xf0) >> 4, cfi_info->vcc_min & 0x0f,
+	(cfi_info->vcc_max & 0xf0) >> 4, cfi_info->vcc_max & 0x0f,
+	(cfi_info->vpp_min & 0xf0) >> 4, cfi_info->vpp_min & 0x0f,
+	(cfi_info->vpp_max & 0xf0) >> 4, cfi_info->vpp_max & 0x0f);
+	buf += printed;
+	buf_size -= printed;
+	
+	printed = snprintf(buf, buf_size, "typ. word write timeout: %u, typ. buf write timeout: %u, typ. block erase timeout: %u, typ. chip erase timeout: %u\n", 1 << cfi_info->word_write_timeout_typ, 1 << cfi_info->buf_write_timeout_typ,
+		  1 << cfi_info->block_erase_timeout_typ, 1 << cfi_info->chip_erase_timeout_typ);
+	buf += printed;
+	buf_size -= printed;
+	
+	printed = snprintf(buf, buf_size, "max. word write timeout: %u, max. buf write timeout: %u, max. block erase timeout: %u, max. chip erase timeout: %u\n", (1 << cfi_info->word_write_timeout_max) * (1 << cfi_info->word_write_timeout_typ),
+		  (1 << cfi_info->buf_write_timeout_max) * (1 << cfi_info->buf_write_timeout_typ),
+		  (1 << cfi_info->block_erase_timeout_max) * (1 << cfi_info->block_erase_timeout_typ),
+		  (1 << cfi_info->chip_erase_timeout_max) * (1 << cfi_info->chip_erase_timeout_typ));
+	buf += printed;
+	buf_size -= printed;
+	
+	printed = snprintf(buf, buf_size, "size: 0x%x, interface desc: %i, max buffer write size: %x\n", 1 << cfi_info->dev_size, cfi_info->interface_desc, cfi_info->max_buf_write_size);
+	buf += printed;
+	buf_size -= printed;
+	
+	switch(cfi_info->pri_id)
+	{
+		case 1:
+		case 3:
+			cfi_intel_info(bank, buf, buf_size);
+			break;
+		default:
+			ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
+			break;
+	}
+	
+	return ERROR_OK;
+}
diff --git a/src/flash/cfi.h b/src/flash/cfi.h
new file mode 100644
index 00000000..d9700be2
--- /dev/null
+++ b/src/flash/cfi.h
@@ -0,0 +1,86 @@
+/***************************************************************************
+ *   Copyright (C) 2005 by Dominic Rath                                    *
+ *   Dominic.Rath@gmx.de                                                   *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifndef CFI_H
+#define CFI_H
+
+#include "flash.h"
+#include "target.h"
+
+typedef struct cfi_flash_bank_s
+{
+	struct target_s *target;
+	working_area_t *write_algorithm;
+	working_area_t *erase_check_algorithm;
+		
+	char qry[3];
+	
+	/* identification string */
+	u16 pri_id;
+	u16 pri_addr;
+	u16 alt_id;
+	u16 alt_addr;
+	
+	/* device-system interface */
+	u8 vcc_min;
+	u8 vcc_max;
+	u8 vpp_min;
+	u8 vpp_max;
+	u8 word_write_timeout_typ;
+	u8 buf_write_timeout_typ;
+	u8 block_erase_timeout_typ;
+	u8 chip_erase_timeout_typ;
+	u8 word_write_timeout_max;
+	u8 buf_write_timeout_max;
+	u8 block_erase_timeout_max;
+	u8 chip_erase_timeout_max;
+	
+	/* flash geometry */
+	u8 dev_size;
+	u16 interface_desc;
+	u16 max_buf_write_size;
+	u8 num_erase_regions;
+	u32 *erase_region_info;
+	
+	void *pri_ext;
+	void *alt_ext;
+} cfi_flash_bank_t;
+
+/* Intel primary extended query table 
+ * as defined for the Advanced+ Boot Block Flash Memory (C3)
+ * and used by the linux kernel cfi driver (as of 2.6.14)
+ */
+typedef struct cfi_intel_pri_ext_s
+{
+	char pri[3];
+	u8 major_version;
+	u8 minor_version;
+	u32 feature_support;
+	u8 suspend_cmd_support;
+	u16 blk_status_reg_mask;
+	u8 vcc_optimal;
+	u8 vpp_optimal;
+	u8 num_protection_fields;
+	u16 prot_reg_addr;
+	u8 fact_prot_reg_size;
+	u8 user_prot_reg_size;
+	u8 extra[0];
+} cfi_intel_pri_ext_t;
+
+#endif /* CFI_H */
diff --git a/src/flash/flash.c b/src/flash/flash.c
new file mode 100644
index 00000000..a5067cd2
--- /dev/null
+++ b/src/flash/flash.c
@@ -0,0 +1,556 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "flash.h"
+#include "command.h"
+#include "log.h"
+#include "target.h"
+
+#include <string.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <errno.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_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_protect_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+/* 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;
+
+flash_driver_t *flash_drivers[] =
+{
+	&lpc2000_flash,
+	&cfi_flash,
+	&at91sam7_flash,
+	&str7x_flash,
+	NULL,
+};
+
+flash_bank_t *flash_banks;
+static 	command_t *flash_cmd;
+
+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, NULL);
+	
+	return ERROR_OK;
+}
+
+int flash_init(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,
+						 "erase sectors at <bank> <first> <last>");
+		register_command(cmd_ctx, flash_cmd, "write", handle_flash_write_command, COMMAND_EXEC,
+						 "write binary <bank> <file> <offset>");
+		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(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 <driver> <base> <size> <chip_width> <bus_width> [driver_options ...]
+ */
+int handle_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	int i;
+	int found = 0;
+		
+	if (argc < 5)
+	{
+		WARNING("incomplete flash_bank configuration");
+		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->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->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)
+	{
+		if (i++ == strtoul(args[0], NULL, 0))
+		{
+			char buf[1024];
+			
+			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%xkB) %s, %s",
+							j, p->sectors[j].offset, p->sectors[j].size,
+							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(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_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));
+		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
+	{
+		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, "usage: flash protect <bank> <first> <last> <on|off>");
+	}
+
+	return ERROR_OK;
+}
+
+int handle_flash_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	FILE *binary;
+	u32 offset;
+	struct stat binary_stat;
+	u32 binary_size;
+	u8 *buffer;
+	u32 buf_cnt;
+	int retval;
+	flash_bank_t *p;
+
+	if (argc < 3)
+	{
+		command_print(cmd_ctx, "usage: flash write <bank> <file> <offset>");
+		return ERROR_OK;
+	}
+	
+	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 (stat(args[1], &binary_stat) == -1)
+	{
+		ERROR("couldn't stat() %s: %s", args[1], strerror(errno));
+		return ERROR_OK;
+	}
+
+	if (S_ISDIR(binary_stat.st_mode))
+	{
+		ERROR("%s is a directory", args[1]);
+		command_print(cmd_ctx,"%s is a directory", args[1]);
+		return ERROR_OK;
+	}
+		
+	if (binary_stat.st_size == 0){
+		ERROR("Empty file %s", args[1]);
+		command_print(cmd_ctx,"Empty file %s", args[1]);
+		return ERROR_OK;
+	}
+		
+	if (!(binary = fopen(args[1], "r")))
+	{
+		ERROR("couldn't open %s: %s", args[1], strerror(errno));
+		command_print(cmd_ctx, "couldn't open %s", args[1]);
+		return ERROR_OK;
+	}
+
+	binary_size = binary_stat.st_size;
+	buffer = malloc(binary_size);
+	buf_cnt = fread(buffer, 1, binary_size, binary);
+
+	if ((retval = p->driver->write(p, buffer, offset, buf_cnt)) != 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 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);
+	fclose(binary);
+	
+	command_print(cmd_ctx, "wrote file %s to flash bank %i at offset 0x%8.8x", args[1], strtoul(args[0], NULL, 0), strtoul(args[2], NULL, 0));
+	
+	return ERROR_OK;
+
+}
diff --git a/src/flash/flash.h b/src/flash/flash.h
new file mode 100644
index 00000000..a8cc1869
--- /dev/null
+++ b/src/flash/flash.h
@@ -0,0 +1,76 @@
+/***************************************************************************
+ *   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"
+
+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);
+} flash_driver_t;
+
+typedef struct flash_bank_s
+{
+	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(struct command_context_s *cmd_ctx);
+
+extern flash_bank_t *get_flash_bank_by_num(int num);
+
+#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
new file mode 100644
index 00000000..b6fcb30b
--- /dev/null
+++ b/src/flash/lpc2000.c
@@ -0,0 +1,685 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#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
+ */
+
+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,
+	.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;
+
+	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 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 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 = lpc2000_info->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 */
+		buf_set_u32(jump_gate, 0, 32, ARMV4_5_BX(12));
+		buf_set_u32(jump_gate, 32, 32, 0xeafffffe);
+		target->type->write_memory(target, lpc2000_info->iap_working_area->address, 4, 2, (u8*)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);
+	buf_set_u32(mem_params[0].value, 0, 32, code);
+	buf_set_u32(mem_params[0].value, 32, 32, param_table[0]);
+	buf_set_u32(mem_params[0].value, 64, 32, param_table[1]);
+	buf_set_u32(mem_params[0].value, 96, 32, param_table[2]);
+	buf_set_u32(mem_params[0].value, 128, 32, param_table[3]);
+	buf_set_u32(mem_params[0].value, 160, 32, 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] = buf_get_u32(mem_params[1].value, 32, 32);
+	result_table[1] = buf_get_u32(mem_params[1].value, 64, 32);
+	
+	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 <lpc_variant> <target#> <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[5], "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[5], "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->target = get_target_by_num(strtoul(args[6], NULL, 0));
+	if (!lpc2000_info->target)
+	{
+		ERROR("no target '%s' configured", args[6]);
+		exit(-1);
+	}
+	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 (lpc2000_info->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 = lpc2000_info->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 (lpc2000_info->target->state != TARGET_HALTED)
+	{
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	
+	/* allocate a working area */
+	if (target_alloc_working_area(target, 4096, &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 >= 4096)
+			thisrun_bytes = 4096;
+		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(lpc2000_info->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(lpc2000_info->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)
+{
+	lpc2000_flash_bank_t *lpc2000_info = bank->driver_priv;
+	
+	if (lpc2000_info->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 (lpc2000_info->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/lpc2000.h b/src/flash/lpc2000.h
new file mode 100644
index 00000000..dbbe4b6a
--- /dev/null
+++ b/src/flash/lpc2000.h
@@ -0,0 +1,54 @@
+/***************************************************************************
+ *   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 LPC2000_H
+#define LPC2000_H
+
+#include "flash.h"
+#include "target.h"
+
+typedef struct lpc2000_flash_bank_s
+{
+	int variant;
+	struct target_s *target;
+	struct working_area_s *iap_working_area;
+	u32 cclk;
+	int cmd51_dst_boundary;
+	int cmd51_can_256b;
+	int cmd51_can_8192b;
+	int calc_checksum;
+} lpc2000_flash_bank_t;
+
+enum lpc2000_status_codes
+{
+	LPC2000_CMD_SUCCESS = 0,
+	LPC2000_INVALID_COMMAND = 1,
+	LPC2000_SRC_ADDR_ERROR = 2,
+	LPC2000_DST_ADDR_ERROR = 3,
+	LPC2000_SRC_ADDR_NOT_MAPPED = 4,
+	LPC2000_DST_ADDR_NOT_MAPPED = 5,
+	LPC2000_COUNT_ERROR = 6,
+	LPC2000_INVALID_SECTOR = 7,
+	LPC2000_SECTOR_NOT_BLANK = 8,
+	LPC2000_SECTOR_NOT_PREPARED = 9,
+	LPC2000_COMPARE_ERROR = 10,
+	LPC2000_BUSY = 11
+};
+
+#endif /* LPC2000_H */
diff --git a/src/flash/str7x.c b/src/flash/str7x.c
new file mode 100644
index 00000000..2e3a6c8c
--- /dev/null
+++ b/src/flash/str7x.c
@@ -0,0 +1,469 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+
+#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, 0x01},
+	{0x00004000, 0x02000, 0x01},
+	{0x00006000, 0x02000, 0x01},
+	{0x00008000, 0x08000, 0x01},
+	{0x00010000, 0x10000, 0x01},
+	{0x00020000, 0x10000, 0x01},
+	{0x00030000, 0x10000, 0x01},
+	{0x000C0000, 0x02000, 0x10},
+	{0x000C2000, 0x02000, 0x10},
+	{0,0},
+};
+
+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);
+
+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,
+	.erase_check = str7x_erase_check,
+	.protect_check = str7x_protect_check,
+	.info = str7x_info
+};
+
+int str7x_register_commands(struct command_context_s *cmd_ctx)
+{
+
+	return ERROR_OK;
+}
+
+int str7x_get_flash_adr(struct flash_bank_s *bank, u32 reg)
+{
+	str7x_flash_bank_t *str7x_info = bank->driver_priv;
+	return (str7x_info->flash_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;
+		
+	switch (bank->size)
+	{
+		case 16 * 1024:
+			num_sectors = 2;
+			break;
+		case 64 * 1024:
+			num_sectors = 5;
+			break;
+		case 128 * 1024:
+			num_sectors = 6;
+			break;
+		case 256 * 1024:
+			num_sectors = 8;
+			break;
+		default:
+			ERROR("BUG: unknown bank->size encountered");
+			exit(-1);
+	}
+	
+	if( str7x_info->bank1 == 1 )
+	{
+		num_sectors += 2;
+	}
+	
+	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 = mem_layout[i].sector_start;
+		bank->sectors[i].size = mem_layout[i].sector_size;
+		bank->sectors[i].is_erased = -1;
+		bank->sectors[i].is_protected = 1;
+	}
+
+	return ERROR_OK;
+}
+
+/* flash bank str7x <base> <size> 0 0 <str71_variant> <target#>
+ */
+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;
+	
+	if (strcmp(args[5], "STR71x") == 0)
+	{
+		str7x_info->bank1 = 1;
+		str7x_info->flash_base = 0x40000000;
+	}
+	else if (strcmp(args[5], "STR73x") == 0)
+	{
+		str7x_info->bank1 = 0;
+		str7x_info->flash_base = 0x80000000;
+	}
+	else
+	{
+		ERROR("unknown STR7x variant");
+		free(str7x_info);
+		return ERROR_FLASH_BANK_INVALID;
+	}
+	
+	str7x_info->target = get_target_by_num(strtoul(args[6], NULL, 0));
+	if (!str7x_info->target)
+	{
+		ERROR("no target '%i' configured", args[6]);
+		exit(-1);
+	}
+
+	str7x_build_block_list(bank);
+	
+	return ERROR_OK;
+}
+
+u32 str7x_status(struct flash_bank_s *bank)
+{
+	str7x_flash_bank_t *str7x_info = bank->driver_priv;
+	target_t *target = str7x_info->target;
+	u32 retval;
+
+	target->type->read_memory(target, str7x_get_flash_adr(bank, FLASH_CR0), 4, 1, (u8*)&retval);
+
+	return retval;
+}
+
+u32 str7x_result(struct flash_bank_s *bank)
+{
+	str7x_flash_bank_t *str7x_info = bank->driver_priv;
+	target_t *target = str7x_info->target;
+	u32 retval;
+
+	target->type->read_memory(target, str7x_get_flash_adr(bank, FLASH_ER), 4, 1, (u8*)&retval);
+	
+	return retval;
+}
+
+int str7x_blank_check(struct flash_bank_s *bank, int first, int last)
+{
+	str7x_flash_bank_t *str7x_info = bank->driver_priv;
+	target_t *target = str7x_info->target;
+	u8 *buffer;
+	int i;
+	int nBytes;
+	
+	if ((first < 0) || (last > bank->num_sectors))
+		return ERROR_FLASH_SECTOR_INVALID;
+
+	if (str7x_info->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 = str7x_info->target;
+	
+	int i;
+	int retval;
+
+	if (str7x_info->target->state != TARGET_HALTED)
+	{
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	target->type->read_memory(target, str7x_get_flash_adr(bank, FLASH_NVWPAR), 4, 1, (u8*)&retval);
+
+	for (i = 0; i < bank->num_sectors; i++)
+	{
+		if (retval & (mem_layout[i].reg_offset << 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 = str7x_info->target;
+	
+	int i;
+	u32 cmd;
+	u32 retval;
+	u32 erase_blocks;
+	
+	if (str7x_info->target->state != TARGET_HALTED)
+	{
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	
+	erase_blocks = 0;
+	
+	for (i = first; i <= last; i++)
+		erase_blocks |= (mem_layout[i].reg_offset << i);
+	
+	cmd = FLASH_SER;
+	target->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_CR0), 4, 1, (u8*)&cmd);
+	
+	cmd = erase_blocks;
+	target->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_CR1), 4, 1, (u8*)&cmd);
+	
+	cmd = FLASH_SER|FLASH_WMS;
+	target->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_CR0), 4, 1, (u8*)&cmd);
+	
+	while (((retval = str7x_status(bank)) & (FLASH_BSYA1|FLASH_BSYA2))){
+		usleep(1000);
+	}
+	
+	retval = str7x_result(bank);
+	
+	if (retval & FLASH_ERER)
+		return ERROR_FLASH_SECTOR_NOT_ERASED;
+	else if (retval & FLASH_WPF)
+		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 = str7x_info->target;
+	int i;
+	u32 cmd;
+	u32 retval;
+	u32 protect_blocks;
+	
+	if (str7x_info->target->state != TARGET_HALTED)
+	{
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	
+	protect_blocks = 0xFFFFFFFF;
+
+	if( set )
+	{
+		for (i = first; i <= last; i++)
+			protect_blocks &= ~(mem_layout[i].reg_offset << i);
+	}
+
+	cmd = FLASH_SPR;
+	target->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_CR0), 4, 1, (u8*)&cmd);
+	
+	cmd = str7x_get_flash_adr(bank, FLASH_NVWPAR);
+	target->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_AR), 4, 1, (u8*)&cmd);
+	
+	cmd = protect_blocks;
+	target->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_DR0), 4, 1, (u8*)&cmd);
+	
+	cmd = FLASH_SPR|FLASH_WMS;
+	target->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_CR0), 4, 1, (u8*)&cmd);
+	
+	while (((retval = str7x_status(bank)) & (FLASH_BSYA1|FLASH_BSYA2))){
+		usleep(1000);
+	}
+	
+	retval = str7x_result(bank);
+	
+	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(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
+{
+	str7x_flash_bank_t *str7x_info = bank->driver_priv;
+	target_t *target = str7x_info->target;
+	u32 dwords_remaining = (count / 8);
+	u32 bytes_remaining = (count & 0x00000007);
+	u32 address = bank->base + offset;
+	u32 *wordbuffer = (u32*)buffer;
+	u32 bytes_written = 0;
+	u32 cmd;
+	u32 retval;
+	
+	if (str7x_info->target->state != TARGET_HALTED)
+	{
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	
+	if (offset + count > bank->size)
+		return ERROR_FLASH_DST_OUT_OF_BANK;
+	
+	while (dwords_remaining > 0)
+	{
+		// command
+		cmd = FLASH_DWPG;
+		target->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_CR0), 4, 1, (u8*)&cmd);
+		
+		// address
+		cmd = address;
+		target->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_AR), 4, 1, (u8*)&cmd);
+		
+		// data byte 1
+		cmd = wordbuffer[bytes_written/4];
+		target->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_DR0), 4, 1, (u8*)&cmd);
+		bytes_written += 4;
+		
+		// data byte 2
+		cmd = wordbuffer[bytes_written/4];
+		target->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_DR1), 4, 1, (u8*)&cmd);
+		bytes_written += 4;
+		
+		/* start programming cycle */
+		cmd = FLASH_DWPG|FLASH_WMS;
+		target->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_CR0), 4, 1, (u8*)&cmd);
+		
+		while (((retval = str7x_status(bank)) & (FLASH_BSYA1|FLASH_BSYA2))){
+			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;
+	}
+	
+	while( bytes_remaining > 0 )
+	{
+		// command
+		cmd = FLASH_WPG;
+		target->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_CR0), 4, 1, (u8*)&cmd);
+		
+		// address
+		cmd = address;
+		target->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_AR), 4, 1, (u8*)&cmd);
+		
+		// data byte
+		cmd = buffer[bytes_written];
+		target->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_DR0), 4, 1, (u8*)&cmd);
+		
+		/* start programming cycle */
+		cmd = FLASH_WPG|FLASH_WMS;
+		target->type->write_memory(target, str7x_get_flash_adr(bank, FLASH_CR0), 4, 1, (u8*)&cmd);
+		
+		while (((retval = str7x_status(bank)) & (FLASH_BSYA1|FLASH_BSYA2))){
+			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;
+
+		address++;
+		bytes_remaining--;
+		bytes_written++;
+	}
+	
+	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;
+}
diff --git a/src/flash/str7x.h b/src/flash/str7x.h
new file mode 100644
index 00000000..fe63b5e5
--- /dev/null
+++ b/src/flash/str7x.h
@@ -0,0 +1,106 @@
+/***************************************************************************
+ *   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 STR7X_H
+#define STR7X_H
+
+#include "flash.h"
+#include "target.h"
+
+typedef struct str7x_flash_bank_s
+{
+	int bank1;
+	struct target_s *target;
+	u32 flash_base;
+} str7x_flash_bank_t;
+
+enum str7x_status_codes
+{
+	STR7X_CMD_SUCCESS = 0,
+	STR7X_INVALID_COMMAND = 1,
+	STR7X_SRC_ADDR_ERROR = 2,
+	STR7X_DST_ADDR_ERROR = 3,
+	STR7X_SRC_ADDR_NOT_MAPPED = 4,
+	STR7X_DST_ADDR_NOT_MAPPED = 5,
+	STR7X_COUNT_ERROR = 6,
+	STR7X_INVALID_SECTOR = 7,
+	STR7X_SECTOR_NOT_BLANK = 8,
+	STR7X_SECTOR_NOT_PREPARED = 9,
+	STR7X_COMPARE_ERROR = 10,
+	STR7X_BUSY = 11
+};
+
+/*  Flash registers */
+
+#define FLASH_CR0		0x00100000
+#define FLASH_CR1		0x00100004
+#define FLASH_DR0		0x00100008
+#define FLASH_DR1		0x0010000C
+#define FLASH_AR		0x00100010
+#define FLASH_ER		0x00100014
+#define FLASH_NVWPAR	0x0010DFB0
+#define FLASH_NVAPR0 	0x0010DFB8
+#define FLASH_NVAPR1 	0x0010DFBC
+
+/* FLASH_CR0 register bits */
+
+#define FLASH_WMS		0x80000000
+#define FLASH_SUSP		0x40000000
+#define FLASH_WPG   	0x20000000
+#define FLASH_DWPG		0x10000000
+#define FLASH_SER		0x08000000
+#define FLASH_SPR		0x01000000
+#define FLASH_BER		0x04000000
+#define FLASH_MER		0x02000000
+#define FLASH_BSYA1		0x00000002
+#define FLASH_BSYA2		0x00000004
+
+/* FLASH_CR1 regsiter bits */
+
+#define FLASH_B1S		0x02000000
+#define FLASH_B0S		0x01000000
+#define FLASH_B1F1		0x00020000
+#define FLASH_B1F0		0x00010000
+#define FLASH_B0F7		0x00000080
+#define FLASH_B0F6		0x00000040
+#define FLASH_B0F5		0x00000020
+#define FLASH_B0F4		0x00000010
+#define FLASH_B0F3		0x00000008
+#define FLASH_B0F2		0x00000004
+#define FLASH_B0F1		0x00000002
+#define FLASH_B0F0		0x00000001
+
+/* FLASH_ER register bits */
+
+#define FLASH_WPF		0x00000100
+#define FLASH_RESER		0x00000080
+#define FLASH_SEQER		0x00000040
+#define FLASH_10ER		0x00000008
+#define FLASH_PGER		0x00000004
+#define FLASH_ERER		0x00000002
+#define FLASH_ERR		0x00000001
+
+typedef struct str7x_mem_layout_s {
+	u32 sector_start;
+	u32 sector_size;
+	u32 reg_offset;
+} str7x_mem_layout_t;
+
+#endif /* STR7X_H */
+
diff --git a/src/helper/Makefile.am b/src/helper/Makefile.am
new file mode 100644
index 00000000..5fb2241d
--- /dev/null
+++ b/src/helper/Makefile.am
@@ -0,0 +1,6 @@
+INCLUDES = $(all_includes)
+METASOURCES = AUTO
+noinst_LIBRARIES = libhelper.a
+libhelper_a_SOURCES = binarybuffer.c configuration.c log.c interpreter.c command.c time_support.c
+noinst_HEADERS = binarybuffer.h configuration.h types.h log.h command.h \
+	interpreter.h time_support.h
diff --git a/src/helper/binarybuffer.c b/src/helper/binarybuffer.c
new file mode 100644
index 00000000..357d05c3
--- /dev/null
+++ b/src/helper/binarybuffer.c
@@ -0,0 +1,246 @@
+/***************************************************************************
+ *   Copyright (C) 2004, 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.             *
+ ***************************************************************************/
+
+#include <stdlib.h>
+#include <string.h>
+
+#include "types.h"
+#include "log.h"
+
+#include "binarybuffer.h"
+
+int buf_set_u32(u8* buffer, unsigned int first, unsigned int num, u32 value);
+u32 buf_get_u32(u8* buffer, unsigned int first, unsigned int num);
+u32 flip_u32(u32 value, unsigned int num);
+
+const unsigned char bit_reverse_table256[] = 
+{
+  0x00, 0x80, 0x40, 0xC0, 0x20, 0xA0, 0x60, 0xE0, 0x10, 0x90, 0x50, 0xD0, 0x30, 0xB0, 0x70, 0xF0, 
+  0x08, 0x88, 0x48, 0xC8, 0x28, 0xA8, 0x68, 0xE8, 0x18, 0x98, 0x58, 0xD8, 0x38, 0xB8, 0x78, 0xF8, 
+  0x04, 0x84, 0x44, 0xC4, 0x24, 0xA4, 0x64, 0xE4, 0x14, 0x94, 0x54, 0xD4, 0x34, 0xB4, 0x74, 0xF4, 
+  0x0C, 0x8C, 0x4C, 0xCC, 0x2C, 0xAC, 0x6C, 0xEC, 0x1C, 0x9C, 0x5C, 0xDC, 0x3C, 0xBC, 0x7C, 0xFC, 
+  0x02, 0x82, 0x42, 0xC2, 0x22, 0xA2, 0x62, 0xE2, 0x12, 0x92, 0x52, 0xD2, 0x32, 0xB2, 0x72, 0xF2, 
+  0x0A, 0x8A, 0x4A, 0xCA, 0x2A, 0xAA, 0x6A, 0xEA, 0x1A, 0x9A, 0x5A, 0xDA, 0x3A, 0xBA, 0x7A, 0xFA,
+  0x06, 0x86, 0x46, 0xC6, 0x26, 0xA6, 0x66, 0xE6, 0x16, 0x96, 0x56, 0xD6, 0x36, 0xB6, 0x76, 0xF6, 
+  0x0E, 0x8E, 0x4E, 0xCE, 0x2E, 0xAE, 0x6E, 0xEE, 0x1E, 0x9E, 0x5E, 0xDE, 0x3E, 0xBE, 0x7E, 0xFE,
+  0x01, 0x81, 0x41, 0xC1, 0x21, 0xA1, 0x61, 0xE1, 0x11, 0x91, 0x51, 0xD1, 0x31, 0xB1, 0x71, 0xF1,
+  0x09, 0x89, 0x49, 0xC9, 0x29, 0xA9, 0x69, 0xE9, 0x19, 0x99, 0x59, 0xD9, 0x39, 0xB9, 0x79, 0xF9, 
+  0x05, 0x85, 0x45, 0xC5, 0x25, 0xA5, 0x65, 0xE5, 0x15, 0x95, 0x55, 0xD5, 0x35, 0xB5, 0x75, 0xF5,
+  0x0D, 0x8D, 0x4D, 0xCD, 0x2D, 0xAD, 0x6D, 0xED, 0x1D, 0x9D, 0x5D, 0xDD, 0x3D, 0xBD, 0x7D, 0xFD,
+  0x03, 0x83, 0x43, 0xC3, 0x23, 0xA3, 0x63, 0xE3, 0x13, 0x93, 0x53, 0xD3, 0x33, 0xB3, 0x73, 0xF3, 
+  0x0B, 0x8B, 0x4B, 0xCB, 0x2B, 0xAB, 0x6B, 0xEB, 0x1B, 0x9B, 0x5B, 0xDB, 0x3B, 0xBB, 0x7B, 0xFB,
+  0x07, 0x87, 0x47, 0xC7, 0x27, 0xA7, 0x67, 0xE7, 0x17, 0x97, 0x57, 0xD7, 0x37, 0xB7, 0x77, 0xF7, 
+  0x0F, 0x8F, 0x4F, 0xCF, 0x2F, 0xAF, 0x6F, 0xEF, 0x1F, 0x9F, 0x5F, 0xDF, 0x3F, 0xBF, 0x7F, 0xFF
+};
+
+int buf_set_u32(u8* buffer, unsigned int first, unsigned int num, u32 value)
+{
+	unsigned int i;
+	
+	if (!buffer)
+		return ERROR_INVALID_ARGUMENTS;
+
+	for (i=first; i<first+num; i++)
+	{
+		if (((value >> (i-first))&1) == 1)
+			buffer[i/8] |= 1 << (i%8);
+		else
+			buffer[i/8] &= ~(1 << (i%8));
+	}
+	
+	return ERROR_OK;
+}
+
+u32 buf_get_u32(u8* buffer, unsigned int first, unsigned int num)
+{
+	u32 result = 0;
+	unsigned int i;
+	
+	if (!buffer)
+	{
+		ERROR("buffer not initialized");
+		return 0;
+	}
+
+	for (i=first; i<first+num; i++)
+	{
+		if (((buffer[i/8]>>(i%8))&1) == 1)
+			result |= 1 << (i-first);
+	}
+	
+	return result;
+}
+
+u8* buf_cpy(u8 *from, u8 *to, int size)
+{
+	int num_bytes = CEIL(size, 8);
+	unsigned int i;
+
+	if (from == NULL)
+		return NULL;
+
+	for (i = 0; i < num_bytes; i++)
+		to[i] = from[i];
+
+	return to;
+}
+
+int buf_cmp(u8 *buf1, u8 *buf2, int size)
+{
+	int num_bytes = CEIL(size, 8);
+	int i;
+
+	if (!buf1 || !buf2)
+		return 1;
+
+	for (i = 0; i < num_bytes; i++)
+	{
+		if (buf1[i] != buf2[i])
+			return 1;
+	}
+
+	return 0;
+}
+
+int buf_cmp_mask(u8 *buf1, u8 *buf2, u8 *mask, int size)
+{
+	int num_bytes = CEIL(size, 8);
+	int i;
+
+	for (i = 0; i < num_bytes; i++)
+	{
+		if ((buf1[i] & mask[i]) != (buf2[i] & mask[i]))
+			return 1;
+	}
+
+	return 0;
+}
+
+u8* buf_set_ones(u8 *buf, int count)
+{
+	int num_bytes = CEIL(count, 8);
+	int i;
+
+	for (i = 0; i < num_bytes; i++)
+	{
+		if (count >= 8)
+			buf[i] = 0xff;
+		else
+			buf[i] = (1 << count) - 1;
+	
+		count -= 8;
+	}
+	
+	return buf;
+}
+
+u8* buf_set_buf(u8 *src, int src_start, u8 *dst, int dst_start, int len)
+{
+	int src_idx = src_start, dst_idx = dst_start;
+	int i;
+	
+	for (i = 0; i < len; i++)
+	{
+		if (((src[src_idx/8] >> (src_idx % 8)) & 1) == 1)
+			dst[dst_idx/8] |= 1 << (dst_idx%8);
+		else
+			dst[dst_idx/8] &= ~(1 << (dst_idx%8));
+		dst_idx++;
+		src_idx++;
+	}
+
+	return dst;
+}
+
+u32 flip_u32(u32 value, unsigned int num)
+{
+	u32 c;
+	
+	c = (bit_reverse_table256[value & 0xff] << 24) | 
+		(bit_reverse_table256[(value >> 8) & 0xff] << 16) | 
+		(bit_reverse_table256[(value >> 16) & 0xff] << 8) |
+		(bit_reverse_table256[(value >> 24) & 0xff]);
+
+	if (num < 32)
+		c = c >> (32 - num);
+
+	return c;
+}
+
+char* buf_to_char(u8 *buf, int size)
+{
+	int char_len = CEIL(size, 8) * 2;
+	char *char_buf = malloc(char_len + 1);
+	int i;
+	int bits_left = size;
+	
+	char_buf[char_len] = 0;
+	
+	for (i = 0; i < CEIL(size, 8); i++)
+	{
+		if (bits_left < 8)
+		{
+			buf[i] &= ((1 << bits_left) - 1);
+		}
+		
+		if (((buf[i] & 0x0f) >= 0) && ((buf[i] & 0x0f) <= 9))
+			char_buf[char_len - 2*i - 1] = '0' + (buf[i] & 0xf);
+		else
+			char_buf[char_len - 2*i - 1] = 'a' + (buf[i] & 0xf) - 10;
+		
+		if (((buf[i] & 0xf0) >> 4 >= 0) && ((buf[i] & 0xf0) >> 4 <= 9))
+			char_buf[char_len - 2*i - 2] = '0' + ((buf[i] & 0xf0) >> 4);
+		else
+			char_buf[char_len - 2*i - 2] = 'a' + ((buf[i] & 0xf0) >> 4) - 10;
+		
+	}
+
+	return char_buf;
+}
+
+int char_to_buf(char *buf, int len, u8 *bin_buf, int buf_size)
+{
+	int bin_len = CEIL(len, 2);
+	int i;
+	
+	if (buf_size < CEIL(bin_len, 8))
+		return 0;
+	
+	if (len % 2)
+		return 0;
+	
+	for (i = 0; i < strlen(buf); i++)
+	{
+		u32 tmp;
+		sscanf(buf + 2*i, "%2x", &tmp);
+		bin_buf[i] = tmp & 0xff;
+	}
+	
+	return bin_len * 8;
+}
+
+int buf_to_u32_handler(u8 *in_buf, void *priv)
+{
+	u32 *dest = priv;
+	
+	*dest = buf_get_u32(in_buf, 0, 32);
+	
+	return ERROR_OK;
+}
diff --git a/src/helper/binarybuffer.h b/src/helper/binarybuffer.h
new file mode 100644
index 00000000..0a688945
--- /dev/null
+++ b/src/helper/binarybuffer.h
@@ -0,0 +1,48 @@
+/***************************************************************************
+ *   Copyright (C) 2004, 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 BINARYBUFFER_H
+#define BINARYBUFFER_H
+
+#include "types.h"
+
+/* support functions to access arbitrary bits in a byte array
+ * flip_u32 inverses the bit order inside a 32-bit word (31..0 -> 0..31)
+ */
+
+extern int buf_set_u32(u8* buffer, unsigned int first, unsigned int num, u32 value);
+extern u32 buf_get_u32(u8* buffer, unsigned int first, unsigned int num);
+
+extern u32 flip_u32(u32 value, unsigned int num);
+
+extern int buf_cmp(u8 *buf1, u8 *buf2, int size);
+extern int buf_cmp_mask(u8 *buf1, u8 *buf2, u8 *mask, int size);
+extern u8* buf_cpy(u8 *from, u8 *to, int size);
+
+extern u8* buf_set_ones(u8 *buf, int count);
+extern u8* buf_set_buf(u8 *src, int src_start, u8 *dst, int dst_start, int len);
+
+extern char* buf_to_char(u8 *buf, int size);
+extern int char_to_buf(char *buf, int len, u8 *bin_buf, int buf_size);
+
+extern int buf_to_u32_handler(u8 *in_buf, void *priv);
+
+#define CEIL(m, n)	((m + n - 1) / n)
+
+#endif /* BINARYBUFFER_H */
diff --git a/src/helper/command.c b/src/helper/command.c
new file mode 100644
index 00000000..26eada62
--- /dev/null
+++ b/src/helper/command.c
@@ -0,0 +1,508 @@
+/***************************************************************************
+ *   Copyright (C) 2005 by Dominic Rath                                    *
+ *   Dominic.Rath@gmx.de                                                   *
+ *                                                                         *
+ *   part of this file is taken from libcli (libcli.sourceforge.net)       *
+ *   Copyright (C) David Parrish (david@dparrish.com)                      *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#include "command.h"
+
+#include "log.h"
+
+#include <stdlib.h>
+#include <string.h>
+#include <ctype.h>
+#include <stdarg.h>
+#include <stdio.h>
+#include <unistd.h>
+
+int handle_sleep_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+int build_unique_lenghts(command_context_t *context, command_t *commands)
+{
+	command_t *c, *p;
+
+	/* iterate through all commands */
+	for (c = commands; c; c = c->next)
+	{
+		/* find out how many characters are required to uniquely identify a command */
+		for (c->unique_len = 1; c->unique_len <= strlen(c->name); c->unique_len++)
+		{
+			int foundmatch = 0;
+			
+			/* for every command, see if the current length is enough */
+			for (p = commands; p; p = p->next)
+			{
+				/* ignore the command itself */
+				if (c == p)
+					continue;
+				
+				/* compare commands up to the current length */
+				if (strncmp(p->name, c->name, c->unique_len) == 0)
+					foundmatch++;
+			}
+			
+			/* when none of the commands matched, we've found the minimum length required */
+			if (!foundmatch)
+				break;
+		}
+		
+		/* if the current command has children, build the unique lengths for them */
+		if (c->children)
+			build_unique_lenghts(context, c->children);
+	}
+	
+	return ERROR_OK;
+}
+
+command_t* register_command(command_context_t *context, command_t *parent, char *name, int (*handler)(struct command_context_s *context, char* name, char** args, int argc), enum command_mode mode, char *help)
+{
+	command_t *c, *p;
+	
+	if (!context || !name)
+		return NULL;
+				
+	c = malloc(sizeof(command_t));
+	
+	c->name = strdup(name);
+	c->parent = parent;
+	c->children = NULL;
+	c->handler = handler;
+	c->mode = mode;
+	if (help)
+		c->help = strdup(help);
+	else
+		c->help = NULL;
+	c->unique_len = 0;
+	c->next = NULL;
+	
+	/* place command in tree */
+	if (parent)
+	{
+		if (parent->children)
+		{
+			/* find last child */
+			for (p = parent->children; p && p->next; p = p->next);
+			if (p)
+				p->next = c;
+		}
+		else
+		{
+			parent->children = c;
+		}
+	}
+	else
+	{
+		if (context->commands)
+		{
+			/* find last command */
+			for (p = context->commands; p && p->next; p = p->next);
+			if (p)
+				p->next = c;
+		}
+		else
+		{
+			context->commands = c;
+		}
+	}
+	
+	/* update unique lengths */
+	build_unique_lenghts(context, (parent) ? parent : context->commands);
+	
+	return c;
+}
+
+int unregister_command(command_context_t *context, char *name)
+{
+	command_t *c, *p = NULL, *c2;
+	
+	if ((!context) || (!name))
+		return ERROR_INVALID_ARGUMENTS;
+	
+	/* find command */
+	for (c = context->commands; c; c = c->next)
+	{
+		if (strcmp(name, c->name) == 0)
+		{
+			/* unlink command */
+			if (p)
+			{
+				p->next = c->next;
+			}
+			else
+			{
+				context->commands = c->next;
+			}
+			
+			/* unregister children */
+			if (c->children)
+			{
+				for (c2 = c->children; c2; c2 = c2->next)
+				{
+					free(c2->name);
+					if (c2->help)
+						free(c2->help);
+					free(c2);
+				}
+			}
+			
+			/* delete command */
+			free(c->name);
+			if (c->help)
+				free(c->help);
+			free(c);
+		}
+		
+		/* remember the last command for unlinking */
+		p = c;
+	}
+	
+	return ERROR_OK;
+}
+
+int parse_line(char *line, char *words[], int max_words)
+{
+	int nwords = 0;
+	char *p = line;
+	char *word_start = line;
+	int inquote = 0;
+
+	while (nwords < max_words - 1)
+	{
+		/* check if we reached
+		 * a terminating NUL
+		 * a matching closing quote character " or '
+		 * we're inside a word but not a quote, and the current character is whitespace
+		 */
+		if (!*p || *p == inquote || (word_start && !inquote && isspace(*p)))
+		{
+			/* we're inside a word or quote, and reached its end*/
+			if (word_start)
+			{
+				int len = p - word_start;
+
+				/* copy the word */
+				memcpy(words[nwords] = malloc(len + 1), word_start, len);
+				/* add terminating NUL */
+				words[nwords++][len] = 0;
+			}
+
+			/* we're done parsing the line */
+			if (!*p)
+				break;
+
+			/* skip over trailing quote or whitespace*/
+			if (inquote || isspace(*p))
+				p++;
+
+			inquote = 0;
+			word_start = 0;
+		}
+		else if (*p == '"' || *p == '\'')
+		{
+			/* we've reached the beginning of a quote */
+			inquote = *p++;
+			word_start = p;
+		}
+		else
+		{
+			/* we've reached the beginning of a new word */
+			if (!word_start)
+				word_start = p;
+			
+			/* normal character, skip */
+			p++;
+		}
+	}
+	
+	return nwords;
+}
+
+void command_print(command_context_t *context, char *format, ...)
+{
+	va_list ap;
+	char *buffer = NULL;
+	int n, size = 0;
+	char *p;
+
+	va_start(ap, format);
+	
+	/* process format string */
+	/* TODO: possible bug. va_list is undefined after the first call to vsnprintf */
+	while (!buffer || (n = vsnprintf(buffer, size, format, ap)) >= size)
+	{
+		/* increase buffer until it fits the whole string */
+		if (!(p = realloc(buffer, size += 4096)))
+			return;
+
+		buffer = p;
+	}
+	
+	/* vsnprintf failed */
+	if (n < 0)
+		return;
+
+	p = buffer;
+	
+	/* process lines in buffer */
+	do {
+		char *next = strchr(p, '\n');
+		
+		if (next)
+			*next++ = 0;
+
+		if (context->output_handler)
+			context->output_handler(context, p);
+
+		p = next;
+	} while (p);
+	
+	if (buffer)
+		free(buffer);
+	
+	va_end(ap);
+}
+
+int find_and_run_command(command_context_t *context, command_t *commands, char *words[], int num_words, int start_word)
+{
+	command_t *c;
+	
+	for (c = commands; c; c = c->next)
+	{
+		if (strncasecmp(c->name, words[start_word], c->unique_len))
+			continue;
+
+		if (strncasecmp(c->name, words[start_word], strlen(words[start_word])))
+			continue;
+		
+		if ((c->mode == context->mode) || (c->mode == COMMAND_ANY))
+		{
+			if (!c->children)
+			{
+				if (!c->handler)
+				{
+					command_print(context, "No handler for command");
+					break;
+				}
+				else
+				{
+					return c->handler(context, c->name, words + start_word + 1, num_words - start_word - 1);
+				}
+			}
+			else
+			{
+				if (start_word == num_words - 1)
+				{
+					command_print(context, "Incomplete command");
+					break;
+				}
+				return find_and_run_command(context, c->children, words, num_words, start_word + 1);
+			}
+		}
+	}
+	
+	command_print(context, "Command %s not found", words[start_word]);
+	return ERROR_OK;
+}
+
+int command_run_line(command_context_t *context, char *line)
+{
+	int nwords;
+	char *words[128] = {0};
+	int retval;
+	int i;
+	
+	if ((!context) || (!line))
+		return ERROR_INVALID_ARGUMENTS;
+	
+	/* skip preceding whitespace */
+	while (isspace(*line))
+		line++;
+	
+	/* empty line, ignore */
+	if (!*line)
+		return ERROR_OK;
+	
+	if (context->echo)
+	{
+		command_print(context, "%s", line);
+	}
+
+	nwords = parse_line(line, words, sizeof(words) / sizeof(words[0]));
+	
+	if (nwords > 0)
+		retval = find_and_run_command(context, context->commands, words, nwords, 0);
+	else
+		return ERROR_INVALID_ARGUMENTS;
+	
+	for (i = 0; i < nwords; i++)
+		free(words[i]);
+	
+	return retval;
+}
+
+int command_run_file(command_context_t *context, FILE *file, enum command_mode mode)
+{
+	int retval;
+	int old_command_mode;
+	char buffer[4096];
+	
+	old_command_mode = context->mode;
+	context->mode = mode;
+	
+	while (fgets(buffer, 4096, file))
+	{
+		char *p;
+		char *cmd, *end;
+		
+		/* stop processing line after a comment (#, !) or a LF, CR were encountered */
+		if ((p = strpbrk(buffer, "#!\r\n")))
+			*p = 0;
+
+		/* skip over leading whitespace */
+		cmd = buffer;
+		while (isspace(*cmd))
+			cmd++;
+
+		/* empty (all whitespace) line? */
+		if (!*cmd)
+			continue;
+		
+		/* search the end of the current line, ignore trailing whitespace */
+		for (p = end = cmd; *p; p++)
+			if (!isspace(*p))
+				end = p;
+		
+		/* terminate end */
+		*++end = 0;
+		if (strcasecmp(cmd, "quit") == 0)
+			break;
+
+		/* run line */
+		if (command_run_line(context, cmd) == ERROR_COMMAND_CLOSE_CONNECTION)
+			break;
+	}
+	
+	context->mode = old_command_mode;
+	
+	return retval;
+}
+
+void command_print_help_line(command_context_t* context, struct command_s *command, int indent)
+{
+	command_t *c;
+	char indents[32] = {0};
+	char *help = "no help available";
+	char name_buf[64];
+	int i;
+	
+	for (i = 0; i < indent; i+=2)
+	{
+		indents[i*2] = ' ';
+		indents[i*2+1] = '-';
+	}
+	indents[i*2] = 0;
+	
+	if ((command->mode == COMMAND_EXEC) || (command->mode == COMMAND_ANY))
+	{
+		if (command->help)
+			help = command->help;
+		
+		snprintf(name_buf, 64, command->name);
+		strncat(name_buf, indents, 64);
+		command_print(context, "%20s\t%s", name_buf, help);
+	}
+	
+	if (command->children)
+	{
+		for (c = command->children; c; c = c->next)
+		{
+			command_print_help_line(context, c, indent + 1);
+		}
+	}
+}
+
+int command_print_help(command_context_t* context, char* name, char** args, int argc)
+{
+	command_t *c;
+
+	for (c = context->commands; c; c = c->next)
+	{
+		command_print_help_line(context, c, 0);
+	}
+	
+	return ERROR_OK;
+}
+
+void command_set_output_handler(command_context_t* context, int (*output_handler)(struct command_context_s *context, char* line), void *priv)
+{
+	context->output_handler = output_handler;
+	context->output_handler_priv = priv;
+}
+
+command_context_t* copy_command_context(command_context_t* context)
+{
+	command_context_t* copy_context = malloc(sizeof(command_context_t));
+
+	*copy_context = *context;
+	
+	return copy_context;
+}
+
+int command_done(command_context_t *context)
+{
+	free(context);
+	
+	return ERROR_OK;
+}
+
+command_context_t* command_init()
+{
+	command_context_t* context = malloc(sizeof(command_context_t));
+	
+	context->mode = COMMAND_EXEC;
+	context->commands = NULL;
+	context->current_target = 0;
+	context->echo = 0;
+	context->output_handler = NULL;
+	context->output_handler_priv = NULL;
+	
+	register_command(context, NULL, "help", command_print_help,
+					 COMMAND_EXEC, "display this help");
+	
+	register_command(context, NULL, "sleep", handle_sleep_command,
+					 COMMAND_ANY, "sleep for <n> milliseconds");
+	
+	return context;
+}
+
+/* sleep command sleeps for <n> miliseconds
+ * this is useful in target startup scripts
+ */
+int handle_sleep_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	unsigned long duration = 0;
+	
+	if (argc == 1)
+	{
+		duration = strtoul(args[0], NULL, 0);
+		usleep(duration * 1000);
+	}
+
+	return ERROR_OK;
+}
diff --git a/src/helper/command.h b/src/helper/command.h
new file mode 100644
index 00000000..262786a8
--- /dev/null
+++ b/src/helper/command.h
@@ -0,0 +1,67 @@
+/***************************************************************************
+ *   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 COMMAND_H
+#define COMMAND_H
+
+#include <stdio.h>
+
+enum command_mode
+{
+	COMMAND_EXEC,
+	COMMAND_CONFIG,
+	COMMAND_ANY,
+};
+
+typedef struct command_context_s
+{
+	enum command_mode mode;
+	struct command_s *commands;
+	int current_target;
+	int echo;
+	int (*output_handler)(struct command_context_s *context, char* line);
+	void *output_handler_priv;
+} command_context_t;
+
+typedef struct command_s
+{
+	char *name;
+	struct command_s *parent;
+	struct command_s *children;
+	int (*handler)(struct command_context_s *context, char* name, char** args, int argc);
+	enum command_mode mode;
+	char *help;
+	int unique_len;
+	struct command_s *next;
+} command_t;
+
+extern command_t* register_command(command_context_t *context, command_t *parent, char *name, int (*handler)(struct command_context_s *context, char* name, char** args, int argc), enum command_mode mode, char *help);
+extern int unregister_command(command_context_t *context, char *name);
+extern void command_set_output_handler(command_context_t* context, int (*output_handler)(struct command_context_s *context, char* line), void *priv);
+extern command_context_t* copy_command_context(command_context_t* context);
+extern command_context_t* command_init();
+extern int command_done(command_context_t *context);
+extern void command_print(command_context_t *context, char *format, ...);
+extern int command_run_line(command_context_t *context, char *line);
+extern int command_run_file(command_context_t *context, FILE *file, enum command_mode mode);
+
+
+#define		ERROR_COMMAND_CLOSE_CONNECTION		(-600)
+
+#endif /* COMMAND_H */
diff --git a/src/helper/configuration.c b/src/helper/configuration.c
new file mode 100644
index 00000000..e59ca6d1
--- /dev/null
+++ b/src/helper/configuration.c
@@ -0,0 +1,131 @@
+/***************************************************************************
+ *   Copyright (C) 2004, 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 "types.h"
+#include "command.h"
+#include "configuration.h"
+#include "log.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <getopt.h>
+
+char* config_file_name;
+
+static int help_flag;
+
+static struct option long_options[] =
+{
+	{"help",			no_argument,	&help_flag, 1},
+
+	{"debug",			optional_argument,	0, 'd'},
+	{"file", 			required_argument,	0, 'f'},
+	{"log_output",		required_argument,	0, 'l'},
+	
+	{0, 0, 0, 0}
+};
+
+int configuration_output_handler(struct command_context_s *context, char* line)
+{
+	INFO(line);
+	
+	return ERROR_OK;
+}
+
+int parse_cmdline_args(struct command_context_s *cmd_ctx, int argc, char *argv[])
+{
+	int c;
+	char command_buffer[128];
+			
+	while (1)
+	{	
+		/* getopt_long stores the option index here. */
+		int option_index = 0;
+		
+		c = getopt_long(argc, argv, "hd::l:f:", long_options, &option_index);
+		
+		/* Detect the end of the options. */
+		if (c == -1)
+			break;
+		
+		switch (c)
+		{
+			case 0:
+				break;
+			case 'h':	/* --help | -h */
+				help_flag = 1;
+				break;
+			case 'f':	/* --file | -f */
+				config_file_name = optarg;
+				break;
+			case 'd':	/* --debug | -d */
+				if (optarg)
+					snprintf(command_buffer, 128, "debug_level %s", optarg);
+				else
+					snprintf(command_buffer, 128, "debug_level 3");
+				command_run_line(cmd_ctx, command_buffer);
+				break;
+			case 'l':	/* --log_output | -l */
+				if (optarg)
+				{
+					snprintf(command_buffer, 128, "log_output %s", optarg);
+					command_run_line(cmd_ctx, command_buffer);
+				}	
+				break;
+		}
+	}
+
+	if (help_flag)
+	{
+		printf("Open On-Chip Debugger\n(c) 2005 by Dominic Rath\n\n");
+		printf("--help       | -h\tdisplay this help\n");
+		printf("--file       | -f\tuse configuration file <name>\n");
+		printf("--debug      | -d\tset debug level <0-3>\n");
+		printf("--log_output | -l\tredirect log output to file <name>\n");
+		exit(-1);
+	}	
+
+	return ERROR_OK;
+}
+
+int parse_config_file(struct command_context_s *cmd_ctx)
+{
+	FILE *config_file;
+
+	if (!config_file_name)
+		config_file_name = "openocd.cfg";
+
+	config_file = fopen(config_file_name, "r");
+	if (!config_file)
+	{
+		ERROR("couldn't open config file");
+		return ERROR_NO_CONFIG_FILE;
+	}
+
+	command_run_file(cmd_ctx, config_file, COMMAND_CONFIG);
+
+	fclose(config_file);
+
+	return ERROR_OK;
+}
+
diff --git a/src/helper/configuration.h b/src/helper/configuration.h
new file mode 100644
index 00000000..cd96e2f7
--- /dev/null
+++ b/src/helper/configuration.h
@@ -0,0 +1,31 @@
+/***************************************************************************
+ *   Copyright (C) 2004, 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 CONFIGURATION_H
+#define CONFIGURATION_H
+
+#include "command.h"
+#include "types.h"
+
+extern int parse_cmdline_args(struct command_context_s *cmd_ctx, int argc, char *argv[]);
+extern int parse_config_file(struct command_context_s *cmd_ctx);
+extern int configuration_output_handler(struct command_context_s *context, char* line);
+
+extern char* config_file_name;
+#endif /* CONFIGURATION_H */
diff --git a/src/helper/interpreter.c b/src/helper/interpreter.c
new file mode 100644
index 00000000..7e88263b
--- /dev/null
+++ b/src/helper/interpreter.c
@@ -0,0 +1,237 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "interpreter.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)
+{
+	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 = fopen(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, COMMAND_EXEC);
+	
+	cmd_ctx->echo = echo;
+	
+	fclose(script_file);
+
+	return ERROR_OK;
+}
diff --git a/src/helper/interpreter.h b/src/helper/interpreter.h
new file mode 100644
index 00000000..93e8d39d
--- /dev/null
+++ b/src/helper/interpreter.h
@@ -0,0 +1,48 @@
+/***************************************************************************
+ *   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 INTERPRETER_H
+#define INTERPRETER_H
+
+#include "types.h"
+#include "command.h"
+#include "log.h"
+
+typedef struct var_field_s
+{
+	int num_bits;
+	u32 value;
+} var_field_t;
+
+typedef struct var_s
+{
+	char *name;
+	int num_fields;
+	var_field_t *fields;
+	struct var_s *next;
+} var_t;
+
+extern var_t *variables;
+
+extern int field_le_to_host(u8 *buffer, void *priv);
+
+extern var_t* get_var_by_namenum(char *namenum);
+extern int interpreter_register_commands(struct command_context_s *cmd_ctx);
+
+#endif /* INTERPRETER_H */
diff --git a/src/helper/log.c b/src/helper/log.c
new file mode 100644
index 00000000..60ba80bd
--- /dev/null
+++ b/src/helper/log.c
@@ -0,0 +1,134 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "log.h"
+#include "configuration.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdarg.h>
+
+int debug_level = -1;
+
+static FILE* log_output;
+
+static char *log_strings[4] = 
+{
+	"Error:  ",
+	"Warning:",
+	"Info:   ",
+	"Debug:  ",
+};
+
+void log_printf(enum log_levels level, const char *file, int line, const char *function, const char *format, ...)
+{
+	va_list args;
+	char buffer[512];
+
+	if (level > debug_level)
+		return;
+
+	va_start(args, format);
+	vsnprintf(buffer, 512, format, args);
+
+	fprintf(log_output, "%s %s:%d %s(): %s\n", log_strings[level], file, line, function, buffer);
+	fflush(log_output);
+	
+	va_end(args);
+}
+
+void short_log_printf(enum log_levels level, const char *format, ...)
+{
+	va_list args;
+	char buffer[512];
+
+	if (level > debug_level)
+		return;
+
+	va_start(args, format);
+	vsnprintf(buffer, 512, format, args);
+
+	fprintf(log_output, "%s %s\n", log_strings[level], buffer);
+	fflush(log_output);
+
+	va_end(args);
+}
+
+/* change the current debug level on the fly
+ * 0: only ERRORS
+ * 1: + WARNINGS
+ * 2: + INFORMATIONAL MSGS
+ * 3: + DEBUG MSGS
+ */
+int handle_debug_level_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	if (argc == 0)
+		command_print(cmd_ctx, "debug_level: %i", debug_level);
+
+	if (argc > 0)
+		debug_level = strtoul(args[0], NULL, 0);
+
+	if (debug_level < 0)
+		debug_level = 0;
+
+	if (debug_level > 3)
+		debug_level = 3;
+
+	return ERROR_OK;
+}
+
+int handle_log_output_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	if (argc == 1)
+	{
+		FILE* file = fopen(args[0], "w");
+		
+		if (file)
+		{
+			log_output = file;
+		}
+	}
+
+	return ERROR_OK;
+}
+
+int log_register_commands(struct command_context_s *cmd_ctx)
+{
+	register_command(cmd_ctx, NULL, "log_output", handle_log_output_command,
+		COMMAND_ANY, "redirect logging to <file> (default: stderr)");
+	register_command(cmd_ctx, NULL, "debug_level", handle_debug_level_command,
+		COMMAND_ANY, "adjust debug level <0-3>");
+
+	return ERROR_OK;
+}
+
+int log_init(struct command_context_s *cmd_ctx)
+{
+	/* set defaults for daemon configuration, if not set by cmdline or cfgfile */
+	if (debug_level == -1)
+		debug_level = LOG_INFO;
+	
+	if (log_output == NULL)
+	{
+		log_output = stderr;
+	}
+	
+	return ERROR_OK;
+}
diff --git a/src/helper/log.h b/src/helper/log.h
new file mode 100644
index 00000000..c495524c
--- /dev/null
+++ b/src/helper/log.h
@@ -0,0 +1,96 @@
+/***************************************************************************
+ *   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 ERROR_H
+#define ERROR_H
+
+#include "command.h"
+
+#include <stdarg.h>
+
+/* logging priorities 
+ * LOG_ERROR - fatal errors, that are likely to cause program abort
+ * LOG_WARNING - non-fatal errors, that may be resolved later
+ * LOG_INFO - state information, etc.
+ * LOG_DEBUG - debug statements, execution trace
+ */
+enum log_levels
+{
+	LOG_ERROR = 0,
+	LOG_WARNING = 1,
+	LOG_INFO = 2,
+	LOG_DEBUG = 3
+};
+
+extern void log_printf(enum log_levels level, const char *file, int line, 
+	const char *function, const char *format, ...);
+extern int log_register_commands(struct command_context_s *cmd_ctx);
+extern int log_init(struct command_context_s *cmd_ctx);
+
+extern int debug_level;
+
+#define DEBUG(expr ...) \
+	do { \
+		log_printf (LOG_DEBUG, __FILE__, __LINE__, __FUNCTION__, expr); \
+	} while(0)
+
+#define INFO(expr ...) \
+	do { \
+		log_printf (LOG_INFO, __FILE__, __LINE__, __FUNCTION__, expr); \
+	} while(0)
+
+#define WARNING(expr ...) \
+	do { \
+		log_printf (LOG_WARNING, __FILE__, __LINE__, __FUNCTION__, expr); \
+	} while(0)
+
+#define ERROR(expr ...) \
+	do { \
+		log_printf (LOG_ERROR, __FILE__, __LINE__, __FUNCTION__, expr); \
+	} while(0)
+
+#define SDEBUG(expr ...) \
+	do { \
+		short_log_printf (LOG_DEBUG, expr); \
+	} while(0)
+
+#define SINFO(expr ...) \
+	do { \
+		short_log_printf (LOG_INFO, expr); \
+	} while(0)
+
+#define SWARNING(expr ...) \
+	do { \
+		short_log_printf (LOG_WARNING, expr); \
+	} while(0)
+
+#define SERROR(expr ...) \
+	do { \
+		short_log_printf (LOG_ERROR, expr); \
+	} while(0)
+
+/* general failures
+ * error codes < 100
+ */
+#define ERROR_OK					(0)
+#define ERROR_INVALID_ARGUMENTS		(-1)
+#define ERROR_NO_CONFIG_FILE		(-2)
+#define ERROR_BUF_TOO_SMALL			(-3)
+
+#endif /* ERROR_H */
diff --git a/src/helper/time_support.c b/src/helper/time_support.c
new file mode 100644
index 00000000..5a7869d9
--- /dev/null
+++ b/src/helper/time_support.c
@@ -0,0 +1,82 @@
+/***************************************************************************
+ *   Copyright (C) 2006 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.             *
+ ***************************************************************************/
+#include "config.h"
+
+#include "time_support.h"
+
+#include <sys/time.h>
+#include <time.h>
+
+int timeval_subtract(struct timeval *result, struct timeval *x, struct timeval *y);
+int timeval_add(struct timeval *result, struct timeval *x, struct timeval *y);
+int timeval_add_time(struct timeval *result, int sec, int usec);
+
+/* calculate difference between two struct timeval values */
+int timeval_subtract(struct timeval *result, struct timeval *x, struct timeval *y)
+{
+	if (x->tv_usec < y->tv_usec)
+	{
+		int nsec = (y->tv_usec - x->tv_usec) / 1000000 + 1;
+		y->tv_usec -= 1000000 * nsec;
+		y->tv_sec += nsec;
+	}
+	if (x->tv_usec - y->tv_usec > 1000000) {
+		int nsec = (x->tv_usec - y->tv_usec) / 1000000;
+		y->tv_usec += 1000000 * nsec;
+		y->tv_sec -= nsec;
+	}
+
+	result->tv_sec = x->tv_sec - y->tv_sec;
+	result->tv_usec = x->tv_usec - y->tv_usec;
+
+	/* Return 1 if result is negative. */
+	return x->tv_sec < y->tv_sec;
+}
+
+/* add two struct timeval values */
+int timeval_add(struct timeval *result, struct timeval *x, struct timeval *y)
+{
+	result->tv_sec = x->tv_sec + y->tv_sec;
+	
+	result->tv_usec = x->tv_usec + y->tv_usec;
+	
+	while (result->tv_usec > 1000000)
+	{
+		result->tv_usec -= 1000000;
+		result->tv_sec++;
+	}
+	
+	return 0;
+}
+
+int timeval_add_time(struct timeval *result, int sec, int usec)
+{
+	result->tv_sec += sec;
+	result->tv_usec += usec;
+	
+	while (result->tv_usec > 1000000)
+	{
+		result->tv_usec -= 1000000;
+		result->tv_sec++;
+	}
+	
+	return 0;
+}
+
diff --git a/src/helper/time_support.h b/src/helper/time_support.h
new file mode 100644
index 00000000..d8b7fe5c
--- /dev/null
+++ b/src/helper/time_support.h
@@ -0,0 +1,30 @@
+/***************************************************************************
+ *   Copyright (C) 2006 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 TIME_SUPPORT_H
+#define TIME_SUPPORT_H
+
+#include <sys/time.h>
+#include <time.h>
+
+extern int timeval_subtract(struct timeval *result, struct timeval *x, struct timeval *y);
+extern int timeval_add(struct timeval *result, struct timeval *x, struct timeval *y);
+extern int timeval_add_time(struct timeval *result, int sec, int usec);
+
+#endif /* TIME_SUPPORT_H */
diff --git a/src/helper/types.h b/src/helper/types.h
new file mode 100644
index 00000000..6d49bbb0
--- /dev/null
+++ b/src/helper/types.h
@@ -0,0 +1,36 @@
+/***************************************************************************
+ *   Copyright (C) 2004, 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 TYPES_H
+#define TYPES_H
+
+#ifndef u8
+typedef unsigned char u8;
+#endif
+
+#ifndef u16
+typedef unsigned short u16;
+#endif
+
+#ifndef u32
+typedef unsigned int u32;
+#endif
+
+#endif /* TYPES_H */
diff --git a/src/jtag/Makefile.am b/src/jtag/Makefile.am
new file mode 100644
index 00000000..a3a06606
--- /dev/null
+++ b/src/jtag/Makefile.am
@@ -0,0 +1,50 @@
+
+if FTD2XXDIR
+FTD2XXINC = -I@WITH_FTD2XX@/
+else
+FTD2XXINC =
+endif
+
+INCLUDES = -I$(top_srcdir)/src/helper $(FTD2XXINC) $(all_includes)
+METASOURCES = AUTO
+noinst_LIBRARIES = libjtag.a
+
+if BITBANG
+BITBANGFILES = bitbang.c
+else
+BITBANGFILES =
+endif
+
+if PARPORT
+PARPORTFILES = parport.c
+else
+PARPORTFILES =
+endif
+
+if FTDI2232
+FTDI2232FILES = ftdi2232.c
+else
+FTDI2232FILES =
+endif
+
+if FTD2XX
+FTD2XXFILES = ftd2xx.c
+else
+FTD2XXFILES =
+endif
+
+if AMTJTAGACCEL
+AMTJTAGACCELFILES = amt_jtagaccel.c
+else
+AMTJTAGACCELFILES =
+endif
+
+if EP93XX
+EP93XXFILES = ep93xx.c
+else
+EP93XXFILES =
+endif
+
+libjtag_a_SOURCES = jtag.c $(BITBANGFILES) $(PARPORTFILES) $(FTDI2232FILES) $(FTD2XXFILES) $(AMTJTAGACCELFILES) $(EP93XXFILES)
+
+noinst_HEADERS = bitbang.h jtag.h
diff --git a/src/jtag/amt_jtagaccel.c b/src/jtag/amt_jtagaccel.c
new file mode 100644
index 00000000..42f8bc36
--- /dev/null
+++ b/src/jtag/amt_jtagaccel.c
@@ -0,0 +1,510 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "config.h"
+#include "log.h"
+#include "jtag.h"
+
+/* system includes */
+#include <sys/io.h>
+#include <string.h>
+#include <stdlib.h>
+
+#include <sys/time.h>
+#include <time.h>
+
+#if PARPORT_USE_PPDEV == 1
+#include <linux/parport.h>
+#include <linux/ppdev.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+#endif
+
+/* configuration */
+unsigned long amt_jtagaccel_port;
+
+/* interface variables
+ */
+static u8 aw_control_rst = 0x00;
+static u8 aw_control_fsm = 0x10;
+static u8 aw_control_baudrate = 0x20;
+
+static int rtck_enabled = 0;
+
+#if PARPORT_USE_PPDEV == 1
+static int device_handle;
+int addr_mode = IEEE1284_MODE_EPP | IEEE1284_ADDR ;
+int data_mode = IEEE1284_MODE_EPP | IEEE1284_DATA ;
+#define AMT_AW(val)	do { ioctl(device_handle, PPSETMODE, &addr_mode); write(device_handle, &val, 1); } while (0)
+#define AMT_AR(val)	do { ioctl(device_handle, PPSETMODE, &addr_mode); read(device_handle, &val, 1); } while (0)
+#define AMT_DW(val)	do { ioctl(device_handle, PPSETMODE, &data_mode); write(device_handle, &val, 1); } while (0)
+#define AMT_DR(val)	do { ioctl(device_handle, PPSETMODE, &data_mode); read(device_handle, &val, 1); } while (0)
+#else
+#define AMT_AW(val)	do { outb(val, amt_jtagaccel_port + 3); } while (0)
+#define AMT_AR(val)	do { val = inb(amt_jtagaccel_port + 3); } while (0)
+#define AMT_DW(val)	do { outb(val, amt_jtagaccel_port + 4); } while (0)
+#define AMT_DR(val)	do { val = inb(amt_jtagaccel_port + 4); } while (0)
+#endif
+
+int amt_jtagaccel_execute_queue(void);
+int amt_jtagaccel_register_commands(struct command_context_s *cmd_ctx);
+int amt_jtagaccel_speed(int speed);
+int amt_jtagaccel_init(void);
+int amt_jtagaccel_quit(void);
+
+int amt_jtagaccel_handle_parport_port_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int amt_jtagaccel_handle_rtck_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+/* tap_move[i][j]: tap movement command to go from state i to state j
+ * 0: Test-Logic-Reset
+ * 1: Run-Test/Idle
+ * 2: Shift-DR
+ * 3: Pause-DR
+ * 4: Shift-IR
+ * 5: Pause-IR
+ */
+u8 amt_jtagaccel_tap_move[6][6][2] =
+{
+	/*	   TLR           RTI              SD            PD            SI            PI             */
+	{{0x1f, 0x00}, {0x0f, 0x00}, {0x8a, 0x04}, {0x0a, 0x00}, {0x06, 0x00}, {0x96, 0x00}},	/* TLR */
+	{{0x1f, 0x00}, {0x00, 0x00}, {0x85, 0x08}, {0x05, 0x00}, {0x8b, 0x08}, {0x0b, 0x00}},	/* RTI */
+	{{0x1f, 0x00}, {0x0d, 0x00}, {0x00, 0x00}, {0x01, 0x00}, {0x8f, 0x09}, {0x8f, 0x01}},	/* SD  */
+	{{0x1f, 0x00}, {0x0c, 0x00}, {0x08, 0x00}, {0x00, 0x00}, {0x8f, 0x09}, {0x8f, 0x01}},	/* PD  */
+	{{0x1f, 0x00}, {0x0d, 0x00}, {0x07, 0x00}, {0x97, 0x00}, {0x00, 0x00}, {0x01, 0x00}},	/* SI  */
+	{{0x1f, 0x00}, {0x0c, 0x00}, {0x07, 0x00}, {0x97, 0x00}, {0x08, 0x00}, {0x00, 0x00}},	/* PI  */
+};
+
+jtag_interface_t amt_jtagaccel_interface = 
+{
+	.name = "amt_jtagaccel",
+	
+	.execute_queue = amt_jtagaccel_execute_queue,
+
+	.support_statemove = 0,
+
+	.speed = amt_jtagaccel_speed,	
+	.register_commands = amt_jtagaccel_register_commands,
+	.init = amt_jtagaccel_init,
+	.quit = amt_jtagaccel_quit,
+};
+
+int amt_jtagaccel_register_commands(struct command_context_s *cmd_ctx)
+{
+	register_command(cmd_ctx, NULL, "parport_port", amt_jtagaccel_handle_parport_port_command,
+					 COMMAND_CONFIG, NULL);
+	register_command(cmd_ctx, NULL, "rtck", amt_jtagaccel_handle_rtck_command,
+					 COMMAND_CONFIG, NULL);
+	
+	return ERROR_OK;
+}
+
+void amt_jtagaccel_reset(int trst, int srst)
+{
+	if (trst == 1)
+		aw_control_rst |= 0x4;
+	else if (trst == 0)
+		aw_control_rst &= ~0x4;
+
+	if (srst == 1)
+		aw_control_rst |= 0x1;
+	else if (srst == 0)
+		aw_control_rst &= ~0x1;
+	
+	AMT_AW(aw_control_rst);
+}
+
+int amt_jtagaccel_speed(int speed)
+{
+	aw_control_baudrate &= 0xf0;
+	aw_control_baudrate |= speed & 0x0f;
+	AMT_AW(aw_control_baudrate);
+	
+	return ERROR_OK;
+}
+
+void amt_jtagaccel_end_state(state)
+{
+	if (tap_move_map[state] != -1)
+		end_state = state;
+	else
+	{
+		ERROR("BUG: %i is not a valid end state", state);
+		exit(-1);
+	}
+}
+
+void amt_wait_scan_busy()
+{
+	int timeout = 4096;
+	u8 ar_status;
+	
+	AMT_AR(ar_status);
+	while (((ar_status) & 0x80) && (timeout-- > 0))
+		AMT_AR(ar_status);
+	
+	if (ar_status & 0x80)
+	{
+		ERROR("amt_jtagaccel timed out while waiting for end of scan, rtck was %s", (rtck_enabled) ? "enabled" : "disabled");
+		exit(-1);
+	}
+}
+
+void amt_jtagaccel_state_move(void)
+{
+	u8 aw_scan_tms_5;
+	u8 tms_scan[2];
+	 
+	tms_scan[0] = amt_jtagaccel_tap_move[tap_move_map[cur_state]][tap_move_map[end_state]][0];
+	tms_scan[1] = amt_jtagaccel_tap_move[tap_move_map[cur_state]][tap_move_map[end_state]][1];
+	
+	aw_scan_tms_5 = 0x40 | (tms_scan[0] & 0x1f);
+	AMT_AW(aw_scan_tms_5);
+	if (jtag_speed > 3 || rtck_enabled)
+		amt_wait_scan_busy();
+		
+	if (tms_scan[0] & 0x80)
+	{
+		aw_scan_tms_5 = 0x40 | (tms_scan[1] & 0x1f);
+		AMT_AW(aw_scan_tms_5);
+		if (jtag_speed > 3 || rtck_enabled)
+			amt_wait_scan_busy();
+	}
+	
+	cur_state = end_state;
+}
+
+void amt_jtagaccel_runtest(int num_cycles)
+{
+	int i = 0;
+	u8 aw_scan_tms_5;
+	u8 aw_scan_tms_1to4;
+
+	enum tap_state saved_end_state = end_state;
+	
+	/* only do a state_move when we're not already in RTI */
+	if (cur_state != TAP_RTI)
+	{
+		amt_jtagaccel_end_state(TAP_RTI);
+		amt_jtagaccel_state_move();
+	}
+	
+	while (num_cycles - i >= 5)
+	{
+		aw_scan_tms_5 = 0x40;
+		AMT_AW(aw_scan_tms_5);
+		i += 5;
+	}
+	
+	if (num_cycles - i > 0)
+	{
+		aw_scan_tms_1to4 = 0x80 | ((num_cycles - i - 1) & 0x3) << 4;
+		AMT_AW(aw_scan_tms_1to4);
+	}
+	
+	amt_jtagaccel_end_state(saved_end_state);
+	if (cur_state != end_state)
+		amt_jtagaccel_state_move();
+}
+
+void amt_jtagaccel_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size)
+{
+	int bits_left = scan_size;
+	int bit_count = 0;
+	enum tap_state saved_end_state = end_state;
+	u8 aw_tdi_option;
+	u8 dw_tdi_scan;
+	u8 dr_tdo;
+	u8 aw_tms_scan;
+	u8 tms_scan[2];
+
+	if (ir_scan)
+		amt_jtagaccel_end_state(TAP_SI);
+	else
+		amt_jtagaccel_end_state(TAP_SD);
+
+	amt_jtagaccel_state_move();
+	amt_jtagaccel_end_state(saved_end_state);
+
+	/* handle unaligned bits at the beginning */
+	if ((scan_size - 1) % 8)
+	{
+		aw_tdi_option = 0x30 | (((scan_size - 1) % 8) - 1);
+		AMT_AW(aw_tdi_option);
+		
+		dw_tdi_scan = buf_get_u32(buffer, bit_count, (scan_size - 1) % 8) & 0xff;
+		AMT_DW(dw_tdi_scan);
+		if (jtag_speed > 3 || rtck_enabled)
+			amt_wait_scan_busy();
+
+		if ((type == SCAN_IN) || (type == SCAN_IO))
+		{
+			AMT_DR(dr_tdo);
+			dr_tdo = dr_tdo >> (8 - ((scan_size - 1) % 8));
+			buf_set_u32(buffer, bit_count, (scan_size - 1) % 8, dr_tdo);
+		}
+		
+		bit_count += (scan_size - 1) % 8;
+		bits_left -= (scan_size - 1) % 8;
+	}
+	
+	while (bits_left - 1 >= 8)
+	{
+		dw_tdi_scan = buf_get_u32(buffer, bit_count, 8) & 0xff;
+		AMT_DW(dw_tdi_scan);
+		if (jtag_speed > 3 || rtck_enabled)
+			amt_wait_scan_busy();
+
+		if ((type == SCAN_IN) || (type == SCAN_IO))
+		{
+			AMT_DR(dr_tdo);
+			buf_set_u32(buffer, bit_count, 8, dr_tdo);
+		}
+				
+		bit_count += 8;
+		bits_left -= 8;
+	}
+		
+	tms_scan[0] = amt_jtagaccel_tap_move[tap_move_map[cur_state]][tap_move_map[end_state]][0];
+	tms_scan[1] = amt_jtagaccel_tap_move[tap_move_map[cur_state]][tap_move_map[end_state]][1];
+	aw_tms_scan = 0x40 | (tms_scan[0] & 0x1f) | (buf_get_u32(buffer, bit_count, 1) << 5);
+	AMT_AW(aw_tms_scan);
+	if (jtag_speed > 3 || rtck_enabled)
+		amt_wait_scan_busy();
+
+	if ((type == SCAN_IN) || (type == SCAN_IO))
+	{
+		AMT_DR(dr_tdo);
+		dr_tdo = dr_tdo >> 7;
+		buf_set_u32(buffer, bit_count, 1, dr_tdo);
+	}
+	
+	if (tms_scan[0] & 0x80)
+	{
+		aw_tms_scan = 0x40 | (tms_scan[1] & 0x1f);
+		AMT_AW(aw_tms_scan);
+		if (jtag_speed > 3 || rtck_enabled)
+			amt_wait_scan_busy();
+	}
+	cur_state = end_state;
+}
+
+int amt_jtagaccel_execute_queue(void)
+{
+	jtag_command_t *cmd = jtag_command_queue; /* currently processed command */
+	int scan_size;
+	enum scan_type type;
+	u8 *buffer;
+		
+	while (cmd)
+	{
+		switch (cmd->type)
+		{
+			case JTAG_END_STATE:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("end_state: %i", cmd->cmd.end_state->end_state);
+#endif
+				if (cmd->cmd.end_state->end_state != -1)
+					amt_jtagaccel_end_state(cmd->cmd.end_state->end_state);
+				break;
+			case JTAG_RESET:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("reset trst: %i srst %i", cmd->cmd.reset->trst, cmd->cmd.reset->srst);
+#endif
+				if (cmd->cmd.reset->trst == 1)
+				{
+					cur_state = TAP_TLR;
+				}
+				amt_jtagaccel_reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
+				break;
+			case JTAG_RUNTEST:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("runtest %i cycles, end in %i", cmd->cmd.runtest->num_cycles, cmd->cmd.runtest->end_state);
+#endif
+				if (cmd->cmd.runtest->end_state != -1)
+					amt_jtagaccel_end_state(cmd->cmd.runtest->end_state);
+				amt_jtagaccel_runtest(cmd->cmd.runtest->num_cycles);
+				break;
+			case JTAG_STATEMOVE:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("statemove end in %i", cmd->cmd.statemove->end_state);
+#endif
+				if (cmd->cmd.statemove->end_state != -1)
+					amt_jtagaccel_end_state(cmd->cmd.statemove->end_state);
+				amt_jtagaccel_state_move();
+				break;
+			case JTAG_SCAN:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("scan end in %i", cmd->cmd.scan->end_state);
+#endif
+				if (cmd->cmd.scan->end_state != -1)
+					amt_jtagaccel_end_state(cmd->cmd.scan->end_state);
+				scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
+				type = jtag_scan_type(cmd->cmd.scan);
+				amt_jtagaccel_scan(cmd->cmd.scan->ir_scan, type, buffer, scan_size);
+				if (jtag_read_buffer(buffer, cmd->cmd.scan) != ERROR_OK)
+					return ERROR_JTAG_QUEUE_FAILED;
+				if (buffer)
+					free(buffer);
+				break;
+			case JTAG_SLEEP:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("sleep", cmd->cmd.sleep->us);
+#endif
+				jtag_sleep(cmd->cmd.sleep->us);
+				break;
+			default:
+				ERROR("BUG: unknown JTAG command type encountered");
+				exit(-1);
+		}
+		cmd = cmd->next;
+	}
+	
+	return ERROR_OK;
+}
+
+int amt_jtagaccel_init(void)
+{
+#if PARPORT_USE_PPDEV == 1
+	char buffer[256];
+	int i = 0;
+	u8 control_port;
+#else
+	u8 status_port;
+#endif
+
+#if PARPORT_USE_PPDEV == 1
+	if (device_handle > 0)
+	{
+		ERROR("device is already opened");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	snprintf(buffer, 256, "/dev/parport%d", amt_jtagaccel_port);
+	device_handle = open(buffer, O_RDWR);
+	
+	if (device_handle < 0)
+	{
+		ERROR("cannot open device. check it exists and that user read and write rights are set");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	i = ioctl(device_handle, PPCLAIM);
+	if (i < 0)
+	{
+		ERROR("cannot claim device");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	i = IEEE1284_MODE_EPP;
+	i = ioctl(device_handle, PPSETMODE, & i);
+	if (i < 0)
+	{
+		ERROR(" cannot set compatible mode to device");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	
+	control_port = 0x00;
+	i = ioctl(device_handle, PPWCONTROL, &control_port);
+
+	control_port = 0x04;
+	i = ioctl(device_handle, PPWCONTROL, &control_port);
+
+#else
+	if (amt_jtagaccel_port == 0)
+	{
+		amt_jtagaccel_port = 0x378;
+		WARNING("No parport port specified, using default '0x378' (LPT1)");
+	}
+	
+	if (ioperm(amt_jtagaccel_port, 5, 1) != 0) {
+		ERROR("missing privileges for direct i/o");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	
+	/* prepare epp port */
+	/* clear timeout */
+	status_port = inb(amt_jtagaccel_port + 1);
+	outb(status_port | 0x1, amt_jtagaccel_port + 1);
+	
+	/* reset epp port */
+	outb(0x00, amt_jtagaccel_port + 2);
+	outb(0x04, amt_jtagaccel_port + 2);
+#endif
+	
+	/* enable JTAG port */
+	aw_control_fsm |= 0x04;
+	AMT_AW(aw_control_fsm);
+	
+	amt_jtagaccel_speed(jtag_speed);
+	
+	if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
+		aw_control_rst &= ~0x8;
+	else
+		aw_control_rst |= 0x8;
+	
+	if (jtag_reset_config & RESET_SRST_PUSH_PULL)
+		aw_control_rst &= ~0x2;
+	else
+		aw_control_rst |= 0x2;
+	
+	amt_jtagaccel_reset(0, 0);
+	
+	return ERROR_OK;
+}
+
+int amt_jtagaccel_quit(void)
+{
+	
+	return ERROR_OK;
+}
+
+int amt_jtagaccel_handle_parport_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 (amt_jtagaccel_port == 0)
+		amt_jtagaccel_port = strtoul(args[0], NULL, 0);
+
+	return ERROR_OK;
+}
+
+int amt_jtagaccel_handle_rtck_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	if (argc == 0)
+	{
+		command_print(cmd_ctx, "amt_jtagaccel RTCK feature %s", (rtck_enabled) ? "enabled" : "disabled");
+		return ERROR_OK;
+	}
+	else
+	{
+		if (strcmp(args[0], "enabled") == 0)
+		{
+			rtck_enabled = 1;
+			
+			/* set RTCK enable bit */
+			aw_control_fsm |= 0x02;
+			AMT_AW(aw_control_fsm);
+		}
+	}
+	
+	return ERROR_OK;
+}
diff --git a/src/jtag/bitbang.c b/src/jtag/bitbang.c
new file mode 100644
index 00000000..d6ff2898
--- /dev/null
+++ b/src/jtag/bitbang.c
@@ -0,0 +1,219 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+
+#include "bitbang.h"
+
+/* project specific includes */
+#include "log.h"
+#include "types.h"
+#include "jtag.h"
+#include "configuration.h"
+
+/* system includes */
+#include <string.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+#include <sys/time.h>
+#include <time.h>
+
+bitbang_interface_t *bitbang_interface;
+
+int bitbang_execute_queue(void);
+
+void bitbang_end_state(enum tap_state state)
+{
+	if (tap_move_map[state] != -1)
+		end_state = state;
+	else
+	{
+		ERROR("BUG: %i is not a valid end state", state);
+		exit(-1);
+	}
+}
+
+void bitbang_state_move(void) {
+	
+	int i=0, tms=0;
+	u8 tms_scan = TAP_MOVE(cur_state, end_state);
+	
+	for (i = 0; i < 7; i++)
+	{
+		tms = (tms_scan >> i) & 1;
+		bitbang_interface->write(0, tms, 0);
+		bitbang_interface->write(1, tms, 0);
+	}
+	bitbang_interface->write(0, tms, 0);
+	
+	cur_state = end_state;
+}
+
+void bitbang_runtest(int num_cycles)
+{
+	int i;
+	
+	enum tap_state saved_end_state = end_state;
+	
+	/* only do a state_move when we're not already in RTI */
+	if (cur_state != TAP_RTI)
+	{
+		bitbang_end_state(TAP_RTI);
+		bitbang_state_move();
+	}
+	
+	/* execute num_cycles */
+	bitbang_interface->write(0, 0, 0);
+	for (i = 0; i < num_cycles; i++)
+	{
+		bitbang_interface->write(1, 0, 0);
+		bitbang_interface->write(0, 0, 0);
+	}
+	
+	/* finish in end_state */
+	bitbang_end_state(saved_end_state);
+	if (cur_state != end_state)
+		bitbang_state_move();
+}
+
+void bitbang_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size)
+{
+	enum tap_state saved_end_state = end_state;
+	int bit_cnt;
+	
+	if (ir_scan)
+		bitbang_end_state(TAP_SI);
+	else
+		bitbang_end_state(TAP_SD);
+
+	bitbang_state_move();
+	bitbang_end_state(saved_end_state);
+
+	for (bit_cnt = 0; bit_cnt < scan_size; bit_cnt++)
+	{
+		if ((buffer[bit_cnt/8] >> (bit_cnt % 8)) & 0x1) {
+			bitbang_interface->write(0, (bit_cnt==scan_size-1) ? 1 : 0, 1);
+			bitbang_interface->write(1, (bit_cnt==scan_size-1) ? 1 : 0, 1);
+		} else {
+			bitbang_interface->write(0, (bit_cnt==scan_size-1) ? 1 : 0, 0);
+			bitbang_interface->write(1, (bit_cnt==scan_size-1) ? 1 : 0, 0);
+		}
+		
+		if (type != SCAN_OUT)
+		{
+			if (bitbang_interface->read())
+				buffer[(bit_cnt)/8] |= 1 << ((bit_cnt) % 8);
+			else
+				buffer[(bit_cnt)/8] &= ~(1 << ((bit_cnt) % 8));
+		}
+	}
+	
+	/* Exit1 -> Pause */
+	bitbang_interface->write(0, 0, 0);
+	bitbang_interface->write(1, 0, 0);
+	
+	if (ir_scan)
+		cur_state = TAP_PI;
+	else
+		cur_state = TAP_PD;
+	
+	if (cur_state != end_state)
+		bitbang_state_move();
+}
+
+int bitbang_execute_queue(void)
+{
+	jtag_command_t *cmd = jtag_command_queue; /* currently processed command */
+	int scan_size;
+	enum scan_type type;
+	u8 *buffer;
+	
+	if (!bitbang_interface)
+	{
+		ERROR("BUG: Bitbang interface called, but not yet initialized");
+		exit(-1);
+	}
+		
+	while (cmd)
+	{
+		switch (cmd->type)
+		{
+			case JTAG_END_STATE:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("end_state: %i", cmd->cmd.end_state->end_state);
+#endif
+				if (cmd->cmd.end_state->end_state != -1)
+					bitbang_end_state(cmd->cmd.end_state->end_state);
+				break;
+			case JTAG_RESET:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("reset trst: %i srst %i", cmd->cmd.reset->trst, cmd->cmd.reset->srst);
+#endif
+				if (cmd->cmd.reset->trst == 1)
+				{
+					cur_state = TAP_TLR;
+				}
+				bitbang_interface->reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
+				break;
+			case JTAG_RUNTEST:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("runtest %i cycles, end in %i", cmd->cmd.runtest->num_cycles, cmd->cmd.runtest->end_state);
+#endif
+				if (cmd->cmd.runtest->end_state != -1)
+					bitbang_end_state(cmd->cmd.runtest->end_state);
+				bitbang_runtest(cmd->cmd.runtest->num_cycles);
+				break;
+			case JTAG_STATEMOVE:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("statemove end in %i", cmd->cmd.statemove->end_state);
+#endif
+				if (cmd->cmd.statemove->end_state != -1)
+					bitbang_end_state(cmd->cmd.statemove->end_state);
+				bitbang_state_move();
+				break;
+			case JTAG_SCAN:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("scan end in %i", cmd->cmd.scan->end_state);
+#endif
+				if (cmd->cmd.scan->end_state != -1)
+					bitbang_end_state(cmd->cmd.scan->end_state);
+				scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
+				type = jtag_scan_type(cmd->cmd.scan);
+				bitbang_scan(cmd->cmd.scan->ir_scan, type, buffer, scan_size);
+				if (jtag_read_buffer(buffer, cmd->cmd.scan) != ERROR_OK)
+					return ERROR_JTAG_QUEUE_FAILED;
+				if (buffer)
+					free(buffer);
+				break;
+			case JTAG_SLEEP:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("sleep", cmd->cmd.sleep->us);
+#endif
+				jtag_sleep(cmd->cmd.sleep->us);
+				break;
+			default:
+				ERROR("BUG: unknown JTAG command type encountered");
+				exit(-1);
+		}
+		cmd = cmd->next;
+	}
+	
+	return ERROR_OK;
+}
+
diff --git a/src/jtag/bitbang.h b/src/jtag/bitbang.h
new file mode 100644
index 00000000..7049f435
--- /dev/null
+++ b/src/jtag/bitbang.h
@@ -0,0 +1,36 @@
+/***************************************************************************
+ *   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 BITBANG_H
+#define BITBANG_H
+
+typedef struct bitbang_interface_s
+{
+	/* low level callbacks (for bitbang)
+	 */
+	int (*read)(void);
+	void (*write)(int tck, int tms, int tdi);
+	void (*reset)(int trst, int srst);
+} bitbang_interface_t;
+
+extern bitbang_interface_t *bitbang_interface;
+
+extern int bitbang_execute_queue(void);
+
+#endif /* BITBANG_H */
diff --git a/src/jtag/ep93xx.c b/src/jtag/ep93xx.c
new file mode 100644
index 00000000..9c24dba4
--- /dev/null
+++ b/src/jtag/ep93xx.c
@@ -0,0 +1,236 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "config.h"
+#include "log.h"
+#include "jtag.h"
+#include "bitbang.h"
+
+#define TDO_BIT		1
+#define TDI_BIT		2
+#define TCK_BIT		4
+#define TMS_BIT		8
+#define TRST_BIT	16
+#define SRST_BIT	32
+#define VCC_BIT		64
+
+/* system includes */
+#include <sys/io.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <sys/mman.h>
+#include <unistd.h>
+#include <fcntl.h>
+
+static u8 output_value = 0x0;
+static int dev_mem_fd;
+static void *gpio_controller;
+static volatile u8 *gpio_data_register;
+static volatile u8 *gpio_data_direction_register;
+
+/* low level command set
+ */
+int ep93xx_read(void);
+void ep93xx_write(int tck, int tms, int tdi);
+void ep93xx_reset(int trst, int srst);
+
+int ep93xx_speed(int speed);
+int ep93xx_register_commands(struct command_context_s *cmd_ctx);
+int ep93xx_init(void);
+int ep93xx_quit(void);
+
+struct timespec ep93xx_zzzz;
+
+jtag_interface_t ep93xx_interface = 
+{
+	.name = "ep93xx",
+	
+	.execute_queue = bitbang_execute_queue,
+
+	.support_statemove = 0,
+
+	.speed = ep93xx_speed,	
+	.register_commands = ep93xx_register_commands,
+	.init = ep93xx_init,
+	.quit = ep93xx_quit,
+};
+
+bitbang_interface_t ep93xx_bitbang =
+{
+	.read = ep93xx_read,
+	.write = ep93xx_write,
+	.reset = ep93xx_reset
+};
+
+int ep93xx_read(void)
+{
+	return !!(*gpio_data_register & TDO_BIT);
+}
+
+void ep93xx_write(int tck, int tms, int tdi)
+{
+	if (tck)
+		output_value |= TCK_BIT;
+	else
+		output_value &= TCK_BIT;
+	
+	if (tms)
+		output_value |= TMS_BIT;
+	else
+		output_value &= TMS_BIT;
+	
+	if (tdi)
+		output_value |= TDI_BIT;
+	else
+		output_value &= TDI_BIT;
+
+	*gpio_data_register = output_value;
+	nanosleep(ep93xx_zzzz);
+}
+
+/* (1) assert or (0) deassert reset lines */
+void ep93xx_reset(int trst, int srst)
+{
+	if (trst == 0)
+		output_value |= TRST_BIT;
+	else if (trst == 1)
+		output_value &= TRST_BIT;
+
+	if (srst == 0)
+		output_value |= SRST_BIT;
+	else if (srst == 1)
+		output_value &= SRST_BIT;
+	
+	*gpio_data_register = output_value;
+	nanosleep(ep93xx_zzzz);
+}
+
+int ep93xx_speed(int speed)
+{
+	
+	return ERROR_OK;
+}
+
+int ep93xx_register_commands(struct command_context_s *cmd_ctx)
+{
+
+	return ERROR_OK;
+}
+
+static int set_gonk_mode(void)
+{
+	void *syscon;
+	u32 devicecfg;
+
+	syscon = mmap(NULL, 4096, PROT_READ | PROT_WRITE,
+			MAP_SHARED, dev_mem_fd, 0x80930000);
+	if (syscon == MAP_FAILED) {
+		perror("mmap");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	devicecfg = *((volatile int *)(syscon + 0x80));
+	*((volatile int *)(syscon + 0xc0)) = 0xaa;
+	*((volatile int *)(syscon + 0x80)) = devicecfg | 0x08000000;
+
+	munmap(syscon, 4096);
+
+	return ERROR_OK;
+}
+
+int ep93xx_init(void)
+{
+	int ret;
+
+	bitbang_interface = &ep93xx_bitbang;	
+
+	ep93xx_zzzz.tv_sec = 0;
+	ep93xx_zzzz.tv_nsec = 10000000;
+
+	dev_mem_fd = open("/dev/mem", O_RDWR | O_SYNC);
+	if (dev_mem_fd < 0) {
+		perror("open");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	gpio_controller = mmap(NULL, 4096, PROT_READ | PROT_WRITE,
+				MAP_SHARED, dev_mem_fd, 0x80840000);
+	if (gpio_controller == MAP_FAILED) {
+		perror("mmap");
+		close(dev_mem_fd);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	ret = set_gonk_mode();
+	if (ret != ERROR_OK) {
+		munmap(gpio_controller, 4096);
+		close(dev_mem_fd);
+		return ret;
+	}
+
+#if 0
+	/* Use GPIO port A.  */
+	gpio_data_register = gpio_controller + 0x00;
+	gpio_data_direction_register = gpio_controller + 0x10;
+
+
+	/* Use GPIO port B.  */
+	gpio_data_register = gpio_controller + 0x04;
+	gpio_data_direction_register = gpio_controller + 0x14;
+
+	/* Use GPIO port C.  */
+	gpio_data_register = gpio_controller + 0x08;
+	gpio_data_direction_register = gpio_controller + 0x18;
+
+	/* Use GPIO port D.  */
+	gpio_data_register = gpio_controller + 0x0c;
+	gpio_data_direction_register = gpio_controller + 0x1c;
+#endif
+
+	/* Use GPIO port C.  */
+	gpio_data_register = gpio_controller + 0x08;
+	gpio_data_direction_register = gpio_controller + 0x18;
+
+	printf("gpio_data_register      = %08x\n", gpio_data_register);
+        printf("gpio_data_direction_reg = %08x\n", gpio_data_direction_register); 
+	/*
+	 * Configure bit 0 (TDO) as an input, and bits 1-5 (TDI, TCK
+	 * TMS, TRST, SRST) as outputs.  Drive TDI and TCK low, and
+	 * TMS/TRST/SRST high.
+	 */
+	output_value = TMS_BIT | TRST_BIT | SRST_BIT | VCC_BIT;
+	*gpio_data_register = output_value;
+	nanosleep(ep93xx_zzzz);
+
+	/*
+	 * Configure the direction register.  1 = output, 0 = input.
+	 */
+	*gpio_data_direction_register =
+		TDI_BIT | TCK_BIT | TMS_BIT | TRST_BIT | SRST_BIT | VCC_BIT;
+
+	nanosleep(ep93xx_zzzz);
+	return ERROR_OK;
+}
+
+int ep93xx_quit(void)
+{
+
+	return ERROR_OK;
+}
diff --git a/src/jtag/ftd2xx.c b/src/jtag/ftd2xx.c
new file mode 100644
index 00000000..c73c8d58
--- /dev/null
+++ b/src/jtag/ftd2xx.c
@@ -0,0 +1,1004 @@
+/***************************************************************************
+ *   Copyright (C) 2004 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.             *
+ ***************************************************************************/
+#include "config.h"
+#if IS_CYGWIN == 1
+#include "windows.h"
+#undef ERROR
+#endif
+
+/* project specific includes */
+#include "log.h"
+#include "types.h"
+#include "jtag.h"
+#include "configuration.h"
+
+/* system includes */
+#include <string.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <ftd2xx.h>
+
+#include <sys/time.h>
+#include <time.h>
+
+/* enable this to debug io latency
+ */
+#if 0
+#define _DEBUG_USB_IO_
+#endif
+
+/* enable this to debug communication
+ */
+#if 0
+#define _DEBUG_USB_COMMS_
+#endif
+
+/* enable this to work around ftd2xx deadlock
+ */
+#if 0
+#define _FTD2XX_QUEUE_DELAY_
+#endif
+
+int ftd2xx_execute_queue(void);
+
+int ftd2xx_speed(int speed);
+int ftd2xx_register_commands(struct command_context_s *cmd_ctx);
+int ftd2xx_init(void);
+int ftd2xx_quit(void);
+
+int ftd2xx_handle_device_desc_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int ftd2xx_handle_layout_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int ftd2xx_handle_vid_pid_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+char *ftd2xx_device_desc = NULL;
+char *ftd2xx_layout = NULL;
+u16 ftd2xx_vid = 0x0403;
+u16 ftd2xx_pid = 0x6010;
+
+typedef struct ftd2xx_layout_s
+{
+	char* name;
+	int(*init)(void);
+	void(*reset)(int trst, int srst);
+} ftd2xx_layout_t;
+
+int usbjtag_init(void);
+int jtagkey_init(void);
+void usbjtag_reset(int trst, int srst);
+void jtagkey_reset(int trst, int srst);
+
+ftd2xx_layout_t ftd2xx_layouts[] =
+{
+	{"usbjtag", usbjtag_init, usbjtag_reset},
+	{"jtagkey", jtagkey_init, jtagkey_reset},
+	{"jtagkey_prototype_v1", jtagkey_init, jtagkey_reset},
+	{NULL, NULL, NULL},
+};
+
+static u8 nTRST, nTRSTnOE, nSRST, nSRSTnOE;
+
+static ftd2xx_layout_t *layout;
+static u8 low_output = 0x0;
+static u8 low_direction = 0x0;
+static u8 high_output = 0x0;
+static u8 high_direction = 0x0;
+static FT_HANDLE ftdih = NULL;
+
+static u8 *ftd2xx_buffer = NULL;
+static int ftd2xx_buffer_size = 0;
+static int ftd2xx_read_pointer = 0;
+static int ftd2xx_expect_read = 0;
+#define FTD2XX_BUFFER_SIZE	131072
+#define BUFFER_ADD ftd2xx_buffer[ftd2xx_buffer_size++]
+#define BUFFER_READ ftd2xx_buffer[ftd2xx_read_pointer++]
+
+jtag_interface_t ftd2xx_interface = 
+{
+	
+	.name = "ftd2xx",
+	
+	.execute_queue = ftd2xx_execute_queue,
+	
+	.support_statemove = 1,
+	
+	.speed = ftd2xx_speed,
+	.register_commands = ftd2xx_register_commands,
+	.init = ftd2xx_init,
+	.quit = ftd2xx_quit,
+};
+
+int ftd2xx_speed(int speed)
+{
+	u8 buf[3];
+	FT_STATUS status;
+	DWORD bytes_written;
+
+	buf[0] = 0x86; /* command "set divisor" */
+	buf[1] = speed & 0xff; /* valueL (0=6MHz, 1=3MHz, 2=1.5MHz, ...*/
+	buf[2] = (speed >> 8) & 0xff; /* valueH */
+	
+	DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
+	if (((status = FT_Write(ftdih, buf, 3, &bytes_written)) != FT_OK) || (bytes_written != 3))
+	{
+		ERROR("couldn't write to ftdi device: %i", status);
+		return status;
+	}
+	
+	return ERROR_OK;
+}
+
+int ftd2xx_register_commands(struct command_context_s *cmd_ctx)
+{
+	register_command(cmd_ctx, NULL, "ftd2xx_device_desc", ftd2xx_handle_device_desc_command,
+		COMMAND_CONFIG, NULL);
+	register_command(cmd_ctx, NULL, "ftd2xx_layout", ftd2xx_handle_layout_command,
+		COMMAND_CONFIG, NULL);
+	register_command(cmd_ctx, NULL, "ftd2xx_vid_pid", ftd2xx_handle_vid_pid_command,
+					 COMMAND_CONFIG, NULL);
+	return ERROR_OK;
+}
+
+void ftd2xx_end_state(state)
+{
+	if (tap_move_map[state] != -1)
+		end_state = state;
+	else
+	{
+		ERROR("BUG: %i is not a valid end state", state);
+		exit(-1);
+	}
+}
+
+void ftd2xx_read_scan(enum scan_type type, u8* buffer, int scan_size)
+{
+	int num_bytes = ((scan_size + 7) / 8);
+	int bits_left = scan_size;
+	int cur_byte = 0;
+
+	while(num_bytes-- > 1)
+	{
+		buffer[cur_byte] = BUFFER_READ;
+		cur_byte++;
+		bits_left -= 8;
+	}
+
+	buffer[cur_byte] = 0x0;
+
+	if (bits_left > 1)
+	{
+		buffer[cur_byte] = BUFFER_READ >> 1;
+	}
+
+	buffer[cur_byte] = (buffer[cur_byte] | ((BUFFER_READ & 0x02) << 6)) >> (8 - bits_left);
+
+}
+
+void ftd2xx_debug_dump_buffer(void)
+{
+	int i;
+	char line[256];
+	char *line_p = line;
+	
+	for (i = 0; i < ftd2xx_buffer_size; i++)
+	{
+		line_p += snprintf(line_p, 256 - (line_p - line), "%2.2x ", ftd2xx_buffer[i]);
+		if (i % 16 == 15)
+		{
+			DEBUG("%s", line);
+			line_p = line;
+		}
+	}
+	
+	if (line_p != line)
+		DEBUG("%s", line);
+}
+
+int ftd2xx_send_and_recv(jtag_command_t *first, jtag_command_t *last)
+{
+	jtag_command_t *cmd;
+	u8 *buffer;
+	int scan_size;
+	enum scan_type type;
+	FT_STATUS status;
+	DWORD bytes_written;
+	DWORD bytes_read;
+	
+#ifdef _DEBUG_USB_IO_
+	struct timeval start, inter, inter2, end;
+#endif
+
+#ifdef _DEBUG_USB_COMMS_
+	DEBUG("write buffer (size %i):", ftd2xx_buffer_size);
+	ftd2xx_debug_dump_buffer();
+#endif
+
+#ifdef _DEBUG_USB_IO_
+	gettimeofday(&start, NULL);	
+#endif
+
+	if ((status = FT_Write(ftdih, ftd2xx_buffer, ftd2xx_buffer_size, &bytes_written)) != FT_OK)
+	{
+		ERROR("couldn't write to ftdi device: %i", status);
+		exit(-1);
+	}
+	
+#ifdef _DEBUG_USB_IO_
+	gettimeofday(&inter, NULL);	
+#endif
+	
+	if (ftd2xx_expect_read)
+	{
+		int timeout = 100;
+		ftd2xx_buffer_size = 0;
+		
+#ifdef _FTD2XX_QUEUE_DELAY_
+		DWORD inrxqueue = 0;
+		while (inrxqueue < ftd2xx_expect_read)
+		{
+			FT_GetQueueStatus(ftdih, &inrxqueue);
+			if (inrxqueue >= ftd2xx_expect_read)
+				break;
+			usleep(1000);
+		};
+#endif
+		
+#ifdef _DEBUG_USB_IO_
+	gettimeofday(&inter2, NULL);	
+#endif
+			
+		if ((status = FT_Read(ftdih, ftd2xx_buffer, ftd2xx_expect_read, &bytes_read)) != FT_OK)
+		{
+			ERROR("couldn't read from ftdi device: %i", status);
+			exit(-1);
+		}
+
+#ifdef _DEBUG_USB_IO_
+		gettimeofday(&end, NULL);	
+
+		INFO("inter: %i.%i, inter2: %i.%i end: %i.%i", inter.tv_sec - start.tv_sec, inter.tv_usec - start.tv_usec,
+			inter2.tv_sec - start.tv_sec, inter2.tv_usec - start.tv_usec,
+			end.tv_sec - start.tv_sec, end.tv_usec - start.tv_usec);
+#endif
+	
+		
+		ftd2xx_buffer_size = bytes_read;
+		
+		if (ftd2xx_expect_read != ftd2xx_buffer_size)
+		{
+			ERROR("ftd2xx_expect_read (%i) != ftd2xx_buffer_size (%i) (%i retries)", ftd2xx_expect_read, ftd2xx_buffer_size, 100 - timeout);
+			ftd2xx_debug_dump_buffer();	
+
+			exit(-1);
+		}
+
+#ifdef _DEBUG_USB_COMMS_
+		DEBUG("read buffer (%i retries): %i bytes", 100 - timeout, ftd2xx_buffer_size);
+		ftd2xx_debug_dump_buffer();
+#endif
+	}
+
+	ftd2xx_expect_read = 0;
+	ftd2xx_read_pointer = 0;
+
+	cmd = first;
+	while (cmd != last)
+	{
+		switch (cmd->type)
+		{
+			case JTAG_SCAN:
+				type = jtag_scan_type(cmd->cmd.scan);
+				if (type != SCAN_OUT)
+				{
+					scan_size = jtag_scan_size(cmd->cmd.scan);
+					buffer = calloc(CEIL(scan_size, 8), 1);
+					ftd2xx_read_scan(type, buffer, scan_size);
+					jtag_read_buffer(buffer, cmd->cmd.scan);
+					free(buffer);
+				}
+				break;
+			default:
+				break;
+		}
+		cmd = cmd->next;
+	}
+	
+	ftd2xx_buffer_size = 0;
+
+	return ERROR_OK;
+}
+
+void ftd2xx_add_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size)
+{
+	int num_bytes = (scan_size + 7) / 8;
+	int bits_left = scan_size;
+	int cur_byte = 0;
+	int last_bit;
+
+	/* command "Clock Data to TMS/CS Pin (no Read)" */
+	BUFFER_ADD = 0x4b;
+	/* scan 7 bit */
+	BUFFER_ADD = 0x6;
+	/* TMS data bits */
+	if (ir_scan)
+	{
+		BUFFER_ADD = TAP_MOVE(cur_state, TAP_SI);
+		cur_state = TAP_SI;
+	}
+	else
+	{
+		BUFFER_ADD = TAP_MOVE(cur_state, TAP_SD);
+		cur_state = TAP_SD;
+	}
+	//DEBUG("added TMS scan (no read)");
+
+	/* add command for complete bytes */
+	if (num_bytes > 1)
+	{
+		if (type == SCAN_IO)
+		{
+			/* Clock Data Bytes In and Out LSB First */
+			BUFFER_ADD = 0x39;
+			//DEBUG("added TDI bytes (io %i)", num_bytes);
+		}
+		else if (type == SCAN_OUT)
+		{
+			/* Clock Data Bytes Out on -ve Clock Edge LSB First (no Read) */
+			BUFFER_ADD = 0x19;
+			//DEBUG("added TDI bytes (o)");
+		}
+		else if (type == SCAN_IN)
+		{
+			/* Clock Data Bytes In on +ve Clock Edge LSB First (no Write) */
+			BUFFER_ADD = 0x28;
+			//DEBUG("added TDI bytes (i %i)", num_bytes);
+		}
+		BUFFER_ADD = (num_bytes-2) & 0xff;
+		BUFFER_ADD = (num_bytes >> 8) & 0xff;
+	}
+	if (type != SCAN_IN)
+	{
+		/* add complete bytes */
+		while(num_bytes-- > 1)
+		{
+			BUFFER_ADD = buffer[cur_byte];
+			cur_byte++;
+			bits_left -= 8;
+		}
+	}
+	if (type == SCAN_IN)
+	{
+		bits_left -= 8 * (num_bytes - 1);
+	}
+
+	/* the most signifcant bit is scanned during TAP movement */
+	if (type != SCAN_IN)
+		last_bit = (buffer[cur_byte] >> (bits_left - 1)) & 0x1;
+	else
+		last_bit = 0;
+
+	/* process remaining bits but the last one */
+	if (bits_left > 1)
+	{
+		if (type == SCAN_IO)
+		{
+			/* Clock Data Bits In and Out LSB First */
+			BUFFER_ADD = 0x3b;
+			//DEBUG("added TDI bits (io) %i", bits_left - 1);
+		}
+		else if (type == SCAN_OUT)
+		{
+			/* Clock Data Bits Out on -ve Clock Edge LSB First (no Read) */
+			BUFFER_ADD = 0x1b;
+			//DEBUG("added TDI bits (o)");
+		}
+		else if (type == SCAN_IN)
+		{
+			/* Clock Data Bits In on +ve Clock Edge LSB First (no Write) */
+			BUFFER_ADD = 0x2a;
+			//DEBUG("added TDI bits (i %i)", bits_left - 1);
+		}
+		BUFFER_ADD = bits_left - 2;
+		if (type != SCAN_IN)
+			BUFFER_ADD = buffer[cur_byte];
+	}
+
+	/* move from Shift-IR/DR to end state */
+	if (type != SCAN_OUT)
+	{
+		/* Clock Data to TMS/CS Pin with Read */
+		BUFFER_ADD = 0x6b;
+		//DEBUG("added TMS scan (read)");
+	}
+	else
+	{
+		/* Clock Data to TMS/CS Pin (no Read) */
+		BUFFER_ADD = 0x4b;
+		//DEBUG("added TMS scan (no read)");
+	}
+	BUFFER_ADD = 0x6;
+	BUFFER_ADD = TAP_MOVE(cur_state, end_state) | (last_bit << 7);
+	cur_state = end_state;
+
+}
+
+int ftd2xx_predict_scan_out(int scan_size, enum scan_type type)
+{
+	int predicted_size = 6;
+	if (type == SCAN_IN)	/* only from device to host */
+	{
+		predicted_size += (CEIL(scan_size, 8) > 1) ? 3 : 0;
+		predicted_size += ((scan_size - 1) % 8) ? 2 : 0;
+	}
+	else					/* host to device, or bidirectional */
+	{
+		predicted_size += (CEIL(scan_size, 8) > 1) ? (CEIL(scan_size, 8) + 3 - 1) : 0;
+		predicted_size += ((scan_size - 1) % 8) ? 3 : 0;
+	}
+
+	return predicted_size;
+}
+
+int ftd2xx_predict_scan_in(int scan_size, enum scan_type type)
+{
+	int predicted_size = 0;
+	
+	if (type != SCAN_OUT)
+	{
+		/* complete bytes */
+		predicted_size += (CEIL(scan_size, 8) > 1) ? (CEIL(scan_size, 8) - 1) : 0;
+		/* remaining bits - 1 */
+		predicted_size += ((scan_size - 1) % 8) ? 1 : 0;
+		/* last bit (from TMS scan) */
+		predicted_size += 1;
+	}
+	
+	//DEBUG("scan_size: %i, predicted_size: %i", scan_size, predicted_size);
+
+	return predicted_size;
+}
+
+void usbjtag_reset(int trst, int srst)
+{
+	if (trst == 1)
+	{
+		cur_state = TAP_TLR;
+		if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
+			low_direction |= nTRSTnOE;	/* switch to output pin (output is low) */
+		else
+			low_output &= ~nTRST;	/* switch output low */
+	}
+	else if (trst == 0)
+	{
+		if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
+			low_direction &= ~nTRSTnOE; /* switch to input pin (high-Z + internal and external pullup) */
+		else
+			low_output |= nTRST; /* switch output high */
+	}
+
+	if (srst == 1)
+	{
+		if (jtag_reset_config & RESET_SRST_PUSH_PULL)
+			low_output &= ~nSRST;	/* switch output low */
+		else
+			low_direction |= nSRSTnOE;	/* switch to output pin (output is low) */
+	}
+	else if (srst == 0)
+	{
+		if (jtag_reset_config & RESET_SRST_PUSH_PULL)
+			low_output |= nSRST;	/* switch output high */
+		else
+			low_direction &= ~nSRSTnOE;	/* switch to input pin (high-Z) */
+	}
+	
+	/* command "set data bits low byte" */
+	BUFFER_ADD = 0x80;
+	BUFFER_ADD = low_output;
+	BUFFER_ADD = low_direction;
+
+}
+
+void jtagkey_reset(int trst, int srst)
+{
+	if (trst == 1)
+	{
+		cur_state = TAP_TLR;
+		if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
+			high_output &= ~nTRSTnOE;
+		else
+			high_output &= ~nTRST;
+	}
+	else if (trst == 0)
+	{
+		if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
+			high_output |= nTRSTnOE;
+		else
+			high_output |= nTRST;
+	}
+
+	if (srst == 1)
+	{
+		if (jtag_reset_config & RESET_SRST_PUSH_PULL)
+			high_output &= ~nSRST;
+		else
+			high_output &= ~nSRSTnOE;
+	}
+	else if (srst == 0)
+	{
+		if (jtag_reset_config & RESET_SRST_PUSH_PULL)
+			high_output |= nSRST;
+		else
+			high_output |= nSRSTnOE;
+	}
+	
+	/* command "set data bits high byte" */
+	BUFFER_ADD = 0x82;
+	BUFFER_ADD = high_output;
+	BUFFER_ADD = high_direction;
+	DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, high_direction: 0x%2.2x", trst, srst, high_output, high_direction);
+}
+
+int ftd2xx_execute_queue()
+{
+	jtag_command_t *cmd = jtag_command_queue; /* currently processed command */
+	jtag_command_t *first_unsent = cmd;	/* next command that has to be sent */
+	u8 *buffer;
+	int scan_size;	/* size of IR or DR scan */
+	enum scan_type type;
+	int i;
+	int predicted_size = 0;
+	int require_send = 0;
+
+	ftd2xx_buffer_size = 0;
+	ftd2xx_expect_read = 0;
+
+	while (cmd)
+	{
+		switch(cmd->type)
+		{
+			case JTAG_END_STATE:
+				if (cmd->cmd.end_state->end_state != -1)
+					ftd2xx_end_state(cmd->cmd.end_state->end_state);
+				break;
+			case JTAG_RESET:
+				/* only send the maximum buffer size that FT2232C can handle */
+				predicted_size = 3;
+				if (ftd2xx_buffer_size + predicted_size + 1 > FTD2XX_BUFFER_SIZE)
+				{
+					ftd2xx_send_and_recv(first_unsent, cmd);
+					require_send = 0;
+					first_unsent = cmd;
+				}
+
+				layout->reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
+				require_send = 1;
+
+				break;
+			case JTAG_RUNTEST:
+				/* only send the maximum buffer size that FT2232C can handle */
+				predicted_size = 0;
+				if (cur_state != TAP_RTI)
+					predicted_size += 3;
+				predicted_size += 3 * CEIL(cmd->cmd.runtest->num_cycles, 7);
+				if ((cmd->cmd.runtest->end_state != -1) && (cmd->cmd.runtest->end_state != TAP_RTI))
+					predicted_size += 3;
+				if ((cmd->cmd.runtest->end_state == -1) && (end_state != TAP_RTI))
+					predicted_size += 3;
+				if (ftd2xx_buffer_size + predicted_size + 1 > FTD2XX_BUFFER_SIZE)
+				{
+					ftd2xx_send_and_recv(first_unsent, cmd);
+					require_send = 0;
+					first_unsent = cmd;
+				}
+				if (cur_state != TAP_RTI)
+				{
+					/* command "Clock Data to TMS/CS Pin (no Read)" */
+					BUFFER_ADD = 0x4b;
+					/* scan 7 bit */
+					BUFFER_ADD = 0x6;
+					/* TMS data bits */
+					BUFFER_ADD = TAP_MOVE(cur_state, TAP_RTI);
+					cur_state = TAP_RTI;
+					require_send = 1;
+				}
+				i = cmd->cmd.runtest->num_cycles;
+				while (i > 0)
+				{
+					/* command "Clock Data to TMS/CS Pin (no Read)" */
+					BUFFER_ADD = 0x4b;
+					/* scan 7 bit */
+					BUFFER_ADD = (i > 7) ? 6 : (i - 1);
+					/* TMS data bits */
+					BUFFER_ADD = 0x0;
+					cur_state = TAP_RTI;
+					i -= (i > 7) ? 7 : i;
+					//DEBUG("added TMS scan (no read)");
+				}
+				if (cmd->cmd.runtest->end_state != -1)
+					ftd2xx_end_state(cmd->cmd.runtest->end_state);
+				if (cur_state != end_state)
+				{
+					/* command "Clock Data to TMS/CS Pin (no Read)" */
+					BUFFER_ADD = 0x4b;
+					/* scan 7 bit */
+					BUFFER_ADD = 0x6;
+					/* TMS data bits */
+					BUFFER_ADD = TAP_MOVE(cur_state, end_state);
+					cur_state = end_state;
+					//DEBUG("added TMS scan (no read)");
+				}
+				require_send = 1;
+				break;
+			case JTAG_STATEMOVE:
+				/* only send the maximum buffer size that FT2232C can handle */
+				predicted_size = 3;
+				if (ftd2xx_buffer_size + predicted_size + 1 > FTD2XX_BUFFER_SIZE)
+				{
+					ftd2xx_send_and_recv(first_unsent, cmd);
+					require_send = 0;
+					first_unsent = cmd;
+				}
+				if (cmd->cmd.statemove->end_state != -1)
+					ftd2xx_end_state(cmd->cmd.statemove->end_state);
+				/* command "Clock Data to TMS/CS Pin (no Read)" */
+				BUFFER_ADD = 0x4b;
+				/* scan 7 bit */
+				BUFFER_ADD = 0x6;
+				/* TMS data bits */
+				BUFFER_ADD = TAP_MOVE(cur_state, end_state);
+				//DEBUG("added TMS scan (no read)");
+				cur_state = end_state;
+				require_send = 1;
+				break;
+			case JTAG_SCAN:
+				scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
+				type = jtag_scan_type(cmd->cmd.scan);
+				predicted_size = ftd2xx_predict_scan_out(scan_size, type);
+				if (ftd2xx_buffer_size + predicted_size + 1 > FTD2XX_BUFFER_SIZE)
+				{
+					DEBUG("ftd2xx buffer size reached, sending queued commands (first_unsent: %x, cmd: %x)", first_unsent, cmd);
+					ftd2xx_send_and_recv(first_unsent, cmd);
+					require_send = 0;
+					first_unsent = cmd;
+				}
+				ftd2xx_expect_read += ftd2xx_predict_scan_in(scan_size, type);
+				//DEBUG("new read size: %i", ftd2xx_expect_read);
+				if (cmd->cmd.scan->end_state != -1)
+					ftd2xx_end_state(cmd->cmd.scan->end_state);
+				ftd2xx_add_scan(cmd->cmd.scan->ir_scan, type, buffer, scan_size);
+				require_send = 1;
+				if (buffer)
+					free(buffer);
+				break;
+			case JTAG_SLEEP:
+				ftd2xx_send_and_recv(first_unsent, cmd);
+				first_unsent = cmd->next;
+				jtag_sleep(cmd->cmd.sleep->us);
+				break;
+			default:
+				ERROR("BUG: unknown JTAG command type encountered");
+				exit(-1);
+		}
+		cmd = cmd->next;
+	}
+
+	if (require_send > 0)
+		ftd2xx_send_and_recv(first_unsent, cmd);
+
+	return ERROR_OK;
+}
+
+int ftd2xx_init(void)
+{
+	u8 latency_timer;
+	FT_STATUS status;
+	DWORD num_devices;
+	
+	ftd2xx_layout_t *cur_layout = ftd2xx_layouts;
+	
+	if ((ftd2xx_layout == NULL) || (ftd2xx_layout[0] == 0))
+	{
+		ftd2xx_layout = "usbjtag";
+		WARNING("No ftd2xx layout specified, using default 'usbjtag'");
+	}
+	
+	while (cur_layout->name)
+	{
+		if (strcmp(cur_layout->name, ftd2xx_layout) == 0)
+		{
+			layout = cur_layout;
+			break;
+		}
+		cur_layout++;
+	}
+
+	if (!layout)
+	{
+		ERROR("No matching layout found for %s", ftd2xx_layout);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	
+	if (ftd2xx_device_desc == NULL)
+	{
+		WARNING("no ftd2xx device description specified, using default 'Dual RS232'");
+		ftd2xx_device_desc = "Dual RS232";
+	}
+	
+#if IS_CYGWIN != 1
+	/* Add JTAGkey Vid/Pid to the linux driver */
+	if ((status = FT_SetVIDPID(ftd2xx_vid, ftd2xx_pid)) != FT_OK)
+	{
+		WARNING("couldn't add %4.4x:%4.4x", ftd2xx_vid, ftd2xx_pid);
+	}
+#endif
+
+	if ((status = FT_OpenEx(ftd2xx_device_desc, FT_OPEN_BY_DESCRIPTION, &ftdih)) != FT_OK)
+	{
+		ERROR("unable to open ftdi device: %i", status);
+		status = FT_ListDevices(&num_devices, NULL, FT_LIST_NUMBER_ONLY);
+		if (status == FT_OK)
+		{
+			char **desc_array = malloc(sizeof(char*) * (num_devices + 1));
+			int i;
+
+			for (i = 0; i < num_devices; i++)
+				desc_array[i] = malloc(64);
+			desc_array[num_devices] = NULL;
+
+			status = FT_ListDevices(desc_array, &num_devices, FT_LIST_ALL | FT_OPEN_BY_DESCRIPTION);
+
+			if (status == FT_OK)
+			{
+				ERROR("ListDevices: %d\n", num_devices);
+				for (i = 0; i < num_devices; i++)
+					ERROR("%i: %s", i, desc_array[i]);
+			}
+			
+			for (i = 0; i < num_devices; i++)
+				free(desc_array[i]);
+			free(desc_array);
+		}
+		else
+		{
+			printf("ListDevices: NONE\n");
+		}
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	if ((status = FT_SetLatencyTimer(ftdih, 2)) != FT_OK)
+	{
+		ERROR("unable to set latency timer: %i", status);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	
+	if ((status = FT_GetLatencyTimer(ftdih, &latency_timer)) != FT_OK)
+	{
+		ERROR("unable to get latency timer: %i", status);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	else
+	{
+		DEBUG("current latency timer: %i", latency_timer);
+	}
+
+	if ((status = FT_SetBitMode(ftdih, 0x0b, 2)) != FT_OK)
+	{
+		ERROR("unable to enable bit i/o mode: %i", status);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	ftd2xx_buffer_size = 0;
+	ftd2xx_buffer = malloc(FTD2XX_BUFFER_SIZE);
+
+	if (layout->init() != ERROR_OK)
+		return ERROR_JTAG_INIT_FAILED;
+
+	ftd2xx_speed(jtag_speed);
+	
+	if ((status = FT_Purge(ftdih, FT_PURGE_RX | FT_PURGE_TX)) != FT_OK)
+	{
+		ERROR("error purging ftd2xx device: %i", status);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	return ERROR_OK;
+}
+
+int usbjtag_init(void)
+{
+	u8 buf[3];
+	FT_STATUS status;
+	DWORD bytes_written;
+	
+	low_output = 0x08;
+	low_direction = 0x0b;
+	
+	nTRST = 0x10;
+	nTRSTnOE = 0x10;
+	nSRST = 0x40;
+	nSRSTnOE = 0x40;
+	
+	if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
+	{
+		low_direction &= ~nTRSTnOE; /* nTRST input */
+		low_output &= ~nTRST; /* nTRST = 0 */
+	}
+	else
+	{
+		low_direction |= nTRSTnOE; /* nTRST output */
+		low_output |= nTRST; /* nTRST = 1 */
+	}
+	
+	if (jtag_reset_config & RESET_SRST_PUSH_PULL)
+	{
+		low_direction |= nSRSTnOE; /* nSRST output */
+		low_output |= nSRST; /* nSRST = 1 */
+	}
+	else
+	{
+		low_direction &= ~nSRSTnOE; /* nSRST input */
+		low_output &= ~nSRST; /* nSRST = 0 */
+	}
+	
+	/* initialize low byte for jtag */
+	buf[0] = 0x80; /* command "set data bits low byte" */
+	buf[1] = low_output; /* value (TMS=1,TCK=0, TDI=0, xRST high) */
+	buf[2] = low_direction; /* dir (output=1), TCK/TDI/TMS=out, TDO=in */
+	DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
+	
+	if (((FT_Write(ftdih, buf, 3, &bytes_written)) != FT_OK) || (bytes_written != 3))
+	{
+		ERROR("couldn't write to ftdi device: %i", status);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	return ERROR_OK;
+}
+
+int jtagkey_init(void)
+{
+	u8 buf[3];
+	FT_STATUS status;
+	DWORD bytes_written;
+	
+	low_output = 0x08;
+	low_direction = 0x1b;
+	
+	/* initialize low byte for jtag */
+	buf[0] = 0x80; /* command "set data bits low byte" */
+	buf[1] = low_output; /* value (TMS=1,TCK=0, TDI=0, nOE=0) */
+	buf[2] = low_direction; /* dir (output=1), TCK/TDI/TMS=out, TDO=in, nOE=out */
+	DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
+	
+	if (((FT_Write(ftdih, buf, 3, &bytes_written)) != FT_OK) || (bytes_written != 3))
+	{
+		ERROR("couldn't write to ftdi device: %i", status);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	
+	if (strcmp(layout->name, "jtagkey") == 0)
+	{
+		nTRST = 0x01;
+		nTRSTnOE = 0x4;
+		nSRST = 0x02;
+		nSRSTnOE = 0x08;
+	}
+	else if (strcmp(layout->name, "jtagkey_prototype_v1") == 0)
+	{
+		nTRST = 0x02;
+		nTRSTnOE = 0x1;
+		nSRST = 0x08;
+		nSRSTnOE = 0x04;
+	}
+	else
+	{
+		ERROR("BUG: jtagkey_init called for non jtagkey layout");
+		exit(-1);
+	}
+	
+	high_output = 0x0;
+	high_direction = 0x0f;
+
+	if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
+	{
+		high_output |= nTRSTnOE;
+		high_output &= ~nTRST;
+	}
+	else
+	{
+		high_output &= ~nTRSTnOE;
+		high_output |= nTRST;
+	}
+	
+	if (jtag_reset_config & RESET_SRST_PUSH_PULL)
+	{
+		high_output &= ~nSRSTnOE;
+		high_output |= nSRST;
+	}
+	else
+	{
+		high_output |= nSRSTnOE;
+		high_output &= ~nSRST;
+	}
+	
+	/* initialize high port */
+	buf[0] = 0x82; /* command "set data bits low byte" */
+	buf[1] = high_output; /* value */
+	buf[2] = high_direction;   /* all outputs (xRST and xRSTnOE) */
+	DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
+	
+	if (((FT_Write(ftdih, buf, 3, &bytes_written)) != FT_OK) || (bytes_written != 3))
+	{
+		ERROR("couldn't write to ftdi device: %i", status);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	
+	return ERROR_OK;
+}
+
+int ftd2xx_quit(void)
+{
+	FT_STATUS status;
+
+	status = FT_Close(ftdih);
+
+	free(ftd2xx_buffer);
+
+	return ERROR_OK;
+}
+
+int ftd2xx_handle_device_desc_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	if (argc == 1)
+	{
+		ftd2xx_device_desc = strdup(args[0]);
+	}
+	else
+	{
+		ERROR("expected exactly one argument to ftd2xx_device_desc <description>");
+	}
+	
+	return ERROR_OK;
+}
+
+int ftd2xx_handle_layout_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	if (argc == 0)
+		return ERROR_OK;
+
+	ftd2xx_layout = malloc(strlen(args[0]));
+	strcpy(ftd2xx_layout, args[0]);
+
+	return ERROR_OK;
+}
+
+int ftd2xx_handle_vid_pid_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	if (argc >= 2)
+	{
+		ftd2xx_vid = strtol(args[0], NULL, 0);
+		ftd2xx_pid = strtol(args[1], NULL, 0);
+	}
+	else
+	{
+		WARNING("incomplete ftd2xx_vid_pid configuration directive");
+	}
+	
+	return ERROR_OK;
+}
diff --git a/src/jtag/ftdi2232.c b/src/jtag/ftdi2232.c
new file mode 100644
index 00000000..efd528c3
--- /dev/null
+++ b/src/jtag/ftdi2232.c
@@ -0,0 +1,630 @@
+/***************************************************************************
+ *   Copyright (C) 2004 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.             *
+ ***************************************************************************/
+
+/* project specific includes */
+#include "log.h"
+#include "types.h"
+#include "jtag.h"
+#include "configuration.h"
+#include "command.h"
+
+/* system includes */
+#include <string.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <usb.h>
+#include <ftdi.h>
+
+#include <sys/time.h>
+#include <time.h>
+
+/* enable this to debug io latency
+ */
+#if 0
+#define _DEBUG_USB_IO_
+#endif
+
+int ftdi2232_execute_queue(void);
+
+int ftdi2232_speed(int speed);
+int ftdi2232_register_commands(struct command_context_s *cmd_ctx);
+int ftdi2232_init(void);
+int ftdi2232_quit(void);
+
+enum { FTDI2232_TRST = 0x10, FTDI2232_SRST = 0x40 };
+static u8 discrete_output = 0x0 | FTDI2232_TRST | FTDI2232_SRST;
+static struct ftdi_context ftdic;
+
+static u8 *ftdi2232_buffer = NULL;
+static int ftdi2232_buffer_size = 0;
+static int ftdi2232_read_pointer = 0;
+static int ftdi2232_expect_read = 0;
+#define FTDI2232_BUFFER_SIZE	131072
+#define BUFFER_ADD ftdi2232_buffer[ftdi2232_buffer_size++]
+#define BUFFER_READ ftdi2232_buffer[ftdi2232_read_pointer++]
+
+#define FTDI2232_SAVE_SIZE	1024
+
+int ftdi2232_handle_vid_pid_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+static u16 ftdi2232_vid = 0x0403;
+static u16 ftdi2232_pid = 0x6010;
+
+jtag_interface_t ftdi2232_interface = 
+{
+	
+	.name = "ftdi2232",
+	
+	.execute_queue = ftdi2232_execute_queue,
+	
+	.support_statemove = 1,
+	
+	.speed = ftdi2232_speed,
+	.register_commands = ftdi2232_register_commands,
+	.init = ftdi2232_init,
+	.quit = ftdi2232_quit,
+};
+
+int ftdi2232_speed(int speed)
+{
+	u8 buf[3];
+
+	buf[0] = 0x86; /* command "set divisor" */
+	buf[1] = speed & 0xff; /* valueL (0=6MHz, 1=3MHz, 2=1.5MHz, ...*/
+	buf[2] = (speed >> 8) & 0xff; /* valueH */
+	
+	DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
+	ftdi_write_data(&ftdic, buf, 3);
+
+	return ERROR_OK;
+}
+
+int ftdi2232_register_commands(struct command_context_s *cmd_ctx)
+{
+	register_command(cmd_ctx, NULL, "ftdi2232_vid_pid", ftdi2232_handle_vid_pid_command,
+		COMMAND_CONFIG, NULL);
+	
+	return ERROR_OK;
+}
+
+void ftdi2232_end_state(state)
+{
+	if (tap_move_map[state] != -1)
+		end_state = state;
+	else
+	{
+		ERROR("BUG: %i is not a valid end state", state);
+		exit(-1);
+	}
+}
+
+void ftdi2232_read_scan(enum scan_type type, u8* buffer, int scan_size)
+{
+	int num_bytes = ((scan_size + 7) / 8);
+	int bits_left = scan_size;
+	int cur_byte = 0;
+
+	while(num_bytes-- > 1)
+	{
+		buffer[cur_byte] = BUFFER_READ;
+		cur_byte++;
+		bits_left -= 8;
+	}
+
+	buffer[cur_byte] = 0x0;
+
+	if (bits_left > 1)
+	{
+		buffer[cur_byte] = BUFFER_READ >> 1;
+	}
+
+	buffer[cur_byte] = (buffer[cur_byte] | ((BUFFER_READ & 0x02) << 6)) >> (8 - bits_left);
+
+}
+
+void ftdi2232_debug_dump_buffer(void)
+{
+	int i;
+	for (i = 0; i < ftdi2232_buffer_size; i++)
+	{
+		printf("%2.2x ", ftdi2232_buffer[i]);
+		if (i % 16 == 15)
+			printf("\n");
+	}
+	printf("\n");
+	fflush(stdout);
+}
+
+int ftdi2232_send_and_recv(jtag_command_t *first, jtag_command_t *last)
+{
+	jtag_command_t *cmd;
+	u8 *buffer;
+	int scan_size;
+	enum scan_type type;
+	int retval;
+
+	BUFFER_ADD = 0x87;	/* send immediate command */
+	
+	if (ftdi2232_buffer_size > FTDI2232_SAVE_SIZE)
+	{
+		ERROR("BUG: ftdi2232_buffer grew beyond %i byte (%i) - this is going to fail", FTDI2232_SAVE_SIZE,  ftdi2232_buffer_size);
+	}
+
+#ifdef _DEBUG_USB_IO_
+	DEBUG("write buffer (size %i):", ftdi2232_buffer_size);
+	ftdi2232_debug_dump_buffer();
+#endif
+
+	if ((retval = ftdi_write_data(&ftdic, ftdi2232_buffer, ftdi2232_buffer_size)) < 0)
+	{
+		ERROR("ftdi_write_data returned %i", retval);
+		exit(-1);
+	}
+
+	if (ftdi2232_expect_read)
+	{
+		int timeout = 100;
+		ftdi2232_buffer_size = 0;
+		
+		while ((ftdi2232_buffer_size < ftdi2232_expect_read) && timeout)
+		{
+			ftdi2232_buffer_size += ftdi_read_data(&ftdic, ftdi2232_buffer + ftdi2232_buffer_size, FTDI2232_BUFFER_SIZE - ftdi2232_buffer_size);
+			timeout--;
+		}
+
+		if (ftdi2232_expect_read != ftdi2232_buffer_size)
+		{
+			ERROR("ftdi2232_expect_read (%i) != ftdi2232_buffer_size (%i) (%i retries)", ftdi2232_expect_read, ftdi2232_buffer_size, 100 - timeout);
+			ftdi2232_debug_dump_buffer();
+
+			exit(-1);
+		}
+
+#ifdef _DEBUG_USB_IO_
+		DEBUG("read buffer (%i retries): %i bytes", 100 - timeout, ftdi2232_buffer_size);
+		ftdi2232_debug_dump_buffer();	
+#endif
+	}
+
+	ftdi2232_expect_read = 0;
+	ftdi2232_read_pointer = 0;
+
+	cmd = first;
+	while (cmd != last)
+	{
+		switch (cmd->type)
+		{
+			case JTAG_SCAN:
+				type = jtag_scan_type(cmd->cmd.scan);
+				if (type != SCAN_OUT)
+				{
+					scan_size = jtag_scan_size(cmd->cmd.scan);
+					buffer = calloc(CEIL(scan_size, 8), 1);
+					ftdi2232_read_scan(type, buffer, scan_size);
+					jtag_read_buffer(buffer, cmd->cmd.scan);
+					free(buffer);
+				}
+				break;
+			default:
+				break;
+		}
+		cmd = cmd->next;
+	}
+	
+	ftdi2232_buffer_size = 0;
+
+	return ERROR_OK;
+}
+
+void ftdi2232_add_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size)
+{
+	int num_bytes = (scan_size + 7) / 8;
+	int bits_left = scan_size;
+	int cur_byte = 0;
+	int last_bit;
+
+	/* command "Clock Data to TMS/CS Pin (no Read)" */
+	BUFFER_ADD = 0x4b;
+	/* scan 7 bit */
+	BUFFER_ADD = 0x6;
+	/* TMS data bits */
+	if (ir_scan)
+	{
+		BUFFER_ADD = TAP_MOVE(cur_state, TAP_SI);
+		cur_state = TAP_SI;
+	}
+	else
+	{
+		BUFFER_ADD = TAP_MOVE(cur_state, TAP_SD);
+		cur_state = TAP_SD;
+	}
+	//DEBUG("added TMS scan (no read)");
+
+	/* add command for complete bytes */
+	if (num_bytes > 1)
+	{
+		if (type == SCAN_IO)
+		{
+			/* Clock Data Bytes In and Out LSB First */
+			BUFFER_ADD = 0x39;
+			//DEBUG("added TDI bytes (io %i)", num_bytes);
+		}
+		else if (type == SCAN_OUT)
+		{
+			/* Clock Data Bytes Out on -ve Clock Edge LSB First (no Read) */
+			BUFFER_ADD = 0x19;
+			//DEBUG("added TDI bytes (o)");
+		}
+		else if (type == SCAN_IN)
+		{
+			/* Clock Data Bytes In on +ve Clock Edge LSB First (no Write) */
+			BUFFER_ADD = 0x28;
+			//DEBUG("added TDI bytes (i %i)", num_bytes);
+		}
+		BUFFER_ADD = (num_bytes-2) & 0xff;
+		BUFFER_ADD = ((num_bytes-2) >> 8) & 0xff;
+	}
+	if (type != SCAN_IN)
+	{
+		/* add complete bytes */
+		while(num_bytes-- > 1)
+		{
+			BUFFER_ADD = buffer[cur_byte];
+			cur_byte++;
+			bits_left -= 8;
+		}
+	}
+	if (type == SCAN_IN)
+	{
+		bits_left -= 8 * (num_bytes - 1);
+	}
+
+	/* the most signifcant bit is scanned during TAP movement */
+	if (type != SCAN_IN)
+		last_bit = (buffer[cur_byte] >> (bits_left - 1)) & 0x1;
+	else
+		last_bit = 0;
+
+	/* process remaining bits but the last one */
+	if (bits_left > 1)
+	{
+		if (type == SCAN_IO)
+		{
+			/* Clock Data Bits In and Out LSB First */
+			BUFFER_ADD = 0x3b;
+			//DEBUG("added TDI bits (io) %i", bits_left - 1);
+		}
+		else if (type == SCAN_OUT)
+		{
+			/* Clock Data Bits Out on -ve Clock Edge LSB First (no Read) */
+			BUFFER_ADD = 0x1b;
+			//DEBUG("added TDI bits (o)");
+		}
+		else if (type == SCAN_IN)
+		{
+			/* Clock Data Bits In on +ve Clock Edge LSB First (no Write) */
+			BUFFER_ADD = 0x2a;
+			//DEBUG("added TDI bits (i %i)", bits_left - 1);
+		}
+		BUFFER_ADD = bits_left - 2;
+		if (type != SCAN_IN)
+			BUFFER_ADD = buffer[cur_byte];
+	}
+
+	/* move from Shift-IR/DR to end state */
+	if (type != SCAN_OUT)
+	{
+		/* Clock Data to TMS/CS Pin with Read */
+		BUFFER_ADD = 0x6b;
+		//DEBUG("added TMS scan (read)");
+	}
+	else
+	{
+		/* Clock Data to TMS/CS Pin (no Read) */
+		BUFFER_ADD = 0x4b;
+		//DEBUG("added TMS scan (no read)");
+	}
+	BUFFER_ADD = 0x6;
+	BUFFER_ADD = TAP_MOVE(cur_state, end_state) | (last_bit << 7);
+	cur_state = end_state;
+
+}
+
+int ftdi2232_predict_scan_out(int scan_size, enum scan_type type)
+{
+	int predicted_size = 6;
+	if (type == SCAN_IN)	/* only from device to host */
+	{
+		predicted_size += (CEIL(scan_size, 8) > 1) ? 3 : 0;
+		predicted_size += ((scan_size - 1) % 8) ? 2 : 0;
+	}
+	else					/* host to device, or bidirectional */
+	{
+		predicted_size += (CEIL(scan_size, 8) > 1) ? (CEIL(scan_size, 8) + 3 - 1) : 0;
+		predicted_size += ((scan_size - 1) % 8) ? 3 : 0;
+	}
+
+	return predicted_size;
+}
+
+int ftdi2232_predict_scan_in(int scan_size, enum scan_type type)
+{
+	int predicted_size = 0;
+	
+	if (type != SCAN_OUT)
+	{
+		/* complete bytes */
+		predicted_size += (CEIL(scan_size, 8) > 1) ? (CEIL(scan_size, 8) - 1) : 0;
+		/* remaining bits - 1 */
+		predicted_size += ((scan_size - 1) % 8) ? 1 : 0;
+		/* last bit (from TMS scan) */
+		predicted_size += 1;
+	}
+	
+	//DEBUG("scan_size: %i, predicted_size: %i", scan_size, predicted_size);
+
+	return predicted_size;
+}
+
+int ftdi2232_execute_queue()
+{
+	jtag_command_t *cmd = jtag_command_queue; /* currently processed command */
+	jtag_command_t *first_unsent = cmd;	/* next command that has to be sent */
+	u8 *buffer;
+	int scan_size;	/* size of IR or DR scan */
+	enum scan_type type;
+	int i;
+	int predicted_size = 0;
+	int require_send = 0;
+
+	ftdi2232_buffer_size = 0;
+	ftdi2232_expect_read = 0;
+
+	while (cmd)
+	{
+		switch(cmd->type)
+		{
+			case JTAG_END_STATE:
+				if (cmd->cmd.end_state->end_state != -1)
+					ftdi2232_end_state(cmd->cmd.end_state->end_state);
+				break;
+			case JTAG_RESET:
+				/* only send the maximum buffer size that FT2232C can handle */
+				predicted_size = 3;
+				if (ftdi2232_buffer_size + predicted_size + 1 > FTDI2232_SAVE_SIZE)
+				{
+					ftdi2232_send_and_recv(first_unsent, cmd);
+					require_send = 0;
+					first_unsent = cmd;
+				}
+
+				if (cmd->cmd.reset->trst == 1)
+				{
+					cur_state = TAP_TLR;
+					discrete_output &= ~FTDI2232_TRST;
+				}
+				else if (cmd->cmd.reset->trst == 0)
+				{
+					discrete_output |= FTDI2232_TRST;
+				}
+
+				if (cmd->cmd.reset->srst == 1)
+					discrete_output &= ~FTDI2232_SRST;
+				else if (cmd->cmd.reset->srst == 0)
+					discrete_output |= FTDI2232_SRST;
+				/* command "set data bits low byte" */
+				BUFFER_ADD = 0x80;
+				/* value (TMS=1,TCK=0, TDI=0, TRST/SRST */
+				BUFFER_ADD = 0x08 | discrete_output;
+				/* dir (output=1), TCK/TDI/TMS=out, TDO=in, TRST/SRST=out */
+				BUFFER_ADD = 0x0b | FTDI2232_SRST | FTDI2232_TRST;
+				require_send = 1;
+				break;
+			case JTAG_RUNTEST:
+				/* only send the maximum buffer size that FT2232C can handle */
+				predicted_size = 0;
+				if (cur_state != TAP_RTI)
+					predicted_size += 3;
+				predicted_size += 3 * CEIL(cmd->cmd.runtest->num_cycles, 7);
+				if ((cmd->cmd.runtest->end_state != -1) && (cmd->cmd.runtest->end_state != TAP_RTI))
+					predicted_size += 3;
+				if ((cmd->cmd.runtest->end_state == -1) && (end_state != TAP_RTI))
+					predicted_size += 3;
+				if (ftdi2232_buffer_size + predicted_size + 1 > FTDI2232_SAVE_SIZE)
+				{
+					ftdi2232_send_and_recv(first_unsent, cmd);
+					require_send = 0;
+					first_unsent = cmd;
+				}
+				if (cur_state != TAP_RTI)
+				{
+					/* command "Clock Data to TMS/CS Pin (no Read)" */
+					BUFFER_ADD = 0x4b;
+					/* scan 7 bit */
+					BUFFER_ADD = 0x6;
+					/* TMS data bits */
+					BUFFER_ADD = TAP_MOVE(cur_state, TAP_RTI);
+					cur_state = TAP_RTI;
+					require_send = 1;
+				}
+				i = cmd->cmd.runtest->num_cycles;
+				while (i > 0)
+				{
+					/* command "Clock Data to TMS/CS Pin (no Read)" */
+					BUFFER_ADD = 0x4b;
+					/* scan 7 bit */
+					BUFFER_ADD = (i > 7) ? 6 : (i - 1);
+					/* TMS data bits */
+					BUFFER_ADD = 0x0;
+					cur_state = TAP_RTI;
+					i -= (i > 7) ? 7 : i;
+					//DEBUG("added TMS scan (no read)");
+				}
+				if (cmd->cmd.runtest->end_state != -1)
+					ftdi2232_end_state(cmd->cmd.runtest->end_state);
+				if (cur_state != end_state)
+				{
+					/* command "Clock Data to TMS/CS Pin (no Read)" */
+					BUFFER_ADD = 0x4b;
+					/* scan 7 bit */
+					BUFFER_ADD = 0x6;
+					/* TMS data bits */
+					BUFFER_ADD = TAP_MOVE(cur_state, end_state);
+					cur_state = end_state;
+					//DEBUG("added TMS scan (no read)");
+				}
+				require_send = 1;
+				break;
+			case JTAG_STATEMOVE:
+				/* only send the maximum buffer size that FT2232C can handle */
+				predicted_size = 3;
+				if (ftdi2232_buffer_size + predicted_size + 1 > FTDI2232_SAVE_SIZE)
+				{
+					ftdi2232_send_and_recv(first_unsent, cmd);
+					require_send = 0;
+					first_unsent = cmd;
+				}
+				if (cmd->cmd.statemove->end_state != -1)
+					ftdi2232_end_state(cmd->cmd.statemove->end_state);
+				/* command "Clock Data to TMS/CS Pin (no Read)" */
+				BUFFER_ADD = 0x4b;
+				/* scan 7 bit */
+				BUFFER_ADD = 0x6;
+				/* TMS data bits */
+				BUFFER_ADD = TAP_MOVE(cur_state, end_state);
+				//DEBUG("added TMS scan (no read)");
+				cur_state = end_state;
+				require_send = 1;
+				break;
+			case JTAG_SCAN:
+				scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
+				type = jtag_scan_type(cmd->cmd.scan);
+				predicted_size = ftdi2232_predict_scan_out(scan_size, type);
+				if (ftdi2232_buffer_size + predicted_size + 1 > FTDI2232_SAVE_SIZE)
+				{
+					ftdi2232_send_and_recv(first_unsent, cmd);
+					require_send = 0;
+					first_unsent = cmd;
+				}
+				ftdi2232_expect_read += ftdi2232_predict_scan_in(scan_size, type);
+				//DEBUG("new read size: %i", ftdi2232_expect_read);
+				if (cmd->cmd.scan->end_state != -1)
+					ftdi2232_end_state(cmd->cmd.scan->end_state);
+				ftdi2232_add_scan(cmd->cmd.scan->ir_scan, type, buffer, scan_size);
+				require_send = 1;
+				if (buffer)
+					free(buffer);
+				break;
+			case JTAG_SLEEP:
+				jtag_sleep(cmd->cmd.sleep->us);
+				break;
+			default:
+				ERROR("BUG: unknown JTAG command type encountered");
+				exit(-1);
+		}
+		cmd = cmd->next;
+	}
+
+	if (require_send > 0)
+		ftdi2232_send_and_recv(first_unsent, cmd);
+
+	return ERROR_OK;
+}
+
+int ftdi2232_init(void)
+{
+	if (ftdi_init(&ftdic) < 0)
+		return ERROR_JTAG_INIT_FAILED;
+
+	/* context, vendor id, product id */
+	if (ftdi_usb_open(&ftdic, ftdi2232_vid, ftdi2232_pid) < 0)
+	{
+		ERROR("unable to open ftdi device: %s", ftdic.error_str);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	if (ftdi_usb_reset(&ftdic) < 0)
+	{
+		ERROR("unable to reset ftdi device");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	if (ftdi_set_latency_timer(&ftdic, 1) < 0)
+	{
+		ERROR("unable to set latency timer");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	ftdi2232_buffer_size = 0;
+	ftdi2232_buffer = malloc(FTDI2232_BUFFER_SIZE);
+
+	ftdic.bitbang_mode = 0; /* Reset controller */
+	ftdi_enable_bitbang(&ftdic, 0x0b | FTDI2232_SRST | FTDI2232_TRST); /* ctx, i/o mask (out=1, in=0) */
+
+	ftdic.bitbang_mode = 2; /* MPSSE mode */
+	ftdi_enable_bitbang(&ftdic, 0x0b | FTDI2232_SRST | FTDI2232_TRST); /* ctx, i/o mask (out=1, in=0) */
+	
+	if (ftdi_usb_purge_buffers(&ftdic) < 0)
+	{
+		ERROR("ftdi_purge_buffers: %s", ftdic.error_str);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	/* initialize low byte for jtag */
+	BUFFER_ADD = 0x80; /* command "set data bits low byte" */
+	BUFFER_ADD = 0x08 | FTDI2232_SRST | FTDI2232_TRST; /* value (TMS=1,TCK=0, TDI=0, xRST high) */
+	BUFFER_ADD = 0x0b | FTDI2232_SRST | FTDI2232_TRST; /* dir (output=1), TCK/TDI/TMS=out, TDO=in */
+	BUFFER_ADD = 0x85; /* command "Disconnect TDI/DO to TDO/DI for Loopback" */
+	ftdi2232_debug_dump_buffer();
+	if (ftdi_write_data(&ftdic, ftdi2232_buffer, ftdi2232_buffer_size) != 4)
+		return ERROR_JTAG_INIT_FAILED;
+
+	ftdi2232_speed(jtag_speed);
+
+	return ERROR_OK;
+}
+
+int ftdi2232_quit(void)
+{
+	ftdi_disable_bitbang(&ftdic);
+	
+	ftdi_usb_close(&ftdic);
+	
+	ftdi_deinit(&ftdic);
+
+	free(ftdi2232_buffer);
+
+	return ERROR_OK;
+}
+
+int ftdi2232_handle_vid_pid_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	if (argc >= 2)
+	{
+		ftdi2232_vid = strtol(args[0], NULL, 0);
+		ftdi2232_pid = strtol(args[1], NULL, 0);
+	}
+	else
+	{
+		WARNING("incomplete ftdi2232_vid_pid configuration directive");
+	}
+	
+	return ERROR_OK;
+}
diff --git a/src/jtag/jtag.c b/src/jtag/jtag.c
new file mode 100644
index 00000000..e306b0a2
--- /dev/null
+++ b/src/jtag/jtag.c
@@ -0,0 +1,1585 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "config.h"
+#include "jtag.h"
+
+#include "command.h"
+#include "log.h"
+#include "interpreter.h"
+
+#include "stdlib.h"
+#include "string.h"
+#include <unistd.h>
+
+char* tap_state_strings[16] =
+{
+	"tlr", 
+	"sds", "cd", "sd", "e1d", "pd", "e2d", "ud",
+	"rti",
+	"sis", "ci", "si", "e1i", "pi", "e2i", "ui"
+};
+
+typedef struct cmd_queue_page_s
+{
+	void *address;
+	size_t used;
+	struct cmd_queue_page_s *next;
+} cmd_queue_page_t;
+
+#define CMD_QUEUE_PAGE_SIZE (1024 * 1024)
+static cmd_queue_page_t *cmd_queue_pages = NULL;
+
+/* tap_move[i][j]: tap movement command to go from state i to state j
+ * 0: Test-Logic-Reset
+ * 1: Run-Test/Idle
+ * 2: Shift-DR
+ * 3: Pause-DR
+ * 4: Shift-IR
+ * 5: Pause-IR
+ */
+u8 tap_move[6][6] =
+{
+/*	  TLR   RTI   SD    PD    SI    PI             */
+	{0x7f, 0x00, 0x17, 0x0a, 0x1b, 0x16},	/* TLR */
+	{0x7f, 0x00, 0x25, 0x05, 0x2b, 0x0b},	/* RTI */
+	{0x7f, 0x31, 0x00, 0x01, 0x0f, 0x2f},	/* SD  */
+	{0x7f, 0x30, 0x20, 0x17, 0x1e, 0x2f},	/* PD  */
+	{0x7f, 0x31, 0x07, 0x17, 0x00, 0x01},	/* SI  */
+	{0x7f, 0x30, 0x1c, 0x17, 0x20, 0x2f}	/* PI  */
+};
+
+int tap_move_map[16] = {
+	0, -1, -1,  2, -1,  3, -1, -1,
+	1, -1, -1,  4, -1,  5, -1, -1
+};
+
+tap_transition_t tap_transitions[16] =
+{
+	{TAP_TLR, TAP_RTI},		/* TLR */
+	{TAP_SIS, TAP_CD},		/* SDS */
+	{TAP_E1D, TAP_SD},		/* CD  */
+	{TAP_E1D, TAP_SD},		/* SD  */
+	{TAP_UD,  TAP_PD}, 		/* E1D */
+	{TAP_E2D, TAP_PD},		/* PD  */
+	{TAP_UD,  TAP_SD},		/* E2D */
+	{TAP_SDS, TAP_RTI},		/* UD  */
+	{TAP_SDS, TAP_RTI},		/* RTI */
+	{TAP_TLR, TAP_CI},		/* SIS */
+	{TAP_E1I, TAP_SI},		/* CI  */
+	{TAP_E1I, TAP_SI},		/* SI  */
+	{TAP_UI,  TAP_PI}, 		/* E1I */
+	{TAP_E2I, TAP_PI},		/* PI  */
+	{TAP_UI,  TAP_SI},		/* E2I */
+	{TAP_SDS, TAP_RTI}		/* UI  */
+};
+
+enum tap_state end_state = TAP_TLR;
+enum tap_state cur_state = TAP_TLR;
+int jtag_trst = 0;
+int jtag_srst = 0;
+
+jtag_command_t *jtag_command_queue = NULL;
+jtag_command_t **last_comand_pointer = &jtag_command_queue;
+jtag_device_t *jtag_devices = NULL;
+int jtag_num_devices = 0;
+int jtag_ir_scan_size = 0;
+enum reset_types jtag_reset_config = RESET_NONE;
+enum tap_state cmd_queue_end_state = TAP_TLR;
+enum tap_state cmd_queue_cur_state = TAP_TLR;
+
+int jtag_verify_capture_ir = 1;
+
+/* callbacks to inform high-level handlers about JTAG state changes */
+jtag_event_callback_t *jtag_event_callbacks;
+
+/* jtag interfaces (parport, FTDI-USB, TI-USB, ...)
+ */
+#if BUILD_PARPORT == 1
+	extern jtag_interface_t parport_interface;
+#endif
+
+#if BUILD_FTDI2232 == 1
+	extern jtag_interface_t ftdi2232_interface;
+#endif
+
+#if BUILD_FTD2XX == 1
+	extern jtag_interface_t ftd2xx_interface;
+#endif
+
+#if BUILD_AMTJTAGACCEL == 1
+	extern jtag_interface_t amt_jtagaccel_interface;
+#endif
+
+#if BUILD_EP93XX == 1
+	extern jtag_interface_t ep93xx_interface;
+#endif
+
+jtag_interface_t *jtag_interfaces[] = {
+#if BUILD_PARPORT == 1
+	&parport_interface,
+#endif
+#if BUILD_FTDI2232 == 1
+	&ftdi2232_interface,
+#endif
+#if BUILD_FTD2XX == 1
+	&ftd2xx_interface,
+#endif
+#if BUILD_AMTJTAGACCEL == 1
+	&amt_jtagaccel_interface,
+#endif
+#if BUILD_EP93XX == 1
+	&ep93xx_interface,
+#endif
+	NULL,
+};
+
+jtag_interface_t *jtag = NULL;
+
+/* configuration */
+char* jtag_interface = NULL;
+int jtag_speed = -1;
+
+/* forward declarations */
+
+/* jtag commands */
+int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_runtest_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_statemove_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_drscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+int jtag_register_event_callback(int (*callback)(enum jtag_event event, void *priv), void *priv)
+{
+	jtag_event_callback_t **callbacks_p = &jtag_event_callbacks;
+	
+	if (callback == NULL)
+	{
+		return ERROR_INVALID_ARGUMENTS;
+	}
+	
+	if (*callbacks_p)
+	{
+		while ((*callbacks_p)->next)
+			callbacks_p = &((*callbacks_p)->next);
+		callbacks_p = &((*callbacks_p)->next);
+	}
+	
+	(*callbacks_p) = malloc(sizeof(jtag_event_callback_t));
+	(*callbacks_p)->callback = callback;
+	(*callbacks_p)->priv = priv;
+	(*callbacks_p)->next = NULL;
+	
+	return ERROR_OK;
+}
+
+int jtag_unregister_event_callback(int (*callback)(enum jtag_event event, void *priv))
+{
+	jtag_event_callback_t **callbacks_p = &jtag_event_callbacks;
+	
+	if (callback == NULL)
+	{
+		return ERROR_INVALID_ARGUMENTS;
+	}
+		
+	while (*callbacks_p)
+	{
+		jtag_event_callback_t **next = &((*callbacks_p)->next);
+		if ((*callbacks_p)->callback == callback)
+		{
+			free(*callbacks_p);
+			*callbacks_p = *next;
+		}
+		callbacks_p = next;
+	}
+	
+	return ERROR_OK;
+}
+
+int jtag_call_event_callbacks(enum jtag_event event)
+{
+	jtag_event_callback_t *callback = jtag_event_callbacks;
+	
+	DEBUG("jtag event: %i", event);
+	
+	while (callback)
+	{
+		callback->callback(event, callback->priv);
+		callback = callback->next;
+	}
+	
+	return ERROR_OK;
+}
+
+/* returns a pointer to the pointer of the last command in queue
+ * this may be a pointer to the root pointer (jtag_command_queue)
+ * or to the next member of the last but one command
+ */
+jtag_command_t** jtag_get_last_command_p(void)
+{
+/*	jtag_command_t *cmd = jtag_command_queue;
+	
+	if (cmd)
+		while (cmd->next)
+			cmd = cmd->next;
+	else
+		return &jtag_command_queue;
+	
+	return &cmd->next;*/
+	
+	return last_comand_pointer;
+}
+
+/* returns a pointer to the n-th device in the scan chain */
+jtag_device_t* jtag_get_device(int num)
+{
+	jtag_device_t *device = jtag_devices;
+	int i = 0;
+
+	while (device)
+	{
+		if (num == i)
+			return device;
+		device = device->next;
+		i++;
+	}
+
+	return NULL;
+}
+
+void* cmd_queue_alloc(size_t size)
+{
+	cmd_queue_page_t **p_page = &cmd_queue_pages;
+	int offset;
+
+	if (*p_page)
+	{
+		while ((*p_page)->next)
+			p_page = &((*p_page)->next);
+		if (CMD_QUEUE_PAGE_SIZE - (*p_page)->used < size)
+			p_page = &((*p_page)->next);
+	}
+
+	if (!*p_page)
+	{
+		*p_page = malloc(sizeof(cmd_queue_page_t));
+		(*p_page)->used = 0;
+		(*p_page)->address = malloc(CMD_QUEUE_PAGE_SIZE);
+		(*p_page)->next = NULL;
+	}
+
+	offset = (*p_page)->used;
+	(*p_page)->used += size;
+	
+	return ((*p_page)->address) + offset;
+}
+
+void cmd_queue_free()
+{
+	cmd_queue_page_t *page = cmd_queue_pages;
+
+	while (page)
+	{
+		cmd_queue_page_t *last = page;
+		free(page->address);
+		page = page->next;
+		free(last);
+	}
+
+	cmd_queue_pages = NULL;
+}
+
+int jtag_add_ir_scan(int num_fields, scan_field_t *fields, enum tap_state state)
+{
+	jtag_command_t **last_cmd;
+	jtag_device_t *device;
+	int i, j;
+	int scan_size = 0;
+	/*	int changed = 0; */
+
+	if (jtag_trst == 1)
+	{
+		WARNING("JTAG command queued, while TRST is low (TAP in reset)");
+		return ERROR_JTAG_TRST_ASSERTED;
+	}
+
+	/*
+	for (i=0; i<num_fields; i++)
+	{
+		device = jtag_get_device(fields[i].device);
+		if (device)
+		{
+			if (buf_cmp(device->cur_instr, fields[i].out_value, device->ir_length))
+				changed = 1;
+		}
+		else
+		{
+			ERROR("inexistant device specified for ir scan");
+			return ERROR_INVALID_ARGUMENTS;
+		}
+	}
+
+	if (!changed)
+		return ERROR_OK;
+	*/
+	
+	last_cmd = jtag_get_last_command_p();
+	
+	/* allocate memory for a new list member */
+	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+	(*last_cmd)->next = NULL;
+	last_comand_pointer = &((*last_cmd)->next);
+	(*last_cmd)->type = JTAG_SCAN;
+
+	/* allocate memory for ir scan command */
+	(*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
+	(*last_cmd)->cmd.scan->ir_scan = 1;
+	(*last_cmd)->cmd.scan->num_fields = jtag_num_devices;	/* one field per device */
+	(*last_cmd)->cmd.scan->fields = cmd_queue_alloc(jtag_num_devices * sizeof(scan_field_t));
+	(*last_cmd)->cmd.scan->end_state = state;
+	
+	if (state != -1)
+		cmd_queue_end_state = state;
+
+	if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_RELEASED);
+	
+	if (cmd_queue_end_state == TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
+	
+	cmd_queue_cur_state = cmd_queue_end_state;
+		
+	for (i=0; i < jtag_num_devices; i++)
+	{
+		int found = 0;
+		device = jtag_get_device(i);
+		scan_size = device->ir_length;
+		(*last_cmd)->cmd.scan->fields[i].device = i;
+		(*last_cmd)->cmd.scan->fields[i].num_bits = scan_size;
+		(*last_cmd)->cmd.scan->fields[i].in_value = NULL;
+		if (jtag_verify_capture_ir)
+		{
+			(*last_cmd)->cmd.scan->fields[i].in_check_value = buf_cpy(device->expected, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
+			(*last_cmd)->cmd.scan->fields[i].in_check_mask = buf_cpy(device->expected_mask, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
+		}
+		else
+		{
+			(*last_cmd)->cmd.scan->fields[i].in_check_value = NULL;
+			(*last_cmd)->cmd.scan->fields[i].in_check_mask = NULL;
+		}			
+		(*last_cmd)->cmd.scan->fields[i].in_handler = NULL;
+		(*last_cmd)->cmd.scan->fields[i].in_handler_priv = NULL;
+
+		/* search the list */
+		for (j=0; j < num_fields; j++)
+		{
+			if (i == fields[j].device)
+			{
+				found = 1;
+				(*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
+				(*last_cmd)->cmd.scan->fields[i].out_mask = buf_cpy(fields[j].out_mask, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
+				
+				device->bypass = 0;
+				break;
+			}
+		}
+	
+		if (!found)
+		{
+			/* if a device isn't listed, set it to BYPASS */
+			(*last_cmd)->cmd.scan->fields[i].out_value = buf_set_ones(cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
+			(*last_cmd)->cmd.scan->fields[i].out_mask = NULL;
+			device->bypass = 1;
+		
+		}
+		
+		/* update device information */
+		buf_cpy((*last_cmd)->cmd.scan->fields[i].out_value, jtag_get_device(i)->cur_instr, scan_size);
+	}
+	
+	return ERROR_OK;
+}
+
+int jtag_add_plain_ir_scan(int num_fields, scan_field_t *fields, enum tap_state state)
+{
+	jtag_command_t **last_cmd;
+	int i;
+
+	if (jtag_trst == 1)
+	{
+		WARNING("JTAG command queued, while TRST is low (TAP in reset)");
+		return ERROR_JTAG_TRST_ASSERTED;
+	}
+
+	last_cmd = jtag_get_last_command_p();
+	
+	/* allocate memory for a new list member */
+	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+	(*last_cmd)->next = NULL;
+	last_comand_pointer = &((*last_cmd)->next);
+	(*last_cmd)->type = JTAG_SCAN;
+
+	/* allocate memory for ir scan command */
+	(*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
+	(*last_cmd)->cmd.scan->ir_scan = 1;
+	(*last_cmd)->cmd.scan->num_fields = num_fields;
+	(*last_cmd)->cmd.scan->fields = cmd_queue_alloc(num_fields * sizeof(scan_field_t));
+	(*last_cmd)->cmd.scan->end_state = state;
+
+	if (state != -1)
+		cmd_queue_end_state = state;
+
+	if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_RELEASED);
+	
+	if (cmd_queue_end_state == TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
+		
+	cmd_queue_cur_state = cmd_queue_end_state;
+	
+	for (i = 0; i < num_fields; i++)
+	{
+		int num_bits = fields[i].num_bits;
+		int num_bytes = CEIL(fields[i].num_bits, 8);
+		(*last_cmd)->cmd.scan->fields[i].device = fields[i].device;
+		(*last_cmd)->cmd.scan->fields[i].num_bits = num_bits;
+		(*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[i].out_value, cmd_queue_alloc(num_bytes), num_bits);
+		(*last_cmd)->cmd.scan->fields[i].out_mask = buf_cpy(fields[i].out_mask, cmd_queue_alloc(num_bytes), num_bits);
+		(*last_cmd)->cmd.scan->fields[i].in_value = fields[i].in_value;
+		(*last_cmd)->cmd.scan->fields[i].in_check_value = buf_cpy(fields[i].in_check_value, cmd_queue_alloc(num_bytes), num_bits);
+		(*last_cmd)->cmd.scan->fields[i].in_check_mask = buf_cpy(fields[i].in_check_mask, cmd_queue_alloc(num_bytes), num_bits);
+		(*last_cmd)->cmd.scan->fields[i].in_handler = NULL;
+		(*last_cmd)->cmd.scan->fields[i].in_handler_priv = NULL;
+	}
+	return ERROR_OK;
+}
+
+int jtag_add_dr_scan(int num_fields, scan_field_t *fields, enum tap_state state)
+{
+	int i, j;
+	int bypass_devices = 0;
+	int field_count = 0;
+	jtag_command_t **last_cmd = jtag_get_last_command_p();
+	jtag_device_t *device = jtag_devices;
+	int scan_size;
+
+	if (jtag_trst == 1)
+	{
+		WARNING("JTAG command queued, while TRST is low (TAP in reset)");
+		return ERROR_JTAG_TRST_ASSERTED;
+	}
+
+	/* count devices in bypass */
+	while (device)
+	{
+		if (device->bypass)
+			bypass_devices++;
+		device = device->next;
+	}
+	
+	/* allocate memory for a new list member */
+	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+	last_comand_pointer = &((*last_cmd)->next);
+	(*last_cmd)->next = NULL;
+	(*last_cmd)->type = JTAG_SCAN;
+
+	/* allocate memory for dr scan command */
+	(*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
+	(*last_cmd)->cmd.scan->ir_scan = 0;
+	(*last_cmd)->cmd.scan->num_fields = num_fields + bypass_devices;
+	(*last_cmd)->cmd.scan->fields = cmd_queue_alloc((num_fields + bypass_devices) * sizeof(scan_field_t));
+	(*last_cmd)->cmd.scan->end_state = state;
+
+	if (state != -1)
+		cmd_queue_end_state = state;
+
+	if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_RELEASED);
+	
+	if (cmd_queue_end_state == TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
+			
+	cmd_queue_cur_state = cmd_queue_end_state;
+	
+	for (i=0; i < jtag_num_devices; i++)
+	{
+		int found = 0;
+		(*last_cmd)->cmd.scan->fields[field_count].device = i;
+	
+		for (j=0; j < num_fields; j++)
+		{
+			if (i == fields[j].device)
+			{
+				found = 1;
+				scan_size = fields[j].num_bits;
+				(*last_cmd)->cmd.scan->fields[field_count].num_bits = scan_size;
+				(*last_cmd)->cmd.scan->fields[field_count].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
+				(*last_cmd)->cmd.scan->fields[field_count].out_mask = buf_cpy(fields[j].out_mask, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
+				(*last_cmd)->cmd.scan->fields[field_count].in_value = fields[j].in_value;
+				(*last_cmd)->cmd.scan->fields[field_count].in_check_value = buf_cpy(fields[j].in_check_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
+				(*last_cmd)->cmd.scan->fields[field_count].in_check_mask = buf_cpy(fields[j].in_check_mask, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
+				(*last_cmd)->cmd.scan->fields[field_count].in_handler = fields[j].in_handler;
+				(*last_cmd)->cmd.scan->fields[field_count++].in_handler_priv = fields[j].in_handler_priv;
+			}
+		}
+		if (!found)
+		{
+			/* if a device isn't listed, the BYPASS register should be selected */
+			if (!jtag_get_device(i)->bypass)
+			{
+				ERROR("BUG: no scan data for a device not in BYPASS");
+				exit(-1);
+			}
+	
+			/* program the scan field to 1 bit length, and ignore it's value */
+			(*last_cmd)->cmd.scan->fields[field_count].num_bits = 1;
+			(*last_cmd)->cmd.scan->fields[field_count].out_value = NULL;
+			(*last_cmd)->cmd.scan->fields[field_count].out_mask = NULL;
+			(*last_cmd)->cmd.scan->fields[field_count].in_value = NULL;
+			(*last_cmd)->cmd.scan->fields[field_count].in_check_value = NULL;
+			(*last_cmd)->cmd.scan->fields[field_count].in_check_mask = NULL;
+			(*last_cmd)->cmd.scan->fields[field_count].in_handler = NULL;
+			(*last_cmd)->cmd.scan->fields[field_count++].in_handler_priv = NULL;
+		}
+		else
+		{
+			/* if a device is listed, the BYPASS register must not be selected */
+			if (jtag_get_device(i)->bypass)
+			{
+				ERROR("BUG: scan data for a device in BYPASS");
+				exit(-1);
+			}
+		}
+	}
+	return ERROR_OK;
+}
+
+int jtag_add_plain_dr_scan(int num_fields, scan_field_t *fields, enum tap_state state)
+{
+	int i;
+	jtag_command_t **last_cmd = jtag_get_last_command_p();
+	
+	if (jtag_trst == 1)
+	{
+		WARNING("JTAG command queued, while TRST is low (TAP in reset)");
+		return ERROR_JTAG_TRST_ASSERTED;
+	}
+
+	/* allocate memory for a new list member */
+	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+	last_comand_pointer = &((*last_cmd)->next);
+	(*last_cmd)->next = NULL;
+	(*last_cmd)->type = JTAG_SCAN;
+
+	/* allocate memory for scan command */
+	(*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
+	(*last_cmd)->cmd.scan->ir_scan = 0;
+	(*last_cmd)->cmd.scan->num_fields = num_fields;
+	(*last_cmd)->cmd.scan->fields = cmd_queue_alloc(num_fields * sizeof(scan_field_t));
+	(*last_cmd)->cmd.scan->end_state = state;
+	
+	if (state != -1)
+		cmd_queue_end_state = state;
+
+	if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_RELEASED);
+	
+	if (cmd_queue_end_state == TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
+			
+	cmd_queue_cur_state = cmd_queue_end_state;
+	
+	for (i = 0; i < num_fields; i++)
+	{
+		int num_bits = fields[i].num_bits;
+		int num_bytes = CEIL(fields[i].num_bits, 8);
+		(*last_cmd)->cmd.scan->fields[i].device = fields[i].device;
+		(*last_cmd)->cmd.scan->fields[i].num_bits = num_bits;
+		(*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[i].out_value, cmd_queue_alloc(num_bytes), num_bits);
+		(*last_cmd)->cmd.scan->fields[i].out_mask = buf_cpy(fields[i].out_mask, cmd_queue_alloc(num_bytes), num_bits);
+		(*last_cmd)->cmd.scan->fields[i].in_value = fields[i].in_value;
+		(*last_cmd)->cmd.scan->fields[i].in_check_value = buf_cpy(fields[i].in_check_value, cmd_queue_alloc(num_bytes), num_bits);
+		(*last_cmd)->cmd.scan->fields[i].in_check_mask = buf_cpy(fields[i].in_check_mask, cmd_queue_alloc(num_bytes), num_bits);
+		(*last_cmd)->cmd.scan->fields[i].in_handler = fields[i].in_handler;
+		(*last_cmd)->cmd.scan->fields[i].in_handler_priv = fields[i].in_handler_priv;
+	}
+
+	return ERROR_OK;
+}
+int jtag_add_statemove(enum tap_state state)
+{
+	jtag_command_t **last_cmd = jtag_get_last_command_p();
+	
+	if (jtag_trst == 1)
+	{
+		WARNING("JTAG command queued, while TRST is low (TAP in reset)");
+		return ERROR_JTAG_TRST_ASSERTED;
+	}
+
+	/* allocate memory for a new list member */
+	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+	last_comand_pointer = &((*last_cmd)->next);
+	(*last_cmd)->next = NULL;
+	(*last_cmd)->type = JTAG_STATEMOVE;
+
+	(*last_cmd)->cmd.statemove = cmd_queue_alloc(sizeof(statemove_command_t));
+	(*last_cmd)->cmd.statemove->end_state = state;
+	
+	if (state != -1)
+		cmd_queue_end_state = state;
+
+	if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_RELEASED);
+	
+	if (cmd_queue_end_state == TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
+			
+	cmd_queue_cur_state = cmd_queue_end_state;
+	
+	return ERROR_OK;
+}
+
+int jtag_add_pathmove(int num_states, enum tap_state *path)
+{
+	jtag_command_t **last_cmd = jtag_get_last_command_p();
+	int i;
+	
+	if (jtag_trst == 1)
+	{
+		WARNING("JTAG command queued, while TRST is low (TAP in reset)");
+		return ERROR_JTAG_TRST_ASSERTED;
+	}
+	
+	/* the last state has to be a stable state */
+	if (tap_move_map[path[num_states - 1]] == -1)
+	{
+		ERROR("TAP path doesn't finish in a stable state");
+		return ERROR_JTAG_NOT_IMPLEMENTED;
+	}
+	
+	if (jtag->support_statemove)
+	{
+		/* allocate memory for a new list member */
+		*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+		last_comand_pointer = &((*last_cmd)->next);
+		(*last_cmd)->next = NULL;
+		(*last_cmd)->type = JTAG_RUNTEST;
+	
+		(*last_cmd)->cmd.pathmove = cmd_queue_alloc(sizeof(pathmove_command_t));
+		(*last_cmd)->cmd.pathmove->num_states = num_states;
+		(*last_cmd)->cmd.pathmove->path = cmd_queue_alloc(sizeof(enum tap_state) * num_states);
+		
+		for (i = 0; i < num_states; i++)
+			(*last_cmd)->cmd.pathmove->path[i] = path[i];
+	}
+	else
+	{
+		/* validate the desired path, and see if it fits a default path */
+		int begin = 0;
+		int end = 0;
+		int j;
+		
+		for (i = 0; i < num_states; i++)
+		{
+			for (j = i; j < num_states; j++)
+			{
+				if (tap_move_map[path[j]] != -1)
+				{	
+					end = j;
+					break;
+				}
+			}
+			
+			if (begin - end <= 7) 	/* a default path spans no more than 7 states */
+			{
+				jtag_add_statemove(path[end]);
+			}
+			else
+			{
+				ERROR("encountered a TAP path that can't be fulfilled by default paths");	
+				return ERROR_JTAG_NOT_IMPLEMENTED;
+			}
+			
+			i = end;
+		}
+	}
+
+	if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_RELEASED);
+	
+	if (cmd_queue_end_state == TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
+	
+	cmd_queue_cur_state = path[num_states - 1];
+	
+	return ERROR_OK;
+}
+
+int jtag_add_runtest(int num_cycles, enum tap_state state)
+{
+	jtag_command_t **last_cmd = jtag_get_last_command_p();
+	
+	if (jtag_trst == 1)
+	{
+		WARNING("JTAG command queued, while TRST is low (TAP in reset)");
+		return ERROR_JTAG_TRST_ASSERTED;
+	}
+
+	/* allocate memory for a new list member */
+	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+	(*last_cmd)->next = NULL;
+	last_comand_pointer = &((*last_cmd)->next);
+	(*last_cmd)->type = JTAG_RUNTEST;
+
+	(*last_cmd)->cmd.runtest = cmd_queue_alloc(sizeof(runtest_command_t));
+	(*last_cmd)->cmd.runtest->num_cycles = num_cycles;
+	(*last_cmd)->cmd.runtest->end_state = state;
+	
+	if (state != -1)
+		cmd_queue_end_state = state;
+
+	if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_RELEASED);
+	
+	if (cmd_queue_end_state == TAP_TLR)
+		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
+			
+	cmd_queue_cur_state = cmd_queue_end_state;
+	
+	return ERROR_OK;
+}
+
+int jtag_add_reset(int req_trst, int req_srst)
+{
+	int trst_with_tms = 0;
+	
+	jtag_command_t **last_cmd = jtag_get_last_command_p();
+	
+	if (req_trst == -1)
+		req_trst = jtag_trst;
+	
+	if (req_srst == -1)
+		req_srst = jtag_srst;
+
+	/* Make sure that jtag_reset_config allows the requested reset */
+	/* if SRST pulls TRST, we can't fulfill srst == 1 with trst == 0 */
+	if (((jtag_reset_config & RESET_SRST_PULLS_TRST) && (req_srst == 1)) && (req_trst == 0))
+		return ERROR_JTAG_RESET_WOULD_ASSERT_TRST;
+		
+	/* if TRST pulls SRST, we reset with TAP T-L-R */
+	if (((jtag_reset_config & RESET_TRST_PULLS_SRST) && (req_trst == 1)) && (req_srst == 0))
+	{
+		req_trst = 0;
+		trst_with_tms = 1;
+	}
+	
+	if (req_srst && !(jtag_reset_config & RESET_HAS_SRST))
+		return ERROR_JTAG_RESET_CANT_SRST;
+	
+	if (req_trst && !(jtag_reset_config & RESET_HAS_TRST))
+	{
+		req_trst = 0;
+		trst_with_tms = 1;
+	}
+	
+	/* allocate memory for a new list member */
+	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+	(*last_cmd)->next = NULL;
+	last_comand_pointer = &((*last_cmd)->next);
+	(*last_cmd)->type = JTAG_RESET;
+
+	(*last_cmd)->cmd.reset = cmd_queue_alloc(sizeof(reset_command_t));
+	(*last_cmd)->cmd.reset->trst = req_trst;
+	(*last_cmd)->cmd.reset->srst = req_srst;
+
+	jtag_trst = req_trst;
+	jtag_srst = req_srst;
+
+	if (jtag_srst)
+		jtag_call_event_callbacks(JTAG_SRST_ASSERTED);
+	else
+		jtag_call_event_callbacks(JTAG_SRST_RELEASED);
+	
+	if (trst_with_tms)
+	{
+		last_cmd = &((*last_cmd)->next);
+		
+		/* allocate memory for a new list member */
+		*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+		(*last_cmd)->next = NULL;
+		last_comand_pointer = &((*last_cmd)->next);
+		(*last_cmd)->type = JTAG_STATEMOVE;
+
+		(*last_cmd)->cmd.statemove = cmd_queue_alloc(sizeof(statemove_command_t));
+		(*last_cmd)->cmd.statemove->end_state = TAP_TLR;
+		
+		jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
+		cmd_queue_cur_state = TAP_TLR;
+		cmd_queue_end_state = TAP_TLR;
+		
+		return ERROR_OK;
+	}
+	else
+	{
+		if (jtag_trst)
+		{
+			cmd_queue_cur_state = TAP_TLR;
+			jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
+		}
+	}
+
+	return ERROR_OK;
+}
+
+int jtag_add_end_state(enum tap_state state)
+{
+	jtag_command_t **last_cmd = jtag_get_last_command_p();
+	
+	/* allocate memory for a new list member */
+	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+	(*last_cmd)->next = NULL;
+	last_comand_pointer = &((*last_cmd)->next);
+	(*last_cmd)->type = JTAG_END_STATE;
+
+	(*last_cmd)->cmd.end_state = cmd_queue_alloc(sizeof(end_state_command_t));
+	(*last_cmd)->cmd.end_state->end_state = state;
+
+	if (state != -1)
+		cmd_queue_end_state = state;
+	
+	return ERROR_OK;
+}
+
+int jtag_add_sleep(u32 us)
+{
+	jtag_command_t **last_cmd = jtag_get_last_command_p();
+	
+	/* allocate memory for a new list member */
+	*last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+	(*last_cmd)->next = NULL;
+	last_comand_pointer = &((*last_cmd)->next);
+	(*last_cmd)->type = JTAG_SLEEP;
+
+	(*last_cmd)->cmd.sleep = cmd_queue_alloc(sizeof(sleep_command_t));
+	(*last_cmd)->cmd.sleep->us = us;
+	
+	return ERROR_OK;
+}
+
+int jtag_scan_size(scan_command_t *cmd)
+{
+	int bit_count = 0;
+	int i;
+
+	/* count bits in scan command */
+	for (i=0; i<cmd->num_fields; i++)
+	{
+		bit_count += cmd->fields[i].num_bits;
+	}
+
+	return bit_count;
+}
+
+int jtag_build_buffer(scan_command_t *cmd, u8 **buffer)
+{
+	int bit_count = 0;
+	int i;
+	
+	bit_count = jtag_scan_size(cmd);
+	*buffer = malloc(CEIL(bit_count, 8));
+	
+	bit_count = 0;
+
+	for (i = 0; i < cmd->num_fields; i++)
+	{
+		if (cmd->fields[i].out_value)
+		{
+			char* char_buf = buf_to_char(cmd->fields[i].out_value, cmd->fields[i].num_bits);
+			buf_set_buf(cmd->fields[i].out_value, 0, *buffer, bit_count, cmd->fields[i].num_bits);
+#ifdef _DEBUG_JTAG_IO_
+			DEBUG("fields[%i].out_value: %s", i, char_buf);
+#endif
+			free(char_buf);
+		}
+		
+		bit_count += cmd->fields[i].num_bits;
+	}
+
+	return bit_count;
+
+}
+
+int jtag_read_buffer(u8 *buffer, scan_command_t *cmd)
+{
+	int i;
+	int bit_count = 0;
+	int retval = ERROR_OK;
+
+	for (i=0; i < cmd->num_fields; i++)
+	{
+		/* if neither in_value nor in_check_value are specified we don't have to examine this field */
+		if (cmd->fields[i].in_value || cmd->fields[i].in_check_value)
+		{
+			int num_bits = cmd->fields[i].num_bits;
+
+			if (cmd->fields[i].in_value)
+			{
+				char *char_buf;
+				buf_set_buf(buffer, bit_count, cmd->fields[i].in_value, 0, num_bits);
+				char_buf = buf_to_char(cmd->fields[i].in_value, num_bits);
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("fields[%i].in_value: %s", i, char_buf);
+#endif
+				free(char_buf);
+				if (cmd->fields[i].in_handler)
+				{
+					if (cmd->fields[i].in_handler(cmd->fields[i].in_value, cmd->fields[i].in_handler_priv) != ERROR_OK)
+					{
+						/* TODO: error reporting */
+						WARNING("in_handler reported a failed check");
+						retval = ERROR_JTAG_QUEUE_FAILED;
+					}
+				}
+			}
+
+			if (cmd->fields[i].in_check_value)
+			{
+				u8 *captured = buf_set_buf(buffer, bit_count, malloc(CEIL(num_bits, 8)), 0, num_bits);
+				if ((cmd->fields[i].in_check_mask && buf_cmp_mask(captured, cmd->fields[i].in_check_value, cmd->fields[i].in_check_mask, num_bits))
+					|| (!cmd->fields[i].in_check_mask && buf_cmp(captured, cmd->fields[i].in_check_mask, num_bits)))
+				{
+					char *captured_char = buf_to_char(captured, num_bits);
+					char *in_check_value_char = buf_to_char(cmd->fields[i].in_check_value, num_bits);
+					char *in_check_mask_char = buf_to_char(cmd->fields[i].in_check_mask, num_bits);
+					/* TODO: error reporting */
+					WARNING("value captured during scan didn't pass the requested check: captured: %s check_value: %s check_mask: %s", captured_char, in_check_value_char, in_check_mask_char);
+					retval = ERROR_JTAG_QUEUE_FAILED;
+					free(captured_char);
+					free(in_check_value_char);
+					free(in_check_mask_char);
+				}
+				free(captured);
+			}
+		}
+		bit_count += cmd->fields[i].num_bits;
+	}
+
+	return retval;
+}
+
+enum scan_type jtag_scan_type(scan_command_t *cmd)
+{
+	int i;
+	int type = 0;
+	
+	for (i=0; i < cmd->num_fields; i++)
+	{
+		if (cmd->fields[i].in_check_value || cmd->fields[i].in_value)
+			type |= SCAN_IN;
+		if (cmd->fields[i].out_value)
+			type |= SCAN_OUT;
+	}
+
+	return type;
+}
+
+int jtag_execute_queue(void)
+{
+	int retval;
+
+	retval = jtag->execute_queue();
+	
+	cmd_queue_free();
+
+	jtag_command_queue = NULL;
+	last_comand_pointer = &jtag_command_queue;
+
+	return retval;
+}
+
+int jtag_cancel_queue(void)
+{
+	cmd_queue_free();
+	jtag_command_queue = NULL;
+	last_comand_pointer = &jtag_command_queue;
+
+	return ERROR_OK;
+}
+
+int jtag_reset_callback(enum jtag_event event, void *priv)
+{
+	jtag_device_t *device = priv;
+
+	DEBUG("");
+	
+	if (event == JTAG_TRST_ASSERTED)
+	{
+		buf_set_ones(device->cur_instr, device->ir_length);
+		device->bypass = 1;
+	}
+	
+	return ERROR_OK;
+}
+
+void jtag_sleep(u32 us)
+{
+	usleep(us);
+}
+
+int jtag_validate_chain()
+{
+	jtag_device_t *device = jtag_devices;
+	int total_ir_length = 0;
+	u8 *ir_test = NULL;
+	scan_field_t field;
+	int chain_pos = 0;
+	
+	while (device)
+	{
+		total_ir_length += device->ir_length;
+		device = device->next;
+	}
+	
+	total_ir_length += 2;
+	ir_test = malloc(CEIL(total_ir_length, 8));
+	buf_set_ones(ir_test, total_ir_length);
+	
+	field.device = 0;
+	field.num_bits = total_ir_length;
+	field.out_value = ir_test;
+	field.out_mask = NULL;
+	field.in_value = ir_test;
+	field.in_check_value = NULL;
+	field.in_check_mask = NULL;
+	field.in_handler = NULL;
+	field.in_handler_priv = NULL;
+	
+	jtag_add_plain_ir_scan(1, &field, TAP_TLR);
+	jtag_execute_queue();
+	
+	device = jtag_devices;
+	while (device)
+	{
+		if (buf_get_u32(ir_test, chain_pos, 2) != 0x1)
+		{
+			ERROR("Error validating JTAG scan chain, IR mismatch");
+			exit(-1);
+		}
+		chain_pos += device->ir_length;
+		device = device->next;
+	}
+	
+	if (buf_get_u32(ir_test, chain_pos, 2) != 0x3)
+	{
+		ERROR("Error validating JTAG scan chain, IR mismatch");
+		exit(-1);
+	}
+	
+	free(ir_test);
+	
+	return ERROR_OK;
+}
+
+int jtag_register_commands(struct command_context_s *cmd_ctx)
+{
+	register_command(cmd_ctx, NULL, "interface", handle_interface_command,
+		COMMAND_CONFIG, NULL);
+	register_command(cmd_ctx, NULL, "jtag_speed", handle_jtag_speed_command,
+		COMMAND_ANY, "set jtag speed (if supported) <speed>");
+	register_command(cmd_ctx, NULL, "jtag_device", handle_jtag_device_command,
+		COMMAND_CONFIG, NULL);
+	register_command(cmd_ctx, NULL, "reset_config", handle_reset_config_command,
+		COMMAND_CONFIG, NULL);
+
+	register_command(cmd_ctx, NULL, "scan_chain", handle_scan_chain_command,
+		COMMAND_EXEC, "print current scan chain configuration");
+
+	register_command(cmd_ctx, NULL, "endstate", handle_endstate_command,
+		COMMAND_EXEC, "finish JTAG operations in <tap_state>");
+	register_command(cmd_ctx, NULL, "jtag_reset", handle_jtag_reset_command,
+		COMMAND_EXEC, "toggle reset lines <trst> <srst>");
+	register_command(cmd_ctx, NULL, "runtest", handle_runtest_command,
+		COMMAND_EXEC, "move to Run-Test/Idle, and execute <num_cycles>");
+	register_command(cmd_ctx, NULL, "statemove", handle_statemove_command,
+		COMMAND_EXEC, "move to current endstate or [tap_state]");
+	register_command(cmd_ctx, NULL, "irscan", handle_irscan_command,
+		COMMAND_EXEC, "execute IR scan <device> <instr> [dev2] [instr2] ...");
+	register_command(cmd_ctx, NULL, "drscan", handle_drscan_command,
+		COMMAND_EXEC, "execute DR scan <device> <var> [dev2] [var2] ...");
+
+	register_command(cmd_ctx, NULL, "verify_ircapture", handle_verify_ircapture_command,
+		COMMAND_ANY, "verify value captured during Capture-IR <enable|disable>");
+	return ERROR_OK;
+}
+
+int jtag_init(struct command_context_s *cmd_ctx)
+{
+	int i;
+	
+	DEBUG("");
+
+	if (jtag_speed == -1)
+		jtag_speed = 0;
+	
+	if (jtag_interface && (jtag_interface[0] != 0))
+		/* configuration var 'jtag_interface' is set, and not empty */
+		for (i = 0; jtag_interfaces[i]; i++)
+		{
+			if (strcmp(jtag_interface, jtag_interfaces[i]->name) == 0)
+			{
+				jtag_device_t *device;
+				device = jtag_devices;
+	
+				if (jtag_interfaces[i]->init() != ERROR_OK)
+					return ERROR_JTAG_INIT_FAILED;
+				jtag = jtag_interfaces[i];
+
+				jtag_ir_scan_size = 0;
+				jtag_num_devices = 0;
+				while (device != NULL)
+				{
+					jtag_ir_scan_size += device->ir_length;
+					jtag_num_devices++;
+					device = device->next;
+				}
+				
+				jtag_add_statemove(TAP_TLR);
+				jtag_execute_queue();
+				
+				jtag_validate_chain();
+				
+				return ERROR_OK;
+			}
+		}
+	
+	/* no valid interface was found (i.e. the configuration option,
+	 * didn't match one of the compiled-in interfaces
+	 */
+	ERROR("No valid jtag interface found (%s)", jtag_interface);
+	jtag = NULL;
+	return ERROR_JTAG_INVALID_INTERFACE;
+}
+
+int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	int i;
+	
+	/* only if the configuration var isn't overwritten from cmdline */
+	if (!jtag_interface)
+	{
+		if (args[0] && (args[0][0] != 0))
+		{
+			for (i=0; jtag_interfaces[i]; i++)
+			{
+				if (strcmp(args[0], jtag_interfaces[i]->name) == 0)
+				{
+					if (jtag_interfaces[i]->register_commands(cmd_ctx) != ERROR_OK)
+						exit(-1);
+				
+					jtag_interface = jtag_interfaces[i]->name;
+		
+					return ERROR_OK;
+				}
+			}
+		}
+		
+		/* remember the requested interface name, so we can complain about it later */
+		jtag_interface = strdup(args[0]);
+		DEBUG("'interface' command didn't specify a valid interface");
+	}
+	
+	return ERROR_OK;
+}
+
+int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	jtag_device_t **last_device_p = &jtag_devices;
+
+	if (*last_device_p)
+	{
+		while ((*last_device_p)->next)
+			last_device_p = &((*last_device_p)->next);
+		last_device_p = &((*last_device_p)->next);
+	}
+
+	if (argc < 3)
+		return ERROR_OK;
+
+	*last_device_p = malloc(sizeof(jtag_device_t));
+	(*last_device_p)->ir_length = strtoul(args[0], NULL, 0);
+
+	(*last_device_p)->expected = malloc((*last_device_p)->ir_length);
+	buf_set_u32((*last_device_p)->expected, 0, (*last_device_p)->ir_length, strtoul(args[1], NULL, 0));
+	(*last_device_p)->expected_mask = malloc((*last_device_p)->ir_length);
+	buf_set_u32((*last_device_p)->expected_mask, 0, (*last_device_p)->ir_length, strtoul(args[2], NULL, 0));
+
+	(*last_device_p)->cur_instr = malloc((*last_device_p)->ir_length);
+	(*last_device_p)->bypass = 1;
+	buf_set_ones((*last_device_p)->cur_instr, (*last_device_p)->ir_length);
+	
+	(*last_device_p)->next = NULL;
+	
+	jtag_register_event_callback(jtag_reset_callback, (*last_device_p));
+	
+	jtag_num_devices++;
+
+	return ERROR_OK;
+}
+
+int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	jtag_device_t *device = jtag_devices;
+	int device_count = 0;
+	
+	while (device)
+	{
+		u32 expected, expected_mask, cur_instr;
+		expected = buf_get_u32(device->expected, 0, device->ir_length);
+		expected_mask = buf_get_u32(device->expected_mask, 0, device->ir_length);
+		cur_instr = buf_get_u32(device->cur_instr, 0, device->ir_length);
+		command_print(cmd_ctx, "%i: idcode: 0x%8.8x ir length %i, ir capture 0x%x, ir mask 0x%x, current instruction 0x%x", device_count, device->idcode, device->ir_length, expected, expected_mask, cur_instr);
+		device = device->next;
+		device_count++;
+	}
+
+	return ERROR_OK;
+}
+
+int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	if (argc >= 1)
+	{
+		if (strcmp(args[0], "none") == 0)
+			jtag_reset_config = RESET_NONE;
+		else if (strcmp(args[0], "trst_only") == 0)
+			jtag_reset_config = RESET_HAS_TRST;
+		else if (strcmp(args[0], "srst_only") == 0)
+			jtag_reset_config = RESET_HAS_SRST;
+		else if (strcmp(args[0], "trst_and_srst") == 0)
+			jtag_reset_config = RESET_TRST_AND_SRST;
+		else
+		{
+			ERROR("invalid reset_config argument");
+			exit(-1);
+		}
+	}
+	
+	if (argc >= 2)
+	{
+		if (strcmp(args[1], "srst_pulls_trst") == 0)
+			jtag_reset_config |= RESET_SRST_PULLS_TRST;
+		else if (strcmp(args[1], "trst_pulls_srst") == 0)
+			jtag_reset_config |= RESET_TRST_PULLS_SRST;
+		else if (strcmp(args[1], "combined") == 0)
+			jtag_reset_config |= RESET_SRST_PULLS_TRST | RESET_TRST_PULLS_SRST;
+		else if (strcmp(args[1], "separate") == 0)
+			jtag_reset_config &= ~(RESET_SRST_PULLS_TRST | RESET_TRST_PULLS_SRST);
+		else
+		{
+			ERROR("invalid reset_config argument");
+			exit(-1);
+		}
+	}
+	
+	if (argc >= 3)
+	{
+		if (strcmp(args[2], "trst_open_drain") == 0)
+			jtag_reset_config |= RESET_TRST_OPEN_DRAIN;
+		else if (strcmp(args[2], "trst_push_pull") == 0)
+			jtag_reset_config &= ~RESET_TRST_OPEN_DRAIN;
+		else
+		{
+			ERROR("invalid reset_config argument");
+			exit(-1);
+		}
+	}
+
+	if (argc >= 4)
+	{
+		if (strcmp(args[3], "srst_push_pull") == 0)
+			jtag_reset_config |= RESET_SRST_PUSH_PULL;
+		else if (strcmp(args[3], "srst_open_drain") == 0)
+			jtag_reset_config &= ~RESET_SRST_PUSH_PULL;
+		else
+		{
+			ERROR("invalid reset_config argument");
+			exit(-1);
+		}
+	}
+	
+	return ERROR_OK;
+}
+
+int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	if (argc == 0)
+		command_print(cmd_ctx, "jtag_speed: %i", jtag_speed);
+
+	if (argc > 0)
+	{
+		/* this command can be called during CONFIG, 
+		 * in which case jtag isn't initialized */
+		if (jtag)
+			jtag->speed(strtoul(args[0], NULL, 0));
+		else
+			jtag_speed = strtoul(args[0], NULL, 0);
+	}
+
+	return ERROR_OK;
+}
+
+int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	enum tap_state state;
+
+	if (argc < 1)
+	{
+		command_print(cmd_ctx, "usage: endstate <tap_state>");
+		return ERROR_OK;
+	}
+
+	for (state = 0; state < 16; state++)
+	{
+		if (strcmp(args[0], tap_state_strings[state]) == 0)
+		{
+			jtag_add_end_state(state);
+			jtag_execute_queue();
+		}
+	}
+	
+	return ERROR_OK;
+}
+
+int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	int trst = -1;
+	int srst = -1;
+	char *usage = "usage: jtag_reset <trst> <srst>";
+	int retval;
+	
+	if (argc < 1)
+	{
+		command_print(cmd_ctx, usage);
+		return ERROR_OK;
+	}
+
+	if (args[0][0] == '1')
+		trst = 1;
+	else if (args[0][0] == '0')
+		trst = 0;
+	else
+	{
+		command_print(cmd_ctx, usage);
+		return ERROR_OK;
+	}
+
+	if (args[1][0] == '1')
+		srst = 1;
+	else if (args[1][0] == '0')
+		srst = 0;
+	else
+	{
+		command_print(cmd_ctx, usage);
+		return ERROR_OK;
+	}
+
+	if ((retval = jtag_add_reset(trst, srst)) != ERROR_OK)
+	{
+		switch (retval)
+		{
+			case ERROR_JTAG_RESET_WOULD_ASSERT_TRST:
+				command_print(cmd_ctx, "requested reset would assert trst\nif this is acceptable, use jtag_reset 1 %c", args[1][0]);
+				break;
+			case ERROR_JTAG_RESET_CANT_SRST:
+				command_print(cmd_ctx, "can't assert srst because the current reset_config doesn't support it");
+				break;
+			default:
+				command_print(cmd_ctx, "unknown error");
+		}
+	}
+	jtag_execute_queue();
+
+	return ERROR_OK;
+}
+
+int handle_runtest_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	if (argc < 1)
+	{
+		command_print(cmd_ctx, "usage: runtest <num_cycles>");
+		return ERROR_OK;
+	}
+
+	jtag_add_runtest(strtol(args[0], NULL, 0), -1);
+	jtag_execute_queue();
+
+	return ERROR_OK;
+
+}
+
+int handle_statemove_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	enum tap_state state;
+
+	state = -1;
+	if (argc == 1)
+	{
+		for (state = 0; state < 16; state++)
+		{
+			if (strcmp(args[0], tap_state_strings[state]) == 0)
+			{
+				break;
+			}
+		}
+	}
+
+	jtag_add_statemove(state);
+	jtag_execute_queue();
+
+	return ERROR_OK;
+
+}
+
+int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	int i;
+	scan_field_t *fields;
+	
+	if ((argc < 2) || (argc % 2))
+	{
+		command_print(cmd_ctx, "usage: irscan <device> <instr> [dev2] [instr2] ...");
+		return ERROR_OK;
+	}
+
+	fields = malloc(sizeof(scan_field_t) * argc / 2);
+	
+	for (i = 0; i < argc / 2; i++)
+	{
+		int device = strtoul(args[i*2], NULL, 0);
+		int field_size = jtag_get_device(device)->ir_length;
+		fields[i].device = device;
+		fields[i].out_value = malloc(CEIL(field_size, 8));
+		buf_set_u32(fields[i].out_value, 0, field_size, strtoul(args[i*2+1], NULL, 0));
+		fields[i].out_mask = NULL;
+		fields[i].in_value = NULL;
+		fields[i].in_check_mask = NULL;
+		fields[i].in_handler = NULL;
+		fields[i].in_handler_priv = NULL;
+	}
+
+	jtag_add_ir_scan(argc / 2, fields, -1);
+	jtag_execute_queue();
+
+	for (i = 0; i < argc / 2; i++)
+		free(fields[i].out_value);
+
+	free (fields);
+
+	return ERROR_OK;
+}
+
+int handle_drscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	scan_field_t *fields;
+	int num_fields = 0;
+	int field_count = 0;
+	var_t *var;
+	int i, j;
+	
+	if ((argc < 2) || (argc % 2))
+	{
+		command_print(cmd_ctx, "usage: drscan <device> <var> [dev2] [var2]");
+		return ERROR_OK;
+	}
+
+	for (i = 0; i < argc; i+=2)
+	{
+		var = get_var_by_namenum(args[i+1]);
+		if (var)
+		{
+			num_fields += var->num_fields;
+		}
+		else
+		{
+			command_print(cmd_ctx, "variable %s doesn't exist", args[i+1]);
+			return ERROR_OK;
+		}
+	}
+
+	fields = malloc(sizeof(scan_field_t) * num_fields);
+
+	for (i = 0; i < argc; i+=2)
+	{
+		var = get_var_by_namenum(args[i+1]);
+	
+		for (j = 0; j < var->num_fields; j++)
+		{
+			fields[field_count].device = strtol(args[i], NULL, 0);
+			fields[field_count].num_bits = var->fields[j].num_bits;
+			fields[field_count].out_value = malloc(CEIL(var->fields[j].num_bits, 8));
+			buf_set_u32(fields[field_count].out_value, 0, var->fields[j].num_bits, var->fields[j].value);
+			fields[field_count].out_mask = NULL;
+			fields[field_count].in_value = fields[field_count].out_value;
+			fields[field_count].in_check_mask = NULL;
+			fields[field_count].in_check_value = NULL;
+			fields[field_count].in_handler = field_le_to_host;
+			fields[field_count++].in_handler_priv = &(var->fields[j]);
+		}
+	}
+
+	jtag_add_dr_scan(num_fields, fields, -1);
+	jtag_execute_queue();
+	
+	for (i = 0; i < argc / 2; i++)
+		free(fields[i].out_value);
+
+	free(fields);
+
+	return ERROR_OK;
+}
+
+int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	if (argc == 0)
+	{
+		command_print(cmd_ctx, "verify Capture-IR is %s", (jtag_verify_capture_ir) ? "enabled": "disabled");
+		return ERROR_OK;
+	}
+	
+	if (strcmp(args[0], "enable") == 0)
+	{
+		jtag_verify_capture_ir = 1;
+	}
+	else if (strcmp(args[0], "disable") == 0)
+	{
+		jtag_verify_capture_ir = 0;
+	}
+	
+	return ERROR_OK;
+}
diff --git a/src/jtag/jtag.h b/src/jtag/jtag.h
new file mode 100644
index 00000000..124150ce
--- /dev/null
+++ b/src/jtag/jtag.h
@@ -0,0 +1,270 @@
+/***************************************************************************
+ *   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 JTAG_H
+#define JTAG_H
+
+#include "types.h"
+#include "binarybuffer.h"
+
+#include "command.h"
+
+#if 0
+#define _DEBUG_JTAG_IO_
+#endif
+
+/* Tap States
+ * TLR - Test-Logic-Reset, RTI - Run-Test/Idle, 
+ * SDS - Select-DR-Scan, CD - Capture-DR, SD - Shift-DR, E1D - Exit1-DR,
+ * PD - Pause-DR, E2D - Exit2-DR, UD - Update-DR,
+ * SIS - Select-IR-Scan, CI - Capture-IR, SI - Shift-IR, E1I - Exit1-IR,
+ * PI - Pause-IR, E2I - Exit2-IR, UI - Update-IR 
+ */
+enum tap_state
+{
+	TAP_TLR = 0x0, TAP_RTI = 0x8, 
+	TAP_SDS = 0x1, TAP_CD = 0x2, TAP_SD = 0x3, TAP_E1D = 0x4, 
+	TAP_PD = 0x5, TAP_E2D = 0x6, TAP_UD = 0x7,
+	TAP_SIS = 0x9, TAP_CI = 0xa, TAP_SI = 0xb, TAP_E1I = 0xc,
+	TAP_PI = 0xd, TAP_E2I = 0xe, TAP_UI = 0xf
+};
+
+typedef struct tap_transition_s
+{
+	enum tap_state high;
+	enum tap_state low;
+} tap_transition_t;
+
+extern char* tap_state_strings[16];
+extern int tap_move_map[16];	/* map 16 TAP states to 6 stable states */
+extern u8 tap_move[6][6];		/* value scanned to TMS to move from one of six stable states to another */
+extern tap_transition_t tap_transitions[16];	/* describe the TAP state diagram */
+
+extern enum tap_state end_state;		/* finish DR scans in dr_end_state */
+extern enum tap_state cur_state;		/* current TAP state */
+
+#define TAP_MOVE(from, to) tap_move[tap_move_map[from]][tap_move_map[to]]
+
+typedef struct scan_field_s
+{
+	int device;			/* ordinal device number this instruction refers to */
+	int num_bits;		/* number of bits this field specifies (up to 32) */
+	u8 *out_value;		/* value to be scanned into the device */
+	u8 *out_mask;		/* only masked bits care */
+	u8 *in_value;		/* pointer to a 32-bit memory location to take data scanned out */
+	u8 *in_check_value;		/* used to validate scan results */ 
+	u8 *in_check_mask;		/* check specified bits against check_value */
+	int (*in_handler)(u8 *in_value, void *priv);	/* process received buffer using this handler */
+	void *in_handler_priv;	/* additional information for the in_handler */
+} scan_field_t;
+
+enum scan_type
+{
+	/* IN: from device to host, OUT: from host to device */
+	SCAN_IN = 1, SCAN_OUT = 2, SCAN_IO = 3
+};
+
+typedef struct scan_command_s
+{
+	int ir_scan;	/* instruction/not data scan */
+	int num_fields;		/* number of fields in *fields array */
+	scan_field_t *fields;	/* pointer to an array of data scan fields */
+	enum tap_state end_state;	/* TAP state in which JTAG commands should finish */
+} scan_command_t;
+
+typedef struct statemove_command_s
+{
+	enum tap_state end_state;	/* TAP state in which JTAG commands should finish */
+} statemove_command_t;
+
+typedef struct pathmove_command_s
+{
+	int num_states;				/* number of states in *path */
+	enum tap_state *path;		/* states that have to be passed */
+} pathmove_command_t;
+
+typedef struct runtest_command_s
+{
+	int num_cycles;		/* number of cycles that should be spent in Run-Test/Idle */
+	enum tap_state end_state;	/* TAP state in which JTAG commands should finish */
+} runtest_command_t;
+
+typedef struct reset_command_s
+{
+	int trst;			/* trst/srst 0: deassert, 1: assert, -1: don't change */
+	int srst;
+} reset_command_t;
+
+typedef struct end_state_command_s
+{
+	enum tap_state end_state;	/* TAP state in which JTAG commands should finish */
+} end_state_command_t;
+
+typedef struct sleep_command_s
+{
+	u32 us;		/* number of microseconds to sleep */
+} sleep_command_t;
+
+typedef union jtag_command_container_u
+{
+	scan_command_t *scan;
+	statemove_command_t *statemove;
+	pathmove_command_t *pathmove;
+	runtest_command_t *runtest;
+	reset_command_t *reset;
+	end_state_command_t *end_state;
+	sleep_command_t *sleep;
+} jtag_command_container_t;
+
+enum jtag_command_type
+{
+	JTAG_SCAN = 1,
+	JTAG_STATEMOVE = 2, JTAG_RUNTEST = 3,
+	JTAG_RESET = 4, JTAG_END_STATE = 5,
+	JTAG_PATHMOVE = 6, JTAG_SLEEP = 7
+};
+
+typedef struct jtag_command_s
+{
+	jtag_command_container_t cmd;
+	enum jtag_command_type type;
+	struct jtag_command_s *next;
+} jtag_command_t;
+
+extern jtag_command_t *jtag_command_queue;
+
+typedef struct jtag_device_s
+{
+	int ir_length;		/* size of instruction register */
+	u8 *expected;		/* Capture-IR expected value */
+	u8 *expected_mask;	/* Capture-IR expected mask */
+	u32 idcode;			/* device identification code */
+	u8 *cur_instr;		/* current instruction */
+	int bypass;			/* bypass register selected */
+	struct jtag_device_s *next;
+} jtag_device_t;
+
+extern jtag_device_t *jtag_devices;
+extern int jtag_num_devices;
+extern int jtag_ir_scan_size;
+
+enum reset_line_mode
+{
+	LINE_OPEN_DRAIN = 0x0,
+	LINE_PUSH_PULL = 0x1,
+};
+
+typedef struct jtag_interface_s
+{
+	char* name;
+	
+	/* queued command execution
+	 */
+	int (*execute_queue)(void);
+	
+	/* optional command support 
+	 */
+	int support_statemove;
+	
+	/* interface initalization
+	 */
+	int (*speed)(int speed);
+	int (*register_commands)(struct command_context_s *cmd_ctx);
+	int (*init)(void);
+	int (*quit)(void);
+	
+} jtag_interface_t;
+
+enum jtag_event
+{
+	JTAG_SRST_ASSERTED,
+	JTAG_TRST_ASSERTED,
+	JTAG_SRST_RELEASED,
+	JTAG_TRST_RELEASED,
+};
+
+typedef struct jtag_event_callback_s
+{
+	int (*callback)(enum jtag_event event, void *priv);
+	void *priv;
+	struct jtag_event_callback_s *next;
+} jtag_event_callback_t;
+
+extern jtag_event_callback_t *jtag_event_callbacks;
+
+extern jtag_interface_t *jtag;	/* global pointer to configured JTAG interface */
+extern enum tap_state end_state;
+extern enum tap_state cur_state;
+
+extern char* jtag_interface;
+extern int jtag_speed;
+
+enum reset_types
+{
+	RESET_NONE = 0x0, 
+	RESET_HAS_TRST = 0x1, 
+	RESET_HAS_SRST = 0x2, 
+	RESET_TRST_AND_SRST = 0x3, 
+	RESET_SRST_PULLS_TRST = 0x4,
+	RESET_TRST_PULLS_SRST = 0x8,
+	RESET_TRST_OPEN_DRAIN = 0x10,
+	RESET_SRST_PUSH_PULL = 0x20,
+};
+
+extern enum reset_types jtag_reset_config;
+
+/* JTAG subsystem */
+extern int jtag_init(struct command_context_s *cmd_ctx);
+extern int jtag_register_commands(struct command_context_s *cmd_ctx);
+
+/* JTAG interface */
+extern int jtag_add_ir_scan(int num_fields, scan_field_t *fields, enum tap_state endstate);
+extern int jtag_add_dr_scan(int num_fields, scan_field_t *fields, enum tap_state endstate);
+extern int jtag_add_plain_ir_scan(int num_fields, scan_field_t *fields, enum tap_state endstate);
+extern int jtag_add_plain_dr_scan(int num_fields, scan_field_t *fields, enum tap_state endstate);
+extern int jtag_add_statemove(enum tap_state endstate);
+extern int jtag_add_pathmove(int num_states, enum tap_state *path);
+extern int jtag_add_runtest(int num_cycles, enum tap_state endstate);
+extern int jtag_add_reset(int trst, int srst);
+extern int jtag_add_end_state(enum tap_state endstate);
+extern int jtag_add_sleep(u32 us);
+extern int jtag_execute_queue(void);
+extern int jtag_cancel_queue(void);
+
+/* JTAG support functions */
+extern enum scan_type jtag_scan_type(scan_command_t *cmd);
+extern int jtag_scan_size(scan_command_t *cmd);
+extern int jtag_read_buffer(u8 *buffer, scan_command_t *cmd);
+extern int jtag_build_buffer(scan_command_t *cmd, u8 **buffer);
+extern jtag_device_t* jtag_get_device(int num);
+extern void jtag_sleep(u32 us);
+extern int jtag_call_event_callbacks(enum jtag_event event);
+extern int jtag_register_event_callback(int (*callback)(enum jtag_event event, void *priv), void *priv);
+
+/* error codes
+ * JTAG subsystem uses codes between -100 and -199 */
+
+#define ERROR_JTAG_INIT_FAILED			(-100)
+#define ERROR_JTAG_INVALID_INTERFACE	(-101)
+#define ERROR_JTAG_NOT_IMPLEMENTED		(-102)
+#define ERROR_JTAG_TRST_ASSERTED		(-103)
+#define ERROR_JTAG_QUEUE_FAILED			(-104)
+#define ERROR_JTAG_RESET_WOULD_ASSERT_TRST		(-105)
+#define ERROR_JTAG_RESET_CANT_SRST				(-106)
+#endif /* JTAG_H */
diff --git a/src/jtag/parport.c b/src/jtag/parport.c
new file mode 100644
index 00000000..8265ada8
--- /dev/null
+++ b/src/jtag/parport.c
@@ -0,0 +1,351 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "config.h"
+#include "log.h"
+#include "jtag.h"
+#include "bitbang.h"
+
+/* system includes */
+// -ino: 060521-1036
+#ifdef __FreeBSD__
+#include <sys/types.h>
+#include <machine/sysarch.h>
+#include <machine/cpufunc.h>
+#define ioperm(startport,length,enable)\
+  i386_set_ioperm((startport), (length), (enable))
+#else
+#include <sys/io.h>
+#endif
+
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+
+#if PARPORT_USE_PPDEV == 1
+#include <linux/parport.h>
+#include <linux/ppdev.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#endif
+
+/* parallel port cable description
+ */
+typedef struct cable_s
+{
+	char* name;
+	u8 TDO_MASK;	/* status port bit containing current TDO value */
+	u8 TRST_MASK;	/* data port bit for TRST */
+	u8 TMS_MASK;	/* data port bit for TMS */
+	u8 TCK_MASK;	/* data port bit for TCK */
+	u8 TDI_MASK;	/* data port bit for TDI */
+	u8 SRST_MASK;	/* data port bit for SRST */
+	u8 OUTPUT_INVERT;	/* data port bits that should be inverted */
+	u8 INPUT_INVERT;	/* status port that should be inverted */
+	u8 PORT_INIT;	/* initialize data port with this value */
+} cable_t;
+
+cable_t cables[] = 
+{	
+	/* name					tdo   trst  tms   tck   tdi   srst  o_inv i_inv init */
+	{ "wiggler",			0x80, 0x10, 0x02, 0x04, 0x08, 0x01, 0x01, 0x80, 0x80 },
+	{ "old_amt_wiggler",	0x80, 0x01, 0x02, 0x04, 0x08, 0x10, 0x11, 0x80, 0x80 },
+	{ "chameleon",			0x80, 0x00, 0x04, 0x01, 0x02, 0x00, 0x00, 0x80, 0x00 },
+	{ "dlc5",				0x10, 0x00, 0x04, 0x02, 0x01, 0x00, 0x00, 0x00, 0x10 },
+	{ "triton",				0x80, 0x08, 0x04, 0x01, 0x02, 0x00, 0x00, 0x80, 0x00 },
+	{ NULL,					0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }
+};
+
+/* configuration */
+char* parport_cable;
+unsigned long parport_port;
+
+/* interface variables
+ */
+static cable_t* cable;
+static u8 dataport_value = 0x0;
+
+#if PARPORT_USE_PPDEV == 1
+static int device_handle;
+#else
+static unsigned long dataport;
+static unsigned long statusport;
+#endif
+
+/* low level command set
+ */
+int parport_read(void);
+void parport_write(int tck, int tms, int tdi);
+void parport_reset(int trst, int srst);
+
+int parport_speed(int speed);
+int parport_register_commands(struct command_context_s *cmd_ctx);
+int parport_init(void);
+int parport_quit(void);
+
+/* interface commands */
+int parport_handle_parport_port_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int parport_handle_parport_cable_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+jtag_interface_t parport_interface = 
+{
+	.name = "parport",
+	
+	.execute_queue = bitbang_execute_queue,
+
+	.support_statemove = 0,
+
+	.speed = parport_speed,	
+	.register_commands = parport_register_commands,
+	.init = parport_init,
+	.quit = parport_quit,
+};
+
+bitbang_interface_t parport_bitbang =
+{
+	.read = parport_read,
+	.write = parport_write,
+	.reset = parport_reset
+};
+
+int parport_read(void)
+{
+	int data = 0;
+	
+#if PARPORT_USE_PPDEV == 1
+	ioctl(device_handle, PPRSTATUS, & data);
+#else
+	data = inb(statusport);
+#endif
+
+	if ((data ^ cable->INPUT_INVERT) & cable->TDO_MASK)
+		return 1;
+	else
+		return 0;
+}
+
+void parport_write(int tck, int tms, int tdi)
+{
+	u8 output;
+	int i = jtag_speed + 1;
+	
+	if (tck)
+		dataport_value |= cable->TCK_MASK;
+	else
+		dataport_value &= ~cable->TCK_MASK;
+	
+	if (tms)
+		dataport_value |= cable->TMS_MASK;
+	else
+		dataport_value &= ~cable->TMS_MASK;
+	
+	if (tdi)
+		dataport_value |= cable->TDI_MASK;
+	else
+		dataport_value &= ~cable->TDI_MASK;
+		
+	output = dataport_value ^ cable->OUTPUT_INVERT;
+
+	while (i-- > 0)
+#if PARPORT_USE_PPDEV == 1
+		ioctl(device_handle, PPWDATA, &output);
+#else
+#ifdef __FreeBSD__
+	outb(dataport, output);
+#else
+	outb(output, dataport);
+#endif
+#endif
+}
+
+/* (1) assert or (0) deassert reset lines */
+void parport_reset(int trst, int srst)
+{
+	u8 output;
+	DEBUG("trst: %i, srst: %i", trst, srst);
+
+	if (trst == 0)
+		dataport_value |= cable->TRST_MASK;
+	else if (trst == 1)
+		dataport_value &= ~cable->TRST_MASK;
+
+	if (srst == 0)
+		dataport_value |= cable->SRST_MASK;
+	else if (srst == 1)
+		dataport_value &= ~cable->SRST_MASK;
+	
+	output = dataport_value ^ cable->OUTPUT_INVERT;
+	
+#if PARPORT_USE_PPDEV == 1
+	ioctl(device_handle, PPWDATA, &output);
+#else
+#ifdef __FreeBSD__
+	outb(dataport, output);
+#else
+	outb(output, dataport);
+#endif
+#endif
+
+}
+
+int parport_speed(int speed)
+{
+	jtag_speed = speed;
+	
+	return ERROR_OK;
+}
+
+int parport_register_commands(struct command_context_s *cmd_ctx)
+{
+	register_command(cmd_ctx, NULL, "parport_port", parport_handle_parport_port_command,
+		COMMAND_CONFIG, NULL);
+	register_command(cmd_ctx, NULL, "parport_cable", parport_handle_parport_cable_command,
+		COMMAND_CONFIG, NULL);
+
+	return ERROR_OK;
+}
+
+int parport_init(void)
+{
+	cable_t *cur_cable;
+#if PARPORT_USE_PPDEV == 1
+	char buffer[256];
+	int i = 0;
+#endif
+	
+	cur_cable = cables;
+	
+	if ((parport_cable == NULL) || (parport_cable[0] == 0))
+	{
+		parport_cable = "wiggler";
+		WARNING("No parport cable specified, using default 'wiggler'");
+	}
+	
+	while (cur_cable->name)
+	{
+		if (strcmp(cur_cable->name, parport_cable) == 0)
+		{
+			cable = cur_cable;
+			break;
+		}
+		cur_cable++;
+	}
+
+	if (!cable)
+	{
+		ERROR("No matching cable found for %s", parport_cable);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+	
+	dataport_value = cable->PORT_INIT;
+	
+#if PARPORT_USE_PPDEV == 1
+	if (device_handle>0)
+	{
+		ERROR("device is already opened");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	snprintf(buffer, 256, "/dev/parport%d", parport_port);
+	device_handle = open(buffer, O_WRONLY);
+	
+	if (device_handle<0)
+	{
+		ERROR("cannot open device. check it exists and that user read and write rights are set");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	i=ioctl(device_handle, PPCLAIM);
+	if (i<0)
+	{
+		ERROR("cannot claim device");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	i = PARPORT_MODE_COMPAT;
+	i= ioctl(device_handle, PPSETMODE, & i);
+	if (i<0)
+	{
+		ERROR(" cannot set compatible mode to device");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	i = IEEE1284_MODE_COMPAT;
+	i = ioctl(device_handle, PPNEGOT, & i);
+	if (i<0)
+	{
+		ERROR("cannot set compatible 1284 mode to device");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+#else
+	if (parport_port == 0)
+	{
+		parport_port = 0x378;
+		WARNING("No parport port specified, using default '0x378' (LPT1)");
+	}
+	
+	dataport = parport_port;
+	statusport = parport_port + 1;
+		
+	if (ioperm(dataport, 3, 1) != 0) {
+		ERROR("missing privileges for direct i/o");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+#endif
+	
+	parport_reset(0, 0);
+	parport_write(0, 0, 0);
+
+	bitbang_interface = &parport_bitbang;	
+
+	return ERROR_OK;
+}
+
+int parport_quit(void)
+{
+
+	return ERROR_OK;
+}
+
+int parport_handle_parport_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 (parport_port == 0)
+		parport_port = strtoul(args[0], NULL, 0);
+
+	return ERROR_OK;
+}
+
+int parport_handle_parport_cable_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	if (argc == 0)
+		return ERROR_OK;
+
+	/* only if the cable name wasn't overwritten by cmdline */
+	if (parport_cable == 0)
+	{
+		parport_cable = malloc(strlen(args[0]) + sizeof(char));
+		strcpy(parport_cable, args[0]);
+	}
+
+	return ERROR_OK;
+}
diff --git a/src/openocd.c b/src/openocd.c
new file mode 100644
index 00000000..71b111a0
--- /dev/null
+++ b/src/openocd.c
@@ -0,0 +1,113 @@
+/***************************************************************************
+ *   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 "log.h"
+#include "types.h"
+#include "jtag.h"
+#include "configuration.h"
+#include "interpreter.h"
+#include "xsvf.h"
+#include "target.h"
+#include "flash.h"
+
+#include "command.h"
+#include "server.h"
+#include "telnet_server.h"
+#include "gdb_server.h"
+
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/poll.h>
+#include <strings.h>
+#include <netinet/in.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <signal.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+
+int main(int argc, char *argv[])
+{
+	/* initialize commandline interface */
+	command_context_t *cmd_ctx, *cfg_cmd_ctx;
+	cmd_ctx = command_init();
+	
+	/* register subsystem commands */
+	server_register_commands(cmd_ctx);
+	telnet_register_commands(cmd_ctx);
+	gdb_register_commands(cmd_ctx);
+	log_register_commands(cmd_ctx);
+	jtag_register_commands(cmd_ctx);
+	interpreter_register_commands(cmd_ctx);
+	xsvf_register_commands(cmd_ctx);
+	target_register_commands(cmd_ctx);
+	flash_register_commands(cmd_ctx);
+	
+	if (log_init(cmd_ctx) != ERROR_OK)
+		return EXIT_FAILURE;
+	DEBUG("log init complete");
+	
+	INFO("Open On-Chip Debugger (Revision 63)");
+
+	cfg_cmd_ctx = copy_command_context(cmd_ctx);
+	cfg_cmd_ctx->mode = COMMAND_CONFIG;
+	command_set_output_handler(cfg_cmd_ctx, configuration_output_handler, NULL);
+	
+	if (parse_cmdline_args(cfg_cmd_ctx, argc, argv) != ERROR_OK)
+		return EXIT_FAILURE;
+
+	if (parse_config_file(cfg_cmd_ctx) != ERROR_OK)
+		return EXIT_FAILURE;
+	
+	command_done(cfg_cmd_ctx);
+
+	if (jtag_init(cmd_ctx) != ERROR_OK)
+		return EXIT_FAILURE;
+	DEBUG("jtag init complete");
+
+	if (target_init(cmd_ctx) != ERROR_OK)
+		return EXIT_FAILURE;
+	DEBUG("target init complete");
+
+	if (flash_init(cmd_ctx) != ERROR_OK)
+		return EXIT_FAILURE;
+	DEBUG("flash init complete");
+
+	/* initialize tcp server */
+	server_init();
+	
+	/* initialize telnet subsystem */
+	telnet_init("Open On-Chip Debugger");
+	gdb_init();
+
+	/* handle network connections */
+	server_loop(cmd_ctx);
+	
+	/* free commandline interface */
+	command_done(cmd_ctx);
+	
+	return EXIT_SUCCESS;
+}
diff --git a/src/server/Makefile.am b/src/server/Makefile.am
new file mode 100644
index 00000000..2397a7f0
--- /dev/null
+++ b/src/server/Makefile.am
@@ -0,0 +1,5 @@
+INCLUDES = -I$(top_srcdir)/src/helper -I$(top_srcdir)/src/target $(all_includes)
+METASOURCES = AUTO
+noinst_LIBRARIES = libserver.a
+noinst_HEADERS = server.h telnet_server.h gdb_server.h
+libserver_a_SOURCES = server.c telnet_server.c gdb_server.c
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
new file mode 100644
index 00000000..125206dc
--- /dev/null
+++ b/src/server/gdb_server.c
@@ -0,0 +1,1108 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "gdb_server.h"
+
+#include "server.h"
+#include "log.h"
+#include "binarybuffer.h"
+#include "breakpoints.h"
+
+#define __USE_GNU
+#include <string.h>
+#include <errno.h>
+#include <unistd.h>
+#include <stdlib.h>
+
+// -ino: 060521-1116
+#ifdef __FreeBSD__
+#include <stdio.h>
+char * strndup(char * str, int n) {
+  unsigned char * tmp = malloc((size_t)n+1);
+  if (! tmp) perror("gdb_server malloc failed");
+  if (strlcpy(tmp, str, n) > n) perror("gdb_server strndup:  too long");
+  return tmp;
+}
+#endif
+#if 0
+#define _DEBUG_GDB_IO_
+#endif
+
+static unsigned short gdb_port;
+
+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;
+	char *debug_buffer;
+	
+	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;
+	}
+
+	while ((gdb_con->buf_cnt = read(connection->fd, gdb_con->buffer, GDB_BUFFER_SIZE)) <= 0)
+	{
+		if (gdb_con->buf_cnt == 0)
+			return ERROR_SERVER_REMOTE_CLOSED;
+		
+		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));
+				exit(-1);
+		}
+	}
+	
+	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);
+
+	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_put_packet(connection_t *connection, char *buffer, int len)
+{
+	int i;
+	unsigned char my_checksum = 0;
+	char checksum[3];
+	char *debug_buffer;
+	int reply;
+	int retval;
+	gdb_connection_t *gdb_con = connection->priv;
+
+	for (i = 0; i < len; i++)
+		my_checksum += buffer[i];
+	
+	while (1)
+	{
+			
+		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);
+		
+		write(connection->fd, "$", 1);
+		if (len > 0)
+			write(connection->fd, buffer, len);
+		write(connection->fd, "#", 1);
+	
+		snprintf(checksum, 3, "%2.2x", my_checksum);
+	
+		write(connection->fd, checksum, 2);
+
+		if ((retval = gdb_get_char(connection, &reply)) != ERROR_OK)
+			return retval;
+
+		if (reply == '+')
+			break;
+		else if (reply == '-')
+			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 == '-')
+				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;
+		}
+	}
+	
+	return ERROR_OK;
+}
+
+int gdb_get_packet(connection_t *connection, char *buffer, int *len)
+{
+	int character;
+	int count = 0;
+	int retval;
+	int first_char = 0;
+	int packet_type;
+	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;
+
+			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;
+			
+		do
+		{
+			if ((retval = gdb_get_char(connection, &character)) != ERROR_OK)
+				return retval;
+			
+			if( !first_char ) {
+				packet_type = character;
+				first_char = 1;	
+			}
+			
+			if( packet_type == 'X' )
+			{
+				switch (character)
+				{
+					case '#':
+						break;
+					case 0x7d:
+						/* data transmitted in binary mode (X packet)
+						* uses 0x7d as escape character */
+						my_checksum += character & 0xff;
+						gdb_get_char(connection, &character);
+						my_checksum += character & 0xff;
+						buffer[count++] = (character ^ 0x20) & 0xff;
+						if (count > *len)
+						{
+							ERROR("packet buffer too small");
+							return ERROR_GDB_BUFFER_TOO_SMALL;
+						}
+						break;
+					default:
+						buffer[count++] = character & 0xff;
+						my_checksum += character & 0xff;
+						if (count > *len)
+						{
+							ERROR("packet buffer too small");
+							return ERROR_GDB_BUFFER_TOO_SMALL;
+						}
+						break;
+				}
+			}
+			else
+			{
+				switch (character)
+				{
+					case '#':
+						break;
+					case 0x3:
+						gdb_con->ctrl_c = 1;
+						break;
+					default:
+						buffer[count++] = character & 0xff;
+						my_checksum += character & 0xff;
+						if (count > *len)
+						{
+							ERROR("packet buffer too small");
+							return ERROR_GDB_BUFFER_TOO_SMALL;
+						}
+						break;
+				}
+			}
+		} while (character != '#');
+
+		*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))
+		{
+			write (connection->fd, "+", 1);
+			break;
+		}
+
+		WARNING("checksum error, requesting retransmission");
+		write(connection->fd, "-", 1);
+	}
+
+	return ERROR_OK;
+}
+
+int gdb_output(struct command_context_s *context, char* line)
+{
+	connection_t *connection = context->output_handler_priv;
+	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_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:
+			if (gdb_connection->frontend_state == TARGET_RUNNING)
+			{
+				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_RESUMED:
+			if (gdb_connection->frontend_state == TARGET_HALTED)
+			{
+				gdb_connection->frontend_state = TARGET_RUNNING;
+			}
+			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;
+	
+	/* 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 when trying to halt target");
+		exit(-1);
+	}
+	
+	while (gdb_service->target->state != TARGET_HALTED)
+	{
+		gdb_service->target->type->poll(gdb_service->target);
+	}
+	
+	/* remove the initial ACK from the incoming buffer */
+	if ((retval = gdb_get_char(connection, &initial_ack)) != ERROR_OK)
+		return retval;
+		
+	return ERROR_OK;
+}
+
+int gdb_connection_closed(connection_t *connection)
+{
+	if (connection->priv)
+		free(connection->priv);
+	else
+		ERROR("BUG: connection->priv == NULL");
+	
+	target_unregister_event_callback(gdb_target_callback_event_handler, connection);
+
+	return ERROR_OK;
+}
+
+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;
+}
+
+void 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;
+	
+	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 requested registers, but we're not halted");
+				exit(-1);
+			default:
+				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++)
+	{
+		int j;
+		char *hex_buf = buf_to_char(reg_list[i]->value, reg_list[i]->size);
+		DEBUG("hex_buf: %s", hex_buf);
+		for (j = CEIL(reg_list[i]->size, 8) * 2; j > 0; j -= 2)
+		{
+			*reg_packet_p++ = hex_buf[j - 2];
+			*reg_packet_p++ = hex_buf[j - 1];
+		}
+		free(hex_buf);
+	}
+
+	reg_packet_p = strndup(reg_packet, CEIL(reg_packet_size, 8) * 2);
+	DEBUG("reg_packet: %s", reg_packet_p);
+	free(reg_packet_p);
+	
+	gdb_put_packet(connection, reg_packet, CEIL(reg_packet_size, 8) * 2);
+	free(reg_packet);
+	
+}
+
+void 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;
+	
+	DEBUG("");
+
+	/* skip command character */
+	packet++;
+	packet_size--;
+
+	if (packet_size % 2)
+	{
+		WARNING("GDB set_registers packet with uneven characters received");
+		return;
+	}
+
+	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");
+				exit(-1);
+			default:
+				ERROR("BUG: unexpected error returned by get_gdb_reg_list()");
+				exit(-1);
+		}
+	}
+
+	packet_p = packet;
+	for (i = 0; i < reg_list_size; i++)
+	{
+		char_to_buf(packet, CEIL(reg_list[i]->size, 8) * 2, reg_list[i]->value, reg_list[i]->size);
+		reg_list[i]->dirty = 1;
+	}
+
+	gdb_put_packet(connection, "OK", 2);
+}
+
+void gdb_get_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
+{
+	char *hex_buf;
+	char *reg_packet;
+	char *reg_packet_p;
+	int reg_num = strtoul(packet + 1, NULL, 16);
+	reg_t **reg_list;
+	int reg_list_size;
+	int retval;
+	int i;
+	
+	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 requested registers, but we're not halted");
+				exit(-1);
+			default:
+				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);
+	}
+
+	hex_buf = buf_to_char(reg_list[reg_num]->value, reg_list[reg_num]->size);
+	reg_packet = reg_packet_p = malloc(CEIL(reg_list[reg_num]->size, 8) * 2);
+	
+	for (i = CEIL(reg_list[reg_num]->size, 8) * 2; i > 0; i -= 2)
+	{
+		*reg_packet_p++ = hex_buf[i - 2];
+		*reg_packet_p++ = hex_buf[i - 1];
+	}
+	
+	gdb_put_packet(connection, reg_packet, CEIL(reg_list[reg_num]->size, 8) * 2);
+	
+	free(reg_packet);
+	free(hex_buf);
+	
+}
+
+void gdb_set_register_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
+{
+	char *separator;
+	int reg_num = strtoul(packet + 1, &separator, 16);
+	reg_t **reg_list;
+	int reg_list_size;
+	int retval;
+
+	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 requested registers, but we're not halted");
+				exit(-1);
+			default:
+				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);
+	}
+
+	if (*separator != '=')
+	{
+		ERROR("GDB set register packet, but no '=' following the register number");
+		exit(-1);
+	}
+	
+	char_to_buf(separator + 1, CEIL(reg_list[reg_num]->size, 8) * 2, reg_list[reg_num]->value, reg_list[reg_num]->size);
+	reg_list[reg_num]->dirty = 1;
+
+	gdb_put_packet(connection, "OK", 2);
+
+}
+
+void 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 i;
+
+	/* skip command character */
+	packet++;
+
+	addr = strtoul(packet, &separator, 16);
+	
+	if (*separator != ',')
+		return;
+
+	len = strtoul(separator+1, NULL, 16);
+
+	buffer = malloc(len);
+
+	DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
+
+	switch (len)
+	{
+		case 4:
+			if ((addr % 4) == 0)
+				target->type->read_memory(target, addr, 4, 1, buffer);
+			else
+				target->type->read_memory(target, addr, 1, len, buffer);
+			break;
+		case 2:
+			if ((addr % 2) == 0)
+				target->type->read_memory(target, addr, 2, 1, buffer);
+			else
+				target->type->read_memory(target, addr, 1, len, buffer);
+			break;
+		default:
+			if (((addr % 4) == 0) && ((len % 4) == 0))
+				target->type->read_memory(target, addr, 4, len / 4, buffer);
+			else
+				target->type->read_memory(target, addr, 1, len, buffer);
+	}
+
+	hex_buffer = malloc(len * 2 + 1);
+	
+	for (i=0; i<len; i++)
+		snprintf(hex_buffer + 2*i, 3, "%2.2x", buffer[i]);
+
+	gdb_put_packet(connection, hex_buffer, len * 2);
+	
+	free(hex_buffer);
+	free(buffer);
+}
+
+void 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;
+
+	/* skip command character */
+	packet++;
+
+	addr = strtoul(packet, &separator, 16);
+	
+	if (*separator != ',')
+		return;
+
+	len = strtoul(separator+1, &separator, 16);
+
+	if (*(separator++) != ':')
+		return;
+
+	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;
+	}
+
+	switch (len)
+	{
+		/* handle sized writes */
+		case 4:
+			if ((addr % 4) == 0)
+				target->type->write_memory(target, addr, 4, 1, buffer);
+			else
+				target->type->write_memory(target, addr, 1, len, buffer);
+			break;
+		case 2:
+			if ((addr % 2) == 0)
+				target->type->write_memory(target, addr, 2, 1, buffer);
+			else
+				target->type->write_memory(target, addr, 1, len, buffer);
+			break;
+		case 3:
+		case 1:
+			target->type->write_memory(target, addr, 1, len, buffer);
+			break;
+		/* handle bulk writes */
+		default:
+			target_write_buffer(target, addr, len, buffer);
+			break;
+	}
+
+	gdb_put_packet(connection, "OK", 2);
+	
+	free(buffer);
+}
+
+void gdb_write_memory_binary_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
+{
+	char *separator;
+	u32 addr = 0;
+	u32 len = 0;
+
+	u8 *buffer;
+
+	/* skip command character */
+	packet++;
+
+	addr = strtoul(packet, &separator, 16);
+	
+	if (*separator != ',')
+		return;
+
+	len = strtoul(separator+1, &separator, 16);
+
+	if (*(separator++) != ':')
+		return;
+
+	if( len ) {
+		
+		buffer = malloc(len);
+	
+		DEBUG("addr: 0x%8.8x, len: 0x%8.8x", addr, len);
+		
+		memcpy( buffer, separator, len );
+	
+		switch (len)
+		{
+			case 4:
+				if ((addr % 4) == 0)
+					target->type->write_memory(target, addr, 4, 1, buffer);
+				else
+					target->type->write_memory(target, addr, 1, len, buffer);
+				break;
+			case 2:
+				if ((addr % 2) == 0)
+					target->type->write_memory(target, addr, 2, 1, buffer);
+				else
+					target->type->write_memory(target, addr, 1, len, buffer);
+				break;
+			case 3:
+			case 1:
+				target->type->write_memory(target, addr, 1, len, buffer);
+				break;
+			default:
+				target_write_buffer(target, addr, len, buffer);
+				break;
+		}
+		
+		free(buffer);
+	}
+
+	gdb_put_packet(connection, "OK", 2);
+}
+
+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)
+	{
+		u32 address = 0;
+		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 */
+	}
+}
+
+void gdb_breakpoint_watchpoint_packet(connection_t *connection, target_t *target, char *packet, int packet_size)
+{
+	int type;
+	enum breakpoint_type bp_type;
+	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 != ',')
+		return;
+
+	address = strtoul(separator+1, &separator, 16);
+
+	if (*separator != ',')
+		return;
+
+	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 == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
+					{
+						gdb_put_packet(connection, "E00", 3);
+						break;
+					}
+				}
+			}
+			else
+			{
+				breakpoint_remove(target, address);
+			}
+			gdb_put_packet(connection, "OK", 2);
+			break;
+		case 2:
+		case 3:
+		case 4:
+		{
+			if (packet[0] == 'Z')
+				watchpoint_add(target, address, size, type-2, 0, 0xffffffffu);
+			else
+				watchpoint_remove(target, address);
+			gdb_put_packet(connection, "OK", 2);
+			break;
+		}
+		default:
+			break;
+	}
+
+}
+
+void gdb_query_packet(connection_t *connection, char *packet, int packet_size)
+{
+	command_context_t *cmd_ctx = connection->cmd_ctx;
+	gdb_service_t *gdb_service = connection->service->priv;
+	target_t *target = gdb_service->target;
+
+	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;
+			command_run_line(cmd_ctx, cmd);
+			free(cmd);
+		}
+		gdb_put_packet(connection, "OK", 2);
+		return;
+	}
+	
+	gdb_put_packet(connection, "", 0);
+}
+
+int gdb_input(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;
+
+	/* drain input buffer */
+	do
+	{
+		packet_size = GDB_BUFFER_SIZE-1;
+		if ((retval = gdb_get_packet(connection, packet, &packet_size)) != ERROR_OK)
+		{
+			switch (retval)
+			{
+				case ERROR_GDB_BUFFER_TOO_SMALL:
+					ERROR("BUG: buffer supplied for gdb packet was too small");
+					exit(-1);
+				case ERROR_SERVER_REMOTE_CLOSED:
+					return ERROR_SERVER_REMOTE_CLOSED;
+				default:
+					ERROR("unexpected error");
+					exit(-1);
+			}
+		}
+		
+		/* terminate with zero */
+		packet[packet_size] = 0;
+		
+		DEBUG("recevied packet: '%s'", packet);
+		
+		if (packet_size > 0)
+		{
+			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':
+					gdb_query_packet(connection, packet, packet_size);
+					break;
+				case 'g':
+					gdb_get_registers_packet(connection, target, packet, packet_size);
+					break;
+				case 'G':
+					gdb_set_registers_packet(connection, target, packet, packet_size);
+					break;
+				case 'p':
+					gdb_get_register_packet(connection, target, packet, packet_size);
+					break;
+				case 'P':
+					gdb_set_register_packet(connection, target, packet, packet_size);
+					break;
+				case 'm':
+					gdb_read_memory_packet(connection, target, packet, packet_size);
+					break;
+				case 'M':
+					gdb_write_memory_packet(connection, target, packet, packet_size);
+					break;
+				case 'z':
+				case 'Z':
+					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':
+					gdb_step_continue_packet(connection, target, packet, packet_size);
+					break;
+				case 'D':
+					target->type->resume(target, 1, 0, 1, 0);
+					gdb_put_packet(connection, "OK", 2);
+					break;
+				case 'X':
+					gdb_write_memory_binary_packet(connection, target, packet, packet_size);
+					break;
+				case 'k':
+					gdb_put_packet(connection, "OK", 2);
+					return ERROR_SERVER_REMOTE_CLOSED;
+				default:
+					/* ignore unkown packets */
+					DEBUG("ignoring 0x%2.2x packet", packet[0]);
+					gdb_put_packet(connection, NULL, 0);
+					break;
+			}
+		}
+				
+		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_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);
+		
+		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 gdb_register_commands(command_context_t *command_context)
+{
+	register_command(command_context, NULL, "gdb_port", handle_gdb_port_command,
+					 COMMAND_CONFIG, "");
+	
+	return ERROR_OK;
+}
diff --git a/src/server/gdb_server.h b/src/server/gdb_server.h
new file mode 100644
index 00000000..860b29ca
--- /dev/null
+++ b/src/server/gdb_server.h
@@ -0,0 +1,47 @@
+/***************************************************************************
+ *   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 GDB_SERVER_H
+#define GDB_SERVER_H
+
+#include "target.h"
+#include "server.h"
+
+#define GDB_BUFFER_SIZE	2048
+
+typedef struct gdb_connection_s
+{
+	char buffer[GDB_BUFFER_SIZE];
+	char *buf_p;
+	int buf_cnt;
+	int ctrl_c;
+	enum target_state frontend_state;
+} gdb_connection_t;
+
+typedef struct gdb_service_s
+{
+	struct target_s *target;
+} gdb_service_t;
+
+extern int gdb_init();
+extern int gdb_register_commands(command_context_t *command_context);
+
+#define ERROR_GDB_BUFFER_TOO_SMALL (-800)
+
+#endif /* GDB_SERVER_H */
diff --git a/src/server/server.c b/src/server/server.c
new file mode 100644
index 00000000..a034da79
--- /dev/null
+++ b/src/server/server.c
@@ -0,0 +1,368 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "server.h"
+
+#include "log.h"
+#include "telnet_server.h"
+#include "target.h"
+
+#include <command.h>
+#include <string.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <fcntl.h>
+#include <signal.h>
+
+service_t *services = NULL;
+
+/* shutdown_openocd == 1: exit the main event loop, and quit the debugger */
+static int shutdown_openocd = 0;
+int handle_shutdown_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+int add_connection(service_t *service, command_context_t *cmd_ctx)
+{
+	unsigned int address_size;
+	connection_t *c, *p;
+	int retval;
+	
+	c = malloc(sizeof(connection_t));
+	c->fd = -1;
+	memset(&c->sin, 0, sizeof(c->sin));
+	c->cmd_ctx = copy_command_context(cmd_ctx);
+	c->service = service;
+	c->input_pending = 0;
+	c->priv = NULL;
+	c->next = NULL;
+
+	address_size = sizeof(c->sin);
+	c->fd = accept(service->fd, (struct sockaddr *)&service->sin, &address_size);
+				
+	if ((retval = service->new_connection(c)) == ERROR_OK)
+	{
+		INFO("accepted '%s' connection from %i", service->name, c->sin.sin_port);
+	}
+	else
+	{
+		close(c->fd);
+		INFO("attempted '%s' connection rejected", service->name);
+		free(c);
+	}
+	
+	if (service->connections)
+	{
+		for (p = service->connections; p && p->next; p = p->next);
+		if (p)
+			p->next = c;
+	}
+	else
+	{
+		service->connections = c;
+	}
+	
+	service->max_connections--;
+	
+	return ERROR_OK;
+}
+
+int remove_connection(service_t *service, connection_t *connection)
+{
+	connection_t *c, *p = NULL;
+
+	/* find connection */
+	for (c = service->connections; c; c = c->next)
+	{
+		if (c->fd == connection->fd)
+		{
+			/* unlink connection */
+			if (p)
+				p->next = c->next;
+			else
+				service->connections = c->next;
+			
+			service->connection_closed(c);
+			close(c->fd);
+			
+			command_done(c->cmd_ctx);
+			
+			/* delete connection */
+			free(c);
+			
+			service->max_connections++;
+			break;
+		}
+		
+		/* remember the last connection for unlinking */
+		p = c;
+	}
+	
+	return ERROR_OK;
+}
+
+int add_service(char *name, enum connection_type type, unsigned short port, int max_connections, new_connection_handler_t new_connection_handler, input_handler_t input_handler, connection_closed_handler_t connection_closed_handler, void *priv)
+{
+	service_t *c, *p;
+	int so_reuseaddr_option = 1;
+	int oldopts;
+	
+	c = malloc(sizeof(service_t));
+	
+	c->name = strdup(name);
+	c->type = type;
+	c->port = port;
+	c->max_connections = max_connections;
+	c->fd = -1;
+	c->connections = NULL;
+	c->new_connection = new_connection_handler;
+	c->input = input_handler;
+	c->connection_closed = connection_closed_handler;
+	c->priv = priv;
+	c->next = NULL;
+	
+	if ((c->fd = socket(AF_INET, SOCK_STREAM, 0)) == -1)
+	{
+		ERROR("error creating socket: %s", strerror(errno));
+		exit(-1);
+	}
+	
+	setsockopt(c->fd, SOL_SOCKET, SO_REUSEADDR, &so_reuseaddr_option, sizeof(int));
+	
+	oldopts = fcntl(c->fd, F_GETFL, 0);
+	fcntl(c->fd, F_SETFL, oldopts | O_NONBLOCK);
+	
+	memset(&c->sin, 0, sizeof(c->sin));
+	c->sin.sin_family = AF_INET;
+	c->sin.sin_addr.s_addr = INADDR_ANY;
+	c->sin.sin_port = htons(port);
+	
+	if (bind(c->fd, (struct sockaddr *)&c->sin, sizeof(c->sin)) == -1)
+	{
+		ERROR("couldn't bind to socket: %s", strerror(errno));
+		exit(-1);
+	}
+	
+	if (listen(c->fd, 1) == -1)
+	{
+		ERROR("couldn't listen on socket: %s", strerror(errno));
+		exit(-1);
+	}
+	
+	if (services)
+	{
+		for (p = services; p && p->next; p = p->next);
+		if (p)
+			p->next = c;
+	}
+	else
+	{
+		services = c;
+	}
+	
+	return ERROR_OK;
+}
+
+int remove_service(unsigned short port)
+{
+	service_t *c, *p = NULL;
+
+	/* find service */
+	for (c = services; c; c = c->next)
+	{
+		if (c->port == port)
+		{
+			/* unlink service */
+			if (p)
+				p->next = c->next;
+			else
+				services = c->next;
+			
+			if (c->name)
+				free(c->name);
+			
+			/* delete service */
+			free(c);
+		}
+		
+		/* remember the last service for unlinking */
+		p = c;
+	}
+	
+	return ERROR_OK;
+}
+
+int server_loop(command_context_t *command_context)
+{
+	service_t *service;
+
+	/* used in select() */
+	fd_set read_fds;
+	struct timeval tv;
+	int fd_max;
+	
+	/* used in accept() */
+	int retval;
+	
+	if (signal(SIGPIPE, SIG_IGN) == SIG_ERR)
+		ERROR("couldn't set SIGPIPE to SIG_IGN");
+	
+	/* do regular tasks after at most 10ms */
+	tv.tv_sec = 0;
+	tv.tv_usec = 10000;
+	
+	while(!shutdown_openocd)
+	{
+		/* monitor sockets for acitvity */
+		fd_max = 0;
+		FD_ZERO(&read_fds);
+
+		/* add service and connection fds to read_fds */
+		for (service = services; service; service = service->next)
+		{
+			if (service->fd != -1)
+			{
+				/* listen for new connections */
+				FD_SET(service->fd, &read_fds);
+
+				if (service->fd > fd_max)
+					fd_max = service->fd;
+			}
+			
+			if (service->connections)
+			{
+				connection_t *c;
+				
+				for (c = service->connections; c; c = c->next)
+				{
+					/* check for activity on the connection */
+					FD_SET(c->fd, &read_fds);
+					if (c->fd > fd_max)
+						fd_max = c->fd;
+				}
+			}
+		}
+		
+		/* add STDIN to read_fds */
+		FD_SET(fileno(stdin), &read_fds);
+
+		if ((retval = select(fd_max + 1, &read_fds, NULL, NULL, &tv)) == -1)
+		{
+			if (errno == EINTR)
+				FD_ZERO(&read_fds);
+			else
+			{
+				ERROR("error during select: %s", strerror(errno));
+				exit(-1);
+			}
+		}
+		
+		target_call_timer_callbacks();
+
+		if (retval == 0)
+		{
+			/* do regular tasks after at most 100ms */
+			tv.tv_sec = 0;
+			tv.tv_usec = 10000;
+				
+#if 0
+			if (shutdown_openocd)
+				return ERROR_COMMAND_CLOSE_CONNECTION;
+			
+			handle_target();
+#endif
+		}
+		
+		for (service = services; service; service = service->next)
+		{
+			/* handle new connections on listeners */
+			if ((service->fd != -1) 
+				&& (FD_ISSET(service->fd, &read_fds))) 
+			{
+				if (service->max_connections > 0)
+					add_connection(service, command_context);
+				else
+				{
+					struct sockaddr_in sin;
+					unsigned int address_size = sizeof(sin);
+					int tmp_fd;
+					tmp_fd = accept(service->fd, (struct sockaddr *)&service->sin, &address_size);
+					close(tmp_fd);
+					INFO("rejected '%s' connection, no more connections allowed", service->name);
+				}
+			}
+			
+			/* handle activity on connections */
+			if (service->connections)
+			{
+				connection_t *c;
+				
+				for (c = service->connections; c;)
+				{
+					if ((FD_ISSET(c->fd, &read_fds)) || c->input_pending)
+					{
+						if (service->input(c) != ERROR_OK)
+						{
+							connection_t *next = c->next;
+							remove_connection(service, c);
+							INFO("dropped '%s' connection", service->name);
+							c = next;
+							continue;
+						}
+					}
+					c = c->next;
+				}
+			}
+		}
+		
+		if (FD_ISSET(fileno(stdin), &read_fds))
+		{
+			if (getc(stdin) == 'x')
+			{
+				shutdown_openocd = 1;
+			}
+		}
+	}
+	
+	return ERROR_OK;
+}
+
+int server_init()
+{
+	
+	return ERROR_OK;
+}
+
+int server_register_commands(command_context_t *context)
+{
+	register_command(context, NULL, "shutdown", handle_shutdown_command,
+					 COMMAND_ANY, "shut the server down");
+	
+	return ERROR_OK;
+}
+
+/* tell the server we want to shut down */
+int handle_shutdown_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	shutdown_openocd = 1;
+
+	return ERROR_COMMAND_CLOSE_CONNECTION;
+}
+
diff --git a/src/server/server.h b/src/server/server.h
new file mode 100644
index 00000000..625c364e
--- /dev/null
+++ b/src/server/server.h
@@ -0,0 +1,75 @@
+/***************************************************************************
+ *   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 SERVER_H
+#define SERVER_H
+
+#include "command.h"
+#include "binarybuffer.h"
+
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+
+enum connection_type
+{
+	CONNECTION_GDB,
+	CONNECTION_TELNET,
+};
+
+typedef struct connection_s
+{
+	int fd;
+	struct sockaddr_in sin;
+	command_context_t *cmd_ctx;
+	struct service_s *service;
+	int input_pending;
+	void *priv;
+	struct connection_s *next;
+} connection_t;
+
+typedef int (*new_connection_handler_t)(connection_t *connection);
+typedef int (*input_handler_t)(connection_t *connection);
+typedef int (*connection_closed_handler_t)(connection_t *connection);
+
+typedef struct service_s
+{
+	char *name;
+	enum connection_type type;
+	unsigned short port;
+	int fd;
+	struct sockaddr_in sin;
+	int max_connections;
+	connection_t *connections;
+	new_connection_handler_t new_connection;
+	input_handler_t input;
+	connection_closed_handler_t connection_closed;
+	void *priv;
+	struct service_s *next;
+} service_t;
+
+extern int add_service(char *name, enum connection_type type, unsigned short port, int max_connections, new_connection_handler_t new_connection_handler, input_handler_t input_handler, connection_closed_handler_t connection_closed_handler, void *priv);
+extern int server_init();
+extern int server_loop(command_context_t *command_context);
+extern int server_register_commands(command_context_t *context);
+
+#define ERROR_SERVER_REMOTE_CLOSED	(-400)
+#define ERROR_CONNECTION_REJECTED	(-401)
+
+#endif /* SERVER_H */
diff --git a/src/server/telnet_server.c b/src/server/telnet_server.c
new file mode 100644
index 00000000..d7933293
--- /dev/null
+++ b/src/server/telnet_server.c
@@ -0,0 +1,570 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "telnet_server.h"
+
+#include "server.h"
+#include "log.h"
+#include "command.h"
+#include "target.h"
+
+#include <stdlib.h>
+#include <unistd.h>
+#include <errno.h>
+#include <string.h>
+#include <ctype.h>
+
+static unsigned short telnet_port = 0;
+
+int handle_exit_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_telnet_port_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+static char *negotiate =
+		"\xFF\xFB\x03"		/* IAC WILL Suppress Go Ahead */
+		"\xFF\xFB\x01"		/* IAC WILL Echo */
+		"\xFF\xFD\x03"		/* IAC DO Suppress Go Ahead */
+		"\xFF\xFE\x01";		/* IAC DON'T Echo */
+		
+#define CTRL(c) (c - '@')
+	
+void telnet_prompt(connection_t *connection)
+{
+	telnet_connection_t *t_con = connection->priv;
+
+	write(connection->fd, t_con->prompt, strlen(t_con->prompt));
+}
+
+int telnet_output(struct command_context_s *cmd_ctx, char* line)
+{
+	connection_t *connection = cmd_ctx->output_handler_priv;
+	
+	write(connection->fd, line, strlen(line));
+	write(connection->fd, "\r\n\0", 3);
+	
+	return ERROR_OK;
+}
+
+int telnet_target_callback_event_handler(struct target_s *target, enum target_event event, void *priv)
+{
+	struct command_context_s *cmd_ctx = priv;
+	connection_t *connection = cmd_ctx->output_handler_priv;
+	telnet_connection_t *t_con = connection->priv;
+	char buffer[512];
+	
+	switch (event)
+	{
+		case TARGET_EVENT_HALTED:
+			command_print(cmd_ctx, "Target %i halted", get_num_by_target(target));
+			target->type->arch_state(target, buffer, 512);
+			buffer[511] = 0;
+			command_print(cmd_ctx, "%s", buffer);
+			telnet_prompt(connection);
+			t_con->surpress_prompt = 1;
+			break;
+		case TARGET_EVENT_RESUMED:
+			command_print(cmd_ctx, "Target %i resumed", get_num_by_target(target));
+			telnet_prompt(connection);
+			t_con->surpress_prompt = 1;
+			break;
+		default:
+			break;
+	}
+
+	return ERROR_OK;
+}
+
+int telnet_new_connection(connection_t *connection)
+{
+	telnet_connection_t *telnet_connection = malloc(sizeof(telnet_connection_t));
+	telnet_service_t *telnet_service = connection->service->priv;
+	int i;
+	
+	connection->priv = telnet_connection;
+	
+	/* initialize telnet connection information */
+	telnet_connection->line_size = 0;
+	telnet_connection->line_cursor = 0;
+	telnet_connection->option_size = 0;
+	telnet_connection->prompt = strdup("> ");
+	telnet_connection->surpress_prompt = 0;
+	telnet_connection->state = TELNET_STATE_DATA;
+	
+	/* output goes through telnet connection */
+	command_set_output_handler(connection->cmd_ctx, telnet_output, connection);
+	
+	/* negotiate telnet options */
+	write(connection->fd, negotiate, strlen(negotiate));
+	
+	/* print connection banner */
+	if (telnet_service->banner)
+	{
+		write(connection->fd, telnet_service->banner, strlen(telnet_service->banner));
+		write(connection->fd, "\r\n\0", 3);
+	}
+	
+	telnet_prompt(connection);
+	
+	/* initialize history */
+	for (i = 0; i < TELNET_LINE_HISTORY_SIZE; i++)
+	{
+		telnet_connection->history[i] = NULL;
+	}
+	telnet_connection->next_history = 0;
+	telnet_connection->current_history = 0;
+	
+	target_register_event_callback(telnet_target_callback_event_handler, connection->cmd_ctx);
+	
+	return ERROR_OK;
+}
+
+void telnet_clear_line(connection_t *connection, telnet_connection_t *t_con)
+{
+	/* move to end of line */
+	if (t_con->line_cursor < t_con->line_size)
+	{
+		write(connection->fd, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor);
+	}
+							
+	/* backspace, overwrite with space, backspace */
+	while (t_con->line_size > 0)
+	{
+		write(connection->fd, "\b \b", 3);
+		t_con->line_size--;
+	}
+	t_con->line_cursor = 0;
+}
+
+int telnet_input(connection_t *connection)
+{
+	int bytes_read;
+	char buffer[TELNET_BUFFER_SIZE];
+	char *buf_p;
+	telnet_connection_t *t_con = connection->priv;
+	command_context_t *command_context = connection->cmd_ctx;
+	
+	bytes_read = read(connection->fd, buffer, TELNET_BUFFER_SIZE);
+	
+	if (bytes_read == 0)
+		return ERROR_SERVER_REMOTE_CLOSED;
+	else if (bytes_read == -1)
+	{
+		ERROR("error during read: %s", strerror(errno));
+		return ERROR_SERVER_REMOTE_CLOSED;
+	}
+	
+	buf_p = buffer;
+	while (bytes_read)
+	{
+		switch (t_con->state)
+		{
+			case TELNET_STATE_DATA:
+				if (*buf_p == '\xff')
+				{
+					t_con->state = TELNET_STATE_IAC;
+				}
+				else
+				{
+					if (isprint(*buf_p)) /* printable character */
+					{
+						write(connection->fd, buf_p, 1);
+						if (t_con->line_cursor == t_con->line_size)
+						{
+							t_con->line[t_con->line_size++] = *buf_p;
+							t_con->line_cursor++;
+						}
+						else
+						{
+							int i;
+							memmove(t_con->line + t_con->line_cursor + 1, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor);
+							t_con->line[t_con->line_cursor++] = *buf_p;
+							t_con->line_size++;
+							write(connection->fd, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor);
+							for (i = t_con->line_cursor; i < t_con->line_size; i++)
+							{
+								write(connection->fd, "\b", 1);
+							}
+						}
+					}
+					else /* non-printable */
+					{
+						if (*buf_p == 0x1b) /* escape */
+						{
+							t_con->state = TELNET_STATE_ESCAPE;
+							t_con->last_escape = '\x00';
+						}
+						else if ((*buf_p == 0xd) || (*buf_p == 0xa)) /* CR/LF */
+						{
+							int retval;
+							
+							/* skip over combinations with CR/LF + NUL */
+							if (((*(buf_p + 1) == 0xa) || (*(buf_p + 1) == 0xd)) && (bytes_read > 1))
+							{
+								buf_p++;
+								bytes_read--;
+							}
+							if ((*(buf_p + 1) == 0) && (bytes_read > 1))
+							{
+								buf_p++;
+								bytes_read--;
+							}
+							t_con->line[t_con->line_size] = 0;
+							
+							write(connection->fd, "\r\n\x00", 3);
+							
+							if (strcmp(t_con->line, "history") == 0)
+							{
+								int i;
+								for (i = 0; i < TELNET_LINE_HISTORY_SIZE; i++)
+								{
+									if (t_con->history[i])
+									{
+										write(connection->fd, t_con->history[i], strlen(t_con->history[i]));
+										write(connection->fd, "\r\n\x00", 3);
+									}
+								}
+								telnet_prompt(connection);
+								t_con->line_size = 0;
+								t_con->line_cursor = 0;
+								continue;
+							}
+							
+							/* we're running a command, so we need a prompt
+							 * if the output handler is called, this gets set again */
+							t_con->surpress_prompt = 0;
+							if ((retval = command_run_line(command_context, t_con->line)) != ERROR_OK)
+							{
+								if (retval == ERROR_COMMAND_CLOSE_CONNECTION)
+								{
+									return ERROR_SERVER_REMOTE_CLOSED;
+								}
+							}
+							
+							/* if the history slot is already taken, free it */
+							if (t_con->history[t_con->next_history])
+							{
+								free(t_con->history[t_con->next_history]);
+							}
+							
+							/* add line to history */
+							t_con->history[t_con->next_history++] = strdup(t_con->line);
+							
+							/* current history line starts at the new entry */
+							t_con->current_history = t_con->next_history;
+							
+							if (t_con->history[t_con->current_history])
+							{
+								free(t_con->history[t_con->current_history]);
+							}
+							t_con->history[t_con->current_history] = strdup("");
+							
+							/* wrap history at TELNET_LINE_HISTORY_SIZE */
+							if (t_con->next_history > TELNET_LINE_HISTORY_SIZE - 1)
+								t_con->next_history = 0;
+							
+							if (!t_con->surpress_prompt)
+							{
+								telnet_prompt(connection);
+							}
+							else
+							{
+								t_con->surpress_prompt = 0;
+							}
+							
+							t_con->line_size = 0;
+							t_con->line_cursor = 0;
+						}
+						else if ((*buf_p == 0x7f) || (*buf_p == 0x8)) /* delete character */
+						{
+							if (t_con->line_cursor > 0)
+							{
+								if (t_con->line_cursor != t_con->line_size)
+								{
+									int i;
+									write(connection->fd, "\b", 1);
+									t_con->line_cursor--;
+									t_con->line_size--;
+									memmove(t_con->line + t_con->line_cursor, t_con->line + t_con->line_cursor + 1, t_con->line_size - t_con->line_cursor);
+									
+									write(connection->fd, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor);
+									write(connection->fd, " \b", 2);
+									for (i = t_con->line_cursor; i < t_con->line_size; i++)
+									{
+										write(connection->fd, "\b", 1);
+									}
+								}
+								else
+								{
+									t_con->line_size--;
+									t_con->line_cursor--;
+									/* back space: move the 'printer' head one char back, overwrite with space, move back again */
+									write(connection->fd, "\b \b", 3);
+								}
+							}
+						}
+						else if (*buf_p == 0x15) /* clear line */
+						{
+							telnet_clear_line(connection, t_con);
+						}
+						else if (*buf_p == CTRL('B')) /* cursor left */
+						{
+							if (t_con->line_cursor > 0)
+							{
+								write(connection->fd, "\b", 1);
+								t_con->line_cursor--;
+							}
+							t_con->state = TELNET_STATE_DATA;
+						}
+						else if (*buf_p == CTRL('F')) /* cursor right */
+						{
+							if (t_con->line_cursor < t_con->line_size)
+							{
+								write(connection->fd, t_con->line + t_con->line_cursor++, 1);
+							}
+							t_con->state = TELNET_STATE_DATA;
+						}
+						else
+						{
+							DEBUG("unhandled nonprintable: %2.2x", *buf_p);
+						}
+					}
+				}
+				break;
+			case TELNET_STATE_IAC:
+				switch (*buf_p)
+				{
+					case '\xfe':
+						t_con->state = TELNET_STATE_DONT;
+						break;
+					case '\xfd':
+						t_con->state = TELNET_STATE_DO;
+						break;
+					case '\xfc':
+						t_con->state = TELNET_STATE_WONT;
+						break;
+					case '\xfb':
+						t_con->state = TELNET_STATE_WILL;
+						break;
+				}
+				break;
+			case TELNET_STATE_SB:
+				break;
+			case TELNET_STATE_SE:
+				break;
+			case TELNET_STATE_WILL:
+			case TELNET_STATE_WONT:
+			case TELNET_STATE_DO:
+			case TELNET_STATE_DONT:
+				t_con->state = TELNET_STATE_DATA;
+				break;
+			case TELNET_STATE_ESCAPE:
+				if (t_con->last_escape == '[')
+				{
+					if (*buf_p == 'D') /* cursor left */
+					{
+						if (t_con->line_cursor > 0)
+						{
+							write(connection->fd, "\b", 1);
+							t_con->line_cursor--;
+						}
+						t_con->state = TELNET_STATE_DATA;
+					}
+					else if (*buf_p == 'C') /* cursor right */
+					{
+						if (t_con->line_cursor < t_con->line_size)
+						{
+							write(connection->fd, t_con->line + t_con->line_cursor++, 1);
+						}
+						t_con->state = TELNET_STATE_DATA;
+					}
+					else if (*buf_p == 'A') /* cursor up */
+					{
+						int last_history = (t_con->current_history - 1 >= 0) ? t_con->current_history - 1 : 127;
+						if (t_con->history[last_history])
+						{
+							telnet_clear_line(connection, t_con);
+							t_con->line_size = strlen(t_con->history[last_history]);
+							t_con->line_cursor = t_con->line_size;
+							memcpy(t_con->line, t_con->history[last_history], t_con->line_size + 1);
+							write(connection->fd, t_con->line, t_con->line_size);
+							t_con->current_history = last_history;
+						}
+						t_con->state = TELNET_STATE_DATA;
+					}
+					else if (*buf_p == 'B') /* cursor down */
+					{
+						int next_history = (t_con->current_history + 1 < 128) ? t_con->current_history + 1 : 0;
+						if (t_con->history[next_history])
+						{
+							telnet_clear_line(connection, t_con);
+							t_con->line_size = strlen(t_con->history[next_history]);
+							t_con->line_cursor = t_con->line_size;
+							memcpy(t_con->line, t_con->history[next_history], t_con->line_size + 1);
+							write(connection->fd, t_con->line, t_con->line_size);
+							t_con->current_history = next_history;
+						}
+						t_con->state = TELNET_STATE_DATA;
+					}
+					else if (*buf_p == '3')
+					{
+						t_con->last_escape = *buf_p;
+					}
+					else
+					{
+						t_con->state = TELNET_STATE_DATA;
+					}
+				}
+				else if (t_con->last_escape == '3')
+				{
+					/* Remove character */
+					if (*buf_p == '~')
+					{
+						if (t_con->line_cursor < t_con->line_size)
+						{
+							int i;
+							t_con->line_size--;
+							/* remove char from line buffer */
+							memmove(t_con->line + t_con->line_cursor, t_con->line + t_con->line_cursor + 1, t_con->line_size - t_con->line_cursor);
+							
+							/* print remainder of buffer */
+							write(connection->fd, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor);
+							/* overwrite last char with whitespace */
+							write(connection->fd, " \b", 2);
+							
+							/* move back to cursor position*/
+							for (i = t_con->line_cursor; i < t_con->line_size; i++)
+							{
+								write(connection->fd, "\b", 1);
+							}
+						}
+							
+						t_con->state = TELNET_STATE_DATA;
+					}
+					else
+					{
+						t_con->state = TELNET_STATE_DATA;
+					}
+				}
+				else if (t_con->last_escape == '\x00')
+				{
+					if (*buf_p == '[')
+					{
+						t_con->last_escape = *buf_p;
+					}
+					else
+					{
+						t_con->state = TELNET_STATE_DATA;
+					}
+				}
+				else
+				{
+					ERROR("BUG: unexpected value in t_con->last_escape");
+					t_con->state = TELNET_STATE_DATA;
+				}
+				
+				break;
+			default:
+				ERROR("unknown telnet state");
+				exit(-1);
+		}
+
+		bytes_read--;
+		buf_p++;
+	}
+	
+	return ERROR_OK;
+}
+
+int telnet_connection_closed(connection_t *connection)
+{
+	telnet_connection_t *t_con = connection->priv;
+	int i;
+	
+	if (t_con->prompt)
+		free(t_con->prompt);
+	
+	for (i = 0; i < TELNET_LINE_HISTORY_SIZE; i++)
+	{
+		if (t_con->history[i])
+			free(t_con->history[i]);
+	}
+	
+	if (connection->priv)
+		free(connection->priv);
+	else
+		ERROR("BUG: connection->priv == NULL");
+	
+	target_unregister_event_callback(telnet_target_callback_event_handler, connection->cmd_ctx);
+
+	return ERROR_OK;
+}
+
+int telnet_set_prompt(connection_t *connection, char *prompt)
+{
+	telnet_connection_t *t_con = connection->priv;
+
+	t_con->prompt = strdup(prompt);
+	
+	return ERROR_OK;
+}
+
+int telnet_init(char *banner)
+{
+	telnet_service_t *telnet_service = malloc(sizeof(telnet_service_t));
+	
+	if (telnet_port == 0)
+	{
+		WARNING("no telnet port specified, using default port 4444");
+		telnet_port = 4444;
+	}
+	
+	telnet_service->banner = banner;
+	
+	add_service("telnet", CONNECTION_TELNET, telnet_port, 1, telnet_new_connection, telnet_input, telnet_connection_closed, telnet_service);
+	
+	return ERROR_OK;
+}
+
+int telnet_register_commands(command_context_t *command_context)
+{
+	register_command(command_context, NULL, "exit", handle_exit_command,
+					 COMMAND_EXEC, "exit telnet session");
+	
+	register_command(command_context, NULL, "telnet_port", handle_telnet_port_command,
+					 COMMAND_CONFIG, "");
+	
+	return ERROR_OK;
+}
+
+/* daemon configuration command telnet_port */
+int handle_telnet_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 (telnet_port == 0)
+		telnet_port = strtoul(args[0], NULL, 0);
+
+	return ERROR_OK;
+}
+
+int handle_exit_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	return ERROR_COMMAND_CLOSE_CONNECTION;
+}
diff --git a/src/server/telnet_server.h b/src/server/telnet_server.h
new file mode 100644
index 00000000..d6ca0e86
--- /dev/null
+++ b/src/server/telnet_server.h
@@ -0,0 +1,68 @@
+/***************************************************************************
+ *   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 TELNET_SERVER_H
+#define TELNET_SERVER_H
+
+#include "server.h"
+
+#define TELNET_BUFFER_SIZE (1024)
+
+#define TELNET_OPTION_MAX_SIZE (128)
+#define TELNET_LINE_HISTORY_SIZE (128)
+#define TELNET_LINE_MAX_SIZE (256)
+
+enum telnet_states
+{
+	TELNET_STATE_DATA,
+	TELNET_STATE_IAC,
+	TELNET_STATE_SB,
+	TELNET_STATE_SE,
+	TELNET_STATE_WILL,
+	TELNET_STATE_WONT,
+	TELNET_STATE_DO,
+	TELNET_STATE_DONT,
+	TELNET_STATE_ESCAPE,
+};
+
+typedef struct telnet_connection_s
+{
+	char *prompt;
+	int surpress_prompt;
+	enum telnet_states state;
+	char line[TELNET_LINE_MAX_SIZE];
+	int line_size;
+	int line_cursor;
+	char option[TELNET_OPTION_MAX_SIZE];
+	int option_size;
+	char last_escape;
+	char *history[TELNET_LINE_HISTORY_SIZE];
+	int next_history;
+	int current_history;
+} telnet_connection_t;
+
+typedef struct telnet_service_s
+{
+	char *banner;
+} telnet_service_t;
+
+extern int telnet_init(char *banner);
+extern int telnet_register_commands(command_context_t *command_context);
+
+#endif /* TELNET_SERVER_H */
diff --git a/src/target/Makefile.am b/src/target/Makefile.am
new file mode 100644
index 00000000..7e26762b
--- /dev/null
+++ b/src/target/Makefile.am
@@ -0,0 +1,7 @@
+INCLUDES = -I$(top_srcdir)/src/gdb -I$(top_srcdir)/src/helper 	-I$(top_srcdir)/src/jtag -I$(top_srcdir)/src/xsvf $(all_includes)
+METASOURCES = AUTO
+noinst_LIBRARIES = libtarget.a
+libtarget_a_SOURCES = target.c register.c breakpoints.c armv4_5.c embeddedice.c etm.c arm7tdmi.c arm9tdmi.c \
+	arm_jtag.c arm7_9_common.c algorithm.c arm920t.c arm720t.c armv4_5_mmu.c armv4_5_cache.c
+noinst_HEADERS = target.h register.h armv4_5.h embeddedice.h etm.h arm7tdmi.h arm9tdmi.h \
+		arm_jtag.h arm7_9_common.h arm920t.h arm720t.h armv4_5_mmu.h armv4_5_cache.h breakpoints.h algorithm.h
diff --git a/src/target/algorithm.c b/src/target/algorithm.c
new file mode 100644
index 00000000..fdebfc58
--- /dev/null
+++ b/src/target/algorithm.c
@@ -0,0 +1,54 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "config.h"
+#include "algorithm.h"
+
+#include "log.h"
+#include "configuration.h"
+#include "binarybuffer.h"
+
+#include <stdlib.h>
+
+
+void init_mem_param(mem_param_t *param, u32 address, u32 size, enum param_direction direction)
+{
+	param->address = address;
+	param->size = size;
+	param->value = malloc(size);
+	param->direction = direction;
+}
+
+void destroy_mem_param(mem_param_t *param)
+{
+	free(param->value);
+}
+
+void init_reg_param(reg_param_t *param, char *reg_name, u32 size, enum param_direction direction)
+{
+	param->reg_name = reg_name;
+	param->size = size;
+	param->value = malloc(CEIL(size, 8));
+	param->direction = direction;
+}
+
+void destroy_reg_param(reg_param_t *param)
+{
+	free(param->value);
+}
diff --git a/src/target/algorithm.h b/src/target/algorithm.h
new file mode 100644
index 00000000..e248ba5a
--- /dev/null
+++ b/src/target/algorithm.h
@@ -0,0 +1,53 @@
+/***************************************************************************
+ *   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 ALGORITHM_H
+#define ALGORITHM_H
+
+#include "types.h"
+
+enum param_direction
+{
+	PARAM_IN,
+	PARAM_OUT,
+	PARAM_IN_OUT
+};
+
+typedef struct mem_param_s
+{
+	u32 address;
+	u32 size;
+	u8 *value;
+	enum param_direction direction;
+} mem_param_t; 
+
+typedef struct reg_param_s
+{
+	char *reg_name;
+	u32 size;
+	u8 *value;
+	enum param_direction direction;
+} reg_param_t;
+
+extern void init_mem_param(mem_param_t *param, u32 address, u32 size, enum param_direction direction);
+extern void destroy_mem_param(mem_param_t *param);
+extern void init_reg_param(reg_param_t *param, char *reg_name, u32 size, enum param_direction direction);
+extern void destroy_reg_param(reg_param_t *param);
+
+#endif /* ALGORITHM_H */
diff --git a/src/target/arm720t.c b/src/target/arm720t.c
new file mode 100644
index 00000000..68ea3d1c
--- /dev/null
+++ b/src/target/arm720t.c
@@ -0,0 +1,625 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "config.h"
+
+#include "arm720t.h"
+#include "jtag.h"
+#include "log.h"
+
+#include <stdlib.h>
+#include <string.h>
+
+#if 1
+#define _DEBUG_INSTRUCTION_EXECUTION_
+#endif
+
+/* cli handling */
+int arm720t_register_commands(struct command_context_s *cmd_ctx);
+
+int arm720t_handle_cp15_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int arm720t_handle_virt2phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int arm720t_handle_md_phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int arm720t_handle_mw_phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+/* forward declarations */
+int arm720t_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target);
+int arm720t_init_target(struct command_context_s *cmd_ctx, struct target_s *target);
+int arm720t_quit();
+int arm720t_arch_state(struct target_s *target, char *buf, int buf_size);
+int arm720t_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);
+int arm720t_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);
+int arm720t_soft_reset_halt(struct target_s *target);
+
+target_type_t arm720t_target =
+{
+	.name = "arm720t",
+
+	.poll = arm7_9_poll,
+	.arch_state = arm720t_arch_state,
+
+	.halt = arm7_9_halt,
+	.resume = arm7_9_resume,
+	.step = arm7_9_step,
+
+	.assert_reset = arm7_9_assert_reset,
+	.deassert_reset = arm7_9_deassert_reset,
+	.soft_reset_halt = arm720t_soft_reset_halt,
+	
+	.get_gdb_reg_list = armv4_5_get_gdb_reg_list,
+
+	.read_memory = arm720t_read_memory,
+	.write_memory = arm720t_write_memory,
+	.bulk_write_memory = arm7_9_bulk_write_memory,
+
+	.run_algorithm = armv4_5_run_algorithm,
+
+	.add_breakpoint = arm7_9_add_breakpoint,
+	.remove_breakpoint = arm7_9_remove_breakpoint,
+	.add_watchpoint = arm7_9_add_watchpoint,
+	.remove_watchpoint = arm7_9_remove_watchpoint,
+
+	.register_commands = arm720t_register_commands,
+	.target_command = arm720t_target_command,
+	.init_target = arm720t_init_target,
+	.quit = arm720t_quit
+};
+
+int arm720t_scan_cp15(target_t *target, u32 out, u32 *in, int instruction, int clock)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+	scan_field_t fields[2];
+	u8 out_buf[4];
+	u8 instruction_buf = instruction;
+	
+	out = flip_u32(out, 32);
+	buf_set_u32(out_buf, 0, 32, out);
+	
+	jtag_add_end_state(TAP_PD);
+	arm_jtag_scann(jtag_info, 0xf);
+	arm_jtag_set_instr(jtag_info, jtag_info->intest_instr);
+		
+	fields[0].device = jtag_info->chain_pos;
+	fields[0].num_bits = 1;
+	fields[0].out_value = &instruction_buf;
+	fields[0].out_mask = NULL;
+	fields[0].in_value = NULL;
+	fields[0].in_check_value = NULL;
+	fields[0].in_check_mask = NULL;
+	fields[0].in_handler = NULL;
+	fields[0].in_handler_priv = NULL;
+
+	fields[1].device = jtag_info->chain_pos;
+	fields[1].num_bits = 32;
+	fields[1].out_value = out_buf;
+	fields[1].out_mask = NULL;
+	if (in)
+	{
+		fields[1].in_value = (u8*)in;
+		fields[1].in_handler = arm_jtag_buf_to_u32_flip;
+		fields[1].in_handler_priv = in;
+	} else
+	{
+		fields[1].in_value = NULL;
+		fields[1].in_handler = NULL;
+		fields[1].in_handler_priv = NULL;
+	}
+	fields[1].in_check_value = NULL;
+	fields[1].in_check_mask = NULL;
+	
+	jtag_add_dr_scan(2, fields, -1);
+
+	if (clock)
+		jtag_add_runtest(0, -1);
+
+#ifdef _DEBUG_INSTRUCTION_EXECUTION_
+	jtag_execute_queue();
+
+	if (in)
+		DEBUG("out: %8.8x, in: %8.8x, instruction: %i, clock: %i", out, *in, instruction, clock);
+	else
+		DEBUG("out: %8.8x, instruction: %i, clock: %i", out, instruction, clock);
+#else
+		DEBUG("out: %8.8x, instruction: %i, clock: %i", in, out, instruction, clock);
+#endif
+
+	return ERROR_OK;
+}
+
+int arm720t_read_cp15(target_t *target, u32 opcode, u32 *value)
+{
+	/* fetch CP15 opcode */
+	arm720t_scan_cp15(target, opcode, NULL, 1, 1);
+	/* "DECODE" stage */
+	arm720t_scan_cp15(target, ARMV4_5_NOP, NULL, 1, 1);
+	/* "EXECUTE" stage (1) */
+	arm720t_scan_cp15(target, ARMV4_5_NOP, NULL, 1, 0);
+	arm720t_scan_cp15(target, 0x0, NULL, 0, 1);
+	/* "EXECUTE" stage (2) */
+	arm720t_scan_cp15(target, 0x0, NULL, 0, 1);
+	/* "EXECUTE" stage (3), CDATA is read */
+	arm720t_scan_cp15(target, ARMV4_5_NOP, value, 1, 1);
+	
+	return ERROR_OK;
+}
+
+int arm720t_write_cp15(target_t *target, u32 opcode, u32 value)
+{
+	/* fetch CP15 opcode */
+	arm720t_scan_cp15(target, opcode, NULL, 1, 1);
+	/* "DECODE" stage */
+	arm720t_scan_cp15(target, ARMV4_5_NOP, NULL, 1, 1);
+	/* "EXECUTE" stage (1) */
+	arm720t_scan_cp15(target, ARMV4_5_NOP, NULL, 1, 0);
+	arm720t_scan_cp15(target, 0x0, NULL, 0, 1);
+	/* "EXECUTE" stage (2) */
+	arm720t_scan_cp15(target, value, NULL, 0, 1);
+	arm720t_scan_cp15(target, ARMV4_5_NOP, NULL, 1, 1);
+
+	return ERROR_OK;
+}
+
+u32 arm720t_get_ttb(target_t *target)
+{
+	u32 ttb = 0x0;
+
+	arm720t_read_cp15(target, 0xee120f10, &ttb);
+	jtag_execute_queue();
+	
+	ttb &= 0xffffc000;
+	
+	return ttb;
+}
+
+void arm720t_disable_mmu_caches(target_t *target, int mmu, int d_u_cache, int i_cache)
+{
+	u32 cp15_control;
+
+	/* read cp15 control register */
+	arm720t_read_cp15(target, 0xee110f10, &cp15_control);
+	jtag_execute_queue();
+		
+	if (mmu)
+		cp15_control &= ~0x1U;
+	
+	if (d_u_cache || i_cache)
+		cp15_control &= ~0x4U;
+
+	arm720t_write_cp15(target, 0xee010f10, cp15_control);
+}
+
+void arm720t_enable_mmu_caches(target_t *target, int mmu, int d_u_cache, int i_cache)
+{
+	u32 cp15_control;
+
+	/* read cp15 control register */
+	arm720t_read_cp15(target, 0xee110f10, &cp15_control);
+	jtag_execute_queue();
+		
+	if (mmu)
+		cp15_control |= 0x1U;
+	
+	if (d_u_cache || i_cache)
+		cp15_control |= 0x4U;
+	
+	arm720t_write_cp15(target, 0xee010f10, cp15_control);
+}
+
+void arm720t_post_debug_entry(target_t *target)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm7tdmi_common_t *arm7tdmi = arm7_9->arch_info;
+	arm720t_common_t *arm720t = arm7tdmi->arch_info;
+	
+	/* examine cp15 control reg */
+	arm720t_read_cp15(target, 0xee110f10, &arm720t->cp15_control_reg);
+	jtag_execute_queue();
+	DEBUG("cp15_control_reg: %8.8x", arm720t->cp15_control_reg);
+
+	arm720t->armv4_5_mmu.mmu_enabled = (arm720t->cp15_control_reg & 0x1U) ? 1 : 0;
+	arm720t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = (arm720t->cp15_control_reg & 0x4U) ? 1 : 0;
+	arm720t->armv4_5_mmu.armv4_5_cache.i_cache_enabled = 0;
+
+	/* save i/d fault status and address register */
+	arm720t_read_cp15(target, 0xee150f10, &arm720t->fsr);
+	arm720t_read_cp15(target, 0xee160f10, &arm720t->far);
+	jtag_execute_queue();
+}
+
+void arm720t_pre_restore_context(target_t *target)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm7tdmi_common_t *arm7tdmi = arm7_9->arch_info;
+	arm720t_common_t *arm720t = arm7tdmi->arch_info;
+	
+	/* restore i/d fault status and address register */
+	arm720t_write_cp15(target, 0xee050f10, arm720t->fsr);
+	arm720t_write_cp15(target, 0xee060f10, arm720t->far);
+}
+
+int arm720t_get_arch_pointers(target_t *target, armv4_5_common_t **armv4_5_p, arm7_9_common_t **arm7_9_p, arm7tdmi_common_t **arm7tdmi_p, arm720t_common_t **arm720t_p)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9;
+	arm7tdmi_common_t *arm7tdmi;
+	arm720t_common_t *arm720t;
+	
+	if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
+	{
+		return -1;
+	}
+	
+	arm7_9 = armv4_5->arch_info;
+	if (arm7_9->common_magic != ARM7_9_COMMON_MAGIC)
+	{
+		return -1;
+	}
+	
+	arm7tdmi = arm7_9->arch_info;
+	if (arm7tdmi->common_magic != ARM7TDMI_COMMON_MAGIC)
+	{
+		return -1;
+	}
+	
+	arm720t = arm7tdmi->arch_info;
+	if (arm720t->common_magic != ARM720T_COMMON_MAGIC)
+	{
+		return -1;
+	}
+	
+	*armv4_5_p = armv4_5;
+	*arm7_9_p = arm7_9;
+	*arm7tdmi_p = arm7tdmi;
+	*arm720t_p = arm720t;
+	
+	return ERROR_OK;
+}
+
+int arm720t_arch_state(struct target_s *target, char *buf, int buf_size)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm7tdmi_common_t *arm7tdmi = arm7_9->arch_info;
+	arm720t_common_t *arm720t = arm7tdmi->arch_info;
+	
+	char *state[] = 
+	{
+		"disabled", "enabled"
+	};
+	
+	if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
+	{
+		ERROR("BUG: called for a non-ARMv4/5 target");
+		exit(-1);
+	}
+	
+	snprintf(buf, buf_size,
+			"target halted in %s state due to %s, current mode: %s\n"
+			"cpsr: 0x%8.8x pc: 0x%8.8x\n"
+			"MMU: %s, Cache: %s",
+			 armv4_5_state_strings[armv4_5->core_state],
+			 target_debug_reason_strings[target->debug_reason],
+			 armv4_5_mode_strings[armv4_5_mode_to_number(armv4_5->core_mode)],
+			 buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32),
+			 buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32),
+			 state[arm720t->armv4_5_mmu.mmu_enabled],
+			 state[arm720t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled]);
+	
+	return ERROR_OK;
+}
+
+int arm720t_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
+{
+	int retval;
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm7tdmi_common_t *arm7tdmi = arm7_9->arch_info;
+	arm720t_common_t *arm720t = arm7tdmi->arch_info;
+	
+	/* disable cache, but leave MMU enabled */
+	if (arm720t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled)
+		arm720t_disable_mmu_caches(target, 0, 1, 0);
+	
+	retval = arm7_9_read_memory(target, address, size, count, buffer);
+	
+	if (arm720t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled)
+		arm720t_enable_mmu_caches(target, 0, 1, 0);
+	
+	return retval;
+}
+
+int arm720t_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
+{
+	int retval;
+	
+	if ((retval = arm7_9_write_memory(target, address, size, count, buffer)) != ERROR_OK)
+		return retval;
+
+	return retval;
+}
+
+int arm720t_soft_reset_halt(struct target_s *target)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm7tdmi_common_t *arm7tdmi = arm7_9->arch_info;
+	arm720t_common_t *arm720t = arm7tdmi->arch_info;
+	reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
+	
+	if (target->state == TARGET_RUNNING)
+	{
+		target->type->halt(target);
+	}
+	
+	while (buf_get_u32(dbg_stat->value, EICE_DBG_CONTROL_DBGACK, 1) == 0)
+	{
+		embeddedice_read_reg(dbg_stat);
+		jtag_execute_queue();
+	}
+	
+	target->state = TARGET_HALTED;
+	
+	/* SVC, ARM state, IRQ and FIQ disabled */
+	buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8, 0xd3);
+	armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 1;
+	armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
+	
+	/* start fetching from 0x0 */
+	buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, 0x0);
+	armv4_5->core_cache->reg_list[15].dirty = 1;
+	armv4_5->core_cache->reg_list[15].valid = 1;
+	
+	armv4_5->core_mode = ARMV4_5_MODE_SVC;
+	armv4_5->core_state = ARMV4_5_STATE_ARM;
+	
+	arm720t_disable_mmu_caches(target, 1, 1, 1);
+	arm720t->armv4_5_mmu.mmu_enabled = 0;
+	arm720t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = 0;
+	arm720t->armv4_5_mmu.armv4_5_cache.i_cache_enabled = 0;
+
+	target_call_event_callbacks(target, TARGET_EVENT_HALTED);
+	
+	return ERROR_OK;
+}
+
+int arm720t_init_target(struct command_context_s *cmd_ctx, struct target_s *target)
+{
+	arm7tdmi_init_target(cmd_ctx, target);
+		
+	return ERROR_OK;
+	
+}
+
+int arm720t_quit()
+{
+	
+	return ERROR_OK;
+}
+
+int arm720t_init_arch_info(target_t *target, arm720t_common_t *arm720t, int chain_pos, char *variant)
+{
+	arm7tdmi_common_t *arm7tdmi = &arm720t->arm7tdmi_common;
+	arm7_9_common_t *arm7_9 = &arm7tdmi->arm7_9_common;
+	
+	arm7tdmi_init_arch_info(target, arm7tdmi, chain_pos, variant);
+
+	arm7tdmi->arch_info = arm720t;
+	arm720t->common_magic = ARM720T_COMMON_MAGIC;
+	
+	arm7_9->post_debug_entry = arm720t_post_debug_entry;
+	arm7_9->pre_restore_context = arm720t_pre_restore_context;
+	
+	arm720t->armv4_5_mmu.armv4_5_cache.ctype = -1;
+	arm720t->armv4_5_mmu.get_ttb = arm720t_get_ttb;
+	arm720t->armv4_5_mmu.read_memory = arm7_9_read_memory;
+	arm720t->armv4_5_mmu.write_memory = arm7_9_write_memory;
+	arm720t->armv4_5_mmu.disable_mmu_caches = arm720t_disable_mmu_caches;
+	arm720t->armv4_5_mmu.enable_mmu_caches = arm720t_enable_mmu_caches;
+	arm720t->armv4_5_mmu.has_tiny_pages = 0;
+	arm720t->armv4_5_mmu.mmu_enabled = 0;
+	
+	return ERROR_OK;
+}
+
+int arm720t_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target)
+{
+	int chain_pos;
+	char *variant = NULL;
+	arm720t_common_t *arm720t = malloc(sizeof(arm720t_common_t));
+	
+	if (argc < 4)
+	{
+		ERROR("'target arm720t' requires at least one additional argument");
+		exit(-1);
+	}
+	
+	chain_pos = strtoul(args[3], NULL, 0);
+	
+	if (argc >= 5)
+		variant = strdup(args[4]);
+	
+	DEBUG("chain_pos: %i, variant: %s", chain_pos, variant);
+	
+	arm720t_init_arch_info(target, arm720t, chain_pos, variant);
+
+	return ERROR_OK;
+}
+
+int arm720t_register_commands(struct command_context_s *cmd_ctx)
+{
+	int retval;
+	command_t *arm720t_cmd;
+	
+		
+	retval = arm7tdmi_register_commands(cmd_ctx);
+	
+	arm720t_cmd = register_command(cmd_ctx, NULL, "arm720t", NULL, COMMAND_ANY, NULL);
+
+	register_command(cmd_ctx, arm720t_cmd, "cp15", arm720t_handle_cp15_command, COMMAND_EXEC, "display/modify cp15 register <opcode> [value]");
+	register_command(cmd_ctx, arm720t_cmd, "virt2phys", arm720t_handle_virt2phys_command, COMMAND_EXEC, "translate va to pa <va>");
+
+	register_command(cmd_ctx, arm720t_cmd, "mdw_phys", arm720t_handle_md_phys_command, COMMAND_EXEC, "display memory words <physical addr> [count]");
+	register_command(cmd_ctx, arm720t_cmd, "mdh_phys", arm720t_handle_md_phys_command, COMMAND_EXEC, "display memory half-words <physical addr> [count]");
+	register_command(cmd_ctx, arm720t_cmd, "mdb_phys", arm720t_handle_md_phys_command, COMMAND_EXEC, "display memory bytes <physical addr> [count]");
+
+	register_command(cmd_ctx, arm720t_cmd, "mww_phys", arm720t_handle_mw_phys_command, COMMAND_EXEC, "write memory word <physical addr> <value>");
+	register_command(cmd_ctx, arm720t_cmd, "mwh_phys", arm720t_handle_mw_phys_command, COMMAND_EXEC, "write memory half-word <physical addr> <value>");
+	register_command(cmd_ctx, arm720t_cmd, "mwb_phys", arm720t_handle_mw_phys_command, COMMAND_EXEC, "write memory byte <physical addr> <value>");
+	
+	return ERROR_OK;
+}
+
+int arm720t_handle_cp15_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	int retval;
+	target_t *target = get_current_target(cmd_ctx);
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	arm7tdmi_common_t *arm7tdmi;
+	arm720t_common_t *arm720t;
+	arm_jtag_t *jtag_info;
+
+	if (arm720t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm7tdmi, &arm720t) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM720t target");
+		return ERROR_OK;
+	}
+	
+	jtag_info = &arm7_9->jtag_info;
+	
+	if (target->state != TARGET_HALTED)
+	{
+		command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
+		return ERROR_OK;
+	}
+
+	/* one or more argument, access a single register (write if second argument is given */
+	if (argc >= 1)
+	{
+		u32 opcode = strtoul(args[0], NULL, 0);
+
+		if (argc == 1)
+		{
+			u32 value;
+			if ((retval = arm720t_read_cp15(target, opcode, &value)) != ERROR_OK)
+			{
+				command_print(cmd_ctx, "couldn't access cp15 with opcode 0x%8.8x", opcode);
+				return ERROR_OK;
+			}
+			jtag_execute_queue();
+			
+			command_print(cmd_ctx, "0x%8.8x: 0x%8.8x", opcode, value);
+		}
+		else if (argc == 2)
+		{
+			u32 value = strtoul(args[1], NULL, 0);
+			if ((retval = arm720t_write_cp15(target, opcode, value)) != ERROR_OK)
+			{
+				command_print(cmd_ctx, "couldn't access cp15 with opcode 0x%8.8x", opcode);
+				return ERROR_OK;
+			}
+			command_print(cmd_ctx, "0x%8.8x: 0x%8.8x", opcode, value);
+		}
+	}
+
+	return ERROR_OK;
+}
+
+int arm720t_handle_virt2phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
+{	
+	target_t *target = get_current_target(cmd_ctx);
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	arm7tdmi_common_t *arm7tdmi;
+	arm720t_common_t *arm720t;
+	arm_jtag_t *jtag_info;
+
+	if (arm720t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm7tdmi, &arm720t) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM720t target");
+		return ERROR_OK;
+	}
+	
+	jtag_info = &arm7_9->jtag_info;
+	
+	if (target->state != TARGET_HALTED)
+	{
+		command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
+		return ERROR_OK;
+	}
+		
+	return armv4_5_mmu_handle_virt2phys_command(cmd_ctx, cmd, args, argc, target, &arm720t->armv4_5_mmu);
+}
+
+int arm720t_handle_md_phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
+{	
+	target_t *target = get_current_target(cmd_ctx);
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	arm7tdmi_common_t *arm7tdmi;
+	arm720t_common_t *arm720t;
+	arm_jtag_t *jtag_info;
+
+	if (arm720t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm7tdmi, &arm720t) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM720t target");
+		return ERROR_OK;
+	}
+	
+	jtag_info = &arm7_9->jtag_info;
+	
+	if (target->state != TARGET_HALTED)
+	{
+		command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
+		return ERROR_OK;
+	}
+	
+	return armv4_5_mmu_handle_md_phys_command(cmd_ctx, cmd, args, argc, target, &arm720t->armv4_5_mmu);
+}
+
+int arm720t_handle_mw_phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
+{	
+	target_t *target = get_current_target(cmd_ctx);
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	arm7tdmi_common_t *arm7tdmi;
+	arm720t_common_t *arm720t;
+	arm_jtag_t *jtag_info;
+
+	if (arm720t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm7tdmi, &arm720t) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM720t target");
+		return ERROR_OK;
+	}
+	
+	jtag_info = &arm7_9->jtag_info;
+	
+	if (target->state != TARGET_HALTED)
+	{
+		command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
+		return ERROR_OK;
+	}
+	
+	return armv4_5_mmu_handle_mw_phys_command(cmd_ctx, cmd, args, argc, target, &arm720t->armv4_5_mmu);
+}
+
diff --git a/src/target/arm720t.h b/src/target/arm720t.h
new file mode 100644
index 00000000..2479b548
--- /dev/null
+++ b/src/target/arm720t.h
@@ -0,0 +1,43 @@
+/***************************************************************************
+ *   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 ARM720T_H
+#define ARM720T_H
+
+#include "target.h"
+#include "register.h"
+#include "embeddedice.h"
+#include "arm_jtag.h"
+#include "arm7tdmi.h"
+#include "armv4_5_mmu.h"
+#include "armv4_5_cache.h"
+
+#define	ARM720T_COMMON_MAGIC 0xa720a720
+
+typedef struct arm720t_common_s
+{
+	int common_magic;
+	armv4_5_mmu_common_t armv4_5_mmu;
+	arm7tdmi_common_t arm7tdmi_common;
+	u32 cp15_control_reg;
+	u32 fsr;
+	u32 far;
+} arm720t_common_t;
+
+#endif /* ARM720T_H */
diff --git a/src/target/arm7_9_common.c b/src/target/arm7_9_common.c
new file mode 100644
index 00000000..d167041f
--- /dev/null
+++ b/src/target/arm7_9_common.c
@@ -0,0 +1,2339 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "config.h"
+
+#include "embeddedice.h"
+#include "target.h"
+#include "armv4_5.h"
+#include "arm_jtag.h"
+#include "jtag.h"
+#include "log.h"
+#include "arm7_9_common.h"
+#include "breakpoints.h"
+
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <sys/time.h>
+#include <errno.h>
+
+int arm7_9_debug_entry(target_t *target);
+int arm7_9_enable_sw_bkpts(struct target_s *target);
+
+/* command handler forward declarations */
+int handle_arm7_9_write_xpsr_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_arm7_9_write_xpsr_im8_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_arm7_9_read_core_reg_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_arm7_9_write_core_reg_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_arm7_9_sw_bkpts_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_arm7_9_force_hw_bkpts_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_arm7_9_dbgrq_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_arm7_9_fast_writes_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_arm7_9_dcc_downloads_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+int arm7_9_reinit_embeddedice(target_t *target)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	
+	breakpoint_t *breakpoint = target->breakpoints;
+	
+	arm7_9->wp_available = 2;
+	arm7_9->wp0_used = 0;
+	arm7_9->wp1_used = 0;
+		
+	/* mark all hardware breakpoints as unset */
+	while (breakpoint)
+	{
+		if (breakpoint->type == BKPT_HARD)
+		{
+			breakpoint->set = 0;
+		}
+		breakpoint = breakpoint->next;
+	}
+		
+	if (arm7_9->sw_bkpts_enabled && arm7_9->sw_bkpts_use_wp)
+	{
+		arm7_9->sw_bkpts_enabled = 0;
+		arm7_9_enable_sw_bkpts(target);
+	}
+	
+	arm7_9->reinit_embeddedice = 0;
+	
+	return ERROR_OK;
+}
+
+int arm7_9_jtag_callback(enum jtag_event event, void *priv)
+{
+	target_t *target = priv;
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	
+	/* a test-logic reset occured
+	 * the EmbeddedICE registers have been reset
+	 * hardware breakpoints have been cleared
+	 */
+	if (event == JTAG_TRST_ASSERTED)
+	{
+		arm7_9->reinit_embeddedice = 1;
+	}
+	
+	return ERROR_OK;
+}
+
+int arm7_9_get_arch_pointers(target_t *target, armv4_5_common_t **armv4_5_p, arm7_9_common_t **arm7_9_p)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	
+	if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
+	{
+		return -1;
+	}
+	
+	if (arm7_9->common_magic != ARM7_9_COMMON_MAGIC)
+	{
+		return -1;
+	}
+	
+	*armv4_5_p = armv4_5;
+	*arm7_9_p = arm7_9;
+	
+	return ERROR_OK;
+}
+
+int arm7_9_set_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	
+	if (target->state != TARGET_HALTED)
+	{
+		WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	
+	if (arm7_9->force_hw_bkpts)
+		breakpoint->type = BKPT_HARD;
+
+	if (breakpoint->set)
+	{
+		WARNING("breakpoint already set");
+		return ERROR_OK;
+	}
+
+	if (breakpoint->type == BKPT_HARD)
+	{
+		/* either an ARM (4 byte) or Thumb (2 byte) breakpoint */
+		u32 mask = (breakpoint->length == 4) ? 0x3u : 0x1u;
+		if (!arm7_9->wp0_used)
+		{
+			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_VALUE], breakpoint->address);
+			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], mask);
+			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0xffffffffu);
+			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
+			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
+
+			jtag_execute_queue();
+			arm7_9->wp0_used = 1;
+			breakpoint->set = 1;
+		}
+		else if (!arm7_9->wp1_used)
+		{
+			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_VALUE], breakpoint->address);
+			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK], mask);
+			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK], 0xffffffffu);
+			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
+			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
+
+			jtag_execute_queue();
+			arm7_9->wp1_used = 1;
+			breakpoint->set = 2;
+		}
+		else
+		{
+			ERROR("BUG: no hardware comparator available");
+			return ERROR_OK;
+		}
+	}
+	else if (breakpoint->type == BKPT_SOFT)
+	{
+		if (breakpoint->length == 4)
+		{
+			target->type->read_memory(target, breakpoint->address, 4, 1, breakpoint->orig_instr);
+			target->type->write_memory(target, breakpoint->address, 4, 1, (u8*)(&arm7_9->arm_bkpt));
+		}
+		else
+		{
+			target->type->read_memory(target, breakpoint->address, 2, 1, breakpoint->orig_instr);
+			target->type->read_memory(target, breakpoint->address, 2, 1, (u8*)(&arm7_9->arm_bkpt));
+		}
+		breakpoint->set = 1;
+	}
+
+	return ERROR_OK;
+
+}
+
+int arm7_9_unset_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	
+	if (target->state != TARGET_HALTED)
+	{
+		WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (!breakpoint->set)
+	{
+		WARNING("breakpoint not set");
+		return ERROR_OK;
+	}
+	
+	if (breakpoint->type == BKPT_HARD)
+	{
+		if (breakpoint->set == 1)
+		{
+			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], 0x0);
+			jtag_execute_queue();
+			arm7_9->wp0_used = 0;
+		}
+		else if (breakpoint->set == 2)
+		{
+			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], 0x0);
+			jtag_execute_queue();
+			arm7_9->wp1_used = 0;
+		}
+		breakpoint->set = 0;
+	}
+	else
+	{
+		if (breakpoint->length == 4)
+		{
+			target->type->write_memory(target, breakpoint->address, 4, 1, breakpoint->orig_instr);
+		}
+		else
+		{
+			target->type->write_memory(target, breakpoint->address, 2, 1, breakpoint->orig_instr);
+		}
+		breakpoint->set = 0;
+	}
+
+	return ERROR_OK;
+}
+
+int arm7_9_add_breakpoint(struct target_s *target, u32 address, u32 length, enum breakpoint_type type)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	
+	if (target->state != TARGET_HALTED)
+	{
+		WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	
+	if (arm7_9->force_hw_bkpts)
+	{
+		type = BKPT_HARD;
+	}
+	
+	if ((type == BKPT_SOFT) && (arm7_9->sw_bkpts_enabled == 0))
+	{
+		INFO("sw breakpoint requested, but software breakpoints not enabled");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+	
+	if ((type == BKPT_HARD) && (arm7_9->wp_available < 1))
+	{
+		INFO("no watchpoint unit available for hardware breakpoint");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+	
+	if (type == BKPT_HARD)
+		arm7_9->wp_available--;
+	
+	if ((length != 2) && (length != 4))
+	{
+		INFO("only breakpoints of two (Thumb) or four (ARM) bytes length supported");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+	
+	return ERROR_OK;
+}
+
+int arm7_9_remove_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	
+	if (target->state != TARGET_HALTED)
+	{
+		WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	
+	if (breakpoint->set)
+	{
+		arm7_9_unset_breakpoint(target, breakpoint);
+	}
+	
+	arm7_9->wp_available++;
+	
+	return ERROR_OK;
+}
+
+int arm7_9_set_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	int rw_mask = 1;
+	u32 mask;
+	
+	mask = watchpoint->length - 1;
+	
+	if (target->state != TARGET_HALTED)
+	{
+		WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	
+	if (watchpoint->rw == WPT_ACCESS)
+		rw_mask = 0;
+	else
+		rw_mask = 1;
+	
+	if (!arm7_9->wp0_used)
+	{
+		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_VALUE], watchpoint->address);
+		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], mask);
+		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], watchpoint->mask);
+		if( watchpoint->mask != 0xffffffffu )
+			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_VALUE], watchpoint->value);
+		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], 0xff & ~EICE_W_CTRL_nOPC & ~rw_mask);
+		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE | EICE_W_CTRL_nOPC | (watchpoint->rw & 1));
+
+		jtag_execute_queue();
+		watchpoint->set = 1;
+		arm7_9->wp0_used = 2;
+	}
+	else if (!arm7_9->wp1_used)
+	{
+		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_VALUE], watchpoint->address);
+		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK], mask);
+		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK], watchpoint->mask);
+		if( watchpoint->mask != 0xffffffffu )
+			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_VALUE], watchpoint->value);
+		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK], 0xff & ~EICE_W_CTRL_nOPC & ~rw_mask);
+		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], EICE_W_CTRL_ENABLE | EICE_W_CTRL_nOPC | (watchpoint->rw & 1));
+
+		jtag_execute_queue();
+		watchpoint->set = 2;
+		arm7_9->wp1_used = 2;
+	} 
+	else
+	{
+		ERROR("BUG: no hardware comparator available");
+		return ERROR_OK;
+	}
+	
+	return ERROR_OK;
+}
+
+int arm7_9_unset_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	
+	if (target->state != TARGET_HALTED)
+	{
+		WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	
+	if (!watchpoint->set)
+	{
+		WARNING("breakpoint not set");
+		return ERROR_OK;
+	}
+	
+	if (watchpoint->set == 1)
+	{
+		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], 0x0);
+		jtag_execute_queue();
+		arm7_9->wp0_used = 0;
+	}
+	else if (watchpoint->set == 2)
+	{
+		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], 0x0);
+		jtag_execute_queue();
+		arm7_9->wp1_used = 0;
+	}
+	watchpoint->set = 0;
+
+	return ERROR_OK;
+}
+
+int arm7_9_add_watchpoint(struct target_s *target, u32 address, u32 length, enum watchpoint_rw rw)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	
+	if (target->state != TARGET_HALTED)
+	{
+		WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	
+	if (arm7_9->wp_available < 1)
+	{
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+	
+	if ((length != 1) && (length != 2) && (length != 4))
+	{
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+	
+	arm7_9->wp_available--;
+		
+	return ERROR_OK;
+}
+
+int arm7_9_remove_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	
+	if (target->state != TARGET_HALTED)
+	{
+		WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	
+	if (watchpoint->set)
+	{
+		arm7_9_unset_watchpoint(target, watchpoint);
+	}
+		
+	arm7_9->wp_available++;
+	
+	return ERROR_OK;
+}
+
+int arm7_9_enable_sw_bkpts(struct target_s *target)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	int retval;
+	
+	if (arm7_9->sw_bkpts_enabled)
+		return ERROR_OK;
+	
+	if (arm7_9->wp_available-- < 1)
+	{
+		WARNING("can't enable sw breakpoints with no watchpoint unit available");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+	
+	if (!arm7_9->wp0_used)
+	{
+		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_VALUE], arm7_9->arm_bkpt);
+		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0x0);
+		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], 0xffffffffu);
+		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
+		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
+		arm7_9->sw_bkpts_enabled = 1;
+		arm7_9->wp0_used = 3;
+	}
+	else if (!arm7_9->wp1_used)
+	{
+		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_VALUE], arm7_9->arm_bkpt);
+		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK], 0x0);
+		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK], 0xffffffffu);
+		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
+		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
+		arm7_9->sw_bkpts_enabled = 2;
+		arm7_9->wp1_used = 3;
+	}
+	else
+	{
+		ERROR("BUG: both watchpoints used, but wp_available >= 1");
+		exit(-1);
+	}
+	
+	if ((retval = jtag_execute_queue()) != ERROR_OK)
+	{
+		ERROR("error writing EmbeddedICE registers to enable sw breakpoints");
+		exit(-1);
+	};
+	
+	return ERROR_OK;
+}
+
+int arm7_9_disable_sw_bkpts(struct target_s *target)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	
+	if (!arm7_9->sw_bkpts_enabled)
+		return ERROR_OK;
+	
+	if (arm7_9->sw_bkpts_enabled == 1)
+	{
+		embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], 0x0);
+		arm7_9->sw_bkpts_enabled = 0;
+		arm7_9->wp0_used = 0;
+		arm7_9->wp_available++;
+	}
+	else if (arm7_9->sw_bkpts_enabled == 2)
+	{
+		embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], 0x0);
+		arm7_9->sw_bkpts_enabled = 0;
+		arm7_9->wp1_used = 0;
+		arm7_9->wp_available++;
+	}
+
+	return ERROR_OK;
+}
+
+int arm7_9_execute_sys_speed(struct target_s *target)
+{
+	int timeout;
+	int retval;
+	
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+	reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
+			
+	/* set RESTART instruction */
+	jtag_add_end_state(TAP_RTI);
+	arm_jtag_set_instr(jtag_info, 0x4);
+	
+	for (timeout=0; timeout<50; timeout++)
+	{
+		/* read debug status register */
+		embeddedice_read_reg(dbg_stat);
+		if ((retval = jtag_execute_queue()) != ERROR_OK)
+			return retval;
+		if ((buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1))
+				   && (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_SYSCOMP, 1)))
+			break;
+		usleep(100000);	
+	}
+	if (timeout == 50)
+	{
+		ERROR("timeout waiting for SYSCOMP & DBGACK, last DBG_STATUS: %x", buf_get_u32(dbg_stat->value, 0, dbg_stat->size));
+		return ERROR_TARGET_TIMEOUT;
+	}
+	
+	return ERROR_OK;
+}
+
+int arm7_9_execute_fast_sys_speed(struct target_s *target)
+{
+	u8 check_value[4], check_mask[4];
+	
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+	reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
+			
+	/* set RESTART instruction */
+	jtag_add_end_state(TAP_RTI);
+	arm_jtag_set_instr(jtag_info, 0x4);
+	
+	/* check for DBGACK and SYSCOMP set (others don't care) */
+	buf_set_u32(check_value, 0, 32, 0x9);
+	buf_set_u32(check_mask, 0, 32, 0x9);
+	
+	/* read debug status register */
+	embeddedice_read_reg_w_check(dbg_stat, check_value, check_value);
+
+	return ERROR_OK;
+}
+
+enum target_state arm7_9_poll(target_t *target)
+{
+	int retval;
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
+	reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
+
+	if (arm7_9->reinit_embeddedice)
+	{
+		arm7_9_reinit_embeddedice(target);
+	}
+	
+	/* read debug status register */
+	embeddedice_read_reg(dbg_stat);
+	if ((retval = jtag_execute_queue()) != ERROR_OK)
+	{
+		switch (retval)
+		{
+			case ERROR_JTAG_QUEUE_FAILED:
+				ERROR("JTAG queue failed while reading EmbeddedICE status register");
+				exit(-1);
+				break;
+			default:
+				break;
+		}
+	}
+	
+	if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1))
+	{
+		DEBUG("DBGACK set, dbg_state->value: 0x%x", buf_get_u32(dbg_stat->value, 0, 32));
+		if ((target->state == TARGET_UNKNOWN))
+		{
+			WARNING("DBGACK set while target was in unknown state. Reset or initialize target before resuming");
+			target->state = TARGET_RUNNING;
+		}
+		if ((target->state == TARGET_RUNNING) || (target->state == TARGET_RESET))
+		{
+			target->state = TARGET_HALTED;
+			if ((retval = arm7_9_debug_entry(target)) != ERROR_OK)
+				return retval;
+			
+			target_call_event_callbacks(target, TARGET_EVENT_HALTED);
+		}
+		if (target->state == TARGET_DEBUG_RUNNING)
+		{
+			target->state = TARGET_HALTED;
+			if ((retval = arm7_9_debug_entry(target)) != ERROR_OK)
+				return retval;
+			
+			target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);
+		}
+	}
+	else
+	{
+		if (target->state != TARGET_DEBUG_RUNNING)
+			target->state = TARGET_RUNNING;
+	}
+	
+	return target->state;
+}
+
+int arm7_9_assert_reset(target_t *target)
+{
+	int retval;
+	
+	DEBUG("target->state: %s", target_state_strings[target->state]);
+	
+	if (target->state == TARGET_HALTED || target->state == TARGET_UNKNOWN)
+	{
+		/* assert SRST and TRST */
+		/* system would get ouf sync if we didn't reset test-logic, too */
+		if ((retval = jtag_add_reset(1, 1)) != ERROR_OK)
+		{
+			if (retval == ERROR_JTAG_RESET_CANT_SRST)
+			{
+				WARNING("can't assert srst");
+				return retval;
+			}
+			else
+			{
+				ERROR("unknown error");
+				exit(-1);
+			}
+		}
+		jtag_add_sleep(5000);
+		if ((retval = jtag_add_reset(0, 1)) != ERROR_OK)
+		{
+			if (retval == ERROR_JTAG_RESET_WOULD_ASSERT_TRST)
+			{
+				WARNING("srst resets test logic, too");
+				retval = jtag_add_reset(1, 1);
+			}
+		}
+	}
+	else
+	{
+		if ((retval = jtag_add_reset(0, 1)) != ERROR_OK)
+		{
+			if (retval == ERROR_JTAG_RESET_WOULD_ASSERT_TRST)
+			{
+				WARNING("srst resets test logic, too");
+				retval = jtag_add_reset(1, 1);
+			}
+			
+			if (retval == ERROR_JTAG_RESET_CANT_SRST)
+			{
+				WARNING("can't assert srst");
+				return retval;
+			}
+			else if (retval != ERROR_OK)
+			{
+				ERROR("unknown error");
+				exit(-1);
+			}
+		}
+	}
+	
+	target->state = TARGET_RESET;
+	jtag_add_sleep(50000);
+	
+	armv4_5_invalidate_core_regs(target);
+
+	return ERROR_OK;
+
+}
+
+int arm7_9_deassert_reset(target_t *target)
+{
+	DEBUG("target->state: %s", target_state_strings[target->state]);
+	
+	/* deassert reset lines */
+	jtag_add_reset(0, 0);
+		
+	return ERROR_OK;
+
+}
+
+int arm7_9_soft_reset_halt(struct target_s *target)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
+	int i;
+	
+	if (target->state == TARGET_RUNNING)
+	{
+		target->type->halt(target);
+	}
+	
+	while (buf_get_u32(dbg_stat->value, EICE_DBG_CONTROL_DBGACK, 1) == 0)
+	{
+		embeddedice_read_reg(dbg_stat);
+		jtag_execute_queue();
+	}
+	target->state = TARGET_HALTED;
+	
+	/* all register content is now invalid */
+	armv4_5_invalidate_core_regs(target);
+	
+	/* SVC, ARM state, IRQ and FIQ disabled */
+	buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8, 0xd3);
+	armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 1;
+	armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
+	
+	/* start fetching from 0x0 */
+	buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, 0x0);
+	armv4_5->core_cache->reg_list[15].dirty = 1;
+	armv4_5->core_cache->reg_list[15].valid = 1;
+	
+	armv4_5->core_mode = ARMV4_5_MODE_SVC;
+	armv4_5->core_state = ARMV4_5_STATE_ARM;
+	
+	/* reset registers */
+	for (i = 0; i <= 14; i++)
+	{	
+		buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).value, 0, 32, 0xffffffff);
+		ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = 1;
+		ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).valid = 1;
+	}
+	
+	target_call_event_callbacks(target, TARGET_EVENT_HALTED);
+	
+	return ERROR_OK;
+}
+
+int arm7_9_halt(target_t *target)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
+	
+	DEBUG("target->state: %s", target_state_strings[target->state]);
+	
+	if (target->state == TARGET_HALTED)
+	{
+		WARNING("target was already halted");
+		return ERROR_TARGET_ALREADY_HALTED;
+	} 
+	
+	if (target->state == TARGET_UNKNOWN)
+	{
+		WARNING("target was in unknown state when halt was requested");
+	}
+
+	if (arm7_9->use_dbgrq)
+	{
+		/* program EmbeddedICE Debug Control Register to assert DBGRQ
+		 */
+		buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGRQ, 1, 1);	
+		embeddedice_store_reg(dbg_ctrl);
+	}
+	else
+	{
+		/* program watchpoint unit to match on any address
+		 */
+		embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], 0xffffffff);
+		embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0xffffffff);
+		embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], 0x100);
+		embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], 0xf7);
+	}
+
+	target->debug_reason = DBG_REASON_DBGRQ;
+	
+	return ERROR_OK;
+}
+
+int arm7_9_clear_halt(target_t *target)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
+	
+	if (arm7_9->use_dbgrq)
+	{
+		/* program EmbeddedICE Debug Control Register to deassert DBGRQ
+		 */
+		buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGRQ, 1, 0);	
+		embeddedice_store_reg(dbg_ctrl);
+	}
+	else
+	{
+		/* restore registers if watchpoint unit 0 was in use
+		 */
+		if (arm7_9->wp0_used)
+		{
+			embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK]);
+			embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK]);
+			embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK]);
+		}
+		/* control value always has to be restored, as it was either disabled, 
+		 * or enabled with possibly different bits
+		 */
+		embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE]);
+	}
+	
+	return ERROR_OK;
+}
+
+int arm7_9_debug_entry(target_t *target)
+{
+	int i;
+	u32 context[16];
+	u32* context_p[16];
+	u32 r0_thumb, pc_thumb;
+	u32 cpsr;
+	int retval;
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
+	reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
+
+#ifdef _DEBUG_ARM7_9_
+	DEBUG("");
+#endif
+
+	if (arm7_9->pre_debug_entry)
+		arm7_9->pre_debug_entry(target);
+
+	/* program EmbeddedICE Debug Control Register to assert DBGACK and INTDIS
+	 * ensure that DBGRQ is cleared
+	 */
+	buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 1);
+	buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGRQ, 1, 0);
+	buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_INTDIS, 1, 1);
+	embeddedice_store_reg(dbg_ctrl);
+	
+	arm7_9_clear_halt(target);
+	
+	if ((retval = jtag_execute_queue()) != ERROR_OK)
+	{
+		switch (retval)
+		{
+			case ERROR_JTAG_QUEUE_FAILED:
+				ERROR("JTAG queue failed while writing EmbeddedICE control register");
+				exit(-1);
+				break;
+			default:
+				break;
+		}
+	}
+
+	if ((retval = arm7_9->examine_debug_reason(target)) != ERROR_OK)
+		return retval;
+
+
+	if (target->state != TARGET_HALTED)
+	{
+		WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	
+	/* if the target is in Thumb state, change to ARM state */
+	if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_ITBIT, 1))
+	{
+		DEBUG("target entered debug from Thumb state");
+		/* Entered debug from Thumb mode */
+		armv4_5->core_state = ARMV4_5_STATE_THUMB;
+		arm7_9->change_to_arm(target, &r0_thumb, &pc_thumb);
+		DEBUG("r0_thumb: 0x%8.8x, pc_thumb: 0x%8.8x", r0_thumb, pc_thumb);
+	}
+	else
+	{
+		DEBUG("target entered debug from ARM state");
+		/* Entered debug from ARM mode */
+		armv4_5->core_state = ARMV4_5_STATE_ARM;
+	}
+	
+	for (i = 0; i < 16; i++)
+		context_p[i] = &context[i];
+	/* save core registers (r0 - r15 of current core mode) */
+	arm7_9->read_core_regs(target, 0xffff, context_p);
+
+	arm7_9->read_xpsr(target, &cpsr, 0);
+	
+	if ((retval = jtag_execute_queue()) != ERROR_OK)
+		return retval;
+	buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32, cpsr);
+	armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 0;
+	armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
+	
+	armv4_5->core_mode = cpsr & 0x1f;
+	DEBUG("target entered debug state in %s mode", armv4_5_mode_strings[armv4_5_mode_to_number(armv4_5->core_mode)]);
+	
+	if (armv4_5_mode_to_number(armv4_5->core_mode) == -1)
+	{
+		target->state = TARGET_UNKNOWN;
+		ERROR("cpsr contains invalid mode value - communication failure");
+		return ERROR_TARGET_FAILURE;
+	}
+	
+	if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
+	{
+		DEBUG("thumb state, applying fixups");
+		context[0] = r0_thumb;
+		context[15] = pc_thumb;
+	} else if (armv4_5->core_state == ARMV4_5_STATE_ARM)
+	{
+		/* adjust value stored by STM */
+		context[15] -= 3 * 4;
+	}
+
+	if ((target->debug_reason == DBG_REASON_BREAKPOINT)
+			|| (target->debug_reason == DBG_REASON_SINGLESTEP)
+			|| (target->debug_reason == DBG_REASON_WATCHPOINT)
+			|| (target->debug_reason == DBG_REASON_WPTANDBKPT)
+			|| ((target->debug_reason == DBG_REASON_DBGRQ) && (arm7_9->use_dbgrq == 0)))
+		context[15] -= 3 * ((armv4_5->core_state == ARMV4_5_STATE_ARM) ? 4 : 2);
+	else if (target->debug_reason == DBG_REASON_DBGRQ)
+		context[15] -= arm7_9->dbgreq_adjust_pc * ((armv4_5->core_state == ARMV4_5_STATE_ARM) ? 4 : 2);
+	else
+	{
+		ERROR("unknown debug reason: %i", target->debug_reason);
+	}
+
+	
+	for (i=0; i<=15; i++)
+	{
+		buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).value, 0, 32, context[i]);
+		ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = 0;
+		ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).valid = 1;
+	}
+	
+	DEBUG("entered debug state at PC 0x%x", context[15]);
+
+	/* exceptions other than USR & SYS have a saved program status register */
+	if ((armv4_5_mode_to_number(armv4_5->core_mode) != ARMV4_5_MODE_USR) && (armv4_5_mode_to_number(armv4_5->core_mode) != ARMV4_5_MODE_SYS))
+	{
+		u32 spsr;
+		arm7_9->read_xpsr(target, &spsr, 1);
+		jtag_execute_queue();
+		buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).value, 0, 32, spsr);
+		ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).dirty = 0;
+		ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).valid = 1;
+	}
+
+	/* r0 and r15 (pc) have to be restored later */
+	ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).dirty = 1;
+	ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 15).dirty = 1;
+
+	if ((retval = jtag->execute_queue()) != ERROR_OK)
+		return retval;
+
+	if (arm7_9->post_debug_entry)
+		arm7_9->post_debug_entry(target);
+
+	return ERROR_OK;
+}
+
+int arm7_9_full_context(target_t *target)
+{
+	int i;
+	int retval;
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+
+	DEBUG("");
+	
+	if (target->state != TARGET_HALTED)
+	{
+		WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* iterate through processor modes (User, FIQ, IRQ, SVC, ABT, UND)
+	 * SYS shares registers with User, so we don't touch SYS
+	 */
+	for(i = 0; i < 6; i++)
+	{
+		u32 mask = 0;
+		u32* reg_p[16];
+		int j;
+		int valid = 1;
+		
+		/* check if there are invalid registers in the current mode 
+		 */
+		for (j = 0; j <= 16; j++)
+		{
+			if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid == 0)
+				valid = 0;
+		}
+		
+		if (!valid)
+		{
+			u32 tmp_cpsr;
+			
+			/* change processor mode */
+			tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
+			tmp_cpsr |= armv4_5_number_to_mode(i);
+			arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
+
+			for (j = 0; j < 15; j++)
+			{
+				if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid == 0)
+				{	
+					reg_p[j] = (u32*)ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).value;
+					mask |= 1 << j;
+					ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid = 1;
+					ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).dirty = 0;
+				}
+			}
+			
+			/* if only the PSR is invalid, mask is all zeroes */
+			if (mask)
+				arm7_9->read_core_regs(target, mask, reg_p);
+			
+			/* check if the PSR has to be read */
+			if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).valid == 0)
+			{
+				arm7_9->read_xpsr(target, (u32*)ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).value, 1);
+				ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).valid = 1;
+				ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).dirty = 0;
+			}
+		}
+	}
+
+	/* restore processor mode */
+	arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8), 0, 0);
+	
+	if ((retval = jtag_execute_queue()) != ERROR_OK)
+	{
+		ERROR("JTAG failure");
+		exit(-1);
+	}
+	return ERROR_OK;
+}
+
+int arm7_9_restore_context(target_t *target)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	reg_t *reg; 
+	armv4_5_core_reg_t *reg_arch_info;
+	enum armv4_5_mode current_mode = armv4_5->core_mode;
+	int i, j;
+	int dirty;
+	int mode_change;
+	
+	DEBUG("");
+	
+	if (target->state != TARGET_HALTED)
+	{
+		WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	
+	if (arm7_9->pre_restore_context)
+		arm7_9->pre_restore_context(target);
+	
+	/* iterate through processor modes (User, FIQ, IRQ, SVC, ABT, UND)
+	 * SYS shares registers with User, so we don't touch SYS
+	 */
+	for (i = 0; i < 6; i++)
+	{
+		DEBUG("examining %s mode", armv4_5_mode_strings[i]);
+		dirty = 0;
+		mode_change = 0;
+		/* check if there are dirty registers in the current mode 
+		*/
+		for (j = 0; j <= 16; j++)
+		{
+			reg = &ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j);
+			reg_arch_info = reg->arch_info;
+			if (reg->dirty == 1)
+			{
+				if (reg->valid == 1)
+				{
+					dirty = 1;
+					DEBUG("examining dirty reg: %s", reg->name);
+					if ((reg_arch_info->mode != ARMV4_5_MODE_ANY)
+						&& (reg_arch_info->mode != current_mode)
+						&& !((reg_arch_info->mode == ARMV4_5_MODE_USR) && (armv4_5->core_mode == ARMV4_5_MODE_SYS)) 
+						&& !((reg_arch_info->mode == ARMV4_5_MODE_SYS) && (armv4_5->core_mode == ARMV4_5_MODE_USR)))
+					{
+						mode_change = 1;
+						DEBUG("require mode change");
+					}
+				}
+				else
+				{
+					ERROR("BUG: dirty register '%s', but no valid data", reg->name);
+					exit(-1);
+				}
+			}
+		}
+		
+		if (dirty)
+		{
+			u32 mask = 0x0;
+			int num_regs = 0;
+			u32 regs[16];
+
+			if (mode_change)
+			{
+				u32 tmp_cpsr;
+			
+				/* change processor mode */
+				tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
+				tmp_cpsr |= armv4_5_number_to_mode(i);
+				arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
+				current_mode = armv4_5_number_to_mode(i);
+			}
+			
+			for (j = 0; j <= 14; j++)
+			{
+				reg = &ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j);
+				reg_arch_info = reg->arch_info;
+				
+				
+				if (reg->dirty == 1)
+				{
+					regs[j] = buf_get_u32(reg->value, 0, 32);
+					mask |= 1 << j;
+					num_regs++;
+					reg->dirty = 0;
+					reg->valid = 1;
+					DEBUG("writing register %i of mode %s with value 0x%8.8x", j, armv4_5_mode_strings[i], regs[j]);
+				}
+			}
+			
+			if (mask)
+			{
+				arm7_9->write_core_regs(target, mask, regs);
+			}
+			
+			reg = &ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16);
+			reg_arch_info = reg->arch_info;
+			if ((reg->dirty) && (reg_arch_info->mode != ARMV4_5_MODE_ANY))
+			{
+				DEBUG("writing SPSR of mode %i with value 0x%8.8x", i, buf_get_u32(reg->value, 0, 32));
+				arm7_9->write_xpsr(target, buf_get_u32(reg->value, 0, 32), 1);
+			}
+		}
+	}
+	
+	if ((armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty == 0) && (armv4_5->core_mode != current_mode))
+	{
+		/* restore processor mode */
+		u32 tmp_cpsr;
+			
+		tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
+		tmp_cpsr |= armv4_5_number_to_mode(i);
+		DEBUG("writing lower 8 bit of cpsr with value 0x%2.2x", tmp_cpsr);
+		arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
+	}
+	else if (armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty == 1)
+	{
+		/* CPSR has been changed, full restore necessary */
+		DEBUG("writing cpsr with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));
+		arm7_9->write_xpsr(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32), 0);
+		armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 0;
+		armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
+	}
+	
+	/* restore PC */
+	DEBUG("writing PC with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
+	arm7_9->write_pc(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
+	armv4_5->core_cache->reg_list[15].dirty = 0;
+			
+	if (arm7_9->post_restore_context)
+		arm7_9->post_restore_context(target);
+
+	return ERROR_OK;
+}
+
+int arm7_9_restart_core(struct target_s *target)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+	
+	/* set RESTART instruction */
+	jtag_add_end_state(TAP_RTI);
+	arm_jtag_set_instr(jtag_info, 0x4);
+	
+	jtag_add_runtest(1, TAP_RTI);
+	if ((jtag_execute_queue()) != ERROR_OK)
+	{
+		exit(-1);
+	}
+	
+	return ERROR_OK;
+}
+
+void arm7_9_enable_watchpoints(struct target_s *target)
+{
+	watchpoint_t *watchpoint = target->watchpoints;
+	
+	while (watchpoint)
+	{
+		if (watchpoint->set == 0)
+			arm7_9_set_watchpoint(target, watchpoint);
+		watchpoint = watchpoint->next;
+	}
+}
+
+void arm7_9_enable_breakpoints(struct target_s *target)
+{
+	breakpoint_t *breakpoint = target->breakpoints;
+	
+	/* set any pending breakpoints */
+	while (breakpoint)
+	{
+		if (breakpoint->set == 0)
+			arm7_9_set_breakpoint(target, breakpoint);
+		breakpoint = breakpoint->next;
+	}
+}
+
+void arm7_9_disable_bkpts_and_wpts(struct target_s *target)
+{
+	breakpoint_t *breakpoint = target->breakpoints;
+	watchpoint_t *watchpoint = target->watchpoints;
+
+	/* set any pending breakpoints */
+	while (breakpoint)
+	{
+		if (breakpoint->set != 0)
+			arm7_9_unset_breakpoint(target, breakpoint);
+		breakpoint = breakpoint->next;
+	}
+	
+	while (watchpoint)
+	{
+		if (watchpoint->set != 0)
+			arm7_9_unset_watchpoint(target, watchpoint);
+		watchpoint = watchpoint->next;
+	}
+}
+
+int arm7_9_resume(struct target_s *target, int current, u32 address, int handle_breakpoints, int debug_execution)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	breakpoint_t *breakpoint = target->breakpoints;
+	reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
+	
+	DEBUG("");
+	
+	if (target->state != TARGET_HALTED)
+	{
+		WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	
+	if (!debug_execution)
+	{
+		target_free_all_working_areas(target);
+	}
+	
+	/* current = 1: continue on current pc, otherwise continue at <address> */
+	if (!current)
+		buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, address);
+	
+	/* the front-end may request us not to handle breakpoints */
+	if (handle_breakpoints)
+	{
+		if ((breakpoint = breakpoint_find(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32))))
+		{
+			DEBUG("unset breakpoint at 0x%8.8x", breakpoint->address);
+			arm7_9_unset_breakpoint(target, breakpoint);
+			
+			DEBUG("enable single-step");
+			arm7_9->enable_single_step(target);
+			
+			target->debug_reason = DBG_REASON_SINGLESTEP;
+
+			arm7_9_restore_context(target);
+			
+			if (armv4_5->core_state == ARMV4_5_STATE_ARM)
+				arm7_9->branch_resume(target);
+			else if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
+			{
+				arm7_9->branch_resume_thumb(target);
+			}
+			else
+			{
+				ERROR("unhandled core state");
+				exit(-1);
+			}
+				
+			buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 0);
+			embeddedice_write_reg(dbg_ctrl, buf_get_u32(dbg_ctrl->value, 0, dbg_ctrl->size));
+			arm7_9_execute_sys_speed(target);
+			
+			DEBUG("disable single-step");
+			arm7_9->disable_single_step(target);
+			
+			arm7_9_debug_entry(target);
+			DEBUG("new PC after step: 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
+		
+			DEBUG("set breakpoint at 0x%8.8x", breakpoint->address);
+			arm7_9_set_breakpoint(target, breakpoint);
+		}
+	}
+	
+	/* enable any pending breakpoints and watchpoints */
+	arm7_9_enable_breakpoints(target);
+	arm7_9_enable_watchpoints(target);
+	
+	arm7_9_restore_context(target);
+	
+	if (armv4_5->core_state == ARMV4_5_STATE_ARM)
+	{
+		arm7_9->branch_resume(target);
+	}
+	else if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
+	{
+		arm7_9->branch_resume_thumb(target);
+	}
+	else
+	{
+		ERROR("unhandled core state");
+		exit(-1);
+	}
+	
+	/* deassert DBGACK and INTDIS */
+	buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 0);
+	/* INTDIS only when we really resume, not during debug execution */
+	if (!debug_execution)
+		buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_INTDIS, 1, 0);
+	embeddedice_write_reg(dbg_ctrl, buf_get_u32(dbg_ctrl->value, 0, dbg_ctrl->size));
+	
+	arm7_9_restart_core(target);
+	
+	target->debug_reason = DBG_REASON_NOTHALTED;
+	
+	if (!debug_execution)
+	{
+		/* registers are now invalid */
+		armv4_5_invalidate_core_regs(target);
+		target->state = TARGET_RUNNING;
+		target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
+	}
+	else
+	{
+		target->state = TARGET_DEBUG_RUNNING;
+		target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED);
+	}
+	
+	DEBUG("target resumed");
+	
+	return ERROR_OK;
+}
+
+void arm7_9_enable_eice_step(target_t *target)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	
+	/* setup an inverse breakpoint on the current PC
+	* - comparator 1 matches the current address
+	* - rangeout from comparator 1 is connected to comparator 0 rangein
+	* - comparator 0 matches any address, as long as rangein is low */
+	embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], 0xffffffff);
+	embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0xffffffff);
+	embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], 0x100);
+	embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], 0x77);
+	embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_VALUE], buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
+	embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK], 0);
+	embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK], 0xffffffff);
+	embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], 0x0);
+	embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK], 0xf7);
+}
+
+void arm7_9_disable_eice_step(target_t *target)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+
+	embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK]);
+	embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK]);
+	embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE]);
+	embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK]);
+	embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_VALUE]);
+	embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK]);
+	embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK]);
+	embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK]);
+	embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE]);
+}
+
+int arm7_9_step(struct target_s *target, int current, u32 address, int handle_breakpoints)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	breakpoint_t *breakpoint = target->breakpoints;
+
+	if (target->state != TARGET_HALTED)
+	{
+		WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	
+	/* current = 1: continue on current pc, otherwise continue at <address> */
+	if (!current)
+		buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, address);
+	
+	/* the front-end may request us not to handle breakpoints */
+	if (handle_breakpoints)
+		if ((breakpoint = breakpoint_find(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32))))
+			arm7_9_unset_breakpoint(target, breakpoint);
+	
+	target->debug_reason = DBG_REASON_SINGLESTEP;
+
+	arm7_9_restore_context(target);
+	
+	arm7_9->enable_single_step(target);
+	
+	if (armv4_5->core_state == ARMV4_5_STATE_ARM)
+	{
+		arm7_9->branch_resume(target);
+	}
+	else if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
+	{
+		arm7_9->branch_resume_thumb(target);
+	}
+	else
+	{
+		ERROR("unhandled core state");
+		exit(-1);
+	}
+	
+	target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
+
+	arm7_9_execute_sys_speed(target);
+	arm7_9->disable_single_step(target);
+	
+	/* registers are now invalid */
+	armv4_5_invalidate_core_regs(target);
+	
+	arm7_9_debug_entry(target);
+	
+	target_call_event_callbacks(target, TARGET_EVENT_HALTED);
+
+	if (breakpoint)
+		arm7_9_set_breakpoint(target, breakpoint);
+	
+	DEBUG("target stepped");
+
+	return ERROR_OK;
+
+}
+
+int arm7_9_read_core_reg(struct target_s *target, int num, enum armv4_5_mode mode)
+{
+	u32* reg_p[16];
+	int retval;
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	enum armv4_5_mode reg_mode = ((armv4_5_core_reg_t*)ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).arch_info)->mode;
+	
+	if ((num < 0) || (num > 16))
+		return ERROR_INVALID_ARGUMENTS;
+	
+	if ((mode != ARMV4_5_MODE_ANY)
+			&& (mode != armv4_5->core_mode)
+			&& (reg_mode != ARMV4_5_MODE_ANY))
+	{
+		u32 tmp_cpsr;
+			
+		/* change processor mode */
+		tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
+		tmp_cpsr |= mode;
+		arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
+	}
+	
+	if ((num >= 0) && (num <= 15))
+	{
+		/* read a normal core register */
+		reg_p[num] = (u32*)ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).value;
+		
+		arm7_9->read_core_regs(target, 1 << num, reg_p);
+	}
+	else
+	{
+		/* read a program status register
+		 * if the register mode is MODE_ANY, we read the cpsr, otherwise a spsr
+		 */
+		armv4_5_core_reg_t *arch_info = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).arch_info;
+		int spsr = (arch_info->mode == ARMV4_5_MODE_ANY) ? 0 : 1;
+		
+		arm7_9->read_xpsr(target, (u32*)ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).value, spsr);
+	}
+	
+	ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).valid = 1;
+	ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).dirty = 0;
+		
+	if ((mode != ARMV4_5_MODE_ANY)
+			&& (mode != armv4_5->core_mode)
+			&& (reg_mode != ARMV4_5_MODE_ANY))	{
+		/* restore processor mode */
+		arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8), 0, 0);
+	}
+	
+	if ((retval = jtag_execute_queue()) != ERROR_OK)
+	{
+		ERROR("JTAG failure");
+		exit(-1);
+	}
+	
+	return ERROR_OK;
+	
+}
+
+int arm7_9_write_core_reg(struct target_s *target, int num, enum armv4_5_mode mode, u32 value)
+{
+	u32 reg[16];
+	int retval;
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	enum armv4_5_mode reg_mode = ((armv4_5_core_reg_t*)ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).arch_info)->mode;
+
+	if ((num < 0) || (num > 16))
+		return ERROR_INVALID_ARGUMENTS;
+	
+	if ((mode != ARMV4_5_MODE_ANY)
+			&& (mode != armv4_5->core_mode)
+			&& (reg_mode != ARMV4_5_MODE_ANY))	{
+		u32 tmp_cpsr;
+			
+		/* change processor mode */
+		tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
+		tmp_cpsr |= mode;
+		arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
+	}
+	
+	if ((num >= 0) && (num <= 15))
+	{
+		/* write a normal core register */
+		reg[num] = value;
+		
+		arm7_9->write_core_regs(target, 1 << num, reg);
+	}
+	else
+	{
+		/* write a program status register
+		* if the register mode is MODE_ANY, we write the cpsr, otherwise a spsr
+		*/
+		armv4_5_core_reg_t *arch_info = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).arch_info;
+		int spsr = (arch_info->mode == ARMV4_5_MODE_ANY) ? 0 : 1;
+		
+		arm7_9->write_xpsr(target, value, spsr);
+	}
+	
+	ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).valid = 1;
+	ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).dirty = 0;
+		
+	if ((mode != ARMV4_5_MODE_ANY)
+			&& (mode != armv4_5->core_mode)
+			&& (reg_mode != ARMV4_5_MODE_ANY))	{
+		/* restore processor mode */
+		arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8), 0, 0);
+	}
+	
+	if ((retval = jtag_execute_queue()) != ERROR_OK)
+	{
+		ERROR("JTAG failure");
+		exit(-1);
+	}
+	
+	return ERROR_OK;
+	
+}
+
+int arm7_9_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	
+	u32 reg[16];
+	u32 *reg_p[16];
+	int num_accesses = 0;
+	int thisrun_accesses;
+	u32 *buf32;
+	u16 *buf16;
+	u8 *buf8;
+	int i;
+	u32 cpsr;
+	int retval;
+	int last_reg = 0;
+
+	DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);
+
+	if (target->state != TARGET_HALTED)
+	{
+		WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* sanitize arguments */
+	if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
+		return ERROR_INVALID_ARGUMENTS;
+
+	if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
+		return ERROR_TARGET_UNALIGNED_ACCESS;
+	
+	for (i = 0; i < 16; i++)
+	{
+		reg_p[i] = &reg[i];
+	}
+	
+	/* load the base register with the address of the first word */
+	reg[0] = address;
+	arm7_9->write_core_regs(target, 0x1, reg);
+	
+	switch (size)
+	{
+		case 4:
+			buf32 = (u32*)buffer;
+			while (num_accesses < count)
+			{
+				u32 reg_list;
+				thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
+				reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
+				
+				arm7_9->load_word_regs(target, reg_list);
+				arm7_9_execute_sys_speed(target);
+				
+				arm7_9->read_core_regs(target, reg_list, reg_p);
+				jtag_execute_queue();
+				
+				for (i = 1; i <= thisrun_accesses; i++)
+				{
+					if (i > last_reg)
+						last_reg = i;
+					*(buf32++) = reg[i];
+				}
+				num_accesses += thisrun_accesses;
+			}	
+			break;
+		case 2:
+			buf16 = (u16*)buffer;
+			while (num_accesses < count)
+			{
+				u32 reg_list;
+				thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
+				reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
+				
+				for (i = 1; i <= thisrun_accesses; i++)
+				{
+					if (i > last_reg)
+						last_reg = i;
+					arm7_9->load_hword_reg(target, i);
+					arm7_9_execute_sys_speed(target);
+				}
+				
+				arm7_9->read_core_regs(target, reg_list, reg_p);
+				jtag_execute_queue();
+				
+				for (i = 1; i <= thisrun_accesses; i++)
+				{
+					*(buf16++) = reg[i] & 0xffff;
+				}
+				num_accesses += thisrun_accesses;
+			}	
+			break;
+		case 1:
+			buf8 = buffer;
+			while (num_accesses < count)
+			{
+				u32 reg_list;
+				thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
+				reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
+				
+				for (i = 1; i <= thisrun_accesses; i++)
+				{
+					if (i > last_reg)
+						last_reg = i;
+					arm7_9->load_byte_reg(target, i);
+					arm7_9_execute_sys_speed(target);
+				}
+				
+				arm7_9->read_core_regs(target, reg_list, reg_p);
+				jtag_execute_queue();
+				
+				for (i = 1; i <= thisrun_accesses; i++)
+				{
+					*(buf8++) = reg[i] & 0xff;
+				}
+				num_accesses += thisrun_accesses;
+			}	
+			break;
+		default:
+			ERROR("BUG: we shouldn't get here");
+			exit(-1);
+			break;
+	}
+	
+	for (i=0; i<=last_reg; i++)
+		ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = 1;
+
+	arm7_9->read_xpsr(target, &cpsr, 0);
+	if ((retval = jtag_execute_queue()) != ERROR_OK)
+	{
+		ERROR("JTAG error while reading cpsr");
+		exit(-1);
+	}
+
+	if (((cpsr & 0x1f) == ARMV4_5_MODE_ABT) && (armv4_5->core_mode != ARMV4_5_MODE_ABT))
+	{
+		ERROR("memory read caused data abort");
+
+		arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8), 0, 0);
+
+		return ERROR_TARGET_DATA_ABORT;
+	}
+	
+	return ERROR_OK;
+}
+
+int arm7_9_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	
+	u32 reg[16];
+	int num_accesses = 0;
+	int thisrun_accesses;
+	u32 *buf32;
+	u16 *buf16;
+	u8 *buf8;
+	int i;
+	u32 cpsr;
+	int retval;
+	int last_reg = 0;
+
+	DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);
+
+	if (target->state != TARGET_HALTED)
+	{
+		WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* sanitize arguments */
+	if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
+		return ERROR_INVALID_ARGUMENTS;
+
+	if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
+		return ERROR_TARGET_UNALIGNED_ACCESS;
+	
+	/* load the base register with the address of the first word */
+	reg[0] = address;
+	arm7_9->write_core_regs(target, 0x1, reg);
+	
+	switch (size)
+	{
+		case 4:
+			buf32 = (u32*)buffer;
+			while (num_accesses < count)
+			{
+				u32 reg_list;
+				thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
+				reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
+				
+				for (i = 1; i <= thisrun_accesses; i++)
+				{
+					if (i > last_reg)
+						last_reg = i;
+					reg[i] = *buf32++;
+				}
+				
+				arm7_9->write_core_regs(target, reg_list, reg);
+				
+				arm7_9->store_word_regs(target, reg_list);
+				
+				/* fast memory writes are only safe when the target is running
+				 * from a sufficiently high clock (32 kHz is usually too slow)
+				 */
+				if (arm7_9->fast_memory_writes)
+					arm7_9_execute_fast_sys_speed(target);
+				else
+					arm7_9_execute_sys_speed(target);
+				
+				num_accesses += thisrun_accesses;
+			}	
+			break;
+		case 2:
+			buf16 = (u16*)buffer;
+			while (num_accesses < count)
+			{
+				u32 reg_list;
+				thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
+				reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
+				
+				for (i = 1; i <= thisrun_accesses; i++)
+				{
+					if (i > last_reg)
+						last_reg = i;
+					reg[i] = *buf16++ & 0xffff;
+				}
+				
+				arm7_9->write_core_regs(target, reg_list, reg);
+				
+				for (i = 1; i <= thisrun_accesses; i++)
+				{
+					arm7_9->store_hword_reg(target, i);
+					
+					/* fast memory writes are only safe when the target is running
+					 * from a sufficiently high clock (32 kHz is usually too slow)
+					 */
+					if (arm7_9->fast_memory_writes)
+						arm7_9_execute_fast_sys_speed(target);
+					else
+						arm7_9_execute_sys_speed(target);
+				}
+				
+				num_accesses += thisrun_accesses;
+			}	
+			break;
+		case 1:
+			buf8 = buffer;
+			while (num_accesses < count)
+			{
+				u32 reg_list;
+				thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
+				reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
+				
+				for (i = 1; i <= thisrun_accesses; i++)
+				{
+					if (i > last_reg)
+						last_reg = i;
+					reg[i] = *buf8++ & 0xff;
+				}
+				
+				arm7_9->write_core_regs(target, reg_list, reg);
+				
+				for (i = 1; i <= thisrun_accesses; i++)
+				{
+					arm7_9->store_byte_reg(target, i);
+					/* fast memory writes are only safe when the target is running
+					 * from a sufficiently high clock (32 kHz is usually too slow)
+					 */
+					if (arm7_9->fast_memory_writes)
+						arm7_9_execute_fast_sys_speed(target);
+					else
+						arm7_9_execute_sys_speed(target);
+				}
+				
+				num_accesses += thisrun_accesses;
+			}	
+			break;
+		default:
+			ERROR("BUG: we shouldn't get here");
+			exit(-1);
+			break;
+	}
+	
+	if ((retval = jtag_execute_queue()) != ERROR_OK)
+	{
+		ERROR("JTAG error while writing target memory");
+		exit(-1);
+	}
+	
+	for (i=0; i<=last_reg; i++)
+		ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = 1;
+
+	arm7_9->read_xpsr(target, &cpsr, 0);
+	if ((retval = jtag_execute_queue()) != ERROR_OK)
+	{
+		ERROR("JTAG error while reading cpsr");
+		exit(-1);
+	}
+
+	if (((cpsr & 0x1f) == ARMV4_5_MODE_ABT) && (armv4_5->core_mode != ARMV4_5_MODE_ABT))
+	{
+		ERROR("memory write caused data abort");
+
+		arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8), 0, 0);
+
+		return ERROR_TARGET_DATA_ABORT;
+	}
+	
+	return ERROR_OK;
+}
+
+int arm7_9_bulk_write_memory(target_t *target, u32 address, u32 count, u8 *buffer)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	enum armv4_5_state core_state = armv4_5->core_state;
+	u32 r0 = buf_get_u32(armv4_5->core_cache->reg_list[0].value, 0, 32);
+	u32 r1 = buf_get_u32(armv4_5->core_cache->reg_list[1].value, 0, 32);
+	u32 pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
+	int i;
+	
+	u32 dcc_code[] = 
+	{
+		/* MRC      TST         BNE         MRC         STR         B */
+		0xee101e10, 0xe3110001, 0x0afffffc, 0xee111e10, 0xe4801004, 0xeafffff9
+	};
+	
+	if (!arm7_9->dcc_downloads)
+		return target->type->write_memory(target, address, 4, count, buffer);
+
+	/* regrab previously allocated working_area, or allocate a new one */
+	if (!arm7_9->dcc_working_area)
+	{
+		/* make sure we have a working area */
+		if (target_alloc_working_area(target, 24, &arm7_9->dcc_working_area) != ERROR_OK)
+		{
+			INFO("no working area available, falling back to memory writes");
+			return target->type->write_memory(target, address, 4, count, buffer);
+		}
+		
+		/* write DCC code to working area */
+		target->type->write_memory(target, arm7_9->dcc_working_area->address, 4, 6, (u8*)dcc_code);
+	}
+	
+	buf_set_u32(armv4_5->core_cache->reg_list[0].value, 0, 32, address);
+	armv4_5->core_cache->reg_list[0].valid = 1;
+	armv4_5->core_cache->reg_list[0].dirty = 1;
+	armv4_5->core_state = ARMV4_5_STATE_ARM;
+
+	arm7_9_resume(target, 0, arm7_9->dcc_working_area->address, 1, 1);
+	
+	for (i = 0; i < count; i++)
+	{
+		embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_COMMS_DATA], buf_get_u32(buffer, 0, 32));
+		buffer += 4;
+	}
+	
+	target->type->halt(target);
+	
+	while (target->state != TARGET_HALTED)
+		target->type->poll(target);
+	
+	/* restore target state */
+	buf_set_u32(armv4_5->core_cache->reg_list[0].value, 0, 32, r0);
+	armv4_5->core_cache->reg_list[0].valid = 1;
+	armv4_5->core_cache->reg_list[0].dirty = 1;
+	buf_set_u32(armv4_5->core_cache->reg_list[1].value, 0, 32, r1);
+	armv4_5->core_cache->reg_list[1].valid = 1;
+	armv4_5->core_cache->reg_list[1].dirty = 1;
+	buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, pc);
+	armv4_5->core_cache->reg_list[15].valid = 1;
+	armv4_5->core_cache->reg_list[15].dirty = 1;
+	armv4_5->core_state = core_state;
+	
+	return ERROR_OK;
+}
+
+int arm7_9_register_commands(struct command_context_s *cmd_ctx)
+{
+	command_t *arm7_9_cmd;
+	
+	arm7_9_cmd = register_command(cmd_ctx, NULL, "arm7_9", NULL, COMMAND_ANY, NULL);
+	
+	register_command(cmd_ctx, arm7_9_cmd, "write_xpsr", handle_arm7_9_write_xpsr_command, COMMAND_EXEC, "write program status register <value> <not cpsr|spsr>");
+	register_command(cmd_ctx, arm7_9_cmd, "write_xpsr_im8", handle_arm7_9_write_xpsr_im8_command, COMMAND_EXEC, "write program status register <8bit immediate> <rotate> <not cpsr|spsr>");
+	
+	register_command(cmd_ctx, arm7_9_cmd, "write_core_reg", handle_arm7_9_write_core_reg_command, COMMAND_EXEC, "write core register <num> <mode> <value>");	
+	
+	register_command(cmd_ctx, arm7_9_cmd, "sw_bkpts", handle_arm7_9_sw_bkpts_command, COMMAND_EXEC, "support for software breakpoints <enable|disable>");
+	register_command(cmd_ctx, arm7_9_cmd, "force_hw_bkpts", handle_arm7_9_force_hw_bkpts_command, COMMAND_EXEC, "use hardware breakpoints for all breakpoints (disables sw breakpoint support) <enable|disable>");
+	register_command(cmd_ctx, arm7_9_cmd, "dbgrq", handle_arm7_9_dbgrq_command,
+		COMMAND_ANY, "use EmbeddedICE dbgrq instead of breakpoint for target halt requests <enable|disable>");
+	register_command(cmd_ctx, arm7_9_cmd, "fast_writes", handle_arm7_9_fast_writes_command,
+		 COMMAND_ANY, "use fast memory writes instead of slower but potentially unsafe slow writes <enable|disable>");
+	register_command(cmd_ctx, arm7_9_cmd, "dcc_downloads", handle_arm7_9_dcc_downloads_command,
+		COMMAND_ANY, "use DCC downloads for larger memory writes <enable|disable>");
+
+	armv4_5_register_commands(cmd_ctx);
+	
+	return ERROR_OK;
+}
+
+int handle_arm7_9_write_xpsr_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	u32 value;
+	int spsr;
+	int retval;
+	target_t *target = get_current_target(cmd_ctx);
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+
+	if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
+		return ERROR_OK;
+	}
+	
+	if (target->state != TARGET_HALTED)
+	{
+		command_print(cmd_ctx, "can't write registers while running");
+		return ERROR_OK;
+	}
+	
+	if (argc < 2)
+	{
+		command_print(cmd_ctx, "usage: write_xpsr <value> <not cpsr|spsr>");
+		return ERROR_OK;
+	}
+	
+	value = strtoul(args[0], NULL, 0);
+	spsr = strtol(args[1], NULL, 0);
+	
+	arm7_9->write_xpsr(target, value, spsr);
+	if ((retval = jtag_execute_queue()) != ERROR_OK)
+	{
+		ERROR("JTAG error while writing to xpsr");
+		exit(-1);
+	}
+	
+	return ERROR_OK;
+}
+
+int handle_arm7_9_write_xpsr_im8_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	u32 value;
+	int rotate;
+	int spsr;
+	int retval;
+	target_t *target = get_current_target(cmd_ctx);
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+
+	if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
+		return ERROR_OK;
+	}
+	
+	if (target->state != TARGET_HALTED)
+	{
+		command_print(cmd_ctx, "can't write registers while running");
+		return ERROR_OK;
+	}
+	
+	if (argc < 3)
+	{
+		command_print(cmd_ctx, "usage: write_xpsr_im8 <im8> <rotate> <not cpsr|spsr>");
+		return ERROR_OK;
+	}
+	
+	value = strtoul(args[0], NULL, 0);
+	rotate = strtol(args[1], NULL, 0);
+	spsr = strtol(args[2], NULL, 0);
+	
+	arm7_9->write_xpsr_im8(target, value, rotate, spsr);
+	if ((retval = jtag_execute_queue()) != ERROR_OK)
+	{
+		ERROR("JTAG error while writing 8-bit immediate to xpsr");
+		exit(-1);
+	}
+	
+	return ERROR_OK;
+}
+
+int handle_arm7_9_write_core_reg_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	u32 value;
+	u32 mode;
+	int num;
+	target_t *target = get_current_target(cmd_ctx);
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+		
+	if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
+		return ERROR_OK;
+	}
+	
+	if (target->state != TARGET_HALTED)
+	{
+		command_print(cmd_ctx, "can't write registers while running");
+		return ERROR_OK;
+	}
+	
+	if (argc < 3)
+	{
+		command_print(cmd_ctx, "usage: write_core_reg <num> <mode> <value>");
+		return ERROR_OK;
+	}
+	
+	num = strtol(args[0], NULL, 0);
+	mode = strtoul(args[1], NULL, 0);
+	value = strtoul(args[2], NULL, 0);
+	
+	arm7_9_write_core_reg(target, num, mode, value);
+	
+	return ERROR_OK;
+}
+
+int handle_arm7_9_sw_bkpts_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target = get_current_target(cmd_ctx);
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	
+	if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
+		return ERROR_OK;
+	}
+	
+	if (argc == 0)
+	{
+		command_print(cmd_ctx, "software breakpoints %s", (arm7_9->sw_bkpts_enabled) ? "enabled" : "disabled");
+		return ERROR_OK;
+	}
+	
+	if (strcmp("enable", args[0]) == 0)
+	{
+		if (arm7_9->sw_bkpts_use_wp)
+		{
+			arm7_9_enable_sw_bkpts(target);
+		}
+		else
+		{
+			arm7_9->sw_bkpts_enabled = 1;
+		}
+	}
+	else if (strcmp("disable", args[0]) == 0)
+	{
+		if (arm7_9->sw_bkpts_use_wp)
+		{
+			arm7_9_disable_sw_bkpts(target);
+		}
+		else
+		{
+			arm7_9->sw_bkpts_enabled = 0;
+		}
+	}
+	else
+	{
+		command_print(cmd_ctx, "usage: arm7_9 sw_bkpts <enable|disable>");
+	}
+	
+	command_print(cmd_ctx, "software breakpoints %s", (arm7_9->sw_bkpts_enabled) ? "enabled" : "disabled");
+	
+	return ERROR_OK;
+}
+
+int handle_arm7_9_force_hw_bkpts_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target = get_current_target(cmd_ctx);
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	
+	if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
+		return ERROR_OK;
+	}
+	
+	if ((argc >= 1) && (strcmp("enable", args[0]) == 0))
+	{
+		arm7_9->force_hw_bkpts = 1;
+		if (arm7_9->sw_bkpts_use_wp)
+		{
+			arm7_9_disable_sw_bkpts(target);
+		}
+	}
+	else if ((argc >= 1) && (strcmp("disable", args[0]) == 0))
+	{
+		arm7_9->force_hw_bkpts = 0;
+	}
+	else
+	{
+		command_print(cmd_ctx, "usage: arm7_9 force_hw_bkpts <enable|disable>");
+	}
+		
+	command_print(cmd_ctx, "force hardware breakpoints %s", (arm7_9->force_hw_bkpts) ? "enabled" : "disabled");
+
+	return ERROR_OK;
+}
+
+int handle_arm7_9_dbgrq_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target = get_current_target(cmd_ctx);
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	
+	if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
+		return ERROR_OK;
+	}
+	
+	if (argc > 0)
+	{
+		if (strcmp("enable", args[0]) == 0)
+		{
+			arm7_9->use_dbgrq = 1;
+		}
+		else if (strcmp("disable", args[0]) == 0)
+		{
+			arm7_9->use_dbgrq = 0;
+		}
+		else
+		{
+			command_print(cmd_ctx, "usage: arm7_9 dbgrq <enable|disable>");
+		}
+	}
+		
+	command_print(cmd_ctx, "use of EmbeddedICE dbgrq instead of breakpoint for target halt %s", (arm7_9->use_dbgrq) ? "enabled" : "disabled");
+
+	return ERROR_OK;
+}
+
+int handle_arm7_9_fast_writes_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target = get_current_target(cmd_ctx);
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	
+	if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
+		return ERROR_OK;
+	}
+	
+	if (argc > 0)
+	{
+		if (strcmp("enable", args[0]) == 0)
+		{
+			arm7_9->fast_memory_writes = 1;
+		}
+		else if (strcmp("disable", args[0]) == 0)
+		{
+			arm7_9->fast_memory_writes = 0;
+		}
+		else
+		{
+			command_print(cmd_ctx, "usage: arm7_9 fast_writes <enable|disable>");
+		}
+	}
+		
+	command_print(cmd_ctx, "fast memory writes are %s", (arm7_9->fast_memory_writes) ? "enabled" : "disabled");
+
+	return ERROR_OK;
+}
+
+int handle_arm7_9_dcc_downloads_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target = get_current_target(cmd_ctx);
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	
+	if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
+		return ERROR_OK;
+	}
+	
+	if (argc > 0)
+	{
+		if (strcmp("enable", args[0]) == 0)
+		{
+			arm7_9->dcc_downloads = 1;
+		}
+		else if (strcmp("disable", args[0]) == 0)
+		{
+			arm7_9->dcc_downloads = 0;
+		}
+		else
+		{
+			command_print(cmd_ctx, "usage: arm7_9 dcc_downloads <enable|disable>");
+		}
+	}
+		
+	command_print(cmd_ctx, "dcc downloads are %s", (arm7_9->dcc_downloads) ? "enabled" : "disabled");
+
+	return ERROR_OK;
+}
+
+int arm7_9_init_arch_info(target_t *target, arm7_9_common_t *arm7_9)
+{
+	armv4_5_common_t *armv4_5 = &arm7_9->armv4_5_common;
+	
+	arm7_9->common_magic = ARM7_9_COMMON_MAGIC;
+	
+	arm_jtag_setup_connection(&arm7_9->jtag_info);
+	arm7_9->wp_available = 2;
+	arm7_9->wp0_used = 0;
+	arm7_9->wp1_used = 0;
+	arm7_9->force_hw_bkpts = 0;
+	arm7_9->use_dbgrq = 0;
+	arm7_9->has_etm = 0;
+	
+	arm7_9->reinit_embeddedice = 0;
+	
+	arm7_9->dcc_working_area = NULL;
+	
+	arm7_9->fast_memory_writes = 0;
+	arm7_9->dcc_downloads = 0;
+
+	jtag_register_event_callback(arm7_9_jtag_callback, target);
+
+	armv4_5->arch_info = arm7_9;
+	armv4_5->read_core_reg = arm7_9_read_core_reg;
+	armv4_5->write_core_reg = arm7_9_write_core_reg;
+	armv4_5->full_context = arm7_9_full_context;
+	
+	armv4_5_init_arch_info(target, armv4_5);
+	
+	return ERROR_OK;
+}
diff --git a/src/target/arm7_9_common.h b/src/target/arm7_9_common.h
new file mode 100644
index 00000000..f03ae496
--- /dev/null
+++ b/src/target/arm7_9_common.h
@@ -0,0 +1,129 @@
+/***************************************************************************
+ *   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 ARM7_9_COMMON_H
+#define ARM7_9_COMMON_H
+
+#include "armv4_5.h"
+#include "arm_jtag.h"
+#include "breakpoints.h"
+#include "target.h"
+
+#define	ARM7_9_COMMON_MAGIC 0x0a790a79
+
+typedef struct arm7_9_common_s
+{
+	int common_magic;
+	
+	arm_jtag_t jtag_info;
+	reg_cache_t *eice_cache;
+	reg_cache_t *etm_cache;
+	
+	u32 arm_bkpt;
+	u16 thumb_bkpt;
+	int sw_bkpts_use_wp;
+	int wp_available;
+	int wp0_used;
+	int wp1_used;
+	int sw_bkpts_enabled;
+	int force_hw_bkpts;
+	int dbgreq_adjust_pc;
+	int use_dbgrq;
+	int has_etm;
+	
+	int reinit_embeddedice;
+	
+	struct working_area_s *dcc_working_area;
+	
+	int fast_memory_writes;
+	int dcc_downloads;
+
+	int (*examine_debug_reason)(target_t *target);
+	
+	void (*change_to_arm)(target_t *target, u32 *r0, u32 *pc);
+	
+	void (*read_core_regs)(target_t *target, u32 mask, u32* core_regs[16]);
+	void (*read_xpsr)(target_t *target, u32 *xpsr, int spsr);
+	
+	void (*write_xpsr)(target_t *target, u32 xpsr, int spsr);
+	void (*write_xpsr_im8)(target_t *target, u8 xpsr_im, int rot, int spsr);
+	void (*write_core_regs)(target_t *target, u32 mask, u32 core_regs[16]);
+	
+	void (*load_word_regs)(target_t *target, u32 mask);
+	void (*load_hword_reg)(target_t *target, int num);
+	void (*load_byte_reg)(target_t *target, int num);
+
+	void (*store_word_regs)(target_t *target, u32 mask);
+	void (*store_hword_reg)(target_t *target, int num);
+	void (*store_byte_reg)(target_t *target, int num);
+	
+	void (*write_pc)(target_t *target, u32 pc);
+	void (*branch_resume)(target_t *target);
+	void (*branch_resume_thumb)(target_t *target);
+	
+	void (*enable_single_step)(target_t *target);
+	void (*disable_single_step)(target_t *target);
+	
+	void (*pre_debug_entry)(target_t *target);
+	void (*post_debug_entry)(target_t *target);
+	
+	void (*pre_restore_context)(target_t *target);
+	void (*post_restore_context)(target_t *target);
+	
+	armv4_5_common_t armv4_5_common;
+	void *arch_info;
+
+} arm7_9_common_t;
+
+int arm7_9_register_commands(struct command_context_s *cmd_ctx);
+
+enum target_state arm7_9_poll(target_t *target);
+
+int arm7_9_assert_reset(target_t *target);
+int arm7_9_deassert_reset(target_t *target);
+int arm7_9_reset_request_halt(target_t *target);
+int arm7_9_early_halt(target_t *target);
+int arm7_9_soft_reset_halt(struct target_s *target);
+
+int arm7_9_halt(target_t *target);
+int arm7_9_debug_entry(target_t *target);
+int arm7_9_full_context(target_t *target);
+int arm7_9_resume(struct target_s *target, int current, u32 address, int handle_breakpoints, int debug_execution);
+int arm7_9_step(struct target_s *target, int current, u32 address, int handle_breakpoints);
+int arm7_9_read_core_reg(struct target_s *target, int num, enum armv4_5_mode mode);
+int arm7_9_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);
+int arm7_9_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);
+int arm7_9_bulk_write_memory(target_t *target, u32 address, u32 count, u8 *buffer);
+
+int arm7_9_run_algorithm(struct target_s *target, int num_mem_params, mem_param_t *mem_params, int num_reg_prams, reg_param_t *reg_param, u32 entry_point, void *arch_info);
+
+int arm7_9_add_breakpoint(struct target_s *target, u32 address, u32 length, enum breakpoint_type type);
+int arm7_9_remove_breakpoint(struct target_s *target, breakpoint_t *breakpoint);
+int arm7_9_add_watchpoint(struct target_s *target, u32 address, u32 length, enum watchpoint_rw rw);
+int arm7_9_remove_watchpoint(struct target_s *target, watchpoint_t *watchpoint);
+
+void arm7_9_enable_eice_step(target_t *target);
+void arm7_9_disable_eice_step(target_t *target);
+
+int arm7_9_execute_sys_speed(struct target_s *target);
+
+int arm7_9_init_arch_info(target_t *target, arm7_9_common_t *arm7_9);
+
+
+#endif /* ARM7_9_COMMON_H */
diff --git a/src/target/arm7tdmi.c b/src/target/arm7tdmi.c
new file mode 100644
index 00000000..c2079477
--- /dev/null
+++ b/src/target/arm7tdmi.c
@@ -0,0 +1,780 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "config.h"
+
+#include "arm7tdmi.h"
+
+#include "arm7_9_common.h"
+#include "register.h"
+#include "target.h"
+#include "armv4_5.h"
+#include "embeddedice.h"
+#include "etm.h"
+#include "log.h"
+#include "jtag.h"
+#include "arm_jtag.h"
+
+#include <stdlib.h>
+#include <string.h>
+
+#if 0
+#define _DEBUG_INSTRUCTION_EXECUTION_
+#endif
+
+/* cli handling */
+int arm7tdmi_register_commands(struct command_context_s *cmd_ctx);
+
+/* forward declarations */
+int arm7tdmi_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target);
+int arm7tdmi_init_target(struct command_context_s *cmd_ctx, struct target_s *target);
+int arm7tdmi_quit();
+
+/* target function declarations */
+enum target_state arm7tdmi_poll(struct target_s *target);
+int arm7tdmi_halt(target_t *target);
+int arm7tdmi_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);
+		
+target_type_t arm7tdmi_target =
+{
+	.name = "arm7tdmi",
+
+	.poll = arm7_9_poll,
+	.arch_state = armv4_5_arch_state,
+
+	.halt = arm7_9_halt,
+	.resume = arm7_9_resume,
+	.step = arm7_9_step,
+
+	.assert_reset = arm7_9_assert_reset,
+	.deassert_reset = arm7_9_deassert_reset,
+	.soft_reset_halt = arm7_9_soft_reset_halt,
+
+	.get_gdb_reg_list = armv4_5_get_gdb_reg_list,
+	
+	.read_memory = arm7_9_read_memory,
+	.write_memory = arm7_9_write_memory,
+	.bulk_write_memory = arm7_9_bulk_write_memory,
+
+	.run_algorithm = armv4_5_run_algorithm,
+	
+	.add_breakpoint = arm7_9_add_breakpoint,
+	.remove_breakpoint = arm7_9_remove_breakpoint,
+	.add_watchpoint = arm7_9_add_watchpoint,
+	.remove_watchpoint = arm7_9_remove_watchpoint,
+
+	.register_commands = arm7tdmi_register_commands,
+	.target_command = arm7tdmi_target_command,
+	.init_target = arm7tdmi_init_target,
+	.quit = arm7tdmi_quit
+};
+
+int arm7tdmi_examine_debug_reason(target_t *target)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	
+	/* only check the debug reason if we don't know it already */
+	if ((target->debug_reason != DBG_REASON_DBGRQ)
+			&& (target->debug_reason != DBG_REASON_SINGLESTEP))
+	{
+		scan_field_t fields[2];
+		u8 databus[4];
+		u8 breakpoint;
+		
+		jtag_add_end_state(TAP_PD);
+
+		fields[0].device = arm7_9->jtag_info.chain_pos;
+		fields[0].num_bits = 1;
+		fields[0].out_value = NULL;
+		fields[0].out_mask = NULL;
+		fields[0].in_value = &breakpoint;
+		fields[0].in_check_value = NULL;
+		fields[0].in_check_mask = NULL;
+		fields[0].in_handler = NULL;
+		fields[0].in_handler_priv = NULL;
+		
+		fields[1].device = arm7_9->jtag_info.chain_pos;
+		fields[1].num_bits = 32;
+		fields[1].out_value = NULL;
+		fields[1].out_mask = NULL;
+		fields[1].in_value = databus;
+		fields[1].in_check_value = NULL;
+		fields[1].in_check_mask = NULL;
+		fields[1].in_handler = NULL;
+		fields[1].in_handler_priv = NULL;
+		
+		arm_jtag_scann(&arm7_9->jtag_info, 0x1);
+		arm_jtag_set_instr(&arm7_9->jtag_info, arm7_9->jtag_info.intest_instr);
+
+		jtag_add_dr_scan(2, fields, TAP_PD);
+		jtag_execute_queue();
+		
+		fields[0].in_value = NULL;
+		fields[0].out_value = &breakpoint;
+		fields[1].in_value = NULL;
+		fields[1].out_value = databus;
+		
+		jtag_add_dr_scan(2, fields, TAP_PD);
+
+		if (breakpoint & 1)
+			target->debug_reason = DBG_REASON_WATCHPOINT; 
+		else
+			target->debug_reason = DBG_REASON_BREAKPOINT; 
+	}
+
+	return ERROR_OK;
+}
+
+/* put an instruction in the ARM7TDMI pipeline or write the data bus, and optionally read data */
+int arm7tdmi_clock_out(arm_jtag_t *jtag_info, u32 out, u32 *in, int breakpoint)
+{
+	scan_field_t fields[2];
+	u8 out_buf[4];
+	u8 breakpoint_buf;
+	
+	out = flip_u32(out, 32);
+	buf_set_u32(out_buf, 0, 32, out);
+	buf_set_u32(&breakpoint_buf, 0, 1, breakpoint);
+
+	jtag_add_end_state(TAP_PD);
+	arm_jtag_scann(jtag_info, 0x1);
+	arm_jtag_set_instr(jtag_info, jtag_info->intest_instr);
+	
+	fields[0].device = jtag_info->chain_pos;
+	fields[0].num_bits = 1;
+	fields[0].out_value = &breakpoint_buf;
+	fields[0].out_mask = NULL;
+	fields[0].in_value = NULL;
+	fields[0].in_check_value = NULL;
+	fields[0].in_check_mask = NULL;
+	fields[0].in_handler = NULL;
+	fields[0].in_handler_priv = NULL;
+		
+	fields[1].device = jtag_info->chain_pos;
+	fields[1].num_bits = 32;
+	fields[1].out_value = out_buf;
+	fields[1].out_mask = NULL;
+	if (in)
+	{
+		fields[1].in_value = (u8*)in;
+		fields[1].in_handler = arm_jtag_buf_to_u32_flip;
+		fields[1].in_handler_priv = in;
+	} else
+	{
+		fields[1].in_value = NULL;
+		fields[1].in_handler = NULL;
+		fields[1].in_handler_priv = NULL;
+	}
+
+	fields[1].in_check_value = NULL;
+	fields[1].in_check_mask = NULL;
+
+	jtag_add_dr_scan(2, fields, -1);
+
+	jtag_add_runtest(0, -1);
+	
+#ifdef _DEBUG_INSTRUCTION_EXECUTION_
+{
+		char* in_string;
+		jtag_execute_queue();
+		
+		if (in)
+		{
+			in_string = buf_to_char((u8*)in, 32);
+			DEBUG("out: 0x%8.8x, in: %s", flip_u32(out, 32), in_string);
+			free(in_string);
+		}
+		else
+			DEBUG("out: 0x%8.8x", flip_u32(out, 32));
+}
+#endif
+
+	return ERROR_OK;
+}
+
+/* put an instruction in the ARM7TDMI pipeline, and optionally read data */
+int arm7tdmi_clock_data_in(arm_jtag_t *jtag_info, u32 *in)
+{
+	scan_field_t fields[2];
+
+	jtag_add_end_state(TAP_PD);
+	arm_jtag_scann(jtag_info, 0x1);
+	arm_jtag_set_instr(jtag_info, jtag_info->intest_instr);
+	
+	fields[0].device = jtag_info->chain_pos;
+	fields[0].num_bits = 1;
+	fields[0].out_value = NULL;
+	fields[0].out_mask = NULL;
+	fields[0].in_value = NULL;
+	fields[0].in_check_value = NULL;
+	fields[0].in_check_mask = NULL;
+	fields[0].in_handler = NULL;
+	fields[0].in_handler_priv = NULL;
+		
+	fields[1].device = jtag_info->chain_pos;
+	fields[1].num_bits = 32;
+	fields[1].out_value = NULL;
+	fields[1].out_mask = NULL;
+	fields[1].in_value = (u8*)in;
+	fields[1].in_handler = arm_jtag_buf_to_u32_flip;
+	fields[1].in_handler_priv = in;
+	fields[1].in_check_value = NULL;
+	fields[1].in_check_mask = NULL;
+
+	jtag_add_dr_scan(2, fields, -1);
+
+	jtag_add_runtest(0, -1);
+	
+#ifdef _DEBUG_INSTRUCTION_EXECUTION_
+{
+		char* in_string;
+		jtag_execute_queue();
+			
+		if (in)
+		{
+			in_string = buf_to_char((u8*)in, 32);
+			DEBUG("in: %s", in_string);
+			free(in_string);
+		}
+}
+#endif
+
+	return ERROR_OK;
+}
+
+void arm7tdmi_change_to_arm(target_t *target, u32 *r0, u32 *pc)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+	
+	/* save r0 before using it and put system in ARM state 
+	 * to allow common handling of ARM and THUMB debugging */
+	
+	/* fetch STR r0, [r0] */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_STR(0, 0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	/* nothing fetched, STR r0, [r0] in Execute (2) */
+	arm7tdmi_clock_data_in(jtag_info, r0);
+
+	/* MOV r0, r15 fetched, STR in Decode */	
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_MOV(0, 15), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_STR(0, 0), NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	/* nothing fetched, STR r0, [r0] in Execute (2) */
+	arm7tdmi_clock_data_in(jtag_info, pc);
+
+	/* fetch MOV */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_MOV_IM(0, 0x0), NULL, 0);
+	
+	/* fetch BX */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_BX(0), NULL, 0);
+	/* NOP fetched, BX in Decode, MOV in Execute */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	/* NOP fetched, BX in Execute (1) */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	
+	jtag_execute_queue();
+	
+	/* fix program counter:
+	 * MOV r0, r15 was the 4th instruction (+6)
+	 * reading PC in Thumb state gives address of instruction + 4
+	 */
+	*pc -= 0xa;
+	
+}
+
+void arm7tdmi_read_core_regs(target_t *target, u32 mask, u32* core_regs[16])
+{
+	int i;
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+		
+	/* STMIA r0-15, [r0] at debug speed
+	 * register values will start to appear on 4th DCLK
+	 */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask & 0xffff, 0, 0), NULL, 0);
+
+	/* fetch NOP, STM in DECODE stage */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	/* fetch NOP, STM in EXECUTE stage (1st cycle) */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+
+	for (i = 0; i <= 15; i++)
+	{
+		if (mask & (1 << i))
+			/* nothing fetched, STM still in EXECUTE (1+i cycle) */
+			arm7tdmi_clock_data_in(jtag_info, core_regs[i]);
+	}
+
+}
+
+void arm7tdmi_read_xpsr(target_t *target, u32 *xpsr, int spsr)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+		
+	/* MRS r0, cpsr */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_MRS(0, spsr & 1), NULL, 0);
+	
+	/* STR r0, [r15] */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_STR(0, 15), NULL, 0);
+	/* fetch NOP, STR in DECODE stage */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	/* fetch NOP, STR in EXECUTE stage (1st cycle) */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	/* nothing fetched, STR still in EXECUTE (2nd cycle) */
+	arm7tdmi_clock_data_in(jtag_info, xpsr);
+
+}
+
+void arm7tdmi_write_xpsr(target_t *target, u32 xpsr, int spsr)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+		
+	DEBUG("xpsr: %8.8x, spsr: %i", xpsr, spsr);
+
+	/* MSR1 fetched */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM(xpsr & 0xff, 0, 1, spsr), NULL, 0);
+	/* MSR2 fetched, MSR1 in DECODE */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff00) >> 8, 0xc, 2, spsr), NULL, 0);
+	/* MSR3 fetched, MSR1 in EXECUTE (1), MSR2 in DECODE */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff0000) >> 16, 0x8, 4, spsr), NULL, 0);
+	/* nothing fetched, MSR1 in EXECUTE (2) */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	/* MSR4 fetched, MSR2 in EXECUTE (1), MSR3 in DECODE */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff000000) >> 24, 0x4, 8, spsr), NULL, 0);
+	/* nothing fetched, MSR2 in EXECUTE (2) */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	/* NOP fetched, MSR3 in EXECUTE (1), MSR4 in DECODE */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	/* nothing fetched, MSR3 in EXECUTE (2) */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	/* NOP fetched, MSR4 in EXECUTE (1) */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	/* nothing fetched, MSR4 in EXECUTE (2) */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+}
+
+void arm7tdmi_write_xpsr_im8(target_t *target, u8 xpsr_im, int rot, int spsr)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+		
+	DEBUG("xpsr_im: %2.2x, rot: %i, spsr: %i", xpsr_im, rot, spsr);
+	
+	/* MSR fetched */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM(xpsr_im, rot, 1, spsr), NULL, 0);
+	/* NOP fetched, MSR in DECODE */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	/* NOP fetched, MSR in EXECUTE (1) */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	/* nothing fetched, MSR in EXECUTE (2) */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	
+}
+
+void arm7tdmi_write_core_regs(target_t *target, u32 mask, u32 core_regs[16])
+{
+	int i;
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+		
+	/* LDMIA r0-15, [r0] at debug speed
+	* register values will start to appear on 4th DCLK
+	*/
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, mask & 0xffff, 0, 0), NULL, 0);
+
+	/* fetch NOP, LDM in DECODE stage */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	/* fetch NOP, LDM in EXECUTE stage (1st cycle) */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+
+	for (i = 0; i <= 15; i++)
+	{
+		if (mask & (1 << i))
+			/* nothing fetched, LDM still in EXECUTE (1+i cycle) */
+			arm7tdmi_clock_out(jtag_info, core_regs[i], NULL, 0);
+	}
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	
+}
+
+void arm7tdmi_load_word_regs(target_t *target, u32 mask)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+
+	/* put system-speed load-multiple into the pipeline */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, mask & 0xffff, 0, 1), NULL, 0);
+
+}
+
+void arm7tdmi_load_hword_reg(target_t *target, int num)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+	
+	/* put system-speed load half-word into the pipeline */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDRH_IP(num, 0), NULL, 0);
+
+}
+
+void arm7tdmi_load_byte_reg(target_t *target, int num)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+
+	/* put system-speed load byte into the pipeline */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDRB_IP(num, 0), NULL, 0);
+
+}
+
+void arm7tdmi_store_word_regs(target_t *target, u32 mask)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+
+	/* put system-speed store-multiple into the pipeline */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask, 0, 1), NULL, 0);
+	
+}
+
+void arm7tdmi_store_hword_reg(target_t *target, int num)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+
+	/* put system-speed store half-word into the pipeline */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_STRH_IP(num, 0), NULL, 0);
+
+}
+
+void arm7tdmi_store_byte_reg(target_t *target, int num)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+
+	/* put system-speed store byte into the pipeline */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_STRB_IP(num, 0), NULL, 0);
+
+}
+
+void arm7tdmi_write_pc(target_t *target, u32 pc)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+	
+	/* LDMIA r0-15, [r0] at debug speed
+	 * register values will start to appear on 4th DCLK
+	 */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, 0x8000, 0, 0), NULL, 0);
+	/* fetch NOP, LDM in DECODE stage */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	/* fetch NOP, LDM in EXECUTE stage (1st cycle) */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	/* nothing fetched, LDM in EXECUTE stage (1st cycle) load register */
+	arm7tdmi_clock_out(jtag_info, pc, NULL, 0);
+	/* nothing fetched, LDM in EXECUTE stage (2nd cycle) load register */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	/* nothing fetched, LDM in EXECUTE stage (3rd cycle) load register */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	/* fetch NOP, LDM in EXECUTE stage (4th cycle) */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	/* fetch NOP, LDM in EXECUTE stage (5th cycle) */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+}
+
+void arm7tdmi_branch_resume(target_t *target)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+	
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_B(0xfffffa, 0), NULL, 0);
+
+}
+
+void arm7tdmi_branch_resume_thumb(target_t *target)
+{
+	DEBUG("");
+	
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+	reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
+
+	/* LDMIA r0, [r0] at debug speed
+	 * register values will start to appear on 4th DCLK
+	 */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, 0x1, 0, 0), NULL, 0);
+
+	/* fetch NOP, LDM in DECODE stage */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	/* fetch NOP, LDM in EXECUTE stage (1st cycle) */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	/* nothing fetched, LDM in EXECUTE stage (2nd cycle) */
+	arm7tdmi_clock_out(jtag_info, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32) | 1, NULL, 0);
+	/* nothing fetched, LDM in EXECUTE stage (3rd cycle) */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+
+	/* Branch and eXchange */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_BX(0), NULL, 0);
+	
+	embeddedice_read_reg(dbg_stat);
+	
+	/* fetch NOP, BX in DECODE stage */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+	
+	/* target is now in Thumb state */
+	embeddedice_read_reg(dbg_stat);
+	
+	/* fetch NOP, BX in EXECUTE stage (1st cycle) */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
+
+	/* target is now in Thumb state */
+	embeddedice_read_reg(dbg_stat);
+
+	/* clean r0 bits to avoid alignment problems */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_MOV_IM(0, 0x0), NULL, 0);
+	/* load r0 value, MOV_IM in Decode*/
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_LDR(0, 0), NULL, 0);
+	/* fetch NOP, LDR in Decode, MOV_IM in Execute */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	/* fetch NOP, LDR in Execute */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	/* nothing fetched, LDR in EXECUTE stage (2nd cycle) */
+	arm7tdmi_clock_out(jtag_info, buf_get_u32(armv4_5->core_cache->reg_list[0].value, 0, 32), NULL, 0);
+	/* nothing fetched, LDR in EXECUTE stage (3rd cycle) */
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
+
+	embeddedice_read_reg(dbg_stat);
+	
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 1);
+	arm7tdmi_clock_out(jtag_info, ARMV4_5_T_B(0x7f7), NULL, 0);
+
+}
+		
+void arm7tdmi_build_reg_cache(target_t *target)
+{
+	reg_cache_t **cache_p = register_get_last_cache_p(&target->reg_cache);
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+	arm7tdmi_common_t *arch_info = arm7_9->arch_info;
+
+
+	(*cache_p) = armv4_5_build_reg_cache(target, armv4_5);
+	armv4_5->core_cache = (*cache_p);
+	
+	(*cache_p)->next = embeddedice_build_reg_cache(target, jtag_info, 0);
+	arm7_9->eice_cache = (*cache_p)->next;
+	
+	if (arm7_9->has_etm)
+	{
+		(*cache_p)->next->next = etm_build_reg_cache(target, jtag_info, 0);
+		arm7_9->etm_cache = (*cache_p)->next->next;
+	}
+		
+	if (arch_info->has_monitor_mode)
+		(*cache_p)->next->reg_list[0].size = 6;
+	else
+		(*cache_p)->next->reg_list[0].size = 3;
+	
+	(*cache_p)->next->reg_list[1].size = 5;
+
+}
+
+int arm7tdmi_init_target(struct command_context_s *cmd_ctx, struct target_s *target)
+{
+	
+	arm7tdmi_build_reg_cache(target);
+	
+	return ERROR_OK;
+	
+}
+
+int arm7tdmi_quit()
+{
+	
+	return ERROR_OK;
+}
+
+int arm7tdmi_init_arch_info(target_t *target, arm7tdmi_common_t *arm7tdmi, int chain_pos, char *variant)
+{
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	int has_etm = 0;
+	
+	arm7_9 = &arm7tdmi->arm7_9_common;
+	armv4_5 = &arm7_9->armv4_5_common;
+	
+	/* prepare JTAG information for the new target */
+	arm7_9->jtag_info.chain_pos = chain_pos;
+	arm7_9->jtag_info.scann_size = 4;
+	
+	/* register arch-specific functions */
+	arm7_9->examine_debug_reason = arm7tdmi_examine_debug_reason;
+	arm7_9->change_to_arm = arm7tdmi_change_to_arm;
+	arm7_9->read_core_regs = arm7tdmi_read_core_regs;
+	arm7_9->read_xpsr = arm7tdmi_read_xpsr;
+	
+	arm7_9->write_xpsr = arm7tdmi_write_xpsr;
+	arm7_9->write_xpsr_im8 = arm7tdmi_write_xpsr_im8;
+	arm7_9->write_core_regs = arm7tdmi_write_core_regs;
+	
+	arm7_9->load_word_regs = arm7tdmi_load_word_regs;
+	arm7_9->load_hword_reg = arm7tdmi_load_hword_reg;
+	arm7_9->load_byte_reg = arm7tdmi_load_byte_reg;
+	
+	arm7_9->store_word_regs = arm7tdmi_store_word_regs;
+	arm7_9->store_hword_reg = arm7tdmi_store_hword_reg;
+	arm7_9->store_byte_reg = arm7tdmi_store_byte_reg;
+	
+	arm7_9->write_pc = arm7tdmi_write_pc;
+	arm7_9->branch_resume = arm7tdmi_branch_resume;
+	arm7_9->branch_resume_thumb = arm7tdmi_branch_resume_thumb;
+	
+	arm7_9->enable_single_step = arm7_9_enable_eice_step;
+	arm7_9->disable_single_step = arm7_9_disable_eice_step;
+		
+	arm7_9->pre_debug_entry = NULL;
+	arm7_9->post_debug_entry = NULL;
+	
+	arm7_9->pre_restore_context = NULL;
+	arm7_9->post_restore_context = NULL;
+	
+	/* initialize arch-specific breakpoint handling */
+	buf_set_u32((u8*)(&arm7_9->arm_bkpt), 0, 32, 0xdeeedeee);
+	buf_set_u32((u8*)(&arm7_9->thumb_bkpt), 0, 16, 0xdeee);
+	
+	arm7_9->sw_bkpts_use_wp = 1;
+	arm7_9->sw_bkpts_enabled = 0;
+	arm7_9->dbgreq_adjust_pc = 2;
+	arm7_9->arch_info = arm7tdmi;
+	
+	arm7tdmi->has_monitor_mode = 0;
+	arm7tdmi->arch_info = NULL;
+	arm7tdmi->common_magic = ARM7TDMI_COMMON_MAGIC;
+	
+	if (variant)
+	{
+		if (strcmp(variant, "arm7tdmi-s_r4") == 0)
+			arm7tdmi->has_monitor_mode = 1;
+		else if (strcmp(variant, "arm7tdmi_r4") == 0)
+			arm7tdmi->has_monitor_mode = 1;
+		else if (strcmp(variant, "lpc2000") == 0)
+		{
+			arm7tdmi->has_monitor_mode = 1;
+			has_etm = 1;
+		}
+		arm7tdmi->variant = strdup(variant);
+	}
+	else
+		arm7tdmi->variant = strdup("");
+	
+	arm7_9_init_arch_info(target, arm7_9);
+	
+	arm7_9->has_etm = has_etm;
+
+	return ERROR_OK;
+}
+
+/* target arm7tdmi <endianess> <startup_mode> <chain_pos> <variant> */
+int arm7tdmi_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target)
+{
+	int chain_pos;
+	char *variant = NULL;
+	arm7tdmi_common_t *arm7tdmi = malloc(sizeof(arm7tdmi_common_t));
+
+	if (argc < 4)
+	{
+		ERROR("'target arm7tdmi' requires at least one additional argument");
+		exit(-1);
+	}
+	
+	chain_pos = strtoul(args[2], NULL, 0);
+	
+	if (argc >= 5)
+		variant = args[4];
+	
+	arm7tdmi_init_arch_info(target, arm7tdmi, chain_pos, variant);
+	
+	return ERROR_OK;
+}
+
+int arm7tdmi_register_commands(struct command_context_s *cmd_ctx)
+{
+	int retval;
+	
+	retval = arm7_9_register_commands(cmd_ctx);
+	
+	return ERROR_OK;
+
+}
+
diff --git a/src/target/arm7tdmi.h b/src/target/arm7tdmi.h
new file mode 100644
index 00000000..ca2df8b6
--- /dev/null
+++ b/src/target/arm7tdmi.h
@@ -0,0 +1,46 @@
+/***************************************************************************
+ *   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 ARM7TDMI_H
+#define ARM7TDMI_H
+
+#include "target.h"
+#include "register.h"
+#include "armv4_5.h"
+#include "embeddedice.h"
+#include "arm_jtag.h"
+#include "arm7_9_common.h"
+
+#define	ARM7TDMI_COMMON_MAGIC 0x00a700a7
+
+typedef struct arm7tdmi_common_s
+{
+	int common_magic;
+	char *variant;
+	int has_monitor_mode;
+	void *arch_info;
+	arm7_9_common_t arm7_9_common;
+} arm7tdmi_common_t;
+
+int arm7tdmi_register_commands(struct command_context_s *cmd_ctx);
+int arm7tdmi_init_arch_info(target_t *target, arm7tdmi_common_t *arm7tdmi, int chain_pos, char *variant);
+int arm7tdmi_init_target(struct command_context_s *cmd_ctx, struct target_s *target);
+
+
+#endif /* ARM7TDMI_H */
diff --git a/src/target/arm920t.c b/src/target/arm920t.c
new file mode 100644
index 00000000..eb0fa7df
--- /dev/null
+++ b/src/target/arm920t.c
@@ -0,0 +1,967 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "config.h"
+
+#include "arm920t.h"
+#include "jtag.h"
+#include "log.h"
+
+#include <stdlib.h>
+#include <string.h>
+
+#if 0
+#define _DEBUG_INSTRUCTION_EXECUTION_
+#endif
+
+/* cli handling */
+int arm920t_register_commands(struct command_context_s *cmd_ctx);
+
+int arm920t_handle_cp15_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int arm920t_handle_cp15i_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int arm920t_handle_virt2phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int arm920t_handle_cache_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int arm920t_handle_md_phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int arm920t_handle_mw_phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+/* forward declarations */
+int arm920t_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target);
+int arm920t_init_target(struct command_context_s *cmd_ctx, struct target_s *target);
+int arm920t_quit();
+int arm920t_arch_state(struct target_s *target, char *buf, int buf_size);
+int arm920t_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);
+int arm920t_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);
+int arm920t_soft_reset_halt(struct target_s *target);
+
+target_type_t arm920t_target =
+{
+	.name = "arm920t",
+
+	.poll = arm7_9_poll,
+	.arch_state = arm920t_arch_state,
+
+	.halt = arm7_9_halt,
+	.resume = arm7_9_resume,
+	.step = arm7_9_step,
+
+	.assert_reset = arm7_9_assert_reset,
+	.deassert_reset = arm7_9_deassert_reset,
+	.soft_reset_halt = arm920t_soft_reset_halt,
+	
+	.get_gdb_reg_list = armv4_5_get_gdb_reg_list,
+
+	.read_memory = arm920t_read_memory,
+	.write_memory = arm920t_write_memory,
+	.bulk_write_memory = arm7_9_bulk_write_memory,
+
+	.run_algorithm = armv4_5_run_algorithm,
+
+	.add_breakpoint = arm7_9_add_breakpoint,
+	.remove_breakpoint = arm7_9_remove_breakpoint,
+	.add_watchpoint = arm7_9_add_watchpoint,
+	.remove_watchpoint = arm7_9_remove_watchpoint,
+
+	.register_commands = arm920t_register_commands,
+	.target_command = arm920t_target_command,
+	.init_target = arm920t_init_target,
+	.quit = arm920t_quit
+};
+
+int arm920t_read_cp15_physical(target_t *target, int reg_addr, u32 *value)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+	scan_field_t fields[4];
+	u8 access_type_buf = 1;
+	u8 reg_addr_buf = reg_addr & 0x3f;
+	u8 nr_w_buf = 0;
+	
+	jtag_add_end_state(TAP_RTI);
+	arm_jtag_scann(jtag_info, 0xf);
+	arm_jtag_set_instr(jtag_info, jtag_info->intest_instr);
+
+	fields[0].device = jtag_info->chain_pos;
+	fields[0].num_bits = 1;
+	fields[0].out_value = &access_type_buf;
+	fields[0].out_mask = NULL;
+	fields[0].in_value = NULL;
+	fields[0].in_check_value = NULL;
+	fields[0].in_check_mask = NULL;
+	fields[0].in_handler = NULL;
+	fields[0].in_handler_priv = NULL;
+
+	fields[1].device = jtag_info->chain_pos;
+	fields[1].num_bits = 32;
+	fields[1].out_value = NULL;
+	fields[1].out_mask = NULL;
+	fields[1].in_value = NULL;
+	fields[1].in_check_value = NULL;
+	fields[1].in_check_mask = NULL;
+	fields[1].in_handler = NULL;
+	fields[1].in_handler_priv = NULL;
+
+	fields[2].device = jtag_info->chain_pos;
+	fields[2].num_bits = 6;
+	fields[2].out_value = &reg_addr_buf;
+	fields[2].out_mask = NULL;
+	fields[2].in_value = NULL;
+	fields[2].in_check_value = NULL;
+	fields[2].in_check_mask = NULL;
+	fields[2].in_handler = NULL;
+	fields[2].in_handler_priv = NULL;
+
+	fields[3].device = jtag_info->chain_pos;
+	fields[3].num_bits = 1;
+	fields[3].out_value = &nr_w_buf;
+	fields[3].out_mask = NULL;
+	fields[3].in_value = NULL;
+	fields[3].in_check_value = NULL;
+	fields[3].in_check_mask = NULL;
+	fields[3].in_handler = NULL;
+	fields[3].in_handler_priv = NULL;
+	
+	jtag_add_dr_scan(4, fields, -1);
+
+	fields[1].in_value = (u8*)value;
+
+	jtag_add_dr_scan(4, fields, -1);
+
+	return ERROR_OK;
+}
+
+int arm920t_write_cp15_physical(target_t *target, int reg_addr, u32 value)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+	scan_field_t fields[4];
+	u8 access_type_buf = 1;
+	u8 reg_addr_buf = reg_addr & 0x3f;
+	u8 nr_w_buf = 1;
+	
+	jtag_add_end_state(TAP_RTI);
+	arm_jtag_scann(jtag_info, 0xf);
+	arm_jtag_set_instr(jtag_info, jtag_info->intest_instr);
+
+	fields[0].device = jtag_info->chain_pos;
+	fields[0].num_bits = 1;
+	fields[0].out_value = &access_type_buf;
+	fields[0].out_mask = NULL;
+	fields[0].in_value = NULL;
+	fields[0].in_check_value = NULL;
+	fields[0].in_check_mask = NULL;
+	fields[0].in_handler = NULL;
+	fields[0].in_handler_priv = NULL;
+
+	fields[1].device = jtag_info->chain_pos;
+	fields[1].num_bits = 32;
+	fields[1].out_value = (u8*)&value;
+	fields[1].out_mask = NULL;
+	fields[1].in_value = NULL;
+	fields[1].in_check_value = NULL;
+	fields[1].in_check_mask = NULL;
+	fields[1].in_handler = NULL;
+	fields[1].in_handler_priv = NULL;
+
+	fields[2].device = jtag_info->chain_pos;
+	fields[2].num_bits = 6;
+	fields[2].out_value = &reg_addr_buf;
+	fields[2].out_mask = NULL;
+	fields[2].in_value = NULL;
+	fields[2].in_check_value = NULL;
+	fields[2].in_check_mask = NULL;
+	fields[2].in_handler = NULL;
+	fields[2].in_handler_priv = NULL;
+
+	fields[3].device = jtag_info->chain_pos;
+	fields[3].num_bits = 1;
+	fields[3].out_value = &nr_w_buf;
+	fields[3].out_mask = NULL;
+	fields[3].in_value = NULL;
+	fields[3].in_check_value = NULL;
+	fields[3].in_check_mask = NULL;
+	fields[3].in_handler = NULL;
+	fields[3].in_handler_priv = NULL;
+	
+	jtag_add_dr_scan(4, fields, -1);
+
+	return ERROR_OK;
+}
+
+int arm920t_read_cp15_interpreted(target_t *target, u32 opcode, u32 *value)
+{
+	u32 cp15c15 = 0x0;
+	scan_field_t fields[4];
+	u8 access_type_buf = 0;		/* interpreted access */
+	u8 reg_addr_buf = 0x0;
+	u8 nr_w_buf = 0;
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+	u32* context_p[1];	
+	
+	/* read-modify-write CP15 test state register 
+	* to enable interpreted access mode */
+	arm920t_read_cp15_physical(target, 0x1e, &cp15c15);	
+	jtag_execute_queue();
+	cp15c15 |= 1;	/* set interpret mode */
+	arm920t_write_cp15_physical(target, 0x1e, cp15c15);
+
+	jtag_add_end_state(TAP_RTI);
+	arm_jtag_scann(jtag_info, 0xf);
+	arm_jtag_set_instr(jtag_info, jtag_info->intest_instr);
+
+	fields[0].device = jtag_info->chain_pos;
+	fields[0].num_bits = 1;
+	fields[0].out_value = &access_type_buf;
+	fields[0].out_mask = NULL;
+	fields[0].in_value = NULL;
+	fields[0].in_check_value = NULL;
+	fields[0].in_check_mask = NULL;
+	fields[0].in_handler = NULL;
+	fields[0].in_handler_priv = NULL;
+
+	fields[1].device = jtag_info->chain_pos;
+	fields[1].num_bits = 32;
+	fields[1].out_value = (u8*)&opcode;
+	fields[1].out_mask = NULL;
+	fields[1].in_value = NULL;
+	fields[1].in_check_value = NULL;
+	fields[1].in_check_mask = NULL;
+	fields[1].in_handler = NULL;
+	fields[1].in_handler_priv = NULL;
+
+	fields[2].device = jtag_info->chain_pos;
+	fields[2].num_bits = 6;
+	fields[2].out_value = &reg_addr_buf;
+	fields[2].out_mask = NULL;
+	fields[2].in_value = NULL;
+	fields[2].in_check_value = NULL;
+	fields[2].in_check_mask = NULL;
+	fields[2].in_handler = NULL;
+	fields[2].in_handler_priv = NULL;
+
+	fields[3].device = jtag_info->chain_pos;
+	fields[3].num_bits = 1;
+	fields[3].out_value = &nr_w_buf;
+	fields[3].out_mask = NULL;
+	fields[3].in_value = NULL;
+	fields[3].in_check_value = NULL;
+	fields[3].in_check_mask = NULL;
+	fields[3].in_handler = NULL;
+	fields[3].in_handler_priv = NULL;
+
+	jtag_add_dr_scan(4, fields, -1);
+
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_LDR(0, 15), 0, NULL, 0);
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);
+	arm7_9_execute_sys_speed(target);
+	jtag_execute_queue();
+
+	/* read-modify-write CP15 test state register 
+	* to disable interpreted access mode */
+	arm920t_read_cp15_physical(target, 0x1e, &cp15c15);	
+	jtag_execute_queue();
+	cp15c15 &= ~1U;	/* clear interpret mode */
+	arm920t_write_cp15_physical(target, 0x1e, cp15c15);
+
+	context_p[0] = value;
+	arm9tdmi_read_core_regs(target, 0x1, context_p);
+	jtag_execute_queue();
+	
+	DEBUG("opcode: %8.8x, value: %8.8x", opcode, *value);
+
+	ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).dirty = 1;
+	ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 15).dirty = 1;
+
+	return ERROR_OK;
+}
+
+int arm920t_write_cp15_interpreted(target_t *target, u32 opcode, u32 value, u32 address)
+{
+	u32 cp15c15 = 0x0;
+	scan_field_t fields[4];
+	u8 access_type_buf = 0;		/* interpreted access */
+	u8 reg_addr_buf = 0x0;
+	u8 nr_w_buf = 0;
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+	u32 regs[2];
+
+	regs[0] = value;
+	regs[1] = address;
+	
+	arm9tdmi_write_core_regs(target, 0x3, regs);
+
+	/* read-modify-write CP15 test state register 
+	* to enable interpreted access mode */
+	arm920t_read_cp15_physical(target, 0x1e, &cp15c15);
+	jtag_execute_queue();
+	cp15c15 |= 1;	/* set interpret mode */
+	arm920t_write_cp15_physical(target, 0x1e, cp15c15);
+
+	jtag_add_end_state(TAP_RTI);
+	arm_jtag_scann(jtag_info, 0xf);
+	arm_jtag_set_instr(jtag_info, jtag_info->intest_instr);
+
+	fields[0].device = jtag_info->chain_pos;
+	fields[0].num_bits = 1;
+	fields[0].out_value = &access_type_buf;
+	fields[0].out_mask = NULL;
+	fields[0].in_value = NULL;
+	fields[0].in_check_value = NULL;
+	fields[0].in_check_mask = NULL;
+	fields[0].in_handler = NULL;
+	fields[0].in_handler_priv = NULL;
+
+	fields[1].device = jtag_info->chain_pos;
+	fields[1].num_bits = 32;
+	fields[1].out_value = (u8*)&opcode;
+	fields[1].out_mask = NULL;
+	fields[1].in_value = NULL;
+	fields[1].in_check_value = NULL;
+	fields[1].in_check_mask = NULL;
+	fields[1].in_handler = NULL;
+	fields[1].in_handler_priv = NULL;
+
+	fields[2].device = jtag_info->chain_pos;
+	fields[2].num_bits = 6;
+	fields[2].out_value = &reg_addr_buf;
+	fields[2].out_mask = NULL;
+	fields[2].in_value = NULL;
+	fields[2].in_check_value = NULL;
+	fields[2].in_check_mask = NULL;
+	fields[2].in_handler = NULL;
+	fields[2].in_handler_priv = NULL;
+
+	fields[3].device = jtag_info->chain_pos;
+	fields[3].num_bits = 1;
+	fields[3].out_value = &nr_w_buf;
+	fields[3].out_mask = NULL;
+	fields[3].in_value = NULL;
+	fields[3].in_check_value = NULL;
+	fields[3].in_check_mask = NULL;
+	fields[3].in_handler = NULL;
+	fields[3].in_handler_priv = NULL;
+
+	jtag_add_dr_scan(4, fields, -1);
+
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_STR(0, 1), 0, NULL, 0);
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);
+	arm7_9_execute_sys_speed(target);
+	jtag_execute_queue();
+
+	/* read-modify-write CP15 test state register 
+	* to disable interpreted access mode */
+	arm920t_read_cp15_physical(target, 0x1e, &cp15c15);	
+	jtag_execute_queue();
+	cp15c15 &= ~1U;	/* set interpret mode */
+	arm920t_write_cp15_physical(target, 0x1e, cp15c15);
+
+	DEBUG("opcode: %8.8x, value: %8.8x, address: %8.8x", opcode, value, address);
+
+	return ERROR_OK;
+}
+
+u32 arm920t_get_ttb(target_t *target)
+{
+	int retval;
+	u32 ttb = 0x0;
+
+	if ((retval = arm920t_read_cp15_interpreted(target, 0xeebf0f51, &ttb)) != ERROR_OK)
+		return retval;
+
+	return ttb;
+}
+
+void arm920t_disable_mmu_caches(target_t *target, int mmu, int d_u_cache, int i_cache)
+{
+	u32 cp15_control;
+
+	/* read cp15 control register */
+	arm920t_read_cp15_physical(target, 0x2, &cp15_control);
+	jtag_execute_queue();
+		
+	if (mmu)
+		cp15_control &= ~0x1U;
+	
+	if (d_u_cache)
+		cp15_control &= ~0x4U;
+	
+	if (i_cache)
+		cp15_control &= ~0x1000U;
+
+	arm920t_write_cp15_physical(target, 0x2, cp15_control);
+}
+
+void arm920t_enable_mmu_caches(target_t *target, int mmu, int d_u_cache, int i_cache)
+{
+	u32 cp15_control;
+
+	/* read cp15 control register */
+	arm920t_read_cp15_physical(target, 0x2, &cp15_control);
+	jtag_execute_queue();
+		
+	if (mmu)
+		cp15_control |= 0x1U;
+	
+	if (d_u_cache)
+		cp15_control |= 0x4U;
+	
+	if (i_cache)
+		cp15_control |= 0x1000U;
+	
+	arm920t_write_cp15_physical(target, 0x2, cp15_control);
+}
+
+void arm920t_post_debug_entry(target_t *target)
+{
+	u32 cp15c15;
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;
+	arm920t_common_t *arm920t = arm9tdmi->arch_info;
+	
+	/* examine cp15 control reg */
+	arm920t_read_cp15_physical(target, 0x2, &arm920t->cp15_control_reg);
+	jtag_execute_queue();
+	DEBUG("cp15_control_reg: %8.8x", arm920t->cp15_control_reg);
+
+	if (arm920t->armv4_5_mmu.armv4_5_cache.ctype == -1)
+	{
+		u32 cache_type_reg;
+		/* identify caches */
+		arm920t_read_cp15_physical(target, 0x1, &cache_type_reg);
+		jtag_execute_queue();
+		armv4_5_identify_cache(cache_type_reg, &arm920t->armv4_5_mmu.armv4_5_cache);
+	}
+
+	arm920t->armv4_5_mmu.mmu_enabled = (arm920t->cp15_control_reg & 0x1U) ? 1 : 0;
+	arm920t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = (arm920t->cp15_control_reg & 0x4U) ? 1 : 0;
+	arm920t->armv4_5_mmu.armv4_5_cache.i_cache_enabled = (arm920t->cp15_control_reg & 0x1000U) ? 1 : 0;
+
+	/* save i/d fault status and address register */
+	arm920t_read_cp15_interpreted(target, 0xee150f10, &arm920t->d_fsr);
+	arm920t_read_cp15_interpreted(target, 0xee150f30, &arm920t->i_fsr);
+	arm920t_read_cp15_interpreted(target, 0xee160f10, &arm920t->d_far);
+	arm920t_read_cp15_interpreted(target, 0xee160f30, &arm920t->i_far);
+
+	/* read-modify-write CP15 test state register 
+	* to disable I/D-cache linefills */
+	arm920t_read_cp15_physical(target, 0x1e, &cp15c15);	
+	jtag_execute_queue();
+	cp15c15 |= 0x600;
+	arm920t_write_cp15_physical(target, 0x1e, cp15c15);
+
+}
+
+void arm920t_pre_restore_context(target_t *target)
+{
+	u32 cp15c15;
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;
+	arm920t_common_t *arm920t = arm9tdmi->arch_info;
+	
+	/* restore i/d fault status and address register */
+	arm920t_write_cp15_interpreted(target, 0xee050f10, arm920t->d_fsr, 0x0);
+	arm920t_write_cp15_interpreted(target, 0xee050f30, arm920t->i_fsr, 0x0);
+	arm920t_write_cp15_interpreted(target, 0xee060f10, arm920t->d_far, 0x0);
+	arm920t_write_cp15_interpreted(target, 0xee060f30, arm920t->i_far, 0x0);
+	
+	/* read-modify-write CP15 test state register 
+	* to reenable I/D-cache linefills */
+	arm920t_read_cp15_physical(target, 0x1e, &cp15c15);
+	jtag_execute_queue();
+	cp15c15 &= ~0x600U;
+	arm920t_write_cp15_physical(target, 0x1e, cp15c15);
+
+}
+
+int arm920t_get_arch_pointers(target_t *target, armv4_5_common_t **armv4_5_p, arm7_9_common_t **arm7_9_p, arm9tdmi_common_t **arm9tdmi_p, arm920t_common_t **arm920t_p)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9;
+	arm9tdmi_common_t *arm9tdmi;
+	arm920t_common_t *arm920t;
+	
+	if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
+	{
+		return -1;
+	}
+	
+	arm7_9 = armv4_5->arch_info;
+	if (arm7_9->common_magic != ARM7_9_COMMON_MAGIC)
+	{
+		return -1;
+	}
+	
+	arm9tdmi = arm7_9->arch_info;
+	if (arm9tdmi->common_magic != ARM9TDMI_COMMON_MAGIC)
+	{
+		return -1;
+	}
+	
+	arm920t = arm9tdmi->arch_info;
+	if (arm920t->common_magic != ARM920T_COMMON_MAGIC)
+	{
+		return -1;
+	}
+	
+	*armv4_5_p = armv4_5;
+	*arm7_9_p = arm7_9;
+	*arm9tdmi_p = arm9tdmi;
+	*arm920t_p = arm920t;
+	
+	return ERROR_OK;
+}
+
+int arm920t_arch_state(struct target_s *target, char *buf, int buf_size)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;
+	arm920t_common_t *arm920t = arm9tdmi->arch_info;
+	
+	char *state[] = 
+	{
+		"disabled", "enabled"
+	};
+	
+	if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
+	{
+		ERROR("BUG: called for a non-ARMv4/5 target");
+		exit(-1);
+	}
+	
+	snprintf(buf, buf_size,
+			"target halted in %s state due to %s, current mode: %s\n"
+			"cpsr: 0x%8.8x pc: 0x%8.8x\n"
+			"MMU: %s, D-Cache: %s, I-Cache: %s",
+			 armv4_5_state_strings[armv4_5->core_state],
+			 target_debug_reason_strings[target->debug_reason],
+			 armv4_5_mode_strings[armv4_5_mode_to_number(armv4_5->core_mode)],
+			 buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32),
+			 buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32),
+			 state[arm920t->armv4_5_mmu.mmu_enabled],
+			 state[arm920t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled], 
+			 state[arm920t->armv4_5_mmu.armv4_5_cache.i_cache_enabled]);
+	
+	return ERROR_OK;
+}
+
+int arm920t_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
+{
+	int retval;
+	
+	retval = arm7_9_read_memory(target, address, size, count, buffer);
+	
+	return retval;
+}
+
+int arm920t_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
+{
+	int retval;
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;
+	arm920t_common_t *arm920t = arm9tdmi->arch_info;
+	
+	if ((retval = arm7_9_write_memory(target, address, size, count, buffer)) != ERROR_OK)
+		return retval;
+
+	if (((size == 4) || (size == 2)) && (count == 1))
+	{
+		if (arm920t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled)
+		{
+			DEBUG("D-Cache enabled, writing through to main memory");
+			u32 pa, cb, ap;
+			int type, domain;
+
+			pa = armv4_5_mmu_translate_va(target, &arm920t->armv4_5_mmu, address, &type, &cb, &domain, &ap);
+			if (type == -1)
+				return ERROR_OK;
+			/* cacheable & bufferable means write-back region */
+			if (cb == 3)
+				armv4_5_mmu_write_physical(target, &arm920t->armv4_5_mmu, pa, size, count, buffer);
+		}
+		
+		if (arm920t->armv4_5_mmu.armv4_5_cache.i_cache_enabled)
+		{
+			DEBUG("I-Cache enabled, invalidating affected I-Cache line");
+			arm920t_write_cp15_interpreted(target, 0xee070f35, 0x0, address);
+		}
+	}
+
+	return retval;
+}
+
+int arm920t_soft_reset_halt(struct target_s *target)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;
+	arm920t_common_t *arm920t = arm9tdmi->arch_info;
+	reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
+	
+	if (target->state == TARGET_RUNNING)
+	{
+		target->type->halt(target);
+	}
+	
+	while (buf_get_u32(dbg_stat->value, EICE_DBG_CONTROL_DBGACK, 1) == 0)
+	{
+		embeddedice_read_reg(dbg_stat);
+		jtag_execute_queue();
+	}
+	
+	target->state = TARGET_HALTED;
+	
+	/* SVC, ARM state, IRQ and FIQ disabled */
+	buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8, 0xd3);
+	armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 1;
+	armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
+	
+	/* start fetching from 0x0 */
+	buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, 0x0);
+	armv4_5->core_cache->reg_list[15].dirty = 1;
+	armv4_5->core_cache->reg_list[15].valid = 1;
+	
+	armv4_5->core_mode = ARMV4_5_MODE_SVC;
+	armv4_5->core_state = ARMV4_5_STATE_ARM;
+	
+	arm920t_disable_mmu_caches(target, 1, 1, 1);
+	arm920t->armv4_5_mmu.mmu_enabled = 0;
+	arm920t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = 0;
+	arm920t->armv4_5_mmu.armv4_5_cache.i_cache_enabled = 0;
+
+	target_call_event_callbacks(target, TARGET_EVENT_HALTED);
+	
+	return ERROR_OK;
+}
+
+int arm920t_init_target(struct command_context_s *cmd_ctx, struct target_s *target)
+{
+	arm9tdmi_init_target(cmd_ctx, target);
+		
+	return ERROR_OK;
+	
+}
+
+int arm920t_quit()
+{
+	
+	return ERROR_OK;
+}
+
+int arm920t_init_arch_info(target_t *target, arm920t_common_t *arm920t, int chain_pos, char *variant)
+{
+	arm9tdmi_common_t *arm9tdmi = &arm920t->arm9tdmi_common;
+	arm7_9_common_t *arm7_9 = &arm9tdmi->arm7_9_common;
+	
+	arm9tdmi_init_arch_info(target, arm9tdmi, chain_pos, variant);
+
+	arm9tdmi->arch_info = arm920t;
+	arm920t->common_magic = ARM920T_COMMON_MAGIC;
+	
+	arm7_9->post_debug_entry = arm920t_post_debug_entry;
+	arm7_9->pre_restore_context = arm920t_pre_restore_context;
+	
+	arm920t->armv4_5_mmu.armv4_5_cache.ctype = -1;
+	arm920t->armv4_5_mmu.get_ttb = arm920t_get_ttb;
+	arm920t->armv4_5_mmu.read_memory = arm7_9_read_memory;
+	arm920t->armv4_5_mmu.write_memory = arm7_9_write_memory;
+	arm920t->armv4_5_mmu.disable_mmu_caches = arm920t_disable_mmu_caches;
+	arm920t->armv4_5_mmu.enable_mmu_caches = arm920t_enable_mmu_caches;
+	arm920t->armv4_5_mmu.has_tiny_pages = 1;
+	arm920t->armv4_5_mmu.mmu_enabled = 0;
+	
+	arm9tdmi->has_single_step = 1;
+	
+	return ERROR_OK;
+}
+
+int arm920t_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target)
+{
+	int chain_pos;
+	char *variant = NULL;
+	arm920t_common_t *arm920t = malloc(sizeof(arm920t_common_t));
+	
+	if (argc < 4)
+	{
+		ERROR("'target arm920t' requires at least one additional argument");
+		exit(-1);
+	}
+	
+	chain_pos = strtoul(args[3], NULL, 0);
+	
+	if (argc >= 5)
+		variant = strdup(args[4]);
+	
+	DEBUG("chain_pos: %i, variant: %s", chain_pos, variant);
+	
+	arm920t_init_arch_info(target, arm920t, chain_pos, variant);
+
+	return ERROR_OK;
+}
+
+int arm920t_register_commands(struct command_context_s *cmd_ctx)
+{
+	int retval;
+	command_t *arm920t_cmd;
+	
+		
+	retval = arm9tdmi_register_commands(cmd_ctx);
+	
+	arm920t_cmd = register_command(cmd_ctx, NULL, "arm920t", NULL, COMMAND_ANY, "arm920t specific commands");
+
+	register_command(cmd_ctx, arm920t_cmd, "cp15", arm920t_handle_cp15_command, COMMAND_EXEC, "display/modify cp15 register <num> [value]");
+	register_command(cmd_ctx, arm920t_cmd, "cp15i", arm920t_handle_cp15i_command, COMMAND_EXEC, "display/modify cp15 (interpreted access) <opcode> [value] [address]");
+	register_command(cmd_ctx, arm920t_cmd, "cache_info", arm920t_handle_cache_info_command, COMMAND_EXEC, "display information about target caches");
+	register_command(cmd_ctx, arm920t_cmd, "virt2phys", arm920t_handle_virt2phys_command, COMMAND_EXEC, "translate va to pa <va>");
+
+	register_command(cmd_ctx, arm920t_cmd, "mdw_phys", arm920t_handle_md_phys_command, COMMAND_EXEC, "display memory words <physical addr> [count]");
+	register_command(cmd_ctx, arm920t_cmd, "mdh_phys", arm920t_handle_md_phys_command, COMMAND_EXEC, "display memory half-words <physical addr> [count]");
+	register_command(cmd_ctx, arm920t_cmd, "mdb_phys", arm920t_handle_md_phys_command, COMMAND_EXEC, "display memory bytes <physical addr> [count]");
+
+	register_command(cmd_ctx, arm920t_cmd, "mww_phys", arm920t_handle_mw_phys_command, COMMAND_EXEC, "write memory word <physical addr> <value>");
+	register_command(cmd_ctx, arm920t_cmd, "mwh_phys", arm920t_handle_mw_phys_command, COMMAND_EXEC, "write memory half-word <physical addr> <value>");
+	register_command(cmd_ctx, arm920t_cmd, "mwb_phys", arm920t_handle_mw_phys_command, COMMAND_EXEC, "write memory byte <physical addr> <value>");
+
+	return ERROR_OK;
+}
+
+int arm920t_handle_cp15_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	int retval;
+	target_t *target = get_current_target(cmd_ctx);
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	arm9tdmi_common_t *arm9tdmi;
+	arm920t_common_t *arm920t;
+	arm_jtag_t *jtag_info;
+
+	if (arm920t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm920t) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM920t target");
+		return ERROR_OK;
+	}
+	
+	jtag_info = &arm7_9->jtag_info;
+	
+	if (target->state != TARGET_HALTED)
+	{
+		command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
+		return ERROR_OK;
+	}
+
+	/* one or more argument, access a single register (write if second argument is given */
+	if (argc >= 1)
+	{
+		int address = strtoul(args[0], NULL, 0);
+
+		if (argc == 1)
+		{
+			u32 value;
+			if ((retval = arm920t_read_cp15_physical(target, address, &value)) != ERROR_OK)
+			{
+				command_print(cmd_ctx, "couldn't access reg %i", address);
+				return ERROR_OK;
+			}
+			jtag_execute_queue();
+			
+			command_print(cmd_ctx, "%i: %8.8x", address, value);
+		}
+		else if (argc == 2)
+		{
+			u32 value = strtoul(args[1], NULL, 0);
+			if ((retval = arm920t_write_cp15_physical(target, address, value)) != ERROR_OK)
+			{
+				command_print(cmd_ctx, "couldn't access reg %i", address);
+				return ERROR_OK;
+			}
+			command_print(cmd_ctx, "%i: %8.8x", address, value);
+		}
+	}
+
+	return ERROR_OK;
+}
+
+int arm920t_handle_cp15i_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	int retval;
+	target_t *target = get_current_target(cmd_ctx);
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	arm9tdmi_common_t *arm9tdmi;
+	arm920t_common_t *arm920t;
+	arm_jtag_t *jtag_info;
+
+	if (arm920t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm920t) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM920t target");
+		return ERROR_OK;
+	}
+	
+	jtag_info = &arm7_9->jtag_info;
+	
+	if (target->state != TARGET_HALTED)
+	{
+		command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
+		return ERROR_OK;
+	}
+
+	/* one or more argument, access a single register (write if second argument is given */
+	if (argc >= 1)
+	{
+		u32 opcode = strtoul(args[0], NULL, 0);
+
+		if (argc == 1)
+		{
+			u32 value;
+			if ((retval = arm920t_read_cp15_interpreted(target, opcode, &value)) != ERROR_OK)
+			{
+				command_print(cmd_ctx, "couldn't execute %8.8x", opcode);
+				return ERROR_OK;
+			}
+			
+			command_print(cmd_ctx, "%8.8x: %8.8x", opcode, value);
+		}
+		else if (argc == 2)
+		{
+			u32 value = strtoul(args[1], NULL, 0);
+			if ((retval = arm920t_write_cp15_interpreted(target, opcode, value, 0)) != ERROR_OK)
+			{
+				command_print(cmd_ctx, "couldn't execute %8.8x", opcode);
+				return ERROR_OK;
+			}
+			command_print(cmd_ctx, "%8.8x: %8.8x", opcode, value);
+		}
+		else if (argc == 3)
+		{
+			u32 value = strtoul(args[1], NULL, 0);
+			u32 address = strtoul(args[2], NULL, 0);
+			if ((retval = arm920t_write_cp15_interpreted(target, opcode, value, address)) != ERROR_OK)
+			{
+				command_print(cmd_ctx, "couldn't execute %8.8x", opcode);
+				return ERROR_OK;
+			}
+			command_print(cmd_ctx, "%8.8x: %8.8x %8.8x", opcode, value, address);
+		}
+	}
+
+	return ERROR_OK;
+}
+
+int arm920t_handle_cache_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target = get_current_target(cmd_ctx);
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	arm9tdmi_common_t *arm9tdmi;
+	arm920t_common_t *arm920t;
+	
+	if (arm920t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm920t) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM920t target");
+		return ERROR_OK;
+	}
+	
+	return armv4_5_handle_cache_info_command(cmd_ctx, &arm920t->armv4_5_mmu.armv4_5_cache);
+}
+
+int arm920t_handle_virt2phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
+{	
+	target_t *target = get_current_target(cmd_ctx);
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	arm9tdmi_common_t *arm9tdmi;
+	arm920t_common_t *arm920t;
+	arm_jtag_t *jtag_info;
+
+	if (arm920t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm920t) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM920t target");
+		return ERROR_OK;
+	}
+	
+	jtag_info = &arm7_9->jtag_info;
+	
+	if (target->state != TARGET_HALTED)
+	{
+		command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
+		return ERROR_OK;
+	}
+		
+	return armv4_5_mmu_handle_virt2phys_command(cmd_ctx, cmd, args, argc, target, &arm920t->armv4_5_mmu);
+}
+
+int arm920t_handle_md_phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
+{	
+	target_t *target = get_current_target(cmd_ctx);
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	arm9tdmi_common_t *arm9tdmi;
+	arm920t_common_t *arm920t;
+	arm_jtag_t *jtag_info;
+
+	if (arm920t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm920t) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM920t target");
+		return ERROR_OK;
+	}
+	
+	jtag_info = &arm7_9->jtag_info;
+	
+	if (target->state != TARGET_HALTED)
+	{
+		command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
+		return ERROR_OK;
+	}
+	
+	return armv4_5_mmu_handle_md_phys_command(cmd_ctx, cmd, args, argc, target, &arm920t->armv4_5_mmu);
+}
+
+int arm920t_handle_mw_phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
+{	
+	target_t *target = get_current_target(cmd_ctx);
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	arm9tdmi_common_t *arm9tdmi;
+	arm920t_common_t *arm920t;
+	arm_jtag_t *jtag_info;
+
+	if (arm920t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm920t) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM920t target");
+		return ERROR_OK;
+	}
+	
+	jtag_info = &arm7_9->jtag_info;
+	
+	if (target->state != TARGET_HALTED)
+	{
+		command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
+		return ERROR_OK;
+	}
+	
+	return armv4_5_mmu_handle_mw_phys_command(cmd_ctx, cmd, args, argc, target, &arm920t->armv4_5_mmu);
+}
diff --git a/src/target/arm920t.h b/src/target/arm920t.h
new file mode 100644
index 00000000..bedf31f2
--- /dev/null
+++ b/src/target/arm920t.h
@@ -0,0 +1,45 @@
+/***************************************************************************
+ *   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 ARM920T_H
+#define ARM920T_H
+
+#include "target.h"
+#include "register.h"
+#include "embeddedice.h"
+#include "arm_jtag.h"
+#include "arm9tdmi.h"
+#include "armv4_5_mmu.h"
+#include "armv4_5_cache.h"
+
+#define	ARM920T_COMMON_MAGIC 0xa920a920
+
+typedef struct arm920t_common_s
+{
+	int common_magic;
+	armv4_5_mmu_common_t armv4_5_mmu;
+	arm9tdmi_common_t arm9tdmi_common;
+	u32 cp15_control_reg;
+	u32 d_fsr;
+	u32 i_fsr;
+	u32 d_far;
+	u32 i_far;
+} arm920t_common_t;
+
+#endif /* ARM920T_H */
diff --git a/src/target/arm9tdmi.c b/src/target/arm9tdmi.c
new file mode 100644
index 00000000..48b201a1
--- /dev/null
+++ b/src/target/arm9tdmi.c
@@ -0,0 +1,848 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "config.h"
+
+#include "arm9tdmi.h"
+
+#include "arm7_9_common.h"
+#include "register.h"
+#include "target.h"
+#include "armv4_5.h"
+#include "embeddedice.h"
+#include "log.h"
+#include "jtag.h"
+#include "arm_jtag.h"
+
+#include <stdlib.h>
+#include <string.h>
+
+#if 0
+#define _DEBUG_INSTRUCTION_EXECUTION_
+#endif
+
+/* cli handling */
+int arm9tdmi_register_commands(struct command_context_s *cmd_ctx);
+
+/* forward declarations */
+int arm9tdmi_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target);
+int arm9tdmi_init_target(struct command_context_s *cmd_ctx, struct target_s *target);
+int arm9tdmi_quit();
+
+/* target function declarations */
+enum target_state arm9tdmi_poll(struct target_s *target);
+int arm9tdmi_halt(target_t *target);
+int arm9tdmi_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);
+		
+target_type_t arm9tdmi_target =
+{
+	.name = "arm9tdmi",
+
+	.poll = arm7_9_poll,
+	.arch_state = armv4_5_arch_state,
+
+	.halt = arm7_9_halt,
+	.resume = arm7_9_resume,
+	.step = arm7_9_step,
+
+	.assert_reset = arm7_9_assert_reset,
+	.deassert_reset = arm7_9_deassert_reset,
+	.soft_reset_halt = arm7_9_soft_reset_halt,
+
+	.get_gdb_reg_list = armv4_5_get_gdb_reg_list,
+
+	.read_memory = arm7_9_read_memory,
+	.write_memory = arm7_9_write_memory,
+	.bulk_write_memory = arm7_9_bulk_write_memory,
+
+	.add_breakpoint = arm7_9_add_breakpoint,
+	.remove_breakpoint = arm7_9_remove_breakpoint,
+	.add_watchpoint = arm7_9_add_watchpoint,
+	.remove_watchpoint = arm7_9_remove_watchpoint,
+
+	.register_commands = arm9tdmi_register_commands,
+	.target_command = arm9tdmi_target_command,
+	.init_target = arm9tdmi_init_target,
+	.quit = arm9tdmi_quit
+};
+
+int arm9tdmi_examine_debug_reason(target_t *target)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	
+	/* only check the debug reason if we don't know it already */
+	if ((target->debug_reason != DBG_REASON_DBGRQ)
+			&& (target->debug_reason != DBG_REASON_SINGLESTEP))
+	{
+		scan_field_t fields[3];
+		u8 databus[4];
+		u8 instructionbus[4];
+		u8 debug_reason;
+
+		jtag_add_end_state(TAP_PD);
+
+		fields[0].device = arm7_9->jtag_info.chain_pos;
+		fields[0].num_bits = 32;
+		fields[0].out_value = NULL;
+		fields[0].out_mask = NULL;
+		fields[0].in_value = databus;
+		fields[0].in_check_value = NULL;
+		fields[0].in_check_mask = NULL;
+		fields[0].in_handler = NULL;
+		fields[0].in_handler_priv = NULL;
+		
+		fields[1].device = arm7_9->jtag_info.chain_pos;
+		fields[1].num_bits = 3;
+		fields[1].out_value = NULL;
+		fields[1].out_mask = NULL;
+		fields[1].in_value = &debug_reason;
+		fields[1].in_check_value = NULL;
+		fields[1].in_check_mask = NULL;
+		fields[1].in_handler = NULL;
+		fields[1].in_handler_priv = NULL;
+		
+		fields[2].device = arm7_9->jtag_info.chain_pos;
+		fields[2].num_bits = 32;
+		fields[2].out_value = NULL;
+		fields[2].out_mask = NULL;
+		fields[2].in_value = instructionbus;
+		fields[2].in_check_value = NULL;
+		fields[2].in_check_mask = NULL;
+		fields[2].in_handler = NULL;
+		fields[2].in_handler_priv = NULL;
+		
+		arm_jtag_scann(&arm7_9->jtag_info, 0x1);
+		arm_jtag_set_instr(&arm7_9->jtag_info, arm7_9->jtag_info.intest_instr);
+
+		jtag_add_dr_scan(3, fields, TAP_PD);
+		jtag_execute_queue();
+		
+		fields[0].in_value = NULL;
+		fields[0].out_value = databus;
+		fields[1].in_value = NULL;
+		fields[1].out_value = &debug_reason;
+		fields[2].in_value = NULL;
+		fields[2].out_value = instructionbus;
+		
+		jtag_add_dr_scan(3, fields, TAP_PD);
+
+		if (debug_reason & 0x4)
+			if (debug_reason & 0x2)
+				target->debug_reason = DBG_REASON_WPTANDBKPT;
+		else
+			target->debug_reason = DBG_REASON_WATCHPOINT;
+		else
+			target->debug_reason = DBG_REASON_BREAKPOINT;
+	}
+
+	return ERROR_OK;
+}
+
+/* put an instruction in the ARM9TDMI pipeline or write the data bus, and optionally read data */
+int arm9tdmi_clock_out(arm_jtag_t *jtag_info, u32 instr, u32 out, u32 *in, int sysspeed)
+{
+	scan_field_t fields[3];
+	u8 out_buf[4];
+	u8 instr_buf[4];
+	u8 sysspeed_buf = 0x0;
+	
+	/* prepare buffer */
+	buf_set_u32(out_buf, 0, 32, out);
+	
+	instr = flip_u32(instr, 32);
+	buf_set_u32(instr_buf, 0, 32, instr);
+	
+	if (sysspeed)
+		buf_set_u32(&sysspeed_buf, 2, 1, 1);
+	
+	jtag_add_end_state(TAP_PD);
+	arm_jtag_scann(jtag_info, 0x1);
+	arm_jtag_set_instr(jtag_info, jtag_info->intest_instr);
+		
+	fields[0].device = jtag_info->chain_pos;
+	fields[0].num_bits = 32;
+	fields[0].out_value = out_buf;
+	fields[0].out_mask = NULL;
+	if (in)
+	{
+		fields[0].in_value = (u8*)in;
+	} else
+	{
+		fields[0].in_value = NULL;
+	}
+	fields[0].in_check_value = NULL;
+	fields[0].in_check_mask = NULL;
+	fields[0].in_handler = NULL;
+	fields[0].in_handler_priv = NULL;
+	
+	fields[1].device = jtag_info->chain_pos;
+	fields[1].num_bits = 3;
+	fields[1].out_value = &sysspeed_buf;
+	fields[1].out_mask = NULL;
+	fields[1].in_value = NULL;
+	fields[1].in_check_value = NULL;
+	fields[1].in_check_mask = NULL;
+	fields[1].in_handler = NULL;
+	fields[1].in_handler_priv = NULL;
+		
+	fields[2].device = jtag_info->chain_pos;
+	fields[2].num_bits = 32;
+	fields[2].out_value = instr_buf;
+	fields[2].out_mask = NULL;
+	fields[2].in_value = NULL;
+	fields[2].in_check_value = NULL;
+	fields[2].in_check_mask = NULL;
+	fields[2].in_handler = NULL;
+	fields[2].in_handler_priv = NULL;
+
+	jtag_add_dr_scan(3, fields, -1);
+
+	jtag_add_runtest(0, -1);
+	
+#ifdef _DEBUG_INSTRUCTION_EXECUTION_
+	{
+		char* in_string;
+		jtag_execute_queue();
+		
+		if (in)
+		{
+			in_string = buf_to_char((u8*)in, 32);
+			DEBUG("instr: 0x%8.8x, out: 0x%8.8x, in: %s", flip_u32(instr, 32), out, in_string);
+			free(in_string);
+		}
+		else
+			DEBUG("instr: 0x%8.8x, out: 0x%8.8x", flip_u32(instr, 32), out);
+	}
+#endif
+
+	return ERROR_OK;
+}
+
+/* just read data (instruction and data-out = don't care) */
+int arm9tdmi_clock_data_in(arm_jtag_t *jtag_info, u32 *in)
+{
+	scan_field_t fields[3];
+
+	jtag_add_end_state(TAP_PD);
+	arm_jtag_scann(jtag_info, 0x1);
+	arm_jtag_set_instr(jtag_info, jtag_info->intest_instr);
+		
+	fields[0].device = jtag_info->chain_pos;
+	fields[0].num_bits = 32;
+	fields[0].out_value = NULL;
+	fields[0].out_mask = NULL;
+	fields[0].in_value = (u8*)in;
+	fields[0].in_handler = NULL;
+	fields[0].in_handler_priv = NULL;
+	fields[0].in_check_value = NULL;
+	fields[0].in_check_mask = NULL;
+	
+	fields[1].device = jtag_info->chain_pos;
+	fields[1].num_bits = 3;
+	fields[1].out_value = NULL;
+	fields[1].out_mask = NULL;
+	fields[1].in_value = NULL;
+	fields[1].in_handler = NULL;
+	fields[1].in_handler_priv = NULL;
+	fields[1].in_check_value = NULL;
+	fields[1].in_check_mask = NULL;
+
+	fields[2].device = jtag_info->chain_pos;
+	fields[2].num_bits = 32;
+	fields[2].out_value = NULL;
+	fields[2].out_mask = NULL;
+	fields[2].in_value = NULL;
+	fields[2].in_check_value = NULL;
+	fields[2].in_check_mask = NULL;
+	fields[2].in_handler = NULL;
+	fields[2].in_handler_priv = NULL;
+	
+	jtag_add_dr_scan(3, fields, -1);
+
+	jtag_add_runtest(0, -1);
+	
+#ifdef _DEBUG_INSTRUCTION_EXECUTION_
+	{
+		char* in_string;
+		jtag_execute_queue();
+			
+		if (in)
+		{
+			in_string = buf_to_char((u8*)in, 32);
+			DEBUG("in: %s", in_string);
+			free(in_string);
+		}
+	}
+#endif
+
+	return ERROR_OK;
+}
+
+void arm9tdmi_change_to_arm(target_t *target, u32 *r0, u32 *pc)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+	
+	/* save r0 before using it and put system in ARM state 
+	 * to allow common handling of ARM and THUMB debugging */
+	
+	/* fetch STR r0, [r0] */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_STR(0, 0), 0, NULL, 0);
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
+	/* STR r0, [r0] in Memory */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, r0, 0);
+
+	/* MOV r0, r15 fetched, STR in Decode */	
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_MOV(0, 15), 0, NULL, 0);
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_STR(0, 0), 0, NULL, 0);
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
+	/* nothing fetched, STR r0, [r0] in Memory */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, pc, 0);
+
+	/* fetch MOV */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_MOV_IM(0, 0x0), 0, NULL, 0);
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
+
+	/* fetch BX */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_BX(0), 0, NULL, 0);
+	/* NOP fetched, BX in Decode, MOV in Execute */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
+	/* NOP fetched, BX in Execute (1) */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
+	
+	jtag_execute_queue();
+	
+	/* fix program counter:
+	 * MOV r0, r15 was the 5th instruction (+8)
+	 * reading PC in Thumb state gives address of instruction + 4
+	 */
+	*pc -= 0xc;
+}
+
+void arm9tdmi_read_core_regs(target_t *target, u32 mask, u32* core_regs[16])
+{
+	int i;
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+		
+	/* STMIA r0-15, [r0] at debug speed
+	 * register values will start to appear on 4th DCLK
+	 */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask & 0xffff, 0, 0), 0, NULL, 0);
+
+	/* fetch NOP, STM in DECODE stage */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	/* fetch NOP, STM in EXECUTE stage (1st cycle) */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+
+	for (i = 0; i <= 15; i++)
+	{
+		if (mask & (1 << i))
+			/* nothing fetched, STM in MEMORY (i'th cycle) */
+			arm9tdmi_clock_data_in(jtag_info, core_regs[i]);
+	}
+
+}
+
+void arm9tdmi_read_xpsr(target_t *target, u32 *xpsr, int spsr)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+		
+	/* MRS r0, cpsr */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_MRS(0, spsr & 1), 0, NULL, 0);
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+
+	/* STR r0, [r15] */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_STR(0, 15), 0, NULL, 0);
+	/* fetch NOP, STR in DECODE stage */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	/* fetch NOP, STR in EXECUTE stage (1st cycle) */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	/* nothing fetched, STR in MEMORY */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, xpsr, 0);
+
+}
+
+void arm9tdmi_write_xpsr(target_t *target, u32 xpsr, int spsr)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+		
+	DEBUG("xpsr: %8.8x, spsr: %i", xpsr, spsr);
+
+	/* MSR1 fetched */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM(xpsr & 0xff, 0, 1, spsr), 0, NULL, 0);
+	/* MSR2 fetched, MSR1 in DECODE */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff00) >> 8, 0xc, 2, spsr), 0, NULL, 0);
+	/* MSR3 fetched, MSR1 in EXECUTE (1), MSR2 in DECODE */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff0000) >> 16, 0x8, 4, spsr), 0, NULL, 0);
+	/* nothing fetched, MSR1 in EXECUTE (2) */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	/* nothing fetched, MSR1 in EXECUTE (3) */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	/* MSR4 fetched, MSR2 in EXECUTE (1), MSR3 in DECODE */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff000000) >> 24, 0x4, 8, spsr), 0, NULL, 0);
+	/* nothing fetched, MSR2 in EXECUTE (2) */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	/* nothing fetched, MSR2 in EXECUTE (3) */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	/* NOP fetched, MSR3 in EXECUTE (1), MSR4 in DECODE */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	/* nothing fetched, MSR3 in EXECUTE (2) */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	/* nothing fetched, MSR3 in EXECUTE (3) */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	/* NOP fetched, MSR4 in EXECUTE (1) */
+	/* last MSR writes flags, which takes only one cycle */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+}
+
+void arm9tdmi_write_xpsr_im8(target_t *target, u8 xpsr_im, int rot, int spsr)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+		
+	DEBUG("xpsr_im: %2.2x, rot: %i, spsr: %i", xpsr_im, rot, spsr);
+	
+	/* MSR fetched */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM(xpsr_im, rot, 1, spsr), 0, NULL, 0);
+	/* NOP fetched, MSR in DECODE */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	/* NOP fetched, MSR in EXECUTE (1) */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	
+	/* rot == 4 writes flags, which takes only one cycle */
+	if (rot != 4)
+	{
+		/* nothing fetched, MSR in EXECUTE (2) */
+		arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+		/* nothing fetched, MSR in EXECUTE (3) */
+		arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	}
+}
+
+void arm9tdmi_write_core_regs(target_t *target, u32 mask, u32 core_regs[16])
+{
+	int i;
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+		
+	/* LDMIA r0-15, [r0] at debug speed
+	* register values will start to appear on 4th DCLK
+	*/
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, mask & 0xffff, 0, 0), 0, NULL, 0);
+
+	/* fetch NOP, LDM in DECODE stage */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	/* fetch NOP, LDM in EXECUTE stage (1st cycle) */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+
+	for (i = 0; i <= 15; i++)
+	{
+		if (mask & (1 << i))
+			/* nothing fetched, LDM still in EXECUTE (1+i cycle) */
+			arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, core_regs[i], NULL, 0);
+	}
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	
+}
+
+void arm9tdmi_load_word_regs(target_t *target, u32 mask)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+
+	/* put system-speed load-multiple into the pipeline */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, mask & 0xffff, 0, 1), 0, NULL, 0);
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);
+
+}
+
+void arm9tdmi_load_hword_reg(target_t *target, int num)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+	
+	/* put system-speed load half-word into the pipeline */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_LDRH_IP(num, 0), 0, NULL, 0);
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);
+}
+
+void arm9tdmi_load_byte_reg(target_t *target, int num)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+
+	/* put system-speed load byte into the pipeline */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_LDRB_IP(num, 0), 0, NULL, 0);
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);
+
+}
+
+void arm9tdmi_store_word_regs(target_t *target, u32 mask)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+
+	/* put system-speed store-multiple into the pipeline */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask, 0, 1), 0, NULL, 0);
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);
+
+}
+
+void arm9tdmi_store_hword_reg(target_t *target, int num)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+
+	/* put system-speed store half-word into the pipeline */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_STRH_IP(num, 0), 0, NULL, 0);
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);
+
+}
+
+void arm9tdmi_store_byte_reg(target_t *target, int num)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+
+	/* put system-speed store byte into the pipeline */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_STRB_IP(num, 0), 0, NULL, 0);
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);
+
+}
+
+void arm9tdmi_write_pc(target_t *target, u32 pc)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+	
+	/* LDMIA r0-15, [r0] at debug speed
+	 * register values will start to appear on 4th DCLK
+	 */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, 0x8000, 0, 0), 0, NULL, 0);
+
+	/* fetch NOP, LDM in DECODE stage */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	/* fetch NOP, LDM in EXECUTE stage (1st cycle) */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	/* nothing fetched, LDM in EXECUTE stage (2nd cycle) (output data) */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, pc, NULL, 0);
+	/* nothing fetched, LDM in EXECUTE stage (3rd cycle) */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	/* fetch NOP, LDM in EXECUTE stage (4th cycle) */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	/* fetch NOP, LDM in EXECUTE stage (5th cycle) */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+
+}
+
+void arm9tdmi_branch_resume(target_t *target)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+	
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_B(0xfffffc, 0), 0, NULL, 0);
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);
+
+}
+
+void arm9tdmi_branch_resume_thumb(target_t *target)
+{
+	DEBUG("");
+	
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+	reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
+
+	/* LDMIA r0-15, [r0] at debug speed
+	* register values will start to appear on 4th DCLK
+	*/
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, 0x1, 0, 0), 0, NULL, 0);
+
+	/* fetch NOP, LDM in DECODE stage */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	/* fetch NOP, LDM in EXECUTE stage (1st cycle) */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	/* nothing fetched, LDM in EXECUTE stage (2nd cycle) */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32) | 1, NULL, 0);
+	/* nothing fetched, LDM in EXECUTE stage (3rd cycle) */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+
+	/* Branch and eXchange */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_BX(0), 0, NULL, 0);
+	
+	embeddedice_read_reg(dbg_stat);
+	
+	/* fetch NOP, BX in DECODE stage */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+	
+	embeddedice_read_reg(dbg_stat);
+	
+	/* fetch NOP, BX in EXECUTE stage (1st cycle) */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
+
+	/* target is now in Thumb state */
+	embeddedice_read_reg(dbg_stat);
+
+	/* clean r0 bits to avoid alignment problems */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_MOV_IM(0, 0x0), 0, NULL, 0);
+	/* load r0 value, MOV_IM in Decode*/
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_LDR(0, 0), 0, NULL, 0);
+	/* fetch NOP, LDR in Decode, MOV_IM in Execute */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
+	/* fetch NOP, LDR in Execute */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
+	/* nothing fetched, LDR in EXECUTE stage (2nd cycle) */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, buf_get_u32(armv4_5->core_cache->reg_list[0].value, 0, 32), NULL, 0);
+	/* nothing fetched, LDR in EXECUTE stage (3rd cycle) */
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
+	
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
+
+	embeddedice_read_reg(dbg_stat);
+	
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_B(0x7f6), 0, NULL, 1);
+	arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
+
+}
+
+void arm9tdmi_enable_single_step(target_t *target)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm9tdmi_common_t *arm9 = arm7_9->arch_info;
+	
+	if (arm9->has_single_step)
+	{
+		buf_set_u32(arm7_9->eice_cache->reg_list[EICE_DBG_CTRL].value, 3, 1, 1);
+		embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_DBG_CTRL]);
+	}
+	else
+	{
+		arm7_9_enable_eice_step(target);
+	}
+}
+
+void arm9tdmi_disable_single_step(target_t *target)
+{
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm9tdmi_common_t *arm9 = arm7_9->arch_info;
+	
+	if (arm9->has_single_step)
+	{
+		buf_set_u32(arm7_9->eice_cache->reg_list[EICE_DBG_CTRL].value, 3, 1, 0);
+		embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_DBG_CTRL]);
+	}
+	else
+	{
+		arm7_9_disable_eice_step(target);
+	}
+}
+
+void arm9tdmi_build_reg_cache(target_t *target)
+{
+	reg_cache_t **cache_p = register_get_last_cache_p(&target->reg_cache);
+	/* get pointers to arch-specific information */
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+	arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;
+
+
+	(*cache_p) = armv4_5_build_reg_cache(target, armv4_5);
+	armv4_5->core_cache = (*cache_p);
+	
+	(*cache_p)->next = embeddedice_build_reg_cache(target, jtag_info, 0);
+	arm7_9->eice_cache = (*cache_p)->next;
+	
+	if (arm9tdmi->has_monitor_mode)
+		(*cache_p)->next->reg_list[0].size = 6;
+	else
+		(*cache_p)->next->reg_list[0].size = 4;
+	
+	(*cache_p)->next->reg_list[1].size = 5;
+
+}
+
+int arm9tdmi_init_target(struct command_context_s *cmd_ctx, struct target_s *target)
+{
+	
+	arm9tdmi_build_reg_cache(target);
+	
+	return ERROR_OK;
+	
+}
+
+int arm9tdmi_quit()
+{
+	
+	return ERROR_OK;
+}
+
+int arm9tdmi_init_arch_info(target_t *target, arm9tdmi_common_t *arm9tdmi, int chain_pos, char *variant)
+{
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	
+	arm7_9 = &arm9tdmi->arm7_9_common;
+	armv4_5 = &arm7_9->armv4_5_common;
+	
+	/* prepare JTAG information for the new target */
+	arm7_9->jtag_info.chain_pos = chain_pos;
+	arm7_9->jtag_info.scann_size = 5;
+	
+	/* register arch-specific functions */
+	arm7_9->examine_debug_reason = arm9tdmi_examine_debug_reason;
+	arm7_9->change_to_arm = arm9tdmi_change_to_arm;
+	arm7_9->read_core_regs = arm9tdmi_read_core_regs;
+	arm7_9->read_xpsr = arm9tdmi_read_xpsr;
+	
+	arm7_9->write_xpsr = arm9tdmi_write_xpsr;
+	arm7_9->write_xpsr_im8 = arm9tdmi_write_xpsr_im8;
+	arm7_9->write_core_regs = arm9tdmi_write_core_regs;
+	
+	arm7_9->load_word_regs = arm9tdmi_load_word_regs;
+	arm7_9->load_hword_reg = arm9tdmi_load_hword_reg;
+	arm7_9->load_byte_reg = arm9tdmi_load_byte_reg;
+	
+	arm7_9->store_word_regs = arm9tdmi_store_word_regs;
+	arm7_9->store_hword_reg = arm9tdmi_store_hword_reg;
+	arm7_9->store_byte_reg = arm9tdmi_store_byte_reg;
+	
+	arm7_9->write_pc = arm9tdmi_write_pc;
+	arm7_9->branch_resume = arm9tdmi_branch_resume;
+	arm7_9->branch_resume_thumb = arm9tdmi_branch_resume_thumb;
+
+	arm7_9->enable_single_step = arm9tdmi_enable_single_step;
+	arm7_9->disable_single_step = arm9tdmi_disable_single_step;
+	
+	arm7_9->pre_debug_entry = NULL;
+	arm7_9->post_debug_entry = NULL;
+	
+	arm7_9->pre_restore_context = NULL;
+	arm7_9->post_restore_context = NULL;
+
+	/* initialize arch-specific breakpoint handling */
+	buf_set_u32((u8*)(&arm7_9->arm_bkpt), 0, 32, 0xdeeedeee);
+	buf_set_u32((u8*)(&arm7_9->thumb_bkpt), 0, 16, 0xdeee);
+	
+	arm7_9->sw_bkpts_use_wp = 1;
+	arm7_9->sw_bkpts_enabled = 0;
+	arm7_9->dbgreq_adjust_pc = 3;
+	arm7_9->arch_info = arm9tdmi;
+	arm7_9->use_dbgrq = 1;
+	
+	arm9tdmi->common_magic = ARM9TDMI_COMMON_MAGIC;
+	arm9tdmi->has_monitor_mode = 0;
+	arm9tdmi->has_single_step = 0;
+	arm9tdmi->arch_info = NULL;
+
+	if (variant)
+	{
+		if (strcmp(variant, "arm920t") == 0)
+			arm9tdmi->has_single_step = 1;
+		else if (strcmp(variant, "arm922t") == 0)
+			arm9tdmi->has_single_step = 1;
+		else if (strcmp(variant, "arm940t") == 0)
+			arm9tdmi->has_single_step = 1;
+	}
+	
+	arm7_9_init_arch_info(target, arm7_9);
+	
+	return ERROR_OK;
+}
+
+/* target arm9tdmi <endianess> <startup_mode> <chain_pos> <variant>*/
+int arm9tdmi_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target)
+{
+	int chain_pos;
+	char *variant = NULL;
+	arm9tdmi_common_t *arm9tdmi = malloc(sizeof(arm9tdmi_common_t));
+
+	if (argc < 4)
+	{
+		ERROR("'target arm9tdmi' requires at least one additional argument");
+		exit(-1);
+	}
+	
+	chain_pos = strtoul(args[3], NULL, 0);
+	
+	if (argc >= 5)
+		variant = strdup(args[4]);
+	
+	arm9tdmi_init_arch_info(target, arm9tdmi, chain_pos, variant);
+	
+	return ERROR_OK;
+}
+
+int arm9tdmi_register_commands(struct command_context_s *cmd_ctx)
+{
+	int retval;
+	
+	retval = arm7_9_register_commands(cmd_ctx);
+	
+	return ERROR_OK;
+
+}
+
diff --git a/src/target/arm9tdmi.h b/src/target/arm9tdmi.h
new file mode 100644
index 00000000..7bbaac7c
--- /dev/null
+++ b/src/target/arm9tdmi.h
@@ -0,0 +1,51 @@
+/***************************************************************************
+ *   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 ARM9TDMI_H
+#define ARM9TDMI_H
+
+#include "target.h"
+#include "register.h"
+#include "armv4_5.h"
+#include "embeddedice.h"
+#include "arm_jtag.h"
+#include "arm7_9_common.h"
+
+#define	ARM9TDMI_COMMON_MAGIC 0x00a900a9
+
+typedef struct arm9tdmi_common_s
+{
+	int common_magic;
+	char *variant;
+	int has_monitor_mode;
+	int has_single_step;
+	void *arch_info;
+	arm7_9_common_t arm7_9_common;
+} arm9tdmi_common_t;
+
+extern int arm9tdmi_init_target(struct command_context_s *cmd_ctx, struct target_s *target);
+extern int arm9tdmi_init_arch_info(target_t *target, arm9tdmi_common_t *arm9tdmi, int chain_pos, char *variant);
+extern int arm9tdmi_register_commands(struct command_context_s *cmd_ctx);
+
+extern int arm9tdmi_clock_out(arm_jtag_t *jtag_info, u32 instr, u32 out, u32 *in, int sysspeed);
+extern int arm9tdmi_clock_data_in(arm_jtag_t *jtag_info, u32 *in);
+extern void arm9tdmi_read_core_regs(target_t *target, u32 mask, u32* core_regs[16]);
+extern void arm9tdmi_write_core_regs(target_t *target, u32 mask, u32 core_regs[16]);
+
+#endif /* ARM9TDMI_H */
diff --git a/src/target/arm_jtag.c b/src/target/arm_jtag.c
new file mode 100644
index 00000000..c401d8f3
--- /dev/null
+++ b/src/target/arm_jtag.c
@@ -0,0 +1,116 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "config.h"
+
+#include "arm_jtag.h"
+
+#include "binarybuffer.h"
+#include "log.h"
+#include "jtag.h"
+
+#include <stdlib.h>
+
+int arm_jtag_set_instr(arm_jtag_t *jtag_info, u32 new_instr)
+{
+	jtag_device_t *device = jtag_get_device(jtag_info->chain_pos);
+	
+	if (buf_get_u32(device->cur_instr, 0, device->ir_length) != new_instr)
+	{
+		scan_field_t field;
+	
+		field.device = jtag_info->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, -1);
+		
+		free(field.out_value);
+	}
+	
+	return ERROR_OK;
+}
+
+int arm_jtag_scann(arm_jtag_t *jtag_info, u32 new_scan_chain)
+{
+	if(jtag_info->cur_scan_chain != new_scan_chain)
+	{
+		scan_field_t field;
+		
+		field.device = jtag_info->chain_pos;
+		field.num_bits = jtag_info->scann_size;
+		field.out_value = calloc(CEIL(field.num_bits, 8), 1);
+		buf_set_u32(field.out_value, 0, field.num_bits, new_scan_chain);
+		field.out_mask = NULL;
+		//field.in_value = &scan_n_capture;
+		field.in_value = NULL;
+		field.in_check_value = NULL;
+		field.in_check_mask = NULL;
+		field.in_handler = NULL;
+		field.in_handler_priv = NULL;
+		
+		arm_jtag_set_instr(jtag_info, jtag_info->scann_instr);
+		jtag_add_dr_scan(1, &field, -1);
+		
+		jtag_info->cur_scan_chain = new_scan_chain;
+		
+		free(field.out_value);
+	}
+	
+	return ERROR_OK;
+}
+
+int arm_jtag_reset_callback(enum jtag_event event, void *priv)
+{
+	arm_jtag_t *jtag_info = priv;
+	
+	if (event == JTAG_TRST_ASSERTED)
+	{
+		jtag_info->cur_scan_chain = 0;
+	}
+	
+	return ERROR_OK;
+}
+
+int arm_jtag_setup_connection(arm_jtag_t *jtag_info)
+{
+	jtag_info->scann_instr = 0x2;
+	jtag_info->cur_scan_chain = 0;
+	jtag_info->intest_instr = 0xc;
+
+	jtag_register_event_callback(arm_jtag_reset_callback, jtag_info);
+	
+	return ERROR_OK;
+}
+
+int arm_jtag_buf_to_u32_flip(u8 *in_buf, void *priv)
+{
+	u32 *dest = priv;
+	
+	*dest = flip_u32(buf_get_u32(in_buf, 0, 32), 32);
+	
+	return ERROR_OK;
+}
diff --git a/src/target/arm_jtag.h b/src/target/arm_jtag.h
new file mode 100644
index 00000000..a9f90846
--- /dev/null
+++ b/src/target/arm_jtag.h
@@ -0,0 +1,42 @@
+/***************************************************************************
+ *   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 ARM_JTAG
+#define ARM_JTAG
+
+#include "types.h"
+
+typedef struct arm_jtag_s
+{
+	int chain_pos;
+	
+	int scann_size;
+	u32 scann_instr;
+	int cur_scan_chain;
+	
+	u32 intest_instr;
+} arm_jtag_t;
+
+extern int arm_jtag_set_instr(arm_jtag_t *jtag_info, u32 new_instr);
+extern int arm_jtag_scann(arm_jtag_t *jtag_info, u32 new_scan_chain);
+extern int arm_jtag_buf_to_u32_flip(u8 *in_buf, void *priv);
+extern int arm_jtag_setup_connection(arm_jtag_t *jtag_info);
+
+#endif /* ARM_JTAG */
+
diff --git a/src/target/armv4_5.c b/src/target/armv4_5.c
new file mode 100644
index 00000000..51fd4b42
--- /dev/null
+++ b/src/target/armv4_5.c
@@ -0,0 +1,583 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "config.h"
+
+#include "armv4_5.h"
+
+#include "target.h"
+#include "register.h"
+#include "log.h"
+#include "binarybuffer.h"
+#include "command.h"
+
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+bitfield_desc_t armv4_5_psr_bitfield_desc[] = 
+{
+	{"M[4:0]", 5},
+	{"T", 1},
+	{"F", 1},
+	{"I", 1},
+	{"reserved", 16},
+	{"J", 1},
+	{"reserved", 2},
+	{"Q", 1},
+	{"V", 1},
+	{"C", 1},
+	{"Z", 1},
+	{"N", 1},
+};
+
+char* armv4_5_core_reg_list[] =
+{
+	"r0", "r1", "r2", "r3", "r4", "r5", "r6", "r7", "r8", "r9", "r10", "r11", "r12", "r13_usr", "lr_usr", "pc",
+	
+	"r8_fiq", "r9_fiq", "r10_fiq", "r11_fiq", "r12_fiq", "r13_fiq", "lr_fiq",
+	
+	"r13_irq", "lr_irq",
+	
+	"r13_svc", "lr_svc",
+	
+	"r13_abt", "lr_abt",
+	
+	"r13_und", "lr_und",
+	
+	"cpsr", "spsr_fiq", "spsr_irq", "spsr_svc", "spsr_abt", "spsr_und"
+};
+
+char* armv4_5_mode_strings[] =
+{
+	"User", "FIQ", "IRQ", "Supervisor", "Abort", "Undefined", "System"
+};
+
+char* armv4_5_state_strings[] =
+{
+	"ARM", "Thumb", "Jazelle"
+};
+
+int armv4_5_core_reg_arch_type = -1;
+
+armv4_5_core_reg_t armv4_5_core_reg_list_arch_info[] = 
+{
+	{0, ARMV4_5_MODE_ANY, NULL, NULL},
+	{1, ARMV4_5_MODE_ANY, NULL, NULL},
+	{2, ARMV4_5_MODE_ANY, NULL, NULL},
+	{3, ARMV4_5_MODE_ANY, NULL, NULL},
+	{4, ARMV4_5_MODE_ANY, NULL, NULL},
+	{5, ARMV4_5_MODE_ANY, NULL, NULL},
+	{6, ARMV4_5_MODE_ANY, NULL, NULL},
+	{7, ARMV4_5_MODE_ANY, NULL, NULL},
+	{8, ARMV4_5_MODE_ANY, NULL, NULL},
+	{9, ARMV4_5_MODE_ANY, NULL, NULL},
+	{10, ARMV4_5_MODE_ANY, NULL, NULL},
+	{11, ARMV4_5_MODE_ANY, NULL, NULL},
+	{12, ARMV4_5_MODE_ANY, NULL, NULL},
+	{13, ARMV4_5_MODE_USR, NULL, NULL},
+	{14, ARMV4_5_MODE_USR, NULL, NULL},
+	{15, ARMV4_5_MODE_ANY, NULL, NULL},
+	
+	{8, ARMV4_5_MODE_FIQ, NULL, NULL},
+	{9, ARMV4_5_MODE_FIQ, NULL, NULL},
+	{10, ARMV4_5_MODE_FIQ, NULL, NULL},
+	{11, ARMV4_5_MODE_FIQ, NULL, NULL},
+	{12, ARMV4_5_MODE_FIQ, NULL, NULL},
+	{13, ARMV4_5_MODE_FIQ, NULL, NULL},
+	{14, ARMV4_5_MODE_FIQ, NULL, NULL},
+	
+	{13, ARMV4_5_MODE_IRQ, NULL, NULL},
+	{14, ARMV4_5_MODE_IRQ, NULL, NULL},
+
+	{13, ARMV4_5_MODE_SVC, NULL, NULL},
+	{14, ARMV4_5_MODE_SVC, NULL, NULL},
+
+	{13, ARMV4_5_MODE_ABT, NULL, NULL},
+	{14, ARMV4_5_MODE_ABT, NULL, NULL},
+	
+	{13, ARMV4_5_MODE_UND, NULL, NULL},
+	{14, ARMV4_5_MODE_UND, NULL, NULL},
+	
+	{16, ARMV4_5_MODE_ANY, NULL, NULL},
+	{16, ARMV4_5_MODE_FIQ, NULL, NULL},
+	{16, ARMV4_5_MODE_IRQ, NULL, NULL},
+	{16, ARMV4_5_MODE_SVC, NULL, NULL},
+	{16, ARMV4_5_MODE_ABT, NULL, NULL},
+	{16, ARMV4_5_MODE_UND, NULL, NULL}
+};
+
+/* map core mode (USR, FIQ, ...) and register number to indizes into the register cache */
+int armv4_5_core_reg_map[7][17] =
+{
+	{	/* USR */
+		0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 31
+	},
+	{	/* FIQ */
+		0, 1, 2, 3, 4, 5, 6, 7, 16, 17, 18, 19, 20, 21, 22, 15, 32
+	},
+	{	/* IRQ */
+		0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 23, 24, 15, 33
+	},
+	{	/* SVC */
+		0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 25, 26, 15, 34
+	},
+	{	/* ABT */
+		0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 27, 28, 15, 35
+	},
+	{	/* UND */
+		0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 29, 30, 15, 36
+	},
+	{	/* SYS */
+		0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 31
+	}
+};
+
+u8 armv4_5_gdb_dummy_fp_value[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+
+reg_t armv4_5_gdb_dummy_fp_reg =
+{
+	"GDB dummy floating-point register", armv4_5_gdb_dummy_fp_value, 0, 1, 96, NULL, 0, NULL, 0
+};
+
+u8 armv4_5_gdb_dummy_fps_value[] = {0, 0, 0, 0};
+
+reg_t armv4_5_gdb_dummy_fps_reg =
+{
+	"GDB dummy floating-point status register", armv4_5_gdb_dummy_fps_value, 0, 1, 32, NULL, 0, NULL, 0
+};
+
+/* map psr mode bits to linear number */
+int armv4_5_mode_to_number(enum armv4_5_mode mode)
+{
+	switch (mode)
+	{
+		case 16: return 0; break;
+		case 17: return 1; break;
+		case 18: return 2; break;
+		case 19: return 3; break;
+		case 23: return 4; break;
+		case 27: return 5; break;
+		case 31: return 6; break;
+		case -1: return 0; break;	/* map MODE_ANY to user mode */
+		default: 
+			ERROR("invalid mode value encountered");
+			return -1;
+	}
+}
+
+/* map linear number to mode bits */
+enum armv4_5_mode armv4_5_number_to_mode(int number)
+{
+	switch(number)
+	{
+		case 0: return ARMV4_5_MODE_USR; break;
+		case 1: return ARMV4_5_MODE_FIQ; break;
+		case 2: return ARMV4_5_MODE_IRQ; break;
+		case 3: return ARMV4_5_MODE_SVC; break;
+		case 4: return ARMV4_5_MODE_ABT; break;
+		case 5: return ARMV4_5_MODE_UND; break;
+		case 6: return ARMV4_5_MODE_SYS; break;
+		default: 
+			ERROR("mode index out of bounds");
+			return -1;
+	}
+};
+
+int armv4_5_get_core_reg(reg_t *reg)
+{
+	int retval;
+	armv4_5_core_reg_t *armv4_5 = reg->arch_info;
+	target_t *target = armv4_5->target;
+	
+	if (target->state != TARGET_HALTED)
+	{
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	
+	//retval = armv4_5->armv4_5_common->full_context(target);
+	retval = armv4_5->armv4_5_common->read_core_reg(target, armv4_5->num, armv4_5->mode);
+	
+	return retval;
+}
+
+int armv4_5_set_core_reg(reg_t *reg, u32 value)
+{
+	armv4_5_core_reg_t *armv4_5 = reg->arch_info;
+	target_t *target = armv4_5->target;
+		
+	if (target->state != TARGET_HALTED)
+	{
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	
+	buf_set_u32(reg->value, 0, 32, value);
+	reg->dirty = 1;
+	reg->valid = 1;
+
+	return ERROR_OK;
+}
+
+int armv4_5_invalidate_core_regs(target_t *target)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	int i;
+	
+	for (i = 0; i < 37; i++)
+	{
+		armv4_5->core_cache->reg_list[i].valid = 0;
+		armv4_5->core_cache->reg_list[i].dirty = 0;
+	}
+	
+	return ERROR_OK;
+}
+
+reg_cache_t* armv4_5_build_reg_cache(target_t *target, armv4_5_common_t *armv4_5_common)
+{
+	int num_regs = 37;
+	reg_cache_t *cache = malloc(sizeof(reg_cache_t));
+	reg_t *reg_list = malloc(sizeof(reg_t) * num_regs);
+	armv4_5_core_reg_t *arch_info = malloc(sizeof(reg_t) * num_regs);
+	int i;
+	
+	cache->name = "arm v4/5 registers";
+	cache->next = NULL;
+	cache->reg_list = reg_list;
+	cache->num_regs = num_regs;
+	
+	if (armv4_5_core_reg_arch_type == -1)
+		armv4_5_core_reg_arch_type = register_reg_arch_type(armv4_5_get_core_reg, armv4_5_set_core_reg);
+		
+	for (i = 0; i < 37; i++)
+	{
+		arch_info[i] = armv4_5_core_reg_list_arch_info[i];
+		arch_info[i].target = target;
+		arch_info[i].armv4_5_common = armv4_5_common;
+		reg_list[i].name = armv4_5_core_reg_list[i];
+		reg_list[i].size = 32;
+		reg_list[i].value = calloc(1, 4);
+		reg_list[i].dirty = 0;
+		reg_list[i].valid = 0;
+		reg_list[i].bitfield_desc = NULL;
+		reg_list[i].num_bitfields = 0;
+		reg_list[i].arch_type = armv4_5_core_reg_arch_type;
+		reg_list[i].arch_info = &arch_info[i];
+	}
+	
+	return cache;
+}
+
+int armv4_5_arch_state(struct target_s *target, char *buf, int buf_size)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	
+	if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
+	{
+		ERROR("BUG: called for a non-ARMv4/5 target");
+		exit(-1);
+	}
+	
+	snprintf(buf, buf_size,
+			 "target halted in %s state due to %s, current mode: %s\ncpsr: 0x%8.8x pc: 0x%8.8x",
+			 armv4_5_state_strings[armv4_5->core_state],
+			 target_debug_reason_strings[target->debug_reason],
+			 armv4_5_mode_strings[armv4_5_mode_to_number(armv4_5->core_mode)],
+			 buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32),
+			 buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
+	
+	return ERROR_OK;
+}
+
+int handle_armv4_5_reg_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	char output[128];
+	int output_len;
+	int mode, num;
+	target_t *target = get_current_target(cmd_ctx);
+	armv4_5_common_t *armv4_5 = target->arch_info;
+		
+	if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
+	{
+		command_print(cmd_ctx, "current target isn't an ARMV4/5 target");
+		return ERROR_OK;
+	}
+	
+	if (target->state != TARGET_HALTED)
+	{
+		command_print(cmd_ctx, "error: target must be halted for register accesses");
+		return ERROR_OK;
+	}
+	
+	for (num = 0; num <= 15; num++)
+	{
+		output_len = 0;
+		for (mode = 0; mode < 6; mode++)
+		{
+			if (!ARMV4_5_CORE_REG_MODENUM(armv4_5->core_cache, mode, num).valid)
+			{
+				armv4_5->full_context(target);
+			}
+			output_len += snprintf(output + output_len, 128 - output_len, "%8s: %8.8x ", ARMV4_5_CORE_REG_MODENUM(armv4_5->core_cache, mode, num).name, 
+				buf_get_u32(ARMV4_5_CORE_REG_MODENUM(armv4_5->core_cache, mode, num).value, 0, 32));
+		}
+		command_print(cmd_ctx, output);
+	}
+	command_print(cmd_ctx, "    cpsr: %8.8x spsr_fiq: %8.8x spsr_irq: %8.8x spsr_svc: %8.8x spsr_abt: %8.8x spsr_und: %8.8x",
+			  buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32),
+			  buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_SPSR_FIQ].value, 0, 32),
+			  buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_SPSR_IRQ].value, 0, 32),
+			  buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_SPSR_SVC].value, 0, 32),
+			  buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_SPSR_ABT].value, 0, 32),
+			  buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_SPSR_UND].value, 0, 32));
+	
+	return ERROR_OK;
+}
+
+int handle_armv4_5_core_state_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target = get_current_target(cmd_ctx);
+	armv4_5_common_t *armv4_5 = target->arch_info;
+		
+	if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
+	{
+		command_print(cmd_ctx, "current target isn't an ARMV4/5 target");
+		return ERROR_OK;
+	}
+	
+	if (argc > 0)
+	{
+		if (strcmp(args[0], "arm") == 0)
+		{
+			armv4_5->core_state = ARMV4_5_STATE_ARM;
+		}
+		if (strcmp(args[0], "thumb") == 0)
+		{
+			armv4_5->core_state = ARMV4_5_STATE_THUMB;
+		}
+	}
+	
+	command_print(cmd_ctx, "core state: %s", armv4_5_state_strings[armv4_5->core_state]);
+	
+	return ERROR_OK;
+}
+
+int armv4_5_register_commands(struct command_context_s *cmd_ctx)
+{
+	command_t *armv4_5_cmd;
+
+	armv4_5_cmd = register_command(cmd_ctx, NULL, "armv4_5", NULL, COMMAND_ANY, NULL);
+	
+	register_command(cmd_ctx, armv4_5_cmd, "reg", handle_armv4_5_reg_command, COMMAND_EXEC, "display ARM core registers");
+	register_command(cmd_ctx, armv4_5_cmd, "core_state", handle_armv4_5_core_state_command, COMMAND_EXEC, "display/change ARM core state <arm|thumb>");
+	
+	return ERROR_OK;
+}
+
+int armv4_5_get_gdb_reg_list(target_t *target, reg_t **reg_list[], int *reg_list_size)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	int i;
+	
+	if (target->state != TARGET_HALTED)
+	{
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	
+	*reg_list_size = 26;
+	*reg_list = malloc(sizeof(reg_t*) * (*reg_list_size));
+	
+	for (i = 0; i < 16; i++)
+	{
+		(*reg_list)[i] = &ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i);
+	}
+	
+	for (i = 16; i < 24; i++)
+	{
+		(*reg_list)[i] = &armv4_5_gdb_dummy_fp_reg;
+	}
+	
+	(*reg_list)[24] = &armv4_5_gdb_dummy_fps_reg;
+	(*reg_list)[25] = &armv4_5->core_cache->reg_list[ARMV4_5_CPSR];
+	
+	return ERROR_OK;
+}
+
+int armv4_5_run_algorithm(struct target_s *target, int num_mem_params, mem_param_t *mem_params, int num_reg_params, reg_param_t *reg_params, u32 entry_point, u32 exit_point, int timeout_ms, void *arch_info)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	armv4_5_algorithm_t *armv4_5_algorithm_info = arch_info;
+	enum armv4_5_state core_state = armv4_5->core_state;
+	enum armv4_5_mode core_mode = armv4_5->core_mode;
+	u32 context[17];
+	u32 cpsr;
+	int exit_breakpoint_size = 0;
+	int i;
+	int retval = ERROR_OK;
+	
+	if (armv4_5_algorithm_info->common_magic != ARMV4_5_COMMON_MAGIC)
+	{
+		ERROR("current target isn't an ARMV4/5 target");
+		return ERROR_TARGET_INVALID;
+	}
+	
+	if (target->state != TARGET_HALTED)
+	{
+		WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	
+	for (i = 0; i <= 16; i++)
+	{
+		if (!ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_algorithm_info->core_mode, i).valid)
+			armv4_5->read_core_reg(target, i, armv4_5_algorithm_info->core_mode);
+		context[i] = buf_get_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_algorithm_info->core_mode, i).value, 0, 32);
+	}
+	cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32);
+	
+	for (i = 0; i < num_mem_params; i++)
+	{
+		target_write_buffer(target, mem_params[i].address, mem_params[i].size, mem_params[i].value);
+	}
+	
+	for (i = 0; i < num_reg_params; i++)
+	{
+		reg_t *reg = register_get_by_name(armv4_5->core_cache, reg_params[i].reg_name, 0);
+		if (!reg)
+		{
+			ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
+			exit(-1);
+		}
+		
+		if (reg->size != reg_params[i].size)
+		{
+			ERROR("BUG: register '%s' size doesn't match reg_params[i].size", reg_params[i].reg_name);
+			exit(-1);
+		}
+		
+		armv4_5_set_core_reg(reg, buf_get_u32(reg_params[i].value, 0, 32));
+	}
+	
+	armv4_5->core_state = armv4_5_algorithm_info->core_state;
+	if (armv4_5->core_state == ARMV4_5_STATE_ARM)
+		exit_breakpoint_size = 4;
+	else if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
+		exit_breakpoint_size = 2;
+	else
+	{
+		ERROR("BUG: can't execute algorithms when not in ARM or Thumb state");
+		exit(-1);
+	}
+	
+	if (armv4_5_algorithm_info->core_mode != ARMV4_5_MODE_ANY)
+	{
+		DEBUG("setting core_mode: 0x%2.2x", armv4_5_algorithm_info->core_mode);
+		buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 5, armv4_5_algorithm_info->core_mode);
+		armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 1;
+		armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
+	}
+
+	if ((retval = breakpoint_add(target, exit_point, exit_breakpoint_size, BKPT_HARD)) != ERROR_OK)
+	{
+		ERROR("can't add breakpoint to finish algorithm execution");
+		return ERROR_TARGET_FAILURE;
+	}
+	
+	target->type->resume(target, 0, entry_point, 1, 1);
+	target->type->poll(target);
+	
+	while (target->state != TARGET_HALTED)
+	{
+		usleep(10000);
+		target->type->poll(target);
+		if ((timeout_ms -= 10) <= 0)
+		{
+			ERROR("timeout waiting for algorithm to complete, trying to halt target");
+			target->type->halt(target);
+			timeout_ms = 1000;
+			while (target->state != TARGET_HALTED)
+			{
+				usleep(10000);
+				target->type->poll(target);
+				if ((timeout_ms -= 10) <= 0)
+				{
+					ERROR("target didn't reenter debug state, exiting");
+					exit(-1);
+				}
+			}
+			retval = ERROR_TARGET_TIMEOUT;
+		}
+	}
+	
+	breakpoint_remove(target, exit_point);
+	
+	for (i = 0; i < num_mem_params; i++)
+	{
+		if (mem_params[i].direction != PARAM_OUT)
+			target_read_buffer(target, mem_params[i].address, mem_params[i].size, mem_params[i].value);
+	}
+	
+	for (i = 0; i < num_reg_params; i++)
+	{
+		if (reg_params[i].direction != PARAM_OUT)
+		{
+				
+			reg_t *reg = register_get_by_name(armv4_5->core_cache, reg_params[i].reg_name, 0);
+			if (!reg)
+			{
+				ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
+				exit(-1);
+			}
+			
+			if (reg->size != reg_params[i].size)
+			{
+				ERROR("BUG: register '%s' size doesn't match reg_params[i].size", reg_params[i].reg_name);
+				exit(-1);
+			}
+			
+			buf_set_u32(reg_params[i].value, 0, 32, buf_get_u32(reg->value, 0, 32));
+		}
+	}
+	
+	for (i = 0; i <= 16; i++)
+	{
+		DEBUG("restoring register %s with value 0x%8.8x", ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_algorithm_info->core_mode, i).name, buf_get_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_algorithm_info->core_mode, i).value, 0, 32));
+		buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_algorithm_info->core_mode, i).value, 0, 32, context[i]);
+		ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_algorithm_info->core_mode, i).valid = 1;
+		ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_algorithm_info->core_mode, i).dirty = 1;
+	}
+	buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32, cpsr);
+	armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
+	armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 1;
+	
+	armv4_5->core_state = core_state;
+	armv4_5->core_mode = core_mode;
+
+	return retval;
+}
+
+int armv4_5_init_arch_info(target_t *target, armv4_5_common_t *armv4_5)
+{	
+	target->arch_info = armv4_5;
+
+	armv4_5->common_magic = ARMV4_5_COMMON_MAGIC;
+	armv4_5->core_state = ARMV4_5_STATE_ARM;
+	armv4_5->core_mode = ARMV4_5_MODE_USR;
+	
+	return ERROR_OK;
+}
diff --git a/src/target/armv4_5.h b/src/target/armv4_5.h
new file mode 100644
index 00000000..a28bfa12
--- /dev/null
+++ b/src/target/armv4_5.h
@@ -0,0 +1,237 @@
+/***************************************************************************
+ *   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 ARMV4_5_H
+#define ARMV4_5_H
+
+#include "register.h"
+#include "target.h"
+
+enum armv4_5_mode
+{
+	ARMV4_5_MODE_USR = 16, 
+	ARMV4_5_MODE_FIQ = 17, 
+	ARMV4_5_MODE_IRQ = 18, 
+	ARMV4_5_MODE_SVC = 19, 
+	ARMV4_5_MODE_ABT = 23,
+	ARMV4_5_MODE_UND = 27,
+	ARMV4_5_MODE_SYS = 31,
+	ARMV4_5_MODE_ANY = -1
+};
+
+extern char* armv4_5_mode_strings[];
+
+enum armv4_5_state
+{
+	ARMV4_5_STATE_ARM,
+	ARMV4_5_STATE_THUMB,
+	ARMV4_5_STATE_JAZELLE,
+};
+
+extern char* armv4_5_state_strings[];
+
+extern int armv4_5_core_reg_map[7][17];
+
+#define ARMV4_5_CORE_REG_MODE(cache, mode, num) \
+		cache->reg_list[armv4_5_core_reg_map[armv4_5_mode_to_number(mode)][num]]
+#define ARMV4_5_CORE_REG_MODENUM(cache, mode, num) \
+		cache->reg_list[armv4_5_core_reg_map[mode][num]]
+
+/* offsets into armv4_5 core register cache */
+enum 
+{
+	ARMV4_5_CPSR = 31,
+	ARMV4_5_SPSR_FIQ = 32,
+	ARMV4_5_SPSR_IRQ = 33,
+	ARMV4_5_SPSR_SVC = 34,
+	ARMV4_5_SPSR_ABT = 35,
+	ARMV4_5_SPSR_UND = 36
+};
+
+#define ARMV4_5_COMMON_MAGIC 0x0A450A45
+
+typedef struct armv4_5_common_s
+{
+	int common_magic;
+	reg_cache_t *core_cache;
+	enum armv4_5_mode core_mode;
+	enum armv4_5_state core_state;
+	int (*full_context)(struct target_s *target);
+	int (*read_core_reg)(struct target_s *target, int num, enum armv4_5_mode mode);
+	int (*write_core_reg)(struct target_s *target, int num, enum armv4_5_mode mode, u32 value);
+	void *arch_info;
+} armv4_5_common_t;
+
+typedef struct armv4_5_algorithm_s
+{
+	int common_magic;
+		
+	enum armv4_5_mode core_mode;
+	enum armv4_5_state core_state;
+} armv4_5_algorithm_t;
+
+typedef struct armv4_5_core_reg_s
+{
+	int num;
+	enum armv4_5_mode mode;
+	target_t *target;
+	armv4_5_common_t *armv4_5_common;
+} armv4_5_core_reg_t;
+
+extern reg_cache_t* armv4_5_build_reg_cache(target_t *target, armv4_5_common_t *armv4_5_common);
+extern enum armv4_5_mode armv4_5_number_to_mode(int number);
+extern int armv4_5_mode_to_number(enum armv4_5_mode mode);
+
+extern int armv4_5_arch_state(struct target_s *target, char *buf, int buf_size);
+extern int armv4_5_get_gdb_reg_list(target_t *target, reg_t **reg_list[], int *reg_list_size);
+extern int armv4_5_invalidate_core_regs(target_t *target);
+
+extern int armv4_5_register_commands(struct command_context_s *cmd_ctx);
+extern int armv4_5_init_arch_info(target_t *target, armv4_5_common_t *armv4_5);
+
+extern int armv4_5_run_algorithm(struct target_s *target, int num_mem_params, mem_param_t *mem_params, int num_reg_params, reg_param_t *reg_params, u32 entry_point, u32 exit_point, int timeout_ms, void *arch_info);
+
+extern int armv4_5_invalidate_core_regs(target_t *target);
+
+/* ARM mode instructions
+ */
+ 
+/* Store multiple increment after
+ * Rn: base register
+ * List: for each bit in list: store register
+ * S: in priviledged mode: store user-mode registers
+ * W=1: update the base register. W=0: leave the base register untouched
+ */
+#define ARMV4_5_STMIA(Rn, List, S, W)	(0xe8800000 | (S << 22) | (W << 21) | (Rn << 16) | (List))
+
+/* Load multiple increment after
+ * Rn: base register
+ * List: for each bit in list: store register
+ * S: in priviledged mode: store user-mode registers
+ * W=1: update the base register. W=0: leave the base register untouched
+ */
+#define ARMV4_5_LDMIA(Rn, List, S, W)	(0xe8900000 | (S << 22) | (W << 21) | (Rn << 16) | (List))
+
+/* MOV r8, r8 */
+#define ARMV4_5_NOP					(0xe1a08008)
+
+/* Move PSR to general purpose register
+ * R=1: SPSR R=0: CPSR
+ * Rn: target register
+ */
+#define ARMV4_5_MRS(Rn, R)			(0xe10f0000 | (R << 22) | (Rn << 12))
+
+/* Store register
+ * Rd: register to store
+ * Rn: base register
+ */
+#define ARMV4_5_STR(Rd, Rn)			(0xe5800000 | (Rd << 12) | (Rn << 16))
+
+/* Load register
+ * Rd: register to load
+ * Rn: base register
+ */
+#define ARMV4_5_LDR(Rd, Rn)			(0xe5900000 | (Rd << 12) | (Rn << 16))
+
+/* Move general purpose register to PSR
+ * R=1: SPSR R=0: CPSR
+ * Field: Field mask
+ * 1: control field 2: extension field 4: status field 8: flags field
+ * Rm: source register
+ */
+#define ARMV4_5_MSR_GP(Rm, Field, R)	(0xe120f000 | Rm | (Field << 16) | (R << 22))
+#define ARMV4_5_MSR_IM(Im, Rotate, Field, R)	(0xe320f000 | (Im)  | (Rotate << 8) | (Field << 16) | (R << 22))
+
+/* Load Register Halfword Immediate Post-Index
+ * Rd: register to load
+ * Rn: base register
+ */
+#define ARMV4_5_LDRH_IP(Rd, Rn)	(0xe0d000b2 | (Rd << 12) | (Rn << 16))
+
+/* Load Register Byte Immediate Post-Index
+ * Rd: register to load
+ * Rn: base register
+ */
+#define ARMV4_5_LDRB_IP(Rd, Rn)	(0xe4d00001 | (Rd << 12) | (Rn << 16))
+
+/* Store register Halfword Immediate Post-Index
+ * Rd: register to store
+ * Rn: base register
+ */
+#define ARMV4_5_STRH_IP(Rd, Rn)	(0xe0c000b2 | (Rd << 12) | (Rn << 16))
+
+/* Store register Byte Immediate Post-Index
+ * Rd: register to store
+ * Rn: base register
+ */
+#define ARMV4_5_STRB_IP(Rd, Rn)	(0xe4c00001 | (Rd << 12) | (Rn << 16))
+
+/* Branch (and Link)
+ * Im: Branch target (left-shifted by 2 bits, added to PC)
+ * L: 1: branch and link 0: branch only
+ */
+#define ARMV4_5_B(Im, L) (0xea000000 | Im | (L << 24))
+
+/* Branch and exchange (ARM state)
+ * Rm: register holding branch target address
+ */
+#define ARMV4_5_BX(Rm) (0xe12fff10 | Rm)
+
+/* Thumb mode instructions
+ */
+ 
+/* Store register (Thumb mode)
+ * Rd: source register
+ * Rn: base register
+ */
+#define ARMV4_5_T_STR(Rd, Rn)	((0x6000 | Rd | (Rn << 3)) | ((0x6000 | Rd | (Rn << 3)) << 16))
+
+/* Load register (Thumb state)
+ * Rd: destination register
+ * Rn: base register
+ */
+#define ARMV4_5_T_LDR(Rd, Rn)	((0x6800 | (Rn << 3) | Rd) | ((0x6800 | (Rn << 3) | Rd) << 16))
+
+/* Move hi register (Thumb mode)
+ * Rd: destination register
+ * Rm: source register
+ */
+#define ARMV4_5_T_MOV(Rd, Rm)	((0x4600 | (Rd & 0x7) | ((Rd & 0x8) << 4) | ((Rm & 0x7) << 3) | ((Rm & 0x8) << 3)) | ((0x4600 | (Rd & 0x7) | ((Rd & 0x8) << 4) | ((Rm & 0x7) << 3) | ((Rm & 0x8) << 3)) << 16))
+
+/* No operation (Thumb mode)
+ */
+#define ARMV4_5_T_NOP	(0x1c3f | (0x1c3f << 16))
+
+/* Move immediate to register (Thumb state)
+ * Rd: destination register
+ * Im: 8-bit immediate value
+ */
+#define ARMV4_5_T_MOV_IM(Rd, Im)	((0x2000 | (Rd << 8) | Im) | ((0x2000 | (Rd << 8) | Im) << 16))
+
+/* Branch and Exchange
+ * Rm: register containing branch target
+ */
+#define ARMV4_5_T_BX(Rm)		((0x4700 | (Rm << 3)) | ((0x4700 | (Rm << 3)) << 16))
+
+/* Branch (Thumb state)
+ * Imm: Branch target
+ */
+#define ARMV4_5_T_B(Imm)	((0xe000 | Imm) | ((0xe000 | Imm) << 16))
+
+#endif /* ARMV4_5_H */
diff --git a/src/target/armv4_5_cache.c b/src/target/armv4_5_cache.c
new file mode 100644
index 00000000..326f10ee
--- /dev/null
+++ b/src/target/armv4_5_cache.c
@@ -0,0 +1,112 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "armv4_5_cache.h"
+
+#include "log.h"
+#include "command.h"
+
+int armv4_5_identify_cache(u32 cache_type_reg, armv4_5_cache_common_t *cache)
+{
+	int size, assoc, M, len, multiplier;
+
+	cache->ctype = (cache_type_reg & 0x1e000000U) >> 25;
+	cache->separate = (cache_type_reg & 0x01000000U) >> 24;
+
+	size = (cache_type_reg & 0x1c0000) >> 18;
+	assoc = (cache_type_reg & 0x38000) >> 15;
+	M = (cache_type_reg & 0x4000) >> 14;
+	len = (cache_type_reg & 0x3000) >> 12;
+	multiplier = 2 + M;
+
+	if ((assoc != 0) || (M != 1)) /* assoc 0 and M 1 means cache absent */
+	{
+		/* cache is present */
+		cache->d_u_size.linelen = 1 << (len + 3);
+		cache->d_u_size.associativity = multiplier << (assoc - 1);
+		cache->d_u_size.nsets = 1 << (size + 6 - assoc - len);
+		cache->d_u_size.cachesize = multiplier << (size + 8);
+	}
+	else
+	{
+		/* cache is absent */
+		cache->d_u_size.linelen = -1;
+		cache->d_u_size.associativity = -1;
+		cache->d_u_size.nsets = -1;
+		cache->d_u_size.cachesize = -1;
+	}
+
+	if (cache->separate)
+	{
+		size = (cache_type_reg & 0x1c0) >> 6;
+		assoc = (cache_type_reg & 0x38) >> 3;
+		M = (cache_type_reg & 0x4) >> 2;
+		len = (cache_type_reg & 0x3);
+		multiplier = 2 + M;
+		
+		if ((assoc != 0) || (M != 1)) /* assoc 0 and M 1 means cache absent */
+		{
+			/* cache is present */
+			cache->i_size.linelen = 1 << (len + 3);
+			cache->i_size.associativity = multiplier << (assoc - 1);
+			cache->i_size.nsets = 1 << (size + 6 - assoc - len);
+			cache->i_size.cachesize = multiplier << (size + 8);
+		}
+		else
+		{
+			/* cache is absent */
+			cache->i_size.linelen = -1;
+			cache->i_size.associativity = -1;
+			cache->i_size.nsets = -1;
+			cache->i_size.cachesize = -1;
+		}
+	}
+	else
+	{
+		cache->i_size = cache->d_u_size;
+	}
+	
+	return ERROR_OK;
+}
+
+int armv4_5_handle_cache_info_command(struct command_context_s *cmd_ctx, armv4_5_cache_common_t *armv4_5_cache)
+{
+	if (armv4_5_cache->ctype == -1)
+	{
+		command_print(cmd_ctx, "cache not yet identified");
+		return ERROR_OK;
+	}
+		
+	command_print(cmd_ctx, "cache type: 0x%1.1x, %s", armv4_5_cache->ctype, 
+		(armv4_5_cache->separate) ? "separate caches" : "unified cache");
+
+	command_print(cmd_ctx, "D-Cache: linelen %i, associativity %i, nsets %i, cachesize 0x%x", 
+		armv4_5_cache->d_u_size.linelen,
+		armv4_5_cache->d_u_size.associativity,
+		armv4_5_cache->d_u_size.nsets,
+		armv4_5_cache->d_u_size.cachesize);
+
+	command_print(cmd_ctx, "I-Cache: linelen %i, associativity %i, nsets %i, cachesize 0x%x", 
+		armv4_5_cache->i_size.linelen,
+		armv4_5_cache->i_size.associativity,
+		armv4_5_cache->i_size.nsets,
+		armv4_5_cache->i_size.cachesize);
+	
+	return ERROR_OK;
+}
diff --git a/src/target/armv4_5_cache.h b/src/target/armv4_5_cache.h
new file mode 100644
index 00000000..766718b6
--- /dev/null
+++ b/src/target/armv4_5_cache.h
@@ -0,0 +1,49 @@
+/***************************************************************************
+ *   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 ARMV4_5_CACHE_H
+#define ARMV4_5_CACHE_H
+
+#include "types.h"
+#include "command.h"
+
+typedef struct armv4_5_cachesize_s
+{
+	int linelen;
+	int associativity;
+	int nsets;
+	int cachesize;
+} armv4_5_cachesize_t;
+
+typedef struct armv4_5_cache_common_s
+{
+	int ctype;	/* specify supported cache operations */
+	int separate;	/* separate caches or unified cache */
+	armv4_5_cachesize_t d_u_size;	/* data cache */
+	armv4_5_cachesize_t i_size; /* instruction cache */
+	int i_cache_enabled;
+	int d_u_cache_enabled;
+} armv4_5_cache_common_t;
+
+extern int armv4_5_identify_cache(u32 cache_type_reg, armv4_5_cache_common_t *cache);
+extern int armv4_5_cache_state(u32 cp15_control_reg, armv4_5_cache_common_t *cache);
+
+extern int armv4_5_handle_cache_info_command(struct command_context_s *cmd_ctx, armv4_5_cache_common_t *armv4_5_cache);
+
+#endif /* ARMV4_5_CACHE_H */
diff --git a/src/target/armv4_5_mmu.c b/src/target/armv4_5_mmu.c
new file mode 100644
index 00000000..c7dad3e2
--- /dev/null
+++ b/src/target/armv4_5_mmu.c
@@ -0,0 +1,358 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "arm7_9_common.h"
+#include "log.h"
+#include "command.h"
+#include "armv4_5_mmu.h"
+
+#include <stdlib.h>
+
+u32 armv4mmu_translate_va(target_t *target, armv4_5_mmu_common_t *armv4_5_mmu, u32 va, int *type, u32 *cb, int *domain, u32 *ap);
+int armv4_5_mmu_read_physical(target_t *target, armv4_5_mmu_common_t *armv4_5_mmu, u32 address, u32 size, u32 count, u8 *buffer);
+int armv4_5_mmu_write_physical(target_t *target, armv4_5_mmu_common_t *armv4_5_mmu, u32 address, u32 size, u32 count, u8 *buffer);
+
+char* armv4_5_mmu_page_type_names[] =
+{
+	"section", "large page", "small page", "tiny page"
+};
+
+u32 armv4_5_mmu_translate_va(target_t *target, armv4_5_mmu_common_t *armv4_5_mmu, u32 va, int *type, u32 *cb, int *domain, u32 *ap)
+{
+	u32 first_lvl_descriptor = 0x0;
+	u32 second_lvl_descriptor = 0x0;
+	u32 ttb = armv4_5_mmu->get_ttb(target);
+
+	armv4_5_mmu_read_physical(target, armv4_5_mmu,
+		(ttb & 0xffffc000) | ((va & 0xfff00000) >> 18),
+		4, 1, (u8*)&first_lvl_descriptor);
+
+	DEBUG("1st lvl desc: %8.8x", first_lvl_descriptor);
+
+	if ((first_lvl_descriptor & 0x3) == 0)
+	{
+		*type = -1;
+		return ERROR_TARGET_TRANSLATION_FAULT;
+	}
+
+	if (!armv4_5_mmu->has_tiny_pages && ((first_lvl_descriptor & 0x3) == 3))
+	{
+		*type = -1;
+		return ERROR_TARGET_TRANSLATION_FAULT;
+	}
+
+	/* domain is always specified in bits 8-5 */
+	*domain = (first_lvl_descriptor & 0x1e0) >> 5;
+
+	if ((first_lvl_descriptor & 0x3) == 2)
+	{
+		/* section descriptor */
+		*type = ARMV4_5_SECTION;
+		*cb = (first_lvl_descriptor & 0xc) >> 2;
+		*ap = (first_lvl_descriptor & 0xc00) >> 10;
+		return (first_lvl_descriptor & 0xfff00000) | (va & 0x000fffff);
+	}
+
+	if ((first_lvl_descriptor & 0x3) == 1)
+	{
+		/* coarse page table */
+		armv4_5_mmu_read_physical(target, armv4_5_mmu,
+			(first_lvl_descriptor & 0xfffffc00) | ((va & 0x000ff000) >> 10),
+			4, 1, (u8*)&second_lvl_descriptor);
+	}
+
+	if ((first_lvl_descriptor & 0x3) == 3)
+	{
+		/* fine page table */
+		armv4_5_mmu_read_physical(target, armv4_5_mmu,
+			(first_lvl_descriptor & 0xfffff000) | ((va & 0x000ffc00) >> 8),
+			4, 1, (u8*)&second_lvl_descriptor);
+	}
+
+	DEBUG("2nd lvl desc: %8.8x", first_lvl_descriptor);
+
+	if ((second_lvl_descriptor & 0x3) == 0)
+	{
+		*type = -1;
+		return ERROR_TARGET_TRANSLATION_FAULT;
+	}
+
+	/* cacheable/bufferable is always specified in bits 3-2 */
+	*cb = (second_lvl_descriptor & 0xc) >> 2;
+
+	if ((second_lvl_descriptor & 0x3) == 1)
+	{
+		/* large page descriptor */
+		*type = ARMV4_5_LARGE_PAGE;
+		*ap = (second_lvl_descriptor & 0xff0) >> 4;
+		return (second_lvl_descriptor & 0xffff0000) | (va & 0x0000ffff);
+	}
+
+	if ((second_lvl_descriptor & 0x3) == 2)
+	{
+		/* small page descriptor */
+		*type = ARMV4_5_SMALL_PAGE;
+		*ap = (second_lvl_descriptor & 0xff0) >> 4;
+		return (second_lvl_descriptor & 0xfffff000) | (va & 0x00000fff);
+	}
+
+	if ((second_lvl_descriptor & 0x3) == 3)
+	{
+		/* tiny page descriptor */
+		*type = ARMV4_5_TINY_PAGE;
+		*ap = (second_lvl_descriptor & 0x30) >> 4;
+		return (second_lvl_descriptor & 0xfffffc00) | (va & 0x000003ff);
+	}
+
+	/* should not happen */
+	*type = -1;
+	return ERROR_TARGET_TRANSLATION_FAULT;
+}
+
+int armv4_5_mmu_read_physical(target_t *target, armv4_5_mmu_common_t *armv4_5_mmu, u32 address, u32 size, u32 count, u8 *buffer)
+{
+	int retval;
+
+	if (target->state != TARGET_HALTED)
+		return ERROR_TARGET_NOT_HALTED;
+
+	/* disable MMU and data (or unified) cache */
+	armv4_5_mmu->disable_mmu_caches(target, 1, 1, 0);
+
+	retval = armv4_5_mmu->read_memory(target, address, size, count, buffer);
+
+	/* reenable MMU / cache */
+	armv4_5_mmu->enable_mmu_caches(target, armv4_5_mmu->mmu_enabled,
+		armv4_5_mmu->armv4_5_cache.d_u_cache_enabled,
+		armv4_5_mmu->armv4_5_cache.i_cache_enabled);
+
+	return retval;
+}
+
+int armv4_5_mmu_write_physical(target_t *target, armv4_5_mmu_common_t *armv4_5_mmu, u32 address, u32 size, u32 count, u8 *buffer)
+{
+	int retval;
+
+	if (target->state != TARGET_HALTED)
+		return ERROR_TARGET_NOT_HALTED;
+
+	/* disable MMU and data (or unified) cache */
+	armv4_5_mmu->disable_mmu_caches(target, 1, 1, 0);
+	
+	retval = armv4_5_mmu->write_memory(target, address, size, count, buffer);
+
+	/* reenable MMU / cache */
+	armv4_5_mmu->enable_mmu_caches(target, armv4_5_mmu->mmu_enabled,
+		armv4_5_mmu->armv4_5_cache.d_u_cache_enabled,
+		armv4_5_mmu->armv4_5_cache.i_cache_enabled);
+	
+	return retval;
+}
+
+int armv4_5_mmu_handle_virt2phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc, target_t *target, armv4_5_mmu_common_t *armv4_5_mmu)
+{
+	u32 va;
+	u32 pa;
+	int type;
+	u32 cb;
+	int domain;
+	u32 ap;
+	
+	if (target->state != TARGET_HALTED)
+	{
+		command_print(cmd_ctx, "target must be stopped for \"virt2phys\" command");
+		return ERROR_OK;
+	}
+
+	if (argc == 0)
+	{
+		command_print(cmd_ctx, "usage: virt2phys <virtual address>");
+		return ERROR_OK;
+	}
+
+	if (argc == 1)
+	{
+		va = strtoul(args[0], NULL, 0);
+		pa = armv4_5_mmu_translate_va(target, armv4_5_mmu, va, &type, &cb, &domain, &ap);
+		if (type == -1)
+		{
+			switch (pa)
+			{
+				case ERROR_TARGET_TRANSLATION_FAULT:
+					command_print(cmd_ctx, "no valid translation for 0x%8.8x", va);
+					break;
+				default:
+					command_print(cmd_ctx, "unknown translation error");
+			}
+			return ERROR_OK;
+		}
+	
+		command_print(cmd_ctx, "0x%8.8x -> 0x%8.8x, type: %s, cb: %i, domain: %i, ap: %2.2x",
+			va, pa, armv4_5_mmu_page_type_names[type], cb, domain, ap);
+	}			
+	
+	return ERROR_OK;
+}
+
+int armv4_5_mmu_handle_md_phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc, target_t *target, armv4_5_mmu_common_t *armv4_5_mmu)
+{
+	int count = 1;
+	int size = 4;
+	u32 address = 0;
+	int i;
+
+	char output[128];
+	int output_len;
+
+	int retval;
+
+	u8 *buffer;
+
+	if (target->state != TARGET_HALTED)
+	{
+		command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
+		return ERROR_OK;
+	}
+
+	if (argc < 1)
+		return ERROR_OK;
+
+	if (argc == 2)
+		count = strtoul(args[1], NULL, 0);
+
+	address = strtoul(args[0], NULL, 0);
+
+	switch (cmd[2])
+	{
+		case 'w':
+			size = 4;
+			break;
+		case 'h':
+			size = 2;
+			break;
+		case 'b':
+			size = 1;
+			break;
+		default:
+			return ERROR_OK;
+	}
+
+	buffer = calloc(count, size);
+	if ((retval  = armv4_5_mmu_read_physical(target, armv4_5_mmu, address, size, count, buffer)) != ERROR_OK)
+	{
+		switch (retval)
+		{
+			case ERROR_TARGET_UNALIGNED_ACCESS:
+				command_print(cmd_ctx, "error: address not aligned");
+				break;
+			case ERROR_TARGET_NOT_HALTED:
+				command_print(cmd_ctx, "error: target must be halted for memory accesses");
+				break;			
+			case ERROR_TARGET_DATA_ABORT:
+				command_print(cmd_ctx, "error: access caused data abort, system possibly corrupted");
+				break;
+			default:
+				command_print(cmd_ctx, "error: unknown error");
+		}
+	}
+
+	output_len = 0;
+
+	for (i = 0; i < count; i++)
+	{
+		if (i%8 == 0)
+			output_len += snprintf(output + output_len, 128 - output_len, "0x%8.8x: ", address + (i*size));
+		
+		switch (size)
+		{
+			case 4:
+				output_len += snprintf(output + output_len, 128 - output_len, "%8.8x ", ((u32*)buffer)[i]);
+				break;
+			case 2:
+				output_len += snprintf(output + output_len, 128 - output_len, "%4.4x ", ((u16*)buffer)[i]);
+				break;
+			case 1:
+				output_len += snprintf(output + output_len, 128 - output_len, "%2.2x ", ((u8*)buffer)[i]);
+				break;
+		}
+
+		if ((i%8 == 7) || (i == count - 1))
+		{
+			command_print(cmd_ctx, output);
+			output_len = 0;
+		}
+	}
+
+	free(buffer);
+	
+	return ERROR_OK;
+}
+
+int armv4_5_mmu_handle_mw_phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc, target_t *target, armv4_5_mmu_common_t *armv4_5_mmu)
+{
+	u32 address = 0;
+	u32 value = 0;
+	int retval;
+
+	if (target->state != TARGET_HALTED)
+	{
+		command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
+		return ERROR_OK;
+	}
+
+	if (argc < 2)
+		return ERROR_OK;
+
+	address = strtoul(args[0], NULL, 0);
+	value = strtoul(args[1], NULL, 0);
+
+	switch (cmd[2])
+	{
+		case 'w':
+			retval = armv4_5_mmu_write_physical(target, armv4_5_mmu, address, 4, 1, (u8*)&value);
+			break;
+		case 'h':
+			retval = armv4_5_mmu_write_physical(target, armv4_5_mmu, address, 2, 1, (u8*)&value);
+			break;
+		case 'b':
+			retval = armv4_5_mmu_write_physical(target, armv4_5_mmu, address, 1, 1, (u8*)&value);
+			break;
+		default:
+			return ERROR_OK;
+	}
+
+	switch (retval)
+	{
+		case ERROR_TARGET_UNALIGNED_ACCESS:
+			command_print(cmd_ctx, "error: address not aligned");
+			break;
+		case ERROR_TARGET_DATA_ABORT:
+			command_print(cmd_ctx, "error: access caused data abort, system possibly corrupted");
+			break;
+		case ERROR_TARGET_NOT_HALTED:
+			command_print(cmd_ctx, "error: target must be halted for memory accesses");
+			break;
+		case ERROR_OK:
+			break;
+		default:
+			command_print(cmd_ctx, "error: unknown error");
+	}	
+
+	return ERROR_OK;
+}
diff --git a/src/target/armv4_5_mmu.h b/src/target/armv4_5_mmu.h
new file mode 100644
index 00000000..3adb30e2
--- /dev/null
+++ b/src/target/armv4_5_mmu.h
@@ -0,0 +1,52 @@
+/***************************************************************************
+ *   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 ARMV4_5_MMU_H
+#define ARMV4_5_MMU_H
+
+#include "armv4_5_cache.h"
+
+typedef struct armv4_5_mmu_common_s
+{
+	u32 (*get_ttb)(target_t *target);
+	int (*read_memory)(target_t *target, u32 address, u32 size, u32 count, u8 *buffer);
+	int (*write_memory)(target_t *target, u32 address, u32 size, u32 count, u8 *buffer);
+	void (*disable_mmu_caches)(target_t *target, int mmu, int d_u_cache, int i_cache);
+	void (*enable_mmu_caches)(target_t *target, int mmu, int d_u_cache, int i_cache);
+	armv4_5_cache_common_t armv4_5_cache;
+	int has_tiny_pages;
+	int mmu_enabled;
+} armv4_5_mmu_common_t;
+
+enum
+{
+	ARMV4_5_SECTION, ARMV4_5_LARGE_PAGE, ARMV4_5_SMALL_PAGE, ARMV4_5_TINY_PAGE
+};
+
+extern char* armv4_5_page_type_names[];
+
+extern u32 armv4_5_mmu_translate_va(target_t *target, armv4_5_mmu_common_t *armv4_5_mmu, u32 va, int *type, u32 *cb, int *domain, u32 *ap);
+extern int armv4_5_mmu_read_physical(target_t *target, armv4_5_mmu_common_t *armv4_5_mmu, u32 address, u32 size, u32 count, u8 *buffer);
+extern int armv4_5_mmu_write_physical(target_t *target, armv4_5_mmu_common_t *armv4_5_mmu, u32 address, u32 size, u32 count, u8 *buffer);
+
+extern int armv4_5_mmu_handle_virt2phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, target_t *target, armv4_5_mmu_common_t *armv4_5_mmu);
+extern int armv4_5_mmu_handle_md_phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, target_t *target, armv4_5_mmu_common_t *armv4_5_mmu);
+extern int armv4_5_mmu_handle_mw_phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, target_t *target, armv4_5_mmu_common_t *armv4_5_mmu);
+
+#endif /* ARMV4_5_MMU_H */
diff --git a/src/target/breakpoints.c b/src/target/breakpoints.c
new file mode 100644
index 00000000..efba25c9
--- /dev/null
+++ b/src/target/breakpoints.c
@@ -0,0 +1,219 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "config.h"
+
+#include <stdlib.h>
+
+#include "binarybuffer.h"
+#include "target.h"
+#include "log.h"
+#include "types.h"
+
+#include "breakpoints.h"
+
+char *breakpoint_type_strings[] =
+{
+	"hardware",
+	"software"
+};
+
+char *watchpoint_rw_strings[] =
+{
+	"read",
+	"write",
+	"access"
+};
+
+int breakpoint_add(target_t *target, u32 address, u32 length, enum breakpoint_type type)
+{
+	breakpoint_t *breakpoint = target->breakpoints;
+	breakpoint_t **breakpoint_p = &target->breakpoints;
+	int retval;
+		
+	while (breakpoint)
+	{
+		if (breakpoint->address == address)
+			return ERROR_OK;
+		breakpoint_p = &breakpoint->next;
+		breakpoint = breakpoint->next;
+	}
+	
+	if ((retval = target->type->add_breakpoint(target, address, length, type)) != ERROR_OK)
+	{
+		switch (retval)
+		{
+			case ERROR_TARGET_RESOURCE_NOT_AVAILABLE:
+				INFO("can't add %s breakpoint, resource not available", breakpoint_type_strings[type]);
+				return retval;
+				break;
+			case ERROR_TARGET_NOT_HALTED:
+				INFO("can't add breakpoint while target is running");
+				return retval;
+				break;
+			default:
+				ERROR("unknown error");
+				exit(-1);
+				break;
+		}
+	}
+	
+	(*breakpoint_p) = malloc(sizeof(breakpoint_t));
+	(*breakpoint_p)->address = address;
+	(*breakpoint_p)->length = length;
+	(*breakpoint_p)->type = type;
+	(*breakpoint_p)->set = 0;
+	(*breakpoint_p)->orig_instr = malloc(CEIL(length, 8));
+	(*breakpoint_p)->next = NULL;
+	
+	DEBUG("added %s breakpoint at 0x%8.8x of length 0x%8.8x", breakpoint_type_strings[type], address, length);
+	
+	return ERROR_OK;
+}
+
+int breakpoint_remove(target_t *target, u32 address)
+{
+	breakpoint_t *breakpoint = target->breakpoints;
+	breakpoint_t **breakpoint_p = &target->breakpoints;
+	int retval;
+	
+	while (breakpoint)
+	{
+		if (breakpoint->address == address)
+			break;
+		breakpoint_p = &breakpoint->next;
+		breakpoint = breakpoint->next;
+	}
+	
+	if (breakpoint)
+	{
+		if ((retval = target->type->remove_breakpoint(target, breakpoint)) != ERROR_OK)
+		{
+			switch (retval)
+			{
+				case ERROR_TARGET_NOT_HALTED:
+					INFO("can't remove breakpoint while target is running");
+					return retval;
+					break;
+				default:
+					ERROR("unknown error");
+					exit(-1);
+					break;
+			}
+		}
+		(*breakpoint_p) = breakpoint->next;
+		free(breakpoint->orig_instr);
+		free(breakpoint);
+	}
+	else
+	{
+		ERROR("no breakpoint at address 0x%8.8x found", address);
+	}
+	
+	return ERROR_OK;
+}
+
+breakpoint_t* breakpoint_find(target_t *target, u32 address)
+{
+	breakpoint_t *breakpoint = target->breakpoints;
+	
+	while (breakpoint)
+	{
+		if (breakpoint->address == address)
+			return breakpoint;
+		breakpoint = breakpoint->next;
+	}
+	
+	return NULL;
+}
+
+int watchpoint_add(target_t *target, u32 address, u32 length, enum watchpoint_rw rw, u32 value, u32 mask)
+{
+	watchpoint_t *watchpoint = target->watchpoints;
+	watchpoint_t **watchpoint_p = &target->watchpoints;
+	int retval;
+		
+	while (watchpoint)
+	{
+		if (watchpoint->address == address)
+			return ERROR_OK;
+		watchpoint_p = &watchpoint->next;
+		watchpoint = watchpoint->next;
+	}
+	
+	if ((retval = target->type->add_watchpoint(target, address, length, rw)) != ERROR_OK)
+	{
+		switch (retval)
+		{
+			case ERROR_TARGET_RESOURCE_NOT_AVAILABLE:
+				INFO("can't add %s watchpoint, resource not available", watchpoint_rw_strings[rw]);
+				return retval;
+				break;
+			default:
+				ERROR("unknown error");
+				exit(-1);
+				break;
+		}
+	}
+	
+	(*watchpoint_p) = malloc(sizeof(watchpoint_t));
+	(*watchpoint_p)->address = address;
+	(*watchpoint_p)->length = length;
+	(*watchpoint_p)->value = value;
+	(*watchpoint_p)->mask = mask;
+	(*watchpoint_p)->rw = rw;
+	(*watchpoint_p)->set = 0;
+	(*watchpoint_p)->next = NULL;
+	
+	DEBUG("added %s watchpoint at 0x%8.8x of length 0x%8.8x", watchpoint_rw_strings[rw], address, length);
+	
+	return ERROR_OK;
+}
+
+int watchpoint_remove(target_t *target, u32 address)
+{
+	watchpoint_t *watchpoint = target->watchpoints;
+	watchpoint_t **watchpoint_p = &target->watchpoints;
+	int retval;
+	
+	while (watchpoint)
+	{
+		if (watchpoint->address == address)
+			break;
+		watchpoint_p = &watchpoint->next;
+		watchpoint = watchpoint->next;
+	}
+	
+	if (watchpoint)
+	{
+		if ((retval = target->type->remove_watchpoint(target, watchpoint)) != ERROR_OK)
+		{
+			ERROR("BUG: can't remove watchpoint");
+			exit(-1);
+		}
+		(*watchpoint_p) = watchpoint->next;
+		free(watchpoint);
+	}
+	else
+	{
+		ERROR("no watchpoint at address 0x%8.8x found", address);
+	}
+	
+	return ERROR_OK;
+}
diff --git a/src/target/breakpoints.h b/src/target/breakpoints.h
new file mode 100644
index 00000000..7eba39aa
--- /dev/null
+++ b/src/target/breakpoints.h
@@ -0,0 +1,70 @@
+/***************************************************************************
+ *   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 BREAKPOINTS_H
+#define BREAKPOINTS_H
+
+#include "target.h"
+
+struct target_s;
+
+enum breakpoint_type
+{
+	BKPT_HARD,
+	BKPT_SOFT,
+};
+
+extern char *breakpoint_type_strings[];
+
+enum watchpoint_rw
+{
+	WPT_READ = 0, WPT_WRITE = 1, WPT_ACCESS = 2
+};
+
+extern char *watchpoint_rw_strings[];
+
+typedef struct breakpoint_s
+{
+	u32 address;
+	int length;
+	enum breakpoint_type type;
+	int set;
+	u8 *orig_instr;
+	struct breakpoint_s *next;
+} breakpoint_t;
+
+typedef struct watchpoint_s
+{
+	u32 address;
+	int length;
+	u32 mask;
+	u32 value;
+	enum watchpoint_rw rw;
+	int set;
+	struct watchpoint_s *next;
+} watchpoint_t;
+
+extern int breakpoint_add(struct target_s *target, u32 address, u32 length, enum breakpoint_type type);
+extern int breakpoint_remove(struct target_s *target, u32 address);
+extern breakpoint_t* breakpoint_find(struct target_s *target, u32 address);
+extern int watchpoint_add(struct target_s *target, u32 address, u32 length, enum watchpoint_rw rw, u32 value, u32 mask);
+extern int watchpoint_remove(struct target_s *target, u32 address);
+
+#endif /* BREAKPOINTS_H */
+
diff --git a/src/target/embeddedice.c b/src/target/embeddedice.c
new file mode 100644
index 00000000..e148b888
--- /dev/null
+++ b/src/target/embeddedice.c
@@ -0,0 +1,301 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "config.h"
+
+#include "embeddedice.h"
+
+#include "armv4_5.h"
+#include "arm7_9_common.h"
+
+#include "log.h"
+#include "arm_jtag.h"
+#include "types.h"
+#include "binarybuffer.h"
+#include "target.h"
+#include "register.h"
+#include "jtag.h"
+
+#include <stdlib.h>
+
+bitfield_desc_t embeddedice_comms_ctrl_bitfield_desc[] = 
+{
+	{"R", 1},
+	{"W", 1},
+	{"reserved", 26},
+	{"version", 4}
+};
+
+int embeddedice_reg_arch_info[] =
+{
+	0x0, 0x1, 0x4, 0x5,
+	0x8, 0x9, 0xa, 0xb, 0xc, 0xd,
+	0x10, 0x11, 0x12, 0x13, 0x14, 0x15
+};
+
+char* embeddedice_reg_list[] =
+{
+	"debug_ctrl",
+	"debug_status",
+	
+	"comms_ctrl",
+	"comms_data",
+	
+	"watch 0 addr value",
+	"watch 0 addr mask",
+	"watch 0 data value",
+	"watch 0 data mask",
+	"watch 0 control value",
+	"watch 0 control mask",
+	
+	"watch 1 addr value",
+	"watch 1 addr mask",
+	"watch 1 data value",
+	"watch 1 data mask",
+	"watch 1 control value",
+	"watch 1 control mask"
+};
+
+int embeddedice_reg_arch_type = -1;
+
+int embeddedice_get_reg(reg_t *reg);
+int embeddedice_set_reg(reg_t *reg, u32 value);
+
+int embeddedice_write_reg(reg_t *reg, u32 value);
+int embeddedice_read_reg(reg_t *reg);
+
+reg_cache_t* embeddedice_build_reg_cache(target_t *target, arm_jtag_t *jtag_info, int extra_reg)
+{
+	reg_cache_t *reg_cache = malloc(sizeof(reg_cache_t));
+	reg_t *reg_list = NULL;
+	embeddedice_reg_t *arch_info = NULL;
+	int num_regs = 16 + extra_reg;
+	int i;
+	
+	/* register a register arch-type for EmbeddedICE registers only once */
+	if (embeddedice_reg_arch_type == -1)
+		embeddedice_reg_arch_type = register_reg_arch_type(embeddedice_get_reg, embeddedice_set_reg_w_exec);
+	
+	/* the actual registers are kept in two arrays */
+	reg_list = calloc(num_regs, sizeof(reg_t));
+	arch_info = calloc(num_regs, sizeof(embeddedice_reg_t));
+	
+	/* fill in values for the reg cache */
+	reg_cache->name = "EmbeddedICE registers";
+	reg_cache->next = NULL;
+	reg_cache->reg_list = reg_list;
+	reg_cache->num_regs = num_regs;
+	
+	/* set up registers */
+	for (i = 0; i < num_regs - extra_reg; i++)
+	{
+		reg_list[i].name = embeddedice_reg_list[i];
+		reg_list[i].size = 32;
+		reg_list[i].dirty = 0;
+		reg_list[i].valid = 0;
+		reg_list[i].bitfield_desc = NULL;
+		reg_list[i].num_bitfields = 0;
+		reg_list[i].value = calloc(1, 4);
+		reg_list[i].arch_info = &arch_info[i];
+		reg_list[i].arch_type = embeddedice_reg_arch_type;
+		arch_info[i].addr = embeddedice_reg_arch_info[i];
+		arch_info[i].jtag_info = jtag_info;
+	}
+	
+	/* there may be one extra reg (Abort status (ARM7 rev4) or Vector catch (ARM9)) */
+	if (extra_reg)
+	{
+		reg_list[num_regs - 1].arch_info = &arch_info[num_regs - 1];
+		arch_info[num_regs - 1].jtag_info = jtag_info;
+	}
+	
+	return reg_cache;
+}
+
+int embeddedice_get_reg(reg_t *reg)
+{
+	if (embeddedice_read_reg(reg) != ERROR_OK)
+	{
+		ERROR("BUG: error scheduling EmbeddedICE register read");
+		exit(-1);
+	}
+	
+	if (jtag_execute_queue() != ERROR_OK)
+	{
+		ERROR("register read failed");
+	}
+	
+	return ERROR_OK;
+}
+
+int embeddedice_read_reg_w_check(reg_t *reg, u8* check_value, u8* check_mask)
+{
+	embeddedice_reg_t *ice_reg = reg->arch_info;
+	u8 reg_addr = ice_reg->addr & 0x1f;
+	scan_field_t fields[3];
+	
+	DEBUG("%i", ice_reg->addr);
+
+	jtag_add_end_state(TAP_RTI);
+	arm_jtag_scann(ice_reg->jtag_info, 0x2);
+	arm_jtag_set_instr(ice_reg->jtag_info, ice_reg->jtag_info->intest_instr);
+	
+	fields[0].device = ice_reg->jtag_info->chain_pos;
+	fields[0].num_bits = 32;
+	fields[0].out_value = reg->value;
+	fields[0].out_mask = NULL;
+	fields[0].in_value = NULL;
+	fields[0].in_check_value = NULL;
+	fields[0].in_check_mask = NULL;
+	fields[0].in_handler = NULL;
+	fields[0].in_handler_priv = NULL;
+	
+	fields[1].device = ice_reg->jtag_info->chain_pos;
+	fields[1].num_bits = 5;
+	fields[1].out_value = malloc(1);
+	buf_set_u32(fields[1].out_value, 0, 5, reg_addr);
+	fields[1].out_mask = NULL;
+	fields[1].in_value = NULL;
+	fields[1].in_check_value = NULL;
+	fields[1].in_check_mask = NULL;
+	fields[1].in_handler = NULL;
+	fields[1].in_handler_priv = NULL;
+
+	fields[2].device = ice_reg->jtag_info->chain_pos;
+	fields[2].num_bits = 1;
+	fields[2].out_value = malloc(1);
+	buf_set_u32(fields[2].out_value, 0, 1, 0);
+	fields[2].out_mask = NULL;
+	fields[2].in_value = NULL;
+	fields[2].in_check_value = NULL;
+	fields[2].in_check_mask = NULL;
+	fields[2].in_handler = NULL;
+	fields[2].in_handler_priv = NULL;
+	
+	jtag_add_dr_scan(3, fields, -1);
+	
+	fields[0].in_value = reg->value;
+	fields[0].in_check_value = check_value;
+	fields[0].in_check_mask = check_mask;
+	
+	/* when reading the DCC data register, leaving the address field set to
+	 * EICE_COMMS_DATA would read the register twice
+	 * reading the control register is safe
+	 */
+	buf_set_u32(fields[1].out_value, 0, 5, embeddedice_reg_arch_info[EICE_COMMS_CTRL]);
+	
+	jtag_add_dr_scan(3, fields, -1);
+
+	free(fields[1].out_value);
+	free(fields[2].out_value);
+	
+	return ERROR_OK;
+}
+
+int embeddedice_read_reg(reg_t *reg)
+{
+	return embeddedice_read_reg_w_check(reg, NULL, NULL);	
+}
+
+int embeddedice_set_reg(reg_t *reg, u32 value)
+{
+	if (embeddedice_write_reg(reg, value) != ERROR_OK)
+	{
+		ERROR("BUG: error scheduling EmbeddedICE register write");
+		exit(-1);
+	}
+	
+	buf_set_u32(reg->value, 0, reg->size, value);
+	reg->valid = 1;
+	reg->dirty = 0;
+	
+	return ERROR_OK;
+}
+
+int embeddedice_set_reg_w_exec(reg_t *reg, u32 value)
+{
+	embeddedice_set_reg(reg, value);
+	
+	if (jtag_execute_queue() != ERROR_OK)
+	{
+		ERROR("register write failed");
+		exit(-1);
+	}
+	return ERROR_OK;
+}
+
+int embeddedice_write_reg(reg_t *reg, u32 value)
+{
+	embeddedice_reg_t *ice_reg = reg->arch_info;
+	u8 reg_addr = ice_reg->addr & 0x1f;
+	scan_field_t fields[3];
+	
+	DEBUG("%i: 0x%8.8x", ice_reg->addr, value);
+	
+	jtag_add_end_state(TAP_RTI);
+	arm_jtag_scann(ice_reg->jtag_info, 0x2);
+	arm_jtag_set_instr(ice_reg->jtag_info, ice_reg->jtag_info->intest_instr);
+	
+	fields[0].device = ice_reg->jtag_info->chain_pos;
+	fields[0].num_bits = 32;
+	fields[0].out_value = malloc(4);
+	buf_set_u32(fields[0].out_value, 0, 32, value);
+	fields[0].out_mask = NULL;
+	fields[0].in_value = NULL;
+	fields[0].in_check_value = NULL;
+	fields[0].in_check_mask = NULL;
+	fields[0].in_handler = NULL;
+	fields[0].in_handler_priv = NULL;
+	
+	fields[1].device = ice_reg->jtag_info->chain_pos;
+	fields[1].num_bits = 5;
+	fields[1].out_value = malloc(1);
+	buf_set_u32(fields[1].out_value, 0, 5, reg_addr);
+	fields[1].out_mask = NULL;
+	fields[1].in_value = NULL;
+	fields[1].in_check_value = NULL;
+	fields[1].in_check_mask = NULL;
+	fields[1].in_handler = NULL;
+	fields[1].in_handler_priv = NULL;
+
+	fields[2].device = ice_reg->jtag_info->chain_pos;
+	fields[2].num_bits = 1;
+	fields[2].out_value = malloc(1);
+	buf_set_u32(fields[2].out_value, 0, 1, 1);
+	fields[2].out_mask = NULL;
+	fields[2].in_value = NULL;
+	fields[2].in_check_value = NULL;
+	fields[2].in_check_mask = NULL;
+	fields[2].in_handler = NULL;
+	fields[2].in_handler_priv = NULL;
+	
+	jtag_add_dr_scan(3, fields, -1);
+	
+	free(fields[0].out_value);
+	free(fields[1].out_value);
+	free(fields[2].out_value);
+	
+	return ERROR_OK;
+}
+
+int embeddedice_store_reg(reg_t *reg)
+{
+	return embeddedice_write_reg(reg, buf_get_u32(reg->value, 0, reg->size));
+}
+
diff --git a/src/target/embeddedice.h b/src/target/embeddedice.h
new file mode 100644
index 00000000..0062153f
--- /dev/null
+++ b/src/target/embeddedice.h
@@ -0,0 +1,90 @@
+/***************************************************************************
+ *   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 EMBEDDED_ICE_H
+#define EMBEDDED_ICE_H
+
+#include "target.h"
+#include "register.h"
+#include "arm_jtag.h"
+
+enum
+{
+	EICE_DBG_CTRL = 0,
+	EICE_DBG_STAT = 1,
+	EICE_COMMS_CTRL = 2,
+	EICE_COMMS_DATA = 3,
+	EICE_W0_ADDR_VALUE = 4,
+	EICE_W0_ADDR_MASK = 5,
+	EICE_W0_DATA_VALUE  = 6,
+	EICE_W0_DATA_MASK = 7,
+	EICE_W0_CONTROL_VALUE = 8,
+	EICE_W0_CONTROL_MASK = 9,
+	EICE_W1_ADDR_VALUE = 10,
+	EICE_W1_ADDR_MASK = 11,
+	EICE_W1_DATA_VALUE = 12,
+	EICE_W1_DATA_MASK = 13,
+	EICE_W1_CONTROL_VALUE = 14,
+	EICE_W1_CONTROL_MASK = 15
+};
+
+enum
+{
+	EICE_DBG_CONTROL_INTDIS = 2,
+	EICE_DBG_CONTROL_DBGRQ = 1,
+	EICE_DBG_CONTROL_DBGACK = 0,
+};
+
+enum
+{
+	EICE_DBG_STATUS_ITBIT = 4,
+	EICE_DBG_STATUS_SYSCOMP = 3,
+	EICE_DBG_STATUS_IFEN = 2,
+	EICE_DBG_STATUS_DBGRQ = 1,
+	EICE_DBG_STATUS_DBGACK = 0
+};
+
+enum
+{
+	EICE_W_CTRL_ENABLE = 0x100,
+	EICE_W_CTRL_RANGE = 0x80,
+	EICE_W_CTRL_CHAIN = 0x40,
+	EICE_W_CTRL_EXTERN = 0x20,
+	EICE_W_CTRL_nTRANS = 0x10,
+	EICE_W_CTRL_nOPC = 0x8,
+	EICE_W_CTRL_MAS = 0x6,
+	EICE_W_CTRL_ITBIT = 0x2,
+	EICE_W_CTRL_nRW = 0x1
+};
+
+typedef struct embeddedice_reg_s
+{
+	int addr;
+	arm_jtag_t *jtag_info;
+} embeddedice_reg_t;
+
+extern reg_cache_t* embeddedice_build_reg_cache(target_t *target, arm_jtag_t *jtag_info, int extra_reg);
+extern int embeddedice_read_reg(reg_t *reg);
+extern int embeddedice_write_reg(reg_t *reg, u32 value);
+extern int embeddedice_read_reg_w_check(reg_t *reg, u8* check_value, u8* check_mask);
+extern int embeddedice_store_reg(reg_t *reg);
+extern int embeddedice_set_reg(reg_t *reg, u32 value);
+extern int embeddedice_set_reg_w_exec(reg_t *reg, u32 value);
+
+#endif /* EMBEDDED_ICE_H */
diff --git a/src/target/etm.c b/src/target/etm.c
new file mode 100644
index 00000000..a84e2d67
--- /dev/null
+++ b/src/target/etm.c
@@ -0,0 +1,409 @@
+/***************************************************************************

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

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

+#include "config.h"

+

+#include "etm.h"

+

+#include "armv4_5.h"

+#include "arm7_9_common.h"

+

+#include "log.h"

+#include "arm_jtag.h"

+#include "types.h"

+#include "binarybuffer.h"

+#include "target.h"

+#include "register.h"

+#include "jtag.h"

+

+#include <stdlib.h>

+

+bitfield_desc_t etm_comms_ctrl_bitfield_desc[] = 

+{

+	{"R", 1},

+	{"W", 1},

+	{"reserved", 26},

+	{"version", 4}

+};

+

+int etm_reg_arch_info[] =

+{

+	0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,

+	0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,

+	0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 

+	0x18, 0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, 0x1f, 

+	0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x27,

+	0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, 0x2f, 

+	0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37,

+	0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f,

+	0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47, 

+	0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e, 0x4f, 

+	0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, 0x57,

+	0x58, 0x59, 0x5a, 0x5b, 0x5c, 0x5d, 0x5e, 0x5f,

+	0x60, 0x61, 0x62, 0x63, 0x64, 0x65, 0x67, 

+	0x68, 0x69, 0x6a, 0x6b, 0x6c, 0x6d, 0x6e, 0x6f, 

+};

+

+int etm_reg_arch_size_info[] =

+{

+	32, 32, 17, 8, 3, 9, 32, 17,

+	26, 16, 25, 8, 17, 32, 32, 17,

+	32, 32, 32, 32, 32, 32, 32, 32, 

+	32, 32, 32, 32, 32, 32, 32, 32, 

+	7, 7, 7, 7, 7, 7, 7, 7, 

+	7, 7, 7, 7, 7, 7, 7, 7, 

+	32, 32, 32, 32, 32, 32, 32, 32, 

+	32, 32, 32, 32, 32, 32, 32, 32, 

+	32, 32, 32, 32, 32, 32, 32, 32, 

+	32, 32, 32, 32, 32, 32, 32, 32, 

+	16, 16, 16, 16, 18, 18, 18, 18,

+	17, 17, 17, 17, 16, 16, 16, 16,

+	17, 17, 17, 17, 17, 17, 2, 

+	17, 17, 17, 17, 32, 32, 32, 32 

+};

+

+char* etm_reg_list[] =

+{

+	"ETM_CTRL",

+	"ETM_CONFIG",

+	"ETM_TRIG_EVENT",

+	"ETM_MMD_CTRL",

+	"ETM_STATUS",

+	"ETM_SYS_CONFIG",

+	"ETM_TRACE_RESOURCE_CTRL",

+	"ETM_TRACE_EN_CTRL2",

+	"ETM_TRACE_EN_EVENT",

+	"ETM_TRACE_EN_CTRL1",

+	"ETM_FIFOFULL_REGION",

+	"ETM_FIFOFULL_LEVEL",

+	"ETM_VIEWDATA_EVENT",

+	"ETM_VIEWDATA_CTRL1",

+	"ETM_VIEWDATA_CTRL2",

+	"ETM_VIEWDATA_CTRL3",

+	"ETM_ADDR_COMPARATOR_VALUE1",

+	"ETM_ADDR_COMPARATOR_VALUE2",

+	"ETM_ADDR_COMPARATOR_VALUE3",

+	"ETM_ADDR_COMPARATOR_VALUE4",

+	"ETM_ADDR_COMPARATOR_VALUE5",

+	"ETM_ADDR_COMPARATOR_VALUE6",

+	"ETM_ADDR_COMPARATOR_VALUE7",

+	"ETM_ADDR_COMPARATOR_VALUE8",

+	"ETM_ADDR_COMPARATOR_VALUE9",

+	"ETM_ADDR_COMPARATOR_VALUE10",

+	"ETM_ADDR_COMPARATOR_VALUE11",

+	"ETM_ADDR_COMPARATOR_VALUE12",

+	"ETM_ADDR_COMPARATOR_VALUE13",

+	"ETM_ADDR_COMPARATOR_VALUE14",

+	"ETM_ADDR_COMPARATOR_VALUE15",

+	"ETM_ADDR_COMPARATOR_VALUE16",

+	"ETM_ADDR_ACCESS_TYPE1",

+	"ETM_ADDR_ACCESS_TYPE2",

+	"ETM_ADDR_ACCESS_TYPE3",

+	"ETM_ADDR_ACCESS_TYPE4",

+	"ETM_ADDR_ACCESS_TYPE5",

+	"ETM_ADDR_ACCESS_TYPE6",

+	"ETM_ADDR_ACCESS_TYPE7",

+	"ETM_ADDR_ACCESS_TYPE8",

+	"ETM_ADDR_ACCESS_TYPE9",

+	"ETM_ADDR_ACCESS_TYPE10",

+	"ETM_ADDR_ACCESS_TYPE11",

+	"ETM_ADDR_ACCESS_TYPE12",

+	"ETM_ADDR_ACCESS_TYPE13",

+	"ETM_ADDR_ACCESS_TYPE14",

+	"ETM_ADDR_ACCESS_TYPE15",

+	"ETM_ADDR_ACCESS_TYPE16",

+	"ETM_DATA_COMPARATOR_VALUE1",

+	"ETM_DATA_COMPARATOR_VALUE2",

+	"ETM_DATA_COMPARATOR_VALUE3",

+	"ETM_DATA_COMPARATOR_VALUE4",

+	"ETM_DATA_COMPARATOR_VALUE5",

+	"ETM_DATA_COMPARATOR_VALUE6",

+	"ETM_DATA_COMPARATOR_VALUE7",

+	"ETM_DATA_COMPARATOR_VALUE8",

+	"ETM_DATA_COMPARATOR_VALUE9",

+	"ETM_DATA_COMPARATOR_VALUE10",

+	"ETM_DATA_COMPARATOR_VALUE11",

+	"ETM_DATA_COMPARATOR_VALUE12",

+	"ETM_DATA_COMPARATOR_VALUE13",

+	"ETM_DATA_COMPARATOR_VALUE14",

+	"ETM_DATA_COMPARATOR_VALUE15",

+	"ETM_DATA_COMPARATOR_VALUE16",

+	"ETM_DATA_COMPARATOR_MASK1",

+	"ETM_DATA_COMPARATOR_MASK2",

+	"ETM_DATA_COMPARATOR_MASK3",

+	"ETM_DATA_COMPARATOR_MASK4",

+	"ETM_DATA_COMPARATOR_MASK5",

+	"ETM_DATA_COMPARATOR_MASK6",

+	"ETM_DATA_COMPARATOR_MASK7",

+	"ETM_DATA_COMPARATOR_MASK8",

+	"ETM_DATA_COMPARATOR_MASK9",

+	"ETM_DATA_COMPARATOR_MASK10",

+	"ETM_DATA_COMPARATOR_MASK11",

+	"ETM_DATA_COMPARATOR_MASK12",

+	"ETM_DATA_COMPARATOR_MASK13",

+	"ETM_DATA_COMPARATOR_MASK14",

+	"ETM_DATA_COMPARATOR_MASK15",

+	"ETM_DATA_COMPARATOR_MASK16",

+	"ETM_COUNTER_INITAL_VALUE1",

+	"ETM_COUNTER_INITAL_VALUE2",

+	"ETM_COUNTER_INITAL_VALUE3",

+	"ETM_COUNTER_INITAL_VALUE4",

+	"ETM_COUNTER_ENABLE1",

+	"ETM_COUNTER_ENABLE2",

+	"ETM_COUNTER_ENABLE3",

+	"ETM_COUNTER_ENABLE4",

+	"ETM_COUNTER_RELOAD_VALUE1",

+	"ETM_COUNTER_RELOAD_VALUE2",

+	"ETM_COUNTER_RELOAD_VALUE3",

+	"ETM_COUNTER_RELOAD_VALUE4",

+	"ETM_COUNTER_VALUE1",

+	"ETM_COUNTER_VALUE2",

+	"ETM_COUNTER_VALUE3",

+	"ETM_COUNTER_VALUE4",

+	"ETM_SEQUENCER_CTRL1",

+	"ETM_SEQUENCER_CTRL2",

+	"ETM_SEQUENCER_CTRL3",

+	"ETM_SEQUENCER_CTRL4",

+	"ETM_SEQUENCER_CTRL5",

+	"ETM_SEQUENCER_CTRL6",

+	"ETM_SEQUENCER_STATE",

+	"ETM_EXTERNAL_OUTPUT1",

+	"ETM_EXTERNAL_OUTPUT2",

+	"ETM_EXTERNAL_OUTPUT3",

+	"ETM_EXTERNAL_OUTPUT4",

+	"ETM_CONTEXTID_COMPARATOR_VALUE1",

+	"ETM_CONTEXTID_COMPARATOR_VALUE2",

+	"ETM_CONTEXTID_COMPARATOR_VALUE3",

+	"ETM_CONTEXTID_COMPARATOR_MASK"

+};  

+

+int etm_reg_arch_type = -1;

+

+int etm_get_reg(reg_t *reg);

+int etm_set_reg(reg_t *reg, u32 value);

+

+int etm_write_reg(reg_t *reg, u32 value);

+int etm_read_reg(reg_t *reg);

+

+reg_cache_t* etm_build_reg_cache(target_t *target, arm_jtag_t *jtag_info, int extra_reg)

+{

+	reg_cache_t *reg_cache = malloc(sizeof(reg_cache_t));

+	reg_t *reg_list = NULL;

+	etm_reg_t *arch_info = NULL;

+	int num_regs = sizeof(etm_reg_arch_info)/sizeof(int);

+	int i;

+	

+	/* register a register arch-type for etm registers only once */

+	if (etm_reg_arch_type == -1)

+		etm_reg_arch_type = register_reg_arch_type(etm_get_reg, etm_set_reg_w_exec);

+	

+	/* the actual registers are kept in two arrays */

+	reg_list = calloc(num_regs, sizeof(reg_t));

+	arch_info = calloc(num_regs, sizeof(etm_reg_t));

+	

+	/* fill in values for the reg cache */

+	reg_cache->name = "etm registers";

+	reg_cache->next = NULL;

+	reg_cache->reg_list = reg_list;

+	reg_cache->num_regs = num_regs;

+	

+	/* set up registers */

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

+	{

+		reg_list[i].name = etm_reg_list[i];

+		reg_list[i].size = 32;

+		reg_list[i].dirty = 0;

+		reg_list[i].valid = 0;

+		reg_list[i].bitfield_desc = NULL;

+		reg_list[i].num_bitfields = 0;

+		reg_list[i].value = calloc(1, 4);

+		reg_list[i].arch_info = &arch_info[i];

+		reg_list[i].arch_type = etm_reg_arch_type;

+		reg_list[i].size = etm_reg_arch_size_info[i];

+		arch_info[i].addr = etm_reg_arch_info[i];

+		arch_info[i].jtag_info = jtag_info;

+	}

+	return reg_cache;

+}

+

+int etm_get_reg(reg_t *reg)

+{

+	if (etm_read_reg(reg) != ERROR_OK)

+	{

+		ERROR("BUG: error scheduling etm register read");

+		exit(-1);

+	}

+	

+	if (jtag_execute_queue() != ERROR_OK)

+	{

+		ERROR("register read failed");

+	}

+	

+	return ERROR_OK;

+}

+

+int etm_read_reg_w_check(reg_t *reg, u8* check_value, u8* check_mask)

+{

+	etm_reg_t *etm_reg = reg->arch_info;

+	u8 reg_addr = etm_reg->addr & 0x7f;

+	scan_field_t fields[3];

+	

+	DEBUG("%i", etm_reg->addr);

+

+	jtag_add_end_state(TAP_RTI);

+	arm_jtag_scann(etm_reg->jtag_info, 0x6);

+	arm_jtag_set_instr(etm_reg->jtag_info, etm_reg->jtag_info->intest_instr);

+	

+	fields[0].device = etm_reg->jtag_info->chain_pos;

+	fields[0].num_bits = 32;

+	fields[0].out_value = reg->value;

+	fields[0].out_mask = NULL;

+	fields[0].in_value = NULL;

+	fields[0].in_check_value = NULL;

+	fields[0].in_check_mask = NULL;

+	fields[0].in_handler = NULL;

+	fields[0].in_handler_priv = NULL;

+	

+	fields[1].device = etm_reg->jtag_info->chain_pos;

+	fields[1].num_bits = 7;

+	fields[1].out_value = malloc(1);

+	buf_set_u32(fields[1].out_value, 0, 7, reg_addr);

+	fields[1].out_mask = NULL;

+	fields[1].in_value = NULL;

+	fields[1].in_check_value = NULL;

+	fields[1].in_check_mask = NULL;

+	fields[1].in_handler = NULL;

+	fields[1].in_handler_priv = NULL;

+

+	fields[2].device = etm_reg->jtag_info->chain_pos;

+	fields[2].num_bits = 1;

+	fields[2].out_value = malloc(1);

+	buf_set_u32(fields[2].out_value, 0, 1, 0);

+	fields[2].out_mask = NULL;

+	fields[2].in_value = NULL;

+	fields[2].in_check_value = NULL;

+	fields[2].in_check_mask = NULL;

+	fields[2].in_handler = NULL;

+	fields[2].in_handler_priv = NULL;

+	

+	jtag_add_dr_scan(3, fields, -1);

+	

+	fields[0].in_value = reg->value;

+	fields[0].in_check_value = check_value;

+	fields[0].in_check_mask = check_mask;

+		

+	jtag_add_dr_scan(3, fields, -1);

+

+	free(fields[1].out_value);

+	free(fields[2].out_value);

+	

+	return ERROR_OK;

+}

+

+int etm_read_reg(reg_t *reg)

+{

+	return etm_read_reg_w_check(reg, NULL, NULL);	

+}

+

+int etm_set_reg(reg_t *reg, u32 value)

+{

+	if (etm_write_reg(reg, value) != ERROR_OK)

+	{

+		ERROR("BUG: error scheduling etm register write");

+		exit(-1);

+	}

+	

+	buf_set_u32(reg->value, 0, reg->size, value);

+	reg->valid = 1;

+	reg->dirty = 0;

+	

+	return ERROR_OK;

+}

+

+int etm_set_reg_w_exec(reg_t *reg, u32 value)

+{

+	etm_set_reg(reg, value);

+	

+	if (jtag_execute_queue() != ERROR_OK)

+	{

+		ERROR("register write failed");

+		exit(-1);

+	}

+	return ERROR_OK;

+}

+

+int etm_write_reg(reg_t *reg, u32 value)

+{

+	etm_reg_t *etm_reg = reg->arch_info;

+	u8 reg_addr = etm_reg->addr & 0x7f;

+	scan_field_t fields[3];

+	

+	DEBUG("%i: 0x%8.8x", etm_reg->addr, value);

+	

+	jtag_add_end_state(TAP_RTI);

+	arm_jtag_scann(etm_reg->jtag_info, 0x6);

+	arm_jtag_set_instr(etm_reg->jtag_info, etm_reg->jtag_info->intest_instr);

+	

+	fields[0].device = etm_reg->jtag_info->chain_pos;

+	fields[0].num_bits = 32;

+	fields[0].out_value = malloc(4);

+	buf_set_u32(fields[0].out_value, 0, 32, value);

+	fields[0].out_mask = NULL;

+	fields[0].in_value = NULL;

+	fields[0].in_check_value = NULL;

+	fields[0].in_check_mask = NULL;

+	fields[0].in_handler = NULL;

+	fields[0].in_handler_priv = NULL;

+	

+	fields[1].device = etm_reg->jtag_info->chain_pos;

+	fields[1].num_bits = 7;

+	fields[1].out_value = malloc(1);

+	buf_set_u32(fields[1].out_value, 0, 7, reg_addr);

+	fields[1].out_mask = NULL;

+	fields[1].in_value = NULL;

+	fields[1].in_check_value = NULL;

+	fields[1].in_check_mask = NULL;

+	fields[1].in_handler = NULL;

+	fields[1].in_handler_priv = NULL;

+

+	fields[2].device = etm_reg->jtag_info->chain_pos;

+	fields[2].num_bits = 1;

+	fields[2].out_value = malloc(1);

+	buf_set_u32(fields[2].out_value, 0, 1, 1);

+	fields[2].out_mask = NULL;

+	fields[2].in_value = NULL;

+	fields[2].in_check_value = NULL;

+	fields[2].in_check_mask = NULL;

+	fields[2].in_handler = NULL;

+	fields[2].in_handler_priv = NULL;

+	

+	jtag_add_dr_scan(3, fields, -1);

+	

+	free(fields[0].out_value);

+	free(fields[1].out_value);

+	free(fields[2].out_value);

+	

+	return ERROR_OK;

+}

+

+int etm_store_reg(reg_t *reg)

+{

+	return etm_write_reg(reg, buf_get_u32(reg->value, 0, reg->size));

+}

+

diff --git a/src/target/etm.h b/src/target/etm.h
new file mode 100644
index 00000000..dbe78f35
--- /dev/null
+++ b/src/target/etm.h
@@ -0,0 +1,76 @@
+/***************************************************************************

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

+#define ETM_H

+

+#include "target.h"

+#include "register.h"

+#include "arm_jtag.h"

+

+// ETM registers (V1.2 protocol)

+enum

+{

+	ETM_CTRL = 0x00,

+	ETM_CONFIG = 0x01,

+	ETM_TRIG_EVENT = 0x02,

+	ETM_MMD_CTRL = 0x03,

+	ETM_STATUS = 0x04,

+	ETM_SYS_CONFIG = 0x05,

+	ETM_TRACE_RESOURCE_CTRL = 0x06,

+	ETM_TRACE_EN_CTRL2 = 0x07,

+	ETM_TRACE_EN_EVENT = 0x08,

+	ETM_TRACE_EN_CTRL1 = 0x09,

+	ETM_FIFOFULL_REGION = 0x0a,

+	ETM_FIFOFULL_LEVEL = 0x0b,

+	ETM_VIEWDATA_EVENT = 0x0c,

+	ETM_VIEWDATA_CTRL1 = 0x0d,

+	ETM_VIEWDATA_CTRL2 = 0x0e,

+	ETM_VIEWDATA_CTRL3 = 0x0f,

+	ETM_ADDR_COMPARATOR_VALUE = 0x10,

+	ETM_ADDR_ACCESS_TYPE = 0x20,

+	ETM_DATA_COMPARATOR_VALUE = 0x30,

+	ETM_DATA_COMPARATOR_MASK = 0x40,

+	ETM_COUNTER_INITAL_VALUE = 0x50,

+	ETM_COUNTER_ENABLE = 0x54,

+	ETM_COUNTER_RELOAD_VALUE = 0x58,

+	ETM_COUNTER_VALUE = 0x5c,

+	ETM_SEQUENCER_CTRL = 0x60,

+	ETM_SEQUENCER_STATE = 0x67,

+	ETM_EXTERNAL_OUTPUT = 0x68,

+	ETM_CONTEXTID_COMPARATOR_VALUE = 0x6c,

+	ETM_CONTEXTID_COMPARATOR_MASK = 0x6f,	

+};

+

+

+typedef struct etm_reg_s

+{

+	int addr;

+	arm_jtag_t *jtag_info;

+} etm_reg_t;

+

+extern reg_cache_t* etm_build_reg_cache(target_t *target, arm_jtag_t *jtag_info, int extra_reg);

+extern int etm_read_reg(reg_t *reg);

+extern int etm_write_reg(reg_t *reg, u32 value);

+extern int etm_read_reg_w_check(reg_t *reg, u8* check_value, u8* check_mask);

+extern int etm_store_reg(reg_t *reg);

+extern int etm_set_reg(reg_t *reg, u32 value);

+extern int etm_set_reg_w_exec(reg_t *reg, u32 value);

+

+#endif /* ETM_H */

diff --git a/src/target/register.c b/src/target/register.c
new file mode 100644
index 00000000..7b98cfcf
--- /dev/null
+++ b/src/target/register.c
@@ -0,0 +1,100 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "register.h"
+
+#include "log.h"
+#include "command.h"
+
+#include <string.h>
+#include <stdlib.h>
+
+reg_arch_type_t *reg_arch_types = NULL;
+
+reg_t* register_get_by_name(reg_cache_t *first, char *name, int search_all)
+{
+	int i;
+	reg_cache_t *cache = first;
+	
+	while (cache)
+	{
+		for (i = 0; i < cache->num_regs; i++)
+		{
+			if (strcmp(cache->reg_list[i].name, name) == 0)
+				return &(cache->reg_list[i]);
+		}
+		
+		if (search_all)
+			cache = cache->next;
+		else
+			break;
+	}
+	
+	return NULL;
+}
+
+reg_cache_t** register_get_last_cache_p(reg_cache_t **first)
+{
+	reg_cache_t **cache_p = first;
+	
+	if (*cache_p)
+		while (*cache_p)
+			cache_p = &((*cache_p)->next);
+	else
+		return first;
+	
+	return cache_p;
+}
+
+int register_reg_arch_type(int (*get)(reg_t *reg), int (*set)(reg_t *reg, u32 value))
+{
+	reg_arch_type_t** arch_type_p = &reg_arch_types;
+	int id = 0;
+	
+	if (*arch_type_p)
+	{
+		while (*arch_type_p)
+		{
+			id = (*arch_type_p)->id;
+			arch_type_p = &((*arch_type_p)->next);
+		}
+	}
+	
+	(*arch_type_p) = malloc(sizeof(reg_arch_type_t));
+	(*arch_type_p)->id = id + 1;
+	(*arch_type_p)->set = set;
+	(*arch_type_p)->get = get;
+	(*arch_type_p)->next = NULL;
+			
+	return id + 1;
+}
+
+reg_arch_type_t* register_get_arch_type(int id)
+{
+	reg_arch_type_t *arch_type = reg_arch_types;
+	
+	while (arch_type)
+	{
+		if (arch_type->id == id)
+			return arch_type;
+		arch_type = arch_type->next;
+	}
+	
+	return NULL;
+}
diff --git a/src/target/register.h b/src/target/register.h
new file mode 100644
index 00000000..8a903edd
--- /dev/null
+++ b/src/target/register.h
@@ -0,0 +1,69 @@
+/***************************************************************************
+ *   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 REGISTER_H
+#define REGISTER_H
+
+#include "types.h"
+#include "target.h"
+
+struct target_s;
+
+typedef struct bitfield_desc_s
+{
+	char *name;
+	int num_bits;
+} bitfield_desc_t;
+
+typedef struct reg_s
+{
+	char *name;
+	u8 *value;
+	int dirty;
+	int valid;
+	int size;
+	bitfield_desc_t *bitfield_desc;
+	int num_bitfields;
+	void *arch_info;
+	int arch_type;
+} reg_t;
+
+typedef struct reg_cache_s
+{
+	char *name;
+	struct reg_cache_s *next;
+	reg_t *reg_list;
+	int num_regs;
+} reg_cache_t;
+
+typedef struct reg_arch_type_s
+{
+	int id;
+	int (*get)(reg_t *reg);
+	int (*set)(reg_t *reg, u32 value);
+	struct reg_arch_type_s *next;
+} reg_arch_type_t;
+
+extern reg_t* register_get_by_name(reg_cache_t *first, char *name, int search_all);
+extern reg_cache_t** register_get_last_cache_p(reg_cache_t **first);
+extern int register_reg_arch_type(int (*get)(reg_t *reg), int (*set)(reg_t *reg, u32 value));
+extern reg_arch_type_t* register_get_arch_type(int id);
+
+#endif /* REGISTER_H */
+
diff --git a/src/target/target.c b/src/target/target.c
new file mode 100644
index 00000000..98407afb
--- /dev/null
+++ b/src/target/target.c
@@ -0,0 +1,1701 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "config.h"
+#include "target.h"
+
+#include "log.h"
+#include "configuration.h"
+#include "binarybuffer.h"
+#include "jtag.h"
+
+#include <string.h>
+#include <stdlib.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <unistd.h>
+#include <errno.h>
+
+#include <sys/time.h>
+#include <time.h>
+
+#include <time_support.h>
+
+int cli_target_callback_event_handler(struct target_s *target, enum target_event event, void *priv);
+
+int handle_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_daemon_startup_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_targets_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+int handle_target_script_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_run_and_halt_time_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_working_area_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+int handle_reg_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_poll_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_wait_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_soft_reset_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_resume_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_step_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_md_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_mw_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_load_binary_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_dump_binary_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_bp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_rbp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_wp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+int handle_rwp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+/* targets
+ */
+extern target_type_t arm7tdmi_target;
+extern target_type_t arm720t_target;
+extern target_type_t arm9tdmi_target;
+extern target_type_t arm920t_target;
+
+target_type_t *target_types[] =
+{
+	&arm7tdmi_target,
+	&arm9tdmi_target,
+	&arm920t_target,
+	&arm720t_target,
+	NULL,
+};
+
+target_t *targets = NULL;
+target_event_callback_t *target_event_callbacks = NULL;
+target_timer_callback_t *target_timer_callbacks = NULL;
+
+char *target_state_strings[] =
+{
+	"unknown",
+	"running",
+	"halted",
+	"reset",
+	"debug_running",
+};
+
+char *target_debug_reason_strings[] =
+{
+	"debug request", "breakpoint", "watchpoint",
+	"watchpoint and breakpoint", "single step",
+	"target not halted"
+};
+
+char *target_endianess_strings[] =
+{
+	"big endian",
+	"little endian",
+};
+
+enum daemon_startup_mode startup_mode = DAEMON_ATTACH;
+
+static int target_continous_poll = 1;
+
+/* returns a pointer to the n-th configured target */
+target_t* get_target_by_num(int num)
+{
+	target_t *target = targets;
+	int i = 0;
+
+	while (target)
+	{
+		if (num == i)
+			return target;
+		target = target->next;
+		i++;
+	}
+
+	return NULL;
+}
+
+int get_num_by_target(target_t *query_target)
+{
+	target_t *target = targets;
+	int i = 0;	
+	
+	while (target)
+	{
+		if (target == query_target)
+			return i;
+		target = target->next;
+		i++;
+	}
+	
+	return -1;
+}
+
+target_t* get_current_target(command_context_t *cmd_ctx)
+{
+	target_t *target = get_target_by_num(cmd_ctx->current_target);
+	
+	if (target == NULL)
+	{
+		ERROR("BUG: current_target out of bounds");
+		exit(-1);
+	}
+	
+	return target;
+}
+
+/* Process target initialization, when target entered debug out of reset
+ * the handler is unregistered at the end of this function, so it's only called once
+ */
+int target_init_handler(struct target_s *target, enum target_event event, void *priv)
+{
+	FILE *script;
+	struct command_context_s *cmd_ctx = priv;
+	
+	if ((event == TARGET_EVENT_HALTED) && (target->reset_script))
+	{
+		script = fopen(target->reset_script, "r");
+		if (!script)
+		{
+			ERROR("couldn't open script file %s", target->reset_script);
+				return ERROR_OK;
+		}
+
+		INFO("executing reset script '%s'", target->reset_script);
+		command_run_file(cmd_ctx, script, COMMAND_EXEC);
+		fclose(script);
+
+		jtag_execute_queue();
+
+		target_unregister_event_callback(target_init_handler, priv);
+	}
+	
+	return ERROR_OK;
+}
+
+int target_run_and_halt_handler(void *priv)
+{
+	target_t *target = priv;
+	
+	target->type->halt(target);
+	
+	return ERROR_OK;
+}
+
+int target_process_reset(struct command_context_s *cmd_ctx)
+{
+	int retval = ERROR_OK;
+	target_t *target;
+	 
+	target = targets;
+	while (target)
+	{
+		target->type->assert_reset(target);
+		target = target->next;
+	}
+	jtag_execute_queue();
+	
+	/* request target halt if necessary, and schedule further action */
+	target = targets;
+	while (target)
+	{
+		switch (target->reset_mode)
+		{
+			case RESET_RUN:
+				/* nothing to do if target just wants to be run */
+				break;
+			case RESET_RUN_AND_HALT:
+				/* schedule halt */
+				target_register_timer_callback(target_run_and_halt_handler, target->run_and_halt_time, 0, target);
+				break;
+			case RESET_RUN_AND_INIT:
+				/* schedule halt */
+				target_register_timer_callback(target_run_and_halt_handler, target->run_and_halt_time, 0, target);
+				target_register_event_callback(target_init_handler, cmd_ctx);
+				break;
+			case RESET_HALT:
+				target->type->halt(target);
+				break;
+			case RESET_INIT:
+				target->type->halt(target);
+				target_register_event_callback(target_init_handler, cmd_ctx);
+				break;
+			default:
+				ERROR("BUG: unknown target->reset_mode");
+		}
+		target = target->next;
+	}
+	
+	target = targets;
+	while (target)
+	{
+		target->type->deassert_reset(target);
+		target = target->next;
+	}
+	jtag_execute_queue();
+	
+	return retval;
+}	
+
+int target_init(struct command_context_s *cmd_ctx)
+{
+	target_t *target = targets;
+	
+	while (target)
+	{
+		if (target->type->init_target(cmd_ctx, target) != ERROR_OK)
+		{
+			ERROR("target '%s' init failed", target->type->name);
+			exit(-1);
+		}
+		target = target->next;
+	}
+	
+	if (targets)
+	{
+		target_register_user_commands(cmd_ctx);
+		target_register_timer_callback(handle_target, 100, 1, NULL);
+	}
+		
+	if (startup_mode == DAEMON_RESET)
+		target_process_reset(cmd_ctx);
+	
+	return ERROR_OK;
+}
+
+int target_register_event_callback(int (*callback)(struct target_s *target, enum target_event event, void *priv), void *priv)
+{
+	target_event_callback_t **callbacks_p = &target_event_callbacks;
+	
+	if (callback == NULL)
+	{
+		return ERROR_INVALID_ARGUMENTS;
+	}
+	
+	if (*callbacks_p)
+	{
+		while ((*callbacks_p)->next)
+			callbacks_p = &((*callbacks_p)->next);
+		callbacks_p = &((*callbacks_p)->next);
+	}
+	
+	(*callbacks_p) = malloc(sizeof(target_event_callback_t));
+	(*callbacks_p)->callback = callback;
+	(*callbacks_p)->priv = priv;
+	(*callbacks_p)->next = NULL;
+	
+	return ERROR_OK;
+}
+
+int target_register_timer_callback(int (*callback)(void *priv), int time_ms, int periodic, void *priv)
+{
+	target_timer_callback_t **callbacks_p = &target_timer_callbacks;
+	struct timeval now;
+	
+	if (callback == NULL)
+	{
+		return ERROR_INVALID_ARGUMENTS;
+	}
+	
+	if (*callbacks_p)
+	{
+		while ((*callbacks_p)->next)
+			callbacks_p = &((*callbacks_p)->next);
+		callbacks_p = &((*callbacks_p)->next);
+	}
+	
+	(*callbacks_p) = malloc(sizeof(target_timer_callback_t));
+	(*callbacks_p)->callback = callback;
+	(*callbacks_p)->periodic = periodic;
+	(*callbacks_p)->time_ms = time_ms;
+	
+	gettimeofday(&now, NULL);
+	(*callbacks_p)->when.tv_usec = now.tv_usec + (time_ms % 1000) * 1000;
+	time_ms -= (time_ms % 1000);
+	(*callbacks_p)->when.tv_sec = now.tv_sec + (time_ms / 1000);
+	if ((*callbacks_p)->when.tv_usec > 1000000)
+	{
+		(*callbacks_p)->when.tv_usec = (*callbacks_p)->when.tv_usec - 1000000;
+		(*callbacks_p)->when.tv_sec += 1;
+	}
+	
+	(*callbacks_p)->priv = priv;
+	(*callbacks_p)->next = NULL;
+	
+	return ERROR_OK;
+}
+
+int target_unregister_event_callback(int (*callback)(struct target_s *target, enum target_event event, void *priv), void *priv)
+{
+	target_event_callback_t **p = &target_event_callbacks;
+	target_event_callback_t *c = target_event_callbacks;
+	
+	if (callback == NULL)
+	{
+		return ERROR_INVALID_ARGUMENTS;
+	}
+		
+	while (c)
+	{
+		target_event_callback_t *next = c->next;
+		if ((c->callback == callback) && (c->priv == priv))
+		{
+			*p = next;
+			free(c);
+			return ERROR_OK;
+		}
+		else
+			p = &(c->next);
+		c = next;
+	}
+	
+	return ERROR_OK;
+}
+
+int target_unregister_timer_callback(int (*callback)(void *priv), void *priv)
+{
+	target_timer_callback_t **p = &target_timer_callbacks;
+	target_timer_callback_t *c = target_timer_callbacks;
+	
+	if (callback == NULL)
+	{
+		return ERROR_INVALID_ARGUMENTS;
+	}
+		
+	while (c)
+	{
+		target_timer_callback_t *next = c->next;
+		if ((c->callback == callback) && (c->priv == priv))
+		{
+			*p = next;
+			free(c);
+			return ERROR_OK;
+		}
+		else
+			p = &(c->next);
+		c = next;
+	}
+	
+	return ERROR_OK;
+}
+
+int target_call_event_callbacks(target_t *target, enum target_event event)
+{
+	target_event_callback_t *callback = target_event_callbacks;
+	target_event_callback_t *next_callback;
+	
+	DEBUG("target event %i", event);
+	
+	while (callback)
+	{
+		next_callback = callback->next;
+		callback->callback(target, event, callback->priv);
+		callback = next_callback;
+	}
+	
+	return ERROR_OK;
+}
+
+int target_call_timer_callbacks()
+{
+	target_timer_callback_t *callback = target_timer_callbacks;
+	target_timer_callback_t *next_callback;
+	struct timeval now;
+
+	gettimeofday(&now, NULL);
+	
+	while (callback)
+	{
+		next_callback = callback->next;
+		
+		if (((now.tv_sec >= callback->when.tv_sec) && (now.tv_usec >= callback->when.tv_usec))
+			|| (now.tv_sec > callback->when.tv_sec))
+		{
+			callback->callback(callback->priv);
+			if (callback->periodic)
+			{
+				int time_ms = callback->time_ms;
+				callback->when.tv_usec = now.tv_usec + (time_ms % 1000) * 1000;
+				time_ms -= (time_ms % 1000);
+				callback->when.tv_sec = now.tv_sec + time_ms / 1000;
+				if (callback->when.tv_usec > 1000000)
+				{
+					callback->when.tv_usec = callback->when.tv_usec - 1000000;
+					callback->when.tv_sec += 1;
+				}
+			}
+			else
+				target_unregister_timer_callback(callback->callback, callback->priv);
+		}
+			
+		callback = next_callback;
+	}
+	
+	return ERROR_OK;
+}
+
+int target_alloc_working_area(struct target_s *target, u32 size, working_area_t **area)
+{
+	working_area_t *c = target->working_areas;
+	working_area_t *new_wa = NULL;
+	
+	/* only allocate multiples of 4 byte */
+	if (size % 4)
+	{
+		ERROR("BUG: code tried to allocate unaligned number of bytes, padding");
+		size = CEIL(size, 4);
+	}
+	
+	/* see if there's already a matching working area */
+	while (c)
+	{
+		if ((c->free) && (c->size == size))
+		{
+			new_wa = c;
+			break;
+		}
+		c = c->next;
+	}
+	
+	/* if not, allocate a new one */
+	if (!new_wa)
+	{
+		working_area_t **p = &target->working_areas;
+		u32 first_free = target->working_area;
+		u32 free_size = target->working_area_size;
+		
+		DEBUG("allocating new working area");
+		
+		c = target->working_areas;
+		while (c)
+		{
+			first_free += c->size;
+			free_size -= c->size;
+			p = &c->next;
+			c = c->next;
+		}
+		
+		if (free_size < size)
+		{
+			WARNING("not enough working area available");
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+		
+		new_wa = malloc(sizeof(working_area_t));
+		new_wa->next = NULL;
+		new_wa->size = size;
+		new_wa->address = first_free;
+		
+		if (target->backup_working_area)
+		{
+			new_wa->backup = malloc(new_wa->size);
+			target->type->read_memory(target, new_wa->address, 4, new_wa->size / 4, new_wa->backup);
+		}
+		else
+		{
+			new_wa->backup = NULL;
+		}
+		
+		/* put new entry in list */
+		*p = new_wa;
+	}
+	
+	/* mark as used, and return the new (reused) area */
+	new_wa->free = 0;
+	*area = new_wa;
+	
+	/* user pointer */
+	new_wa->user = area;
+	
+	return ERROR_OK;
+}
+
+int target_free_working_area(struct target_s *target, working_area_t *area)
+{
+	if (area->free)
+		return ERROR_OK;
+	
+	if (target->backup_working_area)
+		target->type->write_memory(target, area->address, 4, area->size / 4, area->backup);
+	
+	area->free = 1;
+	
+	/* mark user pointer invalid */
+	*area->user = NULL;
+	area->user = NULL;
+	
+	return ERROR_OK;
+}
+
+int target_free_all_working_areas(struct target_s *target)
+{
+	working_area_t *c = target->working_areas;
+
+	while (c)
+	{
+		working_area_t *next = c->next;
+		target_free_working_area(target, c);
+		
+		if (c->backup)
+			free(c->backup);
+		
+		free(c);
+		
+		c = next;
+	}
+	
+	target->working_areas = NULL;
+	
+	return ERROR_OK;
+}
+
+int target_register_commands(struct command_context_s *cmd_ctx)
+{
+	register_command(cmd_ctx, NULL, "target", handle_target_command, COMMAND_CONFIG, NULL);
+	register_command(cmd_ctx, NULL, "targets", handle_targets_command, COMMAND_EXEC, NULL);
+	register_command(cmd_ctx, NULL, "daemon_startup", handle_daemon_startup_command, COMMAND_CONFIG, NULL);
+	register_command(cmd_ctx, NULL, "target_script", handle_target_script_command, COMMAND_CONFIG, NULL);
+	register_command(cmd_ctx, NULL, "run_and_halt_time", handle_run_and_halt_time_command, COMMAND_CONFIG, NULL);
+	register_command(cmd_ctx, NULL, "working_area", handle_working_area_command, COMMAND_CONFIG, NULL);
+
+	return ERROR_OK;
+}
+
+int target_write_buffer(struct target_s *target, u32 address, u32 size, u8 *buffer)
+{
+	int retval;
+	
+	DEBUG("writing buffer of %i byte at 0x%8.8x", size, address);
+	
+	/* handle writes of less than 4 byte */
+	if (size < 4)
+	{
+		if ((retval = target->type->write_memory(target, address, 1, size, buffer)) != ERROR_OK)
+			return retval;
+	}
+	
+	/* handle unaligned head bytes */
+	if (address % 4)
+	{
+		int unaligned = 4 - (address % 4);
+		
+		if ((retval = target->type->write_memory(target, address, 1, unaligned, buffer)) != ERROR_OK)
+			return retval;
+		
+		buffer += unaligned;
+		address += unaligned;
+		size -= unaligned;
+	}
+		
+	/* handle aligned words */
+	if (size >= 4)
+	{
+		int aligned = size - (size % 4);
+	
+		/* use bulk writes above a certain limit. This may have to be changed */
+		if (aligned > 128)
+		{
+			if ((retval = target->type->bulk_write_memory(target, address, aligned / 4, buffer)) != ERROR_OK)
+				return retval;
+		}
+		else
+		{
+			if ((retval = target->type->write_memory(target, address, 4, aligned / 4, buffer)) != ERROR_OK)
+				return retval;
+		}
+		
+		buffer += aligned;
+		address += aligned;
+		size -= aligned;
+	}
+	
+	/* handle tail writes of less than 4 bytes */
+	if (size > 0)
+	{
+		if ((retval = target->type->write_memory(target, address, 1, size, buffer)) != ERROR_OK)
+			return retval;
+	}
+	
+	return ERROR_OK;
+}
+
+int target_read_buffer(struct target_s *target, u32 address, u32 size, u8 *buffer)
+{
+	int retval;
+	
+	DEBUG("reading buffer of %i byte at 0x%8.8x", size, address);
+	
+	/* handle reads of less than 4 byte */
+	if (size < 4)
+	{
+		if ((retval = target->type->read_memory(target, address, 1, size, buffer)) != ERROR_OK)
+			return retval;
+	}
+	
+	/* handle unaligned head bytes */
+	if (address % 4)
+	{
+		int unaligned = 4 - (address % 4);
+		
+		if ((retval = target->type->read_memory(target, address, 1, unaligned, buffer)) != ERROR_OK)
+			return retval;
+		
+		address += unaligned;
+		size -= unaligned;
+	}
+		
+	/* handle aligned words */
+	if (size >= 4)
+	{
+		int aligned = size - (size % 4);
+	
+		if ((retval = target->type->read_memory(target, address, 4, aligned / 4, buffer)) != ERROR_OK)
+			return retval;
+		
+		address += aligned;
+		size -= aligned;
+	}
+	
+	/* handle tail writes of less than 4 bytes */
+	if (size > 0)
+	{
+		if ((retval = target->type->read_memory(target, address, 1, size, buffer)) != ERROR_OK)
+			return retval;
+	}
+	
+	return ERROR_OK;
+}
+
+int target_register_user_commands(struct command_context_s *cmd_ctx)
+{
+	register_command(cmd_ctx,  NULL, "reg", handle_reg_command, COMMAND_EXEC, NULL);
+	register_command(cmd_ctx,  NULL, "poll", handle_poll_command, COMMAND_EXEC, "poll target state");
+	register_command(cmd_ctx,  NULL, "wait_halt", handle_wait_halt_command, COMMAND_EXEC, "wait for target halt");
+	register_command(cmd_ctx,  NULL, "halt", handle_halt_command, COMMAND_EXEC, "halt target");
+	register_command(cmd_ctx,  NULL, "resume", handle_resume_command, COMMAND_EXEC, "resume target [addr]");
+	register_command(cmd_ctx,  NULL, "step", handle_step_command, COMMAND_EXEC, "step one instruction");
+	register_command(cmd_ctx,  NULL, "reset", handle_reset_command, COMMAND_EXEC, "reset target [run|halt|init|run_and_halt|run_and_init]");
+	register_command(cmd_ctx,  NULL, "soft_reset_halt", handle_soft_reset_halt_command, COMMAND_EXEC, "halt the target and do a soft reset");
+
+	register_command(cmd_ctx,  NULL, "mdw", handle_md_command, COMMAND_EXEC, "display memory words <addr> [count]");
+	register_command(cmd_ctx,  NULL, "mdh", handle_md_command, COMMAND_EXEC, "display memory half-words <addr> [count]");
+	register_command(cmd_ctx,  NULL, "mdb", handle_md_command, COMMAND_EXEC, "display memory bytes <addr> [count]");
+	
+	register_command(cmd_ctx,  NULL, "mww", handle_mw_command, COMMAND_EXEC, "write memory word <addr> <value>");
+	register_command(cmd_ctx,  NULL, "mwh", handle_mw_command, COMMAND_EXEC, "write memory half-word <addr> <value>");
+	register_command(cmd_ctx,  NULL, "mwb", handle_mw_command, COMMAND_EXEC, "write memory byte <addr> <value>");
+	
+	register_command(cmd_ctx,  NULL, "bp", handle_bp_command, COMMAND_EXEC, "set breakpoint <address> <length> [hw]");	
+	register_command(cmd_ctx,  NULL, "rbp", handle_rbp_command, COMMAND_EXEC, "remove breakpoint <adress>");
+	register_command(cmd_ctx,  NULL, "wp", handle_wp_command, COMMAND_EXEC, "set watchpoint <address> <length> <r/w/a> [value] [mask]");	
+	register_command(cmd_ctx,  NULL, "rwp", handle_rwp_command, COMMAND_EXEC, "remove watchpoint <adress>");
+	
+	register_command(cmd_ctx,  NULL, "load_binary", handle_load_binary_command, COMMAND_EXEC, "load binary <file> <address>");
+	register_command(cmd_ctx,  NULL, "dump_binary", handle_dump_binary_command, COMMAND_EXEC, "dump binary <file> <address> <size>");
+	
+	return ERROR_OK;
+}
+
+int handle_targets_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target = targets;
+	int count = 0;
+	
+	if (argc == 1)
+	{
+		int num = strtoul(args[0], NULL, 0);
+		
+		while (target)
+		{
+			count++;
+			target = target->next;
+		}
+		
+		if (num < count)
+			cmd_ctx->current_target = num;
+		else
+			command_print(cmd_ctx, "%i is out of bounds, only %i targets are configured", num, count);
+			
+		return ERROR_OK;
+	}
+		
+	while (target)
+	{
+		command_print(cmd_ctx, "%i: %s (%s), state: %s", count++, target->type->name, target_endianess_strings[target->endianness], target_state_strings[target->state]);
+		target = target->next;
+	}
+	
+	return ERROR_OK;
+}
+
+int handle_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	int i;
+	int found = 0;
+	
+	if (argc < 3)
+	{
+		ERROR("target command requires at least three arguments: <type> <endianess> <reset_mode>");
+		exit(-1);
+	}
+	
+	/* search for the specified target */
+	if (args[0] && (args[0][0] != 0))
+	{
+		for (i = 0; target_types[i]; i++)
+		{
+			if (strcmp(args[0], target_types[i]->name) == 0)
+			{
+				target_t **last_target_p = &targets;
+				
+				/* register target specific commands */
+				if (target_types[i]->register_commands(cmd_ctx) != ERROR_OK)
+				{
+					ERROR("couldn't register '%s' commands", args[0]);
+					exit(-1);
+				}
+
+				if (*last_target_p)
+				{
+					while ((*last_target_p)->next)
+						last_target_p = &((*last_target_p)->next);
+					last_target_p = &((*last_target_p)->next);
+				}
+
+				*last_target_p = malloc(sizeof(target_t));
+				
+				(*last_target_p)->type = target_types[i];
+				
+				if (strcmp(args[1], "big") == 0)
+					(*last_target_p)->endianness = TARGET_BIG_ENDIAN;
+				else if (strcmp(args[1], "little") == 0)
+					(*last_target_p)->endianness = TARGET_LITTLE_ENDIAN;
+				else
+				{
+					ERROR("endianness must be either 'little' or 'big', not '%s'", args[1]);
+					exit(-1);
+				}
+				
+				/* what to do on a target reset */
+				if (strcmp(args[2], "reset_halt") == 0)
+					(*last_target_p)->reset_mode = RESET_HALT;
+				else if (strcmp(args[2], "reset_run") == 0)
+					(*last_target_p)->reset_mode = RESET_RUN;
+				else if (strcmp(args[2], "reset_init") == 0)
+					(*last_target_p)->reset_mode = RESET_INIT;
+				else if (strcmp(args[2], "run_and_halt") == 0)
+					(*last_target_p)->reset_mode = RESET_RUN_AND_HALT;
+				else if (strcmp(args[2], "run_and_init") == 0)
+					(*last_target_p)->reset_mode = RESET_RUN_AND_INIT;
+				else
+				{
+					ERROR("unknown target startup mode %s", args[2]);
+					exit(-1);
+				}
+				(*last_target_p)->run_and_halt_time = 1000; /* default 1s */
+				
+				(*last_target_p)->reset_script = NULL;
+				(*last_target_p)->post_halt_script = NULL;
+				(*last_target_p)->pre_resume_script = NULL;
+				
+				(*last_target_p)->working_area = 0x0;
+				(*last_target_p)->working_area_size = 0x0;
+				(*last_target_p)->working_areas = NULL;
+				(*last_target_p)->backup_working_area = 0;
+				
+				(*last_target_p)->endianness = TARGET_LITTLE_ENDIAN;
+				(*last_target_p)->state = TARGET_UNKNOWN;
+				(*last_target_p)->reg_cache = NULL;
+				(*last_target_p)->breakpoints = NULL;
+				(*last_target_p)->watchpoints = NULL;
+				(*last_target_p)->next = NULL;
+				(*last_target_p)->arch_info = NULL;
+				
+				(*last_target_p)->type->target_command(cmd_ctx, cmd, args, argc, *last_target_p);
+				
+				found = 1;
+				break;
+			}
+		}
+	}
+	
+	/* no matching target found */
+	if (!found)
+	{
+		ERROR("target '%s' not found", args[0]);
+		exit(-1);
+	}
+
+	return ERROR_OK;
+}
+
+/* usage: target_script <target#> <event> <script_file> */
+int handle_target_script_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target = NULL;
+	
+	if (argc < 3)
+	{
+		ERROR("incomplete target_script command");
+		exit(-1);
+	}
+	
+	target = get_target_by_num(strtoul(args[0], NULL, 0));
+	
+	if (!target)
+	{
+		ERROR("target number '%s' not defined", args[0]);
+		exit(-1);
+	}
+	
+	if (strcmp(args[1], "reset") == 0)
+	{
+		if (target->reset_script)
+			free(target->reset_script);
+		target->reset_script = strdup(args[2]);
+	}
+	else if (strcmp(args[1], "post_halt") == 0)
+	{
+		if (target->post_halt_script)
+			free(target->post_halt_script);
+		target->post_halt_script = strdup(args[2]);
+	}
+	else if (strcmp(args[1], "pre_resume") == 0)
+	{
+		if (target->pre_resume_script)
+			free(target->pre_resume_script);
+		target->pre_resume_script = strdup(args[2]);
+	}
+	else
+	{
+		ERROR("unknown event type: '%s", args[1]);
+		exit(-1);	
+	}
+	
+	return ERROR_OK;
+}
+
+int handle_run_and_halt_time_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target = NULL;
+	
+	if (argc < 2)
+	{
+		ERROR("incomplete run_and_halt_time command");
+		exit(-1);
+	}
+	
+	target = get_target_by_num(strtoul(args[0], NULL, 0));
+	
+	if (!target)
+	{
+		ERROR("target number '%s' not defined", args[0]);
+		exit(-1);
+	}
+	
+	target->run_and_halt_time = strtoul(args[1], NULL, 0);
+	
+	return ERROR_OK;
+}
+
+int handle_working_area_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target = NULL;
+	
+	if (argc < 4)
+	{
+		ERROR("incomplete working_area command. usage: working_area <target#> <address> <size> <'backup'|'nobackup'>");
+		exit(-1);
+	}
+	
+	target = get_target_by_num(strtoul(args[0], NULL, 0));
+	
+	if (!target)
+	{
+		ERROR("target number '%s' not defined", args[0]);
+		exit(-1);
+	}
+	
+	target->working_area = strtoul(args[1], NULL, 0);
+	target->working_area_size = strtoul(args[2], NULL, 0);
+	
+	if (strcmp(args[3], "backup") == 0)
+	{
+		target->backup_working_area = 1;
+	}
+	else if (strcmp(args[3], "nobackup") == 0)
+	{
+		target->backup_working_area = 0;
+	}
+	else
+	{
+		ERROR("unrecognized <backup|nobackup> argument (%s)", args[3]);
+		exit(-1);
+	}
+	
+	return ERROR_OK;
+}
+
+
+/* process target state changes */
+int handle_target(void *priv)
+{
+	int retval;
+	target_t *target = targets;
+	
+	while (target)
+	{
+		/* only poll if target isn't already halted */
+		if (target->state != TARGET_HALTED)
+		{
+			if (target_continous_poll)
+				if ((retval = target->type->poll(target)) < 0)
+				{
+					ERROR("couldn't poll target, exiting");
+					exit(-1);
+				}
+		}
+	
+		target = target->next;
+	}
+	
+	return ERROR_OK;
+}
+
+int handle_reg_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target;
+	reg_t *reg = NULL;
+	int count = 0;
+	char *value;
+	
+	DEBUG("");
+	
+	target = get_current_target(cmd_ctx);
+	
+	/* list all available registers for the current target */
+	if (argc == 0)
+	{
+		reg_cache_t *cache = target->reg_cache;
+		
+		count = 0;
+		while(cache)
+		{
+			int i;
+			for (i = 0; i < cache->num_regs; i++)
+			{
+				value = buf_to_char(cache->reg_list[i].value, cache->reg_list[i].size);
+				command_print(cmd_ctx, "(%i) %s (/%i): 0x%s (dirty: %i, valid: %i)", count++, cache->reg_list[i].name, cache->reg_list[i].size, value, cache->reg_list[i].dirty, cache->reg_list[i].valid);
+				free(value);
+			}
+			cache = cache->next;
+		}
+		
+		return ERROR_OK;
+	}
+	
+	/* access a single register by its ordinal number */
+	if ((args[0][0] >= '0') && (args[0][0] <= '9'))
+	{
+		int num = strtoul(args[0], NULL, 0);
+		reg_cache_t *cache = target->reg_cache;
+		
+		count = 0;
+		while(cache)
+		{
+			int i;
+			for (i = 0; i < cache->num_regs; i++)
+			{
+				if (count++ == num)
+				{
+					reg = &cache->reg_list[i];
+					break;
+				}
+			}
+			if (reg)
+				break;
+			cache = cache->next;
+		}
+		
+		if (!reg)
+		{
+			command_print(cmd_ctx, "%i is out of bounds, the current target has only %i registers (0 - %i)", num, count, count - 1);
+			return ERROR_OK;
+		}
+	} else /* access a single register by its name */
+	{
+		reg = register_get_by_name(target->reg_cache, args[0], 1);
+		
+		if (!reg)
+		{
+			command_print(cmd_ctx, "register %s not found in current target", args[0]);
+			return ERROR_OK;
+		}
+	}
+
+	/* display a register */
+	if ((argc == 1) || ((argc == 2) && !((args[1][0] >= '0') && (args[1][0] <= '9'))))
+	{
+		if ((argc == 2) && (strcmp(args[1], "force") == 0))
+			reg->valid = 0;
+		
+		if (reg->valid == 0)
+		{
+			reg_arch_type_t *arch_type = register_get_arch_type(reg->arch_type);
+			if (arch_type == NULL)
+			{
+				ERROR("BUG: encountered unregistered arch type");
+				return ERROR_OK;
+			}
+			arch_type->get(reg);
+		}
+		value = buf_to_char(reg->value, reg->size);
+		command_print(cmd_ctx, "%s (/%i): 0x%s", reg->name, reg->size, value);
+		free(value);
+		return ERROR_OK;
+	}
+	
+	/* set register value */
+	if (argc == 2)
+	{
+		u32 new_value = strtoul(args[1], NULL, 0);
+		reg_arch_type_t *arch_type = register_get_arch_type(reg->arch_type);
+		if (arch_type == NULL)
+		{
+			ERROR("BUG: encountered unregistered arch type");
+			return ERROR_OK;
+		}
+		
+		arch_type->set(reg, new_value);
+		value = buf_to_char(reg->value, reg->size);
+		command_print(cmd_ctx, "%s (/%i): 0x%s", reg->name, reg->size, value);
+		free(value);
+		
+		return ERROR_OK;
+	}
+	
+	command_print(cmd_ctx, "usage: reg <#|name> [value]");
+	
+	return ERROR_OK;
+}
+
+int handle_poll_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target = get_current_target(cmd_ctx);
+	char buffer[512];
+
+	if (argc == 0)
+	{
+		command_print(cmd_ctx, "target state: %s", target_state_strings[target->type->poll(target)]);
+		if (target->state == TARGET_HALTED)
+		{
+			target->type->arch_state(target, buffer, 512);
+			buffer[511] = 0;
+			command_print(cmd_ctx, "%s", buffer);
+		}
+	}
+	else
+	{
+		if (strcmp(args[0], "on") == 0)
+		{
+			target_continous_poll = 1;
+		}
+		else if (strcmp(args[0], "off") == 0)
+		{
+			target_continous_poll = 0;
+		}
+	}
+	
+	
+	return ERROR_OK;
+}
+
+int handle_wait_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target = get_current_target(cmd_ctx);
+	struct timeval timeout, now;
+	
+	gettimeofday(&timeout, NULL);
+	timeval_add_time(&timeout, 5, 0);
+
+	command_print(cmd_ctx, "waiting for target halted...");
+
+	while(target->type->poll(target))
+	{
+		if (target->state == TARGET_HALTED)
+		{
+			command_print(cmd_ctx, "target halted");
+			break;
+		}
+		target_call_timer_callbacks();
+		
+		gettimeofday(&now, NULL);
+		if ((now.tv_sec >= timeout.tv_sec) && (now.tv_usec >= timeout.tv_usec))
+		{
+			command_print(cmd_ctx, "timed out while waiting for target halt");
+			ERROR("timed out while waiting for target halt");
+			break;
+		}
+	}
+	
+	return ERROR_OK;
+}
+
+int handle_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	int retval;
+	target_t *target = get_current_target(cmd_ctx);
+
+	DEBUG("");
+	
+	command_print(cmd_ctx, "requesting target halt...");
+
+	if ((retval = target->type->halt(target)) != ERROR_OK)
+	{	
+		switch (retval)
+		{
+			case ERROR_TARGET_ALREADY_HALTED:
+				command_print(cmd_ctx, "target already halted");
+				break;
+			case ERROR_TARGET_TIMEOUT:
+				command_print(cmd_ctx, "target timed out... shutting down");
+				exit(-1);
+			default:
+				command_print(cmd_ctx, "unknown error... shutting down");
+				exit(-1);
+		}
+	}
+	
+	return ERROR_OK;
+
+}
+
+/* what to do on daemon startup */
+int handle_daemon_startup_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	if (argc == 1)
+	{
+		if (strcmp(args[0], "attach") == 0)
+		{
+			startup_mode = DAEMON_ATTACH;
+			return ERROR_OK;
+		}
+		else if (strcmp(args[0], "reset") == 0)
+		{
+			startup_mode = DAEMON_RESET;
+			return ERROR_OK;
+		}
+	}
+	
+	WARNING("invalid daemon_startup configuration directive: %s", args[0]);
+	return ERROR_OK;
+
+}
+		
+int handle_soft_reset_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target = get_current_target(cmd_ctx);
+	int retval;
+	
+	command_print(cmd_ctx, "requesting target halt and executing a soft reset");
+	
+	if ((retval = target->type->soft_reset_halt(target)) != ERROR_OK)
+	{	
+		switch (retval)
+		{
+			case ERROR_TARGET_TIMEOUT:
+				command_print(cmd_ctx, "target timed out... shutting down");
+				exit(-1);
+			default:
+				command_print(cmd_ctx, "unknown error... shutting down");
+				exit(-1);
+		}
+	}
+	
+	return ERROR_OK;
+}
+
+int handle_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target = get_current_target(cmd_ctx);
+	enum target_reset_mode reset_mode = RESET_RUN;
+	
+	DEBUG("");
+	
+	if (argc >= 1)
+	{
+		if (strcmp("run", args[0]) == 0)
+			reset_mode = RESET_RUN;
+		else if (strcmp("halt", args[0]) == 0)
+			reset_mode = RESET_HALT;
+		else if (strcmp("init", args[0]) == 0)
+			reset_mode = RESET_INIT;
+		else if (strcmp("run_and_halt", args[0]) == 0)
+		{
+			reset_mode = RESET_RUN_AND_HALT;
+			if (argc >= 2)
+			{
+				target->run_and_halt_time = strtoul(args[1], NULL, 0);
+			}
+		}
+		else if (strcmp("run_and_init", args[0]) == 0)
+		{
+			reset_mode = RESET_RUN_AND_INIT;
+			if (argc >= 2)
+			{
+				target->run_and_halt_time = strtoul(args[1], NULL, 0);
+			}
+		}
+		else
+		{
+			command_print(cmd_ctx, "usage: reset ['run', 'halt', 'init', 'run_and_halt', 'run_and_init]");
+			return ERROR_OK;
+		}
+		target->reset_mode = reset_mode;
+	}
+	
+	target_process_reset(cmd_ctx);
+	
+	return ERROR_OK;
+}
+
+int handle_resume_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	int retval;
+	target_t *target = get_current_target(cmd_ctx);
+	
+	DEBUG("");
+	
+	if (argc == 0)
+		retval = target->type->resume(target, 1, 0, 1, 0); /* current pc, addr = 0, handle breakpoints, not debugging */
+	else if (argc == 1)
+		retval = target->type->resume(target, 0, strtoul(args[0], NULL, 0), 1, 0); /* addr = args[0], handle breakpoints, not debugging */
+	else
+	{
+		command_print(cmd_ctx, "usage: resume [address]");
+		return ERROR_OK;
+	}
+	
+	if (retval != ERROR_OK)
+	{	
+		switch (retval)
+		{
+			case ERROR_TARGET_NOT_HALTED:
+				command_print(cmd_ctx, "target not halted");
+				break;
+			default:
+				command_print(cmd_ctx, "unknown error... shutting down");
+				exit(-1);
+		}
+	}
+
+	return ERROR_OK;
+}
+
+int handle_step_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target = get_current_target(cmd_ctx);
+	
+	DEBUG("");
+	
+	if (argc == 0)
+		target->type->step(target, 1, 0, 1); /* current pc, addr = 0, handle breakpoints */
+
+	if (argc == 1)
+		target->type->step(target, 0, strtoul(args[0], NULL, 0), 1); /* addr = args[0], handle breakpoints */
+	
+	return ERROR_OK;
+}
+
+int handle_md_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	int count = 1;
+	int size = 4;
+	u32 address = 0;
+	int i;
+
+	char output[128];
+	int output_len;
+
+	int retval;
+
+	u8 *buffer;
+	target_t *target = get_current_target(cmd_ctx);
+
+	if (argc < 1)
+		return ERROR_OK;
+
+	if (argc == 2)
+		count = strtoul(args[1], NULL, 0);
+
+	address = strtoul(args[0], NULL, 0);
+	
+
+	switch (cmd[2])
+	{
+		case 'w':
+			size = 4;
+			break;
+		case 'h':
+			size = 2;
+			break;
+		case 'b':
+			size = 1;
+			break;
+		default:
+			return ERROR_OK;
+	}
+
+	buffer = calloc(count, size);
+	if ((retval  = target->type->read_memory(target, address, size, count, buffer)) != ERROR_OK)
+	{
+		switch (retval)
+		{
+			case ERROR_TARGET_UNALIGNED_ACCESS:
+				command_print(cmd_ctx, "error: address not aligned");
+				break;
+			case ERROR_TARGET_NOT_HALTED:
+				command_print(cmd_ctx, "error: target must be halted for memory accesses");
+				break;			
+			case ERROR_TARGET_DATA_ABORT:
+				command_print(cmd_ctx, "error: access caused data abort, system possibly corrupted");
+				break;
+			default:
+				command_print(cmd_ctx, "error: unknown error");
+				break;
+		}
+	}
+
+	output_len = 0;
+
+	for (i = 0; i < count; i++)
+	{
+		if (i%8 == 0)
+			output_len += snprintf(output + output_len, 128 - output_len, "0x%8.8x: ", address + (i*size));
+		
+		switch (size)
+		{
+			case 4:
+				output_len += snprintf(output + output_len, 128 - output_len, "%8.8x ", ((u32*)buffer)[i]);
+				break;
+			case 2:
+				output_len += snprintf(output + output_len, 128 - output_len, "%4.4x ", ((u16*)buffer)[i]);
+				break;
+			case 1:
+				output_len += snprintf(output + output_len, 128 - output_len, "%2.2x ", ((u8*)buffer)[i]);
+				break;
+		}
+
+		if ((i%8 == 7) || (i == count - 1))
+		{
+			command_print(cmd_ctx, output);
+			output_len = 0;
+		}
+	}
+
+	free(buffer);
+	
+	return ERROR_OK;
+}
+
+int handle_mw_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	u32 address = 0;
+	u32 value = 0;
+	int retval;
+	target_t *target = get_current_target(cmd_ctx);
+
+	if (argc < 2)
+		return ERROR_OK;
+
+	address = strtoul(args[0], NULL, 0);
+	value = strtoul(args[1], NULL, 0);
+
+	switch (cmd[2])
+	{
+		case 'w':
+			retval = target->type->write_memory(target, address, 4, 1, (u8*)&value);
+			break;
+		case 'h':
+			retval = target->type->write_memory(target, address, 2, 1, (u8*)&value);
+			break;
+		case 'b':
+			retval = target->type->write_memory(target, address, 1, 1, (u8*)&value);
+			break;
+		default:
+			return ERROR_OK;
+	}
+
+	switch (retval)
+	{
+		case ERROR_TARGET_UNALIGNED_ACCESS:
+			command_print(cmd_ctx, "error: address not aligned");
+			break;
+		case ERROR_TARGET_DATA_ABORT:
+			command_print(cmd_ctx, "error: access caused data abort, system possibly corrupted");
+			break;
+		case ERROR_TARGET_NOT_HALTED:
+			command_print(cmd_ctx, "error: target must be halted for memory accesses");
+			break;
+		case ERROR_OK:
+			break;
+		default:
+			command_print(cmd_ctx, "error: unknown error");
+			break;
+	}
+
+	return ERROR_OK;
+
+}
+
+int handle_load_binary_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	FILE *binary;
+	u32 address;
+	struct stat binary_stat;
+	u32 binary_size;
+
+	u8 *buffer;
+	u32 buf_cnt;
+	
+	struct timeval start, end;
+		
+	target_t *target = get_current_target(cmd_ctx);
+
+	if (argc != 2)
+	{
+		command_print(cmd_ctx, "usage: load_binary <filename> <address>");
+		return ERROR_OK;
+	}
+
+	address = strtoul(args[1], NULL, 0);
+
+	if (stat(args[0], &binary_stat) == -1)
+	{
+		ERROR("couldn't stat() %s: %s", args[0], strerror(errno));
+		command_print(cmd_ctx, "error accessing file %s", args[0]);
+		return ERROR_OK;
+	}
+
+	if (!(binary = fopen(args[0], "r")))
+	{
+		ERROR("couldn't open %s: %s", args[0], strerror(errno));
+		command_print(cmd_ctx, "error accessing file %s", args[0]);
+		return ERROR_OK;
+	}
+	
+	buffer = malloc(128 * 1024);
+
+	gettimeofday(&start, NULL);	
+
+	binary_size = binary_stat.st_size;
+	while (binary_size > 0)
+	{
+		buf_cnt = fread(buffer, 1, 128*1024, binary);
+		target_write_buffer(target, address, buf_cnt, buffer);
+		address += buf_cnt;
+		binary_size -= buf_cnt;
+	}
+
+	gettimeofday(&end, NULL);	
+
+	free(buffer);
+	
+	command_print(cmd_ctx, "downloaded %lli byte in %is %ius", (long long) binary_stat.st_size, end.tv_sec - start.tv_sec, end.tv_usec - start.tv_usec);
+	
+	fclose(binary);
+
+	return ERROR_OK;
+
+}
+
+int handle_dump_binary_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	FILE *binary;
+	u32 address;
+	u32 size;
+	u8 buffer[560];
+	
+	target_t *target = get_current_target(cmd_ctx);
+
+	if (argc != 3)
+	{
+		command_print(cmd_ctx, "usage: dump_binary <filename> <address> <size>");
+		return ERROR_OK;
+	}
+
+	address = strtoul(args[1], NULL, 0);
+	size = strtoul(args[2], NULL, 0);
+
+	if (!(binary = fopen(args[0], "w")))
+	{
+		ERROR("couldn't open %s for writing: %s", args[0], strerror(errno));
+		command_print(cmd_ctx, "error accessing file %s", args[0]);
+		return ERROR_OK;
+	}
+
+	if ((address & 3) || (size & 3))
+	{
+		command_print(cmd_ctx, "only 32-bit aligned address and size are supported");
+		return ERROR_OK;
+	}
+
+	while (size > 0)
+	{
+		u32 this_run_size = (size > 560) ? 560 : size;
+		target->type->read_memory(target, address, 4, this_run_size / 4, buffer);
+		fwrite(buffer, 1, this_run_size, binary);
+		size -= this_run_size;
+		address += this_run_size;
+	}
+
+	fclose(binary);
+
+	return ERROR_OK;
+
+}
+
+int handle_bp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	int retval;
+	target_t *target = get_current_target(cmd_ctx);
+
+	if (argc == 0)
+	{
+		breakpoint_t *breakpoint = target->breakpoints;
+
+		while (breakpoint)
+		{
+			if (breakpoint->type == BKPT_SOFT)
+			{
+				char* buf = buf_to_char(breakpoint->orig_instr, breakpoint->length);
+				command_print(cmd_ctx, "0x%8.8x, 0x%x, %i, 0x%s", breakpoint->address, breakpoint->length, breakpoint->set, buf);
+				free(buf);
+			}
+			else
+			{
+				command_print(cmd_ctx, "0x%8.8x, 0x%x, %i", breakpoint->address, breakpoint->length, breakpoint->set);
+			}
+			breakpoint = breakpoint->next;
+		}
+	}
+	else if (argc >= 2)
+	{
+		int hw = BKPT_SOFT;
+		u32 length = 0;
+
+		length = strtoul(args[1], NULL, 0);
+		
+		if (argc >= 3)
+			if (strcmp(args[2], "hw") == 0)
+				hw = BKPT_HARD;
+
+		if ((retval = breakpoint_add(target, strtoul(args[0], NULL, 0), length, hw)) != ERROR_OK)
+		{
+			switch (retval)
+			{
+				case ERROR_TARGET_NOT_HALTED:
+					command_print(cmd_ctx, "target must be halted to set breakpoints");
+					break;
+				case ERROR_TARGET_RESOURCE_NOT_AVAILABLE:
+					command_print(cmd_ctx, "no more breakpoints available");
+					break;
+				default:
+					command_print(cmd_ctx, "unknown error, breakpoint not set");
+					break;
+			}
+		}
+	}
+
+	return ERROR_OK;
+}
+
+int handle_rbp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target = get_current_target(cmd_ctx);
+
+	if (argc > 0)
+		breakpoint_remove(target, strtoul(args[0], NULL, 0));
+
+	return ERROR_OK;
+}
+
+int handle_wp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target = get_current_target(cmd_ctx);
+
+	if (argc == 0)
+	{
+		watchpoint_t *watchpoint = target->watchpoints;
+
+		while (watchpoint)
+		{
+			command_print(cmd_ctx, "address: 0x%8.8x, mask: 0x%8.8x, r/w/a: %i, value: 0x%8.8x, mask: 0x%8.8x", watchpoint->address, watchpoint->length, watchpoint->rw, watchpoint->value, watchpoint->mask);
+			watchpoint = watchpoint->next;
+		}
+	} 
+	else if (argc >= 2)
+	{
+		enum watchpoint_rw type = WPT_ACCESS;
+		u32 data_value = 0x0;
+		u32 data_mask = 0xffffffff;
+		
+		if (argc >= 3)
+		{
+			switch(args[2][0])
+			{
+				case 'r':
+					type = WPT_READ;
+					break;
+				case 'w':
+					type = WPT_WRITE;
+					break;
+				case 'a':
+					type = WPT_ACCESS;
+					break;
+				default:
+					command_print(cmd_ctx, "usage: wp <address> <length> [r/w/a] [value] [mask]");
+					return ERROR_OK;
+			}
+		}
+		if (argc >= 4)
+		{
+			data_value = strtoul(args[3], NULL, 0);
+		}
+		if (argc >= 5)
+		{
+			data_mask = strtoul(args[4], NULL, 0);
+		}
+		watchpoint_add(target, strtoul(args[0], NULL, 0), strtoul(args[1], NULL, 0), type, data_value, data_mask);
+	}
+	else
+	{
+		command_print(cmd_ctx, "usage: wp <address> <length> [r/w/a] [value] [mask]");
+	}
+		
+	return ERROR_OK;
+}
+
+int handle_rwp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target = get_current_target(cmd_ctx);
+
+	if (argc > 0)
+		watchpoint_remove(target, strtoul(args[0], NULL, 0));
+	
+	return ERROR_OK;
+}
+
diff --git a/src/target/target.h b/src/target/target.h
new file mode 100644
index 00000000..6d3b6d17
--- /dev/null
+++ b/src/target/target.h
@@ -0,0 +1,231 @@
+/***************************************************************************
+ *   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 TARGET_H
+#define TARGET_H
+
+#include "register.h"
+#include "breakpoints.h"
+#include "algorithm.h"
+
+#include "command.h"
+#include "types.h"
+
+#include <sys/time.h>
+#include <time.h>
+
+struct reg_s;
+struct command_context_s;
+
+enum target_state
+{
+	TARGET_UNKNOWN = 0,
+	TARGET_RUNNING = 1,
+	TARGET_HALTED = 2,
+	TARGET_RESET = 3,
+	TARGET_DEBUG_RUNNING = 4,
+};
+
+extern char *target_state_strings[];
+
+enum daemon_startup_mode
+{
+	DAEMON_ATTACH,		/* simply attach to the target */
+	DAEMON_RESET,		/* reset target (behaviour defined by reset_mode */
+};
+
+enum target_reset_mode
+{
+	RESET_RUN = 0,		/* reset and let target run */
+	RESET_HALT = 1,		/* reset and halt target out of reset */
+	RESET_INIT = 2,		/* reset and halt target out of reset, then run init script */
+	RESET_RUN_AND_HALT = 3, /* reset and let target run, halt after n milliseconds */
+	RESET_RUN_AND_INIT = 4, /* reset and let target run, halt after n milliseconds, then run init script */
+};
+
+enum target_debug_reason
+{
+	DBG_REASON_DBGRQ = 0,
+	DBG_REASON_BREAKPOINT = 1,
+	DBG_REASON_WATCHPOINT = 2,
+	DBG_REASON_WPTANDBKPT = 3,
+	DBG_REASON_SINGLESTEP = 4,
+	DBG_REASON_NOTHALTED = 5
+};
+
+extern char *target_debug_reason_strings[];
+
+enum target_endianess
+{
+	TARGET_BIG_ENDIAN = 0, TARGET_LITTLE_ENDIAN = 1
+};
+
+extern char *target_endianess_strings[];
+
+struct target_s;
+
+typedef struct working_area_s
+{
+	u32 address;
+	u32 size;
+	int free;
+	u8 *backup;
+	struct working_area_s **user;
+	struct working_area_s *next;
+} working_area_t;
+
+typedef struct target_type_s
+{
+	char *name;
+
+	/* poll current target status */
+	enum target_state (*poll)(struct target_s *target);
+	/* architecture specific status reply */
+	int (*arch_state)(struct target_s *target, char *buf, int buf_size);
+
+	/* target execution control */
+	int (*halt)(struct target_s *target);
+	int (*resume)(struct target_s *target, int current, u32 address, int handle_breakpoints, int debug_execution);
+	int (*step)(struct target_s *target, int current, u32 address, int handle_breakpoints);
+	
+	/* target reset control */
+	int (*assert_reset)(struct target_s *target);
+	int (*deassert_reset)(struct target_s *target);
+	int (*soft_reset_halt)(struct target_s *target);
+	
+	/* target register access for gdb */
+	int (*get_gdb_reg_list)(struct target_s *target, struct reg_s **reg_list[], int *reg_list_size);
+	
+	/* target memory access 
+	* size: 1 = byte (8bit), 2 = half-word (16bit), 4 = word (32bit)
+	* count: number of items of <size>
+	*/
+	int (*read_memory)(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);
+	int (*write_memory)(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);
+	
+	/* write target memory in multiples of 4 byte, optimized for writing large quantities of data */
+	int (*bulk_write_memory)(struct target_s *target, u32 address, u32 count, u8 *buffer);
+	
+	/* target break-/watchpoint control 
+	* rw: 0 = write, 1 = read, 2 = access
+	*/
+	int (*add_breakpoint)(struct target_s *target, u32 address, u32 length, enum breakpoint_type type);
+	int (*remove_breakpoint)(struct target_s *target, breakpoint_t *breakpoint);
+	int (*add_watchpoint)(struct target_s *target, u32 address, u32 length, enum watchpoint_rw rw);
+	int (*remove_watchpoint)(struct target_s *target, watchpoint_t *watchpoint);
+
+	/* target algorithm support */
+	int (*run_algorithm)(struct target_s *target, int num_mem_params, mem_param_t *mem_params, int num_reg_params, reg_param_t *reg_param, u32 entry_point, u32 exit_point, int timeout_ms, void *arch_info);
+	
+	int (*register_commands)(struct command_context_s *cmd_ctx);
+	int (*target_command)(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target);
+	int (*init_target)(struct command_context_s *cmd_ctx, struct target_s *target);
+	int (*quit)(void);
+	
+} target_type_t;
+
+typedef struct target_s
+{
+	target_type_t *type;				/* target type definition (name, access functions) */
+	enum target_reset_mode reset_mode;	/* what to do after a reset */
+	int run_and_halt_time;				/* how long the target should run after a run_and_halt reset */
+	char *reset_script;					/* script file to initialize the target after a reset */
+	char *post_halt_script;				/* script file to execute after the target halted */
+	char *pre_resume_script;			/* script file to execute before the target resumed */
+	u32 working_area;					/* working area (initialized RAM) */
+	u32 working_area_size;				/* size in bytes */
+	u32 backup_working_area;			/* whether the content of the working area has to be preserved */
+	struct working_area_s *working_areas;/* list of allocated working areas */
+	enum target_debug_reason debug_reason; /* reason why the target entered debug state */
+	enum target_endianess endianness;	/* target endianess */
+	enum target_state state;			/* the current backend-state (running, halted, ...) */
+	struct reg_cache_s *reg_cache;		/* the first register cache of the target (core regs) */
+	struct breakpoint_s *breakpoints;	/* list of breakpoints */
+	struct watchpoint_s *watchpoints;	/* list of watchpoints */
+	void *arch_info;					/* architecture specific information */
+	struct target_s *next;				/* next target in list */
+} target_t;
+
+enum target_event
+{
+	TARGET_EVENT_HALTED,		/* target entered debug state from normal execution or reset */
+	TARGET_EVENT_RESUMED,		/* target resumed to normal execution */
+	TARGET_EVENT_RESET,			/* target entered reset */
+	TARGET_EVENT_DEBUG_HALTED,	/* target entered debug state, but was executing on behalf of the debugger */
+	TARGET_EVENT_DEBUG_RESUMED, /* target resumed to execute on behalf of the debugger */
+};
+
+typedef struct target_event_callback_s
+{
+	int (*callback)(struct target_s *target, enum target_event event, void *priv);
+	void *priv;
+	struct target_event_callback_s *next;
+} target_event_callback_t;
+
+typedef struct target_timer_callback_s
+{
+	int (*callback)(void *priv);
+	int time_ms;
+	int periodic;
+	struct timeval when;
+	void *priv;
+	struct target_timer_callback_s *next;
+} target_timer_callback_t;
+
+extern int target_register_commands(struct command_context_s *cmd_ctx);
+extern int target_register_user_commands(struct command_context_s *cmd_ctx);
+extern int target_init(struct command_context_s *cmd_ctx);
+extern int handle_target(void *priv);
+
+extern int target_register_event_callback(int (*callback)(struct target_s *target, enum target_event event, void *priv), void *priv);
+extern int target_unregister_event_callback(int (*callback)(struct target_s *target, enum target_event event, void *priv), void *priv);
+extern int target_call_event_callbacks(target_t *target, enum target_event event);
+
+extern int target_register_timer_callback(int (*callback)(void *priv), int time_ms, int periodic, void *priv);
+extern int target_unregister_timer_callback(int (*callback)(void *priv), void *priv);
+extern int target_call_timer_callbacks();
+
+extern target_t* get_current_target(struct command_context_s *cmd_ctx);
+extern int get_num_by_target(target_t *query_target);
+extern target_t* get_target_by_num(int num);
+
+extern int target_write_buffer(struct target_s *target, u32 address, u32 size, u8 *buffer);
+extern int target_read_buffer(struct target_s *target, u32 address, u32 size, u8 *buffer);
+
+extern int target_alloc_working_area(struct target_s *target, u32 size, working_area_t **area);
+extern int target_free_working_area(struct target_s *target, working_area_t *area);
+extern int target_free_all_working_areas(struct target_s *target);
+
+extern target_t *targets;
+
+extern target_event_callback_t *target_event_callbacks;
+extern target_timer_callback_t *target_timer_callbacks;
+
+#define ERROR_TARGET_INVALID	(-300)
+#define ERROR_TARGET_INIT_FAILED (-301)
+#define ERROR_TARGET_TIMEOUT	(-302)
+#define ERROR_TARGET_ALREADY_HALTED (-303)
+#define ERROR_TARGET_NOT_HALTED (-304)
+#define ERROR_TARGET_FAILURE	(-305)
+#define ERROR_TARGET_UNALIGNED_ACCESS	(-306)
+#define ERROR_TARGET_DATA_ABORT	(-307)
+#define ERROR_TARGET_RESOURCE_NOT_AVAILABLE	(-308)
+#define ERROR_TARGET_TRANSLATION_FAULT	(-309)
+
+#endif /* TARGET_H */
diff --git a/src/xsvf/Makefile.am b/src/xsvf/Makefile.am
new file mode 100644
index 00000000..d9e80875
--- /dev/null
+++ b/src/xsvf/Makefile.am
@@ -0,0 +1,5 @@
+INCLUDES = -I$(top_srcdir)/src/gdb -I$(top_srcdir)/src/helper 	-I$(top_srcdir)/src/jtag $(all_includes)
+METASOURCES = AUTO
+noinst_LIBRARIES = libxsvf.a
+noinst_HEADERS = xsvf.h
+libxsvf_a_SOURCES = xsvf.c
diff --git a/src/xsvf/xsvf.c b/src/xsvf/xsvf.c
new file mode 100644
index 00000000..013803f6
--- /dev/null
+++ b/src/xsvf/xsvf.c
@@ -0,0 +1,506 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#include "xsvf.h"
+
+#include "jtag.h"
+#include "command.h"
+#include "log.h"
+
+#include <stdlib.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <netinet/in.h>
+#include <string.h>
+
+#include <sys/time.h>
+#include <time.h>
+
+#define XSTATE_MAX_PATH (12)
+
+int handle_xsvf_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+
+int xsvf_fd = 0;
+
+u8 *dr_out_buf;	/* from host to device (TDI) */
+u8 *dr_in_buf;	/* from device to host (TDO) */
+u8 *dr_in_mask;
+
+int xsdrsize = 0;
+int xruntest = 0;	/* number of TCK cycles / microseconds */
+int xrepeat = 0x20; /* number of XC9500 retries */
+
+int xendir = 0;
+int xenddr = 0;
+
+enum tap_state xsvf_to_tap[] =
+{
+	TAP_TLR, TAP_RTI,
+	TAP_SDS, TAP_CD, TAP_SD, TAP_E1D, TAP_PD, TAP_E2D, TAP_UD,
+	TAP_SIS, TAP_CI, TAP_SI, TAP_E1I, TAP_PI, TAP_E2I, TAP_UI,
+};
+
+int tap_to_xsvf[] =
+{
+	0x0, 0x2, 0x3, 0x4, 0x5, 0x6, 0x7, 0x8, 0x1, 0x9, 0xa, 0xb, 0xc, 0xe, 0xf
+};
+
+int xsvf_register_commands(struct command_context_s *cmd_ctx)
+{
+	register_command(cmd_ctx, NULL, "xsvf", handle_xsvf_command,
+		COMMAND_EXEC, "run xsvf <file>");
+
+	return ERROR_OK;
+}
+
+int xsvf_read_buffer(int num_bits, int fd, u8* buf)
+{
+	int num_bytes;
+
+	for (num_bytes = (num_bits + 7) / 8; num_bytes > 0; num_bytes--)
+	{
+		if (read(fd, buf + num_bytes - 1, 1) < 0)
+			return ERROR_XSVF_EOF;
+	}
+
+	return ERROR_OK;
+}
+
+int xsvf_read_xstates(int fd, enum tap_state *path, int max_path, int *path_len)
+{
+	char c;
+	unsigned char uc;
+	
+	while ((read(fd, &c, 1) > 0) && (c == 0x12))
+	{
+		if (*path_len > max_path)
+		{
+			WARNING("XSTATE path longer than max_path");
+			break;
+		}
+		if (read(fd, &uc, 1) < 0)
+		{
+			return ERROR_XSVF_EOF;
+		}
+		path[(*path_len)++] = xsvf_to_tap[uc];
+	}
+	
+	lseek(fd, -1, SEEK_CUR);
+	
+	return ERROR_OK;
+}
+
+int handle_xsvf_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	char c;
+	unsigned char uc, uc2;
+	unsigned int ui;
+	unsigned short us;
+
+	int do_abort = 0;
+	int unsupported = 0;
+	int tdo_mismatch = 0;
+	
+	int runtest_requires_tck = 0;
+	
+	int device = -1;	/* use -1 to indicate a "plain" xsvf file which accounts for additional devices in the scan chain, otherwise the device that should be affected */
+
+	if (argc < 2)
+	{
+		command_print(cmd_ctx, "usage: xsvf <device#|plain> <file> <variant>");
+		return ERROR_OK;
+	}
+
+	if (strcmp(args[0], "plain") != 0)
+	{
+		device = strtoul(args[0], NULL, 0);
+	}
+
+	if ((xsvf_fd = open(args[1], O_RDONLY)) < 0)
+	{
+		command_print(cmd_ctx, "file %s not found", args[0]);
+		return ERROR_OK;
+	}
+	
+	if ((argc > 2) && (strcmp(args[2], "virt2") == 0))
+	{
+		runtest_requires_tck = 1;
+	}
+
+	while (read(xsvf_fd, &c, 1) > 0)
+	{
+		switch (c)
+		{
+			case 0x00:	/* XCOMPLETE */
+				DEBUG("XCOMPLETE");
+				if (jtag_execute_queue() != ERROR_OK)
+				{
+					tdo_mismatch = 1;
+					break;	
+				}
+				break;
+			case 0x01:	/* XTDOMASK */
+				DEBUG("XTDOMASK");
+				if (dr_in_mask && (xsvf_read_buffer(xsdrsize, xsvf_fd, dr_in_mask) != ERROR_OK))
+					do_abort = 1;
+				break;
+			case 0x02:	/* XSIR */
+				DEBUG("XSIR");
+				if (read(xsvf_fd, &c, 1) < 0)
+					do_abort = 1;
+				else
+				{
+					u8 *ir_buf = malloc((c + 7) / 8);
+					if (xsvf_read_buffer(c, xsvf_fd, ir_buf) != ERROR_OK)
+						do_abort = 1;
+					else
+					{
+						scan_field_t field;
+						field.device = device;
+						field.num_bits = c;
+						field.out_value = ir_buf;
+						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;
+						if (device == -1)
+							jtag_add_plain_ir_scan(1, &field, TAP_PI);
+						else
+							jtag_add_ir_scan(1, &field, TAP_PI);
+						if (jtag_execute_queue() != ERROR_OK)
+						{
+							tdo_mismatch = 1;
+							free(ir_buf);
+							break;
+						}
+						if (xruntest)
+						{
+							if (runtest_requires_tck)
+								jtag_add_runtest(xruntest, xsvf_to_tap[xendir]);
+							else
+							{
+								jtag_add_statemove(TAP_RTI);
+								jtag_add_sleep(xruntest);
+								jtag_add_statemove(xsvf_to_tap[xendir]);
+							}
+						}
+						else if (xendir != 0xd)	/* Pause-IR */
+							jtag_add_statemove(xsvf_to_tap[xendir]);
+					}
+					free(ir_buf);
+				}
+				break;
+			case 0x03:	/* XSDR */
+				DEBUG("XSDR");
+				if (xsvf_read_buffer(xsdrsize, xsvf_fd, dr_out_buf) != ERROR_OK)
+					do_abort = 1;
+				else
+				{
+					scan_field_t field;
+					field.device = device;
+					field.num_bits = xsdrsize;
+					field.out_value = dr_out_buf;
+					field.out_mask = NULL;
+					field.in_value = NULL;
+					field.in_check_value = dr_in_buf;
+					field.in_check_mask = dr_in_mask;
+					field.in_handler = NULL;
+					field.in_handler_priv = NULL;
+					if (device == -1)
+						jtag_add_plain_dr_scan(1, &field, TAP_PD);
+					else
+						jtag_add_dr_scan(1, &field, TAP_PD);
+					if (jtag_execute_queue() != ERROR_OK)
+					{
+						tdo_mismatch = 1;
+						break;	
+					}
+					if (xruntest)
+					{
+						if (runtest_requires_tck)
+							jtag_add_runtest(xruntest, xsvf_to_tap[xenddr]);
+						else
+						{
+							jtag_add_statemove(TAP_RTI);
+							jtag_add_sleep(xruntest);
+							jtag_add_statemove(xsvf_to_tap[xenddr]);
+						}
+					}
+					else if (xendir != 0x6)	/* Pause-DR */
+						jtag_add_statemove(xsvf_to_tap[xenddr]);
+				}
+				break;
+			case 0x04:	/* XRUNTEST */
+				DEBUG("XRUNTEST");
+				if (read(xsvf_fd, &ui, 4) < 0)
+					do_abort = 1;
+				else
+				{
+					xruntest = ntohl(ui);
+				}
+				break;
+			case 0x07:	/* XREPEAT */
+				DEBUG("XREPEAT");
+				if (read(xsvf_fd, &c, 1) < 0)
+					do_abort = 1;
+				else
+				{
+					xrepeat = c;
+				}
+				break;
+			case 0x08:	/* XSDRSIZE */
+				DEBUG("XSDRSIZE");
+				if (read(xsvf_fd, &ui, 4) < 0)
+					do_abort = 1;
+				else
+				{
+					xsdrsize = ntohl(ui);
+					free(dr_out_buf);
+					free(dr_in_buf);
+					free(dr_in_mask);
+					dr_out_buf = malloc((xsdrsize + 7) / 8);
+					dr_in_buf = malloc((xsdrsize + 7) / 8);
+					dr_in_mask = malloc((xsdrsize + 7) / 8);
+				}
+				break;
+			case 0x09:	/* XSDRTDO */
+				DEBUG("XSDRTDO");
+				if (xsvf_read_buffer(xsdrsize, xsvf_fd, dr_out_buf) != ERROR_OK)
+					do_abort = 1;
+				else
+				{
+					if (xsvf_read_buffer(xsdrsize, xsvf_fd, dr_in_buf) != ERROR_OK)
+						do_abort = 1;
+					else
+					{
+						scan_field_t field;
+						field.device = device;
+						field.num_bits = xsdrsize;
+						field.out_value = dr_out_buf;
+						field.out_mask = NULL;
+						field.in_value = NULL;
+						field.in_check_value = dr_in_buf;
+						field.in_check_mask = dr_in_mask;
+						field.in_handler = NULL;
+						field.in_handler_priv = NULL;
+						if (device == -1)
+							jtag_add_plain_dr_scan(1, &field, TAP_PD);
+						else
+							jtag_add_dr_scan(1, &field, TAP_PD);
+						if (jtag_execute_queue() != ERROR_OK)
+						{
+							tdo_mismatch = 1;
+							break;	
+						}
+						if (xruntest)
+						{
+							if (runtest_requires_tck)
+								jtag_add_runtest(xruntest, xsvf_to_tap[xenddr]);
+							else
+							{
+								jtag_add_statemove(TAP_RTI);
+								jtag_add_sleep(xruntest);
+								jtag_add_statemove(xsvf_to_tap[xenddr]);
+							}
+						}
+						else if (xendir != 0x6)	/* Pause-DR */
+							jtag_add_statemove(xsvf_to_tap[xenddr]);
+					}
+				}
+				break;
+			case 0x0a:	/* XSETDRMASKS */
+				printf("unsupported XSETSDRMASKS\n");
+				unsupported = 1;
+				break;
+			case 0x0b:	/* XSDRINC */
+				printf("unsupported XSDRINC\n");
+				unsupported = 1;
+				break;
+			case 0x0c:	/* XSDRB */
+				unsupported = 1;
+				break;
+			case 0x0d:	/* XSDRC */
+				unsupported = 1;
+				break;
+			case 0x0e:	/* XSDRE */
+				unsupported = 1;
+				break;
+			case 0x0f:	/* XSDRTDOB */
+				unsupported = 1;
+				break;
+			case 0x10:	/* XSDRTDOB */
+				unsupported = 1;
+				break;
+			case 0x11:	/* XSDRTDOB */
+				unsupported = 1;
+				break;
+			case 0x12:	/* XSTATE */
+				DEBUG("XSTATE");
+				if (read(xsvf_fd, &uc, 1) < 0)
+					do_abort = 1;
+				else
+				{
+					enum tap_state *path = calloc(XSTATE_MAX_PATH, 4);
+					int path_len = 1;
+					path[0] = xsvf_to_tap[uc];
+					if (xsvf_read_xstates(xsvf_fd, path, XSTATE_MAX_PATH, &path_len) != ERROR_OK)
+						do_abort = 1;
+					else
+					{
+						jtag_add_pathmove(path_len, path);
+					}
+					free(path);
+				}
+				break;
+			case 0x13:	/* XENDIR */
+				DEBUG("XENDIR");
+				if (read(xsvf_fd, &c, 1) < 0)
+					do_abort = 1;
+				else
+				{
+					if (c == 0)
+						xendir = 1;
+					else if (c == 1)
+						xendir = 0xd;
+					else
+					{
+						ERROR("unknown XENDIR endstate");
+						unsupported = 1;
+					}
+				}
+				break;
+			case 0x14:	/* XENDDR */
+				DEBUG("XENDDR");
+				if (read(xsvf_fd, &c, 1) < 0)
+					do_abort = 1;
+				else
+				{
+					if (c == 0)
+						xenddr = 1;
+					else if (c == 1)
+						xenddr = 0x6;
+					else
+					{
+						ERROR("unknown XENDDR endstate");
+						unsupported = 1;
+					}
+				}
+				break;
+			case 0x15:	/* XSIR2 */
+				DEBUG("XSIR2");
+				if (read(xsvf_fd, &us, 2) < 0)
+					do_abort = 1;
+				else
+				{
+					u8 *ir_buf;
+					us = ntohs(us);
+					ir_buf = malloc((us + 7) / 8);
+					if (xsvf_read_buffer(us, xsvf_fd, ir_buf) != ERROR_OK)
+						do_abort = 1;
+					else
+					{
+						scan_field_t field;
+						field.device = device;
+						field.num_bits = us;
+						field.out_value = ir_buf;
+						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;
+						if (device == -1)
+							jtag_add_plain_ir_scan(1, &field, xsvf_to_tap[xendir]);
+						else
+							jtag_add_ir_scan(1, &field, xsvf_to_tap[xendir]);
+					}
+					free(ir_buf);
+				}
+				break;
+			case 0x16:	/* XCOMMENT */
+				do
+				{
+					if (read(xsvf_fd, &c, 1) < 0)
+					{
+						do_abort = 1;
+						break;
+					}
+				} while (c != 0);
+				break;
+			case 0x17:	/* XWAIT */
+				DEBUG("XWAIT");
+				if ((read(xsvf_fd, &uc, 1) < 0) || (read(xsvf_fd, &uc2, 1) < 0) || (read(xsvf_fd, &ui, 4) < 0))
+					do_abort = 1;
+				else
+				{
+					jtag_add_statemove(xsvf_to_tap[uc]);
+					ui = ntohl(ui);
+					jtag_add_sleep(ui);
+					jtag_add_statemove(xsvf_to_tap[uc2]);
+				}
+				break;
+			default:
+				printf("unknown xsvf command (0x%2.2x)\n", c);
+				unsupported = 1;
+		}
+
+		if (do_abort || unsupported || tdo_mismatch)
+			break;
+	}
+	
+	if (tdo_mismatch)
+	{
+		command_print(cmd_ctx, "TDO mismatch, aborting");
+		jtag_cancel_queue();
+		return ERROR_OK;
+	}
+
+	if (unsupported)
+	{
+		command_print(cmd_ctx, "unsupported xsvf command encountered, aborting");
+		jtag_cancel_queue();
+		return ERROR_OK;
+	}
+
+	if (do_abort)
+	{
+		command_print(cmd_ctx, "premature end detected, aborting");
+		jtag_cancel_queue();
+		return ERROR_OK;
+	}
+	
+	if (dr_out_buf)
+		free(dr_out_buf);
+	
+	if (dr_in_buf)
+		free(dr_in_buf);
+	
+	if (dr_in_mask)
+		free(dr_in_mask);
+
+	close(xsvf_fd);
+	
+	command_print(cmd_ctx, "XSVF file programmed successfully");
+
+	return ERROR_OK;
+}
diff --git a/src/xsvf/xsvf.h b/src/xsvf/xsvf.h
new file mode 100644
index 00000000..017af882
--- /dev/null
+++ b/src/xsvf/xsvf.h
@@ -0,0 +1,30 @@
+/***************************************************************************
+ *   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 XSVF_H
+#define XSVF_H
+
+#include "command.h"
+
+extern int xsvf_register_commands(struct command_context_s *cmd_ctx);
+
+#define ERROR_XSVF_EOF	(-200)
+#define ERROR_XSVF_FAILED	(-201)
+
+#endif /* XSVF_H */