From patchwork Wed Dec 10 20:00:02 2014 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Andrew Jones X-Patchwork-Id: 5471941 Return-Path: X-Original-To: patchwork-kvm@patchwork.kernel.org Delivered-To: patchwork-parsemail@patchwork2.web.kernel.org Received: from mail.kernel.org (mail.kernel.org [198.145.19.201]) by patchwork2.web.kernel.org (Postfix) with ESMTP id B5AFEBEEA8 for ; Wed, 10 Dec 2014 20:00:56 +0000 (UTC) Received: from mail.kernel.org (localhost [127.0.0.1]) by mail.kernel.org (Postfix) with ESMTP id F2B8E2017D for ; Wed, 10 Dec 2014 20:00:54 +0000 (UTC) Received: from vger.kernel.org (vger.kernel.org [209.132.180.67]) by mail.kernel.org (Postfix) with ESMTP id D8FF920125 for ; Wed, 10 Dec 2014 20:00:52 +0000 (UTC) Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S933023AbaLJUAf (ORCPT ); Wed, 10 Dec 2014 15:00:35 -0500 Received: from mx1.redhat.com ([209.132.183.28]:53977 "EHLO mx1.redhat.com" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S933005AbaLJUAc (ORCPT ); Wed, 10 Dec 2014 15:00:32 -0500 Received: from int-mx09.intmail.prod.int.phx2.redhat.com (int-mx09.intmail.prod.int.phx2.redhat.com [10.5.11.22]) by mx1.redhat.com (8.14.4/8.14.4) with ESMTP id sBAK0TLt017833 (version=TLSv1/SSLv3 cipher=DHE-RSA-AES256-GCM-SHA384 bits=256 verify=FAIL); Wed, 10 Dec 2014 15:00:29 -0500 Received: from hawk.usersys.redhat.com ([10.34.1.145]) by int-mx09.intmail.prod.int.phx2.redhat.com (8.14.4/8.14.4) with ESMTP id sBAK0CaU028915; Wed, 10 Dec 2014 15:00:27 -0500 From: Andrew Jones To: kvmarm@lists.cs.columbia.edu, kvm@vger.kernel.org Cc: christoffer.dall@linaro.org, pbonzini@redhat.com, alex.bennee@linaro.org Subject: [PATCH 09/15] arm64: vectors support Date: Wed, 10 Dec 2014 21:00:02 +0100 Message-Id: <1418241608-13966-10-git-send-email-drjones@redhat.com> In-Reply-To: <1418241608-13966-1-git-send-email-drjones@redhat.com> References: <1418241608-13966-1-git-send-email-drjones@redhat.com> X-Scanned-By: MIMEDefang 2.68 on 10.5.11.22 Sender: kvm-owner@vger.kernel.org Precedence: bulk List-ID: X-Mailing-List: kvm@vger.kernel.org X-Spam-Status: No, score=-6.9 required=5.0 tests=BAYES_00, RCVD_IN_DNSWL_HI, T_RP_MATCHES_RCVD, UNPARSEABLE_RELAY autolearn=unavailable version=3.3.1 X-Spam-Checker-Version: SpamAssassin 3.3.1 (2010-03-16) on mail.kernel.org X-Virus-Scanned: ClamAV using ClamSMTP Signed-off-by: Andrew Jones --- arm/cstart64.S | 142 ++++++++++++++++++++++++++++++++++++++- arm/selftest.c | 129 ++++++++++++++++++++++++++++++++--- arm/unittests.cfg | 2 - config/config-arm64.mak | 1 + lib/arm64/asm-offsets.c | 16 +++++ lib/arm64/asm/esr.h | 43 ++++++++++++ lib/arm64/asm/processor.h | 52 ++++++++++++++ lib/arm64/asm/ptrace.h | 95 ++++++++++++++++++++++++++ lib/arm64/processor.c | 168 ++++++++++++++++++++++++++++++++++++++++++++++ 9 files changed, 637 insertions(+), 11 deletions(-) create mode 100644 lib/arm64/asm/esr.h create mode 100644 lib/arm64/asm/processor.h create mode 100644 lib/arm64/asm/ptrace.h create mode 100644 lib/arm64/processor.c diff --git a/arm/cstart64.S b/arm/cstart64.S index 1d98066d0e187..d1860a94fb2d3 100644 --- a/arm/cstart64.S +++ b/arm/cstart64.S @@ -7,6 +7,7 @@ */ #define __ASSEMBLY__ #include +#include .section .init @@ -26,7 +27,7 @@ start: msr cpacr_el1, x0 /* set up exception handling */ -// bl exceptions_init + bl exceptions_init /* complete setup */ ldp x0, x1, [sp], #16 @@ -40,9 +41,148 @@ start: bl exit b halt +exceptions_init: + adr x0, vector_table + msr vbar_el1, x0 + isb + ret + .text .globl halt halt: 1: wfi b 1b + +/* + * Vectors + * Adapted from arch/arm64/kernel/entry.S + */ +.macro vector_stub, name, vec +\name: + stp x0, x1, [sp, #-S_FRAME_SIZE]! + stp x2, x3, [sp, #16] + stp x4, x5, [sp, #32] + stp x6, x7, [sp, #48] + stp x8, x9, [sp, #64] + stp x10, x11, [sp, #80] + stp x12, x13, [sp, #96] + stp x14, x15, [sp, #112] + stp x16, x17, [sp, #128] + stp x18, x19, [sp, #144] + stp x20, x21, [sp, #160] + stp x22, x23, [sp, #176] + stp x24, x25, [sp, #192] + stp x26, x27, [sp, #208] + stp x28, x29, [sp, #224] + + str x30, [sp, #S_LR] + + .if \vec >= 8 + mrs x1, sp_el0 + .else + add x1, sp, #S_FRAME_SIZE + .endif + str x1, [sp, #S_SP] + + mrs x1, elr_el1 + mrs x2, spsr_el1 + stp x1, x2, [sp, #S_PC] + + and x2, x2, #PSR_MODE_MASK + cmp x2, #PSR_MODE_EL0t + b.ne 1f + adr x2, user_mode + str xzr, [x2] /* we're in kernel mode now */ + +1: mov x0, \vec + mov x1, sp + mrs x2, esr_el1 + bl do_handle_exception + + ldp x1, x2, [sp, #S_PC] + msr spsr_el1, x2 + msr elr_el1, x1 + + and x2, x2, #PSR_MODE_MASK + cmp x2, #PSR_MODE_EL0t + b.ne 1f + adr x2, user_mode + mov x1, #1 + str x1, [x2] /* we're going back to user mode */ + +1: + .if \vec >= 8 + ldr x1, [sp, #S_SP] + msr sp_el0, x1 + .endif + + ldr x30, [sp, #S_LR] + + ldp x28, x29, [sp, #224] + ldp x26, x27, [sp, #208] + ldp x24, x25, [sp, #192] + ldp x22, x23, [sp, #176] + ldp x20, x21, [sp, #160] + ldp x18, x19, [sp, #144] + ldp x16, x17, [sp, #128] + ldp x14, x15, [sp, #112] + ldp x12, x13, [sp, #96] + ldp x10, x11, [sp, #80] + ldp x8, x9, [sp, #64] + ldp x6, x7, [sp, #48] + ldp x4, x5, [sp, #32] + ldp x2, x3, [sp, #16] + ldp x0, x1, [sp], #S_FRAME_SIZE + + eret +.endm + +vector_stub el1t_sync, 0 +vector_stub el1t_irq, 1 +vector_stub el1t_fiq, 2 +vector_stub el1t_error, 3 + +vector_stub el1h_sync, 4 +vector_stub el1h_irq, 5 +vector_stub el1h_fiq, 6 +vector_stub el1h_error, 7 + +vector_stub el0_sync_64, 8 +vector_stub el0_irq_64, 9 +vector_stub el0_fiq_64, 10 +vector_stub el0_error_64, 11 + +vector_stub el0_sync_32, 12 +vector_stub el0_irq_32, 13 +vector_stub el0_fiq_32, 14 +vector_stub el0_error_32, 15 + +.section .text.ex + +.macro ventry, label +.align 7 + b \label +.endm + +.align 11 +vector_table: + ventry el1t_sync // Synchronous EL1t + ventry el1t_irq // IRQ EL1t + ventry el1t_fiq // FIQ EL1t + ventry el1t_error // Error EL1t + + ventry el1h_sync // Synchronous EL1h + ventry el1h_irq // IRQ EL1h + ventry el1h_fiq // FIQ EL1h + ventry el1h_error // Error EL1h + + ventry el0_sync_64 // Synchronous 64-bit EL0 + ventry el0_irq_64 // IRQ 64-bit EL0 + ventry el0_fiq_64 // FIQ 64-bit EL0 + ventry el0_error_64 // Error 64-bit EL0 + + ventry el0_sync_32 // Synchronous 32-bit EL0 + ventry el0_irq_32 // IRQ 32-bit EL0 + ventry el0_fiq_32 // FIQ 32-bit EL0 + ventry el0_error_32 // Error 32-bit EL0 diff --git a/arm/selftest.c b/arm/selftest.c index 30f44261d47db..824af2f3c15af 100644 --- a/arm/selftest.c +++ b/arm/selftest.c @@ -8,12 +8,10 @@ #include #include #include -#ifdef __arm__ #include #include #include #include -#endif #define TESTGRP "selftest" @@ -80,8 +78,10 @@ static void check_setup(int argc, char **argv) assert_args(nr_tests, 2); } -#ifdef __arm__ static struct pt_regs expected_regs; +static bool und_works; +static bool svc_works; +#if defined(__arm__) /* * Capture the current register state and execute an instruction * that causes an exception. The test handler will check that its @@ -122,7 +122,6 @@ static bool check_regs(struct pt_regs *regs) return true; } -static bool und_works; static void und_handler(struct pt_regs *regs) { und_works = check_regs(regs); @@ -140,7 +139,6 @@ static bool check_und(void) return und_works; } -static bool svc_works; static void svc_handler(struct pt_regs *regs) { u32 svc = *(u32 *)(regs->ARM_pc - 4) & 0xffffff; @@ -181,13 +179,130 @@ static bool check_svc(void) return svc_works; } +#elif defined(__aarch64__) +#include + +/* + * Capture the current register state and execute an instruction + * that causes an exception. The test handler will check that its + * capture of the current register state matches the capture done + * here. + * + * NOTE: update clobber list if passed insns needs more than x0,x1 + */ +#define test_exception(pre_insns, excptn_insn, post_insns) \ + asm volatile( \ + pre_insns "\n" \ + "mov x1, %0\n" \ + "ldr x0, [x1, #" xstr(S_PSTATE) "]\n" \ + "mrs x1, nzcv\n" \ + "orr w0, w0, w1\n" \ + "mov x1, %0\n" \ + "str w0, [x1, #" xstr(S_PSTATE) "]\n" \ + "mov x0, sp\n" \ + "str x0, [x1, #" xstr(S_SP) "]\n" \ + "adr x0, 1f\n" \ + "str x0, [x1, #" xstr(S_PC) "]\n" \ + "stp x2, x3, [x1, #16]\n" \ + "stp x4, x5, [x1, #32]\n" \ + "stp x6, x7, [x1, #48]\n" \ + "stp x8, x9, [x1, #64]\n" \ + "stp x10, x11, [x1, #80]\n" \ + "stp x12, x13, [x1, #96]\n" \ + "stp x14, x15, [x1, #112]\n" \ + "stp x16, x17, [x1, #128]\n" \ + "stp x18, x19, [x1, #144]\n" \ + "stp x20, x21, [x1, #160]\n" \ + "stp x22, x23, [x1, #176]\n" \ + "stp x24, x25, [x1, #192]\n" \ + "stp x26, x27, [x1, #208]\n" \ + "stp x28, x29, [x1, #224]\n" \ + "str x30, [x1, #" xstr(S_LR) "]\n" \ + "stp x0, x1, [x1]\n" \ + "1:" excptn_insn "\n" \ + post_insns "\n" \ + :: "r" (&expected_regs) : "x0", "x1") + +static bool check_regs(struct pt_regs *regs) +{ + unsigned i; + + /* exception handlers should always run in EL1 */ + if (current_level() != CurrentEL_EL1) + return false; + + for (i = 0; i < ARRAY_SIZE(regs->regs); ++i) { + if (regs->regs[i] != expected_regs.regs[i]) + return false; + } + + regs->pstate &= 0xf0000000 /* NZCV */ | 0x3c0 /* DAIF */ + | PSR_MODE_MASK; + + return regs->sp == expected_regs.sp + && regs->pc == expected_regs.pc + && regs->pstate == expected_regs.pstate; +} + +static enum vector check_vector_prep(void) +{ + unsigned long daif; + + if (user_mode) + return EL0_SYNC_64; + + asm volatile("mrs %0, daif" : "=r" (daif) ::); + expected_regs.pstate = daif | PSR_MODE_EL1h; + return EL1H_SYNC; +} + +static void unknown_handler(struct pt_regs *regs, unsigned int esr __unused) +{ + und_works = check_regs(regs); + regs->pc += 4; +} + +static bool check_und(void) +{ + enum vector v = check_vector_prep(); + + install_exception_handler(v, ESR_EL1_EC_UNKNOWN, unknown_handler); + + /* try to read an el2 sysreg from el0/1 */ + test_exception("", "mrs x0, sctlr_el2", ""); + + install_exception_handler(v, ESR_EL1_EC_UNKNOWN, NULL); + + return und_works; +} + +static void svc_handler(struct pt_regs *regs, unsigned int esr) +{ + u16 svc = esr & 0xffff; + + expected_regs.pc += 4; + svc_works = check_regs(regs) && svc == 123; +} + +static bool check_svc(void) +{ + enum vector v = check_vector_prep(); + + install_exception_handler(v, ESR_EL1_EC_SVC64, svc_handler); + + test_exception("", "svc #123", ""); + + install_exception_handler(v, ESR_EL1_EC_SVC64, NULL); + + return svc_works; +} +#endif static void check_vectors(void *arg __unused) { report("%s", check_und() && check_svc(), testname); exit(report_summary()); } -#endif int main(int argc, char **argv) { @@ -199,7 +314,6 @@ int main(int argc, char **argv) check_setup(argc-1, &argv[1]); -#ifdef __arm__ } else if (strcmp(argv[0], "vectors-kernel") == 0) { check_vectors(NULL); @@ -209,7 +323,6 @@ int main(int argc, char **argv) void *sp = memalign(PAGE_SIZE, PAGE_SIZE); memset(sp, 0, PAGE_SIZE); start_usr(check_vectors, NULL, (unsigned long)sp + PAGE_SIZE); -#endif } return report_summary(); diff --git a/arm/unittests.cfg b/arm/unittests.cfg index 9ac6ecaa55d3b..efcca6bf24af6 100644 --- a/arm/unittests.cfg +++ b/arm/unittests.cfg @@ -22,11 +22,9 @@ groups = selftest file = selftest.flat extra_params = -append 'vectors-kernel' groups = selftest -arch = arm # Test vector setup and exception handling (user mode). [selftest::vectors-user] file = selftest.flat extra_params = -append 'vectors-user' groups = selftest -arch = arm diff --git a/config/config-arm64.mak b/config/config-arm64.mak index 37db3d6026424..91255e7d01432 100644 --- a/config/config-arm64.mak +++ b/config/config-arm64.mak @@ -10,6 +10,7 @@ kernel_offset = 0x80000 CFLAGS += -D__aarch64__ cstart.o = $(TEST_DIR)/cstart64.o +cflatobjs += lib/arm64/processor.o # arm64 specific tests tests = diff --git a/lib/arm64/asm-offsets.c b/lib/arm64/asm-offsets.c index c85b9a1e97e44..d7d33f4d917ab 100644 --- a/lib/arm64/asm-offsets.c +++ b/lib/arm64/asm-offsets.c @@ -7,8 +7,24 @@ */ #include #include +#include int main(void) { + OFFSET(S_X0, pt_regs, regs[0]); + OFFSET(S_X1, pt_regs, regs[1]); + OFFSET(S_X2, pt_regs, regs[2]); + OFFSET(S_X3, pt_regs, regs[3]); + OFFSET(S_X4, pt_regs, regs[4]); + OFFSET(S_X5, pt_regs, regs[5]); + OFFSET(S_X6, pt_regs, regs[6]); + OFFSET(S_X7, pt_regs, regs[7]); + OFFSET(S_LR, pt_regs, regs[30]); + OFFSET(S_SP, pt_regs, sp); + OFFSET(S_PC, pt_regs, pc); + OFFSET(S_PSTATE, pt_regs, pstate); + OFFSET(S_ORIG_X0, pt_regs, orig_x0); + OFFSET(S_SYSCALLNO, pt_regs, syscallno); + DEFINE(S_FRAME_SIZE, sizeof(struct pt_regs)); return 0; } diff --git a/lib/arm64/asm/esr.h b/lib/arm64/asm/esr.h new file mode 100644 index 0000000000000..8407b003afa7f --- /dev/null +++ b/lib/arm64/asm/esr.h @@ -0,0 +1,43 @@ +/* + * From Linux kernel arch/arm64/include/asm/esr.h + */ +#ifndef _ASMARM64_ESR_H_ +#define _ASMARM64_ESR_H_ + +#define ESR_EL1_WRITE (1 << 6) +#define ESR_EL1_CM (1 << 8) +#define ESR_EL1_IL (1 << 25) + +#define ESR_EL1_EC_SHIFT (26) +#define ESR_EL1_EC_UNKNOWN (0x00) +#define ESR_EL1_EC_WFI (0x01) +#define ESR_EL1_EC_CP15_32 (0x03) +#define ESR_EL1_EC_CP15_64 (0x04) +#define ESR_EL1_EC_CP14_MR (0x05) +#define ESR_EL1_EC_CP14_LS (0x06) +#define ESR_EL1_EC_FP_ASIMD (0x07) +#define ESR_EL1_EC_CP10_ID (0x08) +#define ESR_EL1_EC_CP14_64 (0x0C) +#define ESR_EL1_EC_ILL_ISS (0x0E) +#define ESR_EL1_EC_SVC32 (0x11) +#define ESR_EL1_EC_SVC64 (0x15) +#define ESR_EL1_EC_SYS64 (0x18) +#define ESR_EL1_EC_IABT_EL0 (0x20) +#define ESR_EL1_EC_IABT_EL1 (0x21) +#define ESR_EL1_EC_PC_ALIGN (0x22) +#define ESR_EL1_EC_DABT_EL0 (0x24) +#define ESR_EL1_EC_DABT_EL1 (0x25) +#define ESR_EL1_EC_SP_ALIGN (0x26) +#define ESR_EL1_EC_FP_EXC32 (0x28) +#define ESR_EL1_EC_FP_EXC64 (0x2C) +#define ESR_EL1_EC_SERROR (0x2F) +#define ESR_EL1_EC_BREAKPT_EL0 (0x30) +#define ESR_EL1_EC_BREAKPT_EL1 (0x31) +#define ESR_EL1_EC_SOFTSTP_EL0 (0x32) +#define ESR_EL1_EC_SOFTSTP_EL1 (0x33) +#define ESR_EL1_EC_WATCHPT_EL0 (0x34) +#define ESR_EL1_EC_WATCHPT_EL1 (0x35) +#define ESR_EL1_EC_BKPT32 (0x38) +#define ESR_EL1_EC_BRK64 (0x3C) + +#endif /* _ASMARM64_ESR_H_ */ diff --git a/lib/arm64/asm/processor.h b/lib/arm64/asm/processor.h new file mode 100644 index 0000000000000..66296f549f87e --- /dev/null +++ b/lib/arm64/asm/processor.h @@ -0,0 +1,52 @@ +#ifndef _ASMARM64_PROCESSOR_H_ +#define _ASMARM64_PROCESSOR_H_ +/* + * Copyright (C) 2014, Red Hat Inc, Andrew Jones + * + * This work is licensed under the terms of the GNU LGPL, version 2. + */ +#include + +enum vector { + EL1T_SYNC, + EL1T_IRQ, + EL1T_FIQ, + EL1T_ERROR, + EL1H_SYNC, + EL1H_IRQ, + EL1H_FIQ, + EL1H_ERROR, + EL0_SYNC_64, + EL0_IRQ_64, + EL0_FIQ_64, + EL0_ERROR_64, + EL0_SYNC_32, + EL0_IRQ_32, + EL0_FIQ_32, + EL0_ERROR_32, + VECTOR_MAX, +}; + +#define EC_MAX 64 + +typedef void (*vector_fn)(enum vector v, struct pt_regs *regs, + unsigned int esr); +typedef void (*exception_fn)(struct pt_regs *regs, unsigned int esr); +extern void install_vector_handler(enum vector v, vector_fn fn); +extern void install_exception_handler(enum vector v, unsigned int ec, + exception_fn fn); + +extern void show_regs(struct pt_regs *regs); +extern void *get_sp(void); + +static inline unsigned long current_level(void) +{ + unsigned long el; + asm volatile("mrs %0, CurrentEL" : "=r" (el)); + return el & 0xc; +} + +extern bool user_mode; +extern void start_usr(void (*func)(void *arg), void *arg, unsigned long sp_usr); + +#endif /* _ASMARM64_PROCESSOR_H_ */ diff --git a/lib/arm64/asm/ptrace.h b/lib/arm64/asm/ptrace.h new file mode 100644 index 0000000000000..dd89d82063b1d --- /dev/null +++ b/lib/arm64/asm/ptrace.h @@ -0,0 +1,95 @@ +#ifndef _ASMARM64_PTRACE_H_ +#define _ASMARM64_PTRACE_H_ +/* + * Adapted from Linux kernel headers + * arch/arm64/include/asm/ptrace.h + * arch/arm64/include/uapi/asm/ptrace.h + */ + +/* Current Exception Level values, as contained in CurrentEL */ +#define CurrentEL_EL1 (1 << 2) +#define CurrentEL_EL2 (2 << 2) + +/* + * PSR bits + */ +#define PSR_MODE_EL0t 0x00000000 +#define PSR_MODE_EL1t 0x00000004 +#define PSR_MODE_EL1h 0x00000005 +#define PSR_MODE_EL2t 0x00000008 +#define PSR_MODE_EL2h 0x00000009 +#define PSR_MODE_EL3t 0x0000000c +#define PSR_MODE_EL3h 0x0000000d +#define PSR_MODE_MASK 0x0000000f + +/* AArch32 CPSR bits */ +#define PSR_MODE32_BIT 0x00000010 + +/* AArch64 SPSR bits */ +#define PSR_F_BIT 0x00000040 +#define PSR_I_BIT 0x00000080 +#define PSR_A_BIT 0x00000100 +#define PSR_D_BIT 0x00000200 +#define PSR_Q_BIT 0x08000000 +#define PSR_V_BIT 0x10000000 +#define PSR_C_BIT 0x20000000 +#define PSR_Z_BIT 0x40000000 +#define PSR_N_BIT 0x80000000 + +/* + * Groups of PSR bits + */ +#define PSR_f 0xff000000 /* Flags */ +#define PSR_s 0x00ff0000 /* Status */ +#define PSR_x 0x0000ff00 /* Extension */ +#define PSR_c 0x000000ff /* Control */ + +#ifndef __ASSEMBLY__ +#include + +struct user_pt_regs { + u64 regs[31]; + u64 sp; + u64 pc; + u64 pstate; +}; + +struct user_fpsimd_state { + __uint128_t vregs[32]; + u32 fpsr; + u32 fpcr; +}; + +/* + * This struct defines the way the registers are stored on the stack during an + * exception. Note that sizeof(struct pt_regs) has to be a multiple of 16 (for + * stack alignment). struct user_pt_regs must form a prefix of struct pt_regs. + */ +struct pt_regs { + union { + struct user_pt_regs user_regs; + struct { + u64 regs[31]; + u64 sp; + u64 pc; + u64 pstate; + }; + }; + u64 orig_x0; + u64 syscallno; +}; + +#define user_mode(regs) \ + (((regs)->pstate & PSR_MODE_MASK) == PSR_MODE_EL0t) + +#define processor_mode(regs) \ + ((regs)->pstate & PSR_MODE_MASK) + +#define interrupts_enabled(regs) \ + (!((regs)->pstate & PSR_I_BIT)) + +#define fast_interrupts_enabled(regs) \ + (!((regs)->pstate & PSR_F_BIT)) + +#endif /* !__ASSEMBLY__ */ +#endif /* _ASMARM64_PTRACE_H_ */ diff --git a/lib/arm64/processor.c b/lib/arm64/processor.c new file mode 100644 index 0000000000000..7230a8ab3f702 --- /dev/null +++ b/lib/arm64/processor.c @@ -0,0 +1,168 @@ +/* + * processor control and status functions + * + * Copyright (C) 2014, Red Hat Inc, Andrew Jones + * + * This work is licensed under the terms of the GNU LGPL, version 2. + */ +#include +#include +#include +#include + +static char *vector_names[] = { + "el1t_sync", + "el1t_irq", + "el1t_fiq", + "el1t_error", + "el1h_sync", + "el1h_irq", + "el1h_fiq", + "el1h_error", + "el0_sync_64", + "el0_irq_64", + "el0_fiq_64", + "el0_error_64", + "el0_sync_32", + "el0_irq_32", + "el0_fiq_32", + "el0_error_32", +}; + +static char *ec_names[EC_MAX] = { + [ESR_EL1_EC_UNKNOWN] = "UNKNOWN", + [ESR_EL1_EC_WFI] = "WFI", + [ESR_EL1_EC_CP15_32] = "CP15_32", + [ESR_EL1_EC_CP15_64] = "CP15_64", + [ESR_EL1_EC_CP14_MR] = "CP14_MR", + [ESR_EL1_EC_CP14_LS] = "CP14_LS", + [ESR_EL1_EC_FP_ASIMD] = "FP_ASMID", + [ESR_EL1_EC_CP10_ID] = "CP10_ID", + [ESR_EL1_EC_CP14_64] = "CP14_64", + [ESR_EL1_EC_ILL_ISS] = "ILL_ISS", + [ESR_EL1_EC_SVC32] = "SVC32", + [ESR_EL1_EC_SVC64] = "SVC64", + [ESR_EL1_EC_SYS64] = "SYS64", + [ESR_EL1_EC_IABT_EL0] = "IABT_EL0", + [ESR_EL1_EC_IABT_EL1] = "IABT_EL1", + [ESR_EL1_EC_PC_ALIGN] = "PC_ALIGN", + [ESR_EL1_EC_DABT_EL0] = "DABT_EL0", + [ESR_EL1_EC_DABT_EL1] = "DABT_EL1", + [ESR_EL1_EC_SP_ALIGN] = "SP_ALIGN", + [ESR_EL1_EC_FP_EXC32] = "FP_EXC32", + [ESR_EL1_EC_FP_EXC64] = "FP_EXC64", + [ESR_EL1_EC_SERROR] = "SERROR", + [ESR_EL1_EC_BREAKPT_EL0] = "BREAKPT_EL0", + [ESR_EL1_EC_BREAKPT_EL1] = "BREAKPT_EL1", + [ESR_EL1_EC_SOFTSTP_EL0] = "SOFTSTP_EL0", + [ESR_EL1_EC_SOFTSTP_EL1] = "SOFTSTP_EL1", + [ESR_EL1_EC_WATCHPT_EL0] = "WATCHPT_EL0", + [ESR_EL1_EC_WATCHPT_EL1] = "WATCHPT_EL1", + [ESR_EL1_EC_BKPT32] = "BKPT32", + [ESR_EL1_EC_BRK64] = "BRK64", +}; + +void show_regs(struct pt_regs *regs) +{ + int i; + + printf("pc : [<%016llx>] lr : [<%016llx>] pstate: %08llx\n", + regs->pc, regs->regs[30], regs->pstate); + printf("sp : %016llx\n", regs->sp); + + for (i = 29; i >= 0; --i) { + printf("x%-2d: %016llx ", i, regs->regs[i]); + if (i % 2 == 0) + printf("\n"); + } + printf("\n"); +} + +void *get_sp(void) +{ + register unsigned long sp asm("sp"); + return (void *)sp; +} + +static void bad_exception(enum vector v, struct pt_regs *regs, + unsigned int esr, bool bad_vector) +{ + unsigned int ec = esr >> ESR_EL1_EC_SHIFT; + + if (bad_vector) { + if (v < VECTOR_MAX) + printf("Unhandled vector %d (%s)\n", v, + vector_names[v]); + else + printf("Got bad vector=%d\n", v); + } else { + if (ec_names[ec]) + printf("Unhandled exception ec=0x%x (%s)\n", ec, + ec_names[ec]); + else + printf("Got bad ec=0x%x\n", ec); + } + + printf("Vector: %d (%s)\n", v, vector_names[v]); + printf("ESR_EL1: %08lx, ec=0x%x (%s)\n", esr, ec, ec_names[ec]); + printf("Exception frame registers:\n"); + show_regs(regs); + abort(); +} + +static exception_fn exception_handlers[VECTOR_MAX][EC_MAX]; + +void install_exception_handler(enum vector v, unsigned int ec, exception_fn fn) +{ + if (v < VECTOR_MAX && ec < EC_MAX) + exception_handlers[v][ec] = fn; +} + +static void default_vector_handler(enum vector v, struct pt_regs *regs, + unsigned int esr) +{ + unsigned int ec = esr >> ESR_EL1_EC_SHIFT; + + if (ec < EC_MAX && exception_handlers[v][ec]) + exception_handlers[v][ec](regs, esr); + else + bad_exception(v, regs, esr, false); +} + +static vector_fn vector_handlers[VECTOR_MAX] = { + [EL1H_SYNC] = default_vector_handler, + [EL1H_IRQ] = default_vector_handler, + [EL0_SYNC_64] = default_vector_handler, + [EL0_IRQ_64] = default_vector_handler, +}; + +void do_handle_exception(enum vector v, struct pt_regs *regs, unsigned int esr) +{ + if (v < VECTOR_MAX && vector_handlers[v]) + vector_handlers[v](v, regs, esr); + else + bad_exception(v, regs, esr, true); +} + +void install_vector_handler(enum vector v, vector_fn fn) +{ + if (v < VECTOR_MAX) + vector_handlers[v] = fn; +} + +bool user_mode; +void start_usr(void (*func)(void *arg), void *arg, unsigned long sp_usr) +{ + sp_usr &= (~15UL); /* stack ptr needs 16-byte alignment */ + + user_mode = true; + + asm volatile( + "mov x0, %0\n" + "msr sp_el0, %1\n" + "msr elr_el1, %2\n" + "mov x3, xzr\n" /* clear and "set" PSR_MODE_EL0t */ + "msr spsr_el1, x3\n" + "eret\n" + :: "r" (arg), "r" (sp_usr), "r" (func) : "x0", "x3"); +}