?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Reenabled some debug output.
[AROS.git] / arch / ppc-sam440 / kernel / intr.c
blobf9dcf15a703b9f6ca9488395addb33894503ff6a
1 #define DEBUG 1
2 #include <aros/kernel.h>
3 #include <aros/libcall.h>
4 #include <hardware/intbits.h>
5 #include <asm/amcc440.h>
6 #include <stddef.h>
7
8
9 #include "kernel_base.h"
10 #include "kernel_intern.h"
11 #include "kernel_syscall.h"
12 #include "kernel_globals.h"
13 #include "kernel_interrupts.h"
14 #include "kernel_intr.h"
15
16 void *__cEXCEPTION_0_Prolog();
17 void *__mcEXCEPTION_1_Prolog();
18 void *__EXCEPTION_2_Prolog();
19 void *__EXCEPTION_3_Prolog();
20 void *__EXCEPTION_4_Prolog();
21 void *__EXCEPTION_5_Prolog();
22 void *__EXCEPTION_6_Prolog();
23 void *__EXCEPTION_7_Prolog();
24 void *__EXCEPTION_8_Prolog();
25 void *__EXCEPTION_9_Prolog();
26 void *__EXCEPTION_10_Prolog();
27 void *__EXCEPTION_11_Prolog();
28 void *__cEXCEPTION_12_Prolog();
29 void *__EXCEPTION_13_Prolog();
30 void *__EXCEPTION_14_Prolog();
31 void *__cEXCEPTION_15_Prolog();
32
33 void intr_init()
34 {
35 D(bug("[KRN] Setting up exception handlers\n"));
36 wrspr(IVPR, ((uint32_t)&__cEXCEPTION_0_Prolog) & 0xffff0000);
37
38 wrspr(IVOR0, ((uint32_t)&__cEXCEPTION_0_Prolog) & 0x0000fff0);
39 wrspr(IVOR1, ((uint32_t)&__mcEXCEPTION_1_Prolog) & 0x0000fff0);
40 wrspr(IVOR2, ((uint32_t)&__EXCEPTION_2_Prolog) & 0x0000fff0);
41 wrspr(IVOR3, ((uint32_t)&__EXCEPTION_3_Prolog) & 0x0000fff0);
42 wrspr(IVOR4, ((uint32_t)&__EXCEPTION_4_Prolog) & 0x0000fff0);
43 wrspr(IVOR5, ((uint32_t)&__EXCEPTION_5_Prolog) & 0x0000fff0);
44 wrspr(IVOR6, ((uint32_t)&__EXCEPTION_6_Prolog) & 0x0000fff0);
45 wrspr(IVOR7, ((uint32_t)&__EXCEPTION_7_Prolog) & 0x0000fff0);
46 wrspr(IVOR8, ((uint32_t)&__EXCEPTION_8_Prolog) & 0x0000fff0);
47 wrspr(IVOR9, ((uint32_t)&__EXCEPTION_9_Prolog) & 0x0000fff0);
48 wrspr(IVOR10, ((uint32_t)&__EXCEPTION_10_Prolog) & 0x0000fff0);
49 wrspr(IVOR11, ((uint32_t)&__EXCEPTION_11_Prolog) & 0x0000fff0);
50 wrspr(IVOR12, ((uint32_t)&__cEXCEPTION_12_Prolog) & 0x0000fff0);
51 wrspr(IVOR13, ((uint32_t)&__EXCEPTION_13_Prolog) & 0x0000fff0);
52 wrspr(IVOR14, ((uint32_t)&__EXCEPTION_14_Prolog) & 0x0000fff0);
53 wrspr(IVOR15, ((uint32_t)&__cEXCEPTION_15_Prolog) & 0x0000fff0);
54
55 uic_init();
56 }
57
58 #define EXCEPTION_STACK_SIZE 8192
59 /* Exception Stack
60 * Principle of operation:
61 *
62 * If an exception occurs from MSR_PR context (user space), switch
63 * the stack to the exception stack.
64 *
65 * If an exception occurs from ~MSR_PR context (supervisor), use
66 * the existing stack (whether exception or original supervisor).
67 */
68 ULONG __attribute__((aligned(16))) exception_stack[EXCEPTION_STACK_SIZE / sizeof(ULONG)];
69
70 exception_handler * const exception_handlers[16] = {
71 generic_handler, /* 0 - Critical Input CE */
72 generic_handler, /* 1 - Machine Check ME */
73 generic_handler, /* 2 - Data Storage -- */
74 generic_handler, /* 3 - Instr. Storage -- */
75 uic_handler, /* 4 - External Input EE */
76 alignment_handler, /* 5 - Alignment -- */
77 generic_handler, /* 6 - Program -- */
78 generic_handler, /* 7 - FP Unavailable -- */
79 syscall_handler, /* 8 - System Call -- */
80 generic_handler, /* 9 - AP Unavailable -- */
81 decrementer_handler, /* 10 - Decrementer EE */
82 generic_handler, /* 11 - Fixed Interval EE */
83 generic_handler, /* 12 - Watchdog CE */
84 mmu_handler, /* 13 - Data TLB -- */
85 mmu_handler, /* 14 - Inst TLB -- */
86 generic_handler, /* 15 - Debug DE */
87 };
88
89 #if DEBUG
90
91 #include <proto/debug.h>
92 #include <libraries/debug.h>
93
94 static CONST_STRPTR symbolfor(struct Library *DebugBase, IPTR addr)
95 {
96 STRPTR modname = "(unknown)";
97 STRPTR symname = "(unknown)";
98 IPTR offset = 0;
99 static TEXT buff[70];
100 struct TagItem tags[] = {
101 { DL_ModuleName, (IPTR)&modname },
102 { DL_SymbolName, (IPTR)&symname },
103 { DL_SymbolStart, (IPTR)&offset },
104 { TAG_END }
105 };
106 if (DebugBase) {
107 DecodeLocationA((APTR)addr, tags);
108 snprintf(buff, sizeof(buff), "%s %s+0x%x", modname, symname, (unsigned)(addr - offset));
109 buff[sizeof(buff)-1]=0;
110 } else {
111 buff[0] = 0;
112 }
113
114 return buff;
115 }
116 #else
117 static inline CONST_STRPTR symbolfor(struct Library *DebugBase, IPTR addr) { return ""; }
118 #endif
119
120
121 void dumpregs(context_t *ctx, uint8_t exception)
122 {
123 uint32_t *sp;
124 ULONG *p;
125 int i;
126 struct Library *DebugBase = (APTR)FindName(&SysBase->LibList, "debug.library");
127
128 bug("[KRN] Exception %d handler. Context @ %p, SysBase @ %p, KernelBase @ %p\n", exception, ctx, SysBase, KernelBase);
129 bug("[KRN] SRR0=%08x, SRR1=%08x DEAR=%08x ESR=%08x\n",ctx->cpu.srr0, ctx->cpu.srr1, rdspr(DEAR), rdspr(ESR));
130 bug("[KRN] CTR=%08x LR=%08x XER=%08x CCR=%08x\n", ctx->cpu.ctr, ctx->cpu.lr, ctx->cpu.xer, ctx->cpu.ccr);
131 bug("[KRN] DAR=%08x DSISR=%08x\n", ctx->cpu.dar, ctx->cpu.dsisr);
132 if (exception == 1)
133 {
134 bug("[KRN] MCSR=%08x\n", rdspr(MCSR));
135 }
136
137 for (i = 0; i < 32; i++) {
138 if ((i & 3) == 0)
139 bug("[KRN]");
140 bug(" GPR%02d=%08x", i, ctx->cpu.gpr[i]);
141 if ((i & 3) == 3)
142 bug("\n");
143 }
144
145 bug("[KRN] Instruction dump:");
146 p = (ULONG*)ctx->cpu.srr0;
147 for (i=0; i < 8; i++)
148 {
149 if ((i % 4) == 0)
150 bug("\n[KRN] %08x:", &p[i]);
151 bug(" %08x", p[i]);
152 }
153
154 bug("\n[KRN] Stackdump:");
155 sp = (uint32_t *)ctx->cpu.gpr[1];
156 for (i = 0; i < 64; i++) {
157 if ((i % 4) == 0)
158 bug("\n[KRN] %08x:", &sp[i]);
159 bug(" %08x", sp[i]);
160 }
161
162 bug("\n[KRN] Backtrace: %s\n", symbolfor(DebugBase, ctx->cpu.srr0));
163 bug("[KRN] LR=%08x %s\n", ctx->cpu.lr, symbolfor(DebugBase, ctx->cpu.lr));
164 if (exception != 13) {
165 sp = (uint32_t *)ctx->cpu.gpr[1];
166 while(*sp)
167 {
168 sp = (uint32_t *)sp[0];
169 bug("[KRN] %08x %s\n", sp[1], symbolfor(DebugBase, sp[1]));
170 }
171 }
172 CloseLibrary(DebugBase);
173
174 Debug(0);
175 }
176
177 void handle_exception(context_t *ctx, uint8_t exception)
178 {
179 const ULONG marker = 0x00dead00 | (exception << 24) | exception;
180
181 if (SysBase) {
182 struct Task *task = SysBase->ThisTask;
183 if (task && (ctx->cpu.srr1 & MSR_PR) && ((APTR)ctx->cpu.gpr[1] <= task->tc_SPLower ||
184 (APTR)ctx->cpu.gpr[1] > task->tc_SPUpper)) {
185 bug("[KRN]: When did my stack base go from %p to %p?\n",
186 task->tc_SPReg, (APTR)ctx->cpu.gpr[1]);
187 dumpregs(ctx, exception);
188 for (;;);
189 }
190 }
191
192 exception_stack[exception] = marker;
193 exception_handlers[exception](ctx, exception);
194 if ( exception_stack[exception] != marker) {
195 bug("[KRN]: Stack overflow processing exception %d\n", exception);
196 dumpregs(ctx, exception);
197 for (;;);
198 }
199 DB2(bug("[KRN]: Exception %d: Done\n", exception));
200
201 /* Always disable MSR_POW when exiting */
202 ctx->cpu.srr1 &= ~MSR_POW;
203 }
204
205 #define _STR(x) #x
206 #define STR(x) _STR(x)
207 #define PUT_INTR_TEMPLATE(num, type) \
208 asm volatile(".section .text,\"ax\"\n\t.align 5\n\t.globl __" #type "EXCEPTION_" STR(num) "_Prolog\n\t.type __" #type "EXCEPTION_" STR(num) "_Prolog,@function\n" \
209 "__" #type "EXCEPTION_" STR(num) "_Prolog: \n\t" \
210 "mtsprg1 %%r3 \n\t" /* SPRG1 = %r3 */ \
211 "mfcr %%r3 \n\t" \
212 "mtsprg2 %%r3 \n\t" /* SPRG2 = %ccr */ \
213 "mfsrr1 %%r3 \n\t" \
214 "andi. %%r3, %%r3, %[msr_pr] \n\t" \
215 "beq 1f \n\t" \
216 "lis %%r3, exception_stack+%[exc]@ha \n\t" \
217 "la %%r3, exception_stack+%[exc]@l(%%r3) \n\t" \
218 "b 2f \n\t" \
219 "1: \n\t" \
220 "addi %%r3, %%r1, -%[ctx] \n\t" \
221 "2: \n\t" \
222 "stw %%r0, %[gpr0](%%r3) \n\t" \
223 "stw %%r1, %[gpr1](%%r3) \n\t" \
224 "stw %%r2, %[gpr2](%%r3) \n\t" \
225 "mfsprg2 %%r0 \n\t" \
226 "stw %%r0, %[ccr](%%r3) \n\t" \
227 "mfsprg1 %%r0 \n\t" \
228 "stw %%r0, %[gpr3](%%r3) \n\t" \
229 "mr %%r1, %%r3 \n\t" \
230 :: \
231 [gpr0]"i"(offsetof(context_t, cpu.gpr[0])), \
232 [gpr1]"i"(offsetof(context_t, cpu.gpr[1])), \
233 [gpr2]"i"(offsetof(context_t, cpu.gpr[2])), \
234 [gpr3]"i"(offsetof(context_t, cpu.gpr[3])), \
235 [ccr]"i"(offsetof(context_t, cpu.ccr)), \
236 [ctx]"i"(sizeof(context_t)), \
237 [exc]"i"(sizeof(exception_stack) - sizeof(context_t)), \
238 [msr_pr]"i"(MSR_PR)); \
239 asm volatile (\
240 "mf" #type "srr0 %%r0 \n\t" \
241 "stw %%r0,%[srr0](%%r3) \n\t" \
242 "mf" #type "srr1 %%r0 \n\t" \
243 "stw %%r0,%[srr1](%%r3) \n\t" \
244 "mfctr %%r0 \n\t" \
245 "stw %%r0,%[ctr](%%r3) \n\t" \
246 "mflr %%r0 \n\t" \
247 "stw %%r0,%[lr](%%r3) \n\t" \
248 "mfxer %%r0 \n\t" \
249 "stw %%r0,%[xer](%%r3) \n\t" \
250 "stw %%r4, %[gpr4](%%r3) \n\t" \
251 "stw %%r5, %[gpr5](%%r3) \n\t" \
252 "li %%r4, %[irq] \n\t" \
253 "bl __EXCEPTION_Trampoline\n\t" \
254 "lwz %%r5, %[gpr5](%%r3) \n\t" \
255 "lwz %%r4, %[gpr4](%%r3) \n\t" \
256 "addi %%r0, %%r3, -4 \n\t" /* Dummy write */ \
257 "stwcx. %%r0, 0, %%r0 \n\t" /* to clear resv. */ \
258 "lwz %%r0, %[xer](%%r3) \n\t" \
259 "mtxer %%r0 \n\t" \
260 "lwz %%r0, %[lr](%%r3) \n\t" \
261 "mtlr %%r0 \n\t" \
262 "lwz %%r0, %[ctr](%%r3) \n\t" \
263 "mtctr %%r0 \n\t" \
264 "lwz %%r0,%[srr1](%%r3) \n\t" \
265 "mt" #type "srr1 %%r0 \n\t" \
266 "lwz %%r0,%[srr0](%%r3) \n\t" \
267 "mt" #type "srr0 %%r0 \n\t" \
268 "lwz %%r0, %[ccr](%%r3) \n\t" \
269 "mtcr %%r0 \n\t" \
270 "lwz %%r0, %[gpr0](%%r3) \n\t" \
271 "lwz %%r1, %[gpr1](%%r3) \n\t" \
272 "lwz %%r2, %[gpr2](%%r3) \n\t" \
273 "lwz %%r3, %[gpr3](%%r3) \n\t" \
274 "sync; isync; rf" #type "i \n\t" \
275 \
276 :: \
277 [gpr0]"i"(offsetof(context_t, cpu.gpr[0])), \
278 [gpr1]"i"(offsetof(context_t, cpu.gpr[1])), \
279 [gpr2]"i"(offsetof(context_t, cpu.gpr[2])), \
280 [gpr3]"i"(offsetof(context_t, cpu.gpr[3])), \
281 [gpr4]"i"(offsetof(context_t, cpu.gpr[4])), \
282 [gpr5]"i"(offsetof(context_t, cpu.gpr[5])), \
283 [ccr]"i"(offsetof(context_t, cpu.ccr)), \
284 [srr0]"i"(offsetof(context_t, cpu.srr0)), \
285 [srr1]"i"(offsetof(context_t, cpu.srr1)), \
286 [ctr]"i"(offsetof(context_t, cpu.ctr)), \
287 [lr]"i"(offsetof(context_t, cpu.lr)), \
288 [xer]"i"(offsetof(context_t, cpu.xer)), \
289 [irq]"i"(num) \
290 ); \
291
292 uint64_t idle_time;
293 static uint64_t last_calc;
294
295 void decrementer_handler(context_t *ctx, uint8_t exception)
296 {
297 /* Clear the DIS bit - we have received decrementer exception */
298 wrspr(TSR, TSR_DIS);
299 DB2(bug("[KRN] Decrementer handler. Context @ %p. srr1=%08x\n", ctx, ctx->cpu.srr1));
300
301 if (!KernelBase)
302 return;
303
304 /* Idle time calculator */
305
306 uint64_t current = mftbu();
307 if (current - last_calc > KernelBase->kb_PlatformData->pd_OPBFreq)
308 {
309 uint32_t total_time = current - last_calc;
310
311 if (total_time < idle_time)
312 total_time = idle_time;
313
314 KernelBase->kb_PlatformData->pd_CPUUsage = 1000 - ((uint32_t)idle_time) / (total_time / 1000);
315
316 if (KernelBase->kb_PlatformData->pd_CPUUsage > 999)
317 {
318 DB2(bug("[KRN] CPU usage: %3d.%d (%s)\n", KernelBase->kb_PlatformData->pd_CPUUsage / 10, KernelBase->kb_PlatformData->pd_CPUUsage % 10,
319 SysBase->ThisTask->tc_Node.ln_Name));
320 }
321 else
322 DB2(bug("[KRN] CPU usage: %3d.%d\n", KernelBase->kb_PlatformData->pd_CPUUsage / 10, KernelBase->kb_PlatformData->pd_CPUUsage % 10));
323
324 idle_time = 0;
325 last_calc = current;
326 }
327
328 /* Signal the Exec VBlankServer */
329 if (SysBase && (SysBase->IDNestCnt < 0)) {
330 core_Cause(INTB_VERTB, 1L << INTB_VERTB);
331 }
332
333 ExitInterrupt(ctx);
334 }
335
336 void generic_handler(context_t *ctx, uint8_t exception)
337 {
338 struct KernelBase *KernelBase = getKernelBase();
339
340 DB2(bug("[KRN] Generic handler. Context @ %p. srr1=%08x\n", ctx, ctx->cpu.srr1));
341
342 if (!krnRunExceptionHandlers(KernelBase, exception, ctx))
343 {
344 D(dumpregs(ctx, exception));
345 D(bug("[KRN] **UNHANDLED EXCEPTION** stopping here...\n"));
346
347 while(1) {
348 wrmsr(rdmsr() | MSR_POW);
349 }
350 }
351
352 ExitInterrupt(ctx);
353 }
354
355 void mmu_handler(context_t *ctx, uint8_t exception)
356 {
357 if (!!krnRunExceptionHandlers(KernelBase, exception, ctx))
358 {
359 /* Any unhandled MMU activity is fatal for now. */
360 dumpregs(ctx, exception);
361 }
362 }
363
364 double lfd(intptr_t addr)
365 {
366 union {
367 uint8_t u8[8];
368 uint16_t u16[4];
369 uint32_t u32[2];
370 uint64_t u64;
371 float f[2];
372 double d;
373 } conv;
374
375 switch ((intptr_t)addr & 3)
376 {
377 case 0:
378 conv.u32[0] = ((uint32_t *)addr)[0];
379 conv.u32[1] = ((uint32_t *)addr)[1];
380 break;
381
382 case 2:
383 conv.u16[0] = ((uint16_t *)addr)[0];
384 conv.u16[1] = ((uint16_t *)addr)[1];
385 conv.u16[2] = ((uint16_t *)addr)[2];
386 conv.u16[3] = ((uint16_t *)addr)[3];
387 break;
388
389 default:
390 conv.u8[0] = ((uint8_t *)addr)[0];
391 conv.u8[1] = ((uint8_t *)addr)[1];
392 conv.u8[2] = ((uint8_t *)addr)[2];
393 conv.u8[3] = ((uint8_t *)addr)[3];
394 conv.u8[4] = ((uint8_t *)addr)[4];
395 conv.u8[5] = ((uint8_t *)addr)[5];
396 conv.u8[6] = ((uint8_t *)addr)[6];
397 conv.u8[7] = ((uint8_t *)addr)[7];
398 break;
399 }
400
401 return conv.d;
402 }
403
404 float lfs(intptr_t addr)
405 {
406 union {
407 uint8_t u8[8];
408 uint16_t u16[4];
409 uint32_t u32[2];
410 uint64_t u64;
411 float f[2];
412 double d;
413 } conv;
414
415 switch ((intptr_t)addr & 3)
416 {
417 case 0:
418 conv.u32[0] = ((uint32_t *)addr)[0];
419 break;
420
421 case 2:
422 conv.u16[0] = ((uint16_t *)addr)[0];
423 conv.u16[1] = ((uint16_t *)addr)[1];
424 break;
425
426 default:
427 conv.u8[0] = ((uint8_t *)addr)[0];
428 conv.u8[1] = ((uint8_t *)addr)[1];
429 conv.u8[2] = ((uint8_t *)addr)[2];
430 conv.u8[3] = ((uint8_t *)addr)[3];
431 break;
432 }
433
434 return conv.f[0];
435 }
436
437 void stfd(double v, intptr_t addr)
438 {
439 union {
440 uint8_t u8[8];
441 uint16_t u16[4];
442 uint32_t u32[2];
443 uint64_t u64;
444 float f[2];
445 double d;
446 } conv;
447
448 conv.d = v;
449
450 switch ((intptr_t)addr & 3)
451 {
452 case 0:
453 ((uint32_t *)addr)[0] = conv.u32[0];
454 ((uint32_t *)addr)[1] = conv.u32[1];
455 break;
456
457 case 2:
458 ((uint16_t *)addr)[0] = conv.u16[0];
459 ((uint16_t *)addr)[1] = conv.u16[1];
460 ((uint16_t *)addr)[2] = conv.u16[2];
461 ((uint16_t *)addr)[3] = conv.u16[3];
462 break;
463
464 default:
465 ((uint8_t *)addr)[0] = conv.u8[0];
466 ((uint8_t *)addr)[1] = conv.u8[1];
467 ((uint8_t *)addr)[2] = conv.u8[2];
468 ((uint8_t *)addr)[3] = conv.u8[3];
469 ((uint8_t *)addr)[4] = conv.u8[4];
470 ((uint8_t *)addr)[5] = conv.u8[5];
471 ((uint8_t *)addr)[6] = conv.u8[6];
472 ((uint8_t *)addr)[7] = conv.u8[7];
473 break;
474 }
475 }
476
477 void stfs(float v, intptr_t addr)
478 {
479 union {
480 uint8_t u8[8];
481 uint16_t u16[4];
482 uint32_t u32[2];
483 uint64_t u64;
484 float f[2];
485 double d;
486 } conv;
487
488 conv.f[0] = v;
489
490 switch ((intptr_t)addr & 3)
491 {
492 case 0:
493 ((uint32_t *)addr)[0] = conv.u32[0];
494 break;
495
496 case 2:
497 ((uint16_t *)addr)[0] = conv.u16[0];
498 ((uint16_t *)addr)[1] = conv.u16[1];
499 break;
500
501 default:
502 ((uint8_t *)addr)[0] = conv.u8[0];
503 ((uint8_t *)addr)[1] = conv.u8[1];
504 ((uint8_t *)addr)[2] = conv.u8[2];
505 ((uint8_t *)addr)[3] = conv.u8[3];
506 break;
507 }
508 }
509
510 void alignment_handler(context_t *ctx, uint8_t exception)
511 {
512 int fixed = 1;
513
514 intptr_t dear = rdspr(DEAR);
515 uint32_t insn = *(uint32_t *)ctx->cpu.srr0;
516
517 uint8_t reg = (insn >> 21) & 0x1f; // source/dest register
518 uint8_t areg = (insn >> 16) & 0x1f; // register to be updated with dear value
519
520 D(bug("[KRN] Alignment handler. Context @ %p. srr1=%08x\n", ctx, ctx->cpu.srr1));
521
522
523 switch (insn >> 26)
524 {
525 case 50: // lfd
526 ctx->fpu.fpr[reg] = lfd(dear);
527 break;
528 case 51: // lfdu
529 ctx->fpu.fpr[reg] = lfd(dear);
530 ctx->cpu.gpr[areg] = dear;
531 break;
532 case 48: // lfs
533 ctx->fpu.fpr[reg] = lfs(dear);
534 break;
535 case 49: // lfsu
536 ctx->fpu.fpr[reg] = lfs(dear);
537 ctx->cpu.gpr[areg] = dear;
538 break;
539 case 54: // stfd
540 stfd(ctx->fpu.fpr[reg], dear);
541 break;
542 case 55: // stfdu
543 stfd(ctx->fpu.fpr[reg], dear);
544 ctx->cpu.gpr[areg] = dear;
545 break;
546 case 52: // stfs
547 stfs(ctx->fpu.fpr[reg], dear);
548 break;
549 case 53: // stfsu
550 stfs(ctx->fpu.fpr[reg], dear);
551 ctx->cpu.gpr[areg] = dear;
552 break;
553 case 31: // lfdux, lfdx, lfsux, lfsx, stfdux, stfdx, stfsux, stfsx
554 switch ((insn & 0x00001ffe) >> 1)
555 {
556 case 631: // lfdux
557 ctx->fpu.fpr[reg] = lfd(dear);
558 ctx->cpu.gpr[areg] = dear;
559 break;
560 case 599: // lfdx
561 ctx->fpu.fpr[reg] = lfd(dear);
562 break;
563 case 567: // lfsux
564 ctx->fpu.fpr[reg] = lfs(dear);
565 ctx->cpu.gpr[areg] = dear;
566 break;
567 case 535: // lfsx
568 ctx->fpu.fpr[reg] = lfs(dear);
569 break;
570 case 759: // stfdux
571 stfd(ctx->fpu.fpr[reg], dear);
572 ctx->cpu.gpr[areg] = dear;
573 break;
574 case 727: // stfdx
575 stfd(ctx->fpu.fpr[reg], dear);
576 break;
577 case 695: // stfsux
578 stfs(ctx->fpu.fpr[reg], dear);
579 ctx->cpu.gpr[areg] = dear;
580 break;
581 case 663: // stfsx
582 stfs(ctx->fpu.fpr[reg], dear);
583 break;
584 default:
585 fixed = 0;
586 break;
587 }
588 break;
589 default:
590 fixed = 0;
591 break;
592 }
593
594 if (fixed)
595 {
596 ctx->cpu.srr0 += 4;
597 return;
598 }
599 else
600 {
601 D(bug("[KRN] Alignment exception handler failed to help... INSN=%08x, DEAR=%08x\n", insn, dear));
602 generic_handler(ctx, exception);
603 }
604 }
605
606
607 static void __attribute__((used)) __EXCEPTION_Prolog_template()
608 {
609
610 /*
611 * Create partial context on the stack. It is impossible to save it fully since the
612 * exception handlers have limited size. This code will do as much as possible and then
613 * jump to general trampoline...
614 */
615
616 PUT_INTR_TEMPLATE(0,c); /* crit */
617 PUT_INTR_TEMPLATE(1,mc); /* machine check */
618 PUT_INTR_TEMPLATE(2,);
619 PUT_INTR_TEMPLATE(3,);
620 PUT_INTR_TEMPLATE(4,);
621 PUT_INTR_TEMPLATE(5,);
622 PUT_INTR_TEMPLATE(6,);
623 PUT_INTR_TEMPLATE(7,);
624 PUT_INTR_TEMPLATE(8,);
625 PUT_INTR_TEMPLATE(9,);
626 PUT_INTR_TEMPLATE(10,);
627 PUT_INTR_TEMPLATE(11,);
628 PUT_INTR_TEMPLATE(12, c); /* crit */
629 PUT_INTR_TEMPLATE(13,);
630 PUT_INTR_TEMPLATE(14,);
631 PUT_INTR_TEMPLATE(15, c); /* crit */
632 }
633
634 static void __attribute__((used)) __EXCEPTION_Trampoline_template()
635 {
636 asm volatile(".section .text,\"ax\"\n\t.align 5\n\t.globl __EXCEPTION_Trampoline\n\t.type __EXCEPTION_Trampoline,@function\n"
637 "__EXCEPTION_Trampoline: \n\t"
638 "stw %%r6,%[gpr6](%%r3) \n\t"
639 "stw %%r7,%[gpr7](%%r3) \n\t"
640 "stw %%r8,%[gpr8](%%r3) \n\t"
641 "stw %%r9,%[gpr9](%%r3) \n\t"
642 "stw %%r10,%[gpr10](%%r3) \n\t"
643 "stw %%r11,%[gpr11](%%r3) \n\t"
644 "stw %%r12,%[gpr12](%%r3) \n\t"
645 "stw %%r13,%[gpr13](%%r3) \n\t"
646 "stw %%r14,%[gpr14](%%r3) \n\t"
647 "stw %%r15,%[gpr15](%%r3) \n\t"
648 "stw %%r16,%[gpr16](%%r3) \n\t"
649 "stw %%r17,%[gpr17](%%r3) \n\t"
650 "stw %%r18,%[gpr18](%%r3) \n\t"
651 "stw %%r19,%[gpr19](%%r3) \n\t"
652 "stw %%r20,%[gpr20](%%r3) \n\t"
653 "stw %%r21,%[gpr21](%%r3) \n\t"
654 "stw %%r22,%[gpr22](%%r3) \n\t"
655 "stw %%r23,%[gpr23](%%r3) \n\t"
656 "stw %%r24,%[gpr24](%%r3) \n\t"
657 "stw %%r25,%[gpr25](%%r3) \n\t"
658 "stw %%r26,%[gpr26](%%r3) \n\t"
659 "stw %%r27,%[gpr27](%%r3) \n\t"
660 "stw %%r28,%[gpr28](%%r3) \n\t"
661 "stw %%r29,%[gpr29](%%r3) \n\t"
662 "stw %%r30,%[gpr30](%%r3) \n\t"
663 "stw %%r31,%[gpr31](%%r3) \n\t"
664 ::
665 [gpr6]"i"(offsetof(context_t, cpu.gpr[6])),
666 [gpr7]"i"(offsetof(context_t, cpu.gpr[7])),
667 [gpr8]"i"(offsetof(context_t, cpu.gpr[8])),
668 [gpr9]"i"(offsetof(context_t, cpu.gpr[9])),
669 [gpr10]"i"(offsetof(context_t, cpu.gpr[10])),
670 [gpr11]"i"(offsetof(context_t, cpu.gpr[11])),
671 [gpr12]"i"(offsetof(context_t, cpu.gpr[12])),
672 [gpr13]"i"(offsetof(context_t, cpu.gpr[13])),
673 [gpr14]"i"(offsetof(context_t, cpu.gpr[14])),
674 [gpr15]"i"(offsetof(context_t, cpu.gpr[15])),
675 [gpr16]"i"(offsetof(context_t, cpu.gpr[16])),
676 [gpr17]"i"(offsetof(context_t, cpu.gpr[17])),
677 [gpr18]"i"(offsetof(context_t, cpu.gpr[18])),
678 [gpr19]"i"(offsetof(context_t, cpu.gpr[19])),
679 [gpr20]"i"(offsetof(context_t, cpu.gpr[20])),
680 [gpr21]"i"(offsetof(context_t, cpu.gpr[21])),
681 [gpr22]"i"(offsetof(context_t, cpu.gpr[22])),
682 [gpr23]"i"(offsetof(context_t, cpu.gpr[23])),
683 [gpr24]"i"(offsetof(context_t, cpu.gpr[24])),
684 [gpr25]"i"(offsetof(context_t, cpu.gpr[25])),
685 [gpr26]"i"(offsetof(context_t, cpu.gpr[26])),
686 [gpr27]"i"(offsetof(context_t, cpu.gpr[27])),
687 [gpr28]"i"(offsetof(context_t, cpu.gpr[28])),
688 [gpr29]"i"(offsetof(context_t, cpu.gpr[29])),
689 [gpr30]"i"(offsetof(context_t, cpu.gpr[30])),
690 [gpr31]"i"(offsetof(context_t, cpu.gpr[31]))
691 );
692
693 asm volatile(
694 "mfmsr %%r0 \n\t"
695 "ori %%r0,%%r0, %[msrval]@l \n\t"
696 "mtmsr %%r0; isync \n\t"
697 "stfd %%f0,%[fr0](%%r3) \n\t"
698 "mffs %%f0 \n\t"
699 "stfd %%f0,%[fpscr](%%r3) \n\t"
700 "stfd %%f1,%[fr1](%%r3) \n\t"
701 "stfd %%f2,%[fr2](%%r3) \n\t"
702 "stfd %%f3,%[fr3](%%r3) \n\t"
703 "stfd %%f4,%[fr4](%%r3) \n\t"
704 "stfd %%f5,%[fr5](%%r3) \n\t"
705 "stfd %%f6,%[fr6](%%r3) \n\t"
706 "stfd %%f7,%[fr7](%%r3) \n\t"
707 "stfd %%f8,%[fr8](%%r3) \n\t"
708 "stfd %%f9,%[fr9](%%r3) \n\t"
709 "stfd %%f10,%[fr10](%%r3) \n\t"
710 "stfd %%f11,%[fr11](%%r3) \n\t"
711 "stfd %%f12,%[fr12](%%r3) \n\t"
712 "stfd %%f13,%[fr13](%%r3) \n\t"
713 "stfd %%f14,%[fr14](%%r3) \n\t"
714 "stfd %%f15,%[fr15](%%r3) \n\t"
715 ::
716 [fpscr]"i"(offsetof(context_t, fpu.fpscr)),
717 [fr0]"i"(offsetof(context_t, fpu.fpr[0])),
718 [fr1]"i"(offsetof(context_t, fpu.fpr[1])),
719 [fr2]"i"(offsetof(context_t, fpu.fpr[2])),
720 [fr3]"i"(offsetof(context_t, fpu.fpr[3])),
721 [fr4]"i"(offsetof(context_t, fpu.fpr[4])),
722 [fr5]"i"(offsetof(context_t, fpu.fpr[5])),
723 [fr6]"i"(offsetof(context_t, fpu.fpr[6])),
724 [fr7]"i"(offsetof(context_t, fpu.fpr[7])),
725 [fr8]"i"(offsetof(context_t, fpu.fpr[8])),
726 [fr9]"i"(offsetof(context_t, fpu.fpr[9])),
727 [fr10]"i"(offsetof(context_t, fpu.fpr[10])),
728 [fr11]"i"(offsetof(context_t, fpu.fpr[11])),
729 [fr12]"i"(offsetof(context_t, fpu.fpr[12])),
730 [fr13]"i"(offsetof(context_t, fpu.fpr[13])),
731 [fr14]"i"(offsetof(context_t, fpu.fpr[14])),
732 [fr15]"i"(offsetof(context_t, fpu.fpr[15])),
733 [msrval]"i"(MSR_FP)
734 );
735
736 asm volatile(
737 "stfd %%f16,%[fr16](%%r3) \n\t"
738 "stfd %%f17,%[fr17](%%r3) \n\t"
739 "stfd %%f18,%[fr18](%%r3) \n\t"
740 "stfd %%f19,%[fr19](%%r3) \n\t"
741 "stfd %%f20,%[fr20](%%r3) \n\t"
742 "stfd %%f21,%[fr21](%%r3) \n\t"
743 "stfd %%f22,%[fr22](%%r3) \n\t"
744 "stfd %%f23,%[fr23](%%r3) \n\t"
745 "stfd %%f24,%[fr24](%%r3) \n\t"
746 "stfd %%f25,%[fr25](%%r3) \n\t"
747 "stfd %%f26,%[fr26](%%r3) \n\t"
748 "stfd %%f27,%[fr27](%%r3) \n\t"
749 "stfd %%f28,%[fr28](%%r3) \n\t"
750 "stfd %%f29,%[fr29](%%r3) \n\t"
751 "stfd %%f30,%[fr30](%%r3) \n\t"
752 "stfd %%f31,%[fr31](%%r3) \n\t"
753 "mflr %%r30 \n\t"
754 "mr %%r31, %%r3 \n\t"
755 "addi %%r1, %%r1, -16 \n\t"
756 "bl handle_exception \n\t"
757 "addi %%r1, %%r1, 16 \n\t"
758 "mr %%r3, %%r31 \n\t"
759 "mtlr %%r30 \n\t"
760 ::
761 [fr16]"i"(offsetof(context_t, fpu.fpr[16])),
762 [fr17]"i"(offsetof(context_t, fpu.fpr[17])),
763 [fr18]"i"(offsetof(context_t, fpu.fpr[18])),
764 [fr19]"i"(offsetof(context_t, fpu.fpr[19])),
765 [fr20]"i"(offsetof(context_t, fpu.fpr[20])),
766 [fr21]"i"(offsetof(context_t, fpu.fpr[21])),
767 [fr22]"i"(offsetof(context_t, fpu.fpr[22])),
768 [fr23]"i"(offsetof(context_t, fpu.fpr[23])),
769 [fr24]"i"(offsetof(context_t, fpu.fpr[24])),
770 [fr25]"i"(offsetof(context_t, fpu.fpr[25])),
771 [fr26]"i"(offsetof(context_t, fpu.fpr[26])),
772 [fr27]"i"(offsetof(context_t, fpu.fpr[27])),
773 [fr28]"i"(offsetof(context_t, fpu.fpr[28])),
774 [fr29]"i"(offsetof(context_t, fpu.fpr[29])),
775 [fr30]"i"(offsetof(context_t, fpu.fpr[30])),
776 [fr31]"i"(offsetof(context_t, fpu.fpr[31]))
777 );
778 asm volatile(
779 "lwz %%r31,%[gpr31](%%r3) \n\t"
780 "lwz %%r30,%[gpr30](%%r3) \n\t"
781 "lwz %%r29,%[gpr29](%%r3) \n\t"
782 "lwz %%r28,%[gpr28](%%r3) \n\t"
783 "lwz %%r27,%[gpr27](%%r3) \n\t"
784 "lwz %%r26,%[gpr26](%%r3) \n\t"
785 "lwz %%r25,%[gpr25](%%r3) \n\t"
786 "lwz %%r24,%[gpr24](%%r3) \n\t"
787 "lwz %%r23,%[gpr23](%%r3) \n\t"
788 "lwz %%r22,%[gpr22](%%r3) \n\t"
789 "lwz %%r21,%[gpr21](%%r3) \n\t"
790 "lwz %%r20,%[gpr20](%%r3) \n\t"
791 "lwz %%r19,%[gpr19](%%r3) \n\t"
792 "lwz %%r18,%[gpr18](%%r3) \n\t"
793 "lwz %%r17,%[gpr17](%%r3) \n\t"
794 "lwz %%r16,%[gpr16](%%r3) \n\t"
795 "lwz %%r15,%[gpr15](%%r3) \n\t"
796 "lwz %%r14,%[gpr14](%%r3) \n\t"
797 "lwz %%r13,%[gpr13](%%r3) \n\t"
798 "lwz %%r12,%[gpr12](%%r3) \n\t"
799 ::
800 [gpr12]"i"(offsetof(context_t, cpu.gpr[12])),
801 [gpr13]"i"(offsetof(context_t, cpu.gpr[13])),
802 [gpr14]"i"(offsetof(context_t, cpu.gpr[14])),
803 [gpr15]"i"(offsetof(context_t, cpu.gpr[15])),
804 [gpr16]"i"(offsetof(context_t, cpu.gpr[16])),
805 [gpr17]"i"(offsetof(context_t, cpu.gpr[17])),
806 [gpr18]"i"(offsetof(context_t, cpu.gpr[18])),
807 [gpr19]"i"(offsetof(context_t, cpu.gpr[19])),
808 [gpr20]"i"(offsetof(context_t, cpu.gpr[20])),
809 [gpr21]"i"(offsetof(context_t, cpu.gpr[21])),
810 [gpr22]"i"(offsetof(context_t, cpu.gpr[22])),
811 [gpr23]"i"(offsetof(context_t, cpu.gpr[23])),
812 [gpr24]"i"(offsetof(context_t, cpu.gpr[24])),
813 [gpr25]"i"(offsetof(context_t, cpu.gpr[25])),
814 [gpr26]"i"(offsetof(context_t, cpu.gpr[26])),
815 [gpr27]"i"(offsetof(context_t, cpu.gpr[27])),
816 [gpr28]"i"(offsetof(context_t, cpu.gpr[28])),
817 [gpr29]"i"(offsetof(context_t, cpu.gpr[29])),
818 [gpr30]"i"(offsetof(context_t, cpu.gpr[30])),
819 [gpr31]"i"(offsetof(context_t, cpu.gpr[31]))
820 );
821
822 asm volatile(
823 "lfd %%f0,%[fpscr](%%r3) \n\t"
824 "mtfsf 255,%%f0 \n\t"
825 "lfd %%f0,%[fr0](%%r3) \n\t"
826 "lfd %%f1,%[fr1](%%r3) \n\t"
827 "lfd %%f2,%[fr2](%%r3) \n\t"
828 "lfd %%f3,%[fr3](%%r3) \n\t"
829 "lfd %%f4,%[fr4](%%r3) \n\t"
830 "lfd %%f5,%[fr5](%%r3) \n\t"
831 "lfd %%f6,%[fr6](%%r3) \n\t"
832 "lfd %%f7,%[fr7](%%r3) \n\t"
833 "lfd %%f8,%[fr8](%%r3) \n\t"
834 "lfd %%f9,%[fr9](%%r3) \n\t"
835 "lfd %%f10,%[fr10](%%r3) \n\t"
836 "lfd %%f11,%[fr11](%%r3) \n\t"
837 "lfd %%f12,%[fr12](%%r3) \n\t"
838 "lfd %%f13,%[fr13](%%r3) \n\t"
839 "lfd %%f14,%[fr14](%%r3) \n\t"
840 "lfd %%f15,%[fr15](%%r3) \n\t"
841 ::
842 [fpscr]"i"(offsetof(context_t, fpu.fpscr)),
843 [fr0]"i"(offsetof(context_t, fpu.fpr[0])),
844 [fr1]"i"(offsetof(context_t, fpu.fpr[1])),
845 [fr2]"i"(offsetof(context_t, fpu.fpr[2])),
846 [fr3]"i"(offsetof(context_t, fpu.fpr[3])),
847 [fr4]"i"(offsetof(context_t, fpu.fpr[4])),
848 [fr5]"i"(offsetof(context_t, fpu.fpr[5])),
849 [fr6]"i"(offsetof(context_t, fpu.fpr[6])),
850 [fr7]"i"(offsetof(context_t, fpu.fpr[7])),
851 [fr8]"i"(offsetof(context_t, fpu.fpr[8])),
852 [fr9]"i"(offsetof(context_t, fpu.fpr[9])),
853 [fr10]"i"(offsetof(context_t, fpu.fpr[10])),
854 [fr11]"i"(offsetof(context_t, fpu.fpr[11])),
855 [fr12]"i"(offsetof(context_t, fpu.fpr[12])),
856 [fr13]"i"(offsetof(context_t, fpu.fpr[13])),
857 [fr14]"i"(offsetof(context_t, fpu.fpr[14])),
858 [fr15]"i"(offsetof(context_t, fpu.fpr[15]))
859 );
860 asm volatile(
861 "lfd %%f16,%[fr16](%%r3) \n\t"
862 "lfd %%f17,%[fr17](%%r3) \n\t"
863 "lfd %%f18,%[fr18](%%r3) \n\t"
864 "lfd %%f19,%[fr19](%%r3) \n\t"
865 "lfd %%f20,%[fr20](%%r3) \n\t"
866 "lfd %%f21,%[fr21](%%r3) \n\t"
867 "lfd %%f22,%[fr22](%%r3) \n\t"
868 "lfd %%f23,%[fr23](%%r3) \n\t"
869 "lfd %%f24,%[fr24](%%r3) \n\t"
870 "lfd %%f25,%[fr25](%%r3) \n\t"
871 "lfd %%f26,%[fr26](%%r3) \n\t"
872 "lfd %%f27,%[fr27](%%r3) \n\t"
873 "lfd %%f28,%[fr28](%%r3) \n\t"
874 "lfd %%f29,%[fr29](%%r3) \n\t"
875 "lfd %%f30,%[fr30](%%r3) \n\t"
876 "lfd %%f31,%[fr31](%%r3) \n\t"
877 ::
878 [fr16]"i"(offsetof(context_t, fpu.fpr[16])),
879 [fr17]"i"(offsetof(context_t, fpu.fpr[17])),
880 [fr18]"i"(offsetof(context_t, fpu.fpr[18])),
881 [fr19]"i"(offsetof(context_t, fpu.fpr[19])),
882 [fr20]"i"(offsetof(context_t, fpu.fpr[20])),
883 [fr21]"i"(offsetof(context_t, fpu.fpr[21])),
884 [fr22]"i"(offsetof(context_t, fpu.fpr[22])),
885 [fr23]"i"(offsetof(context_t, fpu.fpr[23])),
886 [fr24]"i"(offsetof(context_t, fpu.fpr[24])),
887 [fr25]"i"(offsetof(context_t, fpu.fpr[25])),
888 [fr26]"i"(offsetof(context_t, fpu.fpr[26])),
889 [fr27]"i"(offsetof(context_t, fpu.fpr[27])),
890 [fr28]"i"(offsetof(context_t, fpu.fpr[28])),
891 [fr29]"i"(offsetof(context_t, fpu.fpr[29])),
892 [fr30]"i"(offsetof(context_t, fpu.fpr[30])),
893 [fr31]"i"(offsetof(context_t, fpu.fpr[31]))
894 );
895
896 asm volatile(
897 "lwz %%r11,%[gpr11](%%r3) \n\t"
898 "lwz %%r10,%[gpr10](%%r3) \n\t"
899 "lwz %%r9,%[gpr9](%%r3) \n\t"
900 "lwz %%r8,%[gpr8](%%r3) \n\t"
901 "lwz %%r7,%[gpr7](%%r3) \n\t"
902 "lwz %%r6,%[gpr6](%%r3) \n\t"
903 "blr\n"
904 ::
905 [gpr6]"i"(offsetof(context_t, cpu.gpr[6])),
906 [gpr7]"i"(offsetof(context_t, cpu.gpr[7])),
907 [gpr8]"i"(offsetof(context_t, cpu.gpr[8])),
908 [gpr9]"i"(offsetof(context_t, cpu.gpr[9])),
909 [gpr10]"i"(offsetof(context_t, cpu.gpr[10])),
910 [gpr11]"i"(offsetof(context_t, cpu.gpr[11]))
911 );
912 }
Mirror of AROS' svn.