gonzui

    1: /*
    2:  *----------------------------------------------------------------------
    3:  *    micro T-Kernel 3.00.00
    4:  *
    5:  *    Copyright (C) 2006-2019 by Ken Sakamura.
    6:  *    This software is distributed under the T-License 2.1.
    7:  *----------------------------------------------------------------------
    8:  *
    9:  *    Released by TRON Forum(http://www.tron.org) at 2019/12/11.
   10:  *
   11:  *----------------------------------------------------------------------
   12:  */
   13: 
   14: /*
   15:  *      mempfix.c
   16:  *      Fixed Size Memory Pool
   17:  */
   18: 
   19: #include "kernel.h"
   20: #include "klock.h"
   21: #include "wait.h"
   22: #include "check.h"
   23: #include "mempfix.h"
   24: 
   25: #if USE_FIX_MEMORYPOOL
   26: 
   27: Noinit(EXPORT MPFCB     knl_mpfcb_table[NUM_MPFID]);        /* Fixed size memory pool control block */
   28: Noinit(EXPORT QUEUE     knl_free_mpfcb);    /* FreeQue */
   29: 
   30: 
   31: /*
   32:  * Initialization of fixed size memory pool control block
   33:  */
   34: EXPORT ER knl_fix_memorypool_initialize( void )
   35: {
   36:         MPFCB  *mpfcb, *end;
   37: 
   38:         /* Get system information */
   39:         if ( NUM_MPFID < 1 ) {
   40:                 return E_SYS;
   41:         }
   42: 
   43:         /* Register all control blocks onto FreeQue */
   44:         QueInit(&knl_free_mpfcb);
   45:         end = knl_mpfcb_table + NUM_MPFID;
   46:         for ( mpfcb = knl_mpfcb_table; mpfcb < end; mpfcb++ ) {
   47:                 mpfcb->mpfid = 0;
   48:                 knl_InitOBJLOCK(&mpfcb->lock);
   49:                 QueInsert(&mpfcb->wait_queue, &knl_free_mpfcb);
   50:         }
   51: 
   52:         return E_OK;
   53: }
   54: 
   55: 
   56: /*
   57:  * Create fixed size memory pool
   58:  */
   59: SYSCALL ID tk_cre_mpf( CONST T_CMPF *pk_cmpf )
   60: {
   61: #if CHK_RSATR
   62:         const ATR VALID_MPFATR = {
   63:                  TA_TPRI
   64:                 |TA_RNG3
   65:                 |TA_USERBUF
   66: #if USE_OBJECT_NAME
   67:                 |TA_DSNAME
   68: #endif
   69:         };
   70: #endif
   71:         MPFCB  *mpfcb;
   72:         ID     mpfid;
   73:         W      blfsz, mpfsz;
   74:         void   *mempool;
   75: 
   76:         CHECK_RSATR(pk_cmpf->mpfatr, VALID_MPFATR);
   77:         CHECK_PAR(pk_cmpf->mpfcnt > 0);
   78:         CHECK_PAR(pk_cmpf->blfsz > 0);
   79: #if !USE_IMALLOC
   80:         /* TA_USERBUF must be specified if configured in no Imalloc */
   81:         CHECK_PAR((pk_cmpf->mpfatr & TA_USERBUF) != 0);
   82: #endif
   83:         CHECK_DISPATCH();
   84: 
   85:         blfsz = (W)MINSZ(pk_cmpf->blfsz);
   86:         mpfsz = blfsz * pk_cmpf->mpfcnt;
   87: 
   88: #if USE_IMALLOC
   89:         if ( (pk_cmpf->mpfatr & TA_USERBUF) != 0 ) {
   90:                 /* Size of user buffer must be multiples of sizeof(FREEL) */
   91:                 if ( blfsz != pk_cmpf->blfsz ) {
   92:                         return E_PAR;
   93:                 }
   94:                 /* Use user buffer */
   95:                 mempool = pk_cmpf->bufptr;
   96:         } else {
   97:                 /* Allocate memory for memory pool */
   98:                 mempool = knl_Imalloc((UW)mpfsz);
   99:                 if ( mempool == NULL ) {
  100:                         return E_NOMEM;
  101:                 }
  102:         }
  103: #else
  104:         /* Size of user buffer must be larger than sizeof(FREEL) */
  105:         if ( blfsz != pk_cmpf->blfsz ) {
  106:                 return E_PAR;
  107:         }
  108:         /* Use user buffer */
  109:         mempool = pk_cmpf->bufptr;
  110: #endif
  111: 
  112:         /* Get control block from FreeQue */
  113:         DISABLE_INTERRUPT;
  114:         mpfcb = (MPFCB*)QueRemoveNext(&knl_free_mpfcb);
  115:         ENABLE_INTERRUPT;
  116: 
  117:         if ( mpfcb == NULL ) {
  118: #if USE_IMALLOC
  119:                 if ( (pk_cmpf->mpfatr & TA_USERBUF) == 0 ) {
  120:                         knl_Ifree(mempool);
  121:                 }
  122: #endif
  123:                 return E_LIMIT;
  124:         }
  125: 
  126:         knl_LockOBJ(&mpfcb->lock);
  127:         mpfid = ID_MPF(mpfcb - knl_mpfcb_table);
  128: 
  129:         /* Initialize control block */
  130:         QueInit(&mpfcb->wait_queue);
  131:         mpfcb->exinf    = pk_cmpf->exinf;
  132:         mpfcb->mpfatr   = pk_cmpf->mpfatr;
  133:         mpfcb->mpfcnt   = mpfcb->frbcnt = pk_cmpf->mpfcnt;
  134:         mpfcb->blfsz    = blfsz;
  135:         mpfcb->mpfsz    = mpfsz;
  136:         mpfcb->unused   = mpfcb->mempool = mempool;
  137:         mpfcb->freelist = NULL;
  138: #if USE_OBJECT_NAME
  139:         if ( (pk_cmpf->mpfatr & TA_DSNAME) != 0 ) {
  140:                 knl_strncpy((char*)mpfcb->name, (char*)pk_cmpf->dsname, OBJECT_NAME_LENGTH);
  141:         }
  142: #endif
  143: 
  144:         mpfcb->mpfid    = mpfid;  /* Set ID after completion */
  145:         knl_UnlockOBJ(&mpfcb->lock);
  146: 
  147:         return mpfid;
  148: }
  149: 
  150: #ifdef USE_FUNC_TK_DEL_MPF
  151: /*
  152:  * Delete fixed size memory pool 
  153:  */
  154: SYSCALL ER tk_del_mpf( ID mpfid )
  155: {
  156:         MPFCB  *mpfcb;
  157:         void   *mempool = NULL;
  158:         ATR    memattr = 0;
  159:         ER     ercd = E_OK;
  160: 
  161:         CHECK_MPFID(mpfid);
  162:         CHECK_DISPATCH();
  163: 
  164:         mpfcb = get_mpfcb(mpfid);
  165: 
  166:         knl_LockOBJ(&mpfcb->lock);
  167:         if ( mpfcb->mpfid == 0 ) {
  168:                 ercd = E_NOEXS;
  169:         } else {
  170:                 DISABLE_INTERRUPT;
  171:                 mempool = mpfcb->mempool;
  172:                 memattr = mpfcb->mpfatr;
  173: 
  174:                 /* Release wait state of task (E_DLT) */
  175:                 knl_wait_delete(&mpfcb->wait_queue);
  176: 
  177:                 /* Return to FreeQue */
  178:                 QueInsert(&mpfcb->wait_queue, &knl_free_mpfcb);
  179:                 mpfcb->mpfid = 0;
  180:                 ENABLE_INTERRUPT;
  181:         }
  182:         knl_UnlockOBJ(&mpfcb->lock);
  183: 
  184: #if USE_IMALLOC
  185:         if ( (mempool != NULL) && ((memattr & TA_USERBUF) == 0) ) {
  186:                 knl_Ifree(mempool);
  187:         }
  188: #endif
  189: 
  190:         return ercd;
  191: }
  192: #endif /* USE_FUNC_TK_DEL_MPF */
  193: 
  194: /*
  195:  * Processing if the priority of wait task changes
  196:  */
  197: LOCAL void knl_mpf_chg_pri( TCB *tcb, INT oldpri )
  198: {
  199:         MPFCB  *mpfcb;
  200: 
  201:         mpfcb = get_mpfcb(tcb->wid);
  202:         knl_gcb_change_priority((GCB*)mpfcb, tcb);
  203: }
  204: 
  205: /*
  206:  * Definition of fixed size memory pool wait specification
  207:  */
  208: LOCAL CONST WSPEC knl_wspec_mpf_tfifo = { TTW_MPF, NULL, NULL };
  209: LOCAL CONST WSPEC knl_wspec_mpf_tpri  = { TTW_MPF, knl_mpf_chg_pri, NULL };
  210: 
  211: /*
  212:  * Get fixed size memory block 
  213:  */
  214: SYSCALL ER tk_get_mpf( ID mpfid, void **p_blf, TMO tmout )
  215: {
  216:         MPFCB  *mpfcb;
  217:         FREEL  *free;
  218:         ER     ercd = E_OK;
  219: 
  220:         CHECK_MPFID(mpfid);
  221:         CHECK_TMOUT(tmout);
  222:         CHECK_DISPATCH();
  223: 
  224:         mpfcb = get_mpfcb(mpfid);
  225: 
  226:         knl_LockOBJ(&mpfcb->lock);
  227:         if ( mpfcb->mpfid == 0 ) {
  228:                 ercd = E_NOEXS;
  229:                 goto error_exit;
  230:         }
  231: 
  232:         /* If there is no space, ready for wait */
  233:         if ( mpfcb->frbcnt <= 0 ) {
  234:                 goto wait_mpf;
  235:         } else {
  236:                 /* Get memory block */
  237:                 if ( mpfcb->freelist != NULL ) {
  238:                         free = mpfcb->freelist;
  239:                         mpfcb->freelist = free->next;
  240:                         *p_blf = free;
  241:                 } else {
  242:                         *p_blf = mpfcb->unused;
  243:                         mpfcb->unused = (VB*)mpfcb->unused + mpfcb->blfsz;
  244:                 }
  245:                 mpfcb->frbcnt--;
  246:         }
  247: 
  248:     error_exit:
  249:         knl_UnlockOBJ(&mpfcb->lock);
  250: 
  251:         return ercd;
  252: 
  253: wait_mpf:
  254:         /* Ready for wait */
  255:         BEGIN_CRITICAL_SECTION;
  256:         knl_ctxtsk->wspec = ( (mpfcb->mpfatr & TA_TPRI) != 0 )?
  257:                                 &knl_wspec_mpf_tpri: &knl_wspec_mpf_tfifo;
  258:         knl_ctxtsk->wercd = &ercd;
  259:         knl_ctxtsk->winfo.mpf.p_blf = p_blf;
  260:         knl_gcb_make_wait((GCB*)mpfcb, tmout);
  261: 
  262:         knl_UnlockOBJ(&mpfcb->lock);
  263:         END_CRITICAL_SECTION;
  264: 
  265:         return ercd;
  266: }
  267: 
  268: /*
  269:  * Return fixed size memory block 
  270:  */
  271: SYSCALL ER tk_rel_mpf( ID mpfid, void *blf )
  272: {
  273:         MPFCB  *mpfcb;
  274:         TCB    *tcb;
  275:         FREEL  *free;
  276:         ER     ercd = E_OK;
  277: 
  278:         CHECK_MPFID(mpfid);
  279:         CHECK_DISPATCH();
  280: 
  281:         mpfcb = get_mpfcb(mpfid);
  282: 
  283:         knl_LockOBJ(&mpfcb->lock);
  284:         if ( mpfcb->mpfid == 0 ) {
  285:                 ercd = E_NOEXS;
  286:                 goto error_exit;
  287:         }
  288: #if CHK_PAR
  289:         if ( blf < mpfcb->mempool || blf >= knl_mempool_end(mpfcb) || (((VB*)blf - (VB*)mpfcb->mempool) % mpfcb->blfsz) != 0 ) {
  290:                 ercd = E_PAR;
  291:                 goto error_exit;
  292:         }
  293: #endif
  294: 
  295:         DISABLE_INTERRUPT;
  296:         if ( !isQueEmpty(&mpfcb->wait_queue) ) {
  297:                 /* Send memory block to waiting task,
  298:                    and then release the task */
  299:                 tcb = (TCB*)mpfcb->wait_queue.next;
  300:                 *tcb->winfo.mpf.p_blf = blf;
  301:                 knl_wait_release_ok(tcb);
  302:                 ENABLE_INTERRUPT;
  303:         } else {
  304:                 ENABLE_INTERRUPT;
  305:                 /* Free memory block */
  306:                 free = (FREEL*)blf;
  307:                 free->next = mpfcb->freelist;
  308:                 mpfcb->freelist = free;
  309:                 mpfcb->frbcnt++;
  310:         }
  311: 
  312: error_exit:
  313:         knl_UnlockOBJ(&mpfcb->lock);
  314: 
  315:         return ercd;
  316: }
  317: 
  318: #ifdef USE_FUNC_TK_REF_MPF
  319: /*
  320:  * Check fixed size pool state
  321:  */
  322: SYSCALL ER tk_ref_mpf( ID mpfid, T_RMPF *pk_rmpf )
  323: {
  324:         MPFCB  *mpfcb;
  325:         ER     ercd = E_OK;
  326: 
  327:         CHECK_MPFID(mpfid);
  328:         CHECK_DISPATCH();
  329: 
  330:         mpfcb = get_mpfcb(mpfid);
  331: 
  332:         knl_LockOBJ(&mpfcb->lock);
  333:         if ( mpfcb->mpfid == 0 ) {
  334:                 ercd = E_NOEXS;
  335:         } else {
  336:                 DISABLE_INTERRUPT;
  337:                 pk_rmpf->wtsk = knl_wait_tskid(&mpfcb->wait_queue);
  338:                 ENABLE_INTERRUPT;
  339:                 pk_rmpf->exinf = mpfcb->exinf;
  340:                 pk_rmpf->frbcnt = mpfcb->frbcnt;
  341:         }
  342:         knl_UnlockOBJ(&mpfcb->lock);
  343: 
  344:         return ercd;
  345: }
  346: #endif /* USE_FUNC_TK_REF_MPF */
  347: 
  348: /* ------------------------------------------------------------------------ */
  349: /*
  350:  *      Debugger support function
  351:  */
  352: #if USE_DBGSPT
  353: 
  354: #if USE_OBJECT_NAME
  355: /*
  356:  * Get object name from control block
  357:  */
  358: EXPORT ER knl_fix_memorypool_getname(ID id, UB **name)
  359: {
  360:         MPFCB  *mpfcb;
  361:         ER     ercd = E_OK;
  362: 
  363:         CHECK_MPFID(id);
  364: 
  365:         BEGIN_DISABLE_INTERRUPT;
  366:         mpfcb = get_mpfcb(id);
  367:         if ( mpfcb->mpfid == 0 ) {
  368:                 ercd = E_NOEXS;
  369:                 goto error_exit;
  370:         }
  371:         if ( (mpfcb->mpfatr & TA_DSNAME) == 0 ) {
  372:                 ercd = E_OBJ;
  373:                 goto error_exit;
  374:         }
  375:         *name = mpfcb->name;
  376: 
  377:     error_exit:
  378:         END_DISABLE_INTERRUPT;
  379: 
  380:         return ercd;
  381: }
  382: #endif /* USE_OBJECT_NAME */
  383: 
  384: #ifdef USE_FUNC_TD_LST_MPF
  385: /*
  386:  * Refer fixed size memory pool usage state
  387:  */
  388: SYSCALL INT td_lst_mpf( ID list[], INT nent )
  389: {
  390:         MPFCB  *mpfcb, *end;
  391:         INT    n = 0;
  392: 
  393:         BEGIN_DISABLE_INTERRUPT;
  394:         end = knl_mpfcb_table + NUM_MPFID;
  395:         for ( mpfcb = knl_mpfcb_table; mpfcb < end; mpfcb++ ) {
  396:                 if ( mpfcb->mpfid == 0 ) {
  397:                         continue;
  398:                 }
  399: 
  400:                 if ( n++ < nent ) {
  401:                         *list++ = ID_MPF(mpfcb - knl_mpfcb_table);
  402:                 }
  403:         }
  404:         END_DISABLE_INTERRUPT;
  405: 
  406:         return n;
  407: }
  408: #endif /* USE_FUNC_TD_LST_MPF */
  409: 
  410: #ifdef USE_FUNC_TD_REF_MPF
  411: /*
  412:  * Refer fixed size memory pool state 
  413:  */
  414: SYSCALL ER td_ref_mpf( ID mpfid, TD_RMPF *pk_rmpf )
  415: {
  416:         MPFCB  *mpfcb;
  417:         ER     ercd = E_OK;
  418: 
  419:         CHECK_MPFID(mpfid);
  420: 
  421:         mpfcb = get_mpfcb(mpfid);
  422: 
  423:         BEGIN_DISABLE_INTERRUPT;
  424:         if ( mpfcb->mpfid == 0 ) {
  425:                 ercd = E_NOEXS;
  426:         } else if ( knl_isLockedOBJ(&mpfcb->lock) ) {
  427:                 ercd = E_CTX;
  428:         } else {
  429:                 pk_rmpf->wtsk = knl_wait_tskid(&mpfcb->wait_queue);
  430:                 pk_rmpf->exinf = mpfcb->exinf;
  431:                 pk_rmpf->frbcnt = mpfcb->frbcnt;
  432:         }
  433:         END_DISABLE_INTERRUPT;
  434: 
  435:         return ercd;
  436: }
  437: #endif /* USE_FUNC_TD_REF_MPF */
  438: 
  439: #ifdef USE_FUNC_TD_MPF_QUE
  440: /*
  441:  * Refer fixed size memory wait queue 
  442:  */
  443: SYSCALL INT td_mpf_que( ID mpfid, ID list[], INT nent )
  444: {
  445:         MPFCB  *mpfcb;
  446:         QUEUE  *q;
  447:         ER     ercd = E_OK;
  448: 
  449:         CHECK_MPFID(mpfid);
  450: 
  451:         mpfcb = get_mpfcb(mpfid);
  452: 
  453:         BEGIN_DISABLE_INTERRUPT;
  454:         if ( mpfcb->mpfid == 0 ) {
  455:                 ercd = E_NOEXS;
  456:         } else {
  457:                 INT n = 0;
  458:                 for ( q = mpfcb->wait_queue.next; q != &mpfcb->wait_queue; q = q->next ) {
  459:                         if ( n++ < nent ) {
  460:                                 *list++ = ((TCB*)q)->tskid;
  461:                         }
  462:                 }
  463:                 ercd = n;
  464:         }
  465:         END_DISABLE_INTERRUPT;
  466: 
  467:         return ercd;
  468: }
  469: #endif /* USE_FUNC_TD_MPF_QUE */
  470: 
  471: #endif /* USE_DBGSPT */
  472: #endif /* USE_FIX_MEMORYPOOL */