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