F´ Flight Software - C/C++ Documentation
A framework for building embedded system applications to NASA flight quality standards.
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 
15 namespace Svc {
16 
17 // ----------------------------------------------------------------------
18 // Construction, initialization, and destruction
19 // ----------------------------------------------------------------------
20 
21 TlmPacketizer ::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 
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(
87  static_cast<FwPacketDescriptorType>(Fw::ComPacket::FW_PACKET_PACKETIZED_TLM));
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 
129 TlmPacketizer::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 
183 void 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  this->m_lock.lock();
225  this->m_fillBuffers[pkt].updated = true;
226  this->m_fillBuffers[pkt].latestTime = timeTag;
227  U8* ptr = &this->m_fillBuffers[pkt].buffer.getBuffAddr()[entryToUse->packetOffset[pkt]];
228  memcpy(ptr, val.getBuffAddr(), val.getBuffLength());
229  this->m_lock.unLock();
230  }
231  }
232 }
233 
234 void TlmPacketizer ::Run_handler(const NATIVE_INT_TYPE portNum, U32 context) {
235  FW_ASSERT(this->m_configured);
236 
237  // Only write packets if connected
238  if (not this->isConnected_PktSend_OutputPort(0)) {
239  return;
240  }
241 
242  // lock mutex long enough to modify active telemetry buffer
243  // so the data can be read without worrying about updates
244  this->m_lock.lock();
245  // copy buffers from fill side to send side
246  for (NATIVE_UINT_TYPE pkt = 0; pkt < this->m_numPackets; pkt++) {
247  if ((this->m_fillBuffers[pkt].updated) and
248  ((this->m_fillBuffers[pkt].level <= this->m_startLevel) or (this->m_fillBuffers[pkt].requested))) {
249  this->m_sendBuffers[pkt] = this->m_fillBuffers[pkt];
251  this->m_fillBuffers[pkt].updated = false;
252  }
253  this->m_fillBuffers[pkt].requested = false;
254  // PACKET_UPDATE_AFTER_FIRST_CHANGE will be this case - updated flag will not be cleared
255  } else if ((PACKET_UPDATE_ALWAYS == PACKET_UPDATE_MODE) and
256  (this->m_fillBuffers[pkt].level <= this->m_startLevel)) {
257  this->m_sendBuffers[pkt] = this->m_fillBuffers[pkt];
258  this->m_sendBuffers[pkt].updated = true;
259  } else {
260  this->m_sendBuffers[pkt].updated = false;
261  }
262  }
263  this->m_lock.unLock();
264 
265  // push all updated packet buffers
266  for (NATIVE_UINT_TYPE pkt = 0; pkt < this->m_numPackets; pkt++) {
267  if (this->m_sendBuffers[pkt].updated) {
268  // serialize time into time offset in packet
270  &this->m_sendBuffers[pkt]
271  .buffer.getBuffAddr()[sizeof(FwPacketDescriptorType) + sizeof(FwTlmPacketizeIdType)],
273  Fw::SerializeStatus stat = buff.serialize(this->m_sendBuffers[pkt].latestTime);
274  FW_ASSERT(Fw::FW_SERIALIZE_OK == stat, stat);
275 
276  this->PktSend_out(0, this->m_sendBuffers[pkt].buffer, 0);
277  }
278  }
279 }
280 
281 void TlmPacketizer ::pingIn_handler(const NATIVE_INT_TYPE portNum, U32 key) {
282  // return key
283  this->pingOut_out(0, key);
284 }
285 
286 // ----------------------------------------------------------------------
287 // Command handler implementations
288 // ----------------------------------------------------------------------
289 
290 void TlmPacketizer ::SET_LEVEL_cmdHandler(const FwOpcodeType opCode, const U32 cmdSeq, U32 level) {
291  this->m_startLevel = level;
292  if (level > this->m_maxLevel) {
293  this->log_WARNING_LO_MaxLevelExceed(level, this->m_maxLevel);
294  }
295  this->tlmWrite_SendLevel(level);
296  this->log_ACTIVITY_HI_LevelSet(level);
297  this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
298 }
299 
300 void TlmPacketizer ::SEND_PKT_cmdHandler(const FwOpcodeType opCode, const U32 cmdSeq, U32 id) {
301  NATIVE_UINT_TYPE pkt = 0;
302  for (pkt = 0; pkt < this->m_numPackets; pkt++) {
303  if (this->m_fillBuffers[pkt].id == id) {
304  this->m_lock.lock();
305  this->m_fillBuffers[pkt].updated = true;
306  this->m_fillBuffers[pkt].latestTime = this->getTime();
307  this->m_fillBuffers[pkt].requested = true;
308  this->m_lock.unLock();
309 
310  this->log_ACTIVITY_LO_PacketSent(id);
311  break;
312  }
313  }
314 
315  // couldn't find it
316  if (pkt == this->m_numPackets) {
318  this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::VALIDATION_ERROR);
319  return;
320  }
321 
322  this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
323 }
324 
325 NATIVE_UINT_TYPE TlmPacketizer::doHash(FwChanIdType id) {
327 }
328 
329 void TlmPacketizer::missingChannel(FwChanIdType id) {
330  // search to see if missing channel has already been sent
331  for (NATIVE_UINT_TYPE slot = 0; slot < TLMPACKETIZER_MAX_MISSING_TLM_CHECK; slot++) {
332  // if it's been checked, return
333  if (this->m_missTlmCheck[slot].checked and (this->m_missTlmCheck[slot].id == id)) {
334  return;
335  } else if (not this->m_missTlmCheck[slot].checked) {
336  this->m_missTlmCheck[slot].checked = true;
337  this->m_missTlmCheck[slot].id = id;
338  this->log_WARNING_LO_NoChan(id);
339  return;
340  }
341  }
342 }
343 
344 } // end namespace Svc
void setPacketList(const TlmPacketizerPacketList &packetList, const Svc::TlmPacketizerPacket &ignoreList, const NATIVE_UINT_TYPE startLevel)
PlatformUIntType NATIVE_UINT_TYPE
Definition: BasicTypes.h:56
Serialization/Deserialization operation was successful.
Definition: Time.hpp:9
TlmPacketizer(const char *const compName)
FwTlmPacketizeIdType id
packet ID
SerializeStatus serialize(U8 val)
serialize 8-bit unsigned int
PlatformIntType NATIVE_INT_TYPE
Definition: BasicTypes.h:55
const TlmPacketizerPacket * list[MAX_PACKETIZER_PACKETS]
void unLock()
unlock the mutex and assert success
Definition: Mutex.cpp:40
void PktSend_out(FwIndexType portNum, Fw::ComBuffer &data, U32 context)
Invoke output port PktSend.
void cmdResponse_out(FwOpcodeType opCode, U32 cmdSeq, Fw::CmdResponse response)
Emit command response.
const TlmPacketizerChannelEntry * list
pointer to a channel entry
static const NATIVE_UINT_TYPE TLMPACKETIZER_HASH_BUCKETS
SerializeStatus
forward declaration for string
U32 FwPacketDescriptorType
Definition: FpConfig.h:87
FwChanIdType id
Id of channel.
Serializable::SizeType getBuffLength() const
returns current buffer size
static const PacketUpdateMode PACKET_UPDATE_MODE
U32 FwOpcodeType
Definition: FpConfig.h:91
NATIVE_UINT_TYPE size
serialized size of channel in bytes
External serialize buffer with no copy semantics.
#define FW_COM_BUFFER_MAX_SIZE
Definition: FpConfig.h:282
U8 * getBuffAddr()
gets buffer address for data filling
Definition: TlmBuffer.cpp:40
static const NATIVE_UINT_TYPE TLMPACKETIZER_NUM_TLM_HASH_SLOTS
static const NATIVE_UINT_TYPE TLMPACKETIZER_MAX_MISSING_TLM_CHECK
Command successfully executed.
void log_WARNING_LO_MaxLevelExceed(U32 level, U32 max) const
C++-compatible configuration header for fprime configuration.
uint8_t U8
8-bit unsigned integer
Definition: BasicTypes.h:30
void tlmWrite_SendLevel(U32 arg, Fw::Time _tlmTime=Fw::Time()) const
static const NATIVE_UINT_TYPE MAX_PACKETIZER_PACKETS
static const NATIVE_UINT_TYPE TLMPACKETIZER_HASH_MOD_VALUE
NATIVE_UINT_TYPE numEntries
number of channels in packet
Command failed validation.
bool isConnected_PktSend_OutputPort(FwIndexType portNum)
U16 FwTlmPacketizeIdType
Definition: FpConfig.h:111
#define FW_ASSERT(...)
Definition: Assert.hpp:14
void pingOut_out(FwIndexType portNum, U32 key)
Invoke output port pingOut.
void lock()
lock the mutex and assert success
Definition: Mutex.cpp:34
U32 FwChanIdType
Definition: FpConfig.h:95
NATIVE_UINT_TYPE level
packet level - used to select set of packets to send
Auto-generated base for TlmPacketizer component.