GCC Code Coverage Report

Directory: ./
File: pdserv/src/lib/Main.cpp
Date: 2023-11-12 04:06:57
Exec Total Coverage
Lines: 326 398 81.9%
Branches: 183 363 50.4%
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 133 Main::Main( const char *name, const char *version,
83 133 int (*gettime)(struct timespec*)):
84 PdServ::Main(name, version),
85
8/16
✓ Branch 5 taken 133 times.
✗ Branch 6 not taken.
✓ Branch 12 taken 133 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 133 times.
✗ Branch 17 not taken.
✓ Branch 28 taken 133 times.
✗ Branch 29 not taken.
✓ Branch 42 taken 133 times.
✗ Branch 43 not taken.
✓ Branch 60 taken 133 times.
✗ Branch 61 not taken.
✓ Branch 70 taken 133 times.
✗ Branch 71 not taken.
✓ Branch 72 taken 133 times.
✗ Branch 73 not taken.
 133 rttime(gettime ? gettime : &PdServ::Main::localtime)
86 133 {}
87
88 /////////////////////////////////////////////////////////////////////////////
89 399 Main::~Main()
90 {
91 133 int cancel_state;
92 133 pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, &cancel_state);
93
94 133 terminate();
95 133 join();
96
97
98
2/2
✓ Branch 2 taken 266 times.
✓ Branch 3 taken 133 times.
 665 while (task.size()) {
99
1/2
✓ Branch 3 taken 266 times.
✗ Branch 4 not taken.
 266 delete task.front();
100 266 task.pop_front();
101 }
102
103
2/2
✓ Branch 2 taken 798 times.
✓ Branch 3 taken 133 times.
 798 while (parameters.size()) {
104
1/2
✓ Branch 3 taken 798 times.
✗ Branch 4 not taken.
 798 delete parameters.front();
105 798 parameters.pop_front();
106 }
107
108 // close pipes
109 133 ::close(terminatePipe);
110 133 ::close(ipcTx);
111 133 ::close(ipcRx);
112 133 ::close(nrtFeedbackPipe);
113
114
2/2
✓ Branch 3 taken 131 times.
✓ Branch 4 taken 2 times.
 133 if (pid > 0)
115 {
116 131 int stat = 0;
117 131 waitpid(pid, &stat, 0);
118 }
119
120
2/2
✓ Branch 3 taken 131 times.
✓ Branch 4 taken 2 times.
 133 if (shmem)
