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_vcpu_fault.h>
9#include <sel4vm/arch/processor.h>
10#include <sel4vm/arch/guest_arm_context.h>
11#include <sel4vm/sel4_arch/processor.h>
12#include <sel4vmmplatsupport/arch/guest_vcpu_fault.h>
13
14#include "vcpu_fault_handlers.h"
15
16#include "sysreg_exception.h"
17
18static int ignore_sysreg_exception(vm_vcpu_t *vcpu, sysreg_t *sysreg, bool is_read);
19
20sysreg_entry_t sysreg_table[] = {
21#ifdef CONFIG_ARM_CORTEX_A57
22    /* S3_1_c15_c2_0: Write EL1 CPU Auxiliary Control Register */
23    {
24        .sysreg = { .params.op0 = 3, .params.op1 = 1, .params.op2 = 0, .params.crn = 15, .params.crm = 2 },
25        .sysreg_match_mask = { .hsr_val  = SYSREG_MATCH_ALL_MASK },
26        .handler = ignore_sysreg_exception
27    },
28#endif
29    /* Debug and Trace Register Operations */
30    {
31        .sysreg = { .params.op0 = 2 },
32        .sysreg_match_mask = { .hsr_val = SYSREG_OP0_MASK },
33        .handler = ignore_sysreg_exception
34    },
35};
36
37static int ignore_sysreg_exception(vm_vcpu_t *vcpu, sysreg_t *sysreg, bool is_read)
38{
39    advance_vcpu_fault(vcpu);
40    return 0;
41}
42
43static bool is_sysreg_match(sysreg_t *sysreg, sysreg_entry_t *sysreg_entry)
44{
45    sysreg_t match_a = *sysreg;
46    match_a.hsr_val &= sysreg_entry->sysreg_match_mask.hsr_val;
47    sysreg_t match_b = sysreg_entry->sysreg;
48    return (
49               (match_a.params.op0 == match_b.params.op0) &&
50               (match_a.params.op1 == match_b.params.op1) &&
51               (match_a.params.op2 == match_b.params.op2) &&
52               (match_a.params.crn == match_b.params.crn) &&
53               (match_a.params.crm == match_b.params.crm)
54           );
55}
56
57static sysreg_entry_t *find_sysreg_entry(vm_vcpu_t *vcpu, sysreg_t *sysreg_op)
58{
59    for (int i = 0; i < ARRAY_SIZE(sysreg_table); i++) {
60        sysreg_entry_t *sysreg_entry = &sysreg_table[i];
61        sysreg_t match_sysreg_op = *sysreg_op;
62        if (is_sysreg_match(sysreg_op, sysreg_entry)) {
63            return sysreg_entry;
64        }
65    }
66    return NULL;
67}
68
69int sysreg_exception_handler(vm_vcpu_t *vcpu, uint32_t hsr)
70{
71    sysreg_t sysreg_op;
72    sysreg_op.hsr_val = hsr;
73    sysreg_entry_t *entry = find_sysreg_entry(vcpu, &sysreg_op);
74    if (!entry) {
75        return -1;
76    }
77    return entry->handler(vcpu, &entry->sysreg, sysreg_op.params.direction);
78}
79