arm7_9_common.c

/***************************************************************************
 *   Copyright (C) 2005 by Dominic Rath                                    *
 *   Dominic.Rath@gmx.de                                                   *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include "replacements.h"

#include "embeddedice.h"
#include "target.h"
#include "target_request.h"
#include "armv4_5.h"
#include "arm_jtag.h"
#include "jtag.h"
#include "log.h"
#include "arm7_9_common.h"
#include "breakpoints.h"

#include <stdlib.h>
#include <string.h>
#include <unistd.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <errno.h>

int arm7_9_debug_entry(target_t *target);
int arm7_9_enable_sw_bkpts(struct target_s *target);

/* command handler forward declarations */
int handle_arm7_9_write_xpsr_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
int handle_arm7_9_write_xpsr_im8_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
int handle_arm7_9_read_core_reg_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
int handle_arm7_9_write_core_reg_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
int handle_arm7_9_sw_bkpts_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
int handle_arm7_9_force_hw_bkpts_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
int handle_arm7_9_dbgrq_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
int handle_arm7_9_fast_memory_access_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
int handle_arm7_9_dcc_downloads_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
int handle_arm7_9_etm_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);

int arm7_9_reinit_embeddedice(target_t *target)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
	
	breakpoint_t *breakpoint = target->breakpoints;
	
	arm7_9->wp_available = 2;
	arm7_9->wp0_used = 0;
	arm7_9->wp1_used = 0;
		
	/* mark all hardware breakpoints as unset */
	while (breakpoint)
	{
		if (breakpoint->type == BKPT_HARD)
		{
			breakpoint->set = 0;
		}
		breakpoint = breakpoint->next;
	}
		
	if (arm7_9->sw_bkpts_enabled && arm7_9->sw_bkpts_use_wp)
	{
		arm7_9->sw_bkpts_enabled = 0;
		arm7_9_enable_sw_bkpts(target);
	}
	
	return ERROR_OK;
}

/* set things up after a reset / on startup */
int arm7_9_setup(target_t *target)
{
	/* a test-logic reset have occured
	 * the EmbeddedICE registers have been reset
	 * hardware breakpoints have been cleared
	 */
	return arm7_9_reinit_embeddedice(target);
}

int arm7_9_get_arch_pointers(target_t *target, armv4_5_common_t **armv4_5_p, arm7_9_common_t **arm7_9_p)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
	
	if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
	{
		return -1;
	}
	
	if (arm7_9->common_magic != ARM7_9_COMMON_MAGIC)
	{
		return -1;
	}
	
	*armv4_5_p = armv4_5;
	*arm7_9_p = arm7_9;
	
	return ERROR_OK;
}

int arm7_9_set_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
	
	if (target->state != TARGET_HALTED)
	{
		LOG_WARNING("target not halted");
		return ERROR_TARGET_NOT_HALTED;
	}
	
	if (arm7_9->force_hw_bkpts)
		breakpoint->type = BKPT_HARD;

	if (breakpoint->set)
	{
		LOG_WARNING("breakpoint already set");
		return ERROR_OK;
	}

	if (breakpoint->type == BKPT_HARD)
	{
		/* either an ARM (4 byte) or Thumb (2 byte) breakpoint */
		u32 mask = (breakpoint->length == 4) ? 0x3u : 0x1u;
		if (!arm7_9->wp0_used)
		{
			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_VALUE], breakpoint->address);
			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], mask);
			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0xffffffffu);
			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE);

			jtag_execute_queue();
			arm7_9->wp0_used = 1;
			breakpoint->set = 1;
		}
		else if (!arm7_9->wp1_used)
		{
			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_VALUE], breakpoint->address);
			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK], mask);
			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK], 0xffffffffu);
			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], EICE_W_CTRL_ENABLE);

			jtag_execute_queue();
			arm7_9->wp1_used = 1;
			breakpoint->set = 2;
		}
		else
		{
			LOG_ERROR("BUG: no hardware comparator available");
			return ERROR_OK;
		}
	}
	else if (breakpoint->type == BKPT_SOFT)
	{
		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 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)
			{
				LOG_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 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)
			{
				LOG_ERROR("Unable to set thumb software breakpoint at address %08x", breakpoint->address);
				return ERROR_OK;
			}
		}
		breakpoint->set = 1;
	}

	return ERROR_OK;

}

