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