esp-idf/components/rt/FreeRTOS_POSIX_mqueue.c

/*
 * Amazon FreeRTOS+POSIX V1.0.0
 * Copyright (C) 2018 Amazon.com, Inc. or its affiliates.  All Rights Reserved.
 *
 * SPDX-FileCopyrightText: 2018 Amazon.com, Inc. or its affiliates
 *
 * SPDX-License-Identifier: MIT
 *
 * SPDX-FileContributor: 2024 Espressif Systems (Shanghai) CO LTD
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of
 * this software and associated documentation files (the "Software"), to deal in
 * the Software without restriction, including without limitation the rights to
 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
 * the Software, and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
 * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
 * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
 * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 *
 * http://aws.amazon.com/freertos
 * http://www.FreeRTOS.org
 */

/**
 * @file FreeRTOS_POSIX_mqueue.c
 * @brief Implementation of message queue functions in mqueue.h
 */

/* C standard library includes. */
#include <string.h>

#include <errno.h>
#include <fcntl.h>
#include <mqueue.h>
#include <stdarg.h>

/* FreeRTOS+POSIX includes. */
#include "FreeRTOS_POSIX.h"
#include "FreeRTOS_POSIX/utils.h"

#include "aws_doubly_linked_list.h"
#include "esp_private/critical_section.h"

/**
 * @brief Element of the FreeRTOS queues that store mq data.
 */
typedef struct QueueElement
{
    char * pcData;    /**< Data in queue. Type char* to match msg_ptr. */
    size_t xDataSize; /**< Size of data pointed by pcData. */
} QueueElement_t;

/**
 * @brief Data structure of an mq.
 *
 * FreeRTOS isn't guaranteed to have a file-like abstraction, so message
 * queues in this implementation are stored as a linked list (in RAM).
 */
typedef struct QueueListElement
{
    Link_t xLink;              /**< Pointer to the next element in the list. */
    QueueHandle_t xQueue;      /**< FreeRTOS queue handle. */
    size_t xOpenDescriptors;   /**< Number of threads that have opened this queue. */
    char * pcName;             /**< Null-terminated queue name. */
    struct mq_attr xAttr;      /**< Queue attributes. */
    BaseType_t xPendingUnlink; /**< If pdTRUE, this queue will be unlinked once all descriptors close. */
} QueueListElement_t;

/*-----------------------------------------------------------*/

/**
 * @brief Convert an absolute timespec into a tick timeout, taking into account
 * queue flags.
 *
 * @param[in] lMessageQueueFlags Message queue flags to consider.
 * @param[in] pxAbsoluteTimeout The absolute timespec to convert.
 * @param[out] pxTimeoutTicks Output parameter of the timeout in ticks.
 *
 * @return 0 if successful; EINVAL if pxAbsoluteTimeout is invalid, or ETIMEDOUT
 * if pxAbsoluteTimeout is in the past.
 */
static int prvCalculateTickTimeout( long lMessageQueueFlags,
                                    const struct timespec * const pxAbsoluteTimeout,
                                    TickType_t * pxTimeoutTicks );

/**
 * @brief Add a new queue to the queue list.
 *
 * @param[out] ppxMessageQueue Pointer to new queue.
 * @param[in] pxAttr mq_attr of the new queue.
 * @param[in] pcName Name of new queue.
 * @param[in] xNameLength Length of pcName.
 *
 * @return pdTRUE if the queue is found; pdFALSE otherwise.
 */
static BaseType_t prvCreateNewMessageQueue( QueueListElement_t ** ppxMessageQueue,
                                            const struct mq_attr * const pxAttr,
                                            const char * const pcName,
                                            size_t xNameLength );

/**
 * @brief Free all the resources used by a message queue.
 *
 * @param[out] pxMessageQueue Pointer to queue to free.
 *
 * @return nothing
 */
static void prvDeleteMessageQueue( const QueueListElement_t * const pxMessageQueue );

/**
 * @brief Attempt to find the queue identified by pcName or xMqId in the queue list.
 *
 * Matches queues by pcName first; if pcName is NULL, matches by xMqId.
 * @param[out] ppxQueueListElement Output parameter set when queue is found.
 * @param[in] pcName A queue name to match.
 * @param[in] xMessageQueueDescriptor A queue descriptor to match.
 *
 * @return pdTRUE if the queue is found; pdFALSE otherwise.
 */
