GCC Code Coverage Report

Directory: ./
File: pdserv/src/lib/Task.cpp
Date: 2023-11-12 04:06:57
Exec Total Coverage
Lines: 223 243 91.8%
Branches: 84 131 64.1%
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 266 Task::Task(Main *main, size_t index, double ts, const char * /*name*/):
56
1/2
✓ Branch 8 taken 266 times.
✗ Branch 9 not taken.
 266 PdServ::Task(index, ts), main(main)
57 {
58 266 seqNo = 0;
59 266 signalMemSize = 0;
60 266 signalListId = 0;
61
1/2
✓ Branch 3 taken 266 times.
✗ Branch 4 not taken.
 266 std::fill_n(signalTypeCount, 4, 0);
62 266 signalCopyList[0] = 0;
63 266 copyList[0] = 0;
64 266 persist = 0;
65 266 m_time.tv_sec = 0;
66 266 m_time.tv_nsec = 0;
67 266 signal_readlock_cb = 0;
68 266 }
69
70 /////////////////////////////////////////////////////////////////////////////
71 798 Task::~Task()
72 {
73
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 266 times.
 266 delete persist;
74
2/2
✓ Branch 3 taken 252 times.
✓ Branch 4 taken 14 times.
 266 delete[] copyList[0];
75
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 266 times.
 266 delete[] signalCopyList[0];
76
77
2/2
✓ Branch 2 taken 532 times.
✓ Branch 3 taken 266 times.
 798 for (size_t i = 0; i < signals.size(); ++i)
78
1/2
✓ Branch 3 taken 532 times.
✗ Branch 4 not taken.
 532 delete signals[i];
79 532 }
80
81 /////////////////////////////////////////////////////////////////////////////
82 266 void Task::setSignalReadLock(
83 void (*fn)(int state, void* priv_data), void* priv_data)
84 {
85 266 signal_readlock_cb = fn;
86 266 readlock_data = priv_data;
87 266 }
88
89 /////////////////////////////////////////////////////////////////////////////
90 25 void Task::pollSignalValue(
91 const Signal* signal, void* dst, struct timespec* mtime) const
92 {
93 struct LockGuard {
94 25 LockGuard(const Task* t): task(t) {
95
1/2
✓ Branch 6 taken 25 times.
✗ Branch 7 not taken.
 25 if (task->signal_readlock_cb)
96 25 task->signal_readlock_cb(1, task->readlock_data);
97 25 }
98 50 ~LockGuard() {
99
1/2
✓ Branch 6 taken 25 times.
✗ Branch 7 not taken.
 25 if (task->signal_readlock_cb)
100 25 task->signal_readlock_cb(0, task->readlock_data);
101 25 }
102 const Task* const task;
103 };
104
1/2
✓ Branch 2 taken 25 times.
✗ Branch 3 not taken.
 50 LockGuard guard(this);
105
106
1/2
✓ Branch 4 taken 25 times.
✗ Branch 5 not taken.
 25 signal->pollValue(dst, mtime);
107 25 }
108
109 /////////////////////////////////////////////////////////////////////////////
110 532 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 532 Signal *s = new Signal(this, signals.size(),
115
2/4
✓ Branch 1 taken 532 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 532 times.
✗ Branch 6 not taken.
 532 decimation, path, datatype, addr, n, dim);
116
117
1/2
✓ Branch 2 taken 532 times.
✗ Branch 3 not taken.
 532 signals.push_back(s);
118
1/2
✓ Branch 17 taken 532 times.
✗ Branch 18 not taken.
 532 signalTypeCount[s->dataTypeIndex[s->dtype.align()]]++;
