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:  *      time_calls.c (T-Kernel/OS)
   19:  *      Time Management Function
   20:  */
   21: 
   22: #include "kernel.h"
   23: #include "timer.h"
   24: #include "task.h"
   25: #include "wait.h"
   26: #include "check.h"
   27: #include "tkdev_timer.h"
   28: #include <sys/rominfo.h>
   29: 
   30: /*
   31:  * Set system clock
   32:  */
   33: SYSCALL ER _tk_set_tim( CONST SYSTIM *pk_tim )
   34: {
   35:         CHECK_PAR(pk_tim->hi >= 0);
   36: 
   37:         BEGIN_CRITICAL_SECTION;
   38:         real_time_ofs = ll_sub(toLSYSTIM(pk_tim), current_time);
   39:         END_CRITICAL_SECTION;
   40: 
   41:         return E_OK;
   42: }
   43: 
   44: SYSCALL ER _tk_set_tim_u( SYSTIM_U tim_u )
   45: {
   46:         CHECK_PAR(tim_u >= 0);
   47: 
   48:         BEGIN_CRITICAL_SECTION;
   49:         real_time_ofs = ll_sub(ltoll(tim_u), current_time);
   50:         END_CRITICAL_SECTION;
   51: 
   52:         return E_OK;
   53: }
   54: 
   55: /*
   56:  * Refer system clock
   57:  */
   58: SYSCALL ER _tk_get_tim( SYSTIM *pk_tim )
   59: {
   60:         BEGIN_CRITICAL_SECTION;
   61:         *pk_tim = toSYSTIM(real_time(), NULL);
   62:         END_CRITICAL_SECTION;
   63: 
   64:         return E_OK;
   65: }
   66: 
   67: SYSCALL ER _tk_get_tim_u( SYSTIM_U *tim_u, UINT *ofs )
   68: {
   69:         BEGIN_CRITICAL_SECTION;
   70:         *tim_u = lltol(real_time());
   71:         if ( ofs != NULL ) *ofs = get_hw_timer_nsec();
   72:         END_CRITICAL_SECTION;
   73: 
   74:         return E_OK;
   75: }
   76: 
   77: /*
   78:  * Refer system operating time
   79:  */
   80: SYSCALL ER _tk_get_otm( SYSTIM *pk_tim )
   81: {
   82:         BEGIN_CRITICAL_SECTION;
   83:         *pk_tim = toSYSTIM(current_time, NULL);
   84:         END_CRITICAL_SECTION;
   85: 
   86:         return E_OK;
   87: }
   88: 
   89: SYSCALL ER _tk_get_otm_u( SYSTIM_U *tim_u, UINT *ofs )
   90: {
   91:         BEGIN_CRITICAL_SECTION;
   92:         *tim_u = lltol(current_time);
   93:         if ( ofs != NULL ) *ofs = get_hw_timer_nsec();
   94:         END_CRITICAL_SECTION;
   95: 
   96:         return E_OK;
   97: }
   98: 
   99: #if USE_DBGSPT
  100: /*
  101:  * Refer system clock
  102:  */
  103: SYSCALL ER _td_get_tim( SYSTIM *tim, UINT *ofs )
  104: {
  105:         UINT   us;
  106: 
  107:         BEGIN_DISABLE_INTERRUPT;
  108:         *tim = toSYSTIM(real_time(), &us);
  109:         *ofs = us * 1000 + get_hw_timer_nsec();
  110:         END_DISABLE_INTERRUPT;
  111: 
  112:         return E_OK;
  113: }
  114: 
  115: SYSCALL ER _td_get_tim_u( SYSTIM_U *tim_u, UINT *ofs )
  116: {
  117:         BEGIN_DISABLE_INTERRUPT;
  118:         *tim_u = lltol(real_time());
  119:         *ofs = get_hw_timer_nsec();
  120:         END_DISABLE_INTERRUPT;
  121: 
  122:         return E_OK;
  123: }
  124: 
  125: /*
  126:  * Refer system operating time
  127:  */
  128: SYSCALL ER _td_get_otm( SYSTIM *tim, UINT *ofs )
  129: {
  130:         UINT   us;
  131: 
  132:         BEGIN_DISABLE_INTERRUPT;
  133:         *tim = toSYSTIM(current_time, &us);
  134:         *ofs = us * 1000 + get_hw_timer_nsec();
  135:         END_DISABLE_INTERRUPT;
  136: 
  137:         return E_OK;
  138: }
  139: 
  140: SYSCALL ER _td_get_otm_u( SYSTIM_U *tim_u, UINT *ofs )
  141: {
  142:         BEGIN_DISABLE_INTERRUPT;
  143:         *tim_u = lltol(current_time);
  144:         *ofs = get_hw_timer_nsec();
  145:         END_DISABLE_INTERRUPT;
  146: 
  147:         return E_OK;
  148: }
  149: #endif /* USE_DBGSPT */
  150: 
  151: /* ------------------------------------------------------------------------ */
  152: 
  153: /*
  154:  * Definition of task delay wait specification
  155:  */
  156: LOCAL CONST WSPEC wspec_dly = { TTW_DLY, NULL, NULL };
  157: 
  158: /*
  159:  * Task delay
  160:  */
  161: SYSCALL ER _tk_dly_tsk( RELTIM dlytim )
  162: {
  163:         return _tk_dly_tsk_u(to_usec(dlytim));
  164: }
  165: 
  166: SYSCALL ER _tk_dly_tsk_u( RELTIM_U dlytim )
  167: {
  168:         ER     ercd = E_OK;
  169: 
  170:         CHECK_DISPATCH();
  171: 
  172:         if ( dlytim > 0 ) {
  173:                 BEGIN_CRITICAL_SECTION;
  174:                 /* Check wait disable */
  175:                 if ( (ctxtsk->waitmask & TTW_DLY) != 0 ) {
  176:                         ercd = E_DISWAI;
  177:                 } else {
  178:                         ctxtsk->wspec = &wspec_dly;
  179:                         ctxtsk->wid = 0;
  180:                         ctxtsk->wercd = &ercd;
  181:                         make_wait_reltim(dlytim, TA_NULL);
  182:                         QueInit(&ctxtsk->tskque);
  183:                 }
  184:                 END_CRITICAL_SECTION;
  185:         }
  186: 
  187:         return ercd;
  188: }
  189: 
  190: /* ------------------------------------------------------------------------ */
  191: /*
  192:  *      Cyclic handler
  193:  */
  194: 
  195: #ifdef NUM_CYCID
  196: 
  197: EXPORT ID       max_cycid;    /* Maximum interval start ID */
  198: 
  199: /*
  200:  * Cyclic handler control block
  201:  */
  202: typedef struct cyclic_handler_control_block {
  203:         void   *exinf;           /* Extended information */
  204:         ATR    cycatr;            /* Cyclic handler attribute */
  205:         FP     cychdr;             /* Cyclic handler address */
  206:         UINT   cycstat;  /* Cyclic handler state */
  207:         RELTIM_U cyctim;       /* Cyclic time */
  208:         TMEB   cyctmeb;  /* Timer event block */
  209: #if TA_GP
  210:         void   *gp;              /* Global pointer */
  211: #endif
  212: #if USE_OBJECT_NAME
  213:         UB     name[OBJECT_NAME_LENGTH];   /* name */
  214: #endif
  215: } CYCCB;
  216: 
  217: LOCAL CYCCB     *cyccb_table;       /* Cyclic handler control block */
  218: LOCAL QUEUE     free_cyccb; /* FreeQue */
  219: 
  220: #define get_cyccb(id)   ( &cyccb_table[INDEX_CYC(id)] )
  221: 
  222: 
  223: /*
  224:  * Initialization of cyclic handler control block
  225:  */
  226: EXPORT ER cyclichandler_initialize( void )
  227: {
  228:         CYCCB  *cyccb, *end;
  229:         W      n;
  230: 
  231:         /* Get system information */
  232:         n = _tk_get_cfn(SCTAG_TMAXCYCID, &max_cycid, 1);
  233:         if ( n < 1 || NUM_CYCID < 1 ) {
  234:                 return E_SYS;
  235:         }
  236: 
  237:         /* Create cyclic handler control block */
  238:         cyccb_table = Imalloc((UINT)NUM_CYCID * sizeof(CYCCB));
  239:         if ( cyccb_table == NULL ) {
  240:                 return E_NOMEM;
  241:         }
  242: 
  243:         /* Register all control blocks onto FreeQue */
  244:         QueInit(&free_cyccb);
  245:         end = cyccb_table + NUM_CYCID;
  246:         for ( cyccb = cyccb_table; cyccb < end; cyccb++ ) {
  247:                 cyccb->cychdr = NULL; /* Unregistered handler */
  248:                 QueInsert((QUEUE*)cyccb, &free_cyccb);
  249:         }
  250: 
  251:         return E_OK;
  252: }
  253: 
  254: 
  255: /*
  256:  * Next startup time
  257:  */
  258: Inline LSYSTIM cyc_next_time( CYCCB *cyccb )
  259: {
  260:         LSYSTIM                tm;
  261:         longlong       n;
  262: 
  263:         tm = ll_add(cyccb->cyctmeb.time, uitoll(cyccb->cyctim));
  264: 
  265:         if ( ll_cmp(tm, current_time) <= 0 ) {
  266: 
  267:                 /* Adjust time to be later than current time */
  268:                 tm = ll_sub(current_time, cyccb->cyctmeb.time);
  269:                 n  = lui_div(tm, cyccb->cyctim);
  270:                 ll_inc(&n);
  271:                 tm = lui_mul(n, cyccb->cyctim);
  272:                 tm = ll_add(cyccb->cyctmeb.time, tm);
  273:         }
  274: 
  275:         return tm;
  276: }
  277: 
  278: LOCAL void call_cychdr( CYCCB* cyccb );
  279: 
  280: /*
  281:  * Register timer event queue
  282:  */
  283: Inline void cyc_timer_insert( CYCCB *cyccb, LSYSTIM tm )
  284: {
  285:         timer_insert_abs(&cyccb->cyctmeb, tm, (CBACK)call_cychdr, cyccb);
  286: }
  287: 
  288: /*
  289:  * Cyclic handler routine
  290:  */
  291: LOCAL void call_cychdr( CYCCB *cyccb )
  292: {
  293:         /* Set next startup time */
  294:         cyc_timer_insert(cyccb, cyc_next_time(cyccb));
  295: 
  296:         /* Execute cyclic handler / Enable interrupt nest */
  297:         ENABLE_INTERRUPT_UPTO(TIMER_INTLEVEL);
  298:         CallUserHandlerP1(cyccb->exinf, cyccb->cychdr, cyccb);
  299:         DISABLE_INTERRUPT;
  300: }
  301: 
  302: /*
  303:  * Immediate call of cyclic handler
  304:  */
  305: LOCAL void immediate_call_cychdr( CYCCB *cyccb )
  306: {
  307:         /* Set next startup time */
  308:         cyc_timer_insert(cyccb, cyc_next_time(cyccb));
  309: 
  310:         /* Execute cyclic handler in task-independent part
  311:            (Keep interrupt disabled) */
  312:         ENTER_TASK_INDEPENDENT;
  313:         CallUserHandlerP1(cyccb->exinf, cyccb->cychdr, cyccb);
  314:         LEAVE_TASK_INDEPENDENT;
  315: }
  316: 
  317: 
  318: /*
  319:  * Create cyclic handler
  320:  */
  321: SYSCALL ID _tk_cre_cyc P1( CONST T_CCYC *pk_ccyc )
  322: {
  323:         T_CCYC_U lccyc;
  324: 
  325:         lccyc.exinf    = pk_ccyc->exinf;
  326:         lccyc.cycatr   = pk_ccyc->cycatr;
  327:         lccyc.cychdr   = pk_ccyc->cychdr;
  328:         lccyc.cyctim_u = to_usec(pk_ccyc->cyctim);
  329:         lccyc.cycphs_u = to_usec(pk_ccyc->cycphs);
  330: #if USE_OBJECT_NAME
  331:         if ( (pk_ccyc->cycatr & TA_DSNAME) != 0 ) {
  332:                 STRNCPY((char*)lccyc.dsname, (char*)pk_ccyc->dsname, OBJECT_NAME_LENGTH);
  333:         }
  334: #endif
  335: 
  336: #if TA_GP
  337:         lccyc.gp = pk_ccyc->gp;
  338:         return _tk_cre_cyc_u(&lccyc, 0, 0, 0, 0, gp);
  339: #else
  340:         return _tk_cre_cyc_u(&lccyc);
  341: #endif
  342: }
  343: 
  344: SYSCALL ID _tk_cre_cyc_u P1( CONST T_CCYC_U *pk_ccyc )
  345: {
  346: #if CHK_RSATR
  347:         const ATR VALID_CYCATR = {
  348:                  TA_HLNG
  349:                 |TA_STA
  350:                 |TA_PHS
  351:                 |TA_GP
  352: #if USE_OBJECT_NAME
  353:                 |TA_DSNAME
  354: #endif
  355:         };
  356: #endif
  357:         CYCCB  *cyccb;
  358:         LSYSTIM        tm;
  359:         ER     ercd = E_OK;
  360: 
  361:         CHECK_RSATR(pk_ccyc->cycatr, VALID_CYCATR);
  362:         CHECK_PAR(pk_ccyc->cychdr != NULL);
  363:         CHECK_PAR(pk_ccyc->cyctim_u > 0);
  364: 
  365:         BEGIN_CRITICAL_SECTION;
  366:         /* Get control block from FreeQue */
  367:         cyccb = (CYCCB*)QueRemoveNext(&free_cyccb);
  368:         if ( cyccb == NULL ) {
  369:                 ercd = E_LIMIT;
  370:                 goto error_exit;
  371:         }
  372: 
  373:         /* Initialize control block */
  374:         cyccb->exinf   = pk_ccyc->exinf;
  375:         cyccb->cycatr  = pk_ccyc->cycatr;
  376:         cyccb->cychdr  = pk_ccyc->cychdr;
  377:         cyccb->cyctim  = pk_ccyc->cyctim_u;
  378: #if USE_OBJECT_NAME
  379:         if ( (pk_ccyc->cycatr & TA_DSNAME) != 0 ) {
  380:                 STRNCPY((char*)cyccb->name, (char*)pk_ccyc->dsname, OBJECT_NAME_LENGTH);
  381:         }
  382: #endif
  383: #if TA_GP
  384:         if ( (pk_ccyc->cycatr & TA_GP) != 0 ) {
  385:                 gp = pk_ccyc->gp;
  386:         }
  387:         cyccb->gp = gp;
  388: #endif
  389: 
  390:         /* First startup time
  391:          *     To guarantee the start of handler after the specified time,
  392:          *     add adjust_time().
  393:          */
  394:         tm = ll_add(current_time, uitoll(pk_ccyc->cycphs_u));
  395:         tm = ll_add(tm, uitoll(adjust_time()));
  396: 
  397:         if ( (pk_ccyc->cycatr & TA_STA) != 0 ) {
  398:                 /* Start cyclic handler */
  399:                 cyccb->cycstat = TCYC_STA;
  400: 
  401:                 if ( pk_ccyc->cycphs_u == 0 ) {
  402:                         /* Immediate execution */
  403:                         cyccb->cyctmeb.time = tm;
  404:                         immediate_call_cychdr(cyccb);
  405:                 } else {
  406:                         /* Register onto timer event queue */
  407:                         cyc_timer_insert(cyccb, tm);
  408:                 }
  409:         } else {
  410:                 /* Initialize only counter */
  411:                 cyccb->cycstat = TCYC_STP;
  412:                 cyccb->cyctmeb.time = tm;
  413:         }
  414: 
  415:         ercd = ID_CYC(cyccb - cyccb_table);
  416: 
  417:     error_exit:
  418:         END_CRITICAL_SECTION;
  419: 
  420:         return ercd;
  421: }
  422: 
  423: /*
  424:  * Delete cyclic handler
  425:  */
  426: SYSCALL ER _tk_del_cyc( ID cycid )
  427: {
  428:         CYCCB  *cyccb;
  429:         ER     ercd = E_OK;
  430: 
  431:         CHECK_CYCID(cycid);
  432: 
  433:         cyccb = get_cyccb(cycid);
  434: 
  435:         BEGIN_CRITICAL_SECTION;
  436:         if ( cyccb->cychdr == NULL ) { /* Unregistered handler */
  437:                 ercd = E_NOEXS;
  438:         } else {
  439:                 if ( (cyccb->cycstat & TCYC_STA) != 0 ) {
  440:                         /* Delete timer event queue */
  441:                         timer_delete(&cyccb->cyctmeb);
  442:                 }
  443: 
  444:                 /* Return to FreeQue */
  445:                 QueInsert((QUEUE*)cyccb, &free_cyccb);
  446:                 cyccb->cychdr = NULL; /* Unregistered handler */
  447:         }
  448:         END_CRITICAL_SECTION;
  449: 
  450:         return ercd;
  451: }
  452: 
  453: 
  454: /*
  455:  * Start cyclic handler
  456:  */
  457: SYSCALL ER _tk_sta_cyc( ID cycid )
  458: {
  459:         CYCCB  *cyccb;
  460:         LSYSTIM        tm;
  461:         ER     ercd = E_OK;
  462: 
  463:         CHECK_CYCID(cycid);
  464: 
  465:         cyccb = get_cyccb(cycid);
  466: 
  467:         BEGIN_CRITICAL_SECTION;
  468:         if ( cyccb->cychdr == NULL ) { /* Unregistered handler */
  469:                 ercd = E_NOEXS;
  470:                 goto error_exit;
  471:         }
  472: 
  473:         if ( (cyccb->cycatr & TA_PHS) != 0 ) {
  474:                 /* Continue cyclic phase */
  475:                 if ( (cyccb->cycstat & TCYC_STA) == 0 ) {
  476:                         /* Start cyclic handler */
  477:                         tm = cyccb->cyctmeb.time;
  478:                         if ( ll_cmp(tm, current_time) <= 0 ) {
  479:                                 tm = cyc_next_time(cyccb);
  480:                         }
  481:                         cyc_timer_insert(cyccb, tm);
  482:                 }
  483:         } else {
  484:                 /* Reset cyclic interval */
  485:                 if ( (cyccb->cycstat & TCYC_STA) != 0 ) {
  486:                         /* Stop once */
  487:                         timer_delete(&cyccb->cyctmeb);
  488:                 }
  489: 
  490:                 /* FIRST ACTIVATION TIME
  491:                  *    Adjust the first activation time with adjust_time().
  492:                  */
  493:                 tm = ll_add(current_time, uitoll(cyccb->cyctim));
  494:                 tm = ll_add(tm, uitoll(adjust_time()));
  495: 
  496:                 /* Start cyclic handler */
  497:                 cyc_timer_insert(cyccb, tm);
  498:         }
  499:         cyccb->cycstat |= TCYC_STA;
  500: 
  501:     error_exit:
  502:         END_CRITICAL_SECTION;
  503: 
  504:         return ercd;
  505: }
  506: 
  507: /*
  508:  * Stop cyclic handler
  509:  */
  510: SYSCALL ER _tk_stp_cyc( ID cycid )
  511: {
  512:         CYCCB  *cyccb;
  513:         ER     ercd = E_OK;
  514: 
  515:         CHECK_CYCID(cycid);
  516: 
  517:         cyccb = get_cyccb(cycid);
  518: 
  519:         BEGIN_CRITICAL_SECTION;
  520:         if ( cyccb->cychdr == NULL ) { /* Unregistered handler */
  521:                 ercd = E_NOEXS;
  522:         } else {
  523:                 if ( (cyccb->cycstat & TCYC_STA) != 0 ) {
  524:                         /* Stop cyclic handler */
  525:                         timer_delete(&cyccb->cyctmeb);
  526:                 }
  527:                 cyccb->cycstat &= ~TCYC_STA;
  528:         }
  529:         END_CRITICAL_SECTION;
  530: 
  531:         return ercd;
  532: }
  533: 
  534: /*
  535:  * Refer cyclic handler state
  536:  */
  537: SYSCALL ER _tk_ref_cyc( ID cycid, T_RCYC* pk_rcyc )
  538: {
  539:         T_RCYC_U lrcyc;
  540:         ER     ercd;
  541: 
  542:         ercd = _tk_ref_cyc_u(cycid, &lrcyc);
  543: 
  544:         pk_rcyc->exinf  = lrcyc.exinf;
  545:         pk_rcyc->lfttim         = to_msec(lrcyc.lfttim_u);
  546:         pk_rcyc->cycstat = lrcyc.cycstat;
  547: 
  548:         return ercd;
  549: }
  550: 
  551: SYSCALL ER _tk_ref_cyc_u( ID cycid, T_RCYC_U *pk_rcyc )
  552: {
  553:         CYCCB  *cyccb;
  554:         LSYSTIM        tm;
  555:         ER     ercd = E_OK;
  556: 
  557:         CHECK_CYCID(cycid);
  558: 
  559:         cyccb = get_cyccb(cycid);
  560: 
  561:         BEGIN_CRITICAL_SECTION;
  562:         if ( cyccb->cychdr == NULL ) { /* Unregistered handler */
  563:                 ercd = E_NOEXS;
  564:         } else {
  565:                 tm = cyccb->cyctmeb.time;
  566:                 if ( (cyccb->cycstat & TCYC_STA) == 0 ) {
  567:                         if ( ll_cmp(tm, current_time) <= 0 ) {
  568:                                 tm = cyc_next_time(cyccb);
  569:                         }
  570:                 }
  571:                 tm = ll_sub(tm, current_time);
  572:                 tm = ll_sub(tm, uitoll(adjust_time()));
  573:                 if ( ll_sign(tm) < 0 ) {
  574:                         tm = ltoll(0);
  575:                 }
  576: 
  577:                 pk_rcyc->exinf    = cyccb->exinf;
  578:                 pk_rcyc->lfttim_u = (RELTIM_U)tm;
  579:                 pk_rcyc->cycstat  = cyccb->cycstat;
  580:         }
  581:         END_CRITICAL_SECTION;
  582: 
  583:         return ercd;
  584: }
  585: 
  586: #if USE_DBGSPT
  587: 
  588: /*
  589:  * Get object name from control block
  590:  */
  591: #if USE_OBJECT_NAME
  592: EXPORT ER cyclichandler_getname(ID id, UB **name)
  593: {
  594:         CYCCB  *cyccb;
  595:         ER     ercd = E_OK;
  596: 
  597:         CHECK_CYCID(id);
  598: 
  599:         BEGIN_DISABLE_INTERRUPT;
  600:         cyccb = get_cyccb(id);
  601:         if ( cyccb->cychdr == NULL ) {
  602:                 ercd = E_NOEXS;
  603:                 goto error_exit;
  604:         }
  605:         if ( (cyccb->cycatr & TA_DSNAME) == 0 ) {
  606:                 ercd = E_OBJ;
  607:                 goto error_exit;
  608:         }
  609:         *name = cyccb->name;
  610: 
  611:     error_exit:
  612:         END_DISABLE_INTERRUPT;
  613: 
  614:         return ercd;
  615: }
  616: #endif /* USE_OBJECT_NAME */
  617: 
  618: /*
  619:  * Refer cyclic handler usage state
  620:  */
  621: SYSCALL INT _td_lst_cyc( ID list[], INT nent )
  622: {
  623:         CYCCB  *cyccb, *end;
  624:         INT    n = 0;
  625: 
  626:         BEGIN_DISABLE_INTERRUPT;
  627:         end = cyccb_table + NUM_CYCID;
  628:         for ( cyccb = cyccb_table; cyccb < end; cyccb++ ) {
  629:                 /* Unregistered handler */
  630:                 if ( cyccb->cychdr == NULL ) {
  631:                         continue;
  632:                 }
  633: 
  634:                 if ( n++ < nent ) {
  635:                         *list++ = ID_CYC(cyccb - cyccb_table);
  636:                 }
  637:         }
  638:         END_DISABLE_INTERRUPT;
  639: 
  640:         return n;
  641: }
  642: 
  643: /*
  644:  * Refer cyclic handler state
  645:  */
  646: SYSCALL ER _td_ref_cyc( ID cycid, TD_RCYC* pk_rcyc )
  647: {
  648:         TD_RCYC_U      lrcyc;
  649:         ER             ercd;
  650: 
  651:         ercd = _td_ref_cyc_u(cycid, &lrcyc);
  652: 
  653:         pk_rcyc->exinf  = lrcyc.exinf;
  654:         pk_rcyc->lfttim         = to_msec(lrcyc.lfttim_u);
  655:         pk_rcyc->cycstat = lrcyc.cycstat;
  656: 
  657:         return ercd;
  658: }
  659: 
  660: SYSCALL ER _td_ref_cyc_u( ID cycid, TD_RCYC_U *pk_rcyc )
  661: {
  662:         CYCCB  *cyccb;
  663:         LSYSTIM        tm;
  664:         ER     ercd = E_OK;
  665: 
  666:         CHECK_CYCID(cycid);
  667: 
  668:         cyccb = get_cyccb(cycid);
  669: 
  670:         BEGIN_DISABLE_INTERRUPT;
  671:         if ( cyccb->cychdr == NULL ) { /* Unregistered handler */
  672:                 ercd = E_NOEXS;
  673:         } else {
  674:                 tm = cyccb->cyctmeb.time;
  675:                 if ( (cyccb->cycstat & TCYC_STA) == 0 ) {
  676:                         if ( ll_cmp(tm, current_time) <= 0 ) {
  677:                                 tm = cyc_next_time(cyccb);
  678:                         }
  679:                 }
  680:                 tm = ll_sub(tm, current_time);
  681:                 tm = ll_sub(tm, uitoll(adjust_time()));
  682:                 if ( ll_sign(tm) < 0 ) {
  683:                         tm = ltoll(0);
  684:                 }
  685: 
  686:                 pk_rcyc->exinf    = cyccb->exinf;
  687:                 pk_rcyc->lfttim_u = (RELTIM_U)tm;
  688:                 pk_rcyc->cycstat  = cyccb->cycstat;
  689:         }
  690:         END_DISABLE_INTERRUPT;
  691: 
  692:         return ercd;
  693: }
  694: #endif /* USE_DBGSPT */
  695: #endif /* NUM_CYCID */
  696: 
  697: /* ------------------------------------------------------------------------ */
  698: /*
  699:  *      Alarm handler
  700:  */
  701: 
  702: #ifdef NUM_ALMID
  703: 
  704: EXPORT ID       max_almid;    /* Maximum alarm handler ID */
  705: 
  706: /*
  707:  * Alarm handler control block
  708:  */
  709: typedef struct alarm_handler_control_block {
  710:         void   *exinf;           /* Extended information */
  711:         ATR    almatr;            /* Alarm handler attribute */
  712:         FP     almhdr;             /* Alarm handler address */
  713:         UINT   almstat;  /* Alarm handler state */
  714:         TMEB   almtmeb;  /* Timer event block */
  715: #if TA_GP
  716:         void   *gp;              /* Global pointer */
  717: #endif
  718: #if USE_OBJECT_NAME
  719:         UB     name[OBJECT_NAME_LENGTH];   /* name */
  720: #endif
  721: } ALMCB;
  722: 
  723: LOCAL ALMCB     *almcb_table;       /* Alarm handler control block */
  724: LOCAL QUEUE     free_almcb; /* FreeQue */
  725: 
  726: #define get_almcb(id)   ( &almcb_table[INDEX_ALM(id)] )
  727: 
  728: 
  729: /*
  730:  * Initialization of alarm handler control block
  731:  */
  732: EXPORT ER alarmhandler_initialize( void )
  733: {
  734:         ALMCB  *almcb, *end;
  735:         W      n;
  736: 
  737:         /* Get system information */
  738:         n = _tk_get_cfn(SCTAG_TMAXALMID, &max_almid, 1);
  739:         if ( n < 1 || NUM_ALMID < 1 ) {
  740:                 return E_SYS;
  741:         }
  742: 
  743:         /* Create alarm handler control block */
  744:         almcb_table = Imalloc((UINT)NUM_ALMID * sizeof(ALMCB));
  745:         if ( almcb_table == NULL ) {
  746:                 return E_NOMEM;
  747:         }
  748: 
  749:         /* Register all control blocks onto FreeQue */
  750:         QueInit(&free_almcb);
  751:         end = almcb_table + NUM_ALMID;
  752:         for ( almcb = almcb_table; almcb < end; almcb++ ) {
  753:                 almcb->almhdr = NULL; /* Unregistered handler */
  754:                 QueInsert((QUEUE*)almcb, &free_almcb);
  755:         }
  756: 
  757:         return E_OK;
  758: }
  759: 
  760: 
  761: /*
  762:  * Alarm handler start routine
  763:  */
  764: LOCAL void call_almhdr( ALMCB *almcb )
  765: {
  766:         almcb->almstat &= ~TALM_STA;
  767: 
  768:         /* Execute alarm handler/ Enable interrupt nesting */
  769:         ENABLE_INTERRUPT_UPTO(TIMER_INTLEVEL);
  770:         CallUserHandlerP1(almcb->exinf, almcb->almhdr, almcb);
  771:         DISABLE_INTERRUPT;
  772: }
  773: 
  774: /*
  775:  * Alarm handler immediate call
  776:  */
  777: LOCAL void immediate_call_almhdr( ALMCB *almcb )
  778: {
  779:         almcb->almstat &= ~TALM_STA;
  780: 
  781:         /* Execute alarm handler in task-independent part
  782:            (Keep interrupt disabled) */
  783:         ENTER_TASK_INDEPENDENT;
  784:         CallUserHandlerP1(almcb->exinf, almcb->almhdr, almcb);
  785:         LEAVE_TASK_INDEPENDENT;
  786: }
  787: 
  788: /*
  789:  * Register onto timer event queue
  790:  */
  791: Inline void alm_timer_insert( ALMCB *almcb, RELTIM_U reltim )
  792: {
  793:         LSYSTIM        tm;
  794: 
  795:         /* To guarantee to start the handler after the specified time,
  796:            add adjust_time() */
  797:         tm = ll_add(current_time, uitoll(reltim));
  798:         tm = ll_add(tm, uitoll(adjust_time()));
  799: 
  800:         timer_insert_abs(&almcb->almtmeb, tm, (CBACK)call_almhdr, almcb);
  801: }
  802: 
  803: 
  804: /*
  805:  * Create alarm handler
  806:  */
  807: SYSCALL ID _tk_cre_alm P1( CONST T_CALM *pk_calm )
  808: {
  809: #if CHK_RSATR
  810:         const ATR VALID_ALMATR = {
  811:                  TA_HLNG
  812:                 |TA_GP
  813: #if USE_OBJECT_NAME
  814:                 |TA_DSNAME
  815: #endif
  816:         };
  817: #endif
  818:         ALMCB  *almcb;
  819:         ER     ercd = E_OK;
  820: 
  821:         CHECK_RSATR(pk_calm->almatr, VALID_ALMATR);
  822:         CHECK_PAR(pk_calm->almhdr != NULL);
  823: 
  824:         BEGIN_CRITICAL_SECTION;
  825:         /* Get control block from free queue */
  826:         almcb = (ALMCB*)QueRemoveNext(&free_almcb);
  827:         if ( almcb == NULL ) {
  828:                 ercd = E_LIMIT;
  829:                 goto error_exit;
  830:         }
  831: 
  832:         /* Initialize control block */
  833:         almcb->exinf   = pk_calm->exinf;
  834:         almcb->almatr  = pk_calm->almatr;
  835:         almcb->almhdr  = pk_calm->almhdr;
  836:         almcb->almstat = TALM_STP;
  837: #if USE_OBJECT_NAME
  838:         if ( (pk_calm->almatr & TA_DSNAME) != 0 ) {
  839:                 STRNCPY((char*)almcb->name, (char*)pk_calm->dsname, OBJECT_NAME_LENGTH);
  840:         }
  841: #endif
  842: #if TA_GP
  843:         if ( (pk_calm->almatr & TA_GP) != 0 ) {
  844:                 gp = pk_calm->gp;
  845:         }
  846:         almcb->gp = gp;
  847: #endif
  848: 
  849:         ercd = ID_ALM(almcb - almcb_table);
  850: 
  851:     error_exit:
  852:         END_CRITICAL_SECTION;
  853: 
  854:         return ercd;
  855: }
  856: 
  857: /*
  858:  * Delete alarm handler
  859:  */
  860: SYSCALL ER _tk_del_alm( ID almid )
  861: {
  862:         ALMCB  *almcb;
  863:         ER     ercd = E_OK;
  864: 
  865:         CHECK_ALMID(almid);
  866: 
  867:         almcb = get_almcb(almid);
  868: 
  869:         BEGIN_CRITICAL_SECTION;
  870:         if ( almcb->almhdr == NULL ) { /* Unregistered handler */
  871:                 ercd = E_NOEXS;
  872:         } else {
  873:                 if ( (almcb->almstat & TALM_STA) != 0 ) {
  874:                         /* Delete from timer event queue */
  875:                         timer_delete(&almcb->almtmeb);
  876:                 }
  877: 
  878:                 /* Return to FreeQue */
  879:                 QueInsert((QUEUE*)almcb, &free_almcb);
  880:                 almcb->almhdr = NULL; /* Unregistered handler */
  881:         }
  882:         END_CRITICAL_SECTION;
  883: 
  884:         return ercd;
  885: }
  886: 
  887: 
  888: /*
  889:  * Start alarm handler
  890:  */
  891: SYSCALL ER _tk_sta_alm( ID almid, RELTIM almtim )
  892: {
  893:         return _tk_sta_alm_u(almid, to_usec(almtim));
  894: }
  895: 
  896: SYSCALL ER _tk_sta_alm_u( ID almid, RELTIM_U almtim )
  897: {
  898:         ALMCB  *almcb;
  899:         ER     ercd = E_OK;
  900: 
  901:         CHECK_ALMID(almid);
  902: 
  903:         almcb = get_almcb(almid);
  904: 
  905:         BEGIN_CRITICAL_SECTION;
  906:         if ( almcb->almhdr == NULL ) { /* Unregistered handler */
  907:                 ercd = E_NOEXS;
  908:                 goto error_exit;
  909:         }
  910: 
  911:         if ( (almcb->almstat & TALM_STA) != 0 ) {
  912:                 /* Cancel current settings */
  913:                 timer_delete(&almcb->almtmeb);
  914:         }
  915: 
  916:         if ( almtim > 0 ) {
  917:                 /* Register onto timer event queue */
  918:                 alm_timer_insert(almcb, almtim);
  919:                 almcb->almstat |= TALM_STA;
  920:         } else {
  921:                 /* Immediate execution */
  922:                 immediate_call_almhdr(almcb);
  923:         }
  924: 
  925:     error_exit:
  926:         END_CRITICAL_SECTION;
  927: 
  928:         return ercd;
  929: }
  930: 
  931: /*
  932:  * Stop alarm handler
  933:  */
  934: SYSCALL ER _tk_stp_alm( ID almid )
  935: {
  936:         ALMCB  *almcb;
  937:         ER     ercd = E_OK;
  938: 
  939:         CHECK_ALMID(almid);
  940: 
  941:         almcb = get_almcb(almid);
  942: 
  943:         BEGIN_CRITICAL_SECTION;
  944:         if ( almcb->almhdr == NULL ) { /* Unregistered handler */
  945:                 ercd = E_NOEXS;
  946:         } else {
  947:                 if ( (almcb->almstat & TALM_STA) != 0 ) {
  948:                         /* Stop alarm handler address */
  949:                         timer_delete(&almcb->almtmeb);
  950:                         almcb->almstat &= ~TALM_STA;
  951:                 }
  952:         }
  953:         END_CRITICAL_SECTION;
  954: 
  955:         return ercd;
  956: }
  957: 
  958: 
  959: /*
  960:  * Refer alarm handler state
  961:  */
  962: SYSCALL ER _tk_ref_alm( ID almid, T_RALM *pk_ralm )
  963: {
  964:         T_RALM_U lralm;
  965:         ER     ercd;
  966: 
  967:         ercd = _tk_ref_alm_u(almid, &lralm);
  968: 
  969:         pk_ralm->exinf  = lralm.exinf;
  970:         pk_ralm->lfttim         = to_msec(lralm.lfttim_u);
  971:         pk_ralm->almstat = lralm.almstat;
  972: 
  973:         return ercd;
  974: }
  975: 
  976: SYSCALL ER _tk_ref_alm_u( ID almid, T_RALM_U *pk_ralm )
  977: {
  978:         ALMCB  *almcb;
  979:         LSYSTIM        tm;
  980:         ER     ercd = E_OK;
  981: 
  982:         CHECK_ALMID(almid);
  983: 
  984:         almcb = get_almcb(almid);
  985: 
  986:         BEGIN_CRITICAL_SECTION;
  987:         if ( almcb->almhdr == NULL ) { /* Unregistered handler */
  988:                 ercd = E_NOEXS;
  989:         } else {
  990:                 if ( (almcb->almstat & TALM_STA) != 0 ) {
  991:                         tm = almcb->almtmeb.time;
  992:                         tm = ll_sub(tm, current_time);
  993:                         tm = ll_sub(tm, uitoll(adjust_time()));
  994:                         if ( ll_sign(tm) < 0 ) {
  995:                                 tm = ltoll(0);
  996:                         }
  997:                 } else {
  998:                         tm = ltoll(0);
  999:                 }
 1000: 
 1001:                 pk_ralm->exinf    = almcb->exinf;
 1002:                 pk_ralm->lfttim_u = (RELTIM_U)tm;
 1003:                 pk_ralm->almstat  = almcb->almstat;
 1004:         }
 1005:         END_CRITICAL_SECTION;
 1006: 
 1007:         return ercd;
 1008: }
 1009: 
 1010: #if USE_DBGSPT
 1011: 
 1012: /*
 1013:  * Get object name from control block
 1014:  */
 1015: #if USE_OBJECT_NAME
 1016: EXPORT ER alarmhandler_getname(ID id, UB **name)
 1017: {
 1018:         ALMCB  *almcb;
 1019:         ER     ercd = E_OK;
 1020: 
 1021:         CHECK_ALMID(id);
 1022: 
 1023:         BEGIN_DISABLE_INTERRUPT;
 1024:         almcb = get_almcb(id);
 1025:         if ( almcb->almhdr == NULL ) {
 1026:                 ercd = E_NOEXS;
 1027:                 goto error_exit;
 1028:         }
 1029:         if ( (almcb->almatr & TA_DSNAME) == 0 ) {
 1030:                 ercd = E_OBJ;
 1031:                 goto error_exit;
 1032:         }
 1033:         *name = almcb->name;
 1034: 
 1035:     error_exit:
 1036:         END_DISABLE_INTERRUPT;
 1037: 
 1038:         return ercd;
 1039: }
 1040: #endif /* USE_OBJECT_NAME */
 1041: 
 1042: /*
 1043:  * Refer alarm handler usage state
 1044:  */
 1045: SYSCALL INT _td_lst_alm( ID list[], INT nent )
 1046: {
 1047:         ALMCB  *almcb, *end;
 1048:         INT    n = 0;
 1049: 
 1050:         BEGIN_DISABLE_INTERRUPT;
 1051:         end = almcb_table + NUM_ALMID;
 1052:         for ( almcb = almcb_table; almcb < end; almcb++ ) {
 1053:                 /* Unregistered handler */
 1054:                 if ( almcb->almhdr == NULL ) {
 1055:                         continue;
 1056:                 }
 1057: 
 1058:                 if ( n++ < nent ) {
 1059:                         *list++ = ID_ALM(almcb - almcb_table);
 1060:                 }
 1061:         }
 1062:         END_DISABLE_INTERRUPT;
 1063: 
 1064:         return n;
 1065: }
 1066: 
 1067: /*
 1068:  * Refer alarm handler state
 1069:  */
 1070: SYSCALL ER _td_ref_alm( ID almid, TD_RALM *pk_ralm )
 1071: {
 1072:         TD_RALM_U      lralm;
 1073:         ER             ercd;
 1074: 
 1075:         ercd = _td_ref_alm_u(almid, &lralm);
 1076: 
 1077:         pk_ralm->exinf  = lralm.exinf;
 1078:         pk_ralm->lfttim         = to_msec(lralm.lfttim_u);
 1079:         pk_ralm->almstat = lralm.almstat;
 1080: 
 1081:         return ercd;
 1082: }
 1083: 
 1084: SYSCALL ER _td_ref_alm_u( ID almid, TD_RALM_U *pk_ralm )
 1085: {
 1086:         ALMCB  *almcb;
 1087:         LSYSTIM        tm;
 1088:         ER     ercd = E_OK;
 1089: 
 1090:         CHECK_ALMID(almid);
 1091: 
 1092:         almcb = get_almcb(almid);
 1093: 
 1094:         BEGIN_DISABLE_INTERRUPT;
 1095:         if ( almcb->almhdr == NULL ) { /* Unregistered handler */
 1096:                 ercd = E_NOEXS;
 1097:         } else {
 1098:                 if ( (almcb->almstat & TALM_STA) != 0 ) {
 1099:                         tm = almcb->almtmeb.time;
 1100:                         tm = ll_sub(tm, current_time);
 1101:                         tm = ll_sub(tm, uitoll(adjust_time()));
 1102:                         if ( ll_sign(tm) < 0 ) {
 1103:                                 tm = ltoll(0);
 1104:                         }
 1105:                 } else {
 1106:                         tm = ltoll(0);
 1107:                 }
 1108: 
 1109:                 pk_ralm->exinf          = almcb->exinf;
 1110:                 pk_ralm->lfttim_u = (RELTIM_U)tm;
 1111:                 pk_ralm->almstat  = almcb->almstat;
 1112:         }
 1113:         END_DISABLE_INTERRUPT;
 1114: 
 1115:         return ercd;
 1116: }
 1117: #endif /* USE_DBGSPT */
 1118: #endif /* NUM_ALMID */