arm11.c 41 KB
Newer Older
1
2
/***************************************************************************
 *   Copyright (C) 2008 digenius technology GmbH.                          *
3
 *                                                                         *
4
 *   Copyright (C) 2008 Oyvind Harboe oyvind.harboe@zylin.com              *
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
 *                                                                         *
 *   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
21

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

#include "arm11.h"
#include "jtag.h"
#include "log.h"

#include <stdlib.h>
#include <string.h>

#if 0
#define _DEBUG_INSTRUCTION_EXECUTION_
#endif


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

#if 1
45
#define FNC_INFO_NOTIMPLEMENTED    do { LOG_DEBUG("NOT IMPLEMENTED"); /*exit(-1);*/ } while (0)
46
47
48
49
50
51
52
#else
#define FNC_INFO_NOTIMPLEMENTED
#endif

static void arm11_on_enter_debug_state(arm11_common_t * arm11);


oharboe's avatar
oharboe committed
53
54
bool	arm11_config_memwrite_burst		= true;
bool	arm11_config_memwrite_error_fatal	= true;
oharboe's avatar
oharboe committed
55
56
57
u32	arm11_vcr				= 0;


58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
#define ARM11_HANDLER(x)	\
    .x				= arm11_##x

target_type_t arm11_target =
{
    .name			= "arm11",

    ARM11_HANDLER(poll),
    ARM11_HANDLER(arch_state),

    ARM11_HANDLER(target_request_data),

    ARM11_HANDLER(halt),
    ARM11_HANDLER(resume),
    ARM11_HANDLER(step),

    ARM11_HANDLER(assert_reset),
    ARM11_HANDLER(deassert_reset),
    ARM11_HANDLER(soft_reset_halt),
	
    ARM11_HANDLER(get_gdb_reg_list),
	
    ARM11_HANDLER(read_memory),
    ARM11_HANDLER(write_memory),
	
    ARM11_HANDLER(bulk_write_memory),
	
    ARM11_HANDLER(checksum_memory),

    ARM11_HANDLER(add_breakpoint),
    ARM11_HANDLER(remove_breakpoint),
    ARM11_HANDLER(add_watchpoint),
    ARM11_HANDLER(remove_watchpoint),

    ARM11_HANDLER(run_algorithm),
	
    ARM11_HANDLER(register_commands),
95
    ARM11_HANDLER(target_create),
96
    ARM11_HANDLER(init_target),
97
    ARM11_HANDLER(examine),
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
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
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
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
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
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
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
    ARM11_HANDLER(quit),
};

int arm11_regs_arch_type = -1;


enum arm11_regtype
{
    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,
};


typedef struct arm11_reg_defs_s
{
    char *			name;
    u32				num;
    int				gdb_num;
    enum arm11_regtype		type;
} arm11_reg_defs_t;

