summaryrefslogtreecommitdiffstats
path: root/arch/microblaze/kernel/entry.S
diff options
context:
space:
mode:
Diffstat (limited to 'arch/microblaze/kernel/entry.S')
-rw-r--r--arch/microblaze/kernel/entry.S206
1 files changed, 205 insertions, 1 deletions
diff --git a/arch/microblaze/kernel/entry.S b/arch/microblaze/kernel/entry.S
index 4b254fcd6961..4bf9cec516bc 100644
--- a/arch/microblaze/kernel/entry.S
+++ b/arch/microblaze/kernel/entry.S
@@ -30,6 +30,7 @@
#include <linux/errno.h>
#include <asm/signal.h>
+#include <asm/mmu.h>
#undef DEBUG
@@ -287,6 +288,44 @@ syscall_debug_table:
.text
+.extern cpuinfo
+
+C_ENTRY(mb_flush_dcache):
+ addik r1, r1, -PT_SIZE
+ SAVE_REGS
+
+ addik r3, r0, cpuinfo
+ lwi r7, r3, CI_DCS
+ lwi r8, r3, CI_DCL
+ sub r9, r7, r8
+1:
+ wdc.flush r9, r0
+ bgtid r9, 1b
+ addk r9, r9, r8
+
+ RESTORE_REGS
+ addik r1, r1, PT_SIZE
+ rtsd r15, 8
+ nop
+
+C_ENTRY(mb_invalidate_icache):
+ addik r1, r1, -PT_SIZE
+ SAVE_REGS
+
+ addik r3, r0, cpuinfo
+ lwi r7, r3, CI_ICS
+ lwi r8, r3, CI_ICL
+ sub r9, r7, r8
+1:
+ wic r9, r0
+ bgtid r9, 1b
+ addk r9, r9, r8
+
+ RESTORE_REGS
+ addik r1, r1, PT_SIZE
+ rtsd r15, 8
+ nop
+
/*
* User trap.
*
@@ -753,6 +792,160 @@ IRQ_return: /* MS: Make global symbol for debugging */
rtid r14, 0
nop
+#ifdef CONFIG_MB_MANAGER
+
+#define PT_PID PT_SIZE
+#define PT_TLBI PT_SIZE + 4
+#define PT_ZPR PT_SIZE + 8
+#define PT_TLBL0 PT_SIZE + 12
+#define PT_TLBH0 PT_SIZE + 16
+
+C_ENTRY(_xtmr_manager_reset):
+ lwi r1, r0, xmb_manager_stackpointer
+
+ /* Restore MSR */
+ lwi r2, r1, PT_MSR
+ mts rmsr, r2
+ bri 4
+
+ /* restore Special purpose registers */
+ lwi r2, r1, PT_PID
+ mts rpid, r2
+
+ lwi r2, r1, PT_TLBI
+ mts rtlbx, r2
+
+ lwi r2, r1, PT_ZPR
+ mts rzpr, r2
+
+#if CONFIG_XILINX_MICROBLAZE0_USE_FPU
+ lwi r2, r1, PT_FSR
+ mts rfsr, r2
+#endif
+
+ /* restore all the tlb's */
+ addik r3, r0, TOPHYS(tlb_skip)
+ addik r6, r0, PT_TLBL0
+ addik r7, r0, PT_TLBH0
+restore_tlb:
+ add r6, r6, r1
+ add r7, r7, r1
+ lwi r2, r6, 0
+ mts rtlblo, r2
+ lwi r2, r7, 0
+ mts rtlbhi, r2
+ addik r6, r6, 4
+ addik r7, r7, 4
+ bgtid r3, restore_tlb
+ addik r3, r3, -1
+
+ lwi r5, r0, TOPHYS(xmb_manager_dev)
+ lwi r8, r0, TOPHYS(xmb_manager_reset_callback)
+ set_vms
+ /* return from reset need -8 to adjust for rtsd r15, 8 */
+ addik r15, r0, ret_from_reset - 8
+ rtbd r8, 0
+ nop
+
+ret_from_reset:
+ set_bip /* Ints masked for state restore */
+ VM_OFF
+ /* MS: Restore all regs */
+ RESTORE_REGS
+ lwi r14, r1, PT_R14
+ lwi r16, r1, PT_PC
+ addik r1, r1, PT_SIZE + 36
+ rtbd r16, 0
+ nop
+
+/*
+ * Break handler for MB Manager. Enter to _xmb_manager_break by
+ * injecting fault in one of the TMR Microblaze core.
+ * FIXME: This break handler supports getting
+ * called from kernel space only.
+ */
+C_ENTRY(_xmb_manager_break):
+ /*
+ * Reserve memory in the stack for context store/restore
+ * (which includes memory for storing tlbs (max two tlbs))
+ */
+ addik r1, r1, -PT_SIZE - 36
+ swi r1, r0, xmb_manager_stackpointer
+ SAVE_REGS
+ swi r14, r1, PT_R14 /* rewrite saved R14 value */
+ swi r16, r1, PT_PC; /* PC and r16 are the same */
+
+ lwi r6, r0, TOPHYS(xmb_manager_baseaddr)
+ lwi r7, r0, TOPHYS(xmb_manager_crval)
+ /*
+ * When the break vector gets asserted because of error injection,
+ * the break signal must be blocked before exiting from the
+ * break handler, below code configures the tmr manager
+ * control register to block break signal.
+ */
+ swi r7, r6, 0
+
+ /* Save the special purpose registers */
+ mfs r2, rpid
+ swi r2, r1, PT_PID
+
+ mfs r2, rtlbx
+ swi r2, r1, PT_TLBI
+
+ mfs r2, rzpr
+ swi r2, r1, PT_ZPR
+
+#if CONFIG_XILINX_MICROBLAZE0_USE_FPU
+ mfs r2, rfsr
+ swi r2, r1, PT_FSR
+#endif
+ mfs r2, rmsr
+ swi r2, r1, PT_MSR
+
+ /* Save all the tlb's */
+ addik r3, r0, TOPHYS(tlb_skip)
+ addik r6, r0, PT_TLBL0
+ addik r7, r0, PT_TLBH0
+save_tlb:
+ add r6, r6, r1
+ add r7, r7, r1
+ mfs r2, rtlblo
+ swi r2, r6, 0
+ mfs r2, rtlbhi
+ swi r2, r7, 0
+ addik r6, r6, 4
+ addik r7, r7, 4
+ bgtid r3, save_tlb
+ addik r3, r3, -1
+
+ lwi r5, r0, TOPHYS(xmb_manager_dev)
+ lwi r8, r0, TOPHYS(xmb_manager_callback)
+ /* return from break need -8 to adjust for rtsd r15, 8 */
+ addik r15, r0, ret_from_break - 8
+ rtbd r8, 0
+ nop
+
+ret_from_break:
+ /* flush the d-cache */
+ bralid r15, mb_flush_dcache
+ nop
+
+ /*
+ * To make sure microblaze i-cache is in a proper state
+ * invalidate the i-cache.
+ */
+ bralid r15, mb_invalidate_icache
+ nop
+
+ set_bip; /* Ints masked for state restore */
+ VM_OFF;
+ mbar 1
+ mbar 2
+ bri 4
+ suspend
+ nop
+#endif
+
/*
* Debug trap for KGDB. Enter to _debug_exception by brki r16, 0x18
* and call handling function with saved pt_regs
@@ -964,6 +1157,7 @@ ENTRY(_switch_to)
.global xmb_manager_crval
.global xmb_manager_callback
.global xmb_manager_reset_callback
+.global xmb_manager_stackpointer
.align 4
xmb_manager_dev:
.long 0
@@ -975,6 +1169,8 @@ xmb_manager_callback:
.long 0
xmb_manager_reset_callback:
.long 0
+xmb_manager_stackpointer:
+ .long 0
/*
* When the break vector gets asserted because of error injection,
@@ -1008,16 +1204,24 @@ ENTRY(_reset)
/* These are compiled and loaded into high memory, then
* copied into place in mach_early_setup */
.section .init.ivt, "ax"
-#if CONFIG_MANUAL_RESET_VECTOR
+#if CONFIG_MANUAL_RESET_VECTOR && !defined(CONFIG_MB_MANAGER)
.org 0x0
brai CONFIG_MANUAL_RESET_VECTOR
+#elif defined(CONFIG_MB_MANAGER)
+ .org 0x0
+ brai TOPHYS(_xtmr_manager_reset);
#endif
.org 0x8
brai TOPHYS(_user_exception); /* syscall handler */
.org 0x10
brai TOPHYS(_interrupt); /* Interrupt handler */
+#ifdef CONFIG_MB_MANAGER
+ .org 0x18
+ brai TOPHYS(_xmb_manager_break); /* microblaze manager break handler */
+#else
.org 0x18
brai TOPHYS(_debug_exception); /* debug trap handler */
+#endif
.org 0x20
brai TOPHYS(_hw_exception_handler); /* HW exception handler */