1 #include "common.h"
2 #include <ddekit/assert.h>
3 #include <ddekit/condvar.h>
4 #include <ddekit/memory.h>
5 #include <ddekit/panic.h>
6 #include <ddekit/timer.h>
7
8
9 #ifdef DDEBUG_LEVEL_THREAD
10 #undef DDEBUG
11 #define DDEBUG DDEBUG_LEVEL_THREAD
12 #endif
13
14 //#define DDEBUG DDEBUG_VERBOSE
15
16 #include "debug.h"
17 #include "util.h"
18 #include "thread.h"
19 #include "timer.h"
20
21
22 /* Incremented to generate unique thread IDs */
23 static unsigned id;
24
25 static ddekit_thread_t *ready_queue[DDEKIT_THREAD_PRIOS];
26
27 static ddekit_thread_t *sleep_queue;
28
29 /* Handle to the running thread, set in _dde_kit_thread_schedule() */
30 static ddekit_thread_t *current = NULL;
31
32 static void _ddekit_thread_start(ddekit_thread_t *th);
33 static void _ddekit_thread_sleep(unsigned long until);
34 static void _ddekit_dump_queues(void);
35
36 /*****************************************************************************
37 * _ddekit_thread_start *
38 ****************************************************************************/
_ddekit_thread_start(ddekit_thread_t * th)39 static void _ddekit_thread_start(ddekit_thread_t *th)
40 {
41 /* entry point of newly created threads */
42 th->fun(th->arg);
43 ddekit_thread_exit();
44 }
45
46 /*****************************************************************************
47 * _ddekit_thread_sleep *
48 ****************************************************************************/
_ddekit_thread_sleep(unsigned long until)49 static void _ddekit_thread_sleep(unsigned long until)
50 {
51 current->next = sleep_queue;
52 sleep_queue = current;
53 current->sleep_until = until;
54 _ddekit_thread_schedule();
55
56 }
57
58 /*****************************************************************************
59 * _ddekit_dump_queues *
60 ****************************************************************************/
61 static void
_ddekit_dump_queues(void)62 _ddekit_dump_queues(void)
63 {
64 #if DDEBUG >= DDEBUG_VERBOSE
65 ddekit_thread_t * current_thread;
66 int i;
67
68 for (i = 0; i < DDEKIT_THREAD_PRIOS; i++) {
69 current_thread = ready_queue[i];
70
71 ddekit_printf("Ready queue #%d: ", i);
72
73 while (NULL != current_thread) {
74 ddekit_printf("0x%08X ", (int)current_thread);
75 current_thread = current_thread->next;
76 }
77
78 ddekit_printf("\n");
79 }
80
81 {
82 current_thread = sleep_queue;
83
84 ddekit_printf("Sleep queue: ");
85
86 while (NULL != current_thread) {
87 ddekit_printf("0x%08X ", (int)current_thread);
88 current_thread = current_thread->next;
89 }
90
91 ddekit_printf("\n");
92 }
93
94 ddekit_printf("Current thread: 0x%08X\n", (int)current);
95 #endif
96 }
97
98 /*****************************************************************************
99 * DDEKIT public thread API (ddekit/thread.h) *
100 ****************************************************************************/
101
102 /*****************************************************************************
103 * ddekit_yield *
104 ****************************************************************************/
ddekit_yield()105 void ddekit_yield()
106 {
107 ddekit_thread_schedule();
108 }
109
110 /*****************************************************************************
111 * ddekit_thread_schedule *
112 ****************************************************************************/
ddekit_thread_schedule()113 void ddekit_thread_schedule()
114 {
115 _ddekit_thread_enqueue(current);
116 _ddekit_thread_schedule();
117 }
118
119 /*****************************************************************************
120 * ddekit_thread_create *
121 ****************************************************************************/
122 ddekit_thread_t *
ddekit_thread_create(void (* fun)(void *),void * arg,const char * name)123 ddekit_thread_create(void (*fun)(void *), void *arg, const char *name)
124 {
125 ddekit_thread_t *th =
126 (ddekit_thread_t *) ddekit_simple_malloc(sizeof(ddekit_thread_t));
127 memset(th,0,sizeof(ddekit_thread_t));
128 strncpy(th->name, name, DDEKIT_THREAD_NAMELEN);
129 th->name[DDEKIT_THREAD_NAMELEN-1] = 0;
130
131 th->stack = ddekit_simple_malloc(DDEKIT_THREAD_STACKSIZE);
132
133 th->arg = arg;
134 th->fun = fun;
135
136 th->id = id++;
137 th->prio = DDEKIT_THREAD_STDPRIO;
138 th->next = NULL;
139 th->sleep_sem = ddekit_sem_init(0);
140
141 /* Setup thread context */
142 th->ctx.uc_flags |= _UC_IGNSIGM | _UC_IGNFPU;
143 if (getcontext(&th->ctx) != 0) {
144 panic("ddekit thread create thread getcontext error");
145 }
146 th->ctx.uc_stack.ss_sp = th->stack;/* makecontext will determine sp */
147 th->ctx.uc_stack.ss_size = DDEKIT_THREAD_STACKSIZE;
148 makecontext(&th->ctx,_ddekit_thread_start,1 /* argc */,th /* pass thread as argument */);
149 DDEBUG_MSG_VERBOSE("created thread %s, stack at: %p\n", name,
150 th->stack + DDEKIT_THREAD_STACKSIZE);
151 _ddekit_thread_enqueue(th);
152
153 return th;
154 }
155
156 /*****************************************************************************
157 * ddekit_thread_get_data *
158 ****************************************************************************/
ddekit_thread_get_data(ddekit_thread_t * thread)159 void *ddekit_thread_get_data(ddekit_thread_t *thread)
160 {
161 return thread->data;
162 }
163
164 /*****************************************************************************
165 * ddekit_thread_get_my_data *
166 ****************************************************************************/
ddekit_thread_get_my_data(void)167 void *ddekit_thread_get_my_data(void)
168 {
169 return current->data;
170 }
171
172 /*****************************************************************************
173 * ddekit_thread_myself *
174 ****************************************************************************/
175
ddekit_thread_myself(void)176 ddekit_thread_t *ddekit_thread_myself(void)
177 {
178 return current;
179 }
180
181 /*****************************************************************************
182 * ddekit_thread_setup_myself *
183 ****************************************************************************/
184
ddekit_thread_setup_myself(const char * name)185 ddekit_thread_t *ddekit_thread_setup_myself(const char *name) {
186 ddekit_thread_t *th =
187 (ddekit_thread_t *) ddekit_simple_malloc(sizeof(ddekit_thread_t));
188 memset(th,0,sizeof(ddekit_thread_t));
189 strncpy(th->name, name, DDEKIT_THREAD_NAMELEN);
190 th->name[DDEKIT_THREAD_NAMELEN-1] = 0;
191 th->stack = NULL;
192 th->next = NULL;
193 th->id = id++;
194 th->prio = DDEKIT_THREAD_STDPRIO;
195 th->sleep_sem = ddekit_sem_init(0);
196 #if DDEBUG >= 4
197 _ddekit_print_backtrace(th);
198 #endif
199 return th;
200 }
201
202 /*****************************************************************************
203 * ddekit_thread_set_data *
204 ****************************************************************************/
ddekit_thread_set_data(ddekit_thread_t * thread,void * data)205 void ddekit_thread_set_data(ddekit_thread_t *thread, void *data)
206 {
207 thread->data=data;
208 }
209
210 /*****************************************************************************
211 * ddekit_thread_set_my_data *
212 ****************************************************************************/
ddekit_thread_set_my_data(void * data)213 void ddekit_thread_set_my_data(void *data)
214 {
215 current->data = data;
216 }
217
218 /*****************************************************************************
219 * ddekit_thread_usleep *
220 ****************************************************************************/
ddekit_thread_usleep(unsigned long usecs)221 void ddekit_thread_usleep(unsigned long usecs)
222 {
223 /*
224 * Cannot use usleep here, because it's implemented in vfs.
225 * Assuming the anyway no finder granularity than system's HZ value
226 * can be reached. So we use dde_kit_thread_msleep for now.
227 */
228
229 /* If no timeout is 0 return immediately */
230 if (usecs == 0)
231 return;
232
233 unsigned long to = usecs/1000;
234
235 /* round up to to possible granularity */
236
237 if (to == 0)
238 to = 1;
239
240 ddekit_thread_msleep(to);
241 }
242
243 /*****************************************************************************
244 * ddekit_thread_nsleep *
245 ****************************************************************************/
ddekit_thread_nsleep(unsigned long nsecs)246 void ddekit_thread_nsleep(unsigned long nsecs)
247 {
248 /*
249 * Cannot use usleep here, because it's implemented in vfs.
250 * Assuming the anyway no finder granularity than system's HZ value
251 * can be reached. So we use dde_kit_thread_msleep.
252 */
253
254 /* If no timeout is 0 return immediately */
255 if (nsecs == 0)
256 return;
257
258 unsigned long to = nsecs/1000;
259
260 /* round up to to possible granularity */
261
262 if (to == 0)
263 to = 1;
264
265 ddekit_thread_usleep(to);
266 }
267
268 /*****************************************************************************
269 * ddekit_thread_msleep *
270 ****************************************************************************/
ddekit_thread_msleep(unsigned long msecs)271 void ddekit_thread_msleep(unsigned long msecs)
272 {
273 unsigned long to;
274
275 to = (msecs*HZ/1000);
276
277 if (to == 0) {
278 to = 1;
279 }
280
281 ddekit_thread_t *th = ddekit_thread_myself();
282
283 if (th == NULL) {
284 ddekit_panic("th==NULL!");
285 }
286
287 if (th->sleep_sem == NULL) {
288 ddekit_panic("th->sleepsem==NULL! %p %s ", th, th->name);
289 }
290
291 /* generate a timer interrupt at to */
292 ddekit_add_timer(NULL, NULL, to+jiffies);
293 _ddekit_thread_sleep(to+jiffies);
294 }
295
296 /*****************************************************************************
297 * ddekit_thread_sleep *
298 ****************************************************************************/
ddekit_thread_sleep(ddekit_lock_t * lock)299 void ddekit_thread_sleep(ddekit_lock_t *lock)
300 {
301 WARN_UNIMPL;
302 }
303
304 /*****************************************************************************
305 * ddekit_thread_exit *
306 ****************************************************************************/
ddekit_thread_exit()307 void ddekit_thread_exit()
308 {
309 ddekit_sem_down(current->sleep_sem);
310 ddekit_panic("thread running after exit!\n");
311 /* not reached */
312 while(1);
313 }
314
315 /*****************************************************************************
316 * ddekit_thread_terminate *
317 ****************************************************************************/
318 void
ddekit_thread_terminate(ddekit_thread_t * thread)319 ddekit_thread_terminate(ddekit_thread_t * thread)
320 {
321 if (thread == ddekit_thread_myself()) {
322 /* TODO: Whether or not this is an error, is to be decided.
323 * Memory (especially stack) freeing should be somehow
324 * postponed when such termination is legal. */
325 ddekit_panic("Thread attempted termination of itself!\n");
326 }
327
328 _ddekit_thread_dequeue(thread);
329
330 ddekit_sem_deinit(thread->sleep_sem);
331
332 ddekit_simple_free(thread->stack);
333
334 ddekit_simple_free(thread);
335 }
336
337 /*****************************************************************************
338 * ddekit_thread_get_name *
339 ****************************************************************************/
ddekit_thread_get_name(ddekit_thread_t * thread)340 const char *ddekit_thread_get_name(ddekit_thread_t *thread)
341 {
342 return thread->name;
343 }
344
345 /*****************************************************************************
346 * ddekit_thread_get_id *
347 ****************************************************************************/
ddekit_thread_get_id(ddekit_thread_t * thread)348 int ddekit_thread_get_id(ddekit_thread_t *thread)
349 {
350 return thread->id;
351 }
352
353 /*****************************************************************************
354 * ddekit_init_threads *
355 ****************************************************************************/
ddekit_init_threads(void)356 void ddekit_init_threads(void)
357 {
358 int i;
359
360 for (i =0 ; i < DDEKIT_THREAD_PRIOS ; i++) {
361 ready_queue[i] = NULL;
362 }
363
364 current = ddekit_thread_setup_myself("main");
365
366 DDEBUG_MSG_INFO("ddekit thread subsystem initialized");
367 }
368
369 /*****************************************************************************
370 * DDEKIT internals (src/thread.h) *
371 *****************************************************************************/
372
373 /*****************************************************************************
374 * _ddekit_thread_schedule *
375 ****************************************************************************/
_ddekit_thread_schedule()376 void _ddekit_thread_schedule()
377 {
378
379 DDEBUG_MSG_VERBOSE("called schedule id: %d name %s, prio: %d",
380 current->id, current->name, current->prio);
381
382 /* get our tcb */
383 ddekit_thread_t * th = current;
384 volatile int is_callback;
385
386 #if DDEBUG >= 4
387 _ddekit_print_backtrace(th);
388 #endif
389 /* getcontext saves the current context in ctx. When setcontext is called
390 * with that ctx it will return execution at getcontext here. To
391 * discriminate between the initial call to getcontext that simply returns
392 * and the situation where getcontext returns because of a setcontext call
393 * we use the is_callback variable.
394 *
395 * When the program flow passes via the assignment bellow it will enter
396 * the scheduling loop and set is_callback to 1. When the function returns
397 * because of a setcontext call the program skip the scheduling and return
398 * from this method to continue normal execution.
399 */
400 is_callback =0;
401 /* save our context */
402 th->ctx.uc_flags |= _UC_IGNSIGM | _UC_IGNFPU;
403 if (getcontext(&th->ctx) != 0){
404 panic("ddekit thread schedule getcontext error");
405 }
406 if (is_callback == 0) {
407 is_callback = 1;
408 int i;
409
410 /* find a runnable thread */
411
412 current = NULL;
413
414 for (i = DDEKIT_THREAD_PRIOS-1; i >= 0; i--) {
415 if (ready_queue[i]!=NULL) {
416 current = ready_queue[i];
417 ready_queue[i] = current->next;
418 current->next=NULL;
419 break;
420 }
421 }
422
423 if (current == NULL) {
424 ddekit_panic("No runnable threads?!");
425 }
426
427 DDEBUG_MSG_VERBOSE("switching to id: %d name %s, prio: %d",
428 current->id, current->name, current->prio);
429 #if DDEBUG >= 4
430 _ddekit_print_backtrace(current);
431 #endif
432 //th->ctx.uc_flags |= _UC_IGNSIGM | _UC_IGNFPU;
433 if (setcontext(¤t->ctx) == -1){
434 panic("ddekit threading setcontext error");
435 }
436 panic("unreachable code");
437 }
438 DDEBUG_MSG_VERBOSE("continuing thread execution id: %d name %s, prio: %d",
439 current->id, current->name, current->prio);
440
441 }
442
443 /*****************************************************************************
444 * _ddekit_thread_enqueue *
445 ****************************************************************************/
_ddekit_thread_enqueue(ddekit_thread_t * th)446 void _ddekit_thread_enqueue(ddekit_thread_t *th)
447 {
448 DDEBUG_MSG_VERBOSE("Enqueuing thread 0x%08X: id %d, name %s, prio %d",
449 (int)th, th->id, th->name, th->prio);
450
451 #if DDEBUG >= 4
452 _ddekit_print_backtrace(th);
453 #endif
454
455 ddekit_assert(th->next==NULL);
456
457 if (ready_queue[th->prio] != NULL) {
458 ddekit_thread_t *pos = ready_queue[th->prio];
459 while (pos->next != NULL) {
460 pos = pos->next;
461 }
462 pos->next = th;
463 } else {
464 ready_queue[th->prio] = th;
465 }
466 }
467
468 /*****************************************************************************
469 * _ddekit_thread_dequeue *
470 ****************************************************************************/
471 void
_ddekit_thread_dequeue(ddekit_thread_t * th)472 _ddekit_thread_dequeue(ddekit_thread_t * th)
473 {
474 ddekit_thread_t * current_thread;
475 ddekit_thread_t * previous_thread;
476
477 DDEBUG_MSG_VERBOSE("Dequeuing thread 0x%08X: id %d, name %s, prio %d",
478 (int)th, th->id, th->name, th->prio);
479
480 ddekit_assert((th->prio < DDEKIT_THREAD_PRIOS) && (th->prio >= 0));
481
482 /* Dump queues when debugging */
483 _ddekit_dump_queues();
484
485 /* Check ready queue (based on thread's priority) for thread */
486 current_thread = ready_queue[th->prio];
487 previous_thread = NULL;
488
489 while (NULL != current_thread) {
490
491 /* On match... */
492 if (th == current_thread) {
493
494 if (previous_thread) {
495 /* ...fix previous element to remove current */
496 previous_thread->next = current_thread->next;
497 } else {
498 /* ...alter queue start to reflect removal */
499 ready_queue[th->prio] = current_thread->next;
500 }
501
502 /* Thread found and dequeued */
503 DDEBUG_MSG_VERBOSE("Dequeued 'ready[%d]': 0x%08X",
504 th->prio, (int)th);
505 return;
506 }
507
508 /* Next thread */
509 previous_thread = current_thread;
510 current_thread = current_thread->next;
511 }
512
513 /* When previous loop fails, check if thread is sleeping */
514 current_thread = sleep_queue;
515 previous_thread = NULL;
516
517 while (NULL != current_thread) {
518
519 /* On match... */
520 if (th == current_thread) {
521
522 if (previous_thread) {
523 /* ...fix previous element to remove current */
524 previous_thread->next = current_thread->next;
525 } else {
526 /* ...alter queue start to reflect removal */
527 sleep_queue = current_thread->next;
528 }
529
530 /* Thread found and dequeued */
531 DDEBUG_MSG_VERBOSE("Dequeued 'sleep': 0x%08X", (int)th);
532 return;
533 }
534
535 /* Next thread */
536 previous_thread = current_thread;
537 current_thread = current_thread->next;
538 }
539
540 /* Thread may exist and not be enqueued at
541 * all (is bound to semaphore for instance) */
542 DDEBUG_MSG_VERBOSE("Thread 0x%08X was not enqueued!", (int)th);
543 }
544
545 /*****************************************************************************
546 * _ddekit_thread_set_myprio *
547 ****************************************************************************/
_ddekit_thread_set_myprio(int prio)548 void _ddekit_thread_set_myprio(int prio)
549 {
550 DDEBUG_MSG_VERBOSE("changing thread prio, id: %d name %s, old prio: %d, "
551 "new prio: %d", current->id, current->name, current->prio, prio);
552
553 current->prio = prio;
554 ddekit_thread_schedule();
555 }
556
557 /*****************************************************************************
558 * _ddekit_thread_wakeup_sleeping *
559 ****************************************************************************/
_ddekit_thread_wakeup_sleeping()560 void _ddekit_thread_wakeup_sleeping()
561 {
562 ddekit_thread_t *th = sleep_queue;
563
564 sleep_queue = NULL;
565
566 while (th != NULL) {
567 ddekit_thread_t *th1 = th->next;
568 if (th->sleep_until > jiffies) {
569 th->next = sleep_queue;
570 sleep_queue = th;
571 } else {
572 th->next = NULL;
573 _ddekit_thread_enqueue(th);
574 }
575 th = th1;
576 }
577
578 ddekit_thread_schedule();
579 }
580
581 #define FUNC_STACKTRACE(statement) \
582 { \
583 reg_t bp, pc, hbp; \
584 extern reg_t get_bp(void); \
585 \
586 bp= get_bp(); \
587 while(bp) \
588 { \
589 pc= ((reg_t *)bp)[1]; \
590 hbp= ((reg_t *)bp)[0]; \
591 statement; \
592 if (hbp != 0 && hbp <= bp) \
593 { \
594 pc = -1; \
595 statement; \
596 break; \
597 } \
598 bp= hbp; \
599 } \
600 }
601
602 /*****************************************************************************
603 * _ddekit_print_backtrace *
604 ****************************************************************************/
_ddekit_print_backtrace(ddekit_thread_t * th)605 void _ddekit_print_backtrace(ddekit_thread_t *th)
606 {
607 #if defined(__i386)
608 unsigned long bp, pc, hbp;
609
610 ddekit_printf("%s: ", th->name);
611
612 bp = th->ctx.uc_mcontext.__gregs[_REG_EBP];
613 while (bp) {
614 pc = ((unsigned long *)bp)[1];
615 hbp = ((unsigned long *)bp)[0];
616
617 ddekit_printf("0x%lx ", (unsigned long) pc);
618
619 if (hbp != 0 && hbp <= bp) {
620 pc = -1;
621 ddekit_printf("0x%lx ", (unsigned long) pc);
622 break;
623 }
624 bp= hbp;
625 }
626
627 ddekit_printf("\n");
628 #endif
629 }
630