arm11.c

	}

	//	  target->state		= TARGET_HALTED;
	target->debug_reason	= DBG_REASON_SINGLESTEP;

	CHECK_RETVAL(target_call_event_callbacks(target, TARGET_EVENT_HALTED));

	return ERROR_OK;
}

/* target reset control */
int arm11_assert_reset(struct target_s *target)
{
	FNC_INFO;

#if 0
	/* assert reset lines */
	/* resets only the DBGTAP, not the ARM */

	jtag_add_reset(1, 0);
	jtag_add_sleep(5000);

	arm11_common_t * arm11 = target->arch_info;
	arm11->trst_active = true;
#endif

	if (target->reset_halt)
	{
		CHECK_RETVAL(target_halt(target));
	}

	return ERROR_OK;
}

int arm11_deassert_reset(struct target_s *target)
{
	FNC_INFO;

#if 0
	LOG_DEBUG("target->state: %s",
		Jim_Nvp_value2name_simple( nvp_target_state, target->state )->name );


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

	arm11_common_t * arm11 = target->arch_info;
	arm11->trst_active = false;

	if (arm11->halt_requested)
		return arm11_halt(target);
#endif

	return ERROR_OK;
}

int arm11_soft_reset_halt(struct target_s *target)
{
	FNC_INFO_NOTIMPLEMENTED;

	return ERROR_OK;
}

/* target register access for gdb */
int arm11_get_gdb_reg_list(struct target_s *target, struct reg_s **reg_list[], int *reg_list_size)
{
	FNC_INFO;

	arm11_common_t * arm11 = target->arch_info;

	*reg_list_size	= ARM11_GDB_REGISTER_COUNT;
	*reg_list		= malloc(sizeof(reg_t*) * ARM11_GDB_REGISTER_COUNT);

	{size_t i;
	for (i = 16; i < 24; i++)
	{
		(*reg_list)[i] = &arm11_gdb_dummy_fp_reg;
	}}

	(*reg_list)[24] = &arm11_gdb_dummy_fps_reg;

	{size_t i;
	for (i = 0; i < ARM11_REGCACHE_COUNT; i++)
	{
		if (arm11_reg_defs[i].gdb_num == -1)
			continue;

		(*reg_list)[arm11_reg_defs[i].gdb_num] = arm11->reg_list + i;
	}}

	return ERROR_OK;
}

/* target memory access
 * size: 1 = byte (8bit), 2 = half-word (16bit), 4 = word (32bit)
 * count: number of items of <size>
 */
int arm11_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
{
	/** \todo TODO: check if buffer cast to u32* and u16* might cause alignment problems */

	FNC_INFO;

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

	LOG_DEBUG("ADDR %08x  SIZE %08x  COUNT %08x", address, size, count);

	arm11_common_t * arm11 = target->arch_info;

	arm11_run_instr_data_prepare(arm11);

	/* MRC p14,0,r0,c0,c5,0 */
	arm11_run_instr_data_to_core1(arm11, 0xee100e15, address);

	switch (size)
	{
	case 1:
		/** \todo TODO: check if dirty is the right choice to force a rewrite on arm11_resume() */
		arm11->reg_list[ARM11_RC_R1].dirty = 1;

		{size_t i;
		for (i = 0; i < count; i++)
		{
			/* ldrb    r1, [r0], #1 */
			/* ldrb    r1, [r0] */
			arm11_run_instr_no_data1(arm11,
					!arm11_config_memrw_no_increment ? 0xe4d01001 : 0xe5d01000);

			u32 res;
			/* MCR p14,0,R1,c0,c5,0 */
			arm11_run_instr_data_from_core(arm11, 0xEE001E15, &res, 1);

			*buffer++ = res;
		}}

		break;

	case 2:
		{
			arm11->reg_list[ARM11_RC_R1].dirty = 1;

			u16 * buf16 = (u16*)buffer;

			{size_t i;
			for (i = 0; i < count; i++)
			{
				/* ldrh    r1, [r0], #2 */
				arm11_run_instr_no_data1(arm11,
					!arm11_config_memrw_no_increment ? 0xe0d010b2 : 0xe1d010b0);

				u32 res;

				/* MCR p14,0,R1,c0,c5,0 */
				arm11_run_instr_data_from_core(arm11, 0xEE001E15, &res, 1);

				*buf16++ = res;
			}}

			break;
		}

	case 4:

		/* LDC p14,c5,[R0],#4 */
		/* LDC p14,c5,[R0] */
		arm11_run_instr_data_from_core(arm11,
			(!arm11_config_memrw_no_increment ? 0xecb05e01 : 0xed905e00),
			(u32 *)buffer, count);
		break;
	}

	arm11_run_instr_data_finish(arm11);

	return ERROR_OK;
}

