source: mypx/kernel/mailbox.c

/*
 *  TOPPERS/ASP Kernel
 *      Toyohashi Open Platform for Embedded Real-Time Systems/
 *      Advanced Standard Profile Kernel
 * 
 *  Copyright (C) 2000-2003 by Embedded and Real-Time Systems Laboratory
 *                              Toyohashi Univ. of Technology, JAPAN
 *  Copyright (C) 2005-2011 by Embedded and Real-Time Systems Laboratory
 *              Graduate School of Information Science, Nagoya Univ., JAPAN
 * 
 *  Ÿåµ­Ãøºîž¢ŒÔ€Ï¡€°Ê²Œ€Î(1)¡Á(4)€ÎŸò·ï€òËþ€¿€¹Ÿì¹ç€ËžÂ€ê¡€ËÜ¥œ¥Õ¥È¥Š¥§
 *  ¥¢¡ÊËÜ¥œ¥Õ¥È¥Š¥§¥¢€ò²þÊÑ€·€¿€â€Î€òŽÞ€à¡¥°Ê²ŒÆ±€ž¡Ë€ò»ÈÍÑ¡ŠÊ£Àœ¡Š²þ
 *  ÊÑ¡ŠºÆÇÛÉۡʰʲŒ¡€ÍøÍрȞƀ֡ˀ¹€ë€³€È€ò̵œþ€ÇµöÂú€¹€ë¡¥
 *  (1) ËÜ¥œ¥Õ¥È¥Š¥§¥¢€ò¥œ¡Œ¥¹¥³¡Œ¥É€Î·Á€ÇÍøÍÑ€¹€ëŸì¹ç€Ë€Ï¡€Ÿåµ­€ÎÃøºî
 *      ž¢ÉœŒš¡€€³€ÎÍøÍÑŸò·ï€ª€è€Ó²Œµ­€Î̵ÊÝŸÚµ¬Äꀬ¡€€œ€Î€Þ€Þ€Î·Á€Ç¥œ¡Œ
 *      ¥¹¥³¡Œ¥ÉÃæ€ËŽÞ€Þ€ì€Æ€€€ë€³€È¡¥
 *  (2) ËÜ¥œ¥Õ¥È¥Š¥§¥¢€ò¡€¥é¥€¥Ö¥é¥ê·ÁŒ°€Ê€É¡€ÂŸ€Î¥œ¥Õ¥È¥Š¥§¥¢³«È¯€Ë»È
 *      Íрǀ­€ë·Á€ÇºÆÇÛÉÛ€¹€ëŸì¹ç€Ë€Ï¡€ºÆÇÛÉÛ€ËÈŒ€Š¥É¥­¥å¥á¥ó¥È¡ÊÍøÍÑ
 *      ŒÔ¥Þ¥Ë¥å¥¢¥ë€Ê€É¡Ë€Ë¡€Ÿåµ­€ÎÃøºîž¢ÉœŒš¡€€³€ÎÍøÍÑŸò·ï€ª€è€Ó²Œµ­
 *      €Î̵ÊÝŸÚµ¬Äê€ò·ÇºÜ€¹€ë€³€È¡¥
 *  (3) ËÜ¥œ¥Õ¥È¥Š¥§¥¢€ò¡€µ¡Žï€ËÁȀ߹þ€à€Ê€É¡€ÂŸ€Î¥œ¥Õ¥È¥Š¥§¥¢³«È¯€Ë»È
 *      Íрǀ­€Ê€€·Á€ÇºÆÇÛÉÛ€¹€ëŸì¹ç€Ë€Ï¡€Œ¡€Î€€€º€ì€«€ÎŸò·ï€òËþ€¿€¹€³
 *      €È¡¥
 *    (a) ºÆÇÛÉÛ€ËÈŒ€Š¥É¥­¥å¥á¥ó¥È¡ÊÍøÍьԥޥ˥奢¥ë€Ê€É¡Ë€Ë¡€Ÿåµ­€ÎÃø
 *        ºîž¢ÉœŒš¡€€³€ÎÍøÍÑŸò·ï€ª€è€Ó²Œµ­€Î̵ÊÝŸÚµ¬Äê€ò·ÇºÜ€¹€ë€³€È¡¥
 *    (b) ºÆÇÛÉۀηÁÂÖ€ò¡€ÊÌ€ËÄê€á€ëÊýË¡€Ë€è€Ã€Æ¡€TOPPERS¥×¥í¥ž¥§¥¯¥È€Ë
 *        Êó¹ð€¹€ë€³€È¡¥
 *  (4) ËÜ¥œ¥Õ¥È¥Š¥§¥¢€ÎÍøÍрˀè€êÄŸÀÜŪ€Þ€¿€ÏŽÖÀÜŪ€ËÀž€ž€ë€€€«€Ê€ë»
 *      ³²€«€é€â¡€Ÿåµ­Ãøºîž¢ŒÔ€ª€è€ÓTOPPERS¥×¥í¥ž¥§¥¯¥È€òÌÈÀÕ€¹€ë€³€È¡¥
 *      €Þ€¿¡€ËÜ¥œ¥Õ¥È¥Š¥§¥¢€Î¥æ¡Œ¥¶€Þ€¿€Ï¥š¥ó¥É¥æ¡Œ¥¶€«€é€Î€€€«€Ê€ëÍý
 *      ͳ€ËŽð€Å€¯ÀÁµá€«€é€â¡€Ÿåµ­Ãøºîž¢ŒÔ€ª€è€ÓTOPPERS¥×¥í¥ž¥§¥¯¥È€ò
 *      ÌÈÀÕ€¹€ë€³€È¡¥
 * 
 *  ËÜ¥œ¥Õ¥È¥Š¥§¥¢€Ï¡€ÌµÊÝŸÚ€ÇÄ󶡀µ€ì€Æ€€€ë€â€Î€Ç€¢€ë¡¥Ÿåµ­Ãøºîž¢ŒÔ€ª
 *  €è€ÓTOPPERS¥×¥í¥ž¥§¥¯¥È€Ï¡€ËÜ¥œ¥Õ¥È¥Š¥§¥¢€ËŽØ€·€Æ¡€ÆÃÄê€Î»ÈÍÑÌÜŪ
 *  €ËÂЀ¹€ëŬ¹çÀ­€âŽÞ€á€Æ¡€€€€«€Ê€ëÊÝŸÚ€â¹Ô€ï€Ê€€¡¥€Þ€¿¡€ËÜ¥œ¥Õ¥È¥Š¥§
 *  ¥¢€ÎÍøÍрˀè€êÄŸÀÜŪ€Þ€¿€ÏŽÖÀÜŪ€ËÀž€ž€¿€€€«€Ê€ë»³²€ËŽØ€·€Æ€â¡€€œ
 *  €ÎÀÕÇ€€òÉé€ï€Ê€€¡¥
 * 
 *  @(#) $Id$
 */

