X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Ftarget%2Farm7_9_common.c;h=c3dcc2947d54a4cb1a2e931c9318ebba3c3e68c1;hp=a3ef8dea8b85dea94f46410604bf6434261ac464;hb=01a5d87d5f5788542c5d26da1c19fa4e634adc10;hpb=cb582796539d35920e918bec2d0118eb3736d40e

diff --git a/src/target/arm7_9_common.c b/src/target/arm7_9_common.c
index a3ef8dea8b..c3dcc2947d 100644
--- a/src/target/arm7_9_common.c
+++ b/src/target/arm7_9_common.c
@@ -25,6 +25,7 @@
 
 #include "embeddedice.h"
 #include "target.h"
+#include "target_request.h"
 #include "armv4_5.h"
 #include "arm_jtag.h"
 #include "jtag.h"
@@ -185,17 +186,33 @@ int arm7_9_set_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
 	{
 		if (breakpoint->length == 4)
 		{
+			u32 verify = 0xffffffff;
 			/* keep the original instruction in target endianness */
 			target->type->read_memory(target, breakpoint->address, 4, 1, breakpoint->orig_instr);
-			/* write the original instruction in target endianness (arm7_9->arm_bkpt is host endian) */
+			/* write the breakpoint instruction in target endianness (arm7_9->arm_bkpt is host endian) */
 			target_write_u32(target, breakpoint->address, arm7_9->arm_bkpt);
+			
+			target->type->read_memory(target, breakpoint->address, 4, 1, (u8 *)&verify);
+			if (verify != arm7_9->arm_bkpt)
+			{
+				ERROR("Unable to set 32 bit software breakpoint at address %08x", breakpoint->address);
+				return ERROR_OK;
+			}
 		}
 		else
 		{
+			u16 verify = 0xffff;
 			/* keep the original instruction in target endianness */
 			target->type->read_memory(target, breakpoint->address, 2, 1, breakpoint->orig_instr);
-			/* write the original instruction in target endianness (arm7_9->arm_bkpt is host endian) */
-			target_write_u32(target, breakpoint->address, arm7_9->thumb_bkpt);
+			/* write the breakpoint instruction in target endianness (arm7_9->thumb_bkpt is host endian) */
+			target_write_u16(target, breakpoint->address, arm7_9->thumb_bkpt);
+			
+			target->type->read_memory(target, breakpoint->address, 2, 1, (u8 *)&verify);
+			if (verify != arm7_9->thumb_bkpt)
+			{
+				ERROR("Unable to set thumb software breakpoint at address %08x", breakpoint->address);
+				return ERROR_OK;
+			}
 		}
 		breakpoint->set = 1;
 	}
@@ -242,11 +259,19 @@ int arm7_9_unset_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
 		/* restore original instruction (kept in target endianness) */
 		if (breakpoint->length == 4)
 		{
-			target->type->write_memory(target, breakpoint->address, 4, 1, breakpoint->orig_instr);
+			u32 current_instr;
+			/* check that user program as not modified breakpoint instruction */
+			target->type->read_memory(target, breakpoint->address, 4, 1, (u8*)&current_instr);
+			if (current_instr==arm7_9->arm_bkpt)
+				target->type->write_memory(target, breakpoint->address, 4, 1, breakpoint->orig_instr);
 		}
 		else
 		{
-			target->type->write_memory(target, breakpoint->address, 2, 1, breakpoint->orig_instr);
+			u16 current_instr;
+			/* check that user program as not modified breakpoint instruction */
+			target->type->read_memory(target, breakpoint->address, 2, 1, (u8*)&current_instr);
+			if (current_instr==arm7_9->thumb_bkpt)
+				target->type->write_memory(target, breakpoint->address, 2, 1, breakpoint->orig_instr);
 		}
 		breakpoint->set = 0;
 	}
@@ -544,7 +569,7 @@ int arm7_9_execute_sys_speed(struct target_s *target)
 				
 	/* set RESTART instruction */
 	jtag_add_end_state(TAP_RTI);
