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 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 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 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 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 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 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 */ 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 */ 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 */ 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 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 */ 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 */ 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 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 442 static uint32_t offset_prel31(uint32_t addr, int32_t offset) 443 { 444 return (addr + offset) & 0x7FFFFFFFUL; 445 } 446 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 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 483 if (gd->flags & GD_FLG_RELOC) { 484 pc = (ulong)state->registers[PC] - gd->reloc_off; 485 lr = (ulong)state->registers[LR] - gd->reloc_off; 486 } else { 487 pc = (ulong)state->registers[PC]; 488 lr = (ulong)state->registers[LR]; 489 } 490 491 printf("\nCall trace:\n"); 492 printf(" PC: [< %08lx >]\n", pc); 493 printf(" LR: [< %08lx >]\n", lr); 494 495 printf("\nStack:\n"); 496 do { 497 if (gd->flags & GD_FLG_RELOC) 498 pc = (ulong)state->registers[PC] - gd->reloc_off; 499 else 500 pc = (ulong)state->registers[PC]; 501 502 printf(" [< %08lx >]\n", pc); 503 } while (unwind_stack_arm32(state, exidx, exidx_sz, 504 kernel_stack, stack, stack_size)); 505 506 printf("\nNOTE: Please use ./scripts/stacktrace.sh to parse trace info\n"); 507 } 508 509 void dump_core_stack(struct pt_regs *regs) 510 { 511 struct unwind_state_arm32 state; 512 ulong exidx = (ulong)__exidx_start; 513 size_t exidx_sz = (ulong)__exidx_end - (ulong)__exidx_start; 514 ulong stack = gd->start_addr_sp; 515 size_t stack_size = CONFIG_SYS_STACK_SIZE; 516 int i; 517 518 /* Don't use memset(), which updates LR ! */ 519 for (i = 0; i < 16; i++) 520 state.registers[i] = 0; 521 state.update_mask = 0; 522 state.start_pc = 0; 523 state.entries = 0; 524 state.insn = 0; 525 state.byte = 0; 526 527 /* r7: Thumb-style frame pointer */ 528 state.registers[7] = regs->ARM_r7; 529 /* r11: ARM-style frame pointer */ 530 state.registers[FP] = regs->ARM_ip; 531 state.registers[SP] = regs->ARM_sp; 532 state.registers[LR] = regs->ARM_lr; 533 state.registers[PC] = regs->ARM_pc; 534 535 print_stack_arm32(&state, exidx, exidx_sz, 536 true, stack, stack_size); 537 } 538 539 void dump_stack(void) 540 { 541 struct pt_regs regs; 542 543 regs.ARM_r7 = read_r7(); 544 regs.ARM_ip = read_fp(); 545 regs.ARM_sp = read_sp(); 546 regs.ARM_lr = read_lr(); 547 regs.ARM_pc = (uint32_t)dump_stack; 548 549 dump_core_stack(®s); 550 } 551