GCC Code Coverage Report

Directory: ./
File: pdserv/src/lib/Task.cpp
Date: 2025-07-20 04:11:05
Exec Total Coverage
Lines: 225 246 91.5%
Branches: 85 135 63.0%
Line Branch Exec Source
1 /*****************************************************************************
2 *
3 * $Id$
4 *
5 * Copyright 2010 Richard Hacker (lerichi at gmx dot net)
6 *
7 * This file is part of the pdserv library.
8 *
9 * The pdserv library is free software: you can redistribute it and/or modify
10 * it under the terms of the GNU Lesser General Public License as published
11 * by the Free Software Foundation, either version 3 of the License, or (at
12 * your option) any later version.
13 *
14 * The pdserv library is distributed in the hope that it will be useful, but
15 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
16 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
17 * License for more details.
18 *
19 * You should have received a copy of the GNU Lesser General Public License
20 * along with the pdserv library. If not, see <http://www.gnu.org/licenses/>.
21 *
22 *****************************************************************************/
23
24 #include "../Debug.h"
25
26 #include <algorithm>
27 #include <numeric>
28
29 #include "ShmemDataStructures.h"
30 #include "SessionTaskData.h"
31 #include "../SessionTask.h"
32 #include "Main.h"
33 #include "Task.h"
34 #include "Signal.h"
35 #include "Pointer.h"
36
37 /////////////////////////////////////////////////////////////////////////////
38 // Data structures used in Task
39 /////////////////////////////////////////////////////////////////////////////
40 struct CopyList {
41 const Signal *signal;
42 const char *src;
43 size_t len;
44 };
45
46 struct SignalList {
47 enum {Insert = 1, Remove} action;
48 unsigned int signalListId;
49 unsigned int signalPosition;
50 const Signal* signal;
51 };
52
53 /////////////////////////////////////////////////////////////////////////////
54 /////////////////////////////////////////////////////////////////////////////
55 314 Task::Task(Main *main, size_t index, double ts, const char * /*name*/):
56
1/2
✓ Branch 8 taken 314 times.
✗ Branch 9 not taken.
 314 PdServ::Task(index, ts), main(main)