/* update arm11_regcache_ids when changing this */
static const arm11_reg_defs_t arm11_reg_defs[] =
{
    {"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},

#if ARM11_REGCACHE_FREGS
    {"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},
#endif

    {"cpsr",	0,	25,	ARM11_REGISTER_CPSR},

#if ARM11_REGCACHE_MODEREGS
    {"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},
#endif

    /* Debug Registers */
    {"dscr",	0,	-1,	ARM11_REGISTER_DSCR},
    {"wdtr",	0,	-1,	ARM11_REGISTER_WDTR},
    {"rdtr",	0,	-1,	ARM11_REGISTER_RDTR},
};

enum arm11_regcache_ids
{
    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,

#if ARM11_REGCACHE_FREGS
    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,
#endif

    ARM11_RC_CPSR,

#if ARM11_REGCACHE_MODEREGS
    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,
#endif

    ARM11_RC_DSCR,
    ARM11_RC_WDTR,
    ARM11_RC_RDTR,

    ARM11_RC_MAX,
};

#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 =
{
    "GDB dummy floating-point register", arm11_gdb_dummy_fp_value, 0, 1, 96, NULL, 0, NULL, 0
};

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

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



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

    u32			dscr_local_tmp_copy;

    if (!dscr)
    {
	dscr = &dscr_local_tmp_copy;
	*dscr = arm11_read_DSCR(arm11);
    }

    if (!(*dscr & ARM11_DSCR_MODE_SELECT))
    {
329
	LOG_DEBUG("Bringing target into debug mode");
330
331
332
333
334
335

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

	/* add further reset initialization here */

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

338
339
	if (*dscr & ARM11_DSCR_CORE_HALTED)
	{
oharboe's avatar
oharboe committed
340
341
342
343
	    /** \todo TODO: this needs further scrutiny because
	      * arm11_on_enter_debug_state() never gets properly called
	      */

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;
	}

oharboe's avatar
oharboe committed
353
	arm11_sc7_clear_vbw(arm11);
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
    }
}



#define R(x) \
    (arm11->reg_values[ARM11_RC_##x])

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

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

    /* Save DSCR */

    R(DSCR) = arm11_read_DSCR(arm11);

    /* Save wDTR */

    if (R(DSCR) & ARM11_DSCR_WDTR_FULL)
    {
	arm11_add_debug_SCAN_N(arm11, 0x05, -1);

	arm11_add_IR(arm11, ARM11_INTEST, -1);

	scan_field_t	chain5_fields[3];

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

oharboe's avatar
oharboe committed
397
	arm11_add_dr_scan_vc(asizeof(chain5_fields), chain5_fields, TAP_PD);
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
    }
    else
    {
	arm11->reg_list[ARM11_RC_WDTR].valid	= 0;
    }


    /* 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 */

    u32 new_dscr = R(DSCR) | ARM11_DSCR_EXECUTE_ARM_INSTRUCTION_ENABLE;

    /* this executes JTAG queue: */ 

    arm11_write_DSCR(arm11, new_dscr);


    /* From the spec:
	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:*/

    /** \todo TODO: Test drain write buffer. */

#if 0
    while (1)
    {
	/* MRC p14,0,R0,c5,c10,0 */
oharboe's avatar
oharboe committed
426
//	arm11_run_instr_no_data1(arm11, /*0xee150e1a*/0xe320f000);
427
428
429
430
431
432

	/* mcr     15, 0, r0, cr7, cr10, {4} */
	arm11_run_instr_no_data1(arm11, 0xee070f9a);
	        
	u32 dscr = arm11_read_DSCR(arm11);

433
	LOG_DEBUG("DRAIN, DSCR %08x", dscr);
434
435
436
437
438
439
440

	if (dscr & ARM11_DSCR_STICKY_IMPRECISE_DATA_ABORT)
	{
	    arm11_run_instr_no_data1(arm11, 0xe320f000);

	    dscr = arm11_read_DSCR(arm11);

441
	    LOG_DEBUG("DRAIN, DSCR %08x (DONE)", dscr);
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502

	    break;
	}
    }
#endif


    arm11_run_instr_data_prepare(arm11);

    /* save r0 - r14 */


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

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


    /* save rDTR */

    /* check rDTRfull in DSCR */

    if (R(DSCR) & ARM11_DSCR_RDTR_FULL)
    {
	/* 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));
    }
    else
    {
	arm11->reg_list[ARM11_RC_RDTR].valid	= 0;
    }

    /* save CPSR */

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

    /* save PC */

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

    /* adjust PC depending on ARM state */

    if (R(CPSR) & ARM11_CPSR_J)	/* Java state */
    {
	arm11->reg_values[ARM11_RC_PC] -= 0;
    }
    else if (R(CPSR) & ARM11_CPSR_T)	/* Thumb state */
    {
	arm11->reg_values[ARM11_RC_PC] -= 4;
    }
    else					/* ARM state */
    {
	arm11->reg_values[ARM11_RC_PC] -= 8;
    }

oharboe's avatar
oharboe committed
503
504
505
506
    if (arm11->simulate_reset_on_next_halt)
    {
	arm11->simulate_reset_on_next_halt = false;

507
	LOG_DEBUG("Reset c1 Control Register");
oharboe's avatar
oharboe committed
508
509
510
511
512
513
514
515

	/* Write 0 (reset value) to Control register 0 to disable MMU/Cache etc. */

	/* MCR p15,0,R0,c1,c0,0 */
	arm11_run_instr_data_to_core_via_r0(arm11, 0xee010f10, 0);

    }

516
517
    arm11_run_instr_data_finish(arm11);

oharboe's avatar
oharboe committed
518
519
    arm11_dump_reg_changes(arm11);
}
520

oharboe's avatar
oharboe committed
521
522
void arm11_dump_reg_changes(arm11_common_t * arm11)
{
523
524
525
526
527
528
    {size_t i;
    for(i = 0; i < ARM11_REGCACHE_COUNT; i++)
    {
	if (!arm11->reg_list[i].valid)
	{
	    if (arm11->reg_history[i].valid)
529
		LOG_INFO("%8s INVALID    (%08x)", arm11_reg_defs[i].name, arm11->reg_history[i].value);
530
531
532
533
534
535
	}
	else
	{
	    if (arm11->reg_history[i].valid)
	    {
		if (arm11->reg_history[i].value != arm11->reg_values[i])
536
		    LOG_INFO("%8s %08x (%08x)", arm11_reg_defs[i].name, arm11->reg_values[i], arm11->reg_history[i].value);
537
538
539
	    }
	    else
	    {
540
		LOG_INFO("%8s %08x (INVALID)", arm11_reg_defs[i].name, arm11->reg_values[i]);
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
	    }
	}
    }}
}


/** Restore processor state
  *
  * This is called in preparation for the RESTART function.
  *
  */
void arm11_leave_debug_state(arm11_common_t * arm11)
{
    FNC_INFO;

    arm11_run_instr_data_prepare(arm11);

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

    /* restore R1 - R14 */
    {size_t i;
    for (i = 1; i < 15; i++)
    {
	if (!arm11->reg_list[ARM11_RC_RX + i].dirty)
	    continue;

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

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

    arm11_run_instr_data_finish(arm11);


    /* spec says clear wDTR and rDTR; we assume they are clear as
oharboe's avatar
oharboe committed
577
       otherwise our programming would be sloppy */
578
579
580
581
582
583

    {
	u32 DSCR = arm11_read_DSCR(arm11);

	if (DSCR & (ARM11_DSCR_RDTR_FULL | ARM11_DSCR_WDTR_FULL))
	{
584
	    LOG_ERROR("wDTR/rDTR inconsistent (DSCR %08x)", DSCR);
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
	}
    }

    arm11_run_instr_data_prepare(arm11);

    /* restore original wDTR */

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

    /* restore CPSR */

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


    /* restore PC */

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


    /* restore R0 */

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

    arm11_run_instr_data_finish(arm11);


    /* restore DSCR */

    arm11_write_DSCR(arm11, R(DSCR));


    /* restore rDTR */
    
    if (R(DSCR) & ARM11_DSCR_RDTR_FULL || arm11->reg_list[ARM11_RC_RDTR].dirty)
    {
	arm11_add_debug_SCAN_N(arm11, 0x05, -1);

	arm11_add_IR(arm11, ARM11_EXTEST, -1);

	scan_field_t	chain5_fields[3];

	u8			Ready	    = 0;	/* ignored */
	u8			Valid	    = 0;	/* ignored */

	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);

oharboe's avatar
oharboe committed
640
	arm11_add_dr_scan_vc(asizeof(chain5_fields), chain5_fields, TAP_PD);
641
642
    }

oharboe's avatar
oharboe committed
643
644
    arm11_record_register_history(arm11);
}
645

oharboe's avatar
oharboe committed
646
647
void arm11_record_register_history(arm11_common_t * arm11)
{
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
    {size_t i;
    for(i = 0; i < ARM11_REGCACHE_COUNT; i++)
    {
	arm11->reg_history[i].value	= arm11->reg_values[i];
	arm11->reg_history[i].valid	= arm11->reg_list[i].valid;

	arm11->reg_list[i].valid	= 0;
	arm11->reg_list[i].dirty	= 0;
    }}
}


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

    arm11_common_t * arm11 = target->arch_info;

    if (arm11->trst_active)
	return ERROR_OK;

    u32	dscr = arm11_read_DSCR(arm11);

672
    LOG_DEBUG("DSCR %08x", dscr);
673
674
675
676
677
678
679

    arm11_check_init(arm11, &dscr);

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

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

	    target_call_event_callbacks(target,
		old_state == TARGET_DEBUG_RUNNING ? TARGET_EVENT_DEBUG_HALTED : TARGET_EVENT_HALTED);
689
690
691
692
	}
    }
    else
    {
oharboe's avatar
oharboe committed
693
	if (target->state != TARGET_RUNNING && target->state != TARGET_DEBUG_RUNNING)
694
	{
695
	    LOG_DEBUG("enter TARGET_RUNNING");
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
	    target->state		= TARGET_RUNNING;
	    target->debug_reason	= DBG_REASON_NOTHALTED;
	}
    }

    return ERROR_OK;
}
/* architecture specific status reply */
int arm11_arch_state(struct target_s *target)
{
    FNC_INFO_NOTIMPLEMENTED;

    return ERROR_OK;
}


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

    return ERROR_OK;
}



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

    arm11_common_t * arm11 = target->arch_info;

729
730
    LOG_DEBUG("target->state: %s", 
	      Jim_Nvp_value2name_simple( nvp_target_state, target->state )->name );
731

oharboe's avatar
oharboe committed
732
733
734
735
736
    if (target->state == TARGET_UNKNOWN)
    {
	arm11->simulate_reset_on_next_halt = true;
    }

737
738
    if (target->state == TARGET_HALTED)
    {
739
740
		LOG_DEBUG("target was already halted");
		return ERROR_OK;
741
742
743
744
    }

    if (arm11->trst_active)
    {
oharboe's avatar
oharboe committed
745
	arm11->halt_requested = true;
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
	return ERROR_OK;
    }

    arm11_add_IR(arm11, ARM11_HALT, TAP_RTI);

    jtag_execute_queue();

    u32 dscr;

    while (1)
    {
	dscr = arm11_read_DSCR(arm11);

	if (dscr & ARM11_DSCR_CORE_HALTED)
	    break;
    }

    arm11_on_enter_debug_state(arm11);

oharboe's avatar
oharboe committed
765
766
    enum target_state old_state	= target->state;

767
768
    target->state		= TARGET_HALTED;
    target->debug_reason	= arm11_get_DSCR_debug_reason(dscr);
oharboe's avatar
oharboe committed
769
770
771
772

    target_call_event_callbacks(target,
	old_state == TARGET_DEBUG_RUNNING ? TARGET_EVENT_DEBUG_HALTED : TARGET_EVENT_HALTED);

773
774
775
776
777
778
779
780
    return ERROR_OK;
}


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

