- split fileio handling into fileio part and image handling

- reworked etm/etb into a generic etm part with trace capture drivers (currently only etb supported)
- added XScale debug handler binary to repository
- added Thumb disassembling (thanks to Vincent Palatin for this patch)
- added support for non-CFI compatible flashes to cfi driver (currently only SST39VFxxx devices supported)
This checkin is experimental, not suitable for general use


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

26 files changed, 2522 insertions, 938 deletions

diff --git a/src/flash/Makefile.am b/src/flash/Makefile.am
index 6368f626..8f99e05e 100644
--- a/src/flash/Makefile.am
+++ b/src/flash/Makefile.am
@@ -1,5 +1,5 @@
 INCLUDES = -I$(top_srcdir)/src/helper -I$(top_srcdir)/src/jtag -I$(top_srcdir)/src/target $(all_includes)
 METASOURCES = AUTO
 noinst_LIBRARIES = libflash.a
-libflash_a_SOURCES = flash.c lpc2000.c cfi.c at91sam7.c str7x.c str9x.c nand.c lpc3180_nand_controller.c
-noinst_HEADERS = flash.h lpc2000.h cfi.h at91sam7.h str7x.h str9x.h nand.h lpc3180_nand_controller.h
+libflash_a_SOURCES = flash.c lpc2000.c cfi.c non_cfi.c at91sam7.c str7x.c str9x.c nand.c lpc3180_nand_controller.c
+noinst_HEADERS = flash.h lpc2000.h cfi.h non_cfi.h at91sam7.h str7x.h str9x.h nand.h lpc3180_nand_controller.h
diff --git a/src/flash/cfi.c b/src/flash/cfi.c
index 83a8120a..69494b5f 100644
--- a/src/flash/cfi.c
+++ b/src/flash/cfi.c
@@ -66,17 +66,33 @@ flash_driver_t cfi_flash =
 	.info = cfi_info
 };
 
+cfi_unlock_addresses_t cfi_unlock_addresses[] =
+{
+	[CFI_UNLOCK_555_2AA] = { .unlock1 = 0x555, .unlock2 = 0x2aa },
+	[CFI_UNLOCK_5555_2AAA] = { .unlock1 = 0x5555, .unlock2 = 0x2aaa },
+};
+
 /* CFI fixups foward declarations */
+void cfi_fixup_non_cfi(flash_bank_t *flash, void *param);
 void cfi_fixup_0002_erase_regions(flash_bank_t *flash, void *param);
+void cfi_fixup_0002_unlock_addresses(flash_bank_t *flash, void *param);
 void cfi_fixup_atmel_reversed_erase_regions(flash_bank_t *flash, void *param);
 
 /* fixup after identifying JEDEC manufactuer and ID */
 cfi_fixup_t cfi_jedec_fixups[] = {
+	{CFI_MFR_SST, 0x00D4, cfi_fixup_non_cfi, NULL},
+	{CFI_MFR_SST, 0x00D5, cfi_fixup_non_cfi, NULL},
+	{CFI_MFR_SST, 0x00D6, cfi_fixup_non_cfi, NULL},
+	{CFI_MFR_SST, 0x00D7, cfi_fixup_non_cfi, NULL},
 	{0, 0, NULL, NULL}
 };
 
 /* fixup after reading cmdset 0002 primary query table */
 cfi_fixup_t cfi_0002_fixups[] = {
+	{CFI_MFR_SST, 0x00D4, cfi_fixup_0002_unlock_addresses, &cfi_unlock_addresses[CFI_UNLOCK_5555_2AAA]},
+	{CFI_MFR_SST, 0x00D5, cfi_fixup_0002_unlock_addresses, &cfi_unlock_addresses[CFI_UNLOCK_5555_2AAA]},
+	{CFI_MFR_SST, 0x00D6, cfi_fixup_0002_unlock_addresses, &cfi_unlock_addresses[CFI_UNLOCK_5555_2AAA]},
+	{CFI_MFR_SST, 0x00D7, cfi_fixup_0002_unlock_addresses, &cfi_unlock_addresses[CFI_UNLOCK_5555_2AAA]},
 	{CFI_MFR_ATMEL, 0x00C8, cfi_fixup_atmel_reversed_erase_regions, NULL},
 	{CFI_MFR_ANY, CFI_ID_ANY, cfi_fixup_0002_erase_regions, NULL},
 	{0, 0, NULL, NULL}
@@ -421,6 +437,11 @@ int cfi_read_spansion_pri_ext(flash_bank_t *bank)
 	
 	DEBUG("WP# protection 0x%x", pri_ext->TopBottom);
 	
+	/* default values for implementation specific workarounds */
+	pri_ext->_unlock1 = cfi_unlock_addresses[CFI_UNLOCK_555_2AA].unlock1;
+	pri_ext->_unlock2 = cfi_unlock_addresses[CFI_UNLOCK_555_2AA].unlock2;
+	pri_ext->_reversed_geometry = 0;
+	
 	return ERROR_OK;
 }
 
@@ -594,7 +615,9 @@ int cfi_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **
 	cfi_info = malloc(sizeof(cfi_flash_bank_t));
 	bank->driver_priv = cfi_info;
 	
-	cfi_info->x16_as_x8 = 1;
+	cfi_info->x16_as_x8 = 0;
+	cfi_info->jedec_probe = 0;
+	cfi_info->not_cfi = 0;
 	
 	cfi_info->target = get_target_by_num(strtoul(args[5], NULL, 0));
 	if (!cfi_info->target)
@@ -605,9 +628,13 @@ int cfi_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **
 
 	for (i = 6; i < argc; i++)
 	{
-		if (strcmp(args[i], "x16_as_x8") != 0)
+		if (strcmp(args[i], "x16_as_x8") == 0)
 		{
-			cfi_info->x16_as_x8 = 0;
+			cfi_info->x16_as_x8 = 1;
+		}
+		else if (strcmp(args[i], "jedec_probe") == 0)
+		{
+			cfi_info->jedec_probe = 1;
 		}
 	}
 	 	
@@ -665,19 +692,19 @@ int cfi_spansion_erase(struct flash_bank_s *bank, int first, int last)
 	for (i = first; i <= last; i++)
 	{
 		cfi_command(bank, 0xaa, command);
-		target->type->write_memory(target, flash_address(bank, 0, 0x555), bank->bus_width, 1, command);
+		target->type->write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command);
 			
 		cfi_command(bank, 0x55, command);
-		target->type->write_memory(target, flash_address(bank, 0, 0x2aa), bank->bus_width, 1, command);
+		target->type->write_memory(target, flash_address(bank, 0, pri_ext->_unlock2), bank->bus_width, 1, command);
 		
 		cfi_command(bank, 0x80, command);
-		target->type->write_memory(target, flash_address(bank, 0, 0x555), bank->bus_width, 1, command);
+		target->type->write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command);
 
 		cfi_command(bank, 0xaa, command);
-		target->type->write_memory(target, flash_address(bank, 0, 0x555), bank->bus_width, 1, command);
+		target->type->write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command);
 			
 		cfi_command(bank, 0x55, command);
-		target->type->write_memory(target, flash_address(bank, 0, 0x2aa), bank->bus_width, 1, command);
+		target->type->write_memory(target, flash_address(bank, 0, pri_ext->_unlock2), bank->bus_width, 1, command);
 		
 		cfi_command(bank, 0x30, command);
 		target->type->write_memory(target, flash_address(bank, i, 0x0), bank->bus_width, 1, command);
@@ -891,9 +918,10 @@ int cfi_intel_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address, u3
 	armv4_5_algorithm_t armv4_5_info;
 	working_area_t *source;
 	u32 buffer_size = 32768;
-	u8 write_command[CFI_MAX_BUS_WIDTH];
-	u8 busy_pattern[CFI_MAX_BUS_WIDTH];
-	u8 error_pattern[CFI_MAX_BUS_WIDTH];
+	u8 write_command_buf[CFI_MAX_BUS_WIDTH];
+	u8 busy_pattern_buf[CFI_MAX_BUS_WIDTH];
+	u8 error_pattern_buf[CFI_MAX_BUS_WIDTH];
+	u32 write_command_val, busy_pattern_val, error_pattern_val;
 	int retval;
 
 	/* algorithm register usage:
@@ -906,7 +934,7 @@ int cfi_intel_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address, u3
 	 * r6: error test pattern
 	 */
 	 
-	u32 word_32_code[] = {
+	static const u32 word_32_code[] = {
 		0xe4904004,   /* loop:	ldr r4, [r0], #4 */
 		0xe5813000,   /* 		str r3, [r1] */
 		0xe5814000,   /* 		str r4, [r1] */
@@ -923,7 +951,7 @@ int cfi_intel_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address, u3
 		0xeafffffe,   /* done:	b -2 */
 	};
 	
-	u32 word_16_code[] = {
+	static const u32 word_16_code[] = {
 		0xe0d040b2,   /* loop:	ldrh r4, [r0], #2 */
 		0xe1c130b0,   /* 		strh r3, [r1] */
 		0xe1c140b0,   /* 		strh r4, [r1] */
@@ -940,7 +968,7 @@ int cfi_intel_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address, u3
 		0xeafffffe,   /* done:	b -2 */
 	};
 	
-	u32 word_8_code[] = {
+	static const u32 word_8_code[] = {
 		0xe4d04001,   /* loop:	ldrb r4, [r0], #1 */
 		0xe5c13000,   /* 		strb r3, [r1] */
 		0xe5c14000,   /* 		strb r4, [r1] */
@@ -966,29 +994,37 @@ int cfi_intel_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address, u3
 	/* flash write code */
 	if (!cfi_info->write_algorithm)
 	{
+		u8 write_code_buf[14 * 4];
+		int i;
+			
 		if (target_alloc_working_area(target, 4 * 14, &cfi_info->write_algorithm) != ERROR_OK)
 		{
 			WARNING("no working area available, can't do block memory writes");
 			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 		};
-			
+		
 		/* write algorithm code to working area */
 		if (bank->bus_width == 1)
 		{
-			target_write_buffer(target, cfi_info->write_algorithm->address, 14 * 4, (u8*)word_8_code);
+			for (i = 0; i < 14; i++)
+				target_buffer_set_u32(target, write_code_buf + (i*4), word_8_code[i]);
 		}
 		else if (bank->bus_width == 2)
 		{
-			target_write_buffer(target, cfi_info->write_algorithm->address, 14 * 4, (u8*)word_16_code);
+			for (i = 0; i < 14; i++)
+				target_buffer_set_u32(target, write_code_buf + (i*4), word_16_code[i]);
 		}	
 		else if (bank->bus_width == 4)
 		{
-			target_write_buffer(target, cfi_info->write_algorithm->address, 14 * 4, (u8*)word_32_code);
+			for (i = 0; i < 14; i++)
+				target_buffer_set_u32(target, write_code_buf + (i*4), word_32_code[i]);
 		}
 		else
 		{
 			return ERROR_FLASH_OPERATION_FAILED;
 		}
+
+		target_write_buffer(target, cfi_info->write_algorithm->address, 14 * 4, write_code_buf);
 	}
 	
 	while (target_alloc_working_area(target, buffer_size, &source) != ERROR_OK)
@@ -1013,10 +1049,30 @@ int cfi_intel_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address, u3
 	init_reg_param(&reg_params[5], "r5", 32, PARAM_OUT);
 	init_reg_param(&reg_params[6], "r6", 32, PARAM_OUT);
 
-	cfi_command(bank, 0x40, write_command);
-	cfi_command(bank, 0x80, busy_pattern);
-	cfi_command(bank, 0x7e, error_pattern);
-
+	/* prepare command and status register patterns */
+	cfi_command(bank, 0x40, write_command_buf);
+	cfi_command(bank, 0x80, busy_pattern_buf);
+	cfi_command(bank, 0x7e, error_pattern_buf);
+	
+	if (bank->bus_width == 1)
+	{
+		write_command_val = write_command_buf[0];
+		busy_pattern_val = busy_pattern_buf[0];
+		error_pattern_val = error_pattern_buf[0];
+	}
+	else if (bank->bus_width == 2)
+	{
+		write_command_val = target_buffer_get_u16(target, write_command_buf);
+		busy_pattern_val = target_buffer_get_u16(target, busy_pattern_buf);
+		error_pattern_val = target_buffer_get_u16(target, error_pattern_buf);
+	}
+	else if (bank->bus_width == 4)
+	{
+		write_command_val = target_buffer_get_u32(target, write_command_buf);
+		busy_pattern_val = target_buffer_get_u32(target, busy_pattern_buf);
+		error_pattern_val = target_buffer_get_u32(target, error_pattern_buf);
+	}
+	
 	while (count > 0)
 	{
 		u32 thisrun_count = (count > buffer_size) ? buffer_size : count;
@@ -1026,11 +1082,9 @@ int cfi_intel_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address, u3
 		buf_set_u32(reg_params[0].value, 0, 32, source->address);
 		buf_set_u32(reg_params[1].value, 0, 32, address);
 		buf_set_u32(reg_params[2].value, 0, 32, thisrun_count / bank->bus_width);
-		buf_set_u32(reg_params[3].value, 0, 32, target_buffer_get_u32(target, write_command));
-		buf_set_u32(reg_params[5].value, 0, 32, target_buffer_get_u32(target, busy_pattern));
-		buf_set_u32(reg_params[6].value, 0, 32, target_buffer_get_u32(target, error_pattern));
-		buf_set_u32(reg_params[5].value, 0, 32, buf_get_u32(busy_pattern, 0, 32));
-		buf_set_u32(reg_params[6].value, 0, 32, buf_get_u32(error_pattern, 0, 32));
+		buf_set_u32(reg_params[3].value, 0, 32, write_command_val);
+		buf_set_u32(reg_params[5].value, 0, 32, busy_pattern_val);
+		buf_set_u32(reg_params[6].value, 0, 32, error_pattern_val);
 	
 		if ((retval = target->type->run_algorithm(target, 0, NULL, 7, reg_params, cfi_info->write_algorithm->address, cfi_info->write_algorithm->address + (13 * 4), 10000, &armv4_5_info)) != ERROR_OK)
 		{
@@ -1038,7 +1092,7 @@ int cfi_intel_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address, u3
 			return ERROR_FLASH_OPERATION_FAILED;
 		}
 	
-		if (buf_get_u32(reg_params[4].value, 0, 32) & target_buffer_get_u32(target, error_pattern))
+		if (buf_get_u32(reg_params[4].value, 0, 32) & error_pattern_val)
 		{
 			/* read status register (outputs debug inforation) */
 			cfi_intel_wait_status_busy(bank, 100);
@@ -1078,8 +1132,6 @@ int cfi_spansion_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address,
 	int i;
 	int retval;
 	int exit_code = ERROR_OK;
-	int code_size;
-	void *code_p;
 
 	/* input parameters - */
 	/*	R0 = source address */
@@ -1095,8 +1147,8 @@ int cfi_spansion_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address,
 	/* unlock registers - */
 	/*  R8 = unlock1_addr */
 	/*  R9 = unlock1_cmd */
-	/*  R10 = unlock1_addr */
-	/*  R11 = unlock1_cmd */
+	/*  R10 = unlock2_addr */
+	/*  R11 = unlock2_cmd */
 
 	u32 word_32_code[] = {
 						/* 00008100 <sp_32_code>:		*/
@@ -1207,36 +1259,47 @@ int cfi_spansion_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address,
 	/* flash write code */
 	if (!cfi_info->write_algorithm)
 	{
-		/* write algorithm code to working area */
+		u8 *code_p;
+
+		/* convert bus-width dependent algorithm code to correct endiannes */
 		if (bank->bus_width == 1)
 		{
-			code_size = sizeof(word_8_code);
-			code_p = word_8_code;
+			code_p = malloc(24 * 4);
+			
+			for (i = 0; i < 24; i++)
+				target_buffer_set_u32(target, code_p + (i*4), word_8_code[i]);
 		}
 		else if (bank->bus_width == 2)
 		{
-			code_size = sizeof(word_16_code);
-			code_p = word_16_code;
-		}
+			code_p = malloc(24 * 4);
+			
+			for (i = 0; i < 24; i++)
+				target_buffer_set_u32(target, code_p + (i*4), word_16_code[i]);
+		}	
 		else if (bank->bus_width == 4)
 		{
-			code_size = sizeof(word_32_code);
-			code_p = word_32_code;
+			code_p = malloc(24 * 4);
+			
+			for (i = 0; i < 24; i++)
+				target_buffer_set_u32(target, code_p + (i*4), word_32_code[i]);
 		}
 		else
 		{
 			return ERROR_FLASH_OPERATION_FAILED;
 		}
-
-		if (target_alloc_working_area(target, code_size,
-						      &cfi_info->write_algorithm) != ERROR_OK)
+		
+		/* allocate working area */
+		if (target_alloc_working_area(target, 24 * 4,
+			&cfi_info->write_algorithm) != ERROR_OK)
 		{
 			WARNING("no working area available, can't do block memory writes");
 			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 		}
-			
-		target_write_buffer(target, cfi_info->write_algorithm->address, 
-				    code_size, code_p);
+		
+		/* write algorithm code to working area */
+		target_write_buffer(target, cfi_info->write_algorithm->address, 24 * 4, code_p);
+		
+		free(code_p);
 	}
 	
 	while (target_alloc_working_area(target, buffer_size, &source) != ERROR_OK)
@@ -1277,14 +1340,14 @@ int cfi_spansion_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address,
 		buf_set_u32(reg_params[3].value, 0, 32, buf_get_u32(write_command, 0, 32));
 		cfi_command(bank, 0x80, write_command);
 		buf_set_u32(reg_params[4].value, 0, 32, buf_get_u32(write_command, 0, 32));
-		buf_set_u32(reg_params[6].value, 0, 32, flash_address(bank, 0, 0x555));
+		buf_set_u32(reg_params[6].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock1));
 		buf_set_u32(reg_params[7].value, 0, 32, 0xaa);
-		buf_set_u32(reg_params[8].value, 0, 32, flash_address(bank, 0, 0xaaa));
+		buf_set_u32(reg_params[8].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock2));
 		buf_set_u32(reg_params[9].value, 0, 32, 0x55);
 	
 		retval = target->type->run_algorithm(target, 0, NULL, 10, reg_params, 
 						     cfi_info->write_algorithm->address, 
-						     cfi_info->write_algorithm->address + (code_size - 4), 
+						     cfi_info->write_algorithm->address + ((24 * 4) - 4), 
 						     10000, &armv4_5_info);
 
 		status = buf_get_u32(reg_params[5].value, 0, 32);
@@ -1301,6 +1364,8 @@ int cfi_spansion_write_block(struct flash_bank_s *bank, u8 *buffer, u32 address,
 		count -= thisrun_count;
 	}
 	
+	target_free_working_area(target, source);
+	
 	destroy_reg_param(&reg_params[0]);
 	destroy_reg_param(&reg_params[1]);
 	destroy_reg_param(&reg_params[2]);
@@ -1347,13 +1412,13 @@ int cfi_spansion_write_word(struct flash_bank_s *bank, u8 *word, u32 address)
 	u8 command[8];
 	
 	cfi_command(bank, 0xaa, command);
-	target->type->write_memory(target, flash_address(bank, 0, 0x555), bank->bus_width, 1, command);
+	target->type->write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command);
 	
 	cfi_command(bank, 0x55, command);
-	target->type->write_memory(target, flash_address(bank, 0, 0x2aa), bank->bus_width, 1, command);
+	target->type->write_memory(target, flash_address(bank, 0, pri_ext->_unlock2), bank->bus_width, 1, command);
 	
 	cfi_command(bank, 0xa0, command);
-	target->type->write_memory(target, flash_address(bank, 0, 0x555), bank->bus_width, 1, command);
+	target->type->write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command);
 
 	target->type->write_memory(target, address, bank->bus_width, 1, word);
 	
@@ -1554,6 +1619,16 @@ void cfi_fixup_0002_erase_regions(flash_bank_t *bank, void *param)
 	}
 }
 