/*
 *              ¥á¡Œ¥ë¥Ü¥Ã¥¯¥¹µ¡Çœ
 */

#include "kernel_impl.h"
#include "check.h"
#include "task.h"
#include "wait.h"
#include "mailbox.h"

/*
 *  ¥È¥ì¡Œ¥¹¥í¥°¥Þ¥¯¥í€Î¥Ç¥Õ¥©¥ë¥ÈÄêµÁ
 */
#ifndef LOG_SND_MBX_ENTER
#define LOG_SND_MBX_ENTER(mbxid, pk_msg)
#endif /* LOG_SND_MBX_ENTER */

#ifndef LOG_SND_MBX_LEAVE
#define LOG_SND_MBX_LEAVE(ercd)
#endif /* LOG_SND_MBX_LEAVE */

#ifndef LOG_RCV_MBX_ENTER
#define LOG_RCV_MBX_ENTER(mbxid, ppk_msg)
#endif /* LOG_RCV_MBX_ENTER */

#ifndef LOG_RCV_MBX_LEAVE
#define LOG_RCV_MBX_LEAVE(ercd, pk_msg)
#endif /* LOG_RCV_MBX_LEAVE */

#ifndef LOG_PRCV_MBX_ENTER
#define LOG_PRCV_MBX_ENTER(mbxid, ppk_msg)
#endif /* LOG_PRCV_MBX_ENTER */

