arm7_9_common.c

/***************************************************************************
 *   Copyright (C) 2005 by Dominic Rath                                    *
 *   Dominic.Rath@gmx.de                                                   *
 *                                                                         *
 *   Copyright (C) 2007,2008 yvind Harboe                                 *
 *   oyvind.harboe@zylin.com                                               *
 *                                                                         *
 *   Copyright (C) 2008 by Spencer Oliver                                  *
 *   spen@spen-soft.co.uk                                                  *
 *                                                                         *
 *   Copyright (C) 2008 by Hongtao Zheng                                   *
 *   hontor@126.com                                                        *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#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 "time_support.h"
#include "arm_simulator.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_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);


static int arm7_9_clear_watchpoints(arm7_9_common_t *arm7_9)
{
	embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], 0x0);
	embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], 0x0);
	arm7_9->sw_breakpoints_added = 0;
	arm7_9->wp0_used = 0;
	arm7_9->wp1_used = arm7_9->wp1_used_default;
	arm7_9->wp_available = arm7_9->wp_available_max;

	return jtag_execute_queue();
}

/* set up embedded ice registers */
static int arm7_9_set_software_breakpoints(arm7_9_common_t *arm7_9)
{
	if (arm7_9->sw_breakpoints_added)
	{
		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--;

	/* pick a breakpoint unit */
	if (!arm7_9->wp0_used)
	{
		arm7_9->sw_breakpoints_added=1;
		arm7_9->wp0_used = 3;
	} else if (!arm7_9->wp1_used)
	{
		arm7_9->sw_breakpoints_added=2;
		arm7_9->wp1_used = 3;
	}
	else
	{
		LOG_ERROR("BUG: both watchpoints used, but wp_available >= 1");
		return ERROR_FAIL;
	}

	if (arm7_9->sw_breakpoints_added==1)
	{
		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);
	}
	else if (arm7_9->sw_breakpoints_added==2)
	{
		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);
	}
	else
	{
		LOG_ERROR("BUG: both watchpoints used, but wp_available >= 1");
		return ERROR_FAIL;
	}

	return jtag_execute_queue();
}

/* set things up after a reset / on startup */
int arm7_9_setup(target_t *target)
{
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;

	return arm7_9_clear_watchpoints(arm7_9);
}


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

/* we set up the breakpoint even if it is already set. Some action, e.g. reset
 * might have erased the values in embedded ice
 */
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;
	int retval=ERROR_OK;

	if (target->state != TARGET_HALTED)
	{
		LOG_WARNING("target not halted");
		return ERROR_TARGET_NOT_HALTED;
	}

	if (breakpoint->type == BKPT_HARD)
	{
		/* either an ARM (4 byte) or Thumb (2 byte) breakpoint */
		u32 mask = (breakpoint->length == 4) ? 0x3u : 0x1u;
		if (breakpoint->set==1)
		{
			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);
		}
		else if (breakpoint->set==2)
		{
			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);
		}
		else
		{
			LOG_ERROR("BUG: no hardware comparator available");
			return ERROR_OK;
		}

		retval=jtag_execute_queue();
	}
	else if (breakpoint->type == BKPT_SOFT)
	{
		if ((retval=arm7_9_set_software_breakpoints(arm7_9))!=ERROR_OK)
			return retval;

		/* did we already set this breakpoint? */
		if (breakpoint->set)
			return ERROR_OK;

		if (breakpoint->length == 4)
		{
			u32 verify = 0xffffffff;
			/* keep the original instruction in target endianness */
			if ((retval = target->type->read_memory(target, breakpoint->address, 4, 1, breakpoint->orig_instr)) != ERROR_OK)
			{
				return retval;
			}
			/* write the breakpoint instruction in target endianness (arm7_9->arm_bkpt is host endian) */
			if ((retval = target_write_u32(target, breakpoint->address, arm7_9->arm_bkpt)) != ERROR_OK)
			{
				return retval;
			}

			if ((retval = target->type->read_memory(target, breakpoint->address, 4, 1, (u8 *)&verify)) != ERROR_OK)
			{
				return retval;
			}
			if (verify != arm7_9->arm_bkpt)
			{
				LOG_ERROR("Unable to set 32 bit software breakpoint at address %08x - check that memory is read/writable", breakpoint->address);
				return ERROR_OK;
			}
		}
		else
		{
			u16 verify = 0xffff;
			/* keep the original instruction in target endianness */
			if ((retval = target->type->read_memory(target, breakpoint->address, 2, 1, breakpoint->orig_instr)) != ERROR_OK)
			{
				return retval;
			}
			/* write the breakpoint instruction in target endianness (arm7_9->thumb_bkpt is host endian) */
			if ((retval = target_write_u16(target, breakpoint->address, arm7_9->thumb_bkpt)) != ERROR_OK)
			{
				return retval;
			}

			if ((retval = target->type->read_memory(target, breakpoint->address, 2, 1, (u8 *)&verify)) != ERROR_OK)
			{
				return retval;
			}
			if (verify != arm7_9->thumb_bkpt)
			{
				LOG_ERROR("Unable to set thumb software breakpoint at address %08x - check that memory is read/writable", breakpoint->address);
				return ERROR_OK;
			}
		}
		breakpoint->set = 1;
	}

	return retval;

}

