1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3 * Copyright (c) 2019 Fuzhou Rockchip Electronics Co., Ltd
4 *
5 * This file is taken and modified from the OP-TEE project.
6 */
7
8 #include <common.h>
9 #include <stacktrace.h>
10 #include <asm/sections.h>
11
12 DECLARE_GLOBAL_DATA_PTR;
13
14 /* The register names */
15 #define FP 11
16 #define SP 13
17 #define LR 14
18 #define PC 15
19
20 /*
21 * Definitions for the instruction interpreter.
22 *
23 * The ARM EABI specifies how to perform the frame unwinding in the
24 * Exception Handling ABI for the ARM Architecture document. To perform
25 * the unwind we need to know the initial frame pointer, stack pointer,
26 * link register and program counter. We then find the entry within the
27 * index table that points to the function the program counter is within.
28 * This gives us either a list of three instructions to process, a 31-bit
29 * relative offset to a table of instructions, or a value telling us
30 * we can't unwind any further.
31 *
32 * When we have the instructions to process we need to decode them
33 * following table 4 in section 9.3. This describes a collection of bit
34 * patterns to encode that steps to take to update the stack pointer and
35 * link register to the correct values at the start of the function.
36 */
37
38 /* A special case when we are unable to unwind past this function */
39 #define EXIDX_CANTUNWIND 1
40
41 /*
42 * Entry types.
43 * These are the only entry types that have been seen in the kernel.
44 */
45 #define ENTRY_MASK 0xff000000
46 #define ENTRY_ARM_SU16 0x80000000
47 #define ENTRY_ARM_LU16 0x81000000
48
49 /* Instruction masks. */
50 #define INSN_VSP_MASK 0xc0
51 #define INSN_VSP_SIZE_MASK 0x3f
52 #define INSN_STD_MASK 0xf0
53 #define INSN_STD_DATA_MASK 0x0f
54 #define INSN_POP_TYPE_MASK 0x08
55 #define INSN_POP_COUNT_MASK 0x07
56 #define INSN_VSP_LARGE_INC_MASK 0xff
57
58 /* Instruction definitions */
59 #define INSN_VSP_INC 0x00
60 #define INSN_VSP_DEC 0x40
61 #define INSN_POP_MASKED 0x80
62 #define INSN_VSP_REG 0x90
63 #define INSN_POP_COUNT 0xa0
64 #define INSN_FINISH 0xb0
65 #define INSN_POP_REGS 0xb1
66 #define INSN_VSP_LARGE_INC 0xb2
67
68 #define SHIFT_U32(v, shift) ((uint32_t)(v) << (shift))
69
70 /* The state of the unwind process (32-bit mode) */
71 struct unwind_state_arm32 {
72 uint32_t registers[16];
73 uint32_t start_pc;
74 ulong insn;
75 unsigned int entries;
76 unsigned int byte;
77 uint16_t update_mask;
78 };
79
80 /* An item in the exception index table */
81 struct unwind_idx {
82 uint32_t offset;
83 uint32_t insn;
84 };
85
read_pc(void)86 static __always_inline uint32_t read_pc(void)
87 {
88 uint32_t val;
89
90 asm volatile ("adr %0, ." : "=r" (val));
91 return val;
92 }
93
read_sp(void)94 static __always_inline uint32_t read_sp(void)
95 {
96 uint32_t val;
97
98 asm volatile ("mov %0, sp" : "=r" (val));
99 return val;
100 }
101
read_lr(void)102 static __always_inline uint32_t read_lr(void)
103 {
104 uint32_t val;
105
106 asm volatile ("mov %0, lr" : "=r" (val));
107 return val;
108 }
109
read_fp(void)110 static __always_inline uint32_t read_fp(void)
111 {
112 uint32_t val;
113
114 asm volatile ("mov %0, fp" : "=r" (val));
115 return val;
116 }
117
read_r7(void)118 static __always_inline uint32_t read_r7(void)
119 {
120 uint32_t val;
121
122 asm volatile ("mov %0, r7" : "=r" (val));
123 return val;
124 }
125
copy_in(void * dst,const void * src,size_t n,bool kernel_data)126 static bool copy_in(void *dst, const void *src, size_t n, bool kernel_data)
127 {
128 if (!kernel_data)
129 return false;
130
131 memcpy(dst, src, n);
132
133 return true;
134 }
135
136 /* Expand a 31-bit signed value to a 32-bit signed value */
expand_prel31(uint32_t prel31)137 static int32_t expand_prel31(uint32_t prel31)
138 {
139 return prel31 | SHIFT_U32(prel31 & BIT(30), 1);
140 }
141
142 /*
143 * Perform a binary search of the index table to find the function
144 * with the largest address that doesn't exceed addr.
145 */
find_index(uint32_t addr,ulong exidx,size_t exidx_sz)146 static struct unwind_idx *find_index(uint32_t addr, ulong exidx,
147 size_t exidx_sz)
148 {
149 ulong idx_start, idx_end;
150 unsigned int min, mid, max;
151 struct unwind_idx *start;
152 struct unwind_idx *item;
153 int32_t prel31_addr;
154 ulong func_addr;
155
156 start = (struct unwind_idx *)exidx;
157 idx_start = exidx;
158 idx_end = exidx + exidx_sz;
159
160 min = 0;
161 max = (idx_end - idx_start) / sizeof(struct unwind_idx);
162
163 while (min != max) {
164 mid = min + (max - min + 1) / 2;
165
166 item = &start[mid];
167
168 prel31_addr = expand_prel31(item->offset);
169 func_addr = (ulong)&item->offset + prel31_addr;
170
171 if (func_addr <= addr) {
172 min = mid;
173 } else {
174 max = mid - 1;
175 }
176 }
177
178 return &start[min];
179 }
180
181 /* Reads the next byte from the instruction list */
unwind_exec_read_byte(struct unwind_state_arm32 * state,uint32_t * ret_insn,bool kernel_stack)182 static bool unwind_exec_read_byte(struct unwind_state_arm32 *state,
183 uint32_t *ret_insn, bool kernel_stack)
184 {
185 uint32_t insn;
186
187 if (!copy_in(&insn, (void *)state->insn, sizeof(insn), kernel_stack))
188 return false;
189
190 /* Read the unwind instruction */
191 *ret_insn = (insn >> (state->byte * 8)) & 0xff;
192
193 /* Update the location of the next instruction */
194 if (state->byte == 0) {
195 state->byte = 3;
196 state->insn += sizeof(uint32_t);
197 state->entries--;
198 } else {
199 state->byte--;
200 }
201
202 return true;
203 }
204
pop_vsp(uint32_t * reg,ulong * vsp,bool kernel_stack,ulong stack,size_t stack_size)205 static bool pop_vsp(uint32_t *reg, ulong *vsp, bool kernel_stack,
206 ulong stack, size_t stack_size)
207 {
208 if (*vsp > gd->start_addr_sp ||
209 *vsp < gd->start_addr_sp - CONFIG_SYS_STACK_SIZE)
210 return false;
211
212 if (!copy_in(reg, (void *)*vsp, sizeof(*reg), kernel_stack))
213 return false;
214
215 (*vsp) += sizeof(*reg);
216
217 return true;
218 }
219
220 /* Executes the next instruction on the list */
unwind_exec_insn(struct unwind_state_arm32 * state,bool kernel_stack,ulong stack,size_t stack_size)221 static bool unwind_exec_insn(struct unwind_state_arm32 *state,
222 bool kernel_stack, ulong stack,
223 size_t stack_size)
224 {
225 uint32_t insn;
226 ulong vsp = state->registers[SP];
227 int update_vsp = 0;
228
229 /* Read the next instruction */
230 if (!unwind_exec_read_byte(state, &insn, kernel_stack))
231 return false;
232
233 if ((insn & INSN_VSP_MASK) == INSN_VSP_INC) {
234 state->registers[SP] += ((insn & INSN_VSP_SIZE_MASK) << 2) + 4;
235
236 } else if ((insn & INSN_VSP_MASK) == INSN_VSP_DEC) {
237 state->registers[SP] -= ((insn & INSN_VSP_SIZE_MASK) << 2) + 4;
238
239 } else if ((insn & INSN_STD_MASK) == INSN_POP_MASKED) {
240 uint32_t mask;
241 unsigned int reg;
242
243 /* Load the mask */
244 if (!unwind_exec_read_byte(state, &mask, kernel_stack))
245 return false;
246 mask |= (insn & INSN_STD_DATA_MASK) << 8;
247
248 /* We have a refuse to unwind instruction */
249 if (mask == 0)
250 return false;
251
252 /* Update SP */
253 update_vsp = 1;
254
255 /* Load the registers */
256 for (reg = 4; mask && reg < 16; mask >>= 1, reg++) {
257 if (mask & 1) {
258 if (!pop_vsp(&state->registers[reg], &vsp,
259 kernel_stack, stack, stack_size))
260 return false;
261 state->update_mask |= 1 << reg;
262
263 /* If we have updated SP kep its value */
264 if (reg == SP)
265 update_vsp = 0;
266 }
267 }
268
269 } else if ((insn & INSN_STD_MASK) == INSN_VSP_REG &&
270 ((insn & INSN_STD_DATA_MASK) != 13) &&
271 ((insn & INSN_STD_DATA_MASK) != 15)) {
272 /* sp = register */
273 state->registers[SP] =
274 state->registers[insn & INSN_STD_DATA_MASK];
275
276 } else if ((insn & INSN_STD_MASK) == INSN_POP_COUNT) {
277 unsigned int count, reg;
278
279 /* Read how many registers to load */
280 count = insn & INSN_POP_COUNT_MASK;
281
282 /* Update sp */
283 update_vsp = 1;
284
285 /* Pop the registers */
286 for (reg = 4; reg <= 4 + count; reg++) {
287 if (!pop_vsp(&state->registers[reg], &vsp,
288 kernel_stack, stack, stack_size))
289 return false;
290 state->update_mask |= 1 << reg;
291 }
292
293 /* Check if we are in the pop r14 version */
294 if ((insn & INSN_POP_TYPE_MASK) != 0) {
295 if (!pop_vsp(&state->registers[14], &vsp, kernel_stack,
296 stack, stack_size))
297 return false;
298 }
299
300 } else if (insn == INSN_FINISH) {
301 /* Stop processing */
302 state->entries = 0;
303
304 } else if (insn == INSN_POP_REGS) {
305 uint32_t mask;
306 unsigned int reg;
307
308 if (!unwind_exec_read_byte(state, &mask, kernel_stack))
309 return false;
310 if (mask == 0 || (mask & 0xf0) != 0)
311 return false;
312
313 /* Update SP */
314 update_vsp = 1;
315
316 /* Load the registers */
317 for (reg = 0; mask && reg < 4; mask >>= 1, reg++) {
318 if (mask & 1) {
319 if (!pop_vsp(&state->registers[reg], &vsp,
320 kernel_stack, stack, stack_size))
321 return false;
322 state->update_mask |= 1 << reg;
323 }
324 }
325
326 } else if ((insn & INSN_VSP_LARGE_INC_MASK) == INSN_VSP_LARGE_INC) {
327 uint32_t uleb128;
328
329 /* Read the increment value */
330 if (!unwind_exec_read_byte(state, &uleb128, kernel_stack))
331 return false;
332
333 state->registers[SP] += 0x204 + (uleb128 << 2);
334
335 } else {
336 /* We hit a new instruction that needs to be implemented */
337 printf("Unhandled instruction %.2x\n", insn);
338 return false;
339 }
340
341 if (update_vsp)
342 state->registers[SP] = vsp;
343
344 return true;
345 }
346
347 /* Performs the unwind of a function */
unwind_tab(struct unwind_state_arm32 * state,bool kernel_stack,ulong stack,size_t stack_size)348 static bool unwind_tab(struct unwind_state_arm32 *state, bool kernel_stack,
349 ulong stack, size_t stack_size)
350 {
351 uint32_t entry;
352 uint32_t insn;
353
354 /* Set PC to a known value */
355 state->registers[PC] = 0;
356
357 if (!copy_in(&insn, (void *)state->insn, sizeof(insn), kernel_stack)) {
358 printf("Bad insn addr %p", (void *)state->insn);
359 return true;
360 }
361
362 /* Read the personality */
363 entry = insn & ENTRY_MASK;
364
365 if (entry == ENTRY_ARM_SU16) {
366 state->byte = 2;
367 state->entries = 1;
368 } else if (entry == ENTRY_ARM_LU16) {
369 state->byte = 1;
370 state->entries = ((insn >> 16) & 0xFF) + 1;
371 } else {
372 printf("Unknown entry: %x\n", entry);
373 return true;
374 }
375
376 while (state->entries > 0) {
377 if (!unwind_exec_insn(state, kernel_stack, stack, stack_size))
378 return true;
379 }
380
381 /*
382 * The program counter was not updated, load it from the link register.
383 */
384 if (state->registers[PC] == 0) {
385 state->registers[PC] = state->registers[LR];
386
387 /*
388 * If the program counter changed, flag it in the update mask.
389 */
390 if (state->start_pc != state->registers[PC])
391 state->update_mask |= 1 << PC;
392
393 /* Check again */
394 if (state->registers[PC] == 0)
395 return true;
396 }
397
398 return false;
399 }
400
unwind_stack_arm32(struct unwind_state_arm32 * state,ulong exidx,size_t exidx_sz,bool kernel_stack,ulong stack,size_t stack_size)401 bool unwind_stack_arm32(struct unwind_state_arm32 *state, ulong exidx,
402 size_t exidx_sz, bool kernel_stack, ulong stack,
403 size_t stack_size)
404 {
405 struct unwind_idx *index;
406 bool finished;
407
408 if (!exidx_sz)
409 return false;
410
411 /* Reset the mask of updated registers */
412 state->update_mask = 0;
413
414 /* The pc value is correct and will be overwritten, save it */
415 state->start_pc = state->registers[PC];
416
417 /* Find the item to run */
418 index = find_index(state->start_pc, exidx, exidx_sz);
419
420 finished = false;
421 if (index->insn != EXIDX_CANTUNWIND) {
422 if (index->insn & (1U << 31)) {
423 /* The data is within the instruction */
424 state->insn = (ulong)&index->insn;
425 } else {
426 /* A prel31 offset to the unwind table */
427 state->insn = (ulong)&index->insn +
428 expand_prel31(index->insn);
429 }
430
431 /* Run the unwind function */
432 finished = unwind_tab(state, kernel_stack, stack, stack_size);
433 }
434
435 /* This is the top of the stack, finish */
436 if (index->insn == EXIDX_CANTUNWIND)
437 finished = true;
438
439 return !finished;
440 }
441
offset_prel31(uint32_t addr,int32_t offset)442 static uint32_t offset_prel31(uint32_t addr, int32_t offset)
443 {
444 return (addr + offset) & 0x7FFFFFFFUL;
445 }
446
relocate_exidx(void * exidx,size_t exidx_sz,int32_t offset)447 int relocate_exidx(void *exidx, size_t exidx_sz, int32_t offset)
448 {
449 size_t num_items = exidx_sz / sizeof(struct unwind_idx);
450 struct unwind_idx *start = (struct unwind_idx *)exidx;
451 size_t n;
452
453 for (n = 0; n < num_items; n++) {
454 struct unwind_idx *item = &start[n];
455
456 if (item->offset & BIT(31))
457 return -EINVAL;
458
459 /* Offset to the start of the function has to be adjusted */
460 item->offset = offset_prel31(item->offset, offset);
461
462 if (item->insn == EXIDX_CANTUNWIND)
463 continue;
464 if (item->insn & BIT(31)) {
465 /* insn is a table entry itself */
466 continue;
467 }
468 /*
469 * insn is an offset to an entry in .ARM.extab so it has to be
470 * adjusted
471 */
472 item->insn = offset_prel31(item->insn, offset);
473 }
474 return 0;
475 }
476
print_stack_arm32(struct unwind_state_arm32 * state,ulong exidx,size_t exidx_sz,bool kernel_stack,ulong stack,size_t stack_size)477 void print_stack_arm32(struct unwind_state_arm32 *state,
478 ulong exidx, size_t exidx_sz, bool kernel_stack,
479 ulong stack, size_t stack_size)
480 {
481 ulong pc, lr;
482 #if defined(CONFIG_TPL_BUILD)
483 char *build = "tpl";
484 #elif defined(CONFIG_SPL_BUILD)
485 char *build = "spl";
486 #else
487 char *build = "";
488 #endif
489
490 if (gd->flags & GD_FLG_RELOC) {
491 pc = (ulong)state->registers[PC] - gd->reloc_off;
492 lr = (ulong)state->registers[LR] - gd->reloc_off;
493 } else {
494 pc = (ulong)state->registers[PC];
495 lr = (ulong)state->registers[LR];
496 }
497
498 printf("\nCall trace:\n");
499 printf(" PC: [< %08lx >]\n", pc);
500 printf(" LR: [< %08lx >]\n", lr);
501
502 printf("\nStack:\n");
503 do {
504 if (gd->flags & GD_FLG_RELOC)
505 pc = (ulong)state->registers[PC] - gd->reloc_off;
506 else
507 pc = (ulong)state->registers[PC];
508
509 printf(" [< %08lx >]\n", pc);
510 } while (unwind_stack_arm32(state, exidx, exidx_sz,
511 kernel_stack, stack, stack_size));
512
513 printf("\nCopy info from \"Call trace...\" to a file(eg. dump.txt), and run\n"
514 "command in your U-Boot project: "
515 "./scripts/stacktrace.sh dump.txt %s\n\n", build);
516 }
517
dump_core_stack(struct pt_regs * regs)518 void dump_core_stack(struct pt_regs *regs)
519 {
520 struct unwind_state_arm32 state;
521 ulong exidx = (ulong)__exidx_start;
522 size_t exidx_sz = (ulong)__exidx_end - (ulong)__exidx_start;
523 ulong stack = gd->start_addr_sp;
524 size_t stack_size = CONFIG_SYS_STACK_SIZE;
525 int i;
526
527 /* Don't use memset(), which updates LR ! */
528 for (i = 0; i < 16; i++)
529 state.registers[i] = 0;
530 state.update_mask = 0;
531 state.start_pc = 0;
532 state.entries = 0;
533 state.insn = 0;
534 state.byte = 0;
535
536 /* r7: Thumb-style frame pointer */
537 state.registers[7] = regs->ARM_r7;
538 /* r11: ARM-style frame pointer */
539 state.registers[FP] = regs->ARM_ip;
540 state.registers[SP] = regs->ARM_sp;
541 state.registers[LR] = regs->ARM_lr;
542 state.registers[PC] = regs->ARM_pc;
543
544 print_stack_arm32(&state, exidx, exidx_sz,
545 true, stack, stack_size);
546 }
547
dump_stack(void)548 void dump_stack(void)
549 {
550 struct pt_regs regs;
551
552 regs.ARM_r7 = read_r7();
553 regs.ARM_ip = read_fp();
554 regs.ARM_sp = read_sp();
555 regs.ARM_lr = read_lr();
556 regs.ARM_pc = (uint32_t)dump_stack;
557
558 dump_core_stack(®s);
559 }
560