+void cfi_fixup_0002_unlock_addresses(flash_bank_t *bank, void *param)
+{
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	cfi_spansion_pri_ext_t *pri_ext = cfi_info->pri_ext;
+	cfi_unlock_addresses_t *unlock_addresses = param;
+	
+	pri_ext->_unlock1 = unlock_addresses->unlock1;
+	pri_ext->_unlock2 = unlock_addresses->unlock2;
+}
+
 int cfi_probe(struct flash_bank_s *bank)
 {
 	cfi_flash_bank_t *cfi_info = bank->driver_priv;
@@ -1563,14 +1638,25 @@ int cfi_probe(struct flash_bank_s *bank)
 	int i;
 	int sector = 0;
 	u32 offset = 0;
-		
+	u32 unlock1 = 0x555;
+	u32 unlock2 = 0x2aa;
+	
+	/* JEDEC standard JESD21C uses 0x5555 and 0x2aaa as unlock addresses,
+	 * while CFI compatible AMD/Spansion flashes use 0x555 and 0x2aa
+	 */
+	if (cfi_info->jedec_probe)
+	{
+		unlock1 = 0x5555;
+		unlock2 = 0x2aaa;
+	}
+	
 	/* switch to read identifier codes mode ("AUTOSELECT") */
 	cfi_command(bank, 0xaa, command);
-	target->type->write_memory(target, flash_address(bank, 0, 0x555), bank->bus_width, 1, command);
+	target->type->write_memory(target, flash_address(bank, 0, unlock1), bank->bus_width, 1, command);
 	cfi_command(bank, 0x55, command);
-	target->type->write_memory(target, flash_address(bank, 0, 0x2aa), bank->bus_width, 1, command);
+	target->type->write_memory(target, flash_address(bank, 0, unlock2), bank->bus_width, 1, command);
 	cfi_command(bank, 0x90, command);
-	target->type->write_memory(target, flash_address(bank, 0, 0x555), bank->bus_width, 1, command);
+	target->type->write_memory(target, flash_address(bank, 0, unlock1), bank->bus_width, 1, command);
 
 	if (bank->chip_width == 1)
 	{
@@ -1594,105 +1680,132 @@ int cfi_probe(struct flash_bank_s *bank)
 
 	cfi_fixup(bank, cfi_jedec_fixups);
 
-	/* enter CFI query mode
-	 * according to JEDEC Standard No. 68.01,
-	 * a single bus sequence with address = 0x55, data = 0x98 should put
-	 * the device into CFI query mode.
-	 * 
-	 * SST flashes clearly violate this, and we will consider them incompatbile for now
+	/* query only if this is a CFI compatible flash,
+	 * otherwise the relevant info has already been filled in
 	 */
-	cfi_command(bank, 0x98, command);
-	target->type->write_memory(target, flash_address(bank, 0, 0x55), bank->bus_width, 1, command);
-	
-	cfi_info->qry[0] = cfi_query_u8(bank, 0, 0x10);
-	cfi_info->qry[1] = cfi_query_u8(bank, 0, 0x11);
-	cfi_info->qry[2] = cfi_query_u8(bank, 0, 0x12);
-	
-	DEBUG("CFI qry returned: 0x%2.2x 0x%2.2x 0x%2.2x", cfi_info->qry[0], cfi_info->qry[1], cfi_info->qry[2]);
-	
-	if ((cfi_info->qry[0] != 'Q') || (cfi_info->qry[1] != 'R') || (cfi_info->qry[2] != 'Y'))
+	if (cfi_info->not_cfi == 0)
 	{
+		/* enter CFI query mode
+		 * according to JEDEC Standard No. 68.01,
+		 * a single bus sequence with address = 0x55, data = 0x98 should put
+		 * the device into CFI query mode.
+		 * 
+		 * SST flashes clearly violate this, and we will consider them incompatbile for now
+		 */
+		cfi_command(bank, 0x98, command);
+		target->type->write_memory(target, flash_address(bank, 0, 0x55), bank->bus_width, 1, command);
+		
+		cfi_info->qry[0] = cfi_query_u8(bank, 0, 0x10);
+		cfi_info->qry[1] = cfi_query_u8(bank, 0, 0x11);
+		cfi_info->qry[2] = cfi_query_u8(bank, 0, 0x12);
+		
+		DEBUG("CFI qry returned: 0x%2.2x 0x%2.2x 0x%2.2x", cfi_info->qry[0], cfi_info->qry[1], cfi_info->qry[2]);
+		
+		if ((cfi_info->qry[0] != 'Q') || (cfi_info->qry[1] != 'R') || (cfi_info->qry[2] != 'Y'))
+		{
+			cfi_command(bank, 0xf0, command);
+			target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+			cfi_command(bank, 0xff, command);
+			target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
+			return ERROR_FLASH_BANK_INVALID;
+		}
+		
+		cfi_info->pri_id = cfi_query_u16(bank, 0, 0x13);
+		cfi_info->pri_addr = cfi_query_u16(bank, 0, 0x15);
+		cfi_info->alt_id = cfi_query_u16(bank, 0, 0x17);
+		cfi_info->alt_addr = cfi_query_u16(bank, 0, 0x19);
+		
+		DEBUG("qry: '%c%c%c', pri_id: 0x%4.4x, pri_addr: 0x%4.4x, alt_id: 0x%4.4x, alt_addr: 0x%4.4x", cfi_info->qry[0], cfi_info->qry[1], cfi_info->qry[2], cfi_info->pri_id, cfi_info->pri_addr, cfi_info->alt_id, cfi_info->alt_addr);
+		
+		cfi_info->vcc_min = cfi_query_u8(bank, 0, 0x1b);
+		cfi_info->vcc_max = cfi_query_u8(bank, 0, 0x1c);
+		cfi_info->vpp_min = cfi_query_u8(bank, 0, 0x1d);
+		cfi_info->vpp_max = cfi_query_u8(bank, 0, 0x1e);
+		cfi_info->word_write_timeout_typ = cfi_query_u8(bank, 0, 0x1f);
+		cfi_info->buf_write_timeout_typ = cfi_query_u8(bank, 0, 0x20);
+		cfi_info->block_erase_timeout_typ = cfi_query_u8(bank, 0, 0x21);
+		cfi_info->chip_erase_timeout_typ = cfi_query_u8(bank, 0, 0x22);
+		cfi_info->word_write_timeout_max = cfi_query_u8(bank, 0, 0x23);
+		cfi_info->buf_write_timeout_max = cfi_query_u8(bank, 0, 0x24);
+		cfi_info->block_erase_timeout_max = cfi_query_u8(bank, 0, 0x25);
+		cfi_info->chip_erase_timeout_max = cfi_query_u8(bank, 0, 0x26);
+		
+		DEBUG("Vcc min: %1.1x.%1.1x, Vcc max: %1.1x.%1.1x, Vpp min: %1.1x.%1.1x, Vpp max: %1.1x.%1.1x",
+			(cfi_info->vcc_min & 0xf0) >> 4, cfi_info->vcc_min & 0x0f,
+			(cfi_info->vcc_max & 0xf0) >> 4, cfi_info->vcc_max & 0x0f,
+			(cfi_info->vpp_min & 0xf0) >> 4, cfi_info->vpp_min & 0x0f,
+			(cfi_info->vpp_max & 0xf0) >> 4, cfi_info->vpp_max & 0x0f);
+		DEBUG("typ. word write timeout: %u, typ. buf write timeout: %u, typ. block erase timeout: %u, typ. chip erase timeout: %u", 1 << cfi_info->word_write_timeout_typ, 1 << cfi_info->buf_write_timeout_typ,
+			1 << cfi_info->block_erase_timeout_typ, 1 << cfi_info->chip_erase_timeout_typ);
+		DEBUG("max. word write timeout: %u, max. buf write timeout: %u, max. block erase timeout: %u, max. chip erase timeout: %u", (1 << cfi_info->word_write_timeout_max) * (1 << cfi_info->word_write_timeout_typ),
+			(1 << cfi_info->buf_write_timeout_max) * (1 << cfi_info->buf_write_timeout_typ),
+			(1 << cfi_info->block_erase_timeout_max) * (1 << cfi_info->block_erase_timeout_typ),
+			(1 << cfi_info->chip_erase_timeout_max) * (1 << cfi_info->chip_erase_timeout_typ));
+		
+		cfi_info->dev_size = cfi_query_u8(bank, 0, 0x27);
+		cfi_info->interface_desc = cfi_query_u16(bank, 0, 0x28);
+		cfi_info->max_buf_write_size = cfi_query_u16(bank, 0, 0x2a);
+		cfi_info->num_erase_regions = cfi_query_u8(bank, 0, 0x2c);
+		
+		DEBUG("size: 0x%x, interface desc: %i, max buffer write size: %x", 1 << cfi_info->dev_size, cfi_info->interface_desc, (1 << cfi_info->max_buf_write_size));
+		
+		if (((1 << cfi_info->dev_size) * bank->bus_width / bank->chip_width) != bank->size)
+		{
+			WARNING("configuration specifies 0x%x size, but a 0x%x size flash was found", bank->size, 1 << cfi_info->dev_size);
+		}
+		
+		if (cfi_info->num_erase_regions)
+		{
+			cfi_info->erase_region_info = malloc(4 * cfi_info->num_erase_regions);
+			for (i = 0; i < cfi_info->num_erase_regions; i++)
+			{
+				cfi_info->erase_region_info[i] = cfi_query_u32(bank, 0, 0x2d + (4 * i));
+				DEBUG("erase region[%i]: %i blocks of size 0x%x", i, (cfi_info->erase_region_info[i] & 0xffff) + 1, (cfi_info->erase_region_info[i] >> 16) * 256);
+			}
+		}
+		else
+		{
+			cfi_info->erase_region_info = NULL;
+		}
+			
+		/* We need to read the primary algorithm extended query table before calculating
+		 * the sector layout to be able to apply fixups
+		 */	
+		switch(cfi_info->pri_id)
+		{
+			/* Intel command set (standard and extended) */
+			case 0x0001:
+			case 0x0003:
+				cfi_read_intel_pri_ext(bank);
+				break;
+			/* AMD/Spansion, Atmel, ... command set */
+			case 0x0002:
+				cfi_read_0002_pri_ext(bank);
+				break;
+			default:
+				ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
+				break;
+		}
+		
+		/* return to read array mode
+		 * we use both reset commands, as some Intel flashes fail to recognize the 0xF0 command
+		 */
 		cfi_command(bank, 0xf0, command);
 		target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
 		cfi_command(bank, 0xff, command);
 		target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
-		return ERROR_FLASH_BANK_INVALID;
-	}
-	
-	cfi_info->pri_id = cfi_query_u16(bank, 0, 0x13);
-	cfi_info->pri_addr = cfi_query_u16(bank, 0, 0x15);
-	cfi_info->alt_id = cfi_query_u16(bank, 0, 0x17);
-	cfi_info->alt_addr = cfi_query_u16(bank, 0, 0x19);
-	
-	DEBUG("qry: '%c%c%c', pri_id: 0x%4.4x, pri_addr: 0x%4.4x, alt_id: 0x%4.4x, alt_addr: 0x%4.4x", cfi_info->qry[0], cfi_info->qry[1], cfi_info->qry[2], cfi_info->pri_id, cfi_info->pri_addr, cfi_info->alt_id, cfi_info->alt_addr);
-	
-	cfi_info->vcc_min = cfi_query_u8(bank, 0, 0x1b);
-	cfi_info->vcc_max = cfi_query_u8(bank, 0, 0x1c);
-	cfi_info->vpp_min = cfi_query_u8(bank, 0, 0x1d);
-	cfi_info->vpp_max = cfi_query_u8(bank, 0, 0x1e);
-	cfi_info->word_write_timeout_typ = cfi_query_u8(bank, 0, 0x1f);
-	cfi_info->buf_write_timeout_typ = cfi_query_u8(bank, 0, 0x20);
-	cfi_info->block_erase_timeout_typ = cfi_query_u8(bank, 0, 0x21);
-	cfi_info->chip_erase_timeout_typ = cfi_query_u8(bank, 0, 0x22);
-	cfi_info->word_write_timeout_max = cfi_query_u8(bank, 0, 0x23);
-	cfi_info->buf_write_timeout_max = cfi_query_u8(bank, 0, 0x24);
-	cfi_info->block_erase_timeout_max = cfi_query_u8(bank, 0, 0x25);
-	cfi_info->chip_erase_timeout_max = cfi_query_u8(bank, 0, 0x26);
-	
-	DEBUG("Vcc min: %1.1x.%1.1x, Vcc max: %1.1x.%1.1x, Vpp min: %1.1x.%1.1x, Vpp max: %1.1x.%1.1x",
-		(cfi_info->vcc_min & 0xf0) >> 4, cfi_info->vcc_min & 0x0f,
-		(cfi_info->vcc_max & 0xf0) >> 4, cfi_info->vcc_max & 0x0f,
-		(cfi_info->vpp_min & 0xf0) >> 4, cfi_info->vpp_min & 0x0f,
-		(cfi_info->vpp_max & 0xf0) >> 4, cfi_info->vpp_max & 0x0f);
-	DEBUG("typ. word write timeout: %u, typ. buf write timeout: %u, typ. block erase timeout: %u, typ. chip erase timeout: %u", 1 << cfi_info->word_write_timeout_typ, 1 << cfi_info->buf_write_timeout_typ,
-		1 << cfi_info->block_erase_timeout_typ, 1 << cfi_info->chip_erase_timeout_typ);
-	DEBUG("max. word write timeout: %u, max. buf write timeout: %u, max. block erase timeout: %u, max. chip erase timeout: %u", (1 << cfi_info->word_write_timeout_max) * (1 << cfi_info->word_write_timeout_typ),
-		(1 << cfi_info->buf_write_timeout_max) * (1 << cfi_info->buf_write_timeout_typ),
-		(1 << cfi_info->block_erase_timeout_max) * (1 << cfi_info->block_erase_timeout_typ),
-		(1 << cfi_info->chip_erase_timeout_max) * (1 << cfi_info->chip_erase_timeout_typ));
-	
-	cfi_info->dev_size = cfi_query_u8(bank, 0, 0x27);
-	cfi_info->interface_desc = cfi_query_u16(bank, 0, 0x28);
-	cfi_info->max_buf_write_size = cfi_query_u16(bank, 0, 0x2a);
-	cfi_info->num_erase_regions = cfi_query_u8(bank, 0, 0x2c);
-	
-	DEBUG("size: 0x%x, interface desc: %i, max buffer write size: %x", 1 << cfi_info->dev_size, cfi_info->interface_desc, (1 << cfi_info->max_buf_write_size));
-	
-	if (((1 << cfi_info->dev_size) * bank->bus_width / bank->chip_width) != bank->size)
-	{
-		WARNING("configuration specifies 0x%x size, but a 0x%x size flash was found", bank->size, 1 << cfi_info->dev_size);
 	}
 	
-	if (cfi_info->num_erase_regions)
-	{
-		cfi_info->erase_region_info = malloc(4 * cfi_info->num_erase_regions);
-		for (i = 0; i < cfi_info->num_erase_regions; i++)
-		{
-			cfi_info->erase_region_info[i] = cfi_query_u32(bank, 0, 0x2d + (4 * i));
-			DEBUG("erase region[%i]: %i blocks of size 0x%x", i, (cfi_info->erase_region_info[i] & 0xffff) + 1, (cfi_info->erase_region_info[i] >> 16) * 256);
-			
-			num_sectors += (cfi_info->erase_region_info[i] & 0xffff) + 1;
-		}
-	}
-	else
-	{
-		cfi_info->erase_region_info = NULL;
-	}
-		
-	/* We need to read the primary algorithm extended query table before calculating
-	 * the sector layout to be able to apply fixups
-	 */	
+	/* apply fixups depending on the primary command set */
 	switch(cfi_info->pri_id)
 	{
 		/* Intel command set (standard and extended) */
 		case 0x0001:
 		case 0x0003:
-			cfi_read_intel_pri_ext(bank);
 			cfi_fixup(bank, cfi_0001_fixups);
 			break;
 		/* AMD/Spansion, Atmel, ... command set */
-        case 0x0002:
-			cfi_read_0002_pri_ext(bank);
+		case 0x0002:
 			cfi_fixup(bank, cfi_0002_fixups);
 			break;
 		default:
@@ -1713,6 +1826,11 @@ int cfi_probe(struct flash_bank_s *bank)
 	}
 	else
 	{
+		for (i = 0; i < cfi_info->num_erase_regions; i++)
+		{
+			num_sectors += (cfi_info->erase_region_info[i] & 0xffff) + 1;
+		}
+		
 		bank->num_sectors = num_sectors;
 		bank->sectors = malloc(sizeof(flash_sector_t) * num_sectors);
 		
@@ -1731,14 +1849,6 @@ int cfi_probe(struct flash_bank_s *bank)
 		}
 	}
 	
-	/* return to read array mode
-	 * we use both reset commands, as some Intel flashes fail to recognize the 0xF0 command
-	 */
-	cfi_command(bank, 0xf0, command);
-	target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
-	cfi_command(bank, 0xff, command);
-	target->type->write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command);
-	
 	return ERROR_OK;
 }
 
@@ -1893,13 +2003,13 @@ int cfi_spansion_protect_check(struct flash_bank_s *bank)
 	int i;
 	
 	cfi_command(bank, 0xaa, command);
-	target->type->write_memory(target, flash_address(bank, 0, 0x555), bank->bus_width, 1, command);
+	target->type->write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command);
 	
 	cfi_command(bank, 0x55, command);
-	target->type->write_memory(target, flash_address(bank, 0, 0x2aa), bank->bus_width, 1, command);
+	target->type->write_memory(target, flash_address(bank, 0, pri_ext->_unlock2), bank->bus_width, 1, command);
 	
 	cfi_command(bank, 0x90, command);
-	target->type->write_memory(target, flash_address(bank, 0, 0x555), bank->bus_width, 1, command);
+	target->type->write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command);
 
 	for (i = 0; i < bank->num_sectors; i++)
 	{
diff --git a/src/flash/cfi.h b/src/flash/cfi.h
index fa53f0d8..b4e3ab22 100644
--- a/src/flash/cfi.h
+++ b/src/flash/cfi.h
@@ -30,10 +30,12 @@ typedef struct cfi_flash_bank_s
 	working_area_t *erase_check_algorithm;
 	
 	int x16_as_x8;
+	int jedec_probe;
+	int not_cfi;
 	
 	u16 manufacturer;
 	u16 device_id;
-		
+	
 	char qry[3];
 	
 	/* identification string */
@@ -108,6 +110,8 @@ typedef struct cfi_spansion_pri_ext_s
 	u8  VppMax;
 	u8  TopBottom;
 	int _reversed_geometry;
+	u32 _unlock1;
+	u32 _unlock2;
 } cfi_spansion_pri_ext_t;
 
 /* Atmel primary extended query table as defined for and used by
@@ -124,6 +128,17 @@ typedef struct cfi_atmel_pri_ext_s
 	u8 page_mode;
 } cfi_atmel_pri_ext_t;
 
+enum {
+	CFI_UNLOCK_555_2AA,
+	CFI_UNLOCK_5555_2AAA,
+};
+
+typedef struct cfi_unlock_addresses_s
+{
+	u32 unlock1;
+	u32 unlock2;
+} cfi_unlock_addresses_t;
+
 typedef struct cfi_fixup_s
 {
 	u16 mfr;
@@ -135,6 +150,7 @@ typedef struct cfi_fixup_s
 #define CFI_MFR_AMD		0x0001
 #define CFI_MFR_ATMEL	0x001F
 #define CFI_MFR_ST		0x0020	/* STMicroelectronics */
+#define CFI_MFR_SST		0x00BF
 
 #define CFI_MFR_ANY		0xffff
 #define CFI_ID_ANY		0xffff
diff --git a/src/flash/flash.c b/src/flash/flash.c
index f5c83f80..6af29825 100644
--- a/src/flash/flash.c
+++ b/src/flash/flash.c
@@ -35,6 +35,7 @@
 #include <errno.h>
 
 #include <fileio.h>
+#include <image.h>
 
 /* command handlers */
 int handle_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
