www.pudn.com > r&s1.10°æ±¾£«Îĵµ.rar > msgq.c

/* 
=============================================================================== 
| Copyright (C) 2004 RuanHaiShen, All rights reserved. 
| SUMMARY:  
|   Message queue implementation 
| 
| DESCRIPTION: 
|   See http://www.01s.org for documentation, latest information, license  
|   and contact details. 
|   email:ruanhaishen@01s.org 
=============================================================================*/ 
/*===========================================================================*/ 
#include "arch/arch.h" 
#include "inc/queue.h" 
#include "inc/kernel.h" 
#include "inc/memory.h" 
#include "inc/ipc.h" 
#include "inc/kapi.h" 
 
 
#if CFG_MSGQ_EN > 0  
#define __msgq_take_head(msgq, buf) \ 
do { \ 
    __memcpy(buf, msgq->phead, msgq->msgsz); \ 
    msgq->phead += msgq->msgsz; \ 
    if (msgq->phead == msgq->pend) { \ 
        msgq->phead = msgq->pstart; \ 
    } \ 
} while (0) 
 
#define __msgq_take_tail(msgq, buf) \ 
do { \ 
    if (msgq->ptail == msgq->pstart) { \ 
        msgq->ptail = msgq->pend; \ 
    } \ 
    msgq->ptail -= msgq->msgsz; \ 
    __memcpy(buf, msgq->ptail, msgq->msgsz); \ 
} while (0) 
 
 
err_t msgq_init(msgq_t* msgq, u16 size, u16 entries, u8 opt) 
{ 
    __ASSERT(msgq != NULL); 
    __ASSERT(size > 0); 
    __ASSERT(entries > 0); 
 
    msgq->pstart = kmalloc(size * entries); 
    if (msgq->pstart == NULL) { 
        return ENULL; 
    } 
    msgq->phead = 
        msgq->ptail = msgq->pstart; 
    msgq->pend      = msgq->pstart + size * entries; 
    msgq->msgsz     = size; 
    msgq->entries   = entries; 
    msgq->opt       = opt; 
    msgq->avail     = 0; 
    __queue_init(msgq->tasks); 
 
    return EOK; 
} 
 
 
err_t msgq_send(msgq_t* msgq, const void* buf) 
{ 
    __ASSERT(msgq != NULL); 
    __ASSERT(buf != NULL); 
 
    CRITICAL_ENTER; 
    if (msgq->avail < msgq->entries) { 
        __memcpy(msgq->ptail, buf, msgq->msgsz); 
        msgq->ptail += msgq->msgsz; 
        if (msgq->ptail == msgq->pend) { 
            msgq->ptail = msgq->pstart; 
        } 
        if (msgq->avail++ == 0) { 
            __ipc_resume(&msgq->tasks); 
            CRITICAL_EXIT; 
            __schedule(); 
        } else { 
            CRITICAL_EXIT; 
        } 
        return EOK; 
    } else { 
        CRITICAL_EXIT; 
        return EFULL; 
    } 
} 
 
 
err_t msgq_receive(msgq_t* msgq, void* buf) 
{ 
    __ASSERT(msgq != NULL); 
    __ASSERT(buf != NULL); 
  
#if CFG_ARG_CHK > 0 
    if (_sched_lock > 0) { 
        return ELOCK; 
    } 
#else 
    __ASSERT(_sched_lock == 0); 
#endif 
 
    while (true) { 
        CRITICAL_ENTER; 
        if (msgq->avail > 0) { 
            msgq->avail--; 
            if (msgq->opt == MSG_FIFO) { 
                __msgq_take_head(msgq, buf); 
            } else { 
                __msgq_take_tail(msgq, buf); 
            } 
            CRITICAL_EXIT; 
            return EOK; 
        } 
        __ipc_block(&msgq->tasks, 0); 
        CRITICAL_EXIT; 
 
        __schedule(); 
    } 
} 
 
 
#if CFG_IPC_TIMEOUT_EN > 0 
err_t msgq_timereceive(msgq_t* msgq, void* buf, tick_t ticks) 
{ 
    register u8 current; 
 
    __ASSERT(msgq != NULL); 
    __ASSERT(buf != NULL); 
   
#if CFG_ARG_CHK > 0 
    if (_sched_lock > 0) { 
        return ELOCK; 
    } 
#else 
    __ASSERT(_sched_lock == 0); 
#endif 
 
    while (true) { 
        CRITICAL_ENTER; 
        if (msgq->avail > 0) { 
            msgq->avail--; 
            if (msgq->opt == MSG_FIFO) { 
                __msgq_take_head(msgq, buf); 
            } else { 
                __msgq_take_tail(msgq, buf); 
            } 
            CRITICAL_EXIT; 
            return EOK; 
        } 
        __ipc_block(&msgq->tasks, ticks); 
        CRITICAL_EXIT; 
 
        __schedule(); 
 
        CRITICAL_ENTER; 
        current = _current_prio; 
        if (_tasks[current].state & STATE_BLOCKED) { 
            __ipc_timeout(&msgq->tasks); 
            CRITICAL_EXIT; 
            return ETIMEOUT; 
        } 
        __adjust_delay(current, ticks); 
        CRITICAL_EXIT; 
    } 
} 
#endif 
 
 
err_t msgq_flush(msgq_t* msgq) 
{ 
    __ASSERT(msgq != NULL); 
 
    CRITICAL_ENTER; 
    msgq->phead = 
        msgq->ptail = msgq->pstart; 
    msgq->avail = 0; 
    CRITICAL_EXIT; 
 
    return EOK; 
} 
 
 
err_t msgq_destroy(msgq_t* msgq) 
{ 
    __ASSERT(msgq != NULL); 
 
    CRITICAL_ENTER; 
    if (__ipc_resume(&msgq->tasks) != NULL_PRIO) { 
        CRITICAL_EXIT; 
        return EEXIST; 
    } 
    CRITICAL_EXIT; 
 
    CRITICAL_ENTER; 
    if (msgq->pstart != NULL) { 
        kfree(msgq->pstart); 
        msgq->pstart = NULL; 
    } 
    CRITICAL_EXIT; 
 
    return EOK; 
} 
#endif 
 
 
/*===========================================================================*/