int arm7_9_unset_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
	
	if (target->state != TARGET_HALTED)
	{
		LOG_WARNING("target not halted");
		return ERROR_TARGET_NOT_HALTED;
	}

	if (!breakpoint->set)
	{
		LOG_WARNING("breakpoint not set");
		return ERROR_OK;
	}
	
	if (breakpoint->type == BKPT_HARD)
	{
		if (breakpoint->set == 1)
		{
			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], 0x0);
			jtag_execute_queue();
			arm7_9->wp0_used = 0;
		}
		else if (breakpoint->set == 2)
		{
			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], 0x0);
			jtag_execute_queue();
			arm7_9->wp1_used = 0;
		}
		breakpoint->set = 0;
	}
	else
	{
		/* restore original instruction (kept in target endianness) */
		if (breakpoint->length == 4)
		{
			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
		{
			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;
	}

	return ERROR_OK;
}

int arm7_9_add_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
	
	if (target->state != TARGET_HALTED)
	{
		LOG_WARNING("target not halted");
		return ERROR_TARGET_NOT_HALTED;
	}
	
	if (arm7_9->force_hw_bkpts)
	{
		LOG_DEBUG("forcing use of hardware breakpoint at address 0x%8.8x", breakpoint->address);
		breakpoint->type = BKPT_HARD;
	}
	
	if ((breakpoint->type == BKPT_SOFT) && (arm7_9->sw_bkpts_enabled == 0))
	{
		LOG_INFO("sw breakpoint requested, but software breakpoints not enabled");
		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
	}
	
	if ((breakpoint->type == BKPT_HARD) && (arm7_9->wp_available < 1))
	{
		LOG_INFO("no watchpoint unit available for hardware breakpoint");
		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
	}
	
	if ((breakpoint->length != 2) && (breakpoint->length != 4))
	{
		LOG_INFO("only breakpoints of two (Thumb) or four (ARM) bytes length supported");
		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
	}
	
	if (breakpoint->type == BKPT_HARD)
		arm7_9->wp_available--;
	
	return ERROR_OK;
}

int arm7_9_remove_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
	
	if (target->state != TARGET_HALTED)
	{
		LOG_WARNING("target not halted");
		return ERROR_TARGET_NOT_HALTED;
	}
	
	if (breakpoint->set)
	{
		arm7_9_unset_breakpoint(target, breakpoint);
	}
	
	if (breakpoint->type == BKPT_HARD)
		arm7_9->wp_available++;
	
	return ERROR_OK;
}

int arm7_9_set_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
	int rw_mask = 1;
	u32 mask;
	
	mask = watchpoint->length - 1;
	
	if (target->state != TARGET_HALTED)
	{
		LOG_WARNING("target not halted");
		return ERROR_TARGET_NOT_HALTED;
	}
	
	if (watchpoint->rw == WPT_ACCESS)
		rw_mask = 0;
	else
		rw_mask = 1;
	
	if (!arm7_9->wp0_used)
	{
		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_VALUE], watchpoint->address);
		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], mask);
		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], watchpoint->mask);
		if( watchpoint->mask != 0xffffffffu )
			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_VALUE], watchpoint->value);
		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], 0xff & ~EICE_W_CTRL_nOPC & ~rw_mask);
		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE | EICE_W_CTRL_nOPC | (watchpoint->rw & 1));

		jtag_execute_queue();
		watchpoint->set = 1;
		arm7_9->wp0_used = 2;
	}
	else if (!arm7_9->wp1_used)
	{
		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_VALUE], watchpoint->address);
		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK], mask);
		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK], watchpoint->mask);
		if( watchpoint->mask != 0xffffffffu )
			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_VALUE], watchpoint->value);
		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK], 0xff & ~EICE_W_CTRL_nOPC & ~rw_mask);
		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], EICE_W_CTRL_ENABLE | EICE_W_CTRL_nOPC | (watchpoint->rw & 1));

		jtag_execute_queue();
		watchpoint->set = 2;
		arm7_9->wp1_used = 2;
	} 
	else
	{
		LOG_ERROR("BUG: no hardware comparator available");
		return ERROR_OK;
	}
	
	return ERROR_OK;
}

