static int dsp563xx_write_core_reg(struct target *target, int num)
{
	uint32_t reg_value;
	struct dsp563xx_core_reg *dsp563xx_core_reg;
	struct dsp563xx_common *dsp563xx = target_to_dsp563xx(target);

	if ((num < 0) || (num >= DSP563XX_NUMCOREREGS))
		return ERROR_INVALID_ARGUMENTS;

	reg_value = buf_get_u32(dsp563xx->core_cache->reg_list[num].value, 0, 32);
	dsp563xx_core_reg = dsp563xx->core_cache->reg_list[num].arch_info;
	dsp563xx->core_regs[num] = reg_value;
	dsp563xx->core_cache->reg_list[num].valid = 1;
	dsp563xx->core_cache->reg_list[num].dirty = 0;

	return ERROR_OK;
}

static int or1k_write_core_reg(struct target *target, int num)
{
	struct or1k_common *or1k = target_to_or1k(target);

	LOG_DEBUG("-");

	if ((num < 0) || (num >= OR1KNUMCOREREGS))
		return ERROR_COMMAND_SYNTAX_ERROR;

	uint32_t reg_value = buf_get_u32(or1k->core_cache->reg_list[num].value, 0, 32);
	or1k->core_regs[num] = reg_value;
	LOG_DEBUG("Write core reg %i value 0x%08" PRIx32, num , reg_value);
	or1k->core_cache->reg_list[num].valid = 1;
	or1k->core_cache->reg_list[num].dirty = 0;

	return ERROR_OK;
}

static int xtensa_write_memory_inner(struct target *target,
				     uint32_t address,
				     uint32_t size,
				     uint32_t count,
				     const uint8_t *buffer)
{
	int res;
	uint32_t inst;
	uint8_t imm8;

	/* Push base address to a0 via DDR */
	res = xtensa_tap_queue_load_general_reg(target, 0, address);
	if(res != ERROR_OK)
		return res;

	for(imm8 = 0; imm8 < count; imm8++) {
		/* load next word from buffer into a1, via DDR */
		res = xtensa_tap_queue_load_general_reg(target, 1,
							buf_get_u32(buffer+imm8*size, 0, 8*size));
		if(res != ERROR_OK)
			return res;

		/* determine the store instruction (based on size) */
		switch(size) {
		case 4:
			inst = XT_INS_S32I(0, 1, imm8); break;
		case 2:
			inst = XT_INS_S16I(0, 1, imm8); break;
		case 1:
			inst = XT_INS_S8I(0, 1, imm8); break;
		default:
			return ERROR_COMMAND_SYNTAX_ERROR;
		}
		/* queue the store instruction to the address register */
		res = xtensa_tap_queue_cpu_inst(target, inst);
		if(res != ERROR_OK)
			return res;
	}
	res = jtag_execute_queue();
	if(res != ERROR_OK) {
		LOG_ERROR("%s: JTAG scan failed", __func__);
		return res;
	}

	return ERROR_OK;
}

static int avr32_set_core_reg(struct reg *reg, uint8_t *buf)
{
	struct avr32_core_reg *avr32_reg = reg->arch_info;
	struct target *target = avr32_reg->target;
	uint32_t value = buf_get_u32(buf, 0, 32);

	if (target->state != TARGET_HALTED)
	{
		return ERROR_TARGET_NOT_HALTED;
	}

	buf_set_u32(reg->value, 0, 32, value);
	reg->dirty = 1;
	reg->valid = 1;

	return ERROR_OK;
}

static int avr32_write_core_reg(struct target *target, int num)
{
	uint32_t reg_value;

	/* get pointers to arch-specific information */
	struct avr32_ap7k_common *ap7k = target_to_ap7k(target);

	if ((num < 0) || (num >= AVR32NUMCOREREGS))
		return ERROR_COMMAND_SYNTAX_ERROR;

	reg_value = buf_get_u32(ap7k->core_cache->reg_list[num].value, 0, 32);
	ap7k->core_regs[num] = reg_value;
	LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", num, reg_value);
	ap7k->core_cache->reg_list[num].valid = 1;
	ap7k->core_cache->reg_list[num].dirty = 0;

	return ERROR_OK;
}

/** Write JTAG instruction register
 *
 * \param arm11		Target state variable.
 * \param instr		An ARM11 DBGTAP instruction. Use enum #arm11_instructions.
 * \param state		Pass the final TAP state or ARM11_TAP_DEFAULT for the default value (Pause-IR).
 *
 * \remarks			This adds to the JTAG command queue but does \em not execute it.
 */