static BaseType_t prvFindQueueInList( QueueListElement_t ** const ppxQueueListElement,
                                      const char * const pcName,
                                      mqd_t xMessageQueueDescriptor );

/**
 * @brief Initialize the queue list.
 *
 * Performs initialization of the queue list mutex and queue list head.
 *
 * @return nothing
 */
static void prvInitializeQueueList( void );

/**
 * @brief Checks that pcName is a valid name for a message queue.
 *
 * Also outputs the length of pcName.
 * @param[in] pcName The name to check.
 * @param[out] pxNameLength Output parameter for name length.
 *
 * @return pdTRUE if the name is valid; pdFALSE otherwise.
 */
static BaseType_t prvValidateQueueName( const char * const pcName,
                                        size_t * pxNameLength );

/**
 * @brief Guards access to the list of message queues.
 */
static StaticSemaphore_t xQueueListMutex = { { 0 }, .u = { 0 } };

/**
 * @brief Head of the linked list of queues.
 */
static Link_t xQueueListHead = { 0 };

DEFINE_CRIT_SECTION_LOCK_STATIC( critical_section_lock );

/*-----------------------------------------------------------*/

static int prvCalculateTickTimeout( long lMessageQueueFlags,
                                    const struct timespec * const pxAbsoluteTimeout,
                                    TickType_t * pxTimeoutTicks )
{
    int iStatus = 0;

    /* Check for nonblocking queue. */
    if( lMessageQueueFlags & O_NONBLOCK )
    {
        /* No additional checks are done for nonblocking queues. Timeout is 0. */
        *pxTimeoutTicks = 0;
    }
    else
    {
        /* No absolute timeout given. Block forever. */
        if( pxAbsoluteTimeout == NULL )
        {
            *pxTimeoutTicks = portMAX_DELAY;
        }
        else
        {
            /* Check that the given timespec is valid. */
            if( UTILS_ValidateTimespec( pxAbsoluteTimeout ) == false )
            {
                iStatus = EINVAL;
            }

            /* Convert absolute timespec to ticks. */
            if( ( iStatus == 0 ) &&
                ( UTILS_AbsoluteTimespecToTicks( pxAbsoluteTimeout, pxTimeoutTicks ) != 0 ) )
            {
                iStatus = ETIMEDOUT;
            }
        }
    }

    return iStatus;
}

/*-----------------------------------------------------------*/

static BaseType_t prvCreateNewMessageQueue( QueueListElement_t ** ppxMessageQueue,
                                            const struct mq_attr * const pxAttr,
                                            const char * const pcName,
                                            size_t xNameLength )
{
    BaseType_t xStatus = pdTRUE;

    /* Allocate space for a new queue element. */
    *ppxMessageQueue = pvPortMalloc( sizeof( QueueListElement_t ) );

    /* Check that memory allocation succeeded. */
    if( *ppxMessageQueue == NULL )
    {
        xStatus = pdFALSE;
    }

    /* Create the FreeRTOS queue. */
    if( xStatus == pdTRUE )
    {
        ( *ppxMessageQueue )->xQueue =
            xQueueCreate( pxAttr->mq_maxmsg, sizeof( QueueElement_t ) );

        /* Check that queue creation succeeded. */
        if( ( *ppxMessageQueue )->xQueue == NULL )
        {
            vPortFree( *ppxMessageQueue );
            xStatus = pdFALSE;
        }
    }

    if( xStatus == pdTRUE )
    {
        /* Allocate space for the queue name plus null-terminator. */
        ( *ppxMessageQueue )->pcName = pvPortMalloc( xNameLength + 1 );

        /* Check that memory was successfully allocated for queue name. */
        if( ( *ppxMessageQueue )->pcName == NULL )
        {
            vQueueDelete( ( *ppxMessageQueue )->xQueue );
            vPortFree( *ppxMessageQueue );
            xStatus = pdFALSE;
        }
        else
        {
            /* Copy queue name. Copying xNameLength+1 will cause strncpy to add
             * the null-terminator. */
            ( void ) strncpy( ( *ppxMessageQueue )->pcName, pcName, xNameLength + 1 );
        }
    }

    if( xStatus == pdTRUE )
    {
        /* Copy attributes. */
        ( *ppxMessageQueue )->xAttr = *pxAttr;

        /* A newly-created queue will have 1 open descriptor for it. */
        ( *ppxMessageQueue )->xOpenDescriptors = 1;

        /* A newly-created queue will not be pending unlink. */
        ( *ppxMessageQueue )->xPendingUnlink = pdFALSE;

        /* Add the new queue to the list. */
        listADD( &xQueueListHead, &( *ppxMessageQueue )->xLink );
    }

    return xStatus;
}

