arch/all-unix/kernel/coretest.c

   1 /*
   2     Copyright © 1995-2011, The AROS Development Team. All rights reserved.
   3     $Id$
   4 */
   5
   6 #include <exec/lists.h>
   7 #include <exec/tasks.h>
   8 #include <exec/execbase.h>
   9 #include <unistd.h>
  10 #include <signal.h>
  11 #include <stdio.h>
  12 #include <stdlib.h>
  13 #include <string.h>
  14 #include <kernel_cpu.h>
  15 #include <sys/time.h>
  16
  17 /* Prototypes */
  18 void Dispatch (void);
  19 void Enqueue (struct List * list, struct Node * node);
  20 void AddTail (struct List * list, struct Node * node);
  21 void Switch (void);
  22
  23 /* This is used to count the number of interrupts. */
  24 ULONG cnt;
  25
  26 /* This flag means that a task switch is pending */
  27 #define SB_SAR 15
  28 #define SF_SAR 0x8000
  29
  30 /* Dummy SysBase */
  31 struct ExecBase _SysBase;
  32 struct ExecBase* SysBase = &_SysBase;
  33
  34 /* Dummy KernelBase */
  35 struct PlatformData
  36 {
  37     sigset_t     sig_int_mask;
  38 };
  39
  40 struct PlatformData Kernel_PlatformData;
  41
  42 #define PD(x) Kernel_PlatformData
  43
  44 /* Some tasks */
  45 struct Task Task1, Task2, Task3, Task4, TaskMain;
  46
  47 /* List functions */
  48 void AddTail (struct List * list, struct Node * node)
  49 {
  50     ADDTAIL(list,node);
  51 } /* AddTail */
  52
  53 void AddHead (struct List * list, struct Node * node)
  54 {
  55     ADDHEAD (list, node);
  56 } /* AddHead */
  57
  58 void Remove (struct Node * node)
  59 {
  60     REMOVE(node);
  61 } /* Remove */
  62
  63 /* Enter a node into a sorted list */
  64 void Enqueue (struct List * list, struct Node * node)
  65 {
  66     struct Node * next;
  67
  68     ForeachNode (list, next)
  69     {
  70         /* Look for the first node with a lower priority */
  71         if (node->ln_Pri > next->ln_Pri)
  72             break;
  73     }
  74
  75     /* Insert "node" before "next" */
  76     node->ln_Pred          = next->ln_Pred;
  77     next->ln_Pred->ln_Succ = node;
  78     next->ln_Pred          = node;
  79     node->ln_Succ          = next;
  80 } /* Enqueue */
  81
  82 #define STR(x)      (x && x->ln_Name ? (char *)x->ln_Name : "NULL")
  83
  84 /* Print a list with nodes with names. */
  85 void PrintList (struct List * list)
  86 {
  87     struct Node * node;
  88     int t = 0;
  89
  90     printf ("list %p { head=%s, tail=%s } = ", list,
  91         STR(list->lh_Head), STR(list->lh_TailPred));
  92
  93     for (node=GetHead(list); node; node=GetSucc(node))
  94     {
  95         printf ("%s (%p { succ=%s pred=%s pri=%d}), ", node->ln_Name, node,
  96             STR(GetSucc(node)), STR(GetPred(node)),
  97             node->ln_Pri
  98         );
  99         if (++t > 10)
 100             break;
 101     }
 102
 103     printf ("\n");
 104 }
 105
 106 #undef STR
 107
 108 /* Macro to get a pointer to the current running task */
 109 #define THISTASK        (SysBase->ThisTask)
 110
 111 /*
 112     Disable and enable signals. Don't use these in the signal handler
 113     because then some signal might break the signal handler and then
 114     stack will overflow.
 115 */
 116 #define DISABLE         sigprocmask (SIG_BLOCK, &Kernel_PlatformData.sig_int_mask, NULL)
 117 #define ENABLE          sigprocmask (SIG_UNBLOCK, &Kernel_PlatformData.sig_int_mask, NULL)
 118
 119 /* Enable/Disable interrupts */
 120 void Disable (void)
 121 {
 122     /*
 123         Disable signals only if they are not already. The initial value of
 124         IDNestCnt is -1.
 125     */
 126     if (SysBase->IDNestCnt++ < 0)
 127     {
 128         DISABLE;
 129     }
 130 } /* Disable */
 131
 132 void Enable (void)
 133 {
 134     /*
 135         Enable signals only if the number of calls of Enable() matches the
 136         calls of Disable().
 137     */
 138     if (--SysBase->IDNestCnt < 0)
 139     {
 140         ENABLE;
 141     }
 142 } /* Enable */
 143
 144 /* Allow/forbid task switches */
 145 void Forbid (void)
 146 {
 147     /*
 148         Task switches are only allowed if TDNestCnt is < 0. The initial
 149         value of TDNestCnt is -1.
 150     */
 151     ++ SysBase->TDNestCnt;
 152 } /* Forbid */
 153
 154 void Permit (void)
 155 {
 156     /* Count calls and check if interrupts are allowed. */
 157     if (--SysBase->TDNestCnt < 0
 158         && SysBase->IDNestCnt < 0
 159     )
 160     {
 161         /*
 162             Task switches are allowed again. If a switch is pending
 163             right now, do it.
 164         */
 165         if (SysBase->SysFlags & SF_SAR)
 166         {
 167             /* Clear flag */
 168             SysBase->SysFlags &= ~SF_SAR;
 169
 170             /* Do task switch */
 171             Switch ();
 172         }
 173     }
 174 } /* Permit */
 175
 176 /* Main routine: Insert a task into the list of tasks to run. */
 177 void Reschedule (struct Task * task)
 178 {
 179     /*
 180         The code in here defines how "good" the task switching is.
 181         There are seveal things which should be taken into account:
 182
 183         1. No task should block the CPU forever even if it is an
 184             endless loop.
 185
 186         2. Tasks with a higher priority should get the CPU more often.
 187
 188         3. Tasks with a low priority should get the CPU every now and then.
 189
 190         Enqueue() fulfills 2 but not 1 and 3. AddTail() fulfills 1 and 3.
 191
 192         A better way would be to "deteriorate" a task, ie. decrease the
 193         priority and use Enqueue() but at this time, I can't do this
 194         because I have no good way to extend the task structure (I
 195         need a variable to store the original prio).
 196     */
 197     AddTail (&SysBase->TaskReady, (struct Node *)task);
 198 } /* Reschedule */
 199
 200 /* Switch to a new task if the current task is not running and no
 201     exception has been raised. */
 202 ULONG Wait (ULONG sigmask);
 203 void Switch (void)
 204 {
 205     struct Task * task = THISTASK;
 206
 207     /* Check that the task is not running and no exception is pending */
 208     if (task->tc_State != TS_RUN && !(task->tc_Flags & TF_EXCEPT) )
 209     {
 210         /* Allow signals. */
 211         ENABLE;
 212
 213         /*
 214             Make sure there is a signal. This is somewhat tricky: The
 215             current task (which is excuting this funcion) will loose the
 216             CPU (ie. some code of another task will be executed). Then
 217             at some time in the future, the current task (ie. the one
 218             that has executed the kill()) will get the CPU back and
 219             continue with the code after the kill().
 220         */
 221         kill (getpid(), SIGALRM);
 222     }
 223 } /* Switch */
 224
 225 /*
 226     This waits for a "signal". That's not a Unix signal but a flag set
 227     by some other task (eg. if you send a command to a device, the
 228     device will call you back when it has processes the command.
 229     When this happens such a "signal" will be set). The task will be
 230     suspended until any of the bits given to Wait() are set in the
 231     tasks' signal mask. Again, this signal mask has nothing to do
 232     with the Unix signal mask.
 233
 234     It's a dummy right now. All it does is switch to another task.
 235 */
 236 ULONG Wait (ULONG sigmask)
 237 {
 238     struct Task * task = THISTASK;
 239
 240     /*
 241         Task is no longer running. If we didn't do this, Switch() would do
 242         nothing.
 243     */
 244     task->tc_State = TS_READY;
 245
 246     /* Let another task run. */
 247     Switch ();
 248
 249     /* When I get the CPU back, this code is executed */
 250     return 0;
 251 }
 252
 253 /* Simple main for a task: Print a message and wait for a signal. */
 254 void Main1 (void)
 255 {
 256     struct Task * task = THISTASK;
 257     STRPTR name = task->tc_Node.ln_Name;
 258
 259     while (1)
 260     {
 261         printf ("Main1: %s\n", name);
 262
 263         Wait (1);
 264     }
 265 }
 266
 267 /* Another method of waiting (but an inferior one). */
 268 void busy_wait (void)
 269 {
 270     int t;
 271
 272     for (t=cnt; t==cnt; );
 273 }
 274
 275 /* Same as Main1 but wait by polling */
 276 void Main2 (void)
 277 {
 278     struct Task * task = THISTASK;
 279     STRPTR name = task->tc_Node.ln_Name;
 280
 281     while (1)
 282     {
 283         printf ("Main2: %s\n", name);
 284
 285         /*
 286             Kids, don't do this at home. I'm a professional.
 287
 288             This is to make sure even endless loops don't harm the
 289             system. Even if this task has a higher priority than any
 290             other task in the system, the other tasks will get the
 291             CPU every now and then.
 292         */
 293         busy_wait();
 294     }
 295 }
 296
 297 #define DEBUG_STACK     0
 298 #define STACKOFFSET     0
 299
 300 /*
 301     The signal handler. It will store the current tasks context and
 302     switch to another task if this is allowed right now.
 303 */
 304 static void sighandler (int sig, regs_t *sc)
 305 {
 306     cnt ++;
 307
 308     /* Are task switches allowed ? */
 309     if (SysBase->TDNestCnt < 0)
 310     {
 311         struct AROSCPUContext *ctx = THISTASK->tc_UnionETask.tc_ETask->et_RegFrame;
 312
 313 #if DEBUG_STACK
 314         PRINT_SC(sc);
 315 #endif
 316
 317         /* Save all registers and the stack pointer */
 318         SAVEREGS(ctx, sc);
 319         THISTASK->tc_SPReg = (APTR)SP(sc);
 320
 321         /* Find a new task to run */
 322         Dispatch ();
 323         /* Restore signal mask */
 324         if (SysBase->IDNestCnt < 0)
 325             SC_ENABLE(sc);
 326         else
 327             SC_DISABLE(sc);
 328
 329         ctx = THISTASK->tc_UnionETask.tc_ETask->et_RegFrame;
 330         RESTOREREGS(ctx, sc);
 331         SP(sc) = (IPTR)THISTASK->tc_SPReg;
 332
 333 #if DEBUG_STACK
 334         PRINT_SC(sc);
 335 #endif
 336
 337     }
 338     else
 339     {
 340         /* Set flag: switch tasks as soon as switches are allowed again */
 341         SysBase->SysFlags |= SF_SAR;
 342     }
 343 } /* sighandler */
 344
 345 /* Let the sigcore do it's magic */
 346 GLOBAL_SIGNAL_INIT(sighandler)
 347
 348 /* Find another task which is allowed to run and modify SysBase accordingly */
 349 void Dispatch (void)
 350 {
 351     struct Task * this = THISTASK;
 352     struct Task * task;
 353
 354     /* Check the stack */
 355     if (this->tc_SPReg <= this->tc_SPLower
 356         || this->tc_SPReg >= this->tc_SPUpper
 357     )
 358     {
 359         printf ("illegal stack (SP %p, lower %p, upper %p)\n", this->tc_SPReg, this->tc_SPLower, this->tc_SPUpper);
 360     }
 361
 362     /* Try to find a task which is ready to run */
 363     if ((task = (struct Task *)GetHead (&SysBase->TaskReady)))
 364     {
 365         printf ("Dispatch: Old = %s (Stack = %lx), new = %s\n",
 366         this->tc_Node.ln_Name,
 367         (IPTR)this->tc_SPUpper - (IPTR)this->tc_SPReg,
 368         task->tc_Node.ln_Name);
 369
 370         /* Remove new task from the list */
 371         Remove ((struct Node *)task);
 372
 373         /* Sort the old task into the list of tasks which want to run */
 374         Reschedule (this);
 375
 376         /* Save disable counters */
 377         this->tc_TDNestCnt = SysBase->TDNestCnt;
 378         this->tc_IDNestCnt = SysBase->IDNestCnt;
 379
 380         /* Set new counters */
 381         SysBase->TDNestCnt = task->tc_TDNestCnt;
 382         SysBase->IDNestCnt = task->tc_IDNestCnt;
 383
 384         /* Switch task */
 385         THISTASK = task;
 386
 387         /* Set new states of the tasks */
 388         this->tc_State = TS_READY;
 389         task->tc_State = TS_RUN;
 390     }
 391     printf("leaving dispatch!\n");
 392 } /* Dispatch */
 393
 394 /*
 395     Initialize the system: Install an interrupt handler and make sure
 396     it is called at 50Hz
 397 */
 398 void InitCore(void)
 399 {
 400     struct sigaction sa;
 401     struct itimerval interval;
 402
 403     /* Install a handler for the ALARM signal */
 404     sa.sa_handler  = (SIGHANDLER_T)sighandler_gate;
 405     sa.sa_flags    = SA_RESTART;
 406 #ifdef __linux__
 407     sa.sa_restorer = NULL;
 408 #endif /* __linux__ */
 409     sigfillset (&sa.sa_mask);
 410
 411     sigaction (SIGALRM, &sa, NULL);
 412
 413     /* Set 50Hz intervall for ALARM signal */
 414     interval.it_interval.tv_sec  = interval.it_value.tv_sec  = 1;
 415     interval.it_interval.tv_usec = interval.it_value.tv_usec = 1000000/50;
 416
 417     setitimer (ITIMER_REAL, &interval, NULL);
 418 } /* InitCore */
 419
 420 #define STACK_SIZE  4096
 421
 422 /* Create a new task */
 423 void AddTask (struct Task * task, STRPTR name, BYTE pri, APTR pc)
 424 {
 425     IPTR *sp;
 426     struct AROSCPUContext *ctx;
 427
 428     /* Init task structure */
 429     memset (task, 0, sizeof (struct Task));
 430
 431     /* Init fields with real values */
 432     task->tc_Node.ln_Pri = pri;
 433     task->tc_Node.ln_Name = name;
 434     task->tc_State = TS_READY;
 435
 436     /* Allow task switches and signals */
 437     task->tc_TDNestCnt = -1;
 438     task->tc_IDNestCnt = -1;
 439
 440     /* Allocate a stack */
 441     sp = malloc (STACK_SIZE * sizeof(IPTR));
 442
 443     /*
 444         Copy bounds of stack in task structure. Note that the stack
 445         grows to small addresses (ie. storing something on the stack
 446         decreases the stack pointer).
 447     */
 448     task->tc_SPLower = sp;
 449     sp += STACK_SIZE;
 450     task->tc_SPUpper = sp;
 451
 452     /*
 453         Let the sigcore do it's magic. Create a frame from which an
 454         initial task context can be restored from.
 455     */
 456
 457     task->tc_UnionETask.tc_ETask = malloc(sizeof(struct IntETask));
 458     task->tc_Flags |= TF_ETASK;
 459     ctx = malloc(sizeof(struct AROSCPUContext));
 460     task->tc_UnionETask.tc_ETask->et_RegFrame = ctx;
 461
 462     PREPARE_INITIAL_CONTEXT(ctx);
 463     PREPARE_INITIAL_FRAME(ctx, sp, (IPTR)pc);
 464
 465     /* Save new stack pointer */
 466     task->tc_SPReg = sp;
 467
 468     /* Add task to queue by priority */
 469     Enqueue (&SysBase->TaskReady, (struct Node *)task);
 470 } /* AddTask */
 471
 472 /*
 473     Main routine: Create four tasks (three with the Mains above and one
 474     for main(). Wait for some task switches then terminate cleanly.
 475 */
 476 int main (int argc, char ** argv)
 477 {
 478     /* Init SysBase */
 479     NEWLIST (&SysBase->TaskReady);
 480     NEWLIST (&SysBase->TaskWait);
 481
 482     sigfillset(&Kernel_PlatformData.sig_int_mask);
 483
 484     /* Signals and task switches are not allowed right now */
 485     SysBase->IDNestCnt = 0;
 486     SysBase->TDNestCnt = 0;
 487
 488     /* Add three tasks */
 489     AddTask (&Task1, "Task 1", 0, Main1);
 490     AddTask (&Task2, "Task 2", 5, Main2);
 491     AddTask (&Task3, "Task 3", 0, Main2);
 492     PrintList (&SysBase->TaskReady);
 493
 494     /*
 495         Add main task. Make sure the stack check is ok. This task is *not*
 496         added to the list. It is stored in THISTASK and will be added to
 497         the list at the next call to Dispatch().
 498
 499         Also a trick with the stack: This is the stack of the Unix process.
 500         We don't know where it lies in memory nor how big it is (it can
 501         grow), so we set the maximum possible limits.
 502     */
 503     TaskMain.tc_Node.ln_Pri = 0;
 504     TaskMain.tc_Node.ln_Name = "Main";
 505     TaskMain.tc_State = TS_READY;
 506     TaskMain.tc_SPLower = 0;
 507     TaskMain.tc_SPUpper = (APTR)-1;
 508
 509     TaskMain.tc_UnionETask.tc_ETask = malloc(sizeof(struct IntETask));
 510     TaskMain.tc_Flags |= TF_ETASK;
 511     TaskMain.tc_UnionETask.tc_ETask->et_RegFrame = malloc(sizeof(struct AROSCPUContext));
 512
 513     /* The currently running task is me, myself and I */
 514     THISTASK = &TaskMain;
 515
 516     /* Start interrupts and allow them. */
 517     InitCore ();
 518     Enable ();
 519     Permit ();
 520
 521     /* Wait for 10000 signals */
 522     while (cnt < 20)
 523     {
 524         printf ("%6d\n", cnt);
 525
 526         /* Wait for a "signal" from another task. */
 527         Wait (1);
 528     }
 529
 530     /* Make sure we don't get disturbed in the cleanup */
 531     Disable ();
 532
 533     /* Show how many signals have been processed */
 534     printf ("Exit after %d signals\n", cnt);
 535
 536     return 0;
 537 } /* main */
 538