os-mqueue.cpp

Go to the documentation of this file.

/*
 * This file is part of the µOS++ project (https://micro-os-plus.github.io/).
 * Copyright (c) 2016-2025 Liviu Ionescu. All rights reserved.
 *
 * Permission to use, copy, modify, and/or distribute this software
 * for any purpose is hereby granted, under the terms of the MIT license.
 *
 * If a copy of the license was not distributed with this file, it can
 * be obtained from https://opensource.org/licenses/mit.
 */
 
#if defined(OS_USE_OS_APP_CONFIG_H)
#include <cmsis-plus/os-app-config.h>
#endif
 
#include <cmsis-plus/rtos/os.h>
 
// ----------------------------------------------------------------------------
 
#if defined(__clang__)
#pragma clang diagnostic ignored "-Wc++98-compat"
#endif
 
// ----------------------------------------------------------------------------
 
namespace os
{
  namespace rtos
  {
    // ------------------------------------------------------------------------
 
    const message_queue::attributes message_queue::initializer;
 
    // ------------------------------------------------------------------------
 
    // ------------------------------------------------------------------------
    // ------------------------------------------------------------------------
    // Protected internal constructor.
    message_queue::message_queue ()
    {
#if defined(OS_TRACE_RTOS_MQUEUE)
      trace::printf ("%s() @%p %s\n", __func__, this, this->name ());
#endif
    }
 
    message_queue::message_queue (const char* name)
        : object_named_system{ name }
    {
#if defined(OS_TRACE_RTOS_MQUEUE)
      trace::printf ("%s() @%p %s\n", __func__, this, this->name ());
#endif
    }
 
    message_queue::message_queue (std::size_t msgs, std::size_t msg_size_bytes,
                                  const attributes& attr,
                                  const allocator_type& allocator)
        : message_queue{ nullptr, msgs, msg_size_bytes, attr, allocator }
    {
    }
 
    message_queue::message_queue (const char* name, std::size_t msgs,
                                  std::size_t msg_size_bytes,
                                  const attributes& attr,
                                  const allocator_type& allocator)
        : object_named_system{ name }
    {
#if defined(OS_TRACE_RTOS_MQUEUE)
      trace::printf ("%s() @%p %s %u %u\n", __func__, this, this->name (),
                     msgs, msg_size_bytes);
#endif
 
      if (attr.mq_queue_address != nullptr)
        {
          // Do not use any allocator at all.
          internal_construct_ (msgs, msg_size_bytes, attr, nullptr, 0);
        }
      else
        {
          allocator_ = &allocator;
 
          // If no user storage was provided via attributes,
          // allocate it dynamically via the allocator.
          allocated_queue_size_elements_
              = (compute_allocated_size_bytes<
                     typename allocator_type::value_type> (msgs,
                                                           msg_size_bytes)
                 + sizeof (typename allocator_type::value_type) - 1)
                / sizeof (typename allocator_type::value_type);
 
          allocated_queue_addr_
              = const_cast<allocator_type&> (allocator).allocate (
                  allocated_queue_size_elements_);
 
          internal_construct_ (
              msgs, msg_size_bytes, attr, allocated_queue_addr_,
              allocated_queue_size_elements_
                  * sizeof (typename allocator_type::value_type));
        }
    }
 
    message_queue::~message_queue ()
    {
#if defined(OS_TRACE_RTOS_MQUEUE)
      trace::printf ("%s() @%p %s\n", __func__, this, name ());
#endif
 
#if !defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE)
 
      // There must be no threads waiting for this queue.
      assert (send_list_.empty ());
      assert (receive_list_.empty ());
 
#endif
 
      if (allocated_queue_addr_ != nullptr)
        {
          typedef
              typename std::allocator_traits<allocator_type>::pointer pointer;
 
          static_cast<allocator_type*> (const_cast<void*> (allocator_))
              ->deallocate (reinterpret_cast<pointer> (allocated_queue_addr_),
                            allocated_queue_size_elements_);
        }
 
#if defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE)
 
      port::message_queue::destroy (this);
 
#endif
    }
 