int arm7_9_unset_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
	
	if (target->state != TARGET_HALTED)
	{
		LOG_WARNING("target not halted");
		return ERROR_TARGET_NOT_HALTED;
	}
	
	if (!watchpoint->set)
	{
		LOG_WARNING("breakpoint not set");
		return ERROR_OK;
	}
	
	if (watchpoint->set == 1)
	{
		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], 0x0);
		jtag_execute_queue();
		arm7_9->wp0_used = 0;
	}
	else if (watchpoint->set == 2)
	{
		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], 0x0);
		jtag_execute_queue();
		arm7_9->wp1_used = 0;
	}
	watchpoint->set = 0;

	return ERROR_OK;
}

int arm7_9_add_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
	
	if (target->state != TARGET_HALTED)
	{
		LOG_WARNING("target not halted");
		return ERROR_TARGET_NOT_HALTED;
	}
	
	if (arm7_9->wp_available < 1)
	{
		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
	}
	
	if ((watchpoint->length != 1) && (watchpoint->length != 2) && (watchpoint->length != 4))
	{
		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
	}
	
	arm7_9->wp_available--;
		
	return ERROR_OK;
}

int arm7_9_remove_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
	
	if (target->state != TARGET_HALTED)
	{
		LOG_WARNING("target not halted");
		return ERROR_TARGET_NOT_HALTED;
	}
	
	if (watchpoint->set)
	{
		arm7_9_unset_watchpoint(target, watchpoint);
	}
		
	arm7_9->wp_available++;
	
	return ERROR_OK;
}

int arm7_9_enable_sw_bkpts(struct target_s *target)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
	int retval;
	
	if (arm7_9->sw_bkpts_enabled)
		return ERROR_OK;
	
	if (arm7_9->wp_available < 1)
	{
		LOG_WARNING("can't enable sw breakpoints with no watchpoint unit available");
		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
	}
	arm7_9->wp_available--;
	
	if (!arm7_9->wp0_used)
	{
		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_VALUE], arm7_9->arm_bkpt);
		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0x0);
		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], 0xffffffffu);
		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
		arm7_9->sw_bkpts_enabled = 1;
		arm7_9->wp0_used = 3;
	}
	else if (!arm7_9->wp1_used)
	{
		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_VALUE], arm7_9->arm_bkpt);
		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK], 0x0);
		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK], 0xffffffffu);
		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
		arm7_9->sw_bkpts_enabled = 2;
		arm7_9->wp1_used = 3;
	}
	else
	{
		LOG_ERROR("BUG: both watchpoints used, but wp_available >= 1");
		return ERROR_FAIL;
	}
	
	if ((retval = jtag_execute_queue()) != ERROR_OK)
	{
		LOG_ERROR("error writing EmbeddedICE registers to enable sw breakpoints");
		return ERROR_FAIL;
	};
	
	return ERROR_OK;
}

int arm7_9_disable_sw_bkpts(struct target_s *target)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
	
	if (!arm7_9->sw_bkpts_enabled)
		return ERROR_OK;
	
	if (arm7_9->sw_bkpts_enabled == 1)
	{
		embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], 0x0);
		arm7_9->sw_bkpts_enabled = 0;
		arm7_9->wp0_used = 0;
		arm7_9->wp_available++;
	}
	else if (arm7_9->sw_bkpts_enabled == 2)
	{
		embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], 0x0);
		arm7_9->sw_bkpts_enabled = 0;
		arm7_9->wp1_used = 0;
		arm7_9->wp_available++;
	}

	return ERROR_OK;
}

int arm7_9_execute_sys_speed(struct target_s *target)
{
	int timeout;
	int retval;
	
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
	reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
				
	/* set RESTART instruction */
	jtag_add_end_state(TAP_RTI);
	if (arm7_9->need_bypass_before_restart) {
		arm7_9->need_bypass_before_restart = 0;
		arm_jtag_set_instr(jtag_info, 0xf, NULL);
	}
	arm_jtag_set_instr(jtag_info, 0x4, NULL);
	
	for (timeout=0; timeout<50; timeout++)
	{
		/* read debug status register */
		embeddedice_read_reg(dbg_stat);
		if ((retval = jtag_execute_queue()) != ERROR_OK)
			return retval;
		if ((buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1))
				   && (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_SYSCOMP, 1)))
			break;
		usleep(100000);	
	}
	if (timeout == 50)
	{
		LOG_ERROR("timeout waiting for SYSCOMP & DBGACK, last DBG_STATUS: %x", buf_get_u32(dbg_stat->value, 0, dbg_stat->size));
		return ERROR_TARGET_TIMEOUT;
	}
	
	return ERROR_OK;
}

