SharedMemory.cpp

Go to the documentation of this file.

#include "com/SharedMemory.hpp"
 
#include <errno.h>
#include <thread>
#include <iostream>
 
// shared memory
#include <sys/mman.h>
#include <unistd.h>
#include <sys/types.h>
#include <chrono>
#include <cstring>
 
#include <cassert>
 
using namespace com;
using namespace std::chrono_literals;
 
#define PERMISSION (0600)
 
SharedMemory::SharedMemory(std::string name) {
  assert(name.length() > 4);
  if(name.at(0) != '/') {
    name = "/" + name;
  }
  m_shmName = name;
  m_semaphoreName = name;
};
 
SharedMemory::~SharedMemory() {
  this->deallocate();
};
 
bool SharedMemory::allocate(size_t sizeBytes) {
  sizeBytes += 2 * sizeof(SharedData::tv_sec); // timestamps
 
  if(m_shmAddress != -1) {
    std::clog << "[SharedMemory::allocate] Already allocated.\n";
    return false;
  }
 
  m_shmAddress = shm_open(m_shmName.c_str(), O_RDWR | O_CREAT | O_TRUNC, PERMISSION); // maybe switch to 0600
  if (m_shmAddress == -1) {
    std::cerr << "[SharedMemory::allocate] Failed to allocate shared memory.\n";
    return false;
  }
  else {
    std::clog << "[SharedMemory::allocate] Successfully allocated memory\n";
  }
 
  int status = ftruncate(m_shmAddress, sizeBytes);
  if(status != 0) {
    std::cerr << "[SharedMemory::allocate] Failed to truncate shared memory. (Not enough space?)\n";
    this->deallocate();
    return false;
  }
  else {
    m_shmSize = sizeBytes;
  }
 
 
  // map shared_memory to process space
  m_sharedArea = mmap(NULL, m_shmSize, PROT_READ | PROT_WRITE, MAP_SHARED, m_shmAddress, 0);
  if(m_sharedArea == MAP_FAILED) {
    std::cerr << "[SharedMemory::allocate] Failed to create semaphore. (Maybe already existing)\n";
    this->deallocate();
    return false;
  }
 
 
  // create semaphore
  m_semaphore = sem_open(m_semaphoreName.c_str(), O_CREAT | O_EXCL, PERMISSION, 1); // 1 for 1 writer/reader
  if(m_semaphore == SEM_FAILED) {
    std::cerr << "[SharedMemory::allocate] Failed to create semaphore. (Maybe already existing)\n";
    this->deallocate();
    return false;
  }
 
  // write timestamp 0,0 to shm
  SharedData data;
  data.dataSize = 0;
  data.tv_sec   = 0;
  data.tv_nsec  = 0;
  this->write_mutexed(data);
 
  return true;
}
 
bool SharedMemory::deallocate() {
  bool status = true;
  if(m_sharedArea != MAP_FAILED && m_sharedArea != nullptr) {
    if (munmap(m_sharedArea, m_shmSize) == -1) {
      std::cerr << "[SharedMemory::deallocate] Failed to unmap shared area\n";
      status = false;
    }
    else {
      std::clog << "[SharedMemory::deallocate] Unmapped shared area successfully\n";
    }
  }
  if(m_semaphore != SEM_FAILED && m_semaphore != nullptr) {
    if(sem_unlink(m_semaphoreName.c_str()) != 0) {
      std::cerr << "[SharedMemory::deallocate] FAILED to unlink semaphore\n";
      status = false;
    }
    else {
      std::clog << "[SharedMemory::deallocate] Semaphore unlinked successfully\n";
      m_semaphore = NULL;
    }
  }
  if(m_shmAddress != -1 && m_shmSize != -1 && shm_unlink(m_shmName.c_str()) != 0) {
    std::cerr << "[SharedMemory::deallocate] FAILED to deallocate shared memory\n";
    status = false;
  }
  else {
    std::clog << "[SharedMemory::deallocate] Deallocated shared memory successfully\n";
    m_shmAddress = -1;
    m_shmSize = -1;
  }
  return status;
}
 
size_t SharedMemory::get_data_size() {
  return m_shmSize - 2 * sizeof(SharedData::tv_sec);
}
 
