GCC Code Coverage Report

Directory: ./
File: pdserv/src/lib/Main.cpp
Date: 2024-12-15 04:08:34
Exec Total Coverage
Lines: 336 399 84.2%
Branches: 187 363 51.5%
Line Branch Exec Source
1 /*****************************************************************************
2 *
3 * Copyright 2010 Richard Hacker (lerichi at gmx dot net)
4 * 2023 Florian Pose <fp@igh.de>
5 *
6 * This file is part of the pdserv library.
7 *
8 * The pdserv library is free software: you can redistribute it and/or modify
9 * it under the terms of the GNU Lesser General Public License as published
10 * by the Free Software Foundation, either version 3 of the License, or (at
11 * your option) any later version.
12 *
13 * The pdserv library is distributed in the hope that it will be useful, but
14 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
15 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
16 * License for more details.
17 *
18 * You should have received a copy of the GNU Lesser General Public License
19 * along with the pdserv library. If not, see <http://www.gnu.org/licenses/>.
20 *
21 *****************************************************************************/
22
23 #include "config.h"
24
25 #include "../Debug.h"
26
27 #include <iostream>
28 #include <unistd.h> // exit(), sleep()
29 #include <cerrno> // errno
30 #include <cstdio> // perror()
31 #include <sys/mman.h> // mmap(), munmap()
32 #include <signal.h> // signal()
33 #include <cerrno> // EIO
34 #include <sys/time.h> // gettimeofday()
35 #include <sys/wait.h>
36 #include <pthread.h>
37
38 #include "pdserv.h"
39 #include "Main.h"
40 #include "Task.h"
41 #include "Parameter.h"
42 #include "Signal.h"
43 #include "Event.h"
44 #include "Pointer.h"
45 #include "ShmemDataStructures.h"
46 #include "../Exceptions.h"
47 #include "../Session.h"
48 #include "../Config.h"
49 #include "../Database.h"
50
51 #ifdef _PDSERV_CUSTOM_GCOV_HOOK
52 extern "C" void __gcov_dump(void);
53 #endif // _PDSERV_CUSTOM_GCOV_HOOK
54
55
56 /////////////////////////////////////////////////////////////////////////////
57 struct SDO {
58 enum {ParamChange = 1, PollSignal} type;
59
60 union {
61 struct {
62 const Parameter *parameter;
63 unsigned int offset;
64 unsigned int count;
65 } paramChangeReq;
66
67 struct {
68 int rv;
69 struct timespec time;
70 } paramChangeAck;
71
72 const Signal* signal;
73 struct timespec time;
74 };
75 };
76
77 /////////////////////////////////////////////////////////////////////////////
78 const double Main::bufferTime = 2.0;
79
80 /////////////////////////////////////////////////////////////////////////////
81 /////////////////////////////////////////////////////////////////////////////
82 157 Main::Main( const char *name, const char *version,
83 157 int (*gettime)(struct timespec*)):
84 PdServ::Main(name, version),
85
8/16
✓ Branch 5 taken 157 times.
✗ Branch 6 not taken.
✓ Branch 12 taken 157 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 157 times.
✗ Branch 17 not taken.
✓ Branch 28 taken 157 times.
✗ Branch 29 not taken.
✓ Branch 42 taken 157 times.
✗ Branch 43 not taken.
✓ Branch 60 taken 157 times.
✗ Branch 61 not taken.
✓ Branch 70 taken 157 times.
✗ Branch 71 not taken.
✓ Branch 72 taken 157 times.
✗ Branch 73 not taken.
 157 rttime(gettime ? gettime : &PdServ::Main::localtime)
86 157 {}
87
88 /////////////////////////////////////////////////////////////////////////////
89 471 Main::~Main()
90 {
91 157 int cancel_state;
92 157 pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, &cancel_state);
93
94 157 terminate();
95 157 join();
96
97
98
2/2
✓ Branch 2 taken 314 times.
✓ Branch 3 taken 157 times.
 785 while (task.size()) {
99
1/2
✓ Branch 3 taken 314 times.
✗ Branch 4 not taken.
 314 delete task.front();
100 314 task.pop_front();
101 }
102
103
2/2
✓ Branch 2 taken 942 times.
✓ Branch 3 taken 157 times.
 942 while (parameters.size()) {
104
1/2
✓ Branch 3 taken 942 times.
✗ Branch 4 not taken.
 942 delete parameters.front();
105 942 parameters.pop_front();
106 }
107
108 // close pipes
109 157 ::close(terminatePipe);
110 157 ::close(ipcTx);
111 157 ::close(ipcRx);
112 157 ::close(nrtFeedbackPipe);
113
114
2/2
✓ Branch 3 taken 155 times.
✓ Branch 4 taken 2 times.
 157 if (pid > 0)
115 {
116 155 int stat = 0;
117 155 waitpid(pid, &stat, 0);
118 }
119
120
2/2
✓ Branch 3 taken 155 times.
✓ Branch 4 taken 2 times.
 157 if (shmem)
