- prepare OpenOCD for branching, created ./trunk/

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

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) 

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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