- Added support for native MinGW builds (thanks to Spencer Oliver and Michael Fischer) - you still need to install GiveIO (not part of OpenOCD)

- Added state-move support to ftd2xx and bitbang JTAG drivers (required for XScale, possibly useful for other targets, too)
- various fixes


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

47 files changed, 1390 insertions, 569 deletions

diff --git a/src/Makefile.am b/src/Makefile.am
index e1973827..2647fd5e 100644
--- a/src/Makefile.am
+++ b/src/Makefile.am
@@ -10,13 +10,19 @@ INCLUDES = -I$(top_srcdir)/src/helper \
 openocd_LDFLAGS = $(all_libraries) 
 SUBDIRS = helper jtag xsvf target server flash
 
+if IS_MINGW
+MINGWLDADD = -lwsock32
+else
+MINGWLDADD =
+endif
+
 if FTDI2232
 FTDI2232LIB = -lftdi
 else
 FTDI2232LIB =
 endif
 
-if IS_CYGWIN
+if IS_WIN32
 if FTD2XXDIR
 FTD2XXLDADD = @WITH_FTD2XX@/FTD2XX.lib
 else
@@ -37,4 +43,4 @@ openocd_LDADD = $(top_builddir)/src/xsvf/libxsvf.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) 
+	$(FTDI2232LIB) $(FTD2XXLIB) $(MINGWLDADD)
diff --git a/src/flash/at91sam7.c b/src/flash/at91sam7.c
index 348a8652..59ed2aa0 100644
--- a/src/flash/at91sam7.c
+++ b/src/flash/at91sam7.c
@@ -33,6 +33,11 @@ There are some things to notice
 * Lock regions (sectors) are 32 or 64 pages
 *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "replacements.h"
 
 #include "at91sam7.h"
 
@@ -59,7 +64,7 @@ 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);
+int at91sam7_handle_gpnvm_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 
 flash_driver_t at91sam7_flash =
 {
@@ -115,6 +120,15 @@ long SRAMSIZ[16] = {
    0x80000, /* 512K */
 };
 
+int at91sam7_register_commands(struct command_context_s *cmd_ctx)
+{
+	command_t *at91sam7_cmd = register_command(cmd_ctx, NULL, "at91sam7", NULL, COMMAND_ANY, NULL);
+	register_command(cmd_ctx, at91sam7_cmd, "gpnvm", at91sam7_handle_gpnvm_command, COMMAND_EXEC,
+			"at91sam7 gpnvm <num> <bit> set|clear, set or clear at91sam7 gpnvm bit");
+
+	return ERROR_OK;
+}
+
 u32 at91sam7_get_flash_status(flash_bank_t *bank)
 {
 	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;
@@ -126,6 +140,69 @@ u32 at91sam7_get_flash_status(flash_bank_t *bank)
 	return fsr;
 }
 
+/** Read clock configuration and set at91sam7_info->usec_clocks*/ 
+void at91sam7_read_clock_info(flash_bank_t *bank)
+{
+	at91sam7_flash_bank_t *at91sam7_info = bank->driver_priv;
+	target_t *target = at91sam7_info->target;
+	unsigned long mckr, mcfr, pllr, tmp, status, mainfreq;
+	unsigned int css, pres, mul, div;
+
+	/* Read main clock freqency register */
+	target->type->read_memory(target, CKGR_MCFR, 4, 1, (u8 *)&mcfr);
+	/* Read master clock register */
+	target->type->read_memory(target, PMC_MCKR, 4, 1, (u8 *)&mckr);
+	/* Read Clock Generator PLL Register  */
+	target->type->read_memory(target, CKGR_PLLR, 4, 1, (u8 *)&pllr);
+
+	pres = (mckr>>2)&0x7;
+	mul = (pllr>>16)&0x7FF;
+	div = pllr&0xFF;
+
+	at91sam7_info->mck_valid = 0;
+	switch (mckr & PMC_MCKR_CSS) {
+	case 0:			/* Slow Clock */
+		at91sam7_info->mck_valid = 1;
+		tmp = RC_FREQ;
+		break;
+	case 1:			/* Main Clock */
+		if (mcfr & CKGR_MCFR_MAINRDY) 
+		{
+			at91sam7_info->mck_valid = 1;
+			mainfreq = RC_FREQ / 16ul * (mcfr & 0xffff);
+			tmp = mainfreq;
+		}
+		break;
+		  
+	case 2:			/* Reserved */
+		break;
+	case 3:		/* PLL Clock */
+		if (mcfr & CKGR_MCFR_MAINRDY) 
+		{
+		        target->type->read_memory(target, CKGR_PLLR, 4, 1,
+						  (u8 *)&pllr);
+			if (!(pllr & CKGR_PLLR_DIV))
+			        break; /* 0 Hz */
+			at91sam7_info->mck_valid = 1;
+			mainfreq = RC_FREQ / 16ul * (mcfr & 0xffff);
+			/* Integer arithmetic should have sufficient precision
+			   as long as PLL is properly configured. */
+			tmp = mainfreq / (pllr & CKGR_PLLR_DIV) *
+			  (((pllr & CKGR_PLLR_MUL) >> 16) + 1);
+		}
+		break;
+ 	}
+	
+	/* Prescaler adjust */
+	if (((mckr & PMC_MCKR_PRES) >> 2) == 7)
+		at91sam7_info->mck_valid = 0;
+	else
+		at91sam7_info->mck_freq = tmp >> ((mckr & PMC_MCKR_PRES) >> 2);
+
+	/* Forget old flash timing */
+	at91sam7_set_flash_mode(bank,0);
+}
+
 /* Setup the timimg registers for nvbits or normal flash */
 void at91sam7_set_flash_mode(flash_bank_t *bank,int mode)
 {
@@ -133,19 +210,24 @@ void at91sam7_set_flash_mode(flash_bank_t *bank,int mode)
 	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 && (mode != at91sam7_info->flashmode)) {
+		/* Always round up (ceil) */
 		if (mode==1)
 			/* main clocks in 1uS */
-			fmcn = (at91sam7_info->mainf>>9)+1;
-		else
+			fmcn = (at91sam7_info->mck_freq/1000000ul)+1;
+		else if (mode==2)
 			/* main clocks in 1.5uS */
-			fmcn = (at91sam7_info->mainf>>9)+(at91sam7_info->mainf>>10)+1;
+			fmcn = (at91sam7_info->mck_freq/666666ul)+1;
+
+		/* Only allow fmcn=0 if clock period is > 30 us. */
+		if (at91sam7_info->mck_freq <= 33333)
+			fmcn = 0;
+
 		DEBUG("fmcn: %i", fmcn); 
 		fmr = fmcn<<16;
 		target->type->write_memory(target, MC_FSR, 4, 1, (u8 *)&fmr);
-		at91sam7_info->flashmode = mode;		
 	}
+	at91sam7_info->flashmode = mode;		
 }
 
 u8 at91sam7_wait_status_busy(flash_bank_t *bank, int timeout)
@@ -174,6 +256,7 @@ u8 at91sam7_wait_status_busy(flash_bank_t *bank, int timeout)
 	return status;
 }
 
+/* Send one command to the AT91SAM flash controller */
 int at91sam7_flash_command(struct flash_bank_s *bank,u8 cmd,u16 pagen) 
 {
 	u32 fcr;
@@ -222,23 +305,12 @@ int at91sam7_read_part_info(struct flash_bank_s *bank)
 	at91sam7_info->cidr_eproc = (cidr>>5)&0x0007;
 	at91sam7_info->cidr_version = cidr&0x001F;
 	bank->size = NVPSIZ[at91sam7_info->cidr_nvpsiz];
+	at91sam7_info->target_name = "Unknown";
 	
 	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;
-	}
+	/* Read main and master clock freqency register */
+	at91sam7_read_clock_info(bank);
 	
 	status = at91sam7_get_flash_status(bank);
 	at91sam7_info->lockbits = status>>16;
@@ -252,6 +324,7 @@ int at91sam7_read_part_info(struct flash_bank_s *bank)
 		bank->bus_width = 4;
 		if (bank->size==0x40000)  /* AT91SAM7S256 */
 		{
+			at91sam7_info->target_name = "AT91SAM7S256";
 			at91sam7_info->num_lockbits = 16;
 			at91sam7_info->pagesize = 256;
 			at91sam7_info->pages_in_lockregion = 64;
@@ -259,6 +332,7 @@ int at91sam7_read_part_info(struct flash_bank_s *bank)
 		}
 		if (bank->size==0x20000)  /* AT91SAM7S128 */
 		{
+			at91sam7_info->target_name = "AT91SAM7S128";
 			at91sam7_info->num_lockbits = 8;
 			at91sam7_info->pagesize = 256;
 			at91sam7_info->pages_in_lockregion = 64;
@@ -266,6 +340,7 @@ int at91sam7_read_part_info(struct flash_bank_s *bank)
 		}
 		if (bank->size==0x10000)  /* AT91SAM7S64 */
 		{
+			at91sam7_info->target_name = "AT91SAM7S64";
 			at91sam7_info->num_lockbits = 16;
 			at91sam7_info->pagesize = 128;
 			at91sam7_info->pages_in_lockregion = 32;
@@ -273,6 +348,7 @@ int at91sam7_read_part_info(struct flash_bank_s *bank)
 		}
 		if (bank->size==0x08000)  /* AT91SAM7S321/32 */
 		{
+			at91sam7_info->target_name = "AT91SAM7S321/32";
 			at91sam7_info->num_lockbits = 8;
 			at91sam7_info->pagesize = 128;
 			at91sam7_info->pages_in_lockregion = 32;
@@ -290,6 +366,7 @@ int at91sam7_read_part_info(struct flash_bank_s *bank)
 		bank->bus_width = 4;
 		if (bank->size==0x40000)  /* AT91SAM7XC256 */
 		{
+			at91sam7_info->target_name = "AT91SAM7XC256";
 			at91sam7_info->num_lockbits = 16;
 			at91sam7_info->pagesize = 256;
 			at91sam7_info->pages_in_lockregion = 64;
@@ -297,6 +374,7 @@ int at91sam7_read_part_info(struct flash_bank_s *bank)
 		}
 		if (bank->size==0x20000)  /* AT91SAM7XC128 */
 		{
+			at91sam7_info->target_name = "AT91SAM7XC128";
 			at91sam7_info->num_lockbits = 8;
 			at91sam7_info->pagesize = 256;
 			at91sam7_info->pages_in_lockregion = 64;
@@ -314,6 +392,7 @@ int at91sam7_read_part_info(struct flash_bank_s *bank)
 		bank->bus_width = 4;
 		if (bank->size==0x40000)  /* AT91SAM7X256 */
 		{
+			at91sam7_info->target_name = "AT91SAM7X256";
 			at91sam7_info->num_lockbits = 16;
 			at91sam7_info->pagesize = 256;
 			at91sam7_info->pages_in_lockregion = 64;
@@ -321,6 +400,7 @@ int at91sam7_read_part_info(struct flash_bank_s *bank)
 		}
 		if (bank->size==0x20000)  /* AT91SAM7X128 */
 		{
+			at91sam7_info->target_name = "AT91SAM7X128";
 			at91sam7_info->num_lockbits = 8;
 			at91sam7_info->pagesize = 256;
 			at91sam7_info->pages_in_lockregion = 64;
@@ -339,6 +419,7 @@ int at91sam7_read_part_info(struct flash_bank_s *bank)
 		
 		if (bank->size == 0x40000)  /* AT91SAM7A3 */
 		{
+			at91sam7_info->target_name = "AT91SAM7A3";
 			at91sam7_info->num_lockbits = 16;
 			at91sam7_info->pagesize = 256;
 			at91sam7_info->pages_in_lockregion = 64;
@@ -392,14 +473,6 @@ int at91sam7_protect_check(struct flash_bank_s *bank)
 	return ERROR_OK;
 }
 
-
-int at91sam7_register_commands(struct command_context_s *cmd_ctx)
-{
-	command_t *at91sam7_cmd = register_command(cmd_ctx, NULL, "at91sam7", NULL, COMMAND_ANY, "at91sam7 specific commands");
-
-	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;
@@ -452,11 +525,15 @@ int at91sam7_erase(struct flash_bank_s *bank, int first, int last)
 		return ERROR_FLASH_SECTOR_INVALID;
 	}
 
+	/* Configure the flash controller timing */
+	at91sam7_read_clock_info(bank);	
+	at91sam7_set_flash_mode(bank,2);
+
         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;
 }
@@ -490,7 +567,8 @@ int at91sam7_protect(struct flash_bank_s *bank, int set, int first, int last)
 		return ERROR_FLASH_OPERATION_FAILED;
 	}
 	
-	/* Configure the flash controller timing */	
+	/* Configure the flash controller timing */
+	at91sam7_read_clock_info(bank);	
 	at91sam7_set_flash_mode(bank,1);
 	
 	for (lockregion=first;lockregion<=last;lockregion++) 
@@ -560,6 +638,7 @@ int at91sam7_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
 	DEBUG("first_page: %i, last_page: %i, count %i", first_page, last_page, count);
 	
 	/* Configure the flash controller timing */	
+	at91sam7_read_clock_info(bank);	
 	at91sam7_set_flash_mode(bank,2);
 
 	for (pagen=first_page; pagen<last_page; pagen++) {
@@ -611,10 +690,7 @@ 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);
-	}
+	at91sam7_read_part_info(bank);
 
 	if (at91sam7_info->cidr == 0)
 	{
@@ -632,7 +708,7 @@ int at91sam7_info(struct flash_bank_s *bank, char *buf, int buf_size)
 	buf += printed;
 	buf_size -= printed;
 			
-	printed = snprintf(buf, buf_size, "main clock(estimated): %ikHz \n", at91sam7_info->mainf*2);
+	printed = snprintf(buf, buf_size, "master clock(estimated): %ikHz \n", at91sam7_info->mck_freq / 1000);
 	buf += printed;
 	buf_size -= printed;
 	
@@ -648,3 +724,92 @@ int at91sam7_info(struct flash_bank_s *bank, char *buf, int buf_size)
 
 	return ERROR_OK;
 }