bool SharedMemory::write_mutexed(com::SharedData const& data) {
  if(m_sharedArea == nullptr || m_sharedArea == MAP_FAILED) {
    std::cerr << "[SharedMemory::write_mutexed] Shared memory not initialized/allocated! \n";
    return false;
  }
 
  size_t size = data.dataSize;
  if(size > m_shmSize) {
    std::cerr << "[SharedMemory::write_mutexed] Data too large to write into mutex. (Data is shrinked!)\n";
    size = m_shmSize;
  }
  else if(size != m_shmSize - 8 && size != 0) {
    std::clog << "[SharedMemory::write_mutexed] INFO: Data not as large as shared memory.\n";
  }
  
  if(acquire_semaphore(true) == false) {
    std::cerr << "[SharedMemory::write_mutexed] Could NOT acquire_semaphore. Skip writing to shared mem.\n";
    return false;
  }
  
  // write here mutexed
  std::memcpy(m_sharedArea,                        &data.tv_sec,  sizeof(data.tv_sec));
  std::memcpy(m_sharedArea + sizeof(data.tv_sec),  &data.tv_nsec, sizeof(data.tv_nsec));
  std::memcpy(m_sharedArea + sizeof(data.tv_sec) + sizeof(data.tv_nsec), data.data,     size);
 
  if(release_semaphore() == false) {
    std::cerr << "[SharedMemory::write_mutexed] Semaphore NOT released.\n";
    return false;
  }
 
  return true;
}
 
bool SharedMemory::is_new_data_available() {
  if(m_sharedArea == nullptr || m_sharedArea == MAP_FAILED) {
    std::cerr << "[SharedMemory::is_new_data_available] Shared memory not initialized/allocated! \n";
    return false;
  }
 
  if(acquire_semaphore(true) == false) {
    return false;
  }
 
  uint32_t sec;
  uint32_t nsec;
 
  // read here only timestamp
  std::memcpy(&sec,  m_sharedArea, sizeof(sec));
  std::memcpy(&nsec, m_sharedArea + sizeof(sec), sizeof(nsec));
 
  if(release_semaphore() == false) {
    std::cerr << "[SharedMemory::is_new_data_available] Semaphore NOT released.\n";
  }
 
  return (sec != m_lastDataSec ||  nsec != m_lastDataNSec);
}
 
size_t SharedMemory::read_mutexed(com::SharedData& data, size_t sizeToRead) {
  if(m_sharedArea == nullptr || m_sharedArea == MAP_FAILED) {
    std::cerr << "[SharedMemory::read_mutexed] Shared memory not initialized/allocated! \n";
    return 0;
  }
 
  if(acquire_semaphore(false) == false) {
    return 0;
  }
 
  // if larger than shared area
  if(sizeToRead > m_shmSize - 2 * sizeof(data.tv_sec)) {
    std::clog << "[SharedMemory::read_mutexed] Shared Memory not as large as desired size to read.\n";
    sizeToRead = m_shmSize - 2 * sizeof(data.tv_sec);
  }
 
  // read here mutexed
  std::memcpy(&data.tv_sec,  m_sharedArea,                           sizeof(data.tv_sec));
  std::memcpy(&data.tv_nsec, m_sharedArea + sizeof(data.tv_sec),     sizeof(data.tv_nsec));
  std::memcpy(data.data,     m_sharedArea + sizeof(data.tv_sec) + sizeof(data.tv_nsec), sizeToRead);
 
  if(release_semaphore() == false) {
    std::cerr << "[SharedMemory::read_mutexed] Semaphore NOT released.\n";
  }
 
  m_lastDataSec = data.tv_sec;
  m_lastDataNSec = data.tv_nsec;
 
  return true;
}
 
bool SharedMemory::acquire_semaphore(bool timedWait) {
  struct timespec tms;
  if (timedWait && clock_gettime(CLOCK_REALTIME,&tms) == 0) {
    tms.tv_nsec += 10000000; // max 10ms
    if(sem_timedwait(m_semaphore, &tms) != 0) {
      std::cerr << "[SharedMemory::acquire_semaphore] FAILED to get semaphore (timed).\n";
      return false;
    }
  }
  else {
    if(timedWait) std::clog << "[SharedMemory::acquire_semaphore] get clock not worked\n";
    if(sem_wait(m_semaphore) != 0) {
      std::cerr << "[SharedMemory::acquire_semaphore] FAILED to get semaphore.\n";
      return false;
    }
  }
 
  return true;
}
 
bool SharedMemory::release_semaphore() {
  int maxTries = 10;
  while(maxTries > 0 && sem_post(m_semaphore) != 0) {
    std::cerr << "[SharedMemory::release_semaphore] FAILED to post semaphore! Retrying\n";
    std::this_thread::sleep_for(10ms);
    maxTries--;
  }
  return (maxTries > 0);
}