119 532 signalMemSize += s->memSize;
120
121 532 return s;
122 }
123
124 /////////////////////////////////////////////////////////////////////////////
125 534 std::list<const PdServ::Signal*> Task::getSignals() const
126 {
127
1/2
✓ Branch 7 taken 534 times.
✗ Branch 8 not taken.
 534 return std::list<const PdServ::Signal*>(signals.begin(), signals.end());
128 }
129
130 /////////////////////////////////////////////////////////////////////////////
131 25 void Task::getCurrentTime(struct timespec* t) const
132 {
133 25 *t = m_time;
134 25 }
135
136 /////////////////////////////////////////////////////////////////////////////
137 // Initialization methods
138 /////////////////////////////////////////////////////////////////////////////
139 262 size_t Task::getShmemSpace(double T) const
140 {
141 262 size_t n = signals.size();
142 262 size_t minPdoCount = (size_t)(T / sampleTime + 0.5);
143
144
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 262 times.
 262 if (minPdoCount < 10)
145 ✗ minPdoCount = 10;
146
147 return sizeof(*signalListRp) + sizeof(*signalListWp)
148 + 2 * n * sizeof(*signalList)
149 262 + (sizeof(*txPdo) + signalMemSize) * minPdoCount;
150 }
151
152 /////////////////////////////////////////////////////////////////////////////
153 262 void Task::prepare(void *shmem, void *shmem_end)
154 {
155 //log_debug("S(%p): shmem=%p shmem_end=%p", this, shmem, shmem_end);
156 262 size_t n = signals.size();
157
158 262 struct SignalList** ptr = ptr_align<struct SignalList*>(shmem);
159 262 signalListRp = ptr++;
160 262 signalListWp = ptr++;
161
162 262 signalList = ptr_align<struct SignalList>(ptr);
163 262 signalListEnd = signalList + (2*n);
164 262 *signalListRp = signalList;
165 262 *signalListWp = signalList;
166
167 262 txMemBegin = ptr_align<struct Pdo>(signalListEnd);
168 262 txMemEnd = shmem_end;
169 //log_debug("S(%p): txMemBegin=%p", this, txMemBegin);
170
171 262 txPdo = txMemBegin;
172 262 nextTxPdo = ptr_align<struct Pdo*>(shmem_end) - 2;
173 262 }
174
175 /////////////////////////////////////////////////////////////////////////////
176 252 void Task::rt_init()
177 {
178 252 signalMemSize = 0;
179
180
1/2
✓ Branch 2 taken 252 times.
✗ Branch 3 not taken.
 252 copyList[0] = new struct CopyList[signals.size() + 4];
181
2/2
✓ Branch 0 taken 756 times.
✓ Branch 1 taken 252 times.
 1008 for (size_t i = 0; i < 3; i++)
182 756 copyList[i+1] = copyList[i] + signalTypeCount[i] + 1;
183
184
1/2
✓ Branch 3 taken 252 times.
✗ Branch 4 not taken.
 252 std::fill_n(signalTypeCount, 4, 0);
185
186 // Clear src field which is end-of-list marker
187
2/2
✓ Branch 2 taken 1512 times.
✓ Branch 3 taken 252 times.
 1764 for (size_t i = 0; i < signals.size() + 4; ++i)
188 1512 copyList[0][i].src = 0;
189 252 }
190
191 /////////////////////////////////////////////////////////////////////////////
192 260 void Task::nrt_init()
193 {
194
1/2
✓ Branch 2 taken 260 times.
✗ Branch 3 not taken.
 260 signalCopyList[0] = new const Signal*[signals.size()];
195
2/2
✓ Branch 0 taken 780 times.
✓ Branch 1 taken 260 times.
 1040 for (size_t i = 0; i < 3; i++)
196 780 signalCopyList[i+1] = signalCopyList[i] + signalTypeCount[i];
197
198
1/2
✓ Branch 3 taken 260 times.
✗ Branch 4 not taken.
 260 std::fill_n(signalTypeCount, 4, 0);
199 260 }
200
201 /////////////////////////////////////////////////////////////////////////////
202
203 260 void Task::nrt_init_persistent()
204 {
205
2/2
✓ Branch 2 taken 3 times.
✓ Branch 3 taken 257 times.
 260 if (!persistentSet.empty()) {
206
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 persist = new Persistent(this);
207
208 6 for (PersistentSet::iterator it = persistentSet.begin();
209
2/2
✓ Branch 6 taken 3 times.
✓ Branch 7 taken 3 times.
 6 it != persistentSet.end(); ++it)
210
1/2
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
 3 static_cast<const PdServ::Signal*>(*it)->subscribe(persist);
211 }
212 260 }
213
214 /////////////////////////////////////////////////////////////////////////////
215 4 void Task::makePersistent(const Signal* s)
216 {
217
1/2
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 persistentSet.insert(s);
218 4 }
219
220 /////////////////////////////////////////////////////////////////////////////
221 3 bool Task::getPersistentValue(const PdServ::Signal* s,
222 char* buf, const struct timespec** t) const
223 {
224
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()) {
225 3 const char *value = s->getValue(persist);
226 3 std::copy(value, value + s->memSize, buf);
227 3 *t = &m_time;
228 }
229
230 // time will be non-zero rxPdo is called at least once
231 3 return m_time.tv_sec;
232 }
233
234 /////////////////////////////////////////////////////////////////////////////
235 /////////////////////////////////////////////////////////////////////////////
236 3 Task::Persistent::Persistent(Task* task):
237 3 PdServ::SessionTask(task)
238 {
239 3 }
240
241 /////////////////////////////////////////////////////////////////////////////
242 3 void Task::Persistent::newSignal(const PdServ::Signal *signal)
243 {
244
1/2
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 active.insert(static_cast<const PdServ::Signal*>(signal));
245 3 }
246
247 /////////////////////////////////////////////////////////////////////////////
248 // Non-real time methods
249 /////////////////////////////////////////////////////////////////////////////
250 251 void Task::getSignalList(const Signal **signalList, size_t *nelem,
251 unsigned int *signalListId)
252 {
253 502 pthread::MutexLock lock(mutex);
254
255
2/2
✓ Branch 0 taken 1004 times.
✓ Branch 1 taken 251 times.
 1255 for (unsigned int i = 0; i < 4; ++i)
256
2/2
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 1004 times.
 1005 for (unsigned int j = 0; j < signalTypeCount[i]; ++j)
257 1 *signalList++ = signalCopyList[i][j];
258
1/2
✓ Branch 1 taken 251 times.
✗ Branch 2 not taken.
 251 *nelem = std::accumulate(signalTypeCount, signalTypeCount + 4, 0);
259 251 *signalListId = (*signalListWp)->signalListId;
260 251 }
261
262 /////////////////////////////////////////////////////////////////////////////
263 27 bool Task::subscribe(const Signal* cs, SessionTaskData* st, bool insert)
264 {
265 54 pthread::MutexLock lock(mutex);
266 27 Signal* signal = const_cast<Signal*>(cs);
267
268 27 struct SignalList *wp = *signalListWp;
269
270
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 27 times.
 27 if (++wp == signalListEnd)
271 ✗ wp = signalList;
272
273
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 27 times.
 27 while (wp == *signalListRp)
274 ✗ main->sleep(
275 ✗ static_cast<unsigned>(sampleTime * 1000 / 2 + 1));
276
277
1/2
✓ Branch 15 taken 27 times.
✗ Branch 16 not taken.
 27 size_t w = cs->dataTypeIndex[cs->dtype.align()];
278 27 const Signal **scl = signalCopyList[w];
279
280 27 wp->signal = cs;
281
282
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 12 times.
 27 if (insert) {
283 15 bool subscribe = cs->sessions.empty();
284
285
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
 15 signal->sessions.insert(st);
286
287 // return true if the signal is already subscribed
288
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 15 times.
 15 if (!subscribe)
289 ✗ return true;
290
291 15 wp->action = SignalList::Insert;
292
293 15 size_t i = signalTypeCount[w]++;
294 15 scl[i] = cs;
295 15 signal->copyListPos = i;
296 }
297 else {
298 12 bool unsubscribe = !cs->sessions.empty();
299
300
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 12 signal->sessions.erase(st);
301
302
3/6
✓ Branch 0 taken 12 times.
✗ Branch 1 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 12 times.
 12 if (!unsubscribe or !cs->sessions.empty())
303 ✗ return true;
304
305 12 wp->action = SignalList::Remove;
306 12 wp->signalPosition = signal->copyListPos;
307
308 // Replace s with last signal on the list
309 12 cs = scl[--signalTypeCount[w]];
310 12 scl[signal->copyListPos] = cs;
311 12 signals[cs->index]->copyListPos = signal->copyListPos;
312 }
313
314 27 wp->signalListId = ++signalListId;
315 27 signal->subscriptionId = signalListId;
316
317 #ifdef __GNUC__
318 27 __sync_synchronize(); // write memory barrier
319 #endif
320
321 27 *signalListWp = wp;
322
323 27 return false;
324 }
325
326 /////////////////////////////////////////////////////////////////////////////
327 ✗ void Task::updateStatistics(
328 double exec_time, double cycle_time, unsigned int overrun)
329 {
330 ✗ taskStatistics.exec_time = exec_time;
331 ✗ taskStatistics.cycle_time = cycle_time;
332 ✗ taskStatistics.overrun = overrun;
333 }
334
335 /////////////////////////////////////////////////////////////////////////////
336 251 void Task::prepare (PdServ::SessionTask *s) const
337 {
338 251 s->sessionTaskData =
339
1/2
✓ Branch 9 taken 251 times.
✗ Branch 10 not taken.
 251 new SessionTaskData(s, &signals, txMemBegin, txMemEnd);
340 251 }
341
342 /////////////////////////////////////////////////////////////////////////////
343 248 void Task::cleanup (const PdServ::SessionTask *s) const
344 {
345
1/2
✓ Branch 3 taken 248 times.
✗ Branch 4 not taken.
 248 delete s->sessionTaskData;
346 248 }
347
348 /////////////////////////////////////////////////////////////////////////////
349 14878 bool Task::rxPdo (PdServ::SessionTask *s, const struct timespec **time,
350 const PdServ::TaskStatistics **stat) const
351 {
352 14878 return s->sessionTaskData->rxPdo(time, stat);
353 }
354
355 /////////////////////////////////////////////////////////////////////////////
356 946 void Task::nrt_update()
357 {
358
2/2
✓ Branch 3 taken 883 times.
✓ Branch 4 taken 63 times.
 946 if (!persist)
359 883 return;
360
361 63 const PdServ::TaskStatistics *stat;
362 63 const struct timespec *dummy_time;
363
5/6
✓ Branch 9 taken 360 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 297 times.
✓ Branch 12 taken 63 times.
✓ Branch 15 taken 63 times.
✓ Branch 16 taken 883 times.
 657 while (rxPdo(persist, &dummy_time, &stat)) {
364 // Required for persistent signal->parameter pair
365 297 m_time = *dummy_time;
366 }
367 }
368
369 /////////////////////////////////////////////////////////////////////////////
370 // Real time methods
371 /////////////////////////////////////////////////////////////////////////////
372 2103 void Task::rt_update(const struct timespec *t)
373 {
374
2/2
✓ Branch 8 taken 15 times.
✓ Branch 9 taken 2088 times.
 2103 if (*signalListRp != *signalListWp) {
375
2/2
✓ Branch 8 taken 15 times.
✓ Branch 9 taken 15 times.
 45 while (*signalListRp != *signalListWp)
376 15 processSignalList();
377
378 15 calculateCopyList();
379 }
380
381
1/2
✓ Branch 0 taken 2103 times.
✗ Branch 1 not taken.
 2103 if (t) {
382 2103 m_time = *t;
383 } else {
384 ✗ m_time = {0, 0};
385 }
386
387 2103 copyData(t);
388 2103 }
389
390 /////////////////////////////////////////////////////////////////////////////
391 15 void Task::processSignalList()
392 {
393 15 struct SignalList *sp = *signalListRp + 1;
394
395
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 15 times.
 15 if (sp == signalListEnd)
396 ✗ sp = signalList;
397
398 15 signalListId = sp->signalListId;
399 15 const Signal *signal = sp->signal;
400 15 size_t w = signal->dataTypeIndex[signal->dtype.align()];
401 struct CopyList *cl;
402
403
2/5
✗ Branch 1 not taken.
✓ Branch 2 taken 15 times.
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 15 switch (sp->action) {
404 15 case SignalList::Insert:
405 // Insert the signal at list end
406 15 cl = copyList[w] + signalTypeCount[w]++;
407
408 15 cl->src = signal->addr;
409 15 cl->len = signal->memSize;
410 15 cl->signal = signal;
411
412 15 signalMemSize += signal->memSize;
413 15 break;
414
415 ✗ case SignalList::Remove:
416 // Move signal at list end to the deleted position
417 ✗ cl = copyList[w] + --signalTypeCount[w];
418 ✗ copyList[w][sp->signalPosition] = *cl;
419 ✗ cl->src = 0; // End of copy list indicator
420
421 ✗ signalMemSize -= signal->memSize;
422 ✗ break;
423 }
424
425 #ifdef __GNUC__
426 15 __sync_synchronize(); // write memory barrier
427 #endif
428
429 15 *signalListRp = sp;
430 15 }
431
432 /////////////////////////////////////////////////////////////////////////////
433 15 void Task::calculateCopyList()
434 {
435 15 size_t n = std::accumulate(signalTypeCount, signalTypeCount + 4, 0);
436
437
1/2
✗ Branch 6 not taken.
✓ Branch 7 taken 15 times.
 15 if ((&txPdo->signalIdx + n) >= txMemEnd)
438 ✗ txPdo = txMemBegin;
439
440 15 txPdo->next = 0;
441 15 txPdo->type = Pdo::Empty;
442 15 txPdo->signalListId = signalListId;
443 15 txPdo->count = n;
444 15 size_t *sp = &txPdo->signalIdx;
445
2/2
✓ Branch 0 taken 60 times.
✓ Branch 1 taken 15 times.
 75 for (int i = 0; i < 4; i++) {
446
2/2
✓ Branch 5 taken 15 times.
✓ Branch 6 taken 60 times.
 75 for (CopyList *cl = copyList[i]; cl->src; ++cl)
447 15 *sp++ = cl->signal->index;
448 }
449 15 txPdo->type = Pdo::SignalList;
450
451 #ifdef __GNUC__
452 15 __sync_synchronize(); // write memory barrier
453 #endif
454
455 15 *nextTxPdo = txPdo;
456
457 15 nextTxPdo = &txPdo->next;
458 15 txPdo = ptr_align<Pdo>(sp);
459 15 }
460
461 /////////////////////////////////////////////////////////////////////////////
462 2103 void Task::copyData(const struct timespec *t)
463 {
464
2/2
✓ Branch 9 taken 2 times.
✓ Branch 10 taken 2101 times.
 2103 if ( &txPdo->data + signalMemSize >= txMemEnd)
465 2 txPdo = txMemBegin;
466
467 2103 txPdo->next = 0;
468 2103 txPdo->type = Pdo::Empty;
469 2103 txPdo->signalListId = signalListId;
470 2103 txPdo->seqNo = seqNo++;
471
472 2103 txPdo->taskStatistics = taskStatistics;
473
474
1/2
✓ Branch 0 taken 2103 times.
✗ Branch 1 not taken.
 2103 if (t)
475 2103 txPdo->time = *t;
476 else
477 ✗ txPdo->time.tv_sec = txPdo->time.tv_nsec = 0;
478
479 2103 char *p = &txPdo->data;
480
2/2
✓ Branch 0 taken 8412 times.
✓ Branch 1 taken 2103 times.
 10515 for (int i = 0; i < 4; ++i) {
481
2/2
✓ Branch 5 taken 787 times.
✓ Branch 6 taken 8412 times.
 9199 for (CopyList *cl = copyList[i]; cl->src; ++cl) {
482 787 std::copy(cl->src, cl->src + cl->len, p);
483 787 p += cl->len;
484 }
485 }
486
487 2103 txPdo->type = Pdo::Data;
488 2103 txPdo->count = p - &txPdo->data;
489
490 #ifdef __GNUC__
491 2103 __sync_synchronize(); // write memory barrier
492 #endif
493
494 2103 *nextTxPdo = txPdo;
495
496 2103 nextTxPdo = &txPdo->next;
497 2103 txPdo = ptr_align<Pdo>(p);
498 2103 }
499