    void
    message_queue::internal_construct_ (std::size_t msgs,
                                        std::size_t msg_size_bytes,
                                        const attributes& attr,
                                        void* queue_address,
                                        std::size_t queue_size_bytes)
    {
      // Don't call this from interrupt handlers.
      os_assert_throw (!interrupts::in_handler_mode (), EPERM);
 
#if !defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE)
      clock_ = attr.clock != nullptr ? attr.clock : &sysclock;
#endif
      msg_size_bytes_
          = static_cast<message_queue::msg_size_t> (msg_size_bytes);
      assert (msg_size_bytes_ == msg_size_bytes);
      assert (msg_size_bytes_ > 0);
 
      // in order for the list of free messages to not consume additional
      // memory, the pointers are stored at the beginning of the messages, thus
      // messages should be large enough to fit a pointer
      assert (msg_size_bytes_ >= sizeof (void*));
 
      msgs_ = static_cast<message_queue::size_t> (msgs);
      assert (msgs_ == msgs);
      assert (msgs > 0);
 
      // If the storage is given explicitly, override attributes.
      if (queue_address != nullptr)
        {
          // The attributes should not define any storage in this case.
          assert (attr.mq_queue_address == nullptr);
 
          queue_addr_ = queue_address;
          queue_size_bytes_ = queue_size_bytes;
        }
      else
        {
          queue_addr_ = attr.mq_queue_address;
          queue_size_bytes_ = attr.mq_queue_size_bytes;
        }
 
#if defined(OS_TRACE_RTOS_MQUEUE)
      trace::printf ("%s() @%p %s %u %u %p %u\n", __func__, this, name (),
                     msgs_, msg_size_bytes_, queue_addr_, queue_size_bytes_);
#endif
 
#if !defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE)
      std::size_t storage_size
          = compute_allocated_size_bytes<void*> (msgs, msg_size_bytes);
#endif
      if (queue_addr_ != nullptr)
        {
          // The queue must be real, and have a non zero size.
          os_assert_throw (queue_size_bytes_ > 0, EINVAL);
#if defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE)
          os_assert_throw (queue_size_bytes_
                               >= (std::size_t)(msgs * msg_size_bytes),
                           EINVAL);
#else
          // The queue must fit the storage.
          os_assert_throw (queue_size_bytes_ >= storage_size, EINVAL);
#endif
        }
 
#if defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE)
 
      count_ = 0;
      port::message_queue::create (this);
 
#else
 
      head_ = no_index;
 
      // The queue storage must have a real address.
      os_assert_throw (queue_addr_ != nullptr, ENOMEM);
 
#pragma GCC diagnostic push
#if defined(__clang__)
#pragma clang diagnostic ignored "-Wunsafe-buffer-usage"
#endif
      // The array of prev indexes follows immediately after the content array.
      prev_array_ = reinterpret_cast<index_t*> (
          static_cast<char*> (queue_addr_)
          + msgs
                * ((msg_size_bytes + (sizeof (void*) - 1))
                   & ~(sizeof (void*) - 1)));
      // The array of next indexes follows immediately the prev array.
      next_array_ = reinterpret_cast<index_t*> (
          reinterpret_cast<char*> (const_cast<index_t*> (prev_array_))
          + msgs * sizeof (index_t));
      // The array of priorities follows immediately the next array.
      prio_array_ = reinterpret_cast<priority_t*> (
          reinterpret_cast<char*> (const_cast<index_t*> (next_array_))
          + msgs * sizeof (index_t));
#pragma GCC diagnostic pop
 
#if !defined(NDEBUG)
#pragma GCC diagnostic push
#if defined(__clang__)
#pragma clang diagnostic ignored "-Wunsafe-buffer-usage"
#elif defined(__GNUC__)
#pragma GCC diagnostic ignored "-Wuseless-cast"
#endif
      char* p = reinterpret_cast<char*> (
          reinterpret_cast<char*> (const_cast<priority_t*> (prio_array_))
          + msgs * sizeof (priority_t));
#pragma GCC diagnostic pop
 
      assert (p - static_cast<char*> (queue_addr_)
              <= static_cast<ptrdiff_t> (queue_size_bytes_));
#endif
 
      internal_init_ ();
#endif
    }
 