121 155 ::munmap(shmem, shmem_len);
122 157 pthread_setcancelstate(cancel_state, nullptr);
123 314 }
124
125 /////////////////////////////////////////////////////////////////////////////
126 149 void Main::setConfigFile(const char *file)
127 {
128 149 configFile = file;
129 149 }
130
131 /////////////////////////////////////////////////////////////////////////////
132 157 void Main::setParameterWriteLock(void (*fn)(int, void*), void* priv_data)
133 {
134 157 writelock_cb = fn;
135 157 writelock_data = priv_data;
136 157 }
137
138 /////////////////////////////////////////////////////////////////////////////
139 156 int Main::setup()
140 {
141 156 int rv;
142 156 int ipc_pipe[4][2];
143 time_t persistTimeout;
144 156 fd_set fds;
145 156 struct timeval timeout, now, delay;
146 156 struct EventData* eventData = nullptr;
147
148
1/2
✓ Branch 4 taken 156 times.
✗ Branch 5 not taken.
 156 rv = readConfiguration();
149
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 155 times.
 156 if (rv)
150 1 return rv;
151
1/2
✓ Branch 5 taken 155 times.
✗ Branch 6 not taken.
 155 setupLogging(configFile);
152
1/2
✓ Branch 4 taken 155 times.
✗ Branch 5 not taken.
 155 persistTimeout = setupPersistent();
153
154 // Initialize library
155
1/2
✓ Branch 4 taken 155 times.
✗ Branch 5 not taken.
 155 rv = prefork_init();
156
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 155 times.
 155 if (rv)
157 ✗ return rv;
158
159 // Open a pipe between the two processes. This is used to inform the
160 // child that the parent has died
161
3/6
✓ Branch 2 taken 155 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 155 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 155 times.
 465 if (::pipe(ipc_pipe[0]) or ::pipe(ipc_pipe[1]) or ::pipe(ipc_pipe[2])
162
2/4
✓ Branch 0 taken 155 times.
✗ Branch 1 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 155 times.
 310 or ::pipe(ipc_pipe[3])) {
163 ✗ rv = errno;
164 ✗ ::perror("pipe()");
165 ✗ return rv;
166 }
167
168 // Immediately split off a child. The parent returns to the caller so
169 // that he can get on with his job.
170 //
171 // The child continues from here.
172 //
173 // It is intentional that the child has the same process group as
174 // the parent so that it gets all the signals too.
175 155 pid = ::fork();
176
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 309 times.
 309 if (pid < 0) {
177 // Some error occurred
178 ✗ ::perror("fork()");
179 ✗ return errno;
180 }
181
2/2
✓ Branch 3 taken 155 times.
✓ Branch 4 taken 154 times.
 309 else if (pid) {
182 // Parent here. Return to the caller
183
1/2
✓ Branch 1 taken 155 times.
✗ Branch 2 not taken.
 155 ::close(ipc_pipe[0][0]);
184 155 ipcTx = ipc_pipe[0][1];
185
186 155 ipcRx = ipc_pipe[1][0];
187
1/2
✓ Branch 1 taken 155 times.
✗ Branch 2 not taken.
 155 ::close(ipc_pipe[1][1]);
188
189
1/2
✓ Branch 1 taken 155 times.
✗ Branch 2 not taken.
 155 ::close(ipc_pipe[2][0]);
190 155 terminatePipe = ipc_pipe[2][1];
191
192 155 nrtFeedbackPipe = ipc_pipe[3][0];
193
1/2
✓ Branch 1 taken 155 times.
✗ Branch 2 not taken.
 155 ::close(ipc_pipe[3][1]);
194
195 // Send PID to the child, indicating that parent is running
196
2/4
✓ Branch 4 taken 155 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 155 times.
 155 if (::write(terminatePipe, &pid, sizeof(pid)) != sizeof(pid)) {
197 ✗ perror("Main::setup(): pid ::write() failed");
198 ✗ return errno;
199 }
200 // waiting for child to initialize
201
2/4
✓ Branch 4 taken 155 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 155 times.
 155 if (::read(nrtFeedbackPipe, &rv, sizeof(rv)) != sizeof(rv)) {
202 ✗ rv = errno;
203 ✗ perror("Main::setup(): nRT setup: read() failed");
204 ✗ ::close(nrtFeedbackPipe);
205 ✗ nrtFeedbackPipe = -1;
206 ✗ return rv;
207 }
208
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 150 times.
 155 if (rv != 0) {
209 // child init has failed, it will terminate itself
210
1/2
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 ::close(nrtFeedbackPipe);
211 5 nrtFeedbackPipe = -1;
212 5 return rv;
213 }
214
215
1/2
✓ Branch 4 taken 150 times.
✗ Branch 5 not taken.
 150 return postfork_rt_setup();
216 }
217
218 // Only child runs after this point
219 154 ipcRx = ipc_pipe[0][0];
220
1/2
✓ Branch 1 taken 154 times.
✗ Branch 2 not taken.
 154 ::close(ipc_pipe[0][1]);
221
222
1/2
✓ Branch 1 taken 154 times.
✗ Branch 2 not taken.
 154 ::close(ipc_pipe[1][0]);
223 154 ipcTx = ipc_pipe[1][1];
224
225 154 terminatePipe = ipc_pipe[2][0];
226
1/2
✓ Branch 1 taken 154 times.
✗ Branch 2 not taken.
 154 ::close(ipc_pipe[2][1]);
227
228
1/2
✓ Branch 1 taken 154 times.
✗ Branch 2 not taken.
 154 ::close(ipc_pipe[3][0]);
229 154 nrtFeedbackPipe = ipc_pipe[3][1];
230
231
232 // Wait till main thread has been initialized
233
2/4
✓ Branch 4 taken 154 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 154 times.
 308 if (::read(terminatePipe, &pid, sizeof(pid)) != sizeof(pid)
234
2/4
✓ Branch 0 taken 154 times.
✗ Branch 1 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 154 times.
 154 or pid != getpid()) {
235 ✗ perror("Main::setup(): pid ::read() failed");
236 ✗ std::quick_exit(errno);
237 }
238
239 // Ignore common terminating signals
240 154 ::signal(SIGINT, SIG_IGN);
241 154 ::signal(SIGTERM, SIG_IGN);
242
243
1/2
✓ Branch 4 taken 154 times.
✗ Branch 5 not taken.
 154 postfork_nrt_setup();
244
245
246 154 rv = 0;
247 try {
248
2/2
✓ Branch 4 taken 149 times.
✓ Branch 5 taken 5 times.
 154 startServers();
249 }
250
1/4
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 10 catch (PdServ::Errno const & err) {
251 5 rv = -err.getErrno();
252 }
253
254
2/4
✓ Branch 4 taken 154 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 154 times.
 154 if (::write(nrtFeedbackPipe, &rv, sizeof(rv)) != sizeof(rv)) {
255 ✗ perror("Main::setup(): nRT setup: write() failed");
256 ✗ close(nrtFeedbackPipe);
257 ✗ std::quick_exit(errno);
258 }
259
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 154 times.
 154 if (rv == -1)
260 {
261 ✗ close(nrtFeedbackPipe);
262 #ifdef _PDSERV_CUSTOM_GCOV_HOOK
263 ✗ __gcov_dump();
264 #endif
265 ✗ std::quick_exit(-1);
266 }
267
268 try {
269
1/2
✓ Branch 4 taken 154 times.
✗ Branch 5 not taken.
 154 postfork_nrt_subscribe_persistent_signals();
270 ✗ } catch (PdServ::Main::RtProcessExited&) {
271 ✗ goto exit;
272 }
273
274 154 ::gettimeofday(&timeout, 0);
275 154 timeout.tv_sec += persistTimeout;
276
277 154 FD_ZERO(&fds);
278
279 // Stay in this loop until real-time thread exits, in which case
280 // ipc_pipe[0] becomes readable
281 154 eventData = eventDataStart;
282 154 ipc_error = false;
283 725 do {
284 try {
285
0/2
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 2637 for (TaskList::iterator it = task.begin();
286
2/2
✓ Branch 6 taken 1758 times.
✓ Branch 7 taken 879 times.
 2637 it != task.end(); ++it)
287
1/2
✓ Branch 6 taken 1758 times.
✗ Branch 7 not taken.
 1758 static_cast<Task*>(*it)->nrt_update();
288 ✗ } catch (RtProcessExited&) {
289 ✗ rv = 0;
290 ✗ break;
291 }
292
293
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 879 times.
 879 if (persistTimeout) {
294 ✗ if (::gettimeofday(&now, 0)) {
295 ✗ rv = errno;
296 ✗ break;
297 }
298
299 ✗ if ( now.tv_sec >= timeout.tv_sec) {
300 ✗ timeout.tv_sec += persistTimeout;
301 ✗ savePersistent();
302 }
303 }
304
305
2/2
✓ Branch 4 taken 27 times.
✓ Branch 5 taken 879 times.
 933 while (eventData != *eventDataWp) {
306
1/2
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
 54 newEvent(eventData->event, eventData->index,
307 27 eventData->state, &eventData->time);
308
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 27 times.
 27 if (++eventData == eventDataEnd)
309 ✗ eventData = eventDataStart;
310 }
311
312
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 879 times.
 879 FD_SET(terminatePipe, &fds);
313 879 delay.tv_sec = 0;
314 879 delay.tv_usec = 50000; // 20Hz
315
1/2
✓ Branch 4 taken 879 times.
✗ Branch 5 not taken.
 879 rv = ::select(terminatePipe + 1, &fds, 0, 0, &delay);
316
4/6
✓ Branch 0 taken 725 times.
✓ Branch 1 taken 154 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 725 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 725 times.
 879 } while (!(rv or ipc_error));
317
318 154 exit:
319 // Ignore rv if ipc_pipe[0] is readable
320
1/2
✓ Branch 0 taken 154 times.
✗ Branch 1 not taken.
 154 if (rv == 1)
321 154 rv = 0;
322
323
1/2
✓ Branch 4 taken 154 times.
✗ Branch 5 not taken.
 154 stopServers();
324
325
1/2
✓ Branch 4 taken 154 times.
✗ Branch 5 not taken.
 154 close(nrtFeedbackPipe);
326 #ifdef _PDSERV_CUSTOM_GCOV_HOOK
327
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 154 __gcov_dump();
328 #endif
329 ✗ std::quick_exit(rv);
330 }
331
332 /////////////////////////////////////////////////////////////////////////////
333
334 3 void Main::sleep(int msecs) const
335 {
336 3 fd_set fds;
337 3 FD_ZERO(&fds);
338
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
 3 FD_SET(terminatePipe, &fds);
339
340 3 struct timeval delay;
341
342 3 delay.tv_sec = msecs / 1000;
343 3 delay.tv_usec = (msecs - delay.tv_sec * 1000) * 1000;
344
345
1/2
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 const int rv = ::select(terminatePipe + 1, &fds, 0, 0, &delay);
346
2/6
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 3 if (rv == 1 or (rv == -1 && errno != EINTR)) {
347 ✗ throw PdServ::Main::RtProcessExited{};
348 }
349 3 }
350
351 /////////////////////////////////////////////////////////////////////////////
352 156 int Main::readConfiguration()
353 {
354 const char *env;
355 156 const char *err = 0;
356
357 // Load custom configuration file
358
2/2
✓ Branch 2 taken 148 times.
✓ Branch 3 taken 8 times.
 156 if (!configFile.empty()) {
359 148 err = m_config.load(configFile.c_str());
360
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 147 times.
 148 if (err)
361 std::cerr
362 << "Error loading configuration file "
363 << configFile << " specified on command line: "
364 1 << err << std::endl;
365 else {
366 log_debug("Loaded specified configuration file %s",
367 configFile.c_str());
368 }
369 }
370
3/8
✗ Branch 0 not taken.
✓ Branch 1 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 8 times.
 8 else if ((env = ::getenv("PDSERV_CONFIG")) and ::strlen(env)) {
371 // Try to load environment configuration file
372 ✗ err = m_config.load(env);
373
374 ✗ if (err)
375 std::cerr << "Error loading configuration file " << env
376 << " specified in environment variable PDSERV_CONFIG: "
377 ✗ << err << std::endl;
378 else {
379 ✗ configFile = env;
380 log_debug("Loaded ENV config %s", env);
381 }
382 }
383 else {
384 // Try to load default configuration file
385 8 const char *f = QUOTE(SYSCONFDIR) "/pdserv.conf";
386
387
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 8 times.
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 8 if (::access(f, R_OK) == 0) {
388 // file exists
389 8 err = m_config.load(f);
390
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 8 times.
 8 if (err) {
391 std::cerr << "Error loading default configuration file "
392 ✗ << f << ": " << err << std::endl;
393 }
394 else {
395 8 configFile = f;
396 log_debug("Loaded default configuration file %s", f);
397 }
398 }
399 else {
400 std::cerr << "No configuration file found at " << f
401 ✗ << ". Using defaults." << std::endl;
402 }
403 }
404
405 156 if (!m_config) {
406 log_debug("No configuration loaded");
407 }
408
409
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 155 times.
 156 if (err) {
410 1 return -1;
411 }
412
413 155 return 0;
414 }
415
416 /////////////////////////////////////////////////////////////////////////////
417 1314 PdServ::Config Main::config(const char* key) const
418 {
419 1314 return m_config[key];
420 }
421
422 /////////////////////////////////////////////////////////////////////////////
423 314 Task* Main::addTask(double sampleTime, const char *name)
424 {
425
2/4
✓ Branch 7 taken 314 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 314 times.
✗ Branch 12 not taken.
 314 task.push_back(new Task(this, task.size(), sampleTime, name));
426 314 return task.back();
427 }
428
429 /////////////////////////////////////////////////////////////////////////////
430 211 int Main::gettime(struct timespec* t) const
431 {
432 211 return rttime(t);
433 }
434
435 /////////////////////////////////////////////////////////////////////////////
436 314 Event* Main::addEvent (
437 const char *path, size_t nelem, const char * const *messages)
438 {
439
2/4
✓ Branch 5 taken 314 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 314 times.
✗ Branch 11 not taken.
 314 events.push_back(std::unique_ptr<Event>{new Event(this, path, nelem, messages)});
440
441 314 return events.back().get();
442 }
443
444 /////////////////////////////////////////////////////////////////////////////
445 942 Parameter* Main::addParameter( const char *path,
446 unsigned int mode, const PdServ::DataType& datatype,
447 void *addr, size_t n, const size_t *dim)
448 {
449
1/2
✓ Branch 4 taken 942 times.
✗ Branch 5 not taken.
 1884 parameters.push_back(
450
1/2
✓ Branch 3 taken 942 times.
✗ Branch 4 not taken.
 1884 new Parameter(this, addr, path, mode, datatype, n, dim));
451
452 942 return parameters.back();
453 }
454
455 /////////////////////////////////////////////////////////////////////////////
456 16 void Main::setEvent(const Event* event,
457 size_t element, PdServ::Event::Priority prio, const timespec *time)
458 {
459 32 pthread::MutexLock lock(eventMutex);
460
461 16 struct EventData *eventData = *eventDataWp;
462 16 eventData->event = event;
463 16 eventData->index = element;
464 16 eventData->state = prio;
465 16 eventData->time = *time;
466
467
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 16 times.
 16 if (++eventData == eventDataEnd)
468 ✗ eventData = eventDataStart;
469 16 *eventDataWp = eventData;
470 16 }
471
472 /////////////////////////////////////////////////////////////////////////////
473 11 void Main::resetEvent(const Event* event,
474 size_t element, const timespec *time)
475 {
476 22 pthread::MutexLock lock(eventMutex);
477
478 11 struct EventData *eventData = *eventDataWp;
479 11 eventData->event = event;
480 11 eventData->index = element;
481 11 eventData->state = -1;
482 11 eventData->time = *time;
483
484
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 11 times.
 11 if (++eventData == eventDataEnd)
485 ✗ eventData = eventDataStart;
486 11 *eventDataWp = eventData;
487 11 }
488
489 /////////////////////////////////////////////////////////////////////////////
490 43 int Main::setValue(const PdServ::ProcessParameter* p,
491 const char* buf, size_t offset, size_t count,
492 const char** value, const struct timespec** time)
493 {
494 86 pthread::MutexLock lock(sdoMutex);
495
1/2
✓ Branch 0 taken 43 times.
✗ Branch 1 not taken.
 43 const Parameter* param = static_cast<const Parameter*>(p);
496 43 char* shmAddr = param->shmAddr + offset;
497
498 // Backup old values in case of write failure
499
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 43 times.
 43 char backup[count];
500
1/2
✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
 43 std::copy(shmAddr, shmAddr + count, backup);
501
502 // Copy new data to shared memory
503
1/2
✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
 43 std::copy(buf, buf + count, shmAddr);
504
505 // Setup change request
506 43 struct SDO sdo;
507 43 sdo.type = SDO::ParamChange;
508 43 sdo.paramChangeReq.parameter = param;
509 43 sdo.paramChangeReq.offset = offset;
510 43 sdo.paramChangeReq.count = count;
511
512
2/4
✓ Branch 4 taken 43 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 43 times.
 43 if (::write(ipcTx, &sdo, sizeof(sdo)) != sizeof(sdo)) {
513 log_debug("Main::setValue(): SDO ::write() failed");
514 ✗ ipc_error = true;
515 ✗ return -EIO;
516 }
517
518
2/4
✓ Branch 4 taken 43 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 43 times.
 43 if (::read(ipcRx, &sdo, sizeof(sdo)) != sizeof(sdo)) {
519 log_debug("Main::setValue(): SDO ::read() failed");
520 ✗ ipc_error = true;
521 ✗ return -EIO;
522 }
523
524
1/2
✓ Branch 0 taken 43 times.
✗ Branch 1 not taken.
 43 if (!sdo.paramChangeAck.rv)
525 43 param->mtime = sdo.paramChangeAck.time; // Save time of update
526 else
527 // Write failure. Restore data
528 ✗ std::copy(backup, backup + count, shmAddr);
529
530 43 *value = param->shmAddr;
531 43 *time = &param->mtime;
532
533 86 return sdo.paramChangeAck.rv;
534 }
535
536 /////////////////////////////////////////////////////////////////////////////
537 5 void Main::initializeParameter(PdServ::Parameter* p,
538 const char* data, const struct timespec* mtime,
539 const PdServ::Signal* s)
540 {
541
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
 5 if (data) {
542 log_debug("Restoring %s", p->path.c_str());
543
1/2
✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
 4 const Parameter *parameter = static_cast<const Parameter*>(p);
544 4 std::copy(data, data + parameter->memSize, parameter->addr);
545 4 parameter->mtime = *mtime;
546 }
547
548
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
 5 if (s) {
549
1/2
✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
 4 const Signal* signal = static_cast<const Signal*>(s);
550 4 signal->task->makePersistent(signal);
551 }
552 5 }
553
554 /////////////////////////////////////////////////////////////////////////////
555 3 bool Main::getPersistentSignalValue(const PdServ::Signal *s,
556 char* buf, struct timespec* time)
557 {
558 3 const struct timespec* t;
559
1/2
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
 3 bool rv = static_cast<const Signal*>(s)->task->getPersistentValue(
560
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 s, buf, &t);
561
562
2/4
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 if (rv and time)
563 3 *time = *t;
564
565 3 return rv;
566 }
567
568 /////////////////////////////////////////////////////////////////////////////
569 57 int Main::getValue(const Signal *signal, void* dest, struct timespec* time)
570 {
571 114 pthread::MutexLock lock(sdoMutex);
572 57 struct SDO sdo;
573
574 57 sdo.type = SDO::PollSignal;
575 57 sdo.signal = signal;
576
577
2/4
✓ Branch 4 taken 57 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 57 times.
 57 if (::write(ipcTx, &sdo, sizeof(sdo)) != sizeof(sdo)) {
578 log_debug("Main::getValue(): SDO ::write() failed");
579 ✗ ipc_error = true;
580 ✗ return -EIO;
581 }
582
583
2/4
✓ Branch 4 taken 57 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 57 times.
 57 if (::read(ipcRx, &sdo, sizeof(sdo)) != sizeof(sdo)) {
584 log_debug("Main::getValue(): SDO ::read() failed");
585 ✗ ipc_error = true;
586 ✗ return -EIO;
587 }
588
589
1/2
✓ Branch 10 taken 57 times.
✗ Branch 11 not taken.
 57 std::copy(signalData, signalData + signal->memSize,
590 reinterpret_cast<char*>(dest));
591
592
1/2
✓ Branch 0 taken 57 times.
✗ Branch 1 not taken.
 57 if (time)
593 57 *time = sdo.time;
594
595 57 return 0;
596 }
597
598 /////////////////////////////////////////////////////////////////////////////
599 6 PdServ::Parameter* Main::findParameter(const std::string& path) const
600 {
601
2/4
✗ Branch 4 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 6 times.
 28 for (ParameterList::const_iterator it = parameters.begin();
602
1/2
✓ Branch 6 taken 28 times.
✗ Branch 7 not taken.
 28 it != parameters.end(); ++it)
603
2/2
✓ Branch 5 taken 6 times.
✓ Branch 6 taken 22 times.
 28 if ((*it)->path == path)
604 6 return *it;
605 ✗ return 0;
606 }
607
608 /////////////////////////////////////////////////////////////////////////////
609 // Orangization of shared memory:
610 // struct SDOStruct sdo
611 // char parameterData (binary data of all parameters)
612 // char pdoData
613 // struct EventData eventDataStart
614 //
615 155 int Main::prefork_init()
616 {
617 155 size_t numTasks = task.size();
618
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 155 times.
 155 size_t taskMemSize[numTasks];
619 size_t i, eventCount;
620 155 size_t maxSignalSize = 0;
621
622 // Find out the largest signal size to reserve space in
623 // shared memory for polling
624 465 for (TaskList::const_iterator it = task.begin();
625
2/2
✓ Branch 6 taken 310 times.
✓ Branch 7 taken 155 times.
 465 it != task.end(); ++it) {
626 310 std::list<const PdServ::Signal*> signals =
627
1/2
✓ Branch 5 taken 310 times.
✗ Branch 6 not taken.
 620 static_cast<PdServ::Task*>(*it)->getSignals();
628
629
2/2
✓ Branch 1 taken 930 times.
✓ Branch 2 taken 310 times.
 2170 while (!signals.empty()) {
630 930 const PdServ::Signal* s = signals.front();
631
632
2/2
✓ Branch 3 taken 155 times.
✓ Branch 4 taken 775 times.
 930 if (s->memSize > maxSignalSize)
633 155 maxSignalSize = s->memSize;
634
635 930 signals.pop_front();
636 }
637
638 }
639 155 shmem_len += maxSignalSize;
640
641 // The following two variables are used to organize parameters according
642 // to the size of their elements so that their data type alignment is
643 // correct.
644 //
645 // dataTypeIndex[] maps the data type to the index in parameterDataOffset,
646 // e.g. a parameter with data type double (sizeof() = 8) will then go into
647 // container parameterDataOffset[dataTypeIndex[8]]
648 //
649 // parameterDataOffset[] holds the start index of a data types with
650 // 8, 4, 2 and 1 bytes alignment
651 155 const size_t dataTypeIndex[PdServ::DataType::maxWidth+1] = {
652 3 /*0*/, 3 /*1*/, 2 /*2*/, 3 /*3*/,
653 1 /*4*/, 3 /*5*/, 3 /*6*/, 3 /*7*/, 0 /*8*/
654 };
655 155 size_t parameterDataOffset[5] = {0, 0, 0, 0, 0}; // need one extra!
656
657 1085 for (ParameterList::iterator it = parameters.begin();
658
2/2
✓ Branch 6 taken 930 times.
✓ Branch 7 taken 155 times.
 1085 it != parameters.end(); it++) {
659 930 const Parameter *p = static_cast<const Parameter*>(*it);
660
661 // Push the next smaller data type forward by the parameter's
662 // memory requirement
663
1/2
✓ Branch 15 taken 930 times.
✗ Branch 16 not taken.
 930 parameterDataOffset[dataTypeIndex[p->dtype.align()] + 1] += p->memSize;
664 }
665
666 // Accumulate the offsets so that they follow each other in the shared
667 // data space. This also has the effect, that the value of
668 // parameterDataOffset[4] is the total memory requirement of all
669 // parameters
670
2/2
✓ Branch 0 taken 620 times.
✓ Branch 1 taken 155 times.
 775 for (i = 1; i < 5; ++i)
671 620 parameterDataOffset[i] += parameterDataOffset[i-1];
672
673 // Extend shared memory size with the parameter memory requirement
674 // and as many sdo's for every parameter.
675 155 shmem_len += parameterDataOffset[4];
676
677 // Now check how much memory is required for events
678 155 eventCount = 0;
679
2/2
✓ Branch 8 taken 310 times.
✓ Branch 9 taken 155 times.
 465 for (EventList::iterator it = events.begin(); it != events.end(); ++it)
680 310 eventCount += (*it)->nelem;
681
682 // Increase shared memory by the number of events as well as
683 // enough capacity to store eventDataLen event changes
684 155 const size_t eventLen = 10; // Arbitrary
685 155 shmem_len += sizeof(*eventDataStart) * eventLen * eventCount;
686
687 155 shmem_len += sizeof(*eventDataWp); // Memory location for write pointer
688
689 // Find out the memory requirement for the tasks to pipe their variables
690 // out of the real time environment
691 155 i = 0;
692 465 for (TaskList::const_iterator it = task.begin();
693
2/2
✓ Branch 6 taken 310 times.
✓ Branch 7 taken 155 times.
 465 it != task.end(); ++it) {
694
1/2
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
 310 taskMemSize[i] = ptr_align(
695 310 static_cast<const Task*>(*it)->getShmemSpace(bufferTime));
696 310 shmem_len += taskMemSize[i++];
697 }
698
699 // Fudge factor. Every ptr_align can silently increase the pointer in
700 // shmem by sizeof(unsigned long).
701 // At the moment there are roughly 6 ptr_align's, take 10 to make sure!
702 155 shmem_len += (10 + task.size())*sizeof(unsigned long);
703
704 155 shmem = ::mmap(0, shmem_len, PROT_READ | PROT_WRITE,
705 MAP_SHARED | MAP_ANON, -1, 0);
706
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 155 times.
 155 if (MAP_FAILED == shmem) {
707 // log(LOGCRIT, "could not mmap
708 // err << "mmap(): " << strerror(errno);
709 ✗ ::perror("mmap()");
710 ✗ return errno;
711 }
712
713 // Clear memory; at the same time prefault it, so it does not
714 // get swapped out
715
1/2
✗ Branch 6 not taken.
✓ Branch 7 taken 155 times.
 155 ::memset(shmem, 0, shmem_len);
716
717 // Now spread the shared memory for the users thereof
718
719 // 1: Parameter data
720 155 parameterData = ptr_align<char>(shmem);
721 1085 for (ParameterList::iterator it = parameters.begin();
722
2/2
✓ Branch 6 taken 930 times.
✓ Branch 7 taken 155 times.
 1085 it != parameters.end(); it++) {
723 930 Parameter *p = *it;
724 2790 p->shmAddr = parameterData
725
1/2
✓ Branch 15 taken 930 times.
✗ Branch 16 not taken.
 1860 + parameterDataOffset[dataTypeIndex[p->dtype.align()]];
726
1/2
✓ Branch 15 taken 930 times.
✗ Branch 16 not taken.
 930 parameterDataOffset[dataTypeIndex[p->dtype.align()]] += p->memSize;
727
728
1/2
✓ Branch 13 taken 930 times.
✗ Branch 14 not taken.
 930 std::copy(p->addr, p->addr + p->memSize, p->shmAddr);
729 }
730
731 // 2: Signal data area for polling
732 155 signalData = ptr_align<char>(parameterData + parameterDataOffset[4]);
733
734 // 3: Streaming data for tasks
735 155 char* buf = ptr_align<char>(signalData + maxSignalSize);
736 155 i = 0;
737
2/2
✓ Branch 8 taken 310 times.
✓ Branch 9 taken 155 times.
 465 for (TaskList::iterator it = task.begin(); it != task.end(); ++it) {
738
1/2
✓ Branch 8 taken 310 times.
✗ Branch 9 not taken.
 310 static_cast<Task*>(*it)->prepare(buf, buf + taskMemSize[i]);
739 310 buf = ptr_align<char>(buf + taskMemSize[i++]);
740 }
741
742 // 4: Event data
743 155 struct EventData** ptr = ptr_align<struct EventData*>(buf);
744 155 eventDataWp = ptr++;
745 155 eventDataStart = ptr_align<struct EventData>(ptr++);
746 155 eventDataEnd = eventDataStart + eventLen * eventCount;
747 155 *eventDataWp = eventDataStart;
748
749 log_debug("shmem=%p shmem_end=%p(%zu)\n"
750 "param=%p(%zi)\n"
751 "stream=%p(%zi)\n"
752 "end=%p(%zi)\n",
753 shmem, (char*)shmem + shmem_len, shmem_len,
754 signalData, (char*)signalData - (char*)shmem,
755 eventDataWp, (char*)eventDataWp - (char*)shmem,
756 eventDataEnd, (char*)eventDataEnd - (char*)shmem);
757
758
1/2
✗ Branch 9 not taken.
✓ Branch 10 taken 155 times.
 155 if ((void*)(eventDataEnd + 1) > (void*)((char*)shmem + shmem_len)) {
759 log_debug("Not enough memory");
760 ✗ return ENOMEM;
761 }
762
763 310 return 0;
764 }
765
766 /////////////////////////////////////////////////////////////////////////////
767 150 int Main::postfork_rt_setup()
768 {
769 // Set the priority of the real time helper thread. This priority
770 // should be set to the highest real time priority.
771 150 set_priority(sched_get_priority_max(SCHED_FIFO));
772
773 // Parent here; go back to the caller
774 450 for (TaskList::iterator it = task.begin();
775
2/2
✓ Branch 6 taken 300 times.
✓ Branch 7 taken 150 times.
 450 it != task.end(); ++it)
776
1/2
✓ Branch 6 taken 300 times.
✗ Branch 7 not taken.
 300 static_cast<Task*>(*it)->rt_init();
777
778 // Start supervisor thread
779 150 return start();
780 }
781
782 /////////////////////////////////////////////////////////////////////////////
783 154 int Main::postfork_nrt_setup()
784 {
785 // Parent here; go back to the caller
786 462 for (TaskList::iterator it = task.begin();
787
2/2
✓ Branch 6 taken 308 times.
✓ Branch 7 taken 154 times.
 462 it != task.end(); ++it)
788
1/2
✓ Branch 6 taken 308 times.
✗ Branch 7 not taken.
 308 static_cast<Task*>(*it)->nrt_init();
789 154 return 0;
790 }
791
792 /////////////////////////////////////////////////////////////////////////////
793
794 154 void Main::postfork_nrt_subscribe_persistent_signals()
795 {
796
2/2
✓ Branch 6 taken 308 times.
✓ Branch 7 taken 154 times.
 462 for (auto i : task)
797
1/2
✓ Branch 4 taken 308 times.
✗ Branch 5 not taken.
 308 i->nrt_init_persistent();
798 154 }
799
800 /////////////////////////////////////////////////////////////////////////////
801 154 std::list<const PdServ::Parameter*> Main::getParameters() const
802 {
803 return std::list<const PdServ::Parameter*>(
804
1/2
✓ Branch 7 taken 154 times.
✗ Branch 8 not taken.
 154 parameters.begin(), parameters.end());
805 }
806
807 /////////////////////////////////////////////////////////////////////////////
808 ✗ std::list<const PdServ::Event*> Main::getEvents() const
809 {
810 ✗ std::list<const PdServ::Event*> ans;
811 ✗ for (const auto& e : events)
812 ✗ ans.push_back(e.get());
813 ✗ return ans;
814 }
815
816 /////////////////////////////////////////////////////////////////////////////
817 308 std::list<const PdServ::Task*> Main::getTasks() const
818 {
819
1/2
✓ Branch 7 taken 308 times.
✗ Branch 8 not taken.
 308 return std::list<const PdServ::Task*>(task.begin(), task.end());
820 }
821
822 /////////////////////////////////////////////////////////////////////////////
823 148 void Main::prepare(PdServ::Session* session) const
824 {
825 148 PdServ::Main::prepare(session);
826 148 }
827
828 /////////////////////////////////////////////////////////////////////////////
829 148 void Main::cleanup(const PdServ::Session* session) const
830 {
831 148 PdServ::Main::cleanup(session);
832 148 }
833
834 /////////////////////////////////////////////////////////////////////////////
835 147 void Main::run()
836 {
837 147 struct SDO sdo;
838
839 while (true) {
840
3/4
✓ Branch 4 taken 100 times.
✓ Branch 5 taken 147 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 100 times.
 347 if (::read(ipcRx, &sdo, sizeof(sdo)) != sizeof(sdo))
841 ✗ throw std::runtime_error("Main::run(): SDO ::read() failed");
842
843
3/5
✗ Branch 0 not taken.
✓ Branch 1 taken 100 times.
✓ Branch 2 taken 43 times.
✓ Branch 3 taken 57 times.
✗ Branch 4 not taken.
 100 switch (sdo.type) {
844 43 case SDO::ParamChange:
845
1/2
✓ Branch 4 taken 43 times.
✗ Branch 5 not taken.
 129 sdo.paramChangeAck.rv = setParameterValue(
846 sdo.paramChangeReq.parameter,
847 43 sdo.paramChangeReq.offset,
848 43 sdo.paramChangeReq.count,
849 &sdo.paramChangeAck.time);
850
851 log_debug("Parameter change rv=%i\n", sdo.paramChangeAck.rv);
852
853 43 break;
854
855 57 case SDO::PollSignal:
856
1/2
✓ Branch 16 taken 57 times.
✗ Branch 17 not taken.
 114 sdo.signal->task->pollSignalValue(
857 57 sdo.signal, signalData, &sdo.time);
858 57 break;
859 };
860
861
2/4
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 100 times.
 100 if (::write(ipcTx, &sdo, sizeof(sdo)) != sizeof(sdo))
862 ✗ throw std::runtime_error("Main::run(): SDO ::write() failed");
863 }
864 }
865
866 /////////////////////////////////////////////////////////////////////////////
867 43 int Main::setParameterValue(const Parameter* param,
868 size_t offset, size_t len, struct timespec* mtime) const
869 {
870 struct LockGuard {
871 43 LockGuard(const Main* m): main(m) {
872
1/2
✓ Branch 6 taken 43 times.
✗ Branch 7 not taken.
 43 if (main->writelock_cb)
873 43 main->writelock_cb(1, main->writelock_data);
874 43 }
875 86 ~LockGuard() {
876
1/2
✓ Branch 6 taken 43 times.
✗ Branch 7 not taken.
 43 if (main->writelock_cb)
877 43 main->writelock_cb(0, main->writelock_data);
878 43 }
879 const Main* const main;
880 };
881
1/2
✓ Branch 2 taken 43 times.
✗ Branch 3 not taken.
 86 LockGuard guard(this);
882
883
1/2
✓ Branch 4 taken 43 times.
✗ Branch 5 not taken.
 86 return param->setValue(offset, len, mtime);
884 }
885
886
887 /////////////////////////////////////////////////////////////////////////////
888 147 void Main::final()
889 {
890 147 ::close(ipcRx);
891 147 ::close(ipcTx);
892 156 }
893