int arm7_9_unset_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
{
	int retval = ERROR_OK;

	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;

	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);
			arm7_9->wp0_used = 0;
		}
		else if (breakpoint->set == 2)
		{
			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], 0x0);
			arm7_9->wp1_used = 0;
		}
		retval = jtag_execute_queue();
		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 */
			if ((retval = target->type->read_memory(target, breakpoint->address, 4, 1, (u8*)&current_instr)) != ERROR_OK)
			{
				return retval;
			}
			if (current_instr==arm7_9->arm_bkpt)
				if ((retval = target->type->write_memory(target, breakpoint->address, 4, 1, breakpoint->orig_instr)) != ERROR_OK)
				{
					return retval;
				}
		}
		else
		{
			u16 current_instr;
			/* check that user program as not modified breakpoint instruction */
			if ((retval = target->type->read_memory(target, breakpoint->address, 2, 1, (u8*)&current_instr)) != ERROR_OK)
			{
				return retval;
			}
			if (current_instr==arm7_9->thumb_bkpt)
				if ((retval = target->type->write_memory(target, breakpoint->address, 2, 1, breakpoint->orig_instr)) != ERROR_OK)
				{
					return retval;
				}
		}
		breakpoint->set = 0;
	}

	return retval;
}

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->breakpoint_count==0)
	{
		/* make sure we don't have any dangling breakpoints. This is vital upon
		 * GDB connect/disconnect
		 */
		arm7_9_clear_watchpoints(arm7_9);
	}

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

		if (!arm7_9->wp0_used)
		{
			arm7_9->wp0_used = 1;
			breakpoint->set = 1;
		}
		else if (!arm7_9->wp1_used)
		{
			arm7_9->wp1_used = 1;
			breakpoint->set = 2;
		}
		else
		{
			LOG_ERROR("BUG: no hardware comparator available");
		}
	}


	arm7_9->breakpoint_count++;

	return arm7_9_set_breakpoint(target, breakpoint);
}

int arm7_9_remove_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
{
	int retval = ERROR_OK;
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;

	if((retval = arm7_9_unset_breakpoint(target, breakpoint)) != ERROR_OK)
	{
		return retval;
	}

	if (breakpoint->type == BKPT_HARD)
		arm7_9->wp_available++;

	arm7_9->breakpoint_count--;
	if (arm7_9->breakpoint_count==0)
	{
		/* make sure we don't have any dangling breakpoints */
		if((retval = arm7_9_clear_watchpoints(arm7_9)) != ERROR_OK)
		{
			return retval;
		}
	}

	return ERROR_OK;
}

