arm11.c 45.6 KB
Newer Older
1
2
/***************************************************************************
 *   Copyright (C) 2008 digenius technology GmbH.                          *
3
 *   Michael Bruck                                                         *
4
 *                                                                         *
5
 *   Copyright (C) 2008 Oyvind Harboe oyvind.harboe@zylin.com              *
6
 *                                                                         *
7
8
 *   Copyright (C) 2008 Georg Acher <acher@in.tum.de>                      *
 *                                                                         *
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
 *   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.             *
 ***************************************************************************/
oharboe's avatar
oharboe committed
24

25
26
27
28
29
30
31
32
33
34
35
36
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include "arm11.h"


#if 0
#define _DEBUG_INSTRUCTION_EXECUTION_
#endif

#if 0
37
#define FNC_INFO	LOG_DEBUG("-")
38
39
40
41
42
#else
#define FNC_INFO
#endif

#if 1
43
#define FNC_INFO_NOTIMPLEMENTED do { LOG_DEBUG("NOT IMPLEMENTED"); /*exit(-1);*/ } while (0)
44
45
46
47
#else
#define FNC_INFO_NOTIMPLEMENTED
#endif

48
static int arm11_on_enter_debug_state(arm11_common_t * arm11);
49

50
51
52
53
54
bool	arm11_config_memwrite_burst				= true;
bool	arm11_config_memwrite_error_fatal		= true;
u32		arm11_vcr								= 0;
bool	arm11_config_memrw_no_increment			= false;
bool	arm11_config_step_irq_enable			= false;
oharboe's avatar
oharboe committed
55

56
#define ARM11_HANDLER(x)	\
57
	.x				= arm11_##x
58
59
60

target_type_t arm11_target =
{
61
	.name			= "arm11",
62

63
64
	ARM11_HANDLER(poll),
	ARM11_HANDLER(arch_state),
65

66
	ARM11_HANDLER(target_request_data),
67

68
69
70
	ARM11_HANDLER(halt),
	ARM11_HANDLER(resume),
	ARM11_HANDLER(step),
71

72
73
74
	ARM11_HANDLER(assert_reset),
	ARM11_HANDLER(deassert_reset),
	ARM11_HANDLER(soft_reset_halt),
75

76
	ARM11_HANDLER(get_gdb_reg_list),
77

78
79
	ARM11_HANDLER(read_memory),
	ARM11_HANDLER(write_memory),
80

81
	ARM11_HANDLER(bulk_write_memory),
82

83
	ARM11_HANDLER(checksum_memory),
84

85
86
87
88
	ARM11_HANDLER(add_breakpoint),
	ARM11_HANDLER(remove_breakpoint),
	ARM11_HANDLER(add_watchpoint),
	ARM11_HANDLER(remove_watchpoint),
89

90
	ARM11_HANDLER(run_algorithm),
91

92
93
94
95
96
	ARM11_HANDLER(register_commands),
	ARM11_HANDLER(target_create),
	ARM11_HANDLER(init_target),
	ARM11_HANDLER(examine),
	ARM11_HANDLER(quit),
97
98
99
100
101
102
103
};

int arm11_regs_arch_type = -1;


enum arm11_regtype
{
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
	ARM11_REGISTER_CORE,
	ARM11_REGISTER_CPSR,

	ARM11_REGISTER_FX,
	ARM11_REGISTER_FPS,

	ARM11_REGISTER_FIQ,
	ARM11_REGISTER_SVC,
	ARM11_REGISTER_ABT,
	ARM11_REGISTER_IRQ,
	ARM11_REGISTER_UND,
	ARM11_REGISTER_MON,

	ARM11_REGISTER_SPSR_FIQ,
	ARM11_REGISTER_SPSR_SVC,
	ARM11_REGISTER_SPSR_ABT,
	ARM11_REGISTER_SPSR_IRQ,
	ARM11_REGISTER_SPSR_UND,
	ARM11_REGISTER_SPSR_MON,

	/* debug regs */
	ARM11_REGISTER_DSCR,
	ARM11_REGISTER_WDTR,
	ARM11_REGISTER_RDTR,
128
129
130
131
132
};


typedef struct arm11_reg_defs_s
{
133
134
135
	char *					name;
	u32						num;
	int						gdb_num;
136
	enum arm11_regtype		type;
137
138
139
140
141
} arm11_reg_defs_t;

