F´ Flight Software - C/C++ Documentation  devel
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(
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  // 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 
235 void 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 
282 void 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 
291 void 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 
301 void 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) {
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 
326 NATIVE_UINT_TYPE TlmPacketizer::doHash(FwChanIdType id) {
328 }
329 
330 void 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
Definition: Time.hpp:9
@ SERIALIZED_SIZE
Definition: Time.hpp:13
U8 * getBuffAddr()
gets buffer address for data filling
Definition: TlmBuffer.cpp:40
void unLock()
unlock the mutex and assert success
Definition: Mutex.cpp:40
void lock()
lock the mutex and assert success
Definition: Mutex.cpp:34
Auto-generated base for TlmPacketizer component.
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.
bool isConnected_PktSend_OutputPort(FwIndexType portNum)
void tlmWrite_SendLevel(U32 arg, Fw::Time _tlmTime=Fw::Time())
void pingOut_out(FwIndexType portNum, U32 key)
Invoke output port pingOut.
void log_WARNING_LO_MaxLevelExceed(U32 level, U32 max)
TlmPacketizer(const char *const compName)
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]