@@ -493,9 +494,7 @@ int handle_flash_write_command(struct command_context_s *cmd_ctx, char *cmd, cha
 	u8 *buffer;
 	u32 buf_cnt;
 
-	fileio_t file;
-	fileio_image_t image_info;
-	enum fileio_sec_type sec_type;
+	image_t image;
 	
 	duration_t duration;
 	char *duration_text;
@@ -511,7 +510,12 @@ int handle_flash_write_command(struct command_context_s *cmd_ctx, char *cmd, cha
 	
 	duration_start_measure(&duration);
 	
-	fileio_identify_image_type(&sec_type, (argc == 4) ? args[3] : NULL);
+	identify_image_type(&image.type, (argc == 4) ? args[3] : NULL);
+
+	image.base_address_set = 1;
+	image.base_address = strtoul(args[1], NULL, 0);
+	
+	image.start_address_set = 0;
 	
 	offset = strtoul(args[2], NULL, 0);
 	p = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
@@ -521,20 +525,16 @@ int handle_flash_write_command(struct command_context_s *cmd_ctx, char *cmd, cha
 		return ERROR_OK;
 	}
 	
-	image_info.base_address = strtoul(args[2], NULL, 0);
-	image_info.has_start_address = 0;
-
-	if (fileio_open(&file, args[1], FILEIO_READ, 
-		FILEIO_IMAGE, &image_info, sec_type) != ERROR_OK)
+	if (image_open(&image, args[1], FILEIO_READ) != ERROR_OK)
 	{
-		command_print(cmd_ctx, "flash write error: %s", file.error_str);
+		command_print(cmd_ctx, "flash write error: %s", image.error_str);
 		return ERROR_OK;
 	}
 	
-	binary_size = file.size;
+	binary_size = image.size;
 	buffer = malloc(binary_size);
 
-	fileio_read(&file, binary_size, buffer, &buf_cnt);
+	image_read(&image, binary_size, buffer, &buf_cnt);
 	
 	if ((retval = p->driver->write(p, buffer, offset, buf_cnt)) != ERROR_OK)
 	{
@@ -571,12 +571,12 @@ int handle_flash_write_command(struct command_context_s *cmd_ctx, char *cmd, cha
 	{
 		duration_stop_measure(&duration, &duration_text);
 		command_print(cmd_ctx, "wrote file %s to flash bank %i at offset 0x%8.8x in %s",
-			file.url, strtoul(args[0], NULL, 0), offset, duration_text);
+			args[1], strtoul(args[0], NULL, 0), offset, duration_text);
 		free(duration_text);
 	}
 	
 	free(buffer);
-	fileio_close(&file);
+	image_close(&image);
 	
 	return ERROR_OK;
 }
diff --git a/src/flash/nand.c b/src/flash/nand.c
index e0dfa22f..38a70749 100644
--- a/src/flash/nand.c
+++ b/src/flash/nand.c
@@ -38,6 +38,7 @@
 #include "flash.h"
 #include "time_support.h"
 #include "fileio.h"
+#include "image.h"
 
 int nand_register_commands(struct command_context_s *cmd_ctx);
 int handle_nand_list_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
@@ -1163,10 +1164,8 @@ int handle_nand_write_command(struct command_context_s *cmd_ctx, char *cmd, char
 	u32 buf_cnt;
 	enum oob_formats oob_format = NAND_OOB_NONE;
 	
-	fileio_t file;
-	fileio_image_t image_info;
-	int sec_type_identified = 0;
-	enum fileio_sec_type sec_type;
+	image_t image;
+	int image_type_identified = 0;
 	
 	duration_t duration;
 	char *duration_text;
@@ -1201,9 +1200,9 @@ int handle_nand_write_command(struct command_context_s *cmd_ctx, char *cmd, char
 					oob_format |= NAND_OOB_RAW | NAND_OOB_ONLY;
 				else
 				{
-					if (fileio_identify_image_type(&sec_type, args[i]) == ERROR_OK)
+					if (identify_image_type(&image.type, args[i]) == ERROR_OK)
 					{
-						sec_type_identified = 1;
+						image_type_identified = 1;
 					}
 					else
 					{
@@ -1214,27 +1213,27 @@ int handle_nand_write_command(struct command_context_s *cmd_ctx, char *cmd, char
 		}
 		
 		/* if no image type option was encountered, set the default */
-		if (!sec_type_identified)
+		if (!image_type_identified)
 		{
 			
-			fileio_identify_image_type(&sec_type, NULL);
-			sec_type_identified = 1;
+			identify_image_type(&image.type, NULL);
+			image_type_identified = 1;
 		}
 
-		image_info.base_address = strtoul(args[2], NULL, 0);
-		image_info.has_start_address = 0;
+		image.base_address_set = 1;
+		image.base_address = strtoul(args[2], NULL, 0);
+		image.start_address_set = 0;
 	
-		if (fileio_open(&file, args[1], FILEIO_READ, 
-			FILEIO_IMAGE, &image_info, sec_type) != ERROR_OK)
+		if (image_open(&image, args[1], FILEIO_READ) != ERROR_OK)
 		{
-			command_print(cmd_ctx, "flash write error: %s", file.error_str);
+			command_print(cmd_ctx, "flash write error: %s", image.error_str);
 			return ERROR_OK;
 		}
 	
 		/* the offset might have been overwritten by the image base address */
-		offset = image_info.base_address;
+		offset = image.base_address;
 		
-		buf_cnt = binary_size = file.size;
+		buf_cnt = binary_size = image.size;
 		
 		if (!(oob_format & NAND_OOB_ONLY))
 		{
@@ -1263,7 +1262,7 @@ int handle_nand_write_command(struct command_context_s *cmd_ctx, char *cmd, char
 			
 			if (page)
 			{
-				fileio_read(&file, page_size, page, &size_read);
+				image_read(&image, page_size, page, &size_read);
 				buf_cnt -= size_read;
 				if (size_read < page_size)
 				{
@@ -1273,7 +1272,7 @@ int handle_nand_write_command(struct command_context_s *cmd_ctx, char *cmd, char
 				
 			if (oob)
 			{
-				fileio_read(&file, oob_size, oob, &size_read);
+				image_read(&image, oob_size, oob, &size_read);
 				buf_cnt -= size_read;
 				if (size_read < oob_size)
 				{
@@ -1284,7 +1283,7 @@ int handle_nand_write_command(struct command_context_s *cmd_ctx, char *cmd, char
 			if (nand_write_page(p, offset / p->page_size, page, page_size, oob, oob_size) != ERROR_OK)
 			{
 				command_print(cmd_ctx, "failed writing file %s to NAND flash %s at offset 0x%8.8x",
-					file.url, args[0], offset);
+					args[1], args[0], offset);
 				return ERROR_OK;
 			}
 			offset += page_size;
@@ -1292,7 +1291,7 @@ int handle_nand_write_command(struct command_context_s *cmd_ctx, char *cmd, char
 
 		duration_stop_measure(&duration, &duration_text);
 		command_print(cmd_ctx, "wrote file %s to NAND flash %s at offset 0x%8.8x in %s",
-			file.url, args[0], image_info.base_address, duration_text);
+			args[1], args[0], image.base_address, duration_text);
 		free(duration_text);
 	}
 	else
@@ -1318,8 +1317,7 @@ int handle_nand_dump_command(struct command_context_s *cmd_ctx, char *cmd, char
 	{
 		if (p->device)
 		{
-			fileio_t file;
-			fileio_image_t image_info;
+			fileio_t fileio;
 			duration_t duration;
 			char *duration_text;
 			int retval;
@@ -1367,14 +1365,10 @@ int handle_nand_dump_command(struct command_context_s *cmd_ctx, char *cmd, char
 					oob_size = 64;
 				oob = malloc(oob_size);
 			}
-
-			image_info.base_address = address;
-			image_info.has_start_address = 0;
 			
-			if (fileio_open(&file, args[1], FILEIO_WRITE, 
-				FILEIO_IMAGE, &image_info, FILEIO_PLAIN) != ERROR_OK)
+			if (fileio_open(&fileio, args[1], FILEIO_WRITE, FILEIO_BINARY) != ERROR_OK)
 			{
-				command_print(cmd_ctx, "dump_image error: %s", file.error_str);
+				command_print(cmd_ctx, "dump_image error: %s", fileio.error_str);
 				return ERROR_OK;
 			}
 	
@@ -1391,13 +1385,13 @@ int handle_nand_dump_command(struct command_context_s *cmd_ctx, char *cmd, char
 				
 				if (page)
 				{
-					fileio_write(&file, page_size, page, &size_written);
+					fileio_write(&fileio, page_size, page, &size_written);
 					bytes_done += page_size;
 				}
 					
 				if (oob)
 				{
-					fileio_write(&file, oob_size, oob, &size_written);
+					fileio_write(&fileio, oob_size, oob, &size_written);
 					bytes_done += oob_size;
 				}
 					
@@ -1411,10 +1405,10 @@ int handle_nand_dump_command(struct command_context_s *cmd_ctx, char *cmd, char
 			if (oob)
 				free(oob);
 			
-			fileio_close(&file);
+			fileio_close(&fileio);
 
 			duration_stop_measure(&duration, &duration_text);
-			command_print(cmd_ctx, "dumped %lli byte in %s", file.size, duration_text);
+			command_print(cmd_ctx, "dumped %lli byte in %s", fileio.size, duration_text);
 			free(duration_text);
 		}
 		else
diff --git a/src/flash/non_cfi.c b/src/flash/non_cfi.c
new file mode 100644
index 00000000..3a74ff92
--- /dev/null
+++ b/src/flash/non_cfi.c
@@ -0,0 +1,175 @@
+/***************************************************************************
+ *   Copyright (C) 2007 by Dominic Rath                                    *
+ *   Dominic.Rath@gmx.de                                                   *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <stdlib.h>
+
+#include "log.h"
+
+#include "flash.h"
+#include "cfi.h"
+#include "non_cfi.h"
+
+/* non-CFI compatible flashes */
+non_cfi_t non_cfi_flashes[] = {
+	{
+		.mfr = CFI_MFR_SST,
+		.id = 0xd4,
+		.pri_id = 0x02,
+		.dev_size = 0x10,
+		.interface_desc = 0x0,
+		.max_buf_write_size = 0x0,
+		.num_erase_regions = 1,
+		.erase_region_info =
+		{
+			0x0010000f,
+			0x00000000
+		}
+	},
+	{
+		.mfr = CFI_MFR_SST,
+		.id = 0xd5,
+		.pri_id = 0x02,
+		.dev_size = 0x11,
+		.interface_desc = 0x0,
+		.max_buf_write_size = 0x0,
+		.num_erase_regions = 1,
+		.erase_region_info =
+		{
+			0x0010001f,
+			0x00000000
+		}
+	},
+	{
+		.mfr = CFI_MFR_SST,
+		.id = 0xd6,
+		.pri_id = 0x02,
+		.dev_size = 0x12,
+		.interface_desc = 0x0,
+		.max_buf_write_size = 0x0,
+		.num_erase_regions = 1,
+		.erase_region_info =
+		{
+			0x0010003f,
+			0x00000000
+		}
+	},
+	{
+		.mfr = CFI_MFR_SST,
+		.id = 0xd7,
+		.pri_id = 0x02,
+		.dev_size = 0x13,
+		.interface_desc = 0x0,
+		.max_buf_write_size = 0x0,
+		.num_erase_regions = 1,
+		.erase_region_info =
+		{
+			0x0010007f,
+			0x00000000
+		}
+	},
+	{
+		.mfr = 0,
+		.id = 0,
+	}
+};
+
+void cfi_fixup_non_cfi(flash_bank_t *bank, void *param)
+{
+	cfi_flash_bank_t *cfi_info = bank->driver_priv;
+	non_cfi_t *non_cfi = non_cfi_flashes;
+	
+	while (non_cfi->mfr)
+	{
+		if ((cfi_info->manufacturer == non_cfi->mfr)
+			&& (cfi_info->device_id == non_cfi->id))
+		{
+			break;
+		}
+		non_cfi++;
+	}
+	
+	cfi_info->not_cfi = 1;
+	
+	/* fill in defaults for non-critical data */
+	cfi_info->vcc_min = 0x0;
+	cfi_info->vcc_max = 0x0;
+	cfi_info->vpp_min = 0x0;
+	cfi_info->vpp_max = 0x0;
+	cfi_info->word_write_timeout_typ = 0x0;
+	cfi_info->buf_write_timeout_typ = 0x0;
+	cfi_info->block_erase_timeout_typ = 0x0;
+	cfi_info->chip_erase_timeout_typ = 0x0;
+	cfi_info->word_write_timeout_max = 0x0;
+	cfi_info->buf_write_timeout_max = 0x0;
+	cfi_info->block_erase_timeout_max = 0x0;
+	cfi_info->chip_erase_timeout_max = 0x0;
+	
+	cfi_info->qry[0] = 'Q';
+	cfi_info->qry[1] = 'R';
+	cfi_info->qry[2] = 'Y';
+	
+	cfi_info->pri_id = non_cfi->pri_id;
+	cfi_info->pri_addr = 0x0;
+	cfi_info->alt_id = 0x0;
+	cfi_info->alt_addr = 0x0;
+	cfi_info->alt_ext = NULL;
+	
+	cfi_info->interface_desc = non_cfi->interface_desc;
+	cfi_info->max_buf_write_size = non_cfi->max_buf_write_size;
+	cfi_info->num_erase_regions = non_cfi->num_erase_regions;
+	cfi_info->erase_region_info = non_cfi->erase_region_info;
+	
+	if (cfi_info->pri_id == 0x2)
+	{
+		cfi_spansion_pri_ext_t *pri_ext = malloc(sizeof(cfi_spansion_pri_ext_t));
+
+		pri_ext->pri[0] = 'P';
+		pri_ext->pri[1] = 'R';
+		pri_ext->pri[2] = 'I';
+		
+		pri_ext->major_version = '1';
+		pri_ext->minor_version = '0';
+		
+		pri_ext->SiliconRevision = 0x0;
+		pri_ext->EraseSuspend = 0x0;
+		pri_ext->EraseSuspend = 0x0;
+		pri_ext->BlkProt = 0x0;
+		pri_ext->TmpBlkUnprotect = 0x0;
+		pri_ext->BlkProtUnprot = 0x0;
+		pri_ext->SimultaneousOps = 0x0;
+		pri_ext->BurstMode = 0x0;
+		pri_ext->PageMode = 0x0;
+		pri_ext->VppMin = 0x0;
+		pri_ext->VppMax = 0x0;
+		pri_ext->TopBottom = 0x0;
+	
+		pri_ext->_reversed_geometry = 0;
+		
+		cfi_info->pri_ext = pri_ext;
+	} else if ((cfi_info->pri_id == 0x1) || (cfi_info->pri_id == 0x3))
+	{
+		ERROR("BUG: non-CFI flashes using the Intel commandset are not yet supported");
+		exit(-1);
+	}
+}
+
diff --git a/src/flash/non_cfi.h b/src/flash/non_cfi.h
new file mode 100644
index 00000000..e91b2098
--- /dev/null
+++ b/src/flash/non_cfi.h
@@ -0,0 +1,41 @@
+/***************************************************************************
+ *   Copyright (C) 2007 by Dominic Rath                                    *
+ *   Dominic.Rath@gmx.de                                                   *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifndef NON_CFI_H
+#define NON_CFI_H
+
+#include "types.h"
+
+typedef struct non_cfi_s
+{
+	u16 mfr;
+	u16 id;
+	u16 pri_id;
+	u8 dev_size;
+	u16 interface_desc;
+	u16 max_buf_write_size;
+	u8 num_erase_regions;
+	u32 erase_region_info[6];
+} non_cfi_t;
+
+extern non_cfi_t non_cfi_flashes[];
+extern void cfi_fixup_non_cfi(flash_bank_t *bank, void *param);
+
+#endif /* NON_CFI_H */
+
diff --git a/src/helper/fileio.c b/src/helper/fileio.c
index 648f1879..3a760cd8 100644
--- a/src/helper/fileio.c
+++ b/src/helper/fileio.c
@@ -98,7 +98,7 @@ int fileio_open_local(fileio_t *fileio)
 		}
 	}
 	
-	if (fileio->pri_type == FILEIO_IMAGE)
+	if (fileio->type == FILEIO_BINARY)
 		strcat(access, "b");
 	
 	if (!(fileio_local->file = fopen(fileio->url, access)))
@@ -120,126 +120,7 @@ int fileio_open_local(fileio_t *fileio)
 	return ERROR_OK;
 }
 
-//#ifdef FILEIO_BUFFER_COMPLETE_IHEX
-int fileio_ihex_buffer_complete(fileio_t *fileio)
-{
-	fileio_image_t *image = fileio->pri_type_private;
-	fileio_ihex_t *ihex = fileio->sec_type_private;
-	u32 raw_bytes_read, raw_bytes;
-	int retval;
-	u32 full_address = image->base_address;
-	char *buffer = malloc(ihex->raw_size);
-	u32 cooked_bytes = 0x0;
-	
-	ihex->raw_size = fileio->size;
-	ihex->buffer = malloc(ihex->raw_size >> 1);
-	
-	if ((retval = fileio_dispatch_read(fileio, ihex->raw_size, (u8*)buffer, &raw_bytes_read)) != ERROR_OK)
-	{
-		free(buffer);
-		ERROR("failed buffering IHEX file, read failed");
-		return ERROR_FILEIO_OPERATION_FAILED;
-	}
-	
-	if (raw_bytes_read != ihex->raw_size)
-	{
-		free(buffer);
-		ERROR("failed buffering complete IHEX file, only partially read");
-		return ERROR_FILEIO_OPERATION_FAILED;
-	}
-	
-	raw_bytes = 0x0;
-	while (raw_bytes < raw_bytes_read)
-	{
-		u32 count;
-		u32 address;
-		u32 record_type;
-		u32 checksum;
-		
-		if (sscanf(&buffer[raw_bytes], ":%2x%4x%2x", &count, &address, &record_type) != 3)
-		{
-			snprintf(fileio->error_str, FILEIO_MAX_ERROR_STRING, "invalid IHEX record");
-			return ERROR_FILEIO_OPERATION_FAILED;
-		}
-		raw_bytes += 9;
-		
-		if (record_type == 0)
-		{
-			if ((full_address & 0xffff) != address)
-			{
-				free(buffer);
-				ERROR("can't handle non-linear IHEX file");
-				snprintf(fileio->error_str, FILEIO_MAX_ERROR_STRING, "can't handle non-linear IHEX file");
-				return ERROR_FILEIO_OPERATION_FAILED;
-			}
-			
-			while (count-- > 0)
-			{
-				sscanf(&buffer[raw_bytes], "%2hhx", &ihex->buffer[cooked_bytes]);
-				raw_bytes += 2;
-				cooked_bytes += 1;
-				full_address++;
-			}
-		}
-		else if (record_type == 1)
-		{
-			free(buffer);
-			fileio->size = cooked_bytes;
-			return ERROR_OK;
-		}
-		else if (record_type == 4)
-		{
-			u16 upper_address;
-			
-			sscanf(&buffer[raw_bytes], "%4hx", &upper_address);
-			raw_bytes += 4;
-			
-			if ((full_address >> 16) != upper_address)
-			{
-				free(buffer);
-				ERROR("can't handle non-linear IHEX file");
-				snprintf(fileio->error_str, FILEIO_MAX_ERROR_STRING, "can't handle non-linear IHEX file");
-				return ERROR_FILEIO_OPERATION_FAILED;
-			}
-		}
-		else if (record_type == 5)
-		{
-			u32 start_address;
-			
-			sscanf(&buffer[raw_bytes], "%8x", &start_address);
-			raw_bytes += 8;
-			
-			image->has_start_address = 1;
-			image->start_address = be_to_h_u32((u8*)&start_address);
-		}
-		else
-		{
-			free(buffer);
-			ERROR("unhandled IHEX record type: %i", record_type);
-			snprintf(fileio->error_str, FILEIO_MAX_ERROR_STRING, "unhandled IHEX record type: %i", record_type);
-			return ERROR_FILEIO_OPERATION_FAILED;
-		}
-		
-		sscanf(&buffer[raw_bytes], "%2x", &checksum);
-		raw_bytes += 2;
-		
-		/* consume new-line character(s) */
-		if ((buffer[raw_bytes] == '\n') || (buffer[raw_bytes] == '\r'))
-			raw_bytes++;
-
-		if ((buffer[raw_bytes] == '\n') || (buffer[raw_bytes] == '\r'))
-			raw_bytes++;
-	}
-
-	free(buffer);
-	ERROR("premature end of IHEX file, no end-of-file record found");
-	snprintf(fileio->error_str, FILEIO_MAX_ERROR_STRING, "premature end of IHEX file, no end-of-file record found");
-	return ERROR_FILEIO_OPERATION_FAILED;	
-}
-//#endif
-
-int fileio_open(fileio_t *fileio, char *url, enum fileio_access access,
-	enum fileio_pri_type pri_type, void *pri_info, enum fileio_sec_type sec_type)
+int fileio_open(fileio_t *fileio, char *url, enum fileio_access access,	enum fileio_type type)
 {
 	int retval = ERROR_OK;
 	char *resource_identifier = NULL;
@@ -261,9 +142,8 @@ int fileio_open(fileio_t *fileio, char *url, enum fileio_access access,
 		fileio->location = FILEIO_LOCAL;
 	}
 	
+	fileio->type = type;
 	fileio->access = access;
-	fileio->pri_type = pri_type;
-	fileio->sec_type = sec_type;
 	fileio->url = strdup(url);
 	
 	switch (fileio->location)
@@ -279,50 +159,6 @@ int fileio_open(fileio_t *fileio, char *url, enum fileio_access access,
 	if (retval != ERROR_OK)
 		return retval;
 	
-	if (fileio->pri_type == FILEIO_TEXT)
-	{
-		/* do nothing for now */
-		return ERROR_OK;
-	}
-	else if (fileio->pri_type == FILEIO_IMAGE)
-	{
-		fileio_image_t *image = malloc(sizeof(fileio_image_t));
-		fileio_image_t *image_info = pri_info;
-		
-		fileio->pri_type_private = image;
-		*image = *image_info; 
-		
-		if (fileio->sec_type == FILEIO_PLAIN)
-		{
-			fileio->sec_type_private = NULL;
-		}
-		else if (fileio->sec_type == FILEIO_IHEX)
-		{
-			fileio_ihex_t *fileio_ihex;
-			
-			if (fileio->access != FILEIO_READ)
-			{
-				ERROR("can't write/append to a IHEX file");
-				snprintf(fileio->error_str, FILEIO_MAX_ERROR_STRING, "can't write/append to a IHEX file");
-				fileio_close(fileio);
-				return ERROR_FILEIO_OPERATION_FAILED;
-			}
-			
-			fileio_ihex = malloc(sizeof(fileio_ihex_t));
-			fileio->sec_type_private = fileio_ihex;
-			
-			fileio_ihex->position = 0;
-			fileio_ihex->raw_size = fileio->size;
-#ifdef FILEIO_BUFFER_COMPLETE_IHEX
-			if (fileio_ihex_buffer_complete(fileio) != ERROR_OK)
-			{
-				fileio_close(fileio);
-				return ERROR_FILEIO_OPERATION_FAILED;
-			}
-#endif
-		}
-	}
-	
 	return ERROR_OK;
 }
 
@@ -369,29 +205,6 @@ int fileio_close(fileio_t *fileio)
 	
 	free(fileio->url);
 	
-	if (fileio->pri_type == FILEIO_TEXT)
-	{
-		/* do nothing for now */
-	}
-	else if (fileio->pri_type == FILEIO_IMAGE)
-	{
-		if (fileio->sec_type == FILEIO_PLAIN)
-		{
-			/* nothing special to do for plain binary */
-		}
-		else if (fileio->sec_type == FILEIO_IHEX)
-		{
-			fileio_ihex_t *fileio_ihex = fileio->sec_type_private;
-	
-			if (fileio_ihex->buffer)
-				free(fileio_ihex->buffer);
-			
-			free(fileio->sec_type_private);
-		}
-		
-		free(fileio->pri_type_private);
-	}
-	
 	return ERROR_OK;
 }
 
@@ -432,7 +245,7 @@ int fileio_local_read(fileio_t *fileio, u32 size, u8 *buffer, u32 *size_read)
 	return ERROR_OK;
 }
 
-int fileio_dispatch_read(fileio_t *fileio, u32 size, u8 *buffer, u32 *size_read)
+int fileio_read(fileio_t *fileio, u32 size, u8 *buffer, u32 *size_read)
 {
 	switch (fileio->location)
 	{
@@ -445,38 +258,6 @@ int fileio_dispatch_read(fileio_t *fileio, u32 size, u8 *buffer, u32 *size_read)
 	}
 }
 
-int fileio_read_ihex(fileio_t *fileio, u32 size, u8 *buffer, u32 *size_read)
-{
-	fileio_ihex_t *fileio_ihex = fileio->sec_type_private;
-
-	if ((fileio_ihex->position + size) > fileio->size)
-	{
-		/* don't read past the end of the file */
-		size = (fileio->size - fileio_ihex->position);
-	}
-	
-#ifdef FILEIO_BUFFER_COMPLETE_IHEX
-	memcpy(buffer, fileio_ihex->buffer + fileio_ihex->position, size);
-	*size_read = size;
-#endif
-
-	return ERROR_OK;
-}
-
-int fileio_read(fileio_t *fileio, u32 size, u8 *buffer, u32 *size_read)
-{
-	if (fileio->sec_type == FILEIO_PLAIN)
-	{
-		return fileio_dispatch_read(fileio, size, buffer, size_read);
-	}
-	else if (fileio->sec_type == FILEIO_IHEX)
-	{
-		return fileio_read_ihex(fileio, size, buffer, size_read);
-	}
-	
-	return ERROR_OK;
-}
-
 int fileio_local_write(fileio_t *fileio, u32 size, u8 *buffer, u32 *size_written)
 {
 	fileio_local_t *fileio_local = fileio->location_private;
@@ -486,7 +267,7 @@ int fileio_local_write(fileio_t *fileio, u32 size, u8 *buffer, u32 *size_written
 	return ERROR_OK;
 }
 
-int fileio_dispatch_write(fileio_t *fileio, u32 size, u8 *buffer, u32 *size_written)
+int fileio_write(fileio_t *fileio, u32 size, u8 *buffer, u32 *size_written)
 {
 	switch (fileio->location)
 	{
@@ -499,48 +280,3 @@ int fileio_dispatch_write(fileio_t *fileio, u32 size, u8 *buffer, u32 *size_writ
 	
 	return ERROR_OK;
 }
-
-int fileio_write(fileio_t *fileio, u32 size, u8 *buffer, u32 *size_written)
-{
-	int retval = ERROR_FILEIO_OPERATION_NOT_SUPPORTED;
-	if (fileio->sec_type == FILEIO_PLAIN)
-	{
-		retval = fileio_dispatch_write(fileio, size, buffer, size_written);
-	}
-	else if (fileio->sec_type == FILEIO_IHEX)
-	{
-		return ERROR_FILEIO_OPERATION_NOT_SUPPORTED;
-	}
-	
-	if (retval != ERROR_OK)
-		return retval;
-		
-	fileio->size += size;
-	
-	return ERROR_OK;
-}
-
-int fileio_identify_image_type(enum fileio_sec_type *sec_type, char *type_string)
-{
-	if (type_string)
-	{
-		if (!strcmp(type_string, "bin"))
-		{
-			*sec_type = FILEIO_PLAIN;
-		}
-		else if (!strcmp(type_string, "ihex"))
-		{
-			*sec_type = FILEIO_IHEX;
-		}
-		else
-		{
-			return ERROR_FILEIO_RESOURCE_TYPE_UNKNOWN;
-		}
-	}
-	else
-	{
-		*sec_type = FILEIO_PLAIN;
-	}
-	
-	return ERROR_OK;
-}
diff --git a/src/helper/fileio.h b/src/helper/fileio.h
index c047cb87..55e6f323 100644
--- a/src/helper/fileio.h
+++ b/src/helper/fileio.h
@@ -22,28 +22,20 @@
 
 #define FILEIO_MAX_ERROR_STRING		(128)
 
-/* make buffering of complete intel-hex format files optional
- * to account for resource-limited hosts
- */
-#define FILEIO_BUFFER_COMPLETE_IHEX
+#include "types.h"
 
+#include <stdio.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <sys/types.h>
 #include <sys/stat.h>
+#include <errno.h>
+#include <ctype.h>
 
-enum fileio_pri_type
-{
-	FILEIO_TEXT = 0x1,
-	FILEIO_IMAGE = 0x2,
-};
-
-enum fileio_sec_type
+enum fileio_type
 {
-	FILEIO_PLAIN = 0x10,
-	FILEIO_IHEX = 0x20,
-/*
- * Possible future enhancements:
- * FILEIO_ELF,
- * FILEIO_SRECORD,
- */
+	FILEIO_TEXT,
+	FILEIO_BINARY,
 };
 
 enum fileio_location
@@ -73,48 +65,23 @@ typedef struct fileio_s
 	char *url;
 	char error_str[FILEIO_MAX_ERROR_STRING];
 	long long size;
-	enum fileio_pri_type pri_type;
-	enum fileio_sec_type sec_type;
+	enum fileio_type type;
 	enum fileio_location location;
 	enum fileio_access access;
 	void *location_private;
-	void *pri_type_private;
-	void *sec_type_private;
 } fileio_t;
 
-typedef struct fileio_text_s
-{
-} fileio_text_t;
-
-typedef struct fileio_image_s
-{
-	u32 base_address;
-	int has_start_address;
-	u32 start_address;
-} fileio_image_t;
-
 typedef struct fileio_local_s
 {
 	FILE *file;
 	struct stat file_stat;
 } fileio_local_t;
 
-typedef struct fileio_ihex_s
-{
-	u32 position;
-	u32 raw_size;
-#ifdef FILEIO_BUFFER_COMPLETE_IHEX
-	u8 *buffer;
-#endif
-} fileio_ihex_t;
-
-extern int fileio_identify_image_type(enum fileio_sec_type *sec_type, char *type_string);
 extern int fileio_write(fileio_t *fileio, u32 size, u8 *buffer, u32 *size_written);
 extern int fileio_read(fileio_t *fileio, u32 size, u8 *buffer, u32 *size_read);
 extern int fileio_seek(fileio_t *fileio, u32 position);
 extern int fileio_close(fileio_t *fileio);
-extern int fileio_open(fileio_t *fileio, char *url, enum fileio_access access,
-	enum fileio_pri_type pri_type, void *pri_info, enum fileio_sec_type sec_type);
+extern int fileio_open(fileio_t *fileio, char *url, enum fileio_access access, enum fileio_type type);
 	
 #define ERROR_FILEIO_LOCATION_UNKNOWN	(-1200)
 #define ERROR_FILEIO_NOT_FOUND			(-1201)
diff --git a/src/jtag/jtag.c b/src/jtag/jtag.c
index 073ce1f4..6dfde760 100644
--- a/src/jtag/jtag.c
+++ b/src/jtag/jtag.c
@@ -1282,6 +1282,7 @@ int jtag_examine_chain()
 	{
 		ERROR("number of discovered devices in JTAG chain (%i) doesn't match configuration (%i)", 
 			device_count, jtag_num_devices);
+		ERROR("check the config file and ensure proper JTAG communication (connections, speed, ...)");
 		exit(-1);
 	}
 	
diff --git a/src/openocd.c b/src/openocd.c
index 3abdda63..10a4cfd2 100644
--- a/src/openocd.c
+++ b/src/openocd.c
@@ -18,7 +18,7 @@
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
 
-#define OPENOCD_VERSION "Open On-Chip Debugger (2007-04-26 16:40 CEST)"
+#define OPENOCD_VERSION "Open On-Chip Debugger (2007-05-29 13:15 CEST)"
 
 #ifdef HAVE_CONFIG_H
 #include "config.h"
diff --git a/src/target/Makefile.am b/src/target/Makefile.am
index cf1955a0..b1e6f919 100644
--- a/src/target/Makefile.am
+++ b/src/target/Makefile.am
@@ -3,7 +3,7 @@ METASOURCES = AUTO
 noinst_LIBRARIES = libtarget.a
 libtarget_a_SOURCES = target.c register.c breakpoints.c armv4_5.c embeddedice.c etm.c arm7tdmi.c arm9tdmi.c \
 	arm_jtag.c arm7_9_common.c algorithm.c arm920t.c arm720t.c armv4_5_mmu.c armv4_5_cache.c arm_disassembler.c \
-	arm966e.c arm926ejs.c etb.c xscale.c arm_simulator.c
+	arm966e.c arm926ejs.c etb.c xscale.c arm_simulator.c image.c
 noinst_HEADERS = target.h register.h armv4_5.h embeddedice.h etm.h arm7tdmi.h arm9tdmi.h \
 		arm_jtag.h arm7_9_common.h arm920t.h arm720t.h armv4_5_mmu.h armv4_5_cache.h breakpoints.h algorithm.h \
-		arm_disassembler.h arm966e.h arm926ejs.h etb.h xscale.h arm_simulator.h
+		arm_disassembler.h arm966e.h arm926ejs.h etb.h xscale.h arm_simulator.h image.h
diff --git a/src/target/arm7_9_common.c b/src/target/arm7_9_common.c
index 2c82c91e..77a43fee 100644
--- a/src/target/arm7_9_common.c
+++ b/src/target/arm7_9_common.c
@@ -2096,8 +2096,6 @@ int arm7_9_register_commands(struct command_context_s *cmd_ctx)
 	
 	arm7_9_cmd = register_command(cmd_ctx, NULL, "arm7_9", NULL, COMMAND_ANY, "arm7/9 specific commands");
 
-	register_command(cmd_ctx, arm7_9_cmd, "etm", handle_arm7_9_etm_command, COMMAND_CONFIG, NULL);
-	
 	register_command(cmd_ctx, arm7_9_cmd, "write_xpsr", handle_arm7_9_write_xpsr_command, COMMAND_EXEC, "write program status register <value> <not cpsr|spsr>");
 	register_command(cmd_ctx, arm7_9_cmd, "write_xpsr_im8", handle_arm7_9_write_xpsr_im8_command, COMMAND_EXEC, "write program status register <8bit immediate> <rotate> <not cpsr|spsr>");
 	
@@ -2115,7 +2113,8 @@ int arm7_9_register_commands(struct command_context_s *cmd_ctx)
 		COMMAND_ANY, "use DCC downloads for larger memory writes <enable|disable>");
 
 	armv4_5_register_commands(cmd_ctx);
-	etb_register_commands(cmd_ctx, arm7_9_cmd);
+	
+	etm_register_commands(cmd_ctx);
 	
 	return ERROR_OK;
 }
@@ -2425,83 +2424,6 @@ int handle_arm7_9_dcc_downloads_command(struct command_context_s *cmd_ctx, char
 	return ERROR_OK;
 }
 
-int handle_arm7_9_etm_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	target_t *target;
-	armv4_5_common_t *armv4_5;
-	arm7_9_common_t *arm7_9;
-	
-	if (argc != 1)
-	{
-		ERROR("incomplete 'arm7_9 etm <target>' command");
-		exit(-1);
-	}
-	
-	target = get_target_by_num(strtoul(args[0], NULL, 0));
-	
-	if (!target)
-	{
-		ERROR("target number '%s' not defined", args[0]);
-		exit(-1);
-	}
-	
-	if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
-	{
-		command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
-		return ERROR_OK;
-	}
-	
-	arm7_9->has_etm = 1;
-	
-	return ERROR_OK;
-}
-
-int handle_arm7_9_etb_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-	target_t *target;
-	jtag_device_t *jtag_device;
-	armv4_5_common_t *armv4_5;
-	arm7_9_common_t *arm7_9;
-	
-	if (argc != 2)
-	{
-		ERROR("incomplete 'arm7_9 etb <target> <chain_pos>' command");
-		exit(-1);
-	}
-	
-	target = get_target_by_num(strtoul(args[0], NULL, 0));
-	
-	if (!target)
-	{
-		ERROR("target number '%s' not defined", args[0]);
-		exit(-1);
-	}
-	
-	if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
-	{
-		command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
-		return ERROR_OK;
-	}
-	
-	jtag_device = jtag_get_device(strtoul(args[1], NULL, 0));
-	
-	if (!jtag_device)
-	{
-		ERROR("jtag device number '%s' not defined", args[1]);
-		exit(-1);
-	}
-
-	arm7_9->etb = malloc(sizeof(etb_t));
-	
-	arm7_9->etb->chain_pos = strtoul(args[1], NULL, 0);
-	arm7_9->etb->cur_scan_chain = -1;
-	arm7_9->etb->reg_cache = NULL;
-	arm7_9->etb->RAM_width = 0;
-	arm7_9->etb->RAM_depth = 0;
-
-	return ERROR_OK;
-}
-
 int arm7_9_init_arch_info(target_t *target, arm7_9_common_t *arm7_9)
 {
 	armv4_5_common_t *armv4_5 = &arm7_9->armv4_5_common;
@@ -2515,8 +2437,7 @@ int arm7_9_init_arch_info(target_t *target, arm7_9_common_t *arm7_9)
 	arm7_9->force_hw_bkpts = 0;
 	arm7_9->use_dbgrq = 0;
 	
-	arm7_9->has_etm = 0;
-	arm7_9->etb = NULL;
+	arm7_9->etm_ctx = NULL;
 	arm7_9->has_single_step = 0;
 	arm7_9->has_monitor_mode = 0;
 	arm7_9->has_vector_catch = 0;
diff --git a/src/target/arm7_9_common.h b/src/target/arm7_9_common.h
index e77bedac..fbcb920d 100644
--- a/src/target/arm7_9_common.h
+++ b/src/target/arm7_9_common.h
@@ -25,7 +25,7 @@
 #include "breakpoints.h"
 #include "target.h"
 
-#include "etb.h"
+#include "etm.h"
 
 #define	ARM7_9_COMMON_MAGIC 0x0a790a79
 
@@ -35,7 +35,6 @@ typedef struct arm7_9_common_s
 	
 	arm_jtag_t jtag_info;
 	reg_cache_t *eice_cache;
-	reg_cache_t *etm_cache;
 	
 	u32 arm_bkpt;
 	u16 thumb_bkpt;
@@ -48,8 +47,8 @@ typedef struct arm7_9_common_s
 	int dbgreq_adjust_pc;
 	int use_dbgrq;
 	
-	int has_etm;
-	etb_t *etb;
+	etm_context_t *etm_ctx;
+	
 	int has_single_step;
 	int has_monitor_mode;
 	int has_vector_catch;
diff --git a/src/target/arm7tdmi.c b/src/target/arm7tdmi.c
index 5684dcfa..7c6b937f 100644
--- a/src/target/arm7tdmi.c
+++ b/src/target/arm7tdmi.c
@@ -744,10 +744,10 @@ void arm7tdmi_build_reg_cache(target_t *target)
 	(*cache_p)->next = embeddedice_build_reg_cache(target, arm7_9);
 	arm7_9->eice_cache = (*cache_p)->next;
 	
-	if (arm7_9->has_etm)
+	if (arm7_9->etm_ctx)
 	{
-		(*cache_p)->next->next = etm_build_reg_cache(target, jtag_info, 0);
-		arm7_9->etm_cache = (*cache_p)->next->next;
+		(*cache_p)->next->next = etm_build_reg_cache(target, jtag_info, arm7_9->etm_ctx);
+		arm7_9->etm_ctx->reg_cache = (*cache_p)->next->next;
 	}
 }
 
diff --git a/src/target/arm9tdmi.c b/src/target/arm9tdmi.c
index 112926d1..7ecd1f0d 100644
--- a/src/target/arm9tdmi.c
+++ b/src/target/arm9tdmi.c
@@ -874,16 +874,10 @@ void arm9tdmi_build_reg_cache(target_t *target)
 	(*cache_p)->next = embeddedice_build_reg_cache(target, arm7_9);
 	arm7_9->eice_cache = (*cache_p)->next;
 
-	if (arm7_9->has_etm)
+	if (arm7_9->etm_ctx)
 	{
-		(*cache_p)->next->next = etm_build_reg_cache(target, jtag_info, 0);
-		arm7_9->etm_cache = (*cache_p)->next->next;
-	}
-	
-	if (arm7_9->etb)
-	{
-		(*cache_p)->next->next->next = etb_build_reg_cache(arm7_9->etb);
-		arm7_9->etb->reg_cache = (*cache_p)->next->next->next;
+		(*cache_p)->next->next = etm_build_reg_cache(target, jtag_info, arm7_9->etm_ctx);
+		arm7_9->etm_ctx->reg_cache = (*cache_p)->next->next;
 	}
 }
 
diff --git a/src/target/arm_disassembler.c b/src/target/arm_disassembler.c
index dd779282..1c275f54 100644
--- a/src/target/arm_disassembler.c
+++ b/src/target/arm_disassembler.c
@@ -343,7 +343,10 @@ int evaluate_load_store(u32 opcode, u32 address, arm_instruction_t *instruction)
 	if (!I) /* #+-<offset_12> */
 	{
 		u32 offset_12 = (opcode & 0xfff);
-		snprintf(offset, 32, "#%s0x%x", (U) ? "" : "-", offset_12);
+		if (offset_12)
+			snprintf(offset, 32, ", #%s0x%x", (U) ? "" : "-", offset_12);
+		else
+			snprintf(offset, 32, "");
 		
 		instruction->info.load_store.offset_mode = 0;
 		instruction->info.load_store.offset.offset = offset_12;
@@ -376,26 +379,26 @@ int evaluate_load_store(u32 opcode, u32 address, arm_instruction_t *instruction)
 
 		if ((shift_imm == 0x0) && (shift == 0x0)) /* +-<Rm> */
 		{
-			snprintf(offset, 32, "%sr%i", (U) ? "" : "-", Rm);
+			snprintf(offset, 32, ", %sr%i", (U) ? "" : "-", Rm);
 		}
 		else /* +-<Rm>, <Shift>, #<shift_imm> */
 		{
 			switch (shift)
 			{
 				case 0x0: /* LSL */
-					snprintf(offset, 32, "%sr%i, LSL #0x%x", (U) ? "" : "-", Rm, shift_imm);
+					snprintf(offset, 32, ", %sr%i, LSL #0x%x", (U) ? "" : "-", Rm, shift_imm);
 					break;
 				case 0x1: /* LSR */
-					snprintf(offset, 32, "%sr%i, LSR #0x%x", (U) ? "" : "-", Rm, shift_imm);
+					snprintf(offset, 32, ", %sr%i, LSR #0x%x", (U) ? "" : "-", Rm, shift_imm);
 					break;
 				case 0x2: /* ASR */
-					snprintf(offset, 32, "%sr%i, ASR #0x%x", (U) ? "" : "-", Rm, shift_imm);
+					snprintf(offset, 32, ", %sr%i, ASR #0x%x", (U) ? "" : "-", Rm, shift_imm);
 					break;
 				case 0x3: /* ROR */
-					snprintf(offset, 32, "%sr%i, ROR #0x%x", (U) ? "" : "-", Rm, shift_imm);
+					snprintf(offset, 32, ", %sr%i, ROR #0x%x", (U) ? "" : "-", Rm, shift_imm);
 					break;
 				case 0x4: /* RRX */
-					snprintf(offset, 32, "%sr%i, RRX", (U) ? "" : "-", Rm);
+					snprintf(offset, 32, ", %sr%i, RRX", (U) ? "" : "-", Rm);
 					break;
 			}
 		}
@@ -405,7 +408,7 @@ int evaluate_load_store(u32 opcode, u32 address, arm_instruction_t *instruction)
 	{
 		if (W == 0) /* offset */
 		{
-			snprintf(instruction->text, 128, "0x%8.8x\t0x%8.8x\t%s%s%s r%i, [r%i, %s]",
+			snprintf(instruction->text, 128, "0x%8.8x\t0x%8.8x\t%s%s%s r%i, [r%i%s]",
 					 address, opcode, operation, COND(opcode), suffix,
 					 Rd, Rn, offset);
 			
@@ -413,7 +416,7 @@ int evaluate_load_store(u32 opcode, u32 address, arm_instruction_t *instruction)
 		}
 		else /* pre-indexed */
 		{
-			snprintf(instruction->text, 128, "0x%8.8x\t0x%8.8x\t%s%s%s r%i, [r%i, %s]!",
+			snprintf(instruction->text, 128, "0x%8.8x\t0x%8.8x\t%s%s%s r%i, [r%i%s]!",
 					 address, opcode, operation, COND(opcode), suffix,
 					 Rd, Rn, offset);
 			
@@ -422,7 +425,7 @@ int evaluate_load_store(u32 opcode, u32 address, arm_instruction_t *instruction)
 	}
 	else /* post-indexed */
 	{
-		snprintf(instruction->text, 128, "0x%8.8x\t0x%8.8x\t%s%s%s r%i, [r%i], %s",
+		snprintf(instruction->text, 128, "0x%8.8x\t0x%8.8x\t%s%s%s r%i, [r%i]%s",
 				 address, opcode, operation, COND(opcode), suffix,
 				 Rd, Rn, offset);
 		
@@ -1157,7 +1160,10 @@ int evaluate_data_proc(u32 opcode, u32 address, arm_instruction_t *instruction)
 	}
 	else if ((op == 0xd) || (op == 0xf)) /* <opcode1>{<cond>}{S} <Rd>, <shifter_operand> */
 	{
-		snprintf(instruction->text, 128, "0x%8.8x\t0x%8.8x\t%s%s%s r%i, %s",
+		if (opcode==0xe1a00000) /* print MOV r0,r0 as NOP */
+			snprintf(instruction->text, 128, "0x%8.8x\t0x%8.8x\tNOP",address, opcode);
+		else
+			snprintf(instruction->text, 128, "0x%8.8x\t0x%8.8x\t%s%s%s r%i, %s",
 				 address, opcode, mnemonic, COND(opcode),
 				 (S) ? "S" : "", Rd, shifter_operand);
 	}
@@ -1315,3 +1321,762 @@ int arm_evaluate_opcode(u32 opcode, u32 address, arm_instruction_t *instruction)
 	return -1;
 }
 
+int evaluate_b_bl_blx_thumb(u16 opcode, u32 address, arm_instruction_t *instruction)
+{
+	u32 offset = opcode & 0x7ff;
+	u32 opc = (opcode >> 11) & 0x3;
+	u32 target_address;
+	char *mnemonic = NULL;
+	
+	/* sign extend 11-bit offset */
+	if (((opc==0) || (opc==2)) && (offset & 0x00000400))
+		offset = 0xfffff800 | offset;
+	
+	target_address = address + 4 + (offset<<1);
+
+	switch(opc)
+	{
+		/* unconditional branch */
+		case 0:
+			instruction->type = ARM_B;
+			mnemonic = "B";
+			break;
+		/* BLX suffix */
+		case 1:
+			instruction->type = ARM_BLX;
+			mnemonic = "BLX";
+			break;
+		/* BL/BLX prefix */
+		case 2:
+			instruction->type = ARM_UNKNOWN_INSTUCTION;
+			mnemonic = "prefix";
+			target_address = offset<<12;
+			break;
+		/* BL suffix */
+		case 3:
+			instruction->type = ARM_BL;
+			mnemonic = "BL";
+			break;
+	}
+	/* TODO: deals correctly with dual opcodes BL/BLX ... */
+
+	snprintf(instruction->text, 128, "0x%8.8x\t0x%4.4x\t%s 0x%8.8x", address, opcode,mnemonic, target_address);
+	
+	instruction->info.b_bl_bx_blx.reg_operand = -1;
+	instruction->info.b_bl_bx_blx.target_address = target_address;
+
+	return ERROR_OK;
+}
+
+int evaluate_add_sub_thumb(u16 opcode, u32 address, arm_instruction_t *instruction)
+{
+	u8 Rd = (opcode >> 0) & 0x7;
+	u8 Rn = (opcode >> 3) & 0x7;
+	u8 Rm_imm = (opcode >> 6) & 0x7;
+	u32 opc = opcode & (1<<9);
+	u32 reg_imm  = opcode & (1<<10);
+	char *mnemonic;
+	
+	if (opc)
+	{
+		instruction->type = ARM_SUB;
+		mnemonic = "SUBS";
+	}
+	else
+	{
+		instruction->type = ARM_ADD;
+		mnemonic = "ADDS";
+	}
+	
+	instruction->info.data_proc.Rd = Rd;
+	instruction->info.data_proc.Rn = Rn;
+	instruction->info.data_proc.S = 1;
+
+	if (reg_imm)
+	{
+		instruction->info.data_proc.variant = 0; /*immediate*/
+		instruction->info.data_proc.shifter_operand.immediate.immediate = Rm_imm;
+		snprintf(instruction->text, 128, "0x%8.8x\t0x%4.4x\t%s r%i, r%i, #%d",
+				 address, opcode, mnemonic, Rd, Rn, Rm_imm);
+	}
+	else
+	{
+		instruction->info.data_proc.variant = 1; /*immediate shift*/
+		instruction->info.data_proc.shifter_operand.immediate_shift.Rm = Rm_imm;
+		snprintf(instruction->text, 128, "0x%8.8x\t0x%4.4x\t%s r%i, r%i, r%i",
+				 address, opcode, mnemonic, Rd, Rn, Rm_imm);
+	}
+
+	return ERROR_OK;
+}
+
+int evaluate_shift_imm_thumb(u16 opcode, u32 address, arm_instruction_t *instruction)
+{
+	u8 Rd = (opcode >> 0) & 0x7;
+	u8 Rm = (opcode >> 3) & 0x7;
+	u8 imm = (opcode >> 6) & 0x1f;
+	u8 opc = (opcode >> 11) & 0x3;
+	char *mnemonic = NULL;
+	
+	switch(opc)
+	{
+		case 0:
+			instruction->type = ARM_MOV;
+			mnemonic = "LSLS";
+			instruction->info.data_proc.shifter_operand.immediate_shift.shift = 0;
+			break;
+		case 1:
+			instruction->type = ARM_MOV;
+			mnemonic = "LSRS";
+			instruction->info.data_proc.shifter_operand.immediate_shift.shift = 1;
+			break;
+		case 2:
+			instruction->type = ARM_MOV;
+			mnemonic = "ASRS";
+			instruction->info.data_proc.shifter_operand.immediate_shift.shift = 2;
+			break;
+	}
+
+	if ((imm==0) && (opc!=0))
+		imm = 32;
+
+	instruction->info.data_proc.Rd = Rd;
+	instruction->info.data_proc.Rn = -1;
+	instruction->info.data_proc.S = 1;
+
+	instruction->info.data_proc.variant = 1; /*immediate_shift*/
+	instruction->info.data_proc.shifter_operand.immediate_shift.Rm = Rm;
+	instruction->info.data_proc.shifter_operand.immediate_shift.shift_imm = imm;
+
+	snprintf(instruction->text, 128, "0x%8.8x\t0x%4.4x\t%s r%i, r%i, #0x%02x",
+				 address, opcode, mnemonic, Rd, Rm, imm);
+
+	return ERROR_OK;
+}
+
+int evaluate_data_proc_imm_thumb(u16 opcode, u32 address, arm_instruction_t *instruction)
+{
+	u8 imm = opcode & 0xff;
+	u8 Rd = (opcode >> 8) & 0x7;
+	u32 opc = (opcode >> 11) & 0x3;
+	char *mnemonic = NULL;
+	
+	instruction->info.data_proc.Rd = Rd;
+	instruction->info.data_proc.Rn = Rd;
+	instruction->info.data_proc.S = 1;
+	instruction->info.data_proc.variant = 0; /*immediate*/
+	instruction->info.data_proc.shifter_operand.immediate.immediate = imm;
+	
+	switch(opc)
+	{
+		case 0:
+			instruction->type = ARM_MOV;
+			mnemonic = "MOVS";
+			instruction->info.data_proc.Rn = -1;
+			break;
+		case 1:
+			instruction->type = ARM_CMP;
+			mnemonic = "CMP";
+			instruction->info.data_proc.Rd = -1;
+			break;
+		case 2:
+			instruction->type = ARM_ADD;
+			mnemonic = "ADDS";
+			break;
+		case 3:
+			instruction->type = ARM_SUB;
+			mnemonic = "SUBS";
+			break;
+	}
+	
+	snprintf(instruction->text, 128, "0x%8.8x\t0x%4.4x\t%s r%i, #0x%02x",
+				 address, opcode, mnemonic, Rd, imm);
+
+	return ERROR_OK;
+}
+
+int evaluate_data_proc_thumb(u16 opcode, u32 address, arm_instruction_t *instruction)
+{
+	u8 high_reg, op, Rm, Rd,H1,H2;
+	char *mnemonic = NULL;
+	
+	high_reg = (opcode & 0x0400) >> 10;
+	op = (opcode & 0x03C0) >> 6;
+	
+	Rd = (opcode & 0x0007);
+	Rm = (opcode & 0x0038) >> 3;
+	H1 = (opcode & 0x0080) >> 7;
+	H2 = (opcode & 0x0040) >> 6;
+	
+	instruction->info.data_proc.Rd = Rd;
+	instruction->info.data_proc.Rn = Rd;
+	instruction->info.data_proc.S = (!high_reg || (instruction->type == ARM_CMP));
+	instruction->info.data_proc.variant = 1 /*immediate shift*/;
+	instruction->info.data_proc.shifter_operand.immediate_shift.Rm = Rm;
+
+	if (high_reg)
+	{
+		Rd |= H1 << 3;
+		Rm |= H2 << 3;
+		op >>= 2;
+	
+		switch (op)
+		{
+			case 0x0:
+				instruction->type = ARM_ADD;
+				mnemonic = "ADD";
+				break;
+			case 0x1:
+				instruction->type = ARM_CMP;
+				mnemonic = "CMP";
+				break;
+			case 0x2:
+				instruction->type = ARM_MOV;
+				mnemonic = "MOV";
+				break;
+			case 0x3:
+				if ((opcode & 0x7) == 0x0)
+				{
+					instruction->info.b_bl_bx_blx.reg_operand = Rm;
+					if (H1)
+					{
+						instruction->type = ARM_BLX;
+						snprintf(instruction->text, 128, "0x%8.8x\t0x%4.4x\tBLX r%i", address, opcode, Rm);
+					}
+					else
+					{
+						instruction->type = ARM_BX;
+						snprintf(instruction->text, 128, "0x%8.8x\t0x%4.4x\tBX r%i", address, opcode, Rm);
+					}
+				}
+				else
+				{
+					instruction->type = ARM_UNDEFINED_INSTRUCTION;
+					snprintf(instruction->text, 128, "0x%8.8x\t0x%4.4x\tUNDEFINED INSTRUCTION", address, opcode);
+				}
+				return ERROR_OK;	
+				break;
+		}
+	}
+	else
+	{
+		switch (op)
+		{
+			case 0x0:
+				instruction->type = ARM_AND;
+				mnemonic = "ANDS";
+				break;
+			case 0x1:
+				instruction->type = ARM_EOR;
+				mnemonic = "EORS";
+				break;
+			case 0x2:
+				instruction->type = ARM_MOV;
+				mnemonic = "LSLS";
+				instruction->info.data_proc.variant = 2 /*register shift*/;
+				instruction->info.data_proc.shifter_operand.register_shift.shift = 0;
+				instruction->info.data_proc.shifter_operand.register_shift.Rm = Rd;
+				instruction->info.data_proc.shifter_operand.register_shift.Rs = Rm;
+				break;
+			case 0x3:
+				instruction->type = ARM_MOV;
+				mnemonic = "LSRS";
+				instruction->info.data_proc.variant = 2 /*register shift*/;
+				instruction->info.data_proc.shifter_operand.register_shift.shift = 1;
+				instruction->info.data_proc.shifter_operand.register_shift.Rm = Rd;
+				instruction->info.data_proc.shifter_operand.register_shift.Rs = Rm;
+				break;
+			case 0x4:
+				instruction->type = ARM_MOV;
+				mnemonic = "ASRS";
+				instruction->info.data_proc.variant = 2 /*register shift*/;
+				instruction->info.data_proc.shifter_operand.register_shift.shift = 2;
+				instruction->info.data_proc.shifter_operand.register_shift.Rm = Rd;
+				instruction->info.data_proc.shifter_operand.register_shift.Rs = Rm;
+				break;
+			case 0x5:
+				instruction->type = ARM_ADC;
+				mnemonic = "ADCS";
+				break;
+			case 0x6:
+				instruction->type = ARM_SBC;
+				mnemonic = "SBCS";
+				break;
+			case 0x7:
+				instruction->type = ARM_MOV;
+				mnemonic = "RORS";
+				instruction->info.data_proc.variant = 2 /*register shift*/;
+				instruction->info.data_proc.shifter_operand.register_shift.shift = 3;
+				instruction->info.data_proc.shifter_operand.register_shift.Rm = Rd;
+				instruction->info.data_proc.shifter_operand.register_shift.Rs = Rm;
+				break;
+			case 0x8:
+				instruction->type = ARM_TST;
+				mnemonic = "TST";
+				break;
+			case 0x9:
+				instruction->type = ARM_RSB;
+				mnemonic = "NEGS";
+				instruction->info.data_proc.variant = 0 /*immediate*/;
+				instruction->info.data_proc.shifter_operand.immediate.immediate = 0;
+				instruction->info.data_proc.Rn = Rm;
+				break;
+			case 0xA:
+				instruction->type = ARM_CMP;
+				mnemonic = "CMP";
+				break;
+			case 0xB:
+				instruction->type = ARM_CMN;
+				mnemonic = "CMN";
+				break;
+			case 0xC:
+				instruction->type = ARM_ORR;
+				mnemonic = "ORRS";
+				break;
+			case 0xD:
+				instruction->type = ARM_MUL;
+				mnemonic = "MULS";
+				break;
+			case 0xE:
+				instruction->type = ARM_BIC;
+				mnemonic = "BICS";
+				break;
+			case 0xF:
+				instruction->type = ARM_MVN;
+				mnemonic = "MVNS";
+				break;
+		}
+	}
+
+	snprintf(instruction->text, 128, "0x%8.8x\t0x%4.4x\t%s r%i, r%i",
+				 address, opcode, mnemonic, Rd, Rm);
+
+	return ERROR_OK;
+}
+
+int evaluate_load_literal_thumb(u16 opcode, u32 address, arm_instruction_t *instruction)
+{
+	u32 immediate;
+	u8 Rd = (opcode >> 8) & 0x7; 
+
+	instruction->type = ARM_LDR;
+	immediate = opcode & 0x000000ff;
+
+	snprintf(instruction->text, 128, "0x%8.8x\t0x%4.4x\tLDR r%i, [PC, #0x%x]", address, opcode, Rd, immediate*4);
+
+	instruction->info.load_store.Rd = Rd;
+	instruction->info.load_store.Rn = 15 /*PC*/;
+	instruction->info.load_store.index_mode = 0; /*offset*/
+	instruction->info.load_store.offset_mode = 0; /*immediate*/
+	instruction->info.load_store.offset.offset = immediate*4;
+
+	return ERROR_OK;
+}
+
+int evaluate_load_store_reg_thumb(u16 opcode, u32 address, arm_instruction_t *instruction)
+{
+	u8 Rd = (opcode >> 0) & 0x7; 
+	u8 Rn = (opcode >> 3) & 0x7; 
+	u8 Rm = (opcode >> 6) & 0x7; 
+	u8 opc = (opcode >> 9) & 0x7; 
+	char *mnemonic = NULL;
+
+	switch(opc)
+	{
+		case 0:
+			instruction->type = ARM_STR;
+			mnemonic = "STR";
+			break;
+		case 1:
+			instruction->type = ARM_STRH;
+			mnemonic = "STRH";
+			break;
+		case 2:
+			instruction->type = ARM_STRB;
+			mnemonic = "STRB";
+			break;
+		case 3:
+			instruction->type = ARM_LDRSB;
+			mnemonic = "LDRSB";
+			break;
+		case 4:
+			instruction->type = ARM_LDR;
+			mnemonic = "LDR";
+			break;
+		case 5:
+			instruction->type = ARM_LDRH;
+			mnemonic = "LDRH";
+			break;
+		case 6:
+			instruction->type = ARM_LDRB;
+			mnemonic = "LDRB";
+			break;
+		case 7:
+			instruction->type = ARM_LDRSH;
+			mnemonic = "LDRSH";
+			break;
+	}
+
+	snprintf(instruction->text, 128, "0x%8.8x\t0x%4.4x\t%s r%i, [r%i, r%i]", address, opcode, mnemonic, Rd, Rn, Rm);
+	
+	instruction->info.load_store.Rd = Rd;
+	instruction->info.load_store.Rn = Rn;
+	instruction->info.load_store.index_mode = 0; /*offset*/
+	instruction->info.load_store.offset_mode = 1; /*register*/
+	instruction->info.load_store.offset.reg.Rm = Rm;
+
+	return ERROR_OK;
+}
+
+int evaluate_load_store_imm_thumb(u16 opcode, u32 address, arm_instruction_t *instruction)
+{
+	u32 offset = (opcode >> 6) & 0x1f;
+	u8 Rd = (opcode >> 0) & 0x7; 
+	u8 Rn = (opcode >> 3) & 0x7; 
+	u32 L = opcode & (1<<11);
+	u32 B = opcode & (1<<12);
+	char *mnemonic;
+	char suffix = ' ';
+	u32 shift = 2;
+
+	if (L)
+	{
+		instruction->type = ARM_LDR;
+		mnemonic = "LDR";
+	}
+	else
+	{
+		instruction->type = ARM_STR;
+		mnemonic = "STR";
+	}
+
+	if ((opcode&0xF000)==0x8000)
+	{
+		suffix = 'H';
+		shift = 1;
+	}
+	else if (B)
+	{
+		suffix = 'B';
+		shift = 0;
+	}
+
+	snprintf(instruction->text, 128, "0x%8.8x\t0x%4.4x\t%s%c r%i, [r%i, #0x%x]", address, opcode, mnemonic, suffix, Rd, Rn, offset<<shift);
+	
+	instruction->info.load_store.Rd = Rd;
+	instruction->info.load_store.Rn = Rn;
+	instruction->info.load_store.index_mode = 0; /*offset*/
+	instruction->info.load_store.offset_mode = 0; /*immediate*/
+	instruction->info.load_store.offset.offset = offset<<shift;
+
+	return ERROR_OK;
+}
+
+int evaluate_load_store_stack_thumb(u16 opcode, u32 address, arm_instruction_t *instruction)
+{
+	u32 offset = opcode  & 0xff;
+	u8 Rd = (opcode >> 8) & 0x7; 
+	u32 L = opcode & (1<<11);
+	char *mnemonic;
+
+	if (L)
+	{
+		instruction->type = ARM_LDR;
+		mnemonic = "LDR";
+	}
+	else
+	{
+		instruction->type = ARM_STR;
+		mnemonic = "STR";
+	}
+
+	snprintf(instruction->text, 128, "0x%8.8x\t0x%4.4x\t%s r%i, [SP, #0x%x]", address, opcode, mnemonic, Rd, offset*4);
+	
+	instruction->info.load_store.Rd = Rd;
+	instruction->info.load_store.Rn = 13 /*SP*/;
+	instruction->info.load_store.index_mode = 0; /*offset*/
+	instruction->info.load_store.offset_mode = 0; /*immediate*/
+	instruction->info.load_store.offset.offset = offset*4;
+
+	return ERROR_OK;
+}
+
+int evaluate_add_sp_pc_thumb(u16 opcode, u32 address, arm_instruction_t *instruction)
+{
+	u32 imm = opcode  & 0xff;
+	u8 Rd = (opcode >> 8) & 0x7; 
+	u8 Rn;
+	u32 SP = opcode & (1<<11);
+	char *reg_name;
+
+	instruction->type = ARM_ADD;
+	
+	if (SP)
+	{
+		reg_name = "SP";
+		Rn = 13;
+	}
+	else
+	{
+		reg_name = "PC";
+		Rn = 15;
+	}
+
+	snprintf(instruction->text, 128, "0x%8.8x\t0x%4.4x\tADD r%i, %s, #0x%x", address, opcode, Rd,reg_name, imm*4);
+
+	instruction->info.data_proc.variant = 0 /* immediate */;
+	instruction->info.data_proc.Rd = Rd;
+	instruction->info.data_proc.Rn = Rn;
+	instruction->info.data_proc.shifter_operand.immediate.immediate = imm*4;
+
+	return ERROR_OK;
+}
+
+int evaluate_adjust_stack_thumb(u16 opcode, u32 address, arm_instruction_t *instruction)
+{
+	u32 imm = opcode  & 0x7f;
+	u8 opc = opcode & (1<<7);
+	char *mnemonic;
+
+	
+	if (opc)
+	{
+		instruction->type = ARM_SUB;
+		mnemonic = "SUB";
+	}
+	else
+	{
+		instruction->type = ARM_ADD;
+		mnemonic = "ADD";
+	}
+
+	snprintf(instruction->text, 128, "0x%8.8x\t0x%4.4x\t%s SP, #0x%x", address, opcode, mnemonic, imm*4);
+
+	instruction->info.data_proc.variant = 0 /* immediate */;
+	instruction->info.data_proc.Rd = 13 /*SP*/;
+	instruction->info.data_proc.Rn = 13 /*SP*/;
+	instruction->info.data_proc.shifter_operand.immediate.immediate = imm*4;
+
+	return ERROR_OK;
+}
+
+int evaluate_breakpoint_thumb(u16 opcode, u32 address, arm_instruction_t *instruction)
+{
+	u32 imm = opcode  & 0xff;
+	
+	instruction->type = ARM_BKPT;
+
+	snprintf(instruction->text, 128, "0x%8.8x\t0x%4.4x\tBKPT 0x%02x", address, opcode, imm);
+
+	return ERROR_OK;
+}
+
+int evaluate_load_store_multiple_thumb(u16 opcode, u32 address, arm_instruction_t *instruction)
+{
+	u32 reg_list = opcode  & 0xff;
+	u32 L = opcode & (1<<11);
+	u32 R = opcode & (1<<8);
+	u8 Rn = (opcode >> 8) & 7;
+	u8 addr_mode = 0 /* IA */;
+	char reg_names[40];
+	char *reg_names_p;
+	char *mnemonic;
+	char ptr_name[7] = "";
+	int i;	
+
+	if ((opcode & 0xf000) == 0xc000)
+	{ /* generic load/store multiple */
+		if (L)
+		{
+			instruction->type = ARM_LDM;
+			mnemonic = "LDMIA";
+		}
+		else
+		{
+			instruction->type = ARM_STM;
+			mnemonic = "STMIA";
+		}
+		snprintf(ptr_name,7,"r%i!, ",Rn);
+	}
+	else
+	{ /* push/pop */
+		Rn = 13; /* SP */
+		if (L)
+		{
+			instruction->type = ARM_LDM;
+			mnemonic = "POP";
+			if (R)
+				reg_list |= (1<<15) /*PC*/;
+		}
+		else
+		{
+			instruction->type = ARM_STM;
+			mnemonic = "PUSH";
+			addr_mode = 3; /*DB*/
+			if (R)
+				reg_list |= (1<<14) /*LR*/;
+		}
+	}
+
+	reg_names_p = reg_names;
+	for (i = 0; i <= 15; i++)
+	{
+		if (reg_list & (1<<i))
+			reg_names_p += snprintf(reg_names_p, (reg_names + 40 - reg_names_p), "r%i, ", i);
+	}
+	if (reg_names_p>reg_names)
+		reg_names_p[-2] = '\0';
+	else /* invalid op : no registers */
+    		reg_names[0] = '\0';
+
+	snprintf(instruction->text, 128, "0x%8.8x\t0x%4.4x\t%s %s{%s}", address, opcode, mnemonic, ptr_name,reg_names);
+
+	instruction->info.load_store_multiple.register_list = reg_list;
+	instruction->info.load_store_multiple.Rn = Rn;
+	instruction->info.load_store_multiple.addressing_mode = addr_mode;
+
+	return ERROR_OK;
+}
+
+int evaluate_cond_branch_thumb(u16 opcode, u32 address, arm_instruction_t *instruction)
+{
+	u32 offset = opcode  & 0xff;
+	u8 cond = (opcode >> 8) & 0xf;
+	u32 target_address;
+
+	if (cond == 0xf)
+	{
+		instruction->type = ARM_SWI;
+		snprintf(instruction->text, 128, "0x%8.8x\t0x%4.4x\tSWI 0x%02x", address, opcode, offset);
+		return ERROR_OK;
+	}
+	else if (cond == 0xe)
+	{
+		instruction->type = ARM_UNDEFINED_INSTRUCTION;
+		snprintf(instruction->text, 128, "0x%8.8x\t0x%4.4x\tUNDEFINED INSTRUCTION", address, opcode);
+		return ERROR_OK;
+	}
+
+	/* sign extend 8-bit offset */
+	if (offset & 0x00000080)
+		offset = 0xffffff00 | offset;
+	
+	target_address = address + 4 + (offset<<1);
+
+	snprintf(instruction->text, 128, "0x%8.8x\t0x%4.4x\tB%s 0x%8.8x", address, opcode,
+			 arm_condition_strings[cond], target_address);
+	
+	instruction->type = ARM_B;
+	instruction->info.b_bl_bx_blx.reg_operand = -1;
+	instruction->info.b_bl_bx_blx.target_address = target_address;
+
+	return ERROR_OK;
+}
+
+int thumb_evaluate_opcode(u16 opcode, u32 address, arm_instruction_t *instruction)
+{
+	/* clear fields, to avoid confusion */
+	memset(instruction, 0, sizeof(arm_instruction_t));
+	instruction->opcode = opcode;
+	
+	if ((opcode & 0xe000) == 0x0000)
+	{
+		/* add/substract register or immediate */
+		if ((opcode & 0x1800) == 0x1800)
+			return evaluate_add_sub_thumb(opcode, address, instruction);
+		/* shift by immediate */
+		else
+			return evaluate_shift_imm_thumb(opcode, address, instruction);
+	}
+	
+	/* Add/substract/compare/move immediate */
+	if ((opcode & 0xe000) == 0x2000)
+	{
+		return evaluate_data_proc_imm_thumb(opcode, address, instruction);
+	}
+	
+	/* Data processing instructions */
+	if ((opcode & 0xf800) == 0x4000)
+	{
+		return evaluate_data_proc_thumb(opcode, address, instruction);
+	}
+	
+	/* Load from literal pool */
+	if ((opcode & 0xf800) == 0x4800)
+	{
+		return evaluate_load_literal_thumb(opcode, address, instruction);
+	}
+
+	/* Load/Store register offset */
+	if ((opcode & 0xf000) == 0x5000)
+	{
+		return evaluate_load_store_reg_thumb(opcode, address, instruction);
+	}
+
+	/* Load/Store immediate offset */
+	if (((opcode & 0xe000) == 0x6000)
+		||((opcode & 0xf000) == 0x8000))
+	{
+		return evaluate_load_store_imm_thumb(opcode, address, instruction);
+	}
+	
+	/* Load/Store from/to stack */
+	if ((opcode & 0xf000) == 0x9000)
+	{
+		return evaluate_load_store_stack_thumb(opcode, address, instruction);
+	}
+
+	/* Add to SP/PC */
+	if ((opcode & 0xf000) == 0xa000)
+	{
+		return evaluate_add_sp_pc_thumb(opcode, address, instruction);
+	}
+
+	/* Misc */
+	if ((opcode & 0xf000) == 0xb000)
+	{
+		if ((opcode & 0x0f00) == 0x0000)
+			return evaluate_adjust_stack_thumb(opcode, address, instruction);
+		else if ((opcode & 0x0f00) == 0x0e00)
+			return evaluate_breakpoint_thumb(opcode, address, instruction);
+		else if ((opcode & 0x0600) == 0x0400) /* push pop */
+			return evaluate_load_store_multiple_thumb(opcode, address, instruction);
+		else
+		{
+			instruction->type = ARM_UNDEFINED_INSTRUCTION;
+			snprintf(instruction->text, 128, "0x%8.8x\t0x%4.4x\tUNDEFINED INSTRUCTION", address, opcode);
+			return ERROR_OK;
+		}
+	}
+
+	/* Load/Store multiple */
+	if ((opcode & 0xf000) == 0xc000)
+	{
+		return evaluate_load_store_multiple_thumb(opcode, address, instruction);
+	}
+
+	/* Conditional branch + SWI */
+	if ((opcode & 0xf000) == 0xd000)
+	{
+		return evaluate_cond_branch_thumb(opcode, address, instruction);
+	}
+	
+	if ((opcode & 0xe000) == 0xe000)
+	{
+		/* Undefined instructions */
+		if ((opcode & 0xf801) == 0xe801)
+		{
+			instruction->type = ARM_UNDEFINED_INSTRUCTION;
+			snprintf(instruction->text, 128, "0x%8.8x\t0x%8.8x\tUNDEFINED INSTRUCTION", address, opcode);
+			return ERROR_OK;
+		}
+		else
+		{ /* Branch to offset */
+			return evaluate_b_bl_blx_thumb(opcode, address, instruction);
+		}
+	}
+
+	ERROR("should never reach this point (opcode=%04x)",opcode);
+	return -1;
+}
+
diff --git a/src/target/arm_disassembler.h b/src/target/arm_disassembler.h
index b55c8855..bdab113d 100644
--- a/src/target/arm_disassembler.h
+++ b/src/target/arm_disassembler.h
@@ -133,7 +133,7 @@ union arm_shifter_operand
 	} immediate;
 	struct {
 		u8 Rm;
-		u8 shift;
+		u8 shift; /* 0: LSL, 1: LSR, 2: ASR, 3: ROR, 4: RRX */
 		u8 shift_imm;
 	} immediate_shift;
 	struct {
@@ -164,7 +164,7 @@ typedef struct arm_load_store_instr_s
 		u32 offset;
 		struct {
 			u8 Rm;
-			u8 shift;
+			u8 shift; /* 0: LSL, 1: LSR, 2: ASR, 3: ROR, 4: RRX */
 			u8 shift_imm;
 		} reg;
 	} offset;
@@ -195,6 +195,7 @@ typedef struct arm_instruction_s
 } arm_instruction_t;
 
 extern int arm_evaluate_opcode(u32 opcode, u32 address, arm_instruction_t *instruction);
+extern int thumb_evaluate_opcode(u16 opcode, u32 address, arm_instruction_t *instruction);
 
 #define COND(opcode) (arm_condition_strings[(opcode & 0xf0000000)>>28])
 
diff --git a/src/target/arm_simulator.c b/src/target/arm_simulator.c
index fd0b309c..561b14f8 100644
--- a/src/target/arm_simulator.c
+++ b/src/target/arm_simulator.c
@@ -257,6 +257,11 @@ int pass_condition(u32 cpsr, u32 opcode)
 	return 0;
 }
 
+int thumb_pass_branch_condition(u32 cpsr, u16 opcode)
+{
+	return pass_condition(cpsr, (opcode & 0x0f00) << 20); 
+}
+
 /* simulate a single step (if possible)
  * if the dry_run_pc argument is provided, no state is changed,
  * but the new pc is stored in the variable pointed at by the argument
@@ -275,26 +280,43 @@ int arm_simulate_step(target_t *target, u32 *dry_run_pc)
 		target_read_u32(target, current_pc, &opcode);
 		arm_evaluate_opcode(opcode, current_pc, &instruction);
 		instruction_size = 4;
+		
+		/* check condition code (for all instructions) */
+		if (!pass_condition(buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32), opcode))
+		{
+			if (dry_run_pc)
+			{
+				*dry_run_pc = current_pc + instruction_size;
+			}
+			else
+			{
+				buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, current_pc + instruction_size);
+			}
+			
+			return ERROR_OK;
+		}
 	}
 	else
 	{
-		/* TODO: add support for Thumb instruction set */
+		target_read_u32(target, current_pc, &opcode);
+		arm_evaluate_opcode(opcode, current_pc, &instruction);
 		instruction_size = 2;
-	}
-	
-	/* check condition code */
-	if (!pass_condition(buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32), opcode))
-	{
-		if (dry_run_pc)
-		{
-			*dry_run_pc = current_pc + instruction_size;
-		}
-		else
+		
+		/* check condition code (only for branch instructions) */
+		if ((!thumb_pass_branch_condition(buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32), opcode)) &&
+			(instruction.type == ARM_B))
 		{
-			buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, current_pc + instruction_size);
+			if (dry_run_pc)
+			{
+				*dry_run_pc = current_pc + instruction_size;
+			}
+			else
+			{
+				buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, current_pc + instruction_size);
+			}
+			
+			return ERROR_OK;
 		}
-		
-		return ERROR_OK;
 	}
 	
 	/* examine instruction type */
diff --git a/src/target/armv4_5.h b/src/target/armv4_5.h
index 0ba94ff7..36264f35 100644
--- a/src/target/armv4_5.h
+++ b/src/target/armv4_5.h
@@ -23,7 +23,7 @@
 #include "register.h"
 #include "target.h"
 
-enum armv4_5_mode
+typedef enum armv4_5_mode
 {
 	ARMV4_5_MODE_USR = 16, 
 	ARMV4_5_MODE_FIQ = 17, 
@@ -33,16 +33,16 @@ enum armv4_5_mode
 	ARMV4_5_MODE_UND = 27,
 	ARMV4_5_MODE_SYS = 31,
 	ARMV4_5_MODE_ANY = -1
-};
+} armv4_5_mode_t;
 
 extern char* armv4_5_mode_strings[];
 
-enum armv4_5_state
+typedef enum armv4_5_state
 {
 	ARMV4_5_STATE_ARM,
 	ARMV4_5_STATE_THUMB,
 	ARMV4_5_STATE_JAZELLE,
-};
+} armv4_5_state_t;
 
 extern char* armv4_5_state_strings[];
 
diff --git a/src/target/etb.c b/src/target/etb.c
index 21f250aa..257c4b18 100644
--- a/src/target/etb.c
+++ b/src/target/etb.c
@@ -21,8 +21,11 @@
 #include "config.h"
 #endif
 
+#include <string.h>
+
 #include "arm7_9_common.h"
 #include "etb.h"
+#include "etm.h"
 
 #include "log.h"
 #include "types.h"
@@ -55,16 +58,7 @@ int etb_set_reg_w_exec(reg_t *reg, u8 *buf);
 int etb_write_reg(reg_t *reg, u32 value);
 int etb_read_reg(reg_t *reg);
 
-int handle_arm7_9_etb_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_arm7_9_etb_dump_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-char *etmv1_branch_reason_string[] =
-{
-	"normal pc change", "tracing enabled", "restart after FIFO overflow",
-	"exit from debug state", "peridoic synchronization point",
-	"reserved", "reserved", "reserved"
-};
-
+int handle_etb_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 
 int etb_set_instr(etb_t *etb, u32 new_instr)
 {
@@ -180,6 +174,74 @@ int etb_get_reg(reg_t *reg)
 	return ERROR_OK;
 }
 
+int etb_read_ram(etb_t *etb, u32 *data, int num_frames)
+{
+	scan_field_t fields[3];
+	int i;
+	
+	jtag_add_end_state(TAP_RTI);
+	etb_scann(etb, 0x0);
+	etb_set_instr(etb, 0xc);
+	
+	fields[0].device = etb->chain_pos;
+	fields[0].num_bits = 32;
+	fields[0].out_value = NULL;
+	fields[0].out_mask = NULL;
+	fields[0].in_value = NULL;
+	fields[0].in_check_value = NULL;
+	fields[0].in_check_mask = NULL;
+	fields[0].in_handler = NULL;
+	fields[0].in_handler_priv = NULL;
+	
+	fields[1].device = etb->chain_pos;
+	fields[1].num_bits = 7;
+	fields[1].out_value = malloc(1);
+	buf_set_u32(fields[1].out_value, 0, 7, 4);
+	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 = etb->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, NULL);
+
+	fields[0].in_handler = buf_to_u32_handler;
+	
+	for (i = 0; i < num_frames; i++)
+	{
+		/* ensure nR/W reamins set to read */
+		buf_set_u32(fields[2].out_value, 0, 1, 0);
+		
+		/* address remains set to 0x4 (RAM data) until we read the last frame */
+		if (i < num_frames - 1)
+			buf_set_u32(fields[1].out_value, 0, 7, 4);
+		else
+			buf_set_u32(fields[1].out_value, 0, 7, 0);
+		
+		fields[0].in_handler_priv = &data[i];
+		jtag_add_dr_scan(3, fields, -1, NULL);
+	}
+	
+	jtag_execute_queue();
+	
+	free(fields[1].out_value);
+	free(fields[2].out_value);
+	
+	return ERROR_OK;
+}
+
 int etb_read_reg_w_check(reg_t *reg, u8* check_value, u8* check_mask)
 {
 	etb_reg_t *etb_reg = reg->arch_info;
@@ -333,293 +395,266 @@ int etb_store_reg(reg_t *reg)
 	return etb_write_reg(reg, buf_get_u32(reg->value, 0, reg->size));
 }
 
-int etb_register_commands(struct command_context_s *cmd_ctx, command_t *arm7_9_cmd)
+int etb_register_commands(struct command_context_s *cmd_ctx)
 {
-	register_command(cmd_ctx, arm7_9_cmd, "etb", handle_arm7_9_etb_command, COMMAND_CONFIG, NULL);
+	command_t *etb_cmd;
+	
+	etb_cmd = register_command(cmd_ctx, NULL, "etb", NULL, COMMAND_ANY, "Embedded Trace Buffer");
+	
+	register_command(cmd_ctx, etb_cmd, "config", handle_etb_config_command, COMMAND_CONFIG, NULL);
+
+	return ERROR_OK;
+}
 
-	register_command(cmd_ctx, arm7_9_cmd, "etb_dump", handle_arm7_9_etb_dump_command, COMMAND_EXEC, "dump current ETB content");
+int handle_etb_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target;
+	jtag_device_t *jtag_device;
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	
+	if (argc != 2)
+	{
+		ERROR("incomplete 'etb config <target> <chain_pos>' command");
+		exit(-1);
+	}
+	
+	target = get_target_by_num(strtoul(args[0], NULL, 0));
+	
+	if (!target)
+	{
+		ERROR("target number '%s' not defined", args[0]);
+		exit(-1);
+	}
+	
+	if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
+		return ERROR_OK;
+	}
+	
+	jtag_device = jtag_get_device(strtoul(args[1], NULL, 0));
+	
+	if (!jtag_device)
+	{
+		ERROR("jtag device number '%s' not defined", args[1]);
+		exit(-1);
+	}
+	
+	if (arm7_9->etm_ctx)
+	{
+		etb_t *etb = malloc(sizeof(etb_t));
+		
+		arm7_9->etm_ctx->capture_driver_priv = etb;
+		
+		etb->chain_pos = strtoul(args[1], NULL, 0);
+		etb->cur_scan_chain = -1;
+		etb->reg_cache = NULL;
+		etb->ram_width = 0;
+		etb->ram_depth = 0;
+	}
+	else
+	{
+		ERROR("target has no ETM defined, ETB left unconfigured");
+	}
 
 	return ERROR_OK;
 }
 
