arch/ppc-sam440/kernel/scheduler.c

   1 #include <asm/amcc440.h>
   2 #include <aros/kernel.h>
   3 #include <aros/libcall.h>
   4 #include <exec/execbase.h>
   5 #include <hardware/intbits.h>
   6
   7 #include "exec_intern.h"
   8 #include "etask.h"
   9 #include "syscall.h"
  10
  11 #include "kernel_intern.h"
  12
  13 AROS_LH0(KRN_SchedType, KrnGetScheduler,
  14          struct KernelBase *, KernelBase, 1, Kernel)
  15 {
  16     AROS_LIBFUNC_INIT
  17
  18     return SCHED_RR;
  19
  20     AROS_LIBFUNC_EXIT
  21 }
  22
  23 AROS_LH1(void, KrnSetScheduler,
  24          AROS_LHA(KRN_SchedType, sched, D0),
  25          struct KernelBase *, KernelBase, 2, Kernel)
  26 {
  27     AROS_LIBFUNC_INIT
  28
  29     /* Cannot set scheduler yet */
  30
  31     AROS_LIBFUNC_EXIT
  32 }
  33
  34 AROS_LH0(void, KrnCause,
  35          struct KernelBase *, KernelBase, 3, Kernel)
  36 {
  37     AROS_LIBFUNC_INIT
  38
  39     asm volatile("li %%r3,%0; sc"::"i"(SC_CAUSE):"memory","r3");
  40
  41     AROS_LIBFUNC_EXIT
  42 }
  43
  44 AROS_LH0(void , KrnDispatch,
  45          struct KernelBase *, KernelBase, 4, Kernel)
  46 {
  47     AROS_LIBFUNC_INIT
  48
  49
  50     asm volatile("li %%r3,%0; sc"::"i"(SC_DISPATCH):"memory","r3");
  51
  52     AROS_LIBFUNC_EXIT
  53 }
  54
  55 AROS_LH0(void, KrnSwitch,
  56          struct KernelBase *, KernelBase, 5, Kernel)
  57 {
  58     AROS_LIBFUNC_INIT
  59
  60     asm volatile("li %%r3,%0; sc"::"i"(SC_SWITCH):"memory","r3");
  61
  62     AROS_LIBFUNC_EXIT
  63 }
  64
  65 AROS_LH0(void, KrnSchedule,
  66          struct KernelBase *, KernelBase, 6, Kernel)
  67 {
  68     AROS_LIBFUNC_INIT
  69
  70     asm volatile("li %%r3,%0; sc"::"i"(SC_SCHEDULE):"memory","r3");
  71
  72     AROS_LIBFUNC_EXIT
  73 }
  74
  75
  76 /*
  77  * Task dispatcher. Basically it may be the same one no matter what scheduling algorithm is used
  78  */
  79 void core_Dispatch(regs_t *regs)
  80 {
  81     volatile struct ExecBase *SysBase = getSysBase();
  82     struct Task *task;
  83
  84     if (SysBase)
  85     {
  86         __asm__ __volatile__("wrteei 0;");
  87
  88         /*
  89          * Is the list of ready tasks empty? Well, increment the idle switch cound and halt CPU.
  90          * It should be extended by some plugin mechanism which would put CPU and whole machine
  91          * into some more sophisticated sleep states (ACPI?)
  92          */
  93         while (IsListEmpty(&SysBase->TaskReady))
  94         {
  95             SysBase->IdleCount++;
  96             SysBase->AttnResched |= ARF_AttnSwitch;
  97
  98             //D(bug("[KRN] TaskReady list empty. Sleeping for a while...\n"));
  99             /* Sleep almost forever ;) */
 100             //__asm__ __volatile__("wrteei 1; sync; isync;");
 101             wrmsr(rdmsr() | MSR_POW | MSR_EE);
 102             __asm__ __volatile__("wrteei 0");
 103             //D(bug("[\n"));
 104
 105             if (SysBase->SysFlags & SFF_SoftInt)
 106             {
 107                 core_Cause(SysBase);
 108             }
 109         }
 110
 111         SysBase->DispCount++;
 112
 113         /* Get the first task from the TaskReady list, and populate it's settings through Sysbase */
 114         task = (struct Task *)REMHEAD(&SysBase->TaskReady);
 115         SysBase->ThisTask = task;
 116         SysBase->Elapsed = SysBase->Quantum;
 117         SysBase->SysFlags &= ~0x2000;
 118         task->tc_State = TS_RUN;
 119         SysBase->IDNestCnt = task->tc_IDNestCnt;
 120
 121         //D(bug("[KRN] New task = %p (%s)\n", task, task->tc_Node.ln_Name));
 122
 123         /* Handle tasks's flags */
 124         if (task->tc_Flags & TF_EXCEPT)
 125             Exception();
 126
 127         if (task->tc_Flags & TF_LAUNCH)
 128         {
 129             AROS_UFC1(void, task->tc_Launch,
 130                       AROS_UFCA(struct ExecBase *, SysBase, A6));
 131         }
 132
 133         /* Restore the task's state */
 134         bcopy(GetIntETask(task)->iet_Context, regs, sizeof(regs_t));
 135         /* Copy the fpu, mmx, xmm state */
 136 #warning FIXME: Change to the lazy saving of the FPU state!!!!
 137 #warning TODO: No FPU support yet!!!!!!! Yay, it sucks! :-D
 138     //    IPTR sse_ctx = ((IPTR)GetIntETask(task)->iet_Context + sizeof(regs_t) + 15) & ~15;
 139     //    asm volatile("fxrstor (%0)"::"D"(sse_ctx));
 140
 141     }
 142
 143     regs->srr1 &= ~MSR_POW;
 144     /* Leave interrupt and jump to the new task */
 145     core_LeaveInterrupt(regs);
 146 }
 147
 148 void core_Switch(regs_t *regs)
 149 {
 150     struct ExecBase *SysBase = getSysBase();
 151     struct Task *task;
 152
 153     if (SysBase)
 154     {
 155         /* Disable interrupts for a while */
 156         __asm__ __volatile__("wrteei 0");
 157
 158         task = SysBase->ThisTask;
 159
 160         //D(bug("[KRN] Old task = %p (%s)\n", task, task->tc_Node.ln_Name));
 161
 162         /* Copy current task's context into the ETask structure */
 163         bcopy(regs, GetIntETask(task)->iet_Context, sizeof(regs_t));
 164
 165         /* Copy the fpu, mmx, xmm state */
 166 #warning FIXME: Change to the lazy saving of the FPU state!!!!
 167 #warning TODO: Write the damn FPU handling at all!!!!!!!! ;-D LOL
 168     //    IPTR sse_ctx = ((IPTR)GetIntETask(task)->iet_Context + sizeof(regs_t) + 15) & ~15;
 169     //    asm volatile("fxsave (%0)"::"D"(sse_ctx));
 170
 171         /* store IDNestCnt into tasks's structure */
 172         task->tc_IDNestCnt = SysBase->IDNestCnt;
 173         task->tc_SPReg = regs->gpr[1];
 174
 175         /* And enable interrupts */
 176         SysBase->IDNestCnt = -1;
 177         __asm__ __volatile__("wrteei 1");
 178
 179         /* TF_SWITCH flag set? Call the switch routine */
 180         if (task->tc_Flags & TF_SWITCH)
 181         {
 182             AROS_UFC1(void, task->tc_Switch,
 183                       AROS_UFCA(struct ExecBase *, SysBase, A6));
 184         }
 185     }
 186
 187     core_Dispatch(regs);
 188 }
 189
 190
 191 /*
 192  * Schedule the currently running task away. Put it into the TaskReady list
 193  * in some smart way. This function is subject of change and it will be probably replaced
 194  * by some plugin system in the future
 195  */
 196 void core_Schedule(regs_t *regs)
 197 {
 198     struct ExecBase *SysBase = getSysBase();
 199     struct Task *task;
 200
 201     if (SysBase)
 202     {
 203         /* Disable interrupts for a while */
 204         __asm__ __volatile__("wrteei 0"); // CLI
 205
 206         task = SysBase->ThisTask;
 207
 208         /* Clear the pending switch flag. */
 209         SysBase->AttnResched &= ~ARF_AttnSwitch;
 210
 211         /* If task has pending exception, reschedule it so that the dispatcher may handle the exception */
 212         if (!(task->tc_Flags & TF_EXCEPT))
 213         {
 214             /* Is the TaskReady empty? If yes, then the running task is the only one. Let it work */
 215             if (IsListEmpty(&SysBase->TaskReady))
 216                 core_LeaveInterrupt(regs);
 217
 218             /* Does the TaskReady list contains tasks with priority equal or lower than current task?
 219              * If so, then check further... */
 220             if (((struct Task*)GetHead(&SysBase->TaskReady))->tc_Node.ln_Pri <= task->tc_Node.ln_Pri)
 221             {
 222                 /* If the running task did not used it's whole quantum yet, let it work */
 223                 if (!(SysBase->SysFlags & 0x2000))
 224                 {
 225                     core_LeaveInterrupt(regs);
 226                 }
 227             }
 228         }
 229
 230         /*
 231          * If we got here, then the rescheduling is necessary.
 232          * Put the task into the TaskReady list.
 233          */
 234         task->tc_State = TS_READY;
 235         Enqueue(&SysBase->TaskReady, (struct Node *)task);
 236     }
 237
 238     /* Select new task to run */
 239     core_Switch(regs);
 240 }
 241
 242
 243 /*
 244  * Leave the interrupt. This function recieves the register frame used to leave the supervisor
 245  * mode. It never returns and reschedules the task if it was asked for.
 246  */
 247 void core_ExitInterrupt(regs_t *regs)
 248 {
 249     /* Powermode was on? Turn it off now */
 250     regs->srr1 &= ~MSR_POW;
 251
 252     /* Going back into supervisor mode? Then exit immediatelly */
 253     if (!(regs->srr1 & MSR_PR))
 254     {
 255         core_LeaveInterrupt(regs);
 256     }
 257     else
 258     {
 259         /* Prepare to go back into user mode */
 260         struct ExecBase *SysBase = getSysBase();
 261
 262         if (SysBase)
 263         {
 264             /* Soft interrupt requested? It's high time to do it */
 265             if (SysBase->SysFlags & SFF_SoftInt)
 266                 core_Cause(SysBase);
 267
 268             /* If task switching is disabled, leave immediatelly */
 269             if (SysBase->TDNestCnt >= 0)
 270             {
 271                 core_LeaveInterrupt(regs);
 272             }
 273             else
 274             {
 275                 /*
 276                  * Do not disturb task if it's not necessary.
 277                  * Reschedule only if switch pending flag is set. Exit otherwise.
 278                  */
 279                 if (SysBase->AttnResched & ARF_AttnSwitch)
 280                 {
 281                     core_Schedule(regs);
 282                 }
 283                 else
 284                     core_LeaveInterrupt(regs);
 285             }
 286         }
 287         else
 288             core_LeaveInterrupt(regs);
 289     }
 290 }
 291
 292 void core_Cause(struct ExecBase *SysBase)
 293 {
 294     struct IntVector *iv = &SysBase->IntVects[INTB_SOFTINT];
 295
 296     /* If the SoftInt vector in SysBase is set, call it. It will do the rest for us */
 297     if (iv->iv_Code)
 298     {
 299         AROS_UFC5(void, iv->iv_Code,
 300             AROS_UFCA(ULONG, 0, D1),
 301             AROS_UFCA(ULONG, 0, A0),
 302             AROS_UFCA(APTR, 0, A1),
 303             AROS_UFCA(APTR, iv->iv_Code, A5),
 304             AROS_UFCA(struct ExecBase *, SysBase, A6)
 305         );
 306     }
 307 }