+
+/* 
+* On AT91SAM7S: When the gpnmv bits are set with 
+* > at91sam7 gpnvm 0 bitnr set
+* the changes are not visible in the flash controller status register MC_FSR 
+* until the processor has been reset.
+* On the Olimex board this requires a power cycle.
+* Note that the AT91SAM7S has the following errata (doc6175.pdf sec 14.1.3):
+* 	The maximum number of write/erase cycles for Non Volatile Memory bits is 100. This includes
+*	Lock Bits (LOCKx), General Purpose NVM bits (GPNVMx) and the Security Bit.
+*/
+int at91sam7_handle_gpnvm_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	flash_bank_t *bank;
+	int bit;
+	u8  flashcmd;
+	u32 status;
+	char *value;
+	at91sam7_flash_bank_t *at91sam7_info;
+
+	if (argc < 3)
+	{
+		command_print(cmd_ctx, "at91sam7 gpnvm <num> <bit> <set|clear>");
+		return ERROR_OK;
+	}
+	
+	bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
+	bit = atoi(args[1]);
+	value = args[2];
+
+	if (!bank)
+	{
+		command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
+		return ERROR_OK;
+	}
+
+	at91sam7_info = bank->driver_priv;
+
+	if (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 ((bit<0) || (at91sam7_info->num_nvmbits <= bit))
+	{ 
+		command_print(cmd_ctx, "gpnvm bit '#%s' is out of bounds for target %s", args[1],at91sam7_info->target_name);
+		return ERROR_OK;
+	}
+
+	if (strcmp(value, "set") == 0)
+	{
+		flashcmd = SGPB;
+	}
+	else if (strcmp(value, "clear") == 0)
+	{
+		flashcmd = CGPB;
+	}
+	else
+	{
+		command_print(cmd_ctx, "usage: at91sam7 gpnvm <num> <bit> <set|clear>");
+		return ERROR_OK;
+	}
+
+	/* Configure the flash controller timing */
+	at91sam7_read_clock_info(bank);	
+	at91sam7_set_flash_mode(bank,1);
+	
+	if (at91sam7_flash_command(bank, flashcmd, (u16)(bit)) != ERROR_OK) 
+	{
+		return ERROR_FLASH_OPERATION_FAILED;
+	}	
+
+	status = at91sam7_get_flash_status(bank);
+	DEBUG("at91sam7_handle_gpnvm_command: cmd 0x%x, value 0x%x, status 0x%x \n",flashcmd,bit,status);
+	at91sam7_info->nvmbits = (status>>8)&((1<<at91sam7_info->num_nvmbits)-1);
+
+	return ERROR_OK;
+}
diff --git a/src/flash/at91sam7.h b/src/flash/at91sam7.h
index 8f9e3db7..c65600eb 100644
--- a/src/flash/at91sam7.h
+++ b/src/flash/at91sam7.h
@@ -28,7 +28,7 @@ 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;
@@ -39,7 +39,8 @@ typedef struct at91sam7_flash_bank_s
 	u16 cidr_nvpsiz2;
 	u16 cidr_eproc;
 	u16 cidr_version;
-		
+	char * target_name;
+
 	/* flash geometry */
 	u16 num_pages;
 	u16 pagesize;
@@ -54,17 +55,23 @@ typedef struct at91sam7_flash_bank_s
 	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;
+	u8  mck_valid;
+	u32 mck_freq;
 	
 } at91sam7_flash_bank_t;
 
 /* AT91SAM7 control registers */
 #define DBGU_CIDR 0xFFFFF240
 #define CKGR_MCFR 0xFFFFFC24
+#define CKGR_MCFR_MAINRDY  0x10000
+#define CKGR_PLLR 0xFFFFFC2c
+#define CKGR_PLLR_DIV 0xff
+#define CKGR_PLLR_MUL 0x07ff0000
+#define PMC_MCKR  0xFFFFFC30
+#define PMC_MCKR_CSS  0x03
+#define PMC_MCKR_PRES 0x1c
 #define MC_FMR	0xFFFFFF60
 #define MC_FCR	0xFFFFFF64
 #define MC_FSR	0xFFFFFF68
@@ -79,5 +86,7 @@ typedef struct at91sam7_flash_bank_s
 #define  CGPB 0x0D
 #define  SSB  0x0F
 
+/* AT91SAM7 constants */
+#define RC_FREQ  32000
 
 #endif /* AT91SAM7_H */
diff --git a/src/flash/cfi.c b/src/flash/cfi.c
index 5e943676..3dc7885c 100644
--- a/src/flash/cfi.c
+++ b/src/flash/cfi.c
@@ -17,6 +17,12 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "replacements.h"
+
 #include "cfi.h"
 
 #include "flash.h"
diff --git a/src/flash/flash.c b/src/flash/flash.c
index a5067cd2..0a713093 100644
--- a/src/flash/flash.c
+++ b/src/flash/flash.c
@@ -17,6 +17,10 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
 #include "flash.h"
 #include "command.h"
 #include "log.h"
@@ -506,7 +510,7 @@ int handle_flash_write_command(struct command_context_s *cmd_ctx, char *cmd, cha
 		return ERROR_OK;
 	}
 		
