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