rogue
Loading...
Searching...
No Matches
ControllerV2.cpp
Go to the documentation of this file.
1
18
19#include <inttypes.h>
20#include <sys/time.h>
21#include <unistd.h>
22
23#include <cmath>
24#include <cstdlib>
25#include <memory>
26
27#include "rogue/GeneralError.h"
28#include "rogue/GilRelease.h"
34
37
38// Local CRC library
40
41// CRC Lookup table for later use
43
45rpp::ControllerV2Ptr rpp::ControllerV2::create(bool enIbCrc, bool enObCrc, bool enSsi, rpp::TransportPtr tran) {
46 rpp::ControllerV2Ptr r = std::make_shared<rpp::ControllerV2>(enIbCrc, enObCrc, enSsi, tran);
47 return (r);
48}
49
51rpp::ControllerV2::ControllerV2(bool enIbCrc, bool enObCrc, bool enSsi, rpp::TransportPtr tran)
52 : rpp::Controller::Controller(tran, 8, 8, 8, enSsi) {
53 enIbCrc_ = enIbCrc;
54 enObCrc_ = enObCrc;
55}
56
58rpp::ControllerV2::~ControllerV2() {}
59
61void rpp::ControllerV2::transportRx(ris::FramePtr frame) {
62 ris::BufferPtr buff;
63 uint32_t size;
64 uint32_t tmpCount;
65 uint8_t tmpFuser;
66 uint8_t tmpLuser;
67 uint8_t tmpDest;
68 uint8_t tmpId;
69 bool tmpEof;
70 bool tmpSof;
71 bool crcErr;
72 uint32_t flags;
73 uint32_t last;
74 uint32_t crc;
75 uint32_t tmpCrc;
76 uint8_t* data;
77
78 if (frame->isEmpty()) {
79 log_->warning("Empty frame received on transport input");
80 return;
81 }
82
84 ris::FrameLockPtr flock = frame->lock();
85 std::lock_guard<std::mutex> lock(tranMtx_);
86
87 buff = *(frame->beginBuffer());
88 data = buff->begin();
89 size = buff->getPayload();
90
91 // Drop invalid data
92 if (frame->getError() || // Check for frame ERROR
93 (frame->bufferCount() != 1) || // Incoming frame can only have one buffer
94 (size < 24) || // Check for min. size (64-bit header + 64-bit min. payload + 64-bit tail)
95 ((size & 0x7) > 0) || // Check for non 64-bit alignment
96 ((data[0] & 0xF) != 0x2)) { // Check for invalid version only (ignore the CRC mode flag)
97 log_->warning("Dropping frame due to contents: error=0x%" PRIx8 ", payload=%" PRIu32 ", buffers=%" PRIu32
98 ", Version=0x%" PRIx8,
99 frame->getError(),
100 size,
101 frame->bufferCount(),
102 data[0] & 0xF);
103 dropCount_++;
104 return;
105 }
106
107 // Header word 0
108 tmpFuser = data[1];
109 tmpDest = data[2];
110 tmpId = data[3];
111
112 // Header word 1
113 tmpCount = static_cast<uint32_t>(data[4]) << 0;
114 tmpCount |= static_cast<uint32_t>(data[5]) << 8;
115 tmpSof = ((data[7] & 0x80) ? true : false); // SOF (PACKETIZER2_HDR_SOF_BIT_C = 63)
116
117 // Tail word 0
118 tmpLuser = data[size - 8];
119 tmpEof = ((data[size - 7] & 0x1) ? true : false);
120 last = static_cast<uint32_t>(data[size - 6]);
121
122 // Valid byte count in the final 64-bit word must be 1..8
123 if ((last == 0) || (last > 8)) {
124 log_->warning("Dropping frame due to invalid last value: %" PRIu32, last);
125 dropCount_++;
126 return;
127 }
128
129 if (enIbCrc_) {
130 // Tail word 1
131 tmpCrc = static_cast<uint32_t>(data[size - 1]) << 0;
132 tmpCrc |= static_cast<uint32_t>(data[size - 2]) << 8;
133 tmpCrc |= static_cast<uint32_t>(data[size - 3]) << 16;
134 tmpCrc |= static_cast<uint32_t>(data[size - 4]) << 24;
135
136 // Compute CRC
137 if (tmpSof)
138 crc_[tmpDest] = CRC::Calculate(data, size - 4, crcTable_);
139 else
140 crc_[tmpDest] = CRC::Calculate(data, size - 4, crcTable_, crc_[tmpDest]);
141
142 crcErr = (tmpCrc != crc_[tmpDest]);
143 } else {
144 crcErr = false;
145 }
146
147 log_->debug("transportRx: Raw header: 0x%" PRIx8 ", 0x%" PRIx8 ", 0x%" PRIx8 ", 0x%" PRIx8 ", 0x%" PRIx8
148 ", 0x%" PRIx8 ", 0x%" PRIx8 ", 0x%" PRIx8,
149 data[0],
150 data[1],
151 data[2],
152 data[3],
153 data[4],
154 data[5],
155 data[6],
156 data[7]);
157 log_->debug("transportRx: Raw footer: 0x%" PRIx8 ", 0x%" PRIx8 ", 0x%" PRIx8 ", 0x%" PRIx8 ", 0x%" PRIx8
158 ", 0x%" PRIx8 ", 0x%" PRIx8 ", 0x%" PRIx8,
159 data[size - 8],
160 data[size - 7],
161 data[size - 6],
162 data[size - 5],
163 data[size - 4],
164 data[size - 3],
165 data[size - 2],
166 data[size - 1]);
167 log_->debug("transportRx: Got frame: Fuser=0x%" PRIx8 ", Dest=0x%" PRIx8 ", Id=0x%" PRIx8 ", Count=%" PRIu32
168 ", Sof=%" PRIu8 ", Luser=0x%" PRIx8 ", Eof=%" PRIu8 ", Last=%" PRIu32 ", crcErr=%" PRIu8,
169 tmpFuser,
170 tmpDest,
171 tmpId,
172 tmpCount,
173 tmpSof,
174 tmpLuser,
175 tmpEof,
176 last,
177 crcErr);
178
179 // Shorten message by removing tail and adjusting for last value
180 // Do this before adjusting tail reservation
181 buff->adjustPayload(-8 + (static_cast<int32_t>(last) - 8));
182
183 // Add 8 bytes to headroom and tail reservation
184 buff->adjustHeader(8);
185 buff->adjustTail(8);
186
187 // Drop frame and reset state if mismatch
188 if ((transSof_[tmpDest] != tmpSof) || crcErr || tmpCount != tranCount_[tmpDest]) {
189 log_->warning("Dropping frame: gotDest=%" PRIu8 ", gotSof=%" PRIu8 ", crcErr=%" PRIu8 ", expCount=%" PRIu32
190 ", gotCount=%" PRIu32,
191 tmpDest,
192 tmpSof,
193 crcErr,
194 tranCount_[tmpDest],
195 tmpCount);
196 dropCount_++;
197 transSof_[tmpDest] = true;
198 tranCount_[tmpDest] = 0;
199 tranFrame_[tmpDest].reset();
200 return;
201 }
202
203 // First frame
204 if (transSof_[tmpDest]) {
205 transSof_[tmpDest] = false;
206 if ((tranCount_[tmpDest] != 0) || !tmpSof || crcErr) {
207 log_->warning("Dropping frame: gotDest=%" PRIu8 ", gotSof=%" PRIu8 ", crcErr=%" PRIu8 ", expCount=%" PRIu32
208 ", gotCount=%" PRIu32,
209 tmpDest,
210 tmpSof,
211 crcErr,
212 tranCount_[tmpDest],
213 tmpCount);
214 dropCount_++;
215 transSof_[tmpDest] = true;
216 tranCount_[tmpDest] = 0;
217 tranFrame_[tmpDest].reset();
218 return;
219 }
220
221 tranFrame_[tmpDest] = ris::Frame::create();
222 tranCount_[tmpDest] = 0;
223
224 tranFrame_[tmpDest]->setFirstUser(tmpFuser);
225 }
226
227 tranFrame_[tmpDest]->appendBuffer(buff);
228 frame->clear(); // Empty old frame
229
230 // Last of transfer
231 if (tmpEof) {
232 tranFrame_[tmpDest]->setLastUser(tmpLuser);
233 transSof_[tmpDest] = true;
234 tranCount_[tmpDest] = 0;
235
236 // Detect SSI error — must precede pushFrame/reset or it's a null deref.
237 if (enSsi_ & (tmpLuser & 0x1)) tranFrame_[tmpDest]->setError(0x80);
238
239 if (rpp::ApplicationPtr app = app_[tmpDest].lock()) app->pushFrame(tranFrame_[tmpDest]);
240 tranFrame_[tmpDest].reset();
241 } else {
242 tranCount_[tmpDest] = (tranCount_[tmpDest] + 1) & 0xFFFF;
243 }
244}
245
247void rpp::ControllerV2::applicationRx(ris::FramePtr frame, uint8_t tDest) {
248 ris::Frame::BufferIterator it;
249 uint32_t segment;
250 uint8_t* data;
251 uint32_t size;
252 uint8_t fUser;
253 uint8_t lUser;
254 uint32_t crc;
255 uint32_t last;
256 struct timeval startTime;
257 struct timeval currTime;
258 struct timeval endTime;
259
260 gettimeofday(&startTime, NULL);
261 timeradd(&startTime, &timeout_, &endTime);
262
263 if (frame->isEmpty()) {
264 log_->warning("Empty frame received on application input");
265 return;
266 }
267
268 if (frame->getError()) return;
269
270 rogue::GilRelease noGil;
271 ris::FrameLockPtr flock = frame->lock();
272 std::lock_guard<std::mutex> lock(appMtx_);
273
274 // Wait while queue is busy
275 while (tranQueue_.busy()) {
276 usleep(10);
277 gettimeofday(&currTime, NULL);
278 if (timercmp(&currTime, &endTime, >)) {
279 log_->critical("ControllerV2::applicationRx: Timeout waiting for outbound queue after %" PRIu32 ".%" PRIu32
280 " seconds! May be caused by outbound backpressure.",
281 timeout_.tv_sec,
282 timeout_.tv_usec);
283 gettimeofday(&startTime, NULL);
284 timeradd(&startTime, &timeout_, &endTime);
285 }
286 }
287
288 fUser = frame->getFirstUser();
289 lUser = frame->getLastUser();
290
291 // Inject SOF
292 if (enSsi_) fUser |= 0x2;
293
294 segment = 0;
295 for (it = frame->beginBuffer(); it != frame->endBuffer(); ++it) {
296 ris::FramePtr tFrame = ris::Frame::create();
297
298 // Compute last, and aligned payload to 64-bits
299 last = (*it)->getPayload() % 8;
300 if (last == 0) last = 8;
301 (*it)->adjustPayload(8 - last);
302
303 // Rem 8 bytes head and tail reservation before setting new size
304 (*it)->adjustHeader(-8);
305 (*it)->adjustTail(-8);
306
307 // Add tail to payload, set new size (header reduction added 8)
308 (*it)->adjustPayload(8);
309
310 // Get data pointer and new size
311 data = (*it)->begin();
312 size = (*it)->getPayload();
313
314 // Header word 0
315 data[0] = 0x2; // (Version=0x2)
316 if (enObCrc_) data[0] |= 0x20; // Enable CRC
317 data[1] = fUser;
318 data[2] = tDest;
319 data[3] = 0; // TID Unused
320
321 // Header word 1
322 data[4] = segment & 0xFF;
323 data[5] = (segment >> 8) & 0xFF;
324 data[6] = 0;
325 data[7] = (segment == 0) ? 0x80 : 0x0; // SOF (PACKETIZER2_HDR_SOF_BIT_C = 63)
326
327 // Tail word 0
328 data[size - 8] = lUser;
329 data[size - 7] = (it == (frame->endBuffer() - 1)) ? 0x1 : 0x0; // EOF
330 data[size - 6] = last;
331 data[size - 5] = 0;
332
333 if (enObCrc_) {
334 // Compute CRC
335 if (segment == 0)
336 crc = CRC::Calculate(data, size - 4, crcTable_);
337 else
338 crc = CRC::Calculate(data, size - 4, crcTable_, crc);
339
340 // Tail word 1
341 data[size - 1] = (crc >> 0) & 0xFF;
342 data[size - 2] = (crc >> 8) & 0xFF;
343 data[size - 3] = (crc >> 16) & 0xFF;
344 data[size - 4] = (crc >> 24) & 0xFF;
345
346 } else {
347 data[size - 1] = 0;
348 data[size - 2] = 0;
349 data[size - 3] = 0;
350 data[size - 4] = 0;
351 }
352
353 log_->debug("applicationRx: Gen frame: Size=%" PRIu32 ", Fuser=0x%" PRIu8 ", Dest=0x%" PRIu8 ", Count=%" PRIu32
354 ", Sof=%" PRIu8 ", Luser=0x%" PRIu8 ", Eof=%" PRIu8 ", Last=%" PRIu32,
355 (*it)->getPayload(),
356 fUser,
357 tDest,
358 segment,
359 data[7] >> 7,
360 lUser,
361 data[size - 7],
362 last);
363 log_->debug("applicationRx: Raw header: 0x%" PRIx8 ", 0x%" PRIx8 ", 0x%" PRIx8 ", 0x%" PRIx8 ", 0x%" PRIx8
364 ", 0x%" PRIx8 ", 0x%" PRIx8 ", 0x%" PRIx8,
365 data[0],
366 data[1],
367 data[2],
368 data[3],
369 data[4],
370 data[5],
371 data[6],
372 data[7]);
373 log_->debug("applicationRx: Raw footer: 0x%" PRIx8 ", 0x%" PRIx8 ", 0x%" PRIx8 ", 0x%" PRIx8 ", 0x%" PRIx8
374 ", 0x%" PRIx8 ", 0x%" PRIx8 ", 0x%" PRIx8,
375 data[size - 8],
376 data[size - 7],
377 data[size - 6],
378 data[size - 5],
379 data[size - 4],
380 data[size - 3],
381 data[size - 2],
382 data[size - 1]);
383
384 tFrame->appendBuffer(*it);
385 tranQueue_.push(tFrame);
386 segment++;
387 }
388 appIndex_++;
389 frame->clear(); // Empty old frame
390}
static const CRC::Table< uint32_t, 32 > crcTable_(CRC::CRC_32())
static CRCType Calculate(const void *data, crcpp_size size, const Parameters< CRCType, CRCWidth > &parameters)
Computes a CRC.
Definition CRC.h:433
static const Parameters< crcpp_uint32, 32 > & CRC_32()
Returns a set of parameters for CRC-32 (aka CRC-32 ADCCP, CRC-32 PKZip).
Definition CRC.h:1532
RAII helper that releases the Python GIL for a scope.
Definition GilRelease.h:36
Packetizer base controller.
Definition Controller.h:46
std::shared_ptr< rogue::interfaces::stream::Buffer > BufferPtr
Shared pointer alias for Buffer.
Definition Buffer.h:270
std::shared_ptr< rogue::interfaces::stream::Frame > FramePtr
Shared pointer alias for Frame.
Definition Frame.h:549
std::shared_ptr< rogue::interfaces::stream::FrameLock > FrameLockPtr
Shared pointer alias for FrameLock.
Definition FrameLock.h:110
std::shared_ptr< rogue::protocols::packetizer::Transport > TransportPtr
Definition Transport.h:120
std::shared_ptr< rogue::protocols::packetizer::ControllerV2 > ControllerV2Ptr
std::shared_ptr< rogue::protocols::packetizer::Application > ApplicationPtr
CRC lookup table. After construction, the CRC parameters are fixed.
Definition CRC.h:166