-	if (!(binary = fopen(args[1], "r")))
+	if (!(binary = fopen(args[1], "rb")))
 	{
 		ERROR("couldn't open %s: %s", args[1], strerror(errno));
 		command_print(cmd_ctx, "couldn't open %s", args[1]);
diff --git a/src/flash/lpc2000.c b/src/flash/lpc2000.c
index b6fcb30b..68181c6f 100644
--- a/src/flash/lpc2000.c
+++ b/src/flash/lpc2000.c
@@ -17,6 +17,10 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
 #include "lpc2000.h"
 
 #include "flash.h"
diff --git a/src/flash/str7x.c b/src/flash/str7x.c
index 2e3a6c8c..4d8d02e4 100644
--- a/src/flash/str7x.c
+++ b/src/flash/str7x.c
@@ -17,6 +17,11 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "replacements.h"
 
 #include "str7x.h"
 #include "flash.h"
diff --git a/src/helper/Makefile.am b/src/helper/Makefile.am
index 5fb2241d..5720b9d7 100644
--- a/src/helper/Makefile.am
+++ b/src/helper/Makefile.am
@@ -1,6 +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
+libhelper_a_SOURCES = binarybuffer.c configuration.c log.c interpreter.c command.c time_support.c replacements.c
 noinst_HEADERS = binarybuffer.h configuration.h types.h log.h command.h \
-	interpreter.h time_support.h
+	interpreter.h time_support.h replacements.h
diff --git a/src/helper/binarybuffer.c b/src/helper/binarybuffer.c
index 357d05c3..ce33f138 100644
--- a/src/helper/binarybuffer.c
+++ b/src/helper/binarybuffer.c
@@ -17,6 +17,9 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
 
 #include <stdlib.h>
 #include <string.h>
@@ -112,8 +115,18 @@ int buf_cmp(u8 *buf1, u8 *buf2, int size)
 
 	for (i = 0; i < num_bytes; i++)
 	{
-		if (buf1[i] != buf2[i])
-			return 1;
+		/* last byte */
+		/* mask out bits that don't really belong to the buffer if size isn't a multiple of 8 bits */
+		if ((size % 8) && (i == num_bytes -1 ))
+		{
+			if ((buf1[i] & ((1 << (size % 8)) - 1)) != (buf2[i] & ((1 << (size % 8)) - 1)))
+				return 1;
+		}
+		else
+		{
+			if (buf1[i] != buf2[i])
+				return 1;
+		}
 	}
 
 	return 0;
@@ -126,8 +139,19 @@ int buf_cmp_mask(u8 *buf1, u8 *buf2, u8 *mask, int size)
 
 	for (i = 0; i < num_bytes; i++)
 	{
-		if ((buf1[i] & mask[i]) != (buf2[i] & mask[i]))
-			return 1;
+		/* last byte */
+		/* mask out bits that don't really belong to the buffer if size isn't a multiple of 8 bits */
+		if ((size % 8) && (i == num_bytes -1 ))
+		{
+			if (((buf1[i] & ((1 << (size % 8)) - 1)) & ((1 << (size % 8)) - 1)) != 
+				((buf2[i] & ((1 << (size % 8)) - 1)) & ((1 << (size % 8)) - 1)))
+				return 1;
+		}
+		else
+		{
+			if ((buf1[i] & mask[i]) != (buf2[i] & mask[i]))
+				return 1;
+		}
 	}
 
 	return 0;
diff --git a/src/helper/command.c b/src/helper/command.c
index 9fc78422..9a38723c 100644
--- a/src/helper/command.c
+++ b/src/helper/command.c
@@ -20,6 +20,12 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "replacements.h"
+
 #include "command.h"
 
 #include "log.h"
diff --git a/src/helper/configuration.c b/src/helper/configuration.c
index e59ca6d1..d48977c5 100644
--- a/src/helper/configuration.c
+++ b/src/helper/configuration.c
@@ -18,7 +18,7 @@
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
 #ifdef HAVE_CONFIG_H
-#include <config.h>
+#include "config.h"
 #endif
 
 #include "types.h"
diff --git a/src/helper/interpreter.c b/src/helper/interpreter.c
index 7e88263b..17d24b1f 100644
--- a/src/helper/interpreter.c
+++ b/src/helper/interpreter.c
@@ -17,6 +17,10 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
 #include "interpreter.h"
 
 #include "binarybuffer.h"
diff --git a/src/helper/log.c b/src/helper/log.c
index 60ba80bd..74407078 100644
--- a/src/helper/log.c
+++ b/src/helper/log.c
@@ -17,6 +17,10 @@
  *   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 "configuration.h"
 
diff --git a/src/helper/replacements.c b/src/helper/replacements.c
new file mode 100644
index 00000000..769296a0
--- /dev/null
+++ b/src/helper/replacements.c
@@ -0,0 +1,96 @@
+/***************************************************************************
+ *   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.             *
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "replacements.h"
+
+#include <stdio.h>
+
+/* replacements for gettimeofday */
+#ifndef HAVE_GETTIMEOFDAY
+
+/* Windows */
+#ifdef _WIN32
+
+#ifndef __GNUC__
+#define EPOCHFILETIME (116444736000000000i64)
+#else
+#define EPOCHFILETIME (116444736000000000LL)
+#endif
+
+int gettimeofday(struct timeval *tv, struct timezone *tz)
+{
+	FILETIME        ft;
+	LARGE_INTEGER   li;
+	__int64         t;
+	static int      tzflag;
+
+	if (tv)
+	{
+		GetSystemTimeAsFileTime(&ft);
+		li.LowPart  = ft.dwLowDateTime;
+		li.HighPart = ft.dwHighDateTime;
+		t  = li.QuadPart;                   /* In 100-nanosecond intervals */
+		t -= EPOCHFILETIME;                 /* Offset to the Epoch time */
+		t /= 10;                            /* In microseconds */
+		tv->tv_sec  = (long)(t / 1000000);
+		tv->tv_usec = (long)(t % 1000000);
+	}
+
+	if (tz)
+	{
+		if (!tzflag)
+		{
+			_tzset();
+			tzflag++;
+		}
+		tz->tz_minuteswest = _timezone / 60;
+		tz->tz_dsttime = _daylight;
+	}
+
+	return 0;
+}
+#endif /* _WIN32 */
+
+#endif /* HAVE_GETTIMEOFDAY */
+
+#ifndef HAVE_STRNLEN
+size_t strnlen(const char *s, size_t maxlen)
+{
+	const char *end= (const char *)memchr(s, '\0', maxlen);
+	return end ? (size_t) (end - s) : maxlen;
+}
+#endif
+
+#ifndef HAVE_STRNDUP
+char* strndup(const char *s, size_t n)
+{
+	size_t len = strnlen (s, n);
+	char *new = (char *) malloc (len + 1);
+
+	if (new == NULL)
+		return NULL;
+
+	new[len] = '\0';
+	return (char *) memcpy (new, s, len);
+}
+#endif
diff --git a/src/helper/replacements.h b/src/helper/replacements.h
new file mode 100644
index 00000000..70697b29
--- /dev/null
+++ b/src/helper/replacements.h
@@ -0,0 +1,143 @@
+/***************************************************************************
+ *   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 REPLACEMENTS_H
+#define REPLACEMENTS_H
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+/* include necessary headers for socket functionality */
+#ifdef _WIN32
+#include <winsock2.h>
+#else
+#include <sys/socket.h>
+#include <sys/poll.h>
+#include <netinet/in.h>
+#include <unistd.h>
+#include <fcntl.h>
+#endif
+
+/* gettimeofday() */
+#ifndef HAVE_GETTIMEOFDAY
+
+#ifndef _TIMEVAL_DEFINED
+#define _TIMEVAL_DEFINED
+
+struct timeval {
+	long tv_sec;
+	long tv_usec;
+};
+#endif /* _TIMEVAL_DEFINED */
+
+struct timezone {
+    int tz_minuteswest;
+	int tz_dsttime;
+};
+
+extern int gettimeofday(struct timeval *tv, struct timezone *tz);
+#endif
+
+/* GNU extensions to the C library that may be missing on some systems */
+#ifndef HAVE_STRNDUP
+extern char* strndup(const char *s, size_t n);
+#endif /* HAVE_STRNDUP */
+
+#ifndef HAVE_STRNLEN
+extern size_t strnlen(const char *s, size_t maxlen);
+#endif /* HAVE_STRNLEN */
+
+#ifndef HAVE_USLEEP
+static __inline unsigned usleep(unsigned int usecs)
+{
+#ifdef _WIN32
+	Sleep((usecs/1000));
+	return 0;
+#else
+#error no usleep defined for your platform
+#endif
+}
+#endif /* HAVE_USLEEP */
+
+/* Windows specific */
+#ifdef _WIN32
+
+#define WIN32_LEAN_AND_MEAN
+#include <windows.h>
+#include <time.h>
+
+#undef ERROR
+
+#if IS_MINGW == 1
+static __inline unsigned char inb(unsigned short int port)
+{
+	unsigned char _v;
+	__asm__ __volatile__ ("inb %w1,%0":"=a" (_v):"Nd" (port));
+	return _v;
+}
+
+static __inline void outb(unsigned char value, unsigned short int port)
+{
+	__asm__ __volatile__ ("outb %b0,%w1": :"a" (value), "Nd" (port));
+}
+
+#endif /* IS_MINGW */
+#endif  /* _WIN32 */
+
+/* generic socket functions for Windows and Posix */
+static __inline int write_socket( int handle, const void *buffer, unsigned int count )
+{
+#ifdef _WIN32
+    return send(handle, buffer, count, 0);
+#else
+    return write(handle, buffer, count);
+#endif
+}
+
+static __inline int read_socket( int handle, void *buffer, unsigned int count )
+{
+#ifdef _WIN32
+    return recv(handle, buffer, count, 0);
+#else
+    return read(handle, buffer, count);
+#endif
+}
+
+static __inline int close_socket(int sock)
+{
+#ifdef _WIN32
+    return closesocket(sock);
+#else
+    return close(sock);
+#endif
+}
+
+static __inline void socket_nonblock(int fd)
+{
+#ifdef _WIN32
+	long nonblock = 1;
+	ioctlsocket(fd, FIONBIO, &nonblock );
+#else
+	int oldopts = fcntl(fd, F_GETFL, 0);
+	fcntl(fd, F_SETFL, oldopts | O_NONBLOCK);
+#endif
+}
+
+#endif /* REPLACEMENTS_H */
diff --git a/src/helper/time_support.c b/src/helper/time_support.c
index 5a7869d9..620c9c48 100644
--- a/src/helper/time_support.c
+++ b/src/helper/time_support.c
@@ -17,7 +17,9 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
 #include "config.h"
+#endif
 
 #include "time_support.h"
 
diff --git a/src/jtag/Makefile.am b/src/jtag/Makefile.am
index a3a06606..bb9337aa 100644
--- a/src/jtag/Makefile.am
+++ b/src/jtag/Makefile.am
@@ -1,6 +1,10 @@
 
 if FTD2XXDIR
+if IS_MINGW
+FTD2XXINC = -I@WITH_FTD2XX@
+else
 FTD2XXINC = -I@WITH_FTD2XX@/
+endif
 else
 FTD2XXINC =
 endif
diff --git a/src/jtag/amt_jtagaccel.c b/src/jtag/amt_jtagaccel.c
index 42f8bc36..113aee66 100644
--- a/src/jtag/amt_jtagaccel.c
+++ b/src/jtag/amt_jtagaccel.c
@@ -17,7 +17,10 @@
  *   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 "jtag.h"
 
diff --git a/src/jtag/bitbang.c b/src/jtag/bitbang.c
index d6ff2898..3d49d186 100644
--- a/src/jtag/bitbang.c
+++ b/src/jtag/bitbang.c
@@ -17,6 +17,9 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
 
 #include "bitbang.h"
 
@@ -65,6 +68,38 @@ void bitbang_state_move(void) {
 	cur_state = end_state;
 }
 
+void bitbang_path_move(pathmove_command_t *cmd)
+{
+	int num_states = cmd->num_states;
+	int state_count;
+
+	state_count = 0;
+	while (num_states)
+	{
+		if (tap_transitions[cur_state].low == cmd->path[state_count])
+		{
+			bitbang_interface->write(0, 0, 0);
+			bitbang_interface->write(1, 0, 0);
+		}
+		else if (tap_transitions[cur_state].high == cmd->path[state_count])
+		{
+			bitbang_interface->write(0, 1, 0);
+			bitbang_interface->write(1, 1, 0);
+		}			
+		else
+		{
+			ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_strings[cur_state], tap_state_strings[cmd->path[state_count]]);
+			exit(-1);
+		}
+		
+		cur_state = cmd->path[state_count];
+		state_count++;
+		num_states--;
+	}
+	
+	end_state = cur_state;
+}
+
 void bitbang_runtest(int num_cycles)
 {
 	int i;
@@ -187,6 +222,12 @@ int bitbang_execute_queue(void)
 					bitbang_end_state(cmd->cmd.statemove->end_state);
 				bitbang_state_move();
 				break;
+			case JTAG_PATHMOVE:
+#ifdef _DEBUG_JTAG_IO_
+				DEBUG("pathmove: %i states, end in %i", cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
+#endif
+				bitbang_path_move(cmd->cmd.pathmove);
+				break;
 			case JTAG_SCAN:
 #ifdef _DEBUG_JTAG_IO_
 				DEBUG("scan end in %i", cmd->cmd.scan->end_state);
diff --git a/src/jtag/ep93xx.c b/src/jtag/ep93xx.c
index 9c24dba4..e68e3d15 100644
--- a/src/jtag/ep93xx.c
+++ b/src/jtag/ep93xx.c
@@ -17,7 +17,10 @@
  *   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 "jtag.h"
 #include "bitbang.h"
diff --git a/src/jtag/ftd2xx.c b/src/jtag/ftd2xx.c
index a14d0391..e8d29a88 100644
--- a/src/jtag/ftd2xx.c
+++ b/src/jtag/ftd2xx.c
@@ -17,17 +17,23 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
 #include "config.h"
+#endif
+
 #if IS_CYGWIN == 1
 #include "windows.h"
 #undef ERROR
 #endif
 
+#include "replacements.h"
+
 /* project specific includes */
 #include "log.h"
 #include "types.h"
 #include "jtag.h"
 #include "configuration.h"
+#include "time_support.h"
 
 /* system includes */
 #include <string.h>
@@ -222,6 +228,7 @@ int ftd2xx_send_and_recv(jtag_command_t *first, jtag_command_t *last)
 	
 #ifdef _DEBUG_USB_IO_
 	struct timeval start, inter, inter2, end;
+	struct timeval d_inter, d_inter2, d_end;
 #endif
 
 #ifdef _DEBUG_USB_COMMS_
@@ -272,9 +279,11 @@ int ftd2xx_send_and_recv(jtag_command_t *first, jtag_command_t *last)
 #ifdef _DEBUG_USB_IO_
 		gettimeofday(&end, NULL);	
 
-		INFO("inter: %i.%i, inter2: %i.%i end: %i.%i", inter.tv_sec - start.tv_sec, inter.tv_usec - start.tv_usec,
-			inter2.tv_sec - start.tv_sec, inter2.tv_usec - start.tv_usec,
-			end.tv_sec - start.tv_sec, end.tv_usec - start.tv_usec);
+		timeval_subtract(&d_inter, &inter, &start);
+		timeval_subtract(&d_inter2, &inter2, &start);
+		timeval_subtract(&d_end, &end, &start);
+
+		INFO("inter: %i.%i, inter2: %i.%i end: %i.%i", d_inter.tv_sec, d_inter.tv_usec, d_inter2.tv_sec, d_inter2.tv_usec, d_end.tv_sec, d_end.tv_usec);
 #endif
 	
 		
@@ -324,6 +333,46 @@ int ftd2xx_send_and_recv(jtag_command_t *first, jtag_command_t *last)
 	return ERROR_OK;
 }
 
+void ftd2xx_add_pathmove(pathmove_command_t *cmd)
+{
+	int num_states = cmd->num_states;
+	u8 tms_byte;
+	int state_count;
+
+	state_count = 0;
+	while (num_states)
+	{
+		tms_byte = 0x0;
+		int bit_count = 0;
+		
+		/* command "Clock Data to TMS/CS Pin (no Read)" */
+		BUFFER_ADD = 0x4b;
+		/* number of states remaining */
+		BUFFER_ADD = (num_states % 7) - 1;
+		
+		while (num_states % 7)
+		{
+			if (tap_transitions[cur_state].low == cmd->path[state_count])
+				buf_set_u32(&tms_byte, bit_count++, 1, 0x0);
+			else if (tap_transitions[cur_state].high == cmd->path[state_count])
+				buf_set_u32(&tms_byte, bit_count++, 1, 0x1);
+			else
+			{
+				ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_strings[cur_state], tap_state_strings[cmd->path[state_count]]);
+				exit(-1);
+			}
+
+			cur_state = cmd->path[state_count];
+			state_count++;
+			num_states--;
+		}
+		
+		BUFFER_ADD = tms_byte;
+	}
+	
+	end_state = cur_state;
+}
+
 void ftd2xx_add_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size)
 {
 	int num_bytes = (scan_size + 7) / 8;
@@ -331,23 +380,26 @@ void ftd2xx_add_scan(int ir_scan, enum scan_type type, u8 *buffer, int 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
+	if ((!ir_scan && (cur_state != TAP_SD)) || (ir_scan && (cur_state != TAP_SI)))
 	{
-		BUFFER_ADD = TAP_MOVE(cur_state, TAP_SD);
-		cur_state = TAP_SD;
+		/* 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)");
 	}
-	//DEBUG("added TMS scan (no read)");
-
+	
 	/* add command for complete bytes */
 	if (num_bytes > 1)
 	{
@@ -370,7 +422,7 @@ void ftd2xx_add_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size
 			//DEBUG("added TDI bytes (i %i)", num_bytes);
 		}
 		BUFFER_ADD = (num_bytes-2) & 0xff;
-		BUFFER_ADD = (num_bytes >> 8) & 0xff;
+		BUFFER_ADD = ((num_bytes-2) >> 8) & 0xff;
 	}
 	if (type != SCAN_IN)
 	{
@@ -440,15 +492,23 @@ void ftd2xx_add_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size
 
 int ftd2xx_predict_scan_out(int scan_size, enum scan_type type)
 {
-	int predicted_size = 6;
+	int predicted_size = 3;
+	
+	if (cur_state != TAP_SD)
+		predicted_size += 3;
+	
 	if (type == SCAN_IN)	/* only from device to host */
 	{
+		/* complete bytes */
 		predicted_size += (CEIL(scan_size, 8) > 1) ? 3 : 0;
+		/* remaining bits - 1 (up to 7) */
 		predicted_size += ((scan_size - 1) % 8) ? 2 : 0;
 	}
 	else					/* host to device, or bidirectional */
 	{
+		/* complete bytes */
 		predicted_size += (CEIL(scan_size, 8) > 1) ? (CEIL(scan_size, 8) + 3 - 1) : 0;
+		/* remaining bits -1 (up to 7) */
 		predicted_size += ((scan_size - 1) % 8) ? 3 : 0;
 	}
 
@@ -588,7 +648,10 @@ int ftd2xx_execute_queue()
 
 				layout->reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
 				require_send = 1;
-
+				
+#ifdef _DEBUG_JTAG_IO_				
+				DEBUG("trst: %i, srst: %i", cmd->cmd.reset->trst, cmd->cmd.reset->srst);
+#endif
 				break;
 			case JTAG_RUNTEST:
 				/* only send the maximum buffer size that FT2232C can handle */
@@ -644,6 +707,9 @@ int ftd2xx_execute_queue()
 					//DEBUG("added TMS scan (no read)");
 				}
 				require_send = 1;
+#ifdef _DEBUG_JTAG_IO_				
+				DEBUG("runtest: %i, end in %i", cmd->cmd.runtest->num_cycles, end_state);
+#endif
 				break;
 			case JTAG_STATEMOVE:
 				/* only send the maximum buffer size that FT2232C can handle */
@@ -665,6 +731,24 @@ int ftd2xx_execute_queue()
 				//DEBUG("added TMS scan (no read)");
 				cur_state = end_state;
 				require_send = 1;
+#ifdef _DEBUG_JTAG_IO_				
+				DEBUG("statemove: %i", end_state);
+#endif
+				break;
+			case JTAG_PATHMOVE:
+				/* only send the maximum buffer size that FT2232C can handle */
+				predicted_size = 3 * CEIL(cmd->cmd.pathmove->num_states, 7);
+				if (ftd2xx_buffer_size + predicted_size + 1 > FTD2XX_BUFFER_SIZE)
+				{
+					ftd2xx_send_and_recv(first_unsent, cmd);
+					require_send = 0;
+					first_unsent = cmd;
+				}
+				ftd2xx_add_pathmove(cmd->cmd.pathmove);
+				require_send = 1;
+#ifdef _DEBUG_JTAG_IO_				
+				DEBUG("pathmove: %i states, end in %i", cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
+#endif
 				break;
 			case JTAG_SCAN:
 				scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
@@ -685,11 +769,17 @@ int ftd2xx_execute_queue()
 				require_send = 1;
 				if (buffer)
 					free(buffer);
+#ifdef _DEBUG_JTAG_IO_				
+				DEBUG("%s scan, %i bit, end in %i", (cmd->cmd.scan->ir_scan) ? "IR" : "DR", scan_size, end_state);
+#endif
 				break;
 			case JTAG_SLEEP:
 				ftd2xx_send_and_recv(first_unsent, cmd);
 				first_unsent = cmd->next;
 				jtag_sleep(cmd->cmd.sleep->us);
+#ifdef _DEBUG_JTAG_IO_				
+				DEBUG("sleep %i usec", cmd->cmd.sleep->us);
+#endif
 				break;
 			default:
 				ERROR("BUG: unknown JTAG command type encountered");
@@ -740,7 +830,7 @@ int ftd2xx_init(void)
 		ftd2xx_device_desc = "Dual RS232";
 	}
 	
-#if IS_CYGWIN != 1
+#if IS_WIN32 == 0
 	/* Add JTAGkey Vid/Pid to the linux driver */
 	if ((status = FT_SetVIDPID(ftd2xx_vid, ftd2xx_pid)) != FT_OK)
 	{
diff --git a/src/jtag/ftdi2232.c b/src/jtag/ftdi2232.c
index efd528c3..c90cc2f9 100644
--- a/src/jtag/ftdi2232.c
+++ b/src/jtag/ftdi2232.c
@@ -17,6 +17,9 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
 
 /* project specific includes */
 #include "log.h"
diff --git a/src/jtag/jtag.c b/src/jtag/jtag.c
index e306b0a2..d43fafb9 100644
--- a/src/jtag/jtag.c
+++ b/src/jtag/jtag.c
@@ -17,7 +17,12 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
 #include "config.h"
+#endif
+
+#include "replacements.h"
+
 #include "jtag.h"
 
 #include "command.h"
diff --git a/src/jtag/parport.c b/src/jtag/parport.c
index 8265ada8..e78215e2 100644
--- a/src/jtag/parport.c
+++ b/src/jtag/parport.c
@@ -17,22 +17,34 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
 #include "config.h"
-#include "log.h"
+#endif
+
+#include "replacements.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
+
+#ifndef _WIN32
 #include <sys/io.h>
-#endif
+#else
+#include "errno.h"
+#endif /* _WIN32 */
+
+#endif /* __FreeBSD__ */
 
 #include <string.h>
 #include <stdlib.h>
@@ -45,6 +57,16 @@
 #include <sys/ioctl.h>
 #endif
 
+#if PARPORT_USE_GIVEIO == 1
+#if IS_CYGWIN == 1
+#include <windows.h>
+#include <errno.h>
+#undef ERROR
+#endif
+#endif
+
+#include "log.h"
+
 /* parallel port cable description
  */
 typedef struct cable_s
@@ -221,6 +243,32 @@ int parport_register_commands(struct command_context_s *cmd_ctx)
 	return ERROR_OK;
 }
 
+#if PARPORT_USE_GIVEIO == 1
+int parport_get_giveio_access()
+{
+    HANDLE h;
+    OSVERSIONINFO version;
+
+    version.dwOSVersionInfoSize = sizeof version;
+    if (!GetVersionEx( &version )) {
+        errno = EINVAL;
+        return -1;
+    }
+    if (version.dwPlatformId != VER_PLATFORM_WIN32_NT)
+        return 0;
+
+    h = CreateFile( "\\\\.\\giveio", GENERIC_READ, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL );
+    if (h == INVALID_HANDLE_VALUE) {
+        errno = ENODEV;
+        return -1;
+    }
+
+    CloseHandle( h );
+
+    return 0;
+}
+#endif
+
 int parport_init(void)
 {
 	cable_t *cur_cable;
@@ -303,11 +351,16 @@ int parport_init(void)
 	dataport = parport_port;
 	statusport = parport_port + 1;
 		
-	if (ioperm(dataport, 3, 1) != 0) {
+#if PARPORT_USE_GIVEIO == 1
+	if (parport_get_giveio_access() != 0)
+#else /* PARPORT_USE_GIVEIO */
+	if (ioperm(dataport, 3, 1) != 0)
+#endif /* PARPORT_USE_GIVEIO */
+	{
 		ERROR("missing privileges for direct i/o");
 		return ERROR_JTAG_INIT_FAILED;
 	}
-#endif
+#endif /* PARPORT_USE_PPDEV */
 	
 	parport_reset(0, 0);
 	parport_write(0, 0, 0);
diff --git a/src/openocd.c b/src/openocd.c
index 9e71dbeb..e52afd34 100644
--- a/src/openocd.c
+++ b/src/openocd.c
@@ -18,10 +18,10 @@
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
 
-#define OPENOCD_VERSION "Open On-Chip Debugger (2006-06-25 23:00 CEST)"
+#define OPENOCD_VERSION "Open On-Chip Debugger (2006-07-15 12:00 CEST)"
 
 #ifdef HAVE_CONFIG_H
-#include <config.h>
+#include "config.h"
 #endif
 
 #include "log.h"
@@ -40,13 +40,9 @@
 
 #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>
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index b0c09961..4b99922c 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -17,7 +17,11 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
 #include "config.h"
+#endif
+
+#include "replacements.h"
 
 #include "gdb_server.h"
 
@@ -32,21 +36,6 @@
 #include <unistd.h>
 #include <stdlib.h>
 
-#ifndef HAVE_STRNDUP
-#include <stdio.h>
-char* strndup(const char *s, size_t n)
-{
-	size_t len = strnlen (s, n);
-	char *new = (char *) malloc (len + 1);
-
-	if (new == NULL)
-		return NULL;
-
-	new[len] = '\0';
-	return (char *) memcpy (new, s, len);
-}
-#endif
-
 #if 0
 #define _DEBUG_GDB_IO_
 #endif
@@ -93,11 +82,26 @@ int gdb_get_char(connection_t *connection, int* next_char)
 		return ERROR_OK;
 	}
 
-	while ((gdb_con->buf_cnt = read(connection->fd, gdb_con->buffer, GDB_BUFFER_SIZE)) <= 0)
+	while ((gdb_con->buf_cnt = read_socket(connection->fd, gdb_con->buffer, GDB_BUFFER_SIZE)) <= 0)
 	{
 		if (gdb_con->buf_cnt == 0)
 			return ERROR_SERVER_REMOTE_CLOSED;
 		
+#ifdef _WIN32
+		errno = WSAGetLastError();
+
+		switch(errno)
+		{
+			case WSAEWOULDBLOCK:
+				usleep(1000);
+				break;
+			case WSAECONNABORTED:
+				return ERROR_SERVER_REMOTE_CLOSED;
+			default:
+				ERROR("read: %d", strerror(errno));
+				exit(-1);
+		}
+#else
 		switch(errno)
 		{
 			case EAGAIN:
@@ -111,6 +115,7 @@ int gdb_get_char(connection_t *connection, int* next_char)
 				ERROR("read: %s", strerror(errno));
 				exit(-1);
 		}
+#endif
 	}
 	
 	debug_buffer = malloc(gdb_con->buf_cnt + 1);
@@ -155,14 +160,14 @@ int gdb_put_packet(connection_t *connection, char *buffer, int len)
 		DEBUG("sending packet '$%s#%2.2x'", debug_buffer, my_checksum);
 		free(debug_buffer);
 		
-		write(connection->fd, "$", 1);
+		write_socket(connection->fd, "$", 1);
 		if (len > 0)
-			write(connection->fd, buffer, len);
-		write(connection->fd, "#", 1);
+			write_socket(connection->fd, buffer, len);
+		write_socket(connection->fd, "#", 1);
 	
 		snprintf(checksum, 3, "%2.2x", my_checksum);
 	
-		write(connection->fd, checksum, 2);
+		write_socket(connection->fd, checksum, 2);
 
 		if ((retval = gdb_get_char(connection, &reply)) != ERROR_OK)
 			return retval;
@@ -310,12 +315,12 @@ int gdb_get_packet(connection_t *connection, char *buffer, int *len)
 		
 		if (my_checksum == strtoul(checksum, NULL, 16))
 		{
-			write (connection->fd, "+", 1);
+			write_socket(connection->fd, "+", 1);
 			break;
 		}
 
 		WARNING("checksum error, requesting retransmission");
-		write(connection->fd, "-", 1);
+		write_socket(connection->fd, "-", 1);
 	}
 
 	return ERROR_OK;
@@ -1087,6 +1092,7 @@ int gdb_init()
 		
 		DEBUG("gdb service for target %s at port %i", target->type->name, gdb_port + i);
 		
+		i++;
 		target = target->next;
 	}
 	
diff --git a/src/server/server.c b/src/server/server.c
index 628c4925..5d7df1af 100644
--- a/src/server/server.c
+++ b/src/server/server.c
@@ -17,6 +17,12 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "replacements.h"
+
 #include "server.h"
 
 #include "log.h"
@@ -29,7 +35,6 @@
 #include <errno.h>
 #include <unistd.h>
 #include <sys/types.h>
-#include <sys/socket.h>
 #include <fcntl.h>
 #include <signal.h>
 
@@ -63,7 +68,7 @@ int add_connection(service_t *service, command_context_t *cmd_ctx)
 	}
 	else
 	{
-		close(c->fd);
+		close_socket(c->fd);
 		INFO("attempted '%s' connection rejected", service->name);
 		free(c);
 	}
@@ -97,7 +102,7 @@ int remove_connection(service_t *service, connection_t *connection)
 		{	
 			service->connections = next;
 			service->connection_closed(c);
-			close(c->fd);
+			close_socket(c->fd);
 			
 			command_done(c->cmd_ctx);
 			
@@ -119,7 +124,6 @@ int add_service(char *name, enum connection_type type, unsigned short port, int
 {
 	service_t *c, *p;
 	int so_reuseaddr_option = 1;
-	int oldopts;
 	
 	c = malloc(sizeof(service_t));
 	
@@ -141,10 +145,9 @@ int add_service(char *name, enum connection_type type, unsigned short port, int
 		exit(-1);
 	}
 	
-	setsockopt(c->fd, SOL_SOCKET, SO_REUSEADDR, &so_reuseaddr_option, sizeof(int));
+	setsockopt(c->fd, SOL_SOCKET, SO_REUSEADDR, (void*)&so_reuseaddr_option, sizeof(int));
 	
-	oldopts = fcntl(c->fd, F_GETFL, 0);
-	fcntl(c->fd, F_SETFL, oldopts | O_NONBLOCK);
+	socket_nonblock(c->fd);
 	
 	memset(&c->sin, 0, sizeof(c->sin));
 	c->sin.sin_family = AF_INET;
@@ -205,6 +208,31 @@ int remove_service(unsigned short port)
 	return ERROR_OK;
 }
 
+int remove_services()
+{
+	service_t *c = services;
+
+	/* loop service */
+	while(c)
+	{
+		service_t *next = c->next;
+
+		if (c->name)
+			free(c->name);
+
+		if (c->priv)
+			free(c->priv);
+
+		/* delete service */
+		free(c);
+
+		/* remember the last service for unlinking */
+		c = next;
+	}
+
+	return ERROR_OK;
+}
+
 int server_loop(command_context_t *command_context)
 {
 	service_t *service;
@@ -217,8 +245,10 @@ int server_loop(command_context_t *command_context)
 	/* used in accept() */
 	int retval;
 	
+#ifndef _WIN32
 	if (signal(SIGPIPE, SIG_IGN) == SIG_ERR)
 		ERROR("couldn't set SIGPIPE to SIG_IGN");
+#endif
 	
 	/* do regular tasks after at most 10ms */
 	tv.tv_sec = 0;
@@ -256,11 +286,26 @@ int server_loop(command_context_t *command_context)
 			}
 		}
 		
+#ifndef _WIN32
 		/* add STDIN to read_fds */
 		FD_SET(fileno(stdin), &read_fds);
+#endif
 
 		if ((retval = select(fd_max + 1, &read_fds, NULL, NULL, &tv)) == -1)
 		{
+#ifdef _WIN32
+
+			errno = WSAGetLastError();
+
+			if (errno == WSAEINTR)
+				FD_ZERO(&read_fds);
+			else
+			{
+				ERROR("error during select: %d", strerror(errno));
+				exit(-1);
+			}
+#else
+
 			if (errno == EINTR)
 				FD_ZERO(&read_fds);
 			else
@@ -268,6 +313,7 @@ int server_loop(command_context_t *command_context)
 				ERROR("error during select: %s", strerror(errno));
 				exit(-1);
 			}
+#endif
 		}
 		
 		target_call_timer_callbacks();
@@ -300,7 +346,7 @@ int server_loop(command_context_t *command_context)
 					unsigned int address_size = sizeof(sin);
 					int tmp_fd;
 					tmp_fd = accept(service->fd, (struct sockaddr *)&service->sin, &address_size);
-					close(tmp_fd);
+					close_socket(tmp_fd);
 					INFO("rejected '%s' connection, no more connections allowed", service->name);
 				}
 			}
