armv4_5.c 21.2 KB
Newer Older
1
2
3
4
/***************************************************************************
 *   Copyright (C) 2005 by Dominic Rath                                    *
 *   Dominic.Rath@gmx.de                                                   *
 *                                                                         *
5
6
7
 *   Copyright (C) 2008 by Spencer Oliver                                  *
 *   spen@spen-soft.co.uk                                                  *
 *                                                                         *
oharboe's avatar
oharboe committed
8
9
10
 *   Copyright (C) 2008 by Oyvind Harboe                                   *
 *   oyvind.harboe@zylin.com                                               *
 *                                                                         *
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
 *   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.             *
 ***************************************************************************/
26
#ifdef HAVE_CONFIG_H
27
#include "config.h"
28
29
#endif

30
#include "armv4_5.h"
31
#include "arm_disassembler.h"
32
33
34
#include "binarybuffer.h"


oharboe's avatar
oharboe committed
35
bitfield_desc_t armv4_5_psr_bitfield_desc[] =
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
{
	{"M[4:0]", 5},
	{"T", 1},
	{"F", 1},
	{"I", 1},
	{"reserved", 16},
	{"J", 1},
	{"reserved", 2},
	{"Q", 1},
	{"V", 1},
	{"C", 1},
	{"Z", 1},
	{"N", 1},
};

char* armv4_5_core_reg_list[] =
{
	"r0", "r1", "r2", "r3", "r4", "r5", "r6", "r7", "r8", "r9", "r10", "r11", "r12", "r13_usr", "lr_usr", "pc",
oharboe's avatar
oharboe committed
54

55
	"r8_fiq", "r9_fiq", "r10_fiq", "r11_fiq", "r12_fiq", "r13_fiq", "lr_fiq",
oharboe's avatar
oharboe committed
56

57
	"r13_irq", "lr_irq",
oharboe's avatar
oharboe committed
58

59
	"r13_svc", "lr_svc",
oharboe's avatar
oharboe committed
60

61
	"r13_abt", "lr_abt",
oharboe's avatar
oharboe committed
62

63
	"r13_und", "lr_und",
oharboe's avatar
oharboe committed
64

65
66
67
	"cpsr", "spsr_fiq", "spsr_irq", "spsr_svc", "spsr_abt", "spsr_und"
};

68
char * armv4_5_mode_strings_list[] =
69
{
70
	"Illegal mode value", "User", "FIQ", "IRQ", "Supervisor", "Abort", "Undefined", "System"
71
72
};

73
/* Hack! Yuk! allow -1 index, which simplifies codepaths elsewhere in the code */
zwelch's avatar
zwelch committed
74
char** armv4_5_mode_strings = armv4_5_mode_strings_list + 1;
75

76
77
78
79
80
81
82
char* armv4_5_state_strings[] =
{
	"ARM", "Thumb", "Jazelle"
};

int armv4_5_core_reg_arch_type = -1;

