Commit 88707ebe authored by Appana Durga Kedareswara rao's avatar Appana Durga Kedareswara rao Committed by Michal Simek
Browse files

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: default avatarAppana 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: default avatarMichal Simek <michal.simek@amd.com>
parent a5e3aaa6

arch/microblaze/kernel/asm-offsets.c

+7 −0
Original line number Diff line number Diff line
@@ -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;

}

arch/microblaze/kernel/entry.S

+205 −1
Original line number Diff line number Diff line
@@ -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 */