YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
ShmemOutputStream.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7#include "ShmemOutputStream.h"
8#include "ShmemLogComponent.h"
9
10#include <yarp/conf/numeric.h>
11#include <yarp/os/Log.h>
12
13#include <climits>
14#include <cstring>
15
16
18 m_bOpen(false),
19 m_ResizeNum(0),
20 m_Port(0),
21 m_pData(nullptr),
22 m_pHeader(nullptr)
23{
24}
25
30
32{
33 return m_bOpen;
34}
35
37{
38 // Remove the names of all the objects created by this writer, they are
39 // no longer needed (the objects survive until all the users close them)
40 for (int i = 0; i <= m_ResizeNum; ++i) {
42 }
45}
46
47bool ShmemOutputStreamImpl::open(int port, int size)
48{
49 m_pData = nullptr;
50 m_pHeader = nullptr;
51 m_ResizeNum = 0;
52 m_Port = port;
53 m_OldMaps.clear();
54
55 auto map = std::make_unique<shmem::SharedMemory>();
56 if (!map->create(shmem::objectName("data", port, 0), size + sizeof(ShmemHeader_t))) {
57 yCError(SHMEMCARRIER, "ShmemOutputStream can't create shared memory");
58 return false;
59 }
60
61 if (!m_AccessMutex.create(shmem::objectName("mtx", port), 1, 1)
62 || !m_WaitDataSem.create(shmem::objectName("sem", port), 0, INT_MAX)) {
63 yCError(SHMEMCARRIER, "ShmemOutputStream can't create semaphores");
66 unlinkAll();
67 return false;
68 }
69
70 m_pMap = std::move(map);
71 m_pHeader = static_cast<ShmemHeader_t*>(m_pMap->data());
72 m_pData = reinterpret_cast<char*>(m_pHeader + 1);
73
75
76 m_pHeader->resize = false;
77 m_pHeader->close = false;
78 m_pHeader->attached = false;
79
80 m_pHeader->avail = 0;
81 m_pHeader->head = 0;
82 m_pHeader->size = size;
83 m_pHeader->newsize = size;
84 m_pHeader->tail = 0;
85 m_pHeader->waiting = 0;
86
88
89 m_bOpen = true;
90
91 return true;
92}
93
95{
96 yCDebug(SHMEMCARRIER, "output stream resize %d to %d", m_ResizeNum + 1, newsize);
97
98 auto newMap = std::make_unique<shmem::SharedMemory>();
99 if (!newMap->create(shmem::objectName("data", m_Port, m_ResizeNum + 1), newsize + sizeof(ShmemHeader_t))) {
100 yCError(SHMEMCARRIER, "ShmemOutputStream can't create shared memory");
101 return false;
102 }
103 ++m_ResizeNum;
104
105 auto* pNewHeader = static_cast<ShmemHeader_t*>(newMap->data());
106 char* pNewData = reinterpret_cast<char*>(pNewHeader + 1);
107
108 pNewHeader->size = newsize;
109 pNewHeader->newsize = newsize;
110 pNewHeader->resize = false;
111 pNewHeader->close = m_pHeader->close;
112 pNewHeader->attached = false;
113
114 pNewHeader->tail = 0;
115 pNewHeader->head = pNewHeader->avail = m_pHeader->avail;
116 pNewHeader->waiting = m_pHeader->waiting;
117
118 if (m_pHeader->avail) {
119 // one or two blocks in circular queue?
120 if (m_pHeader->tail < m_pHeader->head) {
121 memcpy(pNewData, m_pData + m_pHeader->tail, m_pHeader->avail);
122 } else {
123 int firstchunk = m_pHeader->size - m_pHeader->tail;
124 memcpy(pNewData, m_pData + m_pHeader->tail, firstchunk);
125 memcpy(pNewData + firstchunk, m_pData, m_pHeader->head);
126 }
127 }
128
129 // Tell the reader to move to the new segment
130 m_pHeader->newsize = newsize;
131 m_pHeader->resize = true;
132
133 m_OldMaps.push_back(std::move(m_pMap));
134 m_pMap = std::move(newMap);
135
136 m_pHeader = pNewHeader;
137 m_pData = pNewData;
138
139 return true;
140}
141
143{
144 if (!m_bOpen) {
145 return false;
146 }
147
148 if (!m_AccessMutex.wait()) {
149 return false;
150 }
151
152 if (!m_bOpen) {
154 return false;
155 }
156
157 if (m_pHeader->close) {
159 close();
160 return false;
161 }
162
163 // The reader has moved to the current segment, the old ones are no
164 // longer needed
165 if (m_pHeader->attached && !m_OldMaps.empty()) {
166 m_OldMaps.clear();
167 }
168
169 if ((int)m_pHeader->size - (int)m_pHeader->avail < (int)b.length()) {
170 yarp::conf::ssize_t required = m_pHeader->size + 2 * b.length();
171 if (!Resize((int)required)) {
173 close();
174 return false;
175 }
176 }
177
178 if ((int)m_pHeader->head + (int)b.length() <= (int)m_pHeader->size) {
179 memcpy(m_pData + m_pHeader->head, b.get(), b.length());
180 } else {
181 int first_block_size = m_pHeader->size - m_pHeader->head;
182 memcpy(m_pData + m_pHeader->head, b.get(), first_block_size);
183 memcpy(m_pData, b.get() + first_block_size, b.length() - first_block_size);
184 }
185
186 m_pHeader->avail += (int)b.length();
187 m_pHeader->head += (int)b.length();
189
190 while (m_pHeader->waiting > 0) {
193 }
194
196
197 return true;
198}
199
201{
202 if (!m_bOpen) {
203 return;
204 }
205
206 m_bOpen = false;
207
209 while (m_pHeader->waiting > 0) {
212 }
213 m_pHeader->close = true;
215
218
219 m_OldMaps.clear();
220 m_pMap.reset();
221 m_pHeader = nullptr;
222 m_pData = nullptr;
223
224 unlinkAll();
225}
const yarp::os::LogComponent & SHMEMCARRIER()
bool write(const yarp::os::Bytes &b)
bool open(int port, int size=4096)
shmem::NamedSemaphore m_WaitDataSem
shmem::NamedSemaphore m_AccessMutex
std::vector< std::unique_ptr< shmem::SharedMemory > > m_OldMaps
ShmemHeader_t * m_pHeader
std::unique_ptr< shmem::SharedMemory > m_pMap
bool wait()
Wait until the semaphore is available.
static void unlink(const std::string &name)
Remove the name of the semaphore (no-op on Windows)
bool create(const std::string &name, unsigned int initial, unsigned int max)
Create a new semaphore (removing any stale semaphore with the same name)
static void unlink(const std::string &name)
Remove the name of the segment (no-op on Windows)
A simple abstraction for a block of bytes.
Definition Bytes.h:24
size_t length() const
Definition Bytes.cpp:22
const char * get() const
Definition Bytes.cpp:27
#define yCError(component,...)
#define yCDebug(component,...)
std::string objectName(const char *kind, int port, int index=-1)
Build the name of a named inter-process object.
::ssize_t ssize_t
Definition numeric.h:87