diff options
Diffstat (limited to 'arch/microblaze/kernel/entry.S')
| -rw-r--r-- | arch/microblaze/kernel/entry.S | 1311 |
1 files changed, 1311 insertions, 0 deletions
diff --git a/arch/microblaze/kernel/entry.S b/arch/microblaze/kernel/entry.S new file mode 100644 index 000000000..582d7256d --- /dev/null +++ b/arch/microblaze/kernel/entry.S @@ -0,0 +1,1311 @@ +/* + * Low-level system-call handling, trap handlers and context-switching + * + * Copyright (C) 2008-2009 Michal Simek <monstr@monstr.eu> + * Copyright (C) 2008-2009 PetaLogix + * Copyright (C) 2003 John Williams <jwilliams@itee.uq.edu.au> + * Copyright (C) 2001,2002 NEC Corporation + * Copyright (C) 2001,2002 Miles Bader <miles@gnu.org> + * + * This file is subject to the terms and conditions of the GNU General + * Public License. See the file COPYING in the main directory of this + * archive for more details. + * + * Written by Miles Bader <miles@gnu.org> + * Heavily modified by John Williams for Microblaze + */ + +#include <linux/sys.h> +#include <linux/linkage.h> + +#include <asm/entry.h> +#include <asm/current.h> +#include <asm/processor.h> +#include <asm/exceptions.h> +#include <asm/asm-offsets.h> +#include <asm/thread_info.h> + +#include <asm/page.h> +#include <asm/unistd.h> +#include <asm/xilinx_mb_manager.h> + +#include <linux/errno.h> +#include <asm/signal.h> +#include <asm/mmu.h> + +#undef DEBUG + +#ifdef DEBUG +/* Create space for syscalls counting. */ +.section .data +.global syscall_debug_table +.align 4 +syscall_debug_table: + .space (__NR_syscalls * 4) +#endif /* DEBUG */ + +#define C_ENTRY(name) .globl name; .align 4; name + +/* + * Various ways of setting and clearing BIP in flags reg. + * This is mucky, but necessary using microblaze version that + * allows msr ops to write to BIP + */ +#if CONFIG_XILINX_MICROBLAZE0_USE_MSR_INSTR + .macro clear_bip + msrclr r0, MSR_BIP + .endm + + .macro set_bip + msrset r0, MSR_BIP + .endm + + .macro clear_eip + msrclr r0, MSR_EIP + .endm + + .macro set_ee + msrset r0, MSR_EE + .endm + + .macro disable_irq + msrclr r0, MSR_IE + .endm + + .macro enable_irq + msrset r0, MSR_IE + .endm + + .macro set_ums + msrset r0, MSR_UMS + msrclr r0, MSR_VMS + .endm + + .macro set_vms + msrclr r0, MSR_UMS + msrset r0, MSR_VMS + .endm + + .macro clear_ums + msrclr r0, MSR_UMS + .endm + + .macro clear_vms_ums + msrclr r0, MSR_VMS | MSR_UMS + .endm +#else + .macro clear_bip + mfs r11, rmsr + andi r11, r11, ~MSR_BIP + mts rmsr, r11 + .endm + + .macro set_bip + mfs r11, rmsr + ori r11, r11, MSR_BIP + mts rmsr, r11 + .endm + + .macro clear_eip + mfs r11, rmsr + andi r11, r11, ~MSR_EIP + mts rmsr, r11 + .endm + + .macro set_ee + mfs r11, rmsr + ori r11, r11, MSR_EE + mts rmsr, r11 + .endm + + .macro disable_irq + mfs r11, rmsr + andi r11, r11, ~MSR_IE + mts rmsr, r11 + .endm + + .macro enable_irq + mfs r11, rmsr + ori r11, r11, MSR_IE + mts rmsr, r11 + .endm + + .macro set_ums + mfs r11, rmsr + ori r11, r11, MSR_VMS + andni r11, r11, MSR_UMS + mts rmsr, r11 + .endm + + .macro set_vms + mfs r11, rmsr + ori r11, r11, MSR_VMS + andni r11, r11, MSR_UMS + mts rmsr, r11 + .endm + + .macro clear_ums + mfs r11, rmsr + andni r11, r11, MSR_UMS + mts rmsr,r11 + .endm + + .macro clear_vms_ums + mfs r11, rmsr + andni r11, r11, (MSR_VMS|MSR_UMS) + mts rmsr,r11 + .endm +#endif + +/* Define how to call high-level functions. With MMU, virtual mode must be + * enabled when calling the high-level function. Clobbers R11. + * VM_ON, VM_OFF, DO_JUMP_BIPCLR, DO_CALL + */ + +/* turn on virtual protected mode save */ +#define VM_ON \ + set_ums; \ + rted r0, 2f; \ + nop; \ +2: + +/* turn off virtual protected mode save and user mode save*/ +#define VM_OFF \ + clear_vms_ums; \ + rted r0, TOPHYS(1f); \ + nop; \ +1: + +#define SAVE_REGS \ + swi r2, r1, PT_R2; /* Save SDA */ \ + swi r3, r1, PT_R3; \ + swi r4, r1, PT_R4; \ + swi r5, r1, PT_R5; \ + swi r6, r1, PT_R6; \ + swi r7, r1, PT_R7; \ + swi r8, r1, PT_R8; \ + swi r9, r1, PT_R9; \ + swi r10, r1, PT_R10; \ + swi r11, r1, PT_R11; /* save clobbered regs after rval */\ + swi r12, r1, PT_R12; \ + swi r13, r1, PT_R13; /* Save SDA2 */ \ + swi r14, r1, PT_PC; /* PC, before IRQ/trap */ \ + swi r15, r1, PT_R15; /* Save LP */ \ + swi r16, r1, PT_R16; \ + swi r17, r1, PT_R17; \ + swi r18, r1, PT_R18; /* Save asm scratch reg */ \ + swi r19, r1, PT_R19; \ + swi r20, r1, PT_R20; \ + swi r21, r1, PT_R21; \ + swi r22, r1, PT_R22; \ + swi r23, r1, PT_R23; \ + swi r24, r1, PT_R24; \ + swi r25, r1, PT_R25; \ + swi r26, r1, PT_R26; \ + swi r27, r1, PT_R27; \ + swi r28, r1, PT_R28; \ + swi r29, r1, PT_R29; \ + swi r30, r1, PT_R30; \ + swi r31, r1, PT_R31; /* Save current task reg */ \ + mfs r11, rmsr; /* save MSR */ \ + swi r11, r1, PT_MSR; + +#define RESTORE_REGS_GP \ + lwi r2, r1, PT_R2; /* restore SDA */ \ + lwi r3, r1, PT_R3; \ + lwi r4, r1, PT_R4; \ + lwi r5, r1, PT_R5; \ + lwi r6, r1, PT_R6; \ + lwi r7, r1, PT_R7; \ + lwi r8, r1, PT_R8; \ + lwi r9, r1, PT_R9; \ + lwi r10, r1, PT_R10; \ + lwi r11, r1, PT_R11; /* restore clobbered regs after rval */\ + lwi r12, r1, PT_R12; \ + lwi r13, r1, PT_R13; /* restore SDA2 */ \ + lwi r14, r1, PT_PC; /* RESTORE_LINK PC, before IRQ/trap */\ + lwi r15, r1, PT_R15; /* restore LP */ \ + lwi r16, r1, PT_R16; \ + lwi r17, r1, PT_R17; \ + lwi r18, r1, PT_R18; /* restore asm scratch reg */ \ + lwi r19, r1, PT_R19; \ + lwi r20, r1, PT_R20; \ + lwi r21, r1, PT_R21; \ + lwi r22, r1, PT_R22; \ + lwi r23, r1, PT_R23; \ + lwi r24, r1, PT_R24; \ + lwi r25, r1, PT_R25; \ + lwi r26, r1, PT_R26; \ + lwi r27, r1, PT_R27; \ + lwi r28, r1, PT_R28; \ + lwi r29, r1, PT_R29; \ + lwi r30, r1, PT_R30; \ + lwi r31, r1, PT_R31; /* Restore cur task reg */ + +#define RESTORE_REGS \ + lwi r11, r1, PT_MSR; \ + mts rmsr , r11; \ + RESTORE_REGS_GP + +#define RESTORE_REGS_RTBD \ + lwi r11, r1, PT_MSR; \ + andni r11, r11, MSR_EIP; /* clear EIP */ \ + ori r11, r11, MSR_EE | MSR_BIP; /* set EE and BIP */ \ + mts rmsr , r11; \ + RESTORE_REGS_GP + +#define SAVE_STATE \ + swi r1, r0, TOPHYS(PER_CPU(ENTRY_SP)); /* save stack */ \ + /* See if already in kernel mode.*/ \ + mfs r1, rmsr; \ + andi r1, r1, MSR_UMS; \ + bnei r1, 1f; \ + /* Kernel-mode state save. */ \ + /* Reload kernel stack-ptr. */ \ + lwi r1, r0, TOPHYS(PER_CPU(ENTRY_SP)); \ + /* FIXME: I can add these two lines to one */ \ + /* tophys(r1,r1); */ \ + /* addik r1, r1, -PT_SIZE; */ \ + addik r1, r1, CONFIG_KERNEL_BASE_ADDR - CONFIG_KERNEL_START - PT_SIZE; \ + SAVE_REGS \ + brid 2f; \ + swi r1, r1, PT_MODE; \ +1: /* User-mode state save. */ \ + lwi r1, r0, TOPHYS(PER_CPU(CURRENT_SAVE)); /* get saved current */\ + tophys(r1,r1); \ + lwi r1, r1, TS_THREAD_INFO; /* get the thread info */ \ + /* MS these three instructions can be added to one */ \ + /* addik r1, r1, THREAD_SIZE; */ \ + /* tophys(r1,r1); */ \ + /* addik r1, r1, -PT_SIZE; */ \ + addik r1, r1, THREAD_SIZE + CONFIG_KERNEL_BASE_ADDR - CONFIG_KERNEL_START - PT_SIZE; \ + SAVE_REGS \ + lwi r11, r0, TOPHYS(PER_CPU(ENTRY_SP)); \ + swi r11, r1, PT_R1; /* Store user SP. */ \ + swi r0, r1, PT_MODE; /* Was in user-mode. */ \ + /* MS: I am clearing UMS even in case when I come from kernel space */ \ + clear_ums; \ +2: lwi CURRENT_TASK, r0, TOPHYS(PER_CPU(CURRENT_SAVE)); + +.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. + * + * System calls are handled here. + * + * Syscall protocol: + * Syscall number in r12, args in r5-r10 + * Return value in r3 + * + * Trap entered via brki instruction, so BIP bit is set, and interrupts + * are masked. This is nice, means we don't have to CLI before state save + */ +C_ENTRY(_user_exception): + swi r1, r0, TOPHYS(PER_CPU(ENTRY_SP)) /* save stack */ + addi r14, r14, 4 /* return address is 4 byte after call */ + + lwi r1, r0, TOPHYS(PER_CPU(CURRENT_SAVE)); /* get saved current */ + tophys(r1,r1); + lwi r1, r1, TS_THREAD_INFO; /* get stack from task_struct */ +/* calculate kernel stack pointer from task struct 8k */ + addik r1, r1, THREAD_SIZE; + tophys(r1,r1); + + addik r1, r1, -PT_SIZE; /* Make room on the stack. */ + SAVE_REGS + swi r0, r1, PT_R3 + swi r0, r1, PT_R4 + + swi r0, r1, PT_MODE; /* Was in user-mode. */ + lwi r11, r0, TOPHYS(PER_CPU(ENTRY_SP)); + swi r11, r1, PT_R1; /* Store user SP. */ + clear_ums; +2: lwi CURRENT_TASK, r0, TOPHYS(PER_CPU(CURRENT_SAVE)); + /* Save away the syscall number. */ + swi r12, r1, PT_R0; + tovirt(r1,r1) + +/* where the trap should return need -8 to adjust for rtsd r15, 8*/ +/* Jump to the appropriate function for the system call number in r12 + * (r12 is not preserved), or return an error if r12 is not valid. The LP + * register should point to the location where + * the called function should return. [note that MAKE_SYS_CALL uses label 1] */ + + /* Step into virtual mode */ + rtbd r0, 3f + nop +3: + lwi r11, CURRENT_TASK, TS_THREAD_INFO /* get thread info */ + lwi r11, r11, TI_FLAGS /* get flags in thread info */ + andi r11, r11, _TIF_WORK_SYSCALL_MASK + beqi r11, 4f + + addik r3, r0, -ENOSYS + swi r3, r1, PT_R3 + brlid r15, do_syscall_trace_enter + addik r5, r1, PT_R0 + + # do_syscall_trace_enter returns the new syscall nr. + addk r12, r0, r3 + lwi r5, r1, PT_R5; + lwi r6, r1, PT_R6; + lwi r7, r1, PT_R7; + lwi r8, r1, PT_R8; + lwi r9, r1, PT_R9; + lwi r10, r1, PT_R10; +4: +/* Jump to the appropriate function for the system call number in r12 + * (r12 is not preserved), or return an error if r12 is not valid. + * The LP register should point to the location where the called function + * should return. [note that MAKE_SYS_CALL uses label 1] */ + /* See if the system call number is valid */ + blti r12, 5f + addi r11, r12, -__NR_syscalls; + bgei r11, 5f; + /* Figure out which function to use for this system call. */ + /* Note Microblaze barrel shift is optional, so don't rely on it */ + add r12, r12, r12; /* convert num -> ptr */ + add r12, r12, r12; + addi r30, r0, 1 /* restarts allowed */ + +#ifdef DEBUG + /* Trac syscalls and stored them to syscall_debug_table */ + /* The first syscall location stores total syscall number */ + lwi r3, r0, syscall_debug_table + addi r3, r3, 1 + swi r3, r0, syscall_debug_table + lwi r3, r12, syscall_debug_table + addi r3, r3, 1 + swi r3, r12, syscall_debug_table +#endif + + # Find and jump into the syscall handler. + lwi r12, r12, sys_call_table + /* where the trap should return need -8 to adjust for rtsd r15, 8 */ + addi r15, r0, ret_from_trap-8 + bra r12 + + /* The syscall number is invalid, return an error. */ +5: + braid ret_from_trap + addi r3, r0, -ENOSYS; + +/* Entry point used to return from a syscall/trap */ +/* We re-enable BIP bit before state restore */ +C_ENTRY(ret_from_trap): + swi r3, r1, PT_R3 + swi r4, r1, PT_R4 + + lwi r11, r1, PT_MODE; +/* See if returning to kernel mode, if so, skip resched &c. */ + bnei r11, 2f; + /* We're returning to user mode, so check for various conditions that + * trigger rescheduling. */ + /* FIXME: Restructure all these flag checks. */ + lwi r11, CURRENT_TASK, TS_THREAD_INFO; /* get thread info */ + lwi r11, r11, TI_FLAGS; /* get flags in thread info */ + andi r11, r11, _TIF_WORK_SYSCALL_MASK + beqi r11, 1f + + brlid r15, do_syscall_trace_leave + addik r5, r1, PT_R0 +1: + /* We're returning to user mode, so check for various conditions that + * trigger rescheduling. */ + /* get thread info from current task */ + lwi r11, CURRENT_TASK, TS_THREAD_INFO; + lwi r19, r11, TI_FLAGS; /* get flags in thread info */ + andi r11, r19, _TIF_NEED_RESCHED; + beqi r11, 5f; + + bralid r15, schedule; /* Call scheduler */ + nop; /* delay slot */ + bri 1b + + /* Maybe handle a signal */ +5: + andi r11, r19, _TIF_SIGPENDING | _TIF_NOTIFY_RESUME; + beqi r11, 4f; /* Signals to handle, handle them */ + + addik r5, r1, 0; /* Arg 1: struct pt_regs *regs */ + bralid r15, do_notify_resume; /* Handle any signals */ + add r6, r30, r0; /* Arg 2: int in_syscall */ + add r30, r0, r0 /* no more restarts */ + bri 1b + +/* Finally, return to user state. */ +4: set_bip; /* Ints masked for state restore */ + swi CURRENT_TASK, r0, PER_CPU(CURRENT_SAVE); /* save current */ + VM_OFF; + tophys(r1,r1); + RESTORE_REGS_RTBD; + addik r1, r1, PT_SIZE /* Clean up stack space. */ + lwi r1, r1, PT_R1 - PT_SIZE;/* Restore user stack pointer. */ + bri 6f; + +/* Return to kernel state. */ +2: set_bip; /* Ints masked for state restore */ + VM_OFF; + tophys(r1,r1); + RESTORE_REGS_RTBD; + addik r1, r1, PT_SIZE /* Clean up stack space. */ + tovirt(r1,r1); +6: +TRAP_return: /* Make global symbol for debugging */ + rtbd r14, 0; /* Instructions to return from an IRQ */ + nop; + + +/* This the initial entry point for a new child thread, with an appropriate + stack in place that makes it look like the child is in the middle of a + syscall. This function is actually `returned to' from switch_thread + (copy_thread makes ret_from_fork the return address in each new thread's + saved context). */ +C_ENTRY(ret_from_fork): + bralid r15, schedule_tail; /* ...which is schedule_tail's arg */ + add r5, r3, r0; /* switch_thread returns the prev task */ + /* ( in the delay slot ) */ + brid ret_from_trap; /* Do normal trap return */ + add r3, r0, r0; /* Child's fork call should return 0. */ + +C_ENTRY(ret_from_kernel_thread): + bralid r15, schedule_tail; /* ...which is schedule_tail's arg */ + add r5, r3, r0; /* switch_thread returns the prev task */ + /* ( in the delay slot ) */ + brald r15, r20 /* fn was left in r20 */ + addk r5, r0, r19 /* ... and argument - in r19 */ + brid ret_from_trap + add r3, r0, r0 + +C_ENTRY(sys_rt_sigreturn_wrapper): + addik r30, r0, 0 /* no restarts */ + brid sys_rt_sigreturn /* Do real work */ + addik r5, r1, 0; /* add user context as 1st arg */ + +/* + * HW EXCEPTION rutine start + */ +C_ENTRY(full_exception_trap): + /* adjust exception address for privileged instruction + * for finding where is it */ + addik r17, r17, -4 + SAVE_STATE /* Save registers */ + /* PC, before IRQ/trap - this is one instruction above */ + swi r17, r1, PT_PC; + tovirt(r1,r1) + /* FIXME this can be store directly in PT_ESR reg. + * I tested it but there is a fault */ + /* where the trap should return need -8 to adjust for rtsd r15, 8 */ + addik r15, r0, ret_from_exc - 8 + mfs r6, resr + mfs r7, rfsr; /* save FSR */ + mts rfsr, r0; /* Clear sticky fsr */ + rted r0, full_exception + addik r5, r1, 0 /* parameter struct pt_regs * regs */ + +/* + * Unaligned data trap. + * + * Unaligned data trap last on 4k page is handled here. + * + * Trap entered via exception, so EE bit is set, and interrupts + * are masked. This is nice, means we don't have to CLI before state save + * + * The assembler routine is in "arch/microblaze/kernel/hw_exception_handler.S" + */ +C_ENTRY(unaligned_data_trap): + /* MS: I have to save r11 value and then restore it because + * set_bit, clear_eip, set_ee use r11 as temp register if MSR + * instructions are not used. We don't need to do if MSR instructions + * are used and they use r0 instead of r11. + * I am using ENTRY_SP which should be primary used only for stack + * pointer saving. */ + swi r11, r0, TOPHYS(PER_CPU(ENTRY_SP)); + set_bip; /* equalize initial state for all possible entries */ + clear_eip; + set_ee; + lwi r11, r0, TOPHYS(PER_CPU(ENTRY_SP)); + SAVE_STATE /* Save registers.*/ + /* PC, before IRQ/trap - this is one instruction above */ + swi r17, r1, PT_PC; + tovirt(r1,r1) + /* where the trap should return need -8 to adjust for rtsd r15, 8 */ + addik r15, r0, ret_from_exc-8 + mfs r3, resr /* ESR */ + mfs r4, rear /* EAR */ + rtbd r0, _unaligned_data_exception + addik r7, r1, 0 /* parameter struct pt_regs * regs */ + +/* + * Page fault traps. + * + * If the real exception handler (from hw_exception_handler.S) didn't find + * the mapping for the process, then we're thrown here to handle such situation. + * + * Trap entered via exceptions, so EE bit is set, and interrupts + * are masked. This is nice, means we don't have to CLI before state save + * + * Build a standard exception frame for TLB Access errors. All TLB exceptions + * will bail out to this point if they can't resolve the lightweight TLB fault. + * + * The C function called is in "arch/microblaze/mm/fault.c", declared as: + * void do_page_fault(struct pt_regs *regs, + * unsigned long address, + * unsigned long error_code) + */ +/* data and intruction trap - which is choose is resolved int fault.c */ +C_ENTRY(page_fault_data_trap): + SAVE_STATE /* Save registers.*/ + /* PC, before IRQ/trap - this is one instruction above */ + swi r17, r1, PT_PC; + tovirt(r1,r1) + /* where the trap should return need -8 to adjust for rtsd r15, 8 */ + addik r15, r0, ret_from_exc-8 + mfs r6, rear /* parameter unsigned long address */ + mfs r7, resr /* parameter unsigned long error_code */ + rted r0, do_page_fault + addik r5, r1, 0 /* parameter struct pt_regs * regs */ + +C_ENTRY(page_fault_instr_trap): + SAVE_STATE /* Save registers.*/ + /* PC, before IRQ/trap - this is one instruction above */ + swi r17, r1, PT_PC; + tovirt(r1,r1) + /* where the trap should return need -8 to adjust for rtsd r15, 8 */ + addik r15, r0, ret_from_exc-8 + mfs r6, rear /* parameter unsigned long address */ + ori r7, r0, 0 /* parameter unsigned long error_code */ + rted r0, do_page_fault + addik r5, r1, 0 /* parameter struct pt_regs * regs */ + +/* Entry point used to return from an exception. */ +C_ENTRY(ret_from_exc): + lwi r11, r1, PT_MODE; + bnei r11, 2f; /* See if returning to kernel mode, */ + /* ... if so, skip resched &c. */ + + /* We're returning to user mode, so check for various conditions that + trigger rescheduling. */ +1: + lwi r11, CURRENT_TASK, TS_THREAD_INFO; /* get thread info */ + lwi r19, r11, TI_FLAGS; /* get flags in thread info */ + andi r11, r19, _TIF_NEED_RESCHED; + beqi r11, 5f; + +/* Call the scheduler before returning from a syscall/trap. */ + bralid r15, schedule; /* Call scheduler */ + nop; /* delay slot */ + bri 1b + + /* Maybe handle a signal */ +5: andi r11, r19, _TIF_SIGPENDING | _TIF_NOTIFY_RESUME; + beqi r11, 4f; /* Signals to handle, handle them */ + + /* + * Handle a signal return; Pending signals should be in r18. + * + * Not all registers are saved by the normal trap/interrupt entry + * points (for instance, call-saved registers (because the normal + * C-compiler calling sequence in the kernel makes sure they're + * preserved), and call-clobbered registers in the case of + * traps), but signal handlers may want to examine or change the + * complete register state. Here we save anything not saved by + * the normal entry sequence, so that it may be safely restored + * (in a possibly modified form) after do_notify_resume returns. */ + addik r5, r1, 0; /* Arg 1: struct pt_regs *regs */ + bralid r15, do_notify_resume; /* Handle any signals */ + addi r6, r0, 0; /* Arg 2: int in_syscall */ + bri 1b + +/* Finally, return to user state. */ +4: set_bip; /* Ints masked for state restore */ + swi CURRENT_TASK, r0, PER_CPU(CURRENT_SAVE); /* save current */ + VM_OFF; + tophys(r1,r1); + + RESTORE_REGS_RTBD; + addik r1, r1, PT_SIZE /* Clean up stack space. */ + + lwi r1, r1, PT_R1 - PT_SIZE; /* Restore user stack pointer. */ + bri 6f; +/* Return to kernel state. */ +2: set_bip; /* Ints masked for state restore */ + VM_OFF; + tophys(r1,r1); + RESTORE_REGS_RTBD; + addik r1, r1, PT_SIZE /* Clean up stack space. */ + + tovirt(r1,r1); +6: +EXC_return: /* Make global symbol for debugging */ + rtbd r14, 0; /* Instructions to return from an IRQ */ + nop; + +/* + * HW EXCEPTION rutine end + */ + +/* + * Hardware maskable interrupts. + * + * The stack-pointer (r1) should have already been saved to the memory + * location PER_CPU(ENTRY_SP). + */ +C_ENTRY(_interrupt): +/* MS: we are in physical address */ +/* Save registers, switch to proper stack, convert SP to virtual.*/ + swi r1, r0, TOPHYS(PER_CPU(ENTRY_SP)) + /* MS: See if already in kernel mode. */ + mfs r1, rmsr + nop + andi r1, r1, MSR_UMS + bnei r1, 1f + +/* Kernel-mode state save. */ + lwi r1, r0, TOPHYS(PER_CPU(ENTRY_SP)) + tophys(r1,r1); /* MS: I have in r1 physical address where stack is */ + /* save registers */ +/* MS: Make room on the stack -> activation record */ + addik r1, r1, -PT_SIZE; + SAVE_REGS + brid 2f; + swi r1, r1, PT_MODE; /* 0 - user mode, 1 - kernel mode */ +1: +/* User-mode state save. */ + /* MS: get the saved current */ + lwi r1, r0, TOPHYS(PER_CPU(CURRENT_SAVE)); + tophys(r1,r1); + lwi r1, r1, TS_THREAD_INFO; + addik r1, r1, THREAD_SIZE; + tophys(r1,r1); + /* save registers */ + addik r1, r1, -PT_SIZE; + SAVE_REGS + /* calculate mode */ + swi r0, r1, PT_MODE; + lwi r11, r0, TOPHYS(PER_CPU(ENTRY_SP)); + swi r11, r1, PT_R1; + clear_ums; +2: + lwi CURRENT_TASK, r0, TOPHYS(PER_CPU(CURRENT_SAVE)); + tovirt(r1,r1) + addik r15, r0, irq_call; +irq_call:rtbd r0, do_IRQ; + addik r5, r1, 0; + +/* MS: we are in virtual mode */ +ret_from_irq: + lwi r11, r1, PT_MODE; + bnei r11, 2f; + +1: + lwi r11, CURRENT_TASK, TS_THREAD_INFO; + lwi r19, r11, TI_FLAGS; /* MS: get flags from thread info */ + andi r11, r19, _TIF_NEED_RESCHED; + beqi r11, 5f + bralid r15, schedule; + nop; /* delay slot */ + bri 1b + + /* Maybe handle a signal */ +5: andi r11, r19, _TIF_SIGPENDING | _TIF_NOTIFY_RESUME; + beqid r11, no_intr_resched +/* Handle a signal return; Pending signals should be in r18. */ + addik r5, r1, 0; /* Arg 1: struct pt_regs *regs */ + bralid r15, do_notify_resume; /* Handle any signals */ + addi r6, r0, 0; /* Arg 2: int in_syscall */ + bri 1b + +/* Finally, return to user state. */ +no_intr_resched: + /* Disable interrupts, we are now committed to the state restore */ + disable_irq + swi CURRENT_TASK, r0, PER_CPU(CURRENT_SAVE); + VM_OFF; + tophys(r1,r1); + RESTORE_REGS + addik r1, r1, PT_SIZE /* MS: Clean up stack space. */ + lwi r1, r1, PT_R1 - PT_SIZE; + bri 6f; +/* MS: Return to kernel state. */ +2: +#ifdef CONFIG_PREEMPTION + lwi r11, CURRENT_TASK, TS_THREAD_INFO; + /* MS: get preempt_count from thread info */ + lwi r5, r11, TI_PREEMPT_COUNT; + bgti r5, restore; + + lwi r5, r11, TI_FLAGS; /* get flags in thread info */ + andi r5, r5, _TIF_NEED_RESCHED; + beqi r5, restore /* if zero jump over */ + + /* interrupts are off that's why I am calling preempt_chedule_irq */ + bralid r15, preempt_schedule_irq + nop +restore: +#endif + VM_OFF /* MS: turn off MMU */ + tophys(r1,r1) + RESTORE_REGS + addik r1, r1, PT_SIZE /* MS: Clean up stack space. */ + tovirt(r1,r1); +6: +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 + */ +C_ENTRY(_debug_exception): + /* BIP bit is set on entry, no interrupts can occur */ + swi r1, r0, TOPHYS(PER_CPU(ENTRY_SP)) + + mfs r1, rmsr + nop + andi r1, r1, MSR_UMS + bnei r1, 1f +/* MS: Kernel-mode state save - kgdb */ + lwi r1, r0, TOPHYS(PER_CPU(ENTRY_SP)); /* Reload kernel stack-ptr*/ + + /* BIP bit is set on entry, no interrupts can occur */ + addik r1, r1, CONFIG_KERNEL_BASE_ADDR - CONFIG_KERNEL_START - PT_SIZE; + SAVE_REGS; + /* save all regs to pt_reg structure */ + swi r0, r1, PT_R0; /* R0 must be saved too */ + swi r14, r1, PT_R14 /* rewrite saved R14 value */ + swi r16, r1, PT_PC; /* PC and r16 are the same */ + /* save special purpose registers to pt_regs */ + mfs r11, rear; + swi r11, r1, PT_EAR; + mfs r11, resr; + swi r11, r1, PT_ESR; + mfs r11, rfsr; + swi r11, r1, PT_FSR; + + /* stack pointer is in physical address at it is decrease + * by PT_SIZE but we need to get correct R1 value */ + addik r11, r1, CONFIG_KERNEL_START - CONFIG_KERNEL_BASE_ADDR + PT_SIZE; + swi r11, r1, PT_R1 + /* MS: r31 - current pointer isn't changed */ + tovirt(r1,r1) +#ifdef CONFIG_KGDB + addi r5, r1, 0 /* pass pt_reg address as the first arg */ + addik r15, r0, dbtrap_call; /* return address */ + rtbd r0, microblaze_kgdb_break + nop; +#endif + /* MS: Place handler for brki from kernel space if KGDB is OFF. + * It is very unlikely that another brki instruction is called. */ + bri 0 + +/* MS: User-mode state save - gdb */ +1: lwi r1, r0, TOPHYS(PER_CPU(CURRENT_SAVE)); /* get saved current */ + tophys(r1,r1); + lwi r1, r1, TS_THREAD_INFO; /* get the thread info */ + addik r1, r1, THREAD_SIZE; /* calculate kernel stack pointer */ + tophys(r1,r1); + + addik r1, r1, -PT_SIZE; /* Make room on the stack. */ + SAVE_REGS; + swi r16, r1, PT_PC; /* Save LP */ + swi r0, r1, PT_MODE; /* Was in user-mode. */ + lwi r11, r0, TOPHYS(PER_CPU(ENTRY_SP)); + swi r11, r1, PT_R1; /* Store user SP. */ + lwi CURRENT_TASK, r0, TOPHYS(PER_CPU(CURRENT_SAVE)); + tovirt(r1,r1) + set_vms; + addik r5, r1, 0; + addik r15, r0, dbtrap_call; +dbtrap_call: /* Return point for kernel/user entry + 8 because of rtsd r15, 8 */ + rtbd r0, sw_exception + nop + + /* MS: The first instruction for the second part of the gdb/kgdb */ + set_bip; /* Ints masked for state restore */ + lwi r11, r1, PT_MODE; + bnei r11, 2f; +/* MS: Return to user space - gdb */ +1: + /* Get current task ptr into r11 */ + lwi r11, CURRENT_TASK, TS_THREAD_INFO; /* get thread info */ + lwi r19, r11, TI_FLAGS; /* get flags in thread info */ + andi r11, r19, _TIF_NEED_RESCHED; + beqi r11, 5f; + + /* Call the scheduler before returning from a syscall/trap. */ + bralid r15, schedule; /* Call scheduler */ + nop; /* delay slot */ + bri 1b + + /* Maybe handle a signal */ +5: andi r11, r19, _TIF_SIGPENDING | _TIF_NOTIFY_RESUME; + beqi r11, 4f; /* Signals to handle, handle them */ + + addik r5, r1, 0; /* Arg 1: struct pt_regs *regs */ + bralid r15, do_notify_resume; /* Handle any signals */ + addi r6, r0, 0; /* Arg 2: int in_syscall */ + bri 1b + +/* Finally, return to user state. */ +4: swi CURRENT_TASK, r0, PER_CPU(CURRENT_SAVE); /* save current */ + VM_OFF; + tophys(r1,r1); + /* MS: Restore all regs */ + RESTORE_REGS_RTBD + addik r1, r1, PT_SIZE /* Clean up stack space */ + lwi r1, r1, PT_R1 - PT_SIZE; /* Restore user stack pointer */ +DBTRAP_return_user: /* MS: Make global symbol for debugging */ + rtbd r16, 0; /* MS: Instructions to return from a debug trap */ + nop; + +/* MS: Return to kernel state - kgdb */ +2: VM_OFF; + tophys(r1,r1); + /* MS: Restore all regs */ + RESTORE_REGS_RTBD + lwi r14, r1, PT_R14; + lwi r16, r1, PT_PC; + addik r1, r1, PT_SIZE; /* MS: Clean up stack space */ + tovirt(r1,r1); +DBTRAP_return_kernel: /* MS: Make global symbol for debugging */ + rtbd r16, 0; /* MS: Instructions to return from a debug trap */ + nop; + + +ENTRY(_switch_to) + /* prepare return value */ + addk r3, r0, CURRENT_TASK + + /* save registers in cpu_context */ + /* use r11 and r12, volatile registers, as temp register */ + /* give start of cpu_context for previous process */ + addik r11, r5, TI_CPU_CONTEXT + swi r1, r11, CC_R1 + swi r2, r11, CC_R2 + /* skip volatile registers. + * they are saved on stack when we jumped to _switch_to() */ + /* dedicated registers */ + swi r13, r11, CC_R13 + swi r14, r11, CC_R14 + swi r15, r11, CC_R15 + swi r16, r11, CC_R16 + swi r17, r11, CC_R17 + swi r18, r11, CC_R18 + /* save non-volatile registers */ + swi r19, r11, CC_R19 + swi r20, r11, CC_R20 + swi r21, r11, CC_R21 + swi r22, r11, CC_R22 + swi r23, r11, CC_R23 + swi r24, r11, CC_R24 + swi r25, r11, CC_R25 + swi r26, r11, CC_R26 + swi r27, r11, CC_R27 + swi r28, r11, CC_R28 + swi r29, r11, CC_R29 + swi r30, r11, CC_R30 + /* special purpose registers */ + mfs r12, rmsr + swi r12, r11, CC_MSR + mfs r12, rear + swi r12, r11, CC_EAR + mfs r12, resr + swi r12, r11, CC_ESR + mfs r12, rfsr + swi r12, r11, CC_FSR + + /* update r31, the current-give me pointer to task which will be next */ + lwi CURRENT_TASK, r6, TI_TASK + /* stored it to current_save too */ + swi CURRENT_TASK, r0, PER_CPU(CURRENT_SAVE) + + /* get new process' cpu context and restore */ + /* give me start where start context of next task */ + addik r11, r6, TI_CPU_CONTEXT + + /* non-volatile registers */ + lwi r30, r11, CC_R30 + lwi r29, r11, CC_R29 + lwi r28, r11, CC_R28 + lwi r27, r11, CC_R27 + lwi r26, r11, CC_R26 + lwi r25, r11, CC_R25 + lwi r24, r11, CC_R24 + lwi r23, r11, CC_R23 + lwi r22, r11, CC_R22 + lwi r21, r11, CC_R21 + lwi r20, r11, CC_R20 + lwi r19, r11, CC_R19 + /* dedicated registers */ + lwi r18, r11, CC_R18 + lwi r17, r11, CC_R17 + lwi r16, r11, CC_R16 + lwi r15, r11, CC_R15 + lwi r14, r11, CC_R14 + lwi r13, r11, CC_R13 + /* skip volatile registers */ + lwi r2, r11, CC_R2 + lwi r1, r11, CC_R1 + + /* special purpose registers */ + lwi r12, r11, CC_FSR + mts rfsr, r12 + lwi r12, r11, CC_MSR + mts rmsr, r12 + + rtsd r15, 8 + nop + +#ifdef CONFIG_MB_MANAGER +.global xmb_inject_err +.section .text +.align 2 +.ent xmb_inject_err +.type xmb_inject_err, @function +xmb_inject_err: + addik r1, r1, -PT_SIZE + SAVE_REGS + + /* Switch to real mode */ + VM_OFF; + set_bip; + mbar 1 + mbar 2 + bralid r15, XMB_INJECT_ERR_OFFSET + nop; + + /* enable virtual mode */ + set_vms; + /* barrier for instructions and data accesses */ + mbar 1 + mbar 2 + /* + * Enable Interrupts, Virtual Protected Mode, equalize + * initial state for all possible entries. + */ + rtbd r0, 1f + nop; +1: + RESTORE_REGS + addik r1, r1, PT_SIZE + rtsd r15, 8; + nop; +.end xmb_inject_err + +.section .data +.global xmb_manager_dev +.global xmb_manager_baseaddr +.global xmb_manager_crval +.global xmb_manager_callback +.global xmb_manager_reset_callback +.global xmb_manager_stackpointer +.align 4 +xmb_manager_dev: + .long 0 +xmb_manager_baseaddr: + .long 0 +xmb_manager_crval: + .long 0 +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, + * the break signal must be blocked before exiting from the + * break handler, Below api updates the manager address and + * control register and error count callback arguments, + * which will be used by the break handler to block the + * break and call the callback function. + */ +.global xmb_manager_register +.section .text +.align 2 +.ent xmb_manager_register +.type xmb_manager_register, @function +xmb_manager_register: + swi r5, r0, xmb_manager_baseaddr + swi r6, r0, xmb_manager_crval + swi r7, r0, xmb_manager_callback + swi r8, r0, xmb_manager_dev + swi r9, r0, xmb_manager_reset_callback + + rtsd r15, 8; + nop; +.end xmb_manager_register +#endif + +ENTRY(_reset) + VM_OFF + brai 0; /* Jump to reset vector */ + + /* 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 && !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 */ + +#ifdef CONFIG_MB_MANAGER + /* + * For TMR Inject API which injects the error should + * be executed from LMB. + * TMR Inject is programmed with address of 0x200 so that + * when program counter matches with this address error will + * be injected. 0x200 is expected to be next available bram + * offset, hence used for this api. + */ + .org XMB_INJECT_ERR_OFFSET +xmb_inject_error: + nop + rtsd r15, 8 + nop +#endif + +.section .rodata,"a" +#include "syscall_table.S" + +syscall_table_size=(.-sys_call_table) + +type_SYSCALL: + .ascii "SYSCALL\0" +type_IRQ: + .ascii "IRQ\0" +type_IRQ_PREEMPT: + .ascii "IRQ (PREEMPTED)\0" +type_SYSCALL_PREEMPT: + .ascii " SYSCALL (PREEMPTED)\0" + + /* + * Trap decoding for stack unwinder + * Tuples are (start addr, end addr, string) + * If return address lies on [start addr, end addr], + * unwinder displays 'string' + */ + + .align 4 +.global microblaze_trap_handlers +microblaze_trap_handlers: + /* Exact matches come first */ + .word ret_from_trap; .word ret_from_trap ; .word type_SYSCALL + .word ret_from_irq ; .word ret_from_irq ; .word type_IRQ + /* Fuzzy matches go here */ + .word ret_from_irq ; .word no_intr_resched ; .word type_IRQ_PREEMPT + .word ret_from_trap; .word TRAP_return ; .word type_SYSCALL_PREEMPT + /* End of table */ + .word 0 ; .word 0 ; .word 0 |