int arm11_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
{
	FNC_INFO;

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

	LOG_DEBUG("ADDR %08x  SIZE %08x  COUNT %08x", address, size, count);

	arm11_common_t * arm11 = target->arch_info;

	arm11_run_instr_data_prepare(arm11);

	/* MRC p14,0,r0,c0,c5,0 */
	arm11_run_instr_data_to_core1(arm11, 0xee100e15, address);

	switch (size)
	{
	case 1:
		{
			arm11->reg_list[ARM11_RC_R1].dirty = 1;

			{size_t i;
			for (i = 0; i < count; i++)
			{
				/* MRC p14,0,r1,c0,c5,0 */
				arm11_run_instr_data_to_core1(arm11, 0xee101e15, *buffer++);

				/* strb    r1, [r0], #1 */
				/* strb    r1, [r0] */
				arm11_run_instr_no_data1(arm11,
					!arm11_config_memrw_no_increment ? 0xe4c01001 : 0xe5c01000);
			}}

			break;
		}

	case 2:
		{
			arm11->reg_list[ARM11_RC_R1].dirty = 1;

			u16 * buf16 = (u16*)buffer;

			{size_t i;
			for (i = 0; i < count; i++)
			{
				/* MRC p14,0,r1,c0,c5,0 */
				arm11_run_instr_data_to_core1(arm11, 0xee101e15, *buf16++);

				/* strh    r1, [r0], #2 */
				/* strh    r1, [r0] */
				arm11_run_instr_no_data1(arm11,
					!arm11_config_memrw_no_increment ? 0xe0c010b2 : 0xe1c010b0);
			}}

			break;
		}

	case 4:
		/** \todo TODO: check if buffer cast to u32* might cause alignment problems */

		if (!arm11_config_memwrite_burst)
		{
			/* STC p14,c5,[R0],#4 */
			/* STC p14,c5,[R0]*/
			arm11_run_instr_data_to_core(arm11,
				(!arm11_config_memrw_no_increment ? 0xeca05e01 : 0xed805e00),
				(u32 *)buffer, count);
		}
		else
		{
			/* STC p14,c5,[R0],#4 */
			/* STC p14,c5,[R0]*/
			arm11_run_instr_data_to_core_noack(arm11,
				(!arm11_config_memrw_no_increment ? 0xeca05e01 : 0xed805e00),
				(u32 *)buffer, count);
		}

		break;
	}

#if 1
	/* r0 verification */
	if (!arm11_config_memrw_no_increment)
	{
		u32 r0;

		/* MCR p14,0,R0,c0,c5,0 */
		arm11_run_instr_data_from_core(arm11, 0xEE000E15, &r0, 1);

		if (address + size * count != r0)
		{
			LOG_ERROR("Data transfer failed. (%d)", (r0 - address) - size * count);

			if (arm11_config_memwrite_burst)
				LOG_ERROR("use 'arm11 memwrite burst disable' to disable fast burst mode");

			if (arm11_config_memwrite_error_fatal)
				return ERROR_FAIL;
		}
	}
#endif

	arm11_run_instr_data_finish(arm11);

	return ERROR_OK;
}


/* write target memory in multiples of 4 byte, optimized for writing large quantities of data */
int arm11_bulk_write_memory(struct target_s *target, u32 address, u32 count, u8 *buffer)
{
	FNC_INFO;

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

	return arm11_write_memory(target, address, 4, count, buffer);
}

int arm11_checksum_memory(struct target_s *target, u32 address, u32 count, u32* checksum)
{
	FNC_INFO_NOTIMPLEMENTED;

	return ERROR_OK;
}