/*-----------------------------------------------------------*/

static void prvDeleteMessageQueue( const QueueListElement_t * const pxMessageQueue )
{
    QueueElement_t xQueueElement = { 0 };

    /* Free all data in the queue. It's assumed that no more data will be added
     * to the queue, so xQueueReceive does not block. */
    while( xQueueReceive( pxMessageQueue->xQueue,
                          ( void * ) &xQueueElement,
                          0 ) == pdTRUE )
    {
        vPortFree( xQueueElement.pcData );
    }

    /* Free memory used by this message queue. */
    vQueueDelete( pxMessageQueue->xQueue );
    vPortFree( ( void * ) pxMessageQueue->pcName );
    vPortFree( ( void * ) pxMessageQueue );
}

/*-----------------------------------------------------------*/

static BaseType_t prvFindQueueInList( QueueListElement_t ** const ppxQueueListElement,
                                      const char * const pcName,
                                      mqd_t xMessageQueueDescriptor )
{
    Link_t * pxQueueListLink = NULL;
    QueueListElement_t * pxMessageQueue = NULL;
    BaseType_t xQueueFound = pdFALSE;

    /* Iterate through the list of queues. */
    listFOR_EACH( pxQueueListLink, &xQueueListHead )
    {
        pxMessageQueue = listCONTAINER( pxQueueListLink, QueueListElement_t, xLink );

        /* Match by name first if provided. */
        if( ( pcName != NULL ) && ( strcmp( pxMessageQueue->pcName, pcName ) == 0 ) )
        {
            xQueueFound = pdTRUE;
            break;
        }
        /* If name doesn't match, match by descriptor. */
        else
        {
            if( ( mqd_t ) pxMessageQueue == xMessageQueueDescriptor )
            {
                xQueueFound = pdTRUE;
                break;
            }
        }
    }

    /* If the queue was found, set the output parameter. */
    if( ( xQueueFound == pdTRUE ) && ( ppxQueueListElement != NULL ) )
    {
        *ppxQueueListElement = pxMessageQueue;
    }

    return xQueueFound;
}

/*-----------------------------------------------------------*/

static void prvInitializeQueueList( void )
{
    /* Keep track of whether the queue list has been initialized. */
    static BaseType_t xQueueListInitialized = pdFALSE;

    /* Check if queue list needs to be initialized. */
    if( xQueueListInitialized == pdFALSE )
    {
        /* Initialization must be in a critical section to prevent two threads
         * from initializing at the same time. */
        esp_os_enter_critical( &critical_section_lock );

        /* Check again that queue list is still uninitialized, i.e. it wasn't
         * initialized while this function was waiting to enter the critical
         * section. */
        if( xQueueListInitialized == pdFALSE )
        {
            /* Initialize the queue list mutex and list head. */
            ( void ) xSemaphoreCreateMutexStatic( &xQueueListMutex );
            listINIT_HEAD( &xQueueListHead );
            xQueueListInitialized = pdTRUE;
        }

        /* Exit the critical section. */
        esp_os_exit_critical( &critical_section_lock );
    }
}

/*-----------------------------------------------------------*/

static BaseType_t prvValidateQueueName( const char * const pcName,
                                        size_t * pxNameLength )
{
    BaseType_t xStatus = pdTRUE;
    size_t xNameLength = 0;

    /* All message queue names must start with '/'. */
    if( pcName[ 0 ] != '/' )
    {
        xStatus = pdFALSE;
    }
    else
    {
        /* Get the length of pcName, excluding the first '/' and null-terminator. */
        xNameLength = UTILS_strnlen( pcName, NAME_MAX + 2 );

        if( xNameLength == NAME_MAX + 2 )
        {
            /* Name too long. */
            xStatus = pdFALSE;
        }
        else
        {
            /* Name length passes, set output parameter. */
            *pxNameLength = xNameLength;
        }
    }

    return xStatus;
}

