GCC Code Coverage Report

Directory: ./
File: pdserv-1.1.0/src/lib/Task.cpp
Date: 2024-12-01 04:08:21
Exec Total Coverage
Lines: 117 216 54.2%
Branches: 38 105 36.2%
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 "Task.h"
33 #include "Signal.h"
34 #include "Pointer.h"
35
36 /////////////////////////////////////////////////////////////////////////////
37 // Data structures used in Task
38 /////////////////////////////////////////////////////////////////////////////
39 struct CopyList {
40 const Signal *signal;
41 const char *src;
42 size_t len;
43 };
44
45 struct SignalList {
46 enum {Insert = 1, Remove} action;
47 unsigned int signalListId;
48 unsigned int signalPosition;
49 const Signal* signal;
50 };
51
52 struct PollData {
53 unsigned int request, reply;
54 bool active;
55 struct timespec time;
56 unsigned int count;
57 unsigned int length;
58 const char *addr;
59 struct Data {
60 void *dest;
61 const Signal *signal;
62 } data[];
63 };
64
65 /////////////////////////////////////////////////////////////////////////////
66 /////////////////////////////////////////////////////////////////////////////
67 20 Task::Task(Main *main, double ts, const char * /*name*/):
68
1/2
✓ Branch 8 taken 20 times.
✗ Branch 9 not taken.
 20 PdServ::Task(ts), main(main), mutex(1)