@@ -328,6 +374,7 @@ int server_loop(command_context_t *command_context)
 			}
 		}
 		
+#ifndef _WIN32
 		if (FD_ISSET(fileno(stdin), &read_fds))
 		{
 			if (getc(stdin) == 'x')
@@ -335,17 +382,53 @@ int server_loop(command_context_t *command_context)
 				shutdown_openocd = 1;
 			}
 		}
+#endif
 	}
 	
 	return ERROR_OK;
 }
 
+#ifdef _WIN32
+BOOL WINAPI ControlHandler(DWORD dwCtrlType)
+{
+    shutdown_openocd = 1;
+    return TRUE;
+}
+#endif
+
 int server_init()
 {
+#ifdef _WIN32
+	WORD wVersionRequested;
+	WSADATA wsaData;
+
+	wVersionRequested = MAKEWORD( 2, 2 );
+
+	if (WSAStartup(wVersionRequested, &wsaData) != 0)
+	{
+		ERROR("Failed to Open Winsock");
+		exit(-1);
+	}
+
+	SetConsoleCtrlHandler( ControlHandler, TRUE );
+#endif
+
 	
 	return ERROR_OK;
 }
 
+int server_close()
+{
+	remove_services();
+
+#ifdef _WIN32
+	WSACleanup();
+	SetConsoleCtrlHandler( ControlHandler, FALSE );
+#endif
+
+	return ERROR_OK;
+}
+
 int server_register_commands(command_context_t *context)
 {
 	register_command(context, NULL, "shutdown", handle_shutdown_command,
diff --git a/src/server/server.h b/src/server/server.h
index 625c364e..ddf0b97d 100644
--- a/src/server/server.h
+++ b/src/server/server.h
@@ -22,10 +22,9 @@
 
 #include "command.h"
 #include "binarybuffer.h"
+#include "replacements.h"
 
 #include <sys/types.h>
-#include <sys/socket.h>
-#include <netinet/in.h>
 
 enum connection_type
 {
diff --git a/src/server/telnet_server.c b/src/server/telnet_server.c
index a2704e9c..b573ec5c 100644
--- a/src/server/telnet_server.c
+++ b/src/server/telnet_server.c
@@ -17,6 +17,12 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "replacements.h"
+
 #include "telnet_server.h"
 
 #include "server.h"
@@ -47,15 +53,15 @@ void telnet_prompt(connection_t *connection)
 {
 	telnet_connection_t *t_con = connection->priv;
 
-	write(connection->fd, t_con->prompt, strlen(t_con->prompt));
+	write_socket(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);
+	write_socket(connection->fd, line, strlen(line));
+	write_socket(connection->fd, "\r\n\0", 3);
 	
 	return ERROR_OK;
 }
@@ -109,13 +115,13 @@ int telnet_new_connection(connection_t *connection)
 	command_set_output_handler(connection->cmd_ctx, telnet_output, connection);
 	
 	/* negotiate telnet options */
-	write(connection->fd, negotiate, strlen(negotiate));
+	write_socket(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);
+		write_socket(connection->fd, telnet_service->banner, strlen(telnet_service->banner));
+		write_socket(connection->fd, "\r\n\0", 3);
 	}
 	
 	telnet_prompt(connection);
@@ -138,13 +144,13 @@ 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);
+		write_socket(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);
+		write_socket(connection->fd, "\b \b", 3);
 		t_con->line_size--;
 	}
 	t_con->line_cursor = 0;
