free_list.cpp

Go to the documentation of this file.

 
/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
 
#include <lockfree/free_list.h>
#include <allocators/aligned.h>
 
using namespace ros;
 
namespace lockfree
{
 
FreeList::FreeList()
: blocks_(0)
, next_(0)
, block_size_(0)
, block_count_(0)
{
}
 
FreeList::FreeList(uint32_t block_size, uint32_t block_count)
: blocks_(0)
, next_(0)
, block_size_(0)
, block_count_(0)
{
  initialize(block_size, block_count);
}
 
FreeList::~FreeList()
{
  for (uint32_t i = 0; i < block_count_; ++i)
  {
    next_[i].~atomic_uint32_t();
  }
 
  allocators::alignedFree(blocks_);
  allocators::alignedFree(next_);
}
 
void FreeList::initialize(uint32_t block_size, uint32_t block_count)
{
  ROS_ASSERT(!blocks_);
  ROS_ASSERT(!next_);
 
  block_size_ = block_size;
  block_count_ = block_count;
 
  head_.store(0);
  alloc_count_.store(0);
 
  blocks_ = (uint8_t*)allocators::alignedMalloc(block_size * block_count, ROSRT_CACHELINE_SIZE);
  next_ = (atomic_uint32_t*)allocators::alignedMalloc(sizeof(atomic_uint32_t) * block_count, ROSRT_CACHELINE_SIZE);
 
  memset(blocks_, 0xCD, block_size * block_count);
 
  for (uint32_t i = 0; i < block_count_; ++i)
  {
    new (next_ + i) atomic_uint32_t();
 
    if (i == block_count_ - 1)
    {
      next_[i].store(0xffffffffUL);
    }
    else
    {
      next_[i].store(i + 1);
    }
  }
}
 
bool FreeList::hasOutstandingAllocations()
{
  return alloc_count_.load() == 0;
}
 
void* FreeList::allocate()
{
#if FREE_LIST_DEBUG
  initDebug();
#endif
 
  ROS_ASSERT(blocks_);
 
  while (true)
  {
    uint64_t head = head_.load(memory_order_consume);
 
#if FREE_LIST_DEBUG
    typename Debug::Item i;
    i.head = head;
    i.time = ros::WallTime::now();
    i.op = Debug::Alloc;
#endif
 
    if (getVal(head) == 0xffffffffULL)
    {
#if FREE_LIST_DEBUG
      debug_->items.push_back(i);
#endif
      return 0;  // Allocation failed
    }
 
    FREELIST_DEBUG_YIELD();
 
    // Load the next index
    uint64_t new_head = next_[getVal(head)].load();
 
    FREELIST_DEBUG_YIELD();
 
    // Increment the tag to avoid ABA
    setTag(new_head, getTag(head) + 1);
 
#if FREE_LIST_DEBUG
    i.new_head = new_head;
#endif
 
    FREELIST_DEBUG_YIELD();
 
    // If setting head to next is successful, return the item at next
    if (head_.compare_exchange_strong(head, new_head))
    {
#if FREE_LIST_DEBUG
      i.addr = blocks_ + (block_size_ * getVal(head));
      i.success = 1;
      debug_->items.push_back(i);
#endif
      alloc_count_.fetch_add(1);
      return static_cast<void*>(blocks_ + (block_size_ * getVal(head)));
    }
 
#if FREE_LIST_DEBUG
      i.success = 0;
      debug_->items.push_back(i);
#endif
  }
}
 
void FreeList::free(void const* mem)
{
  if (!mem)
  {
    return;
  }
 
#if FREE_LIST_DEBUG
  initDebug();
#endif
 
  uint32_t index = (static_cast<uint8_t const*>(mem) - blocks_) / block_size_;
 
  ROS_ASSERT(((static_cast<uint8_t const*>(mem) - blocks_) % block_size_) == 0);
  ROS_ASSERT(owns(mem));
 
  while (true)
  {
    // Load head
    uint64_t head = head_.load(memory_order_consume);
 
#if FREE_LIST_DEBUG
    typename Debug::Item i;
    i.head = head;
    i.time = ros::WallTime::now();
    i.op = Debug::Free;
#endif
 
    FREELIST_DEBUG_YIELD();
 
    uint64_t new_head = head;
    // set new head to the index of the block we're currently freeing
    setVal(new_head, index);
    // Increment the tag to avoid ABA
    setTag(new_head, getTag(new_head) + 1);
 
 
    FREELIST_DEBUG_YIELD();
 
    // Store head as next index for this item
    next_[index].store(getVal(head));
 
    FREELIST_DEBUG_YIELD();
 
#if FREE_LIST_DEBUG
    i.new_head = new_head;
#endif
 
    FREELIST_DEBUG_YIELD();
 
    // If setting the head to next is successful, return
    if (head_.compare_exchange_strong(head, new_head))
    {
#if FREE_LIST_DEBUG
      i.success = 1;
      i.addr = blocks_ + (block_size_ * index);
      debug_->items.push_back(i);
#endif
      alloc_count_.fetch_sub(1);
      return;
    }
 
#if FREE_LIST_DEBUG
      i.success = 0;
      debug_->items.push_back(i);
#endif
  }
}
 
bool FreeList::owns(void const* mem)
{
  uint32_t sub = (static_cast<uint8_t const*>(mem) - blocks_);
  return sub < block_count_ * block_size_;
}
 
} // namespace lockfree