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 "embeddedice.h"
#include "target_request.h"
#include "arm7_9_common.h"
#include "time_support.h"
#include "arm_simulator.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);

/**
 * Clear watchpoints for an ARM7/9 target.
 *
 * @param arm7_9 Pointer to the common struct for an ARM7/9 target
 * @return JTAG error status after executing queue
 */
static int arm7_9_clear_watchpoints(arm7_9_common_t *arm7_9)
{
	LOG_DEBUG("-");
	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();
}

/**
 * Assign a watchpoint to one of the two available hardware comparators in an
 * ARM7 or ARM9 target.
 *
 * @param arm7_9 Pointer to the common struct for an ARM7/9 target
 * @param breakpoint Pointer to the breakpoint to be used as a watchpoint
 */
static void arm7_9_assign_wp(arm7_9_common_t *arm7_9, breakpoint_t *breakpoint)
{
	if (!arm7_9->wp0_used)
	{
		arm7_9->wp0_used = 1;
		breakpoint->set = 1;
		arm7_9->wp_available--;
	}
	else if (!arm7_9->wp1_used)
	{
		arm7_9->wp1_used = 1;
		breakpoint->set = 2;
		arm7_9->wp_available--;
	}
	else
	{
		LOG_ERROR("BUG: no hardware comparator available");
	}
	LOG_DEBUG("BPID: %d (0x%08" PRIx32 ") using hw wp: %d", 
			  breakpoint->unique_id,
			  breakpoint->address,
			  breakpoint->set );
}

/**
 * Setup an ARM7/9 target's embedded ICE registers for software breakpoints.
 *
 * @param arm7_9 Pointer to common struct for ARM7/9 targets
 * @return Error codes if there is a problem finding a watchpoint or the result
 *         of executing the JTAG queue
 */
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;
	}
	LOG_DEBUG("SW BP using hw wp: %d", 
			  arm7_9->sw_breakpoints_added );

	return jtag_execute_queue();
}

/**
 * Setup the common pieces for an ARM7/9 target after reset or on startup.
 *
 * @param target Pointer to an ARM7/9 target to setup
 * @return Result of clearing the watchpoints on the target
 */
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);
}

/**
 * Retrieves the architecture information pointers for ARMv4/5 and ARM7/9
 * targets.  A return of ERROR_OK signifies that the target is a valid target
 * and that the pointers have been set properly.
 *
 * @param target Pointer to the target device to get the pointers from
 * @param armv4_5_p Pointer to be filled in with the common struct for ARMV4/5
 *                  targets
 * @param arm7_9_p Pointer to be filled in with the common struct for ARM7/9
 *                 targets
 * @return ERROR_OK if successful
 */
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;
}

/**
 * Set either a hardware or software breakpoint on an ARM7/9 target.  The
 * breakpoint is set up even if it is already set.  Some actions, e.g. reset,
 * might have erased the values in Embedded ICE.
 *
 * @param target Pointer to the target device to set the breakpoints on
 * @param breakpoint Pointer to the breakpoint to be set
 * @return For hardware breakpoints, this is the result of executing the JTAG
 *         queue.  For software breakpoints, this will be the status of the
 *         required memory reads and writes
 */
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;

	LOG_DEBUG("BPID: %d, Address: 0x%08" PRIx32,
			  breakpoint->unique_id,
			  breakpoint->address );

	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 */
		uint32_t mask = (breakpoint->length == 4) ? 0x3u : 0x1u;

		/* reassign a hw breakpoint */
		if (breakpoint->set == 0)
		{
			arm7_9_assign_wp(arm7_9, breakpoint);
		}

		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)
		{
			uint32_t verify = 0xffffffff;
			/* keep the original instruction in target endianness */
			if ((retval = target_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_read_u32(target, breakpoint->address, &verify)) != ERROR_OK)
			{
				return retval;
			}
			if (verify != arm7_9->arm_bkpt)
			{
				LOG_ERROR("Unable to set 32 bit software breakpoint at address %08" PRIx32 " - check that memory is read/writable", breakpoint->address);
				return ERROR_OK;
			}
		}
		else
		{
			uint16_t verify = 0xffff;
			/* keep the original instruction in target endianness */
			if ((retval = target_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_read_u16(target, breakpoint->address, &verify)) != ERROR_OK)
			{
				return retval;
			}
			if (verify != arm7_9->thumb_bkpt)
			{
				LOG_ERROR("Unable to set thumb software breakpoint at address %08" PRIx32 " - check that memory is read/writable", breakpoint->address);
				return ERROR_OK;
			}
		}
		breakpoint->set = 1;
	}

	return retval;
}

