SharedMemoryBlock.cpp
Go to the documentation of this file.
00001 #include "sharedmem_transport/SharedMemoryBlock.h"
00002 
00003 using namespace boost::interprocess;
00004 
00005 namespace sharedmem_transport {
00006 
00007     shm_handle SharedMemoryBlock::connectBlock(boost::interprocess::managed_shared_memory & segment, uint32_t handle) {
00008         assert(handle < ROSSharedMemoryNumBlock);
00009         std::pair<uint8_t *, std::size_t> ret = segment.find<uint8_t>(descriptors[handle].name_);
00010         ROS_DEBUG("Connect block %d: handle %p size %d",handle,ret.first,ret.second);
00011         assert(ret.second >= descriptors[handle].size_);
00012         return shm_handle(handle,descriptors[handle].resize_count_,ret.first);
00013     }
00014 
00015     shm_handle SharedMemoryBlock::findHandle(boost::interprocess::managed_shared_memory & segment, const char * name) {
00016         boost::interprocess::scoped_lock<boost::interprocess::interprocess_mutex> lock(mutex);
00017         check_global_clients(lock);
00018         ROS_DEBUG("Find handle: searching %s",name);
00019         for (int32_t i=0;i<ROSSharedMemoryNumBlock;i++) {
00020             if (!descriptors[i].active_) {
00021                 ROS_DEBUG("Find handle %d inactive",i); 
00022                 continue;
00023             }
00024             ROS_DEBUG("Find handle %d: %s",i,descriptors[i].name_); 
00025             if (descriptors[i].matchName(name)) {
00026                 return connectBlock(segment,i);
00027             }
00028         }
00029         ROS_DEBUG("Find handle: not found");
00030         return shm_handle();
00031     }
00032 
00033     shm_handle SharedMemoryBlock::allocateBlock(boost::interprocess::managed_shared_memory & segment, 
00034             const char * name, uint32_t size) {
00035         uint32_t i;
00036         boost::interprocess::scoped_lock<boost::interprocess::interprocess_mutex> lock(mutex);
00037         check_global_clients(lock);
00038         for (i=0;i<ROSSharedMemoryNumBlock;i++) {
00039             if (!descriptors[i].active_) continue;
00040             if (descriptors[i].matchName(name)) {
00041                 std::pair<uint8_t *, std::size_t> ret = segment.find<uint8_t>(descriptors[i].name_);
00042                 uint8_t *ptr = ret.first;
00043                 if (size > ret.second) {
00044                     segment.destroy<uint8_t>(descriptors[i].name_);
00045                     ptr = segment.find_or_construct<uint8_t>(descriptors[i].name_)[size](0);
00046                     descriptors[i].recordSize(size,size);
00047                 } else {
00048                     descriptors[i].recordSize(size,ret.second);
00049                 }
00050                 ROS_INFO("allocateBlock: reconnected block %s to handle %d, ptr %p",
00051                         name,i,ptr);
00052                 return shm_handle(i,descriptors[i].resize_count_,ptr);
00053             }
00054         }
00055         // new object
00056         for (i=0;i<ROSSharedMemoryNumBlock;i++) {
00057             if (descriptors[i].active_) continue;
00058             descriptors[i].allocate(name);
00059             descriptors[i].recordSize(size,size);
00060             uint8_t * ptr = segment.find_or_construct<uint8_t>(descriptors[i].name_)[size](0);
00061             ROS_INFO("allocateBlock: connected block %s to handle %d, ptr %p",
00062                     name,i,ptr);
00063             return shm_handle(i,descriptors[i].resize_count_,ptr);
00064         }
00065 
00066         ROS_INFO("allocateBlock: No free block for %s",name);
00067         return shm_handle();
00068     }
00069 
00070     void SharedMemoryBlock::resetBlock(boost::interprocess::managed_shared_memory & segment, shm_handle & shm) {
00071         scoped_lock<interprocess_mutex> lock(descriptors[shm.handle].mutex);
00072         descriptors[shm.handle].check_clients(lock);
00073         scoped_lock<interprocess_mutex> lockg(mutex);
00074         check_global_clients(lockg);
00075         if (shm.is_valid()) {
00076             assert(shm.handle < ROSSharedMemoryNumBlock);
00077             segment.destroy<uint8_t>(descriptors[shm.handle].name_);
00078             descriptors[shm.handle].reset();
00079             shm = shm_handle();
00080         }
00081     }
00082 
00083     void SharedMemoryBlock::resetAllBlocks(boost::interprocess::managed_shared_memory & segment) {
00084         boost::interprocess::scoped_lock<boost::interprocess::interprocess_mutex> lock(mutex);
00085         check_global_clients(lock);
00086         for (uint32_t i=0;i<ROSSharedMemoryNumBlock;i++) {
00087             if (!descriptors[i].active_) continue;
00088             segment.destroy<uint8_t>(descriptors[i].name_);
00089             descriptors[i].reset();
00090         }
00091     }
00092 
00093     void SharedMemoryBlock::reallocateBlock(boost::interprocess::managed_shared_memory & segment, 
00094             shm_handle & shm, uint32_t size) {
00095         if (!shm.is_valid()) {
00096             ROS_ERROR("Can't reallocateBlock associated to invalid handle");
00097             return;
00098         }
00099         scoped_lock<interprocess_mutex> lock(descriptors[shm.handle].mutex);
00100         ROS_DEBUG("reallocateBlock: locked %d, checking clients",shm.handle);
00101         descriptors[shm.handle].check_clients(lock);
00102         ROS_DEBUG("reallocateBlock: locked %d, clients checked",shm.handle);
00103         // First check if the block has been changed since last access, and
00104         // update the pointer if necessary
00105         if (shm.resize_count != descriptors[shm.handle].resize_count_) {
00106             scoped_lock<interprocess_mutex> lockg(mutex);
00107             ROS_DEBUG("reallocateBlock: locked global, checking clients");
00108             check_global_clients(lockg);
00109             ROS_DEBUG("reallocateBlock: locked global, client checked");
00110             std::pair<uint8_t *, std::size_t> ret = segment.find<uint8_t>(descriptors[shm.handle].name_);
00111             shm.resize_count = descriptors[shm.handle].resize_count_;
00112             shm.ptr = ret.first;
00113             ROS_DEBUG("reallocateBlock: unlocking global");
00114         }
00115         // Then check if the size need to be expanded
00116         if (size > descriptors[shm.handle].allocated_) {
00117             scoped_lock<interprocess_mutex> lockg(mutex);
00118             ROS_DEBUG("reallocateBlock2: locked global, checking clients");
00119             check_global_clients(lockg);
00120             ROS_INFO("Reallocating block %s:%p from %d to %d bytes",descriptors[shm.handle].name_,
00121                     shm.ptr, descriptors[shm.handle].allocated_,size);
00122             segment.destroy<uint8_t>(descriptors[shm.handle].name_);
00123             try {
00124                 uint8_t * ptr = segment.find_or_construct<uint8_t>(descriptors[shm.handle].name_)[size](0);
00125                 shm.ptr = ptr;
00126                 descriptors[shm.handle].recordSize(size,size);
00127                 shm.resize_count = descriptors[shm.handle].resize_count_;
00128             } catch (boost::interprocess::bad_alloc e) {
00129                 ROS_ERROR("Failed to reallocate memory block, resetting block");
00130                 descriptors[shm.handle].reset();
00131                 shm = shm_handle();
00132             }
00133             ROS_DEBUG("reallocateBlock2: unlocking global");
00134         } else {
00135             descriptors[shm.handle].recordSize(size);
00136         }
00137         ROS_DEBUG("reallocateBlock: unlocking %d",shm.handle);
00138     }
00139 
00140     std::vector<SharedMemBlock> SharedMemoryBlock::getBlockList() const {
00141         unsigned int i;
00142         std::vector<SharedMemBlock> out;
00143         for (i=0;i<ROSSharedMemoryNumBlock;i++) {
00144             if (!descriptors[i].active_) continue;
00145             SharedMemBlock block;
00146             block.handle = i;
00147             block.size = descriptors[i].size_;
00148             block.allocated = descriptors[i].allocated_;
00149             block.name = descriptors[i].name_;
00150             out.push_back(block);
00151         }
00152         return out;
00153     }
00154 
00155 } //namespace image_transport
00156 


sharedmem_transport
Author(s): Cedric Pradalier
autogenerated on Sat Dec 28 2013 16:57:13