trx305/kernel/kernel/dataqueue.c

   1 /*
   2  *  TOPPERS/JSP Kernel
   3  *      Toyohashi Open Platform for Embedded Real-Time Systems/
   4  *      Just Standard Profile Kernel
   5  *
   6  *  Copyright (C) 2000-2003 by Embedded and Real-Time Systems Laboratory
   7  *                              Toyohashi Univ. of Technology, JAPAN
   8  *
   9  *  上記著作権者は，以下の (1)〜(4) の条件か，Free Software Foundation
  10  *  によって公表されている GNU General Public License の Version 2 に記
  11  *  述されている条件を満たす場合に限り，本ソフトウェア（本ソフトウェア
  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  *  本ソフトウェアは，無保証で提供されているものである．上記著作権者お
  32  *  よびTOPPERSプロジェクトは，本ソフトウェアに関して，その適用可能性も
  33  *  含めて，いかなる保証も行わない．また，本ソフトウェアの利用により直
  34  *  接的または間接的に生じたいかなる損害に関しても，その責任を負わない．
  35  *
  36  *  @(#) $Id: dataqueue.c,v 1.1 2009/01/31 05:27:37 suikan Exp $
  37  */
  38
  39 /*
  40  *  データキュー機能
  41  */
  42
  43 #include "jsp_kernel.h"
  44 #include "check.h"
  45 #include "task.h"
  46 #include "wait.h"
  47 #include "dataqueue.h"
  48
  49 /*
  50  *  データキューIDの最大値（kernel_cfg.c）
  51  */
  52 extern const ID tmax_dtqid;
  53
  54 /*
  55  *  データキュー初期化ブロックのエリア（kernel_cfg.c）
  56  */
  57 extern const DTQINIB    dtqinib_table[];
  58
  59 /*
  60  *  データキューの数
  61  */
  62 #define TNUM_DTQ    ((UINT)(tmax_dtqid - TMIN_DTQID + 1))
  63
  64 /*
  65  *  データキュー管理ブロックのエリア（kernel_cfg.c）
  66  */
  67 extern DTQCB    dtqcb_table[];
  68
  69 /*
  70  *  データキューIDからデータキュー管理ブロックを取り出すためのマクロ
  71  */
  72 #define INDEX_DTQ(dtqid)    ((UINT)((dtqid) - TMIN_DTQID))
  73 #define get_dtqcb(dtqid)    (&(dtqcb_table[INDEX_DTQ(dtqid)]))
  74
  75 /*
  76  *  データキュー待ち情報ブロックの定義
  77  *
  78  *  データキューへの送信待ちとデータキューからの受信待ちで，同じ待ち情
  79  *  報ブロックを使う．
  80  */
  81 typedef struct dataqueue_waiting_information {
  82     WINFO   winfo;      /* 標準の待ち情報ブロック */
  83     WOBJCB  *wobjcb;    /* 待ちオブジェクトの管理ブロック */
  84     VP_INT  data;       /* 送受信データ  */
  85 } WINFO_DTQ;
  86
  87 /*
  88  *  データキュー機能の初期化
  89  */
  90 #ifdef __dtqini
  91
  92 void
  93 dataqueue_initialize(void)
  94 {
  95     UINT    i;
  96     DTQCB   *dtqcb;
  97
  98     for (dtqcb = dtqcb_table, i = 0; i < TNUM_DTQ; dtqcb++, i++) {
  99         queue_initialize(&(dtqcb->swait_queue));
 100         dtqcb->dtqinib = &(dtqinib_table[i]);
 101         queue_initialize(&(dtqcb->rwait_queue));
 102         dtqcb->count = 0;
 103         dtqcb->head = 0;
 104         dtqcb->tail = 0;
 105     }
 106 }
 107
 108 #endif /* __dtqini */
 109
 110 /*
 111  *  データキュー領域にデータを格納
 112  */
 113 #ifdef __dtqenq
 114
 115 BOOL
 116 enqueue_data(DTQCB *dtqcb, VP_INT data)
 117 {
 118     if (dtqcb->count < dtqcb->dtqinib->dtqcnt) {
 119         *((VP_INT *)(dtqcb->dtqinib->dtq) + dtqcb->tail) = data;
 120         dtqcb->count++;
 121         dtqcb->tail++;
 122         if (dtqcb->tail >= dtqcb->dtqinib->dtqcnt) {
 123             dtqcb->tail = 0;
 124         }
 125         return(TRUE);
 126     }
 127     return(FALSE);
 128 }
 129
 130 #endif /* __dtqenq */
 131
 132 /*
 133  *  データキュー領域にデータを強制格納
 134  */
 135 #ifdef __dtqfenq
 136
 137 void
 138 force_enqueue_data(DTQCB *dtqcb, VP_INT data)
 139 {
 140     *((VP_INT *)(dtqcb->dtqinib->dtq) + dtqcb->tail) = data;
 141     dtqcb->tail++;
 142     if (dtqcb->tail >= dtqcb->dtqinib->dtqcnt) {
 143         dtqcb->tail = 0;
 144     }
 145     if (dtqcb->count < dtqcb->dtqinib->dtqcnt) {
 146         dtqcb->count++;
 147     }
 148     else {
 149         dtqcb->head = dtqcb->tail;
 150     }
 151 }
 152
 153 #endif /* __dtqfenq */
 154
 155 /*
 156  *  データキュー領域からデータを取出し
 157  */
 158 #ifdef __dtqdeq
 159
 160 BOOL
 161 dequeue_data(DTQCB *dtqcb, VP_INT *p_data)
 162 {
 163     if (dtqcb->count > 0) {
 164         *p_data = *((VP_INT *)(dtqcb->dtqinib->dtq) + dtqcb->head);
 165         dtqcb->count--;
 166         dtqcb->head++;
 167         if (dtqcb->head >= dtqcb->dtqinib->dtqcnt) {
 168             dtqcb->head = 0;
 169         }
 170         return(TRUE);
 171     }
 172     return(FALSE);
 173 }
 174
 175 #endif /* __dtqdeq */
 176
 177 /*
 178  *  受信待ちキューの先頭タスクへのデータ送信
 179  */
 180 #ifdef __dtqsnd
 181
 182 TCB *
 183 send_data_rwait(DTQCB *dtqcb, VP_INT data)
 184 {
 185     TCB *tcb;
 186
 187     if (!(queue_empty(&(dtqcb->rwait_queue)))) {
 188         tcb = (TCB *) queue_delete_next(&(dtqcb->rwait_queue));
 189         ((WINFO_DTQ *)(tcb->winfo))->data = data;
 190         return(tcb);
 191     }
 192     return(NULL);
 193 }
 194
 195 #endif /* __dtqsnd */
 196
 197 /*
 198  *  送信待ちキューの先頭タスクからのデータ受信
 199  */
 200 #ifdef __dtqrcv
 201
 202 TCB *
 203 receive_data_swait(DTQCB *dtqcb, VP_INT *p_data)
 204 {
 205     TCB *tcb;
 206
 207     if (!(queue_empty(&(dtqcb->swait_queue)))) {
 208         tcb = (TCB *) queue_delete_next(&(dtqcb->swait_queue));
 209         *p_data = ((WINFO_DTQ *)(tcb->winfo))->data;
 210         return(tcb);
 211     }
 212     return(NULL);
 213 }
 214
 215 #endif /* __dtqrcv */
 216
 217 /*
 218  *  データキューへの送信
 219  */
 220 #ifdef __snd_dtq
 221
 222 SYSCALL ER
 223 snd_dtq(ID dtqid, VP_INT data)
 224 {
 225     DTQCB   *dtqcb;
 226     WINFO_DTQ winfo;
 227     TCB *tcb;
 228     ER  ercd;
 229
 230     LOG_SND_DTQ_ENTER(dtqid, data);
 231     CHECK_DISPATCH();
 232     CHECK_DTQID(dtqid);
 233     dtqcb = get_dtqcb(dtqid);
 234
 235     t_lock_cpu();
 236     if ((tcb = send_data_rwait(dtqcb, data)) != NULL) {
 237         if (wait_complete(tcb)) {
 238             dispatch();
 239         }
 240         ercd = E_OK;
 241     }
 242     else if (enqueue_data(dtqcb, data)) {
 243         ercd = E_OK;
 244     }
 245     else {
 246         winfo.data = data;
 247         wobj_make_wait((WOBJCB *) dtqcb, (WINFO_WOBJ *) &winfo);
 248         dispatch();
 249         ercd = winfo.winfo.wercd;
 250     }
 251     t_unlock_cpu();
 252
 253     exit:
 254     LOG_SND_DTQ_LEAVE(ercd);
 255     return(ercd);
 256 }
 257
 258 #endif /* __snd_dtq */
 259
 260 /*
 261  *  データキューへの送信（ポーリング）
 262  */
 263 #ifdef __psnd_dtq
 264
 265 SYSCALL ER
 266 psnd_dtq(ID dtqid, VP_INT data)
 267 {
 268     DTQCB   *dtqcb;
 269     TCB *tcb;
 270     ER  ercd;
 271
 272     LOG_PSND_DTQ_ENTER(dtqid, data);
 273     CHECK_TSKCTX_UNL();
 274     CHECK_DTQID(dtqid);
 275     dtqcb = get_dtqcb(dtqid);
 276
 277     t_lock_cpu();
 278     if ((tcb = send_data_rwait(dtqcb, data)) != NULL) {
 279         if (wait_complete(tcb)) {
 280             dispatch();
 281         }
 282         ercd = E_OK;
 283     }
 284     else if (enqueue_data(dtqcb, data)) {
 285         ercd = E_OK;
 286     }
 287     else {
 288         ercd = E_TMOUT;
 289     }
 290     t_unlock_cpu();
 291
 292     exit:
 293     LOG_PSND_DTQ_LEAVE(ercd);
 294     return(ercd);
 295 }
 296
 297 #endif /* __psnd_dtq */
 298
 299 /*
 300  *  データキューへの送信（ポーリング，非タスクコンテキスト用）
 301  */
 302 #ifdef __ipsnd_dtq
 303
 304 SYSCALL ER
 305 ipsnd_dtq(ID dtqid, VP_INT data)
 306 {
 307     DTQCB   *dtqcb;
 308     TCB *tcb;
 309     ER  ercd;
 310
 311     LOG_IPSND_DTQ_ENTER(dtqid, data);
 312     CHECK_INTCTX_UNL();
 313     CHECK_DTQID(dtqid);
 314     dtqcb = get_dtqcb(dtqid);
 315
 316     i_lock_cpu();
 317     if ((tcb = send_data_rwait(dtqcb, data)) != NULL) {
 318         if (wait_complete(tcb)) {
 319             reqflg = TRUE;
 320         }
 321         ercd = E_OK;
 322     }
 323     else if (enqueue_data(dtqcb, data)) {
 324         ercd = E_OK;
 325     }
 326     else {
 327         ercd = E_TMOUT;
 328     }
 329     i_unlock_cpu();
 330
 331     exit:
 332     LOG_IPSND_DTQ_LEAVE(ercd);
 333     return(ercd);
 334 }
 335
 336 #endif /* __ipsnd_dtq */
 337
 338 /*
 339  *  データキューへの送信（タイムアウトあり）
 340  */
 341 #ifdef __tsnd_dtq
 342
 343 SYSCALL ER
 344 tsnd_dtq(ID dtqid, VP_INT data, TMO tmout)
 345 {
 346     DTQCB   *dtqcb;
 347     WINFO_DTQ winfo;
 348     TMEVTB  tmevtb;
 349     TCB *tcb;
 350     ER  ercd;
 351
 352     LOG_TSND_DTQ_ENTER(dtqid, data, tmout);
 353     CHECK_DISPATCH();
 354     CHECK_DTQID(dtqid);
 355     CHECK_TMOUT(tmout);
 356     dtqcb = get_dtqcb(dtqid);
 357
 358     t_lock_cpu();
 359     if ((tcb = send_data_rwait(dtqcb, data)) != NULL) {
 360         if (wait_complete(tcb)) {
 361             dispatch();
 362         }
 363         ercd = E_OK;
 364     }
 365     else if (enqueue_data(dtqcb, data)) {
 366         ercd = E_OK;
 367     }
 368     else if (tmout == TMO_POL) {
 369         ercd = E_TMOUT;
 370     }
 371     else {
 372         winfo.data = data;
 373         wobj_make_wait_tmout((WOBJCB *) dtqcb, (WINFO_WOBJ *) &winfo,
 374                         &tmevtb, tmout);
 375         dispatch();
 376         ercd = winfo.winfo.wercd;
 377     }
 378     t_unlock_cpu();
 379
 380     exit:
 381     LOG_TSND_DTQ_LEAVE(ercd);
 382     return(ercd);
 383 }
 384
 385 #endif /* __tsnd_dtq */
 386
 387 /*
 388  *  データキューへの強制送信
 389  */
 390 #ifdef __fsnd_dtq
 391
 392 SYSCALL ER
 393 fsnd_dtq(ID dtqid, VP_INT data)
 394 {
 395     DTQCB   *dtqcb;
 396     TCB *tcb;
 397     ER  ercd;
 398
 399     LOG_FSND_DTQ_ENTER(dtqid, data);
 400     CHECK_TSKCTX_UNL();
 401     CHECK_DTQID(dtqid);
 402     dtqcb = get_dtqcb(dtqid);
 403     CHECK_ILUSE(dtqcb->dtqinib->dtqcnt > 0);
 404
 405     t_lock_cpu();
 406     if ((tcb = send_data_rwait(dtqcb, data)) != NULL) {
 407         if (wait_complete(tcb)) {
 408             dispatch();
 409         }
 410     }
 411     else {
 412         force_enqueue_data(dtqcb, data);
 413     }
 414     ercd = E_OK;
 415     t_unlock_cpu();
 416
 417     exit:
 418     LOG_FSND_DTQ_LEAVE(ercd);
 419     return(ercd);
 420 }
 421
 422 #endif /* __fsnd_dtq */
 423
 424 /*
 425  *  データキューへの強制送信（非タスクコンテキスト用）
 426  */
 427 #ifdef __ifsnd_dtq
 428
 429 SYSCALL ER
 430 ifsnd_dtq(ID dtqid, VP_INT data)
 431 {
 432     DTQCB   *dtqcb;
 433     TCB *tcb;
 434     ER  ercd;
 435
 436     LOG_IFSND_DTQ_ENTER(dtqid, data);
 437     CHECK_INTCTX_UNL();
 438     CHECK_DTQID(dtqid);
 439     dtqcb = get_dtqcb(dtqid);
 440     CHECK_ILUSE(dtqcb->dtqinib->dtqcnt > 0);
 441
 442     i_lock_cpu();
 443     if ((tcb = send_data_rwait(dtqcb, data)) != NULL) {
 444         if (wait_complete(tcb)) {
 445             reqflg = TRUE;
 446         }
 447     }
 448     else {
 449         force_enqueue_data(dtqcb, data);
 450     }
 451     ercd = E_OK;
 452     i_unlock_cpu();
 453
 454     exit:
 455     LOG_IFSND_DTQ_LEAVE(ercd);
 456     return(ercd);
 457 }
 458
 459 #endif /* __ifsnd_dtq */
 460
 461 /*
 462  *  データキューからの受信
 463  */
 464 #ifdef __rcv_dtq
 465
 466 SYSCALL ER
 467 rcv_dtq(ID dtqid, VP_INT *p_data)
 468 {
 469     DTQCB   *dtqcb;
 470     WINFO_DTQ winfo;
 471     TCB *tcb;
 472     VP_INT  data;
 473     ER  ercd;
 474
 475     LOG_RCV_DTQ_ENTER(dtqid, p_data);
 476     CHECK_DISPATCH();
 477     CHECK_DTQID(dtqid);
 478     dtqcb = get_dtqcb(dtqid);
 479
 480     t_lock_cpu();
 481     if (dequeue_data(dtqcb, p_data)) {
 482         if ((tcb = receive_data_swait(dtqcb, &data)) != NULL) {
 483             enqueue_data(dtqcb, data);
 484             if (wait_complete(tcb)) {
 485                 dispatch();
 486             }
 487         }
 488         ercd = E_OK;
 489     }
 490     else if ((tcb = receive_data_swait(dtqcb, p_data)) != NULL) {
 491         if (wait_complete(tcb)) {
 492             dispatch();
 493         }
 494         ercd = E_OK;
 495     }
 496     else {
 497         runtsk->tstat = (TS_WAITING | TS_WAIT_WOBJ);
 498         make_wait(&(winfo.winfo));
 499         queue_insert_prev(&(dtqcb->rwait_queue),
 500                     &(runtsk->task_queue));
 501         winfo.wobjcb = (WOBJCB *) dtqcb;
 502         LOG_TSKSTAT(runtsk);
 503         dispatch();
 504         ercd = winfo.winfo.wercd;
 505         if (ercd == E_OK) {
 506             *p_data = winfo.data;
 507         }
 508     }
 509     t_unlock_cpu();
 510
 511     exit:
 512     LOG_RCV_DTQ_LEAVE(ercd, *p_data);
 513     return(ercd);
 514 }
 515
 516 #endif /* __rcv_dtq */
 517
 518 /*
 519  *  データキューからの受信（ポーリング）
 520  */
 521 #ifdef __prcv_dtq
 522
 523 SYSCALL ER
 524 prcv_dtq(ID dtqid, VP_INT *p_data)
 525 {
 526     DTQCB   *dtqcb;
 527     TCB *tcb;
 528     VP_INT  data;
 529     ER  ercd;
 530
 531     LOG_PRCV_DTQ_ENTER(dtqid, p_data);
 532     CHECK_TSKCTX_UNL();
 533     CHECK_DTQID(dtqid);
 534     dtqcb = get_dtqcb(dtqid);
 535
 536     t_lock_cpu();
 537     if (dequeue_data(dtqcb, p_data)) {
 538         if ((tcb = receive_data_swait(dtqcb, &data)) != NULL) {
 539             enqueue_data(dtqcb, data);
 540             if (wait_complete(tcb)) {
 541                 dispatch();
 542             }
 543         }
 544         ercd = E_OK;
 545     }
 546     else if ((tcb = receive_data_swait(dtqcb, p_data)) != NULL) {
 547         if (wait_complete(tcb)) {
 548             dispatch();
 549         }
 550         ercd = E_OK;
 551     }
 552     else {
 553         ercd = E_TMOUT;
 554     }
 555     t_unlock_cpu();
 556
 557     exit:
 558     LOG_PRCV_DTQ_LEAVE(ercd, *p_data);
 559     return(ercd);
 560 }
 561
 562 #endif /* __prcv_dtq */
 563
 564 /*
 565  *  データキューからの受信（タイムアウトあり）
 566  */
 567 #ifdef __trcv_dtq
 568
 569 SYSCALL ER
 570 trcv_dtq(ID dtqid, VP_INT *p_data, TMO tmout)
 571 {
 572     DTQCB   *dtqcb;
 573     WINFO_DTQ winfo;
 574     TMEVTB  tmevtb;
 575     TCB *tcb;
 576     VP_INT  data;
 577     ER  ercd;
 578
 579     LOG_TRCV_DTQ_ENTER(dtqid, p_data, tmout);
 580     CHECK_DISPATCH();
 581     CHECK_DTQID(dtqid);
 582     CHECK_TMOUT(tmout);
 583     dtqcb = get_dtqcb(dtqid);
 584
 585     t_lock_cpu();
 586     if (dequeue_data(dtqcb, p_data)) {
 587         if ((tcb = receive_data_swait(dtqcb, &data)) != NULL) {
 588             enqueue_data(dtqcb, data);
 589             if (wait_complete(tcb)) {
 590                 dispatch();
 591             }
 592         }
 593         ercd = E_OK;
 594     }
 595     else if ((tcb = receive_data_swait(dtqcb, p_data)) != NULL) {
 596         if (wait_complete(tcb)) {
 597             dispatch();
 598         }
 599         ercd = E_OK;
 600     }
 601     else if (tmout == TMO_POL) {
 602         ercd = E_TMOUT;
 603     }
 604     else {
 605         runtsk->tstat = (TS_WAITING | TS_WAIT_WOBJ);
 606         make_wait_tmout(&(winfo.winfo), &tmevtb, tmout);
 607         queue_insert_prev(&(dtqcb->rwait_queue),
 608                     &(runtsk->task_queue));
 609         winfo.wobjcb = (WOBJCB *) dtqcb;
 610         LOG_TSKSTAT(runtsk);
 611         dispatch();
 612         ercd = winfo.winfo.wercd;
 613         if (ercd == E_OK) {
 614             *p_data = winfo.data;
 615         }
 616     }
 617     t_unlock_cpu();
 618
 619     exit:
 620     LOG_TRCV_DTQ_LEAVE(ercd, *p_data);
 621     return(ercd);
 622 }
 623
 624 #endif /* __trcv_dtq */