/* target break-/watchpoint control
* rw: 0 = write, 1 = read, 2 = access
*/
int arm11_add_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
{
	FNC_INFO;

	arm11_common_t * arm11 = target->arch_info;

#if 0
	if (breakpoint->type == BKPT_SOFT)
	{
		LOG_INFO("sw breakpoint requested, but software breakpoints not enabled");
		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
	}
#endif

	if (!arm11->free_brps)
	{
		LOG_INFO("no breakpoint unit available for hardware breakpoint");
		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
	}

	if (breakpoint->length != 4)
	{
		LOG_INFO("only breakpoints of four bytes length supported");
		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
	}

	arm11->free_brps--;

	return ERROR_OK;
}

int arm11_remove_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
{
	FNC_INFO;

	arm11_common_t * arm11 = target->arch_info;

	arm11->free_brps++;

	return ERROR_OK;
}

int arm11_add_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
{
	FNC_INFO_NOTIMPLEMENTED;

	return ERROR_OK;
}

int arm11_remove_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
{
	FNC_INFO_NOTIMPLEMENTED;

	return ERROR_OK;
}

// HACKHACKHACK - FIXME mode/state
/* target algorithm support */
int arm11_run_algorithm(struct target_s *target, int num_mem_params, mem_param_t *mem_params,
			int num_reg_params, reg_param_t *reg_params, u32 entry_point, u32 exit_point,
			int timeout_ms, void *arch_info)
{
		arm11_common_t *arm11 = target->arch_info;
	armv4_5_algorithm_t *arm11_algorithm_info = arch_info;
//	enum armv4_5_state core_state = arm11->core_state;
//	enum armv4_5_mode core_mode = arm11->core_mode;
	u32 context[16];
	u32 cpsr;
	int exit_breakpoint_size = 0;
	int i;
	int retval = ERROR_OK;
		LOG_DEBUG("Running algorithm");

	if (arm11_algorithm_info->common_magic != ARMV4_5_COMMON_MAGIC)
	{
		LOG_ERROR("current target isn't an ARMV4/5 target");
		return ERROR_TARGET_INVALID;
	}

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

	// FIXME
//	if (armv4_5_mode_to_number(arm11->core_mode)==-1)
//		return ERROR_FAIL;

	// Save regs
	for (i = 0; i < 16; i++)
	{
		context[i] = buf_get_u32((u8*)(&arm11->reg_values[i]),0,32);
		LOG_DEBUG("Save %i: 0x%x",i,context[i]);
	}

	cpsr = buf_get_u32((u8*)(arm11->reg_values+ARM11_RC_CPSR),0,32);
	LOG_DEBUG("Save CPSR: 0x%x", cpsr);

	for (i = 0; i < num_mem_params; i++)
	{
		target_write_buffer(target, mem_params[i].address, mem_params[i].size, mem_params[i].value);
	}

	// Set register parameters
	for (i = 0; i < num_reg_params; i++)
	{
		reg_t *reg = register_get_by_name(arm11->core_cache, reg_params[i].reg_name, 0);
		if (!reg)
		{
			LOG_ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
			exit(-1);
		}

		if (reg->size != reg_params[i].size)
		{
			LOG_ERROR("BUG: register '%s' size doesn't match reg_params[i].size", reg_params[i].reg_name);
			exit(-1);
		}
		arm11_set_reg(reg,reg_params[i].value);
//		printf("%i: Set %s =%08x\n", i, reg_params[i].reg_name,val);
	}

	exit_breakpoint_size = 4;

/*	arm11->core_state = arm11_algorithm_info->core_state;
	if (arm11->core_state == ARMV4_5_STATE_ARM)
				exit_breakpoint_size = 4;
	else if (arm11->core_state == ARMV4_5_STATE_THUMB)
		exit_breakpoint_size = 2;
	else
	{
		LOG_ERROR("BUG: can't execute algorithms when not in ARM or Thumb state");
		exit(-1);
	}
*/
	if (arm11_algorithm_info->core_mode != ARMV4_5_MODE_ANY)
	{
		LOG_DEBUG("setting core_mode: 0x%2.2x", arm11_algorithm_info->core_mode);
		buf_set_u32(arm11->reg_list[ARM11_RC_CPSR].value, 0, 5, arm11_algorithm_info->core_mode);
		arm11->reg_list[ARM11_RC_CPSR].dirty = 1;
		arm11->reg_list[ARM11_RC_CPSR].valid = 1;
	}

	if ((retval = breakpoint_add(target, exit_point, exit_breakpoint_size, BKPT_HARD)) != ERROR_OK)
	{
		LOG_ERROR("can't add breakpoint to finish algorithm execution");
		retval = ERROR_TARGET_FAILURE;
		goto restore;
	}

	// no debug, otherwise breakpoint is not set
	CHECK_RETVAL(target_resume(target, 0, entry_point, 1, 0));

	CHECK_RETVAL(target_wait_state(target, TARGET_HALTED, timeout_ms));

	if (target->state != TARGET_HALTED)
	{
		CHECK_RETVAL(target_halt(target));

		CHECK_RETVAL(target_wait_state(target, TARGET_HALTED, 500));

		retval = ERROR_TARGET_TIMEOUT;

		goto del_breakpoint;
	}

	if (buf_get_u32(arm11->reg_list[15].value, 0, 32) != exit_point)
	{
		LOG_WARNING("target reentered debug state, but not at the desired exit point: 0x%4.4x",
			buf_get_u32(arm11->reg_list[15].value, 0, 32));
		retval = ERROR_TARGET_TIMEOUT;
		goto del_breakpoint;
	}

	for (i = 0; i < num_mem_params; i++)
	{
		if (mem_params[i].direction != PARAM_OUT)
			target_read_buffer(target, mem_params[i].address, mem_params[i].size, mem_params[i].value);
	}

	for (i = 0; i < num_reg_params; i++)
	{
		if (reg_params[i].direction != PARAM_OUT)
		{
			reg_t *reg = register_get_by_name(arm11->core_cache, reg_params[i].reg_name, 0);
			if (!reg)
			{
				LOG_ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
				exit(-1);
			}

			if (reg->size != reg_params[i].size)
			{
				LOG_ERROR("BUG: register '%s' size doesn't match reg_params[i].size", reg_params[i].reg_name);
				exit(-1);
			}

			buf_set_u32(reg_params[i].value, 0, 32, buf_get_u32(reg->value, 0, 32));
		}
	}

del_breakpoint:
	breakpoint_remove(target, exit_point);

restore:
	// Restore context
	for (i = 0; i < 16; i++)
	{
		LOG_DEBUG("restoring register %s with value 0x%8.8x",
			 arm11->reg_list[i].name, context[i]);
		arm11_set_reg(&arm11->reg_list[i], (u8*)&context[i]);
	}
	LOG_DEBUG("restoring CPSR with value 0x%8.8x", cpsr);
	arm11_set_reg(&arm11->reg_list[ARM11_RC_CPSR], (u8*)&cpsr);

//	arm11->core_state = core_state;
//	arm11->core_mode = core_mode;

	return retval;
}