oharboe's avatar
oharboe committed
83
armv4_5_core_reg_t armv4_5_core_reg_list_arch_info[] =
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
{
	{0, ARMV4_5_MODE_ANY, NULL, NULL},
	{1, ARMV4_5_MODE_ANY, NULL, NULL},
	{2, ARMV4_5_MODE_ANY, NULL, NULL},
	{3, ARMV4_5_MODE_ANY, NULL, NULL},
	{4, ARMV4_5_MODE_ANY, NULL, NULL},
	{5, ARMV4_5_MODE_ANY, NULL, NULL},
	{6, ARMV4_5_MODE_ANY, NULL, NULL},
	{7, ARMV4_5_MODE_ANY, NULL, NULL},
	{8, ARMV4_5_MODE_ANY, NULL, NULL},
	{9, ARMV4_5_MODE_ANY, NULL, NULL},
	{10, ARMV4_5_MODE_ANY, NULL, NULL},
	{11, ARMV4_5_MODE_ANY, NULL, NULL},
	{12, ARMV4_5_MODE_ANY, NULL, NULL},
	{13, ARMV4_5_MODE_USR, NULL, NULL},
	{14, ARMV4_5_MODE_USR, NULL, NULL},
	{15, ARMV4_5_MODE_ANY, NULL, NULL},
oharboe's avatar
oharboe committed
101

102
103
104
105
106
107
108
	{8, ARMV4_5_MODE_FIQ, NULL, NULL},
	{9, ARMV4_5_MODE_FIQ, NULL, NULL},
	{10, ARMV4_5_MODE_FIQ, NULL, NULL},
	{11, ARMV4_5_MODE_FIQ, NULL, NULL},
	{12, ARMV4_5_MODE_FIQ, NULL, NULL},
	{13, ARMV4_5_MODE_FIQ, NULL, NULL},
	{14, ARMV4_5_MODE_FIQ, NULL, NULL},
oharboe's avatar
oharboe committed
109

110
111
112
113
114
115
116
117
	{13, ARMV4_5_MODE_IRQ, NULL, NULL},
	{14, ARMV4_5_MODE_IRQ, NULL, NULL},

	{13, ARMV4_5_MODE_SVC, NULL, NULL},
	{14, ARMV4_5_MODE_SVC, NULL, NULL},

	{13, ARMV4_5_MODE_ABT, NULL, NULL},
	{14, ARMV4_5_MODE_ABT, NULL, NULL},
oharboe's avatar
oharboe committed
118

119
120
	{13, ARMV4_5_MODE_UND, NULL, NULL},
	{14, ARMV4_5_MODE_UND, NULL, NULL},
oharboe's avatar
oharboe committed
121

122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
	{16, ARMV4_5_MODE_ANY, NULL, NULL},
	{16, ARMV4_5_MODE_FIQ, NULL, NULL},
	{16, ARMV4_5_MODE_IRQ, NULL, NULL},
	{16, ARMV4_5_MODE_SVC, NULL, NULL},
	{16, ARMV4_5_MODE_ABT, NULL, NULL},
	{16, ARMV4_5_MODE_UND, NULL, NULL}
};

/* map core mode (USR, FIQ, ...) and register number to indizes into the register cache */
int armv4_5_core_reg_map[7][17] =
{
	{	/* USR */
		0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 31
	},
	{	/* FIQ */
		0, 1, 2, 3, 4, 5, 6, 7, 16, 17, 18, 19, 20, 21, 22, 15, 32
	},
	{	/* IRQ */
		0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 23, 24, 15, 33
	},
	{	/* SVC */
		0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 25, 26, 15, 34
	},
	{	/* ABT */
		0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 27, 28, 15, 35
	},
	{	/* UND */
		0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 29, 30, 15, 36
	},
	{	/* SYS */
		0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 31
	}
};