    void
    message_queue::internal_init_ (void)
    {
      count_ = 0;
 
#if !defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE)
 
      // Construct a linked list of blocks. Store the pointer at
      // the beginning of each block. Each block
      // will hold the address of the next free block,
      // or `nullptr` at the end.
#pragma GCC diagnostic push
#if defined(__clang__)
#pragma clang diagnostic ignored "-Wunsafe-buffer-usage"
#endif
      char* p = static_cast<char*> (queue_addr_);
      for (std::size_t i = 1; i < msgs_; ++i)
        {
          // Compute the address of the next block;
          char* pn = p + msg_size_bytes_;
 
          // Make this block point to the next one.
          *(static_cast<void**> (static_cast<void*> (p))) = pn;
          // Advance pointer
          p = pn;
        }
#pragma GCC diagnostic pop
 
      // Mark end of list.
      *(static_cast<void**> (static_cast<void*> (p))) = nullptr;
 
      first_free_ = queue_addr_; // Pointer to first block.
 
      head_ = no_index;
 
      // Need not be inside the critical section,
      // the lists are protected by inner `resume_one()`.
 
      // Wake-up all threads, if any.
      send_list_.resume_all ();
      receive_list_.resume_all ();
 
#endif /* !defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE) */
    }
 
#if !defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE)
 
    /*
     * Internal function.
     * Should be called from an interrupts critical section.
     */
    bool
    message_queue::internal_try_send_ (const void* msg, std::size_t nbytes,
                                       priority_t mprio)
    {
      if (first_free_ == nullptr)
        {
          // No available space to send the message.
          return false;
        }
 
        // The first step is to remove the free block from the list,
        // so another concurrent call will not get it too.
 
        // Get the address where the message will be copied.
        // This is the first free memory block.
#pragma GCC diagnostic push
#if defined(__clang__)
#pragma clang diagnostic ignored "-Wunsafe-buffer-usage"
#endif
      char* dest = static_cast<char*> (first_free_);
#pragma GCC diagnostic pop
 
      // Update to next free, if any (the last one has nullptr).
      first_free_ = *(static_cast<void**> (first_free_));
 
      // The second step is to copy the message from the user buffer.
      {
        // ----- Enter uncritical section -------------------------------------
        // interrupts::uncritical_section iucs;
 
        // Copy message from user buffer to queue storage.
        std::memcpy (dest, msg, nbytes);
        if (nbytes < msg_size_bytes_)
          {
            // Fill in the remaining space with 0x00.
#pragma GCC diagnostic push
#if defined(__clang__)
#pragma clang diagnostic ignored "-Wunsafe-buffer-usage"
#endif
            std::memset (dest + nbytes, 0x00, msg_size_bytes_ - nbytes);
#pragma GCC diagnostic pop
          }
        // ----- Exit uncritical section --------------------------------------
      }
 
      // The third step is to link the buffer to the list.
 
#pragma GCC diagnostic push
#if defined(__clang__)
#pragma clang diagnostic ignored "-Wunsafe-buffer-usage"
#endif
      // Using the address, compute the index in the array.
      std::size_t msg_ix
          = (static_cast<std::size_t> (dest - static_cast<char*> (queue_addr_))
             / msg_size_bytes_);
      prio_array_[msg_ix] = mprio;
 
      if (head_ == no_index)
        {
          // No other message in the queue, enlist this one
          // as head, with links to itself.
          head_ = static_cast<index_t> (msg_ix);
          prev_array_[msg_ix] = static_cast<index_t> (msg_ix);
          next_array_[msg_ix] = static_cast<index_t> (msg_ix);
        }
      else
        {
          std::size_t ix;
          // Arrange to insert between head and tail.
          ix = prev_array_[head_];
          // Check if the priority is higher than the head priority.
          if (mprio > prio_array_[head_])
            {
              // Having the highest priority, the new message
              // becomes the new head.
              head_ = static_cast<index_t> (msg_ix);
            }
          else
            {
              // If not higher than the head, try to insert at the tail,
              // but advance up until the same priority is found.
              while ((mprio > prio_array_[ix]))
                {
                  ix = prev_array_[ix];
                }
            }
          prev_array_[msg_ix] = static_cast<index_t> (ix);
          next_array_[msg_ix] = next_array_[ix];
 
          // Break the chain and insert the new index.
          std::size_t tmp_ix = next_array_[ix];
          next_array_[ix] = static_cast<index_t> (msg_ix);
          prev_array_[tmp_ix] = static_cast<index_t> (msg_ix);
        }
#pragma GCC diagnostic pop
 
      // One more message added to the queue.
      ++count_;
 
      // Wake-up one thread, if any.
      receive_list_.resume_one ();
 
      return true;
    }
 
#endif /* !defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE) */
 
#if !defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE)
 