/*-----------------------------------------------------------*/

int mq_close( mqd_t mqdes )
{
    int iStatus = 0;
    QueueListElement_t * pxMessageQueue = ( QueueListElement_t * ) mqdes;
    BaseType_t xQueueRemoved = pdFALSE;

    /* Initialize the queue list, if needed. */
    prvInitializeQueueList();

    /* Lock the mutex that guards access to the queue list. This call will
     * never fail because it blocks forever. */
    ( void ) xSemaphoreTake( ( SemaphoreHandle_t ) &xQueueListMutex, portMAX_DELAY );

    /* Attempt to find the message queue based on the given descriptor. */
    if( prvFindQueueInList( NULL, NULL, mqdes ) == pdTRUE )
    {
        /* Decrement the number of open descriptors. */
        if( pxMessageQueue->xOpenDescriptors > 0 )
        {
            pxMessageQueue->xOpenDescriptors--;
        }

        /* Check if the queue has any more open descriptors. */
        if( pxMessageQueue->xOpenDescriptors == 0 )
        {
            /* If no open descriptors remain and mq_unlink has already been called,
             * remove the queue. */
            if( pxMessageQueue->xPendingUnlink == pdTRUE )
            {
                listREMOVE( &pxMessageQueue->xLink );

                /* Set the flag to delete the queue. Deleting the queue is deferred
                 * until xQueueListMutex is released. */
                xQueueRemoved = pdTRUE;
            }
            /* Otherwise, wait for the call to mq_unlink. */
            else
            {
                pxMessageQueue->xPendingUnlink = pdTRUE;
            }
        }
    }
    else
    {
        /* Queue not found; bad descriptor. */
        errno = EBADF;
        iStatus = -1;
    }

    /* Release the mutex protecting the queue list. */
    ( void ) xSemaphoreGive( ( SemaphoreHandle_t ) &xQueueListMutex );

    /* Delete all resources used by the queue if needed. */
    if( xQueueRemoved == pdTRUE )
    {
        prvDeleteMessageQueue( pxMessageQueue );
    }

    return iStatus;
}

/*-----------------------------------------------------------*/

int mq_getattr( mqd_t mqdes,
                struct mq_attr * mqstat )
{
    int iStatus = 0;
    QueueListElement_t * pxMessageQueue = ( QueueListElement_t * ) mqdes;

    /* Lock the mutex that guards access to the queue list. This call will
     * never fail because it blocks forever. */
    ( void ) xSemaphoreTake( ( SemaphoreHandle_t ) &xQueueListMutex, portMAX_DELAY );

    /* Find the mq referenced by mqdes. */
    if( prvFindQueueInList( NULL, NULL, mqdes ) == pdTRUE )
    {
        /* Update the number of messages in the queue and copy the attributes
         * into mqstat. */
        pxMessageQueue->xAttr.mq_curmsgs = ( long ) uxQueueMessagesWaiting( pxMessageQueue->xQueue );
        *mqstat = pxMessageQueue->xAttr;
    }
    else
    {
        /* Queue not found; bad descriptor. */
        errno = EBADF;
        iStatus = -1;
    }

    /* Release the mutex protecting the queue list. */
    ( void ) xSemaphoreGive( ( SemaphoreHandle_t ) &xQueueListMutex );

    return iStatus;
}

/*-----------------------------------------------------------*/

