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 2014/09/10.
   11:  *    Modified by TRON Forum(http://www.tron.org/) at 2015/06/01.
   12:  *
   13:  *----------------------------------------------------------------------
   14:  */
   15: 
   16: /*
   17:  *      subsystem.c (T-Kernel/OS)
   18:  *      Subsystem Manager/Task Exception
   19:  */
   20: 
   21: #include "kernel.h"
   22: #include "task.h"
   23: #include "check.h"
   24: #include <sys/bitop.h>
   25: #include <sys/rominfo.h>
   26: 
   27: EXPORT ID       max_ssyid;    /* Maximum subsystem ID */
   28: EXPORT PRI      max_ssypri;  /* Maximum subsystem priority */
   29: EXPORT ID       max_resid;    /* Maximum resource ID */
   30: 
   31: typedef INT  (*SVC)( void *pk_para, FN fncd );  /* Extended SVC handler */
   32: typedef void (*BrkFN)( ID tskid );              /* Break function */
   33: typedef void (*StuFN)( ID resid, INT info );    /* Startup function */
   34: typedef void (*CluFN)( ID resid, INT info );    /* Cleanup function */
   35: typedef ER   (*EvtFN)( INT evttyp, ID resid, INT info );        /* Event function */
   36: 
   37: /*
   38:  * Definition of subsystem control block
   39:  */
   40: typedef struct subsystem_control_block  SSYCB;
   41: struct subsystem_control_block {
   42:         SSYCB  *link;           /* Same subsystem priority link */
   43:         ATR    ssyatr;            /* Subsystem attribute */
   44:         PRI    ssypri;            /* Subsystem priority */
   45:         SVC    svchdr;            /* Extended SVC handler */
   46:         BrkFN  breakfn; /* Break function */
   47:         StuFN  startupfn;       /* Startup function */
   48:         CluFN  cleanupfn;       /* Cleanup function */
   49:         EvtFN  eventfn; /* Event function */
   50:         INT    resblksz;  /* Resource control block size */
   51:         void   *resblk;  /* Resource control block */
   52: #if TA_GP
   53:         void   *gp;              /* Global pointer */
   54: #endif
   55: };
   56: 
   57: LOCAL SSYCB     *ssycb_table;       /* Subsystem control block */
   58: 
   59: #define get_ssycb(id)   ( &ssycb_table[INDEX_SSY(id)] )
   60: 
   61: /*
   62:  * Control table of subsystem priority
   63:  */
   64: LOCAL SSYCB     **ssypri_table;
   65: 
   66: #define ssypri_index(ssypri)    ( (ssypri) - MIN_SSYPRI )
   67: 
   68: /*
   69:  * Bitmap of use resource ID
   70:  *      If the bit corresponding to the resource ID is 1, it is in use.
   71:  */
   72: LOCAL UW *resid_bitmap;
   73: 
   74: /*
   75:  * Convert bitmap index and resource ID
   76:  */
   77: #define index_to_resid(index)   ( (index) + MIN_RESID )
   78: #define resid_to_index(resid)   ( (resid) - MIN_RESID )
   79: 
   80: /*
   81:  * Adjust alignment of resource control block
   82:  */
   83: #define ROUND_RESBLKSZ(resblksz)        ( ((resblksz) + 3) & ~0x00000003U )
   84: 
   85: /*
   86:  * Undefined extended SVC function
   87:  */
   88: IMPORT INT no_support();
   89: 
   90: /*
   91:  * Initialization of subsystem control block
   92:  */
   93: EXPORT ER subsystem_initialize( void )
   94: {
   95:         INT    i;
   96: 
   97:         /* Get system information */
   98:         i = _tk_get_cfn(SCTAG_TMAXSSYID, &max_ssyid, 1);
   99:         if ( i < 1 || NUM_SSYID < 1 ) {
  100:                 return E_SYS;
  101:         }
  102:         i = _tk_get_cfn(SCTAG_TMAXSSYPRI, &max_ssypri, 1);
  103:         if ( i < 1 || NUM_SSYPRI < 1 ) {
  104:                 return E_SYS;
  105:         }
  106: 
  107:         /* Create subsystem control block */
  108:         ssycb_table = Imalloc((UINT)NUM_SSYID * sizeof(SSYCB));
  109:         if ( ssycb_table == NULL ) {
  110:                 goto err_ret1;
  111:         }
  112: 
  113:         /* Create subsystem priority control table */
  114:         ssypri_table = Imalloc((UINT)NUM_SSYPRI * sizeof(SSYCB*));
  115:         if ( ssypri_table == NULL ) {
  116:                 goto err_ret2;
  117:         }
  118: 
  119:         for ( i = 0; i < NUM_SSYID; i++ ) {
  120:                 ssycb_table[i].svchdr    = no_support;
  121:                 ssycb_table[i].breakfn   = NULL;
  122:                 ssycb_table[i].startupfn = NULL;
  123:                 ssycb_table[i].cleanupfn = NULL;
  124:                 ssycb_table[i].eventfn   = NULL;
  125:         }
  126: 
  127:         for ( i = 0; i < NUM_SSYPRI; i++ ) {
  128:                 ssypri_table[i] = NULL;
  129:         }
  130: 
  131:         return E_OK;
  132: 
  133: err_ret2:
  134:         Ifree(ssycb_table);
  135: err_ret1:
  136:         return E_NOMEM;
  137: }
  138: 
  139: 
  140: /*
  141:  * Call break function
  142:  */
  143: IMPORT void call_brkhdr( TCB *tcb );    /* Call break function */
  144: 
  145: 
  146: /*
  147:  * Definition of subsystem
  148:  */
  149: SYSCALL ER _tk_def_ssy P2( ID ssid, CONST T_DSSY *pk_dssy )
  150: {
  151:         SSYCB  *ssycb;
  152:         void   *resblk;
  153:         SSYCB  **prev;
  154:         INT    i;
  155:         ER     ercd = E_OK;
  156: 
  157:         CHECK_SSYID(ssid);
  158: #if CHK_PAR
  159:         if ( pk_dssy != NULL ) {
  160:                 CHECK_RSATR(pk_dssy->ssyatr, TA_NULL|TA_GP);
  161:                 CHECK_SSYPRI(pk_dssy->ssypri);
  162:                 CHECK_PAR(pk_dssy->resblksz >= 0);
  163:         }
  164: #endif
  165: 
  166:         ssycb = get_ssycb(ssid);
  167: 
  168:         resblk = NULL;
  169:         if ( pk_dssy != NULL ) {
  170:                 if ( pk_dssy->resblksz > 0 ) {
  171:                         /* Create resource control block */
  172:                         i = ROUND_RESBLKSZ(pk_dssy->resblksz);
  173:                         resblk = Icalloc((UINT)NUM_RESID, (UINT)i);
  174:                         if ( resblk == NULL ) {
  175:                                 return E_NOMEM;
  176:                         }
  177:                 }
  178:         }
  179: 
  180:         BEGIN_CRITICAL_SECTION;
  181:         if ( pk_dssy != NULL ) {
  182:                 /* Register */
  183:                 if ( ssycb->svchdr != no_support ) {
  184:                         ercd = E_OBJ;  /* Registered */
  185:                         goto error_exit;
  186:                 }
  187:                 ssycb->ssyatr    = pk_dssy->ssyatr;
  188:                 ssycb->ssypri    = pk_dssy->ssypri;
  189:                 ssycb->svchdr    = (SVC)pk_dssy->svchdr;
  190:                 ssycb->breakfn   = (BrkFN)pk_dssy->breakfn;
  191:                 ssycb->startupfn = (StuFN)pk_dssy->startupfn;
  192:                 ssycb->cleanupfn = (CluFN)pk_dssy->cleanupfn;
  193:                 ssycb->eventfn   = (EvtFN)pk_dssy->eventfn;
  194:                 ssycb->resblksz  = pk_dssy->resblksz;
  195:                 ssycb->resblk    = resblk;
  196: #if TA_GP
  197:                 if ( (pk_dssy->ssyatr & TA_GP) != 0 ) {
  198:                         gp = pk_dssy->gp;
  199:                 }
  200:                 ssycb->gp = gp;
  201: #endif
  202: 
  203:                 /* Register to subsystem priority control table */
  204:                 i = ssypri_index(ssycb->ssypri);
  205:                 ssycb->link = ssypri_table[i];
  206:                 ssypri_table[i] = ssycb;
  207:         } else {
  208:                 /* Delete */
  209:                 if ( ssycb->svchdr == no_support ) {
  210:                         ercd = E_NOEXS;  /* Not registered */
  211:                         goto error_exit;
  212:                 }
  213: 
  214:                 /* Delete from subsystem priority group */
  215:                 prev = &ssypri_table[ssypri_index(ssycb->ssypri)];
  216:                 while ( *prev != NULL ) {
  217:                         if ( *prev == ssycb ) {
  218:                                 *prev = ssycb->link;
  219:                                 break;
  220:                         }
  221:                         prev = &(*prev)->link;
  222:                 }
  223: 
  224:                 resblk = ssycb->resblk;
  225:                 ssycb->svchdr    = no_support;
  226:                 ssycb->breakfn   = NULL;
  227:                 ssycb->startupfn = NULL;
  228:                 ssycb->cleanupfn = NULL;
  229:                 ssycb->eventfn   = NULL;
  230:         }
  231: 
  232:     error_exit:
  233:         END_CRITICAL_SECTION;
  234: 
  235:         if ( ercd < E_OK || pk_dssy == NULL ) {
  236:                 if ( resblk != NULL ) {
  237:                         Ifree(resblk);
  238:                 }
  239:         }
  240: 
  241:         return ercd;
  242: }
  243: 
  244: /*
  245:  * Call startup function
  246:  */
  247: SYSCALL ER _tk_sta_ssy( ID ssid, ID resid, INT info )
  248: {
  249:         SSYCB  *ssycb;
  250:         StuFN  startupfn;
  251:         UH     save_texflg;
  252:         INT    i;
  253:         ER     ercd = E_OK;
  254: 
  255:         CHECK_SSYID_ALL(ssid);
  256:         CHECK_RESID(resid);
  257:         CHECK_DISPATCH();
  258: 
  259:         DISABLE_INTERRUPT;
  260:         /* Suspend task exception while startup function is running */
  261:         save_texflg = ctxtsk->texflg;
  262:         ctxtsk->texflg |= SSFN_RUNNING;
  263:         ctxtsk->sysmode++;
  264:         ENABLE_INTERRUPT;
  265: 
  266:         if ( ssid > 0 ) {
  267:                 /* Call only specified subsystem */
  268:                 ssycb = get_ssycb(ssid);
  269:                 if ( ssycb->svchdr == no_support ) {
  270:                         ercd = E_NOEXS;
  271:                 } else {
  272:                         startupfn = ssycb->startupfn;
  273:                         if ( startupfn != NULL ) {
  274:                                 CallUserHandlerP2(resid, info,
  275:                                                         startupfn, ssycb);
  276:                         }
  277:                 }
  278:         } else {
  279:                 /* Call all subsystems in the order of descending priorities */
  280:                 for ( i = 0; i < NUM_SSYPRI; ++i ) {
  281:                         ssycb = ssypri_table[i];
  282:                         while ( ssycb != NULL ) {
  283:                                 startupfn = ssycb->startupfn;
  284:                                 if ( startupfn != NULL ) {
  285:                                         CallUserHandlerP2(resid, info,
  286:                                                         startupfn, ssycb);
  287:                                 }
  288:                                 ssycb = ssycb->link;
  289:                         }
  290:                 }
  291:         }
  292: 
  293:         DISABLE_INTERRUPT;
  294:         ctxtsk->sysmode--;
  295:         ctxtsk->texflg = save_texflg;
  296:         ENABLE_INTERRUPT;
  297: 
  298:         /* Processing if an exception occurs while suspending a
  299:            task exception */
  300:         if ( ctxtsk->exectex != 0 ) {
  301:                 call_brkhdr(ctxtsk);  /* Execute break function */
  302:         }
  303: 
  304:         return ercd;
  305: }
  306: 
  307: /*
  308:  * Clear resource control block
  309:  */
  310: LOCAL void clean_resblk( SSYCB *ssycb, ID idx )
  311: {
  312:         INT    sz;
  313: 
  314:         if ( ssycb->resblk != NULL ) {
  315:                 sz = ROUND_RESBLKSZ(ssycb->resblksz);
  316:                 MEMSET((B*)ssycb->resblk + sz * idx, 0, (size_t)sz);
  317:         }
  318: }
  319: 
  320: /*
  321:  * Call cleanup function
  322:  */
  323: SYSCALL ER _tk_cln_ssy( ID ssid, ID resid, INT info )
  324: {
  325:         SSYCB  *ssycb;
  326:         CluFN  cleanupfn;
  327:         UH     save_texflg;
  328:         INT    i, idx;
  329:         ER     ercd = E_OK;
  330: 
  331:         CHECK_SSYID_ALL(ssid);
  332:         CHECK_RESID(resid);
  333:         CHECK_DISPATCH();
  334: 
  335:         DISABLE_INTERRUPT;
  336:         /* Suspend task exception while startup function is running */
  337:         save_texflg = ctxtsk->texflg;
  338:         ctxtsk->texflg |= SSFN_RUNNING;
  339:         ctxtsk->sysmode++;
  340:         ENABLE_INTERRUPT;
  341: 
  342:         idx = resid_to_index(resid);
  343: 
  344:         if ( ssid > 0 ) {
  345:                 /* Call only specified subsystem */
  346:                 ssycb = get_ssycb(ssid);
  347:                 if ( ssycb->svchdr == no_support ) {
  348:                         ercd = E_NOEXS;
  349:                 } else {
  350:                         cleanupfn = ssycb->cleanupfn;
  351:                         if ( cleanupfn != NULL ) {
  352:                                 CallUserHandlerP2(resid, info,
  353:                                                         cleanupfn, ssycb);
  354:                         }
  355:                         clean_resblk(ssycb, idx);
  356:                 }
  357:         } else {
  358:                 /*  Call all subsystems in the order of ascending priorities */
  359:                 for ( i = NUM_SSYPRI-1; i >= 0; --i ) {
  360:                         ssycb = ssypri_table[i];
  361:                         while ( ssycb != NULL ) {
  362:                                 cleanupfn = ssycb->cleanupfn;
  363:                                 if ( cleanupfn != NULL ) {
  364:                                         CallUserHandlerP2(resid, info,
  365:                                                         cleanupfn, ssycb);
  366:                                 }
  367:                                 clean_resblk(ssycb, idx);
  368:                                 ssycb = ssycb->link;
  369:                         }
  370:                 }
  371:         }
  372: 
  373:         DISABLE_INTERRUPT;
  374:         ctxtsk->sysmode--;
  375:         ctxtsk->texflg = save_texflg;
  376:         ENABLE_INTERRUPT;
  377: 
  378:         /* Processing if an exception occurs while suspending a
  379:            task exception */
  380:         if ( ctxtsk->exectex != 0 ) {
  381:                 call_brkhdr(ctxtsk);  /* Execute break function */
  382:         }
  383: 
  384:         return ercd;
  385: }
  386: 
  387: /*
  388:  * Call event function
  389:  */
  390: SYSCALL ER _tk_evt_ssy( ID ssid, INT evttyp, ID resid, INT info )
  391: {
  392:         SSYCB  *ssycb;
  393:         EvtFN  eventfn;
  394:         UH     save_texflg;
  395:         INT    i, e, d;
  396:         ER     ercd = E_OK;
  397:         ER     er = E_OK;
  398: 
  399:         CHECK_SSYID_ALL(ssid);
  400:         CHECK_RESID_ANY(resid);
  401:         CHECK_DISPATCH();
  402: 
  403:         DISABLE_INTERRUPT;
  404:         /* Suspend task exception while startup function is running */
  405:         save_texflg = ctxtsk->texflg;
  406:         ctxtsk->texflg |= SSFN_RUNNING;
  407:         ctxtsk->sysmode++;
  408:         ENABLE_INTERRUPT;
  409: 
  410:         if ( ssid > 0 ) {
  411:                 /* Call only specified subsystem */
  412:                 ssycb = get_ssycb(ssid);
  413:                 if ( ssycb->svchdr == no_support ) {
  414:                         ercd = E_NOEXS;
  415:                 } else {
  416:                         eventfn = ssycb->eventfn;
  417:                         if ( eventfn != NULL ) {
  418:                                 ercd = CallUserHandlerP3(evttyp, resid, info,
  419:                                                         eventfn, ssycb);
  420:                         }
  421:                 }
  422:         } else {
  423:                 if ( (evttyp % 2) == 0 ) {
  424:                         /* Even number:  Call all in the order of
  425:                            ascending priorities */
  426:                         i = NUM_SSYPRI-1;
  427:                         e = -1;
  428:                         d = -1;
  429:                 } else {
  430:                         /* Odd number: Call all in the order of
  431:                            descending priorities */
  432:                         i = 0;
  433:                         e = NUM_SSYPRI;
  434:                         d = +1;
  435:                 }
  436:                 for ( ; i != e; i += d ) {
  437:                         ssycb = ssypri_table[i];
  438:                         while ( ssycb != NULL ) {
  439:                                 eventfn = ssycb->eventfn;
  440:                                 if ( eventfn != NULL ) {
  441:                                         er = CallUserHandlerP3(evttyp,resid,info,
  442:                                                         eventfn, ssycb);
  443:                                         if ( er < 0 ) {    /* Set Latest Error        */
  444:                                                 ercd = er;        /* Overwrite!  */
  445:                                         }
  446:                                 }
  447:                                 ssycb = ssycb->link;
  448:                         }
  449:                 }
  450:         }
  451: 
  452:         DISABLE_INTERRUPT;
  453:         ctxtsk->sysmode--;
  454:         ctxtsk->texflg = save_texflg;
  455:         ENABLE_INTERRUPT;
  456: 
  457:         /* Processing if an exception occurs while suspending a
  458:            task exception */
  459:         if ( ctxtsk->exectex != 0 ) {
  460:                 call_brkhdr(ctxtsk);  /* Execute break function */
  461:         }
  462: 
  463:         return ercd;
  464: }
  465: 
  466: /*
  467:  * Refer subsystem definition information
  468:  */
  469: SYSCALL ER _tk_ref_ssy( ID ssid, T_RSSY *pk_rssy )
  470: {
  471:         SSYCB  *ssycb;
  472:         ER     ercd = E_OK;
  473: 
  474:         CHECK_SSYID(ssid);
  475: 
  476:         ssycb = get_ssycb(ssid);
  477: 
  478:         BEGIN_CRITICAL_SECTION;
  479:         if ( ssycb->svchdr == no_support ) {
  480:                 ercd = E_NOEXS;
  481:         } else {
  482:                 pk_rssy->ssypri   = ssycb->ssypri;
  483:                 pk_rssy->resblksz = ssycb->resblksz;
  484:         }
  485:         END_CRITICAL_SECTION;
  486: 
  487:         return ercd;
  488: }
  489: 
  490: #if USE_DBGSPT
  491: 
  492: /*
  493:  * Refer subsystem usage state
  494:  */
  495: SYSCALL INT _td_lst_ssy( ID list[], INT nent )
  496: {
  497:         SSYCB  *ssycb, *end;
  498:         INT    n = 0;
  499: 
  500:         BEGIN_DISABLE_INTERRUPT;
  501:         end = ssycb_table + NUM_SSYID;
  502:         for ( ssycb = ssycb_table; ssycb < end; ssycb++ ) {
  503:                 if ( ssycb->svchdr == no_support ) {
  504:                         continue;
  505:                 }
  506: 
  507:                 if ( n++ < nent ) {
  508:                         *list++ = ID_SSY(ssycb - ssycb_table);
  509:                 }
  510:         }
  511:         END_DISABLE_INTERRUPT;
  512: 
  513:         return n;
  514: }
  515: 
  516: /*
  517:  * Refer subsystem definition information
  518:  */
  519: SYSCALL ER _td_ref_ssy( ID ssid, TD_RSSY *pk_rssy )
  520: {
  521:         SSYCB  *ssycb;
  522:         ER     ercd = E_OK;
  523: 
  524:         CHECK_SSYID(ssid);
  525: 
  526:         ssycb = get_ssycb(ssid);
  527: 
  528:         BEGIN_DISABLE_INTERRUPT;
  529:         if ( ssycb->svchdr == no_support ) {
  530:                 ercd = E_NOEXS;
  531:         } else {
  532:                 pk_rssy->ssypri   = ssycb->ssypri;
  533:                 pk_rssy->resblksz = ssycb->resblksz;
  534:         }
  535:         END_DISABLE_INTERRUPT;
  536: 
  537:         return ercd;
  538: }
  539: #endif /* USE_DBGSPT */
  540: 
  541: /*
  542:  * Branch routine to extended SVC handler
  543:  */
  544: EXPORT ER svc_ientry P2GP( void *pk_para, FN fncd )
  545: {
  546:         ID     ssid;
  547:         SSYCB  *ssycb;
  548:         ID     save_execssid;
  549:         UINT   save_waitmask;
  550:         UINT   save_exectex;
  551:         ER     ercd;
  552: 
  553:         /* Lower 8 bits are subsystem ID */
  554:         ssid = fncd & 0xff;
  555:         if ( ssid < 1 || ssid > MAX_SSYID ) {
  556:                 return E_RSFN;
  557:         }
  558: 
  559:         ssycb = get_ssycb(ssid);
  560: 
  561:         if ( in_indp() ) {
  562:                 /* Execute at task-independent part */
  563:                 ercd = CallUserHandlerP2_GP(pk_para, fncd,
  564:                                                 ssycb->svchdr, ssycb);
  565:         } else {
  566:                 /* Lock to run with break function exclusively */
  567:                 LockSVC(&ctxtsk->svclock);
  568: 
  569:                 if ( (ctxtsk->waitmask & TTX_SVC) != 0 ) {
  570:                         UnlockSVC();
  571:                         return E_DISWAI; /* Disable extended SVC call */
  572:                 }
  573: 
  574:                 DISABLE_INTERRUPT;
  575:                 save_execssid = ctxtsk->execssid;
  576:                 save_waitmask = ctxtsk->waitmask;
  577:                 save_exectex  = ctxtsk->exectex;
  578:                 ctxtsk->execssid = ssid;
  579:                 ctxtsk->waitmask = 0;
  580:                 /* If the break function was already called,
  581:                    clear the task exception to avoid to run the
  582:                    extended SVC break function at called extended SVC */
  583:                 if ( save_execssid < 0 ) {    /* if MSB == 1 break handler was already executed */
  584:                         ctxtsk->exectex = 0;
  585:                 }
  586:                 ctxtsk->sysmode++;
  587:                 ENABLE_INTERRUPT;
  588: 
  589:                 UnlockSVC();
  590: 
  591:                 /* Call extended SVC handler */
  592:                 ercd = CallUserHandlerP2_GP(pk_para, fncd,
  593:                                                 ssycb->svchdr, ssycb);
  594: 
  595:                 /* Lock in order to run with break function exclusively */
  596:                 LockSVC(&ctxtsk->svclock);
  597: 
  598:                 DISABLE_INTERRUPT;
  599:                 ctxtsk->sysmode--;
  600:                 ctxtsk->execssid = save_execssid;
  601:                 ctxtsk->waitmask = save_waitmask;
  602:                 ctxtsk->exectex |= save_exectex;
  603:                 ENABLE_INTERRUPT;
  604: 
  605:                 UnlockSVC();
  606: 
  607:                 if ( ctxtsk->exectex != 0 ) {
  608:                         call_brkhdr(ctxtsk); /* Execute break function */
  609:                 }
  610:         }
  611: 
  612:         return ercd;
  613: }
  614: 
  615: /* ------------------------------------------------------------------------ */
  616: /*
  617:  *      Task exception function
  618:  */
  619: 
  620: /*
  621:  * Definition of task exception handler
  622:  */
  623: SYSCALL ER _tk_def_tex( ID tskid, CONST T_DTEX *pk_dtex )
  624: {
  625:         TCB    *tcb;
  626:         ER     ercd = E_OK;
  627: 
  628:         CHECK_TSKID_SELF(tskid);
  629: #if CHK_PAR
  630:         if ( pk_dtex != NULL ) {
  631:                 CHECK_RSATR(pk_dtex->texatr, TA_NULL);
  632:         }
  633: #endif
  634: 
  635:         tcb = get_tcb_self(tskid);
  636: 
  637:         BEGIN_CRITICAL_SECTION;
  638:         if ( tcb->state == TS_NONEXIST ) {
  639:                 ercd = E_NOEXS;
  640:                 goto error_exit;
  641:         }
  642:         if ( (tcb->tskatr & TA_RNG3) == TA_RNG0 ) {
  643:                 ercd = E_OBJ;
  644:                 goto error_exit;
  645:         }
  646: 
  647:         /* Initialize task exception information */
  648:         tcb->pendtex = 0;
  649:         tcb->exectex = 0;
  650:         tcb->texmask = 0;
  651:         tcb->texhdr  = ( pk_dtex != NULL )? pk_dtex->texhdr: NULL;
  652: 
  653:     error_exit:
  654:         END_CRITICAL_SECTION;
  655: 
  656:         return ercd;
  657: }
  658: 
  659: /*
  660:  * Enable/Disable task exception
  661:  */
  662: LOCAL ER set_tex_mask( ID tskid, UINT texmsk, BOOL enable )
  663: {
  664:         TCB    *tcb;
  665:         ER     ercd = E_OK;
  666: 
  667:         CHECK_TSKID_SELF(tskid);
  668: 
  669:         tcb = get_tcb_self(tskid);
  670: 
  671:         BEGIN_CRITICAL_SECTION;
  672:         if ( tcb->state == TS_NONEXIST || tcb->texhdr == NULL ) {
  673:                 ercd = E_NOEXS;
  674:                 goto error_exit;
  675:         }
  676: 
  677:         if ( enable ) {
  678:                 tcb->texmask |= texmsk;               /* Enable */
  679:         } else {
  680:                 tcb->texmask &= ~texmsk;      /* Disable */
  681:         }
  682: 
  683:         /* Narrow down to only the enabled task exception that is suspended */
  684:         tcb->pendtex &= tcb->texmask;
  685: 
  686:     error_exit:
  687:         END_CRITICAL_SECTION;
  688: 
  689:         return ercd;
  690: }
  691: 
  692: /*
  693:  * Disable task exception
  694:  */
  695: SYSCALL ER _tk_dis_tex( ID tskid, UINT texptn )
  696: {
  697:         return set_tex_mask(tskid, texptn, FALSE);
  698: }
  699: 
  700: /*
  701:  * Enable task exception
  702:  */
  703: SYSCALL ER _tk_ena_tex( ID tskid, UINT texptn )
  704: {
  705:         return set_tex_mask(tskid, texptn, TRUE);
  706: }
  707: 
  708: 
  709: /*
  710:  * Call break function
  711:  *      Call break function for extended SVC which 'tcb' task is executing.
  712:  */
  713: EXPORT void call_brkhdr( TCB *tcb )
  714: {
  715:         SSYCB  *ssycb;
  716:         BrkFN  breakfn = NULL;
  717:         UH     save_texflg = 0;
  718:         INT    priority;
  719: #if TA_GP
  720:         void   *ssy_gp = NULL;
  721: #endif
  722: 
  723:         BEGIN_CRITICAL_SECTION;
  724:         /* If subsystem function (Startup function, Cleanup function,
  725:            Event function, Break function) is running, suspend the
  726:            task exception. */
  727:         if ( (tcb->texflg & SSFN_RUNNING) != 0 ) {
  728:                 goto not_call;
  729:         }
  730: 
  731:         /* When the task exception occurred with extended SVC running,
  732:            call break function only once */
  733:         /* if tcb->execssid:MSB == 1 break handler was already executed */
  734:         if ( !(tcb->exectex != 0 && tcb->execssid > 0) ) {
  735:                 goto not_call;
  736:         }
  737: 
  738:         /* Extended SVC (subsystem) in execution */
  739:         ssycb = get_ssycb(tcb->execssid);
  740: 
  741:         tcb->execssid |= BREAK_RAN;    /* Executed mark: Disable multiple call */
  742:                                         /* BREAK_RAN is MSB bit --> tcb->execssid < 0 */
  743: 
  744:         breakfn = ssycb->breakfn;
  745:         if ( breakfn == NULL ) {
  746:                 goto not_call;  /* Break function is not set */
  747:         }
  748: #if TA_GP
  749:         ssy_gp = ssycb->gp;
  750: #endif
  751: 
  752:         if ( tcb->priority < ctxtsk->priority ) {
  753:                 /* Raise its own task priority to the same level of the task
  754:                    running extended SVC */
  755:                 change_task_priority(ctxtsk, tcb->priority);
  756:         }
  757: 
  758:         /* Suspend the task exception during break function is running */
  759:         save_texflg = ctxtsk->texflg;
  760:         ctxtsk->texflg |= SSFN_RUNNING;
  761:         ctxtsk->sysmode++;
  762: 
  763:     not_call:
  764:         END_CRITICAL_SECTION;
  765:         if ( breakfn == NULL ) {
  766:                 return;  /* Do not call break function */
  767:         }
  768: 
  769:         /* Call break function */
  770: #if TA_GP
  771:         CallUserHandler(tcb->tskid, 0, 0, breakfn, ssy_gp);
  772: #else
  773:         (*breakfn)(tcb->tskid);
  774: #endif
  775: 
  776:         BEGIN_CRITICAL_SECTION;
  777:         ctxtsk->sysmode--;
  778:         ctxtsk->texflg = save_texflg;
  779: 
  780:         /* Set the task priority back to the original level */
  781: #ifdef NUM_MTXID
  782:         priority = chg_pri_mutex(ctxtsk, ctxtsk->bpriority);
  783: #else
  784:         priority = ctxtsk->bpriority;
  785: #endif
  786:         if ( ctxtsk->priority != priority ) {
  787:                 change_task_priority(ctxtsk, priority);
  788:         }
  789:         END_CRITICAL_SECTION;
  790: }
  791: 
  792: 
  793: /*
  794:  * Raise task exception
  795:  */
  796: SYSCALL ER _tk_ras_tex( ID tskid, INT texcd )
  797: {
  798:         TCB    *tcb;
  799:         BOOL   rastex = FALSE;
  800:         ER     ercd = E_OK;
  801: 
  802:         CHECK_INTSK();
  803:         CHECK_TSKID_SELF(tskid);
  804:         CHECK_PAR(texcd >= 0 && texcd <= 31);
  805:         CHECK_DISPATCH();
  806: 
  807:         tcb = get_tcb_self(tskid);
  808: 
  809:         /* Lock in order to run with extended SVC handler exclusively */
  810:         LockSVC(&tcb->svclock);
  811: 
  812:         BEGIN_CRITICAL_SECTION;
  813:         if ( tcb->state == TS_NONEXIST || tcb->texhdr == NULL ) {
  814:                 ercd = E_NOEXS;
  815:                 goto error_exit;
  816:         }
  817:         if ( tcb->state == TS_DORMANT ) {
  818:                 ercd = E_OBJ;
  819:                 goto error_exit;
  820:         }
  821: 
  822:         /* Ignore exceptions if '0' exception code handler is in execution */
  823:         if ( (tcb->texflg & TEX0_RUNNING) != 0 ) {
  824:                 goto error_exit;
  825:         }
  826: 
  827:         tcb->pendtex |= (UINT)(1 << texcd) & tcb->texmask;
  828:         if ( tcb->pendtex == 0 ) {
  829:                 goto error_exit; /* No exception occurred */
  830:         }
  831: 
  832:         /* The exception handler can be nested only when the exception
  833:            code is 0.
  834:            In other cases, if the exception handler is running, suspend it. */
  835:         if ( (tcb->pendtex & 0x00000001U) != 0 || (tcb->texflg & TEX1_RUNNING) == 0 ) {
  836:                 /* Task exception occurred */
  837:                 tcb->exectex |= tcb->pendtex;
  838:                 rastex = TRUE;
  839: 
  840:                 /* Request task exception handler execution */
  841:                 request_tex(tcb);
  842:         }
  843: 
  844:     error_exit:
  845:         END_CRITICAL_SECTION;
  846: 
  847:         if ( rastex ) {
  848:                 /* Execute break function */
  849:                 call_brkhdr(tcb);
  850:         }
  851: 
  852:         UnlockSVC();
  853: 
  854:         if ( rastex ) {
  855:                 /* Execute self-break function
  856:                    if task exception occurred while at break function is in execution */
  857:                 if ( ctxtsk->exectex != 0 ) {
  858:                         call_brkhdr(ctxtsk); /* Execute break function */
  859:                 }
  860:         }
  861: 
  862:         return ercd;
  863: }
  864: 
  865: /*
  866:  * End task exception handler
  867:  */
  868: SYSCALL INT _tk_end_tex( BOOL enatex )
  869: {
  870:         INT    texcd = 0;
  871:         UINT   texptn;
  872: 
  873:         CHECK_DISPATCH();
  874:         if ( (ctxtsk->texflg & (TEX0_RUNNING|TEX1_RUNNING)) != TEX1_RUNNING ) {
  875:                 return E_CTX;
  876:         }
  877: 
  878:         BEGIN_CRITICAL_SECTION;
  879:         texptn = ctxtsk->pendtex & ~0x00000001U;  /* Except exception code 0 */
  880:         if ( texptn != 0 ) {
  881:                 while ( (texptn & 1) == 0 ) {
  882:                         texcd++;
  883:                         texptn >>= 1;
  884:                 }
  885:         }
  886: 
  887:         /* If enatex = TRUE or a task exception didn't occur,
  888:            enable the task exception after executing the task
  889:            exception handler  */
  890:         if ( enatex || texcd == 0 ) {
  891:                 ctxtsk->texflg &= ~TEX1_RUNNING;
  892:                 ctxtsk->exectex |= ctxtsk->pendtex;
  893: 
  894:                 if ( texcd > 0 ) {
  895:                         /* Since there is a task exception suspension,
  896:                            request the task execution */
  897:                         request_tex(ctxtsk);
  898:                 }
  899:         } else {
  900:                 /* Continue task exception handler execution */
  901:                 ctxtsk->pendtex &= ~(UINT)(1 << texcd);
  902:         }
  903:         END_CRITICAL_SECTION;
  904: 
  905:         return texcd;
  906: }
  907: 
  908: /*
  909:  * Refer task exception state
  910:  */
  911: SYSCALL ER _tk_ref_tex( ID tskid, T_RTEX *pk_rtex )
  912: {
  913:         TCB    *tcb;
  914:         ER     ercd = E_OK;
  915: 
  916:         CHECK_TSKID_SELF(tskid);
  917: 
  918:         tcb = get_tcb_self(tskid);
  919: 
  920:         BEGIN_CRITICAL_SECTION;
  921:         if ( tcb->state == TS_NONEXIST ) {
  922:                 ercd = E_NOEXS;
  923:         } else {
  924:                 pk_rtex->pendtex = tcb->pendtex;
  925:                 pk_rtex->texmask = tcb->texmask;
  926:         }
  927:         END_CRITICAL_SECTION;
  928: 
  929:         return ercd;
  930: }
  931: 
  932: #if USE_DBGSPT
  933: /*
  934:  * Refer task exception state
  935:  */
  936: SYSCALL ER _td_ref_tex( ID tskid, TD_RTEX *pk_rtex )
  937: {
  938:         TCB    *tcb;
  939:         ER     ercd = E_OK;
  940: 
  941:         CHECK_TSKID_SELF(tskid);
  942: 
  943:         tcb = get_tcb_self(tskid);
  944: 
  945:         BEGIN_DISABLE_INTERRUPT;
  946:         if ( tcb->state == TS_NONEXIST ) {
  947:                 ercd = E_NOEXS;
  948:         } else {
  949:                 pk_rtex->pendtex = tcb->pendtex;
  950:                 pk_rtex->texmask = tcb->texmask;
  951:         }
  952:         END_DISABLE_INTERRUPT;
  953: 
  954:         return ercd;
  955: }
  956: #endif /* USE_DBGSPT */
  957: 
  958: /* ------------------------------------------------------------------------ */
  959: /*
  960:  *      Resource group management
  961:  */
  962: 
  963: /*
  964:  * Initialization of resource group management
  965:  */
  966: EXPORT ER resource_group_initialize( void )
  967: {
  968:         W      n;
  969: 
  970:         /* Get system information */
  971:         n = _tk_get_cfn(SCTAG_TMAXRESID, &max_resid, 1);
  972:         if ( n < 1 || NUM_RESID < 1 ) {
  973:                 return E_SYS;
  974:         }
  975: 
  976:         /* Create bitmap of resource ID */
  977:         resid_bitmap = Icalloc(1, (UINT)((NUM_RESID + 31) / 32));
  978:         if ( resid_bitmap == NULL ) {
  979:                 return E_NOMEM;
  980:         }
  981: 
  982:         /* System resource always exist */
  983:         BitSet(resid_bitmap, resid_to_index(SYS_RESID));
  984: 
  985:         return E_OK;
  986: }
  987: 
  988: /*
  989:  * Create resource group
  990:  */
  991: SYSCALL ID _tk_cre_res( void )
  992: {
  993:         INT    n;
  994:         ER     ercd;
  995: 
  996:         BEGIN_CRITICAL_SECTION;
  997:         n = BitSearch0_w(resid_bitmap, 0, NUM_RESID);
  998:         if ( n < 0 ) {
  999:                 ercd = E_LIMIT;
 1000:         } else {
 1001:                 BitSet(resid_bitmap, n);
 1002:                 ercd = index_to_resid(n);
 1003:         }
 1004:         END_CRITICAL_SECTION;
 1005: 
 1006:         return ercd;
 1007: }
 1008: 
 1009: /*
 1010:  * Delete resource group
 1011:  */
 1012: SYSCALL ER _tk_del_res( ID resid )
 1013: {
 1014:         INT    idx;
 1015:         ER     ercd = E_OK;
 1016: 
 1017:         CHECK_RESID(resid);
 1018:         if ( resid == SYS_RESID ) {
 1019:                 return E_ID;
 1020:         }
 1021: 
 1022:         BEGIN_CRITICAL_SECTION;
 1023:         idx = resid_to_index(resid);
 1024:         if ( !BitTest(resid_bitmap, idx) ) {
 1025:                 ercd = E_NOEXS;
 1026:         } else {
 1027:                 BitClr(resid_bitmap, idx);
 1028:         }
 1029:         END_CRITICAL_SECTION;
 1030: 
 1031:         return ercd;
 1032: }
 1033: 
 1034: /*
 1035:  * Get resource control block
 1036:  */
 1037: SYSCALL ER _tk_get_res( ID resid, ID ssid, void **p_resblk )
 1038: {
 1039:         INT    idx;
 1040:         SSYCB  *ssycb;
 1041:         ER     ercd = E_OK;
 1042: 
 1043:         CHECK_RESID(resid);
 1044:         CHECK_SSYID(ssid);
 1045: 
 1046:         idx = resid_to_index(resid);
 1047:         ssycb = get_ssycb(ssid);
 1048: 
 1049:         BEGIN_CRITICAL_SECTION;
 1050:         if ( !BitTest(resid_bitmap, idx) || ssycb->svchdr == no_support ) {
 1051:                 ercd = E_NOEXS;
 1052:                 goto error_exit;
 1053:         }
 1054: 
 1055:         /* Resource control block address */
 1056:         *p_resblk = (B*)ssycb->resblk + ROUND_RESBLKSZ(ssycb->resblksz) * idx;
 1057: 
 1058:     error_exit:
 1059:         END_CRITICAL_SECTION;
 1060: 
 1061:         return ercd;
 1062: }