-	arm_jtag_set_instr(jtag_info, 0x4);
+	arm_jtag_set_instr(jtag_info, 0x4, NULL);
 	
 	for (timeout=0; timeout<50; timeout++)
 	{
@@ -568,7 +593,8 @@ int arm7_9_execute_sys_speed(struct target_s *target)
 
 int arm7_9_execute_fast_sys_speed(struct target_s *target)
 {
-	u8 check_value[4], check_mask[4];
+	static int set=0;
+	static u8 check_value[4], check_mask[4];
 	
 	armv4_5_common_t *armv4_5 = target->arch_info;
 	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
@@ -577,11 +603,20 @@ int arm7_9_execute_fast_sys_speed(struct target_s *target)
 				
 	/* set RESTART instruction */
 	jtag_add_end_state(TAP_RTI);
-	arm_jtag_set_instr(jtag_info, 0x4);
+	arm_jtag_set_instr(jtag_info, 0x4, NULL);
 	
-	/* check for DBGACK and SYSCOMP set (others don't care) */
-	buf_set_u32(check_value, 0, 32, 0x9);
-	buf_set_u32(check_mask, 0, 32, 0x9);
+	if (!set)
+	{
+		/* check for DBGACK and SYSCOMP set (others don't care) */
+		
+		/* NB! These are constants that must be available until after next jtag_execute() and
+		   we evaluate the values upon first execution in lieu of setting up these constants
+		   during early setup.
+		*/
+		buf_set_u32(check_value, 0, 32, 0x9);
+		buf_set_u32(check_mask, 0, 32, 0x9);
+		set=1;
+	}
 	
 	/* read debug status register */
 	embeddedice_read_reg_w_check(dbg_stat, check_value, check_value);
@@ -589,7 +624,59 @@ int arm7_9_execute_fast_sys_speed(struct target_s *target)
 	return ERROR_OK;
 }
 
-enum target_state arm7_9_poll(target_t *target)
+int arm7_9_target_request_data(target_t *target, u32 size, u8 *buffer)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
+	u32 *data;
+	int i;
+	
+	data = malloc(size * (sizeof(u32)));
+	
+	embeddedice_receive(jtag_info, data, size);
+	
+	for (i = 0; i < size; i++)
+	{
+		h_u32_to_le(buffer + (i * 4), data[i]);
+	}
+	
+	free(data);
+	
+	return ERROR_OK;
+}
+
+int arm7_9_handle_target_request(void *priv)
+{
+	target_t *target = priv;
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	arm_jtag_t *jtag_info = &arm7_9->jtag_info; 
+	reg_t *dcc_control = &arm7_9->eice_cache->reg_list[EICE_COMMS_CTRL];
+	
+	if (!target->dbg_msg_enabled)
+		return ERROR_OK;
+		
+	if (target->state == TARGET_RUNNING)
+	{
+		/* read DCC control register */
+		embeddedice_read_reg(dcc_control);
+		jtag_execute_queue();
+		
+		/* check W bit */
+		if (buf_get_u32(dcc_control->value, 1, 1) == 1)
+		{
+			u32 request;
+			
+			embeddedice_receive(jtag_info, &request, 1);
+			target_request(target, request);
+		}
+	}
+	
+	return ERROR_OK;
+}
+
+int arm7_9_poll(target_t *target)
 {
 	int retval;
 	armv4_5_common_t *armv4_5 = target->arch_info;
@@ -605,24 +692,16 @@ enum target_state arm7_9_poll(target_t *target)
 	embeddedice_read_reg(dbg_stat);
 	if ((retval = jtag_execute_queue()) != ERROR_OK)
 	{
-		switch (retval)
-		{
-			case ERROR_JTAG_QUEUE_FAILED:
-				ERROR("JTAG queue failed while reading EmbeddedICE status register");
-				exit(-1);
-				break;
-			default:
-				break;
-		}
+		return retval;
 	}
 	
 	if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1))
 	{
 		DEBUG("DBGACK set, dbg_state->value: 0x%x", buf_get_u32(dbg_stat->value, 0, 32));
-		if ((target->state == TARGET_UNKNOWN))
+		if (target->state == TARGET_UNKNOWN)
 		{
-			WARNING("DBGACK set while target was in unknown state. Reset or initialize target before resuming");
 			target->state = TARGET_RUNNING;
+			WARNING("DBGACK set while target was in unknown state. Reset or initialize target.");
 		}
 		if ((target->state == TARGET_RUNNING) || (target->state == TARGET_RESET))
 		{
@@ -640,6 +719,10 @@ enum target_state arm7_9_poll(target_t *target)
 			
 			target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);
 		}