int arm11_target_create(struct target_s *target, Jim_Interp *interp)
{
	FNC_INFO;

	NEW(arm11_common_t, arm11, 1);

	arm11->target = target;

	/* prepare JTAG information for the new target */
	arm11->jtag_info.tap	= target->tap;
	arm11->jtag_info.scann_size	= 5;

	CHECK_RETVAL(arm_jtag_setup_connection(&arm11->jtag_info));

	if (target->tap==NULL)
		return ERROR_FAIL;

	if (target->tap->ir_length != 5)
	{
		LOG_ERROR("'target arm11' expects IR LENGTH = 5");
		return ERROR_COMMAND_SYNTAX_ERROR;
	}

	target->arch_info = arm11;

	return ERROR_OK;
}

int arm11_init_target(struct command_context_s *cmd_ctx, struct target_s *target)
{
	/* Initialize anything we can set up without talking to the target */
	return arm11_build_reg_cache(target);
}

/* talk to the target and set things up */
int arm11_examine(struct target_s *target)
{
	FNC_INFO;

	arm11_common_t * arm11 = target->arch_info;

	/* check IDCODE */

	arm11_add_IR(arm11, ARM11_IDCODE, ARM11_TAP_DEFAULT);

	scan_field_t		idcode_field;

	arm11_setup_field(arm11, 32, NULL, &arm11->device_id, &idcode_field);

	arm11_add_dr_scan_vc(1, &idcode_field, TAP_DRPAUSE);

	/* check DIDR */

	arm11_add_debug_SCAN_N(arm11, 0x00, ARM11_TAP_DEFAULT);

	arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);

	scan_field_t		chain0_fields[2];

	arm11_setup_field(arm11, 32, NULL,	&arm11->didr,		chain0_fields + 0);
	arm11_setup_field(arm11,  8, NULL,	&arm11->implementor,	chain0_fields + 1);

	arm11_add_dr_scan_vc(asizeof(chain0_fields), chain0_fields, TAP_IDLE);

	CHECK_RETVAL(jtag_execute_queue());

	switch (arm11->device_id & 0x0FFFF000)
	{
	case 0x07B36000:	LOG_INFO("found ARM1136"); break;
	case 0x07B56000:	LOG_INFO("found ARM1156"); break;
	case 0x07B76000:	LOG_INFO("found ARM1176"); break;
	default:
	{
		LOG_ERROR("'target arm11' expects IDCODE 0x*7B*7****");
		return ERROR_FAIL;
	}
	}

	arm11->debug_version = (arm11->didr >> 16) & 0x0F;

	if (arm11->debug_version != ARM11_DEBUG_V6 &&
		arm11->debug_version != ARM11_DEBUG_V61)
	{
		LOG_ERROR("Only ARMv6 v6 and v6.1 architectures supported.");
		return ERROR_FAIL;
	}

	arm11->brp	= ((arm11->didr >> 24) & 0x0F) + 1;
	arm11->wrp	= ((arm11->didr >> 28) & 0x0F) + 1;

	/** \todo TODO: reserve one brp slot if we allow breakpoints during step */
	arm11->free_brps = arm11->brp;
	arm11->free_wrps = arm11->wrp;

	LOG_DEBUG("IDCODE %08x IMPLEMENTOR %02x DIDR %08x",
		arm11->device_id,
		arm11->implementor,
		arm11->didr);

	/* as a side-effect this reads DSCR and thus
	 * clears the ARM11_DSCR_STICKY_PRECISE_DATA_ABORT / Sticky Precise Data Abort Flag
	 * as suggested by the spec.
	 */

	arm11_check_init(arm11, NULL);

	target->type->examined = 1;

	return ERROR_OK;
}