    /*
     * Internal function.
     * Should be called from an interrupts critical section.
     */
    bool
    message_queue::internal_try_receive_ (void* msg, std::size_t nbytes,
                                          priority_t* mprio)
    {
 
      if (head_ == no_index)
        {
          return false;
        }
 
        // Compute the message source address.
#pragma GCC diagnostic push
#if defined(__clang__)
#pragma clang diagnostic ignored "-Wunsafe-buffer-usage"
#endif
      char* src = static_cast<char*> (queue_addr_) + head_ * msg_size_bytes_;
      priority_t prio = prio_array_[head_];
#pragma GCC diagnostic pop
 
#if defined(OS_TRACE_RTOS_MQUEUE_)
      trace::printf ("%s(%p,%u) @%p %s src %p %p\n", __func__, msg, nbytes,
                     this, name (), src, first_free_);
#endif
 
      // Unlink it from the list, so another concurrent call will
      // not get it too.
      if (count_ > 1)
        {
          // Remove the current element from the list.
#pragma GCC diagnostic push
#if defined(__clang__)
#pragma clang diagnostic ignored "-Wunsafe-buffer-usage"
#endif
          prev_array_[next_array_[head_]] = prev_array_[head_];
          next_array_[prev_array_[head_]] = next_array_[head_];
 
          // Next becomes the new head.
          head_ = next_array_[head_];
#pragma GCC diagnostic pop
        }
      else
        {
          // If there was only one, the list is empty now.
          head_ = no_index;
        }
 
      --count_;
 
      // Copy to destination
      {
        // ----- Enter uncritical section -------------------------------------
        interrupts::uncritical_section iucs;
 
        // Copy message from queue to user buffer.
        memcpy (msg, src, nbytes);
        if (mprio != nullptr)
          {
            *mprio = prio;
          }
        // ----- Exit uncritical section --------------------------------------
      }
 
      // After the message was copied, the block can be released.
 
      // Perform a push_front() on the single linked LIFO list,
      // i.e. add the block to the beginning of the list.
 
      // Link previous list to this block; may be null, but it does
      // not matter.
      *(static_cast<void**> (static_cast<void*> (src))) = first_free_;
 
      // Now this block is the first one.
      first_free_ = src;
 
      // Wake-up one thread, if any.
      send_list_.resume_one ();
 
      return true;
    }
 
#endif /* !defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE) */
 
    result_t
    message_queue::send (const void* msg, std::size_t nbytes, priority_t mprio)
    {
#if defined(OS_TRACE_RTOS_MQUEUE)
      trace::printf ("%s(%p,%d,%d) @%p %s\n", __func__, msg, nbytes, mprio,
                     this, name ());
#endif
 
      // Don't call this from interrupt handlers.
      os_assert_err (!interrupts::in_handler_mode (), EPERM);
      // Don't call this from critical regions.
      os_assert_err (!scheduler::locked (), EPERM);
 
      os_assert_err (msg != nullptr, EINVAL);
      os_assert_err (nbytes <= msg_size_bytes_, EMSGSIZE);
 
#if defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE)
 
      return port::message_queue::send (this, msg, nbytes, mprio);
 
#else
 
      {
        // ----- Enter critical section ---------------------------------------
        interrupts::critical_section ics;
 
        if (internal_try_send_ (msg, nbytes, mprio))
          {
            return result::ok;
          }
        // ----- Exit critical section ----------------------------------------
      }
 
      thread& crt_thread = this_thread::thread ();
 
      // Prepare a list node pointing to the current thread.
      // Do not worry for being on stack, it is temporarily linked to the
      // list and guaranteed to be removed before this function returns.
      internal::waiting_thread_node node{ crt_thread };
 
      for (;;)
        {
          {
            // ----- Enter critical section -----------------------------------
            interrupts::critical_section ics;
 
            if (internal_try_send_ (msg, nbytes, mprio))
              {
                return result::ok;
              }
 
            // Add this thread to the message queue send waiting list.
            scheduler::internal_link_node (send_list_, node);
            // state::suspended set in above link().
            // ----- Exit critical section ------------------------------------
          }
 
          port::scheduler::reschedule ();
 
          // Remove the thread from the message queue send waiting list,
          // if not already removed by receive().
          scheduler::internal_unlink_node (node);
 
          if (crt_thread.interrupted ())
            {
#if defined(OS_TRACE_RTOS_MQUEUE)
              trace::printf ("%s(%p,%d,%d) EINTR @%p %s\n", __func__, msg,
                             nbytes, mprio, this, name ());
#endif
              return EINTR;
            }
        }
 
      /* NOTREACHED */
      return ENOTRECOVERABLE;
 
#endif
    }
 