/* update arm11_regcache_ids when changing this */
static const arm11_reg_defs_t arm11_reg_defs[] =
{
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
	{"r0",	0,	0,	ARM11_REGISTER_CORE},
	{"r1",	1,	1,	ARM11_REGISTER_CORE},
	{"r2",	2,	2,	ARM11_REGISTER_CORE},
	{"r3",	3,	3,	ARM11_REGISTER_CORE},
	{"r4",	4,	4,	ARM11_REGISTER_CORE},
	{"r5",	5,	5,	ARM11_REGISTER_CORE},
	{"r6",	6,	6,	ARM11_REGISTER_CORE},
	{"r7",	7,	7,	ARM11_REGISTER_CORE},
	{"r8",	8,	8,	ARM11_REGISTER_CORE},
	{"r9",	9,	9,	ARM11_REGISTER_CORE},
	{"r10",	10,	10,	ARM11_REGISTER_CORE},
	{"r11",	11,	11,	ARM11_REGISTER_CORE},
	{"r12",	12,	12,	ARM11_REGISTER_CORE},
	{"sp",	13,	13,	ARM11_REGISTER_CORE},
	{"lr",	14,	14,	ARM11_REGISTER_CORE},
	{"pc",	15,	15,	ARM11_REGISTER_CORE},
158
159

#if ARM11_REGCACHE_FREGS
160
161
162
163
164
165
166
167
168
	{"f0",	0,	16,	ARM11_REGISTER_FX},
	{"f1",	1,	17,	ARM11_REGISTER_FX},
	{"f2",	2,	18,	ARM11_REGISTER_FX},
	{"f3",	3,	19,	ARM11_REGISTER_FX},
	{"f4",	4,	20,	ARM11_REGISTER_FX},
	{"f5",	5,	21,	ARM11_REGISTER_FX},
	{"f6",	6,	22,	ARM11_REGISTER_FX},
	{"f7",	7,	23,	ARM11_REGISTER_FX},
	{"fps",	0,	24,	ARM11_REGISTER_FPS},
169
170
#endif

171
	{"cpsr",	0,	25,	ARM11_REGISTER_CPSR},
172
173

#if ARM11_REGCACHE_MODEREGS
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
	{"r8_fiq",	8,	-1,	ARM11_REGISTER_FIQ},
	{"r9_fiq",	9,	-1,	ARM11_REGISTER_FIQ},
	{"r10_fiq",	10,	-1,	ARM11_REGISTER_FIQ},
	{"r11_fiq",	11,	-1,	ARM11_REGISTER_FIQ},
	{"r12_fiq",	12,	-1,	ARM11_REGISTER_FIQ},
	{"r13_fiq",	13,	-1,	ARM11_REGISTER_FIQ},
	{"r14_fiq",	14,	-1,	ARM11_REGISTER_FIQ},
	{"spsr_fiq", 0,	-1,	ARM11_REGISTER_SPSR_FIQ},

	{"r13_svc",	13,	-1,	ARM11_REGISTER_SVC},
	{"r14_svc",	14,	-1,	ARM11_REGISTER_SVC},
	{"spsr_svc", 0,	-1,	ARM11_REGISTER_SPSR_SVC},

	{"r13_abt",	13,	-1,	ARM11_REGISTER_ABT},
	{"r14_abt",	14,	-1,	ARM11_REGISTER_ABT},
	{"spsr_abt", 0,	-1,	ARM11_REGISTER_SPSR_ABT},

	{"r13_irq",	13,	-1,	ARM11_REGISTER_IRQ},
	{"r14_irq",	14,	-1,	ARM11_REGISTER_IRQ},
	{"spsr_irq", 0,	-1,	ARM11_REGISTER_SPSR_IRQ},

	{"r13_und",	13,	-1,	ARM11_REGISTER_UND},
	{"r14_und",	14,	-1,	ARM11_REGISTER_UND},
	{"spsr_und", 0,	-1,	ARM11_REGISTER_SPSR_UND},

	/* ARM1176 only */
	{"r13_mon",	13,	-1,	ARM11_REGISTER_MON},
	{"r14_mon",	14,	-1,	ARM11_REGISTER_MON},
	{"spsr_mon", 0,	-1,	ARM11_REGISTER_SPSR_MON},
203
204
#endif

205
206
207
208
	/* Debug Registers */
	{"dscr",	0,	-1,	ARM11_REGISTER_DSCR},
	{"wdtr",	0,	-1,	ARM11_REGISTER_WDTR},
	{"rdtr",	0,	-1,	ARM11_REGISTER_RDTR},
209
210
211
212
};

