F´ Flight Software - C/C++ Documentation
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
50TlmPacketizer ::~TlmPacketizer() {}
51
53 const Svc::TlmPacketizerPacket& ignoreList,
54 const NATIVE_UINT_TYPE startLevel) {
55 FW_ASSERT(packetList.list);
56 FW_ASSERT(ignoreList.list);
57 FW_ASSERT(packetList.numEntries <= MAX_PACKETIZER_PACKETS, static_cast<FwAssertArgType>(packetList.numEntries));
58 // validate packet sizes against maximum com buffer size and populate hash
59 // table
60 for (NATIVE_UINT_TYPE pktEntry = 0; pktEntry < packetList.numEntries; pktEntry++) {
61 // Initial size is packetized telemetry descriptor + size of time tag + sizeof packet ID
62 NATIVE_UINT_TYPE packetLen =
64 FW_ASSERT(packetList.list[pktEntry]->list, static_cast<FwAssertArgType>(pktEntry));
65 // add up entries for each defined packet
66 for (NATIVE_UINT_TYPE tlmEntry = 0; tlmEntry < packetList.list[pktEntry]->numEntries; tlmEntry++) {
67 // get hash value for id
68 FwChanIdType id = packetList.list[pktEntry]->list[tlmEntry].id;
69 TlmEntry* entryToUse = this->findBucket(id);
70 // copy into entry
71 FW_ASSERT(entryToUse);
72 entryToUse->used = true;
73 // not ignored channel
74 entryToUse->ignored = false;
75 entryToUse->id = id;
76 // the offset into the buffer will be the current packet length
77 entryToUse->packetOffset[pktEntry] = static_cast<NATIVE_INT_TYPE>(packetLen);
78
79 packetLen += packetList.list[pktEntry]->list[tlmEntry].size;
80
81 } // end channel in packet
82 FW_ASSERT(packetLen <= FW_COM_BUFFER_MAX_SIZE, static_cast<FwAssertArgType>(packetLen), static_cast<FwAssertArgType>(pktEntry));
83 // clear contents
84 memset(this->m_fillBuffers[pktEntry].buffer.getBuffAddr(), 0, packetLen);
85 // serialize packet descriptor and packet ID now since it will always be the same
86 Fw::SerializeStatus stat = this->m_fillBuffers[pktEntry].buffer.serialize(
88 FW_ASSERT(Fw::FW_SERIALIZE_OK == stat, stat);
89 stat = this->m_fillBuffers[pktEntry].buffer.serialize(packetList.list[pktEntry]->id);
90 FW_ASSERT(Fw::FW_SERIALIZE_OK == stat, stat);
91 // set packet buffer length
92 stat = this->m_fillBuffers[pktEntry].buffer.setBuffLen(packetLen);
93 FW_ASSERT(Fw::FW_SERIALIZE_OK == stat, stat);
94 // save ID
95 this->m_fillBuffers[pktEntry].id = packetList.list[pktEntry]->id;
96 // save level
97 this->m_fillBuffers[pktEntry].level = packetList.list[pktEntry]->level;
98 // store max level
99 if (packetList.list[pktEntry]->level > this->m_maxLevel) {
100 this->m_maxLevel = packetList.list[pktEntry]->level;
101 }
102 // save start level
103 this->m_startLevel = startLevel;
104
105 } // end packet list
106
107 // populate hash table with ignore list
108 for (NATIVE_UINT_TYPE channelEntry = 0; channelEntry < ignoreList.numEntries; channelEntry++) {
109 // get hash value for id
110 FwChanIdType id = ignoreList.list[channelEntry].id;
111
112 TlmEntry* entryToUse = this->findBucket(id);
113
114 // copy into entry
115 FW_ASSERT(entryToUse);
116 entryToUse->used = true;
117 // is ignored channel
118 entryToUse->ignored = true;
119 entryToUse->id = id;
120 } // end ignore list
121
122 // store number of packets
123 this->m_numPackets = packetList.numEntries;
124
125 // indicate configured
126 this->m_configured = true;
127}
128
129TlmPacketizer::TlmEntry* TlmPacketizer::findBucket(FwChanIdType id) {
130 NATIVE_UINT_TYPE index = this->doHash(id);
132 TlmEntry* entryToUse = nullptr;
133 TlmEntry* prevEntry = nullptr;
134
135 // Search to see if channel has already been stored or a bucket needs to be added
136 if (this->m_tlmEntries.slots[index]) {
137 entryToUse = this->m_tlmEntries.slots[index];
138 for (NATIVE_UINT_TYPE bucket = 0; bucket < TLMPACKETIZER_HASH_BUCKETS; bucket++) {
139 if (entryToUse) {
140 if (entryToUse->id == id) { // found the matching entry
141 break;
142 } else { // try next entry
143 prevEntry = entryToUse;
144 entryToUse = entryToUse->next;
145 }
146 } else {
147 // Make sure that we haven't run out of buckets
148 FW_ASSERT(this->m_tlmEntries.free < TLMPACKETIZER_HASH_BUCKETS, static_cast<FwAssertArgType>(this->m_tlmEntries.free));
149 // add new bucket from free list
150 entryToUse = &this->m_tlmEntries.buckets[this->m_tlmEntries.free++];
151 // Coverity warning about null dereference - see if it happens
152 FW_ASSERT(prevEntry);
153 prevEntry->next = entryToUse;
154 // clear next pointer
155 entryToUse->next = nullptr;
156 // set all packet offsets to -1 for new entry
157 for (NATIVE_UINT_TYPE pktOffsetEntry = 0; pktOffsetEntry < MAX_PACKETIZER_PACKETS; pktOffsetEntry++) {
158 entryToUse->packetOffset[pktOffsetEntry] = -1;
159 }
160 break;
161 }
162 }
163 } else {
164 // Make sure that we haven't run out of buckets
165 FW_ASSERT(this->m_tlmEntries.free < TLMPACKETIZER_HASH_BUCKETS, static_cast<FwAssertArgType>(this->m_tlmEntries.free));
166 // create new entry at slot head
167 this->m_tlmEntries.slots[index] = &this->m_tlmEntries.buckets[this->m_tlmEntries.free++];
168 entryToUse = this->m_tlmEntries.slots[index];
169 entryToUse->next = nullptr;
170 // set all packet offsets to -1 for new entry
171 for (NATIVE_UINT_TYPE pktOffsetEntry = 0; pktOffsetEntry < MAX_PACKETIZER_PACKETS; pktOffsetEntry++) {
172 entryToUse->packetOffset[pktOffsetEntry] = -1;
173 }
174 }
175
176 return entryToUse;
177}
178
179// ----------------------------------------------------------------------
180// Handler implementations for user-defined typed input ports
181// ----------------------------------------------------------------------
182
183void TlmPacketizer ::TlmRecv_handler(const NATIVE_INT_TYPE portNum,
184 FwChanIdType id,
185 Fw::Time& timeTag,
186 Fw::TlmBuffer& val) {
187 FW_ASSERT(this->m_configured);
188 // get hash value for id
189 NATIVE_UINT_TYPE index = this->doHash(id);
190 TlmEntry* entryToUse = nullptr;
191
192 // Search to see if the channel is being sent
193 entryToUse = this->m_tlmEntries.slots[index];
194
195 // if no entries at hash, channel not part of a packet or is not ignored
196 if (not entryToUse) {
197 this->missingChannel(id);
198 return;
199 }
200
201 for (NATIVE_UINT_TYPE bucket = 0; bucket < TLMPACKETIZER_HASH_BUCKETS; bucket++) {
202 if (entryToUse) {
203 if (entryToUse->id == id) { // found the matching entry
204 // check to see if the channel is ignored. If so, just return.
205 if (entryToUse->ignored) {
206 return;
207 }
208 break;
209 } else { // try next entry
210 entryToUse = entryToUse->next;
211 }
212 } else {
213 // telemetry channel not in any packets
214 this->missingChannel(id);
215 return;
216 }
217 }
218
219 // copy telemetry value into active buffers
220 for (NATIVE_UINT_TYPE pkt = 0; pkt < MAX_PACKETIZER_PACKETS; pkt++) {
221 // check if current packet has this channel
222 if (entryToUse->packetOffset[pkt] != -1) {
223 // get destination address
224 // printf("PK %d CH: %d\n",this->m_fillBuffers[pkt].id,id);
225 this->m_lock.lock();
226 this->m_fillBuffers[pkt].updated = true;
227 this->m_fillBuffers[pkt].latestTime = timeTag;
228 U8* ptr = &this->m_fillBuffers[pkt].buffer.getBuffAddr()[entryToUse->packetOffset[pkt]];
229 memcpy(ptr, val.getBuffAddr(), val.getBuffLength());
230 this->m_lock.unLock();
231 }
232 }
233}
234
235void TlmPacketizer ::Run_handler(const NATIVE_INT_TYPE portNum, U32 context) {
236 FW_ASSERT(this->m_configured);
237
238 // Only write packets if connected
239 if (not this->isConnected_PktSend_OutputPort(0)) {
240 return;
241 }
242
243 // lock mutex long enough to modify active telemetry buffer
244 // so the data can be read without worrying about updates
245 this->m_lock.lock();
246 // copy buffers from fill side to send side
247 for (NATIVE_UINT_TYPE pkt = 0; pkt < this->m_numPackets; pkt++) {
248 if ((this->m_fillBuffers[pkt].updated) and
249 ((this->m_fillBuffers[pkt].level <= this->m_startLevel) or (this->m_fillBuffers[pkt].requested))) {
250 this->m_sendBuffers[pkt] = this->m_fillBuffers[pkt];
252 this->m_fillBuffers[pkt].updated = false;
253 }
254 this->m_fillBuffers[pkt].requested = false;
255 // PACKET_UPDATE_AFTER_FIRST_CHANGE will be this case - updated flag will not be cleared
256 } else if ((PACKET_UPDATE_ALWAYS == PACKET_UPDATE_MODE) and
257 (this->m_fillBuffers[pkt].level <= this->m_startLevel)) {
258 this->m_sendBuffers[pkt] = this->m_fillBuffers[pkt];
259 this->m_sendBuffers[pkt].updated = true;
260 } else {
261 this->m_sendBuffers[pkt].updated = false;
262 }
263 }
264 this->m_lock.unLock();
265
266 // push all updated packet buffers
267 for (NATIVE_UINT_TYPE pkt = 0; pkt < this->m_numPackets; pkt++) {
268 if (this->m_sendBuffers[pkt].updated) {
269 // serialize time into time offset in packet
271 &this->m_sendBuffers[pkt]
272 .buffer.getBuffAddr()[sizeof(FwPacketDescriptorType) + sizeof(FwTlmPacketizeIdType)],
274 Fw::SerializeStatus stat = buff.serialize(this->m_sendBuffers[pkt].latestTime);
275 FW_ASSERT(Fw::FW_SERIALIZE_OK == stat, stat);
276
277 this->PktSend_out(0, this->m_sendBuffers[pkt].buffer, 0);
278 }
279 }
280}
281
282void TlmPacketizer ::pingIn_handler(const NATIVE_INT_TYPE portNum, U32 key) {
283 // return key
284 this->pingOut_out(0, key);
285}
286
287// ----------------------------------------------------------------------
288// Command handler implementations
289// ----------------------------------------------------------------------
290
291void TlmPacketizer ::SET_LEVEL_cmdHandler(const FwOpcodeType opCode, const U32 cmdSeq, U32 level) {
292 this->m_startLevel = level;
293 if (level > this->m_maxLevel) {
294 this->log_WARNING_LO_MaxLevelExceed(level, this->m_maxLevel);
295 }
296 this->tlmWrite_SendLevel(level);
297 this->log_ACTIVITY_HI_LevelSet(level);
298 this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
299}
300
301void TlmPacketizer ::SEND_PKT_cmdHandler(const FwOpcodeType opCode, const U32 cmdSeq, U32 id) {
302 NATIVE_UINT_TYPE pkt = 0;
303 for (pkt = 0; pkt < this->m_numPackets; pkt++) {
304 if (this->m_fillBuffers[pkt].id == id) {
305 this->m_lock.lock();
306 this->m_fillBuffers[pkt].updated = true;
307 this->m_fillBuffers[pkt].latestTime = this->getTime();
308 this->m_fillBuffers[pkt].requested = true;
309 this->m_lock.unLock();
310
311 this->log_ACTIVITY_LO_PacketSent(id);
312 break;
313 }
314 }
315
316 // couldn't find it
317 if (pkt == this->m_numPackets) {
318 log_WARNING_LO_PacketNotFound(id);
319 this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::VALIDATION_ERROR);
320 return;
321 }
322
323 this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
324}
325
326NATIVE_UINT_TYPE TlmPacketizer::doHash(FwChanIdType id) {
328}
329
330void TlmPacketizer::missingChannel(FwChanIdType id) {
331 // search to see if missing channel has already been sent
332 for (NATIVE_UINT_TYPE slot = 0; slot < TLMPACKETIZER_MAX_MISSING_TLM_CHECK; slot++) {
333 // if it's been checked, return
334 if (this->m_missTlmCheck[slot].checked and (this->m_missTlmCheck[slot].id == id)) {
335 return;
336 } else if (not this->m_missTlmCheck[slot].checked) {
337 this->m_missTlmCheck[slot].checked = true;
338 this->m_missTlmCheck[slot].id = id;
339 this->log_WARNING_LO_NoChan(id);
340 return;
341 }
342 }
343}
344
345} // end namespace Svc
#define FW_ASSERT(...)
Definition Assert.hpp:14
PlatformIntType NATIVE_INT_TYPE
Definition BasicTypes.h:55
uint8_t U8
8-bit unsigned integer
Definition BasicTypes.h:30
PlatformUIntType NATIVE_UINT_TYPE
Definition BasicTypes.h:56
PlatformAssertArgType FwAssertArgType
Definition FpConfig.h:39
U32 FwChanIdType
Definition FpConfig.h:95
U32 FwPacketDescriptorType
Definition FpConfig.h:87
U16 FwTlmPacketizeIdType
Definition FpConfig.h:111
U32 FwOpcodeType
Definition FpConfig.h:91
#define FW_COM_BUFFER_MAX_SIZE
Definition FpConfig.h:282
C++-compatible configuration header for fprime configuration.
@ VALIDATION_ERROR
Command failed validation.
@ OK
Command successfully executed.
@ FW_PACKET_PACKETIZED_TLM
Definition ComPacket.hpp:26
External serialize buffer with no copy semantics.
Serializable::SizeType getBuffLength() const
returns current buffer size
@ SERIALIZED_SIZE
Definition Time.hpp:13
U8 * getBuffAddr()
gets buffer address for data filling
Definition TlmBuffer.cpp:40
Auto-generated base for TlmPacketizer component.
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]