arch/ppc-sam440/kernel/intr.c

   1 #include <aros/kernel.h>
   2 #include <aros/libcall.h>
   3 #include <asm/amcc440.h>
   4 #include <stddef.h>
   5
   6 #include <proto/exec.h>
   7 #include <proto/kernel.h>
   8
   9 #include "kernel_intern.h"
  10 #include "syscall.h"
  11
  12 AROS_LH4(void *, KrnAddIRQHandler,
  13          AROS_LHA(uint8_t, irq, D0),
  14          AROS_LHA(void *, handler, A0),
  15          AROS_LHA(void *, handlerData, A1),
  16          AROS_LHA(void *, handlerData2, A2),
  17          struct KernelBase *, KernelBase, 7, Kernel)
  18 {
  19     AROS_LIBFUNC_INIT
  20
  21     struct ExecBase *SysBase = getSysBase();
  22     struct IntrNode *handle = NULL;
  23     D(bug("[KRN] KrnAddIRQHandler(%02x, %012p, %012p, %012p):\n", irq, handler, handlerData, handlerData2));
  24
  25     if (irq < 63)
  26     {
  27         /* Go to supervisor mode */
  28         goSuper();
  29
  30         handle = Allocate(KernelBase->kb_SupervisorMem, sizeof(struct IntrNode));
  31         D(bug("[KRN]   handle=%012p\n", handle));
  32
  33         if (handle)
  34         {
  35             handle->in_Handler = handler;
  36             handle->in_HandlerData = handlerData;
  37             handle->in_HandlerData2 = handlerData2;
  38             handle->in_type = it_interrupt;
  39             handle->in_nr = irq;
  40
  41             Disable();
  42             ADDHEAD(&KernelBase->kb_Interrupts[irq], &handle->in_Node);
  43
  44             if (irq < 32)
  45             {
  46                 wrdcr(UIC0_ER, rddcr(UIC0_ER) | (0x80000000 >> irq));
  47             }
  48             else
  49             {
  50                 wrdcr(UIC1_ER, rddcr(UIC1_ER) | (0x80000000 >> (irq - 32)));
  51                 wrdcr(UIC0_ER, rddcr(UIC0_ER) | 0x00000003);
  52             }
  53
  54             Enable();
  55         }
  56
  57         goUser();
  58     }
  59
  60     return handle;
  61
  62     AROS_LIBFUNC_EXIT
  63 }
  64
  65 AROS_LH1(void, KrnRemIRQHandler,
  66          AROS_LHA(void *, handle, A0),
  67          struct KernelBase *, KernelBase, 8, Kernel)
  68 {
  69     AROS_LIBFUNC_INIT
  70
  71     struct ExecBase *SysBase = getSysBase();
  72     struct IntrNode *h = handle;
  73     uint8_t irq = h->in_nr;
  74
  75     if (h && (h->in_type == it_interrupt))
  76     {
  77         goSuper();
  78
  79         Disable();
  80         REMOVE(h);
  81         if (IsListEmpty(&KernelBase->kb_Interrupts[irq]))
  82         {
  83             if (irq < 30)
  84             {
  85                 wrdcr(UIC0_ER, rddcr(UIC0_ER) & ~(0x80000000 >> irq));
  86             }
  87             else if (irq > 31)
  88             {
  89                 wrdcr(UIC1_ER, rddcr(UIC0_ER) & ~(0x80000000 >> (irq - 32)));
  90             }
  91         }
  92         Enable();
  93
  94         Deallocate(KernelBase->kb_SupervisorMem, h, sizeof(struct IntrNode));
  95
  96         goUser();
  97     }
  98
  99     AROS_LIBFUNC_EXIT
 100 }
 101
 102 AROS_LH4(void *, KrnAddExceptionHandler,
 103          AROS_LHA(uint8_t, irq, D0),
 104          AROS_LHA(void *, handler, A0),
 105          AROS_LHA(void *, handlerData, A1),
 106          AROS_LHA(void *, handlerData2, A2),
 107          struct KernelBase *, KernelBase, 7, Kernel)
 108 {
 109     AROS_LIBFUNC_INIT
 110
 111     struct ExecBase *SysBase = getSysBase();
 112     struct IntrNode *handle = NULL;
 113     D(bug("[KRN] KrnAddExceptionHandler(%02x, %012p, %012p, %012p):\n", irq, handler, handlerData, handlerData2));
 114
 115     if (irq < 16)
 116     {
 117         /* Go to supervisor mode */
 118         goSuper();
 119
 120         handle = Allocate(KernelBase->kb_SupervisorMem, sizeof(struct IntrNode));
 121         D(bug("[KRN]   handle=%012p\n", handle));
 122
 123         if (handle)
 124         {
 125             handle->in_Handler = handler;
 126             handle->in_HandlerData = handlerData;
 127             handle->in_HandlerData2 = handlerData2;
 128             handle->in_type = it_exception;
 129             handle->in_nr = irq;
 130
 131             Disable();
 132             ADDHEAD(&KernelBase->kb_Exceptions[irq], &handle->in_Node);
 133             Enable();
 134         }
 135
 136         goUser();
 137     }
 138
 139     return handle;
 140
 141     AROS_LIBFUNC_EXIT
 142 }
 143
 144 AROS_LH1(void, KrnRemExceptionHandler,
 145          AROS_LHA(void *, handle, A0),
 146          struct KernelBase *, KernelBase, 8, Kernel)
 147 {
 148     AROS_LIBFUNC_INIT
 149
 150     struct ExecBase *SysBase = getSysBase();
 151     struct IntrNode *h = handle;
 152
 153     if (h && (h->in_type == it_exception))
 154     {
 155         goSuper();
 156
 157         Disable();
 158         REMOVE(h);
 159         Enable();
 160
 161         Deallocate(KernelBase->kb_SupervisorMem, h, sizeof(struct IntrNode));
 162
 163         goUser();
 164     }
 165
 166     AROS_LIBFUNC_EXIT
 167 }
 168
 169 AROS_LH0I(void, KrnCli,
 170          struct KernelBase *, KernelBase, 9, Kernel)
 171 {
 172     AROS_LIBFUNC_INIT
 173
 174     asm volatile("li %%r3,%0; sc"::"i"(SC_CLI):"memory","r3");
 175
 176     AROS_LIBFUNC_EXIT
 177 }
 178
 179 AROS_LH0I(void, KrnSti,
 180          struct KernelBase *, KernelBase, 10, Kernel)
 181 {
 182     AROS_LIBFUNC_INIT
 183
 184     asm volatile("li %%r3,%0; sc"::"i"(SC_STI):"memory","r3");
 185
 186     AROS_LIBFUNC_EXIT
 187 }
 188
 189 AROS_LH0I(void, KrnIsSuper,
 190          struct KernelBase *, KernelBase, 12, Kernel)
 191 {
 192     AROS_LIBFUNC_INIT
 193
 194     register int retval asm ("r3");
 195
 196     asm volatile("sc":"=r"(retval):"0"(SC_ISSUPERSTATE):"memory");
 197
 198     return retval;
 199
 200     AROS_LIBFUNC_EXIT
 201 }
 202
 203 void *__EXCEPTION_0_Prolog();
 204 void *__EXCEPTION_1_Prolog();
 205 void *__EXCEPTION_2_Prolog();
 206 void *__EXCEPTION_3_Prolog();
 207 void *__EXCEPTION_4_Prolog();
 208 void *__EXCEPTION_5_Prolog();
 209 void *__EXCEPTION_6_Prolog();
 210 void *__EXCEPTION_7_Prolog();
 211 void *__EXCEPTION_8_Prolog();
 212 void *__EXCEPTION_9_Prolog();
 213 void *__EXCEPTION_10_Prolog();
 214 void *__EXCEPTION_11_Prolog();
 215 void *__EXCEPTION_12_Prolog();
 216 void *__EXCEPTION_13_Prolog();
 217 void *__EXCEPTION_14_Prolog();
 218 void *__EXCEPTION_15_Prolog();
 219
 220 void intr_init()
 221 {
 222     D(bug("[KRN] Setting up exception handlers\n"));
 223     wrspr(IVPR, ((uint32_t)&__EXCEPTION_0_Prolog) & 0xffff0000);
 224
 225     wrspr(IVOR0, ((uint32_t)&__EXCEPTION_0_Prolog) & 0x0000fff0);
 226     wrspr(IVOR1, ((uint32_t)&__EXCEPTION_1_Prolog) & 0x0000fff0);
 227     wrspr(IVOR2, ((uint32_t)&__EXCEPTION_2_Prolog) & 0x0000fff0);
 228     wrspr(IVOR3, ((uint32_t)&__EXCEPTION_3_Prolog) & 0x0000fff0);
 229     wrspr(IVOR4, ((uint32_t)&__EXCEPTION_4_Prolog) & 0x0000fff0);
 230     wrspr(IVOR5, ((uint32_t)&__EXCEPTION_5_Prolog) & 0x0000fff0);
 231     wrspr(IVOR6, ((uint32_t)&__EXCEPTION_6_Prolog) & 0x0000fff0);
 232     wrspr(IVOR7, ((uint32_t)&__EXCEPTION_7_Prolog) & 0x0000fff0);
 233     wrspr(IVOR8, ((uint32_t)&__EXCEPTION_8_Prolog) & 0x0000fff0);
 234     wrspr(IVOR9, ((uint32_t)&__EXCEPTION_9_Prolog) & 0x0000fff0);
 235     wrspr(IVOR10, ((uint32_t)&__EXCEPTION_10_Prolog) & 0x0000fff0);
 236     wrspr(IVOR11, ((uint32_t)&__EXCEPTION_11_Prolog) & 0x0000fff0);
 237     wrspr(IVOR12, ((uint32_t)&__EXCEPTION_12_Prolog) & 0x0000fff0);
 238     wrspr(IVOR13, ((uint32_t)&__EXCEPTION_13_Prolog) & 0x0000fff0);
 239     wrspr(IVOR14, ((uint32_t)&__EXCEPTION_14_Prolog) & 0x0000fff0);
 240     wrspr(IVOR15, ((uint32_t)&__EXCEPTION_15_Prolog) & 0x0000fff0);
 241
 242     /* Disable external interrupts completely */
 243     wrdcr(UIC0_ER, 0);
 244     wrdcr(UIC1_ER, 0);
 245 }
 246
 247 #define _STR(x) #x
 248 #define STR(x) _STR(x)
 249 #define PUT_INTR_TEMPLATE(num, handler) \
 250     asm volatile(".section .aros.init,\"ax\"\n\t.align 5\n\t.globl __EXCEPTION_" STR(num) "_Prolog\n\t.type __EXCEPTION_" STR(num) "_Prolog,@function\n"    \
 251         "__EXCEPTION_" STR(num) "_Prolog: \n\t"                                                       \
 252                  "mtsprg1 %%r3          \n\t"   /* save %r3 */                                                  \
 253                  "mfcr %%r3             \n\t"   /* copy CR to %r3 */                                            \
 254                  "mtsprg2 %%r3          \n\t"   /* save %r3 */                                                  \
 255                                                                                                                 \
 256                  "mfsrr1 %%r3           \n\t"   /* srr1 (previous MSR) reg into %r3 */                          \
 257                  "andi. %%r3,%%r3,%0    \n\t"   /* Was the PR bit set in MSR already? */                        \
 258                  "beq- 1f               \n\t"   /* No, we were in supervisor mode */                           \
 259                                                                                                                 \
 260                  "mfsprg0 %%r3          \n\t"   /* user mode case: SSP into %r3 */                              \
 261                  "b 2f                  \n"                                                                     \
 262         "1:       mr %%r3,%%r1          \n\t"   /* Supervisor case: use current stack */                        \
 263         "2:       addi %%r3,%%r3,%1     "                                                                       \
 264                  ::"i"(MSR_PR),"i"(-sizeof(regs_t))); \
 265         asm volatile("stw %%r0, %[gpr0](%%r3)  \n\t" \
 266                   "stw %%r1, %[gpr1](%%r3)  \n\t" \
 267                   "stw %%r2, %[gpr2](%%r3)  \n\t" \
 268                   "mfsprg1 %%r0             \n\t" \
 269                   "stw %%r4, %[gpr4](%%r3)  \n\t" \
 270                   "stw %%r0, %[gpr3](%%r3)  \n\t" \
 271                   "stw %%r5, %[gpr5](%%r3)  \n\t" \
 272                   "mfsprg2 %%r2             \n\t" \
 273                   "mfsrr0 %%r0              \n\t" \
 274                   "mfsrr1 %%r1              \n\t" \
 275                   "lis %%r5, " #handler "@ha\n\t" \
 276                   "la %%r5, " #handler "@l(%%r5)\n\t" \
 277                   "li %%r4, %[irq]          \n\t" \
 278                   "stw %%r2,%[ccr](%%r3)    \n\t" \
 279                   "stw %%r0,%[srr0](%%r3)   \n\t" \
 280                   "stw %%r1,%[srr1](%%r3)   \n\t" \
 281                   "mfctr %%r0               \n\t" \
 282                   "mflr %%r1                \n\t" \
 283                   "mfxer %%r2               \n\t" \
 284                   "stw %%r0,%[ctr](%%r3)    \n\t" \
 285                   "stw %%r1,%[lr](%%r3)     \n\t" \
 286                   "stw %%r2,%[xer](%%r3)    \n\t" \
 287                   \
 288                   :: \
 289                   [gpr0]"i"(offsetof(regs_t, gpr[0])), \
 290                   [gpr1]"i"(offsetof(regs_t, gpr[1])), \
 291                   [gpr2]"i"(offsetof(regs_t, gpr[2])), \
 292                   [gpr3]"i"(offsetof(regs_t, gpr[3])), \
 293                   [gpr4]"i"(offsetof(regs_t, gpr[4])), \
 294                   [gpr5]"i"(offsetof(regs_t, gpr[5])), \
 295                   \
 296                   [ccr]"i"(offsetof(regs_t, ccr)), \
 297                   [srr0]"i"(offsetof(regs_t, srr0)), \
 298                   [srr1]"i"(offsetof(regs_t, srr1)), \
 299                   [ctr]"i"(offsetof(regs_t, ctr)), \
 300                   [lr]"i"(offsetof(regs_t, lr)), \
 301                   [xer]"i"(offsetof(regs_t, xer)), \
 302                   [irq]"i"(num) \
 303                   ); \
 304                  /* \
 305                   * Registers %r0 to %r5 are now saved together with CPU state. Go to the \
 306                   * trampoline code which will care about the rest. Adjust the stack frame pointer now, \
 307                   * or else it will be destroyed later by C code. \
 308                   */ \
 309                  asm volatile("addi %r1,%r3,-16"); \
 310                  \
 311                  /* \
 312                   * Go to the trampoline code. Use long call within whole 4GB addresspace in order to \
 313                   * avoid any trouble in future. \
 314                   */ \
 315                  asm volatile( "b __EXCEPTION_Trampoline\n\t");
 316
 317 void __attribute__((noreturn)) decrementer_handler(regs_t *ctx, uint8_t exception, void *self)
 318 {
 319     struct KernelBase *KernelBase = getKernelBase();
 320
 321     /* Clear the DIS bit - we have received decrementer exception */
 322     wrspr(TSR, TSR_DIS);
 323 //    D(bug("[KRN] Decrementer handler. Context @ %p. srr1=%08x\n", ctx, ctx->srr1));
 324
 325     if (!IsListEmpty(&KernelBase->kb_Exceptions[exception]))
 326     {
 327         struct IntrNode *in, *in2;
 328
 329         ForeachNodeSafe(&KernelBase->kb_Exceptions[exception], in, in2)
 330         {
 331             if (in->in_Handler)
 332                 in->in_Handler(in->in_HandlerData, in->in_HandlerData2);
 333         }
 334     }
 335
 336     core_ExitInterrupt(ctx);
 337 }
 338
 339
 340 void __attribute__((noreturn)) generic_handler(regs_t *ctx, uint8_t exception, void *self)
 341 {
 342     struct KernelBase *KernelBase = getKernelBase();
 343     struct ExecBase *SysBase = getSysBase();
 344
 345     if (!IsListEmpty(&KernelBase->kb_Exceptions[exception]))
 346     {
 347         struct IntrNode *in, *in2;
 348
 349         ForeachNodeSafe(&KernelBase->kb_Exceptions[exception], in, in2)
 350         {
 351             if (in->in_Handler)
 352                 in->in_Handler(in->in_HandlerData, in->in_HandlerData2);
 353         }
 354     }
 355
 356     D(bug("[KRN] Exception %d handler. Context @ %p, SysBase @ %p, KernelBase @ %p\n", exception, ctx, SysBase, KernelBase));
 357     if (SysBase)
 358     {
 359         struct Task *t = FindTask(NULL);
 360         D(bug("[KRN] %s %p (%s)\n", t->tc_Node.ln_Type == NT_TASK ? "Task":"Process", t, t->tc_Node.ln_Name ? t->tc_Node.ln_Name : "--unknown--"));
 361     }
 362     D(bug("[KRN] SRR0=%08x, SRR1=%08x DEAR=%08x ESR=%08x\n",ctx->srr0, ctx->srr1, rdspr(DEAR), rdspr(ESR)));
 363     D(bug("[KRN] CTR=%08x LR=%08x XER=%08x CCR=%08x\n", ctx->ctr, ctx->lr, ctx->xer, ctx->ccr));
 364     D(bug("[KRN] DAR=%08x DSISR=%08x\n", ctx->dar, ctx->dsisr));
 365     D(bug("[KRN] GPR00=%08x GPR01=%08x GPR02=%08x GPR03=%08x\n",
 366              ctx->gpr[0],ctx->gpr[1],ctx->gpr[2],ctx->gpr[3]));
 367     D(bug("[KRN] GPR04=%08x GPR05=%08x GPR06=%08x GPR07=%08x\n",
 368              ctx->gpr[4],ctx->gpr[5],ctx->gpr[6],ctx->gpr[7]));
 369     D(bug("[KRN] GPR08=%08x GPR09=%08x GPR10=%08x GPR11=%08x\n",
 370              ctx->gpr[8],ctx->gpr[9],ctx->gpr[10],ctx->gpr[11]));
 371     D(bug("[KRN] GPR12=%08x GPR13=%08x GPR14=%08x GPR15=%08x\n",
 372              ctx->gpr[12],ctx->gpr[13],ctx->gpr[14],ctx->gpr[15]));
 373
 374     D(bug("[KRN] GPR16=%08x GPR17=%08x GPR18=%08x GPR19=%08x\n",
 375              ctx->gpr[16],ctx->gpr[17],ctx->gpr[18],ctx->gpr[19]));
 376     D(bug("[KRN] GPR20=%08x GPR21=%08x GPR22=%08x GPR23=%08x\n",
 377              ctx->gpr[20],ctx->gpr[21],ctx->gpr[22],ctx->gpr[23]));
 378     D(bug("[KRN] GPR24=%08x GPR25=%08x GPR26=%08x GPR27=%08x\n",
 379              ctx->gpr[24],ctx->gpr[25],ctx->gpr[26],ctx->gpr[27]));
 380     D(bug("[KRN] GPR28=%08x GPR29=%08x GPR30=%08x GPR31=%08x\n",
 381              ctx->gpr[28],ctx->gpr[29],ctx->gpr[30],ctx->gpr[31]));
 382
 383     D(bug("[KRN] Instruction dump:\n"));
 384     int i;
 385     ULONG *p = (ULONG*)ctx->srr0;
 386     for (i=0; i < 8; i++)
 387     {
 388         D(bug("[KRN] %08x: %08x\n", &p[i], p[i]));
 389     }
 390
 391     if (IsListEmpty(&KernelBase->kb_Exceptions[exception]))
 392     {
 393         D(bug("[KRN] **UNHANDLED EXCEPTION** stopping here...\n"));
 394
 395         while(1) {
 396             wrmsr(rdmsr() | MSR_POW);
 397         }
 398     }
 399
 400     core_ExitInterrupt(ctx);
 401 }
 402
 403 void __attribute__((noreturn)) mmu_handler(regs_t *ctx, uint8_t exception, void *self)
 404 {
 405     struct KernelBase *KernelBase = getKernelBase();
 406
 407     uint32_t insn = *(uint32_t *)ctx->srr0;
 408
 409     /* SysBase access at 4UL? Occurs only with lwz instruction and DEAR=4 */
 410     if ((insn & 0xfc000000) == 0x80000000 && rdspr(DEAR) == 4)
 411     {
 412         int reg = (insn & 0x03e00000) >> 21;
 413
 414 //        D(bug("[KRN] Pagefault exception. Someone tries to get SysBase (%08x) from 0x00000004 into r%d. EVIL EVIL EVIL!\n",
 415 //              getSysBase(), reg));
 416
 417         ctx->gpr[reg] = getSysBase();
 418         ctx->srr0 += 4;
 419
 420         core_LeaveInterrupt(ctx);
 421     }
 422     else
 423         generic_handler(ctx, exception, self);
 424 }
 425
 426
 427
 428 static void __attribute__((used)) __EXCEPTION_Prolog_template()
 429 {
 430
 431     /*
 432      * Create partial context on the stack. It is impossible to save it fully since the
 433      * exception handlers have limited size. This code will do as much as possible and then
 434      * jump to general trampoline...
 435      */
 436
 437     PUT_INTR_TEMPLATE(0, generic_handler); /* crit */
 438     PUT_INTR_TEMPLATE(1, generic_handler);
 439     PUT_INTR_TEMPLATE(2, generic_handler);
 440     PUT_INTR_TEMPLATE(3, generic_handler);
 441     PUT_INTR_TEMPLATE(4, uic_handler);
 442     PUT_INTR_TEMPLATE(5, generic_handler);
 443     PUT_INTR_TEMPLATE(6, generic_handler);
 444     PUT_INTR_TEMPLATE(7, generic_handler);
 445     PUT_INTR_TEMPLATE(8, syscall_handler);
 446     PUT_INTR_TEMPLATE(9, generic_handler);
 447     PUT_INTR_TEMPLATE(10, decrementer_handler);
 448     PUT_INTR_TEMPLATE(11, generic_handler);
 449     PUT_INTR_TEMPLATE(12, generic_handler); /* crit */
 450     PUT_INTR_TEMPLATE(13, mmu_handler);
 451     PUT_INTR_TEMPLATE(14, generic_handler);
 452     PUT_INTR_TEMPLATE(15, generic_handler); /* crit */
 453 }
 454
 455 static void __attribute__((used)) __EXCEPTION_Trampoline_template()
 456 {
 457     asm volatile(".section .aros.init,\"ax\"\n\t.align 5\n\t.globl __EXCEPTION_Trampoline\n\t.type __EXCEPTION_Trampoline,@function\n"
 458         "__EXCEPTION_Trampoline:            \n\t"
 459                  "stw %%r6,%[gpr6](%%r3) \n\t"
 460                  "stw %%r7,%[gpr7](%%r3) \n\t"
 461                  "stw %%r8,%[gpr8](%%r3) \n\t"
 462                  "stw %%r9,%[gpr9](%%r3) \n\t"
 463                  "stw %%r10,%[gpr10](%%r3) \n\t"
 464                  "stw %%r11,%[gpr11](%%r3) \n\t"
 465                  "stw %%r12,%[gpr12](%%r3) \n\t"
 466                  "stw %%r13,%[gpr13](%%r3) \n\t"
 467                  "stw %%r14,%[gpr14](%%r3) \n\t"
 468                  "stw %%r15,%[gpr15](%%r3) \n\t"
 469                  "stw %%r16,%[gpr16](%%r3) \n\t"
 470                  "stw %%r17,%[gpr17](%%r3) \n\t"
 471                  "stw %%r18,%[gpr18](%%r3) \n\t"
 472                  "stw %%r19,%[gpr19](%%r3) \n\t"
 473                  "stw %%r20,%[gpr20](%%r3) \n\t"
 474                  "stw %%r21,%[gpr21](%%r3) \n\t"
 475                  "stw %%r22,%[gpr22](%%r3) \n\t"
 476                  "stw %%r23,%[gpr23](%%r3) \n\t"
 477                  "stw %%r24,%[gpr24](%%r3) \n\t"
 478                  "stw %%r25,%[gpr25](%%r3) \n\t"
 479                  "stw %%r26,%[gpr26](%%r3) \n\t"
 480                  "stw %%r27,%[gpr27](%%r3) \n\t"
 481                  "stw %%r28,%[gpr28](%%r3) \n\t"
 482                  "stw %%r29,%[gpr29](%%r3) \n\t"
 483                  "stw %%r30,%[gpr30](%%r3) \n\t"
 484                  "stw %%r31,%[gpr31](%%r3) \n\t"
 485                  "mr %%r28,%%r3            \n\t"
 486                  "mr %%r29,%%r4            \n\t"
 487                  "mr %%r30,%%r5            \n\t"
 488                  "mtsrr0 %%r5           \n\t"
 489                  "lis %%r9, %[msrval]@ha  \n\t"
 490                  "ori %%r9,%%r9, %[msrval]@l \n\t"
 491                  "mtsrr1 %%r9              \n\t"
 492                  "sync; isync; rfi"
 493                  ::
 494                  [gpr6]"i"(offsetof(regs_t, gpr[6])),
 495                  [gpr7]"i"(offsetof(regs_t, gpr[7])),
 496                  [gpr8]"i"(offsetof(regs_t, gpr[8])),
 497                  [gpr9]"i"(offsetof(regs_t, gpr[9])),
 498                  [gpr10]"i"(offsetof(regs_t, gpr[10])),
 499                  [gpr11]"i"(offsetof(regs_t, gpr[11])),
 500                  [gpr12]"i"(offsetof(regs_t, gpr[12])),
 501                  [gpr13]"i"(offsetof(regs_t, gpr[13])),
 502                  [gpr14]"i"(offsetof(regs_t, gpr[14])),
 503                  [gpr15]"i"(offsetof(regs_t, gpr[15])),
 504                  [gpr16]"i"(offsetof(regs_t, gpr[16])),
 505                  [gpr17]"i"(offsetof(regs_t, gpr[17])),
 506                  [gpr18]"i"(offsetof(regs_t, gpr[18])),
 507                  [gpr19]"i"(offsetof(regs_t, gpr[19])),
 508                  [gpr20]"i"(offsetof(regs_t, gpr[20])),
 509                  [gpr21]"i"(offsetof(regs_t, gpr[21])),
 510                  [gpr22]"i"(offsetof(regs_t, gpr[22])),
 511                  [gpr23]"i"(offsetof(regs_t, gpr[23])),
 512                  [gpr24]"i"(offsetof(regs_t, gpr[24])),
 513                  [gpr25]"i"(offsetof(regs_t, gpr[25])),
 514                  [gpr26]"i"(offsetof(regs_t, gpr[26])),
 515                  [gpr27]"i"(offsetof(regs_t, gpr[27])),
 516                  [gpr28]"i"(offsetof(regs_t, gpr[28])),
 517                  [gpr29]"i"(offsetof(regs_t, gpr[29])),
 518                  [gpr30]"i"(offsetof(regs_t, gpr[30])),
 519                  [gpr31]"i"(offsetof(regs_t, gpr[31])),
 520                  [msrval]"i"(MSR_ME|MSR_CE|MSR_FP)
 521     );
 522 }
 523
 524
 525 static void __attribute__((used)) __core_LeaveInterrupt()
 526 {
 527     asm volatile(".section .aros.init,\"ax\"\n\t.align 5\n\t.globl core_LeaveInterrupt\n\t.type core_LeaveInterrupt,@function\n"
 528         "core_LeaveInterrupt:            \n\t"
 529                  "lwz %%r31,%[gpr31](%%r3)      \n\t"
 530                  "lwz %%r30,%[gpr30](%%r3)      \n\t"
 531                  "lwz %%r29,%[gpr29](%%r3)      \n\t"
 532                  "lwz %%r28,%[gpr28](%%r3)      \n\t"
 533                  "lwz %%r27,%[gpr27](%%r3)      \n\t"
 534                  "lwz %%r26,%[gpr26](%%r3)      \n\t"
 535                  "lwz %%r25,%[gpr25](%%r3)      \n\t"
 536                  "lwz %%r24,%[gpr24](%%r3)      \n\t"
 537                  "lwz %%r23,%[gpr23](%%r3)      \n\t"
 538                  "lwz %%r22,%[gpr22](%%r3)      \n\t"
 539                  "lwz %%r21,%[gpr21](%%r3)      \n\t"
 540                  "lwz %%r20,%[gpr20](%%r3)      \n\t"
 541                  "lwz %%r19,%[gpr19](%%r3)      \n\t"
 542                  "lwz %%r18,%[gpr18](%%r3)      \n\t"
 543                  "lwz %%r17,%[gpr17](%%r3)      \n\t"
 544                  "lwz %%r16,%[gpr16](%%r3)      \n\t"
 545                  "lwz %%r15,%[gpr15](%%r3)      \n\t"
 546                  "lwz %%r14,%[gpr14](%%r3)      \n\t"
 547                  "lwz %%r13,%[gpr13](%%r3)      \n\t"
 548                  "lwz %%r12,%[gpr12](%%r3)      \n\t"
 549         ::
 550         [gpr12]"i"(offsetof(regs_t, gpr[12])),
 551         [gpr13]"i"(offsetof(regs_t, gpr[13])),
 552         [gpr14]"i"(offsetof(regs_t, gpr[14])),
 553         [gpr15]"i"(offsetof(regs_t, gpr[15])),
 554         [gpr16]"i"(offsetof(regs_t, gpr[16])),
 555         [gpr17]"i"(offsetof(regs_t, gpr[17])),
 556         [gpr18]"i"(offsetof(regs_t, gpr[18])),
 557         [gpr19]"i"(offsetof(regs_t, gpr[19])),
 558         [gpr20]"i"(offsetof(regs_t, gpr[20])),
 559         [gpr21]"i"(offsetof(regs_t, gpr[21])),
 560         [gpr22]"i"(offsetof(regs_t, gpr[22])),
 561         [gpr23]"i"(offsetof(regs_t, gpr[23])),
 562         [gpr24]"i"(offsetof(regs_t, gpr[24])),
 563         [gpr25]"i"(offsetof(regs_t, gpr[25])),
 564         [gpr26]"i"(offsetof(regs_t, gpr[26])),
 565         [gpr27]"i"(offsetof(regs_t, gpr[27])),
 566         [gpr28]"i"(offsetof(regs_t, gpr[28])),
 567         [gpr29]"i"(offsetof(regs_t, gpr[29])),
 568         [gpr30]"i"(offsetof(regs_t, gpr[30])),
 569         [gpr31]"i"(offsetof(regs_t, gpr[31]))
 570         );
 571
 572     asm volatile(
 573                  "lwz %%r11,%[gpr11](%%r3)      \n\t"
 574                  "lwz %%r0,%[srr0](%%r3)        \n\t"
 575                  "mtsrr0 %%r0                   \n\t"
 576                  "lwz %%r0,%[srr1](%%r3)        \n\t"
 577                  //"rlwinm %%r0,%%r0,0,14,12      \n\t"
 578                  "mtsrr1 %%r0                   \n\t"
 579                  "lwz %%r0,%[ctr](%%r3)         \n\t"
 580                  "mtctr %%r0                    \n\t"
 581                  "lwz %%r0,%[lr](%%r3)          \n\t"
 582                  "mtlr %%r0                     \n\t"
 583                  "lwz %%r0,%[xer](%%r3)         \n\t"
 584                  "mtxer %%r0                    \n\t"
 585                  "lwz %%r10,%[gpr10](%%r3)      \n\t"
 586                  "lwz %%r9,%[gpr9](%%r3)        \n\t"
 587                  "lwz %%r8,%[gpr8](%%r3)        \n\t"
 588                  "lwz %%r7,%[gpr7](%%r3)        \n\t"
 589                  "lwz %%r6,%[gpr6](%%r3)        \n\t"
 590                  "lwz %%r5,%[gpr5](%%r3)        \n\t"
 591                  "lwz %%r4,%[gpr4](%%r3)        \n\t"
 592                  "lwz %%r0,%[gpr3](%%r3)        \n\t"
 593                  "mtsprg1 %%r0                  \n\t"
 594                  "lwz %%r2,%[gpr2](%%r3)        \n\t"
 595                  "stwcx. %%r0,0,%%r1            \n\t"
 596                  "lwz %%r0,%[ccr](%%r3)         \n\t"
 597                  "mtcr %%r0                     \n\t"
 598                  "lwz %%r1,%[gpr1](%%r3)        \n\t"
 599                  "lwz %%r0,%[gpr0](%%r3)        \n\t"
 600                  "mfsprg1 %%r3                  \n\t"
 601                  "sync; isync; rfi"
 602         ::
 603         [ccr]"i"(offsetof(regs_t, ccr)),        /* */
 604         [srr0]"i"(offsetof(regs_t, srr0)),      /* */
 605         [srr1]"i"(offsetof(regs_t, srr1)),/* */
 606         [ctr]"i"(offsetof(regs_t, ctr)),/**/
 607         [lr]"i"(offsetof(regs_t, lr)),/**/
 608         [xer]"i"(offsetof(regs_t, xer)),
 609         [gpr0]"i"(offsetof(regs_t, gpr[0])),
 610         [gpr1]"i"(offsetof(regs_t, gpr[1])),
 611         [gpr2]"i"(offsetof(regs_t, gpr[2])),
 612         [gpr3]"i"(offsetof(regs_t, gpr[3])),
 613         [gpr4]"i"(offsetof(regs_t, gpr[4])),
 614         [gpr5]"i"(offsetof(regs_t, gpr[5])),
 615         [gpr6]"i"(offsetof(regs_t, gpr[6])),
 616         [gpr7]"i"(offsetof(regs_t, gpr[7])),
 617         [gpr8]"i"(offsetof(regs_t, gpr[8])),
 618         [gpr9]"i"(offsetof(regs_t, gpr[9])),
 619         [gpr10]"i"(offsetof(regs_t, gpr[10])),
 620         [gpr11]"i"(offsetof(regs_t, gpr[11]))
 621     );
 622 }