arch/m68k-all/kernel/kernel_cpu.c

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