-#define PIPESTAT(x) ((x) & 0x7)
-#define TRACEPKT(x) (((x) & 0x7fff8) >> 3)
-#define TRACESYNC(x) (((x) & 0x80000) >> 19)
+int etb_init(etm_context_t *etm_ctx)
+{
+	etb_t *etb = etm_ctx->capture_driver_priv;
+	
+	etb->etm_ctx = etm_ctx;
+	
+	/* identify ETB RAM depth and width */
+	etb_read_reg(&etb->reg_cache->reg_list[ETB_RAM_DEPTH]);
+	etb_read_reg(&etb->reg_cache->reg_list[ETB_RAM_WIDTH]);
+	jtag_execute_queue();
+
+	etb->ram_depth = buf_get_u32(etb->reg_cache->reg_list[ETB_RAM_DEPTH].value, 0, 32);
+	etb->ram_width = buf_get_u32(etb->reg_cache->reg_list[ETB_RAM_WIDTH].value, 0, 32);
+	
+	return ERROR_OK;
+}
 
-int etmv1_next_packet(int trace_depth, u32 *trace_data, int frame, int *port_half, int apo, u8 *packet)
+trace_status_t etb_status(etm_context_t *etm_ctx)
 {
-	while (frame < trace_depth)
+	etb_t *etb = etm_ctx->capture_driver_priv;
+	
+	etb->etm_ctx = etm_ctx;
+	
+	/* if tracing is currently idle, return this information */
+	if (etm_ctx->capture_status == TRACE_IDLE)
 	{
-		if (apo > 0)
-		{
-			if (TRACESYNC(trace_data[frame]))
-				apo--;
-		}
-		else
+		return etm_ctx->capture_status;
+	}
+	else if (etm_ctx->capture_status & TRACE_RUNNING)
+	{
+		reg_t *etb_status_reg = &etb->reg_cache->reg_list[ETB_STATUS];
+		int etb_timeout = 100;
+		
+		/* trace is running, check the ETB status flags */
+		etb_get_reg(etb_status_reg);
+	
+		/* check Full bit to identify an overflow */
+		if (buf_get_u32(etb_status_reg->value, 0, 1) == 1)
+			etm_ctx->capture_status |= TRACE_OVERFLOWED;
+
+		/* check Triggered bit to identify trigger condition */
+		if (buf_get_u32(etb_status_reg->value, 1, 1) == 1)
+			etm_ctx->capture_status |= TRACE_TRIGGERED;
+
+		/* check AcqComp to identify trace completion */
+		if (buf_get_u32(etb_status_reg->value, 2, 1) == 1)
 		{
-			/* we're looking for a branch address, skip if TRACESYNC isn't set */
-			if ((apo == 0) && (!TRACESYNC(trace_data[frame])))
+			while (etb_timeout-- && (buf_get_u32(etb_status_reg->value, 3, 1) == 0))
 			{
-				frame++;
-				continue;
+				/* wait for data formatter idle */
+				etb_get_reg(etb_status_reg);
 			}
-				
-			/* TRACEPKT is valid if this isn't a TD nor a TRIGGER cycle */
-			if (((PIPESTAT(trace_data[frame]) != 0x7) && (PIPESTAT(trace_data[frame]) != 0x6))
-				&& !((apo == 0) && (!TRACESYNC(trace_data[frame]))))
+			
+			if (etb_timeout == 0)
+			{
+				ERROR("AcqComp set but DFEmpty won't go high, ETB status: 0x%x",
+					buf_get_u32(etb_status_reg->value, 0, etb_status_reg->size));
+			}
+			
+			if (!(etm_ctx->capture_status && TRACE_TRIGGERED))
 			{
-				if (*port_half == 0)
-				{
-					*packet = TRACEPKT(trace_data[frame]) & 0xff;
-					*port_half = 1;
-				}
-				else
-				{
-					*packet = (TRACEPKT(trace_data[frame]) & 0xff00) >> 8;
-					*port_half = 0;
-					frame++;
-				}
-				return frame;
+				ERROR("trace completed, but no trigger condition detected");
 			}
+			
+			etm_ctx->capture_status &= ~TRACE_RUNNING;
+			etm_ctx->capture_status |= TRACE_COMPLETED;
 		}
-		frame++;
 	}
 	
-	/* we reached the end of the trace without finding the packet we're looking for
-	 * tracing is finished
-	 */
-	return -1;
+	return etm_ctx->capture_status;
 }
 
