asp/extension/dcre/kernel/mempfix.c

   1 /*
   2  *  TOPPERS/ASP Kernel
   3  *      Toyohashi Open Platform for Embedded Real-Time Systems/
   4  *      Advanced Standard Profile Kernel
   5  *
   6  *  Copyright (C) 2000-2003 by Embedded and Real-Time Systems Laboratory
   7  *                              Toyohashi Univ. of Technology, JAPAN
   8  *  Copyright (C) 2005-2010 by Embedded and Real-Time Systems Laboratory
   9  *              Graduate School of Information Science, Nagoya Univ., JAPAN
  10  *
  11  *  上記著作権者は，以下の(1)〜(4)の条件を満たす場合に限り，本ソフトウェ
  12  *  ア（本ソフトウェアを改変したものを含む．以下同じ）を使用・複製・改
  13  *  変・再配布（以下，利用と呼ぶ）することを無償で許諾する．
  14  *  (1) 本ソフトウェアをソースコードの形で利用する場合には，上記の著作
  15  *      権表示，この利用条件および下記の無保証規定が，そのままの形でソー
  16  *      スコード中に含まれていること．
  17  *  (2) 本ソフトウェアを，ライブラリ形式など，他のソフトウェア開発に使
  18  *      用できる形で再配布する場合には，再配布に伴うドキュメント（利用
  19  *      者マニュアルなど）に，上記の著作権表示，この利用条件および下記
  20  *      の無保証規定を掲載すること．
  21  *  (3) 本ソフトウェアを，機器に組み込むなど，他のソフトウェア開発に使
  22  *      用できない形で再配布する場合には，次のいずれかの条件を満たすこ
  23  *      と．
  24  *    (a) 再配布に伴うドキュメント（利用者マニュアルなど）に，上記の著
  25  *        作権表示，この利用条件および下記の無保証規定を掲載すること．
  26  *    (b) 再配布の形態を，別に定める方法によって，TOPPERSプロジェクトに
  27  *        報告すること．
  28  *  (4) 本ソフトウェアの利用により直接的または間接的に生じるいかなる損
  29  *      害からも，上記著作権者およびTOPPERSプロジェクトを免責すること．
  30  *      また，本ソフトウェアのユーザまたはエンドユーザからのいかなる理
  31  *      由に基づく請求からも，上記著作権者およびTOPPERSプロジェクトを
  32  *      免責すること．
  33  *
  34  *  本ソフトウェアは，無保証で提供されているものである．上記著作権者お
  35  *  よびTOPPERSプロジェクトは，本ソフトウェアに関して，特定の使用目的
  36  *  に対する適合性も含めて，いかなる保証も行わない．また，本ソフトウェ
  37  *  アの利用により直接的または間接的に生じたいかなる損害に関しても，そ
  38  *  の責任を負わない．
  39  *
  40  *  @(#) $Id: mempfix.c 1966 2010-11-20 07:23:56Z ertl-hiro $
  41  */
  42
  43 /*
  44  *              固定長メモリプール機能
  45  */
  46
  47 #include "kernel_impl.h"
  48 #include "check.h"
  49 #include "task.h"
  50 #include "wait.h"
  51 #include "mempfix.h"
  52
  53 /*
  54  *  トレースログマクロのデフォルト定義
  55  */
  56 #ifndef LOG_ACRE_MPF_ENTER
  57 #define LOG_ACRE_MPF_ENTER(pk_cmpf)
  58 #endif /* LOG_ACRE_MPF_ENTER */
  59
  60 #ifndef LOG_ACRE_MPF_LEAVE
  61 #define LOG_ACRE_MPF_LEAVE(ercd)
  62 #endif /* LOG_ACRE_MPF_LEAVE */
  63
  64 #ifndef LOG_DEL_MPF_ENTER
  65 #define LOG_DEL_MPF_ENTER(mpfid)
  66 #endif /* LOG_DEL_MPF_ENTER */
  67
  68 #ifndef LOG_DEL_MPF_LEAVE
  69 #define LOG_DEL_MPF_LEAVE(ercd)
  70 #endif /* LOG_DEL_MPF_LEAVE */
  71
  72 #ifndef LOG_GET_MPF_ENTER
  73 #define LOG_GET_MPF_ENTER(mpfid, p_blk)
  74 #endif /* LOG_GET_MPF_ENTER */
  75
  76 #ifndef LOG_GET_MPF_LEAVE
  77 #define LOG_GET_MPF_LEAVE(ercd, blk)
  78 #endif /* LOG_GET_MPF_LEAVE */
  79
  80 #ifndef LOG_PGET_MPF_ENTER
  81 #define LOG_PGET_MPF_ENTER(mpfid, p_blk)
  82 #endif /* LOG_PGET_MPF_ENTER */
  83
  84 #ifndef LOG_PGET_MPF_LEAVE
  85 #define LOG_PGET_MPF_LEAVE(ercd, blk)
  86 #endif /* LOG_PGET_MPF_LEAVE */
  87
  88 #ifndef LOG_TGET_MPF_ENTER
  89 #define LOG_TGET_MPF_ENTER(mpfid, p_blk, tmout)
  90 #endif /* LOG_TGET_MPF_ENTER */
  91
  92 #ifndef LOG_TGET_MPF_LEAVE
  93 #define LOG_TGET_MPF_LEAVE(ercd, blk)
  94 #endif /* LOG_TGET_MPF_LEAVE */
  95
  96 #ifndef LOG_REL_MPF_ENTER
  97 #define LOG_REL_MPF_ENTER(mpfid, blk)
  98 #endif /* LOG_REL_MPF_ENTER */
  99
 100 #ifndef LOG_REL_MPF_LEAVE
 101 #define LOG_REL_MPF_LEAVE(ercd)
 102 #endif /* LOG_REL_MPF_LEAVE */
 103
 104 #ifndef LOG_INI_MPF_ENTER
 105 #define LOG_INI_MPF_ENTER(mpfid)
 106 #endif /* LOG_INI_MPF_ENTER */
 107
 108 #ifndef LOG_INI_MPF_LEAVE
 109 #define LOG_INI_MPF_LEAVE(ercd)
 110 #endif /* LOG_INI_MPF_LEAVE */
 111
 112 #ifndef LOG_REF_MPF_ENTER
 113 #define LOG_REF_MPF_ENTER(mpfid, pk_rmpf)
 114 #endif /* LOG_REF_MPF_ENTER */
 115
 116 #ifndef LOG_REF_MPF_LEAVE
 117 #define LOG_REF_MPF_LEAVE(ercd, pk_rmpf)
 118 #endif /* LOG_REF_MPF_LEAVE */
 119
 120 /*
 121  *  固定長メモリプールの数
 122  */
 123 #define tnum_mpf        ((uint_t)(tmax_mpfid - TMIN_MPFID + 1))
 124 #define tnum_smpf       ((uint_t)(tmax_smpfid - TMIN_MPFID + 1))
 125
 126 /*
 127  *  固定長メモリプールIDから固定長メモリプール管理ブロックを取り出すた
 128  *  めのマクロ
 129  */
 130 #define INDEX_MPF(mpfid)        ((uint_t)((mpfid) - TMIN_MPFID))
 131 #define get_mpfcb(mpfid)        (&(mpfcb_table[INDEX_MPF(mpfid)]))
 132
 133 /*
 134  *  特殊なインデックス値の定義
 135  */
 136 #define INDEX_NULL              (~0U)           /* 空きブロックリストの最後 */
 137 #define INDEX_ALLOC             (~1U)           /* 割当て済みのブロック */
 138
 139 #ifdef TOPPERS_mpfini
 140
 141 /*
 142  *  使用していない固定長メモリプール管理ブロックのリスト
 143  */
 144 QUEUE   free_mpfcb;
 145
 146 /*
 147  *  固定長メモリプール機能の初期化
 148  */
 149 void
 150 initialize_mempfix(void)
 151 {
 152         uint_t  i, j;
 153         MPFCB   *p_mpfcb;
 154         MPFINIB *p_mpfinib;
 155
 156         for (p_mpfcb = mpfcb_table, i = 0; i < tnum_smpf; p_mpfcb++, i++) {
 157                 queue_initialize(&(p_mpfcb->wait_queue));
 158                 p_mpfcb->p_mpfinib = &(mpfinib_table[i]);
 159                 p_mpfcb->fblkcnt = p_mpfcb->p_mpfinib->blkcnt;
 160                 p_mpfcb->unused = 0U;
 161                 p_mpfcb->freelist = INDEX_NULL;
 162         }
 163         queue_initialize(&free_mpfcb);
 164         for (j = 0; i < tnum_mpf; p_mpfcb++, i++, j++) {
 165                 p_mpfinib = &(ampfinib_table[j]);
 166                 p_mpfinib->mpfatr = TA_NOEXS;
 167                 p_mpfcb->p_mpfinib = ((const MPFINIB *) p_mpfinib);
 168                 queue_insert_prev(&free_mpfcb, &(p_mpfcb->wait_queue));
 169         }
 170 }
 171
 172 #endif /* TOPPERS_mpfini */
 173
 174 /*
 175  *  固定長メモリプールからブロックを獲得
 176  */
 177 #ifdef TOPPERS_mpfget
 178
 179 void
 180 get_mpf_block(MPFCB *p_mpfcb, void **p_blk)
 181 {
 182         uint_t  blkidx;
 183
 184         if (p_mpfcb->freelist != INDEX_NULL) {
 185                 blkidx = p_mpfcb->freelist;
 186                 p_mpfcb->freelist = (p_mpfcb->p_mpfinib->p_mpfmb + blkidx)->next;
 187         }
 188         else {
 189                 blkidx = p_mpfcb->unused;
 190                 p_mpfcb->unused++;
 191         }
 192         *p_blk = (void *)((char *)(p_mpfcb->p_mpfinib->mpf)
 193                                                                 + p_mpfcb->p_mpfinib->blksz * blkidx);
 194         p_mpfcb->fblkcnt--;
 195         (p_mpfcb->p_mpfinib->p_mpfmb + blkidx)->next = INDEX_ALLOC;
 196 }
 197
 198 #endif /* TOPPERS_mpfget */
 199
 200 /*
 201  *  固定長メモリプールの生成
 202  */
 203 #ifdef TOPPERS_acre_mpf
 204
 205 ER_UINT
 206 acre_mpf(const T_CMPF *pk_cmpf)
 207 {
 208         MPFCB   *p_mpfcb;
 209         MPFINIB *p_mpfinib;
 210         ATR             mpfatr;
 211         void    *mpf;
 212         MPFMB   *p_mpfmb;
 213         ER              ercd;
 214
 215         LOG_ACRE_MPF_ENTER(pk_cmpf);
 216         CHECK_TSKCTX_UNL();
 217         CHECK_RSATR(pk_cmpf->mpfatr, TA_TPRI|TA_MPRI);
 218         CHECK_PAR(pk_cmpf->blkcnt != 0);
 219         CHECK_PAR(pk_cmpf->blksz != 0);
 220         CHECK_ALIGN_MPF(pk_cmpf->mpf);
 221         CHECK_ALIGN_MB(pk_cmpf->mpfmb);
 222         mpfatr = pk_cmpf->mpfatr;
 223         mpf = pk_cmpf->mpf;
 224         p_mpfmb = pk_cmpf->mpfmb;
 225
 226         t_lock_cpu();
 227         if (queue_empty(&free_mpfcb)) {
 228                 ercd = E_NOID;
 229         }
 230         else {
 231                 if (mpf == NULL) {
 232                         mpf = kernel_malloc(pk_cmpf->blkcnt * ROUND_MPF_T(pk_cmpf->blksz));
 233                         mpfatr |= TA_MEMALLOC;
 234                 }
 235                 if (mpf == NULL) {
 236                         ercd = E_NOMEM;
 237                 }
 238                 else {
 239                         if (p_mpfmb == NULL) {
 240                                 p_mpfmb = kernel_malloc(sizeof(MPFMB) * pk_cmpf->blkcnt);
 241                                 mpfatr |= TA_MBALLOC;
 242                         }
 243                         if (p_mpfmb == NULL) {
 244                                 if ((mpfatr & TA_MEMALLOC) != 0U) {
 245                                         kernel_free(mpf);
 246                                 }
 247                                 ercd = E_NOMEM;
 248                         }
 249                         else {
 250                                 p_mpfcb = ((MPFCB *) queue_delete_next(&free_mpfcb));
 251                                 p_mpfinib = (MPFINIB *)(p_mpfcb->p_mpfinib);
 252                                 p_mpfinib->mpfatr = mpfatr;
 253                                 p_mpfinib->blkcnt = pk_cmpf->blkcnt;
 254                                 p_mpfinib->blksz = ROUND_MPF_T(pk_cmpf->blksz);
 255                                 p_mpfinib->mpf = mpf;
 256                                 p_mpfinib->p_mpfmb = p_mpfmb;
 257
 258                                 queue_initialize(&(p_mpfcb->wait_queue));
 259                                 p_mpfcb->fblkcnt = p_mpfcb->p_mpfinib->blkcnt;
 260                                 p_mpfcb->unused = 0U;
 261                                 p_mpfcb->freelist = INDEX_NULL;
 262                                 ercd = MPFID(p_mpfcb);
 263                         }
 264                 }
 265         }
 266         t_unlock_cpu();
 267
 268   error_exit:
 269         LOG_ACRE_MPF_LEAVE(ercd);
 270         return(ercd);
 271 }
 272
 273 #endif /* TOPPERS_acre_mpf */
 274
 275 /*
 276  *  固定長メモリプールの削除
 277  */
 278 #ifdef TOPPERS_del_mpf
 279
 280 ER
 281 del_mpf(ID mpfid)
 282 {
 283         MPFCB   *p_mpfcb;
 284         MPFINIB *p_mpfinib;
 285         bool_t  dspreq;
 286         ER              ercd;
 287
 288         LOG_DEL_MPF_ENTER(mpfid);
 289         CHECK_TSKCTX_UNL();
 290         CHECK_MPFID(mpfid);
 291         p_mpfcb = get_mpfcb(mpfid);
 292
 293         t_lock_cpu();
 294         if (p_mpfcb->p_mpfinib->mpfatr == TA_NOEXS) {
 295                 ercd = E_NOEXS;
 296         }
 297         else if (MPFID(p_mpfcb) > tmax_smpfid) {
 298                 dspreq = init_wait_queue(&(p_mpfcb->wait_queue));
 299                 p_mpfinib = (MPFINIB *)(p_mpfcb->p_mpfinib);
 300                 if ((p_mpfinib->mpfatr & TA_MEMALLOC) != 0U) {
 301                         kernel_free(p_mpfinib->mpf);
 302                 }
 303                 if ((p_mpfinib->mpfatr & TA_MBALLOC) != 0U) {
 304                         kernel_free(p_mpfinib->p_mpfmb);
 305                 }
 306                 p_mpfinib->mpfatr = TA_NOEXS;
 307                 queue_insert_prev(&free_mpfcb, &(p_mpfcb->wait_queue));
 308                 if (dspreq) {
 309                         dispatch();
 310                 }
 311                 ercd = E_OK;
 312         }
 313         else {
 314                 ercd = E_OBJ;
 315         }
 316         t_unlock_cpu();
 317
 318   error_exit:
 319         LOG_DEL_MPF_LEAVE(ercd);
 320         return(ercd);
 321 }
 322
 323 #endif /* TOPPERS_del_mpf */
 324
 325 /*
 326  *  固定長メモリブロックの獲得
 327  */
 328 #ifdef TOPPERS_get_mpf
 329
 330 ER
 331 get_mpf(ID mpfid, void **p_blk)
 332 {
 333         MPFCB   *p_mpfcb;
 334         WINFO_MPF winfo_mpf;
 335         ER              ercd;
 336
 337         LOG_GET_MPF_ENTER(mpfid, p_blk);
 338         CHECK_DISPATCH();
 339         CHECK_MPFID(mpfid);
 340         p_mpfcb = get_mpfcb(mpfid);
 341
 342         t_lock_cpu();
 343         if (p_mpfcb->fblkcnt > 0) {
 344                 get_mpf_block(p_mpfcb, p_blk);
 345                 ercd = E_OK;
 346         }
 347         else {
 348                 p_runtsk->tstat = (TS_WAITING | TS_WAIT_MPF);
 349                 wobj_make_wait((WOBJCB *) p_mpfcb, (WINFO_WOBJ *) &winfo_mpf);
 350                 dispatch();
 351                 ercd = winfo_mpf.winfo.wercd;
 352                 if (ercd == E_OK) {
 353                         *p_blk = winfo_mpf.blk;
 354                 }
 355         }
 356         t_unlock_cpu();
 357
 358   error_exit:
 359         LOG_GET_MPF_LEAVE(ercd, *p_blk);
 360         return(ercd);
 361 }
 362
 363 #endif /* TOPPERS_get_mpf */
 364
 365 /*
 366  *  固定長メモリブロックの獲得（ポーリング）
 367  */
 368 #ifdef TOPPERS_pget_mpf
 369
 370 ER
 371 pget_mpf(ID mpfid, void **p_blk)
 372 {
 373         MPFCB   *p_mpfcb;
 374         ER              ercd;
 375
 376         LOG_PGET_MPF_ENTER(mpfid, p_blk);
 377         CHECK_TSKCTX_UNL();
 378         CHECK_MPFID(mpfid);
 379         p_mpfcb = get_mpfcb(mpfid);
 380
 381         t_lock_cpu();
 382         if (p_mpfcb->fblkcnt > 0) {
 383                 get_mpf_block(p_mpfcb, p_blk);
 384                 ercd = E_OK;
 385         }
 386         else {
 387                 ercd = E_TMOUT;
 388         }
 389         t_unlock_cpu();
 390
 391   error_exit:
 392         LOG_PGET_MPF_LEAVE(ercd, *p_blk);
 393         return(ercd);
 394 }
 395
 396 #endif /* TOPPERS_pget_mpf */
 397
 398 /*
 399  *  固定長メモリブロックの獲得（タイムアウトあり）
 400  */
 401 #ifdef TOPPERS_tget_mpf
 402
 403 ER
 404 tget_mpf(ID mpfid, void **p_blk, TMO tmout)
 405 {
 406         MPFCB   *p_mpfcb;
 407         WINFO_MPF winfo_mpf;
 408         TMEVTB  tmevtb;
 409         ER              ercd;
 410
 411         LOG_TGET_MPF_ENTER(mpfid, p_blk, tmout);
 412         CHECK_DISPATCH();
 413         CHECK_MPFID(mpfid);
 414         CHECK_TMOUT(tmout);
 415         p_mpfcb = get_mpfcb(mpfid);
 416
 417         t_lock_cpu();
 418         if (p_mpfcb->fblkcnt > 0) {
 419                 get_mpf_block(p_mpfcb, p_blk);
 420                 ercd = E_OK;
 421         }
 422         else if (tmout == TMO_POL) {
 423                 ercd = E_TMOUT;
 424         }
 425         else {
 426                 p_runtsk->tstat = (TS_WAITING | TS_WAIT_MPF);
 427                 wobj_make_wait_tmout((WOBJCB *) p_mpfcb, (WINFO_WOBJ *) &winfo_mpf,
 428                                                                                                                 &tmevtb, tmout);
 429                 dispatch();
 430                 ercd = winfo_mpf.winfo.wercd;
 431                 if (ercd == E_OK) {
 432                         *p_blk = winfo_mpf.blk;
 433                 }
 434         }
 435         t_unlock_cpu();
 436
 437   error_exit:
 438         LOG_TGET_MPF_LEAVE(ercd, *p_blk);
 439         return(ercd);
 440 }
 441
 442 #endif /* TOPPERS_tget_mpf */
 443
 444 /*
 445  *  固定長メモリブロックの返却
 446  */
 447 #ifdef TOPPERS_rel_mpf
 448
 449 ER
 450 rel_mpf(ID mpfid, void *blk)
 451 {
 452         MPFCB   *p_mpfcb;
 453         SIZE    blkoffset;
 454         uint_t  blkidx;
 455         TCB             *p_tcb;
 456         ER              ercd;
 457
 458         LOG_REL_MPF_ENTER(mpfid, blk);
 459         CHECK_TSKCTX_UNL();
 460         CHECK_MPFID(mpfid);
 461         p_mpfcb = get_mpfcb(mpfid);
 462         CHECK_PAR(p_mpfcb->p_mpfinib->mpf <= blk);
 463         blkoffset = ((char *) blk) - (char *)(p_mpfcb->p_mpfinib->mpf);
 464         CHECK_PAR(blkoffset % p_mpfcb->p_mpfinib->blksz == 0U);
 465         CHECK_PAR(blkoffset / p_mpfcb->p_mpfinib->blksz < p_mpfcb->unused);
 466         blkidx = (uint_t)(blkoffset / p_mpfcb->p_mpfinib->blksz);
 467         CHECK_PAR((p_mpfcb->p_mpfinib->p_mpfmb + blkidx)->next == INDEX_ALLOC);
 468
 469         t_lock_cpu();
 470         if (!queue_empty(&(p_mpfcb->wait_queue))) {
 471                 p_tcb = (TCB *) queue_delete_next(&(p_mpfcb->wait_queue));
 472                 ((WINFO_MPF *)(p_tcb->p_winfo))->blk = blk;
 473                 if (wait_complete(p_tcb)) {
 474                         dispatch();
 475                 }
 476                 ercd = E_OK;
 477         }
 478         else {
 479                 p_mpfcb->fblkcnt++;
 480                 (p_mpfcb->p_mpfinib->p_mpfmb + blkidx)->next = p_mpfcb->freelist;
 481                 p_mpfcb->freelist = blkidx;
 482                 ercd = E_OK;
 483         }
 484         t_unlock_cpu();
 485
 486   error_exit:
 487         LOG_REL_MPF_LEAVE(ercd);
 488         return(ercd);
 489 }
 490
 491 #endif /* TOPPERS_rel_mpf */
 492
 493 /*
 494  *  固定長メモリプールの再初期化
 495  */
 496 #ifdef TOPPERS_ini_mpf
 497
 498 ER
 499 ini_mpf(ID mpfid)
 500 {
 501         MPFCB   *p_mpfcb;
 502         bool_t  dspreq;
 503         ER              ercd;
 504
 505         LOG_INI_MPF_ENTER(mpfid);
 506         CHECK_TSKCTX_UNL();
 507         CHECK_MPFID(mpfid);
 508         p_mpfcb = get_mpfcb(mpfid);
 509
 510         t_lock_cpu();
 511         dspreq = init_wait_queue(&(p_mpfcb->wait_queue));
 512         p_mpfcb->fblkcnt = p_mpfcb->p_mpfinib->blkcnt;
 513         p_mpfcb->unused = 0U;
 514         p_mpfcb->freelist = INDEX_NULL;
 515         if (dspreq) {
 516                 dispatch();
 517         }
 518         ercd = E_OK;
 519         t_unlock_cpu();
 520
 521   error_exit:
 522         LOG_INI_MPF_LEAVE(ercd);
 523         return(ercd);
 524 }
 525
 526 #endif /* TOPPERS_ini_mpf */
 527
 528 /*
 529  *  固定長メモリプールの状態参照
 530  */
 531 #ifdef TOPPERS_ref_mpf
 532
 533 ER
 534 ref_mpf(ID mpfid, T_RMPF *pk_rmpf)
 535 {
 536         MPFCB   *p_mpfcb;
 537         ER              ercd;
 538
 539         LOG_REF_MPF_ENTER(mpfid, pk_rmpf);
 540         CHECK_TSKCTX_UNL();
 541         CHECK_MPFID(mpfid);
 542         p_mpfcb = get_mpfcb(mpfid);
 543
 544         t_lock_cpu();
 545         pk_rmpf->wtskid = wait_tskid(&(p_mpfcb->wait_queue));
 546         pk_rmpf->fblkcnt = p_mpfcb->fblkcnt;
 547         ercd = E_OK;
 548         t_unlock_cpu();
 549
 550   error_exit:
 551         LOG_REF_MPF_LEAVE(ercd, pk_rmpf);
 552         return(ercd);
 553 }
 554
 555 #endif /* TOPPERS_ref_mpf */