microblaze: Add custom break vector handler for mb manager

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@xilinx.com>
Link: https://lore.kernel.org/r/20220627064024.771037-3-appana.durga.rao@xilinx.com
Signed-off-by: Michal Simek <michal.simek@amd.com>

diff --git a/arch/microblaze/kernel/asm-offsets.c b/arch/microblaze/kernel/asm-offsets.c
index 47ee409508b1c227e13ac7ace2a62e96a75328cc..104c3ac5f30c88785b21055b43f38e72cca24a11 100644 (file)
--- 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 4b254fcd69613fca0cd546daedf4313e11aa575f..4bf9cec516bcc310bf8196eec050c5147e399954 100644 (file)
--- 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 */