-int handle_arm7_9_etb_dump_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+int etb_read_trace(etm_context_t *etm_ctx)
 {
-	int retval;
-	target_t *target = get_current_target(cmd_ctx);
-	armv4_5_common_t *armv4_5;
-	arm7_9_common_t *arm7_9;
-	int i, j, k;
+	etb_t *etb = etm_ctx->capture_driver_priv;
 	int first_frame = 0;
-	int last_frame;
-	int addressbits_valid = 0;
-	u32 address = 0x0;
-	u32 *trace_data;
-	int port_half = 0;
-	int last_instruction = -1;
-	u8 branch_reason;
-	u8 packet;
-	char trace_output[256];
-	int trace_output_len;
-	u8 apo;
-
-	if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
-	{
-		command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
-		return ERROR_OK;
-	}
+	int num_frames = etb->ram_depth;
+	u32 *trace_data = NULL;
+	int i, j;
+	
+	etb_read_reg(&etb->reg_cache->reg_list[ETB_STATUS]);
+	etb_read_reg(&etb->reg_cache->reg_list[ETB_RAM_WRITE_POINTER]);
+	jtag_execute_queue();
 	
-	if (!arm7_9->etb)
+	/* check if we overflowed, and adjust first frame of the trace accordingly
+	 * if we didn't overflow, read only up to the frame that would be written next,
+	 * i.e. don't read invalid entries
+	 */
+	if (buf_get_u32(etb->reg_cache->reg_list[ETB_STATUS].value, 0, 1))
 	{
-		command_print(cmd_ctx, "no ETB configured for current target");
-		return ERROR_OK;
+		first_frame = buf_get_u32(etb->reg_cache->reg_list[ETB_RAM_WRITE_POINTER].value, 0, 32);
 	}
-	
-	if (!(arm7_9->etb->RAM_depth && arm7_9->etb->RAM_width))
+	else
 	{
-		/* identify ETB RAM depth and width */
-		etb_read_reg(&arm7_9->etb->reg_cache->reg_list[ETB_RAM_DEPTH]);
-		etb_read_reg(&arm7_9->etb->reg_cache->reg_list[ETB_RAM_WIDTH]);
-		jtag_execute_queue();
-	
-		arm7_9->etb->RAM_depth = buf_get_u32(arm7_9->etb->reg_cache->reg_list[ETB_RAM_DEPTH].value, 0, 32);
-		arm7_9->etb->RAM_width = buf_get_u32(arm7_9->etb->reg_cache->reg_list[ETB_RAM_WIDTH].value, 0, 32);
+		num_frames = buf_get_u32(etb->reg_cache->reg_list[ETB_RAM_WRITE_POINTER].value, 0, 32);
 	}
 	
-	trace_data = malloc(sizeof(u32) * arm7_9->etb->RAM_depth);
-	
-	etb_read_reg(&arm7_9->etb->reg_cache->reg_list[ETB_STATUS]);
-	etb_read_reg(&arm7_9->etb->reg_cache->reg_list[ETB_RAM_WRITE_POINTER]);
-	jtag_execute_queue();
-	
-	/* check if we overflowed, and adjust first and last frame of the trace accordingly */
-	if (buf_get_u32(arm7_9->etb->reg_cache->reg_list[ETB_STATUS].value, 1, 1))
+	etb_write_reg(&etb->reg_cache->reg_list[ETB_RAM_READ_POINTER], first_frame);
+
+	/* read data into temporary array for unpacking */	
+	trace_data = malloc(sizeof(u32) * num_frames);
+	etb_read_ram(etb, trace_data, num_frames);
+
+	if (etm_ctx->trace_depth > 0)
 	{
-		first_frame = buf_get_u32(arm7_9->etb->reg_cache->reg_list[ETB_RAM_WRITE_POINTER].value, 0, 32);
+		free(etm_ctx->trace_data);
 	}
 	
-	last_frame = buf_get_u32(arm7_9->etb->reg_cache->reg_list[ETB_RAM_WRITE_POINTER].value, 0, 32) - 1;
-	
-	etb_write_reg(&arm7_9->etb->reg_cache->reg_list[ETB_RAM_READ_POINTER], first_frame);
+	if ((etm_ctx->portmode & ETM_PORT_MODE_MASK) == ETM_PORT_DEMUXED)
+		etm_ctx->trace_depth = num_frames * 2;
+	else
+		etm_ctx->trace_depth = num_frames;
 
-	/* read trace data from ETB */
-	i = first_frame;
-	j = 0;
-	do {
-		etb_read_reg(&arm7_9->etb->reg_cache->reg_list[ETB_RAM_DATA]);
-		jtag_execute_queue();
-		trace_data[j++] = buf_get_u32(arm7_9->etb->reg_cache->reg_list[ETB_RAM_DATA].value, 0, 32);
-		i++;
-	} while ((i % arm7_9->etb->RAM_depth) != (first_frame % arm7_9->etb->RAM_depth));
+	etm_ctx->trace_data= malloc(sizeof(etmv1_trace_data_t) * etm_ctx->trace_depth);
 	
-	for (i = 0, j = 0; i < arm7_9->etb->RAM_depth; i++)
+	for (i = 0, j = 0; i < num_frames; i++)
 	{
-		int trigger = 0;
-		
-		trace_output_len = 0;
-		
-		/* catch trigger, actual PIPESTAT is encoded in TRACEPKT[2:0] */
-		if (PIPESTAT(trace_data[i]) == 0x6)
+		if ((etm_ctx->portmode & ETM_PORT_MODE_MASK) == ETM_PORT_DEMUXED)
 		{
-			trigger = 1;
-			trace_data[i] &= ~0x7;
-			trace_data[i] |= TRACEPKT(trace_data[i]) & 0x7;
-		}
-	
-		if (addressbits_valid == 32)
-		{
-			trace_output_len += snprintf(trace_output + trace_output_len, 256 - trace_output_len,
-				"%i: 0x%8.8x %s", i, address, (trigger) ? "(TRIGGER) " : "");
-		}
-		else if (addressbits_valid != 0)
-		{
-			trace_output_len += snprintf(trace_output + trace_output_len, 256 - trace_output_len,
-				"%i: 0x...%x %s", i, address, (trigger) ? "(TRIGGER) " : "");
+			etm_ctx->trace_data[j].pipestat = trace_data[i] & 0x7;
+			etm_ctx->trace_data[j].packet = (trace_data[i] & 0x7f8) >> 3;
+			etm_ctx->trace_data[j].tracesync = (trace_data[i] & 0x800) >> 11;
+
+			etm_ctx->trace_data[j+1].pipestat = (trace_data[i] & 0x7000) >> 12;
+			etm_ctx->trace_data[j+1].packet = (trace_data[i] & 0x7f8000) >> 15;
+			etm_ctx->trace_data[j+1].tracesync = (trace_data[i] & 0x800000) >> 23;
+			
+			j += 2;
 		}
 		else
 		{
-			trace_output_len += snprintf(trace_output + trace_output_len, 256 - trace_output_len,
-				"%i: 0xUNK %s", i, (trigger) ? "(TRIGGER) " : "");
-		}
-		
-		switch (PIPESTAT(trace_data[i]))
-		{
-			case 0x0:
-				trace_output_len += snprintf(trace_output + trace_output_len, 256 - trace_output_len,
-					"IE");
-				break;
-			case 0x1:
-				trace_output_len += snprintf(trace_output + trace_output_len, 256 - trace_output_len,
-					"ID");
-				break;
-			case 0x2:
-				/* Instruction exectued - TRACEPKT might be valid, but belongs to another cycle */
-				trace_output_len += snprintf(trace_output + trace_output_len, 256 - trace_output_len,
-					"IN");
-				break;
-			case 0x3:
-				/* WAIT cycle - TRACEPKT is valid, but belongs to another cycle */
-				trace_output_len += snprintf(trace_output + trace_output_len, 256 - trace_output_len,
-					"WT");
-				break;
-			case 0x4:
-				/* following a branch two APO cycles are output on PIPESTAT[1:0]
-				 * but another BE/BD could overwrite the current branch,
-				 * or a trigger could cause the APO to be output on TRACEPKT[1:0]
-				 */
-				if ((PIPESTAT(trace_data[i + 1]) == 0x4)
-					|| (PIPESTAT(trace_data[i + 1]) == 0x5))
-				{
-					/* another branch occured, we ignore this one */
-					j = (j < i + 1) ? i + 1 : j;
-					break;
-				}
-				else if (PIPESTAT(trace_data[i + 1]) == 0x6)
-				{
-					apo = TRACEPKT(trace_data[i + 1]) & 0x3;
-				}
-				else
-				{
-					apo = PIPESTAT(trace_data[i + 1]) & 0x3;
-				}
-
-				if ((PIPESTAT(trace_data[i + 2]) == 0x4)
-					|| (PIPESTAT(trace_data[i + 2]) == 0x5))
-				{
-					j = (j < i + 2) ? i + 1 : j;
-					i = i + 1;
-					break;
-				}
-				else if (PIPESTAT(trace_data[i + 2]) == 0x6)
-				{
-					apo |= (TRACEPKT(trace_data[i + 2]) & 0x3) << 2;
-				}
-				else
-				{
-					apo = (PIPESTAT(trace_data[i + 1]) & 0x3) << 2;
-				}
-				
-				branch_reason = -1;
-				k = 0;
-				do
-				{
-					if ((j = etmv1_next_packet(arm7_9->etb->RAM_depth, trace_data, j, &port_half, apo, &packet)) != -1)
-					{
-						address &= ~(0x7f << (k * 7));
-						address |= (packet & 0x7f) << (k * 7);
-					}
-					else
-					{
-						break;
-					}
-					k++;
-				} while ((k < 5) && (packet & 0x80));
-				
-				if (addressbits_valid < ((k * 7 > 32) ? 32 : k * 7))
-					addressbits_valid = (k * 7 > 32) ? 32 : k * 7;
-				
-				if (k == 5)
-				{
-					branch_reason = (packet & 0x7) >> 4;
-					trace_output_len += snprintf(trace_output + trace_output_len, 256 - trace_output_len,
-						"BE 0x%x (/%i) (%s)", address, addressbits_valid, etmv1_branch_reason_string[branch_reason]);
-				}
-				else
-				{
-					trace_output_len += snprintf(trace_output + trace_output_len, 256 - trace_output_len,
-						"BE 0x%x (/%i)", address, addressbits_valid);
-				}
-				
-				break;
-			case 0x5:
-				trace_output_len += snprintf(trace_output + trace_output_len, 256 - trace_output_len,
-					"BD");
-				break;
-			case 0x6:
-				/* We catch the trigger event before we get here */
-				ERROR("TR pipestat should have been caught earlier");
-				trace_output_len += snprintf(trace_output + trace_output_len, 256 - trace_output_len,
-					"--");
-				break;
-			case 0x7:
-				/* TRACE disabled - TRACEPKT = invalid */
-				trace_output_len += snprintf(trace_output + trace_output_len, 256 - trace_output_len,
-					"TD");
-				break;
-		}
-		
-		/* PIPESTAT other than WT (b011) and TD (b111) mean we executed an instruction */
-		if ((PIPESTAT(trace_data[i]) & 0x3) != 0x3)
-		{
-			last_instruction = i;
-			address += 4;
+			etm_ctx->trace_data[j].pipestat = trace_data[i] & 0x7;
+			etm_ctx->trace_data[j].packet = (trace_data[i] & 0x7fff8) >> 3;
+			etm_ctx->trace_data[j].tracesync = (trace_data[i] & 0x80000) >> 19;
+			
+			j += 1;
 		}
+	}
+	
+	free(trace_data);
+	
+	return ERROR_OK;
+}
 
