IPCQueue.cpp

Go to the documentation of this file.

#include <Fw/Types/Assert.hpp>
#include <Os/Queue.hpp>
#include <Os/IPCQueue.hpp>
 
#ifdef TGT_OS_TYPE_VXWORKS
    #include <vxWorks.h>
#endif
 
#ifdef TGT_OS_TYPE_LINUX
    #include <sys/types.h>
    #include <unistd.h>
#endif
 
#include <mqueue.h>
#include <fcntl.h>
#include <errno.h>
#include <string.h>
#include <stdio.h>
#include <time.h>
#include <sys/time.h>
#include <pthread.h>
 
#define IPC_QUEUE_TIMEOUT_SEC (1)
 
namespace Os {
 
    class QueueHandle {
        public:
        QueueHandle(mqd_t m_handle) {
            this->handle = m_handle;
        }
        ~QueueHandle() {
            // Destroy the handle:
            if (-1 != this->handle) {
                (void) mq_close(this->handle);
            }
        }
        mqd_t handle;
    };
 
    IPCQueue::IPCQueue() : Queue() {
    }
 
    Queue::QueueStatus IPCQueue::create(const Fw::StringBase &name, NATIVE_INT_TYPE depth, NATIVE_INT_TYPE msgSize) {
 
        this->m_name = "/QP_";
        this->m_name += name;
#ifndef TGT_OS_TYPE_VXWORKS
        char pid[40];
        (void)snprintf(pid,sizeof(pid),".%d",getpid());
        pid[sizeof(pid)-1] = 0;
        this->m_name += pid;
#endif
        mq_attr att;
        mqd_t handle;
 
        memset(&att,0,sizeof(att));
        att.mq_maxmsg = depth;
        att.mq_msgsize = msgSize;
        att.mq_flags = 0;
        att.mq_curmsgs = 0;
 
        /* NOTE(mereweth) - O_BLOCK is the default; we use timedsend and
         * timedreceive below if QUEUE_NONBLOCKING is specified
         *
         */
        handle = mq_open(this->m_name.toChar(), O_RDWR | O_CREAT | O_EXCL, 0666, &att);
 
        // If queue already exists, then unlink it and try again.
        if (-1 == (NATIVE_INT_TYPE) handle) {
            switch (errno) {
                case EEXIST:
                    (void)mq_unlink(this->m_name.toChar());
                    break;
                default:
                    return QUEUE_UNINITIALIZED;
            }
 
            handle = mq_open(this->m_name.toChar(), O_RDWR | O_CREAT | O_EXCL, 0666, &att);
 
            if (-1 == (NATIVE_INT_TYPE) handle) {
                return QUEUE_UNINITIALIZED;
            }
        }
 
        // Set up queue handle:
        QueueHandle* queueHandle = new QueueHandle(handle);
        if (NULL == queueHandle) {
          return QUEUE_UNINITIALIZED;
        }
        this->m_handle = (POINTER_CAST) queueHandle;
 
        Queue::s_numQueues++;
 
        return QUEUE_OK;
    }
 
    IPCQueue::~IPCQueue() {
        // Clean up the queue handle:
        QueueHandle* queueHandle = (QueueHandle*) this->m_handle;
        if (NULL != queueHandle) {
          delete queueHandle;
        }
        this->m_handle = (POINTER_CAST) NULL; // important so base Queue class doesn't free it
        (void) mq_unlink(this->m_name.toChar());
    }
 
