F´ Flight Software - C/C++ Documentation NASA-v1.6.0
A framework for building embedded system applications to NASA flight quality standards.
Loading...
Searching...
No Matches
TlmPacketizer.cpp
Go to the documentation of this file.
1// ======================================================================
2// \title TlmPacketizerImpl.cpp
3// \author tcanham
4// \brief cpp file for TlmPacketizer component implementation class
5//
6// \copyright
7// Copyright 2009-2015, by the California Institute of Technology.
8// ALL RIGHTS RESERVED. United States Government Sponsorship
9// acknowledged.
10
11#include <FpConfig.hpp>
12#include <Fw/Com/ComPacket.hpp>
14
15namespace Svc {
16
17// ----------------------------------------------------------------------
18// Construction, initialization, and destruction
19// ----------------------------------------------------------------------
20
21TlmPacketizer ::TlmPacketizer(const char* const compName)
22 : TlmPacketizerComponentBase(compName), m_numPackets(0), m_configured(false), m_startLevel(0), m_maxLevel(0) {
23 // clear slot pointers
24 for (NATIVE_UINT_TYPE entry = 0; entry < TLMPACKETIZER_NUM_TLM_HASH_SLOTS; entry++) {
25 this->m_tlmEntries.slots[entry] = nullptr;
26 }
27 // clear buckets
28 for (NATIVE_UINT_TYPE entry = 0; entry < TLMPACKETIZER_HASH_BUCKETS; entry++) {
29 this->m_tlmEntries.buckets[entry].used = false;
30 this->m_tlmEntries.buckets[entry].bucketNo = entry;
31 this->m_tlmEntries.buckets[entry].next = nullptr;
32 this->m_tlmEntries.buckets[entry].id = 0;
33 }
34 // clear free index
35 this->m_tlmEntries.free = 0;
36 // clear missing tlm channel check
37 for (NATIVE_UINT_TYPE entry = 0; entry < TLMPACKETIZER_MAX_MISSING_TLM_CHECK; entry++) {
38 this->m_missTlmCheck[entry].checked = false;
39 this->m_missTlmCheck[entry].id = 0;
40 }
41
42 // clear packet buffers
43 for (NATIVE_UINT_TYPE buffer = 0; buffer < MAX_PACKETIZER_PACKETS; buffer++) {
44 this->m_fillBuffers[buffer].updated = false;
45 this->m_fillBuffers[buffer].requested = false;
46 this->m_sendBuffers[buffer].updated = false;
47 }
48}
49
50void TlmPacketizer ::init(const NATIVE_INT_TYPE queueDepth, const NATIVE_INT_TYPE instance) {
51 TlmPacketizerComponentBase::init(queueDepth, instance);
52}
53
54TlmPacketizer ::~TlmPacketizer() {}
55
57 const Svc::TlmPacketizerPacket& ignoreList,
58 const NATIVE_UINT_TYPE startLevel) {
59 FW_ASSERT(packetList.list);
60 FW_ASSERT(ignoreList.list);
61 FW_ASSERT(packetList.numEntries <= MAX_PACKETIZER_PACKETS, packetList.numEntries);
62 // validate packet sizes against maximum com buffer size and populate hash
63 // table
64 for (NATIVE_UINT_TYPE pktEntry = 0; pktEntry < packetList.numEntries; pktEntry++) {
65 // Initial size is packetized telemetry descriptor + size of time tag + sizeof packet ID
66 NATIVE_UINT_TYPE packetLen =
68 FW_ASSERT(packetList.list[pktEntry]->list, pktEntry);
69 // add up entries for each defined packet
70 for (NATIVE_UINT_TYPE tlmEntry = 0; tlmEntry < packetList.list[pktEntry]->numEntries; tlmEntry++) {
71 // get hash value for id
72 FwChanIdType id = packetList.list[pktEntry]->list[tlmEntry].id;
73 TlmEntry* entryToUse = this->findBucket(id);
74 // copy into entry
75 FW_ASSERT(entryToUse);
76 entryToUse->used = true;
77 // not ignored channel
78 entryToUse->ignored = false;
79 entryToUse->id = id;
80 // the offset into the buffer will be the current packet length
81 entryToUse->packetOffset[pktEntry] = packetLen;
82
83 packetLen += packetList.list[pktEntry]->list[tlmEntry].size;
84
85 } // end channel in packet
86 FW_ASSERT(packetLen <= FW_COM_BUFFER_MAX_SIZE, packetLen, pktEntry);
87 // clear contents
88 memset(this->m_fillBuffers[pktEntry].buffer.getBuffAddr(), 0, packetLen);
89 // serialize packet descriptor and packet ID now since it will always be the same
90 Fw::SerializeStatus stat = this->m_fillBuffers[pktEntry].buffer.serialize(
92 FW_ASSERT(Fw::FW_SERIALIZE_OK == stat, stat);
93 stat = this->m_fillBuffers[pktEntry].buffer.serialize(packetList.list[pktEntry]->id);
94 FW_ASSERT(Fw::FW_SERIALIZE_OK == stat, stat);
95 // set packet buffer length
96 stat = this->m_fillBuffers[pktEntry].buffer.setBuffLen(packetLen);
97 FW_ASSERT(Fw::FW_SERIALIZE_OK == stat, stat);
98 // save ID
99 this->m_fillBuffers[pktEntry].id = packetList.list[pktEntry]->id;
100 // save level
101 this->m_fillBuffers[pktEntry].level = packetList.list[pktEntry]->level;
102 // store max level
103 if (packetList.list[pktEntry]->level > this->m_maxLevel) {
104 this->m_maxLevel = packetList.list[pktEntry]->level;
105 }
106 // save start level
107 this->m_startLevel = startLevel;
108
109 } // end packet list
110
111 // populate hash table with ignore list
112 for (NATIVE_UINT_TYPE channelEntry = 0; channelEntry < ignoreList.numEntries; channelEntry++) {
113 // get hash value for id
114 FwChanIdType id = ignoreList.list[channelEntry].id;
115
116 TlmEntry* entryToUse = this->findBucket(id);
117
118 // copy into entry
119 FW_ASSERT(entryToUse);
120 entryToUse->used = true;
121 // is ignored channel
122 entryToUse->ignored = true;
123 entryToUse->id = id;
124 } // end ignore list
125
126 // store number of packets
127 this->m_numPackets = packetList.numEntries;
128
129 // indicate configured
130 this->m_configured = true;
131}
132
133TlmPacketizer::TlmEntry* TlmPacketizer::findBucket(FwChanIdType id) {
134 NATIVE_UINT_TYPE index = this->doHash(id);
136 TlmEntry* entryToUse = nullptr;
137 TlmEntry* prevEntry = nullptr;
138
139 // Search to see if channel has already been stored or a bucket needs to be added
140 if (this->m_tlmEntries.slots[index]) {
141 entryToUse = this->m_tlmEntries.slots[index];
142 for (NATIVE_UINT_TYPE bucket = 0; bucket < TLMPACKETIZER_HASH_BUCKETS; bucket++) {
143 if (entryToUse) {
144 if (entryToUse->id == id) { // found the matching entry
145 break;
146 } else { // try next entry
147 prevEntry = entryToUse;
148 entryToUse = entryToUse->next;
149 }
150 } else {
151 // Make sure that we haven't run out of buckets
152 FW_ASSERT(this->m_tlmEntries.free < TLMPACKETIZER_HASH_BUCKETS, this->m_tlmEntries.free);
153 // add new bucket from free list
154 entryToUse = &this->m_tlmEntries.buckets[this->m_tlmEntries.free++];
155 // Coverity warning about null dereference - see if it happens
156 FW_ASSERT(prevEntry);
157 prevEntry->next = entryToUse;
158 // clear next pointer
159 entryToUse->next = nullptr;
160 // set all packet offsets to -1 for new entry
161 for (NATIVE_UINT_TYPE pktOffsetEntry = 0; pktOffsetEntry < MAX_PACKETIZER_PACKETS; pktOffsetEntry++) {
162 entryToUse->packetOffset[pktOffsetEntry] = -1;
163 }
164 break;
165 }
166 }
167 } else {
168 // Make sure that we haven't run out of buckets
169 FW_ASSERT(this->m_tlmEntries.free < TLMPACKETIZER_HASH_BUCKETS, this->m_tlmEntries.free);
170 // create new entry at slot head
171 this->m_tlmEntries.slots[index] = &this->m_tlmEntries.buckets[this->m_tlmEntries.free++];
172 entryToUse = this->m_tlmEntries.slots[index];
173 entryToUse->next = nullptr;
174 // set all packet offsets to -1 for new entry
175 for (NATIVE_UINT_TYPE pktOffsetEntry = 0; pktOffsetEntry < MAX_PACKETIZER_PACKETS; pktOffsetEntry++) {
176 entryToUse->packetOffset[pktOffsetEntry] = -1;
177 }
178 }
179
180 return entryToUse;
181}
182
183// ----------------------------------------------------------------------
184// Handler implementations for user-defined typed input ports
185// ----------------------------------------------------------------------
186
187void TlmPacketizer ::TlmRecv_handler(const NATIVE_INT_TYPE portNum,
188 FwChanIdType id,
189 Fw::Time& timeTag,
190 Fw::TlmBuffer& val) {
191 FW_ASSERT(this->m_configured);
192 // get hash value for id
193 NATIVE_UINT_TYPE index = this->doHash(id);
194 TlmEntry* entryToUse = nullptr;
195
196 // Search to see if the channel is being sent
197 entryToUse = this->m_tlmEntries.slots[index];
198
199 // if no entries at hash, channel not part of a packet or is not ignored
200 if (not entryToUse) {
201 this->missingChannel(id);
202 return;
203 }
204
205 for (NATIVE_UINT_TYPE bucket = 0; bucket < TLMPACKETIZER_HASH_BUCKETS; bucket++) {
206 if (entryToUse) {
207 if (entryToUse->id == id) { // found the matching entry
208 // check to see if the channel is ignored. If so, just return.
209 if (entryToUse->ignored) {
210 return;
211 }
212 break;
213 } else { // try next entry
214 entryToUse = entryToUse->next;
215 }
216 } else {
217 // telemetry channel not in any packets
218 this->missingChannel(id);
219 return;
220 }
221 }
222
223 // copy telemetry value into active buffers
224 for (NATIVE_UINT_TYPE pkt = 0; pkt < MAX_PACKETIZER_PACKETS; pkt++) {
225 // check if current packet has this channel
226 if (entryToUse->packetOffset[pkt] != -1) {
227 // get destination address
228 // printf("PK %d CH: %d\n",this->m_fillBuffers[pkt].id,id);
229 this->m_lock.lock();
230 this->m_fillBuffers[pkt].updated = true;
231 this->m_fillBuffers[pkt].latestTime = timeTag;
232 U8* ptr = &this->m_fillBuffers[pkt].buffer.getBuffAddr()[entryToUse->packetOffset[pkt]];
233 memcpy(ptr, val.getBuffAddr(), val.getBuffLength());
234 this->m_lock.unLock();
235 }
236 }
237}
238
239void TlmPacketizer ::Run_handler(const NATIVE_INT_TYPE portNum, NATIVE_UINT_TYPE context) {
240 FW_ASSERT(this->m_configured);
241
242 // Only write packets if connected
243 if (not this->isConnected_PktSend_OutputPort(0)) {
244 return;
245 }
246
247 // lock mutex long enough to modify active telemetry buffer
248 // so the data can be read without worrying about updates
249 this->m_lock.lock();
250 // copy buffers from fill side to send side
251 for (NATIVE_UINT_TYPE pkt = 0; pkt < this->m_numPackets; pkt++) {
252 if ((this->m_fillBuffers[pkt].updated) and
253 ((this->m_fillBuffers[pkt].level <= this->m_startLevel) or (this->m_fillBuffers[pkt].requested))) {
254 this->m_sendBuffers[pkt] = this->m_fillBuffers[pkt];
256 this->m_fillBuffers[pkt].updated = false;
257 }
258 this->m_fillBuffers[pkt].requested = false;
259 // PACKET_UPDATE_AFTER_FIRST_CHANGE will be this case - updated flag will not be cleared
260 } else if ((PACKET_UPDATE_ALWAYS == PACKET_UPDATE_MODE) and
261 (this->m_fillBuffers[pkt].level <= this->m_startLevel)) {
262 this->m_sendBuffers[pkt] = this->m_fillBuffers[pkt];
263 this->m_sendBuffers[pkt].updated = true;
264 } else {
265 this->m_sendBuffers[pkt].updated = false;
266 }
267 }
268 this->m_lock.unLock();
269
270 // push all updated packet buffers
271 for (NATIVE_UINT_TYPE pkt = 0; pkt < this->m_numPackets; pkt++) {
272 if (this->m_sendBuffers[pkt].updated) {
273 // serialize time into time offset in packet
275 &this->m_sendBuffers[pkt]
276 .buffer.getBuffAddr()[sizeof(FwPacketDescriptorType) + sizeof(FwTlmPacketizeIdType)],
278 Fw::SerializeStatus stat = buff.serialize(this->m_sendBuffers[pkt].latestTime);
279 FW_ASSERT(Fw::FW_SERIALIZE_OK == stat, stat);
280
281 this->PktSend_out(0, this->m_sendBuffers[pkt].buffer, 0);
282 }
283 }
284}
285
286void TlmPacketizer ::pingIn_handler(const NATIVE_INT_TYPE portNum, U32 key) {
287 // return key
288 this->pingOut_out(0, key);
289}
290
291// ----------------------------------------------------------------------
292// Command handler implementations
293// ----------------------------------------------------------------------
294
295void TlmPacketizer ::SET_LEVEL_cmdHandler(const FwOpcodeType opCode, const U32 cmdSeq, U32 level) {
296 this->m_startLevel = level;
297 if (level > this->m_maxLevel) {
298 this->log_WARNING_LO_MaxLevelExceed(level, this->m_maxLevel);
299 }
300 this->tlmWrite_SendLevel(level);
301 this->log_ACTIVITY_HI_LevelSet(level);
302 this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
303}
304
305void TlmPacketizer ::SEND_PKT_cmdHandler(const FwOpcodeType opCode, const U32 cmdSeq, U32 id) {
306 NATIVE_UINT_TYPE pkt = 0;
307 for (pkt = 0; pkt < this->m_numPackets; pkt++) {
308 if (this->m_fillBuffers[pkt].id == id) {
309 this->m_lock.lock();
310 this->m_fillBuffers[pkt].updated = true;
311 this->m_fillBuffers[pkt].latestTime = this->getTime();
312 this->m_fillBuffers[pkt].requested = true;
313 this->m_lock.unLock();
314
315 this->log_ACTIVITY_LO_PacketSent(id);
316 break;
317 }
318 }
319
320 // couldn't find it
321 if (pkt == this->m_numPackets) {
322 log_WARNING_LO_PacketNotFound(id);
323 this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::VALIDATION_ERROR);
324 return;
325 }
326
327 this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
328}
329
330NATIVE_UINT_TYPE TlmPacketizer::doHash(FwChanIdType id) {
332}
333
334void TlmPacketizer::missingChannel(FwChanIdType id) {
335 // search to see if missing channel has already been sent
336 for (NATIVE_UINT_TYPE slot = 0; slot < TLMPACKETIZER_MAX_MISSING_TLM_CHECK; slot++) {
337 // if it's been checked, return
338 if (this->m_missTlmCheck[slot].checked and (this->m_missTlmCheck[slot].id == id)) {
339 return;
340 } else if (not this->m_missTlmCheck[slot].checked) {
341 this->m_missTlmCheck[slot].checked = true;
342 this->m_missTlmCheck[slot].id = id;
343 this->log_WARNING_LO_NoChan(id);
344 return;
345 }
346 }
347}
348
349} // end namespace Svc
#define FW_ASSERT(...)
Definition Assert.hpp:7
PlatformIntType NATIVE_INT_TYPE
Definition BasicTypes.h:51
uint8_t U8
8-bit unsigned integer
Definition BasicTypes.h:26
PlatformUIntType NATIVE_UINT_TYPE
Definition BasicTypes.h:52
U32 FwChanIdType
Definition FpConfig.h:59
U32 FwPacketDescriptorType
Definition FpConfig.h:53
U16 FwTlmPacketizeIdType
Definition FpConfig.h:68
U32 FwOpcodeType
Definition FpConfig.h:56
#define FW_COM_BUFFER_MAX_SIZE
Max size of Fw::Com buffer.
Definition FpConfig.h:220
C++-compatible configuration header for fprime configuration.
@ FW_PACKET_PACKETIZED_TLM
Definition ComPacket.hpp:26
NATIVE_UINT_TYPE getBuffLength() const
returns current buffer size
@ SERIALIZED_SIZE
Definition Time.hpp:13
U8 * getBuffAddr()
gets buffer address for data filling
Definition TlmBuffer.cpp:40
void setPacketList(const TlmPacketizerPacketList &packetList, const Svc::TlmPacketizerPacket &ignoreList, const NATIVE_UINT_TYPE startLevel)
SerializeStatus
forward declaration for string
@ FW_SERIALIZE_OK
Serialization/Deserialization operation was successful.
@ PACKET_UPDATE_ON_CHANGE
@ PACKET_UPDATE_ALWAYS
static const NATIVE_UINT_TYPE TLMPACKETIZER_MAX_MISSING_TLM_CHECK
static const NATIVE_UINT_TYPE TLMPACKETIZER_HASH_MOD_VALUE
static const NATIVE_UINT_TYPE TLMPACKETIZER_HASH_BUCKETS
static const PacketUpdateMode PACKET_UPDATE_MODE
static const NATIVE_UINT_TYPE MAX_PACKETIZER_PACKETS
static const NATIVE_UINT_TYPE TLMPACKETIZER_NUM_TLM_HASH_SLOTS
NATIVE_UINT_TYPE size
serialized size of channel in bytes
FwChanIdType id
Id of channel.
FwTlmPacketizeIdType id
packet ID
NATIVE_UINT_TYPE level
packet level - used to select set of packets to send
const TlmPacketizerChannelEntry * list
pointer to a channel entry
NATIVE_UINT_TYPE numEntries
number of channels in packet
const TlmPacketizerPacket * list[MAX_PACKETIZER_PACKETS]