@@ -158,7 +164,7 @@ int telnet_input(connection_t *connection)
 	telnet_connection_t *t_con = connection->priv;
 	command_context_t *command_context = connection->cmd_ctx;
 	
-	bytes_read = read(connection->fd, buffer, TELNET_BUFFER_SIZE);
+	bytes_read = read_socket(connection->fd, buffer, TELNET_BUFFER_SIZE);
 	
 	if (bytes_read == 0)
 		return ERROR_SERVER_REMOTE_CLOSED;
@@ -182,7 +188,7 @@ int telnet_input(connection_t *connection)
 				{
 					if (isprint(*buf_p)) /* printable character */
 					{
-						write(connection->fd, buf_p, 1);
+						write_socket(connection->fd, buf_p, 1);
 						if (t_con->line_cursor == t_con->line_size)
 						{
 							t_con->line[t_con->line_size++] = *buf_p;
@@ -194,10 +200,10 @@ int telnet_input(connection_t *connection)
 							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);
+							write_socket(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);
+								write_socket(connection->fd, "\b", 1);
 							}
 						}
 					}
@@ -225,7 +231,7 @@ int telnet_input(connection_t *connection)
 							}
 							t_con->line[t_con->line_size] = 0;
 							
-							write(connection->fd, "\r\n\x00", 3);
+							write_socket(connection->fd, "\r\n\x00", 3);
 							
 							if (strcmp(t_con->line, "history") == 0)
 							{
@@ -234,8 +240,8 @@ int telnet_input(connection_t *connection)
 								{
 									if (t_con->history[i])
 									{
-										write(connection->fd, t_con->history[i], strlen(t_con->history[i]));
-										write(connection->fd, "\r\n\x00", 3);
+										write_socket(connection->fd, t_con->history[i], strlen(t_con->history[i]));
+										write_socket(connection->fd, "\r\n\x00", 3);
 									}
 								}
 								telnet_prompt(connection);
@@ -299,16 +305,16 @@ int telnet_input(connection_t *connection)
 								if (t_con->line_cursor != t_con->line_size)
 								{
 									int i;
-									write(connection->fd, "\b", 1);
+									write_socket(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);
+									write_socket(connection->fd, t_con->line + t_con->line_cursor, t_con->line_size - t_con->line_cursor);
+									write_socket(connection->fd, " \b", 2);
 									for (i = t_con->line_cursor; i < t_con->line_size; i++)
 									{
-										write(connection->fd, "\b", 1);
+										write_socket(connection->fd, "\b", 1);
 									}
 								}
 								else
@@ -316,7 +322,7 @@ int telnet_input(connection_t *connection)
 									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);