    Queue::QueueStatus IPCQueue::send(const U8* buffer, NATIVE_INT_TYPE size, NATIVE_INT_TYPE priority, QueueBlocking block) {
 
        QueueHandle* queueHandle = (QueueHandle*) this->m_handle;
        mqd_t handle = queueHandle->handle;
 
        if (-1 == handle) {
            return QUEUE_UNINITIALIZED;
        }
 
        if (NULL == buffer) {
            return QUEUE_EMPTY_BUFFER;
        }
 
        bool keepTrying = true;
        while (keepTrying) {
            struct timeval now;
            gettimeofday(&now,NULL);
            struct timespec wait;
            wait.tv_sec = now.tv_sec;
            wait.tv_nsec = now.tv_usec * 1000;
 
            if (block == QUEUE_BLOCKING) {
                wait.tv_sec += IPC_QUEUE_TIMEOUT_SEC;
            }
            NATIVE_INT_TYPE stat = mq_timedsend(handle, (const char*) buffer, size, priority, &wait);
            if (-1 == stat) {
                switch (errno) {
                    case EINTR:
                        continue;
                    case EMSGSIZE:
                        return QUEUE_SIZE_MISMATCH;
                    case EINVAL:
                        return QUEUE_INVALID_PRIORITY;
                    case ETIMEDOUT:
                        if (block == QUEUE_NONBLOCKING) {
                            // no more messages. If we are
                            // non-blocking, return
                            return QUEUE_FULL;
                        } else {
                            // TODO(mereweth) - multiprocess signalling necessary?
                            // Go to sleep until we receive a signal that something was taken off the queue
                            continue;
                        }
                    default:
                        return QUEUE_UNKNOWN_ERROR;
                }
            } else {
                keepTrying=false;
                // TODO(mereweth) - multiprocess signalling necessary?
                // Wake up a thread that might be waiting on the other end of the queue:
            }
        }
 
        return QUEUE_OK;
    }
 
    Queue::QueueStatus IPCQueue::receive(U8* buffer, NATIVE_INT_TYPE capacity, NATIVE_INT_TYPE &actualSize, NATIVE_INT_TYPE &priority, QueueBlocking block) {
 
        QueueHandle* queueHandle = (QueueHandle*) this->m_handle;
        mqd_t handle = queueHandle->handle;
 
        if (-1 == handle) {
            return QUEUE_UNINITIALIZED;
        }
 
        ssize_t size;
        bool notFinished = true;
        while (notFinished) {
            struct timeval now;
            gettimeofday(&now,NULL);
            struct timespec wait;
            wait.tv_sec = now.tv_sec;
            wait.tv_nsec = now.tv_usec * 1000;
 
            if (block == QUEUE_BLOCKING) {
                wait.tv_sec += IPC_QUEUE_TIMEOUT_SEC;
            }
            size = mq_timedreceive(handle, (char*) buffer, (size_t) capacity,
#ifdef TGT_OS_TYPE_VXWORKS
                        (int*)&priority, &wait);
#else
                        (unsigned int*) &priority, &wait);
#endif
 
            if (-1 == size) { // error
                switch (errno) {
                    case EINTR:
                        continue;
                    case EMSGSIZE:
                        return QUEUE_SIZE_MISMATCH;
                    case ETIMEDOUT:
                        if (block == QUEUE_NONBLOCKING) {
                            // no more messages. If we are
                            // non-blocking, return
                            return QUEUE_NO_MORE_MSGS;
                        } else {
                            // TODO(mereweth) - multiprocess signalling necessary?
                            // Go to sleep until we receive a signal that something was put on the queue:
                            continue;
                        }
                        break;
                    default:
                        return QUEUE_UNKNOWN_ERROR;
                }
            }
            else {
                notFinished = false;
                // TODO(mereweth) - multiprocess signalling necessary?
                // Wake up a thread that might be waiting on the other end of the queue
            }
        }
 
        actualSize = (NATIVE_INT_TYPE) size;
        return QUEUE_OK;
    }
 
    NATIVE_INT_TYPE IPCQueue::getNumMsgs(void) const {
        QueueHandle* queueHandle = (QueueHandle*) this->m_handle;
        mqd_t handle = queueHandle->handle;
 
        struct mq_attr attr;
        int status = mq_getattr(handle, &attr);
        FW_ASSERT(status == 0);
        return (U32) attr.mq_curmsgs;
    }
 
    NATIVE_INT_TYPE IPCQueue::getMaxMsgs(void) const {
        //FW_ASSERT(0);
        return 0;
    }
 
    NATIVE_INT_TYPE IPCQueue::getQueueSize(void) const {
        QueueHandle* queueHandle = (QueueHandle*) this->m_handle;
        mqd_t handle = queueHandle->handle;
 
        struct mq_attr attr;
        int status = mq_getattr(handle, &attr);
        FW_ASSERT(status == 0);
        return (U32) attr.mq_maxmsg;
    }
 
    NATIVE_INT_TYPE IPCQueue::getMsgSize(void) const {
        QueueHandle* queueHandle = (QueueHandle*) this->m_handle;
        mqd_t handle = queueHandle->handle;
 
        struct mq_attr attr;
        int status = mq_getattr(handle, &attr);
        FW_ASSERT(status == 0);
        return (U32) attr.mq_msgsize;
    }
 
}