int arm11_quit(void)
{
	FNC_INFO_NOTIMPLEMENTED;

	return ERROR_OK;
}

/** Load a register that is marked !valid in the register cache */
int arm11_get_reg(reg_t *reg)
{
	FNC_INFO;

	target_t * target = ((arm11_reg_state_t *)reg->arch_info)->target;

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

	/** \todo TODO: Check this. We assume that all registers are fetched at debug entry. */

#if 0
	arm11_common_t *arm11 = target->arch_info;
	const arm11_reg_defs_t * arm11_reg_info = arm11_reg_defs + ((arm11_reg_state_t *)reg->arch_info)->def_index;
#endif

	return ERROR_OK;
}

/** Change a value in the register cache */
int arm11_set_reg(reg_t *reg, u8 *buf)
{
	FNC_INFO;

	target_t * target = ((arm11_reg_state_t *)reg->arch_info)->target;
	arm11_common_t *arm11 = target->arch_info;
//	  const arm11_reg_defs_t * arm11_reg_info = arm11_reg_defs + ((arm11_reg_state_t *)reg->arch_info)->def_index;

	arm11->reg_values[((arm11_reg_state_t *)reg->arch_info)->def_index] = buf_get_u32(buf, 0, 32);
	reg->valid	= 1;
	reg->dirty	= 1;

	return ERROR_OK;
}

