os-mempool.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>
#include <memory>
 
// ----------------------------------------------------------------------------
 
#if defined(__clang__)
#pragma clang diagnostic ignored "-Wc++98-compat"
#endif
 
// ----------------------------------------------------------------------------
 
namespace os
{
  namespace rtos
  {
 
    // ------------------------------------------------------------------------
 
    const memory_pool::attributes memory_pool::initializer;
 
    // ------------------------------------------------------------------------
 
    // ------------------------------------------------------------------------
    // Protected internal constructor.
    memory_pool::memory_pool ()
    {
#if defined(OS_TRACE_RTOS_MEMPOOL)
      trace::printf ("%s() @%p %s\n", __func__, this, this->name ());
#endif
    }
 
    memory_pool::memory_pool (const char* name) :
        object_named_system
          { name }
    {
#if defined(OS_TRACE_RTOS_MEMPOOL)
      trace::printf ("%s() @%p %s\n", __func__, this, this->name ());
#endif
    }
 
    memory_pool::memory_pool (std::size_t blocks, std::size_t block_size_bytes,
                              const attributes& attr,
                              const allocator_type& allocator) :
        memory_pool
          { nullptr, blocks, block_size_bytes, attr, allocator }
    {
      ;
    }
 
    memory_pool::memory_pool (const char* name, std::size_t blocks,
                              std::size_t block_size_bytes,
                              const attributes& attr,
                              const allocator_type& allocator) :
        object_named_system
          { name }
    {
#if defined(OS_TRACE_RTOS_MEMPOOL)
      trace::printf ("%s() @%p %s %u %u\n", __func__, this, this->name (),
                     blocks, block_size_bytes);
#endif
 
      if (attr.mp_pool_address != nullptr)
        {
          // Do not use any allocator at all.
          internal_construct_ (blocks, block_size_bytes, attr, nullptr, 0);
        }
      else
        {
          allocator_ = &allocator;
 
          // If no user storage was provided via attributes,
          // allocate it dynamically via the allocator.
          allocated_pool_size_elements_ = (compute_allocated_size_bytes<
              typename allocator_type::value_type> (blocks, block_size_bytes)
              + sizeof(typename allocator_type::value_type) - 1)
              / sizeof(typename allocator_type::value_type);
 
          allocated_pool_addr_ =
              const_cast<allocator_type&> (allocator).allocate (
                  allocated_pool_size_elements_);
 
          internal_construct_ (
              blocks,
              block_size_bytes,
              attr,
              allocated_pool_addr_,
              allocated_pool_size_elements_
                  * sizeof(typename allocator_type::value_type));
        }
    }
 
    void
    memory_pool::internal_construct_ (std::size_t blocks,
                                      std::size_t block_size_bytes,
                                      const attributes& attr,
                                      void* pool_address,
                                      std::size_t pool_size_bytes)
    {
      // Don't call this from interrupt handlers.
      os_assert_throw(!interrupts::in_handler_mode (), EPERM);
 
#if !defined(OS_USE_RTOS_PORT_MEMORY_POOL)
      clock_ = attr.clock != nullptr ? attr.clock : &sysclock;
#endif
 
      blocks_ = static_cast<memory_pool::size_t> (blocks);
      assert(blocks_ == blocks);
      assert(blocks_ > 0);
 
      // Adjust block size to multiple of pointer.
      // Blocks must be large enough to store a pointer, used
      // to construct the list of free blocks.
      block_size_bytes_ = (static_cast<memory_pool::size_t> (block_size_bytes
          + __SIZEOF_POINTER__ - 1))
          & (static_cast<memory_pool::size_t> (~(__SIZEOF_POINTER__ - 1)));
 
      // If the storage is given explicitly, override attributes.
      if (pool_address != nullptr)
        {
          // The attributes should not define any storage in this case.
          assert(attr.mp_pool_address == nullptr);
 
          pool_addr_ = pool_address;
          pool_size_bytes_ = pool_size_bytes;
        }
      else
        {
          pool_addr_ = attr.mp_pool_address;
          pool_size_bytes_ = attr.mp_pool_size_bytes;
        }
 
      // Blocks must be pointer aligned.
      void* p = pool_addr_;
      std::size_t sz = pool_size_bytes_;
      pool_addr_ = static_cast<char*> (std::align (__SIZEOF_POINTER__,
                                                   blocks_ * block_size_bytes_,
                                                   p, sz));
 
#if defined(OS_TRACE_RTOS_MEMPOOL)
      trace::printf ("%s() @%p %s %u %u %p %u\n", __func__, this, name (),
                     blocks_, block_size_bytes_, pool_addr_, pool_size_bytes_);
#endif
 
      std::size_t storage_size = compute_allocated_size_bytes<void*> (
          blocks_, block_size_bytes_);
 
      if (pool_addr_ != nullptr)
        {
          // The pool must be real, and have a non zero size.
          os_assert_throw(pool_size_bytes_ > 0, EINVAL);
          // The pool must fit the storage.
          os_assert_throw(pool_size_bytes_ >= storage_size, EINVAL);
        }
 
      // The pool must have a real address.
      os_assert_throw(pool_addr_ != nullptr, ENOMEM);
 
      internal_init_ ();
 
    }
 
    memory_pool::~memory_pool ()
    {
#if defined(OS_TRACE_RTOS_MEMPOOL)
      trace::printf ("%s() @%p %s\n", __func__, this, name ());
#endif
 
      // There must be no threads waiting for this pool.
      assert(list_.empty ());
 
      typedef typename std::allocator_traits<allocator_type>::pointer pointer;
 
      if (allocated_pool_addr_ != nullptr)
        {
          static_cast<allocator_type*> (const_cast<void*> (allocator_))->deallocate (
              static_cast<pointer> (allocated_pool_addr_),
              allocated_pool_size_elements_);
        }
    }
 
    /*
     * Construct the linked list of blocks and initialise the
     * internal pointers and counters.
     */
    void
    memory_pool::internal_init_ (void)
    {
      // 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*> (pool_addr_);
      for (std::size_t i = 1; i < blocks_; ++i)
        {
          // Compute the address of the next block;
          char* pn = p + block_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_ = pool_addr_; // Pointer to first block.
 
      count_ = 0; // No allocated blocks.
    }
 
    /*
     * Internal function used to return the first block in the
     * free list.
     * Should be called from an interrupts critical section.
     */
    void*
    memory_pool::internal_try_first_ (void)
    {
      if (first_ != nullptr)
        {
          void* p = static_cast<void*> (first_);
          first_ = *(static_cast<void**> (first_));
 
#pragma GCC diagnostic push
#if defined(__clang__)
#pragma clang diagnostic ignored "-Wdeprecated-volatile"
#endif
          ++count_;
#pragma GCC diagnostic pop
 
          return p;
        }
 
      return nullptr;
    }
 
    void*
    memory_pool::alloc (void)
    {
#if defined(OS_TRACE_RTOS_MEMPOOL)
      trace::printf ("%s() @%p %s\n", __func__, this, name ());
#endif
 
      // Don't call this from interrupt handlers.
      os_assert_throw(!interrupts::in_handler_mode (), EPERM);
      // Don't call this from critical regions.
      os_assert_throw(!scheduler::locked (), EPERM);
 
      void* p;
 
      // Extra test before entering the loop, with its inherent weight.
      // Trade size for speed.
        {
          // ----- Enter critical section -------------------------------------
          interrupts::critical_section ics;
 
          p = internal_try_first_ ();
          if (p != nullptr)
            {
#if defined(OS_TRACE_RTOS_MEMPOOL)
              trace::printf ("%s()=%p @%p %s\n", __func__, p, this, name ());
#endif
              return p;
            }
          // ----- 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;
 
              p = internal_try_first_ ();
              if (p != nullptr)
                {
#if defined(OS_TRACE_RTOS_MEMPOOL)
                  trace::printf ("%s()=%p @%p %s\n", __func__, p, this,
                                 name ());
#endif
                  return p;
                }
 
              // Add this thread to the memory pool waiting list.
              scheduler::internal_link_node (list_, node);
              // state::suspended set in above link().
              // ----- Exit critical section ----------------------------------
            }
 
          port::scheduler::reschedule ();
 
          // Remove the thread from the memory pool waiting list,
          // if not already removed by free().
          scheduler::internal_unlink_node (node);
 
          if (this_thread::thread ().interrupted ())
            {
#if defined(OS_TRACE_RTOS_MEMPOOL)
              trace::printf ("%s() INTR @%p %s\n", __func__, this, name ());
#endif
              return nullptr;
            }
        }
 
      /* NOTREACHED */
    }
 
    void*
    memory_pool::try_alloc (void)
    {
#if defined(OS_TRACE_RTOS_MEMPOOL)
      trace::printf ("%s() @%p %s\n", __func__, this, name ());
#endif
 
      // Don't call this from high priority interrupts.
      assert(port::interrupts::is_priority_valid ());
 
      void* p;
        {
          // ----- Enter critical section -------------------------------------
          interrupts::critical_section ics;
 
          p = internal_try_first_ ();
          // ----- Exit critical section --------------------------------------
        }
 
#if defined(OS_TRACE_RTOS_MEMPOOL)
      trace::printf ("%s()=%p @%p %s\n", __func__, p, this, name ());
#endif
      return p;
    }
 
