diff options
-rw-r--r-- | arch/microblaze/include/asm/exceptions.h | 96 | ||||
-rw-r--r-- | arch/microblaze/kernel/exceptions.c | 124 | ||||
-rw-r--r-- | arch/microblaze/kernel/hw_exception_handler.S | 458 |
3 files changed, 678 insertions, 0 deletions
diff --git a/arch/microblaze/include/asm/exceptions.h b/arch/microblaze/include/asm/exceptions.h new file mode 100644 index 000000000000..4cdd2159f470 --- /dev/null +++ b/arch/microblaze/include/asm/exceptions.h | |||
@@ -0,0 +1,96 @@ | |||
1 | /* | ||
2 | * Preliminary support for HW exception handing for Microblaze | ||
3 | * | ||
4 | * Copyright (C) 2008 Michal Simek | ||
5 | * Copyright (C) 2008 PetaLogix | ||
6 | * Copyright (C) 2005 John Williams <jwilliams@itee.uq.edu.au> | ||
7 | * | ||
8 | * This file is subject to the terms and conditions of the GNU General | ||
9 | * Public License. See the file COPYING in the main directory of this | ||
10 | * archive for more details. | ||
11 | */ | ||
12 | |||
13 | #ifndef _ASM_MICROBLAZE_EXCEPTIONS_H | ||
14 | #define _ASM_MICROBLAZE_EXCEPTIONS_H | ||
15 | |||
16 | #ifdef __KERNEL__ | ||
17 | #ifndef __ASSEMBLY__ | ||
18 | |||
19 | /* Macros to enable and disable HW exceptions in the MSR */ | ||
20 | /* Define MSR enable bit for HW exceptions */ | ||
21 | #define HWEX_MSR_BIT (1 << 8) | ||
22 | |||
23 | #if CONFIG_XILINX_MICROBLAZE0_USE_MSR_INSTR | ||
24 | #define __enable_hw_exceptions() \ | ||
25 | __asm__ __volatile__ (" msrset r0, %0; \ | ||
26 | nop;" \ | ||
27 | : \ | ||
28 | : "i" (HWEX_MSR_BIT) \ | ||
29 | : "memory") | ||
30 | |||
31 | #define __disable_hw_exceptions() \ | ||
32 | __asm__ __volatile__ (" msrclr r0, %0; \ | ||
33 | nop;" \ | ||
34 | : \ | ||
35 | : "i" (HWEX_MSR_BIT) \ | ||
36 | : "memory") | ||
37 | #else /* !CONFIG_XILINX_MICROBLAZE0_USE_MSR_INSTR */ | ||
38 | #define __enable_hw_exceptions() \ | ||
39 | __asm__ __volatile__ (" \ | ||
40 | mfs r12, rmsr; \ | ||
41 | nop; \ | ||
42 | ori r12, r12, %0; \ | ||
43 | mts rmsr, r12; \ | ||
44 | nop;" \ | ||
45 | : \ | ||
46 | : "i" (HWEX_MSR_BIT) \ | ||
47 | : "memory", "r12") | ||
48 | |||
49 | #define __disable_hw_exceptions() \ | ||
50 | __asm__ __volatile__ (" \ | ||
51 | mfs r12, rmsr; \ | ||
52 | nop; \ | ||
53 | andi r12, r12, ~%0; \ | ||
54 | mts rmsr, r12; \ | ||
55 | nop;" \ | ||
56 | : \ | ||
57 | : "i" (HWEX_MSR_BIT) \ | ||
58 | : "memory", "r12") | ||
59 | #endif /* CONFIG_XILINX_MICROBLAZE0_USE_MSR_INSTR */ | ||
60 | |||
61 | asmlinkage void full_exception(struct pt_regs *regs, unsigned int type, | ||
62 | int fsr, int addr); | ||
63 | |||
64 | #if defined(CONFIG_XMON) | ||
65 | extern void xmon(struct pt_regs *regs); | ||
66 | extern int xmon_bpt(struct pt_regs *regs); | ||
67 | extern int xmon_sstep(struct pt_regs *regs); | ||
68 | extern int xmon_iabr_match(struct pt_regs *regs); | ||
69 | extern int xmon_dabr_match(struct pt_regs *regs); | ||
70 | extern void (*xmon_fault_handler)(struct pt_regs *regs); | ||
71 | |||
72 | void (*debugger)(struct pt_regs *regs) = xmon; | ||
73 | int (*debugger_bpt)(struct pt_regs *regs) = xmon_bpt; | ||
74 | int (*debugger_sstep)(struct pt_regs *regs) = xmon_sstep; | ||
75 | int (*debugger_iabr_match)(struct pt_regs *regs) = xmon_iabr_match; | ||
76 | int (*debugger_dabr_match)(struct pt_regs *regs) = xmon_dabr_match; | ||
77 | void (*debugger_fault_handler)(struct pt_regs *regs); | ||
78 | #elif defined(CONFIG_KGDB) | ||
79 | void (*debugger)(struct pt_regs *regs); | ||
80 | int (*debugger_bpt)(struct pt_regs *regs); | ||
81 | int (*debugger_sstep)(struct pt_regs *regs); | ||
82 | int (*debugger_iabr_match)(struct pt_regs *regs); | ||
83 | int (*debugger_dabr_match)(struct pt_regs *regs); | ||
84 | void (*debugger_fault_handler)(struct pt_regs *regs); | ||
85 | #else | ||
86 | #define debugger(regs) do { } while (0) | ||
87 | #define debugger_bpt(regs) 0 | ||
88 | #define debugger_sstep(regs) 0 | ||
89 | #define debugger_iabr_match(regs) 0 | ||
90 | #define debugger_dabr_match(regs) 0 | ||
91 | #define debugger_fault_handler ((void (*)(struct pt_regs *))0) | ||
92 | #endif | ||
93 | |||
94 | #endif /*__ASSEMBLY__ */ | ||
95 | #endif /* __KERNEL__ */ | ||
96 | #endif /* _ASM_MICROBLAZE_EXCEPTIONS_H */ | ||
diff --git a/arch/microblaze/kernel/exceptions.c b/arch/microblaze/kernel/exceptions.c new file mode 100644 index 000000000000..4a8a4064c7ee --- /dev/null +++ b/arch/microblaze/kernel/exceptions.c | |||
@@ -0,0 +1,124 @@ | |||
1 | /* | ||
2 | * HW exception handling | ||
3 | * | ||
4 | * Copyright (C) 2008-2009 Michal Simek <monstr@monstr.eu> | ||
5 | * Copyright (C) 2008 PetaLogix | ||
6 | * | ||
7 | * This file is subject to the terms and conditions of the GNU General | ||
8 | * Public License. See the file COPYING in the main directory of this | ||
9 | * archive for more details. | ||
10 | */ | ||
11 | |||
12 | /* | ||
13 | * This file handles the architecture-dependent parts of hardware exceptions | ||
14 | */ | ||
15 | |||
16 | #include <linux/kernel.h> | ||
17 | #include <linux/signal.h> | ||
18 | #include <linux/sched.h> | ||
19 | #include <linux/kallsyms.h> | ||
20 | #include <linux/module.h> | ||
21 | |||
22 | #include <asm/exceptions.h> | ||
23 | #include <asm/entry.h> /* For KM CPU var */ | ||
24 | #include <asm/uaccess.h> | ||
25 | #include <asm/errno.h> | ||
26 | #include <asm/ptrace.h> | ||
27 | #include <asm/current.h> | ||
28 | |||
29 | #define MICROBLAZE_ILL_OPCODE_EXCEPTION 0x02 | ||
30 | #define MICROBLAZE_IBUS_EXCEPTION 0x03 | ||
31 | #define MICROBLAZE_DBUS_EXCEPTION 0x04 | ||
32 | #define MICROBLAZE_DIV_ZERO_EXCEPTION 0x05 | ||
33 | #define MICROBLAZE_FPU_EXCEPTION 0x06 | ||
34 | #define MICROBLAZE_PRIVILEG_EXCEPTION 0x07 | ||
35 | |||
36 | static DEFINE_SPINLOCK(die_lock); | ||
37 | |||
38 | void die(const char *str, struct pt_regs *fp, long err) | ||
39 | { | ||
40 | console_verbose(); | ||
41 | spin_lock_irq(&die_lock); | ||
42 | printk(KERN_WARNING "Oops: %s, sig: %ld\n", str, err); | ||
43 | show_regs(fp); | ||
44 | spin_unlock_irq(&die_lock); | ||
45 | /* do_exit() should take care of panic'ing from an interrupt | ||
46 | * context so we don't handle it here | ||
47 | */ | ||
48 | do_exit(err); | ||
49 | } | ||
50 | |||
51 | void _exception(int signr, struct pt_regs *regs, int code, unsigned long addr) | ||
52 | { | ||
53 | siginfo_t info; | ||
54 | |||
55 | if (kernel_mode(regs)) { | ||
56 | debugger(regs); | ||
57 | die("Exception in kernel mode", regs, signr); | ||
58 | } | ||
59 | info.si_signo = signr; | ||
60 | info.si_errno = 0; | ||
61 | info.si_code = code; | ||
62 | info.si_addr = (void __user *) addr; | ||
63 | force_sig_info(signr, &info, current); | ||
64 | } | ||
65 | |||
66 | asmlinkage void full_exception(struct pt_regs *regs, unsigned int type, | ||
67 | int fsr, int addr) | ||
68 | { | ||
69 | #if 0 | ||
70 | printk(KERN_WARNING "Exception %02x in %s mode, FSR=%08x PC=%08x ESR=%08x\n", | ||
71 | type, user_mode(regs) ? "user" : "kernel", fsr, | ||
72 | (unsigned int) regs->pc, (unsigned int) regs->esr); | ||
73 | #endif | ||
74 | |||
75 | switch (type & 0x1F) { | ||
76 | case MICROBLAZE_ILL_OPCODE_EXCEPTION: | ||
77 | _exception(SIGILL, regs, ILL_ILLOPC, addr); | ||
78 | break; | ||
79 | case MICROBLAZE_IBUS_EXCEPTION: | ||
80 | if (user_mode(regs)) { | ||
81 | printk(KERN_WARNING "Instruction bus error exception in user mode.\n"); | ||
82 | _exception(SIGBUS, regs, BUS_ADRERR, addr); | ||
83 | return; | ||
84 | } | ||
85 | printk(KERN_WARNING "Instruction bus error exception in kernel mode.\n"); | ||
86 | die("bus exception", regs, SIGBUS); | ||
87 | break; | ||
88 | case MICROBLAZE_DBUS_EXCEPTION: | ||
89 | if (user_mode(regs)) { | ||
90 | printk(KERN_WARNING "Data bus error exception in user mode.\n"); | ||
91 | _exception(SIGBUS, regs, BUS_ADRERR, addr); | ||
92 | return; | ||
93 | } | ||
94 | printk(KERN_WARNING "Data bus error exception in kernel mode.\n"); | ||
95 | die("bus exception", regs, SIGBUS); | ||
96 | break; | ||
97 | case MICROBLAZE_DIV_ZERO_EXCEPTION: | ||
98 | printk(KERN_WARNING "Divide by zero exception\n"); | ||
99 | _exception(SIGILL, regs, ILL_ILLOPC, addr); | ||
100 | break; | ||
101 | |||
102 | case MICROBLAZE_FPU_EXCEPTION: | ||
103 | /* IEEE FP exception */ | ||
104 | /* I removed fsr variable and use code var for storing fsr */ | ||
105 | if (fsr & FSR_IO) | ||
106 | fsr = FPE_FLTINV; | ||
107 | else if (fsr & FSR_OF) | ||
108 | fsr = FPE_FLTOVF; | ||
109 | else if (fsr & FSR_UF) | ||
110 | fsr = FPE_FLTUND; | ||
111 | else if (fsr & FSR_DZ) | ||
112 | fsr = FPE_FLTDIV; | ||
113 | else if (fsr & FSR_DO) | ||
114 | fsr = FPE_FLTRES; | ||
115 | _exception(SIGFPE, regs, fsr, addr); | ||
116 | break; | ||
117 | |||
118 | default: | ||
119 | printk(KERN_WARNING "Unexpected exception %02x " | ||
120 | "PC=%08x in %s mode\n", type, (unsigned int) addr, | ||
121 | kernel_mode(regs) ? "kernel" : "user"); | ||
122 | } | ||
123 | return; | ||
124 | } | ||
diff --git a/arch/microblaze/kernel/hw_exception_handler.S b/arch/microblaze/kernel/hw_exception_handler.S new file mode 100644 index 000000000000..cf9486d99838 --- /dev/null +++ b/arch/microblaze/kernel/hw_exception_handler.S | |||
@@ -0,0 +1,458 @@ | |||
1 | /* | ||
2 | * Exception handling for Microblaze | ||
3 | * | ||
4 | * Rewriten interrupt handling | ||
5 | * | ||
6 | * Copyright (C) 2008-2009 Michal Simek <monstr@monstr.eu> | ||
7 | * Copyright (C) 2008-2009 PetaLogix | ||
8 | * | ||
9 | * uClinux customisation (C) 2005 John Williams | ||
10 | * | ||
11 | * MMU code derived from arch/ppc/kernel/head_4xx.S: | ||
12 | * Copyright (C) 1995-1996 Gary Thomas <gdt@linuxppc.org> | ||
13 | * Initial PowerPC version. | ||
14 | * Copyright (C) 1996 Cort Dougan <cort@cs.nmt.edu> | ||
15 | * Rewritten for PReP | ||
16 | * Copyright (C) 1996 Paul Mackerras <paulus@cs.anu.edu.au> | ||
17 | * Low-level exception handers, MMU support, and rewrite. | ||
18 | * Copyright (C) 1997 Dan Malek <dmalek@jlc.net> | ||
19 | * PowerPC 8xx modifications. | ||
20 | * Copyright (C) 1998-1999 TiVo, Inc. | ||
21 | * PowerPC 403GCX modifications. | ||
22 | * Copyright (C) 1999 Grant Erickson <grant@lcse.umn.edu> | ||
23 | * PowerPC 403GCX/405GP modifications. | ||
24 | * Copyright 2000 MontaVista Software Inc. | ||
25 | * PPC405 modifications | ||
26 | * PowerPC 403GCX/405GP modifications. | ||
27 | * Author: MontaVista Software, Inc. | ||
28 | * frank_rowand@mvista.com or source@mvista.com | ||
29 | * debbie_chu@mvista.com | ||
30 | * | ||
31 | * Original code | ||
32 | * Copyright (C) 2004 Xilinx, Inc. | ||
33 | * | ||
34 | * This program is free software; you can redistribute it and/or modify it | ||
35 | * under the terms of the GNU General Public License version 2 as published | ||
36 | * by the Free Software Foundation. | ||
37 | */ | ||
38 | |||
39 | /* | ||
40 | * Here are the handlers which don't require enabling translation | ||
41 | * and calling other kernel code thus we can keep their design very simple | ||
42 | * and do all processing in real mode. All what they need is a valid current | ||
43 | * (that is an issue for the CONFIG_REGISTER_TASK_PTR case) | ||
44 | * This handlers use r3,r4,r5,r6 and optionally r[current] to work therefore | ||
45 | * these registers are saved/restored | ||
46 | * The handlers which require translation are in entry.S --KAA | ||
47 | * | ||
48 | * Microblaze HW Exception Handler | ||
49 | * - Non self-modifying exception handler for the following exception conditions | ||
50 | * - Unalignment | ||
51 | * - Instruction bus error | ||
52 | * - Data bus error | ||
53 | * - Illegal instruction opcode | ||
54 | * - Divide-by-zero | ||
55 | * | ||
56 | * Note we disable interrupts during exception handling, otherwise we will | ||
57 | * possibly get multiple re-entrancy if interrupt handles themselves cause | ||
58 | * exceptions. JW | ||
59 | */ | ||
60 | |||
61 | #include <asm/exceptions.h> | ||
62 | #include <asm/unistd.h> | ||
63 | #include <asm/page.h> | ||
64 | |||
65 | #include <asm/entry.h> | ||
66 | #include <asm/current.h> | ||
67 | #include <linux/linkage.h> | ||
68 | |||
69 | #include <asm/mmu.h> | ||
70 | #include <asm/pgtable.h> | ||
71 | #include <asm/asm-offsets.h> | ||
72 | |||
73 | /* Helpful Macros */ | ||
74 | #define EX_HANDLER_STACK_SIZ (4*19) | ||
75 | #define NUM_TO_REG(num) r ## num | ||
76 | |||
77 | #define LWREG_NOP \ | ||
78 | bri ex_handler_unhandled; \ | ||
79 | nop; | ||
80 | |||
81 | #define SWREG_NOP \ | ||
82 | bri ex_handler_unhandled; \ | ||
83 | nop; | ||
84 | |||
85 | /* FIXME this is weird - for noMMU kernel is not possible to use brid | ||
86 | * instruction which can shorten executed time | ||
87 | */ | ||
88 | |||
89 | /* r3 is the source */ | ||
90 | #define R3_TO_LWREG_V(regnum) \ | ||
91 | swi r3, r1, 4 * regnum; \ | ||
92 | bri ex_handler_done; | ||
93 | |||
94 | /* r3 is the source */ | ||
95 | #define R3_TO_LWREG(regnum) \ | ||
96 | or NUM_TO_REG (regnum), r0, r3; \ | ||
97 | bri ex_handler_done; | ||
98 | |||
99 | /* r3 is the target */ | ||
100 | #define SWREG_TO_R3_V(regnum) \ | ||
101 | lwi r3, r1, 4 * regnum; \ | ||
102 | bri ex_sw_tail; | ||
103 | |||
104 | /* r3 is the target */ | ||
105 | #define SWREG_TO_R3(regnum) \ | ||
106 | or r3, r0, NUM_TO_REG (regnum); \ | ||
107 | bri ex_sw_tail; | ||
108 | |||
109 | .extern other_exception_handler /* Defined in exception.c */ | ||
110 | |||
111 | /* | ||
112 | * hw_exception_handler - Handler for exceptions | ||
113 | * | ||
114 | * Exception handler notes: | ||
115 | * - Handles all exceptions | ||
116 | * - Does not handle unaligned exceptions during load into r17, r1, r0. | ||
117 | * - Does not handle unaligned exceptions during store from r17 (cannot be | ||
118 | * done) and r1 (slows down common case) | ||
119 | * | ||
120 | * Relevant register structures | ||
121 | * | ||
122 | * EAR - |----|----|----|----|----|----|----|----| | ||
123 | * - < ## 32 bit faulting address ## > | ||
124 | * | ||
125 | * ESR - |----|----|----|----|----| - | - |-----|-----| | ||
126 | * - W S REG EXC | ||
127 | * | ||
128 | * | ||
129 | * STACK FRAME STRUCTURE (for NO_MMU) | ||
130 | * --------------------------------- | ||
131 | * | ||
132 | * +-------------+ + 0 | ||
133 | * | MSR | | ||
134 | * +-------------+ + 4 | ||
135 | * | r1 | | ||
136 | * | . | | ||
137 | * | . | | ||
138 | * | . | | ||
139 | * | . | | ||
140 | * | r18 | | ||
141 | * +-------------+ + 76 | ||
142 | * | . | | ||
143 | * | . | | ||
144 | * | ||
145 | * NO_MMU kernel use the same r0_ram pointed space - look to vmlinux.lds.S | ||
146 | * which is used for storing register values - old style was, that value were | ||
147 | * stored in stack but in case of failure you lost information about register. | ||
148 | * Currently you can see register value in memory in specific place. | ||
149 | * In compare to with previous solution the speed should be the same. | ||
150 | * | ||
151 | * MMU exception handler has different handling compare to no MMU kernel. | ||
152 | * Exception handler use jump table for directing of what happen. For MMU kernel | ||
153 | * is this approach better because MMU relate exception are handled by asm code | ||
154 | * in this file. In compare to with MMU expect of unaligned exception | ||
155 | * is everything handled by C code. | ||
156 | */ | ||
157 | |||
158 | /* | ||
159 | * every of these handlers is entered having R3/4/5/6/11/current saved on stack | ||
160 | * and clobbered so care should be taken to restore them if someone is going to | ||
161 | * return from exception | ||
162 | */ | ||
163 | |||
164 | /* wrappers to restore state before coming to entry.S */ | ||
165 | |||
166 | .global _hw_exception_handler | ||
167 | .section .text | ||
168 | .align 4 | ||
169 | .ent _hw_exception_handler | ||
170 | _hw_exception_handler: | ||
171 | addik r1, r1, -(EX_HANDLER_STACK_SIZ); /* Create stack frame */ | ||
172 | swi r3, r1, PT_R3 | ||
173 | swi r4, r1, PT_R4 | ||
174 | swi r5, r1, PT_R5 | ||
175 | swi r6, r1, PT_R6 | ||
176 | |||
177 | mfs r5, rmsr; | ||
178 | nop | ||
179 | swi r5, r1, 0; | ||
180 | mfs r4, rbtr /* Save BTR before jumping to handler */ | ||
181 | nop | ||
182 | mfs r3, resr | ||
183 | nop | ||
184 | |||
185 | andi r5, r3, 0x1000; /* Check ESR[DS] */ | ||
186 | beqi r5, not_in_delay_slot; /* Branch if ESR[DS] not set */ | ||
187 | mfs r17, rbtr; /* ESR[DS] set - return address in BTR */ | ||
188 | nop | ||
189 | not_in_delay_slot: | ||
190 | swi r17, r1, PT_R17 | ||
191 | |||
192 | andi r5, r3, 0x1F; /* Extract ESR[EXC] */ | ||
193 | |||
194 | /* Exceptions enabled here. This will allow nested exceptions */ | ||
195 | mfs r6, rmsr; | ||
196 | nop | ||
197 | swi r6, r1, 0; /* RMSR_OFFSET */ | ||
198 | ori r6, r6, 0x100; /* Turn ON the EE bit */ | ||
199 | andi r6, r6, ~2; /* Disable interrupts */ | ||
200 | mts rmsr, r6; | ||
201 | nop | ||
202 | |||
203 | xori r6, r5, 1; /* 00001 = Unaligned Exception */ | ||
204 | /* Jump to unalignment exception handler */ | ||
205 | beqi r6, handle_unaligned_ex; | ||
206 | |||
207 | handle_other_ex: /* Handle Other exceptions here */ | ||
208 | /* Save other volatiles before we make procedure calls below */ | ||
209 | swi r7, r1, PT_R7 | ||
210 | swi r8, r1, PT_R8 | ||
211 | swi r9, r1, PT_R9 | ||
212 | swi r10, r1, PT_R10 | ||
213 | swi r11, r1, PT_R11 | ||
214 | swi r12, r1, PT_R12 | ||
215 | swi r14, r1, PT_R14 | ||
216 | swi r15, r1, PT_R15 | ||
217 | swi r18, r1, PT_R18 | ||
218 | |||
219 | or r5, r1, r0 | ||
220 | andi r6, r3, 0x1F; /* Load ESR[EC] */ | ||
221 | lwi r7, r0, PER_CPU(KM) /* MS: saving current kernel mode to regs */ | ||
222 | swi r7, r1, PT_MODE | ||
223 | mfs r7, rfsr | ||
224 | nop | ||
225 | addk r8, r17, r0; /* Load exception address */ | ||
226 | bralid r15, full_exception; /* Branch to the handler */ | ||
227 | nop; | ||
228 | |||
229 | /* | ||
230 | * Trigger execution of the signal handler by enabling | ||
231 | * interrupts and calling an invalid syscall. | ||
232 | */ | ||
233 | mfs r5, rmsr; | ||
234 | nop | ||
235 | ori r5, r5, 2; | ||
236 | mts rmsr, r5; /* enable interrupt */ | ||
237 | nop | ||
238 | addi r12, r0, __NR_syscalls; | ||
239 | brki r14, 0x08; | ||
240 | mfs r5, rmsr; /* disable interrupt */ | ||
241 | nop | ||
242 | andi r5, r5, ~2; | ||
243 | mts rmsr, r5; | ||
244 | nop | ||
245 | |||
246 | lwi r7, r1, PT_R7 | ||
247 | lwi r8, r1, PT_R8 | ||
248 | lwi r9, r1, PT_R9 | ||
249 | lwi r10, r1, PT_R10 | ||
250 | lwi r11, r1, PT_R11 | ||
251 | lwi r12, r1, PT_R12 | ||
252 | lwi r14, r1, PT_R14 | ||
253 | lwi r15, r1, PT_R15 | ||
254 | lwi r18, r1, PT_R18 | ||
255 | |||
256 | bri ex_handler_done; /* Complete exception handling */ | ||
257 | |||
258 | /* 0x01 - Unaligned data access exception | ||
259 | * This occurs when a word access is not aligned on a word boundary, | ||
260 | * or when a 16-bit access is not aligned on a 16-bit boundary. | ||
261 | * This handler perform the access, and returns, except for MMU when | ||
262 | * the unaligned address is last on a 4k page or the physical address is | ||
263 | * not found in the page table, in which case unaligned_data_trap is called. | ||
264 | */ | ||
265 | handle_unaligned_ex: | ||
266 | /* Working registers already saved: R3, R4, R5, R6 | ||
267 | * R3 = ESR | ||
268 | * R4 = BTR | ||
269 | */ | ||
270 | mfs r4, rear; | ||
271 | nop | ||
272 | |||
273 | andi r6, r3, 0x3E0; /* Mask and extract the register operand */ | ||
274 | srl r6, r6; /* r6 >> 5 */ | ||
275 | srl r6, r6; | ||
276 | srl r6, r6; | ||
277 | srl r6, r6; | ||
278 | srl r6, r6; | ||
279 | /* Store the register operand in a temporary location */ | ||
280 | sbi r6, r0, TOPHYS(ex_reg_op); | ||
281 | |||
282 | andi r6, r3, 0x400; /* Extract ESR[S] */ | ||
283 | bnei r6, ex_sw; | ||
284 | ex_lw: | ||
285 | andi r6, r3, 0x800; /* Extract ESR[W] */ | ||
286 | beqi r6, ex_lhw; | ||
287 | lbui r5, r4, 0; /* Exception address in r4 */ | ||
288 | /* Load a word, byte-by-byte from destination address | ||
289 | and save it in tmp space */ | ||
290 | sbi r5, r0, TOPHYS(ex_tmp_data_loc_0); | ||
291 | lbui r5, r4, 1; | ||
292 | sbi r5, r0, TOPHYS(ex_tmp_data_loc_1); | ||
293 | lbui r5, r4, 2; | ||
294 | sbi r5, r0, TOPHYS(ex_tmp_data_loc_2); | ||
295 | lbui r5, r4, 3; | ||
296 | sbi r5, r0, TOPHYS(ex_tmp_data_loc_3); | ||
297 | /* Get the destination register value into r3 */ | ||
298 | lwi r3, r0, TOPHYS(ex_tmp_data_loc_0); | ||
299 | bri ex_lw_tail; | ||
300 | ex_lhw: | ||
301 | lbui r5, r4, 0; /* Exception address in r4 */ | ||
302 | /* Load a half-word, byte-by-byte from destination | ||
303 | address and save it in tmp space */ | ||
304 | sbi r5, r0, TOPHYS(ex_tmp_data_loc_0); | ||
305 | lbui r5, r4, 1; | ||
306 | sbi r5, r0, TOPHYS(ex_tmp_data_loc_1); | ||
307 | /* Get the destination register value into r3 */ | ||
308 | lhui r3, r0, TOPHYS(ex_tmp_data_loc_0); | ||
309 | ex_lw_tail: | ||
310 | /* Get the destination register number into r5 */ | ||
311 | lbui r5, r0, TOPHYS(ex_reg_op); | ||
312 | /* Form load_word jump table offset (lw_table + (8 * regnum)) */ | ||
313 | la r6, r0, TOPHYS(lw_table); | ||
314 | addk r5, r5, r5; | ||
315 | addk r5, r5, r5; | ||
316 | addk r5, r5, r5; | ||
317 | addk r5, r5, r6; | ||
318 | bra r5; | ||
319 | ex_lw_end: /* Exception handling of load word, ends */ | ||
320 | ex_sw: | ||
321 | /* Get the destination register number into r5 */ | ||
322 | lbui r5, r0, TOPHYS(ex_reg_op); | ||
323 | /* Form store_word jump table offset (sw_table + (8 * regnum)) */ | ||
324 | la r6, r0, TOPHYS(sw_table); | ||
325 | add r5, r5, r5; | ||
326 | add r5, r5, r5; | ||
327 | add r5, r5, r5; | ||
328 | add r5, r5, r6; | ||
329 | bra r5; | ||
330 | ex_sw_tail: | ||
331 | mfs r6, resr; | ||
332 | nop | ||
333 | andi r6, r6, 0x800; /* Extract ESR[W] */ | ||
334 | beqi r6, ex_shw; | ||
335 | /* Get the word - delay slot */ | ||
336 | swi r3, r0, TOPHYS(ex_tmp_data_loc_0); | ||
337 | /* Store the word, byte-by-byte into destination address */ | ||
338 | lbui r3, r0, TOPHYS(ex_tmp_data_loc_0); | ||
339 | sbi r3, r4, 0; | ||
340 | lbui r3, r0, TOPHYS(ex_tmp_data_loc_1); | ||
341 | sbi r3, r4, 1; | ||
342 | lbui r3, r0, TOPHYS(ex_tmp_data_loc_2); | ||
343 | sbi r3, r4, 2; | ||
344 | lbui r3, r0, TOPHYS(ex_tmp_data_loc_3); | ||
345 | sbi r3, r4, 3; | ||
346 | bri ex_handler_done; | ||
347 | |||
348 | ex_shw: | ||
349 | /* Store the lower half-word, byte-by-byte into destination address */ | ||
350 | swi r3, r0, TOPHYS(ex_tmp_data_loc_0); | ||
351 | lbui r3, r0, TOPHYS(ex_tmp_data_loc_2); | ||
352 | sbi r3, r4, 0; | ||
353 | lbui r3, r0, TOPHYS(ex_tmp_data_loc_3); | ||
354 | sbi r3, r4, 1; | ||
355 | ex_sw_end: /* Exception handling of store word, ends. */ | ||
356 | |||
357 | ex_handler_done: | ||
358 | lwi r5, r1, 0 /* RMSR */ | ||
359 | mts rmsr, r5 | ||
360 | nop | ||
361 | lwi r3, r1, PT_R3 | ||
362 | lwi r4, r1, PT_R4 | ||
363 | lwi r5, r1, PT_R5 | ||
364 | lwi r6, r1, PT_R6 | ||
365 | lwi r17, r1, PT_R17 | ||
366 | |||
367 | rted r17, 0 | ||
368 | addik r1, r1, (EX_HANDLER_STACK_SIZ); /* Restore stack frame */ | ||
369 | |||
370 | .end _hw_exception_handler | ||
371 | |||
372 | ex_handler_unhandled: | ||
373 | /* FIXME add handle function for unhandled exception - dump register */ | ||
374 | bri 0 | ||
375 | |||
376 | .section .text | ||
377 | .align 4 | ||
378 | lw_table: | ||
379 | lw_r0: R3_TO_LWREG (0); | ||
380 | lw_r1: LWREG_NOP; | ||
381 | lw_r2: R3_TO_LWREG (2); | ||
382 | lw_r3: R3_TO_LWREG_V (3); | ||
383 | lw_r4: R3_TO_LWREG_V (4); | ||
384 | lw_r5: R3_TO_LWREG_V (5); | ||
385 | lw_r6: R3_TO_LWREG_V (6); | ||
386 | lw_r7: R3_TO_LWREG (7); | ||
387 | lw_r8: R3_TO_LWREG (8); | ||
388 | lw_r9: R3_TO_LWREG (9); | ||
389 | lw_r10: R3_TO_LWREG (10); | ||
390 | lw_r11: R3_TO_LWREG (11); | ||
391 | lw_r12: R3_TO_LWREG (12); | ||
392 | lw_r13: R3_TO_LWREG (13); | ||
393 | lw_r14: R3_TO_LWREG (14); | ||
394 | lw_r15: R3_TO_LWREG (15); | ||
395 | lw_r16: R3_TO_LWREG (16); | ||
396 | lw_r17: LWREG_NOP; | ||
397 | lw_r18: R3_TO_LWREG (18); | ||
398 | lw_r19: R3_TO_LWREG (19); | ||
399 | lw_r20: R3_TO_LWREG (20); | ||
400 | lw_r21: R3_TO_LWREG (21); | ||
401 | lw_r22: R3_TO_LWREG (22); | ||
402 | lw_r23: R3_TO_LWREG (23); | ||
403 | lw_r24: R3_TO_LWREG (24); | ||
404 | lw_r25: R3_TO_LWREG (25); | ||
405 | lw_r26: R3_TO_LWREG (26); | ||
406 | lw_r27: R3_TO_LWREG (27); | ||
407 | lw_r28: R3_TO_LWREG (28); | ||
408 | lw_r29: R3_TO_LWREG (29); | ||
409 | lw_r30: R3_TO_LWREG (30); | ||
410 | lw_r31: R3_TO_LWREG (31); | ||
411 | |||
412 | sw_table: | ||
413 | sw_r0: SWREG_TO_R3 (0); | ||
414 | sw_r1: SWREG_NOP; | ||
415 | sw_r2: SWREG_TO_R3 (2); | ||
416 | sw_r3: SWREG_TO_R3_V (3); | ||
417 | sw_r4: SWREG_TO_R3_V (4); | ||
418 | sw_r5: SWREG_TO_R3_V (5); | ||
419 | sw_r6: SWREG_TO_R3_V (6); | ||
420 | sw_r7: SWREG_TO_R3 (7); | ||
421 | sw_r8: SWREG_TO_R3 (8); | ||
422 | sw_r9: SWREG_TO_R3 (9); | ||
423 | sw_r10: SWREG_TO_R3 (10); | ||
424 | sw_r11: SWREG_TO_R3 (11); | ||
425 | sw_r12: SWREG_TO_R3 (12); | ||
426 | sw_r13: SWREG_TO_R3 (13); | ||
427 | sw_r14: SWREG_TO_R3 (14); | ||
428 | sw_r15: SWREG_TO_R3 (15); | ||
429 | sw_r16: SWREG_TO_R3 (16); | ||
430 | sw_r17: SWREG_NOP; | ||
431 | sw_r18: SWREG_TO_R3 (18); | ||
432 | sw_r19: SWREG_TO_R3 (19); | ||
433 | sw_r20: SWREG_TO_R3 (20); | ||
434 | sw_r21: SWREG_TO_R3 (21); | ||
435 | sw_r22: SWREG_TO_R3 (22); | ||
436 | sw_r23: SWREG_TO_R3 (23); | ||
437 | sw_r24: SWREG_TO_R3 (24); | ||
438 | sw_r25: SWREG_TO_R3 (25); | ||
439 | sw_r26: SWREG_TO_R3 (26); | ||
440 | sw_r27: SWREG_TO_R3 (27); | ||
441 | sw_r28: SWREG_TO_R3 (28); | ||
442 | sw_r29: SWREG_TO_R3 (29); | ||
443 | sw_r30: SWREG_TO_R3 (30); | ||
444 | sw_r31: SWREG_TO_R3 (31); | ||
445 | |||
446 | /* Temporary data structures used in the handler */ | ||
447 | .section .data | ||
448 | .align 4 | ||
449 | ex_tmp_data_loc_0: | ||
450 | .byte 0 | ||
451 | ex_tmp_data_loc_1: | ||
452 | .byte 0 | ||
453 | ex_tmp_data_loc_2: | ||
454 | .byte 0 | ||
455 | ex_tmp_data_loc_3: | ||
456 | .byte 0 | ||
457 | ex_reg_op: | ||
458 | .byte 0 | ||