+									write_socket(connection->fd, "\b \b", 3);
 								}
 							}
 						}
@@ -328,7 +334,7 @@ int telnet_input(connection_t *connection)
 						{
 							if (t_con->line_cursor > 0)
 							{
-								write(connection->fd, "\b", 1);
+								write_socket(connection->fd, "\b", 1);
 								t_con->line_cursor--;
 							}
 							t_con->state = TELNET_STATE_DATA;
@@ -337,7 +343,7 @@ int telnet_input(connection_t *connection)
 						{
 							if (t_con->line_cursor < t_con->line_size)
 							{
-								write(connection->fd, t_con->line + t_con->line_cursor++, 1);
+								write_socket(connection->fd, t_con->line + t_con->line_cursor++, 1);
 							}
 							t_con->state = TELNET_STATE_DATA;
 						}
@@ -382,7 +388,7 @@ int telnet_input(connection_t *connection)
 					{
 						if (t_con->line_cursor > 0)
 						{
-							write(connection->fd, "\b", 1);
+							write_socket(connection->fd, "\b", 1);
 							t_con->line_cursor--;
 						}
 						t_con->state = TELNET_STATE_DATA;
@@ -391,7 +397,7 @@ int telnet_input(connection_t *connection)
 					{
 						if (t_con->line_cursor < t_con->line_size)
 						{
-							write(connection->fd, t_con->line + t_con->line_cursor++, 1);
+							write_socket(connection->fd, t_con->line + t_con->line_cursor++, 1);
 						}
 						t_con->state = TELNET_STATE_DATA;
 					}
@@ -404,7 +410,7 @@ int telnet_input(connection_t *connection)
 							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);
+							write_socket(connection->fd, t_con->line, t_con->line_size);
 							t_con->current_history = last_history;
 						}
 						t_con->state = TELNET_STATE_DATA;
@@ -418,7 +424,7 @@ int telnet_input(connection_t *connection)
 							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);
+							write_socket(connection->fd, t_con->line, t_con->line_size);
 							t_con->current_history = next_history;
 						}
 						t_con->state = TELNET_STATE_DATA;
@@ -445,14 +451,14 @@ int telnet_input(connection_t *connection)
 							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);
+							write_socket(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);
+							write_socket(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);
+								write_socket(connection->fd, "\b", 1);
 							}
 						}
 							
diff --git a/src/target/algorithm.c b/src/target/algorithm.c
index fdebfc58..c5ee4708 100644
--- a/src/target/algorithm.c
+++ b/src/target/algorithm.c
@@ -17,7 +17,10 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
 #include "config.h"
+#endif
+
 #include "algorithm.h"
 
 #include "log.h"
diff --git a/src/target/arm720t.c b/src/target/arm720t.c
index 68ea3d1c..31428a27 100644
--- a/src/target/arm720t.c
+++ b/src/target/arm720t.c
@@ -17,7 +17,9 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
 #include "config.h"
+#endif
 
 #include "arm720t.h"
 #include "jtag.h"
diff --git a/src/target/arm7_9_common.c b/src/target/arm7_9_common.c
index 1e58ec9d..62fc3b5b 100644
--- a/src/target/arm7_9_common.c
+++ b/src/target/arm7_9_common.c
@@ -17,7 +17,11 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
 #include "config.h"
+#endif
+
+#include "replacements.h"
 
 #include "embeddedice.h"
 #include "target.h"
@@ -186,7 +190,7 @@ int arm7_9_set_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
 		else
 		{
 			target->type->read_memory(target, breakpoint->address, 2, 1, breakpoint->orig_instr);
-			target->type->write_memory(target, breakpoint->address, 2, 1, (u8*)(&arm7_9->arm_bkpt));
+			target->type->write_memory(target, breakpoint->address, 2, 1, (u8*)(&arm7_9->thumb_bkpt));
 		}
 		breakpoint->set = 1;
 	}
diff --git a/src/target/arm7tdmi.c b/src/target/arm7tdmi.c
index c2079477..f6f0cef7 100644
--- a/src/target/arm7tdmi.c
+++ b/src/target/arm7tdmi.c
@@ -17,7 +17,9 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
 #include "config.h"
+#endif
 
 #include "arm7tdmi.h"
 
diff --git a/src/target/arm920t.c b/src/target/arm920t.c
index eb0fa7df..ac0b6a6a 100644
--- a/src/target/arm920t.c
+++ b/src/target/arm920t.c
@@ -17,7 +17,9 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
 #include "config.h"
+#endif
 
 #include "arm920t.h"
 #include "jtag.h"
diff --git a/src/target/arm9tdmi.c b/src/target/arm9tdmi.c
index 48b201a1..04ea676b 100644
--- a/src/target/arm9tdmi.c
+++ b/src/target/arm9tdmi.c
@@ -17,7 +17,9 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
 #include "config.h"
+#endif
 
 #include "arm9tdmi.h"
 
diff --git a/src/target/arm_disassembler.c b/src/target/arm_disassembler.c
index 3a9c1f87..09024102 100644
--- a/src/target/arm_disassembler.c
+++ b/src/target/arm_disassembler.c
@@ -17,6 +17,10 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
 #include "arm_disassembler.h"
 
 #include "log.h"
diff --git a/src/target/arm_jtag.c b/src/target/arm_jtag.c
index c401d8f3..ccc1adf4 100644
--- a/src/target/arm_jtag.c
+++ b/src/target/arm_jtag.c
@@ -17,7 +17,9 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
 #include "config.h"
+#endif
 
 #include "arm_jtag.h"
 
diff --git a/src/target/armv4_5.c b/src/target/armv4_5.c
index 4932e20a..2ee34e29 100644
--- a/src/target/armv4_5.c
+++ b/src/target/armv4_5.c
@@ -17,7 +17,11 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
 #include "config.h"
+#endif
+
+#include "replacements.h"
 
 #include "arm_disassembler.h"
 
diff --git a/src/target/armv4_5_cache.c b/src/target/armv4_5_cache.c
index 326f10ee..30a41d61 100644
--- a/src/target/armv4_5_cache.c
+++ b/src/target/armv4_5_cache.c
@@ -17,6 +17,10 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
 #include "armv4_5_cache.h"
 
 #include "log.h"
diff --git a/src/target/armv4_5_mmu.c b/src/target/armv4_5_mmu.c
index c7dad3e2..269f9d57 100644
--- a/src/target/armv4_5_mmu.c
+++ b/src/target/armv4_5_mmu.c
@@ -17,6 +17,10 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
 #include "arm7_9_common.h"
 #include "log.h"
 #include "command.h"
diff --git a/src/target/breakpoints.c b/src/target/breakpoints.c
index efba25c9..3da92738 100644
--- a/src/target/breakpoints.c
+++ b/src/target/breakpoints.c
@@ -17,7 +17,9 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
 #include "config.h"
+#endif
 
 #include <stdlib.h>
 
diff --git a/src/target/embeddedice.c b/src/target/embeddedice.c
index e148b888..891f9e52 100644
--- a/src/target/embeddedice.c
+++ b/src/target/embeddedice.c
@@ -17,7 +17,9 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
 #include "config.h"
+#endif
 
 #include "embeddedice.h"
 
