pios_queue.c

Go to the documentation of this file.

/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, see <http://www.gnu.org/licenses/>
 */

#include "pios.h"
#include "pios_queue.h"

#if !defined(PIOS_INCLUDE_CHIBIOS)
#error "pios_queue.c requires PIOS_INCLUDE_CHIBIOS"
#endif

#if defined(PIOS_INCLUDE_CHIBIOS)

#include "ch.h"

struct pios_queue
{
        Mailbox mb;
        MemoryPool mp;
        void *mpb;
};

#if !defined(PIOS_QUEUE_MAX_WAITERS)
#define PIOS_QUEUE_MAX_WAITERS 2
#endif /* !defined(PIOS_QUEUE_MAX_WAITERS) */

struct pios_queue *PIOS_Queue_Create(size_t queue_length, size_t item_size)
{
        struct pios_queue *queuep = PIOS_malloc_no_dma(sizeof(struct pios_queue));
        if (queuep == NULL)
                return NULL;

        /* Create the memory pool. */
        queuep->mpb = PIOS_malloc_no_dma(item_size * (queue_length + PIOS_QUEUE_MAX_WAITERS));
        if (queuep->mpb == NULL) {
                PIOS_free(queuep);
                return NULL;
        }
        chPoolInit(&queuep->mp, item_size, NULL);
        chPoolLoadArray(&queuep->mp, queuep->mpb, queue_length + PIOS_QUEUE_MAX_WAITERS);

        /* Create the mailbox. */
        msg_t *mb_buf = PIOS_malloc_no_dma(sizeof(msg_t) * queue_length);
        chMBInit(&queuep->mb, mb_buf, queue_length);

        return queuep;
}

void PIOS_Queue_Delete(struct pios_queue *queuep)
{
        PIOS_free(queuep->mpb);
        PIOS_free(queuep);
}

bool PIOS_Queue_Send(struct pios_queue *queuep, const void *itemp, uint32_t timeout_ms)
{
        void *buf = chPoolAlloc(&queuep->mp);
        if (buf == NULL)
                return false;

        memcpy(buf, itemp, queuep->mp.mp_object_size);

        systime_t timeout;
        if (timeout_ms == PIOS_QUEUE_TIMEOUT_MAX)
                timeout = TIME_INFINITE;
        else if (timeout_ms == 0)
                timeout = TIME_IMMEDIATE;
        else
                timeout = MS2ST(timeout_ms);

        msg_t result = chMBPost(&queuep->mb, (msg_t)buf, timeout);

        if (result != RDY_OK)
        {
                chPoolFree(&queuep->mp, buf);
                return false;
        }

        return true;
}

bool PIOS_Queue_Send_FromISR(struct pios_queue *queuep, const void *itemp, bool *wokenp)
{
        chSysLockFromIsr();
        void *buf = chPoolAllocI(&queuep->mp);
        if (buf == NULL)
        {
                chSysUnlockFromIsr();
                return false;
        }

        memcpy(buf, itemp, queuep->mp.mp_object_size);

        msg_t result = chMBPostI(&queuep->mb, (msg_t)buf);

        if (result != RDY_OK)
        {
                chPoolFreeI(&queuep->mp, buf);
                chSysUnlockFromIsr();
                return false;
        }

        chSysUnlockFromIsr();

        return true;
}

bool PIOS_Queue_Receive(struct pios_queue *queuep, void *itemp, uint32_t timeout_ms)
{
        msg_t buf;

        systime_t timeout;
        if (timeout_ms == PIOS_QUEUE_TIMEOUT_MAX)
                timeout = TIME_INFINITE;
        else if (timeout_ms == 0)
                timeout = TIME_IMMEDIATE;
        else
                timeout = MS2ST(timeout_ms);

        msg_t result = chMBFetch(&queuep->mb, &buf, timeout);

        if (result != RDY_OK)
                return false;

        memcpy(itemp, (void*)buf, queuep->mp.mp_object_size);

        chPoolFree(&queuep->mp, (void*)buf);

        return true;
}

/*
 * @brief Gets the item size of a queue.
 *
 * @param[in] queuep Pointer to instance of @p struct pios_queue
 *
 * @returns Size of item
 *
 */
size_t PIOS_Queue_GetItemSize(struct pios_queue *queuep)
{
        PIOS_Assert(queuep);
        return queuep->mp.mp_object_size;
}

#endif /* defined(PIOS_INCLUDE_CHIBIOS) */