781
//    LOG_DEBUG("current %d  address %08x  handle_breakpoints %d  debug_execution %d",
oharboe's avatar
oharboe committed
782
//	current, address, handle_breakpoints, debug_execution);
oharboe's avatar
oharboe committed
783

784
785
    arm11_common_t * arm11 = target->arch_info;

786
787
788
    LOG_DEBUG("target->state: %s", 
	      Jim_Nvp_value2name_simple( nvp_target_state, target->state )->name );

789
790

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

    if (!current)
	R(PC) = address;

799
    LOG_INFO("RESUME PC %08x%s", R(PC), !current ? "!" : "");
oharboe's avatar
oharboe committed
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814

    /* clear breakpoints/watchpoints and VCR*/
    arm11_sc7_clear_vbw(arm11);

    /* Set up breakpoints */
    if (!debug_execution)
    {
	/* check if one matches PC and step over it if necessary */

	breakpoint_t *	bp;

	for (bp = target->breakpoints; bp; bp = bp->next)
	{
	    if (bp->address == R(PC))
	    {
815
		LOG_DEBUG("must step over %08x", bp->address);
oharboe's avatar
oharboe committed
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
		arm11_step(target, 1, 0, 0);
		break;
	    }
	}

	/* set all breakpoints */

	size_t		brp_num = 0;
	
	for (bp = target->breakpoints; bp; bp = bp->next)
	{
	    arm11_sc7_action_t	brp[2];

	    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);
    
	    arm11_sc7_run(arm11, brp, asizeof(brp));

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

	    brp_num++;
	}

	arm11_sc7_set_vcr(arm11, arm11_vcr);
    }