/**
 * Unsets an existing breakpoint on an ARM7/9 target.  If it is a hardware
 * breakpoint, the watchpoint used will be freed and the Embedded ICE registers
 * will be updated.  Otherwise, the software breakpoint will be restored to its
 * original instruction if it hasn't already been modified.
 *
 * @param target Pointer to ARM7/9 target to unset the breakpoint from
 * @param breakpoint Pointer to breakpoint to be unset
 * @return For hardware breakpoints, this is the result of executing the JTAG
 *         queue.  For software breakpoints, this will be the status of the
 *         required memory reads and writes
 */
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;

	LOG_DEBUG("BPID: %d, Address: 0x%08" PRIx32,
			  breakpoint->unique_id,
			  breakpoint->address );

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

	if (breakpoint->type == BKPT_HARD)
	{
		LOG_DEBUG("BPID: %d Releasing hw wp: %d", 
				  breakpoint->unique_id,
				  breakpoint->set );
		if (breakpoint->set == 1)
		{
			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], 0x0);
			arm7_9->wp0_used = 0;
			arm7_9->wp_available++;
		}
		else if (breakpoint->set == 2)
		{
			embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], 0x0);
			arm7_9->wp1_used = 0;
			arm7_9->wp_available++;
		}
		retval = jtag_execute_queue();
		breakpoint->set = 0;
	}
	else
	{
		/* restore original instruction (kept in target endianness) */
		if (breakpoint->length == 4)
		{
			uint32_t current_instr;
			/* check that user program as not modified breakpoint instruction */
			if ((retval = target_read_memory(target, breakpoint->address, 4, 1, (uint8_t*)&current_instr)) != ERROR_OK)
			{
				return retval;
			}
			if (current_instr == arm7_9->arm_bkpt)
				if ((retval = target_write_memory(target, breakpoint->address, 4, 1, breakpoint->orig_instr)) != ERROR_OK)
				{
					return retval;
				}
		}
		else
		{
			uint16_t current_instr;
			/* check that user program as not modified breakpoint instruction */
			if ((retval = target_read_memory(target, breakpoint->address, 2, 1, (uint8_t*)&current_instr)) != ERROR_OK)
			{
				return retval;
			}
			if (current_instr == arm7_9->thumb_bkpt)
				if ((retval = target_write_memory(target, breakpoint->address, 2, 1, breakpoint->orig_instr)) != ERROR_OK)
				{
					return retval;
				}
		}
		breakpoint->set = 0;
	}

	return retval;
}

/**
 * Add a breakpoint to an ARM7/9 target.  This makes sure that there are no
 * dangling breakpoints and that the desired breakpoint can be added.
 *
 * @param target Pointer to the target ARM7/9 device to add a breakpoint to
 * @param breakpoint Pointer to the breakpoint to be added
 * @return An error status if there is a problem adding the breakpoint or the
 *         result of setting the breakpoint
 */
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_assign_wp(arm7_9, breakpoint);
	}

	arm7_9->breakpoint_count++;

	return arm7_9_set_breakpoint(target, breakpoint);
}

/**
 * Removes a breakpoint from an ARM7/9 target.  This will make sure there are no
 * dangling breakpoints and updates available watchpoints if it is a hardware
 * breakpoint.
 *
 * @param target Pointer to the target to have a breakpoint removed
 * @param breakpoint Pointer to the breakpoint to be removed
 * @return Error status if there was a problem unsetting the breakpoint or the
 *         watchpoints could not be cleared
 */
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;
}

/**
 * Sets a watchpoint for an ARM7/9 target in one of the watchpoint units.  It is
 * considered a bug to call this function when there are no available watchpoint
 * units.
 *
 * @param target Pointer to an ARM7/9 target to set a watchpoint on
 * @param watchpoint Pointer to the watchpoint to be set
 * @return Error status if watchpoint set fails or the result of executing the
 *         JTAG queue
 */
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;
	uint32_t 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;
}

/**
 * Unset an existing watchpoint and clear the used watchpoint unit.
 *
 * @param target Pointer to the target to have the watchpoint removed
 * @param watchpoint Pointer to the watchpoint to be removed
 * @return Error status while trying to unset the watchpoint or the result of
 *         executing the JTAG queue
 */
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;
}

/**
 * Add a watchpoint to an ARM7/9 target.  If there are no watchpoint units
 * available, an error response is returned.
 *
 * @param target Pointer to the ARM7/9 target to add a watchpoint to
 * @param watchpoint Pointer to the watchpoint to be added
 * @return Error status while trying to add the watchpoint
 */
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;
}

