1
2
3
4
5#include <common.h>
6#include <exports.h>
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
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
43typedef vu_char *jmp_ctx;
44unsigned long setctxsp (vu_char *sp);
45int ppc_setjmp(jmp_ctx env);
46void ppc_longjmp(jmp_ctx env, int val);
47#define setjmp ppc_setjmp
48#define longjmp ppc_longjmp
49
50struct 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};
58static volatile struct lthread lthreads[MAX_THREADS];
59static volatile int current_tid = MASTER_THREAD;
60
61
62static 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
72static int testthread (void *);
73static void sched_init (void);
74static int thread_create (int (*func) (void *), void *arg);
75static int thread_start (int id);
76static void thread_yield (void);
77static int thread_delete (int id);
78static int thread_join (int *ret);
79
80#if 0
81static int thread_stop (int id);
82#endif
83
84
85
86#define NUMTHREADS 7
87int 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
142static 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
169static 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
183static 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
220static 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
238static 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
250static 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
267static 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 *)<hreads[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
292static 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
304
305static 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
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
326
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
341 thread_yield ();
342
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}
357
