2 * Jailhouse, a Linux-based partitioning hypervisor
4 * Copyright (c) ARM Limited, 2014
7 * Jean-Philippe Brucker <jean-philippe.brucker@arm.com>
9 * This work is licensed under the terms of the GNU GPL, version 2. See
10 * the COPYING file in the top-level directory.
12 * Condition check code is copied from Linux's
13 * - arch/arm/kernel/opcodes.c
14 * - arch/arm/kvm/emulate.c
17 #include <asm/control.h>
18 #include <asm/gic_common.h>
19 #include <asm/platform.h>
21 #include <asm/traps.h>
22 #include <asm/sysregs.h>
23 #include <jailhouse/printk.h>
24 #include <jailhouse/control.h>
27 * condition code lookup table
28 * index into the table is test code: EQ, NE, ... LT, GT, AL, NV
30 * bit position in short is condition code: NZCV
32 static const unsigned short cc_map[16] = {
33 0xF0F0, /* EQ == Z set */
35 0xCCCC, /* CS == C set */
37 0xFF00, /* MI == N set */
39 0xAAAA, /* VS == V set */
41 0x0C0C, /* HI == C set && Z clear */
42 0xF3F3, /* LS == C clear || Z set */
43 0xAA55, /* GE == (N==V) */
44 0x55AA, /* LT == (N!=V) */
45 0x0A05, /* GT == (!Z && (N==V)) */
46 0xF5FA, /* LE == (Z || (N!=V)) */
47 0xFFFF, /* AL always */
51 /* Check condition field either from ESR or from SPSR in thumb mode */
52 static bool arch_failed_condition(struct trap_context *ctx)
54 u32 class = ESR_EC(ctx->esr);
55 u32 icc = ESR_ICC(ctx->esr);
57 u32 flags = cpsr >> 28;
60 * Trapped instruction is unconditional, already passed the condition
61 * check, or is invalid
63 if (class & 0x30 || class == 0)
66 /* Is condition field valid? */
67 if (icc & ESR_ICC_CV_BIT) {
68 cond = ESR_ICC_COND(icc);
70 /* This can happen in Thumb mode: examine IT state. */
71 unsigned long it = PSR_IT(cpsr);
73 /* it == 0 => unconditional. */
77 /* The cond for this insn works out as the top 4 bits. */
81 /* Compare the apsr flags with the condition code */
82 if ((cc_map[cond] >> flags) & 1)
89 * When exceptions occur while instructions are executed in Thumb IF-THEN
90 * blocks, the ITSTATE field of the CPSR is not advanced (updated), so we have
91 * to do this little bit of work manually. The fields map like this:
93 * IT[7:0] -> CPSR[26:25],CPSR[15:10]
95 static void arch_advance_itstate(struct trap_context *ctx)
97 unsigned long itbits, cond;
98 unsigned long cpsr = ctx->cpsr;
100 if (!(cpsr & PSR_IT_MASK(0xff)))
103 itbits = PSR_IT(cpsr);
106 if ((itbits & 0x7) == 0)
107 /* One instruction left in the block, next itstate is 0 */
110 itbits = (itbits << 1) & 0x1f;
112 itbits |= (cond << 5);
113 cpsr &= ~PSR_IT_MASK(0xff);
114 cpsr |= PSR_IT_MASK(itbits);
119 void arch_skip_instruction(struct trap_context *ctx)
121 u32 instruction_length = ESR_IL(ctx->esr);
123 ctx->pc += (instruction_length ? 4 : 2);
124 arch_advance_itstate(ctx);
127 void access_cell_reg(struct trap_context *ctx, u8 reg, unsigned long *val,
130 unsigned long mode = ctx->cpsr & PSR_MODE_MASK;
134 access_usr_reg(ctx, reg, val, is_read);
137 if (mode == PSR_FIQ_MODE)
138 access_fiq_reg(reg, val, is_read);
140 access_usr_reg(ctx, reg, val, is_read);
147 * lr is saved on the stack, as it is not banked in HYP
148 * mode. sp is banked, so lr is at offset 13 in the USR
152 access_banked_reg(usr, reg, val, is_read);
154 access_usr_reg(ctx, 13, val, is_read);
157 access_banked_reg(svc, reg, val, is_read);
160 access_banked_reg(und, reg, val, is_read);
163 access_banked_reg(abt, reg, val, is_read);
166 access_banked_reg(irq, reg, val, is_read);
169 access_banked_reg(fiq, reg, val, is_read);
175 * A trapped instruction that accesses the PC? Probably a bug,
176 * but nothing seems to prevent it.
178 printk("WARNING: trapped instruction attempted to explicitly "
186 /* Programming error */
187 printk("ERROR: attempt to write register %d\n", reg);
192 static void dump_guest_regs(struct trap_context *ctx)
195 unsigned long reg_val;
197 panic_printk("pc=0x%08x cpsr=0x%08x esr=0x%08x\n", ctx->pc, ctx->cpsr,
199 for (reg = 0; reg < 15; reg++) {
200 access_cell_reg(ctx, reg, ®_val, true);
201 panic_printk("r%d=0x%08x ", reg, reg_val);
202 if ((reg + 1) % 4 == 0)
208 static int arch_handle_smc(struct trap_context *ctx)
210 unsigned long *regs = ctx->regs;
212 if (IS_PSCI_FN(regs[0]))
213 regs[0] = psci_dispatch(ctx);
215 regs[0] = smc(regs[0], regs[1], regs[2], regs[3]);
217 arch_skip_instruction(ctx);
222 static int arch_handle_hvc(struct trap_context *ctx)
224 unsigned long *regs = ctx->regs;
226 if (IS_PSCI_FN(regs[0]))
227 regs[0] = psci_dispatch(ctx);
229 regs[0] = hypercall(regs[0], regs[1], regs[2]);
234 static int arch_handle_cp15_32(struct trap_context *ctx)
236 u32 opc2 = ctx->esr >> 17 & 0x7;
237 u32 opc1 = ctx->esr >> 14 & 0x7;
238 u32 crn = ctx->esr >> 10 & 0xf;
239 u32 rt = ctx->esr >> 5 & 0xf;
240 u32 crm = ctx->esr >> 1 & 0xf;
241 u32 read = ctx->esr & 1;
243 if (opc1 == 0 && crn == 1 && crm == 0 && opc2 == 1) {
244 /* Do not let the guest disable coherency by writing ACTLR... */
247 arm_read_sysreg(ACTLR_EL1, val);
248 access_cell_reg(ctx, rt, &val, false);
250 arch_skip_instruction(ctx);
255 return TRAP_UNHANDLED;
258 static int arch_handle_cp15_64(struct trap_context *ctx)
260 unsigned long rt_val, rt2_val;
261 u32 opc1 = ctx->esr >> 16 & 0x7;
262 u32 rt2 = ctx->esr >> 10 & 0xf;
263 u32 rt = ctx->esr >> 5 & 0xf;
264 u32 crm = ctx->esr >> 1 & 0xf;
265 u32 read = ctx->esr & 1;
268 access_cell_reg(ctx, rt, &rt_val, true);
269 access_cell_reg(ctx, rt2, &rt2_val, true);
272 #ifdef CONFIG_ARM_GIC_V3
273 /* Trapped ICC_SGI1R write */
274 if (!read && opc1 == 0 && crm == 12) {
275 arch_skip_instruction(ctx);
276 return gicv3_handle_sgir_write((u64)rt2_val << 32 | rt_val);
279 /* Avoid `unused' warning... */
284 return TRAP_UNHANDLED;
287 static const trap_handler trap_handlers[38] =
289 [ESR_EC_CP15_32] = arch_handle_cp15_32,
290 [ESR_EC_CP15_64] = arch_handle_cp15_64,
291 [ESR_EC_HVC] = arch_handle_hvc,
292 [ESR_EC_SMC] = arch_handle_smc,
293 [ESR_EC_DABT] = arch_handle_dabt,
296 void arch_handle_trap(struct per_cpu *cpu_data, struct registers *guest_regs)
298 struct trap_context ctx;
300 int ret = TRAP_UNHANDLED;
302 arm_read_banked_reg(ELR_hyp, ctx.pc);
303 arm_read_banked_reg(SPSR_hyp, ctx.cpsr);
304 arm_read_sysreg(ESR_EL2, ctx.esr);
305 exception_class = ESR_EC(ctx.esr);
306 ctx.regs = guest_regs->usr;
309 * On some implementations, instructions that fail their condition check
312 if (arch_failed_condition(&ctx)) {
313 arch_skip_instruction(&ctx);
314 goto restore_context;
317 if (trap_handlers[exception_class])
318 ret = trap_handlers[exception_class](&ctx);
323 panic_printk("FATAL: %s (exception class 0x%02x)\n",
324 (ret == TRAP_UNHANDLED ? "unhandled trap" :
327 dump_guest_regs(&ctx);
332 arm_write_banked_reg(SPSR_hyp, ctx.cpsr);
333 arm_write_banked_reg(ELR_hyp, ctx.pc);