121 131 ::munmap(shmem, shmem_len);
122 133 pthread_setcancelstate(cancel_state, nullptr);
123 266 }
124
125 /////////////////////////////////////////////////////////////////////////////
126 125 void Main::setConfigFile(const char *file)
127 {
128 125 configFile = file;
129 125 }
130
131 /////////////////////////////////////////////////////////////////////////////
132 133 void Main::setParameterWriteLock(void (*fn)(int, void*), void* priv_data)
133 {
134 133 writelock_cb = fn;
135 133 writelock_data = priv_data;
136 133 }
137
138 /////////////////////////////////////////////////////////////////////////////
139 132 int Main::setup()
140 {
141 132 int rv;
142 132 int ipc_pipe[4][2];
143 time_t persistTimeout;
144 132 fd_set fds;
145 132 struct timeval timeout, now, delay;
146 132 struct EventData* eventData = nullptr;
147
148
1/2
✓ Branch 4 taken 132 times.
✗ Branch 5 not taken.
 132 rv = readConfiguration();
149
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 131 times.
 132 if (rv)
150 1 return rv;
151
1/2
✓ Branch 5 taken 131 times.
✗ Branch 6 not taken.
 131 setupLogging(configFile);
152
1/2
✓ Branch 4 taken 131 times.
✗ Branch 5 not taken.
 131 persistTimeout = setupPersistent();
153
154 // Initialize library
155
1/2
✓ Branch 4 taken 131 times.
✗ Branch 5 not taken.
 131 rv = prefork_init();
156
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 131 times.
 131 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 131 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 131 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 131 times.
 393 if (::pipe(ipc_pipe[0]) or ::pipe(ipc_pipe[1]) or ::pipe(ipc_pipe[2])
162
2/4
✓ Branch 0 taken 131 times.
✗ Branch 1 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 131 times.
 262 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 131 pid = ::fork();
176
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 261 times.
 261 if (pid < 0) {
177 // Some error occurred
178 ✗ ::perror("fork()");
179 ✗ return errno;
180 }
181
2/2
✓ Branch 3 taken 131 times.
✓ Branch 4 taken 130 times.
 261 else if (pid) {
182 // Parent here. Return to the caller
183
1/2
✓ Branch 1 taken 131 times.
✗ Branch 2 not taken.
 131 ::close(ipc_pipe[0][0]);
184 131 ipcTx = ipc_pipe[0][1];
185
186 131 ipcRx = ipc_pipe[1][0];
187
1/2
✓ Branch 1 taken 131 times.
✗ Branch 2 not taken.
 131 ::close(ipc_pipe[1][1]);
188
189
1/2
✓ Branch 1 taken 131 times.
✗ Branch 2 not taken.
 131 ::close(ipc_pipe[2][0]);
190 131 terminatePipe = ipc_pipe[2][1];
191
192 131 nrtFeedbackPipe = ipc_pipe[3][0];
193
1/2
✓ Branch 1 taken 131 times.
✗ Branch 2 not taken.
 131 ::close(ipc_pipe[3][1]);
194
195 // Send PID to the child, indicating that parent is running
196
2/4
✓ Branch 4 taken 131 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 131 times.
 131 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 131 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 131 times.
 131 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 126 times.
 131 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 126 times.
✗ Branch 5 not taken.
 126 return postfork_rt_setup();
216 }
217
218 // Only child runs after this point
219 130 ipcRx = ipc_pipe[0][0];
220
1/2
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
 130 ::close(ipc_pipe[0][1]);
221
222
1/2
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
 130 ::close(ipc_pipe[1][0]);
223 130 ipcTx = ipc_pipe[1][1];
224
225 130 terminatePipe = ipc_pipe[2][0];
226
1/2
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
 130 ::close(ipc_pipe[2][1]);
227
228
1/2
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
 130 ::close(ipc_pipe[3][0]);
229 130 nrtFeedbackPipe = ipc_pipe[3][1];
230
231
232 // Wait till main thread has been initialized
233
2/4
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 130 times.
 260 if (::read(terminatePipe, &pid, sizeof(pid)) != sizeof(pid)
234
2/4
✓ Branch 0 taken 130 times.
✗ Branch 1 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 130 times.
 130 or pid != getpid()) {
235 ✗ perror("Main::setup(): pid ::read() failed");
236 ✗ std::quick_exit(errno);
237 }
238
239 // Ignore common terminating signals
240 130 ::signal(SIGINT, SIG_IGN);
241 130 ::signal(SIGTERM, SIG_IGN);
242
243
1/2
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
 130 postfork_nrt_setup();
244
245
246 130 rv = 0;
247 try {
248
2/2
✓ Branch 4 taken 125 times.
✓ Branch 5 taken 5 times.
 130 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 130 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 130 times.
 130 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 130 times.
 130 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 130 times.
✗ Branch 5 not taken.
 130 postfork_nrt_subscribe_persistent_signals();
270 ✗ } catch (PdServ::Main::RtProcessExited&) {
271 ✗ goto exit;
272 }
273
274 130 ::gettimeofday(&timeout, 0);
275 130 timeout.tv_sec += persistTimeout;
276
277 130 FD_ZERO(&fds);
278
279 // Stay in this loop until real-time thread exits, in which case
280 // ipc_pipe[0] becomes readable
281 130 eventData = eventDataStart;
282 130 ipc_error = false;
283 343 do {
284 try {
285
0/2
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 1419 for (TaskList::iterator it = task.begin();
286
2/2
✓ Branch 6 taken 946 times.
✓ Branch 7 taken 473 times.
 1419 it != task.end(); ++it)
287
1/2
✓ Branch 6 taken 946 times.
✗ Branch 7 not taken.
 946 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 473 times.
 473 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 473 times.
 527 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 473 times.
 473 FD_SET(terminatePipe, &fds);
313 473 delay.tv_sec = 0;
314 473 delay.tv_usec = 50000; // 20Hz
315
1/2
✓ Branch 4 taken 473 times.
✗ Branch 5 not taken.
 473 rv = ::select(terminatePipe + 1, &fds, 0, 0, &delay);
316
4/6
✓ Branch 0 taken 343 times.
✓ Branch 1 taken 130 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 343 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 343 times.
 473 } while (!(rv or ipc_error));
317
318 130 exit:
319 // Ignore rv if ipc_pipe[0] is readable
320
1/2
✓ Branch 0 taken 130 times.
✗ Branch 1 not taken.
 130 if (rv == 1)
321 130 rv = 0;
322
323
1/2
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
 130 stopServers();
324
325
1/2
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
 130 close(nrtFeedbackPipe);
326 #ifdef _PDSERV_CUSTOM_GCOV_HOOK
327
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 130 __gcov_dump();
328 #endif
329 ✗ std::quick_exit(rv);
330 }
331
332 /////////////////////////////////////////////////////////////////////////////
333
334 ✗ void Main::sleep(int msecs) const
335 {
336 ✗ fd_set fds;
337 ✗ FD_ZERO(&fds);
338 ✗ FD_SET(terminatePipe, &fds);
339
340 ✗ struct timeval delay;
341
342 ✗ delay.tv_sec = msecs / 1000;
343 ✗ delay.tv_usec = (msecs - delay.tv_sec * 1000) * 1000;
344
345 ✗ const int rv = ::select(terminatePipe + 1, &fds, 0, 0, &delay);
346 ✗ if (rv == 1 or (rv == -1 && errno != EINTR)) {
347 ✗ throw PdServ::Main::RtProcessExited{};
348 }
349 }
350
351 /////////////////////////////////////////////////////////////////////////////
352 132 int Main::readConfiguration()
353 {
354 const char *env;
355 132 const char *err = 0;
356
357 // Load custom configuration file
358
2/2
✓ Branch 2 taken 124 times.
✓ Branch 3 taken 8 times.
 132 if (!configFile.empty()) {
359 124 err = m_config.load(configFile.c_str());
360
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 123 times.
 124 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 132 if (!m_config) {
406 log_debug("No configuration loaded");
407 }
408
409
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 131 times.
 132 if (err) {
410 1 return -1;
411 }
412
413 131 return 0;
414 }
415
416 /////////////////////////////////////////////////////////////////////////////
417 1098 PdServ::Config Main::config(const char* key) const
418 {
419 1098 return m_config[key];
420 }
421
422 /////////////////////////////////////////////////////////////////////////////
423 266 Task* Main::addTask(double sampleTime, const char *name)
424 {
425
2/4
✓ Branch 7 taken 266 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 266 times.
✗ Branch 12 not taken.
 266 task.push_back(new Task(this, task.size(), sampleTime, name));
426 266 return task.back();
427 }
428
429 /////////////////////////////////////////////////////////////////////////////
430 187 int Main::gettime(struct timespec* t) const
431 {
432 187 return rttime(t);
433 }
434
435 /////////////////////////////////////////////////////////////////////////////
436 266 Event* Main::addEvent (
437 const char *path, size_t nelem, const char * const *messages)
438 {
439
2/4
✓ Branch 5 taken 266 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 266 times.
✗ Branch 11 not taken.
 266 events.push_back(std::unique_ptr<Event>{new Event(this, path, nelem, messages)});
440
441 266 return events.back().get();
442 }
443
444 /////////////////////////////////////////////////////////////////////////////
445 798 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 798 times.
✗ Branch 5 not taken.
 1596 parameters.push_back(
450
1/2
✓ Branch 3 taken 798 times.
✗ Branch 4 not taken.
 1596 new Parameter(this, addr, path, mode, datatype, n, dim));
451
452 798 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 25 int Main::getValue(const Signal *signal, void* dest, struct timespec* time)
570 {
571 50 pthread::MutexLock lock(sdoMutex);
572 25 struct SDO sdo;
573
574 25 sdo.type = SDO::PollSignal;
575 25 sdo.signal = signal;
576
577
2/4
✓ Branch 4 taken 25 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 25 times.
 25 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 25 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 25 times.
 25 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 25 times.
✗ Branch 11 not taken.
 25 std::copy(signalData, signalData + signal->memSize,
590 reinterpret_cast<char*>(dest));
591
592
1/2
✓ Branch 0 taken 25 times.
✗ Branch 1 not taken.
 25 if (time)
593 25 *time = sdo.time;
594
595 25 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 131 int Main::prefork_init()
616 {
617 131 size_t numTasks = task.size();
618
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 131 times.
 131 size_t taskMemSize[numTasks];
619 size_t i, eventCount;
620 131 size_t maxSignalSize = 0;
621
622 // Find out the largest signal size to reserve space in
623 // shared memory for polling
624 393 for (TaskList::const_iterator it = task.begin();
625
2/2
✓ Branch 6 taken 262 times.
✓ Branch 7 taken 131 times.
 393 it != task.end(); ++it) {
626 262 std::list<const PdServ::Signal*> signals =
627
1/2
✓ Branch 5 taken 262 times.
✗ Branch 6 not taken.
 524 static_cast<PdServ::Task*>(*it)->getSignals();
628
629
2/2
✓ Branch 1 taken 524 times.
✓ Branch 2 taken 262 times.
 1310 while (!signals.empty()) {
630 524 const PdServ::Signal* s = signals.front();
631
632
2/2
✓ Branch 3 taken 131 times.
✓ Branch 4 taken 393 times.
 524 if (s->memSize > maxSignalSize)
633 131 maxSignalSize = s->memSize;
634
635 524 signals.pop_front();
636 }
637
638 }
639 131 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 131 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 131 size_t parameterDataOffset[5] = {0, 0, 0, 0, 0}; // need one extra!
656
657 917 for (ParameterList::iterator it = parameters.begin();
658
2/2
✓ Branch 6 taken 786 times.
✓ Branch 7 taken 131 times.
 917 it != parameters.end(); it++) {
659 786 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 786 times.
✗ Branch 16 not taken.
 786 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 524 times.
✓ Branch 1 taken 131 times.
 655 for (i = 1; i < 5; ++i)
671 524 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 131 shmem_len += parameterDataOffset[4];
676
677 // Now check how much memory is required for events
678 131 eventCount = 0;
679
2/2
✓ Branch 8 taken 262 times.
✓ Branch 9 taken 131 times.
 393 for (EventList::iterator it = events.begin(); it != events.end(); ++it)
680 262 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 131 const size_t eventLen = 10; // Arbitrary
685 131 shmem_len += sizeof(*eventDataStart) * eventLen * eventCount;
686
687 131 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 131 i = 0;
692 393 for (TaskList::const_iterator it = task.begin();
693
2/2
✓ Branch 6 taken 262 times.
✓ Branch 7 taken 131 times.
 393 it != task.end(); ++it) {
694
1/2
✓ Branch 1 taken 262 times.
✗ Branch 2 not taken.
 262 taskMemSize[i] = ptr_align(
695 262 static_cast<const Task*>(*it)->getShmemSpace(bufferTime));
696 262 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 131 shmem_len += (10 + task.size())*sizeof(unsigned long);
703
704 131 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 131 times.
 131 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 131 times.
 131 ::memset(shmem, 0, shmem_len);
716
717 // Now spread the shared memory for the users thereof
718
719 // 1: Parameter data
720 131 parameterData = ptr_align<char>(shmem);
721 917 for (ParameterList::iterator it = parameters.begin();
722
2/2
✓ Branch 6 taken 786 times.
✓ Branch 7 taken 131 times.
 917 it != parameters.end(); it++) {
723 786 Parameter *p = *it;
724 2358 p->shmAddr = parameterData
725
1/2
✓ Branch 15 taken 786 times.
✗ Branch 16 not taken.
 1572 + parameterDataOffset[dataTypeIndex[p->dtype.align()]];
726
1/2
✓ Branch 15 taken 786 times.
✗ Branch 16 not taken.
 786 parameterDataOffset[dataTypeIndex[p->dtype.align()]] += p->memSize;
727
728
1/2
✓ Branch 13 taken 786 times.
✗ Branch 14 not taken.
 786 std::copy(p->addr, p->addr + p->memSize, p->shmAddr);
729 }
730
731 // 2: Signal data area for polling
732 131 signalData = ptr_align<char>(parameterData + parameterDataOffset[4]);
733
734 // 3: Streaming data for tasks
735 131 char* buf = ptr_align<char>(signalData + maxSignalSize);
736 131 i = 0;
737
2/2
✓ Branch 8 taken 262 times.
✓ Branch 9 taken 131 times.
 393 for (TaskList::iterator it = task.begin(); it != task.end(); ++it) {
738
1/2
✓ Branch 8 taken 262 times.
✗ Branch 9 not taken.
 262 static_cast<Task*>(*it)->prepare(buf, buf + taskMemSize[i]);
739 262 buf = ptr_align<char>(buf + taskMemSize[i++]);
740 }
741
742 // 4: Event data
743 131 struct EventData** ptr = ptr_align<struct EventData*>(buf);
744 131 eventDataWp = ptr++;
745 131 eventDataStart = ptr_align<struct EventData>(ptr++);
746 131 eventDataEnd = eventDataStart + eventLen * eventCount;
747 131 *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 131 times.
 131 if ((void*)(eventDataEnd + 1) > (void*)((char*)shmem + shmem_len)) {
759 log_debug("Not enough memory");
760 ✗ return ENOMEM;
761 }
762
763 262 return 0;
764 }
765
766 /////////////////////////////////////////////////////////////////////////////
767 126 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 126 set_priority(sched_get_priority_max(SCHED_FIFO));
772
773 // Parent here; go back to the caller
774 378 for (TaskList::iterator it = task.begin();
775
2/2
✓ Branch 6 taken 252 times.
✓ Branch 7 taken 126 times.
 378 it != task.end(); ++it)
776
1/2
✓ Branch 6 taken 252 times.
✗ Branch 7 not taken.
 252 static_cast<Task*>(*it)->rt_init();
777
778 // Start supervisor thread
779 126 return start();
780 }
781
782 /////////////////////////////////////////////////////////////////////////////
783 130 int Main::postfork_nrt_setup()
784 {
785 // Parent here; go back to the caller
786 390 for (TaskList::iterator it = task.begin();
787
2/2
✓ Branch 6 taken 260 times.
✓ Branch 7 taken 130 times.
 390 it != task.end(); ++it)
788
1/2
✓ Branch 6 taken 260 times.
✗ Branch 7 not taken.
 260 static_cast<Task*>(*it)->nrt_init();
789 130 return 0;
790 }
791
792 /////////////////////////////////////////////////////////////////////////////
793
794 130 void Main::postfork_nrt_subscribe_persistent_signals()
795 {
796
2/2
✓ Branch 6 taken 260 times.
✓ Branch 7 taken 130 times.
 390 for (auto i : task)
797
1/2
✓ Branch 4 taken 260 times.
✗ Branch 5 not taken.
 260 i->nrt_init_persistent();
798 130 }
799
800 /////////////////////////////////////////////////////////////////////////////
801 130 std::list<const PdServ::Parameter*> Main::getParameters() const
802 {
803 return std::list<const PdServ::Parameter*>(
804
1/2
✓ Branch 7 taken 130 times.
✗ Branch 8 not taken.
 130 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 260 std::list<const PdServ::Task*> Main::getTasks() const
818 {
819
1/2
✓ Branch 7 taken 260 times.
✗ Branch 8 not taken.
 260 return std::list<const PdServ::Task*>(task.begin(), task.end());
820 }
821
822 /////////////////////////////////////////////////////////////////////////////
823 124 void Main::prepare(PdServ::Session* session) const
824 {
825 124 PdServ::Main::prepare(session);
826 124 }
827
828 /////////////////////////////////////////////////////////////////////////////
829 124 void Main::cleanup(const PdServ::Session* session) const
830 {
831 124 PdServ::Main::cleanup(session);
832 124 }
833
834 /////////////////////////////////////////////////////////////////////////////
835 126 void Main::run()
836 {
837 126 struct SDO sdo;
838
839 while (true) {
840
3/4
✓ Branch 4 taken 68 times.
✓ Branch 5 taken 126 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 68 times.
 262 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 68 times.
✓ Branch 2 taken 43 times.
✓ Branch 3 taken 25 times.
✗ Branch 4 not taken.
 68 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 25 case SDO::PollSignal:
856
1/2
✓ Branch 16 taken 25 times.
✗ Branch 17 not taken.
 50 sdo.signal->task->pollSignalValue(
857 25 sdo.signal, signalData, &sdo.time);
858 25 break;
859 };
860
861
2/4
✓ Branch 4 taken 68 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 68 times.
 68 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 126 void Main::final()
889 {
890 126 ::close(ipcRx);
891 126 ::close(ipcTx);
892 135 }
893