libogc/lwp_threadq.c

   1 #include <stdio.h>
   2 #include <lwp_watchdog.h>
   3 #include "asm.h"
   4 #include "lwp_threadq.h"
   5
   6 //#define _LWPTHRQ_DEBUG
   7
   8 static void __lwp_threadqueue_timeout(void *usr_data)
   9 {
  10         lwp_cntrl *thethread;
  11         lwp_thrqueue *thequeue;
  12
  13         __lwp_thread_dispatchdisable();
  14         thethread = (lwp_cntrl*)usr_data;
  15         thequeue = thethread->wait.queue;
  16         if(thequeue->sync_state!=LWP_THREADQ_SYNCHRONIZED && __lwp_thread_isexec(thethread)) {
  17                 if(thequeue->sync_state!=LWP_THREADQ_SATISFIED) thequeue->sync_state = LWP_THREADQ_TIMEOUT;
  18         } else {
  19                 thethread->wait.ret_code = thethread->wait.queue->timeout_state;
  20                 __lwp_threadqueue_extract(thethread->wait.queue,thethread);
  21         }
  22         __lwp_thread_dispatchunnest();
  23 }
  24
  25 lwp_cntrl* __lwp_threadqueue_firstfifo(lwp_thrqueue *queue)
  26 {
  27         if(!__lwp_queue_isempty(&queue->queues.fifo))
  28                 return (lwp_cntrl*)queue->queues.fifo.first;
  29
  30         return NULL;
  31 }
  32
  33 lwp_cntrl* __lwp_threadqueue_firstpriority(lwp_thrqueue *queue)
  34 {
  35         u32 index;
  36
  37         for(index=0;index<LWP_THREADQ_NUM_PRIOHEADERS;index++) {
  38                 if(!__lwp_queue_isempty(&queue->queues.priority[index]))
  39                         return (lwp_cntrl*)queue->queues.priority[index].first;
  40         }
  41         return NULL;
  42 }
  43
  44 void __lwp_threadqueue_enqueuefifo(lwp_thrqueue *queue,lwp_cntrl *thethread,u64 timeout)
  45 {
  46         u32 level,sync_state;
  47
  48         _CPU_ISR_Disable(level);
  49
  50         sync_state = queue->sync_state;
  51         queue->sync_state = LWP_THREADQ_SYNCHRONIZED;
  52 #ifdef _LWPTHRQ_DEBUG
  53         printf("__lwp_threadqueue_enqueuefifo(%p,%d)\n",thethread,sync_state);
  54 #endif
  55         switch(sync_state) {
  56                 case LWP_THREADQ_SYNCHRONIZED:
  57                         break;
  58                 case LWP_THREADQ_NOTHINGHAPPEND:
  59                         __lwp_queue_appendI(&queue->queues.fifo,&thethread->object.node);
  60                         _CPU_ISR_Restore(level);
  61                         return;
  62                 case LWP_THREADQ_TIMEOUT:
  63                         thethread->wait.ret_code = thethread->wait.queue->timeout_state;
  64                         _CPU_ISR_Restore(level);
  65                         break;
  66                 case LWP_THREADQ_SATISFIED:
  67                         if(__lwp_wd_isactive(&thethread->timer)) {
  68                                 __lwp_wd_deactivate(&thethread->timer);
  69                                 _CPU_ISR_Restore(level);
  70                                 __lwp_wd_remove_ticks(&thethread->timer);
  71                         } else
  72                                 _CPU_ISR_Restore(level);
  73
  74                         break;
  75         }
  76         __lwp_thread_unblock(thethread);
  77 }
  78
  79 lwp_cntrl* __lwp_threadqueue_dequeuefifo(lwp_thrqueue *queue)
  80 {
  81         u32 level;
  82         lwp_cntrl *ret;
  83
  84         _CPU_ISR_Disable(level);
  85         if(!__lwp_queue_isempty(&queue->queues.fifo)) {
  86                 ret = (lwp_cntrl*)__lwp_queue_firstnodeI(&queue->queues.fifo);
  87                 if(!__lwp_wd_isactive(&ret->timer)) {
  88                         _CPU_ISR_Restore(level);
  89                         __lwp_thread_unblock(ret);
  90                 } else {
  91                         __lwp_wd_deactivate(&ret->timer);
  92                         _CPU_ISR_Restore(level);
  93                         __lwp_wd_remove_ticks(&ret->timer);
  94                         __lwp_thread_unblock(ret);
  95                 }
  96                 return ret;
  97         }
  98
  99         switch(queue->sync_state) {
 100                 case LWP_THREADQ_SYNCHRONIZED:
 101                 case LWP_THREADQ_SATISFIED:
 102                         _CPU_ISR_Restore(level);
 103                         return NULL;
 104                 case LWP_THREADQ_NOTHINGHAPPEND:
 105                 case LWP_THREADQ_TIMEOUT:
 106                         queue->sync_state = LWP_THREADQ_SATISFIED;
 107                         _CPU_ISR_Restore(level);
 108                         return _thr_executing;
 109         }
 110         return NULL;
 111 }
 112
 113 void __lwp_threadqueue_enqueuepriority(lwp_thrqueue *queue,lwp_cntrl *thethread,u64 timeout)
 114 {
 115         u32 level,search_prio,header_idx,prio,block_state,sync_state;
 116         lwp_cntrl *search_thread;
 117         lwp_queue *header;
 118         lwp_node *cur_node,*next_node,*prev_node,*search_node;
 119
 120         __lwp_queue_init_empty(&thethread->wait.block2n);
 121
 122         prio = thethread->cur_prio;
 123         header_idx = prio/LWP_THREADQ_PRIOPERHEADER;
 124         header = &queue->queues.priority[header_idx];
 125         block_state = queue->state;
 126
 127         if(prio&LWP_THREADQ_REVERSESEARCHMASK) {
 128 #ifdef _LWPTHRQ_DEBUG
 129                 printf("__lwp_threadqueue_enqueuepriority(%p,reverse_search)\n",thethread);
 130 #endif
 131                 goto reverse_search;
 132         }
 133
 134 #ifdef _LWPTHRQ_DEBUG
 135         printf("__lwp_threadqueue_enqueuepriority(%p,forward_search)\n",thethread);
 136 #endif
 137 forward_search:
 138         search_prio = LWP_PRIO_MIN - 1;
 139         _CPU_ISR_Disable(level);
 140         search_thread = (lwp_cntrl*)header->first;
 141         while(!__lwp_queue_istail(header,(lwp_node*)search_thread)) {
 142                 search_prio = search_thread->cur_prio;
 143                 if(prio<=search_prio) break;
 144                 _CPU_ISR_Flash(level);
 145
 146                 if(!__lwp_statesset(search_thread->cur_state,block_state)) {
 147                         _CPU_ISR_Restore(level);
 148                         goto forward_search;
 149                 }
 150                 search_thread = (lwp_cntrl*)search_thread->object.node.next;
 151         }
 152         if(queue->sync_state!=LWP_THREADQ_NOTHINGHAPPEND) goto synchronize;
 153         queue->sync_state = LWP_THREADQ_SYNCHRONIZED;
 154         if(prio==search_prio) goto equal_prio;
 155
 156         search_node = (lwp_node*)search_thread;
 157         prev_node = search_node->prev;
 158         cur_node = (lwp_node*)thethread;
 159
 160         cur_node->next = search_node;
 161         cur_node->prev = prev_node;
 162         prev_node->next = cur_node;
 163         search_node->prev = cur_node;
 164         _CPU_ISR_Restore(level);
 165         return;
 166
 167 reverse_search:
 168         search_prio = LWP_PRIO_MAX + 1;
 169         _CPU_ISR_Disable(level);
 170         search_thread = (lwp_cntrl*)header->last;
 171         while(!__lwp_queue_ishead(header,(lwp_node*)search_thread)) {
 172                 search_prio = search_thread->cur_prio;
 173                 if(prio>=search_prio) break;
 174                 _CPU_ISR_Flash(level);
 175
 176                 if(!__lwp_statesset(search_thread->cur_state,block_state)) {
 177                         _CPU_ISR_Restore(level);
 178                         goto reverse_search;
 179                 }
 180                 search_thread = (lwp_cntrl*)search_thread->object.node.prev;
 181         }
 182         if(queue->sync_state!=LWP_THREADQ_NOTHINGHAPPEND) goto synchronize;
 183         queue->sync_state = LWP_THREADQ_SYNCHRONIZED;
 184         if(prio==search_prio) goto equal_prio;
 185
 186         search_node = (lwp_node*)search_thread;
 187         next_node = search_node->next;
 188         cur_node = (lwp_node*)thethread;
 189
 190         cur_node->next = next_node;
 191         cur_node->prev = search_node;
 192         search_node->next = cur_node;
 193         next_node->prev = cur_node;
 194         _CPU_ISR_Restore(level);
 195         return;
 196
 197 equal_prio:
 198 #ifdef _LWPTHRQ_DEBUG
 199         printf("__lwp_threadqueue_enqueuepriority(%p,equal_prio)\n",thethread);
 200 #endif
 201         search_node = __lwp_queue_tail(&search_thread->wait.block2n);
 202         prev_node = search_node->prev;
 203         cur_node = (lwp_node*)thethread;
 204
 205         cur_node->next = search_node;
 206         cur_node->prev = prev_node;
 207         prev_node->next = cur_node;
 208         search_node->prev = cur_node;
 209         _CPU_ISR_Restore(level);
 210         return;
 211
 212 synchronize:
 213         sync_state = queue->sync_state;
 214         queue->sync_state = LWP_THREADQ_SYNCHRONIZED;
 215
 216 #ifdef _LWPTHRQ_DEBUG
 217         printf("__lwp_threadqueue_enqueuepriority(%p,sync_state = %d)\n",thethread,sync_state);
 218 #endif
 219         switch(sync_state) {
 220                 case LWP_THREADQ_SYNCHRONIZED:
 221                         break;
 222                 case LWP_THREADQ_NOTHINGHAPPEND:
 223                         break;
 224                 case LWP_THREADQ_TIMEOUT:
 225                         thethread->wait.ret_code = thethread->wait.queue->timeout_state;
 226                         _CPU_ISR_Restore(level);
 227                         break;
 228                 case LWP_THREADQ_SATISFIED:
 229                         if(__lwp_wd_isactive(&thethread->timer)) {
 230                                 __lwp_wd_deactivate(&thethread->timer);
 231                                 _CPU_ISR_Restore(level);
 232                                 __lwp_wd_remove_ticks(&thethread->timer);
 233                         } else
 234                                 _CPU_ISR_Restore(level);
 235                         break;
 236         }
 237         __lwp_thread_unblock(thethread);
 238 }
 239
 240 lwp_cntrl* __lwp_threadqueue_dequeuepriority(lwp_thrqueue *queue)
 241 {
 242         u32 level,idx;
 243         lwp_cntrl *newfirstthr,*ret = NULL;
 244         lwp_node *newfirstnode,*newsecnode,*last_node,*next_node,*prev_node;
 245
 246         _CPU_ISR_Disable(level);
 247         for(idx=0;idx<LWP_THREADQ_NUM_PRIOHEADERS;idx++) {
 248                 if(!__lwp_queue_isempty(&queue->queues.priority[idx])) {
 249                         ret      = (lwp_cntrl*)queue->queues.priority[idx].first;
 250                         goto dequeue;
 251                 }
 252         }
 253
 254 #ifdef _LWPTHRQ_DEBUG
 255         printf("__lwp_threadqueue_dequeuepriority(%p,sync_state = %d)\n",ret,queue->sync_state);
 256 #endif
 257         switch(queue->sync_state) {
 258                 case LWP_THREADQ_SYNCHRONIZED:
 259                 case LWP_THREADQ_SATISFIED:
 260                         _CPU_ISR_Restore(level);
 261                         return NULL;
 262                 case LWP_THREADQ_NOTHINGHAPPEND:
 263                 case LWP_THREADQ_TIMEOUT:
 264                         queue->sync_state = LWP_THREADQ_SATISFIED;
 265                         _CPU_ISR_Restore(level);
 266                         return _thr_executing;
 267         }
 268
 269 dequeue:
 270 #ifdef _LWPTHRQ_DEBUG
 271         printf("__lwp_threadqueue_dequeuepriority(%p,dequeue)\n",ret);
 272 #endif
 273         newfirstnode = ret->wait.block2n.first;
 274         newfirstthr = (lwp_cntrl*)newfirstnode;
 275         next_node = ret->object.node.next;
 276         prev_node = ret->object.node.prev;
 277         if(!__lwp_queue_isempty(&ret->wait.block2n)) {
 278                 last_node = ret->wait.block2n.last;
 279                 newsecnode = newfirstnode->next;
 280                 prev_node->next = newfirstnode;
 281                 next_node->prev = newfirstnode;
 282                 newfirstnode->next = next_node;
 283                 newfirstnode->prev = prev_node;
 284
 285                 if(!__lwp_queue_onenode(&ret->wait.block2n)) {
 286                         newsecnode->prev = __lwp_queue_head(&newfirstthr->wait.block2n);
 287                         newfirstthr->wait.block2n.first = newsecnode;
 288                         newfirstthr->wait.block2n.last = last_node;
 289                         last_node->next = __lwp_queue_tail(&newfirstthr->wait.block2n);
 290                 }
 291         } else {
 292                 prev_node->next = next_node;
 293                 next_node->prev = prev_node;
 294         }
 295
 296         if(!__lwp_wd_isactive(&ret->timer)) {
 297                 _CPU_ISR_Restore(level);
 298                 __lwp_thread_unblock(ret);
 299         } else {
 300                 __lwp_wd_deactivate(&ret->timer);
 301                 _CPU_ISR_Restore(level);
 302                 __lwp_wd_remove_ticks(&ret->timer);
 303                 __lwp_thread_unblock(ret);
 304         }
 305         return ret;
 306 }
 307
 308 void __lwp_threadqueue_init(lwp_thrqueue *queue,u32 mode,u32 state,u32 timeout_state)
 309 {
 310         u32 index;
 311
 312         queue->state = state;
 313         queue->mode = mode;
 314         queue->timeout_state = timeout_state;
 315         queue->sync_state = LWP_THREADQ_SYNCHRONIZED;
 316 #ifdef _LWPTHRQ_DEBUG
 317         printf("__lwp_threadqueue_init(%p,%08x,%d,%d)\n",queue,state,timeout_state,mode);
 318 #endif
 319         switch(mode) {
 320                 case LWP_THREADQ_MODEFIFO:
 321                         __lwp_queue_init_empty(&queue->queues.fifo);
 322                         break;
 323                 case LWP_THREADQ_MODEPRIORITY:
 324                         for(index=0;index<LWP_THREADQ_NUM_PRIOHEADERS;index++)
 325                                 __lwp_queue_init_empty(&queue->queues.priority[index]);
 326                         break;
 327         }
 328 }
 329
 330 lwp_cntrl* __lwp_threadqueue_first(lwp_thrqueue *queue)
 331 {
 332         lwp_cntrl *ret;
 333
 334         switch(queue->mode) {
 335                 case LWP_THREADQ_MODEFIFO:
 336                         ret = __lwp_threadqueue_firstfifo(queue);
 337                         break;
 338                 case LWP_THREADQ_MODEPRIORITY:
 339                         ret = __lwp_threadqueue_firstpriority(queue);
 340                         break;
 341                 default:
 342                         ret = NULL;
 343                         break;
 344         }
 345
 346         return ret;
 347 }
 348
 349 void __lwp_threadqueue_enqueue(lwp_thrqueue *queue,u64 timeout)
 350 {
 351         lwp_cntrl *thethread;
 352
 353         thethread = _thr_executing;
 354         __lwp_thread_setstate(thethread,queue->state);
 355
 356         if(timeout) {
 357                 __lwp_wd_initialize(&thethread->timer,__lwp_threadqueue_timeout,thethread->object.id,thethread);
 358                 __lwp_wd_insert_ticks(&thethread->timer,timeout);
 359         }
 360
 361 #ifdef _LWPTHRQ_DEBUG
 362         printf("__lwp_threadqueue_enqueue(%p,%p,%d)\n",queue,thethread,queue->mode);
 363 #endif
 364         switch(queue->mode) {
 365                 case LWP_THREADQ_MODEFIFO:
 366                         __lwp_threadqueue_enqueuefifo(queue,thethread,timeout);
 367                         break;
 368                 case LWP_THREADQ_MODEPRIORITY:
 369                         __lwp_threadqueue_enqueuepriority(queue,thethread,timeout);
 370                         break;
 371         }
 372 }
 373
 374 lwp_cntrl* __lwp_threadqueue_dequeue(lwp_thrqueue *queue)
 375 {
 376         lwp_cntrl *ret = NULL;
 377
 378 #ifdef _LWPTHRQ_DEBUG
 379         printf("__lwp_threadqueue_dequeue(%p,%p,%d,%d)\n",queue,_thr_executing,queue->mode,queue->sync_state);
 380 #endif
 381         switch(queue->mode) {
 382                 case LWP_THREADQ_MODEFIFO:
 383                         ret = __lwp_threadqueue_dequeuefifo(queue);
 384                         break;
 385                 case LWP_THREADQ_MODEPRIORITY:
 386                         ret = __lwp_threadqueue_dequeuepriority(queue);
 387                         break;
 388                 default:
 389                         ret = NULL;
 390                         break;
 391         }
 392 #ifdef _LWPTHRQ_DEBUG
 393         printf("__lwp_threadqueue_dequeue(%p,%p,%d,%d)\n",queue,ret,queue->mode,queue->sync_state);
 394 #endif
 395         return ret;
 396 }
 397
 398 void __lwp_threadqueue_flush(lwp_thrqueue *queue,u32 status)
 399 {
 400         lwp_cntrl *thethread;
 401         while((thethread=__lwp_threadqueue_dequeue(queue))) {
 402                 thethread->wait.ret_code = status;
 403         }
 404 }
 405
 406 void __lwp_threadqueue_extract(lwp_thrqueue *queue,lwp_cntrl *thethread)
 407 {
 408         switch(queue->mode) {
 409                 case LWP_THREADQ_MODEFIFO:
 410                         __lwp_threadqueue_extractfifo(queue,thethread);
 411                         break;
 412                 case LWP_THREADQ_MODEPRIORITY:
 413                         __lwp_threadqueue_extractpriority(queue,thethread);
 414                         break;
 415         }
 416
 417 }
 418
 419 void __lwp_threadqueue_extractfifo(lwp_thrqueue *queue,lwp_cntrl *thethread)
 420 {
 421         u32 level;
 422
 423         _CPU_ISR_Disable(level);
 424         if(!__lwp_statewaitthreadqueue(thethread->cur_state)) {
 425                 _CPU_ISR_Restore(level);
 426                 return;
 427         }
 428
 429         __lwp_queue_extractI(&thethread->object.node);
 430         if(!__lwp_wd_isactive(&thethread->timer)) {
 431                 _CPU_ISR_Restore(level);
 432         } else {
 433                 __lwp_wd_deactivate(&thethread->timer);
 434                 _CPU_ISR_Restore(level);
 435                 __lwp_wd_remove_ticks(&thethread->timer);
 436         }
 437         __lwp_thread_unblock(thethread);
 438 }
 439
 440 void __lwp_threadqueue_extractpriority(lwp_thrqueue *queue,lwp_cntrl *thethread)
 441 {
 442         u32 level;
 443         lwp_cntrl *first;
 444         lwp_node *curr,*next,*prev,*new_first,*new_sec,*last;
 445
 446         curr = (lwp_node*)thethread;
 447
 448         _CPU_ISR_Disable(level);
 449         if(__lwp_statewaitthreadqueue(thethread->cur_state)) {
 450                 next = curr->next;
 451                 prev = curr->prev;
 452
 453                 if(!__lwp_queue_isempty(&thethread->wait.block2n)) {
 454                         new_first = thethread->wait.block2n.first;
 455                         first = (lwp_cntrl*)new_first;
 456                         last = thethread->wait.block2n.last;
 457                         new_sec = new_first->next;
 458
 459                         prev->next = new_first;
 460                         next->prev = new_first;
 461                         new_first->next = next;
 462                         new_first->prev = prev;
 463
 464                         if(!__lwp_queue_onenode(&thethread->wait.block2n)) {
 465                                 new_sec->prev = __lwp_queue_head(&first->wait.block2n);
 466                                 first->wait.block2n.first = new_sec;
 467                                 first->wait.block2n.last = last;
 468                                 last->next = __lwp_queue_tail(&first->wait.block2n);
 469                         }
 470                 } else {
 471                         prev->next = next;
 472                         next->prev = prev;
 473                 }
 474                 if(!__lwp_wd_isactive(&thethread->timer)) {
 475                         _CPU_ISR_Restore(level);
 476                         __lwp_thread_unblock(thethread);
 477                 } else {
 478                         __lwp_wd_deactivate(&thethread->timer);
 479                         _CPU_ISR_Restore(level);
 480                         __lwp_wd_remove_ticks(&thethread->timer);
 481                         __lwp_thread_unblock(thethread);
 482                 }
 483         } else
 484                 _CPU_ISR_Restore(level);
 485 }
 486
 487 u32 __lwp_threadqueue_extractproxy(lwp_cntrl *thethread)
 488 {
 489         u32 state;
 490
 491         state = thethread->cur_state;
 492         if(__lwp_statewaitthreadqueue(state)) {
 493                 __lwp_threadqueue_extract(thethread->wait.queue,thethread);
 494                 return TRUE;
 495         }
 496         return FALSE;
 497 }