libogc/lwp_threads.c

   1 #include <stdlib.h>
   2 #include <stdio.h>
   3 #include <string.h>
   4 #include "asm.h"
   5 #include "processor.h"
   6 #include "sys_state.h"
   7 #include "lwp_stack.h"
   8 #include "lwp_threads.h"
   9 #include "lwp_threadq.h"
  10 #include "lwp_watchdog.h"
  11
  12 #define LWP_MAXPRIORITIES               256
  13
  14 /* new one */
  15 frame_context core_context;
  16
  17 lwp_cntrl *_thr_main = NULL;
  18 lwp_cntrl *_thr_idle = NULL;
  19
  20 lwp_cntrl *_thr_executing = NULL;
  21 lwp_cntrl *_thr_heir = NULL;
  22 lwp_cntrl *_thr_allocated_fp = NULL;
  23
  24 vu32 _context_switch_want;
  25 vu32 _thread_dispatch_disable_level;
  26
  27 wd_cntrl _lwp_wd_timeslice;
  28 u32 _lwp_ticks_per_timeslice = 0;
  29 void **__lwp_thr_libc_reent = NULL;
  30 lwp_queue _lwp_thr_ready[LWP_MAXPRIORITIES];
  31
  32 static void (*_lwp_exitfunc)(void);
  33
  34 extern void _cpu_context_switch(void *,void *);
  35 extern void _cpu_context_switch_ex(void *,void *);
  36 extern void _cpu_context_save(void *);
  37 extern void _cpu_context_restore(void *);
  38 extern void _cpu_context_save_fp(void *);
  39 extern void _cpu_context_restore_fp(void *);
  40
  41 extern int __libc_create_hook(lwp_cntrl *,lwp_cntrl *);
  42 extern int __libc_start_hook(lwp_cntrl *,lwp_cntrl *);
  43 extern int __libc_delete_hook(lwp_cntrl *, lwp_cntrl *);
  44
  45 extern void kprintf(const char *str, ...);
  46
  47 #ifdef _LWPTHREADS_DEBUG
  48 static void __lwp_dumpcontext(frame_context *ctx)
  49 {
  50         kprintf("GPR00 %08x GPR08 %08x GPR16 %08x GPR24 %08x\n",ctx->GPR[0], ctx->GPR[8], ctx->GPR[16], ctx->GPR[24]);
  51         kprintf("GPR01 %08x GPR09 %08x GPR17 %08x GPR25 %08x\n",ctx->GPR[1], ctx->GPR[9], ctx->GPR[17], ctx->GPR[25]);
  52         kprintf("GPR02 %08x GPR10 %08x GPR18 %08x GPR26 %08x\n",ctx->GPR[2], ctx->GPR[10], ctx->GPR[18], ctx->GPR[26]);
  53         kprintf("GPR03 %08x GPR11 %08x GPR19 %08x GPR27 %08x\n",ctx->GPR[3], ctx->GPR[11], ctx->GPR[19], ctx->GPR[27]);
  54         kprintf("GPR04 %08x GPR12 %08x GPR20 %08x GPR28 %08x\n",ctx->GPR[4], ctx->GPR[12], ctx->GPR[20], ctx->GPR[28]);
  55         kprintf("GPR05 %08x GPR13 %08x GPR21 %08x GPR29 %08x\n",ctx->GPR[5], ctx->GPR[13], ctx->GPR[21], ctx->GPR[29]);
  56         kprintf("GPR06 %08x GPR14 %08x GPR22 %08x GPR30 %08x\n",ctx->GPR[6], ctx->GPR[14], ctx->GPR[22], ctx->GPR[30]);
  57         kprintf("GPR07 %08x GPR15 %08x GPR23 %08x GPR31 %08x\n",ctx->GPR[7], ctx->GPR[15], ctx->GPR[23], ctx->GPR[31]);
  58         kprintf("LR %08x SRR0 %08x SRR1 %08x MSR %08x\n\n", ctx->LR, ctx->SRR0, ctx->SRR1,ctx->MSR);
  59 }
  60
  61 void __lwp_showmsr()
  62 {
  63         register u32 msr;
  64         _CPU_MSR_GET(msr);
  65         kprintf("msr: %08x\n",msr);
  66 }
  67
  68 void __lwp_dumpcontext_fp(lwp_cntrl *thrA,lwp_cntrl *thrB)
  69 {
  70         kprintf("_cpu_contextfp_dump(%p,%p)\n",thrA,thrB);
  71 }
  72 #endif
  73
  74 /*
  75 void __lwp_getthreadlist(lwp_obj **thrs)
  76 {
  77         *thrs = _lwp_objects;
  78 }
  79 */
  80 u32 __lwp_isr_in_progress()
  81 {
  82         register u32 isr_nest_level;
  83         isr_nest_level = mfspr(272);
  84         return isr_nest_level;
  85 }
  86
  87 static inline void __lwp_msr_setlevel(u32 level)
  88 {
  89         register u32 msr;
  90         _CPU_MSR_GET(msr);
  91         if(!(level&CPU_MODES_INTERRUPT_MASK))
  92                 msr |= MSR_EE;
  93         else
  94                 msr &= ~MSR_EE;
  95         _CPU_MSR_SET(msr);
  96 }
  97
  98 static inline u32 __lwp_msr_getlevel()
  99 {
 100         register u32 msr;
 101         _CPU_MSR_GET(msr);
 102         if(msr&MSR_EE) return 0;
 103         else return 1;
 104 }
 105
 106 void __lwp_thread_delayended(void *arg)
 107 {
 108         lwp_cntrl *thethread = (lwp_cntrl*)arg;
 109 #ifdef _LWPTHREADS_DEBUG
 110         kprintf("__lwp_thread_delayended(%p)\n",thethread);
 111 #endif
 112         if(!thethread) return;
 113
 114         __lwp_thread_dispatchdisable();
 115         __lwp_thread_unblock(thethread);
 116         __lwp_thread_dispatchunnest();
 117 }
 118
 119 void __lwp_thread_tickle_timeslice(void *arg)
 120 {
 121         s64 ticks;
 122         lwp_cntrl *exec;
 123
 124         exec = _thr_executing;
 125         ticks = millisecs_to_ticks(1);
 126
 127         __lwp_thread_dispatchdisable();
 128
 129         if(!exec->is_preemptible) {
 130                 __lwp_wd_insert_ticks(&_lwp_wd_timeslice,ticks);
 131                 __lwp_thread_dispatchunnest();
 132                 return;
 133         }
 134         if(!__lwp_stateready(exec->cur_state)) {
 135                 __lwp_wd_insert_ticks(&_lwp_wd_timeslice,ticks);
 136                 __lwp_thread_dispatchunnest();
 137                 return;
 138         }
 139
 140         switch(exec->budget_algo) {
 141                 case LWP_CPU_BUDGET_ALGO_NONE:
 142                         break;
 143                 case LWP_CPU_BUDGET_ALGO_TIMESLICE:
 144                         if((--exec->cpu_time_budget)==0) {
 145                                 __lwp_thread_resettimeslice();
 146                                 exec->cpu_time_budget = _lwp_ticks_per_timeslice;
 147                         }
 148                         break;
 149         }
 150
 151         __lwp_wd_insert_ticks(&_lwp_wd_timeslice,ticks);
 152         __lwp_thread_dispatchunnest();
 153 }
 154
 155 void __thread_dispatch_fp()
 156 {
 157         u32 level;
 158         lwp_cntrl *exec;
 159
 160         _CPU_ISR_Disable(level);
 161         exec = _thr_executing;
 162 #ifdef _LWPTHREADS_DEBUG
 163         __lwp_dumpcontext_fp(exec,_thr_allocated_fp);
 164 #endif
 165         if(!__lwp_thread_isallocatedfp(exec)) {
 166                 if(_thr_allocated_fp) _cpu_context_save_fp(&_thr_allocated_fp->context);
 167                 _cpu_context_restore_fp(&exec->context);
 168                 _thr_allocated_fp = exec;
 169         }
 170         _CPU_ISR_Restore(level);
 171 }
 172
 173 void __thread_dispatch()
 174 {
 175         u32 level;
 176         lwp_cntrl *exec,*heir;
 177
 178         _CPU_ISR_Disable(level);
 179         exec = _thr_executing;
 180         while(_context_switch_want==TRUE) {
 181                 heir = _thr_heir;
 182                 _thread_dispatch_disable_level = 1;
 183                 _context_switch_want = FALSE;
 184                 _thr_executing = heir;
 185                 _CPU_ISR_Restore(level);
 186
 187                 if(__lwp_thr_libc_reent) {
 188                         exec->libc_reent = *__lwp_thr_libc_reent;
 189                         *__lwp_thr_libc_reent = heir->libc_reent;
 190                 }
 191 #ifdef _DEBUG
 192                 _cpu_context_switch_ex((void*)&exec->context,(void*)&heir->context);
 193 #else
 194                 _cpu_context_switch((void*)&exec->context,(void*)&heir->context);
 195 #endif
 196                 exec = _thr_executing;
 197                 _CPU_ISR_Disable(level);
 198         }
 199         _thread_dispatch_disable_level = 0;
 200         _CPU_ISR_Restore(level);
 201 }
 202
 203 static void __lwp_thread_handler()
 204 {
 205         u32 level;
 206         lwp_cntrl *exec;
 207
 208         exec = _thr_executing;
 209 #ifdef _LWPTHREADS_DEBUG
 210         kprintf("__lwp_thread_handler(%p,%d)\n",exec,_thread_dispatch_disable_level);
 211 #endif
 212         level = exec->isr_level;
 213         __lwp_msr_setlevel(level);
 214         __lwp_thread_dispatchenable();
 215         exec->wait.ret_arg = exec->entry(exec->arg);
 216
 217         __lwp_thread_exit(exec->wait.ret_arg);
 218 #ifdef _LWPTHREADS_DEBUG
 219         kprintf("__lwp_thread_handler(%p): thread returned(%p)\n",exec,exec->wait.ret_arg);
 220 #endif
 221 }
 222
 223 void __lwp_rotate_readyqueue(u32 prio)
 224 {
 225         u32 level;
 226         lwp_cntrl *exec;
 227         lwp_queue *ready;
 228         lwp_node *node;
 229
 230         ready = &_lwp_thr_ready[prio];
 231         exec = _thr_executing;
 232
 233         if(ready==exec->ready) {
 234                 __lwp_thread_yield();
 235                 return;
 236         }
 237
 238         _CPU_ISR_Disable(level);
 239         if(!__lwp_queue_isempty(ready) && !__lwp_queue_onenode(ready)) {
 240                 node = __lwp_queue_firstnodeI(ready);
 241                 __lwp_queue_appendI(ready,node);
 242         }
 243         _CPU_ISR_Flash(level);
 244
 245         if(_thr_heir->ready==ready)
 246                 _thr_heir = (lwp_cntrl*)ready->first;
 247
 248         if(exec!=_thr_heir)
 249                 _context_switch_want = TRUE;
 250
 251 #ifdef _LWPTHREADS_DEBUG
 252         kprintf("__lwp_rotate_readyqueue(%d,%p,%p)\n",prio,exec,_thr_heir);
 253 #endif
 254         _CPU_ISR_Restore(level);
 255 }
 256
 257 void __lwp_thread_yield()
 258 {
 259         u32 level;
 260         lwp_cntrl *exec;
 261         lwp_queue *ready;
 262
 263         exec = _thr_executing;
 264         ready = exec->ready;
 265
 266         _CPU_ISR_Disable(level);
 267         if(!__lwp_queue_onenode(ready)) {
 268                 __lwp_queue_extractI(&exec->object.node);
 269                 __lwp_queue_appendI(ready,&exec->object.node);
 270                 _CPU_ISR_Flash(level);
 271                 if(__lwp_thread_isheir(exec))
 272                         _thr_heir = (lwp_cntrl*)ready->first;
 273                 _context_switch_want = TRUE;
 274         } else if(!__lwp_thread_isheir(exec))
 275                 _context_switch_want = TRUE;
 276         _CPU_ISR_Restore(level);
 277 }
 278
 279 void __lwp_thread_resettimeslice()
 280 {
 281         u32 level;
 282         lwp_cntrl *exec;
 283         lwp_queue *ready;
 284
 285         exec = _thr_executing;
 286         ready = exec->ready;
 287
 288         _CPU_ISR_Disable(level);
 289         if(__lwp_queue_onenode(ready)) {
 290                 _CPU_ISR_Restore(level);
 291                 return;
 292         }
 293
 294         __lwp_queue_extractI(&exec->object.node);
 295         __lwp_queue_appendI(ready,&exec->object.node);
 296
 297         _CPU_ISR_Flash(level);
 298
 299         if(__lwp_thread_isheir(exec))
 300                 _thr_heir = (lwp_cntrl*)ready->first;
 301
 302         _context_switch_want = TRUE;
 303         _CPU_ISR_Restore(level);
 304 }
 305
 306 void __lwp_thread_setstate(lwp_cntrl *thethread,u32 state)
 307 {
 308         u32 level;
 309         lwp_queue *ready;
 310
 311         ready = thethread->ready;
 312 #ifdef _LWPTHREADS_DEBUG
 313         kprintf("__lwp_thread_setstate(%d,%p,%p,%08x)\n",_context_switch_want,_thr_heir,thethread,thethread->cur_state);
 314 #endif
 315         _CPU_ISR_Disable(level);
 316         if(!__lwp_stateready(thethread->cur_state)) {
 317                 thethread->cur_state = __lwp_clearstate(thethread->cur_state,state);
 318                 _CPU_ISR_Restore(level);
 319                 return;
 320         }
 321
 322         thethread->cur_state = state;
 323         if(__lwp_queue_onenode(ready)) {
 324                 __lwp_queue_init_empty(ready);
 325                 __lwp_priomap_removefrom(&thethread->priomap);
 326         } else
 327                 __lwp_queue_extractI(&thethread->object.node);
 328         _CPU_ISR_Flash(level);
 329
 330         if(__lwp_thread_isheir(thethread))
 331                 __lwp_thread_calcheir();
 332         if(__lwp_thread_isexec(thethread))
 333                 _context_switch_want = TRUE;
 334 #ifdef _LWPTHREADS_DEBUG
 335         kprintf("__lwp_thread_setstate(%d,%p,%p,%08x)\n",_context_switch_want,_thr_heir,thethread,thethread->cur_state);
 336 #endif
 337         _CPU_ISR_Restore(level);
 338 }
 339
 340 void __lwp_thread_clearstate(lwp_cntrl *thethread,u32 state)
 341 {
 342         u32 level,cur_state;
 343
 344         _CPU_ISR_Disable(level);
 345
 346         cur_state = thethread->cur_state;
 347         if(cur_state&state) {
 348                 cur_state = thethread->cur_state = __lwp_clearstate(cur_state,state);
 349                 if(__lwp_stateready(cur_state)) {
 350                         __lwp_priomap_addto(&thethread->priomap);
 351                         __lwp_queue_appendI(thethread->ready,&thethread->object.node);
 352                         _CPU_ISR_Flash(level);
 353
 354                         if(thethread->cur_prio<_thr_heir->cur_prio) {
 355                                 _thr_heir = thethread;
 356                                 if(_thr_executing->is_preemptible
 357                                         || thethread->cur_prio==0)
 358                                 _context_switch_want = TRUE;
 359                         }
 360                 }
 361         }
 362
 363         _CPU_ISR_Restore(level);
 364 }
 365
 366 u32 __lwp_evaluatemode()
 367 {
 368         lwp_cntrl *exec;
 369
 370         exec = _thr_executing;
 371         if(!__lwp_stateready(exec->cur_state)
 372                 || (!__lwp_thread_isheir(exec) && exec->is_preemptible)){
 373                 _context_switch_want = TRUE;
 374                 return TRUE;
 375         }
 376         return FALSE;
 377 }
 378
 379 void __lwp_thread_changepriority(lwp_cntrl *thethread,u32 prio,u32 prependit)
 380 {
 381         u32 level;
 382
 383         __lwp_thread_settransient(thethread);
 384
 385         if(thethread->cur_prio!=prio)
 386                 __lwp_thread_setpriority(thethread,prio);
 387
 388         _CPU_ISR_Disable(level);
 389
 390         thethread->cur_state = __lwp_clearstate(thethread->cur_state,LWP_STATES_TRANSIENT);
 391         if(!__lwp_stateready(thethread->cur_state)) {
 392                 _CPU_ISR_Restore(level);
 393                 return;
 394         }
 395
 396         __lwp_priomap_addto(&thethread->priomap);
 397         if(prependit)
 398                 __lwp_queue_prependI(thethread->ready,&thethread->object.node);
 399         else
 400                 __lwp_queue_appendI(thethread->ready,&thethread->object.node);
 401
 402         _CPU_ISR_Flash(level);
 403
 404         __lwp_thread_calcheir();
 405
 406         if(!(_thr_executing==_thr_heir)
 407                 && _thr_executing->is_preemptible)
 408                 _context_switch_want = TRUE;
 409
 410         _CPU_ISR_Restore(level);
 411 }
 412
 413 void __lwp_thread_setpriority(lwp_cntrl *thethread,u32 prio)
 414 {
 415         thethread->cur_prio = prio;
 416         thethread->ready = &_lwp_thr_ready[prio];
 417         __lwp_priomap_init(&thethread->priomap,prio);
 418 #ifdef _LWPTHREADS_DEBUG
 419         kprintf("__lwp_thread_setpriority(%p,%d,%p)\n",thethread,prio,thethread->ready);
 420 #endif
 421 }
 422
 423 void __lwp_thread_suspend(lwp_cntrl *thethread)
 424 {
 425         u32 level;
 426         lwp_queue *ready;
 427
 428         ready = thethread->ready;
 429
 430         _CPU_ISR_Disable(level);
 431         thethread->suspendcnt++;
 432         if(!__lwp_stateready(thethread->cur_state)) {
 433                 thethread->cur_state = __lwp_setstate(thethread->cur_state,LWP_STATES_SUSPENDED);
 434                 _CPU_ISR_Restore(level);
 435                 return;
 436         }
 437
 438         thethread->cur_state = LWP_STATES_SUSPENDED;
 439         if(__lwp_queue_onenode(ready)) {
 440                 __lwp_queue_init_empty(ready);
 441                 __lwp_priomap_removefrom(&thethread->priomap);
 442         } else {
 443                 __lwp_queue_extractI(&thethread->object.node);
 444         }
 445         _CPU_ISR_Flash(level);
 446
 447         if(__lwp_thread_isheir(thethread))
 448                 __lwp_thread_calcheir();
 449
 450         if(__lwp_thread_isexec(thethread))
 451                 _context_switch_want = TRUE;
 452
 453         _CPU_ISR_Restore(level);
 454 }
 455
 456 void __lwp_thread_settransient(lwp_cntrl *thethread)
 457 {
 458         u32 level,oldstates;
 459         lwp_queue *ready;
 460
 461         ready = thethread->ready;
 462
 463         _CPU_ISR_Disable(level);
 464
 465         oldstates = thethread->cur_state;
 466         thethread->cur_state = __lwp_setstate(oldstates,LWP_STATES_TRANSIENT);
 467
 468         if(__lwp_stateready(oldstates)) {
 469                 if(__lwp_queue_onenode(ready)) {
 470                         __lwp_queue_init_empty(ready);
 471                         __lwp_priomap_removefrom(&thethread->priomap);
 472                 } else {
 473                         __lwp_queue_extractI(&thethread->object.node);
 474                 }
 475         }
 476
 477         _CPU_ISR_Restore(level);
 478 }
 479
 480 void __lwp_thread_resume(lwp_cntrl *thethread,u32 force)
 481 {
 482         u32 level,state;
 483
 484         _CPU_ISR_Disable(level);
 485
 486         if(force==TRUE)
 487                 thethread->suspendcnt = 0;
 488         else
 489                 thethread->suspendcnt--;
 490
 491         if(thethread->suspendcnt>0) {
 492                 _CPU_ISR_Restore(level);
 493                 return;
 494         }
 495
 496         state = thethread->cur_state;
 497         if(state&LWP_STATES_SUSPENDED) {
 498                 state = thethread->cur_state = __lwp_clearstate(thethread->cur_state,LWP_STATES_SUSPENDED);
 499                 if(__lwp_stateready(state)) {
 500                         __lwp_priomap_addto(&thethread->priomap);
 501                         __lwp_queue_appendI(thethread->ready,&thethread->object.node);
 502                         _CPU_ISR_Flash(level);
 503                         if(thethread->cur_prio<_thr_heir->cur_prio) {
 504                                 _thr_heir = thethread;
 505                                 if(_thr_executing->is_preemptible
 506                                         || thethread->cur_prio==0)
 507                                 _context_switch_want = TRUE;
 508                         }
 509                 }
 510         }
 511         _CPU_ISR_Restore(level);
 512 }
 513
 514 void __lwp_thread_loadenv(lwp_cntrl *thethread)
 515 {
 516         u32 stackbase,sp,size;
 517         u32 r2,r13,msr_value;
 518
 519         thethread->context.FPSCR = 0x000000f8;
 520
 521         stackbase = (u32)thethread->stack;
 522         size = thethread->stack_size;
 523
 524         // tag both bottom & head of stack
 525         *((u32*)stackbase) = 0xDEADBABE;
 526         sp = stackbase+size-CPU_MINIMUM_STACK_FRAME_SIZE;
 527         sp &= ~(CPU_STACK_ALIGNMENT-1);
 528         *((u32*)sp) = 0;
 529
 530         thethread->context.GPR[1] = sp;
 531
 532         msr_value = (MSR_ME|MSR_IR|MSR_DR|MSR_RI);
 533         if(!(thethread->isr_level&CPU_MODES_INTERRUPT_MASK))
 534                 msr_value |= MSR_EE;
 535
 536         thethread->context.MSR = msr_value;
 537         thethread->context.LR = (u32)__lwp_thread_handler;
 538
 539         __asm__ __volatile__ ("mr %0,2; mr %1,13" : "=r" ((r2)), "=r" ((r13)));
 540         thethread->context.GPR[2] = r2;
 541         thethread->context.GPR[13] = r13;
 542
 543 #ifdef _LWPTHREADS_DEBUG
 544         kprintf("__lwp_thread_loadenv(%p,%p,%d,%p)\n",thethread,(void*)stackbase,size,(void*)sp);
 545 #endif
 546
 547 }
 548
 549 void __lwp_thread_ready(lwp_cntrl *thethread)
 550 {
 551         u32 level;
 552         lwp_cntrl *heir;
 553
 554         _CPU_ISR_Disable(level);
 555 #ifdef _LWPTHREADS_DEBUG
 556         kprintf("__lwp_thread_ready(%p)\n",thethread);
 557 #endif
 558         thethread->cur_state = LWP_STATES_READY;
 559         __lwp_priomap_addto(&thethread->priomap);
 560         __lwp_queue_appendI(thethread->ready,&thethread->object.node);
 561         _CPU_ISR_Flash(level);
 562
 563         __lwp_thread_calcheir();
 564         heir = _thr_heir;
 565         if(!(__lwp_thread_isexec(heir)) && _thr_executing->is_preemptible)
 566                 _context_switch_want = TRUE;
 567
 568         _CPU_ISR_Restore(level);
 569 }
 570
 571 u32 __lwp_thread_init(lwp_cntrl *thethread,void *stack_area,u32 stack_size,u32 prio,u32 isr_level,bool is_preemtible)
 572 {
 573         u32 act_stack_size = 0;
 574
 575 #ifdef _LWPTHREADS_DEBUG
 576         kprintf("__lwp_thread_init(%p,%p,%d,%d,%d)\n",thethread,stack_area,stack_size,prio,isr_level);
 577 #endif
 578
 579         if(!stack_area) {
 580                 if(!__lwp_stack_isenough(stack_size))
 581                         act_stack_size = CPU_MINIMUM_STACK_SIZE;
 582                 else
 583                         act_stack_size = stack_size;
 584
 585                 act_stack_size = __lwp_stack_allocate(thethread,act_stack_size);
 586                 if(!act_stack_size) return 0;
 587
 588                 thethread->stack_allocated = TRUE;
 589         } else {
 590                 thethread->stack = stack_area;
 591                 act_stack_size = stack_size;
 592                 thethread->stack_allocated = FALSE;
 593         }
 594         thethread->stack_size = act_stack_size;
 595
 596         __lwp_threadqueue_init(&thethread->join_list,LWP_THREADQ_MODEFIFO,LWP_STATES_WAITING_FOR_JOINATEXIT,0);
 597
 598         memset(&thethread->context,0,sizeof(thethread->context));
 599         memset(&thethread->wait,0,sizeof(thethread->wait));
 600
 601         thethread->budget_algo = (prio<128 ? LWP_CPU_BUDGET_ALGO_NONE : LWP_CPU_BUDGET_ALGO_TIMESLICE);
 602         thethread->is_preemptible = is_preemtible;
 603         thethread->isr_level = isr_level;
 604         thethread->real_prio = prio;
 605         thethread->cur_state = LWP_STATES_DORMANT;
 606         thethread->cpu_time_budget = _lwp_ticks_per_timeslice;
 607         thethread->suspendcnt = 0;
 608         thethread->res_cnt = 0;
 609         __lwp_thread_setpriority(thethread,prio);
 610
 611         __libc_create_hook(_thr_executing,thethread);
 612
 613         return 1;
 614 }
 615
 616 void __lwp_thread_close(lwp_cntrl *thethread)
 617 {
 618         u32 level;
 619         void **value_ptr;
 620         lwp_cntrl *p;
 621
 622         __lwp_thread_setstate(thethread,LWP_STATES_TRANSIENT);
 623
 624         if(!__lwp_threadqueue_extractproxy(thethread)) {
 625                 if(__lwp_wd_isactive(&thethread->timer))
 626                         __lwp_wd_remove_ticks(&thethread->timer);
 627         }
 628
 629         _CPU_ISR_Disable(level);
 630         value_ptr = (void**)thethread->wait.ret_arg;
 631         while((p=__lwp_threadqueue_dequeue(&thethread->join_list))!=NULL) {
 632                 *(void**)p->wait.ret_arg = value_ptr;
 633         }
 634         thethread->cpu_time_budget = 0;
 635         thethread->budget_algo = LWP_CPU_BUDGET_ALGO_NONE;
 636         _CPU_ISR_Restore(level);
 637
 638         __libc_delete_hook(_thr_executing,thethread);
 639
 640         if(__lwp_thread_isallocatedfp(thethread))
 641                 __lwp_thread_deallocatefp();
 642
 643         __lwp_stack_free(thethread);
 644
 645         __lwp_objmgr_close(thethread->object.information,&thethread->object);
 646         __lwp_objmgr_free(thethread->object.information,&thethread->object);
 647 }
 648
 649 void __lwp_thread_closeall()
 650 {
 651         u32 i,level;
 652         lwp_queue *header;
 653         lwp_cntrl *ptr,*next;
 654 #ifdef _LWPTHREADS_DEBUG
 655         kprintf("__lwp_thread_closeall(enter)\n");
 656 #endif
 657         _CPU_ISR_Disable(level);
 658         for(i=0;i<LWP_MAXPRIORITIES;i++) {
 659                 header = &_lwp_thr_ready[i];
 660                 ptr = (lwp_cntrl*)header->first;
 661                 while(ptr!=(lwp_cntrl*)__lwp_queue_tail(&_lwp_thr_ready[i])) {
 662                         next = (lwp_cntrl*)ptr->object.node.next;
 663                         if(ptr!=_thr_executing)
 664                                 __lwp_thread_close(ptr);
 665
 666                         ptr = next;
 667                 }
 668         }
 669         _CPU_ISR_Restore(level);
 670 #ifdef _LWPTHREADS_DEBUG
 671         kprintf("__lwp_thread_closeall(leave)\n");
 672 #endif
 673 }
 674
 675 void __lwp_thread_exit(void *value_ptr)
 676 {
 677         __lwp_thread_dispatchdisable();
 678         _thr_executing->wait.ret_arg = (u32*)value_ptr;
 679         __lwp_thread_close(_thr_executing);
 680         __lwp_thread_dispatchenable();
 681 }
 682
 683 u32 __lwp_thread_start(lwp_cntrl *thethread,void* (*entry)(void*),void *arg)
 684 {
 685 #ifdef _LWPTHREADS_DEBUG
 686         kprintf("__lwp_thread_start(%p,%p,%p,%d)\n",thethread,entry,arg,thethread->cur_state);
 687 #endif
 688         if(__lwp_statedormant(thethread->cur_state)) {
 689                 thethread->entry = entry;
 690                 thethread->arg = arg;
 691                 __lwp_thread_loadenv(thethread);
 692                 __lwp_thread_ready(thethread);
 693                 __libc_start_hook(_thr_executing,thethread);
 694                 return 1;
 695         }
 696         return 0;
 697 }
 698
 699 void __lwp_thread_startmultitasking()
 700 {
 701         _lwp_exitfunc = NULL;
 702
 703         __sys_state_set(SYS_STATE_BEGIN_MT);
 704         __sys_state_set(SYS_STATE_UP);
 705
 706         _context_switch_want = FALSE;
 707         _thr_executing = _thr_heir;
 708 #ifdef _LWPTHREADS_DEBUG
 709         kprintf("__lwp_start_multitasking(%p,%p)\n",_thr_executing,_thr_heir);
 710 #endif
 711         __lwp_thread_starttimeslice();
 712         _cpu_context_switch((void*)&core_context,(void*)&_thr_heir->context);
 713
 714         if(_lwp_exitfunc) _lwp_exitfunc();
 715 }
 716
 717 void __lwp_thread_stopmultitasking(void (*exitfunc)())
 718 {
 719         _lwp_exitfunc = exitfunc;
 720         if(__sys_state_get()!=SYS_STATE_SHUTDOWN) {
 721                 __lwp_thread_stoptimeslice();
 722                 __sys_state_set(SYS_STATE_SHUTDOWN);
 723                 _cpu_context_switch((void*)&_thr_executing->context,(void*)&core_context);
 724         }
 725 }
 726
 727 void __lwp_thread_coreinit()
 728 {
 729         u32 index;
 730
 731 #ifdef _LWPTHREADS_DEBUG
 732         kprintf("__lwp_sys_init()\n\n");
 733 #endif
 734         __lwp_thread_dispatchinitialize();
 735         __lwp_thread_inittimeslice();
 736
 737         _context_switch_want = FALSE;
 738         _thr_executing = NULL;
 739         _thr_heir = NULL;
 740         _thr_allocated_fp = NULL;
 741         _lwp_ticks_per_timeslice = 10;
 742
 743         memset(&core_context,0,sizeof(core_context));
 744
 745         for(index=0;index<=LWP_PRIO_MAX;index++)
 746                 __lwp_queue_init_empty(&_lwp_thr_ready[index]);
 747
 748         __sys_state_set(SYS_STATE_BEFORE_MT);
 749 }
 750