156
uint8_t armv4_5_gdb_dummy_fp_value[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
157
158
159
160
161
162

reg_t armv4_5_gdb_dummy_fp_reg =
{
	"GDB dummy floating-point register", armv4_5_gdb_dummy_fp_value, 0, 1, 96, NULL, 0, NULL, 0
};

163
uint8_t armv4_5_gdb_dummy_fps_value[] = {0, 0, 0, 0};
164
165
166
167
168
169
170
171
172
173
174

reg_t armv4_5_gdb_dummy_fps_reg =
{
	"GDB dummy floating-point status register", armv4_5_gdb_dummy_fps_value, 0, 1, 32, NULL, 0, NULL, 0
};

int armv4_5_get_core_reg(reg_t *reg)
{
	int retval;
	armv4_5_core_reg_t *armv4_5 = reg->arch_info;
	target_t *target = armv4_5->target;
oharboe's avatar
oharboe committed
175

176
177
	if (target->state != TARGET_HALTED)
	{
oharboe's avatar
oharboe committed
178
		LOG_ERROR("Target not halted");
179
180
		return ERROR_TARGET_NOT_HALTED;
	}
oharboe's avatar
oharboe committed
181

182
	/* retval = armv4_5->armv4_5_common->full_context(target); */
183
	retval = armv4_5->armv4_5_common->read_core_reg(target, armv4_5->num, armv4_5->mode);
oharboe's avatar
oharboe committed
184

185
186
187
	return retval;
}

188
int armv4_5_set_core_reg(reg_t *reg, uint8_t *buf)
189
190
191
{
	armv4_5_core_reg_t *armv4_5 = reg->arch_info;
	target_t *target = armv4_5->target;
192
	struct armv4_5_common_s *armv4_5_target = target_to_armv4_5(target);
193
	uint32_t value = buf_get_u32(buf, 0, 32);
oharboe's avatar
oharboe committed
194

195
196
197
198
	if (target->state != TARGET_HALTED)
	{
		return ERROR_TARGET_NOT_HALTED;
	}
oharboe's avatar
oharboe committed
199

200
201
202
203
204
205
206
207
	if (reg == &armv4_5_target->core_cache->reg_list[ARMV4_5_CPSR])
	{
		if (value & 0x20)
		{
			/* T bit should be set */
			if (armv4_5_target->core_state == ARMV4_5_STATE_ARM)
			{
				/* change state to Thumb */
208
				LOG_DEBUG("changing to Thumb state");
oharboe's avatar
oharboe committed
209
				armv4_5_target->core_state = ARMV4_5_STATE_THUMB;
210
211
212
213
214
215
216
217
			}
		}
		else
		{
			/* T bit should be cleared */
			if (armv4_5_target->core_state == ARMV4_5_STATE_THUMB)
			{
				/* change state to ARM */
218
				LOG_DEBUG("changing to ARM state");
oharboe's avatar
oharboe committed
219
				armv4_5_target->core_state = ARMV4_5_STATE_ARM;
220
221
			}
		}
oharboe's avatar
oharboe committed
222

223
		if (armv4_5_target->core_mode != (enum armv4_5_mode)(value & 0x1f))
224
		{
225
			LOG_DEBUG("changing ARM core mode to '%s'", armv4_5_mode_strings[armv4_5_mode_to_number(value & 0x1f)]);
226
227
228
			armv4_5_target->core_mode = value & 0x1f;
			armv4_5_target->write_core_reg(target, 16, ARMV4_5_MODE_ANY, value);
		}
229
	}
oharboe's avatar
oharboe committed
230

231
232
233
234
235
236
237
238
239
	buf_set_u32(reg->value, 0, 32, value);
	reg->dirty = 1;
	reg->valid = 1;

	return ERROR_OK;
}

int armv4_5_invalidate_core_regs(target_t *target)
{
240
	struct armv4_5_common_s *armv4_5 = target_to_armv4_5(target);
241
	int i;
oharboe's avatar
oharboe committed
242

243
244
245
246
247
	for (i = 0; i < 37; i++)
	{
		armv4_5->core_cache->reg_list[i].valid = 0;
		armv4_5->core_cache->reg_list[i].dirty = 0;
	}
oharboe's avatar
oharboe committed
248

249
250
251
252
253
254
255
256
	return ERROR_OK;
}

reg_cache_t* armv4_5_build_reg_cache(target_t *target, armv4_5_common_t *armv4_5_common)
{
	int num_regs = 37;
	reg_cache_t *cache = malloc(sizeof(reg_cache_t));
	reg_t *reg_list = malloc(sizeof(reg_t) * num_regs);
257
	armv4_5_core_reg_t *arch_info = malloc(sizeof(armv4_5_core_reg_t) * num_regs);
258
	int i;
oharboe's avatar
oharboe committed
259

260
261
262
263
	cache->name = "arm v4/5 registers";
	cache->next = NULL;
	cache->reg_list = reg_list;
	cache->num_regs = num_regs;
oharboe's avatar
oharboe committed
264

265
266
	if (armv4_5_core_reg_arch_type == -1)
		armv4_5_core_reg_arch_type = register_reg_arch_type(armv4_5_get_core_reg, armv4_5_set_core_reg);
oharboe's avatar
oharboe committed
267

268
269
270
	register_init_dummy(&armv4_5_gdb_dummy_fp_reg);
	register_init_dummy(&armv4_5_gdb_dummy_fps_reg);

271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
	for (i = 0; i < 37; i++)
	{
		arch_info[i] = armv4_5_core_reg_list_arch_info[i];
		arch_info[i].target = target;
		arch_info[i].armv4_5_common = armv4_5_common;
		reg_list[i].name = armv4_5_core_reg_list[i];
		reg_list[i].size = 32;
		reg_list[i].value = calloc(1, 4);
		reg_list[i].dirty = 0;
		reg_list[i].valid = 0;
		reg_list[i].bitfield_desc = NULL;
		reg_list[i].num_bitfields = 0;
		reg_list[i].arch_type = armv4_5_core_reg_arch_type;
		reg_list[i].arch_info = &arch_info[i];
	}
oharboe's avatar
oharboe committed
286

287
288
289
	return cache;
}

290
int armv4_5_arch_state(struct target_s *target)
291
{
292
	struct armv4_5_common_s *armv4_5 = target_to_armv4_5(target);
oharboe's avatar
oharboe committed
293

294
295
	if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
	{
296
		LOG_ERROR("BUG: called for a non-ARMv4/5 target");
297
298
		exit(-1);
	}
oharboe's avatar
oharboe committed
299

duane's avatar
duane committed
300
	LOG_USER("target halted in %s state due to %s, current mode: %s\ncpsr: 0x%8.8" PRIx32 " pc: 0x%8.8" PRIx32 "",
301
			 armv4_5_state_strings[armv4_5->core_state],
302
			 Jim_Nvp_value2name_simple(nvp_target_debug_reason, target->debug_reason)->name,
303
304
305
			 armv4_5_mode_strings[armv4_5_mode_to_number(armv4_5->core_mode)],
			 buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32),
			 buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
oharboe's avatar
oharboe committed
306

307
308
309
	return ERROR_OK;
}

310
COMMAND_HANDLER(handle_armv4_5_reg_command)
311
312
313
314
315
{
	char output[128];
	int output_len;
	int mode, num;
	target_t *target = get_current_target(cmd_ctx);
316
	struct armv4_5_common_s *armv4_5 = target_to_armv4_5(target);
oharboe's avatar
oharboe committed
317

318
319
320
321
322
	if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
	{
		command_print(cmd_ctx, "current target isn't an ARMV4/5 target");
		return ERROR_OK;
	}
oharboe's avatar
oharboe committed
323

324
325
326
327
328
	if (target->state != TARGET_HALTED)
	{
		command_print(cmd_ctx, "error: target must be halted for register accesses");
		return ERROR_OK;
	}
oharboe's avatar
oharboe committed
329

330
331
332
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;

333
334
335
336
337
338
339
340
341
	for (num = 0; num <= 15; num++)
	{
		output_len = 0;
		for (mode = 0; mode < 6; mode++)
		{
			if (!ARMV4_5_CORE_REG_MODENUM(armv4_5->core_cache, mode, num).valid)
			{
				armv4_5->full_context(target);
			}
342
343
344
			output_len += snprintf(output + output_len,
					       128 - output_len,
					       "%8s: %8.8" PRIx32 " ",
duane's avatar
duane committed
345
346
					       ARMV4_5_CORE_REG_MODENUM(armv4_5->core_cache, mode, num).name,
					       buf_get_u32(ARMV4_5_CORE_REG_MODENUM(armv4_5->core_cache, mode, num).value, 0, 32));
347
		}
348
		command_print(cmd_ctx, "%s", output);
349
	}
duane's avatar
duane committed
350
351
	command_print(cmd_ctx,
		      "    cpsr: %8.8" PRIx32 " spsr_fiq: %8.8" PRIx32 " spsr_irq: %8.8" PRIx32 " spsr_svc: %8.8" PRIx32 " spsr_abt: %8.8" PRIx32 " spsr_und: %8.8" PRIx32 "",
352
353
354
355
356
357
			  buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32),
			  buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_SPSR_FIQ].value, 0, 32),
			  buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_SPSR_IRQ].value, 0, 32),
			  buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_SPSR_SVC].value, 0, 32),
			  buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_SPSR_ABT].value, 0, 32),
			  buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_SPSR_UND].value, 0, 32));
