Context Navigation

source: rtems/cpukit/libdebugger/rtems-debugger-arm.c

Last change on this file was 18b1a591, checked in by Chris Johns <chrisj@…>, on 10/27/22 at 18:47:03

cpukit/libdebugger: Fix stepping on ARM architectures

Closes #4744

Property mode set to 100644

File size: 75.6 KB

Line
1	/*
2	* Copyright (c) 2016-2022 Chris Johns <chrisj@rtems.org>.
3	* All rights reserved.
4	*
5	* Redistribution and use in source and binary forms, with or without
6	* modification, are permitted provided that the following conditions
7	* are met:
8	* 1. Redistributions of source code must retain the above copyright
9	* notice, this list of conditions and the following disclaimer.
10	* 2. Redistributions in binary form must reproduce the above copyright
11	* notice, this list of conditions and the following disclaimer in the
12	* documentation and/or other materials provided with the distribution.
13	*
14	* THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
15	* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16	* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17	* ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
18	* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
19	* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
20	* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
21	* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
22	* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
23	* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
24	* SUCH DAMAGE.
25	*/
26
27	#define TARGET_DEBUG 0
28
29	#define ARM_DUMP_ROM_TABLES 0
30
31	#ifdef HAVE_CONFIG_H
32	#include "config.h"
33	#endif
34
35	#include <errno.h>
36	#include <inttypes.h>
37	#include <stdlib.h>
38
39	#include <rtems.h>
40	#include <rtems/score/threadimpl.h>
41
42	#include <rtems/debugger/rtems-debugger-bsp.h>
43
44	#include "rtems-debugger-target.h"
45	#include "rtems-debugger-threads.h"
46
47	#if TARGET_DEBUG
48	#include <rtems/bspIo.h>
49	#endif
50
51	/*
52	* ARM Variant controls.
53	*/
54	#if (__ARM_ARCH >= 7) && \
55	(__ARM_ARCH_PROFILE == 'A' \|\| __ARM_ARCH_PROFILE == 'R')
56	#define ARM_CP15 1
57	#endif
58
59	#if (__ARM_ARCH >= 7) && \
60	(__ARM_ARCH_PROFILE == 'M')
61	#define ARM_THUMB_ONLY 1
62	#else
63	#define ARM_THUMB_ONLY 0
64	#endif
65
66	#if defined(ARM_MULTILIB_ARCH_V4)
67	#define ARM_PSR_HAS_INT_MASK 1
68	#define ARM_PSR_HAS_THUMB 1
69	#else
70	#define ARM_PSR_HAS_INT_MASK 0
71	#define ARM_PSR_HAS_THUMB 0
72	#endif
73
74	#if ARM_CP15
75	#include <libcpu/arm-cp15.h>
76	#endif
77
78	/**
79	* If thumb build of code switch the asm to thumb as required.
80	*
81	* If the variant only supports thumb insturctions disable the support.
82	*/
83	#define NEEDS_THUMB_SWITCH !ARM_THUMB_ONLY && defined(__thumb__)
84
85	#if NEEDS_THUMB_SWITCH
86	#define ARM_SWITCH_REG uint32_t arm_switch_reg
87	#define ARM_SWITCH_REG_ASM [arm_switch_reg] "=&r" (arm_switch_reg)
88	#define ARM_SWITCH_REG_ASM_L ARM_SWITCH_REG_ASM,
89	#define ASM_ARM_ASM ".align 2\n.arm\n"
90	#define ASM_ARM_MODE ".align 2\nbx pc\n.arm\n"
91	#define ASM_THUMB_MODE "add %[arm_switch_reg], pc, #1\nbx %[arm_switch_reg]\n.thumb\n"
92	#define ARM_THUMB_MODE() __asm__ volatile(ASM_THUMB_MODE : ARM_SWITCH_REG_ASM : :);
93	#define ARM_ARM_MODE() __asm__ volatile(ASM_ARM_MODE : : :);
94	#else
95	#define ARM_SWITCH_REG
96	#define ARM_SWITCH_REG_ASM
97	#define ARM_SWITCH_REG_ASM_L
98	#define ASM_ARM_ASM
99	#define ASM_ARM_MODE
100	#define ASM_THUMB_MODE
101	#define ARM_THUMB_MODE()
102	#define ARM_ARM_MODE()
103	#endif
104
105	#ifdef ARM_MULTILIB_HAS_BARRIER_INSTRUCTIONS
106	#define ARM_SYNC_INST "isb\n"
107	#else
108	#define ARM_SYNC_INST
109	#endif
110
111	/*
112	* Hack to work around ARMv7-M not having a the T and I bits in the PSR.
113	*
114	* This needs to be fixed when real support for this ARM variant is added.
115	*/
116	#if !defined(ARM_PSR_I)
117	#define ARM_PSR_I 0
118	#endif
119	#if !defined(ARM_PSR_T)
120	#define ARM_PSR_T 0
121	#endif
122
123	/*
124	* The ARM has 2 interrupt bits.
125	*/
126	#define CPSR_IRQ_DISABLE 0x80 /* IIQ disabled when 1 */
127	#define CPSR_FIQ_DISABLE 0x40 /* FIQ disabled when 1 */
128	#define CPSR_INTS_MASK (CPSR_IRQ_DISABLE \| CPSR_FIQ_DISABLE)
129
130	/*
131	* Software breakpoint block size.
132	*/
133	#define RTEMS_DEBUGGER_SWBREAK_NUM 64
134
135	/*
136	* Number of registers.
137	*/
138	#define RTEMS_DEBUGGER_NUMREGS 26
139
140	/*
141	* Number of bytes per type of register.
142	*/
143	#define RTEMS_DEBUGGER_REG_BYTES 4
144	#define RTEMS_DEBUGGER_FP_REG_BYTES 12
145
146	/*
147	* Debugger registers layout. See arm-core.xml in GDB source.
148	*/
149	#define REG_R0 0
150	#define REG_R1 1
151	#define REG_R2 2
152	#define REG_R3 3
153	#define REG_R4 4
154	#define REG_R5 5
155	#define REG_R6 6
156	#define REG_R7 7
157	#define REG_R8 8
158	#define REG_R9 9
159	#define REG_R10 10
160	#define REG_R11 11
161	#define REG_R12 12
162	#define REG_SP 13
163	#define REG_LR 14
164	#define REG_PC 15
165	#define REG_F0 16
166	#define REG_F1 17
167	#define REG_F2 18
168	#define REG_F3 19
169	#define REG_F4 20
170	#define REG_F5 21
171	#define REG_F6 22
172	#define REG_F7 23
173	#define REG_FPS 24
174	#define REG_CPSR 25
175
176	/**
177	* Register offset table with the total as the last entry.
178	*
179	* Check this table in gdb with the command:
180	*
181	* maint print registers
182	*/
183	static const size_t arm_reg_offsets[RTEMS_DEBUGGER_NUMREGS + 1] =
184	{
185	0, /* REG_R0 4 uint32_t */
186	4, /* REG_R1 4 uint32_t */
187	8, /* REG_R2 4 uint32_t */
188	12, /* REG_R3 4 uint32_t */
189	16, /* REG_R4 4 uint32_t */
190	20, /* REG_R5 4 uint32_t */
191	24, /* REG_R6 4 uint32_t */
192	28, /* REG_R7 4 uint32_t */
193	32, /* REG_R8 4 uint32_t */
194	36, /* REG_R9 4 uint32_t */
195	40, /* REG_R10 4 uint32_t */
196	44, /* REG_R11 4 uint32_t */
197	48, /* REG_R12 4 uint32_t */
198	52, /* REG_SP 4 1 /
199	56, /* REG_LR 4 uint32_t */
200	60, /* REG_PC 4 1 /
201	64, /* REG_F0 12 _arm_ext */
202	76, /* REG_F1 12 _arm_ext */
203	88, /* REG_F2 12 _arm_ext */
204	100, /* REG_F3 12 _arm_ext */
205	112, /* REG_F4 12 _arm_ext */
206	124, /* REG_F5 12 _arm_ext */
207	136, /* REG_F6 12 _arm_ext */
208	148, /* REG_F7 12 _arm_ext */
209	160, /* REG_FPS 4 uint32_t */
210	164, /* REG_CPSR 4 uint32_t */
211	168 /* total size */
212	};
213
214	/*
215	* Number of bytes of registers.
216	*/
217	#define RTEMS_DEBUGGER_NUMREGBYTES arm_reg_offsets[RTEMS_DEBUGGER_NUMREGS]
218
219	/**
220	* The various status registers.
221	*/
222	#if defined(ARM_MULTILIB_ARCH_V4) \|\| defined(ARM_MULTILIB_ARCH_V6M)
223	#define FRAME_SR(_frame) (_frame)->register_cpsr
224	#elif defined(ARM_MULTILIB_ARCH_V7M)
225	#define FRAME_SR(_frame) (_frame)->register_xpsr
226	#else
227	#error ARM architecture is not supported.
228	#endif
229
230	/**
231	* Print the exception frame.
232	*/
233	#define EXC_FRAME_PRINT(_out, _prefix, _frame) \
234	do { \
235	_out(_prefix " R0 = %08" PRIx32 " R1 = %08" PRIx32 \
236	" R2 = %08" PRIx32 " R3 = %08" PRIx32 "\n", \
237	_frame->register_r0, _frame->register_r1, \
238	_frame->register_r2, _frame->register_r3); \
239	_out(_prefix " R4 = %08" PRIx32 " R5 = %08" PRIx32 \
240	" R6 = %08" PRIx32 " R7 = %08" PRIx32 "\n", \
241	_frame->register_r4, _frame->register_r5, \
242	_frame->register_r6, _frame->register_r7); \
243	_out(_prefix " R8 = %08" PRIx32 " R9 = %08" PRIx32 \
244	" R10 = %08" PRIx32 " R11 = %08" PRIx32 "\n", \
245	_frame->register_r8, _frame->register_r9, \
246	_frame->register_r10, _frame->register_r11); \
247	_out(_prefix " R12 = %08" PRIx32 " SP = %08" PRIx32 \
248	" LR = %08" PRIxPTR " PC = %08" PRIxPTR "\n", \
249	_frame->register_r12, _frame->register_sp, \
250	(intptr_t) _frame->register_lr, (intptr_t) _frame->register_pc); \
251	_out(_prefix " CPSR = %08" PRIx32 " %c%c%c%c%c%c%c%c%c%c%c" \
252	" GE:%" PRIx32 " IT:%02" PRIx32 " M:%" PRIx32 " %s\n", \
253	FRAME_SR(_frame), \
254	(FRAME_SR(_frame) & (1 << 31)) != 0 ? 'N' : '-', \
255	(FRAME_SR(_frame) & (1 << 30)) != 0 ? 'Z' : '-', \
256	(FRAME_SR(_frame) & (1 << 29)) != 0 ? 'C' : '-', \
257	(FRAME_SR(_frame) & (1 << 28)) != 0 ? 'V' : '-', \
258	(FRAME_SR(_frame) & (1 << 27)) != 0 ? 'Q' : '-', \
259	(FRAME_SR(_frame) & (1 << 24)) != 0 ? 'J' : '-', \
260	(FRAME_SR(_frame) & (1 << 9)) != 0 ? 'E' : '-', \
261	(FRAME_SR(_frame) & (1 << 8)) != 0 ? 'A' : '-', \
262	(FRAME_SR(_frame) & (1 << 7)) != 0 ? 'I' : '-', \
263	(FRAME_SR(_frame) & (1 << 6)) != 0 ? 'F' : '-', \
264	(FRAME_SR(_frame) & (1 << 5)) != 0 ? 'T' : '-', \
265	(FRAME_SR(_frame) >> 16) & 0xf, \
266	((FRAME_SR(_frame) >> (25 - 5)) & (0x3 << 5)) \| ((FRAME_SR(_frame) >> 10) & 0x1f), \
267	FRAME_SR(_frame) & 0x1f, arm_mode_label(FRAME_SR(_frame) & 0x1f)); \
268	} while (0)
269
270	/**
271	* The breakpoint.
272	*/
273	#ifdef __thumb__
274	static const uint8_t breakpoint[2] = { 0x55, 0xbe };
275	#else
276	static const uint8_t breakpoint[4] = { 0x75, 0xe0, 0x20, 0xe1 };
277	#endif
278
279	/**
280	* Target lock.
281	*/
282	RTEMS_INTERRUPT_LOCK_DEFINE(static, target_lock, "target_lock")
283
284	/**
285	* An exception offset is added to the return address of the PC on an
286	* exception's stack frame. The PC needs to be adjusted.
287	*/
288	static const size_t exc_offsets[2][5] =
289	{
290	/* ARM undef_ins sup call pref abt data abt */
291	{ 0, 4, 0, 4, 8 },
292	/* TMB undef_ins sup call pref abt data abt */
293	{ 0, 2, 0, 4, 8 }
294	};
295
296	/**
297	* Is a session active?
298	*/
299	static bool debug_session_active;
300
301	/*
302	* ARM debug hardware. These variables are directly access
303	* from assembler so do not change types.
304	*/
305	static int debug_version;
306	static void* debug_registers;
307	static int debug_revision;
308	static int debug_disable_ints;
309	static int hw_breakpoints;
310	static int hw_watchpoints;
311
312	/**
313	* Hardware break and watch points.
314	*/
315	#define ARM_HW_BREAKPOINT_MAX (16)
316	#define ARM_HW_WATCHPOINT_MAX (16)
317
318	/*
319	* Types of break points. Only the 2 we use listed.
320	*/
321	#define ARM_HW_BP_UNLINKED_INSTR_MATCH (0x00)
322	#define ARM_HW_BP_UNLINKED_INSTR_MISMATCH (0x04)
323
324	/*
325	* Privilege levels.
326	*/
327	#define ARM_HW_BP_PRIV_PL0_SUP_SYS (0x00)
328	#define ARM_HW_BP_PRIV_PL1_ONLY (0x01)
329	#define ARM_HW_BP_PRIV_PL0_ONLY (0x02)
330	#define ARM_HW_BP_PRIV_ALL_MODES (0x03)
331
332	/*
333	* A hw breakpoint has DBGBCR and DBGBVR registers. Allocate memory
334	* for each.
335	*
336	* Maintian the values ready to load into the hardware. The loader is
337	* a load of the value and then control for enabled BPs.
338	*/
339	static uint32_t hw_breaks[ARM_HW_BREAKPOINT_MAX * 2];
340
341	/*
342	* The order in the array is important
343	*/
344	#define ARM_HWB_BCR(_bp) (hw_breaks[((_bp) * 2) + 1])
345	#define ARM_HWB_VCR(_bp) (hw_breaks[(_bp) * 2])
346	#define ARM_HWB_ENALBED(_bp) ((ARM_HWB_BCR(_bp) & 1) != 0)
347	#define ARM_HWB_CLEAR(_bp) ARM_HWB_BCR(_bp) = 0; ARM_HWB_VCR(_bp) = 0
348	#define ARM_HWB_CLEAR_ALL() memset(&hw_breaks[0], 0, sizeof(hw_breaks))
349
350	/*
351	* Method of entry (MOE) to debug mode. Bits [5:2] of DBGDSCR.
352	*/
353	#define ARM_HW_DSCR_MOE_HALT_REQUEST (0x0)
354	#define ARM_HW_DSCR_MOE_BREAKPOINT_EVENT (0x1)
355	#define ARM_HW_DSCR_MOE_ASYNC_WATCHPOINT (0x2)
356	#define ARM_HW_DSCR_MOE_BREAKPOINT_INSTR (0x3)
357	#define ARM_HW_DSCR_MOE_EXTERNAL (0x4)
358	#define ARM_HW_DSCR_MOE_VECTOR_CATCH_EVENT (0x5)
359	#define ARM_HW_DSCR_MOE_OS_UNLOCK_EVENT (0x8)
360	#define ARM_HW_DSCR_MOE_SYNC_WATCHPOINT (0xa)
361
362	/*
363	* Use to locally probe and catch exceptions when accessinf suspect addresses.
364	*/
365	#if ARM_CP15
366	static void __attribute__((naked)) arm_debug_unlock_abort(void);
367	#endif
368
369	/*
370	* Target debugging support. Use this to debug the backend.
371	*/
372	#if TARGET_DEBUG
373	void rtems_debugger_printk_lock(rtems_interrupt_lock_context* lock_context);
374	void rtems_debugger_printk_unlock(rtems_interrupt_lock_context* lock_context);
375
376	static void target_printk(const char* format, ...) RTEMS_PRINTFLIKE(1, 2);
377	static void
378	target_printk(const char* format, ...)
379	{
380	rtems_interrupt_lock_context lock_context;
381	va_list ap;
382	va_start(ap, format);
383	rtems_debugger_printk_lock(&lock_context);
384	vprintk(format, ap);
385	rtems_debugger_printk_unlock(&lock_context);
386	va_end(ap);
387	}
388	#else
389	#define target_printk(_fmt, ...)
390	#define mode_labels(_m) NULL
391	#endif
392
393	static const char*
394	arm_mode_label(int mode)
395	{
396	switch (mode) {
397	case 0x10:
398	return "USR";
399	case 0x11:
400	return "FIQ";
401	case 0x12:
402	return "IRQ";
403	case 0x13:
404	return "SVC";
405	case 0x16:
406	return "MON";
407	case 0x17:
408	return "ABT";
409	case 0x1a:
410	return "HYP";
411	case 0x1b:
412	return "UND";
413	case 0x1f:
414	return "SYS";
415	}
416	return "---";
417	}
418
419	#if TARGET_DEBUG
420	static const char*
421	arm_moe_label(uint32_t moe)
422	{
423	switch (moe) {
424	case ARM_HW_DSCR_MOE_HALT_REQUEST:
425	return "HLT";
426	case ARM_HW_DSCR_MOE_BREAKPOINT_EVENT:
427	return "BPE";
428	case ARM_HW_DSCR_MOE_ASYNC_WATCHPOINT:
429	return "AWP";
430	case ARM_HW_DSCR_MOE_BREAKPOINT_INSTR:
431	return "BPI";
432	case ARM_HW_DSCR_MOE_EXTERNAL:
433	return "EXT";
434	case ARM_HW_DSCR_MOE_VECTOR_CATCH_EVENT:
435	return "VCE";
436	case ARM_HW_DSCR_MOE_OS_UNLOCK_EVENT:
437	return "OUL";
438	case ARM_HW_DSCR_MOE_SYNC_WATCHPOINT:
439	return "SWP";
440	default:
441	break;
442	}
443	return "RSV";
444	}
445	#endif
446
447	/*
448	* CP register access.
449	*/
450	#define ARM_CP_INSTR(_opc, _cp, _op1, _val, _CRn, _CRm, _op2) \
451	#_opc " p" #_cp ", " #_op1 ", %[" #_val "], c" #_CRn ", c" #_CRm ", " #_op2 "\n"
452
453	#define ARM_CP_WRITE(_cp, _op1, _val, _CRn, _CRm, _op2) \
454	do { \
455	ARM_SWITCH_REG; \
456	asm volatile( \
457	ASM_ARM_MODE \
458	ARM_CP_INSTR(mcr, _cp, _op1, val, _CRn, _CRm, _op2) \
459	ARM_SYNC_INST \
460	ASM_THUMB_MODE \
461	: ARM_SWITCH_REG_ASM \
462	: [val] "r" (_val)); \
463	} while (0)
464
465	#define ARM_CP_READ(_cp, _op1, _val, _CRn, _CRm, _op2) \
466	do { \
467	ARM_SWITCH_REG; \
468	asm volatile( \
469	ASM_ARM_MODE \
470	ARM_SYNC_INST \
471	ARM_CP_INSTR(mrc, _cp, _op1, val, _CRn, _CRm, _op2) \
472	ASM_THUMB_MODE \
473	: ARM_SWITCH_REG_ASM_L \
474	[val] "=&r" (_val)); \
475	} while (0)
476
477	/*
478	* CP14 register access.
479	*
480	* The registers can be access via the core or they can be memory-mapped.
481	*/
482
483	/*
484	* Read and write a CP14 register.
485	*
486	* The software debug event registers are not easy to program because there are
487	* up to 32 registers and the instructions have to assembler for each of the 32
488	* registers, you cannot program it. This means there is a switch table to do
489	* this.
490	*/
491	#define ARM_CP14_WRITE(_val, _CRn, _CRm, _op2) \
492	ARM_CP_WRITE(14, 0, _val, _CRn, _CRm, _op2)
493
494	#define ARM_CP14_READ(_val, _CRn, _CRm, _op2) \
495	ARM_CP_READ(14, 0, _val, _CRn, _CRm, _op2)
496
497	/*
498	* Read and write a CP15 register.
499	*
500	* The Context ID register is a process level context and used to scope
501	* hardware break points.
502	*/
503	#define ARM_CP15_WRITE(_val, _op1, _CRn, _CRm, _op2) \
504	ARM_CP_WRITE(15, _op1, _val, _CRn, _CRm, _op2)
505
506	#define ARM_CP15_READ(_val, _op1, _CRn, _CRm, _op2) \
507	ARM_CP_READ(15, _op1, _val, _CRn, _CRm, _op2)
508
509	/*
510	* Read and write a memory mapped debug register. The register number is a word
511	* offset from the base address.
512	*/
513	#define ARM_MMAP_ADDR(reg) \
514	(((volatile uint32_t*) debug_registers) + (reg))
515	#define ARM_MMAP_WRITE(reg, val) *ARM_MMAP_ADDR(reg) = (val)
516	#define ARM_MMAP_READ(reg) *ARM_MMAP_ADDR(reg)
517	#define ARM_MMAP_WRITE_SYNC(reg, val) \
518	ARM_MMAP_WRITE(reg, val); \
519	_ARM_Data_synchronization_barrier(); \
520	_ARM_Instruction_synchronization_barrier()
521
522	/*
523	* Debug hardware breakpoint registers.
524	*/
525	#define ARM_MMAP_DBGDSCR 34
526	#define ARM_MMAP_DBGBCR 80
527	#define ARM_MMAP_DBGBVR 64
528
529	static bool
530	arm_debug_authentication(uint32_t dbgauthstatus)
531	{
532	bool granted = (dbgauthstatus & (1 << 0)) != 0;
533	rtems_debugger_printf("rtems-db: arm debug: authentication: %s " \
534	"(%s %s %s %s %s %s %s %s)\n",
535	granted ? "granted" : "denied",
536	(dbgauthstatus & (1 << 0)) == 0 ? "-" : "NSE",
537	(dbgauthstatus & (1 << 1)) == 0 ? "-" : "NSI",
538	(dbgauthstatus & (1 << 2)) == 0 ? "-" : "NSNE",
539	(dbgauthstatus & (1 << 3)) == 0 ? "-" : "NSNI",
540	(dbgauthstatus & (1 << 4)) == 0 ? "-" : "SE",
541	(dbgauthstatus & (1 << 5)) == 0 ? "-" : "SI",
542	(dbgauthstatus & (1 << 6)) == 0 ? "-" : "SNE",
543	(dbgauthstatus & (1 << 7)) == 0 ? "-" : "SNI");
544	return granted;
545	}
546
547	static int
548	arm_debug_cp14_enable(rtems_debugger_target* target)
549	{
550	uint32_t val;
551	ARM_CP14_READ(val, 7, 14, 6);
552	if (!arm_debug_authentication(val))
553	return -1;
554	ARM_CP14_READ(val, 0, 1, 0);
555	if ((val & (1 << 15)) == 0) {
556	switch (debug_version) {
557	case 1:
558	case 2:
559	ARM_CP14_WRITE(val \| (1 << 15), 0, 1, 0);
560	break;
561	case 3:
562	case 5:
563	default:
564	ARM_CP14_WRITE(val \| (1 << 15), 0, 2, 2);
565	break;
566	case 4:
567	rtems_debugger_printf("rtems-db: arm debug: no cp14 access with version 4\n");
568	return -1;
569	}
570	ARM_CP14_READ(val, 0, 1, 0);
571	if ((val & (1 << 15)) == 0) {
572	rtems_debugger_printf("rtems-db: arm debug: cannot enter monitor mode\n");
573	errno = EIO;
574	return -1;
575	}
576	}
577	rtems_debugger_printf("rtems-db: arm debug: using cp14 register access\n");
578	return 0;
579	}
580
581	/*
582	* The write access to the software unlock register can cause an abort. Absorb
583	* it.
584	*/
585	static jmp_buf unlock_abort_jmpbuf;
586	static size_t arm_debug_retries;
587
588	static void
589	arm_debug_dump_rom_table(uint32_t* rom, size_t depth)
590	{
591	uint32_t pidr[7];
592	uint32_t cidr[4];
593	uint32_t memtype;
594	uint32_t pidr4_4KB_count;
595	size_t r;
596
597	static const char *table_class[16] = {
598	"reserved",
599	"ROM table",
600	"reserved", "reserved",
601	"reserved",
602	"reserved",
603	"reserved",
604	"reserved",
605	"reserved",
606	"CoreSight component",
607	"reserved",
608	"Peripheral Test Block",
609	"reserved",
610	"OptimoDE DESS",
611	"Generic IP component",
612	"PrimeCell or System component"
613	};
614
615	#define ROM_READ(b_, o_, r_) b_[((o_) / sizeof(uint32_t)) + (r_)]
616
617	if (depth > 16) {
618	rtems_debugger_printf("]] rom: too deep\n");
619	return;
620	}
621
622	for (r = 0; r < 4; ++r)
623	pidr[r] = ROM_READ(rom, 0xfe0, r) & 0xff;
624	for (r = 0; r < 3; ++r)
625	pidr[r + 4] = ROM_READ(rom, 0xfd0, r) & 0xff;
626	for (r = 0; r < 4; ++r)
627	cidr[r] = ROM_READ(rom, 0xff0, r) & 0xff;
628	memtype = ROM_READ(rom, 0xfcc, 0);
629
630	pidr4_4KB_count = pidr[4] & (((1 << (7 - 4)) - 1) >> 4);
631
632	rtems_debugger_printf("]] rom = %p\n", rom);
633	rtems_debugger_printf(" PIDR: %08x %08x %08x %08x %08x %08x %08x\n",
634	pidr[0], pidr[1], pidr[2], pidr[3],
635	pidr[4], pidr[5], pidr[6]);
636	rtems_debugger_printf(" CIDR: %08x %08x %08x %08x\n",
637	cidr[0], cidr[1], cidr[2], cidr[3]);
638	rtems_debugger_printf(" 4KB count: %u\n", pidr4_4KB_count);
639
640	if ((memtype & 0x01) != 0)
641	rtems_debugger_printf(" MEMTYPE sys memory present on bus\n");
642	else
643	rtems_debugger_printf(" MEMTYPE sys memory not present: dedicated debug bus\n");
644
645	/*
646	* Read ROM table entries until we get 0
647	*/
648	for (r = 0; rom[r] != 0; ++r) {
649	uint32_t romentry = rom[r];
650	uint32_t c_pidr[7];
651	uint32_t c_cidr[4];
652	uint32_t* c_base;
653	uint32_t table_type;
654	size_t i;
655
656	c_base = (uint32_t*) ((intptr_t) rom + (romentry & 0xFFFFF000));
657
658	/*
659	* Read the IDs.
660	*/
661	for (i = 0; i < 4; ++i)
662	c_pidr[i] = ROM_READ(c_base, 0xfe0, i) & 0xff;
663	for (i = 0; i < 3; ++i)
664	c_pidr[i + 4] = ROM_READ(c_base, 0xfd0, i) & 0xff;
665	for (i = 0; i < 4; ++i)
666	c_cidr[i] = ROM_READ(c_base, 0xff0, i) & 0xff;
667
668	table_type = ROM_READ(c_base, 0xfcc, 0);
669
670	rtems_debugger_printf(" > Base: %p, start: 0x%" PRIx32 "\n",
671	c_base,
672	/* component may take multiple 4K pages */
673	(uint32_t)((intptr_t) c_base - 0x1000 * (c_pidr[4] >> 4)));
674	rtems_debugger_printf(" Class is 0x%x, %s\n",
675	(c_cidr[1] >> 4) & 0xf, table_class[(c_cidr[1] >> 4) & 0xf]);
676
677	if (((c_cidr[1] >> 4) & 0x0f) == 1) {
678	arm_debug_dump_rom_table(c_base, depth + 1);
679	}
680	else if (((c_cidr[1] >> 4) & 0x0f) == 9) {
681	const char* major = "reserved";
682	const char* subtype = "reserved";
683	unsigned minor = (table_type >> 4) & 0x0f;
684
685	switch (table_type & 0x0f) {
686	case 0:
687	major = "Miscellaneous";
688	switch (minor) {
689	case 0:
690	subtype = "other";
691	break;
692	case 4:
693	subtype = "Validation component";
694	break;
695	}
696	break;
697	case 1:
698	major = "Trace Sink";
699	switch (minor) {
700	case 0:
701	subtype = "other";
702	break;
703	case 1:
704	subtype = "Port";
705	break;
706	case 2:
707	subtype = "Buffer";
708	break;
709	case 3:
710	subtype = "Router";
711	break;
712	}
713	break;
714	case 2:
715	major = "Trace Link";
716	switch (minor) {
717	case 0:
718	subtype = "other";
719	break;
720	case 1:
721	subtype = "Funnel, router";
722	break;
723	case 2:
724	subtype = "Filter";
725	break;
726	case 3:
727	subtype = "FIFO, buffer";
728	break;
729	}
730	break;
731	case 3:
732	major = "Trace Source";
733	switch (minor) {
734	case 0:
735	subtype = "other";
736	break;
737	case 1:
738	subtype = "Processor";
739	break;
740	case 2:
741	subtype = "DSP";
742	break;
743	case 3:
744	subtype = "Engine/Coprocessor";
745	break;
746	case 4:
747	subtype = "Bus";
748	break;
749	case 6:
750	subtype = "Software";
751	break;
752	}
753	break;
754	case 4:
755	major = "Debug Control";
756	switch (minor) {
757	case 0:
758	subtype = "other";
759	break;
760	case 1:
761	subtype = "Trigger Matrix";
762	break;
763	case 2:
764	subtype = "Debug Auth";
765	break;
766	case 3:
767	subtype = "Power Requestor";
768	break;
769	}
770	break;
771	case 5:
772	major = "Debug Logic";
773	switch (minor) {
774	case 0:
775	subtype = "other";
776	break;
777	case 1:
778	subtype = "Processor";
779	break;
780	case 2:
781	subtype = "DSP";
782	break;
783	case 3:
784	subtype = "Engine/Coprocessor";
785	break;
786	case 4:
787	subtype = "Bus";
788	break;
789	case 5:
790	subtype = "Memory";
791	}
792	break;
793	case 6:
794	major = "Perfomance Monitor";
795	switch (minor) {
796	case 0:
797	subtype = "other";
798	break;
799	case 1:
800	subtype = "Processor";
801	break;
802	case 2:
803	subtype = "DSP";
804	break;
805	case 3:
806	subtype = "Engine/Coprocessor";
807	break;
808	case 4:
809	subtype = "Bus";
810	break;
811	case 5:
812	subtype = "Memory";
813	break;
814	}
815	break;
816	}
817
818	rtems_debugger_printf(" Type: 0x%02" PRIx32 ", %s, %s\n",
819	table_type & 0xff, major, subtype);
820	rtems_debugger_printf(" PID[4..0]: %02x %02x %02x %02x %02x\n",
821	c_pidr[4], c_pidr[3], c_pidr[2], c_pidr[1], c_pidr[0]);
822
823	if (((c_cidr[1] >> 4) & 0x0f) == 1) {
824	arm_debug_dump_rom_table(c_base, depth + 1);
825	}
826	}
827	}
828	}
829
830	static int
831	arm_debug_rom_discover(uint32_t* rom, uint32_t comp, uint32_t** addr, int* index)
832	{
833	size_t r = 0;
834	*addr = 0;
835	while ((rom[r] & 1) != 0) {
836	uint32_t* c_base = (uint32_t*) ((intptr_t) rom + (rom[r] & 0xfffff000));
837	uint32_t c_cid1 = c_base[0xff4 / sizeof(uint32_t)];
838	uint32_t type;
839	if (((c_cid1 >> 4) & 0x0f) == 1) {
840	if (arm_debug_rom_discover(c_base, comp, addr, index))
841	return true;
842	}
843	type = c_base[0xfcc / sizeof(uint32_t)] & 0xff;
844	if (comp == type) {
845	if (*index > 0)
846	--(*index);
847	else {
848	*addr = c_base;
849	return true;
850	}
851	}
852	++r;
853	}
854	return false;
855	}
856
857	static int
858	arm_debug_mmap_enable(rtems_debugger_target* target, uint32_t dbgdidr)
859	{
860	uint32_t val;
861	int rc = -1;
862
863	#if ARM_CP15
864	void* abort_handler;
865	#endif
866
867	/*
868	* File scope as setjmp/longjmp effect the local stack variables.
869	*/
870	arm_debug_retries = 5;
871
872	/*
873	* The DBGDSAR (DSAR) is a signed offset from DBGDRAR. Both need to
874	* be valid for the debug register address to be valid. Read the
875	* DBGRAR first.
876	*/
877	ARM_CP14_READ(val, 1, 0, 0);
878	if ((val & 3) == 3) {
879	uint32_t* rom = (uint32_t*) (val & 0xfffff000);
880	uint32_t* comp_base = NULL;
881	int core = (int) _SMP_Get_current_processor();
882
883	if (ARM_DUMP_ROM_TABLES)
884	arm_debug_dump_rom_table(rom, 0);
885
886	debug_registers = NULL;
887
888	if (arm_debug_rom_discover(rom, 0x15, &comp_base, &core)) {
889	debug_registers = comp_base;
890	rtems_debugger_printf("rtems-db: ram debug: ROM Base: %p\n", comp_base);
891	} else {
892	ARM_CP14_READ(val, 2, 0, 0);
893	if ((val & 3) == 3 ) {
894	debug_registers = (void*) ((intptr_t) rom + (val & ~3));
895	}
896	}
897	}
898
899	if (debug_registers == NULL) {
900	debug_registers = rtems_debugger_arm_debug_registers();
901	if (debug_registers == NULL) {
902	rtems_debugger_printf("rtems-db: arm debug: no valid register map\n");
903	return -1;
904	}
905	rtems_debugger_printf("rtems-db: arm debug: BSP Base: %p\n", debug_registers);
906	}
907
908	/*
909	* Make sure the memory mapped registers return the same ID.
910	*/
911	if (ARM_MMAP_READ(0) != dbgdidr) {
912	debug_registers = NULL;
913	rtems_debugger_printf("rtems-db: arm debug: debug reg map not verified: " \
914	"0x%08x\n", ARM_MMAP_READ(0));
915	return -1;
916	}
917
918	if (!arm_debug_authentication(ARM_MMAP_READ(1006)))
919	return -1;
920
921	#if ARM_CP15
922	abort_handler =
923	arm_cp15_set_exception_handler(ARM_EXCEPTION_DATA_ABORT,
924	arm_debug_unlock_abort);
925	#endif
926
927	while (arm_debug_retries-- > 0) {
928	if (setjmp(unlock_abort_jmpbuf) == 0) {
929	/*
930	* If there is a software lock and it is locked unlock it.
931	*
932	* On the TI am335x this can cause a data abort which we catch and retry
933	* which seems to make the debug hardware work.
934	*/
935	if (ARM_MMAP_READ(1005) == 3) {
936	ARM_MMAP_WRITE_SYNC(1004, 0xC5ACCE55);
937	}
938	/*
939	* Are we already in debug mode?
940	*/
941	val = ARM_MMAP_READ(ARM_MMAP_DBGDSCR);
942	if ((val & (1 << 15)) == 0) {
943	rtems_debugger_printf("rtems-db: arm debug: enable debug mode\n");
944	val = ARM_MMAP_READ(ARM_MMAP_DBGDSCR);
945	ARM_MMAP_WRITE_SYNC(ARM_MMAP_DBGDSCR,
946	ARM_MMAP_READ(ARM_MMAP_DBGDSCR) \| (1 << 15));
947	arm_debug_retries = 0;
948	}
949	}
950	}
951
952	#if ARM_CP15
953	arm_cp15_set_exception_handler(ARM_EXCEPTION_DATA_ABORT, abort_handler);
954	#endif
955
956	if (arm_debug_retries > 0) {
957	rtems_debugger_printf("rtems-db: arm debug: using debug register access\n");
958	rc = 0;
959	}
960	else {
961	rtems_debugger_printf("rtems-db: arm debug: cannot enter debug mode\n");
962	}
963
964	val = ARM_MMAP_READ(1006);
965
966	return rc;
967	}
968
969	static int
970	arm_debug_probe(rtems_debugger_target* target)
971	{
972	#define ID_VALUE(_i, _h, _l) ((_i >> _l) & ((1 << ((_h - _l) + 1)) -1))
973	uint32_t val;
974	const char* vl = "[Invalid version]";
975	const char* const labels[] = {
976	"ARMv6 [v6]",
977	"ARMv6 [v6.1]",
978	"ARMv7 [v7, all CP14 registers]",
979	"ARMv7 [v7, baseline CP14 registers]",
980	"ARMv7 [v7.1]"
981	};
982	int rc = -1;
983
984	#if ARM_CP15
985	ARM_CP15_READ(val, 0, 0, 0, 0);
986	rtems_debugger_printf("rtems-db: arm core: Architecture: %d Variant: %d " \
987	"Implementor: %d Part Number: %d Revision: %d\n",
988	(val >> 16) & ((1 << (19 - 16 + 1)) - 1),
989	(val >> 20) & ((1 << (23 - 20 + 1)) - 1),
990	(val >> 24) & ((1 << (31 - 24 + 1)) - 1),
991	(val >> 4) & ((1 << (15 - 4 + 1)) - 1),
992	(val >> 0) & ((1 << ( 3 - 0 + 1)) - 1));
993	#endif
994
995	ARM_CP14_READ(val, 0, 0, 0);
996
997	debug_version = ID_VALUE(val, 19, 16);
998	if (debug_version < 1 \|\| debug_version > 5) {
999	rtems_debugger_printf("rtems-db: arm debug: (v%d.%d) not supported\n",
1000	debug_version, debug_revision);
1001	errno = EIO;
1002	return -1;
1003	}
1004
1005	vl = labels[debug_version - 1];
1006	debug_revision = ID_VALUE(val, 3, 0);
1007	hw_breakpoints = ID_VALUE(val, 27, 24);
1008	hw_watchpoints = ID_VALUE(val, 31, 28);
1009
1010	rtems_debugger_printf("rtems-db: arm debug: (v%d.%d) %s " \
1011	"breakpoints:%d watchpoints:%d\n",
1012	debug_version, debug_revision, vl,
1013	hw_breakpoints, hw_watchpoints);
1014
1015	if (!rtems_debugger_arm_debug_configure())
1016	return -1;
1017
1018	switch (debug_version) {
1019	case 1:
1020	case 2:
1021	case 3:
1022	case 5:
1023	default:
1024	rc = arm_debug_mmap_enable(target, val);
1025	if (rc != 0)
1026	rc = arm_debug_cp14_enable(target);
1027	break;
1028	case 4:
1029	rc = arm_debug_mmap_enable(target, val);
1030	break;
1031	}
1032
1033	return rc;
1034	}
1035
1036	static inline void
1037	arm_debug_break_setup(int bp,
1038	uint32_t address,
1039	uint32_t type,
1040	uint32_t byte_address_select,
1041	uint32_t privilege)
1042	{
1043	ARM_HWB_BCR(bp) = (((type & 0xf) << 20) \|
1044	((byte_address_select & 0xf) << 5) \|
1045	((privilege & 0x3) << 1) \| 1);
1046	ARM_HWB_VCR(bp) = (intptr_t) (address & (~3));
1047	}
1048
1049	static void
1050	arm_debug_break_c14_write_control(int bp, uint32_t control)
1051	{
1052	switch (bp) {
1053	case 0:
1054	ARM_CP14_WRITE(control, 0, 0, 5);
1055	break;
1056	case 1:
1057	ARM_CP14_WRITE(control, 0, 1, 5);
1058	break;
1059	case 2:
1060	ARM_CP14_WRITE(control, 0, 2, 5);
1061	break;
1062	case 3:
1063	ARM_CP14_WRITE(control, 0, 3, 5);
1064	break;
1065	case 4:
1066	ARM_CP14_WRITE(control, 0, 4, 5);
1067	break;
1068	case 5:
1069	ARM_CP14_WRITE(control, 0, 5, 5);
1070	break;
1071	case 6:
1072	ARM_CP14_WRITE(control, 0, 6, 5);
1073	break;
1074	case 7:
1075	ARM_CP14_WRITE(control, 0, 7, 5);
1076	break;
1077	case 8:
1078	ARM_CP14_WRITE(control, 0, 8, 5);
1079	break;
1080	case 9:
1081	ARM_CP14_WRITE(control, 0, 9, 5);
1082	break;
1083	case 10:
1084	ARM_CP14_WRITE(control, 0, 10, 5);
1085	break;
1086	case 11:
1087	ARM_CP14_WRITE(control, 0, 11, 5);
1088	break;
1089	case 12:
1090	ARM_CP14_WRITE(control, 0, 12, 5);
1091	break;
1092	case 13:
1093	ARM_CP14_WRITE(control, 0, 13, 5);
1094	break;
1095	case 14:
1096	ARM_CP14_WRITE(control, 0, 14, 5);
1097	break;
1098	case 15:
1099	ARM_CP14_WRITE(control, 0, 15, 5);
1100	break;
1101	}
1102	}
1103
1104	static void
1105	arm_debug_break_c14_write_value(int bp, uint32_t value)
1106	{
1107	switch (bp) {
1108	case 0:
1109	ARM_CP14_WRITE(value, 0, 0, 4);
1110	break;
1111	case 1:
1112	ARM_CP14_WRITE(value, 0, 1, 4);
1113	break;
1114	case 2:
1115	ARM_CP14_WRITE(value, 0, 2, 4);
1116	break;
1117	case 3:
1118	ARM_CP14_WRITE(value, 0, 3, 4);
1119	break;
1120	case 4:
1121	ARM_CP14_WRITE(value, 0, 4, 4);
1122	break;
1123	case 5:
1124	ARM_CP14_WRITE(value, 0, 5, 4);
1125	break;
1126	case 6:
1127	ARM_CP14_WRITE(value, 0, 6, 4);
1128	break;
1129	case 7:
1130	ARM_CP14_WRITE(value, 0, 7, 4);
1131	break;
1132	case 8:
1133	ARM_CP14_WRITE(value, 0, 8, 4);
1134	break;
1135	case 9:
1136	ARM_CP14_WRITE(value, 0, 9, 4);
1137	break;
1138	case 10:
1139	ARM_CP14_WRITE(value, 0, 10, 4);
1140	break;
1141	case 11:
1142	ARM_CP14_WRITE(value, 0, 11, 4);
1143	break;
1144	case 12:
1145	ARM_CP14_WRITE(value, 0, 12, 4);
1146	break;
1147	case 13:
1148	ARM_CP14_WRITE(value, 0, 13, 4);
1149	break;
1150	case 14:
1151	ARM_CP14_WRITE(value, 0, 14, 4);
1152	break;
1153	case 15:
1154	ARM_CP14_WRITE(value, 0, 15, 4);
1155	break;
1156	}
1157	}
1158
1159	static uint32_t
1160	arm_debug_dbgdscr_read(void)
1161	{
1162	uint32_t val;
1163	if (debug_registers != NULL) {
1164	val = ARM_MMAP_READ(ARM_MMAP_DBGDSCR);
1165	}
1166	else {
1167	ARM_CP14_READ(val, 0, 1, 0);
1168	}
1169	return val;
1170	}
1171
1172	static void
1173	arm_debug_dbgdscr_write(uint32_t val)
1174	{
1175	if (debug_registers != NULL) {
1176	ARM_MMAP_WRITE_SYNC(ARM_MMAP_DBGDSCR, val);
1177	}
1178	else {
1179	ARM_CP14_WRITE(val, 0, 1, 0);
1180	}
1181	}
1182
1183	static uint32_t
1184	arm_debug_method_of_entry(void)
1185	{
1186	return (arm_debug_dbgdscr_read() >> 2) & 0xf;
1187	}
1188
1189	static void
1190	arm_debug_disable_interrupts(void)
1191	{
1192	debug_disable_ints = 1;
1193	}
1194
1195	static void
1196	arm_debug_enable_interrupts(void)
1197	{
1198	arm_debug_dbgdscr_write(arm_debug_dbgdscr_read() & ~(1 << 11));
1199	}
1200
1201	static void
1202	arm_debug_break_clear(int bp)
1203	{
1204	rtems_interrupt_lock_context lock_context;
1205	rtems_interrupt_lock_acquire(&target_lock, &lock_context);
1206	ARM_HWB_CLEAR(bp);
1207	rtems_interrupt_lock_release(&target_lock, &lock_context);
1208	}
1209
1210	static void
1211	arm_debug_break_clear_all(void)
1212	{
1213	rtems_interrupt_lock_context lock_context;
1214	rtems_interrupt_lock_acquire(&target_lock, &lock_context);
1215	ARM_HWB_CLEAR_ALL();
1216	rtems_interrupt_lock_release(&target_lock, &lock_context);
1217	}
1218
1219	static inline void
1220	arm_debug_set_context_id(const uint32_t id)
1221	{
1222	#if ARM_CP15
1223	ARM_CP15_WRITE(id, 0, 13, 0, 1);
1224	#endif
1225	}
1226
1227	static void
1228	arm_debug_break_unload(void)
1229	{
1230	rtems_interrupt_lock_context lock_context;
1231	int i;
1232	rtems_interrupt_lock_acquire(&target_lock, &lock_context);
1233	if (debug_registers != NULL) {
1234	for (i = 0; i < hw_breakpoints; ++i) {
1235	ARM_MMAP_WRITE(ARM_MMAP_DBGBCR + i, 0);
1236	ARM_MMAP_WRITE(ARM_MMAP_DBGBVR + i, 0);
1237	}
1238	} else {
1239	for (i = 0; i < hw_breakpoints; ++i) {
1240	arm_debug_break_c14_write_control(i, 0);
1241	arm_debug_break_c14_write_value(i, 0);
1242	}
1243	}
1244	rtems_interrupt_lock_release(&target_lock, &lock_context);
1245	}
1246
1247	static void
1248	arm_debug_break_exec_enable(int bp, uintptr_t addr, bool thumb, bool step) {
1249	uint32_t bas;
1250
1251	/*
1252	* See table C3-2 Effect of byte address selection on Breakpoint
1253	* generation and "Instruction address comparision programming
1254	* examples.
1255	*/
1256	if (thumb) {
1257	/*
1258	* Thumb
1259	*/
1260	if ((addr & (1 << 1)) == 0) {
1261	/*
1262	* Instruction address: DBGBVR[31:2]:00 BAS: 0bxx11 Mismatch: Miss
1263	*/
1264	bas = 0x3; /* bxx11 */
1265	}
1266	else {
1267	/*
1268	* Instruction address: DBGBVR[31:2]:10 BAS: 0b11xx Mismatch: Miss
1269	*/
1270	bas = 0xc; /* b11xx */
1271	}
1272	}
1273	else {
1274	/*
1275	* ARM
1276	*
1277	* Instruction address: DBGBVR[31:2]:00 BAS: 0b1111 Mismatch: Miss
1278	*/
1279	bas = 0xf; /* b1111 */
1280	}
1281
1282	target_printk("[} break: addr:%08x bas:%x thumb:%s\n",
1283	addr, bas, thumb ? "yes" : "no");
1284
1285	arm_debug_break_setup(
1286	bp,
1287	addr,
1288	step ? ARM_HW_BP_UNLINKED_INSTR_MISMATCH : ARM_HW_BP_UNLINKED_INSTR_MATCH,
1289	bas,
1290	ARM_HW_BP_PRIV_PL0_SUP_SYS);
1291	}
1292
1293	static void
1294	arm_debug_break_dump(void)
1295	{
1296	#if TARGET_DEBUG
1297	int i;
1298	for (i = 0; i < hw_breakpoints; ++i) {
1299	if (ARM_HWB_ENALBED(i)) {
1300	target_printk("[} bp: %d: control: %08x addr: %08x\n",
1301	i, ARM_HWB_BCR(i), ARM_HWB_VCR(i));
1302	}
1303	}
1304	#endif
1305	}
1306
1307	#if NOT_USED_BUT_KEEPING
1308	static size_t
1309	arm_debug_break_length(void* pc)
1310	{
1311	arm_debug_hwbreak* bp = &hw_breaks[0];
1312	int i;
1313
1314	for (i = 0; i < hw_breakpoints; ++i, ++bp) {
1315	if (bp->enabled && bp->address == pc) {
1316	return bp->length;
1317	}
1318	}
1319	return sizeof(DB_UINT);
1320	}
1321	#endif
1322
1323	int
1324	rtems_debugger_target_configure(rtems_debugger_target* target)
1325	{
1326	target->capabilities = (RTEMS_DEBUGGER_TARGET_CAP_SWBREAK);
1327	target->reg_num = RTEMS_DEBUGGER_NUMREGS;
1328	target->reg_offset = arm_reg_offsets;
1329	target->breakpoint = &breakpoint[0];
1330	target->breakpoint_size = sizeof(breakpoint);
1331	return arm_debug_probe(target);
1332	}
1333
1334	static void
1335	target_print_frame(CPU_Exception_frame* frame)
1336	{
1337	EXC_FRAME_PRINT(target_printk, "[} ", frame);
1338	}
1339
1340	static const size_t
1341	target_exc_offset(CPU_Exception_frame* frame)
1342	{
1343	size_t thumb = (FRAME_SR(frame) & (1 << 5)) == 0 ? 0 : 1;
1344	return exc_offsets[thumb][frame->vector];
1345	}
1346
1347	static void
1348	target_exception(CPU_Exception_frame* frame)
1349	{
1350	#if TARGET_DEBUG
1351	#if ARM_CP15
1352	const uint32_t ifsr = arm_cp15_get_instruction_fault_status();
1353	const uint32_t dfsr = arm_cp15_get_data_fault_status();
1354	const void* far = arm_cp15_get_fault_address();
1355	#else
1356	const uint32_t ifsr = 0;
1357	#endif
1358	const uint32_t mvector = frame->vector;
1359	const uint32_t dbgdscr = arm_debug_dbgdscr_read();
1360	#endif
1361
1362	const uint32_t moe = arm_debug_method_of_entry();
1363	const size_t exc_offset = target_exc_offset(frame);
1364
1365	switch (moe){
1366	case ARM_HW_DSCR_MOE_BREAKPOINT_EVENT:
1367	case ARM_HW_DSCR_MOE_BREAKPOINT_INSTR:
1368	case ARM_HW_DSCR_MOE_ASYNC_WATCHPOINT:
1369	case ARM_HW_DSCR_MOE_SYNC_WATCHPOINT:
1370	frame->vector = 2;
1371	break;
1372	case ARM_HW_DSCR_MOE_HALT_REQUEST:
1373	case ARM_HW_DSCR_MOE_EXTERNAL:
1374	case ARM_HW_DSCR_MOE_VECTOR_CATCH_EVENT:
1375	case ARM_HW_DSCR_MOE_OS_UNLOCK_EVENT:
1376	default:
1377	break;
1378	}
1379
1380	target_printk("[} > frame = %08" PRIx32 \
1381	" sig=%d vector=%u (%u) dbgdscr=%08" PRIx32 " moe=%s" \
1382	" far=%p ifsr=%08" PRIx32 " dfsr=%08" PRIx32
1383	" exc-ret-pc=%08x\n", (uint32_t) frame,
1384	rtems_debugger_target_exception_to_signal(frame),
1385	frame->vector, mvector, dbgdscr, arm_moe_label(moe),
1386	far, ifsr, dfsr, (intptr_t) frame->register_pc);
1387
1388	arm_debug_break_dump();
1389
1390	frame->register_pc = (void*) ((intptr_t) frame->register_pc - exc_offset);
1391
1392	target_print_frame(frame);
1393
1394	arm_debug_break_clear(0);
1395	arm_debug_enable_interrupts();
1396
1397	if (!debug_session_active)
1398	_ARM_Exception_default(frame);
1399
1400	switch (rtems_debugger_target_exception(frame)) {
1401	case rtems_debugger_target_exc_consumed:
1402	default:
1403	break;
1404	case rtems_debugger_target_exc_step:
1405	break;
1406	case rtems_debugger_target_exc_cascade:
1407	target_printk("rtems-db: unhandled exception: cascading\n");
1408	_ARM_Exception_default(frame);
1409	break;
1410	}
1411
1412	target_printk("[} < resuming frame = %08" PRIx32 \
1413	" PC = %08" PRIxPTR " CPSR = %08" PRIx32 "\n",
1414	(uint32_t) frame, (intptr_t) frame->register_pc, FRAME_SR(frame));
1415	target_print_frame(frame);
1416	}
1417
1418	/**
1419	* Exception Handlers
1420	*
1421	* The entry and exit is all assembler and ARM code. This avoids any
1422	* compiler related optimisations effecting the various pieces.
1423	*/
1424
1425	/**
1426	* Exception stack frame size.
1427	*
1428	* The size is the exception stack frame plus the CPSR from the exception. We
1429	* save the CPSR and restore it when we exit the exception.
1430	*/
1431	#define EXCEPTION_FRAME_SIZE (sizeof(CPU_Exception_frame) + sizeof(uint32_t))
1432
1433	/**
1434	* Exception stack frame FPU offsets and sizes.
1435	*/
1436	#define EXCEPTION_FRAME_FPU_SIZE ARM_VFP_CONTEXT_SIZE
1437	#define EXCEPTION_FRAME_FPU_OFFSET ARM_EXCEPTION_FRAME_VFP_CONTEXT_OFFSET
1438
1439	/**
1440	* Exception entry.
1441	*
1442	* We have switched from svc (or even user) to an exception mode. Save the
1443	* current CPSR and create an exception frame on the exception's stack and then
1444	* copy it to the thread's stack. Switch back to the thread's context and mode
1445	* to handle the exception to avoid any stack checks thinking the stack is
1446	* blown. This lets the thread be suspended.
1447	*
1448	* The entry is in two parts, the exception mode entry and the trhead mode
1449	* entry. This lets us disable any hardware breakpoint support. We need to do
1450	* this because it is enabled in PL0 mode.
1451	*
1452	* Note, the code currently assumes cp15 has been set up to match the
1453	* instruction set being used.
1454	*/
1455	#define EXCEPTION_ENTRY_EXC() \
1456	__asm__ volatile( \
1457	ASM_ARM_MODE /* force ARM mode for thumb systems */ \
1458	"sub sp, %[frame_size]\n" /* alloc the frame and CPSR */ \
1459	"stm sp, {r0-r12}\n" /* store r0-r12 */ \
1460	: \
1461	: [frame_size] "i" (EXCEPTION_FRAME_SIZE) \
1462	: "cc", "memory")
1463
1464	/**
1465	* Debugger entry
1466	*
1467	* Check if using debug registers else use CP14.
1468	*
1469	* Set all the break point registers to 0. Enable interrupts.
1470	*/
1471	#if ARM_CP15
1472	#define ARM_HW_BP_UNLOAD(_bp) \
1473	"cmp r0, #" #_bp "\n" \
1474	"ble 3f\n" \
1475	"mcr p14, 0, r1, c0, c" #_bp ", 5\n" \
1476	"mcr p14, 0, r1, c0, c" #_bp ", 4\n"
1477	#define ARM_DGB_ENABLE_INTS \
1478	"mrc p14, 0, r1, c0, c1, 0\n" /* Get the DBGDSCR */ \
1479	"bic r1, r1, #(1 << 11)\n" /* enable interrupts */ \
1480	"mcr p14, 0, r1, c0, c1, 0\n" /* Set the DBGDSCR */
1481	#else
1482	#define ARM_HW_BP_UNLOAD(_bp)
1483	#define ARM_DGB_ENABLE_INTS
1484	#endif
1485
1486	#define EXCEPTION_ENTRY_DEBUGGER() \
1487	__asm__ volatile( \
1488	/* Set up r0 and r1 */ \
1489	"movw r0, #:lower16:hw_breakpoints\n" /* get the num hw breaks */ \
1490	"movt r0, #:upper16:hw_breakpoints\n" \
1491	"ldr r0, [r0]\n" /* r0 = hw_breakpoints */ \
1492	"mov r1, #0\n" /* write zero */ \
1493	/* Check if debug registers are being used */ \
1494	"movw r2, #:lower16:debug_registers\n" /* get the debug regs */ \
1495	"movt r2, #:upper16:debug_registers\n" \
1496	"ldr r2, [r2]\n" /* r2 = debug_registers */ \
1497	"cmp r2, #0\n" /* NULL? */ \
1498	"beq 2f\n" /* if NULL use cp14 */ \
1499	/* Debug registers */ \
1500	"add r3, r2, %[dbgbvr] - 4\n" /* a3 = DBGBCR0, adjusted */ \
1501	"add r2, r2, %[dbgbcr] - 4\n" /* a2 = DBGBVR0, adjusted */ \
1502	"1:\n" \
1503	"str r1, [r3, #4]!\n" /* Store DBGBVR, pre-indexed, modified */ \
1504	"str r1, [r2, #4]!\n" /* Store DBGBCR, pre-indexed, modified */ \
1505	"sub r0, r0, #1\n" /* one less */ \
1506	"cmp r0, #0\n" /* all done? */ \
1507	"bne 1b\n" \
1508	"ldr r1, [r2, %[dbgdscr]]\n" /* Get the DBGDSCR */ \
1509	"bic r1, r1, #(1 << 11)\n" /* enable interrupts */ \
1510	"str r1, [r2, %[dbgdscr]]\n" /* Set the DBGDSCR */ \
1511	"b 4f\n" \
1512	/* CP14 */ \
1513	"2:\n" \
1514	ARM_HW_BP_UNLOAD(0) \
1515	ARM_HW_BP_UNLOAD(1) \
1516	ARM_HW_BP_UNLOAD(2) \
1517	ARM_HW_BP_UNLOAD(3) \
1518	ARM_HW_BP_UNLOAD(4) \
1519	ARM_HW_BP_UNLOAD(5) \
1520	ARM_HW_BP_UNLOAD(6) \
1521	ARM_HW_BP_UNLOAD(7) \
1522	ARM_HW_BP_UNLOAD(8) \
1523	ARM_HW_BP_UNLOAD(9) \
1524	ARM_HW_BP_UNLOAD(10) \
1525	ARM_HW_BP_UNLOAD(11) \
1526	ARM_HW_BP_UNLOAD(12) \
1527	ARM_HW_BP_UNLOAD(12) \
1528	ARM_HW_BP_UNLOAD(13) \
1529	ARM_HW_BP_UNLOAD(14) \
1530	ARM_HW_BP_UNLOAD(15) \
1531	"3:\n" \
1532	ARM_DGB_ENABLE_INTS \
1533	"4:\n" \
1534	ARM_SYNC_INST \
1535	: \
1536	: [dbgdscr] "i" (ARM_MMAP_DBGDSCR * sizeof(uint32_t)), \
1537	[dbgbcr] "i" (ARM_MMAP_DBGBCR * sizeof(uint32_t)), \
1538	[dbgbvr] "i" (ARM_MMAP_DBGBVR * sizeof(uint32_t)) \
1539	: "cc", "r0", "r1", "r2", "r3", "memory")
1540
1541	/*
1542	* FPU entry. Conditionally D16 or D32 support.
1543	*/
1544	#ifdef ARM_MULTILIB_VFP
1545	#ifdef ARM_MULTILIB_VFP_D32
1546	#define FPU_ENTRY_VFP_D32 \
1547	"vstmia r5!, {d16-d31}\n"
1548	#else /* ARM_MULTILIB_VFP_D32 */
1549	#define FPU_ENTRY_VFP_D32 \
1550	"mov r3, #0\n" \
1551	"mov r4, #0\n" \
1552	"adds r6, r5, #128\n" \
1553	"1:\n" \
1554	"stmia r5!, {r3-r4}\n" \
1555	"cmp r5, r6\n" \
1556	"bne 1b\n"
1557	#endif /* ARM_MULTILIB_VFP_D32 */
1558	#define EXCEPTION_ENTRY_FPU(frame_fpu_size) \
1559	"sub sp, %[frame_fpu_size]\n" /* size includes alignment size */ \
1560	"add r5, sp, #4\n" /* up to align down */ \
1561	"bic r5, r5, #7\n" /* align the FPU frame */ \
1562	"str r5, [r2]\n" /* store the FPU frame pointer */ \
1563	"vmrs r3, FPEXC\n" \
1564	"vmrs r4, FPSCR\n" \
1565	"stmia r5!, {r3-r4}\n" \
1566	"vstmia r5!, {d0-d15}\n" \
1567	FPU_ENTRY_VFP_D32
1568	#else /* ARM_MULTILIB_VFP */
1569	#define EXCEPTION_ENTRY_FPU(frame_fpu_size)
1570	#endif /* ARM_MULTILIB_VFP */
1571
1572	#define ARM_CLEAR_THUMB_MODE "bic r1, r1, %[psr_t]\n" /* clear thumb */
1573
1574	#define EXCEPTION_ENTRY_THREAD(_frame) \
1575	__asm__ volatile( \
1576	"add r0, sp, %[r0_r12_size]\n" /* get the sp in the frame */ \
1577	"mrs r1, spsr\n" /* get the saved sr */ \
1578	"mov r6, r1\n" /* stash it for later */ \
1579	ARM_CLEAR_THUMB_MODE /* clear thumb mode */ \
1580	"orr r1, r1, %[psr_i]\n" /* mask irqs */ \
1581	"mrs r2, cpsr\n" /* get the current sr */ \
1582	"str r2, [sp, %[frame_cpsr]]\n" /* save for exc return */ \
1583	"msr cpsr, r1\n" /* switch to user mode */ \
1584	"mov r3, sp\n" /* get the stack pointer */ \
1585	"mov r4, lr\n" /* get the link reg */ \
1586	"msr cpsr, r2\n" /* back to exc mode */ \
1587	"mov r5, lr\n" /* get the PRA */ \
1588	"stm r0, {r3-r6}\n" /* save into the frame: sp,lr,pc,cpsr */ \
1589	"sub r4, r3, %[frame_size]\n" /* destination address */ \
1590	"mov r6, r4\n" /* save the frame */ \
1591	"sub r4, #1\n" /* one before the start */ \
1592	"add r3, #1\n" /* one past the end */ \
1593	"sub r5, sp, #1\n" /* source address */ \
1594	"1:\n" \
1595	"ldrb r0, [r5, #1]!\n" /* get a byte */ \
1596	"strb r0, [r4, #1]!\n" /* put the byte */ \
1597	"cmp r3, r4\n" /* the end? */ \
1598	"bne 1b\n" \
1599	"add sp, %[frame_size]\n" /* free the frame and CPSR */ \
1600	"mrs r1, spsr\n" /* get the thread's saved sr */ \
1601	"orr r2, r1, %[psr_i]\n" /* mask irqs */ \
1602	"msr cpsr, r2\n" /* switch back to the thread's context */ \
1603	"sub sp, %[frame_size]\n" /* alloc in the thread stack */ \
1604	"mov %[o_frame], sp\n" /* save the frame */ \
1605	"add r2, sp, %[o_frame_fpu]\n" /* get the FPU offset */ \
1606	"mov r3, #0\n" \
1607	"str r3, [r2]\n" /* clear the FPU frame pointer */ \
1608	EXCEPTION_ENTRY_FPU(frame_fpu_size) \
1609	"bic r1, r1, %[psr_i]\n" /* clear irq mask, debug checks */ \
1610	"msr cpsr, r1\n" /* restore the state with irq mask clear */ \
1611	: \
1612	[o_frame] "=r" (_frame) \
1613	: [psr_t] "i" (ARM_PSR_T), \
1614	[psr_i] "i" (ARM_PSR_I), \
1615	[r0_r12_size] "i" (13 * sizeof(uint32_t)), \
1616	[frame_cpsr] "i" (EXCEPTION_FRAME_SIZE - sizeof(uint32_t)), \
1617	[frame_size] "i" (EXCEPTION_FRAME_SIZE), \
1618	[o_frame_fpu] "i" (EXCEPTION_FRAME_FPU_OFFSET), \
1619	[frame_fpu_size] "i" (EXCEPTION_FRAME_FPU_SIZE + 4) \
1620	: "cc", "r0", "r1", "r2", "r3", "r4", "r5", "r6", "memory")
1621
1622	/*
1623	* FPU exit. Conditionally D16 or D32 support.
1624	*/
1625	#ifdef ARM_MULTILIB_VFP
1626	#ifdef ARM_MULTILIB_VFP_D32
1627	#define FPU_EXIT_VFP_D32 \
1628	"vldmia r0, {d16-d31}\n"
1629	#else /* ARM_MULTILIB_VFP_D32 */
1630	#define FPU_EXIT_VFP_D32
1631	#endif /* ARM_MULTILIB_VFP_D32 */
1632	#define EXCEPTION_EXIT_FPU(frame_fpu_size) \
1633	"ldmia r0!, {r1-r2}\n" \
1634	"vldmia r0!, {d0-d15}\n" \
1635	FPU_EXIT_VFP_D32 \
1636	"vmsr FPEXC, r1\n" \
1637	"vmsr FPSCR, r2\n" \
1638	"add sp, %[frame_fpu_size]\n" /* size includes alignment size */
1639	#else /* ARM_MULTILIB_VFP */
1640	#define EXCEPTION_EXIT_FPU(frame_fpu_size)
1641	#endif /* ARM_MULTILIB_VFP */
1642
1643	/**
1644	* Exception exit.
1645	*
1646	* The thread is to be resumed so we are still in the thread's mode. Copy the
1647	* exception frame from the thread's stack back to the exception's stack and
1648	* restore the thread's context before returning from the exception to the
1649	* thread.
1650	*
1651	* Note, the code currently assumes cp15 has been set up to match the
1652	* instruction set being used.
1653	*/
1654	#define EXCEPTION_EXIT_THREAD(_frame) \
1655	__asm__ volatile( \
1656	"mov r0, %[i_frame]\n" /* get the frame */ \
1657	"ldr r0, [r0, %[frame_fpu]]\n" /* recover aligned FPU frame ptr */ \
1658	EXCEPTION_EXIT_FPU(frame_fpu_size) \
1659	"ldr r2, [sp, %[frame_cpsr]]\n" /* recover exc CPSR from thread */ \
1660	"mov r0, sp\n" /* get the thread frame pointer */ \
1661	"msr cpsr, r2\n" /* switch back to the exc's context */ \
1662	"add r3, sp, #1\n" /* get the end */ \
1663	"sub sp, %[frame_size]\n" /* alloc the frame */ \
1664	"sub r4, sp, #1\n" /* destination address */ \
1665	"sub r5, r0, #1\n" /* source address */ \
1666	"1:\n" \
1667	"ldrb r1, [r5, #1]!\n" /* get a byte */ \
1668	"strb r1, [r4, #1]!\n" /* put the byte */ \
1669	"cmp r3, r4\n" /* the end? */ \
1670	"bne 1b\n" \
1671	"mov r0, %[i_frame]\n" /* get the frame */ \
1672	"add r1, r0, %[r0_r12_size]\n" /* get the sp in the frame */ \
1673	"ldm r1, {r3-r6}\n" /* recover sp, lr, pc, cpsr */ \
1674	"orr r1, r6, %[psr_i]\n" /* get the thread's psr and mask irqs */ \
1675	"msr cpsr, r1\n" /* switch to user mode */ \
1676	"mov sp, r3\n" /* set the stack pointer */ \
1677	"mov lr, r4\n" /* set the link reg */ \
1678	"msr cpsr, r2\n" /* switch back to the exc's context */ \
1679	"msr spsr, r6\n" /* set the thread's CPSR */ \
1680	"mov lr, r5\n" /* get the PC */ \
1681	: \
1682	: [psr_i] "i" (ARM_PSR_I), \
1683	[r0_r12_size] "i" (13 * sizeof(uint32_t)), \
1684	[frame_cpsr] "i" (EXCEPTION_FRAME_SIZE - sizeof(uint32_t)), \
1685	[frame_size] "i" (EXCEPTION_FRAME_SIZE), \
1686	[frame_fpu] "i" (EXCEPTION_FRAME_FPU_OFFSET), \
1687	[frame_fpu_size] "i" (EXCEPTION_FRAME_FPU_SIZE + 4), \
1688	[i_frame] "r" (_frame) \
1689	: "cc", "r0", "r1", "r2", "r3", "r4", "r5", "r6", "memory")
1690
1691	/*
1692	* Debugger exit
1693	*
1694	* Check if using debug registers else use CP14.
1695	*
1696	* Set all the break point registers to settgins. Disable interrupts
1697	* if debug_disable_ints is true. Clear debug_disable_ints.
1698	*/
1699	#if ARM_CP15
1700	#define ARM_HW_BP_LOAD(_bp) \
1701	"cmp r0, #" #_bp "\n" \
1702	"ble 5f\n" \
1703	"ldm r1!, {r2-r3}\n" \
1704	"mcr p14, 0, r2, c0, c" #_bp ", 4\n" /* value */ \
1705	"mcr p14, 0, r3, c0, c" #_bp ", 5\n" /* control */
1706	#define ARM_DGB_DISABLE_INTS \
1707	"mrc p14, 0, r4, c0, c1, 0\n" /* Get the DBGDSCR */ \
1708	"orr r4, r4, #(1 << 11)\n" /* disable interrupts */ \
1709	"mcr p14, 0, r4, c0, c1, 0\n" /* Set the DBGDSCR */
1710	#else
1711	#define ARM_HW_BP_LOAD(_bp)
1712	#define ARM_DGB_DISABLE_INTS
1713	#endif
1714
1715	#define EXCEPTION_EXIT_DEBUGGER() \
1716	__asm__ volatile( \
1717	/* Set up r0, r1, r4 and r5 */ \
1718	"movw r0, #:lower16:hw_breakpoints\n" /* get the num hw breaks */ \
1719	"movt r0, #:upper16:hw_breakpoints\n" \
1720	"ldr r0, [r0]\n" /* r0 = hw_breakpoints */ \
1721	"movw r1, #:lower16:hw_breaks\n" /* get the hw_breaks pointer */ \
1722	"movt r1, #:upper16:hw_breaks\n" \
1723	"movw r4, #:lower16:debug_disable_ints\n" /* get disable ints */ \
1724	"movt r4, #:upper16:debug_disable_ints\n" \
1725	"ldr r5, [r4]\n" \
1726	"mov r3, #0\n" /* clear debug ints */ \
1727	"str r3, [r4]\n" \
1728	/* Check if debug registers are being used */ \
1729	"movw r2, #:lower16:debug_registers\n" /* get the debug regs */ \
1730	"movt r2, #:upper16:debug_registers\n" \
1731	"ldr r2, [r2]\n" /* r2 = debug_registers */ \
1732	"cmp r2, #0\n" /* NULL? */ \
1733	"beq 3f\n" /* if NULL use cp14 */ \
1734	/* Debug registers */ \
1735	"cmp r5, #0\n" /* false? */ \
1736	"beq 1f\n" /* if false do not set ints disable */ \
1737	"ldr r4, [r2, %[dbgdscr]]\n" /* Get the DBGDSCR */ \
1738	"orr r4, r4, #(1 << 11)\n" /* disable interrupts */ \
1739	"str r4, [r2, %[dbgdscr]]\n" /* Set the DBGDSCR */ \
1740	"1:\n" \
1741	"add r3, r2, %[dbgbvr] - 4\n" /* a3 = DBGBCR0, adjusted */ \
1742	"add r2, r2, %[dbgbcr] - 4\n" /* a2 = DBGBVR0, adjusted */ \
1743	"2:\n" \
1744	"ldm r1!, {r4-r5}\n" /* load vr and cr */ \
1745	"str r4, [r3, #4]!\n" /* Store DBGBVR, pre-indexed, modified */ \
1746	"str r5, [r2, #4]!\n" /* Store DBGBCR, pre-indexed, modified */ \
1747	"sub r0, r0, #1\n" /* one less? */ \
1748	"cmp r0, #1\n" /* all done? */ \
1749	"bne 2b\n" \
1750	"b 5f\n" \
1751	/* CP14 */ \
1752	"3:\n" \
1753	"cmp r5, #0\n" /* false? */ \
1754	"beq 4f\n" /* if false do not set ints disable */ \
1755	ARM_DGB_DISABLE_INTS \
1756	"4:\n" \
1757	ARM_HW_BP_LOAD(0) \
1758	ARM_HW_BP_LOAD(1) \
1759	ARM_HW_BP_LOAD(2) \
1760	ARM_HW_BP_LOAD(3) \
1761	ARM_HW_BP_LOAD(4) \
1762	ARM_HW_BP_LOAD(5) \
1763	ARM_HW_BP_LOAD(6) \
1764	ARM_HW_BP_LOAD(7) \
1765	ARM_HW_BP_LOAD(8) \
1766	ARM_HW_BP_LOAD(9) \
1767	ARM_HW_BP_LOAD(10) \
1768	ARM_HW_BP_LOAD(11) \
1769	ARM_HW_BP_LOAD(12) \
1770	ARM_HW_BP_LOAD(13) \
1771	ARM_HW_BP_LOAD(14) \
1772	ARM_HW_BP_LOAD(15) \
1773	"5:\n" \
1774	ARM_SYNC_INST \
1775	: \
1776	: [disints] "X" (debug_disable_ints), /* make the sym available */ \
1777	[dbgdscr] "i" (ARM_MMAP_DBGDSCR * sizeof(uint32_t)), \
1778	[dbgbcr] "i" (ARM_MMAP_DBGBCR * sizeof(uint32_t)), \
1779	[dbgbvr] "i" (ARM_MMAP_DBGBVR * sizeof(uint32_t)) \
1780	: "cc", "r0", "r1", "r2", "r3", "r4", "r5", "memory")
1781
1782	#define EXCEPTION_EXIT_EXC() \
1783	__asm__ volatile( \
1784	"ldm sp, {r0-r12}\n" /* restore the thread's context */ \
1785	"add sp, %[frame_size]\n" /* free the frame */ \
1786	"subs pc, lr, #0\n" /* return from the exc */ \
1787	ARM_SYNC_INST \
1788	: \
1789	: [frame_size] "i" (EXCEPTION_FRAME_SIZE) \
1790	: "cc", "memory")
1791
1792	#define ARM_PUSH_LR() \
1793	__asm__ volatile( \
1794	"push {lr}\n" \
1795	: : : )
1796
1797	#define ARM_POP_LR() \
1798	__asm__ volatile( \
1799	"pop {lr}\n" \
1800	: : : )
1801
1802	/*
1803	* Entry and exit stacks
1804	*/
1805	#define EXCEPTION_ENTRY(_frame) \
1806	EXCEPTION_ENTRY_EXC(); \
1807	EXCEPTION_ENTRY_DEBUGGER(); \
1808	EXCEPTION_ENTRY_THREAD(_frame); \
1809	ARM_THUMB_MODE() \
1810	ARM_PUSH_LR()
1811
1812	#define EXCEPTION_EXIT(_frame) \
1813	ARM_POP_LR(); \
1814	ARM_ARM_MODE(); \
1815	EXCEPTION_EXIT_THREAD(_frame); \
1816	EXCEPTION_EXIT_DEBUGGER(); \
1817	EXCEPTION_EXIT_EXC()
1818
1819	/*
1820	* This is used to catch faulting accesses.
1821	*/
1822	#if ARM_CP15
1823	static void __attribute__((naked))
1824	arm_debug_unlock_abort(void)
1825	{
1826	CPU_Exception_frame* frame;
1827	ARM_SWITCH_REGISTERS;
1828	EXCEPTION_ENTRY_EXC();
1829	EXCEPTION_ENTRY_THREAD(frame);
1830	ARM_SWITCH_BACK;
1831	longjmp(unlock_abort_jmpbuf, -1);
1832	}
1833	#endif
1834
1835	static void __attribute__((naked))
1836	target_exception_undefined_instruction(void)
1837	{
1838	CPU_Exception_frame* frame;
1839	ARM_SWITCH_REG;
1840	EXCEPTION_ENTRY(frame);
1841	frame->vector = 1;
1842	target_exception(frame);
1843	EXCEPTION_EXIT(frame);
1844	}
1845
1846	static void __attribute__((naked))
1847	target_exception_supervisor_call(void)
1848	{
1849	CPU_Exception_frame* frame;
1850	ARM_SWITCH_REG;
1851	/*
1852	* The PC offset needs to be reviewed so we move past a svc
1853	* instruction. This can then be used as a user breakpoint. The issue is
1854	* this exception is used by the BKPT instruction in the prefetch abort
1855	* handler to signal a TRAP.
1856	*/
1857	EXCEPTION_ENTRY(frame);
1858	frame->vector = 2;
1859	target_exception(frame);
1860	EXCEPTION_EXIT(frame);
1861	}
1862
1863	static void __attribute__((naked))
1864	target_exception_prefetch_abort(void)
1865	{
1866	CPU_Exception_frame* frame;
1867	ARM_SWITCH_REG;
1868	EXCEPTION_ENTRY(frame);
1869	frame->vector = 3;
1870	target_exception(frame);
1871	EXCEPTION_EXIT(frame);
1872	}
1873
1874	static void __attribute__((naked))
1875	target_exception_data_abort(void)
1876	{
1877	CPU_Exception_frame* frame;
1878	ARM_SWITCH_REG;
1879	EXCEPTION_ENTRY(frame);
1880	frame->vector = 4;
1881	target_exception(frame);
1882	EXCEPTION_EXIT(frame);
1883	}
1884
1885	#if ARM_CP15
1886	#if __ARM_ARCH_PROFILE == 'A'
1887	/**
1888	* The init value for the text section.
1889	*/
1890	static uint32_t text_section_flags;
1891
1892	/* Defined by linkcmds.base */
1893	extern char bsp_section_text_begin[];
1894	extern char bsp_section_text_end[];
1895
1896	static void
1897	rtems_debugger_target_set_mmu(void)
1898	{
1899	void* text_begin;
1900	void* text_end;
1901	text_begin = &bsp_section_text_begin[0];
1902	text_end = &bsp_section_text_end[0];
1903	target_printk("[} MMU edit: text_begin: %p text_end: %p\n",
1904	text_begin, text_end);
1905	text_section_flags =
1906	arm_cp15_set_translation_table_entries(text_begin,
1907	text_end,
1908	ARMV7_MMU_DATA_READ_WRITE_CACHED);
1909	}
1910	#else
1911	static void
1912	rtems_debugger_target_set_mmu(void)
1913	{
1914	}
1915	#endif
1916
1917	static void
1918	rtems_debugger_target_set_vectors(void)
1919	{
1920	arm_cp15_set_exception_handler(ARM_EXCEPTION_UNDEF,
1921	target_exception_undefined_instruction);
1922	arm_cp15_set_exception_handler(ARM_EXCEPTION_SWI,
1923	target_exception_supervisor_call);
1924	arm_cp15_set_exception_handler(ARM_EXCEPTION_PREF_ABORT,
1925	target_exception_prefetch_abort);
1926	arm_cp15_set_exception_handler(ARM_EXCEPTION_DATA_ABORT,
1927	target_exception_data_abort);
1928	}
1929	#else
1930	static void
1931	rtems_debugger_target_set_vectors(void)
1932	{
1933	/*
1934	* Dummy, please add support for your ARM variant.
1935	*/
1936	void* ui = target_exception_undefined_instruction;
1937	void* sc = target_exception_supervisor_call;
1938	void* pa = target_exception_prefetch_abort;
1939	void* da = target_exception_data_abort;
1940	(void) ui;
1941	(void) sc;
1942	(void) pa;
1943	(void) da;
1944	}
1945
1946	static void
1947	rtems_debugger_target_set_mmu(void)
1948	{
1949	}
1950	#endif
1951
1952	static bool
1953	rtems_debugger_is_int_reg(size_t reg)
1954	{
1955	const size_t size = arm_reg_offsets[reg + 1] - arm_reg_offsets[reg];
1956	return size == RTEMS_DEBUGGER_REG_BYTES;
1957	}
1958
1959	static void
1960	rtems_debugger_set_int_reg(rtems_debugger_thread* thread,
1961	size_t reg,
1962	const uint32_t value)
1963	{
1964	const size_t offset = arm_reg_offsets[reg];
1965	/*
1966	* Use memcpy to avoid alignment issues.
1967	*/
1968	memcpy(&thread->registers[offset], &value, sizeof(uint32_t));
1969	}
1970
1971	static const uint32_t
1972	rtems_debugger_get_int_reg(rtems_debugger_thread* thread, size_t reg)
1973	{
1974	const size_t offset = arm_reg_offsets[reg];
1975	uint32_t value;
1976	memcpy(&value, &thread->registers[offset], sizeof(uint32_t));
1977	return value;
1978	}
1979
1980	int
1981	rtems_debugger_target_enable(void)
1982	{
1983	rtems_interrupt_lock_context lock_context;
1984	arm_debug_break_unload();
1985	arm_debug_break_clear_all();
1986	rtems_interrupt_lock_acquire(&target_lock, &lock_context);
1987	rtems_debugger_target_set_mmu();
1988	rtems_debugger_target_set_vectors();
1989	rtems_interrupt_lock_release(&target_lock, &lock_context);
1990	debug_session_active = true;
1991	return 0;
1992	}
1993
1994	int
1995	rtems_debugger_target_disable(void)
1996	{
1997	rtems_interrupt_lock_context lock_context;
1998	#if DOES_NOT_WORK
1999	void* text_begin;
2000	void* text_end;
2001	#endif
2002	arm_debug_break_unload();
2003	arm_debug_break_clear_all();
2004	rtems_interrupt_lock_acquire(&target_lock, &lock_context);
2005	debug_disable_ints = 0;
2006	debug_session_active = false;
2007	#if DOES_NOT_WORK
2008	text_begin = &bsp_section_text_begin[0];
2009	text_end = &bsp_section_text_end[0];
2010	arm_cp15_set_translation_table_entries(text_begin,
2011	text_end,
2012	text_section_flags);
2013	#endif
2014	rtems_interrupt_lock_release(&target_lock, &lock_context);
2015	return 0;
2016	}
2017
2018	int
2019	rtems_debugger_target_read_regs(rtems_debugger_thread* thread)
2020	{
2021	if (!rtems_debugger_thread_flag(thread,
2022	RTEMS_DEBUGGER_THREAD_FLAG_REG_VALID)) {
2023	static const uint32_t good_address = (uint32_t) &good_address;
2024	int i;
2025
2026	memset(&thread->registers[0], 0, RTEMS_DEBUGGER_NUMREGBYTES);
2027
2028	for (i = 0; i < RTEMS_DEBUGGER_NUMREGS; ++i) {
2029	if (rtems_debugger_is_int_reg(i))
2030	rtems_debugger_set_int_reg(thread, i, (uint32_t) &good_address);
2031	}
2032
2033	if (thread->frame) {
2034	CPU_Exception_frame* frame = thread->frame;
2035
2036	/*
2037	* Assume interrupts are not masked and if masked set them to the saved
2038	* value.
2039	*/
2040	FRAME_SR(frame) &= ~CPSR_INTS_MASK;
2041
2042	if (rtems_debugger_thread_flag(thread,
2043	RTEMS_DEBUGGER_THREAD_FLAG_INTS_DISABLED)) {
2044	FRAME_SR(frame) \|=
2045	(thread->flags >> RTEMS_DEBUGGER_THREAD_FLAG_TARGET_BASE) & CPSR_INTS_MASK;
2046	thread->flags &= ~RTEMS_DEBUGGER_THREAD_FLAG_INTS_DISABLED;
2047	}
2048
2049	rtems_debugger_set_int_reg(thread, REG_R0, frame->register_r0);
2050	rtems_debugger_set_int_reg(thread, REG_R1, frame->register_r1);
2051	rtems_debugger_set_int_reg(thread, REG_R2, frame->register_r2);
2052	rtems_debugger_set_int_reg(thread, REG_R3, frame->register_r3);
2053	rtems_debugger_set_int_reg(thread, REG_R4, frame->register_r4);
2054	rtems_debugger_set_int_reg(thread, REG_R5, frame->register_r5);
2055	rtems_debugger_set_int_reg(thread, REG_R6, frame->register_r6);
2056	rtems_debugger_set_int_reg(thread, REG_R7, frame->register_r7);
2057	rtems_debugger_set_int_reg(thread, REG_R8, frame->register_r8);
2058	rtems_debugger_set_int_reg(thread, REG_R9, frame->register_r9);
2059	rtems_debugger_set_int_reg(thread, REG_R10, frame->register_r10);
2060	rtems_debugger_set_int_reg(thread, REG_R11, frame->register_r11);
2061	rtems_debugger_set_int_reg(thread, REG_R12, frame->register_r12);
2062	rtems_debugger_set_int_reg(thread, REG_SP, frame->register_sp);
2063	rtems_debugger_set_int_reg(thread, REG_LR, (uint32_t) frame->register_lr);
2064	rtems_debugger_set_int_reg(thread, REG_PC, (uint32_t) frame->register_pc);
2065	rtems_debugger_set_int_reg(thread, REG_CPSR, FRAME_SR(frame));
2066	/*
2067	* Get the signal from the frame.
2068	*/
2069	thread->signal = rtems_debugger_target_exception_to_signal(frame);
2070	}
2071	else {
2072	#if defined(ARM_MULTILIB_ARCH_V4)
2073	rtems_debugger_set_int_reg(thread, REG_R4, thread->tcb->Registers.register_r4);
2074	rtems_debugger_set_int_reg(thread, REG_R5, thread->tcb->Registers.register_r5);
2075	rtems_debugger_set_int_reg(thread, REG_R6, thread->tcb->Registers.register_r6);
2076	rtems_debugger_set_int_reg(thread, REG_R7, thread->tcb->Registers.register_r7);
2077	rtems_debugger_set_int_reg(thread, REG_R8, thread->tcb->Registers.register_r8);
2078	rtems_debugger_set_int_reg(thread, REG_R9, thread->tcb->Registers.register_r9);
2079	rtems_debugger_set_int_reg(thread, REG_R10, thread->tcb->Registers.register_r10);
2080	rtems_debugger_set_int_reg(thread, REG_R11, thread->tcb->Registers.register_fp);
2081	rtems_debugger_set_int_reg(thread, REG_LR, (intptr_t) thread->tcb->Registers.register_lr);
2082	rtems_debugger_set_int_reg(thread, REG_PC, (intptr_t) thread->tcb->Registers.register_lr);
2083	rtems_debugger_set_int_reg(thread, REG_SP, (intptr_t) thread->tcb->Registers.register_sp);
2084	#elif defined(ARM_MULTILIB_ARCH_V7M)
2085	rtems_debugger_set_int_reg(thread, REG_R4, thread->tcb->Registers.register_r4);
2086	rtems_debugger_set_int_reg(thread, REG_R5, thread->tcb->Registers.register_r5);
2087	rtems_debugger_set_int_reg(thread, REG_R6, thread->tcb->Registers.register_r6);
2088	rtems_debugger_set_int_reg(thread, REG_R7, thread->tcb->Registers.register_r7);
2089	rtems_debugger_set_int_reg(thread, REG_R8, thread->tcb->Registers.register_r8);
2090	rtems_debugger_set_int_reg(thread, REG_R9, thread->tcb->Registers.register_r9);
2091	rtems_debugger_set_int_reg(thread, REG_R10, thread->tcb->Registers.register_r10);
2092	rtems_debugger_set_int_reg(thread, REG_R11, thread->tcb->Registers.register_r11);
2093	rtems_debugger_set_int_reg(thread, REG_LR, (intptr_t) thread->tcb->Registers.register_lr);
2094	rtems_debugger_set_int_reg(thread, REG_PC, (intptr_t) thread->tcb->Registers.register_lr);
2095	rtems_debugger_set_int_reg(thread, REG_SP, (intptr_t) thread->tcb->Registers.register_sp);
2096	#endif
2097	/*
2098	* Blocked threads have no signal.
2099	*/
2100	thread->signal = 0;
2101	}
2102
2103	thread->flags \|= RTEMS_DEBUGGER_THREAD_FLAG_REG_VALID;
2104	thread->flags &= ~RTEMS_DEBUGGER_THREAD_FLAG_REG_DIRTY;
2105	}
2106
2107	return 0;
2108	}
2109
2110	int
2111	rtems_debugger_target_write_regs(rtems_debugger_thread* thread)
2112	{
2113	if (rtems_debugger_thread_flag(thread,
2114	RTEMS_DEBUGGER_THREAD_FLAG_REG_DIRTY)) {
2115	/*
2116	* Only write to debugger controlled exception threads. Do not touch the
2117	* registers for threads blocked in the context switcher.
2118	*/
2119	if (rtems_debugger_thread_flag(thread,
2120	RTEMS_DEBUGGER_THREAD_FLAG_EXCEPTION)) {
2121	CPU_Exception_frame* frame = thread->frame;
2122	frame->register_r0 = rtems_debugger_get_int_reg(thread, REG_R0);
2123	frame->register_r1 = rtems_debugger_get_int_reg(thread, REG_R1);
2124	frame->register_r2 = rtems_debugger_get_int_reg(thread, REG_R2);
2125	frame->register_r3 = rtems_debugger_get_int_reg(thread, REG_R3);
2126	frame->register_r4 = rtems_debugger_get_int_reg(thread, REG_R4);
2127	frame->register_r5 = rtems_debugger_get_int_reg(thread, REG_R5);
2128	frame->register_r6 = rtems_debugger_get_int_reg(thread, REG_R6);
2129	frame->register_r7 = rtems_debugger_get_int_reg(thread, REG_R7);
2130	frame->register_r8 = rtems_debugger_get_int_reg(thread, REG_R8);
2131	frame->register_r9 = rtems_debugger_get_int_reg(thread, REG_R9);
2132	frame->register_r10 = rtems_debugger_get_int_reg(thread, REG_R10);
2133	frame->register_r11 = rtems_debugger_get_int_reg(thread, REG_R11);
2134	frame->register_r12 = rtems_debugger_get_int_reg(thread, REG_R12);
2135	frame->register_sp = rtems_debugger_get_int_reg(thread, REG_SP);
2136	frame->register_lr = (void*) rtems_debugger_get_int_reg(thread, REG_LR);
2137	frame->register_pc = (void*) rtems_debugger_get_int_reg(thread, REG_PC);
2138	FRAME_SR(frame) = rtems_debugger_get_int_reg(thread, REG_CPSR);
2139	}
2140	thread->flags &= ~RTEMS_DEBUGGER_THREAD_FLAG_REG_DIRTY;
2141	}
2142	return 0;
2143	}
2144
2145	DB_UINT
2146	rtems_debugger_target_reg_pc(rtems_debugger_thread* thread)
2147	{
2148	int r;
2149	r = rtems_debugger_target_read_regs(thread);
2150	if (r >= 0) {
2151	return rtems_debugger_get_int_reg(thread, REG_PC);
2152	}
2153	return 0;
2154	}
2155
2156	DB_UINT
2157	rtems_debugger_target_frame_pc(CPU_Exception_frame* frame)
2158	{
2159	return (DB_UINT) frame->register_pc;
2160	}
2161
2162	DB_UINT
2163	rtems_debugger_target_reg_sp(rtems_debugger_thread* thread)
2164	{
2165	int r;
2166	r = rtems_debugger_target_read_regs(thread);
2167	if (r >= 0) {
2168	return rtems_debugger_get_int_reg(thread, REG_SP);
2169	}
2170	return 0;
2171	}
2172
2173	DB_UINT
2174	rtems_debugger_target_tcb_sp(rtems_debugger_thread* thread)
2175	{
2176	return (DB_UINT) thread->tcb->Registers.register_sp;
2177	}
2178
2179	int
2180	rtems_debugger_target_thread_stepping(rtems_debugger_thread* thread)
2181	{
2182	if (rtems_debugger_thread_flag(thread, RTEMS_DEBUGGER_THREAD_FLAG_STEP_INSTR)) {
2183	/*
2184	* Single stepping and range stepping uses hardware debug breakpoint
2185	* 0. This is reserved for single stepping.
2186	*/
2187	CPU_Exception_frame* frame = thread->frame;
2188
2189	target_printk("[} stepping: hbp[0] enabled: %s\n", ARM_HWB_ENALBED(0) ? "yes" : "no");
2190
2191	if (!ARM_HWB_ENALBED(0)) {
2192	const uint32_t addr = (intptr_t) frame->register_pc;
2193	const bool thumb = (FRAME_SR(frame) & (1 << 5)) != 0 ? true : false;
2194	arm_debug_break_exec_enable(0, addr, thumb, true);
2195	arm_debug_disable_interrupts();
2196	}
2197	}
2198	return 0;
2199	}
2200
2201	int
2202	rtems_debugger_target_exception_to_signal(CPU_Exception_frame* frame)
2203	{
2204	int sig = RTEMS_DEBUGGER_SIGNAL_HUP;
2205	#if defined(ARM_MULTILIB_ARCH_V4)
2206	switch (frame->vector) {
2207	case ARM_EXCEPTION_RESET:
2208	case ARM_EXCEPTION_SWI:
2209	sig = RTEMS_DEBUGGER_SIGNAL_TRAP;
2210	break;
2211	case ARM_EXCEPTION_UNDEF:
2212	sig = RTEMS_DEBUGGER_SIGNAL_ILL;
2213	break;
2214	case ARM_EXCEPTION_FIQ:
2215	sig = RTEMS_DEBUGGER_SIGNAL_FPE;
2216	break;
2217	case ARM_EXCEPTION_PREF_ABORT:
2218	case ARM_EXCEPTION_DATA_ABORT:
2219	sig = RTEMS_DEBUGGER_SIGNAL_SEGV;
2220	break;
2221	case ARM_EXCEPTION_RESERVED:
2222	case ARM_EXCEPTION_IRQ:
2223	sig = RTEMS_DEBUGGER_SIGNAL_BUS;
2224	break;
2225	default:
2226	break;
2227	}
2228	#endif
2229	return sig;
2230	}
2231
2232	void
2233	rtems_debugger_target_exception_print(CPU_Exception_frame* frame)
2234	{
2235	EXC_FRAME_PRINT(rtems_debugger_printf, "", frame);
2236	}
2237
2238	int
2239	rtems_debugger_target_hwbreak_insert(void)
2240	{
2241	/*
2242	* Do nothing, load on exit of the exception handler.
2243	*/
2244	return 0;
2245	}
2246
2247	int
2248	rtems_debugger_target_hwbreak_remove(void)
2249	{
2250	target_printk("[} hbreak: remove: unload\n");
2251	arm_debug_break_unload();
2252	return 0;
2253	}
2254
2255	int
2256	rtems_debugger_target_hwbreak_control(rtems_debugger_target_watchpoint wp,
2257	bool insert,
2258	DB_UINT addr,
2259	DB_UINT kind)
2260	{
2261	rtems_interrupt_lock_context lock_context;
2262	int i;
2263	if (wp != rtems_debugger_target_hw_execute) {
2264	errno = EIO;
2265	return -1;
2266	}
2267	rtems_interrupt_lock_acquire(&target_lock, &lock_context);
2268	for (i = 1; i < hw_breakpoints; ++i) {
2269	if (insert && !ARM_HWB_ENALBED(i)) {
2270	arm_debug_break_exec_enable(i, addr, kind == 1, false);
2271	break;
2272	} else if (!insert && ARM_HWB_ENALBED(i) && ARM_HWB_VCR(i) == (addr & ~3)) {
2273	arm_debug_break_clear(i);
2274	break;
2275	}
2276	}
2277	rtems_interrupt_lock_release(&target_lock, &lock_context);
2278	if (!insert && i == hw_breakpoints) {
2279	errno = EIO;
2280	return -1;
2281	}
2282	return 0;
2283	}
2284
2285	int
2286	rtems_debugger_target_cache_sync(rtems_debugger_target_swbreak* swbreak)
2287	{
2288	target_printk("[} cache: sync: %p\n", swbreak->address);
2289	/*
2290	* Flush the data cache and invalidate the instruction cache.
2291	*/
2292	rtems_cache_flush_multiple_data_lines(swbreak->address,
2293	sizeof(breakpoint));
2294	rtems_cache_instruction_sync_after_code_change(swbreak->address,
2295	sizeof(breakpoint));
2296	return 0;
2297	}

Note: See TracBrowser for help on using the repository browser.

Download in other formats: