os-mqueue.h

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.
 */

#ifndef CMSIS_PLUS_RTOS_OS_MQUEUE_H_
#define CMSIS_PLUS_RTOS_OS_MQUEUE_H_

// ----------------------------------------------------------------------------

#if defined(__cplusplus)

#include <cmsis-plus/rtos/os-decls.h>
#include <cmsis-plus/rtos/os-memory.h>

#include <cmsis-plus/diag/trace.h>

// ----------------------------------------------------------------------------

namespace os
{
  namespace rtos
  {

    // ========================================================================

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpadded"

    class message_queue : public internal::object_named_system
    {
    public:

      // ======================================================================

#if defined(OS_BOOL_RTOS_MESSAGE_QUEUE_SIZE_16BITS)
      using size_t = uint16_t;
#else
      using size_t = uint8_t;
#endif

      static constexpr message_queue::size_t max_size = 0xFF;

      using msg_size_t = uint16_t;

      static constexpr msg_size_t max_msg_size = 0xFFFF;

      using index_t = message_queue::size_t;

      static constexpr index_t no_index = max_size;

      using priority_t = uint8_t;

      static constexpr priority_t default_priority = 0;

      static constexpr priority_t max_priority = 0xFF;

      // ======================================================================

      class attributes : public internal::attributes_clocked
      {
      public:

        constexpr
        attributes ();

        // The rule of five.
        attributes (const attributes&) = default;
        attributes (attributes&&) = default;
        attributes&
        operator= (const attributes&) = default;
        attributes&
        operator= (attributes&&) = default;

        ~attributes () = default;

      public:

        // Public members; no accessors and mutators required.
        // Warning: must match the type & order of the C file header.
        void* mq_queue_address = nullptr;

        std::size_t mq_queue_size_bytes = 0;

        // Add more attributes here.

      }; /* class attributes */

      static const attributes initializer;

      using allocator_type = memory::allocator<thread::stack::allocation_element_t>;

      template<typename T, std::size_t msgs, std::size_t msg_size_bytes>
        class arena
        {
        public:
          T queue[(msgs * msg_size_bytes + sizeof(T) - 1) / sizeof(T)];
          T links[((2 * msgs) * sizeof(index_t) + sizeof(T) - 1) / sizeof(T)];
          T prios[(msgs * sizeof(priority_t) + sizeof(T) - 1) / sizeof(T)];
        };

      template<typename T>
        constexpr std::size_t
        compute_allocated_size_bytes (std::size_t msgs,
                                      std::size_t msg_size_bytes)
        {
          // Align each message
          return (msgs * ((msg_size_bytes + (sizeof(T) - 1)) & ~(sizeof(T) - 1)))
          // Align the indices array
              + ((2 * msgs * sizeof(index_t) + (sizeof(T) - 1))
                  & ~(sizeof(T) - 1))
              // Align the priority array
              + ((msgs * sizeof(priority_t) + (sizeof(T) - 1))
                  & ~(sizeof(T) - 1));
        }

      // ======================================================================

      message_queue (std::size_t msgs, std::size_t msg_size_bytes,
                     const attributes& attr = initializer,
                     const allocator_type& allocator = allocator_type ());

      message_queue (const char* name, std::size_t msgs,
                     std::size_t msg_size_bytes, const attributes& attr =
                         initializer,
                     const allocator_type& allocator = allocator_type ());

    protected:

      // Internal constructors, used from templates.
      message_queue ();
      message_queue (const char* name);

    public:

      // The rule of five.
      message_queue (const message_queue&) = delete;
      message_queue (message_queue&&) = delete;
      message_queue&
      operator= (const message_queue&) = delete;
      message_queue&
      operator= (message_queue&&) = delete;

      virtual
      ~message_queue ();

      bool
      operator== (const message_queue& rhs) const;

    public:

      result_t
      send (const void* msg, std::size_t nbytes, priority_t mprio =
                default_priority);

      result_t
      try_send (const void* msg, std::size_t nbytes, priority_t mprio =
                    default_priority);

      result_t
      timed_send (const void* msg, std::size_t nbytes,
                  clock::duration_t timeout,
                  priority_t mprio = default_priority);

      result_t
      receive (void* msg, std::size_t nbytes, priority_t* mprio = nullptr);

      result_t
      try_receive (void* msg, std::size_t nbytes, priority_t* mprio = nullptr);

      result_t
      timed_receive (void* msg, std::size_t nbytes, clock::duration_t timeout,
                     priority_t* mprio = nullptr);

      // TODO: check if some kind of peek() is useful.

      std::size_t
      capacity (void) const;

