GCC Code Coverage Report

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