-		/* The group of packets for a particular instruction cannot start on or before any
-		 * previous functional PIPESTAT (IE, IN, ID, BE, or BD)
-		 */
-		if (j < last_instruction)
-		{
-			j = last_instruction + 1;
-		}
+int etb_start_capture(etm_context_t *etm_ctx)
+{
+	etb_t *etb = etm_ctx->capture_driver_priv;
+	u32 etb_ctrl_value = 0x1;
 
-		/* restore trigger PIPESTAT to ensure TRACEPKT is ignored */		
-		if (trigger == 1)
+	if ((etm_ctx->portmode & ETM_PORT_MODE_MASK) == ETM_PORT_DEMUXED)
+	{
+		if ((etm_ctx->portmode & ETM_PORT_WIDTH_MASK) == ETM_PORT_16BIT)
 		{
-			trace_data[i] &= ~0x7;
-			trace_data[i] |= 0x6;	
+			DEBUG("ETB can't run in demultiplexed mode with a 16-bit port");
+			return ERROR_ETM_PORTMODE_NOT_SUPPORTED;
 		}
-		
-		command_print(cmd_ctx, "%s (raw: 0x%8.8x)", trace_output, trace_data[i]);
+		etb_ctrl_value |= 0x2;
 	}
 	
+	if ((etm_ctx->portmode & ETM_PORT_MODE_MASK) == ETM_PORT_MUXED)
+		return ERROR_ETM_PORTMODE_NOT_SUPPORTED;
+	
+	etb_write_reg(&etb->reg_cache->reg_list[ETB_TRIGGER_COUNTER], 0x600);
+	etb_write_reg(&etb->reg_cache->reg_list[ETB_RAM_WRITE_POINTER], 0x0);
+	etb_write_reg(&etb->reg_cache->reg_list[ETB_CTRL], etb_ctrl_value);
+	jtag_execute_queue();
+	
+	/* we're starting a new trace, initialize capture status */
+	etm_ctx->capture_status = TRACE_RUNNING;
+	
+	return ERROR_OK; 
+}
+
+int etb_stop_capture(etm_context_t *etm_ctx)
+{
+	etb_t *etb = etm_ctx->capture_driver_priv;
+	reg_t *etb_ctrl_reg = &etb->reg_cache->reg_list[ETB_CTRL];
+
+	etb_write_reg(etb_ctrl_reg, 0x0);
+	jtag_execute_queue();
+	
+	/* trace stopped, just clear running flag, but preserve others */ 
+	etm_ctx->capture_status &= ~TRACE_RUNNING;
+	
 	return ERROR_OK;
 }