int arm7_9_execute_fast_sys_speed(struct target_s *target)
{
	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;
	arm_jtag_t *jtag_info = &arm7_9->jtag_info;
	reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
				
	/* set RESTART instruction */
	jtag_add_end_state(TAP_RTI);
	if (arm7_9->need_bypass_before_restart) {
		arm7_9->need_bypass_before_restart = 0;
		arm_jtag_set_instr(jtag_info, 0xf, NULL);
	}
	arm_jtag_set_instr(jtag_info, 0x4, NULL);
	
	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);

	return ERROR_OK;
}

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;
	if (!target->type->examined)
		return ERROR_OK;
	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;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
	reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];

	/* read debug status register */
	embeddedice_read_reg(dbg_stat);
	if ((retval = jtag_execute_queue()) != ERROR_OK)
	{
		return retval;
	}
	
	if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1))
	{
/*		LOG_DEBUG("DBGACK set, dbg_state->value: 0x%x", buf_get_u32(dbg_stat->value, 0, 32));*/
		if (target->state == TARGET_UNKNOWN)
		{
			target->state = TARGET_RUNNING;
			LOG_WARNING("DBGACK set while target was in unknown state. Reset or initialize target.");
		}
		if ((target->state == TARGET_RUNNING) || (target->state == TARGET_RESET))
		{
			int check_pc=0;
			if (target->state == TARGET_RESET)
			{
				if ((target->reset_mode == RESET_HALT) || (target->reset_mode == RESET_INIT))
				{
					if ((jtag_reset_config & RESET_SRST_PULLS_TRST)==0)
					{
						check_pc = 1;
					}
				}
			}
			
			target->state = TARGET_HALTED;
			
			if ((retval = arm7_9_debug_entry(target)) != ERROR_OK)
				return retval;
			
			if (check_pc)
			{
				reg_t *reg = register_get_by_name(target->reg_cache, "pc", 1);
				u32 t=*((u32 *)reg->value);
				if (t!=0)
				{
					LOG_ERROR("PC was not 0. Does this target need srst_pulls_trst?");
				}
			}
			
			target_call_event_callbacks(target, TARGET_EVENT_HALTED);
		}
		if (target->state == TARGET_DEBUG_RUNNING)
		{
			target->state = TARGET_HALTED;
			if ((retval = arm7_9_debug_entry(target)) != ERROR_OK)
				return retval;
			
			target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);
		}
		if (target->state != TARGET_HALTED)
		{
			LOG_WARNING("DBGACK set, but the target did not end up in the halted stated %d", target->state);
		}
	}
	else
	{
		if (target->state != TARGET_DEBUG_RUNNING)
			target->state = TARGET_RUNNING;
	}
	
	return ERROR_OK;
}

/*
  Some -S targets (ARM966E-S in the STR912 isn't affected, ARM926EJ-S
  in the LPC3180 and AT91SAM9260 is affected) completely stop the JTAG clock
  while the core is held in reset(SRST). It isn't possible to program the halt
  condition once reset was asserted, hence a hook that allows the target to set
  up its reset-halt condition prior to asserting reset.
*/

int arm7_9_assert_reset(target_t *target)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
	LOG_DEBUG("target->state: %s", target_state_strings[target->state]);
	
	if (!(jtag_reset_config & RESET_HAS_SRST))
	{
		LOG_ERROR("Can't assert SRST");
		return ERROR_FAIL;
	}

	if ((target->reset_mode == RESET_HALT) || (target->reset_mode == RESET_INIT))
	{
		/*
		 * Some targets do not support communication while SRST is asserted. We need to
		 * set up the reset vector catch here.
		 * 
		 * If TRST is asserted, then these settings will be reset anyway, so setting them
		 * here is harmless.  
		 */
		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_VALUE], 0x0);
			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], 0xffffffff);
			embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
			embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
		}
	}

	/* here we should issue a srst only, but we may have to assert trst as well */
	if (jtag_reset_config & RESET_SRST_PULLS_TRST)
	{
		jtag_add_reset(1, 1);
	} else
	{
		jtag_add_reset(0, 1);
	}
	

	target->state = TARGET_RESET;
	jtag_add_sleep(50000);

	armv4_5_invalidate_core_regs(target);

	return ERROR_OK;

}