int arm11_build_reg_cache(target_t *target)
{
	arm11_common_t *arm11 = target->arch_info;

	NEW(reg_cache_t,		cache,				1);
	NEW(reg_t,				reg_list,			ARM11_REGCACHE_COUNT);
	NEW(arm11_reg_state_t,	arm11_reg_states,	ARM11_REGCACHE_COUNT);

	if (arm11_regs_arch_type == -1)
		arm11_regs_arch_type = register_reg_arch_type(arm11_get_reg, arm11_set_reg);

	register_init_dummy(&arm11_gdb_dummy_fp_reg);
	register_init_dummy(&arm11_gdb_dummy_fps_reg);

	arm11->reg_list	= reg_list;

	/* Build the process context cache */
	cache->name		= "arm11 registers";
	cache->next		= NULL;
	cache->reg_list	= reg_list;
	cache->num_regs	= ARM11_REGCACHE_COUNT;

	reg_cache_t **cache_p = register_get_last_cache_p(&target->reg_cache);
	(*cache_p) = cache;

	arm11->core_cache = cache;
//	  armv7m->process_context = cache;

	size_t i;

	/* Not very elegant assertion */
	if (ARM11_REGCACHE_COUNT != asizeof(arm11->reg_values) ||
		ARM11_REGCACHE_COUNT != asizeof(arm11_reg_defs) ||
		ARM11_REGCACHE_COUNT != ARM11_RC_MAX)
	{
		LOG_ERROR("BUG: arm11->reg_values inconsistent (%d " ZU " " ZU " %d)", ARM11_REGCACHE_COUNT, asizeof(arm11->reg_values), asizeof(arm11_reg_defs), ARM11_RC_MAX);
		exit(-1);
	}

	for (i = 0; i < ARM11_REGCACHE_COUNT; i++)
	{
		reg_t *						r	= reg_list			+ i;
		const arm11_reg_defs_t *	rd	= arm11_reg_defs	+ i;
		arm11_reg_state_t *			rs	= arm11_reg_states	+ i;

		r->name				= rd->name;
		r->size				= 32;
		r->value			= (u8 *)(arm11->reg_values + i);
		r->dirty			= 0;
		r->valid			= 0;
		r->bitfield_desc	= NULL;
		r->num_bitfields	= 0;
		r->arch_type		= arm11_regs_arch_type;
		r->arch_info		= rs;

		rs->def_index		= i;
		rs->target			= target;
	}

	return ERROR_OK;
}

int arm11_handle_bool(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, bool * var, char * name)
{
	if (argc == 0)
	{
		LOG_INFO("%s is %s.", name, *var ? "enabled" : "disabled");
		return ERROR_OK;
	}

	if (argc != 1)
		return ERROR_COMMAND_SYNTAX_ERROR;

	switch (args[0][0])
	{
	case '0':	/* 0 */
	case 'f':	/* false */
	case 'F':
	case 'd':	/* disable */
	case 'D':
		*var = false;
		break;

	case '1':	/* 1 */
	case 't':	/* true */
	case 'T':
	case 'e':	/* enable */
	case 'E':
		*var = true;
		break;
	}

	LOG_INFO("%s %s.", *var ? "Enabled" : "Disabled", name);

	return ERROR_OK;
}

