gonzui

    1: /*
    2:  *----------------------------------------------------------------------
    3:  *    T-Kernel 2.0 Software Package
    4:  *
    5:  *    Copyright 2011 by Ken Sakamura.
    6:  *    This software is distributed under the latest version of T-License 2.x.
    7:  *----------------------------------------------------------------------
    8:  *
    9:  *    Released by T-Engine Forum(http://www.t-engine.org/) at 2011/05/17.
   10:  *    Modified by T-Engine Forum at 2015/02/17.
   11:  *    Modified by TRON Forum(http://www.tron.org/) at 2015/06/01.
   12:  *
   13:  *----------------------------------------------------------------------
   14:  */
   15: 
   16: /*
   17:  *      klock.c (T-Kernel/OS)
   18:  *      Kernel Lock
   19:  */
   20: 
   21: #include "kernel.h"
   22: #include "task.h"
   23: #include "ready_queue.h"
   24: 
   25: /*
   26:  * Object lock
   27:  *      Do not call from critical section
   28:  */
   29: EXPORT void LockOBJ( OBJLOCK *loc )
   30: {
   31:         BOOL   klocked;
   32: 
   33:   retry:
   34:         BEGIN_CRITICAL_SECTION;
   35:         klocked = ctxtsk->klocked;
   36:         if ( !klocked ) {
   37:                 if ( loc->wtskq.next == NULL ) {
   38:                         /* Lock */
   39:                         QueInit(&loc->wtskq);
   40: 
   41:                         ctxtsk->klocked = klocked = TRUE;
   42:                         ready_queue.klocktsk = ctxtsk;
   43:                 } else {
   44:                         /* Ready for lock */
   45:                         ready_queue_delete(&ready_queue, ctxtsk);
   46:                         ctxtsk->klockwait = TRUE;
   47:                         QueInsert(&ctxtsk->tskque, &loc->wtskq);
   48: 
   49:                         schedtsk = ready_queue_top(&ready_queue);
   50:                         dispatch_request();
   51:                 }
   52:         }
   53:         END_CRITICAL_SECTION;
   54:         /* Since wait could be freed without getting lock,
   55:            need to re-try if lock is not got */
   56:         if ( !klocked ) {
   57:                 goto retry;
   58:         }
   59: }
   60: 
   61: /*
   62:  * Object unlock
   63:  *      It may be called from a critical section.
   64:  */
   65: EXPORT void UnlockOBJ( OBJLOCK *loc )
   66: {
   67:         TCB    *tcb;
   68: 
   69:         BEGIN_CRITICAL_SECTION;
   70:         ctxtsk->klocked = FALSE;
   71:         ready_queue.klocktsk = NULL;
   72: 
   73:         tcb = (TCB*)QueRemoveNext(&loc->wtskq);
   74:         if ( tcb == NULL ) {
   75:                 /* Free lock */
   76:                 loc->wtskq.next = NULL;
   77:         } else {
   78:                 /* Wake lock wait task */
   79:                 tcb->klockwait = FALSE;
   80:                 tcb->klocked = TRUE;
   81:                 ready_queue_insert_top(&ready_queue, tcb);
   82:         }
   83: 
   84:         schedtsk = ready_queue_top(&ready_queue);
   85:         if ( ctxtsk != schedtsk ) {
   86:                 dispatch_request();
   87:         }
   88:         END_CRITICAL_SECTION;
   89: }
   90: 
   91: /* ------------------------------------------------------------------------ */
   92: 
   93: /*
   94:  * Extended SVC lock
   95:  *      Do not call from critical section
   96:  */
   97: EXPORT void LockSVC( SVCLOCK *loc )
   98: {
   99:         BOOL   lock;
  100: 
  101:   retry:
  102:         BEGIN_CRITICAL_SECTION;
  103:         lock = ( ctxtsk->svclocked == loc );
  104:         if ( !lock ) {
  105:                 if ( loc->wtskq.next == NULL ) {
  106:                         /* Lock */
  107:                         QueInit(&loc->wtskq);
  108: 
  109:                         loc->locklist = ctxtsk->svclocked;
  110:                         ctxtsk->svclocked = loc;
  111:                         lock = TRUE;
  112:                 } else {
  113:                         /* Ready for lock */
  114:                         ready_queue_delete(&ready_queue, ctxtsk);
  115:                         ctxtsk->klockwait = TRUE;
  116:                         QueInsert(&ctxtsk->tskque, &loc->wtskq);
  117: 
  118:                         schedtsk = ready_queue_top(&ready_queue);
  119:                         dispatch_request();
  120:                 }
  121:         }
  122:         END_CRITICAL_SECTION;
  123:         /* Since wait could be freed without getting lock,
  124:            need to re-try if lock is not got */
  125:         if ( !lock ) {
  126:                 goto retry;
  127:         }
  128: }
  129: 
  130: /*
  131:  * Extended SVC unlock
  132:  */
  133: EXPORT void UnlockSVC( void )
  134: {
  135:         SVCLOCK        *loc;
  136:         TCB    *tcb;
  137: 
  138:         BEGIN_CRITICAL_SECTION;
  139:         loc = ctxtsk->svclocked;
  140:         ctxtsk->svclocked = loc->locklist;
  141: 
  142:         tcb = (TCB*)QueRemoveNext(&loc->wtskq);
  143:         if ( tcb == NULL ) {
  144:                 /* Free lock */
  145:                 loc->wtskq.next = NULL;
  146:         } else {
  147:                 /* Wake lock wait task */
  148:                 tcb->klockwait = FALSE;
  149:                 loc->locklist = tcb->svclocked;
  150:                 tcb->svclocked = loc;
  151:                 ready_queue_insert(&ready_queue, tcb);
  152: 
  153:                 schedtsk = ready_queue_top(&ready_queue);
  154:                 if ( ctxtsk != schedtsk ) {
  155:                         dispatch_request();
  156:                 }
  157:         }
  158:         END_CRITICAL_SECTION;
  159: }
  160: 
  161: /*
  162:  * Unlock all extended SVCs in which 'tcb' tasks are locked
  163:  */
  164: EXPORT void AllUnlockSVC( TCB *loctsk )
  165: {
  166:         SVCLOCK        *loc;
  167:         TCB    *tcb;
  168: 
  169:         BEGIN_CRITICAL_SECTION;
  170:         while ( (loc = loctsk->svclocked) != NULL ) {
  171:                 loctsk->svclocked = loc->locklist;
  172: 
  173:                 tcb = (TCB*)QueRemoveNext(&loc->wtskq);
  174:                 if ( tcb == NULL ) {
  175:                         /* Free lock */
  176:                         loc->wtskq.next = NULL;
  177:                 } else {
  178:                         /* Wake lock wait task */
  179:                         tcb->klockwait = FALSE;
  180:                         loc->locklist = tcb->svclocked;
  181:                         tcb->svclocked = loc;
  182:                         ready_queue_insert(&ready_queue, tcb);
  183:                 }
  184:         }
  185:         schedtsk = ready_queue_top(&ready_queue);
  186:         if ( ctxtsk != schedtsk ) {
  187:                 dispatch_request();
  188:         }
  189:         END_CRITICAL_SECTION;
  190: }