1/* 2 * Copyright 2019, Data61, CSIRO (ABN 41 687 119 230) 3 * 4 * SPDX-License-Identifier: BSD-2-Clause 5 */ 6 7#include <sel4vm/guest_vm.h> 8#include <sel4vm/guest_ram.h> 9#include <sel4vm/guest_vm_util.h> 10 11#include "fault.h" 12#include "arch_fault.h" 13 14#include <sel4/sel4.h> 15#include <sel4/messages.h> 16#include <vka/capops.h> 17 18#include <utils/ansi.h> 19#include <stdlib.h> 20#include <sel4/sel4_arch/constants.h> 21 22//#define DEBUG_FAULTS 23//#define DEBUG_ERRATA 24 25#ifdef DEBUG_FAULTS 26#define DFAULT(...) printf(__VA_ARGS__) 27#else 28#define DFAULT(...) do{}while(0) 29#endif 30 31#ifdef DEBUG_ERRATA 32#define DERRATA(...) printf(__VA_ARGS__) 33#else 34#define DERRATA(...) do{}while(0) 35#endif 36 37#ifdef PLAT_EXYNOS5250 38/* Stores in thumb mode trigger this errata */ 39#define HAS_ERRATA766422(f) ( fault_is_write(f) && fault_is_thumb(f)) 40#else 41#define HAS_ERRATA766422(f) 0 42#endif 43 44#define HSR_INST32 BIT(25) 45#define HSR_IS_INST32(x) ((x) & HSR_INST32) 46#define HSR_SYNDROME_VALID BIT(24) 47#define HSR_IS_SYNDROME_VALID(hsr) ((hsr) & HSR_SYNDROME_VALID) 48#define HSR_SYNDROME_RT(x) (((x) >> 16) & SRT_MASK) 49#define HSR_SYNDROME_WIDTH(x) (((x) >> 22) & 0x3) 50 51#define CONTENT_REGS BIT(0) 52#define CONTENT_DATA BIT(1) 53#define CONTENT_INST BIT(2) 54#define CONTENT_WIDTH BIT(3) 55#define CONTENT_STAGE BIT(4) 56#define CONTENT_PMODE BIT(5) 57 58/************************* 59 *** Primary functions *** 60 *************************/ 61static inline int thumb_is_32bit_instruction(seL4_Word instruction) 62{ 63 switch ((instruction >> 11) & 0x1f) { 64 case 0b11101: 65 case 0b11110: 66 case 0b11111: 67 return 1; 68 default: 69 return 0; 70 } 71} 72 73static int maybe_fetch_fault_instruction(fault_t *f) 74{ 75 if ((f->content & CONTENT_INST) == 0) { 76 seL4_Word inst = 0; 77 /* Fetch the instruction */ 78 if (vm_ram_touch(f->vcpu->vm, f->ip, 4, vm_guest_ram_read_callback, &inst)) { 79 return -1; 80 } 81 /* Fixup the instruction */ 82 if (fault_is_thumb(f)) { 83 if (thumb_is_32bit_instruction(inst)) { 84 f->fsr |= HSR_INST32; 85 } 86 if (HSR_IS_INST32(f->fsr)) { 87 /* Swap half words for a 32 bit instruction */ 88 inst = ((inst & 0xffff)) << 16 | ((inst >> 16) & 0xffff); 89 } else { 90 /* Mask instruction for 16 bit instruction */ 91 inst &= 0xffff; 92 } 93 } else { 94 /* All ARM instructions are 32 bit so force the HSR flag to be set */ 95 f->fsr |= HSR_INST32; 96 } 97 f->instruction = inst; 98 f->content |= CONTENT_INST; 99 } 100 return 0; 101} 102 103static int errata766422_get_rt(fault_t *f, seL4_Word hsr) 104{ 105 seL4_Word inst; 106 int err; 107 err = maybe_fetch_fault_instruction(f); 108 inst = f->instruction; 109 if (err) { 110 return err; 111 } 112 if (HSR_IS_INST32(hsr)) { 113 DERRATA("Errata766422 @ 0x%08x (0x%08x)\n", fault_get_ctx(f)->pc, inst); 114 if ((inst & 0xff700000) == 0xf8400000) { 115 return (inst >> 12) & 0xf; 116 } else if ((inst & 0xfff00000) == 0xf8800000) { 117 return (inst >> 12) & 0xf; 118 } else if ((inst & 0xfff00000) == 0xf0000000) { 119 return (inst >> 12) & 0xf; 120 } else if ((inst & 0x0e500000) == 0x06400000) { 121 return (inst >> 12) & 0xf; 122 } else if ((inst & 0xfff00000) == 0xf8000000) { 123 return (inst >> 12) & 0xf; 124 } else { 125 printf("Unable to decode inst %08lx\n", (long) inst); 126 return -1; 127 } 128 } else { 129 DERRATA("Errata766422 @ 0x%08lx (0x%04lx)\n", (long) fault_get_ctx(f)->pc, (long) inst); 130 /* 16 bit insts */ 131 if ((inst & 0xf800) == 0x6000) { 132 return (inst >> 0) & 0x7; 133 } else if ((inst & 0xf800) == 0x9000) { 134 return (inst >> 8) & 0x7; 135 } else if ((inst & 0xf800) == 0x5000) { 136 return (inst >> 0) & 0x7; 137 } else if ((inst & 0xfe00) == 0x5400) { 138 return (inst >> 0) & 0x7; 139 } else if ((inst & 0xf800) == 0x7000) { 140 return (inst >> 0) & 0x7; 141 } else if ((inst & 0xf800) == 0x8000) { 142 return (inst >> 0) & 0x7; 143 } else { 144 printf("Unable to decode inst 0x%04lx\n", (long) inst); 145 return -1; 146 } 147 } 148} 149 150static int decode_instruction(fault_t *f) 151{ 152 seL4_Word inst; 153 maybe_fetch_fault_instruction(f); 154 inst = f->instruction; 155 /* Single stage by default */ 156 f->stage = 1; 157 f->content |= CONTENT_STAGE; 158 /* Decode */ 159 if (fault_is_thumb(f)) { 160 if (thumb_is_32bit_instruction(inst)) { 161 f->fsr |= BIT(25); /* 32 bit instruction */ 162 /* 32 BIT THUMB insts */ 163 if ((inst & 0xff700000) == 0xf8400000) { 164 print_fault(f); 165 assert(!"No data width"); 166 return (inst >> 12) & 0xf; 167 } else if ((inst & 0xfff00000) == 0xf8800000) { 168 print_fault(f); 169 assert(!"No data width"); 170 return (inst >> 12) & 0xf; 171 } else if ((inst & 0xfff00000) == 0xf0000000) { 172 print_fault(f); 173 assert(!"No data width"); 174 return (inst >> 12) & 0xf; 175 } else if ((inst & 0x0e500000) == 0x06400000) { 176 print_fault(f); 177 assert(!"No data width"); 178 return (inst >> 12) & 0xf; 179 } else if ((inst & 0xfff00000) == 0xf8000000) { 180 print_fault(f); 181 assert(!"No data width"); 182 return (inst >> 12) & 0xf; 183 } else if ((inst & 0xfe400000) == 0xe8400000) { /* LDRD/STRD */ 184 int rt; 185 if ((f->content & CONTENT_STAGE) == 0) { 186 f->stage = 2; 187 f->width = WIDTH_DOUBLEWORD; 188 f->content |= CONTENT_WIDTH | CONTENT_STAGE; 189 } 190 f->addr = f->base_addr + ((2 - f->stage) * sizeof(seL4_Word)); 191 rt = ((inst >> 12) & 0xf) + (2 - f->stage); 192 return rt; 193 } else { 194 printf("Unable to decode THUMB32 inst 0x%08lx\n", (long) inst); 195 print_fault(f); 196 return -1; 197 } 198 } else { 199 /* 16 bit THUMB insts */ 200 if ((inst & 0xf800) == 0x6000) { 201 print_fault(f); 202 assert(!"No data width"); 203 return (inst >> 0) & 0x7; 204 } else if ((inst & 0xf800) == 0x9000) { 205 print_fault(f); 206 assert(!"No data width"); 207 return (inst >> 8) & 0x7; 208 } else if ((inst & 0xf800) == 0x5000) { 209 print_fault(f); 210 assert(!"No data width"); 211 return (inst >> 0) & 0x7; 212 } else if ((inst & 0xfe00) == 0x5400) { 213 print_fault(f); 214 assert(!"No data width"); 215 return (inst >> 0) & 0x7; 216 } else if ((inst & 0xf800) == 0x7000) { 217 print_fault(f); 218 assert(!"No data width"); 219 return (inst >> 0) & 0x7; 220 } else if ((inst & 0xf800) == 0x8000) { 221 print_fault(f); 222 assert(!"No data width"); 223 return (inst >> 0) & 0x7; 224 } else { 225 printf("Unable to decode THUMB16 inst 0x%04lx\n", (long) inst); 226 print_fault(f); 227 return -1; 228 } 229 } 230 } else { 231 printf("32 bit ARM insts not decoded\n"); 232 print_fault(f); 233 return -1; 234 } 235} 236 237static int get_rt(fault_t *f) 238{ 239 240 /* Save processor mode in fault struct */ 241 if ((f->content & CONTENT_PMODE) == 0) { 242#ifdef CONFIG_ARCH_AARCH64 243#else 244 f->pmode = fault_get_ctx(f)->cpsr & 0x1f; 245#endif 246 f->content |= CONTENT_PMODE; 247 } 248 249 int rt; 250 if (HSR_IS_SYNDROME_VALID(f->fsr)) { 251 if (HAS_ERRATA766422(f)) { 252 rt = errata766422_get_rt(f, f->fsr); 253 } else { 254 rt = HSR_SYNDROME_RT(f->fsr); 255 //printf("here rt %d\n", rt); 256 } 257 } else { 258#ifdef CONFIG_ARCH_AARCH64 259 printf("decode_insturction for arm64 not implemented\n"); 260#endif 261 rt = decode_instruction(f); 262 } 263 assert(rt >= 0); 264 return rt; 265} 266 267fault_t *fault_init(vm_vcpu_t *vcpu) 268{ 269 fault_t *fault; 270 seL4_Error err; 271 fault = (fault_t *)calloc(1, sizeof(*fault)); 272 if (fault != NULL) { 273 fault->vcpu = vcpu; 274 /* Reserve a slot for saving reply caps */ 275 err = vka_cspace_alloc_path(vcpu->vm->vka, &fault->reply_cap); 276 if (err) { 277 free(fault); 278 fault = NULL; 279 } 280 } 281 return fault; 282} 283 284int new_vcpu_fault(fault_t *fault, uint32_t hsr) 285{ 286 int err; 287 assert(fault_handled(fault)); 288 fault->type = VCPU; 289 fault->fsr = hsr; 290 fault->instruction = 0; 291 fault->data = 0; 292 fault->width = -1; 293 fault->content = 0; 294 fault->stage = 1; 295 assert(fault->reply_cap.capPtr); 296 err = vka_cnode_saveCaller(&fault->reply_cap); 297 assert(!err); 298 299 return err; 300} 301 302int new_memory_fault(fault_t *fault) 303{ 304 seL4_Word ip, addr, fsr; 305 seL4_Word is_prefetch; 306 int err; 307 vm_t *vm; 308 309 vm = fault->vcpu->vm; 310 assert(vm); 311 312 assert(fault_handled(fault)); 313 314 /* First store message registers on the stack to free our message regs */ 315 is_prefetch = seL4_GetMR(seL4_VMFault_PrefetchFault); 316 addr = seL4_GetMR(seL4_VMFault_Addr), 317 fsr = seL4_GetMR(seL4_VMFault_FSR); 318 ip = seL4_GetMR(seL4_VMFault_IP); 319 DFAULT("%s: New fault @ 0x%x from PC 0x%x\n", vm->vm_name, addr, ip); 320 /* Create the fault object */ 321 fault->type = is_prefetch ? PREFETCH : DATA; 322 fault->ip = ip; 323 fault->base_addr = fault->addr = addr; 324 fault->fsr = fsr; 325 fault->instruction = 0; 326 fault->data = 0; 327 fault->width = -1; 328 if (fault_is_data(fault)) { 329 if (fault_is_read(fault)) { 330 /* No need to load data */ 331 fault->content = CONTENT_DATA; 332 } else { 333 fault->content = 0; 334 } 335 if (HSR_IS_SYNDROME_VALID(fault->fsr)) { 336 fault->stage = 1; 337 fault->content |= CONTENT_STAGE; 338 } else { 339 fault->stage = -1; 340 } 341 } else { 342 /* No need to load width or data */ 343 fault->content = CONTENT_DATA | CONTENT_WIDTH; 344 } 345 346 /* Gather additional information */ 347 assert(fault->reply_cap.capPtr); 348 err = vka_cnode_saveCaller(&fault->reply_cap); 349 assert(!err); 350 351 return err; 352} 353 354int abandon_fault(fault_t *fault) 355{ 356 /* Nothing to do here */ 357 DFAULT("%s: Release fault @ 0x%x from PC 0x%x\n", 358 fault->vcpu->vm->vm_name, fault->addr, fault->ip); 359 return 0; 360} 361 362 363int restart_fault(fault_t *fault) 364{ 365 /* Send the reply */ 366 fault->stage = 0; 367 seL4_MessageInfo_t reply; 368 reply = seL4_MessageInfo_new(0, 0, 0, 0); 369 DFAULT("%s: Restart fault @ 0x%x from PC 0x%x\n", 370 fault->vcpu->vm->vm_name, fault->addr, fault->ip); 371 seL4_Send(fault->reply_cap.capPtr, reply); 372 /* Clean up */ 373 return abandon_fault(fault); 374} 375 376int ignore_fault(fault_t *fault) 377{ 378 seL4_UserContext *regs; 379 int err; 380 381 regs = fault_get_ctx(fault); 382 /* Advance the PC */ 383 regs->pc += fault_is_32bit_instruction(fault) ? 4 : 2; 384 /* Write back CPU registers */ 385 err = seL4_TCB_WriteRegisters(vm_get_vcpu_tcb(fault->vcpu), false, 0, 386 sizeof(*regs) / sizeof(regs->pc), regs); 387 assert(!err); 388 if (err) { 389 abandon_fault(fault); 390 return err; 391 } 392 /* Reply to thread */ 393 return restart_fault(fault); 394} 395 396int advance_fault(fault_t *fault) 397{ 398 /* If data was to be read, load it into the user context */ 399 if (fault_is_data(fault) && fault_is_read(fault)) { 400 /* Get register opearand */ 401 int rt = get_rt(fault); 402 403 /* Decode whether operand is banked */ 404 int reg = decode_vcpu_reg(rt, fault); 405 if (reg == seL4_VCPUReg_Num) { 406 /* register is not banked, use seL4_UserContext */ 407 seL4_Word *reg_ctx = decode_rt(rt, fault_get_ctx(fault)); 408 *reg_ctx = fault_emulate(fault, *reg_ctx); 409 } else { 410 /* register is banked, use vcpu invocations */ 411 seL4_ARM_VCPU_ReadRegs_t res = seL4_ARM_VCPU_ReadRegs(fault->vcpu->vcpu.cptr, reg); 412 if (res.error) { 413 ZF_LOGF("Read registers failed"); 414 return -1; 415 } 416 int error = seL4_ARM_VCPU_WriteRegs(fault->vcpu->vcpu.cptr, reg, fault_emulate(fault, res.value)); 417 if (error) { 418 ZF_LOGF("Write registers failed"); 419 return -1; 420 } 421 } 422 } 423 DFAULT("%s: Emulate fault @ 0x%x from PC 0x%x\n", 424 fault->vcpu->vm->vm_name, fault->addr, fault->ip); 425 /* If this is the final stage of the fault, return to user */ 426 assert(fault->stage > 0); 427 fault->stage--; 428 if (fault->stage) { 429 /* Data becomes invalid */ 430 fault->content &= ~CONTENT_DATA; 431 return 0; 432 } else { 433 return ignore_fault(fault); 434 } 435} 436 437seL4_Word fault_emulate(fault_t *f, seL4_Word o) 438{ 439 seL4_Word n, m, s; 440 s = (f->addr & 0x3) * 8; 441 m = fault_get_data_mask(f); 442 n = fault_get_data(f); 443 if (fault_is_read(f)) { 444 /* Read data must be shifted to lsb */ 445 return (o & ~(m >> s)) | ((n & m) >> s); 446 } else { 447 /* Data to write must be shifted left to compensate for alignment */ 448 return (o & ~m) | ((n << s) & m); 449 } 450} 451 452void print_fault(fault_t *fault) 453{ 454 printf("--------\n"); 455 printf(ANSI_COLOR(RED, BOLD)); 456 printf("Pagefault from [%s]: %s %s " 457 "@ PC: 0x"XFMT" IPA: 0x"XFMT", FSR: 0x"XFMT "\n", 458 fault->vcpu->vm->vm_name, 459 fault_is_read(fault) ? "read" : "write", 460 fault_is_prefetch(fault) ? "prefetch fault" : "fault", 461 fault->ip, 462 fault->addr, 463 fault->fsr); 464 printf("Context:\n"); 465 print_ctx_regs(fault_get_ctx(fault)); 466 printf(ANSI_COLOR(RESET)); 467 printf("--------\n"); 468} 469 470seL4_Word fault_get_data_mask(fault_t *f) 471{ 472 seL4_Word mask = 0; 473 seL4_Word addr = f->addr; 474 switch (fault_get_width(f)) { 475 case WIDTH_BYTE: 476 mask = 0x000000ff; 477 assert(!(addr & 0x0)); 478 break; 479 case WIDTH_HALFWORD: 480 mask = 0x0000ffff; 481 assert(!(addr & 0x1)); 482 break; 483 case WIDTH_WORD: 484 mask = 0xffffffff; 485 assert(!(addr & 0x3)); 486 break; 487 case WIDTH_DOUBLEWORD: 488 mask = ~mask; 489 break; 490 default: 491 /* Should never get here... Keep the compiler happy */ 492 assert(0); 493 return 0; 494 } 495 mask <<= (addr & 0x3) * 8; 496 return mask; 497} 498 499/************************* 500 *** Getters / Setters *** 501 *************************/ 502 503seL4_Word fault_get_data(fault_t *f) 504{ 505 if ((f->content & CONTENT_DATA) == 0) { 506 /* Get register opearand */ 507 int rt = get_rt(f); 508 509 /* Decode whether register is banked */ 510 int reg = decode_vcpu_reg(rt, f); 511 seL4_Word data; 512 if (reg == seL4_VCPUReg_Num) { 513 /* Not banked, use seL4_UserContext */ 514 data = *decode_rt(rt, fault_get_ctx(f)); 515 } else { 516 /* Banked, use VCPU invocations */ 517 seL4_ARM_VCPU_ReadRegs_t res = seL4_ARM_VCPU_ReadRegs(f->vcpu->vcpu.cptr, reg); 518 if (res.error) { 519 ZF_LOGF("Read registers failed"); 520 } 521 data = res.value; 522 } 523 fault_set_data(f, data); 524 } 525 return f->data; 526} 527 528void fault_set_data(fault_t *f, seL4_Word data) 529{ 530 f->data = data; 531 f->content |= CONTENT_DATA; 532} 533 534seL4_Word fault_get_address(fault_t *f) 535{ 536 return f->addr; 537} 538 539seL4_Word fault_get_fsr(fault_t *f) 540{ 541 return f->fsr; 542} 543 544seL4_UserContext *fault_get_ctx(fault_t *f) 545{ 546 if ((f->content & CONTENT_REGS) == 0) { 547 int err; 548 err = seL4_TCB_ReadRegisters(vm_get_vcpu_tcb(f->vcpu), false, 0, 549 sizeof(f->regs) / sizeof(f->regs.pc), 550 &f->regs); 551 assert(!err); 552 f->content |= CONTENT_REGS; 553 } 554 return &f->regs; 555} 556 557void fault_set_ctx(fault_t *f, seL4_UserContext *ctx) 558{ 559 f->regs = *ctx; 560 f->content |= CONTENT_REGS; 561} 562 563int fault_handled(fault_t *f) 564{ 565 return f->stage == 0; 566} 567 568int fault_is_prefetch(fault_t *f) 569{ 570 return f->type == PREFETCH; 571} 572 573int fault_is_wfi(fault_t *f) 574{ 575 return HSR_EXCEPTION_CLASS(f->fsr) == HSR_WFx_EXCEPTION; 576} 577 578int fault_is_vcpu(fault_t *f) 579{ 580 return f->type == VCPU; 581} 582 583int fault_is_32bit_instruction(fault_t *f) 584{ 585 if (fault_is_vcpu(f)) { 586 return !fault_is_thumb(f); 587 } 588 if (!HSR_IS_SYNDROME_VALID(f->fsr)) { 589 /* (maybe) Trigger a decode to update the fsr. */ 590 fault_get_width(f); 591 } 592 return fault_get_fsr(f) & BIT(25); 593} 594 595enum fault_width fault_get_width(fault_t *f) 596{ 597 if ((f->content & CONTENT_WIDTH) == 0) { 598 if (HSR_IS_SYNDROME_VALID(f->fsr)) { 599 switch (HSR_SYNDROME_WIDTH(f->fsr)) { 600 case 0: 601 f->width = WIDTH_BYTE; 602 break; 603 case 1: 604 f->width = WIDTH_HALFWORD; 605 break; 606 case 2: 607 f->width = WIDTH_WORD; 608 break; 609 case 3: 610 f->width = WIDTH_DOUBLEWORD; 611 break; 612 default: 613 print_fault(f); 614 assert(0); 615 return 0; 616 } 617 f->content |= CONTENT_WIDTH; 618 } else { 619 int rt; 620 rt = decode_instruction(f); 621 assert(rt >= 0); 622 } 623 } 624 return f->width; 625} 626 627size_t fault_get_width_size(fault_t *fault) 628{ 629 enum fault_width width = fault_get_width(fault); 630 switch (width) { 631 case WIDTH_DOUBLEWORD: 632 return sizeof(long long); 633 case WIDTH_WORD: 634 return sizeof(int); 635 case WIDTH_HALFWORD: 636 return sizeof(short); 637 case WIDTH_BYTE: 638 return sizeof(char); 639 default: 640 return 0; 641 } 642} 643