?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
revert between 56095 -> 55830 in arch
[AROS.git] / arch / ppc-sam440 / kernel / intr.c
blob446449a1634fc3ac7812d1ad1547fe118829996c
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 program_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 program_handler(context_t *ctx, uint8_t exception)
361 {
362
363 uint32_t pc;
364 BOOL exchandled = FALSE;
365
366 DB2(bug("[KRN] Program handler. Context @ %p. srr1=%08x\n", ctx, ctx->cpu.srr1));
367
368 if(rdspr(ESR) & ESR_PIL)
369 {
370 pc = ctx->cpu.srr0;
371
372 DB2(bug("[KRN] Illegal Instruction @ %08x\n", pc));
373
374 /* Check if lwsync is executed */
375 if (*(uint32_t *)pc == 0x7c2004ac)
376 {
377 bug("[KRN] Warning: emulating 'lwsync'\n");
378
379 exchandled = TRUE;
380
381 /* PPC440 doesn't have lwsync, do sync instead */
382 asm volatile("sync" : : : "memory");
383
384 /* move the return address forward so the
385 instruction isn't run again */
386 ctx->cpu.srr0 += 4;
387 }
388 }
389
390 if (!exchandled)
391 generic_handler(ctx, exception);
392
393 ExitInterrupt(ctx);
394 }
395
396 void mmu_handler(context_t *ctx, uint8_t exception)
397 {
398 if (!!krnRunExceptionHandlers(KernelBase, exception, ctx))
399 {
400 /* Any unhandled MMU activity is fatal for now. */
401 dumpregs(ctx, exception);
402 }
403 }
404
405 double lfd(intptr_t addr)
406 {
407 union {
408 uint8_t u8[8];
409 uint16_t u16[4];
410 uint32_t u32[2];
411 uint64_t u64;
412 float f[2];
413 double d;
414 } conv;
415
416 switch ((intptr_t)addr & 3)
417 {
418 case 0:
419 conv.u32[0] = ((uint32_t *)addr)[0];
420 conv.u32[1] = ((uint32_t *)addr)[1];
421 break;
422
423 case 2:
424 conv.u16[0] = ((uint16_t *)addr)[0];
425 conv.u16[1] = ((uint16_t *)addr)[1];
426 conv.u16[2] = ((uint16_t *)addr)[2];
427 conv.u16[3] = ((uint16_t *)addr)[3];
428 break;
429
430 default:
431 conv.u8[0] = ((uint8_t *)addr)[0];
432 conv.u8[1] = ((uint8_t *)addr)[1];
433 conv.u8[2] = ((uint8_t *)addr)[2];
434 conv.u8[3] = ((uint8_t *)addr)[3];
435 conv.u8[4] = ((uint8_t *)addr)[4];
436 conv.u8[5] = ((uint8_t *)addr)[5];
437 conv.u8[6] = ((uint8_t *)addr)[6];
438 conv.u8[7] = ((uint8_t *)addr)[7];
439 break;
440 }
441
442 return conv.d;
443 }
444
445 float lfs(intptr_t addr)
446 {
447 union {
448 uint8_t u8[8];
449 uint16_t u16[4];
450 uint32_t u32[2];
451 uint64_t u64;
452 float f[2];
453 double d;
454 } conv;
455
456 switch ((intptr_t)addr & 3)
457 {
458 case 0:
459 conv.u32[0] = ((uint32_t *)addr)[0];
460 break;
461
462 case 2:
463 conv.u16[0] = ((uint16_t *)addr)[0];
464 conv.u16[1] = ((uint16_t *)addr)[1];
465 break;
466
467 default:
468 conv.u8[0] = ((uint8_t *)addr)[0];
469 conv.u8[1] = ((uint8_t *)addr)[1];
470 conv.u8[2] = ((uint8_t *)addr)[2];
471 conv.u8[3] = ((uint8_t *)addr)[3];
472 break;
473 }
474
475 return conv.f[0];
476 }
477
478 void stfd(double v, intptr_t addr)
479 {
480 union {
481 uint8_t u8[8];
482 uint16_t u16[4];
483 uint32_t u32[2];
484 uint64_t u64;
485 float f[2];
486 double d;
487 } conv;
488
489 conv.d = v;
490
491 switch ((intptr_t)addr & 3)
492 {
493 case 0:
494 ((uint32_t *)addr)[0] = conv.u32[0];
495 ((uint32_t *)addr)[1] = conv.u32[1];
496 break;
497
498 case 2:
499 ((uint16_t *)addr)[0] = conv.u16[0];
500 ((uint16_t *)addr)[1] = conv.u16[1];
501 ((uint16_t *)addr)[2] = conv.u16[2];
502 ((uint16_t *)addr)[3] = conv.u16[3];
503 break;
504
505 default:
506 ((uint8_t *)addr)[0] = conv.u8[0];
507 ((uint8_t *)addr)[1] = conv.u8[1];
508 ((uint8_t *)addr)[2] = conv.u8[2];
509 ((uint8_t *)addr)[3] = conv.u8[3];
510 ((uint8_t *)addr)[4] = conv.u8[4];
511 ((uint8_t *)addr)[5] = conv.u8[5];
512 ((uint8_t *)addr)[6] = conv.u8[6];
513 ((uint8_t *)addr)[7] = conv.u8[7];
514 break;
515 }
516 }
517
518 void stfs(float v, intptr_t addr)
519 {
520 union {
521 uint8_t u8[8];
522 uint16_t u16[4];
523 uint32_t u32[2];
524 uint64_t u64;
525 float f[2];
526 double d;
527 } conv;
528
529 conv.f[0] = v;
530
531 switch ((intptr_t)addr & 3)
532 {
533 case 0:
534 ((uint32_t *)addr)[0] = conv.u32[0];
535 break;
536
537 case 2:
538 ((uint16_t *)addr)[0] = conv.u16[0];
539 ((uint16_t *)addr)[1] = conv.u16[1];
540 break;
541
542 default:
543 ((uint8_t *)addr)[0] = conv.u8[0];
544 ((uint8_t *)addr)[1] = conv.u8[1];
545 ((uint8_t *)addr)[2] = conv.u8[2];
546 ((uint8_t *)addr)[3] = conv.u8[3];
547 break;
548 }
549 }
550
551 void alignment_handler(context_t *ctx, uint8_t exception)
552 {
553 int fixed = 1;
554
555 intptr_t dear = rdspr(DEAR);
556 uint32_t insn = *(uint32_t *)ctx->cpu.srr0;
557
558 uint8_t reg = (insn >> 21) & 0x1f; // source/dest register
559 uint8_t areg = (insn >> 16) & 0x1f; // register to be updated with dear value
560
561 D(bug("[KRN] Alignment handler. Context @ %p. srr1=%08x\n", ctx, ctx->cpu.srr1));
562
563
564 switch (insn >> 26)
565 {
566 case 50: // lfd
567 ctx->fpu.fpr[reg] = lfd(dear);
568 break;
569 case 51: // lfdu
570 ctx->fpu.fpr[reg] = lfd(dear);
571 ctx->cpu.gpr[areg] = dear;
572 break;
573 case 48: // lfs
574 ctx->fpu.fpr[reg] = lfs(dear);
575 break;
576 case 49: // lfsu
577 ctx->fpu.fpr[reg] = lfs(dear);
578 ctx->cpu.gpr[areg] = dear;
579 break;
580 case 54: // stfd
581 stfd(ctx->fpu.fpr[reg], dear);
582 break;
583 case 55: // stfdu
584 stfd(ctx->fpu.fpr[reg], dear);
585 ctx->cpu.gpr[areg] = dear;
586 break;
587 case 52: // stfs
588 stfs(ctx->fpu.fpr[reg], dear);
589 break;
590 case 53: // stfsu
591 stfs(ctx->fpu.fpr[reg], dear);
592 ctx->cpu.gpr[areg] = dear;
593 break;
594 case 31: // lfdux, lfdx, lfsux, lfsx, stfdux, stfdx, stfsux, stfsx
595 switch ((insn & 0x00001ffe) >> 1)
596 {
597 case 631: // lfdux
598 ctx->fpu.fpr[reg] = lfd(dear);
599 ctx->cpu.gpr[areg] = dear;
600 break;
601 case 599: // lfdx
602 ctx->fpu.fpr[reg] = lfd(dear);
603 break;
604 case 567: // lfsux
605 ctx->fpu.fpr[reg] = lfs(dear);
606 ctx->cpu.gpr[areg] = dear;
607 break;
608 case 535: // lfsx
609 ctx->fpu.fpr[reg] = lfs(dear);
610 break;
611 case 759: // stfdux
612 stfd(ctx->fpu.fpr[reg], dear);
613 ctx->cpu.gpr[areg] = dear;
614 break;
615 case 727: // stfdx
616 stfd(ctx->fpu.fpr[reg], dear);
617 break;
618 case 695: // stfsux
619 stfs(ctx->fpu.fpr[reg], dear);
620 ctx->cpu.gpr[areg] = dear;
621 break;
622 case 663: // stfsx
623 stfs(ctx->fpu.fpr[reg], dear);
624 break;
625 default:
626 fixed = 0;
627 break;
628 }
629 break;
630 default:
631 fixed = 0;
632 break;
633 }
634
635 if (fixed)
636 {
637 ctx->cpu.srr0 += 4;
638 return;
639 }
640 else
641 {
642 D(bug("[KRN] Alignment exception handler failed to help... INSN=%08x, DEAR=%08x\n", insn, dear));
643 generic_handler(ctx, exception);
644 }
645 }
646
647
648 static void __attribute__((used)) __EXCEPTION_Prolog_template()
649 {
650
651 /*
652 * Create partial context on the stack. It is impossible to save it fully since the
653 * exception handlers have limited size. This code will do as much as possible and then
654 * jump to general trampoline...
655 */
656
657 PUT_INTR_TEMPLATE(0,c); /* crit */
658 PUT_INTR_TEMPLATE(1,mc); /* machine check */
659 PUT_INTR_TEMPLATE(2,);
660 PUT_INTR_TEMPLATE(3,);
661 PUT_INTR_TEMPLATE(4,);
662 PUT_INTR_TEMPLATE(5,);
663 PUT_INTR_TEMPLATE(6,);
664 PUT_INTR_TEMPLATE(7,);
665 PUT_INTR_TEMPLATE(8,);
666 PUT_INTR_TEMPLATE(9,);
667 PUT_INTR_TEMPLATE(10,);
668 PUT_INTR_TEMPLATE(11,);
669 PUT_INTR_TEMPLATE(12, c); /* crit */
670 PUT_INTR_TEMPLATE(13,);
671 PUT_INTR_TEMPLATE(14,);
672 PUT_INTR_TEMPLATE(15, c); /* crit */
673 }
674
675 static void __attribute__((used)) __EXCEPTION_Trampoline_template()
676 {
677 asm volatile(".section .text,\"ax\"\n\t.align 5\n\t.globl __EXCEPTION_Trampoline\n\t.type __EXCEPTION_Trampoline,@function\n"
678 "__EXCEPTION_Trampoline: \n\t"
679 "stw %%r6,%[gpr6](%%r3) \n\t"
680 "stw %%r7,%[gpr7](%%r3) \n\t"
681 "stw %%r8,%[gpr8](%%r3) \n\t"
682 "stw %%r9,%[gpr9](%%r3) \n\t"
683 "stw %%r10,%[gpr10](%%r3) \n\t"
684 "stw %%r11,%[gpr11](%%r3) \n\t"
685 "stw %%r12,%[gpr12](%%r3) \n\t"
686 "stw %%r13,%[gpr13](%%r3) \n\t"
687 "stw %%r14,%[gpr14](%%r3) \n\t"
688 "stw %%r15,%[gpr15](%%r3) \n\t"
689 "stw %%r16,%[gpr16](%%r3) \n\t"
690 "stw %%r17,%[gpr17](%%r3) \n\t"
691 "stw %%r18,%[gpr18](%%r3) \n\t"
692 "stw %%r19,%[gpr19](%%r3) \n\t"
693 "stw %%r20,%[gpr20](%%r3) \n\t"
694 "stw %%r21,%[gpr21](%%r3) \n\t"
695 "stw %%r22,%[gpr22](%%r3) \n\t"
696 "stw %%r23,%[gpr23](%%r3) \n\t"
697 "stw %%r24,%[gpr24](%%r3) \n\t"
698 "stw %%r25,%[gpr25](%%r3) \n\t"
699 "stw %%r26,%[gpr26](%%r3) \n\t"
700 "stw %%r27,%[gpr27](%%r3) \n\t"
701 "stw %%r28,%[gpr28](%%r3) \n\t"
702 "stw %%r29,%[gpr29](%%r3) \n\t"
703 "stw %%r30,%[gpr30](%%r3) \n\t"
704 "stw %%r31,%[gpr31](%%r3) \n\t"
705 ::
706 [gpr6]"i"(offsetof(context_t, cpu.gpr[6])),
707 [gpr7]"i"(offsetof(context_t, cpu.gpr[7])),
708 [gpr8]"i"(offsetof(context_t, cpu.gpr[8])),
709 [gpr9]"i"(offsetof(context_t, cpu.gpr[9])),
710 [gpr10]"i"(offsetof(context_t, cpu.gpr[10])),
711 [gpr11]"i"(offsetof(context_t, cpu.gpr[11])),
712 [gpr12]"i"(offsetof(context_t, cpu.gpr[12])),
713 [gpr13]"i"(offsetof(context_t, cpu.gpr[13])),
714 [gpr14]"i"(offsetof(context_t, cpu.gpr[14])),
715 [gpr15]"i"(offsetof(context_t, cpu.gpr[15])),
716 [gpr16]"i"(offsetof(context_t, cpu.gpr[16])),
717 [gpr17]"i"(offsetof(context_t, cpu.gpr[17])),
718 [gpr18]"i"(offsetof(context_t, cpu.gpr[18])),
719 [gpr19]"i"(offsetof(context_t, cpu.gpr[19])),
720 [gpr20]"i"(offsetof(context_t, cpu.gpr[20])),
721 [gpr21]"i"(offsetof(context_t, cpu.gpr[21])),
722 [gpr22]"i"(offsetof(context_t, cpu.gpr[22])),
723 [gpr23]"i"(offsetof(context_t, cpu.gpr[23])),
724 [gpr24]"i"(offsetof(context_t, cpu.gpr[24])),
725 [gpr25]"i"(offsetof(context_t, cpu.gpr[25])),
726 [gpr26]"i"(offsetof(context_t, cpu.gpr[26])),
727 [gpr27]"i"(offsetof(context_t, cpu.gpr[27])),
728 [gpr28]"i"(offsetof(context_t, cpu.gpr[28])),
729 [gpr29]"i"(offsetof(context_t, cpu.gpr[29])),
730 [gpr30]"i"(offsetof(context_t, cpu.gpr[30])),
731 [gpr31]"i"(offsetof(context_t, cpu.gpr[31]))
732 );
733
734 asm volatile(
735 "mfmsr %%r0 \n\t"
736 "ori %%r0,%%r0, %[msrval]@l \n\t"
737 "mtmsr %%r0; isync \n\t"
738 "stfd %%f0,%[fr0](%%r3) \n\t"
739 "mffs %%f0 \n\t"
740 "stfd %%f0,%[fpscr](%%r3) \n\t"
741 "stfd %%f1,%[fr1](%%r3) \n\t"
742 "stfd %%f2,%[fr2](%%r3) \n\t"
743 "stfd %%f3,%[fr3](%%r3) \n\t"
744 "stfd %%f4,%[fr4](%%r3) \n\t"
745 "stfd %%f5,%[fr5](%%r3) \n\t"
746 "stfd %%f6,%[fr6](%%r3) \n\t"
747 "stfd %%f7,%[fr7](%%r3) \n\t"
748 "stfd %%f8,%[fr8](%%r3) \n\t"
749 "stfd %%f9,%[fr9](%%r3) \n\t"
750 "stfd %%f10,%[fr10](%%r3) \n\t"
751 "stfd %%f11,%[fr11](%%r3) \n\t"
752 "stfd %%f12,%[fr12](%%r3) \n\t"
753 "stfd %%f13,%[fr13](%%r3) \n\t"
754 "stfd %%f14,%[fr14](%%r3) \n\t"
755 "stfd %%f15,%[fr15](%%r3) \n\t"
756 ::
757 [fpscr]"i"(offsetof(context_t, fpu.fpscr)),
758 [fr0]"i"(offsetof(context_t, fpu.fpr[0])),
759 [fr1]"i"(offsetof(context_t, fpu.fpr[1])),
760 [fr2]"i"(offsetof(context_t, fpu.fpr[2])),
761 [fr3]"i"(offsetof(context_t, fpu.fpr[3])),
762 [fr4]"i"(offsetof(context_t, fpu.fpr[4])),
763 [fr5]"i"(offsetof(context_t, fpu.fpr[5])),
764 [fr6]"i"(offsetof(context_t, fpu.fpr[6])),
765 [fr7]"i"(offsetof(context_t, fpu.fpr[7])),
766 [fr8]"i"(offsetof(context_t, fpu.fpr[8])),
767 [fr9]"i"(offsetof(context_t, fpu.fpr[9])),
768 [fr10]"i"(offsetof(context_t, fpu.fpr[10])),
769 [fr11]"i"(offsetof(context_t, fpu.fpr[11])),
770 [fr12]"i"(offsetof(context_t, fpu.fpr[12])),
771 [fr13]"i"(offsetof(context_t, fpu.fpr[13])),
772 [fr14]"i"(offsetof(context_t, fpu.fpr[14])),
773 [fr15]"i"(offsetof(context_t, fpu.fpr[15])),
774 [msrval]"i"(MSR_FP)
775 );
776
777 asm volatile(
778 "stfd %%f16,%[fr16](%%r3) \n\t"
779 "stfd %%f17,%[fr17](%%r3) \n\t"
780 "stfd %%f18,%[fr18](%%r3) \n\t"
781 "stfd %%f19,%[fr19](%%r3) \n\t"
782 "stfd %%f20,%[fr20](%%r3) \n\t"
783 "stfd %%f21,%[fr21](%%r3) \n\t"
784 "stfd %%f22,%[fr22](%%r3) \n\t"
785 "stfd %%f23,%[fr23](%%r3) \n\t"
786 "stfd %%f24,%[fr24](%%r3) \n\t"
787 "stfd %%f25,%[fr25](%%r3) \n\t"
788 "stfd %%f26,%[fr26](%%r3) \n\t"
789 "stfd %%f27,%[fr27](%%r3) \n\t"
790 "stfd %%f28,%[fr28](%%r3) \n\t"
791 "stfd %%f29,%[fr29](%%r3) \n\t"
792 "stfd %%f30,%[fr30](%%r3) \n\t"
793 "stfd %%f31,%[fr31](%%r3) \n\t"
794 "mflr %%r30 \n\t"
795 "mr %%r31, %%r3 \n\t"
796 "addi %%r1, %%r1, -16 \n\t"
797 "bl handle_exception \n\t"
798 "addi %%r1, %%r1, 16 \n\t"
799 "mr %%r3, %%r31 \n\t"
800 "mtlr %%r30 \n\t"
801 ::
802 [fr16]"i"(offsetof(context_t, fpu.fpr[16])),
803 [fr17]"i"(offsetof(context_t, fpu.fpr[17])),
804 [fr18]"i"(offsetof(context_t, fpu.fpr[18])),
805 [fr19]"i"(offsetof(context_t, fpu.fpr[19])),
806 [fr20]"i"(offsetof(context_t, fpu.fpr[20])),
807 [fr21]"i"(offsetof(context_t, fpu.fpr[21])),
808 [fr22]"i"(offsetof(context_t, fpu.fpr[22])),
809 [fr23]"i"(offsetof(context_t, fpu.fpr[23])),
810 [fr24]"i"(offsetof(context_t, fpu.fpr[24])),
811 [fr25]"i"(offsetof(context_t, fpu.fpr[25])),
812 [fr26]"i"(offsetof(context_t, fpu.fpr[26])),
813 [fr27]"i"(offsetof(context_t, fpu.fpr[27])),
814 [fr28]"i"(offsetof(context_t, fpu.fpr[28])),
815 [fr29]"i"(offsetof(context_t, fpu.fpr[29])),
816 [fr30]"i"(offsetof(context_t, fpu.fpr[30])),
817 [fr31]"i"(offsetof(context_t, fpu.fpr[31]))
818 );
819 asm volatile(
820 "lwz %%r31,%[gpr31](%%r3) \n\t"
821 "lwz %%r30,%[gpr30](%%r3) \n\t"
822 "lwz %%r29,%[gpr29](%%r3) \n\t"
823 "lwz %%r28,%[gpr28](%%r3) \n\t"
824 "lwz %%r27,%[gpr27](%%r3) \n\t"
825 "lwz %%r26,%[gpr26](%%r3) \n\t"
826 "lwz %%r25,%[gpr25](%%r3) \n\t"
827 "lwz %%r24,%[gpr24](%%r3) \n\t"
828 "lwz %%r23,%[gpr23](%%r3) \n\t"
829 "lwz %%r22,%[gpr22](%%r3) \n\t"
830 "lwz %%r21,%[gpr21](%%r3) \n\t"
831 "lwz %%r20,%[gpr20](%%r3) \n\t"
832 "lwz %%r19,%[gpr19](%%r3) \n\t"
833 "lwz %%r18,%[gpr18](%%r3) \n\t"
834 "lwz %%r17,%[gpr17](%%r3) \n\t"
835 "lwz %%r16,%[gpr16](%%r3) \n\t"
836 "lwz %%r15,%[gpr15](%%r3) \n\t"
837 "lwz %%r14,%[gpr14](%%r3) \n\t"
838 "lwz %%r13,%[gpr13](%%r3) \n\t"
839 "lwz %%r12,%[gpr12](%%r3) \n\t"
840 ::
841 [gpr12]"i"(offsetof(context_t, cpu.gpr[12])),
842 [gpr13]"i"(offsetof(context_t, cpu.gpr[13])),
843 [gpr14]"i"(offsetof(context_t, cpu.gpr[14])),
844 [gpr15]"i"(offsetof(context_t, cpu.gpr[15])),
845 [gpr16]"i"(offsetof(context_t, cpu.gpr[16])),
846 [gpr17]"i"(offsetof(context_t, cpu.gpr[17])),
847 [gpr18]"i"(offsetof(context_t, cpu.gpr[18])),
848 [gpr19]"i"(offsetof(context_t, cpu.gpr[19])),
849 [gpr20]"i"(offsetof(context_t, cpu.gpr[20])),
850 [gpr21]"i"(offsetof(context_t, cpu.gpr[21])),
851 [gpr22]"i"(offsetof(context_t, cpu.gpr[22])),
852 [gpr23]"i"(offsetof(context_t, cpu.gpr[23])),
853 [gpr24]"i"(offsetof(context_t, cpu.gpr[24])),
854 [gpr25]"i"(offsetof(context_t, cpu.gpr[25])),
855 [gpr26]"i"(offsetof(context_t, cpu.gpr[26])),
856 [gpr27]"i"(offsetof(context_t, cpu.gpr[27])),
857 [gpr28]"i"(offsetof(context_t, cpu.gpr[28])),
858 [gpr29]"i"(offsetof(context_t, cpu.gpr[29])),
859 [gpr30]"i"(offsetof(context_t, cpu.gpr[30])),
860 [gpr31]"i"(offsetof(context_t, cpu.gpr[31]))
861 );
862
863 asm volatile(
864 "lfd %%f0,%[fpscr](%%r3) \n\t"
865 "mtfsf 255,%%f0 \n\t"
866 "lfd %%f0,%[fr0](%%r3) \n\t"
867 "lfd %%f1,%[fr1](%%r3) \n\t"
868 "lfd %%f2,%[fr2](%%r3) \n\t"
869 "lfd %%f3,%[fr3](%%r3) \n\t"
870 "lfd %%f4,%[fr4](%%r3) \n\t"
871 "lfd %%f5,%[fr5](%%r3) \n\t"
872 "lfd %%f6,%[fr6](%%r3) \n\t"
873 "lfd %%f7,%[fr7](%%r3) \n\t"
874 "lfd %%f8,%[fr8](%%r3) \n\t"
875 "lfd %%f9,%[fr9](%%r3) \n\t"
876 "lfd %%f10,%[fr10](%%r3) \n\t"
877 "lfd %%f11,%[fr11](%%r3) \n\t"
878 "lfd %%f12,%[fr12](%%r3) \n\t"
879 "lfd %%f13,%[fr13](%%r3) \n\t"
880 "lfd %%f14,%[fr14](%%r3) \n\t"
881 "lfd %%f15,%[fr15](%%r3) \n\t"
882 ::
883 [fpscr]"i"(offsetof(context_t, fpu.fpscr)),
884 [fr0]"i"(offsetof(context_t, fpu.fpr[0])),
885 [fr1]"i"(offsetof(context_t, fpu.fpr[1])),
886 [fr2]"i"(offsetof(context_t, fpu.fpr[2])),
887 [fr3]"i"(offsetof(context_t, fpu.fpr[3])),
888 [fr4]"i"(offsetof(context_t, fpu.fpr[4])),
889 [fr5]"i"(offsetof(context_t, fpu.fpr[5])),
890 [fr6]"i"(offsetof(context_t, fpu.fpr[6])),
891 [fr7]"i"(offsetof(context_t, fpu.fpr[7])),
892 [fr8]"i"(offsetof(context_t, fpu.fpr[8])),
893 [fr9]"i"(offsetof(context_t, fpu.fpr[9])),
894 [fr10]"i"(offsetof(context_t, fpu.fpr[10])),
895 [fr11]"i"(offsetof(context_t, fpu.fpr[11])),
896 [fr12]"i"(offsetof(context_t, fpu.fpr[12])),
897 [fr13]"i"(offsetof(context_t, fpu.fpr[13])),
898 [fr14]"i"(offsetof(context_t, fpu.fpr[14])),
899 [fr15]"i"(offsetof(context_t, fpu.fpr[15]))
900 );
901 asm volatile(
902 "lfd %%f16,%[fr16](%%r3) \n\t"
903 "lfd %%f17,%[fr17](%%r3) \n\t"
904 "lfd %%f18,%[fr18](%%r3) \n\t"
905 "lfd %%f19,%[fr19](%%r3) \n\t"
906 "lfd %%f20,%[fr20](%%r3) \n\t"
907 "lfd %%f21,%[fr21](%%r3) \n\t"
908 "lfd %%f22,%[fr22](%%r3) \n\t"
909 "lfd %%f23,%[fr23](%%r3) \n\t"
910 "lfd %%f24,%[fr24](%%r3) \n\t"
911 "lfd %%f25,%[fr25](%%r3) \n\t"
912 "lfd %%f26,%[fr26](%%r3) \n\t"
913 "lfd %%f27,%[fr27](%%r3) \n\t"
914 "lfd %%f28,%[fr28](%%r3) \n\t"
915 "lfd %%f29,%[fr29](%%r3) \n\t"
916 "lfd %%f30,%[fr30](%%r3) \n\t"
917 "lfd %%f31,%[fr31](%%r3) \n\t"
918 ::
919 [fr16]"i"(offsetof(context_t, fpu.fpr[16])),
920 [fr17]"i"(offsetof(context_t, fpu.fpr[17])),
921 [fr18]"i"(offsetof(context_t, fpu.fpr[18])),
922 [fr19]"i"(offsetof(context_t, fpu.fpr[19])),
923 [fr20]"i"(offsetof(context_t, fpu.fpr[20])),
924 [fr21]"i"(offsetof(context_t, fpu.fpr[21])),
925 [fr22]"i"(offsetof(context_t, fpu.fpr[22])),
926 [fr23]"i"(offsetof(context_t, fpu.fpr[23])),
927 [fr24]"i"(offsetof(context_t, fpu.fpr[24])),
928 [fr25]"i"(offsetof(context_t, fpu.fpr[25])),
929 [fr26]"i"(offsetof(context_t, fpu.fpr[26])),
930 [fr27]"i"(offsetof(context_t, fpu.fpr[27])),
931 [fr28]"i"(offsetof(context_t, fpu.fpr[28])),
932 [fr29]"i"(offsetof(context_t, fpu.fpr[29])),
933 [fr30]"i"(offsetof(context_t, fpu.fpr[30])),
934 [fr31]"i"(offsetof(context_t, fpu.fpr[31]))
935 );
936
937 asm volatile(
938 "lwz %%r11,%[gpr11](%%r3) \n\t"
939 "lwz %%r10,%[gpr10](%%r3) \n\t"
940 "lwz %%r9,%[gpr9](%%r3) \n\t"
941 "lwz %%r8,%[gpr8](%%r3) \n\t"
942 "lwz %%r7,%[gpr7](%%r3) \n\t"
943 "lwz %%r6,%[gpr6](%%r3) \n\t"
944 "blr\n"
945 ::
946 [gpr6]"i"(offsetof(context_t, cpu.gpr[6])),
947 [gpr7]"i"(offsetof(context_t, cpu.gpr[7])),
948 [gpr8]"i"(offsetof(context_t, cpu.gpr[8])),
949 [gpr9]"i"(offsetof(context_t, cpu.gpr[9])),
950 [gpr10]"i"(offsetof(context_t, cpu.gpr[10])),
951 [gpr11]"i"(offsetof(context_t, cpu.gpr[11]))
952 );
953 }
Mirror of AROS' svn.