void arm11_add_IR(struct arm11_common * arm11, uint8_t instr, tap_state_t state)
{
	struct jtag_tap *tap = arm11->arm.target->tap;

	if (buf_get_u32(tap->cur_instr, 0, 5) == instr)
	{
		JTAG_DEBUG("IR <= 0x%02x SKIPPED", instr);
		return;
	}

	JTAG_DEBUG("IR <= %s (0x%02x)", arm11_ir_to_string(instr), instr);

	struct scan_field field;

	arm11_setup_field(arm11, 5, &instr, NULL, &field);

	arm11_add_ir_scan_vc(1, &field, state == ARM11_TAP_DEFAULT ? TAP_IRPAUSE : state);
}

static int mips32_write_core_reg(struct target *target, unsigned int num)
{
	uint32_t reg_value;

	/* get pointers to arch-specific information */
	struct mips32_common *mips32 = target_to_mips32(target);

	if (num >= MIPS32_NUM_REGS)
		return ERROR_COMMAND_SYNTAX_ERROR;

	reg_value = buf_get_u32(mips32->core_cache->reg_list[num].value, 0, 32);
	mips32->core_regs[num] = reg_value;
	LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", num , reg_value);
	mips32->core_cache->reg_list[num].valid = 1;
	mips32->core_cache->reg_list[num].dirty = 0;

	return ERROR_OK;
}

int dsp563xx_set_core_reg(struct reg *reg, uint8_t * buf)
{
	LOG_DEBUG("%s", __FUNCTION__);

	struct dsp563xx_core_reg *dsp563xx_reg = reg->arch_info;
	struct target *target = dsp563xx_reg->target;
	uint32_t value = buf_get_u32(buf, 0, 32);

	if (target->state != TARGET_HALTED)
	{
		return ERROR_TARGET_NOT_HALTED;
	}

	buf_set_u32(reg->value, 0, reg->size, value);
	reg->dirty = 1;
	reg->valid = 1;

	return ERROR_OK;
}

static int str9xpec_protect_check(struct flash_bank *bank)
{
	uint8_t status;
	int i;

	struct str9xpec_flash_controller *str9xpec_info = bank->driver_priv;

	status = str9xpec_read_config(bank);

	for (i = 0; i < bank->num_sectors; i++) {
		if (buf_get_u32(str9xpec_info->options, str9xpec_info->sector_bits[i], 1))
			bank->sectors[i].is_protected = 1;
		else
			bank->sectors[i].is_protected = 0;
	}

	if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
		return ERROR_FLASH_OPERATION_FAILED;
	return ERROR_OK;
}

static int mips32_write_core_reg(struct target *target, int num)
{
	uint32_t reg_value;
	struct mips32_core_reg *mips_core_reg;

	/* get pointers to arch-specific information */
	struct mips32_common *mips32 = target_to_mips32(target);

	if ((num < 0) || (num >= MIPS32NUMCOREREGS))
		return ERROR_INVALID_ARGUMENTS;

	reg_value = buf_get_u32(mips32->core_cache->reg_list[num].value, 0, 32);
	mips_core_reg = mips32->core_cache->reg_list[num].arch_info;
	mips32->core_regs[num] = reg_value;
	LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", num , reg_value);
	mips32->core_cache->reg_list[num].valid = 1;
	mips32->core_cache->reg_list[num].dirty = 0;

	return ERROR_OK;
}

/**
 * Restores target context using the cache of core registers set up
 * by armv7m_build_reg_cache(), calling optional core-specific hooks.
 */
int armv7m_restore_context(struct target *target)
{
	int i;
	struct armv7m_common *armv7m = target_to_armv7m(target);
	struct reg_cache *cache = armv7m->arm.core_cache;

	LOG_DEBUG(" ");

	if (armv7m->pre_restore_context)
		armv7m->pre_restore_context(target);

	for (i = ARMV7M_NUM_REGS - 1; i >= 0; i--) {
		if (cache->reg_list[i].dirty) {
			uint32_t value = buf_get_u32(cache->reg_list[i].value, 0, 32);
			armv7m->arm.write_core_reg(target, &cache->reg_list[i], i, ARM_MODE_ANY, value);
		}
	}

	return ERROR_OK;
}

void mips_ejtag_set_instr(struct mips_ejtag *ejtag_info, int new_instr)
{
	struct jtag_tap *tap;

	tap = ejtag_info->tap;
	assert(tap != NULL);

	if (buf_get_u32(tap->cur_instr, 0, tap->ir_length) != (uint32_t)new_instr) {

		struct scan_field field;
		uint8_t t[4];

		field.num_bits = tap->ir_length;
		field.out_value = t;
		buf_set_u32(t, 0, field.num_bits, new_instr);
		field.in_value = NULL;

		jtag_add_ir_scan(tap, &field, TAP_IDLE);
	}
}