+
+etm_capture_driver_t etb_capture_driver =
+{
+	.name = "etb",
+	.register_commands = etb_register_commands,
+	.init = etb_init,
+	.status = etb_status,
+	.start_capture = etb_start_capture,
+	.stop_capture = etb_stop_capture,
+	.read_trace = etb_read_trace,
+};
diff --git a/src/target/etb.h b/src/target/etb.h
index 12e613ff..1a579cb3 100644
--- a/src/target/etb.h
+++ b/src/target/etb.h
@@ -25,6 +25,9 @@
 #include "register.h"

 #include "arm_jtag.h"

 

+#include "etb.h"

+#include "etm.h"

+

 /* ETB registers */

 enum

 {

@@ -41,13 +44,14 @@ enum
 

 typedef struct etb_s

 {

+	etm_context_t *etm_ctx;

 	int chain_pos;

 	int cur_scan_chain;

 	reg_cache_t *reg_cache;

 	

 	/* ETB parameters */

-	int RAM_depth;

-	int RAM_width;

+	int ram_depth;

+	int ram_width;

 } etb_t;

 

 typedef struct etb_reg_s

@@ -56,6 +60,8 @@ typedef struct etb_reg_s
 	etb_t *etb;

 } etb_reg_t;

 

+extern etm_capture_driver_t etb_capture_driver;

+

 extern reg_cache_t* etb_build_reg_cache(etb_t *etb);

 extern int etb_read_reg(reg_t *reg);

 extern int etb_write_reg(reg_t *reg, u32 value);

@@ -64,6 +70,6 @@ extern int etb_store_reg(reg_t *reg);
 extern int etb_set_reg(reg_t *reg, u32 value);

 extern int etb_set_reg_w_exec(reg_t *reg, u8 *buf);

 

-extern int etb_register_commands(struct command_context_s *cmd_ctx, command_t *arm7_9_cmd);

+extern int etb_register_commands(struct command_context_s *cmd_ctx);

 

 #endif /* ETB_H */

diff --git a/src/target/etm.c b/src/target/etm.c
index 016130d5..02c33104 100644
--- a/src/target/etm.c
+++ b/src/target/etm.c
@@ -21,7 +21,10 @@
 #include "config.h"
 #endif
 
+#include <string.h>
+
 #include "etm.h"
+#include "etb.h"
 
 #include "armv4_5.h"
 #include "arm7_9_common.h"
@@ -33,9 +36,14 @@
 #include "target.h"
 #include "register.h"
 #include "jtag.h"
+#include "fileio.h"
 
 #include <stdlib.h>
 
+/* ETM register access functionality 
+ * 
+ */
+
 bitfield_desc_t etm_comms_ctrl_bitfield_desc[] = 
 {
 	{"R", 1},
@@ -204,13 +212,16 @@ int etm_set_reg_w_exec(reg_t *reg, u8 *buf);
 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)
+command_t *etm_cmd = NULL;
+
+reg_cache_t* etm_build_reg_cache(target_t *target, arm_jtag_t *jtag_info, etm_context_t *etm_ctx)
 {
 	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;
+	u32 etm_ctrl_value;
 	
 	/* register a register arch-type for etm registers only once */
 	if (etm_reg_arch_type == -1)
@@ -242,6 +253,44 @@ reg_cache_t* etm_build_reg_cache(target_t *target, arm_jtag_t *jtag_info, int ex
 		arch_info[i].addr = etm_reg_arch_info[i];
 		arch_info[i].jtag_info = jtag_info;
 	}
+
+	/* initialize some ETM control register settings */	
+	etm_get_reg(&reg_list[ETM_CTRL]);
+	etm_ctrl_value = buf_get_u32(reg_list[ETM_CTRL].value, 0, reg_list[ETM_CTRL].size);
+	
+	/* clear the ETM powerdown bit (0) */
+	etm_ctrl_value &= ~0x1;
+		
+	/* configure port width (6:4), mode (17:16) and clocking (13) */
+	etm_ctrl_value = (etm_ctrl_value & 
+		~ETM_PORT_WIDTH_MASK & ~ETM_PORT_MODE_MASK & ~ETM_PORT_CLOCK_MASK)
+		| etm_ctx->portmode;
+	
+	buf_set_u32(reg_list[ETM_CTRL].value, 0, reg_list[ETM_CTRL].size, etm_ctrl_value);
+	etm_store_reg(&reg_list[ETM_CTRL]);
+	
+	/* the ETM might have an ETB connected */
+	if (strcmp(etm_ctx->capture_driver->name, "etb") == 0)
+	{
+		etb_t *etb = etm_ctx->capture_driver_priv;
+		
+		if (!etb)
+		{
+			ERROR("etb selected as etm capture driver, but no ETB configured");
+			return ERROR_OK;
+		}
+		
+		reg_cache->next = etb_build_reg_cache(etb);
+		
+		etb->reg_cache = reg_cache->next;
+		
+		if (etm_ctx->capture_driver->init(etm_ctx) != ERROR_OK)
+		{
+			ERROR("ETM capture driver initialization failed");
+			exit(-1);
+		}
+	}
+	
 	return reg_cache;
 }
 
@@ -410,3 +459,638 @@ int etm_store_reg(reg_t *reg)
 	return etm_write_reg(reg, buf_get_u32(reg->value, 0, reg->size));
 }
 