+		if (target->state != TARGET_HALTED)
+		{
+			WARNING("DBGACK set, but the target did not end up in the halted stated %d", target->state);
+		}
 	}
 	else
 	{
@@ -647,7 +730,7 @@ enum target_state arm7_9_poll(target_t *target)
 			target->state = TARGET_RUNNING;
 	}
 	
-	return target->state;
+	return ERROR_OK;
 }
 
 int arm7_9_assert_reset(target_t *target)
@@ -681,7 +764,6 @@ int arm7_9_assert_reset(target_t *target)
 		{
 			if (retval == ERROR_JTAG_RESET_WOULD_ASSERT_TRST)
 			{
-				WARNING("srst resets test logic, too");
 				retval = jtag_add_reset(1, 1);
 			}
 		}
@@ -692,7 +774,6 @@ int arm7_9_assert_reset(target_t *target)
 		{
 			if (retval == ERROR_JTAG_RESET_WOULD_ASSERT_TRST)
 			{
-				WARNING("srst resets test logic, too");
 				retval = jtag_add_reset(1, 1);
 			}
 			
@@ -724,9 +805,8 @@ int arm7_9_deassert_reset(target_t *target)
 	
 	/* deassert reset lines */
 	jtag_add_reset(0, 0);
-		
+	
 	return ERROR_OK;
-
 }
 
 int arm7_9_clear_halt(target_t *target)
@@ -735,7 +815,8 @@ int arm7_9_clear_halt(target_t *target)
 	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 	reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
 	
-	if (arm7_9->use_dbgrq)
+	/* we used DBGRQ only if we didn't come out of reset */
+	if (!arm7_9->debug_entry_from_reset && arm7_9->use_dbgrq)
 	{
 		/* program EmbeddedICE Debug Control Register to deassert DBGRQ
 		 */
@@ -744,18 +825,29 @@ int arm7_9_clear_halt(target_t *target)
 	}
 	else
 	{
-		/* restore registers if watchpoint unit 0 was in use
-		 */
-		if (arm7_9->wp0_used)
+		if (arm7_9->debug_entry_from_reset && arm7_9->has_vector_catch)
 		{
-			embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK]);
-			embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK]);
-			embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK]);
+			/* if we came out of reset, and vector catch is supported, we used
+			 * vector catch to enter debug state
+			 * restore the register in that case
+			 */
+			embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_VEC_CATCH]);
+		}
+		else
+		{
+			/* restore registers if watchpoint unit 0 was in use
+			 */
+			if (arm7_9->wp0_used)
+			{
+				embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK]);
+				embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK]);
+				embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK]);
+			}
+			/* control value always has to be restored, as it was either disabled, 
+			 * or enabled with possibly different bits
+			 */
+			embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE]);
 		}
-		/* control value always has to be restored, as it was either disabled, 
-		 * or enabled with possibly different bits
-		 */
-		embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE]);
 	}
 	
 	return ERROR_OK;
@@ -774,7 +866,7 @@ int arm7_9_soft_reset_halt(struct target_s *target)
 		target->type->halt(target);
 	}
 	