int lakemont_resume(struct target *t, int current, uint32_t address,
			int handle_breakpoints, int debug_execution)
{
	struct breakpoint *bp = NULL;
	struct x86_32_common *x86_32 = target_to_x86_32(t);

	if (check_not_halted(t))
		return ERROR_TARGET_NOT_HALTED;
	/* TODO lakemont_enable_breakpoints(t); */
	if (t->state == TARGET_HALTED) {

		/* running away for a software breakpoint needs some special handling */
		uint32_t eip = buf_get_u32(x86_32->cache->reg_list[EIP].value, 0, 32);
		bp = breakpoint_find(t, eip);
		if (bp != NULL /*&& bp->type == BKPT_SOFT*/) {
			/* the step will step over the breakpoint */
			if (lakemont_step(t, 0, 0, 1) != ERROR_OK) {
				LOG_ERROR("%s stepping over a software breakpoint at 0x%08" PRIx32 " "
						"failed to resume the target", __func__, eip);
				return ERROR_FAIL;
			}
		}

		/* if breakpoints are enabled, we need to redirect these into probe mode */
		struct breakpoint *activeswbp = t->breakpoints;
		while (activeswbp != NULL && activeswbp->set == 0)
			activeswbp = activeswbp->next;
		struct watchpoint *activehwbp = t->watchpoints;
		while (activehwbp != NULL && activehwbp->set == 0)
			activehwbp = activehwbp->next;
		if (activeswbp != NULL || activehwbp != NULL)
			buf_set_u32(x86_32->cache->reg_list[PMCR].value, 0, 32, 1);

		if (do_resume(t) != ERROR_OK)
			return ERROR_FAIL;
	} else {
		LOG_USER("target not halted");
		return ERROR_FAIL;
	}
	return ERROR_OK;
}

static int armv4_5_set_core_reg(struct reg *reg, uint8_t *buf)
{
	struct arm_reg *reg_arch_info = reg->arch_info;
	struct target *target = reg_arch_info->target;
	struct arm *armv4_5_target = target_to_arm(target);
	uint32_t value = buf_get_u32(buf, 0, 32);

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

	/* Except for CPSR, the "reg" command exposes a writeback model
	 * for the register cache.
	 */
	if (reg == armv4_5_target->cpsr) {
		arm_set_cpsr(armv4_5_target, value);

		/* Older cores need help to be in ARM mode during halt
		 * mode debug, so we clear the J and T bits if we flush.
		 * For newer cores (v6/v7a/v7r) we don't need that, but
		 * it won't hurt since CPSR is always flushed anyway.
		 */
		if (armv4_5_target->core_mode !=
			(enum arm_mode)(value & 0x1f)) {
			LOG_DEBUG("changing ARM core mode to '%s'",
				arm_mode_name(value & 0x1f));
			value &= ~((1 << 24) | (1 << 5));
			uint8_t t[4];
			buf_set_u32(t, 0, 32, value);
			armv4_5_target->write_core_reg(target, reg,
				16, ARM_MODE_ANY, t);
		}
	} else {
		buf_set_u32(reg->value, 0, 32, value);
		reg->valid = 1;
	}
	reg->dirty = 1;

	return ERROR_OK;
}

static int str9xpec_set_instr(struct jtag_tap *tap, uint32_t new_instr, tap_state_t end_state)
{
	if (tap == NULL)
		return ERROR_TARGET_INVALID;

	if (buf_get_u32(tap->cur_instr, 0, tap->ir_length) != new_instr) {
		struct scan_field field;

		field.num_bits = tap->ir_length;
		void *t = calloc(DIV_ROUND_UP(field.num_bits, 8), 1);
		field.out_value = t;
		buf_set_u32(t, 0, field.num_bits, new_instr);
		field.in_value = NULL;

		jtag_add_ir_scan(tap, &field, end_state);

		free(t);
	}

	return ERROR_OK;
}

/** Read STATUS register. */
int arc_jtag_status(struct arc_jtag * const jtag_info, uint32_t * const value)
{
	assert(jtag_info != NULL);
	assert(jtag_info->tap != NULL);

	uint8_t buffer[4];

	/* Fill command queue. */
	arc_jtag_reset_transaction(jtag_info);
	arc_jtag_write_ir(jtag_info, ARC_JTAG_STATUS_REG);
	arc_jtag_read_dr(jtag_info, buffer, TAP_IDLE);
	arc_jtag_reset_transaction(jtag_info);

	/* Execute queue. */
	CHECK_RETVAL(jtag_execute_queue());

	/* Parse output. */
	*value = buf_get_u32(buffer, 0, 32);

	return ERROR_OK;
}

static int virtex2_set_instr(struct jtag_tap *tap, uint32_t new_instr)
{
	if (tap == NULL)
		return ERROR_FAIL;

	if (buf_get_u32(tap->cur_instr, 0, tap->ir_length) != new_instr) {
		struct scan_field field;

		field.num_bits = tap->ir_length;
		void *t = calloc(DIV_ROUND_UP(field.num_bits, 8), 1);
		field.out_value = t;
		buf_set_u32(t, 0, field.num_bits, new_instr);
		field.in_value = NULL;

		jtag_add_ir_scan(tap, &field, TAP_IDLE);

		free(t);
	}

	return ERROR_OK;
}

