Make sure that the whole CPU state is saved/restored during switch_to() to enable a clean context switch and return. Since we have this logic we can also re-use it for initial entry into OpenBIOS.
Finally also update context.h to reflect the new CPU state stucture.
Signed-off-by: Mark Cave-Ayland mark.cave-ayland@ilande.co.uk --- arch/sparc32/boot.h | 2 +- arch/sparc32/context.c | 8 +- arch/sparc32/context.h | 6 +- arch/sparc32/cpustate.h | 160 ++++++++++++++++++++++++++++++++++++++++ arch/sparc32/entry.S | 26 ++++++- arch/sparc32/switch.S | 187 +++++++++++++++-------------------------------- 6 files changed, 249 insertions(+), 140 deletions(-) create mode 100644 arch/sparc32/cpustate.h
diff --git a/arch/sparc32/boot.h b/arch/sparc32/boot.h index 55e391a..9bfc07e 100644 --- a/arch/sparc32/boot.h +++ b/arch/sparc32/boot.h @@ -10,7 +10,7 @@ int linux_load(struct sys_info *info, const char *file, const char *cmdline);
// context.c -extern struct context *__context; +extern struct context * volatile __context; unsigned int start_elf(unsigned long entry_point, unsigned long param);
// boot.c diff --git a/arch/sparc32/context.c b/arch/sparc32/context.c index d4d8530..555f628 100644 --- a/arch/sparc32/context.c +++ b/arch/sparc32/context.c @@ -24,7 +24,6 @@ void __exit_context(void); /* assembly routine */ * to start us up. */ static struct context main_ctx = { - .regs[REG_SP] = (uint32_t) &_estack - 96, .pc = (uint32_t) start_main, .npc = (uint32_t) start_main + 4, .return_addr = (uint32_t) __exit_context, @@ -32,7 +31,7 @@ static struct context main_ctx = {
/* This is used by assembly routine to load/store the context which * it is to switch/switched. */ -struct context *__context = &main_ctx; +struct context * volatile __context = &main_ctx;
/* Stack for loaded ELF image */ static uint8_t image_stack[IMAGE_STACK_SIZE]; @@ -66,7 +65,8 @@ init_context(uint8_t *stack, uint32_t stack_size, int num_params)
ctx = (struct context *) (stack + stack_size - (sizeof(*ctx) + num_params*sizeof(uint32_t))); - memset(ctx, 0, sizeof(*ctx)); + /* Use valid window state from startup */ + memcpy(ctx, &main_ctx, sizeof(struct context));
/* Fill in reasonable default for flat memory model */ ctx->regs[REG_SP] = virt_to_phys(SP_LOC(ctx)); @@ -106,7 +106,7 @@ unsigned int start_elf(unsigned long entry_point, unsigned long param)
ctx = init_context(image_stack, sizeof image_stack, 1); ctx->pc = entry_point; - ctx->param[0] = param; + ctx->regs[REG_O0] = param;
ctx = switch_to(ctx); return ctx->regs[REG_O0]; diff --git a/arch/sparc32/context.h b/arch/sparc32/context.h index 8689d56..ef454d0 100644 --- a/arch/sparc32/context.h +++ b/arch/sparc32/context.h @@ -3,11 +3,11 @@
struct context { /* General registers */ - uint32_t regs[32]; + uint32_t regs[148]; uint32_t pc; uint32_t npc; -#define REG_O0 8 -#define REG_SP 14 +#define REG_O0 12 +#define REG_SP 18 #define SP_LOC(ctx) (&(ctx)->regs[REG_SP]) /* Flags */ /* Optional stack contents */ diff --git a/arch/sparc32/cpustate.h b/arch/sparc32/cpustate.h new file mode 100644 index 0000000..1dab6ad --- /dev/null +++ b/arch/sparc32/cpustate.h @@ -0,0 +1,160 @@ +/* + * Save/restore CPU state macros + * + * Copyright (C) 2016 Mark Cave-Ayland (mark.cave-ayland@ilande.co.uk>) + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * version 2 + * + */ + +#include "autoconf.h" + +#define STACKFRAME_SZ 0x60 + +/* These are just handy. */ +#define _SV save %sp, -STACKFRAME_SZ, %sp +#define _RS restore + +#define FLUSH_ALL_KERNEL_WINDOWS \ + _SV; _SV; _SV; _SV; _SV; _SV; _SV; \ + _RS; _RS; _RS; _RS; _RS; _RS; _RS; + + +#define SAVE_CPU_GENERAL_STATE(type) \ + /* Save general state into context at %g1 */ \ + rd %psr, %g4; \ + st %g4, [%g1]; \ + rd %wim, %g4; \ + st %g4, [%g1 + 0x4]; + +#define SAVE_CPU_WINDOW_STATE(type) \ + /* Save window state into context at %g1 */ \ + st %o0, [%g1 + 0x30]; \ + st %o1, [%g1 + 0x34]; \ + st %o2, [%g1 + 0x38]; \ + st %o3, [%g1 + 0x3c]; \ + st %o4, [%g1 + 0x40]; \ + st %o5, [%g1 + 0x44]; \ + st %o6, [%g1 + 0x48]; \ + st %o7, [%g1 + 0x4c]; \ + \ + set nwindows, %g6; \ + ld [%g6], %g6; /* nwindows */ \ + mov %g6, %g5; \ + sub %g5, 1, %g5; /* mask */ \ + \ + rd %psr, %g4; \ + and %g4, %g5, %g4; /* window */ \ + \ + rd %psr, %g3; \ + srl %g3, 5, %g3; \ + sll %g3, 5, %g3; /* psr hi */ \ + \ + mov %g1, %g2; \ + add %g2, 0x50, %g2; \ + \ +save_cpu_window_##type: \ + mov %g3, %g7; \ + or %g7, %g4, %g7; \ + wr %g7, %psr; \ + \ + st %l0, [%g2]; \ + st %l1, [%g2 + 0x4]; \ + st %l2, [%g2 + 0x8]; \ + st %l3, [%g2 + 0xc]; \ + st %l4, [%g2 + 0x10]; \ + st %l5, [%g2 + 0x14]; \ + st %l6, [%g2 + 0x18]; \ + st %l7, [%g2 + 0x1c]; \ + st %i0, [%g2 + 0x20]; \ + st %i1, [%g2 + 0x24]; \ + st %i2, [%g2 + 0x28]; \ + st %i3, [%g2 + 0x2c]; \ + st %i4, [%g2 + 0x30]; \ + st %i5, [%g2 + 0x34]; \ + st %i6, [%g2 + 0x38]; \ + st %i7, [%g2 + 0x3c]; \ + dec %g4; \ + and %g4, %g5, %g4; \ + subcc %g6, 1, %g6; \ + bne save_cpu_window_##type; \ + add %g2, 0x40, %g2; \ + \ + /* Get back to the correct window */ \ + ld [%g1], %g2; \ + wr %g2, %psr; + +#define SAVE_CPU_STATE(type) \ + SAVE_CPU_GENERAL_STATE(type); \ + SAVE_CPU_WINDOW_STATE(type); + + +#define RESTORE_CPU_GENERAL_STATE(type) \ + /* Restore window state from context at %g1 */ \ + ld [%g1], %g2; \ + wr %g2, %psr; \ + ld [%g1 + 0x4], %g2; \ + wr %g2, %wim; + +#define RESTORE_CPU_WINDOW_STATE(type) \ + /* Restore window state from context at %g1 */ \ + set nwindows, %g6; \ + ld [%g6], %g6; /* nwindows */ \ + mov %g6, %g5; \ + sub %g5, 1, %g5; /* mask */ \ + \ + rd %psr, %g4; \ + and %g4, %g5, %g4; /* window */ \ + \ + rd %psr, %g3; \ + srl %g3, 5, %g3; \ + sll %g3, 5, %g3; /* psr hi */ \ + \ + mov %g1, %g2; \ + add %g2, 0x50, %g2; \ + \ +restore_cpu_window_##type: \ + mov %g3, %g7; \ + or %g7, %g4, %g7; \ + wr %g7, %psr; \ + \ + ld [%g2], %l0; \ + ld [%g2 + 0x4], %l1; \ + ld [%g2 + 0x8], %l2; \ + ld [%g2 + 0xc], %l3; \ + ld [%g2 + 0x10], %l4; \ + ld [%g2 + 0x14], %l5; \ + ld [%g2 + 0x18], %l6; \ + ld [%g2 + 0x1c], %l7; \ + ld [%g2 + 0x20], %i0; \ + ld [%g2 + 0x24], %i1; \ + ld [%g2 + 0x28], %i2; \ + ld [%g2 + 0x2c], %i3; \ + ld [%g2 + 0x30], %i4; \ + ld [%g2 + 0x34], %i5; \ + ld [%g2 + 0x38], %i6; \ + ld [%g2 + 0x3c], %i7; \ + dec %g4; \ + and %g4, %g5, %g4; \ + subcc %g6, 1, %g6; \ + bne restore_cpu_window_##type; \ + add %g2, 0x40, %g2; \ + \ + /* Get back to the correct window */ \ + ld [%g1], %g2; \ + wr %g2, %psr; \ + \ + ld [%g1 + 0x30], %o0; \ + ld [%g1 + 0x34], %o1; \ + ld [%g1 + 0x38], %o2; \ + ld [%g1 + 0x3c], %o3; \ + ld [%g1 + 0x40], %o4; \ + ld [%g1 + 0x44], %o5; \ + ld [%g1 + 0x48], %o6; \ + ld [%g1 + 0x4c], %o7; + +#define RESTORE_CPU_STATE(type) \ + RESTORE_CPU_GENERAL_STATE(type); \ + RESTORE_CPU_WINDOW_STATE(type); diff --git a/arch/sparc32/entry.S b/arch/sparc32/entry.S index 72cb338..82aa88e 100644 --- a/arch/sparc32/entry.S +++ b/arch/sparc32/entry.S @@ -11,6 +11,7 @@ #include "psr.h" #include "asm/asi.h" #include "asm/crs.h" +#include "cpustate.h" #define NO_QEMU_PROTOS #define NO_OPENBIOS_PROTOS #include "arch/common/fw_cfg.h" @@ -30,7 +31,7 @@
#define WRITE_PAUSE nop; nop; nop; /* Have to do this after %wim/%psr chg */
- .globl entry, _entry + .globl entry, _entry, nwindows
.section ".text", "ax" .align 8 @@ -69,6 +70,9 @@ * Bottom */
+nwindows: + .word 0 + /* * Entry point * We start execution from here. @@ -460,6 +464,9 @@ highmem: wr %g1, 0x0, %wim ! make window 1 invalid WRITE_PAUSE
+ set nwindows, %g2 ! store nwindows + st %g3, [%g2] + cmp %g3, 0x7 bne 1f nop @@ -490,8 +497,21 @@ highmem: wr %g3, PSR_ET, %psr WRITE_PAUSE
- set 0, %fp - call __switch_context_nosave + /* Set up a default context */ + set __context, %g1 + ld [%g1], %g1 + + SAVE_CPU_GENERAL_STATE(entry) + SAVE_CPU_WINDOW_STATE(entry) + + /* Set up local stack pointer */ + set _estack - 0x40, %sp + + /* And for the main context */ + add %sp, -0x260, %g2 + st %g2, [%g1 + 0x48] + + call __switch_context nop
/* We get here when the main context switches back to diff --git a/arch/sparc32/switch.S b/arch/sparc32/switch.S index d5b1b65..e4dbc9a 100644 --- a/arch/sparc32/switch.S +++ b/arch/sparc32/switch.S @@ -1,23 +1,14 @@ #define __ASSEMBLY #include "psr.h" #include "asm/asi.h" +#include "cpustate.h" #define ASI_BP ASI_M_BYPASS #define REGWIN_SZ 0x40
- .globl __switch_context, __switch_context_nosave, __exit_context, halt + .globl __switch_context, __switch_context_nosave, __exit_context, halt
- .text - .align 4 - -#define STACKFRAME_SZ 0x60 - -/* These are just handy. */ -#define _SV save %sp, -STACKFRAME_SZ, %sp -#define _RS restore - -#define FLUSH_ALL_KERNEL_WINDOWS \ - _SV; _SV; _SV; _SV; _SV; _SV; _SV; \ - _RS; _RS; _RS; _RS; _RS; _RS; _RS; + .text + .align 4
/* * Switch execution context @@ -31,124 +22,62 @@
__switch_context: FLUSH_ALL_KERNEL_WINDOWS - /* Save everything in stack */ - st %fp, [%fp + 120 -144] - add %fp, -144, %fp - st %g1, [%fp + 4] - st %g2, [%fp + 8] - st %g3, [%fp + 12] - st %g4, [%fp + 16] - st %g5, [%fp + 20] - st %g6, [%fp + 24] - st %g7, [%fp + 28] - - st %o0, [%fp + 32] - st %o1, [%fp + 36] - st %o2, [%fp + 40] - st %o3, [%fp + 44] - st %o4, [%fp + 48] - st %o5, [%fp + 52] - st %sp, [%fp + 56] - st %o7, [%fp + 60] - - st %l0, [%fp + 64] - st %l1, [%fp + 68] - st %l2, [%fp + 72] - st %l3, [%fp + 76] - st %l4, [%fp + 80] - st %l5, [%fp + 84] - st %l6, [%fp + 88] - st %l7, [%fp + 92] + + /* Save everything in stack */ + st %g1, [%sp - 0x260 + 0x14] + st %g2, [%sp - 0x260 + 0x18] + st %g3, [%sp - 0x260 + 0x1c] + st %g4, [%sp - 0x260 + 0x20] + st %g5, [%sp - 0x260 + 0x24] + st %g6, [%sp - 0x260 + 0x28] + st %g7, [%sp - 0x260 + 0x2c] + + mov %sp, %g1 + add %g1, -0x260, %g1 + + SAVE_CPU_STATE(switch) + + /* Return PC value */ + mov %o7, %g2 + add %g2, 4, %g2 + st %g2, [%sp - 0x260 + 0x250] + + /* swap context */ + set __context, %g3 + ld [%g3], %g2 + st %g1, [%g3] + mov %g2, %g1 + + ba __set_context + nop
- st %i0, [%fp + 96] - st %i1, [%fp + 100] - st %i2, [%fp + 104] - st %i3, [%fp + 108] - st %i4, [%fp + 112] - st %i5, [%fp + 116] - st %i7, [%fp + 124] - - /* ctx->return_address: Return to caller */ - st %o7, [%fp + 128] - - /* Interrupts are not allowed... */ - - /* Turn on Supervisor, EnableFloating, and all the PIL bits. - * Also puts us in register window zero with traps off. - */ -#if 0 - set (PSR_PS | PSR_S | PSR_PIL | PSR_EF), %g2 - wr %g2, 0x0, %psr -#endif - set __context, %g1 - /* Swap ctx pointer with %fp and jump*/ - ba __set_context - swap [%g1], %fp __switch_context_nosave: + FLUSH_ALL_KERNEL_WINDOWS + set __context, %g1 - /* load %fp from ctx pointer */ - ld [%g1], %fp + ld [%g1], %g1 + __set_context: - /* Load all registers */ - /* offset 0: %g0, no need to load */ - ld [%fp + 4], %g1 - ld [%fp + 8], %g2 - ld [%fp + 12], %g3 - ld [%fp + 16], %g4 - ld [%fp + 20], %g5 - ld [%fp + 24], %g6 - ld [%fp + 28], %g7 - - /* offset 32: %o0, loaded from ctx->param */ - ld [%fp + 36], %o1 - ld [%fp + 40], %o2 - ld [%fp + 44], %o3 - ld [%fp + 48], %o4 - ld [%fp + 52], %o5 - ld [%fp + 56], %sp - /* offset 60: %o7, loaded from ctx->return_addr */ - - ld [%fp + 64], %l0 - ld [%fp + 68], %l1 - ld [%fp + 72], %l2 - ld [%fp + 76], %l3 - ld [%fp + 80], %l4 - ld [%fp + 84], %l5 - ld [%fp + 88], %l6 - ld [%fp + 92], %l7 - - ld [%fp + 96], %i0 - ld [%fp + 100], %i1 - ld [%fp + 104], %i2 - ld [%fp + 108], %i3 - ld [%fp + 112], %i4 - ld [%fp + 116], %i5 - ld [%fp + 124], %i7 - - /* ctx->return_addr */ - ld [%fp + 136], %o7 - - /* ctx->param */ - ld [%fp + 140], %o0 - - /* ctx->pc, save %g1 to %y and load to %g1 */ - mov %g1, %y - ld [%fp + 128], %g1 - /* %fp last */ - ld [%fp + 120], %fp - /* Finally, get the new %pc from %g1 and restore %g1*/ - jmp %g1 - mov %y, %g1 - - FLUSH_ALL_KERNEL_WINDOWS + RESTORE_CPU_STATE(switch) + + /* Restore globals */ + mov %g1, %g2 + add %g2, 0x14, %g2 + st %g2, [%sp - 96] + + ld [%g1 + 0x18], %g2 + ld [%g1 + 0x1c], %g3 + ld [%g1 + 0x20], %g4 + ld [%g1 + 0x24], %g5 + ld [%g1 + 0x28], %g6 + ld [%g1 + 0x2c], %g7 + + /* Finally, load new %pc */ + ld [%g1 + 0x250], %g1 + jmpl %g1, %o7 + ld [%sp - 96], %g1 + __exit_context: - /* Get back to the original context */ - call __switch_context - nop - - /* We get here if the other context attempt to switch to this - * dead context. This should not happen. */ - -halt: - b halt - nop + /* Get back to the original context */ + ba __switch_context + nop