/* Changed function signature and argument processing to match the POSIX standard */
mqd_t mq_open( const char * name,
               int oflag,
               ... )
{
    mode_t mode = 0;
    struct mq_attr * attr = NULL;

    if( oflag & O_CREAT )
    {
        va_list args;
        va_start( args, oflag );
        mode = va_arg( args, mode_t );
        attr = va_arg( args, struct mq_attr * );
        va_end( args );
    }

    mqd_t xMessageQueue = NULL;
    size_t xNameLength = 0;

    /* Default mq_attr. */
    struct mq_attr xQueueCreationAttr =
    {
        .mq_flags   = 0,
        .mq_maxmsg  = posixconfigMQ_MAX_MESSAGES,
        .mq_msgsize = posixconfigMQ_MAX_SIZE,
        .mq_curmsgs = 0
    };

    /* Silence warnings about unused parameters. */
    ( void ) mode;

    /* Initialize the queue list, if needed. */
    prvInitializeQueueList();

    /* Check queue name. */
    if( prvValidateQueueName( name, &xNameLength ) == pdFALSE )
    {
        /* Invalid name. */
        errno = EINVAL;
        xMessageQueue = ( mqd_t ) -1;
    }

    /* Check attributes, if O_CREATE is specified and attr is given. */
    if( xMessageQueue == NULL )
    {
        if( ( oflag & O_CREAT ) && ( attr != NULL ) && ( ( attr->mq_maxmsg <= 0 ) || ( attr->mq_msgsize <= 0 ) ) )
        {
            /* Invalid mq_attr.mq_maxmsg or mq_attr.mq_msgsize. */
            errno = EINVAL;
            xMessageQueue = ( mqd_t ) -1;
        }
    }

    if( xMessageQueue == NULL )
    {
        /* Lock the mutex that guards access to the queue list. This call will
         * never fail because it blocks forever. */
        ( void ) xSemaphoreTake( ( SemaphoreHandle_t ) &xQueueListMutex, portMAX_DELAY );

        /* Search the queue list to check if the queue exists. */
        if( prvFindQueueInList( ( QueueListElement_t ** ) &xMessageQueue,
                                name,
                                ( mqd_t ) NULL ) == pdTRUE )
        {
            /* If the mq exists, check that this function wasn't called with
             * O_CREAT and O_EXCL. */
            if( ( oflag & O_EXCL ) && ( oflag & O_CREAT ) )
            {
                errno = EEXIST;
                xMessageQueue = ( mqd_t ) -1;
            }
            else
            {
                /* Check if the mq has been unlinked and is pending removal. */
                if( ( ( QueueListElement_t * ) xMessageQueue )->xPendingUnlink == pdTRUE )
                {
                    /* Queue pending deletion. Don't allow it to be re-opened. */
                    errno = EINVAL;
                    xMessageQueue = ( mqd_t ) -1;
                }
                else
                {
                    /* Increase count of open file descriptors for queue. */
                    ( ( QueueListElement_t * ) xMessageQueue )->xOpenDescriptors++;
                }
            }
        }
        /* Queue does not exist. */
        else
        {
            /* Only create the new queue if O_CREAT was specified. */
            if( oflag & O_CREAT )
            {
                /* Copy attributes if provided. */
                if( attr != NULL )
                {
                    xQueueCreationAttr = *attr;
                }

                /* Copy oflags. */
                xQueueCreationAttr.mq_flags = ( long ) oflag;

                /* Create the new message queue. */
                if( prvCreateNewMessageQueue( ( QueueListElement_t ** ) &xMessageQueue,
                                              &xQueueCreationAttr,
                                              name,
                                              xNameLength ) == pdFALSE )
                {
                    errno = ENOSPC;
                    xMessageQueue = ( mqd_t ) -1;
                }
            }
            else
            {
                errno = ENOENT;
                xMessageQueue = ( mqd_t ) -1;
            }
        }

        /* Release the mutex protecting the queue list. */
        ( void ) xSemaphoreGive( ( SemaphoreHandle_t ) &xQueueListMutex );
    }

    return xMessageQueue;
}

/*-----------------------------------------------------------*/

ssize_t mq_receive( mqd_t mqdes,
                    char * msg_ptr,
                    size_t msg_len,
                    unsigned int * msg_prio )
{
    return mq_timedreceive( mqdes, msg_ptr, msg_len, msg_prio, NULL );
}

/*-----------------------------------------------------------*/

int mq_send( mqd_t mqdes,
             const char * msg_ptr,
             size_t msg_len,
             unsigned msg_prio )
{
    return mq_timedsend( mqdes, msg_ptr, msg_len, msg_prio, NULL );
}

/*-----------------------------------------------------------*/