57 {
58 314 seqNo = 0;
59 314 signalMemSize = 0;
60 314 signalListId = 0;
61
1/2
✓ Branch 3 taken 314 times.
✗ Branch 4 not taken.
 314 std::fill_n(signalTypeCount, 4, 0);
62 314 signalCopyList[0] = 0;
63 314 copyList[0] = 0;
64 314 persist = 0;
65 314 m_time.tv_sec = 0;
66 314 m_time.tv_nsec = 0;
67 314 signal_readlock_cb = 0;
68 314 }
69
70 /////////////////////////////////////////////////////////////////////////////
71 942 Task::~Task()
72 {
73
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 314 times.
 314 delete persist;
74
2/2
✓ Branch 3 taken 300 times.
✓ Branch 4 taken 14 times.
 314 delete[] copyList[0];
75
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 314 times.
 314 delete[] signalCopyList[0];
76
77
2/2
✓ Branch 2 taken 942 times.
✓ Branch 3 taken 314 times.
 1256 for (size_t i = 0; i < signals.size(); ++i)
78
1/2
✓ Branch 3 taken 942 times.
✗ Branch 4 not taken.
 942 delete signals[i];
79 628 }
80
81 /////////////////////////////////////////////////////////////////////////////
82 314 void Task::setSignalReadLock(
83 void (*fn)(int state, void* priv_data), void* priv_data)
84 {
85 314 signal_readlock_cb = fn;
86 314 readlock_data = priv_data;
87 314 }
88
89 /////////////////////////////////////////////////////////////////////////////
90 57 void Task::pollSignalValue(
91 const Signal* signal, void* dst, struct timespec* mtime) const
92 {
93 struct LockGuard {
94 57 LockGuard(const Task* t): task(t) {
95
1/2
✓ Branch 6 taken 57 times.
✗ Branch 7 not taken.
 57 if (task->signal_readlock_cb)
96 57 task->signal_readlock_cb(1, task->readlock_data);
97 57 }
98 114 ~LockGuard() {
99
1/2
✓ Branch 6 taken 57 times.
✗ Branch 7 not taken.
 57 if (task->signal_readlock_cb)
100 57 task->signal_readlock_cb(0, task->readlock_data);
101 57 }
102 const Task* const task;
103 };
104
1/2
✓ Branch 2 taken 57 times.
✗ Branch 3 not taken.
 114 LockGuard guard(this);
105
106
1/2
✓ Branch 4 taken 57 times.
✗ Branch 5 not taken.
 57 signal->pollValue(dst, mtime);
107 57 }
108
109 /////////////////////////////////////////////////////////////////////////////
110 942 Signal* Task::addSignal( unsigned int decimation,
111 const char *path, const PdServ::DataType& datatype,
112 const void *addr, size_t n, const size_t *dim)
113 {
114 942 Signal *s = new Signal(this, signals.size(),
115
2/4
✓ Branch 1 taken 942 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 942 times.
✗ Branch 6 not taken.
 942 decimation, path, datatype, addr, n, dim);
116
117
1/2
✓ Branch 2 taken 942 times.
✗ Branch 3 not taken.
 942 signals.push_back(s);
118
1/2
✓ Branch 17 taken 942 times.
✗ Branch 18 not taken.
 942 signalTypeCount[s->dataTypeIndex[s->dtype.align()]]++;
119 942 signalMemSize += s->memSize;
120
121 942 return s;
122 }
123
124 /////////////////////////////////////////////////////////////////////////////
125 630 std::list<const PdServ::Signal*> Task::getSignals() const
126 {
127
1/2
✓ Branch 7 taken 630 times.
✗ Branch 8 not taken.
 630 return std::list<const PdServ::Signal*>(signals.begin(), signals.end());
128 }
129
130 /////////////////////////////////////////////////////////////////////////////
131 57 void Task::getCurrentTime(struct timespec* t) const
132 {
133 57 *t = m_time;
134 57 }
135
136 /////////////////////////////////////////////////////////////////////////////
137 // Initialization methods
138 /////////////////////////////////////////////////////////////////////////////
139 310 size_t Task::getShmemSpace(double T, size_t limit) const
140 {
141 310 size_t n = signals.size();
142 310 size_t minPdoCount = (size_t)(T / sampleTime + 0.5);
143
144
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
 310 if (minPdoCount < 10)
145 ✗ minPdoCount = 10;
146
147 310 size_t pdoSize = (sizeof(*txPdo) + signalMemSize) * minPdoCount;
148
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 310 if (limit and pdoSize > limit)
149 ✗ pdoSize = limit;
150
151 return sizeof(*signalListRp) + sizeof(*signalListWp)
152 + 2 * n * sizeof(*signalList)
153 310 + pdoSize;
154 }
155
156 /////////////////////////////////////////////////////////////////////////////
157 310 void Task::prepare(void *shmem, void *shmem_end)
158 {
159 //log_debug("S(%p): shmem=%p shmem_end=%p", this, shmem, shmem_end);
160 310 size_t n = signals.size();
161
162 310 struct SignalList** ptr = ptr_align<struct SignalList*>(shmem);
163 310 signalListRp = ptr++;
164 310 signalListWp = ptr++;
165
166 310 signalList = ptr_align<struct SignalList>(ptr);
167 310 signalListEnd = signalList + (2*n);
168 310 *signalListRp = signalList;
169 310 *signalListWp = signalList;
170
171 310 txMemBegin = ptr_align<struct Pdo>(signalListEnd);
172 310 txMemEnd = shmem_end;
173 //log_debug("S(%p): txMemBegin=%p", this, txMemBegin);
174
175 310 txPdo = txMemBegin;
176 310 nextTxPdo = ptr_align<struct Pdo*>(shmem_end) - 2;
177 310 }
178
179 /////////////////////////////////////////////////////////////////////////////
180 300 void Task::rt_init()
181 {
182 300 signalMemSize = 0;
183
184
1/2
✓ Branch 2 taken 300 times.
✗ Branch 3 not taken.
 300 copyList[0] = new struct CopyList[signals.size() + 4];
185
2/2
✓ Branch 0 taken 900 times.
✓ Branch 1 taken 300 times.
 1200 for (size_t i = 0; i < 3; i++)
186 900 copyList[i+1] = copyList[i] + signalTypeCount[i] + 1;
187
188
1/2
✓ Branch 3 taken 300 times.
✗ Branch 4 not taken.
 300 std::fill_n(signalTypeCount, 4, 0);
189
190 // Clear src field which is end-of-list marker
191
2/2
✓ Branch 2 taken 2100 times.
✓ Branch 3 taken 300 times.
 2400 for (size_t i = 0; i < signals.size() + 4; ++i)
192 2100 copyList[0][i].src = 0;
193 300 }
194
195 /////////////////////////////////////////////////////////////////////////////
196 308 void Task::nrt_init()
197 {
198
1/2
✓ Branch 2 taken 308 times.
✗ Branch 3 not taken.
 308 signalCopyList[0] = new const Signal*[signals.size()];
199
2/2
✓ Branch 0 taken 924 times.
✓ Branch 1 taken 308 times.
 1232 for (size_t i = 0; i < 3; i++)
200 924 signalCopyList[i+1] = signalCopyList[i] + signalTypeCount[i];
201
202
1/2
✓ Branch 3 taken 308 times.
✗ Branch 4 not taken.
 308 std::fill_n(signalTypeCount, 4, 0);
203 308 }
204
205 /////////////////////////////////////////////////////////////////////////////
206
207 308 void Task::nrt_init_persistent()
208 {
209
2/2
✓ Branch 2 taken 3 times.
✓ Branch 3 taken 305 times.
 308 if (!persistentSet.empty()) {
210
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 persist = new Persistent(this);
211
212 6 for (PersistentSet::iterator it = persistentSet.begin();
213
2/2
✓ Branch 6 taken 3 times.
✓ Branch 7 taken 3 times.
 6 it != persistentSet.end(); ++it)
214
1/2
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
 3 static_cast<const PdServ::Signal*>(*it)->subscribe(persist);
215 }
216 308 }
217
218 /////////////////////////////////////////////////////////////////////////////
219 4 void Task::makePersistent(const Signal* s)
220 {
221
1/2
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 persistentSet.insert(s);
222 4 }
223
224 /////////////////////////////////////////////////////////////////////////////
225 3 bool Task::getPersistentValue(const PdServ::Signal* s,
226 char* buf, const struct timespec** t) const
227 {
228
2/4
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
 3 if (persist->active.find(s) != persist->active.end()) {
229 3 const char *value = s->getValue(persist);
230 3 std::copy(value, value + s->memSize, buf);
231 3 *t = &m_time;
232 }
233
234 // time will be non-zero rxPdo is called at least once
235 3 return m_time.tv_sec;
236 }
237
238 /////////////////////////////////////////////////////////////////////////////
239 /////////////////////////////////////////////////////////////////////////////
240 3 Task::Persistent::Persistent(Task* task):
241 3 PdServ::SessionTask(task)
242 {
243 3 }
244
245 /////////////////////////////////////////////////////////////////////////////
246 3 void Task::Persistent::newSignal(const PdServ::Signal *signal)
247 {
248
1/2
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 active.insert(static_cast<const PdServ::Signal*>(signal));
249 3 }
250
251 /////////////////////////////////////////////////////////////////////////////
252 // Non-real time methods
253 /////////////////////////////////////////////////////////////////////////////
254 302 void Task::getSignalList(const Signal **signalList, size_t *nelem,
255 unsigned int *signalListId)
256 {
257 604 pthread::MutexLock lock(mutex);
258
259
2/2
✓ Branch 0 taken 1208 times.
✓ Branch 1 taken 302 times.
 1510 for (unsigned int i = 0; i < 4; ++i)
260
2/2
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 1208 times.
 1209 for (unsigned int j = 0; j < signalTypeCount[i]; ++j)
261 1 *signalList++ = signalCopyList[i][j];
262
1/2
✓ Branch 1 taken 302 times.
✗ Branch 2 not taken.
 302 *nelem = std::accumulate(signalTypeCount, signalTypeCount + 4, 0);
263 302 *signalListId = (*signalListWp)->signalListId;
264 302 }
265
266 /////////////////////////////////////////////////////////////////////////////
267 91 bool Task::subscribe(const Signal* cs, SessionTaskData* st, bool insert)
268 {
269 182 pthread::MutexLock lock(mutex);
270 91 Signal* signal = const_cast<Signal*>(cs);
271
272 91 struct SignalList *wp = *signalListWp;
273
274
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 91 times.
 91 if (++wp == signalListEnd)
275 ✗ wp = signalList;
276
277
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 91 times.
 91 while (wp == *signalListRp)
278 ✗ main->sleep(
279 ✗ static_cast<unsigned>(sampleTime * 1000 / 2 + 1));
280
281
1/2
✓ Branch 15 taken 91 times.
✗ Branch 16 not taken.
 91 size_t w = cs->dataTypeIndex[cs->dtype.align()];
282 91 const Signal **scl = signalCopyList[w];
283
284 91 wp->signal = cs;
285
286
2/2
✓ Branch 0 taken 47 times.
✓ Branch 1 taken 44 times.
 91 if (insert) {
287 47 bool subscribe = cs->sessions.empty();
288
289
1/2
✓ Branch 2 taken 47 times.
✗ Branch 3 not taken.
 47 signal->sessions.insert(st);
290
291 // return true if the signal is already subscribed
292
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 47 times.
 47 if (!subscribe)
293 ✗ return true;
294
295 47 wp->action = SignalList::Insert;
296
297 47 size_t i = signalTypeCount[w]++;
298 47 scl[i] = cs;
299 47 signal->copyListPos = i;
300 }
301 else {
302 44 bool unsubscribe = !cs->sessions.empty();
303
304
1/2
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
 44 signal->sessions.erase(st);
305
306
3/6
✓ Branch 0 taken 44 times.
✗ Branch 1 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 44 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 44 times.
 44 if (!unsubscribe or !cs->sessions.empty())
307 ✗ return true;
308
309 44 wp->action = SignalList::Remove;
310 44 wp->signalPosition = signal->copyListPos;
311
312 // Replace s with last signal on the list
313 44 cs = scl[--signalTypeCount[w]];
314 44 scl[signal->copyListPos] = cs;
315 44 signals[cs->index]->copyListPos = signal->copyListPos;
316 }
317
318 91 wp->signalListId = ++signalListId;
319 91 signal->subscriptionId = signalListId;
320
321 #ifdef __GNUC__
322 91 __sync_synchronize(); // write memory barrier
323 #endif
324
325 91 *signalListWp = wp;
326
327 91 return false;
328 }
329
330 /////////////////////////////////////////////////////////////////////////////
331 ✗ void Task::updateStatistics(
332 double exec_time, double cycle_time, unsigned int overrun)
333 {
334 ✗ taskStatistics.exec_time = exec_time;
335 ✗ taskStatistics.cycle_time = cycle_time;
336 ✗ taskStatistics.overrun = overrun;
337 }
338
339 /////////////////////////////////////////////////////////////////////////////
340 299 void Task::prepare (PdServ::SessionTask *s) const
341 {
342 299 s->sessionTaskData =
343
1/2
✓ Branch 9 taken 299 times.
✗ Branch 10 not taken.
 299 new SessionTaskData(s, &signals, txMemBegin, txMemEnd);
344 299 }
345
346 /////////////////////////////////////////////////////////////////////////////
347 296 void Task::cleanup (const PdServ::SessionTask *s) const
348 {
349
1/2
✓ Branch 3 taken 296 times.
✗ Branch 4 not taken.
 296 delete s->sessionTaskData;
350 296 }
351
352 /////////////////////////////////////////////////////////////////////////////
353 23551 bool Task::rxPdo (PdServ::SessionTask *s, const struct timespec **time,
354 const PdServ::TaskStatistics **stat) const
355 {
356 23551 return s->sessionTaskData->rxPdo(time, stat);
357 }
358
359 /////////////////////////////////////////////////////////////////////////////
360 1754 void Task::nrt_update()
361 {
362
2/2
✓ Branch 3 taken 1694 times.
✓ Branch 4 taken 60 times.
 1754 if (!persist)
363 1694 return;
364
365 60 const PdServ::TaskStatistics *stat;
366 60 const struct timespec *dummy_time;
367
5/6
✓ Branch 9 taken 343 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 283 times.
✓ Branch 12 taken 60 times.
✓ Branch 15 taken 60 times.
✓ Branch 16 taken 1694 times.
 626 while (rxPdo(persist, &dummy_time, &stat)) {
368 // Required for persistent signal->parameter pair
369 283 m_time = *dummy_time;
370 }
371 }
372
373 /////////////////////////////////////////////////////////////////////////////
374 // Real time methods
375 /////////////////////////////////////////////////////////////////////////////
376 4164 void Task::rt_update(const struct timespec *t)
377 {
378
2/2
✓ Branch 8 taken 42 times.
✓ Branch 9 taken 4122 times.
 4164 if (*signalListRp != *signalListWp) {
379
2/2
✓ Branch 8 taken 47 times.
✓ Branch 9 taken 42 times.
 136 while (*signalListRp != *signalListWp)
380 47 processSignalList();
381
382 42 calculateCopyList();
383 }
384
385
1/2
✓ Branch 0 taken 4164 times.
✗ Branch 1 not taken.
 4164 if (t) {
386 4164 m_time = *t;
387 } else {
388 ✗ m_time = {0, 0};
389 }
390
391 4164 copyData(t);
392 4164 }
393
394 /////////////////////////////////////////////////////////////////////////////
395 47 void Task::processSignalList()
396 {
397 47 struct SignalList *sp = *signalListRp + 1;
398
399
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 47 times.
 47 if (sp == signalListEnd)
400 ✗ sp = signalList;
401
402 47 signalListId = sp->signalListId;
403 47 const Signal *signal = sp->signal;
404 47 size_t w = signal->dataTypeIndex[signal->dtype.align()];
405 struct CopyList *cl;
406
407
2/5
✗ Branch 1 not taken.
✓ Branch 2 taken 47 times.
✓ Branch 3 taken 47 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 47 switch (sp->action) {
408 47 case SignalList::Insert:
409 // Insert the signal at list end
410 47 cl = copyList[w] + signalTypeCount[w]++;
411
412 47 cl->src = signal->addr;
413 47 cl->len = signal->memSize;
414 47 cl->signal = signal;
415
416 47 signalMemSize += signal->memSize;
417 47 break;
418
419 ✗ case SignalList::Remove:
420 // Move signal at list end to the deleted position
421 ✗ cl = copyList[w] + --signalTypeCount[w];
422 ✗ copyList[w][sp->signalPosition] = *cl;
423 ✗ cl->src = 0; // End of copy list indicator
424
425 ✗ signalMemSize -= signal->memSize;
426 ✗ break;
427 }
428
429 #ifdef __GNUC__
430 47 __sync_synchronize(); // write memory barrier
431 #endif
432
433 47 *signalListRp = sp;
434 47 }
435
436 /////////////////////////////////////////////////////////////////////////////
437 42 void Task::calculateCopyList()
438 {
439 42 size_t n = std::accumulate(signalTypeCount, signalTypeCount + 4, 0);
440
441
1/2
✗ Branch 6 not taken.
✓ Branch 7 taken 42 times.
 42 if ((&txPdo->signalIdx + n) >= txMemEnd)
442 ✗ txPdo = txMemBegin;
443
444 42 txPdo->next = 0;
445 42 txPdo->type = Pdo::Empty;
446 42 txPdo->signalListId = signalListId;
447 42 txPdo->count = n;
448 42 size_t *sp = &txPdo->signalIdx;
449
2/2
✓ Branch 0 taken 168 times.
✓ Branch 1 taken 42 times.
 210 for (int i = 0; i < 4; i++) {
450
2/2
✓ Branch 5 taken 50 times.
✓ Branch 6 taken 168 times.
 218 for (CopyList *cl = copyList[i]; cl->src; ++cl)
451 50 *sp++ = cl->signal->index;
452 }
453 42 txPdo->type = Pdo::SignalList;
454
455 #ifdef __GNUC__
456 42 __sync_synchronize(); // write memory barrier
457 #endif
458
459 42 *nextTxPdo = txPdo;
460
461 42 nextTxPdo = &txPdo->next;
462 42 txPdo = ptr_align<Pdo>(sp);
463 42 }
464
465 /////////////////////////////////////////////////////////////////////////////
466 4164 void Task::copyData(const struct timespec *t)
467 {
468
2/2
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 4163 times.
 4164 if ( &txPdo->data + signalMemSize >= txMemEnd)
469 1 txPdo = txMemBegin;
470
471 4164 txPdo->next = 0;
472 4164 txPdo->type = Pdo::Empty;
473 4164 txPdo->signalListId = signalListId;
474 4164 txPdo->seqNo = seqNo++;
475
476 4164 txPdo->taskStatistics = taskStatistics;
477
478
1/2
✓ Branch 0 taken 4164 times.
✗ Branch 1 not taken.
 4164 if (t)
479 4164 txPdo->time = *t;
480 else
481 ✗ txPdo->time.tv_sec = txPdo->time.tv_nsec = 0;
482
483 4164 char *p = &txPdo->data;
484
2/2
✓ Branch 0 taken 16656 times.
✓ Branch 1 taken 4164 times.
 20820 for (int i = 0; i < 4; ++i) {
485
2/2
✓ Branch 5 taken 2924 times.
✓ Branch 6 taken 16656 times.
 19580 for (CopyList *cl = copyList[i]; cl->src; ++cl) {
486 2924 std::copy(cl->src, cl->src + cl->len, p);
487 2924 p += cl->len;
488 }
489 }
490
491 4164 txPdo->type = Pdo::Data;
492 4164 txPdo->count = p - &txPdo->data;
493
494 #ifdef __GNUC__
495 4164 __sync_synchronize(); // write memory barrier
496 #endif
497
498 4164 *nextTxPdo = txPdo;
499
500 4164 nextTxPdo = &txPdo->next;
501 4164 txPdo = ptr_align<Pdo>(p);
502 4164 }
503