Re: [PATCH 17/17] arm: vectors support

Christoffer Dall
2014-02-02T02:29:24Z

On Tue, Jan 21, 2014 at 05:22:03PM +0100, Andrew Jones wrote:
> Add support for tests to use exception handlers.
>
> v2 -> v3:
> - squashed in 'arm: Simplify exceptions_init in cstart.S' from
> Christoffer Dall
> - suggested function name changes and comment additions [Christoffer Dall]
> - fix a bug with stack restore from usr mode exceptions that Christoffer
> pointed out. Add a get_sp() accessor too.

hmmm, how did you address this, the only change I can see is using r6
instead of r2, which I'm not sure how changes anything.

The problem is that your ldmia is replacing the usr sp, not the svc sp,
so maybe you need to do ldmia sp!, [ r0 - pc ]^ (don't remember if that
particular combination works) or you need to do something more fancy...

Otherwise this is looking good.

Thanks,
Christoffer

>
> Signed-off-by: Andrew Jones <drjones@redhat.com>
> ---
> arm/cstart.S | 168 ++++++++++++++++++++++++++++++++++++++++++++++++++
> arm/flat.lds | 7 ++-
> arm/selftest.c | 119 +++++++++++++++++++++++++++++++++++
> arm/unittests.cfg | 20 +++++-
> config/config-arm.mak | 1 +
> lib/arm/processor.c | 103 +++++++++++++++++++++++++++++++
> lib/arm/processor.h | 35 +++++++++++
> lib/libcflat.h | 2 +
> 8 files changed, 453 insertions(+), 2 deletions(-)
> create mode 100644 lib/arm/processor.c
> create mode 100644 lib/arm/processor.h
>
> diff --git a/arm/cstart.S b/arm/cstart.S
> index 4de04b62c28c2..74674fd41c1b3 100644
> --- a/arm/cstart.S
> +++ b/arm/cstart.S
> @@ -1,3 +1,7 @@
> +#define __ASSEMBLY__
> +#include "arm/asm-offsets.h"
> +#include "arm/ptrace.h"
> +#include "arm/cp15.h"
>
> .arm
>
> @@ -10,6 +14,13 @@ start:
> * See the kernel doc Documentation/arm/Booting
> */
> ldr sp, =stacktop
> + push {r0-r3}
> +
> + /* set up vector table and mode stacks */
> + bl exceptions_init
> +
> + /* complete setup */
> + pop {r0-r3}
> bl setup
>
> /* start the test */
> @@ -20,9 +31,166 @@ start:
> bl exit
> b halt
>
> +.macro set_mode_stack mode, stack
> + add \stack, #S_FRAME_SIZE
> + msr cpsr_c, #(\mode | PSR_I_BIT | PSR_F_BIT)
> + mov sp, \stack
> +.endm
> +
> +exceptions_init:
> + mrc p15, 0, r2, c1, c0, 0 @ read SCTLR
> + bic r2, #CR_V @ SCTLR.V := 0
> + mcr p15, 0, r2, c1, c0, 0 @ write SCTLR
> + ldr r2, =vector_table
> + mcr p15, 0, r2, c12, c0, 0 @ write VBAR
> +
> + mrs r2, cpsr
> + ldr r1, =exception_stacks
> + /* first frame for svc mode */
> + set_mode_stack UND_MODE, r1
> + set_mode_stack ABT_MODE, r1
> + set_mode_stack IRQ_MODE, r1
> + set_mode_stack FIQ_MODE, r1
> + msr cpsr_cxsf, r2 @ back to svc mode
> + mov pc, lr
> +
> .text
>
> .globl halt
> halt:
> 1: wfi
> b 1b
> +
> +/*
> + * Vector stubs.
> + * Simplified version of the Linux kernel implementation
> + * arch/arm/kernel/entry-armv.S
> + *
> + * Each mode has an S_FRAME_SIZE sized stack initialized
> + * in exceptions_init
> + */
> +.macro vector_stub, name, vec, mode, correction=0
> +.align 5
> +vector_\name:
> +.if \correction
> + sub lr, lr, #\correction
> +.endif
> + /*
> + * Save r0, r1, lr_<exception> (parent PC)
> + * and spsr_<exception> (parent CPSR)
> + */
> + str r0, [sp, #S_R0]
> + str r1, [sp, #S_R1]
> + str lr, [sp, #S_PC]
> + mrs r0, spsr
> + str r0, [sp, #S_PSR]
> +
> + /* Prepare for SVC32 mode. */
> + mrs r0, cpsr
> + bic r0, #MODE_MASK
> + orr r0, #SVC_MODE
> + msr spsr_cxsf, r0
> +
> + /* Branch to handler in SVC mode */
> + mov r0, #\vec
> + mov r1, sp
> + ldr lr, =vector_common
> + movs pc, lr
> +.endm
> +
> +vector_stub rst, 0, UND_MODE
> +vector_stub und, 1, UND_MODE
> +vector_stub pabt, 3, ABT_MODE, 4
> +vector_stub dabt, 4, ABT_MODE, 8
> +vector_stub irq, 6, IRQ_MODE, 4
> +vector_stub fiq, 7, FIQ_MODE, 4
> +
> +.align 5
> +vector_svc:
> + /*
> + * Save r0, r1, lr_<exception> (parent PC)
> + * and spsr_<exception> (parent CPSR)
> + */
> + push { r1 }
> + ldr r1, =exception_stacks
> + str r0, [r1, #S_R0]
> + pop { r0 }
> + str r0, [r1, #S_R1]
> + str lr, [r1, #S_PC]
> + mrs r0, spsr
> + str r0, [r1, #S_PSR]
> +
> + /*
> + * Branch to handler, still in SVC mode.
> + * r0 := 2 is the svc vector number.
> + */
> + mov r0, #2
> + ldr lr, =vector_common
> + mov pc, lr
> +
> +vector_common:
> + /* make room for pt_regs */
> + sub sp, #S_FRAME_SIZE
> + tst sp, #4 @ check stack alignment
> + subne sp, #4
> +
> + /* store registers r0-r12 */
> + stmia sp, { r0-r12 } @ stored wrong r0 and r1, fix later
> +
> + /* get registers saved in the stub */
> + ldr r2, [r1, #S_R0] @ r0
> + ldr r3, [r1, #S_R1] @ r1
> + ldr r4, [r1, #S_PC] @ lr_<exception> (parent PC)
> + ldr r5, [r1, #S_PSR] @ spsr_<exception> (parent CPSR)
> +
> + /* fix r0 and r1 */
> + str r2, [sp, #S_R0]
> + str r3, [sp, #S_R1]
> +
> + /* store sp_svc, if we were in usr mode we'll fix this later */
> + add r6, sp, #S_FRAME_SIZE
> + addne r6, #4 @ stack wasn't aligned
> + str r6, [sp, #S_SP]
> +
> + str lr, [sp, #S_LR] @ store lr_svc, fix later for usr mode
> + str r4, [sp, #S_PC] @ store lr_<exception>
> + str r5, [sp, #S_PSR] @ store spsr_<exception>
> +
> + /* set ORIG_r0 */
> + mov r2, #-1
> + str r2, [sp, #S_OLD_R0]
> +
> + /* if we were in usr mode then we need sp_usr and lr_usr instead */
> + and r1, r5, #MODE_MASK
> + cmp r1, #USR_MODE
> + bne 1f
> + add r1, sp, #S_SP
> + stmia r1, { sp,lr }^
> +
> + /* Call the handler. r0 is the vector number, r1 := pt_regs */
> +1: mov r1, sp
> + bl do_handle_exception
> +
> + /* make sure we restore sp_svc and lr_svc on mode change */
> + str r6, [sp, #S_SP]
> + str lr, [sp, #S_LR]
> +
> + /* return from exception */
> + msr spsr_cxsf, r5
> + ldmia sp, { r0-pc }^
> +
> +.align 5
> +vector_addrexcptn:
> + b vector_addrexcptn
> +
> +.section .text.ex
> +.align 5
> +vector_table:
> + b vector_rst
> + b vector_und
> + b vector_svc
> + b vector_pabt
> + b vector_dabt
> + b vector_addrexcptn @ should never happen
> + b vector_irq
> + b vector_fiq
> diff --git a/arm/flat.lds b/arm/flat.lds
> index 3e5d72e24989b..ee9fc0ab79abc 100644
> --- a/arm/flat.lds
> +++ b/arm/flat.lds
> @@ -3,7 +3,12 @@ SECTIONS
> {
> .text : { *(.init) *(.text) *(.text.*) }
> . = ALIGN(4K);
> - .data : { *(.data) }
> + .data : {
> + exception_stacks = .;
> + . += 4K;
> + exception_stacks_end = .;
> + *(.data)
> + }
> . = ALIGN(16);
> .rodata : { *(.rodata) }
> . = ALIGN(16);
> diff --git a/arm/selftest.c b/arm/selftest.c
> index 3d47a16cbcfad..96ebfe3454e63 100644
> --- a/arm/selftest.c
> +++ b/arm/selftest.c
> @@ -1,9 +1,119 @@
> #include "libcflat.h"
> #include "arm/sysinfo.h"
> +#include "arm/ptrace.h"
> +#include "arm/processor.h"
> +#include "arm/asm-offsets.h"
>
> #define PASS 0
> #define FAIL 1
>
> +static struct pt_regs expected_regs;
> +/*
> + * 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 r0,r1
> + */
> +#define test_exception(pre_insns, excptn_insn, post_insns) \
> + asm volatile( \
> + pre_insns "\n" \
> + "mov r0, %0\n" \
> + "stmia r0, { r0-lr }\n" \
> + "mrs r1, cpsr\n" \
> + "str r1, [r0, #" __stringify(S_PSR) "]\n" \
> + "mov r1, #-1\n" \
> + "str r1, [r0, #" __stringify(S_OLD_R0) "]\n" \
> + "add r1, pc, #8\n" \
> + "str r1, [r0, #" __stringify(S_R1) "]\n" \
> + "str r1, [r0, #" __stringify(S_PC) "]\n" \
> + excptn_insn "\n" \
> + post_insns "\n" \
> + :: "r" (&expected_regs) : "r0", "r1")
> +
> +static bool check_regs(struct pt_regs *regs)
> +{
> + unsigned i;
> +
> + /* exception handlers should always run in svc mode */
> + if (current_mode() != SVC_MODE)
> + return false;
> +
> + for (i = 0; i < ARRAY_SIZE(regs->uregs); ++i) {
> + if (regs->uregs[i] != expected_regs.uregs[i])
> + return false;
> + }
> +
> + return true;
> +}
> +
> +static bool und_works;
> +static void und_handler(struct pt_regs *regs)
> +{
> + und_works = check_regs(regs);
> +}
> +
> +static bool check_und(void)
> +{
> + install_exception_handler(EXCPTN_UND, und_handler);
> +
> + /* issue an instruction to a coprocessor we don't have */
> + test_exception("", "mcr p2, 0, r0, c0, c0", "");
> +
> + install_exception_handler(EXCPTN_UND, NULL);
> +
> + return und_works;
> +}
> +
> +static bool svc_works;
> +static void svc_handler(struct pt_regs *regs)
> +{
> + u32 svc = *(u32 *)(regs->ARM_pc - 4) & 0xffffff;
> +
> + if (processor_mode(regs) == SVC_MODE) {
> + /*
> + * When issuing an svc from supervisor mode lr_svc will
> + * get corrupted. So before issuing the svc, callers must
> + * always push it on the stack. We pushed it to offset 4.
> + */
> + regs->ARM_lr = *(unsigned long *)(regs->ARM_sp + 4);
> + }
> +
> + svc_works = check_regs(regs) && svc == 123;
> +}
> +
> +static bool check_svc(void)
> +{
> + install_exception_handler(EXCPTN_SVC, svc_handler);
> +
> + if (current_mode() == SVC_MODE) {
> + /*
> + * An svc from supervisor mode will corrupt lr_svc and
> + * spsr_svc. We need to save/restore them separately.
> + */
> + test_exception(
> + "mrs r0, spsr\n"
> + "push { r0,lr }\n",
> + "svc #123\n",
> + "pop { r0,lr }\n"
> + "msr spsr_cxsf, r0\n"
> + );
> + } else {
> + test_exception("", "svc #123", "");
> + }
> +
> + install_exception_handler(EXCPTN_SVC, NULL);
> +
> + return svc_works;
> +}
> +
> +static void check_vectors(void)
> +{
> + int ret = check_und() && check_svc() ? PASS : FAIL;
> + exit(ret);
> +}
> +
> static void assert_enough_args(int nargs, int needed)
> {
> if (nargs < needed) {
> @@ -24,6 +134,15 @@ int main(int argc, char **argv)
>
> if (mem_size/1024/1024 == (size_t)atol(argv[1]))
> ret = PASS;
> +
> + } else if (strcmp(argv[0], "vectors") == 0) {
> +
> + check_vectors(); /* doesn't return */
> +
> + } else if (strcmp(argv[0], "vectors_usr") == 0) {
> +
> + start_usr(check_vectors); /* doesn't return */
> +
> }
>
> return ret;
> diff --git a/arm/unittests.cfg b/arm/unittests.cfg
> index aff684892e90b..75e2a2e3d25bc 100644
> --- a/arm/unittests.cfg
> +++ b/arm/unittests.cfg
> @@ -6,6 +6,24 @@
> # arch = arm/arm64 # Only if the test case works only on one of them
> # groups = group1 group2 # Used to identify test cases with run_tests -g ...
>
> -[selftest]
> +#
> +# The selftest group tests the initial booting of a guest, as well as
> +# the test framework itself.
> +#
> +# Test bootinfo reading; configured mem-size should equal expected mem-size
> +[selftest_mem]
> file = selftest.flat
> extra_params = -m 256 -append 'mem 256'
> +groups = selftest
> +
> +# Test vector setup and exception handling (svc mode).
> +[selftest_vectors]
> +file = selftest.flat
> +extra_params = -append 'vectors'
> +groups = selftest
> +
> +# Test vector setup and exception handling (usr mode).
> +[selftest_vectors_usr]
> +file = selftest.flat
> +extra_params = -append 'vectors_usr'
> +groups = selftest
> diff --git a/config/config-arm.mak b/config/config-arm.mak
> index a863b3e3511c9..99349bf8b0c6b 100644
> --- a/config/config-arm.mak
> +++ b/config/config-arm.mak
> @@ -16,6 +16,7 @@ cflatobjs += \
> lib/virtio.o \
> lib/virtio-testdev.o \
> lib/arm/io.o \
> + lib/arm/processor.o \
> lib/arm/setup.o
>
> libeabi := lib/arm/libeabi.a
> diff --git a/lib/arm/processor.c b/lib/arm/processor.c
> new file mode 100644
> index 0000000000000..1fededee6977f
> --- /dev/null
> +++ b/lib/arm/processor.c
> @@ -0,0 +1,103 @@
> +#include "libcflat.h"
> +#include "arm/processor.h"
> +#include "arm/sysinfo.h"
> +#include "arm/ptrace.h"
> +#include "heap.h"
> +
> +static const char *processor_modes[] = {
> + "USER_26", "FIQ_26" , "IRQ_26" , "SVC_26" ,
> + "UK4_26" , "UK5_26" , "UK6_26" , "UK7_26" ,
> + "UK8_26" , "UK9_26" , "UK10_26", "UK11_26",
> + "UK12_26", "UK13_26", "UK14_26", "UK15_26",
> + "USER_32", "FIQ_32" , "IRQ_32" , "SVC_32" ,
> + "UK4_32" , "UK5_32" , "UK6_32" , "ABT_32" ,
> + "UK8_32" , "UK9_32" , "UK10_32", "UND_32" ,
> + "UK12_32", "UK13_32", "UK14_32", "SYS_32"
> +};
> +
> +static char *vector_names[] = {
> + "rst", "und", "svc", "pabt", "dabt", "addrexcptn", "irq", "fiq"
> +};
> +
> +void show_regs(struct pt_regs *regs)
> +{
> + unsigned long flags;
> + char buf[64];
> +
> + printf("pc : [<%08lx>] lr : [<%08lx>] psr: %08lx\n"
> + "sp : %08lx ip : %08lx fp : %08lx\n",
> + regs->ARM_pc, regs->ARM_lr, regs->ARM_cpsr,
> + regs->ARM_sp, regs->ARM_ip, regs->ARM_fp);
> + printf("r10: %08lx r9 : %08lx r8 : %08lx\n",
> + regs->ARM_r10, regs->ARM_r9, regs->ARM_r8);
> + printf("r7 : %08lx r6 : %08lx r5 : %08lx r4 : %08lx\n",
> + regs->ARM_r7, regs->ARM_r6, regs->ARM_r5, regs->ARM_r4);
> + printf("r3 : %08lx r2 : %08lx r1 : %08lx r0 : %08lx\n",
> + regs->ARM_r3, regs->ARM_r2, regs->ARM_r1, regs->ARM_r0);
> +
> + flags = regs->ARM_cpsr;
> + buf[0] = flags & PSR_N_BIT ? 'N' : 'n';
> + buf[1] = flags & PSR_Z_BIT ? 'Z' : 'z';
> + buf[2] = flags & PSR_C_BIT ? 'C' : 'c';
> + buf[3] = flags & PSR_V_BIT ? 'V' : 'v';
> + buf[4] = '\0';
> +
> + printf("Flags: %s IRQs o%s FIQs o%s Mode %s\n",
> + buf, interrupts_enabled(regs) ? "n" : "ff",
> + fast_interrupts_enabled(regs) ? "n" : "ff",
> + processor_modes[processor_mode(regs)]);
> +
> + if (!user_mode(regs)) {
> + unsigned int ctrl, transbase, dac;
> + asm volatile(
> + "mrc p15, 0, %0, c1, c0\n"
> + "mrc p15, 0, %1, c2, c0\n"
> + "mrc p15, 0, %2, c3, c0\n"
> + : "=r" (ctrl), "=r" (transbase), "=r" (dac));
> + printf("Control: %08x Table: %08x DAC: %08x\n",
> + ctrl, transbase, dac);
> + }
> +}
> +
> +void *get_sp(void)
> +{
> + register unsigned long sp asm("sp");
> + return (void *)sp;
> +}
> +
> +static exception_fn exception_handlers[EXCPTN_MAX];
> +
> +void install_exception_handler(enum vector v, exception_fn fn)
> +{
> + if (v < EXCPTN_MAX)
> + exception_handlers[v] = fn;
> +}
> +
> +void do_handle_exception(enum vector v, struct pt_regs *regs)
> +{
> + if (v < EXCPTN_MAX && exception_handlers[v]) {
> + exception_handlers[v](regs);
> + return;
> + }
> +
> + if (v < EXCPTN_MAX)
> + printf("Unhandled exception %d (%s)\n", v, vector_names[v]);
> + else
> + printf("%s called with vector=%d\n", __func__, v);
> + printf("Exception frame registers:\n");
> + show_regs(regs);
> + exit(EINTR);
> +}
> +
> +void start_usr(void (*func)(void))
> +{
> + void *sp_usr = alloc_page() + PAGE_SIZE;
> + asm volatile(
> + "mrs r0, cpsr\n"
> + "bic r0, #" __stringify(MODE_MASK) "\n"
> + "orr r0, #" __stringify(USR_MODE) "\n"
> + "msr cpsr_c, r0\n"
> + "mov sp, %0\n"
> + "mov pc, %1\n"
> + :: "r" (sp_usr), "r" (func) : "r0");
> +}
> diff --git a/lib/arm/processor.h b/lib/arm/processor.h
> new file mode 100644
> index 0000000000000..12c1902de97fd
> --- /dev/null
> +++ b/lib/arm/processor.h
> @@ -0,0 +1,35 @@
> +#ifndef _ARM_PROCESSOR_H_
> +#define _ARM_PROCESSOR_H_
> +#include "libcflat.h"
> +#include "ptrace.h"
> +
> +enum vector {
> + EXCPTN_RST,
> + EXCPTN_UND,
> + EXCPTN_SVC,
> + EXCPTN_PABT,
> + EXCPTN_DABT,
> + EXCPTN_ADDREXCPTN,
> + EXCPTN_IRQ,
> + EXCPTN_FIQ,
> + EXCPTN_MAX,
> +};
> +
> +typedef void (*exception_fn)(struct pt_regs *);
> +extern void install_exception_handler(enum vector v, exception_fn fn);
> +
> +extern void show_regs(struct pt_regs *regs);
> +extern void *get_sp(void);
> +
> +extern void start_usr(void (*func)(void));
> +
> +static inline unsigned long current_cpsr(void)
> +{
> + unsigned long cpsr;
> + asm volatile("mrs %0, cpsr" : "=r" (cpsr));
> + return cpsr;
> +}
> +
> +#define current_mode() (current_cpsr() & MODE_MASK)
> +
> +#endif
> diff --git a/lib/libcflat.h b/lib/libcflat.h
> index 84b8783bacfdc..3d47a3331b7fc 100644
> --- a/lib/libcflat.h
> +++ b/lib/libcflat.h
> @@ -65,6 +65,8 @@ extern long atol(const char *ptr);
> (type *)( (char *)__mptr - offsetof(type,member) );})
>
> #define __unused __attribute__((__unused__))
> +#define __stringify_1(x...) #x
> +#define __stringify(x...) __stringify_1(x)
> #define NULL ((void *)0UL)
> #include "errno.h"
> #endif
> --
> 1.8.1.4
>