#ifndef LOG_PRCV_MBX_LEAVE
#define LOG_PRCV_MBX_LEAVE(ercd, pk_msg)
#endif /* LOG_PRCV_MBX_LEAVE */

#ifndef LOG_TRCV_MBX_ENTER
#define LOG_TRCV_MBX_ENTER(mbxid, ppk_msg, tmout)
#endif /* LOG_TRCV_MBX_ENTER */

#ifndef LOG_TRCV_MBX_LEAVE
#define LOG_TRCV_MBX_LEAVE(ercd, pk_msg)
#endif /* LOG_TRCV_MBX_LEAVE */

#ifndef LOG_INI_MBX_ENTER
#define LOG_INI_MBX_ENTER(mbxid)
#endif /* LOG_INI_MBX_ENTER */

#ifndef LOG_INI_MBX_LEAVE
#define LOG_INI_MBX_LEAVE(ercd)
#endif /* LOG_INI_MBX_LEAVE */

#ifndef LOG_REF_MBX_ENTER
#define LOG_REF_MBX_ENTER(mbxid, pk_rmbx)
#endif /* LOG_REF_MBX_ENTER */

#ifndef LOG_REF_MBX_LEAVE
#define LOG_REF_MBX_LEAVE(ercd, pk_rmbx)
#endif /* LOG_REF_MBX_LEAVE */

/*
 *  ¥á¡Œ¥ë¥Ü¥Ã¥¯¥¹€Î¿ô
 */
#define tnum_mbx        ((uint_t)(tmax_mbxid - TMIN_MBXID + 1))

/*
 *  ¥á¡Œ¥ë¥Ü¥Ã¥¯¥¹ID€«€é¥á¡Œ¥ë¥Ü¥Ã¥¯¥¹ŽÉÍý¥Ö¥í¥Ã¥¯€òŒè€êœÐ€¹€¿€á€Î¥Þ¥¯¥í
 */
#define INDEX_MBX(mbxid)        ((uint_t)((mbxid) - TMIN_MBXID))
#define get_mbxcb(mbxid)        (&(mbxcb_table[INDEX_MBX(mbxid)]))

/* 
 *  ¥á¡Œ¥ë¥Ü¥Ã¥¯¥¹µ¡Çœ€ÎœéŽü²œ
 */
#ifdef TOPPERS_mbxini

void
initialize_mailbox(void)
{
        uint_t  i;
        MBXCB   *p_mbxcb;

        for (i = 0; i < tnum_mbx; i++) {
                p_mbxcb = &(mbxcb_table[i]);
                queue_initialize(&(p_mbxcb->wait_queue));
                p_mbxcb->p_mbxinib = &(mbxinib_table[i]);
                p_mbxcb->pk_head = NULL;
        }
}

#endif /* TOPPERS_mbxini */

/*
 *  ¥á¥Ã¥»¡Œ¥žÍ¥ÀèÅـΌèœÐ€·
 */
#define MSGPRI(pk_msg)  (((T_MSG_PRI *)(pk_msg))->msgpri)

/*
 *  Í¥ÀèÅÙœç¥á¥Ã¥»¡Œ¥ž¥­¥å¡Œ€Ø€ÎÁÞÆþ
 */
Inline void
enqueue_msg_pri(T_MSG **ppk_prevmsg_next, T_MSG *pk_msg)
{
        T_MSG   *pk_nextmsg;

        while ((pk_nextmsg = *ppk_prevmsg_next) != NULL) {
                if (MSGPRI(pk_nextmsg) > MSGPRI(pk_msg)) {
                        break;
                }
                ppk_prevmsg_next = &(pk_nextmsg->pk_next);
        }
        pk_msg->pk_next = pk_nextmsg;
        *ppk_prevmsg_next = pk_msg;
}

/*
 *  ¥á¡Œ¥ë¥Ü¥Ã¥¯¥¹€Ø€ÎÁ÷¿®
 */
#ifdef TOPPERS_snd_mbx

