[PATCH 2/3] microblaze: Add custom break vector handler for mb manager
Appana Durga Kedareswara rao
appana.durga.rao at xilinx.com
Sun Jun 26 23:40:23 PDT 2022
When the TMR Manager detects a fault Lockstep state it is signaled to the
MicroBlaze processors by asserting a break signal, When Microblaze gets
a break vector from tmr Microblaze it's needed to clear/block the break
bit in the tmr manager before performing recovery.
In order to perform recovery need to perform the following steps.
1) Store all internal MicroBlaze registers in RAM
2) Execute a suspend instruction which asserts the reset signal
3) Restore all registers from RAM and execute an RTBD instruction to
return from the reset handler, to resume execution at the place
where the break occurred.
This API supports getting called from kernel space only.
Signed-off-by: Appana Durga Kedareswara rao <appana.durga.rao at xilinx.com>
---
arch/microblaze/kernel/asm-offsets.c | 7 +
arch/microblaze/kernel/entry.S | 206 ++++++++++++++++++++++++++-
2 files changed, 212 insertions(+), 1 deletion(-)
diff --git a/arch/microblaze/kernel/asm-offsets.c b/arch/microblaze/kernel/asm-offsets.c
index 47ee409508b1..104c3ac5f30c 100644
--- a/arch/microblaze/kernel/asm-offsets.c
+++ b/arch/microblaze/kernel/asm-offsets.c
@@ -120,5 +120,12 @@ int main(int argc, char *argv[])
DEFINE(CC_FSR, offsetof(struct cpu_context, fsr));
BLANK();
+ /* struct cpuinfo */
+ DEFINE(CI_DCS, offsetof(struct cpuinfo, dcache_size));
+ DEFINE(CI_DCL, offsetof(struct cpuinfo, dcache_line_length));
+ DEFINE(CI_ICS, offsetof(struct cpuinfo, icache_size));
+ DEFINE(CI_ICL, offsetof(struct cpuinfo, icache_line_length));
+ BLANK();
+
return 0;
}
diff --git a/arch/microblaze/kernel/entry.S b/arch/microblaze/kernel/entry.S
index b8e1dfe02d58..df367bf94b26 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 */
--
2.25.1
More information about the linux-arm-kernel
mailing list