1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | /* |
3 | * Copyright (C) 2020 SiFive |
4 | */ |
5 | |
6 | #include <linux/ptrace.h> |
7 | #include <linux/kdebug.h> |
8 | #include <linux/bug.h> |
9 | #include <linux/kgdb.h> |
10 | #include <linux/irqflags.h> |
11 | #include <linux/string.h> |
12 | #include <asm/cacheflush.h> |
13 | #include <asm/gdb_xml.h> |
14 | #include <asm/insn.h> |
15 | |
16 | enum { |
17 | NOT_KGDB_BREAK = 0, |
18 | KGDB_SW_BREAK, |
19 | KGDB_COMPILED_BREAK, |
20 | KGDB_SW_SINGLE_STEP |
21 | }; |
22 | |
23 | static unsigned long stepped_address; |
24 | static unsigned int stepped_opcode; |
25 | |
26 | static int decode_register_index(unsigned long opcode, int offset) |
27 | { |
28 | return (opcode >> offset) & 0x1F; |
29 | } |
30 | |
31 | static int decode_register_index_short(unsigned long opcode, int offset) |
32 | { |
33 | return ((opcode >> offset) & 0x7) + 8; |
34 | } |
35 | |
36 | /* Calculate the new address for after a step */ |
37 | static int get_step_address(struct pt_regs *regs, unsigned long *next_addr) |
38 | { |
39 | unsigned long pc = regs->epc; |
40 | unsigned long *regs_ptr = (unsigned long *)regs; |
41 | unsigned int rs1_num, rs2_num; |
42 | int op_code; |
43 | |
44 | if (get_kernel_nofault(op_code, (void *)pc)) |
45 | return -EINVAL; |
46 | if ((op_code & __INSN_LENGTH_MASK) != __INSN_LENGTH_GE_32) { |
47 | if (riscv_insn_is_c_jalr(op_code) || |
48 | riscv_insn_is_c_jr(op_code)) { |
49 | rs1_num = decode_register_index(opcode: op_code, offset: RVC_C2_RS1_OPOFF); |
50 | *next_addr = regs_ptr[rs1_num]; |
51 | } else if (riscv_insn_is_c_j(op_code) || |
52 | riscv_insn_is_c_jal(op_code)) { |
53 | *next_addr = RVC_EXTRACT_JTYPE_IMM(op_code) + pc; |
54 | } else if (riscv_insn_is_c_beqz(op_code)) { |
55 | rs1_num = decode_register_index_short(opcode: op_code, |
56 | offset: RVC_C1_RS1_OPOFF); |
57 | if (!rs1_num || regs_ptr[rs1_num] == 0) |
58 | *next_addr = RVC_EXTRACT_BTYPE_IMM(op_code) + pc; |
59 | else |
60 | *next_addr = pc + 2; |
61 | } else if (riscv_insn_is_c_bnez(op_code)) { |
62 | rs1_num = |
63 | decode_register_index_short(opcode: op_code, offset: RVC_C1_RS1_OPOFF); |
64 | if (rs1_num && regs_ptr[rs1_num] != 0) |
65 | *next_addr = RVC_EXTRACT_BTYPE_IMM(op_code) + pc; |
66 | else |
67 | *next_addr = pc + 2; |
68 | } else { |
69 | *next_addr = pc + 2; |
70 | } |
71 | } else { |
72 | if ((op_code & __INSN_OPCODE_MASK) == __INSN_BRANCH_OPCODE) { |
73 | bool result = false; |
74 | long imm = RV_EXTRACT_BTYPE_IMM(op_code); |
75 | unsigned long rs1_val = 0, rs2_val = 0; |
76 | |
77 | rs1_num = decode_register_index(opcode: op_code, offset: RVG_RS1_OPOFF); |
78 | rs2_num = decode_register_index(opcode: op_code, offset: RVG_RS2_OPOFF); |
79 | if (rs1_num) |
80 | rs1_val = regs_ptr[rs1_num]; |
81 | if (rs2_num) |
82 | rs2_val = regs_ptr[rs2_num]; |
83 | |
84 | if (riscv_insn_is_beq(op_code)) |
85 | result = (rs1_val == rs2_val) ? true : false; |
86 | else if (riscv_insn_is_bne(op_code)) |
87 | result = (rs1_val != rs2_val) ? true : false; |
88 | else if (riscv_insn_is_blt(op_code)) |
89 | result = |
90 | ((long)rs1_val < |
91 | (long)rs2_val) ? true : false; |
92 | else if (riscv_insn_is_bge(op_code)) |
93 | result = |
94 | ((long)rs1_val >= |
95 | (long)rs2_val) ? true : false; |
96 | else if (riscv_insn_is_bltu(op_code)) |
97 | result = (rs1_val < rs2_val) ? true : false; |
98 | else if (riscv_insn_is_bgeu(op_code)) |
99 | result = (rs1_val >= rs2_val) ? true : false; |
100 | if (result) |
101 | *next_addr = imm + pc; |
102 | else |
103 | *next_addr = pc + 4; |
104 | } else if (riscv_insn_is_jal(op_code)) { |
105 | *next_addr = RV_EXTRACT_JTYPE_IMM(op_code) + pc; |
106 | } else if (riscv_insn_is_jalr(op_code)) { |
107 | rs1_num = decode_register_index(opcode: op_code, offset: RVG_RS1_OPOFF); |
108 | if (rs1_num) |
109 | *next_addr = ((unsigned long *)regs)[rs1_num]; |
110 | *next_addr += RV_EXTRACT_ITYPE_IMM(op_code); |
111 | } else if (riscv_insn_is_sret(op_code)) { |
112 | *next_addr = pc; |
113 | } else { |
114 | *next_addr = pc + 4; |
115 | } |
116 | } |
117 | return 0; |
118 | } |
119 | |
120 | static int do_single_step(struct pt_regs *regs) |
121 | { |
122 | /* Determine where the target instruction will send us to */ |
123 | unsigned long addr = 0; |
124 | int error = get_step_address(regs, next_addr: &addr); |
125 | |
126 | if (error) |
127 | return error; |
128 | |
129 | /* Store the op code in the stepped address */ |
130 | error = get_kernel_nofault(stepped_opcode, (void *)addr); |
131 | if (error) |
132 | return error; |
133 | |
134 | stepped_address = addr; |
135 | |
136 | /* Replace the op code with the break instruction */ |
137 | error = copy_to_kernel_nofault(dst: (void *)stepped_address, |
138 | src: arch_kgdb_ops.gdb_bpt_instr, |
139 | BREAK_INSTR_SIZE); |
140 | /* Flush and return */ |
141 | if (!error) { |
142 | flush_icache_range(start: addr, end: addr + BREAK_INSTR_SIZE); |
143 | kgdb_single_step = 1; |
144 | atomic_set(v: &kgdb_cpu_doing_single_step, |
145 | raw_smp_processor_id()); |
146 | } else { |
147 | stepped_address = 0; |
148 | stepped_opcode = 0; |
149 | } |
150 | return error; |
151 | } |
152 | |
153 | /* Undo a single step */ |
154 | static void undo_single_step(struct pt_regs *regs) |
155 | { |
156 | if (stepped_opcode != 0) { |
157 | copy_to_kernel_nofault(dst: (void *)stepped_address, |
158 | src: (void *)&stepped_opcode, BREAK_INSTR_SIZE); |
159 | flush_icache_range(start: stepped_address, |
160 | end: stepped_address + BREAK_INSTR_SIZE); |
161 | } |
162 | stepped_address = 0; |
163 | stepped_opcode = 0; |
164 | kgdb_single_step = 0; |
165 | atomic_set(v: &kgdb_cpu_doing_single_step, i: -1); |
166 | } |
167 | |
168 | struct dbg_reg_def_t dbg_reg_def[DBG_MAX_REG_NUM] = { |
169 | {DBG_REG_ZERO, GDB_SIZEOF_REG, -1}, |
170 | {DBG_REG_RA, GDB_SIZEOF_REG, offsetof(struct pt_regs, ra)}, |
171 | {DBG_REG_SP, GDB_SIZEOF_REG, offsetof(struct pt_regs, sp)}, |
172 | {DBG_REG_GP, GDB_SIZEOF_REG, offsetof(struct pt_regs, gp)}, |
173 | {DBG_REG_TP, GDB_SIZEOF_REG, offsetof(struct pt_regs, tp)}, |
174 | {DBG_REG_T0, GDB_SIZEOF_REG, offsetof(struct pt_regs, t0)}, |
175 | {DBG_REG_T1, GDB_SIZEOF_REG, offsetof(struct pt_regs, t1)}, |
176 | {DBG_REG_T2, GDB_SIZEOF_REG, offsetof(struct pt_regs, t2)}, |
177 | {DBG_REG_FP, GDB_SIZEOF_REG, offsetof(struct pt_regs, s0)}, |
178 | {DBG_REG_S1, GDB_SIZEOF_REG, offsetof(struct pt_regs, a1)}, |
179 | {DBG_REG_A0, GDB_SIZEOF_REG, offsetof(struct pt_regs, a0)}, |
180 | {DBG_REG_A1, GDB_SIZEOF_REG, offsetof(struct pt_regs, a1)}, |
181 | {DBG_REG_A2, GDB_SIZEOF_REG, offsetof(struct pt_regs, a2)}, |
182 | {DBG_REG_A3, GDB_SIZEOF_REG, offsetof(struct pt_regs, a3)}, |
183 | {DBG_REG_A4, GDB_SIZEOF_REG, offsetof(struct pt_regs, a4)}, |
184 | {DBG_REG_A5, GDB_SIZEOF_REG, offsetof(struct pt_regs, a5)}, |
185 | {DBG_REG_A6, GDB_SIZEOF_REG, offsetof(struct pt_regs, a6)}, |
186 | {DBG_REG_A7, GDB_SIZEOF_REG, offsetof(struct pt_regs, a7)}, |
187 | {DBG_REG_S2, GDB_SIZEOF_REG, offsetof(struct pt_regs, s2)}, |
188 | {DBG_REG_S3, GDB_SIZEOF_REG, offsetof(struct pt_regs, s3)}, |
189 | {DBG_REG_S4, GDB_SIZEOF_REG, offsetof(struct pt_regs, s4)}, |
190 | {DBG_REG_S5, GDB_SIZEOF_REG, offsetof(struct pt_regs, s5)}, |
191 | {DBG_REG_S6, GDB_SIZEOF_REG, offsetof(struct pt_regs, s6)}, |
192 | {DBG_REG_S7, GDB_SIZEOF_REG, offsetof(struct pt_regs, s7)}, |
193 | {DBG_REG_S8, GDB_SIZEOF_REG, offsetof(struct pt_regs, s8)}, |
194 | {DBG_REG_S9, GDB_SIZEOF_REG, offsetof(struct pt_regs, s9)}, |
195 | {DBG_REG_S10, GDB_SIZEOF_REG, offsetof(struct pt_regs, s10)}, |
196 | {DBG_REG_S11, GDB_SIZEOF_REG, offsetof(struct pt_regs, s11)}, |
197 | {DBG_REG_T3, GDB_SIZEOF_REG, offsetof(struct pt_regs, t3)}, |
198 | {DBG_REG_T4, GDB_SIZEOF_REG, offsetof(struct pt_regs, t4)}, |
199 | {DBG_REG_T5, GDB_SIZEOF_REG, offsetof(struct pt_regs, t5)}, |
200 | {DBG_REG_T6, GDB_SIZEOF_REG, offsetof(struct pt_regs, t6)}, |
201 | {DBG_REG_EPC, GDB_SIZEOF_REG, offsetof(struct pt_regs, epc)}, |
202 | {DBG_REG_STATUS, GDB_SIZEOF_REG, offsetof(struct pt_regs, status)}, |
203 | {DBG_REG_BADADDR, GDB_SIZEOF_REG, offsetof(struct pt_regs, badaddr)}, |
204 | {DBG_REG_CAUSE, GDB_SIZEOF_REG, offsetof(struct pt_regs, cause)}, |
205 | }; |
206 | |
207 | char *dbg_get_reg(int regno, void *mem, struct pt_regs *regs) |
208 | { |
209 | if (regno >= DBG_MAX_REG_NUM || regno < 0) |
210 | return NULL; |
211 | |
212 | if (dbg_reg_def[regno].offset != -1) |
213 | memcpy(mem, (void *)regs + dbg_reg_def[regno].offset, |
214 | dbg_reg_def[regno].size); |
215 | else |
216 | memset(mem, 0, dbg_reg_def[regno].size); |
217 | return dbg_reg_def[regno].name; |
218 | } |
219 | |
220 | int dbg_set_reg(int regno, void *mem, struct pt_regs *regs) |
221 | { |
222 | if (regno >= DBG_MAX_REG_NUM || regno < 0) |
223 | return -EINVAL; |
224 | |
225 | if (dbg_reg_def[regno].offset != -1) |
226 | memcpy((void *)regs + dbg_reg_def[regno].offset, mem, |
227 | dbg_reg_def[regno].size); |
228 | return 0; |
229 | } |
230 | |
231 | void |
232 | sleeping_thread_to_gdb_regs(unsigned long *gdb_regs, struct task_struct *task) |
233 | { |
234 | /* Initialize to zero */ |
235 | memset((char *)gdb_regs, 0, NUMREGBYTES); |
236 | |
237 | gdb_regs[DBG_REG_SP_OFF] = task->thread.sp; |
238 | gdb_regs[DBG_REG_FP_OFF] = task->thread.s[0]; |
239 | gdb_regs[DBG_REG_S1_OFF] = task->thread.s[1]; |
240 | gdb_regs[DBG_REG_S2_OFF] = task->thread.s[2]; |
241 | gdb_regs[DBG_REG_S3_OFF] = task->thread.s[3]; |
242 | gdb_regs[DBG_REG_S4_OFF] = task->thread.s[4]; |
243 | gdb_regs[DBG_REG_S5_OFF] = task->thread.s[5]; |
244 | gdb_regs[DBG_REG_S6_OFF] = task->thread.s[6]; |
245 | gdb_regs[DBG_REG_S7_OFF] = task->thread.s[7]; |
246 | gdb_regs[DBG_REG_S8_OFF] = task->thread.s[8]; |
247 | gdb_regs[DBG_REG_S9_OFF] = task->thread.s[10]; |
248 | gdb_regs[DBG_REG_S10_OFF] = task->thread.s[11]; |
249 | gdb_regs[DBG_REG_EPC_OFF] = task->thread.ra; |
250 | } |
251 | |
252 | void kgdb_arch_set_pc(struct pt_regs *regs, unsigned long pc) |
253 | { |
254 | regs->epc = pc; |
255 | } |
256 | |
257 | void kgdb_arch_handle_qxfer_pkt(char *remcom_in_buffer, |
258 | char *remcom_out_buffer) |
259 | { |
260 | if (!strncmp(remcom_in_buffer, gdb_xfer_read_target, |
261 | sizeof(gdb_xfer_read_target))) |
262 | strcpy(remcom_out_buffer, riscv_gdb_stub_target_desc); |
263 | else if (!strncmp(remcom_in_buffer, gdb_xfer_read_cpuxml, |
264 | sizeof(gdb_xfer_read_cpuxml))) |
265 | strcpy(remcom_out_buffer, riscv_gdb_stub_cpuxml); |
266 | } |
267 | |
268 | static inline void kgdb_arch_update_addr(struct pt_regs *regs, |
269 | char *remcom_in_buffer) |
270 | { |
271 | unsigned long addr; |
272 | char *ptr; |
273 | |
274 | ptr = &remcom_in_buffer[1]; |
275 | if (kgdb_hex2long(ptr: &ptr, long_val: &addr)) |
276 | regs->epc = addr; |
277 | } |
278 | |
279 | int kgdb_arch_handle_exception(int vector, int signo, int err_code, |
280 | char *remcom_in_buffer, char *remcom_out_buffer, |
281 | struct pt_regs *regs) |
282 | { |
283 | int err = 0; |
284 | |
285 | undo_single_step(regs); |
286 | |
287 | switch (remcom_in_buffer[0]) { |
288 | case 'c': |
289 | case 'D': |
290 | case 'k': |
291 | if (remcom_in_buffer[0] == 'c') |
292 | kgdb_arch_update_addr(regs, remcom_in_buffer); |
293 | break; |
294 | case 's': |
295 | kgdb_arch_update_addr(regs, remcom_in_buffer); |
296 | err = do_single_step(regs); |
297 | break; |
298 | default: |
299 | err = -1; |
300 | } |
301 | return err; |
302 | } |
303 | |
304 | static int kgdb_riscv_kgdbbreak(unsigned long addr) |
305 | { |
306 | if (stepped_address == addr) |
307 | return KGDB_SW_SINGLE_STEP; |
308 | if (atomic_read(v: &kgdb_setting_breakpoint)) |
309 | if (addr == (unsigned long)&kgdb_compiled_break) |
310 | return KGDB_COMPILED_BREAK; |
311 | |
312 | return kgdb_has_hit_break(addr); |
313 | } |
314 | |
315 | static int kgdb_riscv_notify(struct notifier_block *self, unsigned long cmd, |
316 | void *ptr) |
317 | { |
318 | struct die_args *args = (struct die_args *)ptr; |
319 | struct pt_regs *regs = args->regs; |
320 | unsigned long flags; |
321 | int type; |
322 | |
323 | if (user_mode(regs)) |
324 | return NOTIFY_DONE; |
325 | |
326 | type = kgdb_riscv_kgdbbreak(addr: regs->epc); |
327 | if (type == NOT_KGDB_BREAK && cmd == DIE_TRAP) |
328 | return NOTIFY_DONE; |
329 | |
330 | local_irq_save(flags); |
331 | |
332 | if (kgdb_handle_exception(ex_vector: type == KGDB_SW_SINGLE_STEP ? 0 : 1, |
333 | signo: args->signr, err_code: cmd, regs)) |
334 | return NOTIFY_DONE; |
335 | |
336 | if (type == KGDB_COMPILED_BREAK) |
337 | regs->epc += 4; |
338 | |
339 | local_irq_restore(flags); |
340 | |
341 | return NOTIFY_STOP; |
342 | } |
343 | |
344 | static struct notifier_block kgdb_notifier = { |
345 | .notifier_call = kgdb_riscv_notify, |
346 | }; |
347 | |
348 | int kgdb_arch_init(void) |
349 | { |
350 | register_die_notifier(nb: &kgdb_notifier); |
351 | |
352 | return 0; |
353 | } |
354 | |
355 | void kgdb_arch_exit(void) |
356 | { |
357 | unregister_die_notifier(nb: &kgdb_notifier); |
358 | } |
359 | |
360 | /* |
361 | * Global data |
362 | */ |
363 | #ifdef CONFIG_RISCV_ISA_C |
364 | const struct kgdb_arch arch_kgdb_ops = { |
365 | .gdb_bpt_instr = {0x02, 0x90}, /* c.ebreak */ |
366 | }; |
367 | #else |
368 | const struct kgdb_arch arch_kgdb_ops = { |
369 | .gdb_bpt_instr = {0x73, 0x00, 0x10, 0x00}, /* ebreak */ |
370 | }; |
371 | #endif |
372 | |