    void*
    memory_pool::timed_alloc (clock::duration_t timeout)
    {
#if defined(OS_TRACE_RTOS_MEMPOOL)
      trace::printf ("%s(%u) @%p %s\n", __func__,
                     static_cast<unsigned int> (timeout), this, name ());
#endif
 
      // Don't call this from interrupt handlers.
      os_assert_throw(!interrupts::in_handler_mode (), EPERM);
      // Don't call this from critical regions.
      os_assert_throw(!scheduler::locked (), EPERM);
 
      void* p;
 
      // Extra test before entering the loop, with its inherent weight.
      // Trade size for speed.
        {
          // ----- Enter critical section -------------------------------------
          interrupts::critical_section ics;
 
          p = internal_try_first_ ();
          if (p != nullptr)
            {
#if defined(OS_TRACE_RTOS_MEMPOOL)
              trace::printf ("%s()=%p @%p %s\n", __func__, p, this, name ());
#endif
              return p;
            }
          // ----- 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;
 
              p = internal_try_first_ ();
              if (p != nullptr)
                {
#if defined(OS_TRACE_RTOS_MEMPOOL)
                  trace::printf ("%s()=%p @%p %s\n", __func__, p, this,
                                 name ());
#endif
                  return p;
                }
 
              // Add this thread to the memory pool waiting list,
              // and the clock timeout list.
              scheduler::internal_link_node (list_, node, clock_list,
                                             timeout_node);
              // state::suspended set in above link().
              // ----- Exit critical section ----------------------------------
            }
 
          port::scheduler::reschedule ();
 
          // Remove the thread from the memory pool waiting list,
          // if not already removed by free() and from the clock
          // timeout list, if not already removed by the timer.
          scheduler::internal_unlink_node (node, timeout_node);
 
          if (this_thread::thread ().interrupted ())
            {
#if defined(OS_TRACE_RTOS_MEMPOOL)
              trace::printf ("%s() INTR @%p %s\n", __func__, this, name ());
#endif
              return nullptr;
            }
 
          if (clock_->steady_now () >= timeout_timestamp)
            {
#if defined(OS_TRACE_RTOS_MEMPOOL)
              trace::printf ("%s() TMO @%p %s\n", __func__, this, name ());
#endif
              return nullptr;
            }
        }
 
      /* NOTREACHED */
    }
 
    result_t
    memory_pool::free (void* block)
    {
#if defined(OS_TRACE_RTOS_MEMPOOL)
      trace::printf ("%s(%p) @%p %s\n", __func__, block, this, name ());
#endif
 
      // Don't call this from high priority interrupts.
      assert(port::interrupts::is_priority_valid ());
 
      // Validate pointer.
      if ((block < pool_addr_)
          || (block
              >= (static_cast<char*> (pool_addr_) + blocks_ * block_size_bytes_)))
        {
#if defined(OS_TRACE_RTOS_MEMPOOL)
          trace::printf ("%s(%p) EINVAL @%p %s\n", __func__, block, this,
                         name ());
#endif
          return EINVAL;
        }
 
        {
          // ----- Enter critical section -------------------------------------
          interrupts::critical_section ics;
 
          // 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**> (block)) = first_;
 
          // Now this block is the first one.
          first_ = block;
 
#pragma GCC diagnostic push
#if defined(__clang__)
#pragma clang diagnostic ignored "-Wdeprecated-volatile"
#endif
          --count_;
#pragma GCC diagnostic pop
 
          // ----- Exit critical section --------------------------------------
        }
 
      // Wake-up one thread, if any.
      list_.resume_one ();
 
      return result::ok;
    }
 
    result_t
    memory_pool::reset (void)
    {
#if defined(OS_TRACE_RTOS_MEMPOOL)
      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);
 
        {
          // ----- Enter critical section -------------------------------------
          interrupts::critical_section ics;
 
          internal_init_ ();
          // ----- Exit critical section --------------------------------------
        }
 
      // Wake-up all threads, if any.
      // Need not be inside the critical section,
      // the list is protected by inner `resume_one()`.
      list_.resume_all ();
 
      return result::ok;
    }
 
  // --------------------------------------------------------------------------
 
  } /* namespace rtos */
} /* namespace os */
 
// ----------------------------------------------------------------------------