oharboe's avatar
oharboe committed
358

359
360
361
	return ERROR_OK;
}

362
COMMAND_HANDLER(handle_armv4_5_core_state_command)
363
364
{
	target_t *target = get_current_target(cmd_ctx);
365
	struct armv4_5_common_s *armv4_5 = target_to_armv4_5(target);
oharboe's avatar
oharboe committed
366

367
368
369
370
371
	if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
	{
		command_print(cmd_ctx, "current target isn't an ARMV4/5 target");
		return ERROR_OK;
	}
oharboe's avatar
oharboe committed
372

373
374
375
376
377
378
379
380
381
382
383
	if (argc > 0)
	{
		if (strcmp(args[0], "arm") == 0)
		{
			armv4_5->core_state = ARMV4_5_STATE_ARM;
		}
		if (strcmp(args[0], "thumb") == 0)
		{
			armv4_5->core_state = ARMV4_5_STATE_THUMB;
		}
	}
oharboe's avatar
oharboe committed
384

385
	command_print(cmd_ctx, "core state: %s", armv4_5_state_strings[armv4_5->core_state]);
oharboe's avatar
oharboe committed
386

387
388
389
	return ERROR_OK;
}

390
COMMAND_HANDLER(handle_armv4_5_disassemble_command)
391
{
392
	int retval = ERROR_OK;
393
	target_t *target = get_current_target(cmd_ctx);
394
	struct armv4_5_common_s *armv4_5 = target_to_armv4_5(target);
395
	uint32_t address;
396
	int count = 1;
397
398
	int i;
	arm_instruction_t cur_instruction;
399
	uint32_t opcode;
zwelch's avatar
zwelch committed
400
	uint16_t thumb_opcode;
401
	int thumb = 0;
oharboe's avatar
oharboe committed
402

403
404
405
406
407
	if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
	{
		command_print(cmd_ctx, "current target isn't an ARMV4/5 target");
		return ERROR_OK;
	}
oharboe's avatar
oharboe committed
408

409
410
411
412
413
414
415
	switch (argc) {
	case 3:
		if (strcmp(args[2], "thumb") != 0)
			goto usage;
		thumb = 1;
		/* FALL THROUGH */
	case 2:
416
		COMMAND_PARSE_NUMBER(int, args[1], count);
417
418
		/* FALL THROUGH */
	case 1:
419
		COMMAND_PARSE_NUMBER(u32, args[0], address);
420
421
422
423
424
425
426
427
428
429
430
431
		if (address & 0x01) {
			if (!thumb) {
				command_print(cmd_ctx, "Disassemble as Thumb");
				thumb = 1;
			}
			address &= ~1;
		}
		break;
	default:
usage:
		command_print(cmd_ctx,
			"usage: armv4_5 disassemble <address> [<count> ['thumb']]");
432
433
		return ERROR_OK;
	}
oharboe's avatar
oharboe committed
434

435
436
	for (i = 0; i < count; i++)
	{
437
		if (thumb)
438
		{
439
			if ((retval = target_read_u16(target, address, &thumb_opcode)) != ERROR_OK)
440
441
442
			{
				return retval;
			}
443
			if ((retval = thumb_evaluate_opcode(thumb_opcode, address, &cur_instruction)) != ERROR_OK)
444
445
446
			{
				return retval;
			}
447
		}
448
		else {
449
			if ((retval = target_read_u32(target, address, &opcode)) != ERROR_OK)
450
451
452
			{
				return retval;
			}
453
			if ((retval = arm_evaluate_opcode(opcode, address, &cur_instruction)) != ERROR_OK)
454
455
456
			{
				return retval;
			}
457
		}
458
459
460
		command_print(cmd_ctx, "%s", cur_instruction.text);
		address += (thumb) ? 2 : 4;
	}
oharboe's avatar
oharboe committed
461

462
463
464
	return ERROR_OK;
}