diff --git a/src/target/etm.c b/src/target/etm.c
index a84e2d67..a6b63451 100644
--- a/src/target/etm.c
+++ b/src/target/etm.c
@@ -1,409 +1,411 @@
-/***************************************************************************

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

-}

-

+/***************************************************************************
+ *   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 "etm.h"
+
+#include "armv4_5.h"
+#include "arm7_9_common.h"
+
+#include "log.h"
+#include "arm_jtag.h"
+#include "types.h"
+#include "binarybuffer.h"
+#include "target.h"
+#include "register.h"
+#include "jtag.h"
+
+#include <stdlib.h>
+
+bitfield_desc_t etm_comms_ctrl_bitfield_desc[] = 
+{
+	{"R", 1},
+	{"W", 1},
+	{"reserved", 26},
+	{"version", 4}
+};
+
+int etm_reg_arch_info[] =
+{
+	0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+	0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
+	0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 
+	0x18, 0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, 0x1f, 
+	0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x27,
+	0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, 0x2f, 
+	0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37,
+	0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f,
+	0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47, 
+	0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e, 0x4f, 
+	0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, 0x57,
+	0x58, 0x59, 0x5a, 0x5b, 0x5c, 0x5d, 0x5e, 0x5f,
+	0x60, 0x61, 0x62, 0x63, 0x64, 0x65, 0x67, 
+	0x68, 0x69, 0x6a, 0x6b, 0x6c, 0x6d, 0x6e, 0x6f, 
+};
+
+int etm_reg_arch_size_info[] =
+{
+	32, 32, 17, 8, 3, 9, 32, 17,
+	26, 16, 25, 8, 17, 32, 32, 17,
+	32, 32, 32, 32, 32, 32, 32, 32, 
+	32, 32, 32, 32, 32, 32, 32, 32, 
+	7, 7, 7, 7, 7, 7, 7, 7, 
+	7, 7, 7, 7, 7, 7, 7, 7, 
+	32, 32, 32, 32, 32, 32, 32, 32, 
+	32, 32, 32, 32, 32, 32, 32, 32, 
+	32, 32, 32, 32, 32, 32, 32, 32, 
+	32, 32, 32, 32, 32, 32, 32, 32, 
+	16, 16, 16, 16, 18, 18, 18, 18,
+	17, 17, 17, 17, 16, 16, 16, 16,
+	17, 17, 17, 17, 17, 17, 2, 
+	17, 17, 17, 17, 32, 32, 32, 32 
+};
+
+char* etm_reg_list[] =
+{
+	"ETM_CTRL",
+	"ETM_CONFIG",
+	"ETM_TRIG_EVENT",
+	"ETM_MMD_CTRL",
+	"ETM_STATUS",
+	"ETM_SYS_CONFIG",
+	"ETM_TRACE_RESOURCE_CTRL",
+	"ETM_TRACE_EN_CTRL2",
+	"ETM_TRACE_EN_EVENT",
+	"ETM_TRACE_EN_CTRL1",
+	"ETM_FIFOFULL_REGION",
+	"ETM_FIFOFULL_LEVEL",
+	"ETM_VIEWDATA_EVENT",
+	"ETM_VIEWDATA_CTRL1",
+	"ETM_VIEWDATA_CTRL2",
+	"ETM_VIEWDATA_CTRL3",
+	"ETM_ADDR_COMPARATOR_VALUE1",
+	"ETM_ADDR_COMPARATOR_VALUE2",
+	"ETM_ADDR_COMPARATOR_VALUE3",
+	"ETM_ADDR_COMPARATOR_VALUE4",
+	"ETM_ADDR_COMPARATOR_VALUE5",
+	"ETM_ADDR_COMPARATOR_VALUE6",
+	"ETM_ADDR_COMPARATOR_VALUE7",
+	"ETM_ADDR_COMPARATOR_VALUE8",
+	"ETM_ADDR_COMPARATOR_VALUE9",
+	"ETM_ADDR_COMPARATOR_VALUE10",
+	"ETM_ADDR_COMPARATOR_VALUE11",
+	"ETM_ADDR_COMPARATOR_VALUE12",
+	"ETM_ADDR_COMPARATOR_VALUE13",
+	"ETM_ADDR_COMPARATOR_VALUE14",
+	"ETM_ADDR_COMPARATOR_VALUE15",
+	"ETM_ADDR_COMPARATOR_VALUE16",
+	"ETM_ADDR_ACCESS_TYPE1",
+	"ETM_ADDR_ACCESS_TYPE2",
+	"ETM_ADDR_ACCESS_TYPE3",
+	"ETM_ADDR_ACCESS_TYPE4",
+	"ETM_ADDR_ACCESS_TYPE5",
+	"ETM_ADDR_ACCESS_TYPE6",
+	"ETM_ADDR_ACCESS_TYPE7",
+	"ETM_ADDR_ACCESS_TYPE8",
+	"ETM_ADDR_ACCESS_TYPE9",
+	"ETM_ADDR_ACCESS_TYPE10",
+	"ETM_ADDR_ACCESS_TYPE11",
+	"ETM_ADDR_ACCESS_TYPE12",
+	"ETM_ADDR_ACCESS_TYPE13",
+	"ETM_ADDR_ACCESS_TYPE14",
+	"ETM_ADDR_ACCESS_TYPE15",
+	"ETM_ADDR_ACCESS_TYPE16",
+	"ETM_DATA_COMPARATOR_VALUE1",
+	"ETM_DATA_COMPARATOR_VALUE2",
+	"ETM_DATA_COMPARATOR_VALUE3",
+	"ETM_DATA_COMPARATOR_VALUE4",
+	"ETM_DATA_COMPARATOR_VALUE5",
+	"ETM_DATA_COMPARATOR_VALUE6",
+	"ETM_DATA_COMPARATOR_VALUE7",
+	"ETM_DATA_COMPARATOR_VALUE8",
+	"ETM_DATA_COMPARATOR_VALUE9",
+	"ETM_DATA_COMPARATOR_VALUE10",
+	"ETM_DATA_COMPARATOR_VALUE11",
+	"ETM_DATA_COMPARATOR_VALUE12",
+	"ETM_DATA_COMPARATOR_VALUE13",
+	"ETM_DATA_COMPARATOR_VALUE14",
+	"ETM_DATA_COMPARATOR_VALUE15",
+	"ETM_DATA_COMPARATOR_VALUE16",
+	"ETM_DATA_COMPARATOR_MASK1",
+	"ETM_DATA_COMPARATOR_MASK2",
+	"ETM_DATA_COMPARATOR_MASK3",
+	"ETM_DATA_COMPARATOR_MASK4",
+	"ETM_DATA_COMPARATOR_MASK5",
+	"ETM_DATA_COMPARATOR_MASK6",
+	"ETM_DATA_COMPARATOR_MASK7",
+	"ETM_DATA_COMPARATOR_MASK8",
+	"ETM_DATA_COMPARATOR_MASK9",
+	"ETM_DATA_COMPARATOR_MASK10",
+	"ETM_DATA_COMPARATOR_MASK11",
+	"ETM_DATA_COMPARATOR_MASK12",
+	"ETM_DATA_COMPARATOR_MASK13",
+	"ETM_DATA_COMPARATOR_MASK14",
+	"ETM_DATA_COMPARATOR_MASK15",
+	"ETM_DATA_COMPARATOR_MASK16",
+	"ETM_COUNTER_INITAL_VALUE1",
+	"ETM_COUNTER_INITAL_VALUE2",
+	"ETM_COUNTER_INITAL_VALUE3",
+	"ETM_COUNTER_INITAL_VALUE4",
+	"ETM_COUNTER_ENABLE1",
+	"ETM_COUNTER_ENABLE2",
+	"ETM_COUNTER_ENABLE3",
+	"ETM_COUNTER_ENABLE4",
+	"ETM_COUNTER_RELOAD_VALUE1",
+	"ETM_COUNTER_RELOAD_VALUE2",
+	"ETM_COUNTER_RELOAD_VALUE3",
+	"ETM_COUNTER_RELOAD_VALUE4",
+	"ETM_COUNTER_VALUE1",
+	"ETM_COUNTER_VALUE2",
+	"ETM_COUNTER_VALUE3",
+	"ETM_COUNTER_VALUE4",
+	"ETM_SEQUENCER_CTRL1",
+	"ETM_SEQUENCER_CTRL2",
+	"ETM_SEQUENCER_CTRL3",
+	"ETM_SEQUENCER_CTRL4",
+	"ETM_SEQUENCER_CTRL5",
+	"ETM_SEQUENCER_CTRL6",
+	"ETM_SEQUENCER_STATE",
+	"ETM_EXTERNAL_OUTPUT1",
+	"ETM_EXTERNAL_OUTPUT2",
+	"ETM_EXTERNAL_OUTPUT3",
+	"ETM_EXTERNAL_OUTPUT4",
+	"ETM_CONTEXTID_COMPARATOR_VALUE1",
+	"ETM_CONTEXTID_COMPARATOR_VALUE2",
+	"ETM_CONTEXTID_COMPARATOR_VALUE3",
+	"ETM_CONTEXTID_COMPARATOR_MASK"
+};  
+
+int etm_reg_arch_type = -1;
+
+int etm_get_reg(reg_t *reg);
+int etm_set_reg(reg_t *reg, u32 value);
+
+int etm_write_reg(reg_t *reg, u32 value);
+int etm_read_reg(reg_t *reg);
+
+reg_cache_t* etm_build_reg_cache(target_t *target, arm_jtag_t *jtag_info, int extra_reg)
+{
+	reg_cache_t *reg_cache = malloc(sizeof(reg_cache_t));
+	reg_t *reg_list = NULL;
+	etm_reg_t *arch_info = NULL;
+	int num_regs = sizeof(etm_reg_arch_info)/sizeof(int);
+	int i;
+	
+	/* register a register arch-type for etm registers only once */
+	if (etm_reg_arch_type == -1)
+		etm_reg_arch_type = register_reg_arch_type(etm_get_reg, etm_set_reg_w_exec);
+	
+	/* the actual registers are kept in two arrays */
+	reg_list = calloc(num_regs, sizeof(reg_t));
+	arch_info = calloc(num_regs, sizeof(etm_reg_t));
+	
+	/* fill in values for the reg cache */
+	reg_cache->name = "etm registers";
+	reg_cache->next = NULL;
+	reg_cache->reg_list = reg_list;
+	reg_cache->num_regs = num_regs;
+	
+	/* set up registers */
+	for (i = 0; i < num_regs; i++)
+	{
+		reg_list[i].name = etm_reg_list[i];
+		reg_list[i].size = 32;
+		reg_list[i].dirty = 0;
+		reg_list[i].valid = 0;
+		reg_list[i].bitfield_desc = NULL;
+		reg_list[i].num_bitfields = 0;
+		reg_list[i].value = calloc(1, 4);
+		reg_list[i].arch_info = &arch_info[i];
+		reg_list[i].arch_type = etm_reg_arch_type;
+		reg_list[i].size = etm_reg_arch_size_info[i];
+		arch_info[i].addr = etm_reg_arch_info[i];
+		arch_info[i].jtag_info = jtag_info;
+	}
+	return reg_cache;
+}
+
+int etm_get_reg(reg_t *reg)
+{
+	if (etm_read_reg(reg) != ERROR_OK)
+	{
+		ERROR("BUG: error scheduling etm register read");
+		exit(-1);
+	}
+	
+	if (jtag_execute_queue() != ERROR_OK)
+	{
+		ERROR("register read failed");
+	}
+	
+	return ERROR_OK;
+}
+
+int etm_read_reg_w_check(reg_t *reg, u8* check_value, u8* check_mask)
+{
+	etm_reg_t *etm_reg = reg->arch_info;
+	u8 reg_addr = etm_reg->addr & 0x7f;
+	scan_field_t fields[3];
+	
+	DEBUG("%i", etm_reg->addr);
+
+	jtag_add_end_state(TAP_RTI);
+	arm_jtag_scann(etm_reg->jtag_info, 0x6);
+	arm_jtag_set_instr(etm_reg->jtag_info, etm_reg->jtag_info->intest_instr);
+	
+	fields[0].device = etm_reg->jtag_info->chain_pos;
+	fields[0].num_bits = 32;
+	fields[0].out_value = reg->value;
+	fields[0].out_mask = NULL;
+	fields[0].in_value = NULL;
+	fields[0].in_check_value = NULL;
+	fields[0].in_check_mask = NULL;
+	fields[0].in_handler = NULL;
+	fields[0].in_handler_priv = NULL;
+	
+	fields[1].device = etm_reg->jtag_info->chain_pos;
+	fields[1].num_bits = 7;
+	fields[1].out_value = malloc(1);
+	buf_set_u32(fields[1].out_value, 0, 7, reg_addr);
+	fields[1].out_mask = NULL;
+	fields[1].in_value = NULL;
+	fields[1].in_check_value = NULL;
+	fields[1].in_check_mask = NULL;
+	fields[1].in_handler = NULL;
+	fields[1].in_handler_priv = NULL;
+
+	fields[2].device = etm_reg->jtag_info->chain_pos;
+	fields[2].num_bits = 1;
+	fields[2].out_value = malloc(1);
+	buf_set_u32(fields[2].out_value, 0, 1, 0);
+	fields[2].out_mask = NULL;
+	fields[2].in_value = NULL;
+	fields[2].in_check_value = NULL;
+	fields[2].in_check_mask = NULL;
+	fields[2].in_handler = NULL;
+	fields[2].in_handler_priv = NULL;
+	
+	jtag_add_dr_scan(3, fields, -1);
+	
+	fields[0].in_value = reg->value;
+	fields[0].in_check_value = check_value;
+	fields[0].in_check_mask = check_mask;
+		
+	jtag_add_dr_scan(3, fields, -1);
+
+	free(fields[1].out_value);
+	free(fields[2].out_value);
+	
+	return ERROR_OK;
+}
+
+int etm_read_reg(reg_t *reg)
+{
+	return etm_read_reg_w_check(reg, NULL, NULL);	
+}
+
+int etm_set_reg(reg_t *reg, u32 value)
+{
+	if (etm_write_reg(reg, value) != ERROR_OK)
+	{
+		ERROR("BUG: error scheduling etm register write");
+		exit(-1);
+	}
+	
+	buf_set_u32(reg->value, 0, reg->size, value);
+	reg->valid = 1;
+	reg->dirty = 0;
+	
+	return ERROR_OK;
+}
+
+int etm_set_reg_w_exec(reg_t *reg, u32 value)
+{
+	etm_set_reg(reg, value);
+	
+	if (jtag_execute_queue() != ERROR_OK)
+	{
+		ERROR("register write failed");
+		exit(-1);
+	}
+	return ERROR_OK;
+}
+
+int etm_write_reg(reg_t *reg, u32 value)
+{
+	etm_reg_t *etm_reg = reg->arch_info;
+	u8 reg_addr = etm_reg->addr & 0x7f;
+	scan_field_t fields[3];
+	
+	DEBUG("%i: 0x%8.8x", etm_reg->addr, value);
+	
+	jtag_add_end_state(TAP_RTI);
+	arm_jtag_scann(etm_reg->jtag_info, 0x6);
+	arm_jtag_set_instr(etm_reg->jtag_info, etm_reg->jtag_info->intest_instr);
+	
+	fields[0].device = etm_reg->jtag_info->chain_pos;
+	fields[0].num_bits = 32;
+	fields[0].out_value = malloc(4);
+	buf_set_u32(fields[0].out_value, 0, 32, value);
+	fields[0].out_mask = NULL;
+	fields[0].in_value = NULL;
+	fields[0].in_check_value = NULL;
+	fields[0].in_check_mask = NULL;
+	fields[0].in_handler = NULL;
+	fields[0].in_handler_priv = NULL;
+	
+	fields[1].device = etm_reg->jtag_info->chain_pos;
+	fields[1].num_bits = 7;
+	fields[1].out_value = malloc(1);
+	buf_set_u32(fields[1].out_value, 0, 7, reg_addr);
+	fields[1].out_mask = NULL;
+	fields[1].in_value = NULL;
+	fields[1].in_check_value = NULL;
+	fields[1].in_check_mask = NULL;
+	fields[1].in_handler = NULL;
+	fields[1].in_handler_priv = NULL;
+
+	fields[2].device = etm_reg->jtag_info->chain_pos;
+	fields[2].num_bits = 1;
+	fields[2].out_value = malloc(1);
+	buf_set_u32(fields[2].out_value, 0, 1, 1);
+	fields[2].out_mask = NULL;
+	fields[2].in_value = NULL;
+	fields[2].in_check_value = NULL;
+	fields[2].in_check_mask = NULL;
+	fields[2].in_handler = NULL;
+	fields[2].in_handler_priv = NULL;
+	
+	jtag_add_dr_scan(3, fields, -1);
+	
+	free(fields[0].out_value);
+	free(fields[1].out_value);
+	free(fields[2].out_value);
+	
+	return ERROR_OK;
+}
+
+int etm_store_reg(reg_t *reg)
+{
+	return etm_write_reg(reg, buf_get_u32(reg->value, 0, reg->size));
+}
+
diff --git a/src/target/register.c b/src/target/register.c
index 7b98cfcf..182ff9a2 100644
--- a/src/target/register.c
+++ b/src/target/register.c
@@ -17,6 +17,10 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
 #include "register.h"
 
 #include "log.h"