-	while (buf_get_u32(dbg_stat->value, EICE_DBG_CONTROL_DBGACK, 1) == 0)
+	while (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1) == 0)
 	{
 		embeddedice_read_reg(dbg_stat);
 		jtag_execute_queue();
@@ -830,6 +922,35 @@ int arm7_9_soft_reset_halt(struct target_s *target)
 	return ERROR_OK;
 }
 
+int arm7_9_prepare_reset_halt(target_t *target)
+{
+	armv4_5_common_t *armv4_5 = target->arch_info;
+	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
+	
+	/* poll the target, and resume if it was currently halted */
+	arm7_9_poll(target);
+	if (target->state == TARGET_HALTED)
+	{
+		arm7_9_resume(target, 1, 0x0, 0, 1);
+	}
+	
+	if (arm7_9->has_vector_catch)
+	{
+		/* program vector catch register to catch reset vector */
+		embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_VEC_CATCH], 0x1);
+	}
+	else
+	{
+		/* program watchpoint unit to match on reset vector address */
+		embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], 0x3);
+		embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0x0);
+		embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], 0x100);
+		embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], 0xf7);
+	}
+	
+	return ERROR_OK;
+}
+
 int arm7_9_halt(target_t *target)
 {
 	armv4_5_common_t *armv4_5 = target->arch_info;
@@ -842,17 +963,29 @@ int arm7_9_halt(target_t *target)
 	{
 		WARNING("target was already halted");
 		return ERROR_TARGET_ALREADY_HALTED;
-	} 
+	}
 	
 	if (target->state == TARGET_UNKNOWN)
 	{
 		WARNING("target was in unknown state when halt was requested");
 	}
 	
-	if ((target->state == TARGET_RESET) && (jtag_reset_config & RESET_SRST_PULLS_TRST) && (jtag_srst))
+	if (target->state == TARGET_RESET) 
 	{
-		ERROR("can't request a halt while in reset if nSRST pulls nTRST");
-		return ERROR_TARGET_FAILURE;
+		if ((jtag_reset_config & RESET_SRST_PULLS_TRST) && jtag_srst)
+		{
+			ERROR("can't request a halt while in reset if nSRST pulls nTRST");
+			return ERROR_TARGET_FAILURE;
+		}
+		else
+		{
+			/* we came here in a reset_halt or reset_init sequence
+			 * debug entry was already prepared in arm7_9_prepare_reset_halt()
+			 */
+			target->debug_reason = DBG_REASON_DBGRQ;
+			
+			return ERROR_OK; 
+		}
 	}
 
 	if (arm7_9->use_dbgrq)
@@ -892,7 +1025,7 @@ int arm7_9_debug_entry(target_t *target)
 	reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
 
 #ifdef _DEBUG_ARM7_9_
-	DEBUG("");
+	DEBUG("-");
 #endif
 
 	if (arm7_9->pre_debug_entry)
@@ -1023,8 +1156,8 @@ int arm7_9_debug_entry(target_t *target)
 	}
 
 	/* r0 and r15 (pc) have to be restored later */
-	ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).dirty = 1;
-	ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 15).dirty = 1;
+	ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).valid;
+	ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 15).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 15).valid;
 
 	if ((retval = jtag->execute_queue()) != ERROR_OK)
 		return retval;
@@ -1042,7 +1175,7 @@ int arm7_9_full_context(target_t *target)
 	armv4_5_common_t *armv4_5 = target->arch_info;
 	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 
-	DEBUG("");
+	DEBUG("-");
 	
 	if (target->state != TARGET_HALTED)
 	{
@@ -1125,7 +1258,7 @@ int arm7_9_restore_context(target_t *target)
 	int dirty;
 	int mode_change;
 	
-	DEBUG("");
+	DEBUG("-");
 	
 	if (target->state != TARGET_HALTED)
 	{
@@ -1168,7 +1301,6 @@ int arm7_9_restore_context(target_t *target)
 				else
 				{
 					ERROR("BUG: dirty register '%s', but no valid data", reg->name);
-					exit(-1);
 				}
 			}
 		}
