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 2012/10/24.
   11:  *    Modified by T-Engine Forum at 2014/09/10.
   12:  *    Modified by TRON Forum(http://www.tron.org/) at 2015/06/01.
   13:  *
   14:  *----------------------------------------------------------------------
   15:  */
   16: 
   17: /*
   18:  *      semaphore.c (T-Kernel/OS)
   19:  *      Semaphore
   20:  */
   21: 
   22: #include "kernel.h"
   23: #include "task.h"
   24: #include "wait.h"
   25: #include "check.h"
   26: #include <sys/rominfo.h>
   27: 
   28: #ifdef NUM_SEMID
   29: 
   30: EXPORT ID       max_semid;    /* Maximum semaphore ID */
   31: 
   32: /*
   33:  * Semaphore control block
   34:  */
   35: typedef struct semaphore_control_block {
   36:         QUEUE  wait_queue;      /* Semaphore wait queue */
   37:         ID     semid;              /* Semaphore ID */
   38:         void   *exinf;           /* Extended information */
   39:         ATR    sematr;            /* Semaphore attribute */
   40:         INT    semcnt;            /* Semaphore current count value */
   41:         INT    maxsem;            /* Semaphore maximum count value */
   42: #if USE_OBJECT_NAME
   43:         UB     name[OBJECT_NAME_LENGTH];   /* name */
   44: #endif
   45: } SEMCB;
   46: 
   47: LOCAL SEMCB     *semcb_table;       /* Semaphore control block */
   48: LOCAL QUEUE     free_semcb; /* FreeQue */
   49: 
   50: #define get_semcb(id)   ( &semcb_table[INDEX_SEM(id)] )
   51: 
   52: 
   53: /*
   54:  * Initialization of semaphore control block
   55:  */
   56: EXPORT ER semaphore_initialize( void )
   57: {
   58:         SEMCB  *semcb, *end;
   59:         W      n;
   60: 
   61:         /* Get system information */
   62:         n = _tk_get_cfn(SCTAG_TMAXSEMID, &max_semid, 1);
   63:         if ( n < 1 || NUM_SEMID < 1 ) {
   64:                 return E_SYS;
   65:         }
   66: 
   67:         /* Semaphore control block */
   68:         semcb_table = Imalloc((UINT)NUM_SEMID * sizeof(SEMCB));
   69:         if ( semcb_table == NULL ) {
   70:                 return E_NOMEM;
   71:         }
   72: 
   73:         /* Register all control blocks onto FreeQue */
   74:         QueInit(&free_semcb);
   75:         end = semcb_table + NUM_SEMID;
   76:         for ( semcb = semcb_table; semcb < end; semcb++ ) {
   77:                 semcb->semid = 0;
   78:                 QueInsert(&semcb->wait_queue, &free_semcb);
   79:         }
   80: 
   81:         return E_OK;
   82: }
   83: 
   84: 
   85: /*
   86:  * Processing if the priority of wait task changes
   87:  */
   88: LOCAL void sem_chg_pri( TCB *tcb, INT oldpri )
   89: {
   90:         SEMCB  *semcb;
   91:         QUEUE  *queue;
   92:         TCB    *top;
   93: 
   94:         semcb = get_semcb(tcb->wid);
   95:         if ( oldpri >= 0 ) {
   96:                 /* Reorder wait line */
   97:                 gcb_change_priority((GCB*)semcb, tcb);
   98:         }
   99: 
  100:         if ( (semcb->sematr & TA_CNT) != 0 ) {
  101:                 return;
  102:         }
  103: 
  104:         /* From the head task in a wait queue, allocate semaphore counts
  105:            and release wait state as much as possible */
  106:         queue = semcb->wait_queue.next;
  107:         while ( queue != &semcb->wait_queue ) {
  108:                 top = (TCB*)queue;
  109:                 queue = queue->next;
  110: 
  111:                 /* Meet condition for releasing wait? */
  112:                 if ( semcb->semcnt < top->winfo.sem.cnt ) {
  113:                         break;
  114:                 }
  115: 
  116:                 /* Release wait */
  117:                 wait_release_ok(top);
  118: 
  119:                 semcb->semcnt -= top->winfo.sem.cnt;
  120:         }
  121: }
  122: 
  123: /*
  124:  * Processing if the wait task is freed
  125:  */
  126: LOCAL void sem_rel_wai( TCB *tcb )
  127: {
  128:         sem_chg_pri(tcb, -1);
  129: }
  130: 
  131: /*
  132:  * Definition of semaphore wait specification
  133:  */
  134: LOCAL CONST WSPEC wspec_sem_tfifo = { TTW_SEM, NULL,        sem_rel_wai };
  135: LOCAL CONST WSPEC wspec_sem_tpri  = { TTW_SEM, sem_chg_pri, sem_rel_wai };
  136: 
  137: 
  138: /*
  139:  * Create semaphore
  140:  */
  141: SYSCALL ID _tk_cre_sem( CONST T_CSEM *pk_csem )
  142: {
  143: #if CHK_RSATR
  144:         const ATR VALID_SEMATR = {
  145:                  TA_TPRI
  146:                 |TA_CNT
  147:                 |TA_NODISWAI
  148: #if USE_OBJECT_NAME
  149:                 |TA_DSNAME
  150: #endif
  151:         };
  152: #endif
  153:         SEMCB  *semcb;
  154:         ID     semid;
  155:         ER     ercd;
  156: 
  157:         CHECK_RSATR(pk_csem->sematr, VALID_SEMATR);
  158:         CHECK_PAR(pk_csem->isemcnt >= 0);
  159:         CHECK_PAR(pk_csem->maxsem > 0);
  160:         CHECK_PAR(pk_csem->maxsem >= pk_csem->isemcnt);
  161: 
  162:         BEGIN_CRITICAL_SECTION;
  163:         /* Get control block from FreeQue */
  164:         semcb = (SEMCB*)QueRemoveNext(&free_semcb);
  165:         if ( semcb == NULL ) {
  166:                 ercd = E_LIMIT;
  167:         } else {
  168:                 semid = ID_SEM(semcb - semcb_table);
  169: 
  170:                 /* Initialize control block */
  171:                 QueInit(&semcb->wait_queue);
  172:                 semcb->semid = semid;
  173:                 semcb->exinf = pk_csem->exinf;
  174:                 semcb->sematr = pk_csem->sematr;
  175:                 semcb->semcnt = pk_csem->isemcnt;
  176:                 semcb->maxsem = pk_csem->maxsem;
  177: #if USE_OBJECT_NAME
  178:                 if ( (pk_csem->sematr & TA_DSNAME) != 0 ) {
  179:                         STRNCPY((char*)semcb->name, (char*)pk_csem->dsname,
  180:                                 OBJECT_NAME_LENGTH);
  181:                 }
  182: #endif
  183:                 ercd = semid;
  184:         }
  185:         END_CRITICAL_SECTION;
  186: 
  187:         return ercd;
  188: }
  189: 
  190: /*
  191:  * Delete semaphore
  192:  */
  193: SYSCALL ER _tk_del_sem( ID semid )
  194: {
  195:         SEMCB  *semcb;
  196:         ER     ercd = E_OK;
  197: 
  198:         CHECK_SEMID(semid);
  199: 
  200:         semcb = get_semcb(semid);
  201: 
  202:         BEGIN_CRITICAL_SECTION;
  203:         if ( semcb->semid == 0 ) {
  204:                 ercd = E_NOEXS;
  205:         } else {
  206:                 /* Release wait state of task (E_DLT) */
  207:                 wait_delete(&semcb->wait_queue);
  208: 
  209:                 /* Return to FreeQue */
  210:                 QueInsert(&semcb->wait_queue, &free_semcb);
  211:                 semcb->semid = 0;
  212:         }
  213:         END_CRITICAL_SECTION;
  214: 
  215:         return ercd;
  216: }
  217: 
  218: /*
  219:  * Signal semaphore
  220:  */
  221: SYSCALL ER _tk_sig_sem( ID semid, INT cnt )
  222: {
  223:         SEMCB  *semcb;
  224:         TCB    *tcb;
  225:         QUEUE  *queue;
  226:         ER     ercd = E_OK;
  227:  
  228:         CHECK_SEMID(semid);
  229:         CHECK_PAR(cnt > 0);
  230: 
  231:         semcb = get_semcb(semid);
  232: 
  233:         BEGIN_CRITICAL_SECTION;
  234:         if ( semcb->semid == 0 ) {
  235:                 ercd = E_NOEXS;
  236:                 goto error_exit;
  237:         }
  238:         if ( cnt > (semcb->maxsem - semcb->semcnt) ) {
  239:                 ercd = E_QOVR;
  240:                 goto error_exit;
  241:         }
  242: 
  243:         /* Return semaphore counts */
  244:         semcb->semcnt += cnt;
  245: 
  246:         /* Search task that frees wait */
  247:         queue = semcb->wait_queue.next;
  248:         while ( queue != &semcb->wait_queue ) {
  249:                 tcb = (TCB*)queue;
  250:                 queue = queue->next;
  251: 
  252:                 /* Meet condition for Releasing wait? */
  253:                 if ( semcb->semcnt < tcb->winfo.sem.cnt ) {
  254:                         if ( (semcb->sematr & TA_CNT) == 0 ) {
  255:                                 break;
  256:                         }
  257:                         continue;
  258:                 }
  259: 
  260:                 /* Release wait */
  261:                 wait_release_ok(tcb);
  262: 
  263:                 semcb->semcnt -= tcb->winfo.sem.cnt;
  264:                 if ( semcb->semcnt <= 0 ) {
  265:                         break;
  266:                 }
  267:         }
  268: 
  269:     error_exit:
  270:         END_CRITICAL_SECTION;
  271: 
  272:         return ercd;
  273: }
  274: 
  275: /*
  276:  * Wait on semaphore
  277:  */
  278: SYSCALL ER _tk_wai_sem( ID semid, INT cnt, TMO tmout )
  279: {
  280:         return _tk_wai_sem_u(semid, cnt, to_usec_tmo(tmout));
  281: }
  282: 
  283: SYSCALL ER _tk_wai_sem_u( ID semid, INT cnt, TMO_U tmout )
  284: {
  285:         SEMCB  *semcb;
  286:         ER     ercd = E_OK;
  287: 
  288:         CHECK_SEMID(semid);
  289:         CHECK_PAR(cnt > 0);
  290:         CHECK_TMOUT(tmout);
  291:         CHECK_DISPATCH();
  292: 
  293:         semcb = get_semcb(semid);
  294: 
  295:         BEGIN_CRITICAL_SECTION;
  296:         if ( semcb->semid == 0 ) {
  297:                 ercd = E_NOEXS;
  298:                 goto error_exit;
  299:         }
  300: #if CHK_PAR
  301:         if ( cnt > semcb->maxsem ) {
  302:                 ercd = E_PAR;
  303:                 goto error_exit;
  304:         }
  305: #endif
  306: 
  307:         /* Check wait disable */
  308:         if ( is_diswai((GCB*)semcb, ctxtsk, TTW_SEM) ) {
  309:                 ercd = E_DISWAI;
  310:                 goto error_exit;
  311:         }
  312: 
  313:         if ( ((semcb->sematr & TA_CNT) != 0
  314:               || gcb_top_of_wait_queue((GCB*)semcb, ctxtsk) == ctxtsk)
  315:           && semcb->semcnt >= cnt ) {
  316:                 /* Get semaphore count */
  317:                 semcb->semcnt -= cnt;
  318: 
  319:         } else {
  320:                 /* Ready for wait */
  321:                 ctxtsk->wspec = ( (semcb->sematr & TA_TPRI) != 0 )?
  322:                                         &wspec_sem_tpri: &wspec_sem_tfifo;
  323:                 ctxtsk->wercd = &ercd;
  324:                 ctxtsk->winfo.sem.cnt = cnt;
  325:                 gcb_make_wait_with_diswai((GCB*)semcb, tmout);
  326:         }
  327: 
  328:     error_exit:
  329:         END_CRITICAL_SECTION;
  330: 
  331:         return ercd;
  332: }
  333: 
  334: /*
  335:  * Refer semaphore state
  336:  */
  337: SYSCALL ER _tk_ref_sem( ID semid, T_RSEM *pk_rsem )
  338: {
  339:         SEMCB  *semcb;
  340:         ER     ercd = E_OK;
  341: 
  342:         CHECK_SEMID(semid);
  343: 
  344:         semcb = get_semcb(semid);
  345: 
  346:         BEGIN_CRITICAL_SECTION;
  347:         if ( semcb->semid == 0 ) {
  348:                 ercd = E_NOEXS;
  349:         } else {
  350:                 pk_rsem->exinf  = semcb->exinf;
  351:                 pk_rsem->wtsk   = wait_tskid(&semcb->wait_queue);
  352:                 pk_rsem->semcnt = semcb->semcnt;
  353:         }
  354:         END_CRITICAL_SECTION;
  355: 
  356:         return ercd;
  357: }
  358: 
  359: /* ------------------------------------------------------------------------ */
  360: /*
  361:  *      Debugger support function
  362:  */
  363: #if USE_DBGSPT
  364: 
  365: /*
  366:  * Get object name from control block
  367:  */
  368: #if USE_OBJECT_NAME
  369: EXPORT ER semaphore_getname(ID id, UB **name)
  370: {
  371:         SEMCB  *semcb;
  372:         ER     ercd = E_OK;
  373: 
  374:         CHECK_SEMID(id);
  375: 
  376:         BEGIN_DISABLE_INTERRUPT;
  377:         semcb = get_semcb(id);
  378:         if ( semcb->semid == 0 ) {
  379:                 ercd = E_NOEXS;
  380:                 goto error_exit;
  381:         }
  382:         if ( (semcb->sematr & TA_DSNAME) == 0 ) {
  383:                 ercd = E_OBJ;
  384:                 goto error_exit;
  385:         }
  386:         *name = semcb->name;
  387: 
  388:     error_exit:
  389:         END_DISABLE_INTERRUPT;
  390: 
  391:         return ercd;
  392: }
  393: #endif /* USE_OBJECT_NAME */
  394: 
  395: /*
  396:  * Refer object usage state
  397:  */
  398: SYSCALL INT _td_lst_sem( ID list[], INT nent )
  399: {
  400:         SEMCB  *semcb, *end;
  401:         INT    n = 0;
  402: 
  403:         BEGIN_DISABLE_INTERRUPT;
  404:         end = semcb_table + NUM_SEMID;
  405:         for ( semcb = semcb_table; semcb < end; semcb++ ) {
  406:                 if ( semcb->semid == 0 ) {
  407:                         continue;
  408:                 }
  409: 
  410:                 if ( n++ < nent ) {
  411:                         *list++ = semcb->semid;
  412:                 }
  413:         }
  414:         END_DISABLE_INTERRUPT;
  415: 
  416:         return n;
  417: }
  418: 
  419: /*
  420:  * Refer object state
  421:  */
  422: SYSCALL ER _td_ref_sem( ID semid, TD_RSEM *rsem )
  423: {
  424:         SEMCB  *semcb;
  425:         ER     ercd = E_OK;
  426: 
  427:         CHECK_SEMID(semid);
  428: 
  429:         semcb = get_semcb(semid);
  430: 
  431:         BEGIN_DISABLE_INTERRUPT;
  432:         if ( semcb->semid == 0 ) {
  433:                 ercd = E_NOEXS;
  434:         } else {
  435:                 rsem->exinf  = semcb->exinf;
  436:                 rsem->wtsk   = wait_tskid(&semcb->wait_queue);
  437:                 rsem->semcnt = semcb->semcnt;
  438:         }
  439:         END_DISABLE_INTERRUPT;
  440: 
  441:         return ercd;
  442: }
  443: 
  444: /*
  445:  * Refer wait queue
  446:  */
  447: SYSCALL INT _td_sem_que( ID semid, ID list[], INT nent )
  448: {
  449:         SEMCB  *semcb;
  450:         QUEUE  *q;
  451:         ER     ercd;
  452: 
  453:         CHECK_SEMID(semid);
  454: 
  455:         semcb = get_semcb(semid);
  456: 
  457:         BEGIN_DISABLE_INTERRUPT;
  458:         if ( semcb->semid == 0 ) {
  459:                 ercd = E_NOEXS;
  460:         } else {
  461:                 INT   n = 0;
  462:                 for ( q = semcb->wait_queue.next; q != &semcb->wait_queue; q = q->next ) {
  463:                         if ( n++ < nent ) {
  464:                                 *list++ = ((TCB*)q)->tskid;
  465:                         }
  466:                 }
  467:                 ercd = n;
  468:         }
  469:         END_DISABLE_INTERRUPT;
  470: 
  471:         return ercd;
  472: }
  473: 
  474: #endif /* USE_DBGSPT */
  475: #endif /* NUM_SEMID */