diff --git a/src/target/target.c b/src/target/target.c
index 34dc60a7..8b9939af 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -17,7 +17,10 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
 #include "config.h"
+#endif
+
 #include "target.h"
 
 #include "log.h"
@@ -1507,7 +1510,7 @@ int handle_load_binary_command(struct command_context_s *cmd_ctx, char *cmd, cha
 	u8 *buffer;
 	u32 buf_cnt;
 	
-	struct timeval start, end;
+	struct timeval start, end, duration;
 		
 	target_t *target = get_current_target(cmd_ctx);
 
@@ -1526,7 +1529,7 @@ int handle_load_binary_command(struct command_context_s *cmd_ctx, char *cmd, cha
 		return ERROR_OK;
 	}
 
-	if (!(binary = fopen(args[0], "r")))
+	if (!(binary = fopen(args[0], "rb")))
 	{
 		ERROR("couldn't open %s: %s", args[0], strerror(errno));
 		command_print(cmd_ctx, "error accessing file %s", args[0]);
@@ -1550,7 +1553,8 @@ int handle_load_binary_command(struct command_context_s *cmd_ctx, char *cmd, cha
 
 	free(buffer);
 	
-	command_print(cmd_ctx, "downloaded %lli byte in %is %ius", (long long) binary_stat.st_size, end.tv_sec - start.tv_sec, end.tv_usec - start.tv_usec);
+	timeval_subtract(&duration, &end, &start);
+	command_print(cmd_ctx, "downloaded %lli byte in %is %ius", (long long) binary_stat.st_size, duration.tv_sec, duration.tv_usec);
 	
 	fclose(binary);
 
@@ -1576,7 +1580,7 @@ int handle_dump_binary_command(struct command_context_s *cmd_ctx, char *cmd, cha
 	address = strtoul(args[1], NULL, 0);
 	size = strtoul(args[2], NULL, 0);
 
-	if (!(binary = fopen(args[0], "w")))
+	if (!(binary = fopen(args[0], "wb")))
 	{
 		ERROR("couldn't open %s for writing: %s", args[0], strerror(errno));
 		command_print(cmd_ctx, "error accessing file %s", args[0]);
diff --git a/src/xsvf/Makefile.am b/src/xsvf/Makefile.am
index d9e80875..bd1b8fa1 100644
--- a/src/xsvf/Makefile.am
+++ b/src/xsvf/Makefile.am
@@ -1,4 +1,4 @@
-INCLUDES = -I$(top_srcdir)/src/gdb -I$(top_srcdir)/src/helper 	-I$(top_srcdir)/src/jtag $(all_includes)
+INCLUDES = -I$(top_srcdir)/src/server -I$(top_srcdir)/src/helper 	-I$(top_srcdir)/src/jtag $(all_includes)
 METASOURCES = AUTO
 noinst_LIBRARIES = libxsvf.a
 noinst_HEADERS = xsvf.h
diff --git a/src/xsvf/xsvf.c b/src/xsvf/xsvf.c
index 013803f6..3a15e2b7 100644
--- a/src/xsvf/xsvf.c
+++ b/src/xsvf/xsvf.c
@@ -17,6 +17,10 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
 #include "xsvf.h"
 
 #include "jtag.h"
@@ -28,7 +32,6 @@
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <fcntl.h>
-#include <netinet/in.h>
 #include <string.h>
 
 #include <sys/time.h>
@@ -111,6 +114,7 @@ int xsvf_read_xstates(int fd, enum tap_state *path, int max_path, int *path_len)
 int handle_xsvf_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
 	char c;
+	u8 buf4[4], buf2[2];
 	unsigned char uc, uc2;
 	unsigned int ui;
 	unsigned short us;
@@ -252,11 +256,11 @@ int handle_xsvf_command(struct command_context_s *cmd_ctx, char *cmd, char **arg
 				break;
 			case 0x04:	/* XRUNTEST */
 				DEBUG("XRUNTEST");
-				if (read(xsvf_fd, &ui, 4) < 0)
+				if (read(xsvf_fd, buf4, 4) < 0)
 					do_abort = 1;
 				else
 				{
-					xruntest = ntohl(ui);
+					xruntest = be_to_h_u32(buf4);
 				}
 				break;
 			case 0x07:	/* XREPEAT */
@@ -270,11 +274,11 @@ int handle_xsvf_command(struct command_context_s *cmd_ctx, char *cmd, char **arg
 				break;
 			case 0x08:	/* XSDRSIZE */
 				DEBUG("XSDRSIZE");
-				if (read(xsvf_fd, &ui, 4) < 0)
+				if (read(xsvf_fd, buf4, 4) < 0)
 					do_abort = 1;
 				else
 				{
-					xsdrsize = ntohl(ui);
+					xsdrsize = be_to_h_u32(buf4);
 					free(dr_out_buf);
 					free(dr_in_buf);
 					free(dr_in_mask);
@@ -408,12 +412,12 @@ int handle_xsvf_command(struct command_context_s *cmd_ctx, char *cmd, char **arg
 				break;
 			case 0x15:	/* XSIR2 */
 				DEBUG("XSIR2");
-				if (read(xsvf_fd, &us, 2) < 0)
+				if (read(xsvf_fd, buf2, 2) < 0)
 					do_abort = 1;
 				else
 				{
 					u8 *ir_buf;
-					us = ntohs(us);
+					us = be_to_h_u16(buf2);
 					ir_buf = malloc((us + 7) / 8);
 					if (xsvf_read_buffer(us, xsvf_fd, ir_buf) != ERROR_OK)
 						do_abort = 1;
@@ -449,12 +453,12 @@ int handle_xsvf_command(struct command_context_s *cmd_ctx, char *cmd, char **arg
 				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))
+				if ((read(xsvf_fd, &uc, 1) < 0) || (read(xsvf_fd, &uc2, 1) < 0) || (read(xsvf_fd, buf4, 4) < 0))
 					do_abort = 1;
 				else
 				{
 					jtag_add_statemove(xsvf_to_tap[uc]);
-					ui = ntohl(ui);
+					ui = be_to_h_u32(buf4);
 					jtag_add_sleep(ui);
 					jtag_add_statemove(xsvf_to_tap[uc2]);
 				}