arch/m68k-mac/exec/sys_dispatch.c

   1 /*
   2     Copyright © 1995-2011, The AROS Development Team. All rights reserved.
   3     $Id$
   4
   5     Desc: The task Dispatcher for m68k; taken from i386-native version
   6     Lang: english
   7 */
   8
   9 #include <exec/execbase.h>
  10 #include <aros/asmcall.h>
  11 #include <exec/interrupts.h>
  12 #include <proto/exec.h>
  13 #include <exec_intern.h>
  14 #include <exec/ptrace.h>
  15
  16 void SaveRegs(struct Task *task, struct pt_regs *regs)
  17 {
  18         /* Copy registers from struct pt_regs into the user stack */
  19         struct pt_regs * dest = (struct pt_regs *)task->tc_UnionETask.tc_ETask->et_RegFrame;
  20         /*
  21          * Use this rather than memcpy! It does NOT work otherwise
  22          */
  23 #if 1
  24         dest->usp = regs->usp;
  25         dest->d0  = regs->d0;
  26         dest->d1  = regs->d1;
  27         dest->d2  = regs->d2;
  28         dest->d3  = regs->d3;
  29         dest->d4  = regs->d4;
  30         dest->d5  = regs->d5;
  31         dest->d6  = regs->d6;
  32         dest->d7  = regs->d7;
  33         dest->a0  = regs->a0;
  34         dest->a1  = regs->a1;
  35         dest->a2  = regs->a2;
  36         dest->a3  = regs->a3;
  37         dest->a4  = regs->a4;
  38         dest->a5  = regs->a5;
  39         dest->a6  = regs->a6;
  40         dest->sr  = regs->sr;
  41         dest->pc  = regs->pc;
  42 #else
  43         ULONG i = 0;
  44         while (i < sizeof(struct pt_regs)) {
  45                 ((UBYTE *)dest)[i] = ((UBYTE *)regs)[i];
  46                 i++;
  47         }
  48 #endif
  49         task->tc_SPReg = (APTR)regs->usp;
  50 }
  51
  52 void RestoreRegs(struct Task *task, struct pt_regs *regs)
  53 {
  54         /* Copy registers from the task's stack into struct pt_regs */
  55         struct pt_regs * src = (struct pt_regs *)task->tc_UnionETask.tc_ETask->et_RegFrame;
  56
  57         /*
  58          * Use this rather than memcpy! It does NOT work otherwise
  59          */
  60 #if 1
  61         regs->usp = src->usp;
  62         regs->d0  = src->d0;
  63         regs->d1  = src->d1;
  64         regs->d2  = src->d2;
  65         regs->d3  = src->d3;
  66         regs->d4  = src->d4;
  67         regs->d5  = src->d5;
  68         regs->d6  = src->d6;
  69         regs->d7  = src->d7;
  70         regs->a0  = src->a0;
  71         regs->a1  = src->a1;
  72         regs->a2  = src->a2;
  73         regs->a3  = src->a3;
  74         regs->a4  = src->a4;
  75         regs->a5  = src->a5;
  76         regs->a6  = src->a6;
  77         regs->sr  = src->sr;
  78         regs->pc  = src->pc;
  79 #else
  80         ULONG i = 0;
  81         while (i < sizeof(struct pt_regs)) {
  82                 ((UBYTE *)regs)[i] = ((UBYTE *)src)[i];
  83                 i++;
  84         }
  85 #endif
  86
  87         task->tc_SPReg = (APTR)regs->usp;
  88 }
  89
  90 #define SC_ENABLE(regs)  (regs->sr &= 0xf8ff)
  91 #define SC_DISABLE(regs) (regs->sr |= 0x0700)
  92
  93 void sys_Dispatch(struct pt_regs * regs)
  94 {
  95         struct ExecBase * SysBase = (struct ExecBase *)*(ULONG *)0x04;
  96
  97         /* Hmm, interrupts are nesting, not a good idea... */
  98         if(!user_mode(regs)) {
  99                 return;
 100         }
 101
 102         /* Check if a task switch is necessary */
 103         /* 1. There has to be another task in the ready-list */
 104         /* 2. The first task in the ready list hast to have the
 105               same or higher priority than the currently active task */
 106
 107         if( SysBase->TaskReady.lh_Head->ln_Succ != NULL  /* &&
 108            ((BYTE)SysBase->ThisTask->tc_Node.ln_Pri <=
 109             (BYTE)((struct Task *)SysBase->TaskReady.lh_Head)->tc_Node.ln_Pri )*/
 110            )
 111         {
 112                 /* Check if task switch is possible */
 113                 if( SysBase->TDNestCnt < 0 )
 114                 {
 115                         if( SysBase->ThisTask->tc_State == TS_RUN )
 116                         {
 117                                 SysBase->ThisTask->tc_State = TS_READY;
 118                                 Reschedule(SysBase->ThisTask);
 119                                 SysBase->AttnResched |= 0x8000;
 120                         }
 121                         else if( SysBase->ThisTask->tc_State == TS_REMOVED )
 122                                 SysBase->AttnResched |= 0x8000;
 123                 }
 124                 else
 125                         SysBase->AttnResched |= 0x80;
 126         }
 127
 128
 129         /* Has an interrupt told us to dispatch when leaving */
 130         if (SysBase->AttnResched & 0x8000)
 131         {
 132                 SysBase->AttnResched &= ~0x8000;
 133
 134                 /* Save registers for this task (if there is one...) */
 135                 if (SysBase->ThisTask && SysBase->ThisTask->tc_State != TS_REMOVED)
 136                         SaveRegs(SysBase->ThisTask, regs);
 137
 138                 /* Tell exec that we have actually switched tasks... */
 139                 Dispatch ();
 140
 141                 /* Get the registers of the old task */
 142                 RestoreRegs(SysBase->ThisTask, regs);
 143                 /* Make sure that the state of the interrupts is what the task
 144                    expects.
 145                 */
 146                 if (SysBase->IDNestCnt < 0)
 147                         SC_ENABLE(regs);
 148                 else
 149                         SC_DISABLE(regs);
 150                 /* Ok, the next step is to either drop back to the new task, or
 151                    give it its Exception() if it wants one... */
 152
 153                 if (SysBase->ThisTask->tc_Flags & TF_EXCEPT)
 154                 {
 155                         Disable();
 156                         Exception();
 157                         Enable();
 158                 }
 159
 160
 161         }
 162
 163         /* Leave the interrupt. */
 164 }