465
466
467
468
int armv4_5_register_commands(struct command_context_s *cmd_ctx)
{
	command_t *armv4_5_cmd;

469
470
471
	armv4_5_cmd = register_command(cmd_ctx, NULL, "armv4_5",
			NULL, COMMAND_ANY,
			"armv4/5 specific commands");
oharboe's avatar
oharboe committed
472

473
474
475
476
477
478
479
480
481
	register_command(cmd_ctx, armv4_5_cmd, "reg",
			handle_armv4_5_reg_command, COMMAND_EXEC,
			"display ARM core registers");
	register_command(cmd_ctx, armv4_5_cmd, "core_state",
			handle_armv4_5_core_state_command, COMMAND_EXEC,
			"display/change ARM core state <arm | thumb>");
	register_command(cmd_ctx, armv4_5_cmd, "disassemble",
			handle_armv4_5_disassemble_command, COMMAND_EXEC,
			"disassemble instructions <address> [<count> ['thumb']]");
oharboe's avatar
oharboe committed
482

483
484
485
486
487
	return ERROR_OK;
}

int armv4_5_get_gdb_reg_list(target_t *target, reg_t **reg_list[], int *reg_list_size)
{
488
	struct armv4_5_common_s *armv4_5 = target_to_armv4_5(target);
489
	int i;
oharboe's avatar
oharboe committed
490

491
492
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;
oharboe's avatar
oharboe committed
493

494
495
	*reg_list_size = 26;
	*reg_list = malloc(sizeof(reg_t*) * (*reg_list_size));
oharboe's avatar
oharboe committed
496

497
498
499
500
	for (i = 0; i < 16; i++)
	{
		(*reg_list)[i] = &ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i);
	}