      std::size_t
      length (void) const;

      std::size_t
      msg_size (void) const;

      bool
      empty (void) const;

      bool
      full (void) const;

      result_t
      reset (void);

    protected:

      void
      internal_construct_ (std::size_t msgs, std::size_t msg_size_bytes,
                           const attributes& attr, void* queue_address,
                           std::size_t queue_size_bytes);

      void
      internal_init_ (void);

#if !defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE)

      bool
      internal_try_send_ (const void* msg, std::size_t nbytes,
                          priority_t mprio);

      bool
      internal_try_receive_ (void* msg, std::size_t nbytes, priority_t* mprio);

#endif /* !defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE) */

    protected:

      // Keep these in sync with the structure declarations in os-c-decl.h.
#if !defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE)

      internal::waiting_threads_list send_list_;
      internal::waiting_threads_list receive_list_;
      clock* clock_ = nullptr;

      // To save space, the double linked list is built
      // using short indexes, not pointers.
      volatile index_t* prev_array_ = nullptr;
      volatile index_t* next_array_ = nullptr;
      volatile priority_t* prio_array_ = nullptr;

      void* volatile first_free_ = nullptr;
#endif /* !defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE) */

      void* queue_addr_ = nullptr;
      void* allocated_queue_addr_ = nullptr;
      const void* allocator_ = nullptr;

#if defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE)
      friend class port::message_queue;
      os_mqueue_port_data_t port_;
#endif

      std::size_t queue_size_bytes_ = 0;
      std::size_t allocated_queue_size_elements_ = 0;

      message_queue::msg_size_t msg_size_bytes_ = 0;
      message_queue::size_t msgs_ = 0;
      message_queue::size_t count_ = 0;

#if !defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE)

      index_t head_ = 0;
#endif /* !defined(OS_USE_RTOS_PORT_MESSAGE_QUEUE) */

    };

    // ========================================================================

    template<typename Allocator = memory::allocator<void*>>
      class message_queue_allocated : public message_queue
      {
      public:

        using allocator_type = Allocator;

        message_queue_allocated (std::size_t msgs, std::size_t msg_size_bytes,
                                 const attributes& attr = initializer,
                                 const allocator_type& allocator =
                                     allocator_type ());

        message_queue_allocated (const char* name, std::size_t msgs,
                                 std::size_t msg_size_bytes,
                                 const attributes& attr = initializer,
                                 const allocator_type& allocator =
                                     allocator_type ());

      public:

        // The rule of five.
        message_queue_allocated (const message_queue_allocated&) = delete;
        message_queue_allocated (message_queue_allocated&&) = delete;
        message_queue_allocated&
        operator= (const message_queue_allocated&) = delete;
        message_queue_allocated&
        operator= (message_queue_allocated&&) = delete;

        virtual
        ~message_queue_allocated ();

      };

    // ========================================================================

    template<typename T, typename Allocator = memory::allocator<void*>>
      class message_queue_typed : public message_queue_allocated<Allocator>
      {
      public:

        using value_type = T;

        using allocator_type = Allocator;

        message_queue_typed (std::size_t msgs,
                             const message_queue::attributes& attr =
                                 message_queue::initializer,
                             const allocator_type& allocator =
                                 allocator_type ());

        message_queue_typed (const char* name, std::size_t msgs,
                             const message_queue::attributes& attr =
                                 message_queue::initializer,
                             const allocator_type& allocator =
                                 allocator_type ());

        // The rule of five.
        message_queue_typed (const message_queue_typed&) = delete;
        message_queue_typed (message_queue_typed&&) = delete;
        message_queue_typed&
        operator= (const message_queue_typed&) = delete;
        message_queue_typed&
        operator= (message_queue_typed&&) = delete;

        virtual
        ~message_queue_typed ();

      public:

        result_t
        send (const value_type* msg, message_queue::priority_t mprio =
                  message_queue::default_priority);

        result_t
        try_send (const value_type* msg, message_queue::priority_t mprio =
                      message_queue::default_priority);

        result_t
        timed_send (const value_type* msg, clock::duration_t timeout,
                    message_queue::priority_t mprio =
                        message_queue::default_priority);

        result_t
        receive (value_type* msg, message_queue::priority_t* mprio = nullptr);

        result_t
        try_receive (value_type* msg,
                     message_queue::priority_t* mprio = nullptr);

        result_t
        timed_receive (value_type* msg, clock::duration_t timeout,
                       message_queue::priority_t* mprio = nullptr);

      };

    // ========================================================================

    template<typename T, std::size_t N>
      class message_queue_inclusive : public message_queue
      {
      public:

        using value_type = T;

        static const std::size_t msgs = N;

        message_queue_inclusive (const attributes& attr = initializer);

        message_queue_inclusive (const char* name, const attributes& attr =
                                     initializer);

        // The rule of five.
        message_queue_inclusive (const message_queue_inclusive&) = delete;
        message_queue_inclusive (message_queue_inclusive&&) = delete;
        message_queue_inclusive&
        operator= (const message_queue_inclusive&) = delete;
        message_queue_inclusive&
        operator= (message_queue_inclusive&&) = delete;

        virtual
        ~message_queue_inclusive ();

      public:

        result_t
        send (const value_type* msg, priority_t mprio = default_priority);

        result_t
        try_send (const value_type* msg, priority_t mprio = default_priority);

        result_t
        timed_send (const value_type* msg, clock::duration_t timeout,
                    priority_t mprio = default_priority);

        result_t
        receive (value_type* msg, priority_t* mprio = nullptr);

        result_t
        try_receive (value_type* msg, priority_t* mprio = nullptr);

        result_t
        timed_receive (value_type* msg, clock::duration_t timeout,
                       priority_t* mprio = nullptr);

      protected:

        arena<void*, msgs, sizeof(value_type)> arena_;

      };

#pragma GCC diagnostic pop

  }
/* namespace rtos */
} /* namespace os */

// ===== Inline & template implementations ====================================

namespace os
{
  namespace rtos
  {
    constexpr
    message_queue::attributes::attributes ()
    {
      ;
    }

    // ========================================================================

    inline bool
    message_queue::operator== (const message_queue& rhs) const
    {
      return this == &rhs;
    }

    inline std::size_t
    message_queue::length (void) const
    {
      return count_;
    }

    inline std::size_t
    message_queue::capacity (void) const
    {
      return msgs_;
    }

    inline std::size_t
    message_queue::msg_size (void) const
    {
      return msg_size_bytes_;
    }

    inline bool
    message_queue::empty (void) const
    {
      return (length () == 0);
    }

    inline bool
    message_queue::full (void) const
    {
      return (length () == capacity ());
    }

    // ========================================================================

    template<typename Allocator>
      inline
      message_queue_allocated<Allocator>::message_queue_allocated (
          std::size_t msgs, std::size_t msg_size_bytes, const attributes& attr,
          const allocator_type& allocator) :
          message_queue_allocated
            { nullptr, msgs, msg_size_bytes, attr, allocator }
      {
        ;
      }

    template<typename Allocator>
      message_queue_allocated<Allocator>::message_queue_allocated (
          const char* name, std::size_t msgs, std::size_t msg_size_bytes,
          const attributes& attr, const allocator_type& allocator) :
          message_queue
            { name }
      {
#if defined(OS_TRACE_RTOS_MQUEUE)
        trace::printf ("%s() @%p %s %d %d\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));
          }
      }

    template<typename Allocator>
      message_queue_allocated<Allocator>::~message_queue_allocated ()
      {
#if defined(OS_TRACE_RTOS_MQUEUE)
        trace::printf ("%s() @%p %s\n", __func__, this, name ());
#endif
        typedef typename std::allocator_traits<allocator_type>::pointer pointer;

        if (allocated_queue_addr_ != nullptr)
          {
            static_cast<allocator_type*> (const_cast<void*> (allocator_))->deallocate (
                static_cast<pointer> (allocated_queue_addr_),
                allocated_queue_size_elements_);

            allocated_queue_addr_ = nullptr;
          }
      }

    // ========================================================================

    template<typename T, typename Allocator>
      inline
      message_queue_typed<T, Allocator>::message_queue_typed (
          std::size_t msgs, const message_queue::attributes& attr,
          const allocator_type& allocator) :
          message_queue_allocated<allocator_type>
            { msgs, sizeof(value_type), attr, allocator }
      {
        ;
      }

    template<typename T, typename Allocator>
      inline
      message_queue_typed<T, Allocator>::message_queue_typed (
          const char* name, std::size_t msgs,
          const message_queue::attributes& attr,
          const allocator_type& allocator) :
          message_queue_allocated<allocator_type>
            { name, msgs, sizeof(value_type), attr, allocator }
      {
        ;
      }

    template<typename T, typename Allocator>
      message_queue_typed<T, Allocator>::~message_queue_typed ()
      {
        ;
      }

    template<typename T, typename Allocator>
      inline result_t
      message_queue_typed<T, Allocator>::send (const value_type* msg,
                                               message_queue::priority_t mprio)
      {
        return message_queue_allocated<allocator_type>::send (
            reinterpret_cast<const char*> (msg), sizeof(value_type), mprio);
      }

    template<typename T, typename Allocator>
      inline result_t
      message_queue_typed<T, Allocator>::try_send (
          const value_type* msg, message_queue::priority_t mprio)
      {
        return message_queue_allocated<allocator_type>::try_send (
            reinterpret_cast<const char*> (msg), sizeof(value_type), mprio);
      }

    template<typename T, typename Allocator>
      inline result_t
      message_queue_typed<T, Allocator>::timed_send (
          const value_type* msg, clock::duration_t timeout,
          message_queue::priority_t mprio)
      {
        return message_queue_allocated<allocator_type>::timed_send (
            reinterpret_cast<const char*> (msg), sizeof(value_type), timeout,
            mprio);
      }

    template<typename T, typename Allocator>
      inline result_t
      message_queue_typed<T, Allocator>::receive (
          value_type* msg, message_queue::priority_t* mprio)
      {
        return message_queue_allocated<allocator_type>::receive (
            reinterpret_cast<char*> (msg), sizeof(value_type), mprio);
      }

    template<typename T, typename Allocator>
      inline result_t
      message_queue_typed<T, Allocator>::try_receive (
          value_type* msg, message_queue::priority_t* mprio)
      {
        return message_queue_allocated<allocator_type>::try_receive (
            reinterpret_cast<char*> (msg), sizeof(value_type), mprio);
      }

    template<typename T, typename Allocator>
      inline result_t
      message_queue_typed<T, Allocator>::timed_receive (
          value_type* msg, clock::duration_t timeout,
          message_queue::priority_t* mprio)
      {
        return message_queue_allocated<allocator_type>::timed_receive (
            reinterpret_cast<char*> (msg), sizeof(value_type), timeout, mprio);
      }

    // ========================================================================

    template<typename T, std::size_t N>
      inline
      message_queue_inclusive<T, N>::message_queue_inclusive (
          const attributes& attr) :
          message_queue_inclusive
            { nullptr, attr }
      {
        ;
      }

    template<typename T, std::size_t N>
      message_queue_inclusive<T, N>::message_queue_inclusive (
          const char* name, const attributes& attr) :
          message_queue (name)
      {
        static_assert(sizeof(T) >= sizeof(void*), "Messages of message_queue need to have at least the size of a pointer");

        internal_construct_ (msgs, sizeof(value_type), attr, &arena_,
                             sizeof(arena_));
      }

    template<typename T, std::size_t N>
      message_queue_inclusive<T, N>::~message_queue_inclusive ()
      {
        ;
      }

    template<typename T, std::size_t N>
      inline result_t
      message_queue_inclusive<T, N>::send (const value_type* msg,
                                           priority_t mprio)
      {
        return message_queue::send (reinterpret_cast<const char*> (msg),
                                    sizeof(value_type), mprio);
      }

    template<typename T, std::size_t N>
      inline result_t
      message_queue_inclusive<T, N>::try_send (const value_type* msg,
                                               priority_t mprio)
      {
        return message_queue::try_send (reinterpret_cast<const char*> (msg),
                                        sizeof(value_type), mprio);
      }

    template<typename T, std::size_t N>
      inline result_t
      message_queue_inclusive<T, N>::timed_send (const value_type* msg,
                                                 clock::duration_t timeout,
                                                 priority_t mprio)
      {
        return message_queue::timed_send (reinterpret_cast<const char*> (msg),
                                          sizeof(value_type), timeout, mprio);
      }

    template<typename T, std::size_t N>
      inline result_t
      message_queue_inclusive<T, N>::receive (value_type* msg,
                                              priority_t* mprio)
      {
        return message_queue::receive (reinterpret_cast<char*> (msg),
                                       sizeof(value_type), mprio);
      }

    template<typename T, std::size_t N>
      inline result_t
      message_queue_inclusive<T, N>::try_receive (value_type* msg,
                                                  priority_t* mprio)
      {
        return message_queue::try_receive (reinterpret_cast<char*> (msg),
                                           sizeof(value_type), mprio);
      }

    template<typename T, std::size_t N>
      inline result_t
      message_queue_inclusive<T, N>::timed_receive (value_type* msg,
                                                    clock::duration_t timeout,
                                                    priority_t* mprio)
      {
        return message_queue::timed_receive (reinterpret_cast<char*> (msg),
                                             sizeof(value_type), timeout, mprio);
      }

  } /* namespace rtos */
} /* namespace os */

// ----------------------------------------------------------------------------

#endif /* __cplusplus */

#endif /* CMSIS_PLUS_RTOS_OS_MQUEUE_H_ */