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 ****************************************************************************/ 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 ****************************************************************************/ 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 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 ****************************************************************************/ 105 void ddekit_yield() 106 { 107 ddekit_thread_schedule(); 108 } 109 110 /***************************************************************************** 111 * ddekit_thread_schedule * 112 ****************************************************************************/ 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 * 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 ****************************************************************************/ 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 ****************************************************************************/ 167 void *ddekit_thread_get_my_data(void) 168 { 169 return current->data; 170 } 171 172 /***************************************************************************** 173 * ddekit_thread_myself * 174 ****************************************************************************/ 175 176 ddekit_thread_t *ddekit_thread_myself(void) 177 { 178 return current; 179 } 180 181 /***************************************************************************** 182 * ddekit_thread_setup_myself * 183 ****************************************************************************/ 184 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 ****************************************************************************/ 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 ****************************************************************************/ 213 void ddekit_thread_set_my_data(void *data) 214 { 215 current->data = data; 216 } 217 218 /***************************************************************************** 219 * ddekit_thread_usleep * 220 ****************************************************************************/ 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 ****************************************************************************/ 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 ****************************************************************************/ 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 ****************************************************************************/ 299 void ddekit_thread_sleep(ddekit_lock_t *lock) 300 { 301 WARN_UNIMPL; 302 } 303 304 /***************************************************************************** 305 * ddekit_thread_exit * 306 ****************************************************************************/ 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 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 ****************************************************************************/ 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 ****************************************************************************/ 348 int ddekit_thread_get_id(ddekit_thread_t *thread) 349 { 350 return thread->id; 351 } 352 353 /***************************************************************************** 354 * ddekit_init_threads * 355 ****************************************************************************/ 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 ****************************************************************************/ 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 ****************************************************************************/ 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 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 ****************************************************************************/ 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 ****************************************************************************/ 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 ****************************************************************************/ 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