int arm7_9_deassert_reset(target_t *target)
{
	LOG_DEBUG("target->state: %s", target_state_strings[target->state]);
	
	/* deassert reset lines */
	jtag_add_reset(0, 0);
	
	return ERROR_OK;
}

int arm7_9_clear_halt(target_t *target)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
	reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
	
	/* 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
		 */
		buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGRQ, 1, 0);	
		embeddedice_store_reg(dbg_ctrl);
	}
	else
	{
		if (arm7_9->debug_entry_from_reset && arm7_9->has_vector_catch)
		{
			/* 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)
			{
				if (arm7_9->debug_entry_from_reset)
				{
					embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_VALUE]);
				}
				embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK]);
				embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK]);
				embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK]);
			}
			/* control value always has to be restored, as it was either disabled, 
			 * or enabled with possibly different bits
			 */
			embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE]);
		}
	}
	
	return ERROR_OK;
}

int arm7_9_soft_reset_halt(struct target_s *target)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
	reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
	reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
	int i;
	int retval;
	
	if ((retval=target_halt(target))!=ERROR_OK)
		return retval;
	
	for (i=0; i<10; i++)
	{
		if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1) != 0)
			break;
		embeddedice_read_reg(dbg_stat);
		if ((retval=jtag_execute_queue())!=ERROR_OK)
			return retval;
		/* do not eat all CPU, time out after 1 se*/
		usleep(100*1000);
		
	}
	if (i==10)
	{
		LOG_ERROR("Failed to halt CPU after 1 sec");
		return ERROR_TARGET_TIMEOUT;
	}
	target->state = TARGET_HALTED;
	
	/* program EmbeddedICE Debug Control Register to assert DBGACK and INTDIS
	 * ensure that DBGRQ is cleared
	 */
	buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 1);
	buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGRQ, 1, 0);
	buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_INTDIS, 1, 1);
	embeddedice_store_reg(dbg_ctrl);
	
	arm7_9_clear_halt(target);
	
	/* if the target is in Thumb state, change to ARM state */
	if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_ITBIT, 1))
	{
		u32 r0_thumb, pc_thumb;
		LOG_DEBUG("target entered debug from Thumb state, changing to ARM");
		/* Entered debug from Thumb mode */
		armv4_5->core_state = ARMV4_5_STATE_THUMB;
		arm7_9->change_to_arm(target, &r0_thumb, &pc_thumb);
	}
	
	/* all register content is now invalid */
	armv4_5_invalidate_core_regs(target);
	
	/* SVC, ARM state, IRQ and FIQ disabled */
	buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8, 0xd3);
	armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 1;
	armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
	
	/* start fetching from 0x0 */
	buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, 0x0);
	armv4_5->core_cache->reg_list[15].dirty = 1;
	armv4_5->core_cache->reg_list[15].valid = 1;
	
	armv4_5->core_mode = ARMV4_5_MODE_SVC;
	armv4_5->core_state = ARMV4_5_STATE_ARM;

	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;
	
	/* reset registers */
	for (i = 0; i <= 14; i++)
	{	
		buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).value, 0, 32, 0xffffffff);
		ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = 1;
		ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).valid = 1;
	}
	
	target_call_event_callbacks(target, TARGET_EVENT_HALTED);
	
	return ERROR_OK;
}

int arm7_9_halt(target_t *target)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;
	reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
	
	LOG_DEBUG("target->state: %s", target_state_strings[target->state]);
	
	if (target->state == TARGET_HALTED)
	{
		LOG_DEBUG("target was already halted");
		return ERROR_OK;
	}
	
	if (target->state == TARGET_UNKNOWN)
	{
		LOG_WARNING("target was in unknown state when halt was requested");
	}
	
	if (target->state == TARGET_RESET) 
	{
		if ((jtag_reset_config & RESET_SRST_PULLS_TRST) && jtag_srst)
		{
			LOG_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_assert_reset()
			 */
			target->debug_reason = DBG_REASON_DBGRQ;
			
			return ERROR_OK; 
		}
	}

	if (arm7_9->use_dbgrq)
	{
		/* program EmbeddedICE Debug Control Register to assert DBGRQ
		 */
		if (arm7_9->set_special_dbgrq) {