@@ -1262,7 +1394,7 @@ int arm7_9_restart_core(struct target_s *target)
 	
 	/* set RESTART instruction */
 	jtag_add_end_state(TAP_RTI);
-	arm_jtag_set_instr(jtag_info, 0x4);
+	arm_jtag_set_instr(jtag_info, 0x4, NULL);
 	
 	jtag_add_runtest(1, TAP_RTI);
 	if ((jtag_execute_queue()) != ERROR_OK)
@@ -1325,8 +1457,9 @@ int arm7_9_resume(struct target_s *target, int current, u32 address, int handle_
 	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 	breakpoint_t *breakpoint = target->breakpoints;
 	reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
+	int err;
 	
-	DEBUG("");
+	DEBUG("-");
 	
 	if (target->state != TARGET_HALTED)
 	{
@@ -1372,14 +1505,21 @@ int arm7_9_resume(struct target_s *target, int current, u32 address, int handle_
 				
 			buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 0);
 			embeddedice_write_reg(dbg_ctrl, buf_get_u32(dbg_ctrl->value, 0, dbg_ctrl->size));
-			arm7_9_execute_sys_speed(target);
+			err = arm7_9_execute_sys_speed(target);
 			
 			DEBUG("disable single-step");
 			arm7_9->disable_single_step(target);
-			
+
+			if (err != ERROR_OK)
+			{
+				arm7_9_set_breakpoint(target, breakpoint);
+				target->state = TARGET_UNKNOWN;
+				return err;
+			}
+
 			arm7_9_debug_entry(target);
 			DEBUG("new PC after step: 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
-		
+
 			DEBUG("set breakpoint at 0x%8.8x", breakpoint->address);
 			arm7_9_set_breakpoint(target, breakpoint);
 		}
@@ -1475,6 +1615,7 @@ int arm7_9_step(struct target_s *target, int current, u32 address, int handle_br
 	armv4_5_common_t *armv4_5 = target->arch_info;
 	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 	breakpoint_t *breakpoint = NULL;
+	int err;
 
 	if (target->state != TARGET_HALTED)
 	{
@@ -1513,22 +1654,25 @@ int arm7_9_step(struct target_s *target, int current, u32 address, int handle_br
 	
 	target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
 
-	arm7_9_execute_sys_speed(target);
+	err = arm7_9_execute_sys_speed(target);
 	arm7_9->disable_single_step(target);
 	
 	/* registers are now invalid */
 	armv4_5_invalidate_core_regs(target);
 	
-	arm7_9_debug_entry(target);
+	if (err != ERROR_OK)
+	{
+		target->state = TARGET_UNKNOWN;
+	} else {
+		arm7_9_debug_entry(target);
+		target_call_event_callbacks(target, TARGET_EVENT_HALTED);
+		DEBUG("target stepped");
+	}
 	
-	target_call_event_callbacks(target, TARGET_EVENT_HALTED);
-
 	if (breakpoint)
 		arm7_9_set_breakpoint(target, breakpoint);
 	
-	DEBUG("target stepped");
-
-	return ERROR_OK;
+	return err;
 
 }
 
@@ -1785,13 +1929,13 @@ int arm7_9_read_memory(struct target_s *target, u32 address, u32 size, u32 count
 	}
 	
 	for (i=0; i<=last_reg; i++)
-		ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = 1;
+		ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).valid;
 
 	arm7_9->read_xpsr(target, &cpsr, 0);
 	if ((retval = jtag_execute_queue()) != ERROR_OK)
 	{
 		ERROR("JTAG error while reading cpsr");
-		exit(-1);
+		return ERROR_TARGET_DATA_ABORT;
 	}
 
 	if (((cpsr & 0x1f) == ARMV4_5_MODE_ABT) && (armv4_5->core_mode != ARMV4_5_MODE_ABT))
@@ -1820,7 +1964,9 @@ int arm7_9_write_memory(struct target_s *target, u32 address, u32 size, u32 coun
 	int retval;
 	int last_reg = 0;
 
+#ifdef _DEBUG_ARM7_9_
 	DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);
+#endif
 
 	if (target->state != TARGET_HALTED)
 	{
@@ -1950,13 +2096,13 @@ int arm7_9_write_memory(struct target_s *target, u32 address, u32 size, u32 coun
 	embeddedice_store_reg(dbg_ctrl);
 	
 	for (i=0; i<=last_reg; i++)
-		ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = 1;
+		ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).valid;
 
 	arm7_9->read_xpsr(target, &cpsr, 0);
 	if ((retval = jtag_execute_queue()) != ERROR_OK)
 	{
 		ERROR("JTAG error while reading cpsr");
-		exit(-1);
+		return ERROR_TARGET_DATA_ABORT;
 	}
 
 	if (((cpsr & 0x1f) == ARMV4_5_MODE_ABT) && (armv4_5->core_mode != ARMV4_5_MODE_ABT))
@@ -2019,10 +2165,44 @@ int arm7_9_bulk_write_memory(target_t *target, u32 address, u32 count, u8 *buffe
 
 	arm7_9_resume(target, 0, arm7_9->dcc_working_area->address, 1, 1);
 	
-	for (i = 0; i < count; i++)
+	int little=target->endianness==TARGET_LITTLE_ENDIAN;
+	if (count>2)
 	{
-		embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_COMMS_DATA], target_buffer_get_u32(target, buffer));
-		buffer += 4;
+		/* Handle first & last using standard embeddedice_write_reg and the middle ones w/the
+		   core function repeated. 
+		 */
+		embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_COMMS_DATA], fast_target_buffer_get_u32(buffer, little));
+		buffer+=4;
+		
+		embeddedice_reg_t *ice_reg = arm7_9->eice_cache->reg_list[EICE_COMMS_DATA].arch_info;
+		u8 reg_addr = ice_reg->addr & 0x1f;
+		int chain_pos = ice_reg->jtag_info->chain_pos;
+		/* we want the compiler to duplicate the code, which it does not
+		 * do automatically.
+		 */
+		if (little)
+		{
+			for (i = 1; i < count - 1; i++)
+			{
+				embeddedice_write_reg_inner(chain_pos, reg_addr, fast_target_buffer_get_u32(buffer, little));
+				buffer += 4;
+			}
+		} else
+		{
+			for (i = 1; i < count - 1; i++)
+			{
+				embeddedice_write_reg_inner(chain_pos, reg_addr, fast_target_buffer_get_u32(buffer, little));
+				buffer += 4;
+			}
+		}
+		embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_COMMS_DATA], fast_target_buffer_get_u32(buffer, little));
+	} else
+	{
+		for (i = 0; i < count; i++)
+		{
+			embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_COMMS_DATA], fast_target_buffer_get_u32(buffer, little));
+			buffer += 4;
+		}
 	}
 	
 	target->type->halt(target);