846
847
848
849
850
851
852
853
854
855
856

    arm11_leave_debug_state(arm11);

    arm11_add_IR(arm11, ARM11_RESTART, TAP_RTI);

    jtag_execute_queue();

    while (1)
    {
	u32 dscr = arm11_read_DSCR(arm11);

857
	LOG_DEBUG("DSCR %08x", dscr);
858
859
860
861
862

	if (dscr & ARM11_DSCR_CORE_RESTARTED)
	    break;
    }

oharboe's avatar
oharboe committed
863
864
865
866
867
868
869
870
871
872
873
874
    if (!debug_execution)
    {
	target->state		= TARGET_RUNNING;
	target->debug_reason	= DBG_REASON_NOTHALTED;
	target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
    }
    else
    {
	target->state		= TARGET_DEBUG_RUNNING;
	target->debug_reason	= DBG_REASON_NOTHALTED;
	target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED);
    }
875
876
877
878
879
880
881
882

    return ERROR_OK;
}

int arm11_step(struct target_s *target, int current, u32 address, int handle_breakpoints)
{
    FNC_INFO;

883
884
    LOG_DEBUG("target->state: %s", 
	      Jim_Nvp_value2name_simple( nvp_target_state, target->state )->name );
885
886
887

    if (target->state != TARGET_HALTED)
    {
888
	LOG_WARNING("target was not halted");
889
890
891
892
893
	return ERROR_TARGET_NOT_HALTED;
    }

    arm11_common_t * arm11 = target->arch_info;

oharboe's avatar
oharboe committed
894
895
896
    if (!current)
	R(PC) = address;

897
    LOG_INFO("STEP PC %08x%s", R(PC), !current ? "!" : "");
898

oharboe's avatar
oharboe committed
899
    /** \todo TODO: Thumb not supported here */
900

oharboe's avatar
oharboe committed
901
    u32	next_instruction;
902

oharboe's avatar
oharboe committed
903
    arm11_read_memory_word(arm11, R(PC), &next_instruction);
904

oharboe's avatar
oharboe committed
905
    /* skip over BKPT */
oharboe's avatar
oharboe committed
906
907
908
909
910
    if ((next_instruction & 0xFFF00070) == 0xe1200070)
    {
	R(PC) += 4;
	arm11->reg_list[ARM11_RC_PC].valid = 1;
	arm11->reg_list[ARM11_RC_PC].dirty = 0;
911
	LOG_INFO("Skipping BKPT");
oharboe's avatar
oharboe committed
912
    }
oharboe's avatar
oharboe committed
913
914
915
916
917
918
919
    /* skip over Wait for interrupt / Standby */
    /* mcr	15, 0, r?, cr7, cr0, {4} */
    else if ((next_instruction & 0xFFFF0FFF) == 0xee070f90)
    {
	R(PC) += 4;
	arm11->reg_list[ARM11_RC_PC].valid = 1;
	arm11->reg_list[ARM11_RC_PC].dirty = 0;
920
	LOG_INFO("Skipping WFI");
oharboe's avatar
oharboe committed
921
    }
oharboe's avatar
oharboe committed
922
923
924
    /* ignore B to self */
    else if ((next_instruction & 0xFEFFFFFF) == 0xeafffffe)
    {
925
	LOG_INFO("Not stepping jump to self");
oharboe's avatar
oharboe committed
926
927
928
929
930
    }
    else
    {
	/** \todo TODO: check if break-/watchpoints make any sense at all in combination
	  * with this. */
931

oharboe's avatar
oharboe committed
932
933
	/** \todo TODO: check if disabling IRQs might be a good idea here. Alternatively
	  * the VCR might be something worth looking into. */
934
935


oharboe's avatar
oharboe committed
936
	/* Set up breakpoint for stepping */
937

oharboe's avatar
oharboe committed
938
	arm11_sc7_action_t	brp[2];
939

oharboe's avatar
oharboe committed
940
941
942
943
944
945
	brp[0].write	= 1;
	brp[0].address	= ARM11_SC7_BVR0;
	brp[0].value	= R(PC);
	brp[1].write	= 1;
	brp[1].address	= ARM11_SC7_BCR0;
	brp[1].value	= 0x1 | (3 << 1) | (0x0F << 5) | (0 << 14) | (0 << 16) | (0 << 20) | (2 << 21);
946

oharboe's avatar
oharboe committed
947
	arm11_sc7_run(arm11, brp, asizeof(brp));
948

oharboe's avatar
oharboe committed
949
	/* resume */
950

oharboe's avatar
oharboe committed
951
	arm11_leave_debug_state(arm11);
952

oharboe's avatar
oharboe committed
953
	arm11_add_IR(arm11, ARM11_RESTART, TAP_RTI);
954

oharboe's avatar
oharboe committed
955
	jtag_execute_queue();
956

oharboe's avatar
oharboe committed
957
	/** \todo TODO: add a timeout */
958

oharboe's avatar
oharboe committed
959
	/* wait for halt */
960

oharboe's avatar
oharboe committed
961
962
963
	while (1)
	{
	    u32 dscr = arm11_read_DSCR(arm11);
964

965
	    LOG_DEBUG("DSCR %08x", dscr);
966

oharboe's avatar
oharboe committed
967
968
969
970
	    if ((dscr & (ARM11_DSCR_CORE_RESTARTED | ARM11_DSCR_CORE_HALTED)) ==
		(ARM11_DSCR_CORE_RESTARTED | ARM11_DSCR_CORE_HALTED))
		break;
	}
971

oharboe's avatar
oharboe committed
972
973
974
975
976
977
	/* clear breakpoint */
	arm11_sc7_clear_vbw(arm11);

	/* save state */
	arm11_on_enter_debug_state(arm11);
    }
978

oharboe's avatar
oharboe committed
979
//    target->state		= TARGET_HALTED;
980
981
    target->debug_reason	= DBG_REASON_SINGLESTEP;

oharboe's avatar
oharboe committed
982
983
    target_call_event_callbacks(target, TARGET_EVENT_HALTED);

984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
    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;