+/* ETM trace analysis functionality
+ * 
+ */
+extern etm_capture_driver_t etb_capture_driver;
+
+etm_capture_driver_t *etm_capture_drivers[] = 
+{
+	&etb_capture_driver,
+	NULL
+};
+
+char *etmv1v1_branch_reason_strings[] =
+{
+	"normal PC change",
+	"tracing enabled",
+	"trace restarted after overflow",
+	"exit from debug",
+	"periodic synchronization",
+	"reserved",
+	"reserved",
+	"reserved",
+};
+
+int etmv1_next_packet(etm_context_t *ctx, u8 *packet)
+{
+	
+	
+	return ERROR_OK;
+}
+
+int etmv1_analyse_trace(etm_context_t *ctx)
+{
+	ctx->pipe_index = 0;
+	ctx->data_index = 0;
+	
+	while (ctx->pipe_index < ctx->trace_depth)
+	{
+		switch (ctx->trace_data[ctx->pipe_index].pipestat)
+		{
+			case STAT_IE:
+			case STAT_ID:
+				break;
+			case STAT_IN:
+				DEBUG("IN");
+				break;
+			case STAT_WT:
+				DEBUG("WT");
+				break;
+			case STAT_BE:
+			case STAT_BD:
+				break;
+			case STAT_TD:
+				/* TODO: in cycle accurate trace, we have to count cycles */
+				DEBUG("TD");
+				break;
+		}
+	}
+	
+	return ERROR_OK;
+}
+
+int handle_etm_tracemode_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target;
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	etmv1_tracemode_t tracemode;
+	
+	target = get_current_target(cmd_ctx);
+	
+	if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
+		return ERROR_OK;
+	}
+	
+	if (!arm7_9->etm_ctx)
+	{
+		command_print(cmd_ctx, "current target doesn't have an ETM configured");
+		return ERROR_OK;
+	}
+	
+	tracemode = arm7_9->etm_ctx->tracemode;
+
+	if (argc == 3)
+	{
+		if (strcmp(args[0], "none") == 0)
+		{
+			tracemode = ETMV1_TRACE_NONE;
+		}
+		else if (strcmp(args[0], "data") == 0)
+		{
+			tracemode = ETMV1_TRACE_DATA;
+		}
+		else if (strcmp(args[0], "address") == 0)
+		{
+			tracemode = ETMV1_TRACE_ADDR;
+		}
+		else if (strcmp(args[0], "all") == 0)
+		{
+			tracemode = ETMV1_TRACE_DATA | ETMV1_TRACE_ADDR;
+		}
+		else
+		{
+			command_print(cmd_ctx, "invalid option '%s'", args[0]);
+			return ERROR_OK;
+		}
+		
+		switch (strtol(args[1], NULL, 0))
+		{
+			case 0:
+				tracemode |= ETMV1_CONTEXTID_NONE;
+				break;
+			case 8:
+				tracemode |= ETMV1_CONTEXTID_8;
+				break;
+			case 16:
+				tracemode |= ETMV1_CONTEXTID_16;
+				break;
+			case 32:
+				tracemode |= ETMV1_CONTEXTID_32;
+				break;
+			default:
+				command_print(cmd_ctx, "invalid option '%s'", args[1]);
+				return ERROR_OK;
+		}
+		
+		if (strcmp(args[2], "enable") == 0)
+		{
+			tracemode |= ETMV1_CYCLE_ACCURATE;
+		}
+		else if (strcmp(args[2], "disable") == 0)
+		{
+			tracemode |= 0;
+		}
+		else
+		{
+			command_print(cmd_ctx, "invalid option '%s'", args[2]);
+			return ERROR_OK;
+		}
+	}
+	else if (argc != 0)
+	{
+		command_print(cmd_ctx, "usage: configure trace mode <none|data|address|all> <context id bits> <enable|disable cycle accurate>");
+		return ERROR_OK;
+	}
+	
+	command_print(cmd_ctx, "current tracemode configuration:");
+	
+	switch (tracemode & ETMV1_TRACE_MASK)
+	{
+		case ETMV1_TRACE_NONE:
+			command_print(cmd_ctx, "data tracing: none");
+			break;
+		case ETMV1_TRACE_DATA:
+			command_print(cmd_ctx, "data tracing: data only");
+			break;
+		case ETMV1_TRACE_ADDR:
+			command_print(cmd_ctx, "data tracing: address only");
+			break;
+		case ETMV1_TRACE_DATA | ETMV1_TRACE_ADDR:
+			command_print(cmd_ctx, "data tracing: address and data");
+			break;
+	}
+	
+	switch (tracemode & ETMV1_CONTEXTID_MASK)
+	{
+		case ETMV1_CONTEXTID_NONE:
+			command_print(cmd_ctx, "contextid tracing: none");
+			break;
+		case ETMV1_CONTEXTID_8:
+			command_print(cmd_ctx, "contextid tracing: 8 bit");
+			break;
+		case ETMV1_CONTEXTID_16:
+			command_print(cmd_ctx, "contextid tracing: 16 bit");
+			break;
+		case ETMV1_CONTEXTID_32:
+			command_print(cmd_ctx, "contextid tracing: 32 bit");
+			break;
+	}
+	
+	if (tracemode & ETMV1_CYCLE_ACCURATE)
+	{
+		command_print(cmd_ctx, "cycle-accurate tracing enabled");
+	}
+	else
+	{
+		command_print(cmd_ctx, "cycle-accurate tracing disabled");
+	}
+	
+	/* only update ETM_CTRL register if tracemode changed */
+	if (arm7_9->etm_ctx->tracemode != tracemode)
+	{
+		reg_t *etm_ctrl_reg = &arm7_9->etm_ctx->reg_cache->reg_list[ETM_CTRL];
+		
+		etm_get_reg(etm_ctrl_reg);
+		
+		buf_set_u32(etm_ctrl_reg->value, 2, 2, tracemode & ETMV1_TRACE_MASK);
+		buf_set_u32(etm_ctrl_reg->value, 14, 2, (tracemode & ETMV1_CONTEXTID_MASK) >> 4);
+		buf_set_u32(etm_ctrl_reg->value, 12, 1, (tracemode & ETMV1_CYCLE_ACCURATE) >> 8);
+		
+		etm_store_reg(etm_ctrl_reg);
+		
+		arm7_9->etm_ctx->tracemode = tracemode;
+		
+		/* invalidate old trace data */
+		arm7_9->etm_ctx->capture_status = TRACE_IDLE;
+		if (arm7_9->etm_ctx->trace_depth > 0)
+		{
+			free(arm7_9->etm_ctx->trace_data);
+		}
+		arm7_9->etm_ctx->trace_depth = 0;
+	}
+	
+	return ERROR_OK;
+}
+
+int handle_etm_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target;
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	etm_portmode_t portmode = 0x0;
+	etm_context_t *etm_ctx = malloc(sizeof(etm_context_t));
+	int i;
+	
+	if (argc != 5)
+	{
+		ERROR("incomplete 'etm config <target> <port_width> <port_mode> <clocking> <capture_driver>' command");
+		exit(-1);
+	}
+	
+	target = get_target_by_num(strtoul(args[0], NULL, 0));
+	
+	if (!target)
+	{
+		ERROR("target number '%s' not defined", args[0]);
+		exit(-1);
+	}
+	
+	if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
+		return ERROR_OK;
+	}
+	
+	switch (strtoul(args[1], NULL, 0))
+	{
+		case 4:
+			portmode |= ETM_PORT_4BIT;
+			break;
+		case 8:
+			portmode |= ETM_PORT_8BIT;
+			break;
+		case 16:
+			portmode |= ETM_PORT_16BIT;
+			break;
+		default:
+			command_print(cmd_ctx, "unsupported ETM port width '%s', must be 4, 8 or 16", args[1]);
+			return ERROR_OK;
+	}
+	
+	if (strcmp("normal", args[2]) == 0)
+	{
+		portmode |= ETM_PORT_NORMAL;
+	}
+	else if (strcmp("multiplexed", args[2]) == 0)
+	{
+		portmode |= ETM_PORT_MUXED;
+	}
+	else if (strcmp("demultiplexed", args[2]) == 0)
+	{
+		portmode |= ETM_PORT_DEMUXED;
+	}
+	else
+	{
+		command_print(cmd_ctx, "unsupported ETM port mode '%s', must be 'normal', 'multiplexed' or 'demultiplexed'", args[2]);
+		return ERROR_OK;
+	}
+	
+	if (strcmp("half", args[3]) == 0)
+	{
+		portmode |= ETM_PORT_HALF_CLOCK;
+	}
+	else if (strcmp("full", args[3]) == 0)
+	{
+		portmode |= ETM_PORT_FULL_CLOCK;
+	}
+	else
+	{
+		command_print(cmd_ctx, "unsupported ETM port clocking '%s', must be 'full' or 'half'", args[3]);
+		return ERROR_OK;
+	}
+	
+	for (i=0; etm_capture_drivers[i]; i++)
+	{
+		if (strcmp(args[4], etm_capture_drivers[i]->name) == 0)
+		{
+			if (etm_capture_drivers[i]->register_commands(cmd_ctx) != ERROR_OK)
+			{
+				free(etm_ctx);
+				exit(-1);
+			}
+		
+			etm_ctx->capture_driver = etm_capture_drivers[i];
+
+			break;
+		}
+	}
+	
+	etm_ctx->trace_data = NULL;
+	etm_ctx->trace_depth = 0;
+	etm_ctx->portmode = portmode;
+	etm_ctx->tracemode = 0x0;
+	etm_ctx->core_state = ARMV4_5_STATE_ARM;
+	etm_ctx->pipe_index = 0;
+	etm_ctx->data_index = 0;
+	etm_ctx->current_pc = 0x0;
+	etm_ctx->pc_ok = 0;
+	etm_ctx->last_branch = 0x0;
+	etm_ctx->last_ptr = 0x0;
+	etm_ctx->context_id = 0x0;
+	
+	arm7_9->etm_ctx = etm_ctx;
+	
+	etm_register_user_commands(cmd_ctx);
+	
+	return ERROR_OK;
+}
+
+int handle_etm_status_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target;
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	trace_status_t trace_status;
+	
+	target = get_current_target(cmd_ctx);
+	
+	if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
+		return ERROR_OK;
+	}
+	
+	if (!arm7_9->etm_ctx)
+	{
+		command_print(cmd_ctx, "current target doesn't have an ETM configured");
+		return ERROR_OK;
+	}
+	
+	trace_status = arm7_9->etm_ctx->capture_driver->status(arm7_9->etm_ctx);
+	
+	if (trace_status == TRACE_IDLE)
+	{
+		command_print(cmd_ctx, "tracing is idle");
+	}
+	else
+	{
+		static char *completed = " completed";
+		static char *running = " is running";
+		static char *overflowed = ", trace overflowed";
+		static char *triggered = ", trace triggered";
+		
+		command_print(cmd_ctx, "trace collection%s%s%s", 
+			(trace_status & TRACE_RUNNING) ? running : completed,
+			(trace_status & TRACE_OVERFLOWED) ? overflowed : "",
+			(trace_status & TRACE_TRIGGERED) ? triggered : "");
+		
+		if (arm7_9->etm_ctx->trace_depth > 0)
+		{
+			command_print(cmd_ctx, "%i frames of trace data read", arm7_9->etm_ctx->trace_depth);
+		}
+	}
+	
+	return ERROR_OK;
+}
+
+int handle_etm_dump_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	fileio_t file;
+	target_t *target;
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	etm_context_t *etm_ctx;
+	u32 size_written;
+	
+	if (argc != 1)
+	{
+		command_print(cmd_ctx, "usage: etm dump <file>");
+		return ERROR_OK;
+	}
+	
+	target = get_current_target(cmd_ctx);
+	
+	if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
+		return ERROR_OK;
+	}
+	
+	if (!(etm_ctx = arm7_9->etm_ctx))
+	{
+		command_print(cmd_ctx, "current target doesn't have an ETM configured");
+		return ERROR_OK;
+	}
+	
+	if (etm_ctx->capture_driver->status == TRACE_IDLE)
+	{
+		command_print(cmd_ctx, "trace capture wasn't enabled, no trace data captured");
+		return ERROR_OK;
+	}
+
+	if (etm_ctx->capture_driver->status(etm_ctx) & TRACE_RUNNING)
+	{
+		/* TODO: if on-the-fly capture is to be supported, this needs to be changed */
+		command_print(cmd_ctx, "trace capture not completed");
+		return ERROR_OK;
+	}
+	
+	/* read the trace data if it wasn't read already */
+	if (etm_ctx->trace_depth == 0)
+		etm_ctx->capture_driver->read_trace(etm_ctx);
+	
+	if (fileio_open(&file, args[0], FILEIO_WRITE, FILEIO_BINARY) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "file open error: %s", file.error_str);
+		return ERROR_OK;
+	}
+	
+	//fileio_write(&file, etm_ctx->trace_depth * 4, (u8*)etm_ctx->trace_data, &size_written);
+	
+	fileio_close(&file);
+	
+	return ERROR_OK;	
+}
+
+int handle_etm_load_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	fileio_t file;
+	target_t *target;
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	etm_context_t *etm_ctx;
+	u32 size_read;
+	
+	if (argc != 1)
+	{
+		command_print(cmd_ctx, "usage: etm load <file>");
+		return ERROR_OK;
+	}
+	
+	target = get_current_target(cmd_ctx);
+	
+	if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
+		return ERROR_OK;
+	}
+	
+	if (!(etm_ctx = arm7_9->etm_ctx))
+	{
+		command_print(cmd_ctx, "current target doesn't have an ETM configured");
+		return ERROR_OK;
+	}
+	
+	if (etm_ctx->capture_driver->status(etm_ctx) & TRACE_RUNNING)
+	{
+		command_print(cmd_ctx, "trace capture running, stop first");
+		return ERROR_OK;
+	}
+	
+	if (fileio_open(&file, args[0], FILEIO_READ, FILEIO_BINARY) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "file open error: %s", file.error_str);
+		return ERROR_OK;
+	}
+	
+	if (file.size % 4)
+	{
+		command_print(cmd_ctx, "size isn't a multiple of 4, no valid trace data");
+		return ERROR_OK;
+	}
+	
+	if (etm_ctx->trace_depth > 0)
+	{
+		free(etm_ctx->trace_data);
+	}
+	
+	//fileio_read(&file, file.size, (u8*)etm_ctx->trace_data, &size_read);
+	etm_ctx->trace_depth = file.size / 4;
+	etm_ctx->capture_status = TRACE_COMPLETED;
+	
+	fileio_close(&file);
+	
+	return ERROR_OK;	
+}
+
+int handle_etm_start_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target;
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	etm_context_t *etm_ctx;
+	reg_t *etm_ctrl_reg;
+
+	target = get_current_target(cmd_ctx);
+	
+	if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
+		return ERROR_OK;
+	}
+	
+	if (!(etm_ctx = arm7_9->etm_ctx))
+	{
+		command_print(cmd_ctx, "current target doesn't have an ETM configured");
+		return ERROR_OK;
+	}
+	
+	/* invalidate old tracing data */
+	arm7_9->etm_ctx->capture_status = TRACE_IDLE;
+	if (arm7_9->etm_ctx->trace_depth > 0)
+	{
+		free(arm7_9->etm_ctx->trace_data);
+	}
+	arm7_9->etm_ctx->trace_depth = 0;
+		
+	etm_ctrl_reg = &arm7_9->etm_ctx->reg_cache->reg_list[ETM_CTRL];
+	etm_get_reg(etm_ctrl_reg);
+		
+	/* Clear programming bit (10), set port selection bit (11) */
+	buf_set_u32(etm_ctrl_reg->value, 10, 2, 0x2);
+
+	etm_store_reg(etm_ctrl_reg);
+	jtag_execute_queue();
+
+	etm_ctx->capture_driver->start_capture(etm_ctx);
+
+	return ERROR_OK;
+}
+
+int handle_etm_stop_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target;
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	etm_context_t *etm_ctx;
+	reg_t *etm_ctrl_reg;
+
+	target = get_current_target(cmd_ctx);
+	
+	if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
+		return ERROR_OK;
+	}
+	
+	if (!(etm_ctx = arm7_9->etm_ctx))
+	{
+		command_print(cmd_ctx, "current target doesn't have an ETM configured");
+		return ERROR_OK;
+	}
+	
+	etm_ctrl_reg = &arm7_9->etm_ctx->reg_cache->reg_list[ETM_CTRL];
+	etm_get_reg(etm_ctrl_reg);
+		
+	/* Set programming bit (10), clear port selection bit (11) */
+	buf_set_u32(etm_ctrl_reg->value, 10, 2, 0x1);
+
+	etm_store_reg(etm_ctrl_reg);	
+	jtag_execute_queue();
+	
+	etm_ctx->capture_driver->stop_capture(etm_ctx);
+	
+	return ERROR_OK;
+}
+
+int handle_etm_analyse_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+{
+	target_t *target;
+	armv4_5_common_t *armv4_5;
+	arm7_9_common_t *arm7_9;
+	etm_context_t *etm_ctx;
+
+	target = get_current_target(cmd_ctx);
+	
+	if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
+	{
+		command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
+		return ERROR_OK;
+	}
+	
+	if (!(etm_ctx = arm7_9->etm_ctx))
+	{
+		command_print(cmd_ctx, "current target doesn't have an ETM configured");
+		return ERROR_OK;
+	}
+	
+	etmv1_analyse_trace(etm_ctx);
+	
+	return ERROR_OK;
+}
+
+int etm_register_commands(struct command_context_s *cmd_ctx)
+{
+	etm_cmd = register_command(cmd_ctx, NULL, "etm", NULL, COMMAND_ANY, "Embedded Trace Macrocell");
+
+	register_command(cmd_ctx, etm_cmd, "config", handle_etm_config_command, COMMAND_CONFIG, NULL);
+
+	return ERROR_OK;
+}
+
+int etm_register_user_commands(struct command_context_s *cmd_ctx)
+{
+	register_command(cmd_ctx, etm_cmd, "tracemode", handle_etm_tracemode_command,
+		COMMAND_EXEC, "configure trace mode <none|data|address|all> <context id bits> <enable|disable cycle accurate>");
+
+	register_command(cmd_ctx, etm_cmd, "status", handle_etm_status_command,
+		COMMAND_EXEC, "display current target's ETM status");
+	register_command(cmd_ctx, etm_cmd, "start", handle_etm_start_command,
+		COMMAND_EXEC, "start ETM trace collection");
+	register_command(cmd_ctx, etm_cmd, "stop", handle_etm_stop_command,
+		COMMAND_EXEC, "stop ETM trace collection");
+
+	register_command(cmd_ctx, etm_cmd, "analyze", handle_etm_stop_command,
+		COMMAND_EXEC, "anaylze collected ETM trace");
+
+	register_command(cmd_ctx, etm_cmd, "dump", handle_etm_dump_command,
+		COMMAND_EXEC, "dump captured trace data <file>");
+	register_command(cmd_ctx, etm_cmd, "load", handle_etm_load_command,
+		COMMAND_EXEC, "load trace data for analysis <file>");
+
+	return ERROR_OK;
+}
diff --git a/src/target/etm.h b/src/target/etm.h
index 4b24e5c8..59591788 100644
--- a/src/target/etm.h
+++ b/src/target/etm.h
@@ -1,7 +1,10 @@
 /***************************************************************************

- *   Copyright (C) 2005 by Dominic Rath                                    *

+ *   Copyright (C) 2005, 2007 by Dominic Rath                              *

  *   Dominic.Rath@gmx.de                                                   *

  *                                                                         *

+ *   Copyright (C) 2007 by Vincent Palatin                                 *

+ *   vincent.palatin_openocd@m4x.org                                       *

+ *                                                                         *

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

@@ -20,11 +23,14 @@
 #ifndef ETM_H

 #define ETM_H

 

+#include "trace.h"

 #include "target.h"

 #include "register.h"

 #include "arm_jtag.h"

 

-// ETM registers (V1.2 protocol)

+#include "armv4_5.h"

+

+/* ETM registers (V1.3 protocol) */

 enum

 {

 	ETM_CTRL = 0x00,

@@ -58,14 +64,123 @@ enum
 	ETM_CONTEXTID_COMPARATOR_MASK = 0x6f,	

 };

 

-

 typedef struct etm_reg_s

 {

 	int addr;

 	arm_jtag_t *jtag_info;

 } etm_reg_t;

 

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

+typedef enum

+{

+	/* Port width */

+	ETM_PORT_4BIT		= 0x00,

+	ETM_PORT_8BIT		= 0x10,

+	ETM_PORT_16BIT		= 0x20,

+	ETM_PORT_WIDTH_MASK	= 0x70, 

+	/* Port modes */

+	ETM_PORT_NORMAL    = 0x00000,

+	ETM_PORT_MUXED     = 0x10000,

+	ETM_PORT_DEMUXED   = 0x20000,

+	ETM_PORT_MODE_MASK = 0x30000,

+	/* Clocking modes */

+	ETM_PORT_FULL_CLOCK = 0x0000,

+	ETM_PORT_HALF_CLOCK = 0x1000,

+	ETM_PORT_CLOCK_MASK = 0x1000,

+} etm_portmode_t;

+

+typedef enum

+{

+	/* Data trace */

+	ETMV1_TRACE_NONE	 = 0x00,

+	ETMV1_TRACE_DATA     = 0x01,

+	ETMV1_TRACE_ADDR     = 0x02,

+	ETMV1_TRACE_MASK     = 0x03,

+	/* ContextID */

+	ETMV1_CONTEXTID_NONE = 0x00,

+	ETMV1_CONTEXTID_8    = 0x10,

+	ETMV1_CONTEXTID_16   = 0x20,

+	ETMV1_CONTEXTID_32   = 0x30,

+	ETMV1_CONTEXTID_MASK = 0x30,

+	/* Misc */

+	ETMV1_CYCLE_ACCURATE = 0x100

+} etmv1_tracemode_t;

+

+/* forward-declare ETM context */

+struct etm_context_s;

+

+typedef struct etm_capture_driver_s

+{

+	char *name;

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

+	int (*init)(struct etm_context_s *etm_ctx);

+	trace_status_t (*status)(struct etm_context_s *etm_ctx);

+	int (*read_trace)(struct etm_context_s *etm_ctx);

+	int (*start_capture)(struct etm_context_s *etm_ctx);

+	int (*stop_capture)(struct etm_context_s *etm_ctx);

+} etm_capture_driver_t;

+

+typedef struct etmv1_trace_data_s

+{

+	u8 pipestat;	/* pipeline cycle this packet belongs to */

+	u16 packet;	/* packet data (4, 8 or 16 bit) */

+	int tracesync;	/* 1 if tracesync was set on this packet */

+} etmv1_trace_data_t;

+

+/* describe a trace context

+ * if support for ETMv2 or ETMv3 is to be implemented,

+ * this will have to be split into version independent elements

+ * and a version specific part

+ */

+typedef struct etm_context_s

+{

+	reg_cache_t *reg_cache;			/* ETM register cache */

+	etm_capture_driver_t *capture_driver;	/* driver used to access ETM data */

+	void *capture_driver_priv;		/* capture driver private data */

+	trace_status_t capture_status;	/* current state of capture run */ 

+	etmv1_trace_data_t *trace_data;	/* trace data */

+	u32 trace_depth;				/* number of trace cycles to be analyzed, 0 if no trace data available */

+	etm_portmode_t portmode;		/* normal, multiplexed or demultiplexed */

+	etmv1_tracemode_t tracemode;	/* type of information the trace contains (data, addres, contextID, ...) */ 

+	armv4_5_state_t core_state;		/* current core state (ARM, Thumb, Jazelle) */

+//	trace_image_provider_t image_provider; 	/* source for target opcodes */

+	u32 pipe_index;					/* current trace cycle */

+	u32 data_index;					/* cycle holding next data packet */

+	u32 current_pc;					/* current program counter */

+	u32 pc_ok;						/* full PC has been acquired */

+	u32 last_branch;				/* last branch address output */ 

+	u32 last_ptr;					/* address of the last data access */

+	u32 context_id;					/* context ID of the code being traced */

+} etm_context_t;

+

+/* PIPESTAT values */

+typedef enum

+{

+	STAT_IE = 0x0,

+	STAT_ID = 0x1,

+	STAT_IN = 0x2,

+	STAT_WT = 0x3,

+	STAT_BE = 0x4,

+	STAT_BD = 0x5,

+	STAT_TR = 0x6,

+	STAT_TD = 0x7

+} etmv1_pipestat_t;

+

+/* branch reason values */

+typedef enum

+{

+	BR_NORMAL  = 0x0, /* Normal PC change : periodic synchro (ETMv1.1) */

+	BR_ENABLE  = 0x1, /* Trace has been enabled */

+	BR_RESTART = 0x2, /* Trace restarted after a FIFO overflow */

+	BR_NODEBUG = 0x3, /* ARM has exited for debug state */

+	BR_PERIOD  = 0x4, /* Peridioc synchronization point (ETM>=v1.2)*/

+	BR_RSVD5   = 0x5, /* reserved */

+	BR_RSVD6   = 0x6, /* reserved */

+	BR_RSVD7   = 0x7, /* reserved */

+} etmv1_branch_reason_t;

+

+extern char *etmv1v1_branch_reason_strings[];

+

+extern reg_cache_t* etm_build_reg_cache(target_t *target, arm_jtag_t *jtag_info, etm_context_t *etm_ctx);

 extern int etm_read_reg(reg_t *reg);

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

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

@@ -73,4 +188,12 @@ extern int etm_store_reg(reg_t *reg);
 extern int etm_set_reg(reg_t *reg, u32 value);

 extern int etm_set_reg_w_exec(reg_t *reg, u8 *buf);

 

+int etm_register_commands(struct command_context_s *cmd_ctx);

+int etm_register_user_commands(struct command_context_s *cmd_ctx);

+extern etm_context_t* etm_create_context(etm_portmode_t portmode, char *capture_driver_name);

+

+#define ERROR_ETM_INVALID_DRIVER	(-1300)

+#define ERROR_ETM_PORTMODE_NOT_SUPPORTED	(-1301)

+#define ERROR_ETM_CAPTURE_INIT_FAILED	(-1302)

+

 #endif /* ETM_H */

diff --git a/src/target/target.c b/src/target/target.c
index 050a523e..e980ae4a 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -43,6 +43,7 @@
 #include <time_support.h>
 
 #include <fileio.h>
+#include <image.h>
 
 int cli_target_callback_event_handler(struct target_s *target, enum target_event event, void *priv);
 
@@ -1656,12 +1657,9 @@ int handle_load_image_command(struct command_context_s *cmd_ctx, char *cmd, char
 	u32 address;
 	u8 *buffer;
 	u32 buf_cnt;
-	u32 binary_size;
-	
-	fileio_t file;
-	enum fileio_pri_type pri_type = FILEIO_IMAGE;
-	fileio_image_t image_info;
-	enum fileio_sec_type sec_type;
+	u32 image_size;
+
+	image_t image;	
 	
 	duration_t duration;
 	char *duration_text;
@@ -1674,40 +1672,41 @@ int handle_load_image_command(struct command_context_s *cmd_ctx, char *cmd, char
 		return ERROR_OK;
 	}
 	
-	memset(&file, 0, sizeof(fileio_t));
-	fileio_identify_image_type(&sec_type, (argc == 3) ? args[2] : NULL);
+	identify_image_type(&image.type, (argc == 3) ? args[2] : NULL);
 
-	image_info.base_address = strtoul(args[1], NULL, 0);
-	image_info.has_start_address = 0;
+	image.base_address_set = 1;
+	image.base_address = strtoul(args[1], NULL, 0);
+	
+	image.start_address_set = 0;
 	
 	buffer = malloc(128 * 1024);
 
 	duration_start_measure(&duration);
 	
-	if (fileio_open(&file, args[0], FILEIO_READ, 
-		pri_type, &image_info, sec_type) != ERROR_OK)
+	if (image_open(&image, args[0], FILEIO_READ) != ERROR_OK)
 	{
-		command_print(cmd_ctx, "load_image error: %s", file.error_str);
+		command_print(cmd_ctx, "load_image error: %s", image.error_str);
 		return ERROR_OK;
 	}
 	
-	binary_size = file.size;
-	address = image_info.base_address;
-	while ((binary_size > 0) &&
-		(fileio_read(&file, 128 * 1024, buffer, &buf_cnt) == ERROR_OK))
+	image_size = image.size;
+	address = image.base_address;
+	
+	while ((image_size > 0) &&
+		(image_read(&image, 128 * 1024, buffer, &buf_cnt) == ERROR_OK))
 	{
 		target_write_buffer(target, address, buf_cnt, buffer);
 		address += buf_cnt;
-		binary_size -= buf_cnt;
+		image_size -= buf_cnt;
 	}
 
 	free(buffer);
 	
 	duration_stop_measure(&duration, &duration_text);
-	command_print(cmd_ctx, "downloaded %lli byte in %s", file.size, duration_text);
+	command_print(cmd_ctx, "downloaded %u byte in %s", image.size, duration_text);
 	free(duration_text);
 	
-	fileio_close(&file);
+	image_close(&image);
 
 	return ERROR_OK;
 
@@ -1715,8 +1714,7 @@ int handle_load_image_command(struct command_context_s *cmd_ctx, char *cmd, char
 
 int handle_dump_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
-	fileio_t file;
-	fileio_image_t image_info;
+	fileio_t fileio;
 	
 	u32 address;
 	u32 size;
@@ -1742,13 +1740,9 @@ int handle_dump_image_command(struct command_context_s *cmd_ctx, char *cmd, char
 		return ERROR_OK;
 	}
 	
-	image_info.base_address = address;
-	image_info.has_start_address = 0;
-	
-	if (fileio_open(&file, args[0], FILEIO_WRITE, 
-		FILEIO_IMAGE, &image_info, FILEIO_PLAIN) != ERROR_OK)
+	if (fileio_open(&fileio, args[0], FILEIO_WRITE, FILEIO_BINARY) != ERROR_OK)
 	{
-		command_print(cmd_ctx, "dump_image error: %s", file.error_str);
+		command_print(cmd_ctx, "dump_image error: %s", fileio.error_str);
 		return ERROR_OK;
 	}
 	
@@ -1760,16 +1754,16 @@ int handle_dump_image_command(struct command_context_s *cmd_ctx, char *cmd, char
 		u32 this_run_size = (size > 560) ? 560 : size;
 		
 		target->type->read_memory(target, address, 4, this_run_size / 4, buffer);
-		fileio_write(&file, this_run_size, buffer, &size_written);
+		fileio_write(&fileio, this_run_size, buffer, &size_written);
 		
 		size -= this_run_size;
 		address += this_run_size;
 	}
 
-	fileio_close(&file);
+	fileio_close(&fileio);
 
 	duration_stop_measure(&duration, &duration_text);
-	command_print(cmd_ctx, "dumped %lli byte in %s", file.size, duration_text);
+	command_print(cmd_ctx, "dumped %lli byte in %s", fileio.size, duration_text);
 	free(duration_text);
 	
 	return ERROR_OK;
diff --git a/src/target/xscale/debug_handler.bin b/src/target/xscale/debug_handler.bin
new file mode 100755
index 00000000..2dde1853
--- /dev/null
+++ b/src/target/xscale/debug_handler.bin