ER
snd_mbx(ID mbxid, T_MSG *pk_msg)
{
        MBXCB   *p_mbxcb;
        TCB             *p_tcb;
        ER              ercd;
    
        LOG_SND_MBX_ENTER(mbxid, pk_msg);
        CHECK_TSKCTX_UNL();
        CHECK_MBXID(mbxid);
        p_mbxcb = get_mbxcb(mbxid);
        CHECK_PAR((p_mbxcb->p_mbxinib->mbxatr & TA_MPRI) == 0U
                                || (TMIN_MPRI <= MSGPRI(pk_msg)
                                        && MSGPRI(pk_msg) <= p_mbxcb->p_mbxinib->maxmpri));

        t_lock_cpu();
        if (!queue_empty(&(p_mbxcb->wait_queue))) {
                p_tcb = (TCB *) queue_delete_next(&(p_mbxcb->wait_queue));
                ((WINFO_MBX *)(p_tcb->p_winfo))->pk_msg = pk_msg;
                if (wait_complete(p_tcb)) {
                        dispatch();
                }
                ercd = E_OK;
        }
        else if ((p_mbxcb->p_mbxinib->mbxatr & TA_MPRI) != 0U) {
                enqueue_msg_pri(&(p_mbxcb->pk_head), pk_msg);
                ercd = E_OK;
        }
        else {
                pk_msg->pk_next = NULL;
                if (p_mbxcb->pk_head != NULL) {
                        p_mbxcb->pk_last->pk_next = pk_msg;
                }
                else {
                        p_mbxcb->pk_head = pk_msg;
                }
                p_mbxcb->pk_last = pk_msg;
                ercd = E_OK;
        }
        t_unlock_cpu();

  error_exit:
        LOG_SND_MBX_LEAVE(ercd);
        return(ercd);
}

#endif /* TOPPERS_snd_mbx */

/*
 *  ¥á¡Œ¥ë¥Ü¥Ã¥¯¥¹€«€é€ÎŒõ¿®
 */
#ifdef TOPPERS_rcv_mbx

ER
rcv_mbx(ID mbxid, T_MSG **ppk_msg)
{
        MBXCB   *p_mbxcb;
        WINFO_MBX winfo_mbx;
        ER              ercd;
    
        LOG_RCV_MBX_ENTER(mbxid, ppk_msg);
        CHECK_DISPATCH();
        CHECK_MBXID(mbxid);
        p_mbxcb = get_mbxcb(mbxid);
    
        t_lock_cpu();
        if (p_mbxcb->pk_head != NULL) {
                *ppk_msg = p_mbxcb->pk_head;
                p_mbxcb->pk_head = (*ppk_msg)->pk_next;
                ercd = E_OK;
        }
        else {
                p_runtsk->tstat = (TS_WAITING | TS_WAIT_MBX);
                wobj_make_wait((WOBJCB *) p_mbxcb, (WINFO_WOBJ *) &winfo_mbx);
                dispatch();
                ercd = winfo_mbx.winfo.wercd;
                if (ercd == E_OK) {
                        *ppk_msg = winfo_mbx.pk_msg;
                }
        }
        t_unlock_cpu();

  error_exit:
        LOG_RCV_MBX_LEAVE(ercd, *ppk_msg);
        return(ercd);
}

#endif /* TOPPERS_rcv_mbx */

/*
 *  ¥á¡Œ¥ë¥Ü¥Ã¥¯¥¹€«€é€ÎŒõ¿®¡Ê¥Ý¡Œ¥ê¥ó¥°¡Ë
 */
#ifdef TOPPERS_prcv_mbx

ER
prcv_mbx(ID mbxid, T_MSG **ppk_msg)
{
        MBXCB   *p_mbxcb;
        ER              ercd;
    
        LOG_PRCV_MBX_ENTER(mbxid, ppk_msg);
        CHECK_TSKCTX_UNL();
        CHECK_MBXID(mbxid);
        p_mbxcb = get_mbxcb(mbxid);
    
        t_lock_cpu();
        if (p_mbxcb->pk_head != NULL) {
                *ppk_msg = p_mbxcb->pk_head;
                p_mbxcb->pk_head = (*ppk_msg)->pk_next;
                ercd = E_OK;
        }
        else {
                ercd = E_TMOUT;
        }
        t_unlock_cpu();

  error_exit:
        LOG_PRCV_MBX_LEAVE(ercd, *ppk_msg);
        return(ercd);
}

