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:  *      mempfix.c (T-Kernel/OS)
   19:  *      Fixed Size Memory Pool
   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_MPFID
   29: 
   30: EXPORT ID       max_mpfid;    /* Maximum fixed size memory pool ID */
   31: 
   32: /*
   33:  * Fixed size memory pool control block
   34:  */
   35: typedef struct free_list {
   36:         struct free_list *next;
   37: } FREEL;
   38: 
   39: typedef struct fix_memorypool_control_block {
   40:         QUEUE  wait_queue;      /* Memory pool wait queue */
   41:         ID     mpfid;              /* Fixed size memory pool ID */
   42:         void   *exinf;           /* Extended information */
   43:         ATR    mpfatr;            /* Memory pool attribute */
   44:         INT    mpfcnt;            /* Number of blocks in whole memory pool */
   45:         INT    blfsz;             /* Fixed size memory block size */
   46:         INT    mpfsz;             /* Whole memory pool size */
   47:         INT    frbcnt;            /* Number of blocks in free area */
   48:         void   *mempool; /* Top address of memory pool */
   49:         void   *unused;  /* Top address of unused area */
   50:         FREEL  *freelist;       /* Free block list */
   51:         OBJLOCK        lock;          /* Lock for object exclusive access */
   52: #if USE_OBJECT_NAME
   53:         UB     name[OBJECT_NAME_LENGTH];   /* name */
   54: #endif
   55: } MPFCB;
   56: 
   57: LOCAL MPFCB     *mpfcb_table;       /* Fixed size memory pool control block */
   58: LOCAL QUEUE     free_mpfcb; /* FreeQue */
   59: 
   60: #define get_mpfcb(id)   ( &mpfcb_table[INDEX_MPF(id)] )
   61: 
   62: 
   63: /*
   64:  * Initialization of fixed size memory pool control block
   65:  */
   66: EXPORT ER fix_memorypool_initialize( void )
   67: {
   68:         MPFCB  *mpfcb, *end;
   69:         W      n;
   70: 
   71:         /* Get system information */
   72:         n = _tk_get_cfn(SCTAG_TMAXMPFID, &max_mpfid, 1);
   73:         if ( n < 1 || NUM_MPFID < 1 ) {
   74:                 return E_SYS;
   75:         }
   76: 
   77:         /* Create fixed size memory pool control block */
   78:         mpfcb_table = Imalloc((UINT)NUM_MPFID * sizeof(MPFCB));
   79:         if ( mpfcb_table == NULL ) {
   80:                 return E_NOMEM;
   81:         }
   82: 
   83:         /* Register all control blocks onto FreeQue */
   84:         QueInit(&free_mpfcb);
   85:         end = mpfcb_table + NUM_MPFID;
   86:         for ( mpfcb = mpfcb_table; mpfcb < end; mpfcb++ ) {
   87:                 mpfcb->mpfid = 0;
   88:                 InitOBJLOCK(&mpfcb->lock);
   89:                 QueInsert(&mpfcb->wait_queue, &free_mpfcb);
   90:         }
   91: 
   92:         return E_OK;
   93: }
   94: 
   95: 
   96: #define MINSIZE         ( sizeof(FREEL) )
   97: #define MINSZ(sz)       ( ((sz) + (MINSIZE-1)) & ~(MINSIZE-1) )
   98: 
   99: /*
  100:  * Return end address in memory pool area
  101:  */
  102: Inline void* mempool_end( MPFCB *mpfcb )
  103: {
  104:         return (VB*)mpfcb->mempool + mpfcb->mpfsz;
  105: }
  106: 
  107: 
  108: /*
  109:  * Processing if the priority of wait task changes
  110:  */
  111: LOCAL void mpf_chg_pri( TCB *tcb, INT oldpri )
  112: {
  113:         MPFCB  *mpfcb;
  114: 
  115:         mpfcb = get_mpfcb(tcb->wid);
  116:         gcb_change_priority((GCB*)mpfcb, tcb);
  117: }
  118: 
  119: /*
  120:  * Definition of fixed size memory pool wait specification
  121:  */
  122: LOCAL CONST WSPEC wspec_mpf_tfifo = { TTW_MPF, NULL, NULL };
  123: LOCAL CONST WSPEC wspec_mpf_tpri  = { TTW_MPF, mpf_chg_pri, NULL };
  124: 
  125: 
  126: /*
  127:  * Create fixed size memory pool
  128:  */
  129: SYSCALL ID _tk_cre_mpf( CONST T_CMPF *pk_cmpf )
  130: {
  131: #if CHK_RSATR
  132:         const ATR VALID_MPFATR = {
  133:                  TA_TPRI
  134:                 |TA_RNG3
  135:                 |TA_NORESIDENT
  136:                 |TA_NODISWAI
  137: #if USE_OBJECT_NAME
  138:                 |TA_DSNAME
  139: #endif
  140:         };
  141: #endif
  142:         MPFCB  *mpfcb;
  143:         ID     mpfid;
  144:         INT    blfsz, mpfsz;
  145:         void   *mempool;
  146: 
  147:         CHECK_RSATR(pk_cmpf->mpfatr, VALID_MPFATR);
  148:         CHECK_PAR(pk_cmpf->mpfcnt > 0);
  149:         CHECK_PAR(pk_cmpf->blfsz > 0);
  150:         CHECK_DISPATCH();
  151: 
  152:         blfsz = (INT)MINSZ(pk_cmpf->blfsz);
  153:         mpfsz = blfsz * pk_cmpf->mpfcnt;
  154: 
  155:         /* Allocate memory for memory pool */
  156:         mempool = IAmalloc((UINT)mpfsz, pk_cmpf->mpfatr);
  157:         if ( mempool == NULL ) {
  158:                 return E_NOMEM;
  159:         }
  160: 
  161:         /* Get control block from FreeQue */
  162:         DISABLE_INTERRUPT;
  163:         mpfcb = (MPFCB*)QueRemoveNext(&free_mpfcb);
  164:         ENABLE_INTERRUPT;
  165:         if ( mpfcb == NULL ) {
  166:                 IAfree(mempool, pk_cmpf->mpfatr);
  167:                 return E_LIMIT;
  168:         }
  169: 
  170:         LockOBJ(&mpfcb->lock);
  171:         mpfid = ID_MPF(mpfcb - mpfcb_table);
  172: 
  173:         /* Initialize control block */
  174:         QueInit(&mpfcb->wait_queue);
  175:         mpfcb->exinf    = pk_cmpf->exinf;
  176:         mpfcb->mpfatr   = pk_cmpf->mpfatr;
  177:         mpfcb->mpfcnt   = mpfcb->frbcnt = pk_cmpf->mpfcnt;
  178:         mpfcb->blfsz    = blfsz;
  179:         mpfcb->mpfsz    = mpfsz;
  180:         mpfcb->unused   = mpfcb->mempool = mempool;
  181:         mpfcb->freelist = NULL;
  182: #if USE_OBJECT_NAME
  183:         if ( (pk_cmpf->mpfatr & TA_DSNAME) != 0 ) {
  184:                 STRNCPY((char*)mpfcb->name, (char*)pk_cmpf->dsname, OBJECT_NAME_LENGTH);
  185:         }
  186: #endif
  187: 
  188:         mpfcb->mpfid    = mpfid;  /* Set ID after completion */
  189:         UnlockOBJ(&mpfcb->lock);
  190: 
  191:         return mpfid;
  192: }
  193: 
  194: /*
  195:  * Delete fixed size memory pool
  196:  */
  197: SYSCALL ER _tk_del_mpf( ID mpfid )
  198: {
  199:         MPFCB  *mpfcb;
  200:         void   *mempool = NULL;
  201:         ATR    memattr = 0;
  202:         ER     ercd = E_OK;
  203: 
  204:         CHECK_MPFID(mpfid);
  205:         CHECK_DISPATCH();
  206: 
  207:         mpfcb = get_mpfcb(mpfid);
  208: 
  209:         LockOBJ(&mpfcb->lock);
  210:         if ( mpfcb->mpfid == 0 ) {
  211:                 ercd = E_NOEXS;
  212:         } else {
  213:                 DISABLE_INTERRUPT;
  214:                 mempool = mpfcb->mempool;
  215:                 memattr = mpfcb->mpfatr;
  216: 
  217:                 /* Release wait state of task (E_DLT) */
  218:                 wait_delete(&mpfcb->wait_queue);
  219: 
  220:                 /* Return to FreeQue */
  221:                 QueInsert(&mpfcb->wait_queue, &free_mpfcb);
  222:                 mpfcb->mpfid = 0;
  223:                 ENABLE_INTERRUPT;
  224:         }
  225:         UnlockOBJ(&mpfcb->lock);
  226: 
  227:         if ( mempool != NULL ) {
  228:                 IAfree(mempool, memattr);
  229:         }
  230: 
  231:         return ercd;
  232: }
  233: 
  234: /*
  235:  * Get fixed size memory block
  236:  */
  237: SYSCALL ER _tk_get_mpf( ID mpfid, void **p_blf, TMO tmout )
  238: {
  239:         return _tk_get_mpf_u(mpfid, p_blf, to_usec_tmo(tmout));
  240: }
  241: 
  242: SYSCALL ER _tk_get_mpf_u( ID mpfid, void **p_blf, TMO_U tmout )
  243: {
  244:         MPFCB  *mpfcb;
  245:         FREEL  *free;
  246:         ER     ercd = E_OK;
  247: 
  248:         CHECK_MPFID(mpfid);
  249:         CHECK_TMOUT(tmout);
  250:         CHECK_DISPATCH();
  251: 
  252:         mpfcb = get_mpfcb(mpfid);
  253: 
  254:         LockOBJ(&mpfcb->lock);
  255:         if ( mpfcb->mpfid == 0 ) {
  256:                 ercd = E_NOEXS;
  257:                 goto error_exit;
  258:         }
  259: 
  260:         /* Check wait disable */
  261:         if ( is_diswai((GCB*)mpfcb, ctxtsk, TTW_MPF) ) {
  262:                 ercd = E_DISWAI;
  263:                 goto error_exit;
  264:         }
  265: 
  266:         /* If there is no space, ready for wait */
  267:         if ( mpfcb->frbcnt <= 0 ) {
  268:                 goto wait_mpf;
  269:         }
  270: 
  271:         /* Get memory block */
  272:         if ( mpfcb->freelist != NULL ) {
  273:                 free = mpfcb->freelist;
  274:                 mpfcb->freelist = free->next;
  275:                 *p_blf = free;
  276:         } else {
  277:                 *p_blf = mpfcb->unused;
  278:                 mpfcb->unused = (VB*)mpfcb->unused + mpfcb->blfsz;
  279:         }
  280:         mpfcb->frbcnt--;
  281: 
  282:     error_exit:
  283:         UnlockOBJ(&mpfcb->lock);
  284: 
  285:         return ercd;
  286: 
  287:     wait_mpf:
  288:         /* Ready for wait */
  289:         BEGIN_CRITICAL_SECTION;
  290:         ctxtsk->wspec = ( (mpfcb->mpfatr & TA_TPRI) != 0 )?
  291:                                 &wspec_mpf_tpri: &wspec_mpf_tfifo;
  292:         ctxtsk->wercd = &ercd;
  293:         ctxtsk->winfo.mpf.p_blf = p_blf;
  294:         gcb_make_wait_with_diswai((GCB*)mpfcb, tmout);
  295: 
  296:         UnlockOBJ(&mpfcb->lock);
  297:         END_CRITICAL_SECTION;
  298: 
  299:         return ercd;
  300: }
  301: 
  302: /*
  303:  * Return fixed size memory block
  304:  */
  305: SYSCALL ER _tk_rel_mpf( ID mpfid, void *blf )
  306: {
  307:         MPFCB  *mpfcb;
  308:         TCB    *tcb;
  309:         FREEL  *free;
  310:         ER     ercd = E_OK;
  311: 
  312:         CHECK_MPFID(mpfid);
  313:         CHECK_DISPATCH();
  314: 
  315:         mpfcb = get_mpfcb(mpfid);
  316: 
  317:         LockOBJ(&mpfcb->lock);
  318:         if ( mpfcb->mpfid == 0 ) {
  319:                 ercd = E_NOEXS;
  320:                 goto error_exit;
  321:         }
  322: #if CHK_PAR
  323:         if ( blf < mpfcb->mempool || blf >= mempool_end(mpfcb) || (((VB*)blf - (VB*)mpfcb->mempool) % mpfcb->blfsz) != 0 ) {
  324:                 ercd = E_PAR;
  325:                 goto error_exit;
  326:         }
  327: #endif
  328: 
  329:         DISABLE_INTERRUPT;
  330:         if ( !isQueEmpty(&mpfcb->wait_queue) ) {
  331:                 /* Send memory block to waiting task,
  332:                    and then release the task */
  333:                 tcb = (TCB*)mpfcb->wait_queue.next;
  334:                 *tcb->winfo.mpf.p_blf = blf;
  335:                 wait_release_ok(tcb);
  336:                 ENABLE_INTERRUPT;
  337:         } else {
  338:                 ENABLE_INTERRUPT;
  339:                 /* Free memory block */
  340:                 free = (FREEL*)blf;
  341:                 free->next = mpfcb->freelist;
  342:                 mpfcb->freelist = free;
  343:                 mpfcb->frbcnt++;
  344:         }
  345: 
  346:     error_exit:
  347:         UnlockOBJ(&mpfcb->lock);
  348: 
  349:         return ercd;
  350: }
  351: 
  352: /*
  353:  * Check fixed size pool state
  354:  */
  355: SYSCALL ER _tk_ref_mpf( ID mpfid, T_RMPF *pk_rmpf )
  356: {
  357:         MPFCB  *mpfcb;
  358:         ER     ercd = E_OK;
  359: 
  360:         CHECK_MPFID(mpfid);
  361:         CHECK_DISPATCH();
  362: 
  363:         mpfcb = get_mpfcb(mpfid);
  364: 
  365:         LockOBJ(&mpfcb->lock);
  366:         if ( mpfcb->mpfid == 0 ) {
  367:                 ercd = E_NOEXS;
  368:         } else {
  369:                 DISABLE_INTERRUPT;
  370:                 pk_rmpf->wtsk = wait_tskid(&mpfcb->wait_queue);
  371:                 ENABLE_INTERRUPT;
  372:                 pk_rmpf->exinf = mpfcb->exinf;
  373:                 pk_rmpf->frbcnt = mpfcb->frbcnt;
  374:         }
  375:         UnlockOBJ(&mpfcb->lock);
  376: 
  377:         return ercd;
  378: }
  379: 
  380: /* ------------------------------------------------------------------------ */
  381: /*
  382:  *      Debugger support function
  383:  */
  384: #if USE_DBGSPT
  385: 
  386: /*
  387:  * Get object name from control block
  388:  */
  389: #if USE_OBJECT_NAME
  390: EXPORT ER fix_memorypool_getname(ID id, UB **name)
  391: {
  392:         MPFCB  *mpfcb;
  393:         ER     ercd = E_OK;
  394: 
  395:         CHECK_MPFID(id);
  396: 
  397:         BEGIN_DISABLE_INTERRUPT;
  398:         mpfcb = get_mpfcb(id);
  399:         if ( mpfcb->mpfid == 0 ) {
  400:                 ercd = E_NOEXS;
  401:                 goto error_exit;
  402:         }
  403:         if ( (mpfcb->mpfatr & TA_DSNAME) == 0 ) {
  404:                 ercd = E_OBJ;
  405:                 goto error_exit;
  406:         }
  407:         *name = mpfcb->name;
  408: 
  409:     error_exit:
  410:         END_DISABLE_INTERRUPT;
  411: 
  412:         return ercd;
  413: }
  414: #endif /* USE_OBJECT_NAME */
  415: 
  416: /*
  417:  * Refer fixed size memory pool usage state
  418:  */
  419: SYSCALL INT _td_lst_mpf( ID list[], INT nent )
  420: {
  421:         MPFCB  *mpfcb, *end;
  422:         INT    n = 0;
  423: 
  424:         BEGIN_DISABLE_INTERRUPT;
  425:         end = mpfcb_table + NUM_MPFID;
  426:         for ( mpfcb = mpfcb_table; mpfcb < end; mpfcb++ ) {
  427:                 if ( mpfcb->mpfid == 0 ) {
  428:                         continue;
  429:                 }
  430: 
  431:                 if ( n++ < nent ) {
  432:                         *list++ = ID_MPF(mpfcb - mpfcb_table);
  433:                 }
  434:         }
  435:         END_DISABLE_INTERRUPT;
  436: 
  437:         return n;
  438: }
  439: 
  440: /*
  441:  * Refer fixed size memory pool state
  442:  */
  443: SYSCALL ER _td_ref_mpf( ID mpfid, TD_RMPF *pk_rmpf )
  444: {
  445:         MPFCB  *mpfcb;
  446:         ER     ercd = E_OK;
  447: 
  448:         CHECK_MPFID(mpfid);
  449: 
  450:         mpfcb = get_mpfcb(mpfid);
  451: 
  452:         BEGIN_DISABLE_INTERRUPT;
  453:         if ( mpfcb->mpfid == 0 ) {
  454:                 ercd = E_NOEXS;
  455:         } else if ( isLockedOBJ(&mpfcb->lock) ) {
  456:                 ercd = E_CTX;
  457:         } else {
  458:                 pk_rmpf->wtsk = wait_tskid(&mpfcb->wait_queue);
  459:                 pk_rmpf->exinf = mpfcb->exinf;
  460:                 pk_rmpf->frbcnt = mpfcb->frbcnt;
  461:         }
  462:         END_DISABLE_INTERRUPT;
  463: 
  464:         return ercd;
  465: }
  466: 
  467: /*
  468:  * Refer fixed size memory wait queue
  469:  */
  470: SYSCALL INT _td_mpf_que( ID mpfid, ID list[], INT nent )
  471: {
  472:         MPFCB  *mpfcb;
  473:         QUEUE  *q;
  474:         ER     ercd = E_OK;
  475: 
  476:         CHECK_MPFID(mpfid);
  477: 
  478:         mpfcb = get_mpfcb(mpfid);
  479: 
  480:         BEGIN_DISABLE_INTERRUPT;
  481:         if ( mpfcb->mpfid == 0 ) {
  482:                 ercd = E_NOEXS;
  483:         } else {
  484:                 INT n = 0;
  485:                 for ( q = mpfcb->wait_queue.next; q != &mpfcb->wait_queue; q = q->next ) {
  486:                         if ( n++ < nent ) {
  487:                                 *list++ = ((TCB*)q)->tskid;
  488:                         }
  489:                 }
  490:                 ercd = n;
  491:         }
  492:         END_DISABLE_INTERRUPT;
  493: 
  494:         return ercd;
  495: }
  496: 
  497: #endif /* USE_DBGSPT */
  498: #endif /* NUM_MPFID */