GCC Code Coverage Report

Directory: ./
File: pdserv/src/lib/Task.cpp
Date: 2024-11-17 04:08:36
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 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) 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 return sizeof(*signalListRp) + sizeof(*signalListWp)
148 + 2 * n * sizeof(*signalList)
149 310 + (sizeof(*txPdo) + signalMemSize) * minPdoCount;
150 }
151
152 /////////////////////////////////////////////////////////////////////////////
153 310 void Task::prepare(void *shmem, void *shmem_end)
154 {
155 //log_debug("S(%p): shmem=%p shmem_end=%p", this, shmem, shmem_end);
156 310 size_t n = signals.size();
157
158 310 struct SignalList** ptr = ptr_align<struct SignalList*>(shmem);
159 310 signalListRp = ptr++;
160 310 signalListWp = ptr++;
161
162 310 signalList = ptr_align<struct SignalList>(ptr);
163 310 signalListEnd = signalList + (2*n);
164 310 *signalListRp = signalList;
165 310 *signalListWp = signalList;
166
167 310 txMemBegin = ptr_align<struct Pdo>(signalListEnd);
168 310 txMemEnd = shmem_end;
169 //log_debug("S(%p): txMemBegin=%p", this, txMemBegin);
170
171 310 txPdo = txMemBegin;
172 310 nextTxPdo = ptr_align<struct Pdo*>(shmem_end) - 2;
173 310 }
174
175 /////////////////////////////////////////////////////////////////////////////
176 300 void Task::rt_init()
177 {
178 300 signalMemSize = 0;
179
180
1/2
✓ Branch 2 taken 300 times.
✗ Branch 3 not taken.
 300 copyList[0] = new struct CopyList[signals.size() + 4];
181
2/2
✓ Branch 0 taken 900 times.
✓ Branch 1 taken 300 times.
 1200 for (size_t i = 0; i < 3; i++)
182 900 copyList[i+1] = copyList[i] + signalTypeCount[i] + 1;
183
184
1/2
✓ Branch 3 taken 300 times.
✗ Branch 4 not taken.
 300 std::fill_n(signalTypeCount, 4, 0);
185
186 // Clear src field which is end-of-list marker
187
2/2
✓ Branch 2 taken 2100 times.
✓ Branch 3 taken 300 times.
 2400 for (size_t i = 0; i < signals.size() + 4; ++i)
188 2100 copyList[0][i].src = 0;
189 300 }
190
191 /////////////////////////////////////////////////////////////////////////////
192 308 void Task::nrt_init()
193 {
194
1/2
✓ Branch 2 taken 308 times.
✗ Branch 3 not taken.
 308 signalCopyList[0] = new const Signal*[signals.size()];
195
2/2
✓ Branch 0 taken 924 times.
✓ Branch 1 taken 308 times.
 1232 for (size_t i = 0; i < 3; i++)
196 924 signalCopyList[i+1] = signalCopyList[i] + signalTypeCount[i];
197
198
1/2
✓ Branch 3 taken 308 times.
✗ Branch 4 not taken.
 308 std::fill_n(signalTypeCount, 4, 0);
199 308 }
200
201 /////////////////////////////////////////////////////////////////////////////
202
203 308 void Task::nrt_init_persistent()
204 {
205
2/2
✓ Branch 2 taken 3 times.
✓ Branch 3 taken 305 times.
 308 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 308 }
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 302 void Task::getSignalList(const Signal **signalList, size_t *nelem,
251 unsigned int *signalListId)
252 {
253 604 pthread::MutexLock lock(mutex);
254
255
2/2
✓ Branch 0 taken 1208 times.
✓ Branch 1 taken 302 times.
 1510 for (unsigned int i = 0; i < 4; ++i)
256
2/2
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 1208 times.
 1209 for (unsigned int j = 0; j < signalTypeCount[i]; ++j)
257 1 *signalList++ = signalCopyList[i][j];
258
1/2
✓ Branch 1 taken 302 times.
✗ Branch 2 not taken.
 302 *nelem = std::accumulate(signalTypeCount, signalTypeCount + 4, 0);
259 302 *signalListId = (*signalListWp)->signalListId;
260 302 }
261
262 /////////////////////////////////////////////////////////////////////////////
263 91 bool Task::subscribe(const Signal* cs, SessionTaskData* st, bool insert)
264 {
265 182 pthread::MutexLock lock(mutex);
266 91 Signal* signal = const_cast<Signal*>(cs);
267
268 91 struct SignalList *wp = *signalListWp;
269
270
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 91 times.
 91 if (++wp == signalListEnd)
271 ✗ wp = signalList;
272
273
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 91 times.
 91 while (wp == *signalListRp)
274 ✗ main->sleep(
275 ✗ static_cast<unsigned>(sampleTime * 1000 / 2 + 1));
276
277
1/2
✓ Branch 15 taken 91 times.
✗ Branch 16 not taken.
 91 size_t w = cs->dataTypeIndex[cs->dtype.align()];
278 91 const Signal **scl = signalCopyList[w];
279
280 91 wp->signal = cs;
281
282
2/2
✓ Branch 0 taken 47 times.
✓ Branch 1 taken 44 times.
 91 if (insert) {
283 47 bool subscribe = cs->sessions.empty();
284
285
1/2
✓ Branch 2 taken 47 times.
✗ Branch 3 not taken.
 47 signal->sessions.insert(st);
286
287 // return true if the signal is already subscribed
288
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 47 times.
 47 if (!subscribe)
289 ✗ return true;
290
291 47 wp->action = SignalList::Insert;
292
293 47 size_t i = signalTypeCount[w]++;
294 47 scl[i] = cs;
295 47 signal->copyListPos = i;
296 }
297 else {
298 44 bool unsubscribe = !cs->sessions.empty();
299
300
1/2
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
 44 signal->sessions.erase(st);
301
302
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())
303 ✗ return true;
304
305 44 wp->action = SignalList::Remove;
306 44 wp->signalPosition = signal->copyListPos;
307
308 // Replace s with last signal on the list
309 44 cs = scl[--signalTypeCount[w]];
310 44 scl[signal->copyListPos] = cs;
311 44 signals[cs->index]->copyListPos = signal->copyListPos;
312 }
313
314 91 wp->signalListId = ++signalListId;
315 91 signal->subscriptionId = signalListId;
316
317 #ifdef __GNUC__
318 91 __sync_synchronize(); // write memory barrier
319 #endif
320
321 91 *signalListWp = wp;
322
323 91 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 299 void Task::prepare (PdServ::SessionTask *s) const
337 {
338 299 s->sessionTaskData =
339
1/2
✓ Branch 9 taken 299 times.
✗ Branch 10 not taken.
 299 new SessionTaskData(s, &signals, txMemBegin, txMemEnd);
340 299 }
341
342 /////////////////////////////////////////////////////////////////////////////
343 296 void Task::cleanup (const PdServ::SessionTask *s) const
344 {
345
1/2
✓ Branch 3 taken 296 times.
✗ Branch 4 not taken.
 296 delete s->sessionTaskData;
346 296 }
347
348 /////////////////////////////////////////////////////////////////////////////
349 23442 bool Task::rxPdo (PdServ::SessionTask *s, const struct timespec **time,
350 const PdServ::TaskStatistics **stat) const
351 {
352 23442 return s->sessionTaskData->rxPdo(time, stat);
353 }
354
355 /////////////////////////////////////////////////////////////////////////////
356 1764 void Task::nrt_update()
357 {
358
2/2
✓ Branch 3 taken 1704 times.
✓ Branch 4 taken 60 times.
 1764 if (!persist)
359 1704 return;
360
361 60 const PdServ::TaskStatistics *stat;
362 60 const struct timespec *dummy_time;
363
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 1704 times.
 630 while (rxPdo(persist, &dummy_time, &stat)) {
364 // Required for persistent signal->parameter pair
365 285 m_time = *dummy_time;
366 }
367 }
368
369 /////////////////////////////////////////////////////////////////////////////
370 // Real time methods
371 /////////////////////////////////////////////////////////////////////////////
372 4192 void Task::rt_update(const struct timespec *t)
373 {
374
2/2
✓ Branch 8 taken 44 times.
✓ Branch 9 taken 4148 times.
 4192 if (*signalListRp != *signalListWp) {
375
2/2
✓ Branch 8 taken 47 times.
✓ Branch 9 taken 44 times.
 138 while (*signalListRp != *signalListWp)
376 47 processSignalList();
377
378 44 calculateCopyList();
379 }
380
381
1/2
✓ Branch 0 taken 4192 times.
✗ Branch 1 not taken.
 4192 if (t) {
382 4192 m_time = *t;
383 } else {
384 ✗ m_time = {0, 0};
385 }
386
387 4192 copyData(t);
388 4192 }
389
390 /////////////////////////////////////////////////////////////////////////////
391 47 void Task::processSignalList()
392 {
393 47 struct SignalList *sp = *signalListRp + 1;
394
395
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 47 times.
 47 if (sp == signalListEnd)
396 ✗ sp = signalList;
397
398 47 signalListId = sp->signalListId;
399 47 const Signal *signal = sp->signal;
400 47 size_t w = signal->dataTypeIndex[signal->dtype.align()];
401 struct CopyList *cl;
402
403
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) {
404 47 case SignalList::Insert:
405 // Insert the signal at list end
406 47 cl = copyList[w] + signalTypeCount[w]++;
407
408 47 cl->src = signal->addr;
409 47 cl->len = signal->memSize;
410 47 cl->signal = signal;
411
412 47 signalMemSize += signal->memSize;
413 47 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 47 __sync_synchronize(); // write memory barrier
427 #endif
428
429 47 *signalListRp = sp;
430 47 }
431
432 /////////////////////////////////////////////////////////////////////////////
433 44 void Task::calculateCopyList()
434 {
435 44 size_t n = std::accumulate(signalTypeCount, signalTypeCount + 4, 0);
436
437
1/2
✗ Branch 6 not taken.
✓ Branch 7 taken 44 times.
 44 if ((&txPdo->signalIdx + n) >= txMemEnd)
438 ✗ txPdo = txMemBegin;
439
440 44 txPdo->next = 0;
441 44 txPdo->type = Pdo::Empty;
442 44 txPdo->signalListId = signalListId;
443 44 txPdo->count = n;
444 44 size_t *sp = &txPdo->signalIdx;
445
2/2
✓ Branch 0 taken 176 times.
✓ Branch 1 taken 44 times.
 220 for (int i = 0; i < 4; i++) {
446
2/2
✓ Branch 5 taken 52 times.
✓ Branch 6 taken 176 times.
 228 for (CopyList *cl = copyList[i]; cl->src; ++cl)
447 52 *sp++ = cl->signal->index;
448 }
449 44 txPdo->type = Pdo::SignalList;
450
451 #ifdef __GNUC__
452 44 __sync_synchronize(); // write memory barrier
453 #endif
454
455 44 *nextTxPdo = txPdo;
456
457 44 nextTxPdo = &txPdo->next;
458 44 txPdo = ptr_align<Pdo>(sp);
459 44 }
460
461 /////////////////////////////////////////////////////////////////////////////
462 4192 void Task::copyData(const struct timespec *t)
463 {
464
2/2
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 4191 times.
 4192 if ( &txPdo->data + signalMemSize >= txMemEnd)
465 1 txPdo = txMemBegin;
466
467 4192 txPdo->next = 0;
468 4192 txPdo->type = Pdo::Empty;
469 4192 txPdo->signalListId = signalListId;
470 4192 txPdo->seqNo = seqNo++;
471
472 4192 txPdo->taskStatistics = taskStatistics;
473
474
1/2
✓ Branch 0 taken 4192 times.
✗ Branch 1 not taken.
 4192 if (t)
475 4192 txPdo->time = *t;
476 else
477 ✗ txPdo->time.tv_sec = txPdo->time.tv_nsec = 0;
478
479 4192 char *p = &txPdo->data;
480
2/2
✓ Branch 0 taken 16768 times.
✓ Branch 1 taken 4192 times.
 20960 for (int i = 0; i < 4; ++i) {
481
2/2
✓ Branch 5 taken 2919 times.
✓ Branch 6 taken 16768 times.
 19687 for (CopyList *cl = copyList[i]; cl->src; ++cl) {
482 2919 std::copy(cl->src, cl->src + cl->len, p);
483 2919 p += cl->len;
484 }
485 }
486
487 4192 txPdo->type = Pdo::Data;
488 4192 txPdo->count = p - &txPdo->data;
489
490 #ifdef __GNUC__
491 4192 __sync_synchronize(); // write memory barrier
492 #endif
493
494 4192 *nextTxPdo = txPdo;
495
496 4192 nextTxPdo = &txPdo->next;
497 4192 txPdo = ptr_align<Pdo>(p);
498 4192 }
499