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

Andrew Jones
2014-02-03T16:54:09Z

On Sat, Feb 01, 2014 at 06:29:27PM -0800, Christoffer Dall wrote:
> 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.

It also adds

/* make sure we restore sp_svc and lr_svc on mode change */
str r6, [sp, #S_SP]
str lr, [sp, #S_LR]

lower down. Needed to switch to r6 from r2 for that

>
> 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...

I'm not sure about the magic ! + ^ stuff, but what I've done does fix
the problem. I tested before/after the fix (actually that's why I also
added the get_sp).

>
> Otherwise this is looking good.

Thanks!! v4 coming soon.

>
> 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
> >