source: asp3_wo_tecs/trunk/extension/dcre/kernel/mempfix.c@ 302

Last change on this file since 302 was 302, checked in by ertl-honda, 7 years ago

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