    result_t
    message_queue::try_send (const void* msg, std::size_t nbytes,
                             priority_t mprio)
    {
#if defined(OS_TRACE_RTOS_MQUEUE)
      trace::printf ("%s(%p,%u,%u) @%p %s\n", __func__, msg, nbytes, mprio,
                     this, name ());
#endif
 
      os_assert_err (msg != nullptr, EINVAL);
      os_assert_err (nbytes <= msg_size_bytes_, EMSGSIZE);
 
#if defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE)
 
      return port::message_queue::try_send (this, msg, nbytes, mprio);
 
#else
      // Don't call this from high priority interrupts.
      assert (port::interrupts::is_priority_valid ());
 
      {
        // ----- Enter critical section ---------------------------------------
        interrupts::critical_section ics;
 
        if (internal_try_send_ (msg, nbytes, mprio))
          {
            return result::ok;
          }
        else
          {
            return EWOULDBLOCK;
          }
        // ----- Exit critical section ----------------------------------------
      }
 
#endif
    }
 
    result_t
    message_queue::timed_send (const void* msg, std::size_t nbytes,
                               clock::duration_t timeout, priority_t mprio)
    {
#if defined(OS_TRACE_RTOS_MQUEUE)
      trace::printf ("%s(%p,%u,%u,%u) @%p %s\n", __func__, msg, nbytes, mprio,
                     timeout, this, name ());
#endif
 
      // Don't call this from interrupt handlers.
      os_assert_err (!interrupts::in_handler_mode (), EPERM);
      // Don't call this from critical regions.
      os_assert_err (!scheduler::locked (), EPERM);
 
      os_assert_err (msg != nullptr, EINVAL);
      os_assert_err (nbytes <= msg_size_bytes_, EMSGSIZE);
 
#if defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE)
 
      return port::message_queue::timed_send (this, msg, nbytes, timeout,
                                              mprio);
 
#else
 
      // Extra test before entering the loop, with its inherent weight.
      // Trade size for speed.
      {
        // ----- Enter critical section ---------------------------------------
        interrupts::critical_section ics;
 
        if (internal_try_send_ (msg, nbytes, mprio))
          {
            return result::ok;
          }
        // ----- Exit critical section ----------------------------------------
      }
 
      thread& crt_thread = this_thread::thread ();
 
      // Prepare a list node pointing to the current thread.
      // Do not worry for being on stack, it is temporarily linked to the
      // list and guaranteed to be removed before this function returns.
      internal::waiting_thread_node node{ crt_thread };
 
      internal::clock_timestamps_list& clock_list = clock_->steady_list ();
 
      clock::timestamp_t timeout_timestamp = clock_->steady_now () + timeout;
 
      // Prepare a timeout node pointing to the current thread.
      internal::timeout_thread_node timeout_node{ timeout_timestamp,
                                                  crt_thread };
 
      for (;;)
        {
          {
            // ----- Enter critical section -----------------------------------
            interrupts::critical_section ics;
 
            if (internal_try_send_ (msg, nbytes, mprio))
              {
                return result::ok;
              }
 
            // Add this thread to the semaphore waiting list,
            // and the clock timeout list.
            scheduler::internal_link_node (send_list_, node, clock_list,
                                           timeout_node);
            // state::suspended set in above link().
            // ----- Exit critical section ------------------------------------
          }
 
          port::scheduler::reschedule ();
 
          // Remove the thread from the message queue send waiting list,
          // if not already removed by receive() and from the clock timeout
          // list, if not already removed by the timer.
          scheduler::internal_unlink_node (node, timeout_node);
 
          if (crt_thread.interrupted ())
            {
#if defined(OS_TRACE_RTOS_MQUEUE)
              trace::printf ("%s(%p,%u,%u,%u) EINTR @%p %s\n", __func__, msg,
                             nbytes, mprio, timeout, this, name ());
#endif
              return EINTR;
            }
 
          if (clock_->steady_now () >= timeout_timestamp)
            {
#if defined(OS_TRACE_RTOS_MQUEUE)
              trace::printf ("%s(%p,%u,%u,%u) ETIMEDOUT @%p %s\n", __func__,
                             msg, nbytes, mprio, timeout, this, name ());
#endif
              return ETIMEDOUT;
            }
        }
 
      /* NOTREACHED */
      return ENOTRECOVERABLE;
 
#endif
    }
 