#define BOOL_WRAPPER(name, print_name)	\
int arm11_handle_bool_##name(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc) \
{ \
	return arm11_handle_bool(cmd_ctx, cmd, args, argc, &arm11_config_##name, print_name); \
}

#define RC_TOP(name, descr, more)  \
{ \
	command_t * new_cmd = register_command(cmd_ctx, top_cmd, name, NULL, COMMAND_ANY, descr);  \
	command_t * top_cmd = new_cmd; \
	more \
}

#define RC_FINAL(name, descr, handler)	\
	register_command(cmd_ctx, top_cmd, name, handler, COMMAND_ANY, descr);

#define RC_FINAL_BOOL(name, descr, var)  \
	register_command(cmd_ctx, top_cmd, name, arm11_handle_bool_##var, COMMAND_ANY, descr);

BOOL_WRAPPER(memwrite_burst,			"memory write burst mode")
BOOL_WRAPPER(memwrite_error_fatal,		"fatal error mode for memory writes")
BOOL_WRAPPER(memrw_no_increment,		"\"no increment\" mode for memory transfers")
BOOL_WRAPPER(step_irq_enable,			"IRQs while stepping")

int arm11_handle_vcr(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
{
	if (argc == 1)
	{
		arm11_vcr = strtoul(args[0], NULL, 0);
	}
	else if (argc != 0)
	{
		return ERROR_COMMAND_SYNTAX_ERROR;
	}

	LOG_INFO("VCR 0x%08X", arm11_vcr);
	return ERROR_OK;
}

const u32 arm11_coproc_instruction_limits[] =
{
	15,				/* coprocessor */
	7,				/* opcode 1 */
	15,				/* CRn */
	15,				/* CRm */
	7,				/* opcode 2 */
	0xFFFFFFFF,		/* value */
};

const char arm11_mrc_syntax[] = "Syntax: mrc <jtag_target> <coprocessor> <opcode 1> <CRn> <CRm> <opcode 2>. All parameters are numbers only.";
const char arm11_mcr_syntax[] = "Syntax: mcr <jtag_target> <coprocessor> <opcode 1> <CRn> <CRm> <opcode 2> <32bit value to write>. All parameters are numbers only.";

arm11_common_t * arm11_find_target(const char * arg)
{
	jtag_tap_t *	tap;
	target_t *		t;

	tap = jtag_TapByString(arg);

	if (!tap)
		return 0;

	for (t = all_targets; t; t = t->next)
	{
		if (t->tap != tap)
			continue;

		/* if (t->type == arm11_target) */
		if (0 == strcmp(t->type->name, "arm11"))
			return t->arch_info;
	}

	return 0;
}

int arm11_handle_mrc_mcr(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, bool read)
{
	if (argc != (read ? 6 : 7))
	{
		LOG_ERROR("Invalid number of arguments. %s", read ? arm11_mrc_syntax : arm11_mcr_syntax);
		return -1;
	}

	arm11_common_t * arm11 = arm11_find_target(args[0]);

	if (!arm11)
	{
		LOG_ERROR("Parameter 1 is not a the JTAG chain position of an ARM11 device. %s",
			read ? arm11_mrc_syntax : arm11_mcr_syntax);

		return -1;
	}

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

	u32	values[6];

	{size_t i;
	for (i = 0; i < (read ? 5 : 6); i++)
	{
		values[i] = strtoul(args[i + 1], NULL, 0);

		if (values[i] > arm11_coproc_instruction_limits[i])
		{
			LOG_ERROR("Parameter %ld out of bounds (%d max). %s",
				(long)(i + 2), arm11_coproc_instruction_limits[i],
				read ? arm11_mrc_syntax : arm11_mcr_syntax);
			return -1;
		}
	}}

	u32 instr = 0xEE000010	|
		(values[0] <<  8) |
		(values[1] << 21) |
		(values[2] << 16) |
		(values[3] <<  0) |
		(values[4] <<  5);

	if (read)
		instr |= 0x00100000;

	arm11_run_instr_data_prepare(arm11);

	if (read)
	{
		u32 result;
		arm11_run_instr_data_from_core_via_r0(arm11, instr, &result);

		LOG_INFO("MRC p%d, %d, R0, c%d, c%d, %d = 0x%08x (%d)",
			values[0], values[1], values[2], values[3], values[4], result, result);
	}
	else
	{
		arm11_run_instr_data_to_core_via_r0(arm11, instr, values[5]);

		LOG_INFO("MRC p%d, %d, R0 (#0x%08x), c%d, c%d, %d",
			values[0], values[1],
			values[5],
			values[2], values[3], values[4]);
	}

	arm11_run_instr_data_finish(arm11);


	return ERROR_OK;
}

int arm11_handle_mrc(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
{
	return arm11_handle_mrc_mcr(cmd_ctx, cmd, args, argc, true);
}

int arm11_handle_mcr(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
{
	return arm11_handle_mrc_mcr(cmd_ctx, cmd, args, argc, false);
}

int arm11_register_commands(struct command_context_s *cmd_ctx)
{
	FNC_INFO;

	command_t * top_cmd = NULL;

	RC_TOP(				"arm11",				"arm11 specific commands",

	RC_TOP(				"memwrite",				"Control memory write transfer mode",

		RC_FINAL_BOOL(	"burst",				"Enable/Disable non-standard but fast burst mode (default: enabled)",
						memwrite_burst)

		RC_FINAL_BOOL(	"error_fatal",			"Terminate program if transfer error was found (default: enabled)",
						memwrite_error_fatal)
	)

	RC_FINAL_BOOL(		"no_increment",			"Don't increment address on multi-read/-write (default: disabled)",
						memrw_no_increment)

	RC_FINAL_BOOL(		"step_irq_enable",		"Enable interrupts while stepping (default: disabled)",
						step_irq_enable)

	RC_FINAL(			"vcr",					"Control (Interrupt) Vector Catch Register",
						arm11_handle_vcr)

	RC_FINAL(			"mrc",					"Read Coprocessor register",
						arm11_handle_mrc)

	RC_FINAL(			"mcr",					"Write Coprocessor register",
						arm11_handle_mcr)
	)

	return ERROR_OK;
}