@@ -2045,14 +2225,88 @@ int arm7_9_bulk_write_memory(target_t *target, u32 address, u32 count, u8 *buffe
 	return ERROR_OK;
 }
 
+int arm7_9_checksum_memory(struct target_s *target, u32 address, u32 count, u32* checksum)
+{
+	working_area_t *crc_algorithm;
+	armv4_5_algorithm_t armv4_5_info;
+	reg_param_t reg_params[2];
+	int retval;
+	
+	u32 arm7_9_crc_code[] = {
+		0xE1A02000,				/* mov		r2, r0 */
+		0xE3E00000,				/* mov		r0, #0xffffffff */
+		0xE1A03001,				/* mov		r3, r1 */
+		0xE3A04000,				/* mov		r4, #0 */
+		0xEA00000B,				/* b		ncomp */
+								/* nbyte: */
+		0xE7D21004,				/* ldrb	r1, [r2, r4] */
+		0xE59F7030,				/* ldr		r7, CRC32XOR */
+		0xE0200C01,				/* eor		r0, r0, r1, asl 24 */
+		0xE3A05000,				/* mov		r5, #0 */
+								/* loop: */
+		0xE3500000,				/* cmp		r0, #0 */
+		0xE1A06080,				/* mov		r6, r0, asl #1 */
+		0xE2855001,				/* add		r5, r5, #1 */
+		0xE1A00006,				/* mov		r0, r6 */
+		0xB0260007,				/* eorlt	r0, r6, r7 */
+		0xE3550008,				/* cmp		r5, #8 */
+		0x1AFFFFF8,				/* bne		loop */
+		0xE2844001,				/* add		r4, r4, #1 */
+								/* ncomp: */
+		0xE1540003,				/* cmp		r4, r3 */
+		0x1AFFFFF1,				/* bne		nbyte */
+								/* end: */
+		0xEAFFFFFE,				/* b		end */
+		0x04C11DB7				/* CRC32XOR:	.word 0x04C11DB7 */
+	};
+	
+	int i;
+	
+	if (target_alloc_working_area(target, sizeof(arm7_9_crc_code), &crc_algorithm) != ERROR_OK)
+	{
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+	
+	/* convert flash writing code into a buffer in target endianness */
+	for (i = 0; i < (sizeof(arm7_9_crc_code)/sizeof(u32)); i++)
+		target_write_u32(target, crc_algorithm->address + i*sizeof(u32), arm7_9_crc_code[i]);
+	
+	armv4_5_info.common_magic = ARMV4_5_COMMON_MAGIC;
+	armv4_5_info.core_mode = ARMV4_5_MODE_SVC;
+	armv4_5_info.core_state = ARMV4_5_STATE_ARM;
+	
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT);
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
+	
+	buf_set_u32(reg_params[0].value, 0, 32, address);
+	buf_set_u32(reg_params[1].value, 0, 32, count);
+		
+	if ((retval = target->type->run_algorithm(target, 0, NULL, 2, reg_params,
+		crc_algorithm->address, crc_algorithm->address + (sizeof(arm7_9_crc_code) - 8), 20000, &armv4_5_info)) != ERROR_OK)
+	{
+		ERROR("error executing arm7_9 crc algorithm");
+		destroy_reg_param(&reg_params[0]);
+		destroy_reg_param(&reg_params[1]);
+		target_free_working_area(target, crc_algorithm);
+		return retval;
+	}
+	
+	*checksum = buf_get_u32(reg_params[0].value, 0, 32);
+	
+	destroy_reg_param(&reg_params[0]);
+	destroy_reg_param(&reg_params[1]);
+	
+	target_free_working_area(target, crc_algorithm);
+	
+	return ERROR_OK;
+}
+
 int arm7_9_register_commands(struct command_context_s *cmd_ctx)
 {
 	command_t *arm7_9_cmd;
 	
 	arm7_9_cmd = register_command(cmd_ctx, NULL, "arm7_9", NULL, COMMAND_ANY, "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>");
 	
@@ -2071,6 +2325,8 @@ int arm7_9_register_commands(struct command_context_s *cmd_ctx)
 
 	armv4_5_register_commands(cmd_ctx);
 	
+	etm_register_commands(cmd_ctx);
+	
 	return ERROR_OK;
 }
 
@@ -2379,37 +2635,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 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;
@@ -2423,13 +2648,15 @@ 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->etm_ctx = NULL;
 	arm7_9->has_single_step = 0;
 	arm7_9->has_monitor_mode = 0;
 	arm7_9->has_vector_catch = 0;
 	
 	arm7_9->reinit_embeddedice = 0;
 	
+	arm7_9->debug_entry_from_reset = 0;
+	
 	arm7_9->dcc_working_area = NULL;
 	
 	arm7_9->fast_memory_access = 0;
@@ -2444,5 +2671,7 @@ int arm7_9_init_arch_info(target_t *target, arm7_9_common_t *arm7_9)
 	
 	armv4_5_init_arch_info(target, armv4_5);
 	
+	target_register_timer_callback(arm7_9_handle_target_request, 1, 1, target);
+	
 	return ERROR_OK;
 }