static void amt_jtagaccel_scan(bool ir_scan, enum scan_type type, uint8_t *buffer, int scan_size)
{
	int bits_left = scan_size;
	int bit_count = 0;
	tap_state_t saved_end_state = tap_get_end_state();
	uint8_t aw_tdi_option;
	uint8_t dw_tdi_scan;
	uint8_t dr_tdo;
	uint8_t aw_tms_scan;
	uint8_t tms_scan[2];
	int jtag_speed_var;
	int retval = jtag_get_speed(&jtag_speed_var);
	assert(retval == ERROR_OK);

	if (ir_scan)
		amt_jtagaccel_end_state(TAP_IRSHIFT);
	else
		amt_jtagaccel_end_state(TAP_DRSHIFT);

	/* Only move if we're not already there */
	if (tap_get_state() != tap_get_end_state())
		amt_jtagaccel_state_move();

	amt_jtagaccel_end_state(saved_end_state);

	/* handle unaligned bits at the beginning */
	if ((scan_size - 1) % 8) {
		aw_tdi_option = 0x30 | (((scan_size - 1) % 8) - 1);
		AMT_AW(aw_tdi_option);

		dw_tdi_scan = buf_get_u32(buffer, bit_count, (scan_size - 1) % 8) & 0xff;
		AMT_DW(dw_tdi_scan);
		if (jtag_speed_var > 3 || rtck_enabled)
			amt_wait_scan_busy();

		if ((type == SCAN_IN) || (type == SCAN_IO)) {
			AMT_DR(dr_tdo);
			dr_tdo = dr_tdo >> (8 - ((scan_size - 1) % 8));
			buf_set_u32(buffer, bit_count, (scan_size - 1) % 8, dr_tdo);
		}

static int runCode(struct ecosflash_flash_bank *info,
                   uint32_t codeStart, uint32_t codeStop, uint32_t r0, uint32_t r1, uint32_t r2,
                   uint32_t *result,
                   /* timeout in ms */
                   int timeout)
{
    struct target *target = info->target;

    struct reg_param reg_params[3];
    struct arm_algorithm armv4_5_info;
    armv4_5_info.common_magic = ARM_COMMON_MAGIC;
    armv4_5_info.core_mode = ARM_MODE_SVC;
    armv4_5_info.core_state = ARM_STATE_ARM;

    init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT);
    init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
    init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);

    buf_set_u32(reg_params[0].value, 0, 32, r0);
    buf_set_u32(reg_params[1].value, 0, 32, r1);
    buf_set_u32(reg_params[2].value, 0, 32, r2);

    int retval;
    if ((retval = target_run_algorithm(target, 0, NULL, 3, reg_params,
                                       codeStart,
                                       codeStop, timeout,
                                       &armv4_5_info)) != ERROR_OK)
    {
        LOG_ERROR("error executing eCos flash algorithm");
        return retval;
    }

    *result = buf_get_u32(reg_params[0].value, 0, 32);

    destroy_reg_param(&reg_params[0]);
    destroy_reg_param(&reg_params[1]);
    destroy_reg_param(&reg_params[2]);

    return ERROR_OK;
}

int avr32_jtag_mwa_set_address(struct avr32_jtag *jtag_info, int slave,
		uint32_t addr, int mode)
{
	struct scan_field fields[2];
	uint8_t addr_buf[4];
	uint8_t slave_buf[4];
	uint8_t busy_buf[4];
	int busy;

	memset(fields, 0, sizeof(fields));

	do {
		memset(addr_buf, 0, sizeof(addr_buf));
		memset(busy_buf, 0, sizeof(busy_buf));
		memset(slave_buf, 0, sizeof(slave_buf));

		buf_set_u32(slave_buf, 0, 4, slave);
		buf_set_u32(addr_buf, 0, 1, mode);
		buf_set_u32(addr_buf, 1, 30, addr >> 2);

		fields[0].num_bits = 31;
		fields[0].in_value = NULL;
		fields[0].out_value = addr_buf;

		fields[1].num_bits = 4;
		fields[1].in_value = busy_buf;
		fields[1].out_value = slave_buf;

		jtag_add_dr_scan(jtag_info->tap, 2, fields, TAP_IDLE);
		if (jtag_execute_queue() != ERROR_OK)
		{
			LOG_ERROR("%s: setting address failed", __func__);
			return ERROR_FAIL;
		}
		busy = buf_get_u32(busy_buf, 1, 1);
	} while(busy);

	return ERROR_OK;
}