    result_t
    message_queue::receive (void* msg, std::size_t nbytes, priority_t* mprio)
    {
#if defined(OS_TRACE_RTOS_MQUEUE)
      trace::printf ("%s(%p,%u) @%p %s\n", __func__, msg, nbytes, this,
                     name ());
#endif
 
      // Don't call this from interrupt handlers.
      os_assert_err (!interrupts::in_handler_mode (), EPERM);
      // Don't call this from critical regions.
      os_assert_err (!scheduler::locked (), EPERM);
 
      os_assert_err (msg != nullptr, EINVAL);
      os_assert_err (nbytes <= msg_size_bytes_, EMSGSIZE);
 
#if defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE)
 
      return port::message_queue::receive (this, msg, nbytes, mprio);
 
#else
 
      // Extra test before entering the loop, with its inherent weight.
      // Trade size for speed.
      {
        // ----- Enter critical section ---------------------------------------
        interrupts::critical_section ics;
 
        if (internal_try_receive_ (msg, nbytes, mprio))
          {
            return result::ok;
          }
        // ----- Exit critical section ----------------------------------------
      }
 
      thread& crt_thread = this_thread::thread ();
 
      // Prepare a list node pointing to the current thread.
      // Do not worry for being on stack, it is temporarily linked to the
      // list and guaranteed to be removed before this function returns.
      internal::waiting_thread_node node{ crt_thread };
 
      for (;;)
        {
          {
            // ----- Enter critical section -----------------------------------
            interrupts::critical_section ics;
 
            if (internal_try_receive_ (msg, nbytes, mprio))
              {
                return result::ok;
              }
 
            // Add this thread to the message queue receive waiting list.
            scheduler::internal_link_node (receive_list_, node);
            // state::suspended set in above link().
            // ----- Exit critical section ------------------------------------
          }
 
          port::scheduler::reschedule ();
 
          // Remove the thread from the message queue receive waiting list,
          // if not already removed by send().
          scheduler::internal_unlink_node (node);
 
          if (crt_thread.interrupted ())
            {
#if defined(OS_TRACE_RTOS_MQUEUE)
              trace::printf ("%s(%p,%u) EINTR @%p %s\n", __func__, msg, nbytes,
                             this, name ());
#endif
              return EINTR;
            }
        }
 
      /* NOTREACHED */
      return ENOTRECOVERABLE;
 
#endif
    }
 
    result_t
    message_queue::try_receive (void* msg, std::size_t nbytes,
                                priority_t* mprio)
    {
#if defined(OS_TRACE_RTOS_MQUEUE)
      trace::printf ("%s(%p,%u) @%p %s\n", __func__, msg, nbytes, this,
                     name ());
#endif
 
      os_assert_err (msg != nullptr, EINVAL);
      os_assert_err (nbytes <= msg_size_bytes_, EMSGSIZE);
 
#if defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE)
 
      return port::message_queue::try_receive (this, msg, nbytes, mprio);
 
#else
 
      // Don't call this from high priority interrupts.
      assert (port::interrupts::is_priority_valid ());
 
      {
        // ----- Enter critical section ---------------------------------------
        interrupts::critical_section ics;
 
        if (internal_try_receive_ (msg, nbytes, mprio))
          {
            return result::ok;
          }
        else
          {
            return EWOULDBLOCK;
          }
        // ----- Exit critical section ----------------------------------------
      }
 
#endif
    }
 
