arch/.unmaintained/arm-all/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 i386native version of Cause().
   6     Lang: english
   7 */
   8
   9 #include <exec/execbase.h>
  10 #include <exec/tasks.h>
  11 #include <aros/asmcall.h>
  12 #include <exec/interrupts.h>
  13 #include <proto/exec.h>
  14 #include <exec_intern.h>
  15 #include <exec/ptrace.h>
  16 #include "etask.h"
  17
  18 #undef DEBUG
  19 #define DEBUG 1
  20 #include <aros/debug.h>
  21
  22 void SaveRegs(struct Task *task, struct pt_regs *regs)
  23 {
  24         /* Copy registers from struct pt_regs into the user stack */
  25         struct pt_regs * dest = (struct pt_regs *)task->tc_UnionETask.tc_ETask->et_RegFrame;
  26         /*
  27          * Use this rather than memcpy! It does NOT work otherwise
  28          */
  29 #if 1
  30         dest->r0     = regs->r0;
  31         dest->r1     = regs->r1;
  32         dest->r2     = regs->r2;
  33         dest->r3     = regs->r3;
  34         dest->r4     = regs->r4;
  35         dest->r5     = regs->r5;
  36         dest->r6     = regs->r6;
  37         dest->r7     = regs->r7;
  38         dest->r8     = regs->r8;
  39         dest->r9     = regs->r9;
  40         dest->r10    = regs->r10;
  41         dest->r11    = regs->r11;
  42         dest->r12    = regs->r12;
  43         dest->sp     = regs->sp;
  44         dest->lr     = regs->lr;
  45         dest->lr_svc = regs->lr_svc;
  46         dest->cpsr   = regs->cpsr;
  47 #else
  48         ULONG i = 0;
  49         while (i < sizeof(struct pt_regs)) {
  50                 ((UBYTE *)dest)[i] = ((UBYTE *)regs)[i];
  51                 i++;
  52         }
  53 #endif
  54         task->tc_SPReg = (APTR)regs->sp;
  55 }
  56
  57 void RestoreRegs(struct Task *task, struct pt_regs *regs)
  58 {
  59         /* Copy registers from the task's stack into struct pt_regs */
  60         struct pt_regs * src = (struct pt_regs *task->tc_UnionETask.tc_ETask->et_RegFrame;
  61
  62         /*
  63          * Use this rather than memcpy! It does NOT work otherwise
  64          */
  65 #if 1
  66         regs->r0     = src->r0;
  67         regs->r1     = src->r1;
  68         regs->r2     = src->r2;
  69         regs->r3     = src->r3;
  70         regs->r4     = src->r4;
  71         regs->r5     = src->r5;
  72         regs->r6     = src->r6;
  73         regs->r7     = src->r7;
  74         regs->r8     = src->r8;
  75         regs->r9     = src->r9;
  76         regs->r10    = src->r10;
  77         regs->r11    = src->r11;
  78         regs->r12    = src->r12;
  79         regs->sp     = src->sp;
  80         regs->lr     = src->lr;
  81         regs->lr_svc = src->lr_svc;
  82         regs->cpsr   = src->cpsr;
  83 #else
  84         ULONG i = 0;
  85         while (i < sizeof(struct pt_regs)) {
  86                 ((UBYTE *)regs)[i] = ((UBYTE *)src)[i];
  87                 i++;
  88         }
  89 #endif
  90
  91         task->tc_SPReg = (APTR)regs->sp;
  92 }
  93
  94 #define SC_ENABLE(regs)  (regs->cpsr &= 0xffffff7f)
  95 #define SC_DISABLE(regs) (regs->cpsr |= 0x000000C0)
  96
  97 void sys_Dispatch(struct pt_regs * regs, LONG adjust)
  98 {
  99         struct ExecBase * SysBase = (struct ExecBase *)*(ULONG *)0x04;
 100
 101         /* Hmm, interrupts are nesting, not a good idea... */
 102 //      if(user_mode(regs)) {
 103 //              return;
 104 //      }
 105 #if 0
 106         D(bug("In %s!!!\n",__FUNCTION__));
 107         D(bug("lr_svc=%x, r0=%x, r1=%x, r2=%x, r9=%x, r12=%x, sp=%x, lr=%x, cpsr=%x\n",
 108               regs->lr_svc,
 109               regs->r0,
 110               regs->r1,
 111               regs->r2,
 112               regs->r9,
 113               regs->r12,
 114               regs->sp,
 115               regs->lr,
 116               regs->cpsr));
 117 #endif
 118         /* Check if a task switch is necessary */
 119         /* 1. There has to be another task in the ready-list */
 120         /* 2. The first task in the ready list hast to have the
 121               same or higher priority than the currently active task */
 122
 123         if( SysBase->TaskReady.lh_Head->ln_Succ != NULL /*
 124            ((BYTE)SysBase->ThisTask->tc_Node.ln_Pri <=
 125             (BYTE)((struct Task *)SysBase->TaskReady.lh_Head)->tc_Node.ln_Pri ) */
 126            )
 127         {
 128                 /* Check if task switch is possible */
 129                 if( SysBase->TDNestCnt < 0 )
 130                 {
 131                         if( SysBase->ThisTask->tc_State == TS_RUN )
 132                         {
 133                                 SysBase->ThisTask->tc_State = TS_READY;
 134                                 Reschedule(SysBase->ThisTask);
 135                                 SysBase->AttnResched |= 0x8000;
 136                         }
 137                         else if( SysBase->ThisTask->tc_State == TS_REMOVED )
 138                                 SysBase->AttnResched |= 0x8000;
 139                 }
 140                 else
 141                         SysBase->AttnResched |= 0x80;
 142         }
 143
 144
 145         /* Has an interrupt told us to dispatch when leaving */
 146         if (SysBase->AttnResched & 0x8000)
 147         {
 148                 SysBase->AttnResched &= ~0x8000;
 149
 150                 /* Save registers for this task (if there is one...) */
 151                 if (SysBase->ThisTask && SysBase->ThisTask->tc_State != TS_REMOVED) {
 152                         regs->lr_svc -= adjust; // adjust is 0 or -4
 153                         SaveRegs(SysBase->ThisTask, regs);
 154                 }
 155
 156                 /* Tell exec that we have actually switched tasks... */
 157                 Dispatch ();
 158 //D(bug("DISPATCHER: New task: %s\n",SysBase->ThisTask->tc_Node.ln_Name));
 159
 160                 /* Get the registers of the old task */
 161                 RestoreRegs(SysBase->ThisTask, regs);
 162                 regs->lr_svc += adjust; // adjust is 0 or -4
 163
 164                 /* Make sure that the state of the interrupts is what the task
 165                    expects.
 166                 */
 167                 if (SysBase->IDNestCnt < 0)
 168                         SC_ENABLE(regs);
 169                 else
 170                         SC_DISABLE(regs);
 171 #if 0
 172                 D(bug("after: lr_svc=%x, r0=%x, r1=%x, r2=%x, r9=%x, r12=%x, sp=%x, lr=%x, cpsr=%x (adjust=%d)\n",
 173                       regs->lr_svc,
 174                       regs->r0,
 175                       regs->r1,
 176                       regs->r2,
 177                       regs->r9,
 178                       regs->r12,
 179                       regs->sp,
 180                       regs->lr,
 181                       regs->cpsr,
 182                       adjust));
 183 #endif
 184                 /* Ok, the next step is to either drop back to the new task, or
 185                    give it its Exception() if it wants one... */
 186
 187                 if (SysBase->ThisTask->tc_Flags & TF_EXCEPT) {
 188                         Disable();
 189                         Exception();
 190                         Enable();
 191                 }
 192
 193
 194         }
 195
 196         /* Leave the interrupt. */
 197 }