ssize_t mq_timedreceive( mqd_t mqdes,
                         char * msg_ptr,
                         size_t msg_len,
                         unsigned * msg_prio,
                         const struct timespec * abstime )
{
    ssize_t xStatus = 0;
    int iCalculateTimeoutReturn = 0;
    TickType_t xTimeoutTicks = 0;
    QueueListElement_t * pxMessageQueue = ( QueueListElement_t * ) mqdes;
    QueueElement_t xReceiveData = { 0 };

    /* Silence warnings about unused parameters. */
    ( void ) msg_prio;

    /* Lock the mutex that guards access to the queue list. This call will
     * never fail because it blocks forever. */
    ( void ) xSemaphoreTake( ( SemaphoreHandle_t ) &xQueueListMutex, portMAX_DELAY );

    /* Find the mq referenced by mqdes. */
    if( prvFindQueueInList( NULL, NULL, mqdes ) == pdFALSE )
    {
        /* Queue not found; bad descriptor. */
        errno = EBADF;
        xStatus = -1;
    }

    /* Verify that msg_len is large enough. */
    if( xStatus == 0 )
    {
        if( msg_len < ( size_t ) pxMessageQueue->xAttr.mq_msgsize )
        {
            /* msg_len too small. */
            errno = EMSGSIZE;
            xStatus = -1;
        }
    }

    if( xStatus == 0 )
    {
        /* Convert abstime to a tick timeout. */
        iCalculateTimeoutReturn = prvCalculateTickTimeout( pxMessageQueue->xAttr.mq_flags,
                                                           abstime,
                                                           &xTimeoutTicks );

        if( iCalculateTimeoutReturn != 0 )
        {
            errno = iCalculateTimeoutReturn;
            xStatus = -1;
        }
    }

    /* Release the mutex protecting the queue list. */
    ( void ) xSemaphoreGive( ( SemaphoreHandle_t ) &xQueueListMutex );

    if( xStatus == 0 )
    {
        /* Receive data from the FreeRTOS queue. */
        if( xQueueReceive( pxMessageQueue->xQueue,
                           &xReceiveData,
                           xTimeoutTicks ) == pdFALSE )
        {
            /* If queue receive fails, set the appropriate errno. */
            if( pxMessageQueue->xAttr.mq_flags & O_NONBLOCK )
            {
                /* Set errno to EAGAIN for nonblocking mq. */
                errno = EAGAIN;
            }
            else
            {
                /* Otherwise, set errno to ETIMEDOUT. */
                errno = ETIMEDOUT;
            }

            xStatus = -1;
        }
    }

    if( xStatus == 0 )
    {
        /* Get the length of data for return value. */
        xStatus = ( ssize_t ) xReceiveData.xDataSize;

        /* Copy received data into given buffer, then free it. */
        ( void ) memcpy( msg_ptr, xReceiveData.pcData, xReceiveData.xDataSize );
        vPortFree( xReceiveData.pcData );
    }

    return xStatus;
}

/*-----------------------------------------------------------*/