    result_t
    message_queue::timed_receive (void* msg, std::size_t nbytes,
                                  clock::duration_t timeout, priority_t* mprio)
    {
#if defined(OS_TRACE_RTOS_MQUEUE)
      trace::printf ("%s(%p,%u,%u) @%p %s\n", __func__, msg, nbytes, timeout,
                     this, name ());
#endif
 
      // Don't call this from interrupt handlers.
      os_assert_err (!interrupts::in_handler_mode (), EPERM);
      // Don't call this from critical regions.
      os_assert_err (!scheduler::locked (), EPERM);
 
      os_assert_err (msg != nullptr, EINVAL);
      os_assert_err (nbytes <= msg_size_bytes_, EMSGSIZE);
 
#if defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE)
 
      return port::message_queue::timed_receive (this, msg, nbytes, timeout,
                                                 mprio);
 
#else
 
      // Extra test before entering the loop, with its inherent weight.
      // Trade size for speed.
      {
        // ----- Enter critical section ---------------------------------------
        interrupts::critical_section ics;
 
        if (internal_try_receive_ (msg, nbytes, mprio))
          {
            return result::ok;
          }
        // ----- Exit critical section ----------------------------------------
      }
 
      thread& crt_thread = this_thread::thread ();
 
      // Prepare a list node pointing to the current thread.
      // Do not worry for being on stack, it is temporarily linked to the
      // list and guaranteed to be removed before this function returns.
      internal::waiting_thread_node node{ crt_thread };
 
      internal::clock_timestamps_list& clock_list = clock_->steady_list ();
      clock::timestamp_t timeout_timestamp = clock_->steady_now () + timeout;
 
      // Prepare a timeout node pointing to the current thread.
      internal::timeout_thread_node timeout_node{ timeout_timestamp,
                                                  crt_thread };
 
      for (;;)
        {
          {
            // ----- Enter critical section -----------------------------------
            interrupts::critical_section ics;
 
            if (internal_try_receive_ (msg, nbytes, mprio))
              {
                return result::ok;
              }
 
            // Add this thread to the message queue receive waiting list,
            // and the clock timeout list.
            scheduler::internal_link_node (receive_list_, node, clock_list,
                                           timeout_node);
            // state::suspended set in above link().
            // ----- Exit critical section ------------------------------------
          }
 
          port::scheduler::reschedule ();
 
          // Remove the thread from the semaphore waiting list,
          // if not already removed by send()and from the clock
          // timeout list, if not already removed by the timer.
          scheduler::internal_unlink_node (node, timeout_node);
 
          if (crt_thread.interrupted ())
            {
#if defined(OS_TRACE_RTOS_MQUEUE)
              trace::printf ("%s(%p,%u,%u) EINTR @%p %s\n", __func__, msg,
                             nbytes, timeout, this, name ());
#endif
              return EINTR;
            }
 
          if (clock_->steady_now () >= timeout_timestamp)
            {
#if defined(OS_TRACE_RTOS_MQUEUE)
              trace::printf ("%s(%p,%u,%u) ETIMEDOUT @%p %s\n", __func__, msg,
                             nbytes, timeout, this, name ());
#endif
              return ETIMEDOUT;
            }
        }
 
      /* NOTREACHED */
      return ENOTRECOVERABLE;
 
#endif
    }
 
    result_t
    message_queue::reset (void)
    {
#if defined(OS_TRACE_RTOS_MQUEUE)
      trace::printf ("%s() @%p %s\n", __func__, this, name ());
#endif
 
      // Don't call this from interrupt handlers.
      os_assert_err (!interrupts::in_handler_mode (), EPERM);
 
#if defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE)
 
      return port::message_queue::reset (this);
 
#else
 
      {
        // ----- Enter critical section ---------------------------------------
        interrupts::critical_section ics;
 
        internal_init_ ();
        return result::ok;
        // ----- Exit critical section ----------------------------------------
      }
 
#endif
    }
 
    // ------------------------------------------------------------------------
 
  } /* namespace rtos */
} /* namespace os */
 
// ----------------------------------------------------------------------------