69 {
70 20 seqNo = 0;
71 20 signalMemSize = 0;
72 20 signalPosition = 0;
73 20 signalListId = 0;
74
1/2
✓ Branch 3 taken 20 times.
✗ Branch 4 not taken.
 20 std::fill_n(signalTypeCount, 4, 0);
75 20 signalCopyList[0] = 0;
76 20 copyList[0] = 0;
77 20 }
78
79 /////////////////////////////////////////////////////////////////////////////
80 ✗ Task::~Task()
81 {
82 ✗ delete copyList[0];
83 ✗ delete signalCopyList[0];
84 ✗ delete signalPosition;
85
86 ✗ for (Signals::iterator it = signals.begin(); it != signals.end(); ++it)
87 ✗ delete *it;
88 }
89
90 /////////////////////////////////////////////////////////////////////////////
91 20 size_t Task::getShmemSpace(double T) const
92 {
93 20 size_t n = signals.size();
94 20 size_t minPdoCount = (size_t)(T / sampleTime + 0.5);
95
96
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 20 times.
 20 if (minPdoCount < 10)
97 ✗ minPdoCount = 10;
98
99 return sizeof(*signalListRp) + sizeof(*signalListWp)
100 + sizeof(*poll) + n*sizeof(*poll->data)
101 + 2 * n * sizeof(*signalList)
102 20 + (sizeof(*txPdo) + signalMemSize) * minPdoCount;
103 }
104
105 /////////////////////////////////////////////////////////////////////////////
106 ✗ SessionTaskData* Task::newSession(PdServ::Session * /*session*/)
107 {
108 ✗ return 0; //new SessionTaskData(session, this, txMemBegin, txMemEnd);
109 }
110
111 /////////////////////////////////////////////////////////////////////////////
112 20 void Task::prepare(void *shmem, void *shmem_end)
113 {
114 log_debug("S(%p): shmem=%p shmem_end=%p", this, shmem, shmem_end);
115 20 size_t n = signals.size();
116
117 20 signalListRp = ptr_align<struct SignalList*>(shmem);
118 20 signalListWp = signalListRp + 1;
119 20 signalList = ptr_align<struct SignalList>(signalListWp + 1);
120 20 signalListEnd = signalList + (2*n);
121 20 *signalListRp = signalList;
122 20 *signalListWp = signalList;
123
124 20 poll = ptr_align<struct PollData>(signalListEnd);
125
126 20 txMemBegin = ptr_align<struct Pdo>(poll->data + n);
127 20 txMemEnd = shmem_end;
128 log_debug("S(%p): txMemBegin=%p", this, txMemBegin);
129 // cerr_debug() << "signallen=" << signalMemSize << " txpdosize="
130 // << sizeof(*txPdo) << " space="
131 // << ((const char*)txMemEnd - (const char *)txMemBegin);
132
133 20 txPdo = txMemBegin;
134 20 nextTxPdo = ptr_align<struct Pdo*>(shmem_end) - 2;
135 20 }
136
137 /////////////////////////////////////////////////////////////////////////////
138 20 void Task::rt_init()
139 {
140 20 signalMemSize = 0;
141
142
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 copyList[0] = new struct CopyList[signals.size() + 4];
143
2/2
✓ Branch 0 taken 60 times.
✓ Branch 1 taken 20 times.
 80 for (size_t i = 0; i < 3; i++)
144 60 copyList[i+1] = copyList[i] + signalTypeCount[i] + 1;
145
146
1/2
✓ Branch 3 taken 20 times.
✗ Branch 4 not taken.
 20 std::fill_n(signalTypeCount, 4, 0);
147
148 // Clear src field which is end-of-list marker
149
2/2
✓ Branch 2 taken 140 times.
✓ Branch 3 taken 20 times.
 160 for (size_t i = 0; i < signals.size() + 4; ++i)
150 140 copyList[0][i].src = 0;
151 20 }
152
153 /////////////////////////////////////////////////////////////////////////////
154 ✗ void Task::nrt_init()
155 {
156 ✗ signalVector.resize(signals.size());
157 ✗ for (Signals::const_iterator it = signals.begin();
158 ✗ it != signals.end(); ++it) {
159 ✗ const Signal *s = static_cast<const Signal*>(*it);
160 ✗ signalVector[s->index] = s;
161 }
162
163 ✗ signalPosition = new unsigned int[signals.size()];
164
165 ✗ signalCopyList[0] = new const Signal*[signals.size()];
166 ✗ for (size_t i = 0; i < 3; i++)
167 ✗ signalCopyList[i+1] = signalCopyList[i] + signalTypeCount[i];
168
169 ✗ std::fill_n(signalTypeCount, 4, 0);
170 }
171
172 /////////////////////////////////////////////////////////////////////////////
173 60 Signal* Task::addSignal( unsigned int decimation,
174 const char *path, const PdServ::DataType& datatype,
175 const void *addr, size_t n, const size_t *dim)
176 {
177 60 Signal *s = new Signal(this, signals.size(),
178
1/2
✓ Branch 3 taken 60 times.
✗ Branch 4 not taken.
 60 decimation, path, datatype, addr, n, dim);
179
180
1/2
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
 60 signals.push_back(s);
181 60 signalTypeCount[s->dataTypeIndex[s->dtype.align()]]++;
182 60 signalMemSize += s->memSize;
183
184 60 return s;
185 }
186
187 /////////////////////////////////////////////////////////////////////////////
188 ✗ size_t Task::signalCount() const
189 {
190 ✗ return signals.size();
191 }
192
193 /////////////////////////////////////////////////////////////////////////////
194 ✗ void Task::getSignalList(const Signal **signalList, size_t *nelem,
195 unsigned int *signalListId) const
196 {
197 ✗ ost::SemaphoreLock lock(mutex);
198
199 ✗ for (unsigned int i = 0; i < 4; ++i)
200 ✗ for (unsigned int j = 0; j < signalTypeCount[i]; ++j)
201 ✗ *signalList++ = signalCopyList[i][j];
202 ✗ *nelem = std::accumulate(signalTypeCount, signalTypeCount + 4, 0);
203 ✗ *signalListId = (*signalListWp)->signalListId;
204 }
205
206 /////////////////////////////////////////////////////////////////////////////
207 ✗ void Task::subscribe(const Signal* s, bool insert) const
208 {
209 ✗ ost::SemaphoreLock lock(mutex);
210 ✗ struct SignalList *wp = *signalListWp;
211
212 ✗ if (++wp == signalListEnd)
213 ✗ wp = signalList;
214
215 ✗ while (wp == *signalListRp)
216 ✗ ost::Thread::sleep(static_cast<unsigned>(sampleTime * 1000 / 2 + 1));
217
218 //log_debug("%i %s", insert, s->path.c_str());
219
220 ✗ size_t w = s->dataTypeIndex[s->dtype.align()];
221 ✗ const Signal **scl = signalCopyList[w];
222
223 ✗ wp->signal = s;
224
225 ✗ if (insert) {
226
227 ✗ wp->action = SignalList::Insert;
228
229 ✗ size_t i = signalTypeCount[w]++;
230 ✗ scl[i] = s;
231 ✗ signalPosition[s->index] = i;
232
233 // log_debug("insert %s @ %zu[%zu]", s->path.c_str(), w, i);
234 // debug() << "insert" << s->path << w << i << (void*)signalCopyList[w];
235 }
236 else {
237 ✗ size_t pos = signalPosition[s->index];
238
239 // log_debug("erase %s @ %zu", s->path.c_str(), pos);
240
241 ✗ wp->action = SignalList::Remove;
242 ✗ wp->signalPosition = pos;
243
244 // Replace s with last signal on the list
245 ✗ s = scl[--signalTypeCount[w]];
246 ✗ scl[pos] = s;
247 ✗ signalPosition[s->index] = pos;
248
249 // debug() << "erase" << s->path << w << signalPosition[s->index]
250 // << "copy("
251 // << (void*)(signalCopyList[w])
252 // << signalPosition[s->index] + 1
253 // << signalTypeCount[w]
254 // << signalPosition[s->index];
255 }
256
257 ✗ wp->signalListId = ++signalListId;
258 // cout << __func__ << s->index << ' ' << insert
259 // << " pos=" << wp->signalPosition << endl;
260
261 ✗ *signalListWp = wp;
262 }
263
264 /////////////////////////////////////////////////////////////////////////////
265 ✗ void Task::pollPrepare( const Signal *signal, void *dest) const
266 {
267 ✗ poll->data[poll->count].dest = dest;
268 ✗ poll->data[poll->count].signal = signal;
269 ✗ poll->length += signal->memSize;
270 ✗ poll->count++;
271 }
272
273 /////////////////////////////////////////////////////////////////////////////
274 ✗ bool Task::pollFinished( const PdServ::Signal * const *, size_t /*nelem*/,
275 void * const *, struct timespec *t) const
276 {
277 ✗ if (!poll->count)
278 ✗ return true;
279
280 ✗ if (!poll->active) {
281 ✗ poll->active = true;
282 ✗ poll->request++;
283 ✗ return false;
284 }
285
286 ✗ if (poll->request != poll->reply)
287 ✗ return false;
288
289 ✗ const char *buf = poll->addr;
290 ✗ for (size_t i = 0; i < poll->count; ++i) {
291 ✗ const Signal *s = poll->data[i].signal;
292 ✗ std::copy(buf, buf + s->memSize,
293 ✗ reinterpret_cast<char*>(poll->data[i].dest));
294 ✗ buf += s->memSize;
295 }
296
297 ✗ poll->active = false;
298 ✗ poll->count = 0;
299 ✗ poll->length = 0;
300 ✗ if (t)
301 ✗ *t = poll->time;
302
303 ✗ return true;
304 }
305
306 /////////////////////////////////////////////////////////////////////////////
307 ✗ void Task::updateStatistics(
308 double exec_time, double cycle_time, unsigned int overrun)
309 {
310 ✗ taskStatistics.exec_time = exec_time;
311 ✗ taskStatistics.cycle_time = cycle_time;
312 ✗ taskStatistics.overrun = overrun;
313 }
314
315 /////////////////////////////////////////////////////////////////////////////
316 1309 void Task::update(const struct timespec *t)
317 {
318
2/2
✓ Branch 8 taken 4 times.
✓ Branch 9 taken 1305 times.
 1309 if (poll->request != poll->reply) {
319
1/2
✗ Branch 10 not taken.
✓ Branch 11 taken 4 times.
 4 if (&txPdo->data + poll->length >= txMemEnd) {
320 ✗ txPdo = txMemBegin;
321 ✗ txPdo->type = Pdo::Empty;
322 ✗ txPdo->next = 0;
323 }
324
325 4 char *dst = &txPdo->data;
326
327
1/2
✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
 4 if (t)
328 4 poll->time = *t;
329 else
330 ✗ poll->time.tv_sec = poll->time.tv_nsec = 0;
331
332 4 poll->addr = dst;
333
334
2/2
✓ Branch 4 taken 4 times.
✓ Branch 5 taken 4 times.
 8 for (size_t i = 0; i < poll->count; ++i) {
335 4 const Signal *s = poll->data[i].signal;
336 4 std::copy(s->addr, s->addr + s->memSize, dst);
337 4 dst += s->memSize;
338 }
339
340 4 txPdo = ptr_align<Pdo>(dst);
341
342
1/2
✗ Branch 7 not taken.
✓ Branch 8 taken 4 times.
 4 if (poll->addr == &txMemBegin->data) {
343 ✗ txPdo->next = 0;
344 ✗ txPdo->type = Pdo::Empty;
345 ✗ txMemBegin->next = txPdo;
346 }
347
348 4 poll->reply = poll->request;
349 //log_debug("S(%p): poll", this);
350 }
351
352 1309 struct SignalList *sp = 0;
353
354
2/2
✓ Branch 8 taken 4 times.
✓ Branch 9 taken 1309 times.
 1317 while (*signalListRp != *signalListWp) {
355 4 sp = *signalListRp + 1;
356
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 4 times.
 4 if (sp == signalListEnd)
357 ✗ sp = signalList;
358
359 4 signalListId = sp->signalListId;
360 4 const Signal *signal = sp->signal;
361 4 size_t w = signal->dataTypeIndex[signal->dtype.align()];
362 struct CopyList *cl;
363 // cout << "Signal " << signal->index << " pos=" << sp->signalPosition;
364
365
3/5
✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4 switch (sp->action) {
366 3 case SignalList::Insert:
367 // Insert the signal at list end
368 // log_debug("RT insert %s @ %zu", signal->path.c_str(),
369 // signalTypeCount[w]);
370
371 3 cl = copyList[w] + signalTypeCount[w]++;
372
373 3 cl->src = signal->addr;
374 3 cl->len = signal->memSize;
375 3 cl->signal = signal;
376
377 3 signalMemSize += signal->memSize;
378
379
380 // cout << " added" << endl;
381 3 break;
382
383 1 case SignalList::Remove:
384 // Move signal at list end to the deleted position
385 1 cl = copyList[w] + --signalTypeCount[w];
386 1 copyList[w][sp->signalPosition] = *cl;
387 1 cl->src = 0; // End of copy list indicator
388
389 1 signalMemSize -= signal->memSize;
390
391 // log_debug("RT remove %s @ %zu", signal->path.c_str(),
392 // signalTypeCount[w]);
393
394 // cout << " removed" << endl;
395 1 break;
396 }
397
398 4 *signalListRp = sp;
399 }
400 // cout << sp << endl;
401
402
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1305 times.
 1309 if (sp) {
403 4 size_t n = std::accumulate(signalTypeCount, signalTypeCount + 4, 0);
404 // cout << "New signals " << n << ' ' << "signalMemSize=" << signalMemSize;
405
406
1/2
✗ Branch 6 not taken.
✓ Branch 7 taken 4 times.
 4 if ((&txPdo->signalIdx + n) >= txMemEnd)
407 ✗ txPdo = txMemBegin;
408
409 4 txPdo->next = 0;
410 4 txPdo->type = Pdo::Empty;
411 4 txPdo->signalListId = signalListId;
412 4 txPdo->count = n;
413 4 size_t *sp = &txPdo->signalIdx;
414
2/2
✓ Branch 0 taken 16 times.
✓ Branch 1 taken 4 times.
 20 for (int i = 0; i < 4; i++) {
415
2/2
✓ Branch 5 taken 5 times.
✓ Branch 6 taken 16 times.
 21 for (CopyList *cl = copyList[i]; cl->src; ++cl)
416 5 *sp++ = cl->signal->index;
417 }
418 4 txPdo->type = Pdo::SignalList;
419 // cout << endl;
420
421 4 *nextTxPdo = txPdo;
422
423 //log_debug("S(%p): TxPdo=%p (signalList)<- %p", this, txPdo, nextTxPdo);
424
425 4 nextTxPdo = &txPdo->next;
426 4 txPdo = ptr_align<Pdo>(sp);
427 }
428
429
2/2
✓ Branch 9 taken 5 times.
✓ Branch 10 taken 1304 times.
 1309 if ( &txPdo->data + signalMemSize >= txMemEnd) {
430 5 txPdo = txMemBegin;
431 // cout << "wrap" << endl;
432 }
433
434 1309 txPdo->next = 0;
435 1309 txPdo->type = Pdo::Empty;
436 1309 txPdo->signalListId = signalListId;
437 1309 txPdo->seqNo = seqNo++;
438
439 1309 txPdo->taskStatistics = taskStatistics;
440
441
1/2
✓ Branch 0 taken 1309 times.
✗ Branch 1 not taken.
 1309 if (t) {
442 1309 txPdo->time = *t;
443 }
444 else
445 ✗ txPdo->time.tv_sec = txPdo->time.tv_nsec = 0;
446
447 1309 char *p = &txPdo->data;
448
2/2
✓ Branch 0 taken 5236 times.
✓ Branch 1 taken 1309 times.
 6545 for (int i = 0; i < 4; ++i) {
449 // cout << i << ": ";
450
2/2
✓ Branch 5 taken 1328 times.
✓ Branch 6 taken 5236 times.
 6564 for (CopyList *cl = copyList[i]; cl->src; ++cl) {
451 1328 std::copy(cl->src, cl->src + cl->len, p);
452 1328 p += cl->len;
453 // cout << cl->signal->index << ' ' << cl->len << ' ';
454 }
455 }
456 1309 txPdo->type = Pdo::Data;
457 // cout << '=' << p - txPdo->data << endl;
458
459 1309 txPdo->count = p - &txPdo->data;
460 1309 *nextTxPdo = txPdo;
461
462 //log_debug("S(%p): TxPdo=%p<-%p", this, txPdo, nextTxPdo);
463 1309 nextTxPdo = &txPdo->next;
464 1309 txPdo = ptr_align<Pdo>(p);
465 1309 }
466
467 /////////////////////////////////////////////////////////////////////////////
468 ✗ void Task::prepare (PdServ::SessionTask *s) const
469 {
470 ✗ s->sessionTaskData =
471 ✗ new SessionTaskData(s, this, signalVector, txMemBegin, txMemEnd);
472 }
473
474 /////////////////////////////////////////////////////////////////////////////
475 ✗ void Task::cleanup (const PdServ::SessionTask *s) const
476 {
477 ✗ delete s->sessionTaskData;
478 }
479
480 /////////////////////////////////////////////////////////////////////////////
481 ✗ bool Task::rxPdo (PdServ::SessionTask *s, const struct timespec **time,
482 const PdServ::TaskStatistics **stat) const
483 {
484 ✗ return s->sessionTaskData->rxPdo(time, stat);
485 3 }
486