examples/standalone/sched.c

   1 /*
   2  * SPDX-License-Identifier:     GPL-2.0+
   3  */
   4
   5 #include <common.h>
   6 #include <exports.h>
   7
   8 /*
   9  * Author: Arun Dharankar <ADharankar@ATTBI.Com>
  10  *
  11  * A very simple thread/schedular model:
  12  *   - only one master thread, and no parent child relation maintained
  13  *   - parent thread cannot be stopped or deleted
  14  *   - no permissions or credentials
  15  *   - no elaborate safety checks
  16  *   - cooperative multi threading
  17  *   - Simple round-robin scheduleing with no priorities
  18  *   - no metering/statistics collection
  19  *
  20  * Basic idea of implementing this is to allow more than one tests to
  21  * execute "simultaneously".
  22  *
  23  * This may be modified such thread_yield may be called in syscalls, and
  24  * timer interrupts.
  25  */
  26
  27
  28 #define MAX_THREADS 8
  29
  30 #define CTX_SIZE 512
  31 #define STK_SIZE 8*1024
  32
  33 #define STATE_EMPTY 0
  34 #define STATE_RUNNABLE 1
  35 #define STATE_STOPPED 2
  36 #define STATE_TERMINATED 2
  37
  38 #define MASTER_THREAD 0
  39
  40 #define RC_FAILURE      (-1)
  41 #define RC_SUCCESS      (0)
  42
  43 typedef vu_char *jmp_ctx;
  44 unsigned long setctxsp (vu_char *sp);
  45 int ppc_setjmp(jmp_ctx env);
  46 void ppc_longjmp(jmp_ctx env, int val);
  47 #define setjmp  ppc_setjmp
  48 #define longjmp ppc_longjmp
  49
  50 struct lthread {
  51         int state;
  52         int retval;
  53         char stack[STK_SIZE];
  54         uchar context[CTX_SIZE];
  55         int (*func) (void *);
  56         void *arg;
  57 };
  58 static volatile struct lthread lthreads[MAX_THREADS];
  59 static volatile int current_tid = MASTER_THREAD;
  60
  61
  62 static uchar dbg = 0;
  63
  64 #define PDEBUG(fmt, args...)     {                                      \
  65         if(dbg != 0) {                                                  \
  66                 printf("[%s %d %s]: ",__FILE__,__LINE__,__FUNCTION__);\
  67                 printf(fmt, ##args);                            \
  68                 printf("\n");                                   \
  69         }                                                               \
  70 }
  71
  72 static int testthread (void *);
  73 static void sched_init (void);
  74 static int thread_create (int (*func) (void *), void *arg);
  75 static int thread_start (int id);
  76 static void thread_yield (void);
  77 static int thread_delete (int id);
  78 static int thread_join (int *ret);
  79
  80 #if 0                                                   /* not used yet */
  81 static int thread_stop (int id);
  82 #endif                                                  /* not used yet */
  83
  84 /* An example of schedular test */
  85
  86 #define NUMTHREADS 7
  87 int sched (int ac, char *av[])
  88 {
  89         int i, j;
  90         int tid[NUMTHREADS];
  91         int names[NUMTHREADS];
  92
  93         app_startup(av);
  94
  95         sched_init ();
  96
  97         for (i = 0; i < NUMTHREADS; i++) {
  98                 names[i] = i;
  99                 j = thread_create (testthread, (void *) &names[i]);
 100                 if (j == RC_FAILURE)
 101                         printf ("schedtest: Failed to create thread %d\n", i);
 102                 if (j > 0) {
 103                         printf ("schedtest: Created thread with id %d, name %d\n",
 104                                                 j, i);
 105                         tid[i] = j;
 106                 }
 107         }
 108         printf ("schedtest: Threads created\n");
 109
 110         printf ("sched_test: function=0x%08x\n", (unsigned)testthread);
 111         for (i = 0; i < NUMTHREADS; i++) {
 112                 printf ("schedtest: Setting thread %d runnable\n", tid[i]);
 113                 thread_start (tid[i]);
 114                 thread_yield ();
 115         }
 116         printf ("schedtest: Started %d threads\n", NUMTHREADS);
 117
 118         while (1) {
 119                 printf ("schedtest: Waiting for threads to complete\n");
 120                 if (tstc () && getc () == 0x3) {
 121                         printf ("schedtest: Aborting threads...\n");
 122                         for (i = 0; i < NUMTHREADS; i++) {
 123                                 printf ("schedtest: Deleting thread %d\n", tid[i]);
 124                                 thread_delete (tid[i]);
 125                         }
 126                         return RC_SUCCESS;
 127                 }
 128                 j = -1;
 129                 i = thread_join (&j);
 130                 if (i == RC_FAILURE) {
 131                         printf ("schedtest: No threads pending, "
 132                                                 "exiting schedular test\n");
 133                         return RC_SUCCESS;
 134                 }
 135                 printf ("schedtest: thread is %d returned %d\n", i, j);
 136                 thread_yield ();
 137         }
 138
 139         return RC_SUCCESS;
 140 }
 141
 142 static int testthread (void *name)
 143 {
 144         int i;
 145
 146         printf ("testthread: Begin executing thread, myname %d, &i=0x%08x\n",
 147                 *(int *) name, (unsigned)&i);
 148
 149         printf ("Thread %02d, i=%d\n", *(int *) name, i);
 150
 151         for (i = 0; i < 0xffff * (*(int *) name + 1); i++) {
 152                 if (tstc () && getc () == 0x3) {
 153                         printf ("testthread: myname %d terminating.\n",
 154                                                 *(int *) name);
 155                         return *(int *) name + 1;
 156                 }
 157
 158                 if (i % 100 == 0)
 159                         thread_yield ();
 160         }
 161
 162         printf ("testthread: returning %d, i=0x%x\n",
 163                                 *(int *) name + 1, i);
 164
 165         return *(int *) name + 1;
 166 }
 167
 168
 169 static void sched_init (void)
 170 {
 171         int i;
 172
 173         for (i = MASTER_THREAD + 1; i < MAX_THREADS; i++)
 174                 lthreads[i].state = STATE_EMPTY;
 175
 176         current_tid = MASTER_THREAD;
 177         lthreads[current_tid].state = STATE_RUNNABLE;
 178         PDEBUG ("sched_init: master context = 0x%08x",
 179                 (unsigned)lthreads[current_tid].context);
 180         return;
 181 }
 182
 183 static void thread_yield (void)
 184 {
 185         static int i;
 186
 187         PDEBUG ("thread_yield: current tid=%d", current_tid);
 188
 189 #define SWITCH(new)                                                     \
 190         if(lthreads[new].state == STATE_RUNNABLE) {                     \
 191                 PDEBUG("thread_yield: %d match, ctx=0x%08x",            \
 192                         new,                                            \
 193                         (unsigned)lthreads[current_tid].context);       \
 194                 if(setjmp(lthreads[current_tid].context) == 0) {        \
 195                         current_tid = new;                              \
 196                         PDEBUG("thread_yield: tid %d returns 0",        \
 197                                 new);                                   \
 198                         longjmp(lthreads[new].context, 1);              \
 199                 } else {                                                \
 200                         PDEBUG("thread_yield: tid %d returns 1",        \
 201                                 new);                                   \
 202                         return;                                         \
 203                 }                                                       \
 204         }
 205
 206         for (i = current_tid + 1; i < MAX_THREADS; i++) {
 207                 SWITCH (i);
 208         }
 209
 210         if (current_tid != 0) {
 211                 for (i = 0; i <= current_tid; i++) {
 212                         SWITCH (i);
 213                 }
 214         }
 215
 216         PDEBUG ("thread_yield: returning from thread_yield");
 217         return;
 218 }
 219
 220 static int thread_create (int (*func) (void *), void *arg)
 221 {
 222         int i;
 223
 224         for (i = MASTER_THREAD + 1; i < MAX_THREADS; i++) {
 225                 if (lthreads[i].state == STATE_EMPTY) {
 226                         lthreads[i].state = STATE_STOPPED;
 227                         lthreads[i].func = func;
 228                         lthreads[i].arg = arg;
 229                         PDEBUG ("thread_create: returns new tid %d", i);
 230                         return i;
 231                 }
 232         }
 233
 234         PDEBUG ("thread_create: returns failure");
 235         return RC_FAILURE;
 236 }
 237
 238 static int thread_delete (int id)
 239 {
 240         if (id <= MASTER_THREAD || id > MAX_THREADS)
 241                 return RC_FAILURE;
 242
 243         if (current_tid == id)
 244                 return RC_FAILURE;
 245
 246         lthreads[id].state = STATE_EMPTY;
 247         return RC_SUCCESS;
 248 }
 249
 250 static void thread_launcher (void)
 251 {
 252         PDEBUG ("thread_launcher: invoking func=0x%08x",
 253                    (unsigned)lthreads[current_tid].func);
 254
 255         lthreads[current_tid].retval =
 256                         lthreads[current_tid].func (lthreads[current_tid].arg);
 257
 258         PDEBUG ("thread_launcher: tid %d terminated", current_tid);
 259
 260         lthreads[current_tid].state = STATE_TERMINATED;
 261         thread_yield ();
 262         printf ("thread_launcher: should NEVER get here!\n");
 263
 264         return;
 265 }
 266
 267 static int thread_start (int id)
 268 {
 269         PDEBUG ("thread_start: id=%d", id);
 270         if (id <= MASTER_THREAD || id > MAX_THREADS) {
 271                 return RC_FAILURE;
 272         }
 273
 274         if (lthreads[id].state != STATE_STOPPED)
 275                 return RC_FAILURE;
 276
 277         if (setjmp (lthreads[current_tid].context) == 0) {
 278                 lthreads[id].state = STATE_RUNNABLE;
 279                 current_tid = id;
 280                 PDEBUG ("thread_start: to be stack=0%08x",
 281                         (unsigned)lthreads[id].stack);
 282                 setctxsp ((vu_char *)&lthreads[id].stack[STK_SIZE]);
 283                 thread_launcher ();
 284         }
 285
 286         PDEBUG ("thread_start: Thread id=%d started, parent returns", id);
 287
 288         return RC_SUCCESS;
 289 }
 290
 291 #if 0   /* not used so far */
 292 static int thread_stop (int id)
 293 {
 294         if (id <= MASTER_THREAD || id >= MAX_THREADS)
 295                 return RC_FAILURE;
 296
 297         if (current_tid == id)
 298                 return RC_FAILURE;
 299
 300         lthreads[id].state = STATE_STOPPED;
 301         return RC_SUCCESS;
 302 }
 303 #endif  /* not used so far */
 304
 305 static int thread_join (int *ret)
 306 {
 307         int i, j = 0;
 308
 309         PDEBUG ("thread_join: *ret = %d", *ret);
 310
 311         if (!(*ret == -1 || *ret > MASTER_THREAD || *ret < MAX_THREADS)) {
 312                 PDEBUG ("thread_join: invalid tid %d", *ret);
 313                 return RC_FAILURE;
 314         }
 315
 316         if (*ret == -1) {
 317                 PDEBUG ("Checking for tid = -1");
 318                 while (1) {
 319                         /* PDEBUG("thread_join: start while-loopn"); */
 320                         j = 0;
 321                         for (i = MASTER_THREAD + 1; i < MAX_THREADS; i++) {
 322                                 if (lthreads[i].state == STATE_TERMINATED) {
 323                                         *ret = lthreads[i].retval;
 324                                         lthreads[i].state = STATE_EMPTY;
 325                                         /* PDEBUG("thread_join: returning retval %d of tid %d",
 326                                            ret, i); */
 327                                         return RC_SUCCESS;
 328                                 }
 329
 330                                 if (lthreads[i].state != STATE_EMPTY) {
 331                                         PDEBUG ("thread_join: %d used slots tid %d state=%d",
 332                                                    j, i, lthreads[i].state);
 333                                         j++;
 334                                 }
 335                         }
 336                         if (j == 0) {
 337                                 PDEBUG ("thread_join: all slots empty!");
 338                                 return RC_FAILURE;
 339                         }
 340                         /*  PDEBUG("thread_join: yielding"); */
 341                         thread_yield ();
 342                         /*  PDEBUG("thread_join: back from yield"); */
 343                 }
 344         }
 345
 346         if (lthreads[*ret].state == STATE_TERMINATED) {
 347                 i = *ret;
 348                 *ret = lthreads[*ret].retval;
 349                 lthreads[*ret].state = STATE_EMPTY;
 350                 PDEBUG ("thread_join: returing %d for tid %d", *ret, i);
 351                 return RC_SUCCESS;
 352         }
 353
 354         PDEBUG ("thread_join: thread %d is not terminated!", *ret);
 355         return RC_FAILURE;
 356 }