oharboe's avatar
oharboe committed
501

502
503
504
505
	for (i = 16; i < 24; i++)
	{
		(*reg_list)[i] = &armv4_5_gdb_dummy_fp_reg;
	}
oharboe's avatar
oharboe committed
506

507
508
	(*reg_list)[24] = &armv4_5_gdb_dummy_fps_reg;
	(*reg_list)[25] = &armv4_5->core_cache->reg_list[ARMV4_5_CPSR];
oharboe's avatar
oharboe committed
509

510
511
512
	return ERROR_OK;
}

513
/* wait for execution to complete and check exit point */
514
static int armv4_5_run_algorithm_completion(struct target_s *target, uint32_t exit_point, int timeout_ms, void *arch_info)
515
516
{
	int retval;
517
	struct armv4_5_common_s *armv4_5 = target_to_armv4_5(target);
518

519
	if ((retval = target_wait_state(target, TARGET_HALTED, timeout_ms)) != ERROR_OK)
520
521
522
	{
		return retval;
	}
523
524
	if (target->state != TARGET_HALTED)
	{
zwelch's avatar
zwelch committed
525
		if ((retval = target_halt(target)) != ERROR_OK)
526
			return retval;
zwelch's avatar
zwelch committed
527
		if ((retval = target_wait_state(target, TARGET_HALTED, 500)) != ERROR_OK)
528
529
530
531
532
		{
			return retval;
		}
		return ERROR_TARGET_TIMEOUT;
	}
533
534
535
536

	/* fast exit: ARMv5+ code can use BKPT */
	if (exit_point && buf_get_u32(armv4_5->core_cache->reg_list[15].value,
				0, 32) != exit_point)
537
	{
duane's avatar
duane committed
538
		LOG_WARNING("target reentered debug state, but not at the desired exit point: 0x%4.4" PRIx32 "",
539
540
541
542
543
544
545
			buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
		return ERROR_TARGET_TIMEOUT;
	}

	return ERROR_OK;
}

546
int armv4_5_run_algorithm_inner(struct target_s *target, int num_mem_params, struct mem_param *mem_params, int num_reg_params, struct reg_param *reg_params, uint32_t entry_point, uint32_t exit_point, int timeout_ms, void *arch_info, int (*run_it)(struct target_s *target, uint32_t exit_point, int timeout_ms, void *arch_info))
547
{
548
	struct armv4_5_common_s *armv4_5 = target_to_armv4_5(target);
549
550
551
	armv4_5_algorithm_t *armv4_5_algorithm_info = arch_info;
	enum armv4_5_state core_state = armv4_5->core_state;
	enum armv4_5_mode core_mode = armv4_5->core_mode;
552
553
	uint32_t context[17];
	uint32_t cpsr;
554
555
556
	int exit_breakpoint_size = 0;
	int i;
	int retval = ERROR_OK;
oharboe's avatar
   
oharboe committed
557
	LOG_DEBUG("Running algorithm");
oharboe's avatar
oharboe committed
558

559
560
	if (armv4_5_algorithm_info->common_magic != ARMV4_5_COMMON_MAGIC)
	{
561
		LOG_ERROR("current target isn't an ARMV4/5 target");
562
563
		return ERROR_TARGET_INVALID;
	}
oharboe's avatar
oharboe committed
564

565
566
	if (target->state != TARGET_HALTED)
	{
567
		LOG_WARNING("target not halted");
568
569
		return ERROR_TARGET_NOT_HALTED;
	}
oharboe's avatar
oharboe committed
570

571
572
573
	if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
		return ERROR_FAIL;

574
575
576
577
578
579
580
	/* armv5 and later can terminate with BKPT instruction; less overhead */
	if (!exit_point && armv4_5->is_armv4)
	{
		LOG_ERROR("ARMv4 target needs HW breakpoint location");
		return ERROR_FAIL;
	}

581
582
583
584
585
586
587
	for (i = 0; i <= 16; i++)
	{
		if (!ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_algorithm_info->core_mode, i).valid)
			armv4_5->read_core_reg(target, i, armv4_5_algorithm_info->core_mode);
		context[i] = buf_get_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_algorithm_info->core_mode, i).value, 0, 32);
	}
	cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32);