/**
 * Remove a watchpoint from an ARM7/9 target.  The watchpoint will be unset and
 * the used watchpoint unit will be reopened.
 *
 * @param target Pointer to the target to remove a watchpoint from
 * @param watchpoint Pointer to the watchpoint to be removed
 * @return Result of trying to unset the watchpoint
 */
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;
}

/**
 * Restarts the target by sending a RESTART instruction and moving the JTAG
 * state to IDLE.  This includes a timeout waiting for DBGACK and SYSCOMP to be
 * asserted by the processor.
 *
 * @param target Pointer to target to issue commands to
 * @return Error status if there is a timeout or a problem while executing the
 *         JTAG queue
 */
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_set_end_state(TAP_IDLE);
	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: %" PRIx32 "", buf_get_u32(dbg_stat->value, 0, dbg_stat->size));
		return ERROR_TARGET_TIMEOUT;
	}

	return ERROR_OK;
}

/**
 * Restarts the target by sending a RESTART instruction and moving the JTAG
 * state to IDLE.  This validates that DBGACK and SYSCOMP are set without
 * waiting until they are.
 *
 * @param target Pointer to the target to issue commands to
 * @return Always ERROR_OK
 */
int arm7_9_execute_fast_sys_speed(struct target_s *target)
{
	static int set = 0;
	static uint8_t 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_set_end_state(TAP_IDLE);
	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_mask);

	return ERROR_OK;
}

/**
 * Get some data from the ARM7/9 target.
 *
 * @param target Pointer to the ARM7/9 target to read data from
 * @param size The number of 32bit words to be read
 * @param buffer Pointer to the buffer that will hold the data
 * @return The result of receiving data from the Embedded ICE unit
 */
int arm7_9_target_request_data(target_t *target, uint32_t size, uint8_t *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;
	uint32_t *data;
	int retval = ERROR_OK;
	uint32_t i;

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

	retval = embeddedice_receive(jtag_info, data, size);

	/* return the 32-bit ints in the 8-bit array */
	for (i = 0; i < size; i++)
	{
		h_u32_to_le(buffer + (i * 4), data[i]);
	}

	free(data);

	return retval;
}

/**
 * Handles requests to an ARM7/9 target.  If debug messaging is enabled, the
 * target is running and the DCC control register has the W bit high, this will
 * execute the request on the target.
 *
 * @param priv Void pointer expected to be a target_t pointer
 * @return ERROR_OK unless there are issues with the JTAG queue or when reading
 *                  from the Embedded ICE unit
 */
int arm7_9_handle_target_request(void *priv)
{
	int retval = ERROR_OK;
	target_t *target = priv;
	if (!target_was_examined(target))
		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)
		{
			uint32_t 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;
}

/**
 * Polls an ARM7/9 target for its current status.  If DBGACK is set, the target
 * is manipulated to the right halted state based on its current state.  This is
 * what happens:
 *
 * <table>
 * 		<tr><th > State</th><th > Action</th></tr>
 * 		<tr><td > TARGET_RUNNING | TARGET_RESET</td><td > Enters debug mode.  If TARGET_RESET, pc may be checked</td></tr>
 * 		<tr><td > TARGET_UNKNOWN</td><td > Warning is logged</td></tr>
 * 		<tr><td > TARGET_DEBUG_RUNNING</td><td > Enters debug mode</td></tr>
 * 		<tr><td > TARGET_HALTED</td><td > Nothing</td></tr>
 * </table>
 *
 * If the target does not end up in the halted state, a warning is produced.  If
 * DBGACK is cleared, then the target is expected to either be running or
 * running in debug.
 *
 * @param target Pointer to the ARM7/9 target to poll
 * @return ERROR_OK or an error status if a command fails
 */
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)
				{
					enum reset_types jtag_reset_config = jtag_get_reset_config();
					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);
				uint32_t t=*((uint32_t *)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 state %d", target->state);
		}
	}
	else
	{
		if (target->state != TARGET_DEBUG_RUNNING)
			target->state = TARGET_RUNNING;
	}

	return ERROR_OK;
}

/**
 * Asserts the reset (SRST) on an ARM7/9 target.  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 is
 * asserted, hence a hook that allows the target to set up its reset-halt
 * condition is setup prior to asserting reset.
 *
 * @param target Pointer to an ARM7/9 target to assert reset on
 * @return ERROR_FAIL if the JTAG device does not have SRST, otherwise ERROR_OK
 */
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);

	enum reset_types jtag_reset_config = jtag_get_reset_config();
	if (!(jtag_reset_config & RESET_HAS_SRST))
	{
		LOG_ERROR("Can't assert SRST");