enum arm11_regcache_ids
{
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
	ARM11_RC_R0,
	ARM11_RC_RX			= ARM11_RC_R0,

	ARM11_RC_R1,
	ARM11_RC_R2,
	ARM11_RC_R3,
	ARM11_RC_R4,
	ARM11_RC_R5,
	ARM11_RC_R6,
	ARM11_RC_R7,
	ARM11_RC_R8,
	ARM11_RC_R9,
	ARM11_RC_R10,
	ARM11_RC_R11,
	ARM11_RC_R12,
	ARM11_RC_R13,
	ARM11_RC_SP			= ARM11_RC_R13,
	ARM11_RC_R14,
	ARM11_RC_LR			= ARM11_RC_R14,
	ARM11_RC_R15,
	ARM11_RC_PC			= ARM11_RC_R15,
234
235

#if ARM11_REGCACHE_FREGS
236
237
238
239
240
241
242
243
244
245
	ARM11_RC_F0,
	ARM11_RC_FX			= ARM11_RC_F0,
	ARM11_RC_F1,
	ARM11_RC_F2,
	ARM11_RC_F3,
	ARM11_RC_F4,
	ARM11_RC_F5,
	ARM11_RC_F6,
	ARM11_RC_F7,
	ARM11_RC_FPS,
246
247
#endif

248
	ARM11_RC_CPSR,
249
250

#if ARM11_REGCACHE_MODEREGS
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
	ARM11_RC_R8_FIQ,
	ARM11_RC_R9_FIQ,
	ARM11_RC_R10_FIQ,
	ARM11_RC_R11_FIQ,
	ARM11_RC_R12_FIQ,
	ARM11_RC_R13_FIQ,
	ARM11_RC_R14_FIQ,
	ARM11_RC_SPSR_FIQ,

	ARM11_RC_R13_SVC,
	ARM11_RC_R14_SVC,
	ARM11_RC_SPSR_SVC,

	ARM11_RC_R13_ABT,
	ARM11_RC_R14_ABT,
	ARM11_RC_SPSR_ABT,

	ARM11_RC_R13_IRQ,
	ARM11_RC_R14_IRQ,
	ARM11_RC_SPSR_IRQ,

	ARM11_RC_R13_UND,
	ARM11_RC_R14_UND,
	ARM11_RC_SPSR_UND,

	ARM11_RC_R13_MON,
	ARM11_RC_R14_MON,
	ARM11_RC_SPSR_MON,
279
280
#endif

281
282
283
	ARM11_RC_DSCR,
	ARM11_RC_WDTR,
	ARM11_RC_RDTR,
284

285
	ARM11_RC_MAX,
286
287
288
289
290
291
292
293
};

#define ARM11_GDB_REGISTER_COUNT	26