oharboe's avatar
oharboe committed
588

589
590
	for (i = 0; i < num_mem_params; i++)
	{
591
		if ((retval = target_write_buffer(target, mem_params[i].address, mem_params[i].size, mem_params[i].value)) != ERROR_OK)
592
593
594
		{
			return retval;
		}
595
	}
oharboe's avatar
oharboe committed
596

597
598
599
600
601
	for (i = 0; i < num_reg_params; i++)
	{
		reg_t *reg = register_get_by_name(armv4_5->core_cache, reg_params[i].reg_name, 0);
		if (!reg)
		{
602
			LOG_ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
603
604
			exit(-1);
		}
oharboe's avatar
oharboe committed
605

606
607
		if (reg->size != reg_params[i].size)
		{
608
			LOG_ERROR("BUG: register '%s' size doesn't match reg_params[i].size", reg_params[i].reg_name);
609
610
			exit(-1);
		}
oharboe's avatar
oharboe committed
611

612
		if ((retval = armv4_5_set_core_reg(reg, reg_params[i].value)) != ERROR_OK)
613
614
615
		{
			return retval;
		}
616
	}
oharboe's avatar
oharboe committed
617

618
619
620
621
622
623
624
	armv4_5->core_state = armv4_5_algorithm_info->core_state;
	if (armv4_5->core_state == ARMV4_5_STATE_ARM)
		exit_breakpoint_size = 4;
	else if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
		exit_breakpoint_size = 2;
	else
	{
625
		LOG_ERROR("BUG: can't execute algorithms when not in ARM or Thumb state");
626
627
		exit(-1);
	}
oharboe's avatar
oharboe committed
628

629
630
	if (armv4_5_algorithm_info->core_mode != ARMV4_5_MODE_ANY)
	{
631
		LOG_DEBUG("setting core_mode: 0x%2.2x", armv4_5_algorithm_info->core_mode);
632
633
634
635
636
		buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 5, armv4_5_algorithm_info->core_mode);
		armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 1;
		armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
	}

637
638
639
	/* terminate using a hardware or (ARMv5+) software breakpoint */
	if (exit_point && (retval = breakpoint_add(target, exit_point,
				exit_breakpoint_size, BKPT_HARD)) != ERROR_OK)
