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:  *      mailbox.c (T-Kernel/OS)
   19:  *      Mailbox
   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_MBXID
   29: 
   30: EXPORT ID       max_mbxid;    /* Maximum mailbox ID */
   31: 
   32: /*
   33:  * Mailbox control block
   34:  *
   35:  *      'mq_head' is the first message queue pointer that
   36:  *      points a message.
   37:  *      It is NULL if the message queue is empty.
   38:  *      'mq_tail' is a pointer that points end of message
   39:  *      queue that is not empty.
   40:  *      The message queue value is not guaranteed if the
   41:  *      message queue is empty.
   42:  *      It is used only if the message queue is FIFO (TA_MFIFO).
   43:  */
   44: typedef struct mailbox_control_block {
   45:         QUEUE  wait_queue;      /* Mailbox wait queue */
   46:         ID     mbxid;              /* Mailbox ID */
   47:         void   *exinf;           /* Extended information */
   48:         ATR    mbxatr;            /* Mailbox attribute */
   49:         T_MSG  mq_head; /* Head of message queue */
   50:         T_MSG  *mq_tail;        /* End of message queue */
   51: #if USE_OBJECT_NAME
   52:         UB     name[OBJECT_NAME_LENGTH];   /* name */
   53: #endif
   54: } MBXCB;
   55: 
   56: LOCAL MBXCB     *mbxcb_table;       /* Mailbox control block */
   57: LOCAL QUEUE     free_mbxcb; /* FreeQue */
   58: 
   59: #define get_mbxcb(id)   ( &mbxcb_table[INDEX_MBX(id)] )
   60: 
   61: 
   62: /*
   63:  * Initialization of mailbox control block
   64:  */
   65: EXPORT ER mailbox_initialize( void )
   66: {
   67:         MBXCB  *mbxcb, *end;
   68:         W      n;
   69: 
   70:         /* Get system information */
   71:         n = _tk_get_cfn(SCTAG_TMAXMBXID, &max_mbxid, 1);
   72:         if ( n < 1 || NUM_MBXID < 1 ) {
   73:                 return E_SYS;
   74:         }
   75: 
   76:         /* Create mailbox control block */
   77:         mbxcb_table = Imalloc((UINT)NUM_MBXID * sizeof(MBXCB));
   78:         if ( mbxcb_table == NULL ) {
   79:                 return E_NOMEM;
   80:         }
   81: 
   82:         /* Register all control blocks onto FreeQue */
   83:         QueInit(&free_mbxcb);
   84:         end = mbxcb_table + NUM_MBXID;
   85:         for ( mbxcb = mbxcb_table; mbxcb < end; mbxcb++ ) {
   86:                 mbxcb->mbxid = 0;
   87:                 QueInsert(&mbxcb->wait_queue, &free_mbxcb);
   88:         }
   89: 
   90:         return E_OK;
   91: }
   92: 
   93: 
   94: /*
   95:  * Head message
   96:  */
   97: #define headmsg(mbxcb)  ( (mbxcb)->mq_head.msgque[0] )
   98: 
   99: /*
  100:  * Next message
  101:  */
  102: #define nextmsg(msg)    ( ((T_MSG*)(msg))->msgque[0] )
  103: 
  104: /*
  105:  * Insert a message queue following priority
  106:  */
  107: Inline void queue_insert_mpri( T_MSG_PRI *pk_msg, T_MSG *head )
  108: {
  109:         T_MSG_PRI      *msg;
  110:         T_MSG          *prevmsg = head;
  111: 
  112:         while ( (msg = (T_MSG_PRI*)nextmsg(prevmsg)) != NULL ) {
  113:                 if ( msg->msgpri > pk_msg->msgpri ) {
  114:                         break;
  115:                 }
  116:                 prevmsg = (T_MSG*)msg;
  117:         }
  118:         nextmsg(pk_msg) = msg;
  119:         nextmsg(prevmsg) = pk_msg;
  120: }
  121: 
  122: /*
  123:  * Processing if the priority of wait task changes
  124:  */
  125: LOCAL void mbx_chg_pri( TCB *tcb, INT oldpri )
  126: {
  127:         MBXCB  *mbxcb;
  128: 
  129:         mbxcb = get_mbxcb(tcb->wid);
  130:         gcb_change_priority((GCB*)mbxcb, tcb);
  131: }
  132: 
  133: /*
  134:  * Definition of mailbox wait specification
  135:  */
  136: LOCAL CONST WSPEC wspec_mbx_tfifo = { TTW_MBX, NULL, NULL };
  137: LOCAL CONST WSPEC wspec_mbx_tpri  = { TTW_MBX, mbx_chg_pri, NULL };
  138: 
  139: 
  140: /*
  141:  * Create mailbox
  142:  */
  143: SYSCALL ID _tk_cre_mbx( CONST T_CMBX *pk_cmbx )
  144: {
  145: #if CHK_RSATR
  146:         const ATR VALID_MBXATR = {
  147:                  TA_MPRI
  148:                 |TA_TPRI
  149:                 |TA_NODISWAI
  150: #if USE_OBJECT_NAME
  151:                 |TA_DSNAME
  152: #endif
  153:         };
  154: #endif
  155:         MBXCB  *mbxcb;
  156:         ID     mbxid;
  157:         ER     ercd;
  158: 
  159:         CHECK_RSATR(pk_cmbx->mbxatr, VALID_MBXATR);
  160: 
  161:         BEGIN_CRITICAL_SECTION;
  162:         /* Get control block from FreeQue */
  163:         mbxcb = (MBXCB*)QueRemoveNext(&free_mbxcb);
  164:         if ( mbxcb == NULL ) {
  165:                 ercd = E_LIMIT;
  166:         } else {
  167:                 mbxid = ID_MBX(mbxcb - mbxcb_table);
  168: 
  169:                 /* Initialize control block */
  170:                 QueInit(&mbxcb->wait_queue);
  171:                 mbxcb->mbxid  = mbxid;
  172:                 mbxcb->exinf  = pk_cmbx->exinf;
  173:                 mbxcb->mbxatr = pk_cmbx->mbxatr;
  174:                 mbxcb->mq_head.msgque[0] = NULL;
  175: #if USE_OBJECT_NAME
  176:                 if ( (pk_cmbx->mbxatr & TA_DSNAME) != 0 ) {
  177:                         STRNCPY((char*)mbxcb->name, (char*)pk_cmbx->dsname,
  178:                                 OBJECT_NAME_LENGTH);
  179:                 }
  180: #endif
  181:                 ercd = mbxid;
  182:         }
  183:         END_CRITICAL_SECTION;
  184: 
  185:         return ercd;
  186: }
  187: 
  188: /*
  189:  * Delete mailbox
  190:  */
  191: SYSCALL ER _tk_del_mbx( ID mbxid )
  192: {
  193:         MBXCB  *mbxcb;
  194:         ER     ercd = E_OK;
  195: 
  196:         CHECK_MBXID(mbxid);
  197: 
  198:         mbxcb = get_mbxcb(mbxid);
  199: 
  200:         BEGIN_CRITICAL_SECTION;
  201:         if ( mbxcb->mbxid == 0 ) {
  202:                 ercd = E_NOEXS;
  203:         } else {
  204:                 /* Release wait state of task (E_DLT) */
  205:                 wait_delete(&mbxcb->wait_queue);
  206: 
  207:                 /* Return to FreeQue */
  208:                 QueInsert(&mbxcb->wait_queue, &free_mbxcb);
  209:                 mbxcb->mbxid = 0;
  210:         }
  211:         END_CRITICAL_SECTION;
  212: 
  213:         return ercd;
  214: }
  215: 
  216: /*
  217:  * Send to mailbox
  218:  */
  219: SYSCALL ER _tk_snd_mbx( ID mbxid, T_MSG *pk_msg )
  220: {
  221:         MBXCB  *mbxcb;
  222:         TCB    *tcb;
  223:         ER     ercd = E_OK;
  224: 
  225:         CHECK_MBXID(mbxid);
  226: 
  227:         mbxcb = get_mbxcb(mbxid);
  228: 
  229:         BEGIN_CRITICAL_SECTION;
  230:         if (mbxcb->mbxid == 0) {
  231:                 ercd = E_NOEXS;
  232:                 goto error_exit;
  233:         }
  234: 
  235:         if ( (mbxcb->mbxatr & TA_MPRI) != 0 ) {
  236:                 if ( ((T_MSG_PRI*)pk_msg)->msgpri <= 0 ) {
  237:                         ercd = E_PAR;
  238:                         goto error_exit;
  239:                 }
  240:         }
  241: 
  242:         if ( !isQueEmpty(&mbxcb->wait_queue) ) {
  243:                 /* Directly send to receive wait task */
  244:                 tcb = (TCB*)(mbxcb->wait_queue.next);
  245:                 *tcb->winfo.mbx.ppk_msg = pk_msg;
  246:                 wait_release_ok(tcb);
  247: 
  248:         } else {
  249:                 /* Connect message to queue */
  250:                 if ( (mbxcb->mbxatr & TA_MPRI) != 0 ) {
  251:                         /* Connect message to queue following priority */
  252:                         queue_insert_mpri((T_MSG_PRI*)pk_msg, &mbxcb->mq_head);
  253:                 } else {
  254:                         /* Connect to end of queue */
  255:                         nextmsg(pk_msg) = NULL;
  256:                         if ( headmsg(mbxcb) == NULL ) {
  257:                                 headmsg(mbxcb) = pk_msg;
  258:                         } else {
  259:                                 nextmsg(mbxcb->mq_tail) = pk_msg;
  260:                         }
  261:                         mbxcb->mq_tail = pk_msg;
  262:                 }
  263:         }
  264: 
  265:     error_exit:
  266:         END_CRITICAL_SECTION;
  267: 
  268:         return ercd;
  269: }
  270: 
  271: /*
  272:  * Receive from mailbox
  273:  */
  274: SYSCALL ER _tk_rcv_mbx( ID mbxid, T_MSG **ppk_msg, TMO tmout )
  275: {
  276:         return _tk_rcv_mbx_u(mbxid, ppk_msg, to_usec_tmo(tmout));
  277: }
  278: 
  279: SYSCALL ER _tk_rcv_mbx_u( ID mbxid, T_MSG **ppk_msg, TMO_U tmout )
  280: {
  281:         MBXCB  *mbxcb;
  282:         ER     ercd = E_OK;
  283: 
  284:         CHECK_MBXID(mbxid);
  285:         CHECK_TMOUT(tmout);
  286:         CHECK_DISPATCH();
  287: 
  288:         mbxcb = get_mbxcb(mbxid);
  289: 
  290:         BEGIN_CRITICAL_SECTION;
  291:         if ( mbxcb->mbxid == 0 ) {
  292:                 ercd = E_NOEXS;
  293:                 goto error_exit;
  294:         }
  295: 
  296:         /* Check receive wait disable */
  297:         if ( is_diswai((GCB*)mbxcb, ctxtsk, TTW_MBX) ) {
  298:                 ercd = E_DISWAI;
  299:                 goto error_exit;
  300:         }
  301: 
  302:         if ( headmsg(mbxcb) != NULL ) {
  303:                 /* Get message from head of queue */
  304:                 *ppk_msg = headmsg(mbxcb);
  305:                 headmsg(mbxcb) = nextmsg(*ppk_msg);
  306:         } else {
  307:                 /* Ready for receive wait */
  308:                 ctxtsk->wspec = ( (mbxcb->mbxatr & TA_TPRI) != 0 )?
  309:                                         &wspec_mbx_tpri: &wspec_mbx_tfifo;
  310:                 ctxtsk->wercd = &ercd;
  311:                 ctxtsk->winfo.mbx.ppk_msg = ppk_msg;
  312:                 gcb_make_wait_with_diswai((GCB*)mbxcb, tmout);
  313:         }
  314: 
  315:     error_exit:
  316:         END_CRITICAL_SECTION;
  317: 
  318:         return ercd;
  319: }
  320: 
  321: /*
  322:  * Refer mailbox state
  323:  */
  324: SYSCALL ER _tk_ref_mbx( ID mbxid, T_RMBX *pk_rmbx )
  325: {
  326:         MBXCB  *mbxcb;
  327:         ER     ercd = E_OK;
  328: 
  329:         CHECK_MBXID(mbxid);
  330: 
  331:         mbxcb = get_mbxcb(mbxid);
  332: 
  333:         BEGIN_CRITICAL_SECTION;
  334:         if ( mbxcb->mbxid == 0 ) {
  335:                 ercd = E_NOEXS;
  336:         } else {
  337:                 pk_rmbx->exinf = mbxcb->exinf;
  338:                 pk_rmbx->wtsk = wait_tskid(&mbxcb->wait_queue);
  339:                 pk_rmbx->pk_msg = headmsg(mbxcb);
  340:         }
  341:         END_CRITICAL_SECTION;
  342: 
  343:         return ercd;
  344: }
  345: 
  346: /* ------------------------------------------------------------------------ */
  347: /*
  348:  *      Debugger support function
  349:  */
  350: #if USE_DBGSPT
  351: 
  352: /*
  353:  * Get object name from control block
  354:  */
  355: #if USE_OBJECT_NAME
  356: EXPORT ER mailbox_getname(ID id, UB **name)
  357: {
  358:         MBXCB  *mbxcb;
  359:         ER     ercd = E_OK;
  360: 
  361:         CHECK_MBXID(id);
  362: 
  363:         BEGIN_DISABLE_INTERRUPT;
  364:         mbxcb = get_mbxcb(id);
  365:         if ( mbxcb->mbxid == 0 ) {
  366:                 ercd = E_NOEXS;
  367:                 goto error_exit;
  368:         }
  369:         if ( (mbxcb->mbxatr & TA_DSNAME) == 0 ) {
  370:                 ercd = E_OBJ;
  371:                 goto error_exit;
  372:         }
  373:         *name = mbxcb->name;
  374: 
  375:     error_exit:
  376:         END_DISABLE_INTERRUPT;
  377: 
  378:         return ercd;
  379: }
  380: #endif /* USE_OBJECT_NAME */
  381: 
  382: /*
  383:  * Refer mailbox usage state
  384:  */
  385: SYSCALL INT _td_lst_mbx( ID list[], INT nent )
  386: {
  387:         MBXCB  *mbxcb, *end;
  388:         INT    n = 0;
  389: 
  390:         BEGIN_DISABLE_INTERRUPT;
  391:         end = mbxcb_table + NUM_MBXID;
  392:         for ( mbxcb = mbxcb_table; mbxcb < end; mbxcb++ ) {
  393:                 if ( mbxcb->mbxid == 0 ) {
  394:                         continue;
  395:                 }
  396: 
  397:                 if ( n++ < nent ) {
  398:                         *list++ = mbxcb->mbxid;
  399:                 }
  400:         }
  401:         END_DISABLE_INTERRUPT;
  402: 
  403:         return n;
  404: }
  405: 
  406: /*
  407:  * Refer mailbox state
  408:  */
  409: SYSCALL ER _td_ref_mbx( ID mbxid, TD_RMBX *pk_rmbx )
  410: {
  411:         MBXCB  *mbxcb;
  412:         ER     ercd = E_OK;
  413: 
  414:         CHECK_MBXID(mbxid);
  415: 
  416:         mbxcb = get_mbxcb(mbxid);
  417: 
  418:         BEGIN_DISABLE_INTERRUPT;
  419:         if ( mbxcb->mbxid == 0 ) {
  420:                 ercd = E_NOEXS;
  421:         } else {
  422:                 pk_rmbx->exinf = mbxcb->exinf;
  423:                 pk_rmbx->wtsk = wait_tskid(&mbxcb->wait_queue);
  424:                 pk_rmbx->pk_msg = headmsg(mbxcb);
  425:         }
  426:         END_DISABLE_INTERRUPT;
  427: 
  428:         return ercd;
  429: }
  430: 
  431: /*
  432:  * Refer mailbox wait queue
  433:  */
  434: SYSCALL INT _td_mbx_que( ID mbxid, ID list[], INT nent )
  435: {
  436:         MBXCB  *mbxcb;
  437:         QUEUE  *q;
  438:         ER     ercd = E_OK;
  439: 
  440:         CHECK_MBXID(mbxid);
  441: 
  442:         mbxcb = get_mbxcb(mbxid);
  443: 
  444:         BEGIN_DISABLE_INTERRUPT;
  445:         if ( mbxcb->mbxid == 0 ) {
  446:                 ercd = E_NOEXS;
  447:         } else {
  448:                 INT n = 0;
  449:                 for ( q = mbxcb->wait_queue.next; q != &mbxcb->wait_queue; q = q->next ) {
  450:                         if ( n++ < nent ) {
  451:                                 *list++ = ((TCB*)q)->tskid;
  452:                         }
  453:                 }
  454:                 ercd = n;
  455:         }
  456:         END_DISABLE_INTERRUPT;
  457: 
  458:         return ercd;
  459: }
  460: 
  461: #endif /* USE_DBGSPT */
  462: #endif /* NUM_MBXID */