arch/m68k-all/kernel/kernel_cpu.c

   1 /*
   2     Copyright © 1995-2014, The AROS Development Team. All rights reserved.
   3     $Id$
   4 */
   5
   6 /*
   7  * M68K Schedule functions
   8  */
   9
  10 #include <exec/execbase.h>
  11 #include <exec/alerts.h>
  12 #include <proto/exec.h>
  13 #include <defines/kernel.h>
  14
  15 #include <kernel_base.h>
  16 #include <kernel_debug.h>
  17 #include <kernel_scheduler.h>
  18
  19 #ifndef D
  20 # ifdef DEBUG
  21 #  define D(x) x
  22 # else
  23 #  define D(x)
  24 # endif
  25 #endif
  26
  27 extern void cpu_Exception(void);
  28 asm (
  29         "       .text\n"
  30         "       .align 4\n"
  31         "       .globl cpu_Exception\n"
  32         "cpu_Exception:\n"
  33         "       movem.l %d0-%d1/%a0-%a1/%a6,%sp@-\n"
  34         "       move.l  (4),%a6\n"
  35         "       jsr     %a6@(-1 * 6 * 11 /* Exception */)\n"
  36         "       movem.l %sp@+,%d0-%d1/%a0-%a1/%a6\n"
  37         "       rts\n"
  38 );
  39
  40 void cpu_Switch(regs_t *regs)
  41 {
  42     struct Task *task = SysBase->ThisTask;
  43     struct AROSCPUContext *ctx = task->tc_UnionETask.tc_ETask->et_RegFrame;
  44
  45     /* Actually save the context */
  46     CopyMem(regs, &ctx->cpu, sizeof(regs_t));
  47
  48     /* If we have an FPU, save the FPU context */
  49     if (SysBase->AttnFlags & AFF_FPU)
  50             AROS_UFC2NR(void, FpuSaveContext,
  51                             AROS_UFCA(struct FpuContext *, &ctx->fpu, A0),
  52                             AROS_UFCA(UWORD, (SysBase->AttnFlags & AFF_68060) ? 2 : 0, D0));
  53
  54     /* Update tc_SPReg */
  55     task->tc_SPReg = (APTR)regs->a[7];
  56
  57     core_Switch();
  58 }
  59
  60 void cpu_Dispatch(regs_t *regs)
  61 {
  62     struct Task *task;
  63     struct AROSCPUContext *ctx;
  64
  65     for (;;) {
  66         asm volatile ("ori  #0x0700, %sr\n");    // Disable CPU interrupts
  67
  68         task = core_Dispatch();
  69         if (task != NULL)
  70             break;
  71         D(bug("-- IDLE HALT --\n"));
  72
  73         /* Break IDNestCnt */
  74         if (SysBase->IDNestCnt >= 0) {
  75             SysBase->IDNestCnt=-1;
  76             asm volatile ("move.w #0xc000,0xdff09a\n");
  77         }
  78         asm volatile ("stop #0x2000\n"); // Wait for an interrupt
  79     }
  80
  81     ctx = task->tc_UnionETask.tc_ETask->et_RegFrame;
  82     CopyMem(&ctx->cpu, regs, sizeof(regs_t));
  83     regs->a[7] = (IPTR)task->tc_SPReg;
  84
  85     /* If we have an FPU, restore the FPU context */
  86     if (SysBase->AttnFlags & AFF_FPU)
  87             AROS_UFC2NR(void, FpuRestoreContext,
  88                             AROS_UFCA(struct FpuContext *, &ctx->fpu, A0),
  89                             AROS_UFCA(UWORD, (SysBase->AttnFlags & AFF_68060) ? 2 : 0, D0));
  90
  91     /* Re-enable interrupts if needed */
  92     if (SysBase->IDNestCnt < 0) {
  93         asm volatile ("move.w #0xc000,0xdff09a\n");
  94     } else {
  95         asm volatile ("move.w #0x4000,0xdff09a\n");
  96     }
  97
  98     if (task->tc_Flags & TF_EXCEPT) {
  99         /* Exec_Exception() will Enable() */
 100         Disable();
 101
 102         /* Manipulate the current CPU context so Exec_Exception gets
 103          * executed after we leave Supervisor mode.
 104          */
 105         task->tc_SPReg -= sizeof(ULONG);        /* RTS to original PC */
 106         if (task->tc_SPReg <= task->tc_SPLower)
 107                 Alert(AT_DeadEnd|AN_StackProbe);
 108         *(ULONG *)(task->tc_SPReg) = regs->pc;
 109
 110         regs->a[7] = (IPTR)task->tc_SPReg;
 111         regs->pc   = (IPTR)cpu_Exception;
 112     }
 113 }