u8 arm11_gdb_dummy_fp_value[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

reg_t arm11_gdb_dummy_fp_reg =
{
294
	"GDB dummy floating-point register", arm11_gdb_dummy_fp_value, 0, 1, 96, NULL, 0, NULL, 0
295
296
297
298
299
300
};

u8 arm11_gdb_dummy_fps_value[] = {0, 0, 0, 0};

reg_t arm11_gdb_dummy_fps_reg =
{
301
	"GDB dummy floating-point status register", arm11_gdb_dummy_fps_value, 0, 1, 32, NULL, 0, NULL, 0
302
303
304
305
306
307
308
309
};



/** Check and if necessary take control of the system
 *
 * \param arm11		Target state variable.
 * \param dscr		If the current DSCR content is
310
311
 *					available a pointer to a word holding the
 *					DSCR can be passed. Otherwise use NULL.
312
 */
313
int arm11_check_init(arm11_common_t * arm11, u32 * dscr)
314
{
315
	FNC_INFO;
316

317
	u32			dscr_local_tmp_copy;
318

319
320
	if (!dscr)
	{
321
		dscr = &dscr_local_tmp_copy;
322

323
		CHECK_RETVAL(arm11_read_DSCR(arm11, dscr));
324
	}
325

326
327
	if (!(*dscr & ARM11_DSCR_MODE_SELECT))
	{
328
		LOG_DEBUG("Bringing target into debug mode");
329

330
331
		*dscr |= ARM11_DSCR_MODE_SELECT;		/* Halt debug-mode */
		arm11_write_DSCR(arm11, *dscr);
332

333
		/* add further reset initialization here */
334

335
		arm11->simulate_reset_on_next_halt = true;
oharboe's avatar
oharboe committed
336

337
338
339
		if (*dscr & ARM11_DSCR_CORE_HALTED)
		{
			/** \todo TODO: this needs further scrutiny because
340
341
342
			  * arm11_on_enter_debug_state() never gets properly called.
			  * As a result we don't read the actual register states from
			  * the target.
343
			  */
oharboe's avatar
oharboe committed
344

345
346
347
348
349
350
351
352
			arm11->target->state	= TARGET_HALTED;
			arm11->target->debug_reason	= arm11_get_DSCR_debug_reason(*dscr);
		}
		else
		{
			arm11->target->state	= TARGET_RUNNING;
			arm11->target->debug_reason	= DBG_REASON_NOTHALTED;
		}
353

354
		arm11_sc7_clear_vbw(arm11);
355
	}
356
357

	return ERROR_OK;
358
359
360
361
362
}



#define R(x) \
363
	(arm11->reg_values[ARM11_RC_##x])
364
365
366
367
368
369
370

/** Save processor state.
  *
  * This is called when the HALT instruction has succeeded
  * or on other occasions that stop the processor.
  *
  */
371
static int arm11_on_enter_debug_state(arm11_common_t * arm11)
372
{
373
	FNC_INFO;
374

375
	for (size_t i = 0; i < asizeof(arm11->reg_values); i++)
376
	{
377
378
		arm11->reg_list[i].valid	= 1;
		arm11->reg_list[i].dirty	= 0;
379
	}
380

381
	/* Save DSCR */
382
	CHECK_RETVAL(arm11_read_DSCR(arm11, &R(DSCR)));
383

384
	/* Save wDTR */
385

386
387
	if (R(DSCR) & ARM11_DSCR_WDTR_FULL)
	{
388
		arm11_add_debug_SCAN_N(arm11, 0x05, ARM11_TAP_DEFAULT);
389

390
		arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
391

392
		scan_field_t	chain5_fields[3];
393

394
		arm11_setup_field(arm11, 32, NULL, &R(WDTR),	chain5_fields + 0);
395
396
		arm11_setup_field(arm11,  1, NULL, NULL,		chain5_fields + 1);
		arm11_setup_field(arm11,  1, NULL, NULL,		chain5_fields + 2);
397

398
		arm11_add_dr_scan_vc(asizeof(chain5_fields), chain5_fields, TAP_DRPAUSE);
399
400
401
	}
	else
	{
402
		arm11->reg_list[ARM11_RC_WDTR].valid	= 0;
403
	}
404
405


406
407
408
	/* DSCR: set ARM11_DSCR_EXECUTE_ARM_INSTRUCTION_ENABLE */
	/* ARM1176 spec says this is needed only for wDTR/rDTR's "ITR mode", but not to issue ITRs
	   ARM1136 seems to require this to issue ITR's as well */
409

410
	u32 new_dscr = R(DSCR) | ARM11_DSCR_EXECUTE_ARM_INSTRUCTION_ENABLE;
411

412
	/* this executes JTAG queue: */
413

414
	arm11_write_DSCR(arm11, new_dscr);
415
416


417
	/* From the spec:
418
419
	   Before executing any instruction in debug state you have to drain the write buffer.
	   This ensures that no imprecise Data Aborts can return at a later point:*/
420

421
	/** \todo TODO: Test drain write buffer. */
422
423

#if 0
424
425
	while (1)
	{
426
427
		/* MRC p14,0,R0,c5,c10,0 */
		//	arm11_run_instr_no_data1(arm11, /*0xee150e1a*/0xe320f000);
428

429
430
		/* mcr	   15, 0, r0, cr7, cr10, {4} */
		arm11_run_instr_no_data1(arm11, 0xee070f9a);
431

432
		u32 dscr = arm11_read_DSCR(arm11);
433

434
		LOG_DEBUG("DRAIN, DSCR %08x", dscr);
435

436
437
438
		if (dscr & ARM11_DSCR_STICKY_IMPRECISE_DATA_ABORT)
		{
			arm11_run_instr_no_data1(arm11, 0xe320f000);
439

440
			dscr = arm11_read_DSCR(arm11);
441

442
			LOG_DEBUG("DRAIN, DSCR %08x (DONE)", dscr);
443

444
445
			break;
		}
446
447
448
	}
#endif

449
	arm11_run_instr_data_prepare(arm11);
450

451
	/* save r0 - r14 */
452

453
	/** \todo TODO: handle other mode registers */
454

455
	for (size_t i = 0; i < 15; i++)
456
	{
457
458
		/* MCR p14,0,R?,c0,c5,0 */
		arm11_run_instr_data_from_core(arm11, 0xEE000E15 | (i << 12), &R(RX + i), 1);
459
	}
460

461
	/* save rDTR */
462

463
	/* check rDTRfull in DSCR */
464

465
466
	if (R(DSCR) & ARM11_DSCR_RDTR_FULL)
	{
467
468
		/* MRC p14,0,R0,c0,c5,0 (move rDTR -> r0 (-> wDTR -> local var)) */
		arm11_run_instr_data_from_core_via_r0(arm11, 0xEE100E15, &R(RDTR));
469
470
471
	}
	else
	{
472
		arm11->reg_list[ARM11_RC_RDTR].valid	= 0;
473
	}
474

475
	/* save CPSR */
476

477
478
	/* MRS r0,CPSR (move CPSR -> r0 (-> wDTR -> local var)) */
	arm11_run_instr_data_from_core_via_r0(arm11, 0xE10F0000, &R(CPSR));
479

480
	/* save PC */
481

482
483
	/* MOV R0,PC (move PC -> r0 (-> wDTR -> local var)) */
	arm11_run_instr_data_from_core_via_r0(arm11, 0xE1A0000F, &R(PC));
484

485
	/* adjust PC depending on ARM state */
486

487
488
	if (R(CPSR) & ARM11_CPSR_J)	/* Java state */
	{
489
		arm11->reg_values[ARM11_RC_PC] -= 0;
490
491
492
	}
	else if (R(CPSR) & ARM11_CPSR_T)	/* Thumb state */
	{
493
		arm11->reg_values[ARM11_RC_PC] -= 4;
494
495
496
	}
	else					/* ARM state */
	{
497
		arm11->reg_values[ARM11_RC_PC] -= 8;
498
	}
499

500
501
	if (arm11->simulate_reset_on_next_halt)
	{
502
		arm11->simulate_reset_on_next_halt = false;
oharboe's avatar
oharboe committed
503

504
		LOG_DEBUG("Reset c1 Control Register");
oharboe's avatar
oharboe committed
505

506
		/* Write 0 (reset value) to Control register 0 to disable MMU/Cache etc. */
oharboe's avatar
oharboe committed
507

508
509
		/* MCR p15,0,R0,c1,c0,0 */
		arm11_run_instr_data_to_core_via_r0(arm11, 0xee010f10, 0);
oharboe's avatar
oharboe committed
510

511
	}
oharboe's avatar
oharboe committed
512

513
	arm11_run_instr_data_finish(arm11);
514

515
	arm11_dump_reg_changes(arm11);
516
517

	return ERROR_OK;
oharboe's avatar
oharboe committed
518
}
519

oharboe's avatar
oharboe committed
520
521
void arm11_dump_reg_changes(arm11_common_t * arm11)
{
522
523
524
525
526
527

	if (!(debug_level >= LOG_LVL_DEBUG))
	{
		return;
	}

528
	for (size_t i = 0; i < ARM11_REGCACHE_COUNT; i++)
529
	{
530
		if (!arm11->reg_list[i].valid)
531
		{
532
			if (arm11->reg_history[i].valid)
533
				LOG_DEBUG("%8s INVALID	 (%08x)", arm11_reg_defs[i].name, arm11->reg_history[i].value);
534
535
536
		}
		else
		{
537
538
539
			if (arm11->reg_history[i].valid)
			{
				if (arm11->reg_history[i].value != arm11->reg_values[i])
540
					LOG_DEBUG("%8s %08x (%08x)", arm11_reg_defs[i].name, arm11->reg_values[i], arm11->reg_history[i].value);
541
542
543
			}
			else
			{
544
				LOG_DEBUG("%8s %08x (INVALID)", arm11_reg_defs[i].name, arm11->reg_values[i]);
545
			}
546
		}
547
	}
548
549
550
551
552
553
554
}

/** Restore processor state
  *
  * This is called in preparation for the RESTART function.
  *
  */
555
int arm11_leave_debug_state(arm11_common_t * arm11)
556
{
557
	FNC_INFO;
558

559
	arm11_run_instr_data_prepare(arm11);
560

561
	/** \todo TODO: handle other mode registers */
562

563
	/* restore R1 - R14 */
564
565

	for (size_t i = 1; i < 15; i++)
566
	{
567
568
		if (!arm11->reg_list[ARM11_RC_RX + i].dirty)
			continue;
569

570
571
		/* MRC p14,0,r?,c0,c5,0 */
		arm11_run_instr_data_to_core1(arm11, 0xee100e15 | (i << 12), R(RX + i));
572

573
		//	LOG_DEBUG("RESTORE R" ZU " %08x", i, R(RX + i));
574
	}
575

576
	arm11_run_instr_data_finish(arm11);
577

578
579
580
	/* spec says clear wDTR and rDTR; we assume they are clear as
	   otherwise our programming would be sloppy */
	{
581
		u32 DSCR;
582
583

		CHECK_RETVAL(arm11_read_DSCR(arm11, &DSCR));
584

585
586
587
588
		if (DSCR & (ARM11_DSCR_RDTR_FULL | ARM11_DSCR_WDTR_FULL))
		{
			LOG_ERROR("wDTR/rDTR inconsistent (DSCR %08x)", DSCR);
		}
589
590
	}

591
	arm11_run_instr_data_prepare(arm11);
592

593
	/* restore original wDTR */
594

595
596
	if ((R(DSCR) & ARM11_DSCR_WDTR_FULL) || arm11->reg_list[ARM11_RC_WDTR].dirty)
	{
597
598
		/* MCR p14,0,R0,c0,c5,0 */
		arm11_run_instr_data_to_core_via_r0(arm11, 0xee000e15, R(WDTR));
599
	}
600

601
	/* restore CPSR */
602

603
604
	/* MSR CPSR,R0*/
	arm11_run_instr_data_to_core_via_r0(arm11, 0xe129f000, R(CPSR));
605

606
	/* restore PC */
607

608
609
	/* MOV PC,R0 */
	arm11_run_instr_data_to_core_via_r0(arm11, 0xe1a0f000, R(PC));
610

611
	/* restore R0 */
612

613
614
	/* MRC p14,0,r0,c0,c5,0 */
	arm11_run_instr_data_to_core1(arm11, 0xee100e15, R(R0));
615

616
	arm11_run_instr_data_finish(arm11);
617

618
	/* restore DSCR */
619

620
	arm11_write_DSCR(arm11, R(DSCR));
621

622
	/* restore rDTR */
623

624
625
	if (R(DSCR) & ARM11_DSCR_RDTR_FULL || arm11->reg_list[ARM11_RC_RDTR].dirty)
	{
626
		arm11_add_debug_SCAN_N(arm11, 0x05, ARM11_TAP_DEFAULT);
627

628
		arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
629

630
		scan_field_t	chain5_fields[3];
631

632
633
		u8			Ready		= 0;	/* ignored */
		u8			Valid		= 0;	/* ignored */
634

635
636
637
		arm11_setup_field(arm11, 32, &R(RDTR),	NULL, chain5_fields + 0);
		arm11_setup_field(arm11,  1, &Ready,	NULL, chain5_fields + 1);
		arm11_setup_field(arm11,  1, &Valid,	NULL, chain5_fields + 2);
638

639
		arm11_add_dr_scan_vc(asizeof(chain5_fields), chain5_fields, TAP_DRPAUSE);
640
	}
641

642
	arm11_record_register_history(arm11);
643
644

	return ERROR_OK;
oharboe's avatar
oharboe committed
645
}
646

oharboe's avatar
oharboe committed
647
648
void arm11_record_register_history(arm11_common_t * arm11)
{
649
	for (size_t i = 0; i < ARM11_REGCACHE_COUNT; i++)
650
	{
651
652
		arm11->reg_history[i].value	= arm11->reg_values[i];
		arm11->reg_history[i].valid	= arm11->reg_list[i].valid;
653

654
655
		arm11->reg_list[i].valid	= 0;
		arm11->reg_list[i].dirty	= 0;
656
	}
657
658
659
660
661
662
}


/* poll current target status */
int arm11_poll(struct target_s *target)
{
663
	FNC_INFO;
664

665
	arm11_common_t * arm11 = target->arch_info;
666

667
	if (arm11->trst_active)
668
		return ERROR_OK;
669

670
	u32	dscr;
671

672
	CHECK_RETVAL(arm11_read_DSCR(arm11, &dscr));
673

674
	LOG_DEBUG("DSCR %08x", dscr);
675

676
	CHECK_RETVAL(arm11_check_init(arm11, &dscr));
677

678
679
	if (dscr & ARM11_DSCR_CORE_HALTED)
	{
680
681
682
		if (target->state != TARGET_HALTED)
		{
			enum target_state old_state = target->state;
oharboe's avatar
oharboe committed
683

684
			LOG_DEBUG("enter TARGET_HALTED");
685
			target->state			= TARGET_HALTED;
686
687
			target->debug_reason	= arm11_get_DSCR_debug_reason(dscr);
			arm11_on_enter_debug_state(arm11);
oharboe's avatar
oharboe committed
688

689
690
691
			target_call_event_callbacks(target,
				old_state == TARGET_DEBUG_RUNNING ? TARGET_EVENT_DEBUG_HALTED : TARGET_EVENT_HALTED);
		}
692
693
694
	}
	else
	{
695
696
697
		if (target->state != TARGET_RUNNING && target->state != TARGET_DEBUG_RUNNING)
		{
			LOG_DEBUG("enter TARGET_RUNNING");
698
			target->state			= TARGET_RUNNING;
699
700
			target->debug_reason	= DBG_REASON_NOTHALTED;
		}
701
702
	}

703
	return ERROR_OK;
704
705
706
707
}
/* architecture specific status reply */
int arm11_arch_state(struct target_s *target)
{
708
709
710
711
712
713
	arm11_common_t * arm11 = target->arch_info;

	LOG_USER("target halted due to %s\ncpsr: 0x%8.8x pc: 0x%8.8x",
			 Jim_Nvp_value2name_simple( nvp_target_debug_reason, target->debug_reason )->name,
			 R(CPSR),
			 R(PC));
714

715
	return ERROR_OK;
716
717
718
719
720
}

/* target request support */
int arm11_target_request_data(struct target_s *target, u32 size, u8 *buffer)
{
721
	FNC_INFO_NOTIMPLEMENTED;
722

723
	return ERROR_OK;
724
725
726
727
728
}

/* target execution control */
int arm11_halt(struct target_s *target)
{
729
	FNC_INFO;
730

731
	arm11_common_t * arm11 = target->arch_info;
732

733
	LOG_DEBUG("target->state: %s",
734
		Jim_Nvp_value2name_simple( nvp_target_state, target->state )->name );
735

736
737
	if (target->state == TARGET_UNKNOWN)
	{
738
		arm11->simulate_reset_on_next_halt = true;
739
	}
oharboe's avatar
oharboe committed
740

741
742
	if (target->state == TARGET_HALTED)
	{
743
744
		LOG_DEBUG("target was already halted");
		return ERROR_OK;
745
	}
746

747
748
	if (arm11->trst_active)
	{
749
750
		arm11->halt_requested = true;
		return ERROR_OK;
751
	}
752

753
	arm11_add_IR(arm11, ARM11_HALT, TAP_IDLE);
754

755
	CHECK_RETVAL(jtag_execute_queue());
756

757
	u32 dscr;
758

759
760
	while (1)
	{
761
		CHECK_RETVAL(arm11_read_DSCR(arm11, &dscr));
762

763
764
		if (dscr & ARM11_DSCR_CORE_HALTED)
			break;
765
	}
766

767
	arm11_on_enter_debug_state(arm11);
768

769
	enum target_state old_state	= target->state;
oharboe's avatar
oharboe committed
770

771
772
	target->state		= TARGET_HALTED;
	target->debug_reason	= arm11_get_DSCR_debug_reason(dscr);
oharboe's avatar
oharboe committed
773

774
775
776
	CHECK_RETVAL(
		target_call_event_callbacks(target,
			old_state == TARGET_DEBUG_RUNNING ? TARGET_EVENT_DEBUG_HALTED : TARGET_EVENT_HALTED));
oharboe's avatar
oharboe committed
777

778
	return ERROR_OK;
779
780
781
782
}

int arm11_resume(struct target_s *target, int current, u32 address, int handle_breakpoints, int debug_execution)
{
783
	FNC_INFO;
784

785
786
	//	  LOG_DEBUG("current %d  address %08x  handle_breakpoints %d  debug_execution %d",
	//	current, address, handle_breakpoints, debug_execution);
oharboe's avatar
oharboe committed
787

788
	arm11_common_t * arm11 = target->arch_info;
789

790
	LOG_DEBUG("target->state: %s",
791
		Jim_Nvp_value2name_simple( nvp_target_state, target->state )->name );
792

793

794
	if (target->state != TARGET_HALTED)
oharboe's avatar
oharboe committed
795
796
797
798
	{
		LOG_ERROR("Target not halted");
		return ERROR_TARGET_NOT_HALTED;
	}
799

800
	if (!current)
801
		R(PC) = address;
802

803
	LOG_DEBUG("RESUME PC %08x%s", R(PC), !current ? "!" : "");
oharboe's avatar
oharboe committed
804

805
806
	/* clear breakpoints/watchpoints and VCR*/
	arm11_sc7_clear_vbw(arm11);
oharboe's avatar
oharboe committed
807

808
809
810
	/* Set up breakpoints */
	if (!debug_execution)
	{
811
		/* check if one matches PC and step over it if necessary */
oharboe's avatar
oharboe committed
812

813
		breakpoint_t *	bp;
oharboe's avatar
oharboe committed
814

815
		for (bp = target->breakpoints; bp; bp = bp->next)
816
		{
817
818
819
820
821
822
			if (bp->address == R(PC))
			{
				LOG_DEBUG("must step over %08x", bp->address);
				arm11_step(target, 1, 0, 0);
				break;
			}
823
		}
oharboe's avatar
oharboe committed
824

825
		/* set all breakpoints */
oharboe's avatar
oharboe committed
826

827
		size_t		brp_num = 0;
828

829
830
831
		for (bp = target->breakpoints; bp; bp = bp->next)
		{
			arm11_sc7_action_t	brp[2];
oharboe's avatar
oharboe committed
832

833
834
835
836
837
838
			brp[0].write	= 1;
			brp[0].address	= ARM11_SC7_BVR0 + brp_num;
			brp[0].value	= bp->address;
			brp[1].write	= 1;
			brp[1].address	= ARM11_SC7_BCR0 + brp_num;
			brp[1].value	= 0x1 | (3 << 1) | (0x0F << 5) | (0 << 14) | (0 << 16) | (0 << 20) | (0 << 21);
839

840
			arm11_sc7_run(arm11, brp, asizeof(brp));
oharboe's avatar
oharboe committed
841

842
			LOG_DEBUG("Add BP " ZU " at %08x", brp_num, bp->address);
oharboe's avatar
oharboe committed
843

844
845
			brp_num++;
		}
oharboe's avatar
oharboe committed
846

847
		arm11_sc7_set_vcr(arm11, arm11_vcr);
848
	}
849

850
	arm11_leave_debug_state(arm11);
851

852
	arm11_add_IR(arm11, ARM11_RESTART, TAP_IDLE);
853

854
	CHECK_RETVAL(jtag_execute_queue());
855

856
857
	while (1)
	{
858
		u32 dscr;
859

860
		CHECK_RETVAL(arm11_read_DSCR(arm11, &dscr));
861

862
		LOG_DEBUG("DSCR %08x", dscr);
863

864
865
		if (dscr & ARM11_DSCR_CORE_RESTARTED)
			break;
866
	}
867