640
	{
641
		LOG_ERROR("can't add HW breakpoint to terminate algorithm");
642
643
		return ERROR_TARGET_FAILURE;
	}
oharboe's avatar
oharboe committed
644

645
	if ((retval = target_resume(target, 0, entry_point, 1, 1)) != ERROR_OK)
646
647
648
649
	{
		return retval;
	}
	int retvaltemp;
zwelch's avatar
zwelch committed
650
	retval = run_it(target, exit_point, timeout_ms, arch_info);
oharboe's avatar
oharboe committed
651

652
653
	if (exit_point)
		breakpoint_remove(target, exit_point);
oharboe's avatar
oharboe committed
654

zwelch's avatar
zwelch committed
655
	if (retval != ERROR_OK)
oharboe's avatar
oharboe committed
656
657
		return retval;

658
659
660
	for (i = 0; i < num_mem_params; i++)
	{
		if (mem_params[i].direction != PARAM_OUT)
661
			if ((retvaltemp = target_read_buffer(target, mem_params[i].address, mem_params[i].size, mem_params[i].value)) != ERROR_OK)
662
663
664
			{
					retval = retvaltemp;
			}
665
	}
oharboe's avatar
oharboe committed
666

667
668
669
670
	for (i = 0; i < num_reg_params; i++)
	{
		if (reg_params[i].direction != PARAM_OUT)
		{
oharboe's avatar
oharboe committed
671

672
673
674
			reg_t *reg = register_get_by_name(armv4_5->core_cache, reg_params[i].reg_name, 0);
			if (!reg)
			{
675
				LOG_ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
676
677
				exit(-1);
			}
oharboe's avatar
oharboe committed
678

679
680
			if (reg->size != reg_params[i].size)
			{
681
				LOG_ERROR("BUG: register '%s' size doesn't match reg_params[i].size", reg_params[i].reg_name);
682
683
				exit(-1);
			}
oharboe's avatar
oharboe committed
684

685
686
687
			buf_set_u32(reg_params[i].value, 0, 32, buf_get_u32(reg->value, 0, 32));
		}
	}
oharboe's avatar
oharboe committed
688

689
690
	for (i = 0; i <= 16; i++)
	{
691
		uint32_t regvalue;
692
693
694
		regvalue = buf_get_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_algorithm_info->core_mode, i).value, 0, 32);
		if (regvalue != context[i])
		{
duane's avatar
duane committed
695
			LOG_DEBUG("restoring register %s with value 0x%8.8" PRIx32 "", ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_algorithm_info->core_mode, i).name, context[i]);
696
697
698
699
			buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_algorithm_info->core_mode, i).value, 0, 32, context[i]);
			ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_algorithm_info->core_mode, i).valid = 1;
			ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_algorithm_info->core_mode, i).dirty = 1;
		}
700
701
702
703
	}
	buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32, cpsr);
	armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
	armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 1;
oharboe's avatar
oharboe committed
704

705
706
707
708
709
710
	armv4_5->core_state = core_state;
	armv4_5->core_mode = core_mode;

	return retval;
}

711
int armv4_5_run_algorithm(struct target_s *target, int num_mem_params, struct mem_param *mem_params, int num_reg_params, struct reg_param *reg_params, uint32_t entry_point, uint32_t exit_point, int timeout_ms, void *arch_info)
712
713
714
715
{
	return armv4_5_run_algorithm_inner(target, num_mem_params, mem_params, num_reg_params, reg_params, entry_point, exit_point, timeout_ms, arch_info, armv4_5_run_algorithm_completion);
}

716
int armv4_5_init_arch_info(target_t *target, armv4_5_common_t *armv4_5)
oharboe's avatar
oharboe committed
717
{
718
719
720
721
722
	target->arch_info = armv4_5;

	armv4_5->common_magic = ARMV4_5_COMMON_MAGIC;
	armv4_5->core_state = ARMV4_5_STATE_ARM;
	armv4_5->core_mode = ARMV4_5_MODE_USR;
oharboe's avatar
oharboe committed
723

724
725
	return ERROR_OK;
}