Merge remote-tracking branch 'kiszka/master'

[jailhouse.git] / hypervisor / arch / arm / traps.c

diff --git a/hypervisor/arch/arm/traps.c b/hypervisor/arch/arm/traps.c
index d88dc7f542ed34b351a88e9399d63267f02026b5..ae0990f13749cf52bb017d5cdcda86a8ae833f45 100644 (file)
--- a/hypervisor/arch/arm/traps.c
+++ b/hypervisor/arch/arm/traps.c
@@ -189,7 +189,7 @@ void access_cell_reg(struct trap_context *ctx, u8 reg, unsigned long *val,
        }
 }
 
-static void dump_guest_regs(struct per_cpu *cpu_data, struct trap_context *ctx)
+static void dump_guest_regs(struct trap_context *ctx)
 {
        u8 reg;
        unsigned long reg_val;
@@ -205,12 +205,12 @@ static void dump_guest_regs(struct per_cpu *cpu_data, struct trap_context *ctx)
        panic_printk("\n");
 }
 
-static int arch_handle_smc(struct per_cpu *cpu_data, struct trap_context *ctx)
+static int arch_handle_smc(struct trap_context *ctx)
 {
        unsigned long *regs = ctx->regs;
 
-       if (IS_PSCI_FN(regs[0]))
-               regs[0] = psci_dispatch(cpu_data, ctx);
+       if (IS_PSCI_32(regs[0]) || IS_PSCI_UBOOT(regs[0]))
+               regs[0] = psci_dispatch(ctx);
        else
                regs[0] = smc(regs[0], regs[1], regs[2], regs[3]);
 
@@ -219,19 +219,43 @@ static int arch_handle_smc(struct per_cpu *cpu_data, struct trap_context *ctx)
        return TRAP_HANDLED;
 }
 
-static int arch_handle_hvc(struct per_cpu *cpu_data, struct trap_context *ctx)
+static int arch_handle_hvc(struct trap_context *ctx)
 {
        unsigned long *regs = ctx->regs;
 
-       if (IS_PSCI_FN(regs[0]))
-               regs[0] = psci_dispatch(cpu_data, ctx);
+       if (IS_PSCI_32(regs[0]) || IS_PSCI_UBOOT(regs[0]))
+               regs[0] = psci_dispatch(ctx);
        else
                regs[0] = hypercall(regs[0], regs[1], regs[2]);
 
        return TRAP_HANDLED;
 }
 
-static int arch_handle_cp15_64(struct per_cpu *cpu_data, struct trap_context *ctx)
+static int arch_handle_cp15_32(struct trap_context *ctx)
+{
+       u32 opc2        = ctx->esr >> 17 & 0x7;
+       u32 opc1        = ctx->esr >> 14 & 0x7;
+       u32 crn         = ctx->esr >> 10 & 0xf;
+       u32 rt          = ctx->esr >> 5 & 0xf;
+       u32 crm         = ctx->esr >> 1 & 0xf;
+       u32 read        = ctx->esr & 1;
+
+       if (opc1 == 0 && crn == 1 && crm == 0 && opc2 == 1) {
+               /* Do not let the guest disable coherency by writing ACTLR... */
+               if (read) {
+                       unsigned long val;
+                       arm_read_sysreg(ACTLR_EL1, val);
+                       access_cell_reg(ctx, rt, &val, false);
+               }
+               arch_skip_instruction(ctx);
+
+               return TRAP_HANDLED;
+       }
+
+       return TRAP_UNHANDLED;
+}
+
+static int arch_handle_cp15_64(struct trap_context *ctx)
 {
        unsigned long rt_val, rt2_val;
        u32 opc1        = ctx->esr >> 16 & 0x7;
@@ -249,8 +273,8 @@ static int arch_handle_cp15_64(struct per_cpu *cpu_data, struct trap_context *ct
        /* Trapped ICC_SGI1R write */
        if (!read && opc1 == 0 && crm == 12) {
                arch_skip_instruction(ctx);
-               return gicv3_handle_sgir_write(cpu_data,
-                               (u64)rt2_val << 32 | rt_val);
+               gicv3_handle_sgir_write((u64)rt2_val << 32 | rt_val);
+               return TRAP_HANDLED;
        }
 #else
        /* Avoid `unused' warning... */
@@ -263,6 +287,7 @@ static int arch_handle_cp15_64(struct per_cpu *cpu_data, struct trap_context *ct
 
 static const trap_handler trap_handlers[38] =
 {
+       [ESR_EC_CP15_32]        = arch_handle_cp15_32,
        [ESR_EC_CP15_64]        = arch_handle_cp15_64,
        [ESR_EC_HVC]            = arch_handle_hvc,
        [ESR_EC_SMC]            = arch_handle_smc,
@@ -291,15 +316,16 @@ void arch_handle_trap(struct per_cpu *cpu_data, struct registers *guest_regs)
        }
 
        if (trap_handlers[exception_class])
-               ret = trap_handlers[exception_class](cpu_data, &ctx);
+               ret = trap_handlers[exception_class](&ctx);
 
        switch (ret) {
        case TRAP_UNHANDLED:
        case TRAP_FORBIDDEN:
-               panic_printk("FATAL: %s on CPU%d\n", (ret == TRAP_UNHANDLED ?
-                               "unhandled trap" : "forbidden access"),
-                               cpu_data->cpu_id);
-               dump_guest_regs(cpu_data, &ctx);
+               panic_printk("FATAL: %s (exception class 0x%02x)\n",
+                            (ret == TRAP_UNHANDLED ? "unhandled trap" :
+                                                     "forbidden access"),
+                            exception_class);
+               dump_guest_regs(&ctx);
                panic_park();
        }