int mq_timedsend( mqd_t mqdes,
                  const char * msg_ptr,
                  size_t msg_len,
                  unsigned int msg_prio,
                  const struct timespec * abstime )
{
    int iStatus = 0, iCalculateTimeoutReturn = 0;
    TickType_t xTimeoutTicks = 0;
    QueueListElement_t * pxMessageQueue = ( QueueListElement_t * ) mqdes;
    QueueElement_t xSendData = { 0 };

    /* Silence warnings about unused parameters. */
    ( void ) msg_prio;

    /* Lock the mutex that guards access to the queue list. This call will
     * never fail because it blocks forever. */
    ( void ) xSemaphoreTake( ( SemaphoreHandle_t ) &xQueueListMutex, portMAX_DELAY );

    /* Find the mq referenced by mqdes. */
    if( prvFindQueueInList( NULL, NULL, mqdes ) == pdFALSE )
    {
        /* Queue not found; bad descriptor. */
        errno = EBADF;
        iStatus = -1;
    }

    /* Verify that mq_msgsize is large enough. */
    if( iStatus == 0 )
    {
        if( msg_len > ( size_t ) pxMessageQueue->xAttr.mq_msgsize )
        {
            /* msg_len too large. */
            errno = EMSGSIZE;
            iStatus = -1;
        }
    }

    if( iStatus == 0 )
    {
        /* Convert abstime to a tick timeout. */
        iCalculateTimeoutReturn = prvCalculateTickTimeout( pxMessageQueue->xAttr.mq_flags,
                                                           abstime,
                                                           &xTimeoutTicks );

        if( iCalculateTimeoutReturn != 0 )
        {
            errno = iCalculateTimeoutReturn;
            iStatus = -1;
        }
    }

    /* Release the mutex protecting the queue list. */
    ( void ) xSemaphoreGive( ( SemaphoreHandle_t ) &xQueueListMutex );

    /* Allocate memory for the message. */
    if( iStatus == 0 )
    {
        xSendData.xDataSize = msg_len;
        xSendData.pcData = pvPortMalloc( msg_len );

        /* Check that memory allocation succeeded. */
        if( xSendData.pcData == NULL )
        {
            /* msg_len too large. */
            errno = EMSGSIZE;
            iStatus = -1;
        }
        else
        {
            /* Copy the data to send. */
            ( void ) memcpy( xSendData.pcData, msg_ptr, msg_len );
        }
    }

    if( iStatus == 0 )
    {
        /* Send data to the FreeRTOS queue. */
        if( xQueueSend( pxMessageQueue->xQueue,
                        &xSendData,
                        xTimeoutTicks ) == pdFALSE )
        {
            /* If queue send fails, set the appropriate errno. */
            if( pxMessageQueue->xAttr.mq_flags & O_NONBLOCK )
            {
                /* Set errno to EAGAIN for nonblocking mq. */
                errno = EAGAIN;
            }
            else
            {
                /* Otherwise, set errno to ETIMEDOUT. */
                errno = ETIMEDOUT;
            }

            /* Free the allocated queue data. */
            vPortFree( xSendData.pcData );

            iStatus = -1;
        }
    }

    return iStatus;
}

/*-----------------------------------------------------------*/

int mq_unlink( const char * name )
{
    int iStatus = 0;
    size_t xNameSize = 0;
    BaseType_t xQueueRemoved = pdFALSE;
    QueueListElement_t * pxMessageQueue = NULL;

    /* Initialize the queue list, if needed. */
    prvInitializeQueueList();

    /* Check queue name. */
    if( prvValidateQueueName( name, &xNameSize ) == pdFALSE )
    {
        /* Error with mq name. */
        errno = EINVAL;
        iStatus = -1;
    }

    if( iStatus == 0 )
    {
        /* Lock the mutex that guards access to the queue list. This call will
         * never fail because it blocks forever. */
        ( void ) xSemaphoreTake( ( SemaphoreHandle_t ) &xQueueListMutex, portMAX_DELAY );

        /* Check if the named queue exists. */
        if( prvFindQueueInList( &pxMessageQueue, name, ( mqd_t ) NULL ) == pdTRUE )
        {
            /* If the queue exists and there are no open descriptors to it,
             * remove it from the list. */
            if( pxMessageQueue->xOpenDescriptors == 0 )
            {
                listREMOVE( &pxMessageQueue->xLink );

                /* Set the flag to delete the queue. Deleting the queue is deferred
                 * until xQueueListMutex is released. */
                xQueueRemoved = pdTRUE;
            }
            else
            {
                /* If the queue has open descriptors, set the pending unlink flag
                 * so that mq_close will free its resources. */
                pxMessageQueue->xPendingUnlink = pdTRUE;
            }
        }
        else
        {
            /* The named message queue doesn't exist. */
            errno = ENOENT;
            iStatus = -1;
        }

        /* Release the mutex protecting the queue list. */
        ( void ) xSemaphoreGive( ( SemaphoreHandle_t ) &xQueueListMutex );
    }

    /* Delete all resources used by the queue if needed. */
    if( xQueueRemoved == pdTRUE )
    {
        prvDeleteMessageQueue( pxMessageQueue );
    }

    return iStatus;
}

/* specified but not implemented functions, return ENOSYS */
int mq_notify( mqd_t,
               const struct sigevent * )
{
    return ENOSYS;
}

int mq_setattr( mqd_t,
                const struct mq_attr * restrict,
                struct mq_attr * restrict )
{
    return ENOSYS;
}

/*-----------------------------------------------------------*/