#endif /* TOPPERS_prcv_mbx */

/*
 *  ¥á¡Œ¥ë¥Ü¥Ã¥¯¥¹€«€é€ÎŒõ¿®¡Ê¥¿¥€¥à¥¢¥Š¥È€¢€ê¡Ë
 */
#ifdef TOPPERS_trcv_mbx

ER
trcv_mbx(ID mbxid, T_MSG **ppk_msg, TMO tmout)
{
        MBXCB   *p_mbxcb;
        WINFO_MBX winfo_mbx;
        TMEVTB  tmevtb;
        ER              ercd;
    
        LOG_TRCV_MBX_ENTER(mbxid, ppk_msg, tmout);
        CHECK_DISPATCH();
        CHECK_MBXID(mbxid);
        CHECK_TMOUT(tmout);
        p_mbxcb = get_mbxcb(mbxid);
    
        t_lock_cpu();
        if (p_mbxcb->pk_head != NULL) {
                *ppk_msg = p_mbxcb->pk_head;
                p_mbxcb->pk_head = (*ppk_msg)->pk_next;
                ercd = E_OK;
        }
        else if (tmout == TMO_POL) {
                ercd = E_TMOUT;
        }
        else {
                p_runtsk->tstat = (TS_WAITING | TS_WAIT_MBX);
                wobj_make_wait_tmout((WOBJCB *) p_mbxcb, (WINFO_WOBJ *) &winfo_mbx,
                                                                                                                &tmevtb, tmout);
                dispatch();
                ercd = winfo_mbx.winfo.wercd;
                if (ercd == E_OK) {
                        *ppk_msg = winfo_mbx.pk_msg;
                }
        }
        t_unlock_cpu();

  error_exit:
        LOG_TRCV_MBX_LEAVE(ercd, *ppk_msg);
        return(ercd);
}

#endif /* TOPPERS_trcv_mbx */

/*
 *  ¥á¡Œ¥ë¥Ü¥Ã¥¯¥¹€ÎºÆœéŽü²œ
 */
#ifdef TOPPERS_ini_mbx

ER
ini_mbx(ID mbxid)
{
        MBXCB   *p_mbxcb;
        bool_t  dspreq;
        ER              ercd;
    
        LOG_INI_MBX_ENTER(mbxid);
        CHECK_TSKCTX_UNL();
        CHECK_MBXID(mbxid);
        p_mbxcb = get_mbxcb(mbxid);

        t_lock_cpu();
        dspreq = init_wait_queue(&(p_mbxcb->wait_queue));
        p_mbxcb->pk_head = NULL;
        if (dspreq) {
                dispatch();
        }
        ercd = E_OK;
        t_unlock_cpu();

  error_exit:
        LOG_INI_MBX_LEAVE(ercd);
        return(ercd);
}

#endif /* TOPPERS_ini_mbx */

/*
 *  ¥á¡Œ¥ë¥Ü¥Ã¥¯¥¹€ÎŸõÂÖ»²ŸÈ
 */
#ifdef TOPPERS_ref_mbx

ER
ref_mbx(ID mbxid, T_RMBX *pk_rmbx)
{
        MBXCB   *p_mbxcb;
        ER              ercd;
    
        LOG_REF_MBX_ENTER(mbxid, pk_rmbx);
        CHECK_TSKCTX_UNL();
        CHECK_MBXID(mbxid);
        p_mbxcb = get_mbxcb(mbxid);

        t_lock_cpu();
        pk_rmbx->wtskid = wait_tskid(&(p_mbxcb->wait_queue));
        pk_rmbx->pk_msg = p_mbxcb->pk_head;
        ercd = E_OK;
        t_unlock_cpu();

  error_exit:
        LOG_REF_MBX_LEAVE(ercd, pk_rmbx);
        return(ercd);
}

#endif /* TOPPERS_ref_mbx */