| Line |
Branch |
Exec |
Source |
| 1 |
|
|
/***************************************************************************** |
| 2 |
|
|
* |
| 3 |
|
|
* $Id$ |
| 4 |
|
|
* |
| 5 |
|
|
* Copyright 2010 - 2012 Richard Hacker (lerichi at gmx dot net) |
| 6 |
|
|
* Florian Pose <fp@igh-essen.com> |
| 7 |
|
|
* |
| 8 |
|
|
* This file is part of the pdserv library. |
| 9 |
|
|
* |
| 10 |
|
|
* The pdserv library is free software: you can redistribute it and/or modify |
| 11 |
|
|
* it under the terms of the GNU Lesser General Public License as published |
| 12 |
|
|
* by the Free Software Foundation, either version 3 of the License, or (at |
| 13 |
|
|
* your option) any later version. |
| 14 |
|
|
* |
| 15 |
|
|
* The pdserv library is distributed in the hope that it will be useful, but |
| 16 |
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY |
| 17 |
|
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public |
| 18 |
|
|
* License for more details. |
| 19 |
|
|
* |
| 20 |
|
|
* You should have received a copy of the GNU Lesser General Public License |
| 21 |
|
|
* along with the pdserv library. If not, see <http://www.gnu.org/licenses/>. |
| 22 |
|
|
* |
| 23 |
|
|
*****************************************************************************/ |
| 24 |
|
|
|
| 25 |
|
|
#include "../Debug.h" |
| 26 |
|
|
#include "../SessionTask.h" |
| 27 |
|
|
#include "SessionTaskData.h" |
| 28 |
|
|
#include "ShmemDataStructures.h" |
| 29 |
|
|
#include "Signal.h" |
| 30 |
|
|
|
| 31 |
|
|
//////////////////////////////////////////////////////////////////////////// |
| 32 |
|
✗ |
SessionTaskData::SessionTaskData (PdServ::SessionTask *st, const Task* t, |
| 33 |
|
|
const Task::SignalVector& signals, |
| 34 |
|
✗ |
struct Pdo *txMemBegin, const void *txMemEnd): |
| 35 |
|
|
task(t), sessionTask(st), signals(signals), |
| 36 |
|
✗ |
txMemBegin(txMemBegin), txMemEnd(txMemEnd) |
| 37 |
|
|
{ |
| 38 |
|
✗ |
signalListId = 0; |
| 39 |
|
✗ |
pdoSize = 0; |
| 40 |
|
|
|
| 41 |
|
✗ |
signalPosition.resize(task->signalCount()); |
| 42 |
|
|
|
| 43 |
|
✗ |
init(); |
| 44 |
|
|
} |
| 45 |
|
|
|
| 46 |
|
|
//////////////////////////////////////////////////////////////////////////// |
| 47 |
|
✗ |
SessionTaskData::~SessionTaskData () |
| 48 |
|
|
{ |
| 49 |
|
✗ |
for (SignalSet::const_iterator it = pdoSignals.begin(); |
| 50 |
|
✗ |
it != pdoSignals.end(); it++) { |
| 51 |
|
|
//log_debug("Auto unsubscribe from %s", (*it)->path.c_str()); |
| 52 |
|
✗ |
static_cast<const PdServ::Signal*>(*it) ->unsubscribe(sessionTask); |
| 53 |
|
|
} |
| 54 |
|
|
} |
| 55 |
|
|
|
| 56 |
|
|
//////////////////////////////////////////////////////////////////////////// |
| 57 |
|
|
// When this function exits, pdo |
| 58 |
|
|
// * points to the end of the pdo list, |
| 59 |
|
|
// * is a Data Pdo |
| 60 |
|
|
// and its signalListId is valid |
| 61 |
|
✗ |
void SessionTaskData::init() |
| 62 |
|
|
{ |
| 63 |
|
✗ |
const Signal *signals[signalPosition.size()]; |
| 64 |
|
✗ |
size_t nelem; |
| 65 |
|
|
|
| 66 |
|
✗ |
pdo = txMemBegin; |
| 67 |
|
|
|
| 68 |
|
✗ |
do { |
| 69 |
|
✗ |
do { |
| 70 |
|
✗ |
while (!pdo->next) { |
| 71 |
|
✗ |
ost::Thread::sleep( static_cast<unsigned>( |
| 72 |
|
✗ |
task->sampleTime * 1000 / 2 + 1)); |
| 73 |
|
|
} |
| 74 |
|
|
|
| 75 |
|
✗ |
pdo = pdo->next; |
| 76 |
|
|
|
| 77 |
|
|
// Check that the pdo is valid and everyting is accessible |
| 78 |
|
✗ |
if (pdo < txMemBegin |
| 79 |
|
✗ |
or &pdo->data >= txMemEnd |
| 80 |
|
✗ |
or (pdo->type != Pdo::Data |
| 81 |
|
✗ |
and pdo->type != Pdo::SignalList) |
| 82 |
|
✗ |
or (pdo->type == Pdo::Data |
| 83 |
|
✗ |
and &pdo->data + pdo->count >= txMemEnd) |
| 84 |
|
✗ |
or (pdo->type == Pdo::SignalList |
| 85 |
|
✗ |
and &pdo->signalIdx + pdo->count >= txMemEnd)) { |
| 86 |
|
|
// Pdo is invalid. Start over |
| 87 |
|
✗ |
pdo = txMemBegin; |
| 88 |
|
|
} |
| 89 |
|
✗ |
} while (pdo->next or pdo->type != Pdo::Data); |
| 90 |
|
|
|
| 91 |
|
|
// At this point, we have a valid Data Pdo at the end of the list |
| 92 |
|
✗ |
seqNo = pdo->seqNo; |
| 93 |
|
|
|
| 94 |
|
✗ |
task->getSignalList(signals, &nelem, &signalListId); |
| 95 |
|
✗ |
loadSignalList(signals, nelem, signalListId); |
| 96 |
|
|
|
| 97 |
|
|
log_debug("Loaded signal list with ID %u", signalListId); |
| 98 |
|
|
|
| 99 |
|
✗ |
} while (signalListId != pdo->signalListId); |
| 100 |
|
|
|
| 101 |
|
|
log_debug("Session %p sync'ed: pdo=%p seqNo=%u signalListId=%u", |
| 102 |
|
✗ |
this, (void *) pdo, seqNo, signalListId); |
| 103 |
|
|
} |
| 104 |
|
|
|
| 105 |
|
|
//////////////////////////////////////////////////////////////////////////// |
| 106 |
|
✗ |
bool SessionTaskData::isBusy (const Signal *s) |
| 107 |
|
|
{ |
| 108 |
|
✗ |
return pdoSignals.find(s) != pdoSignals.end(); |
| 109 |
|
|
} |
| 110 |
|
|
|
| 111 |
|
|
//////////////////////////////////////////////////////////////////////////// |
| 112 |
|
✗ |
bool SessionTaskData::rxPdo (const struct timespec **time, |
| 113 |
|
|
const PdServ::TaskStatistics **statistics) |
| 114 |
|
|
{ |
| 115 |
|
✗ |
while (pdo->next) { |
| 116 |
|
|
size_t n; |
| 117 |
|
|
|
| 118 |
|
✗ |
pdo = pdo->next; |
| 119 |
|
✗ |
if (pdo < txMemBegin or &pdo->data > txMemEnd) { |
| 120 |
|
|
goto out; |
| 121 |
|
|
} |
| 122 |
|
|
|
| 123 |
|
✗ |
n = pdo->count; |
| 124 |
|
|
|
| 125 |
|
✗ |
switch (pdo->type) { |
| 126 |
|
✗ |
case Pdo::SignalList: |
| 127 |
|
|
{ |
| 128 |
|
✗ |
const Signal *sp[signals.size()]; |
| 129 |
|
|
|
| 130 |
|
✗ |
if (&pdo->signalIdx + n > txMemEnd) { |
| 131 |
|
✗ |
goto out; |
| 132 |
|
|
} |
| 133 |
|
|
|
| 134 |
|
✗ |
for (size_t i = 0; i < n; ++i) { |
| 135 |
|
✗ |
size_t idx = (&pdo->signalIdx)[i]; |
| 136 |
|
✗ |
if (idx >= signals.size()) { |
| 137 |
|
✗ |
goto out; |
| 138 |
|
|
} |
| 139 |
|
✗ |
sp[i] = static_cast<const Signal*>(signals[idx]); |
| 140 |
|
✗ |
sessionTask->newSignal(sp[i]); |
| 141 |
|
|
} |
| 142 |
|
✗ |
loadSignalList(sp, n, pdo->signalListId); |
| 143 |
|
|
} |
| 144 |
|
|
|
| 145 |
|
✗ |
break; |
| 146 |
|
|
|
| 147 |
|
✗ |
case Pdo::Data: |
| 148 |
|
✗ |
if (&pdo->data + pdoSize >= txMemEnd |
| 149 |
|
✗ |
or pdo->signalListId != signalListId |
| 150 |
|
✗ |
or pdo->seqNo - seqNo != 1) { |
| 151 |
|
|
log_debug("%p + %zu >= %p; %u != %u; %i != 1; %u %u %u", |
| 152 |
|
|
(void *) pdo->data, pdoSize, (void *) txMemEnd, |
| 153 |
|
|
pdo->signalListId, signalListId, |
| 154 |
|
|
pdo->seqNo - seqNo, |
| 155 |
|
|
pdo->data + pdoSize >= txMemEnd, |
| 156 |
|
|
pdo->signalListId != signalListId, |
| 157 |
|
|
pdo->seqNo - seqNo != 1 |
| 158 |
|
|
); |
| 159 |
|
|
goto out; |
| 160 |
|
|
} |
| 161 |
|
|
|
| 162 |
|
✗ |
seqNo = pdo->seqNo; |
| 163 |
|
✗ |
signalBuffer = &pdo->data; |
| 164 |
|
✗ |
*time = &pdo->time; |
| 165 |
|
✗ |
*statistics = &pdo->taskStatistics; |
| 166 |
|
|
|
| 167 |
|
✗ |
return true; |
| 168 |
|
|
|
| 169 |
|
✗ |
default: |
| 170 |
|
✗ |
goto out; |
| 171 |
|
|
} |
| 172 |
|
|
} |
| 173 |
|
|
|
| 174 |
|
✗ |
return false; |
| 175 |
|
|
|
| 176 |
|
✗ |
out: |
| 177 |
|
|
log_debug("Session %p out of sync.", this); |
| 178 |
|
✗ |
init(); |
| 179 |
|
✗ |
return true; |
| 180 |
|
|
} |
| 181 |
|
|
|
| 182 |
|
|
//////////////////////////////////////////////////////////////////////////// |
| 183 |
|
✗ |
void SessionTaskData::loadSignalList(const Signal * const* sp, size_t n, |
| 184 |
|
|
unsigned int id) |
| 185 |
|
|
{ |
| 186 |
|
|
log_debug("Loading %zu signals with id %u", n, id); |
| 187 |
|
|
// cout << __func__ << " n=" << n << " id=" << id; |
| 188 |
|
✗ |
std::fill(signalPosition.begin(), signalPosition.end(), ~0U); |
| 189 |
|
✗ |
pdoSignals.clear(); |
| 190 |
|
|
|
| 191 |
|
✗ |
signalListId = id; |
| 192 |
|
✗ |
pdoSize = 0; |
| 193 |
|
✗ |
for (size_t i = 0; i < n; ++i) { |
| 194 |
|
✗ |
signalPosition[sp[i]->index] = pdoSize; |
| 195 |
|
✗ |
pdoSize += sp[i]->memSize; |
| 196 |
|
✗ |
pdoSignals.insert(sp[i]); |
| 197 |
|
|
// cout << ' ' << sp[i]->index << '(' << pdoSize << ')'; |
| 198 |
|
|
} |
| 199 |
|
|
log_debug("pdosize=%zu", pdoSize); |
| 200 |
|
|
// cout << endl; |
| 201 |
|
|
} |
| 202 |
|
|
|
| 203 |
|
|
//////////////////////////////////////////////////////////////////////////// |
| 204 |
|
✗ |
const char *SessionTaskData::getValue(const PdServ::Signal *s) const |
| 205 |
|
|
{ |
| 206 |
|
✗ |
return signalBuffer + signalPosition[static_cast<const Signal*>(s)->index]; |
| 207 |
|
|
} |
| 208 |
|
|
|
| 209 |
|
|
//////////////////////////////////////////////////////////////////////////// |
| 210 |
|
✗ |
const struct timespec *SessionTaskData::getTaskTime() const |
| 211 |
|
|
{ |
| 212 |
|
✗ |
return &pdo->time; |
| 213 |
|
|
} |
| 214 |
|
|
|
| 215 |
|
|
//////////////////////////////////////////////////////////////////////////// |
| 216 |
|
✗ |
const PdServ::TaskStatistics* SessionTaskData::getTaskStatistics() const |
| 217 |
|
|
{ |
| 218 |
|
✗ |
return &pdo->taskStatistics; |
| 219 |
|
3 |
} |
| 220 |
|
|
|