int arm7_9_set_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
{
	int retval = ERROR_OK;
	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));

		if((retval = jtag_execute_queue()) != ERROR_OK)
		{
			return retval;
		}
		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));

		if((retval = jtag_execute_queue()) != ERROR_OK)
		{
			return retval;
		}
		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)
{
	int retval = ERROR_OK;
	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);
		if((retval = jtag_execute_queue()) != ERROR_OK)
		{
			return retval;
		}
		arm7_9->wp0_used = 0;
	}
	else if (watchpoint->set == 2)
	{
		embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], 0x0);
		if((retval = jtag_execute_queue()) != ERROR_OK)
		{
			return retval;
		}
		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)
{
	int retval = ERROR_OK;
	armv4_5_common_t *armv4_5 = target->arch_info;
	arm7_9_common_t *arm7_9 = armv4_5->arch_info;

	if (watchpoint->set)
	{
		if((retval = arm7_9_unset_watchpoint(target, watchpoint)) != ERROR_OK)
		{
			return retval;
		}
	}

	arm7_9->wp_available++;

	return ERROR_OK;
}




int arm7_9_execute_sys_speed(struct target_s *target)
{
	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);

	long long then=timeval_ms();
	int timeout;
	while (!(timeout=((timeval_ms()-then)>1000)))
	{
		/* 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;
		if (debug_level>=3)
		{
			alive_sleep(100);
		} else
		{
			keep_alive();
		}
	}
	if (timeout)
	{
		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, retval = ERROR_OK;

	data = malloc(size * (sizeof(u32)));

	retval = embeddedice_receive(jtag_info, data, size);

	for (i = 0; i < size; i++)
	{
		h_u32_to_le(buffer + (i * 4), data[i]);
	}

	free(data);

	return retval;
}

int arm7_9_handle_target_request(void *priv)
{
	int retval = ERROR_OK;
	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);
		if ((retval = jtag_execute_queue()) != ERROR_OK)
		{
			return retval;
		}

		/* check W bit */
		if (buf_get_u32(dcc_control->value, 1, 1) == 1)
		{
			u32 request;

			if ((retval = embeddedice_receive(jtag_info, &request, 1)) != ERROR_OK)
			{
				return retval;
			}
			if ((retval = target_request(target, request)) != ERROR_OK)
			{
				return retval;
			}
		}
	}

	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_halt)
				{
					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?");
				}
			}

			if ((retval = target_call_event_callbacks(target, TARGET_EVENT_HALTED)) != ERROR_OK)
			{
				return retval;
			}
		}
		if (target->state == TARGET_DEBUG_RUNNING)
		{
			target->state = TARGET_HALTED;
			if ((retval = arm7_9_debug_entry(target)) != ERROR_OK)
				return retval;

			if ((retval = target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED)) != ERROR_OK)
			{
				return retval;
			}
		}
		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",
		  Jim_Nvp_value2name_simple( nvp_target_state,target->state)->name);

	if (!(jtag_reset_config & RESET_HAS_SRST))
	{
		LOG_ERROR("Can't assert SRST");
		return ERROR_FAIL;
	}

	if (target->reset_halt)
	{
		/*
		 * 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);

    if ((target->reset_halt)&&((jtag_reset_config & RESET_SRST_PULLS_TRST)==0))
	{
		/* debug entry was already prepared in arm7_9_assert_reset() */
		target->debug_reason = DBG_REASON_DBGRQ;
	}

	return ERROR_OK;

}

int arm7_9_deassert_reset(target_t *target)
{
	int retval=ERROR_OK;
	LOG_DEBUG("target->state: %s",
		  Jim_Nvp_value2name_simple( nvp_target_state,target->state)->name);


	/* deassert reset lines */
	jtag_add_reset(0, 0);

	if (target->reset_halt&&(jtag_reset_config & RESET_SRST_PULLS_TRST)!=0)
	{
		LOG_WARNING("srst pulls trst - can not reset into halted mode. Issuing halt after reset.");
		/* set up embedded ice registers again */
		if ((retval=target->type->examine(target))!=ERROR_OK)
			return retval;

		if ((retval=target_poll(target))!=ERROR_OK)
		{
			return retval;
		}

		if ((retval=target_halt(target))!=ERROR_OK)
		{
			return retval;
		}

	}
	return retval;
}

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;

	long long then=timeval_ms();
	int timeout;
	while (!(timeout=((timeval_ms()-then)>1000)))
	{
		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;
		if (debug_level>=3)
		{
			alive_sleep(100);
		} else
		{
			keep_alive();
		}
	}
	if (timeout)
	{
		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);

	if ((retval = arm7_9_clear_halt(target)) != ERROR_OK)
	{