/* Injects an Abort exception into the current vcpu, PC is the exact
* address of the faulting instruction (without pipeline
* adjustments). See TakePrefetchAbortException and
* TakeDataAbortException pseudocode in ARM ARM.
*/
static void inject_abt32_exception(struct cpu_user_regs *regs,
int prefetch,
register_t addr)
{
uint32_t spsr = regs->cpsr;
int is_thumb = (regs->cpsr & PSR_THUMB);
/* Saved PC points to the instruction past the faulting instruction. */
uint32_t return_offset = is_thumb ? 4 : 0;
register_t fsr;
BUG_ON( !is_pv32_domain(current->domain) );
cpsr_switch_mode(regs, PSR_MODE_ABT);
/* Update banked registers */
regs->spsr_abt = spsr;
regs->lr_abt = regs->pc32 + return_offset;
regs->pc32 = exception_handler(prefetch ? VECTOR32_PABT : VECTOR32_DABT);
/* Inject a debug fault, best we can do right now */
if ( READ_SYSREG(TCR_EL1) & TTBCR_EAE )
fsr = FSR_LPAE | FSRL_STATUS_DEBUG;
else
fsr = FSRS_FS_DEBUG;
if ( prefetch )
{
/* Set IFAR and IFSR */
#ifdef CONFIG_ARM_32
WRITE_SYSREG(addr, IFAR);
WRITE_SYSREG(fsr, IFSR);
#else
/* FAR_EL1[63:32] is AArch32 register IFAR */
register_t far = READ_SYSREG(FAR_EL1) & 0xffffffffUL;
far |= addr << 32;
WRITE_SYSREG(far, FAR_EL1);
WRITE_SYSREG(fsr, IFSR32_EL2);
#endif
}
else
{
#ifdef CONFIG_ARM_32
/* Set DFAR and DFSR */
WRITE_SYSREG(addr, DFAR);
WRITE_SYSREG(fsr, DFSR);
#else
/* FAR_EL1[31:0] is AArch32 register DFAR */
register_t far = READ_SYSREG(FAR_EL1) & ~0xffffffffUL;
far |= addr;
WRITE_SYSREG(far, FAR_EL1);
/* ESR_EL1 is AArch32 register DFSR */
WRITE_SYSREG(fsr, ESR_EL1);
#endif
}
}
int do_psci_cpu_on(uint32_t vcpuid, register_t entry_point)
{
struct vcpu *v;
struct domain *d = current->domain;
struct vcpu_guest_context *ctxt;
int rc;
int is_thumb = entry_point & 1;
if ( (vcpuid < 0) || (vcpuid >= MAX_VIRT_CPUS) )
return PSCI_EINVAL;
if ( vcpuid >= d->max_vcpus || (v = d->vcpu[vcpuid]) == NULL )
return PSCI_EINVAL;
/* THUMB set is not allowed with 64-bit domain */
if ( is_pv64_domain(d) && is_thumb )
return PSCI_EINVAL;
if ( (ctxt = alloc_vcpu_guest_context()) == NULL )
return PSCI_DENIED;
vgic_clear_pending_irqs(v);
memset(ctxt, 0, sizeof(*ctxt));
ctxt->user_regs.pc64 = (u64) entry_point;
ctxt->sctlr = SCTLR_GUEST_INIT;
ctxt->ttbr0 = 0;
ctxt->ttbr1 = 0;
ctxt->ttbcr = 0; /* Defined Reset Value */
if ( is_pv32_domain(d) )
ctxt->user_regs.cpsr = PSR_GUEST32_INIT;
#ifdef CONFIG_ARM_64
else
ctxt->user_regs.cpsr = PSR_GUEST64_INIT;
#endif
/* Start the VCPU with THUMB set if it's requested by the kernel */
if ( is_thumb )
ctxt->user_regs.cpsr |= PSR_THUMB;
ctxt->flags = VGCF_online;
domain_lock(d);
rc = arch_set_info_guest(v, ctxt);
free_vcpu_guest_context(ctxt);
if ( rc < 0 )
{
domain_unlock(d);
return PSCI_DENIED;
}
domain_unlock(d);
vcpu_wake(v);
return PSCI_SUCCESS;
}
static void inject_dabt_exception(struct cpu_user_regs *regs,
register_t addr,
int instr_len)
{
if ( is_pv32_domain(current->domain) )
inject_dabt32_exception(regs, addr);
#ifdef CONFIG_ARM_64
else
inject_dabt64_exception(regs, addr, instr_len);
#endif
}
void vcpu_regs_user_to_hyp(struct vcpu *vcpu,
const struct vcpu_guest_core_regs *regs)
{
#define C(hyp,user) vcpu->arch.cpu_info->guest_cpu_user_regs.hyp = regs->user
ALLREGS;
if ( is_pv32_domain(vcpu->domain) )
{
ALLREGS32;
}
else
{
ALLREGS64;
}
#undef C
}
/* Injects an Undefined Instruction exception into the current vcpu,
* PC is the exact address of the faulting instruction (without
* pipeline adjustments). See TakeUndefInstrException pseudocode in
* ARM ARM.
*/
static void inject_undef32_exception(struct cpu_user_regs *regs)
{
uint32_t spsr = regs->cpsr;
int is_thumb = (regs->cpsr & PSR_THUMB);
/* Saved PC points to the instruction past the faulting instruction. */
uint32_t return_offset = is_thumb ? 2 : 4;
BUG_ON( !is_pv32_domain(current->domain) );
/* Update processor mode */
cpsr_switch_mode(regs, PSR_MODE_UND);
/* Update banked registers */
regs->spsr_und = spsr;
regs->lr_und = regs->pc32 + return_offset;
/* Branch to exception vector */
regs->pc32 = exception_handler(VECTOR32_UND);
}
/* Inject an undefined exception into a 64 bit guest */
static void inject_undef64_exception(struct cpu_user_regs *regs, int instr_len)
{
union hsr esr = {
.iss = 0,
.len = instr_len,
.ec = HSR_EC_UNKNOWN,
};
BUG_ON( is_pv32_domain(current->domain) );
regs->spsr_el1 = regs->cpsr;
regs->elr_el1 = regs->pc;
regs->cpsr = PSR_MODE_EL1h | PSR_ABT_MASK | PSR_FIQ_MASK | \
PSR_IRQ_MASK | PSR_DBG_MASK;
regs->pc = READ_SYSREG(VBAR_EL1) + VECTOR64_CURRENT_SPx_SYNC;
WRITE_SYSREG32(esr.bits, ESR_EL1);
}
/* Inject an abort exception into a 64 bit guest */
static void inject_abt64_exception(struct cpu_user_regs *regs,
int prefetch,
register_t addr,
int instr_len)
{
union hsr esr = {
.iss = 0,
.len = instr_len,
};
/*
* Trap may have been taken from EL0, which might be in AArch32
* mode (PSR_MODE_BIT set), or in AArch64 mode (PSR_MODE_EL0t).
*
* Since we know the kernel must be 64-bit any trap from a 32-bit
* mode must have been from EL0.
*/
if ( psr_mode_is_32bit(regs->cpsr) || psr_mode(regs->cpsr,PSR_MODE_EL0t) )
esr.ec = prefetch
? HSR_EC_INSTR_ABORT_LOWER_EL : HSR_EC_DATA_ABORT_LOWER_EL;
else
esr.ec = prefetch
? HSR_EC_INSTR_ABORT_CURR_EL : HSR_EC_DATA_ABORT_CURR_EL;
BUG_ON( is_pv32_domain(current->domain) );
regs->spsr_el1 = regs->cpsr;
regs->elr_el1 = regs->pc;
regs->cpsr = PSR_MODE_EL1h | PSR_ABT_MASK | PSR_FIQ_MASK | \
PSR_IRQ_MASK | PSR_DBG_MASK;
regs->pc = READ_SYSREG(VBAR_EL1) + VECTOR64_CURRENT_SPx_SYNC;
WRITE_SYSREG(addr, FAR_EL1);
WRITE_SYSREG32(esr.bits, ESR_EL1);
}
static void inject_dabt64_exception(struct cpu_user_regs *regs,
register_t addr,
int instr_len)
{
inject_abt64_exception(regs, 0, addr, instr_len);
}
static void inject_iabt64_exception(struct cpu_user_regs *regs,
register_t addr,
int instr_len)
{
inject_abt64_exception(regs, 1, addr, instr_len);
}
