os-mqueue.cpp

Go to the documentation of this file.

/*
 * This file is part of the µOS++ distribution.
 *   (https://github.com/micro-os-plus)
 * Copyright (c) 2016 Liviu Ionescu.
 *
 * 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.
 */
 
#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);
 
      // 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));
 
#if !defined(NDEBUG)
      char* p =
          reinterpret_cast<char*> (reinterpret_cast<char*> (const_cast<priority_t*> (prio_array_))
              + msgs * sizeof(priority_t));
 
      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.
      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;
        }
 
      // 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.
      char* dest = static_cast<char*> (first_free_);
 
      // 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.
              std::memset (dest + nbytes, 0x00, msg_size_bytes_ - nbytes);
            }
          // ----- Exit uncritical section ------------------------------------
        }
 
      // The third step is to link the buffer to the list.
 
      // 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);
        }
 
      // 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.
      char* src = static_cast<char*> (queue_addr_) + head_ * msg_size_bytes_;
      priority_t prio = prio_array_[head_];
 
#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.
          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_];
        }
      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 */
 
// ----------------------------------------------------------------------------