gonzui

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