1 // SPDX-License-Identifier: GPL-2.0
2 #include <test_util.h>
3 #include <kvm_util.h>
4 #include <processor.h>
5 
6 #define VCPU_ID 0
7 
8 #define MDSCR_KDE	(1 << 13)
9 #define MDSCR_MDE	(1 << 15)
10 #define MDSCR_SS	(1 << 0)
11 
12 #define DBGBCR_LEN8	(0xff << 5)
13 #define DBGBCR_EXEC	(0x0 << 3)
14 #define DBGBCR_EL1	(0x1 << 1)
15 #define DBGBCR_E	(0x1 << 0)
16 
17 #define DBGWCR_LEN8	(0xff << 5)
18 #define DBGWCR_RD	(0x1 << 3)
19 #define DBGWCR_WR	(0x2 << 3)
20 #define DBGWCR_EL1	(0x1 << 1)
21 #define DBGWCR_E	(0x1 << 0)
22 
23 #define SPSR_D		(1 << 9)
24 #define SPSR_SS		(1 << 21)
25 
26 extern unsigned char sw_bp, hw_bp, bp_svc, bp_brk, hw_wp, ss_start;
27 static volatile uint64_t sw_bp_addr, hw_bp_addr;
28 static volatile uint64_t wp_addr, wp_data_addr;
29 static volatile uint64_t svc_addr;
30 static volatile uint64_t ss_addr[4], ss_idx;
31 #define  PC(v)  ((uint64_t)&(v))
32 
33 static void reset_debug_state(void)
34 {
35 	asm volatile("msr daifset, #8");
36 
37 	write_sysreg(0, osdlr_el1);
38 	write_sysreg(0, oslar_el1);
39 	isb();
40 
41 	write_sysreg(0, mdscr_el1);
42 	/* This test only uses the first bp and wp slot. */
43 	write_sysreg(0, dbgbvr0_el1);
44 	write_sysreg(0, dbgbcr0_el1);
45 	write_sysreg(0, dbgwcr0_el1);
46 	write_sysreg(0, dbgwvr0_el1);
47 	isb();
48 }
49 
50 static void install_wp(uint64_t addr)
51 {
52 	uint32_t wcr;
53 	uint32_t mdscr;
54 
55 	wcr = DBGWCR_LEN8 | DBGWCR_RD | DBGWCR_WR | DBGWCR_EL1 | DBGWCR_E;
56 	write_sysreg(wcr, dbgwcr0_el1);
57 	write_sysreg(addr, dbgwvr0_el1);
58 	isb();
59 
60 	asm volatile("msr daifclr, #8");
61 
62 	mdscr = read_sysreg(mdscr_el1) | MDSCR_KDE | MDSCR_MDE;
63 	write_sysreg(mdscr, mdscr_el1);
64 	isb();
65 }
66 
67 static void install_hw_bp(uint64_t addr)
68 {
69 	uint32_t bcr;
70 	uint32_t mdscr;
71 
72 	bcr = DBGBCR_LEN8 | DBGBCR_EXEC | DBGBCR_EL1 | DBGBCR_E;
73 	write_sysreg(bcr, dbgbcr0_el1);
74 	write_sysreg(addr, dbgbvr0_el1);
75 	isb();
76 
77 	asm volatile("msr daifclr, #8");
78 
79 	mdscr = read_sysreg(mdscr_el1) | MDSCR_KDE | MDSCR_MDE;
80 	write_sysreg(mdscr, mdscr_el1);
81 	isb();
82 }
83 
84 static void install_ss(void)
85 {
86 	uint32_t mdscr;
87 
88 	asm volatile("msr daifclr, #8");
89 
90 	mdscr = read_sysreg(mdscr_el1) | MDSCR_KDE | MDSCR_SS;
91 	write_sysreg(mdscr, mdscr_el1);
92 	isb();
93 }
94 
95 static volatile char write_data;
96 
97 static void guest_code(void)
98 {
99 	GUEST_SYNC(0);
100 
101 	/* Software-breakpoint */
102 	asm volatile("sw_bp: brk #0");
103 	GUEST_ASSERT_EQ(sw_bp_addr, PC(sw_bp));
104 
105 	GUEST_SYNC(1);
106 
107 	/* Hardware-breakpoint */
108 	reset_debug_state();
109 	install_hw_bp(PC(hw_bp));
110 	asm volatile("hw_bp: nop");
111 	GUEST_ASSERT_EQ(hw_bp_addr, PC(hw_bp));
112 
113 	GUEST_SYNC(2);
114 
115 	/* Hardware-breakpoint + svc */
116 	reset_debug_state();
117 	install_hw_bp(PC(bp_svc));
118 	asm volatile("bp_svc: svc #0");
119 	GUEST_ASSERT_EQ(hw_bp_addr, PC(bp_svc));
120 	GUEST_ASSERT_EQ(svc_addr, PC(bp_svc) + 4);
121 
122 	GUEST_SYNC(3);
123 
124 	/* Hardware-breakpoint + software-breakpoint */
125 	reset_debug_state();
126 	install_hw_bp(PC(bp_brk));
127 	asm volatile("bp_brk: brk #0");
128 	GUEST_ASSERT_EQ(sw_bp_addr, PC(bp_brk));
129 	GUEST_ASSERT_EQ(hw_bp_addr, PC(bp_brk));
130 
131 	GUEST_SYNC(4);
132 
133 	/* Watchpoint */
134 	reset_debug_state();
135 	install_wp(PC(write_data));
136 	write_data = 'x';
137 	GUEST_ASSERT_EQ(write_data, 'x');
138 	GUEST_ASSERT_EQ(wp_data_addr, PC(write_data));
139 
140 	GUEST_SYNC(5);
141 
142 	/* Single-step */
143 	reset_debug_state();
144 	install_ss();
145 	ss_idx = 0;
146 	asm volatile("ss_start:\n"
147 		     "mrs x0, esr_el1\n"
148 		     "add x0, x0, #1\n"
149 		     "msr daifset, #8\n"
150 		     : : : "x0");
151 	GUEST_ASSERT_EQ(ss_addr[0], PC(ss_start));
152 	GUEST_ASSERT_EQ(ss_addr[1], PC(ss_start) + 4);
153 	GUEST_ASSERT_EQ(ss_addr[2], PC(ss_start) + 8);
154 
155 	GUEST_DONE();
156 }
157 
158 static void guest_sw_bp_handler(struct ex_regs *regs)
159 {
160 	sw_bp_addr = regs->pc;
161 	regs->pc += 4;
162 }
163 
164 static void guest_hw_bp_handler(struct ex_regs *regs)
165 {
166 	hw_bp_addr = regs->pc;
167 	regs->pstate |= SPSR_D;
168 }
169 
170 static void guest_wp_handler(struct ex_regs *regs)
171 {
172 	wp_data_addr = read_sysreg(far_el1);
173 	wp_addr = regs->pc;
174 	regs->pstate |= SPSR_D;
175 }
176 
177 static void guest_ss_handler(struct ex_regs *regs)
178 {
179 	GUEST_ASSERT_1(ss_idx < 4, ss_idx);
180 	ss_addr[ss_idx++] = regs->pc;
181 	regs->pstate |= SPSR_SS;
182 }
183 
184 static void guest_svc_handler(struct ex_regs *regs)
185 {
186 	svc_addr = regs->pc;
187 }
188 
189 static int debug_version(struct kvm_vm *vm)
190 {
191 	uint64_t id_aa64dfr0;
192 
193 	get_reg(vm, VCPU_ID, KVM_ARM64_SYS_REG(SYS_ID_AA64DFR0_EL1), &id_aa64dfr0);
194 	return id_aa64dfr0 & 0xf;
195 }
196 
197 int main(int argc, char *argv[])
198 {
199 	struct kvm_vm *vm;
200 	struct ucall uc;
201 	int stage;
202 
203 	vm = vm_create_default(VCPU_ID, 0, guest_code);
204 	ucall_init(vm, NULL);
205 
206 	vm_init_descriptor_tables(vm);
207 	vcpu_init_descriptor_tables(vm, VCPU_ID);
208 
209 	if (debug_version(vm) < 6) {
210 		print_skip("Armv8 debug architecture not supported.");
211 		kvm_vm_free(vm);
212 		exit(KSFT_SKIP);
213 	}
214 
215 	vm_install_sync_handler(vm, VECTOR_SYNC_CURRENT,
216 				ESR_EC_BRK_INS, guest_sw_bp_handler);
217 	vm_install_sync_handler(vm, VECTOR_SYNC_CURRENT,
218 				ESR_EC_HW_BP_CURRENT, guest_hw_bp_handler);
219 	vm_install_sync_handler(vm, VECTOR_SYNC_CURRENT,
220 				ESR_EC_WP_CURRENT, guest_wp_handler);
221 	vm_install_sync_handler(vm, VECTOR_SYNC_CURRENT,
222 				ESR_EC_SSTEP_CURRENT, guest_ss_handler);
223 	vm_install_sync_handler(vm, VECTOR_SYNC_CURRENT,
224 				ESR_EC_SVC64, guest_svc_handler);
225 
226 	for (stage = 0; stage < 7; stage++) {
227 		vcpu_run(vm, VCPU_ID);
228 
229 		switch (get_ucall(vm, VCPU_ID, &uc)) {
230 		case UCALL_SYNC:
231 			TEST_ASSERT(uc.args[1] == stage,
232 				"Stage %d: Unexpected sync ucall, got %lx",
233 				stage, (ulong)uc.args[1]);
234 			break;
235 		case UCALL_ABORT:
236 			TEST_FAIL("%s at %s:%ld\n\tvalues: %#lx, %#lx",
237 				(const char *)uc.args[0],
238 				__FILE__, uc.args[1], uc.args[2], uc.args[3]);
239 			break;
240 		case UCALL_DONE:
241 			goto done;
242 		default:
243 			TEST_FAIL("Unknown ucall %lu", uc.cmd);
244 		}
245 	}
246 
247 done:
248 	kvm_vm_free(vm);
249 	return 0;
250 }
251