1 /*
2 * \file trc_pkt_proc_stm.cpp
3 * \brief OpenCSD :
4 *
5 * \copyright Copyright (c) 2015, ARM Limited. All Rights Reserved.
6 */
7
8 /*
9 * Redistribution and use in source and binary forms, with or without modification,
10 * are permitted provided that the following conditions are met:
11 *
12 * 1. Redistributions of source code must retain the above copyright notice,
13 * this list of conditions and the following disclaimer.
14 *
15 * 2. Redistributions in binary form must reproduce the above copyright notice,
16 * this list of conditions and the following disclaimer in the documentation
17 * and/or other materials provided with the distribution.
18 *
19 * 3. Neither the name of the copyright holder nor the names of its contributors
20 * may be used to endorse or promote products derived from this software without
21 * specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND
24 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
25 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
26 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
27 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
28 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
30 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
31 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
32 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33 */
34
35 #include "opencsd/stm/trc_pkt_proc_stm.h"
36
37
38 // processor object construction
39 // ************************
40
41 #ifdef __GNUC__
42 // G++ doesn't like the ## pasting
43 #define STM_PKTS_NAME "PKTP_STM"
44 #else
45 #define STM_PKTS_NAME OCSD_CMPNAME_PREFIX_PKTPROC##"_STM"
46 #endif
47
48 static const uint32_t STM_SUPPORTED_OP_FLAGS = OCSD_OPFLG_PKTPROC_COMMON;
49
TrcPktProcStm()50 TrcPktProcStm::TrcPktProcStm() : TrcPktProcBase(STM_PKTS_NAME)
51 {
52 initObj();
53 }
54
TrcPktProcStm(int instIDNum)55 TrcPktProcStm::TrcPktProcStm(int instIDNum) : TrcPktProcBase(STM_PKTS_NAME, instIDNum)
56 {
57 initObj();
58 }
59
~TrcPktProcStm()60 TrcPktProcStm::~TrcPktProcStm()
61 {
62 getRawPacketMonAttachPt()->set_notifier(0);
63 }
64
initObj()65 void TrcPktProcStm::initObj()
66 {
67 m_supported_op_flags = STM_SUPPORTED_OP_FLAGS;
68 initProcessorState();
69 getRawPacketMonAttachPt()->set_notifier(&mon_in_use);
70 buildOpTables();
71 }
72
73 // implementation packet processing interface overrides
74 // ************************
processData(const ocsd_trc_index_t index,const uint32_t dataBlockSize,const uint8_t * pDataBlock,uint32_t * numBytesProcessed)75 ocsd_datapath_resp_t TrcPktProcStm::processData( const ocsd_trc_index_t index,
76 const uint32_t dataBlockSize,
77 const uint8_t *pDataBlock,
78 uint32_t *numBytesProcessed)
79 {
80 ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
81 m_p_data_in = pDataBlock;
82 m_data_in_size = dataBlockSize;
83 m_data_in_used = 0;
84
85 // while there is data and a continue response on the data path
86 while( dataToProcess() && OCSD_DATA_RESP_IS_CONT(resp) )
87 {
88 try
89 {
90 switch(m_proc_state)
91 {
92 case WAIT_SYNC:
93 waitForSync(index);
94 break;
95
96 case PROC_HDR:
97 m_packet_index = index + m_data_in_used;
98 if(readNibble())
99 {
100 m_proc_state = PROC_DATA; // read the header nibble, next if any has to be data
101 m_pCurrPktFn = m_1N_ops[m_nibble]; // set packet function and fall through
102 }
103 else
104 break;
105
106 case PROC_DATA:
107 (this->*m_pCurrPktFn)();
108
109 // if we have enough to send, fall through, otherwise stop
110 if(m_proc_state != SEND_PKT)
111 break;
112
113 case SEND_PKT:
114 resp = outputPacket();
115 break;
116 }
117 }
118 catch(ocsdError &err)
119 {
120 LogError(err);
121 if( ((err.getErrorCode() == OCSD_ERR_BAD_PACKET_SEQ) ||
122 (err.getErrorCode() == OCSD_ERR_INVALID_PCKT_HDR)) &&
123 !(getComponentOpMode() & OCSD_OPFLG_PKTPROC_ERR_BAD_PKTS))
124 {
125 // send invalid packets up the pipe to let the next stage decide what to do.
126 resp = outputPacket();
127 if(getComponentOpMode() & OCSD_OPFLG_PKTPROC_UNSYNC_ON_BAD_PKTS)
128 m_proc_state = WAIT_SYNC;
129 }
130 else
131 {
132 // bail out on any other error.
133 resp = OCSD_RESP_FATAL_INVALID_DATA;
134 }
135 }
136 catch(...)
137 {
138 /// vv bad at this point.
139 resp = OCSD_RESP_FATAL_SYS_ERR;
140 ocsdError fatal = ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_FAIL,m_packet_index,m_config->getTraceID());
141 fatal.setMessage("Unknown System Error decoding trace.");
142 LogError(fatal);
143 }
144 }
145
146 *numBytesProcessed = m_data_in_used;
147 return resp;
148
149 }
150
onEOT()151 ocsd_datapath_resp_t TrcPktProcStm::onEOT()
152 {
153 ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
154 if(m_num_nibbles > 0) // there is a partial packet in flight
155 {
156 m_curr_packet.updateErrType(STM_PKT_INCOMPLETE_EOT); // re mark as incomplete
157 resp = outputPacket();
158 }
159 return resp;
160 }
161
onReset()162 ocsd_datapath_resp_t TrcPktProcStm::onReset()
163 {
164 initProcessorState();
165 return OCSD_RESP_CONT;
166 }
167
onFlush()168 ocsd_datapath_resp_t TrcPktProcStm::onFlush()
169 {
170 // packet processor never holds on to flushable data (may have partial packet,
171 // but any full packets are immediately sent)
172 return OCSD_RESP_CONT;
173 }
174
onProtocolConfig()175 ocsd_err_t TrcPktProcStm::onProtocolConfig()
176 {
177 return OCSD_OK; // nothing to do on config for this processor
178 }
179
isBadPacket() const180 const bool TrcPktProcStm::isBadPacket() const
181 {
182 return m_curr_packet.isBadPacket();
183 }
184
outputPacket()185 ocsd_datapath_resp_t TrcPktProcStm::outputPacket()
186 {
187 ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
188 resp = outputOnAllInterfaces(m_packet_index,&m_curr_packet,&m_curr_packet.type,m_packet_data);
189 m_packet_data.clear();
190 initNextPacket();
191 if(m_nibble_2nd_valid)
192 savePacketByte(m_nibble_2nd << 4); // put the unused nibble back on to the data stack and pad for output next time.
193 m_proc_state = m_bStreamSync ? PROC_HDR : WAIT_SYNC;
194 return resp;
195 }
196
throwBadSequenceError(const char * pszMessage)197 void TrcPktProcStm::throwBadSequenceError(const char *pszMessage /*= ""*/)
198 {
199 m_curr_packet.updateErrType(STM_PKT_BAD_SEQUENCE);
200 throw ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_BAD_PACKET_SEQ,m_packet_index,this->m_config->getTraceID(),pszMessage);
201 }
202
throwReservedHdrError(const char * pszMessage)203 void TrcPktProcStm::throwReservedHdrError(const char *pszMessage /*= ""*/)
204 {
205 m_curr_packet.setPacketType(STM_PKT_RESERVED,false);
206 throw ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_INVALID_PCKT_HDR,m_packet_index,this->m_config->getTraceID(),pszMessage);
207 }
208
209 // processor / packet init
210 // ************************
211
initProcessorState()212 void TrcPktProcStm::initProcessorState()
213 {
214 // clear any state that persists between packets
215 setProcUnsynced();
216 clearSyncCount();
217 m_curr_packet.initStartState();
218 m_nibble_2nd_valid = false;
219 initNextPacket();
220 m_bWaitSyncSaveSuppressed = false;
221
222 m_packet_data.clear();
223 }
224
initNextPacket()225 void TrcPktProcStm::initNextPacket()
226 {
227 // clear state that is unique to each packet
228 m_bNeedsTS = false;
229 m_bIsMarker = false;
230 m_num_nibbles = 0;
231 m_num_data_nibbles = 0;
232 m_curr_packet.initNextPacket();
233 }
234
235 // search remaining buffer for a start of sync or full sync packet
waitForSync(const ocsd_trc_index_t blk_st_index)236 void TrcPktProcStm::waitForSync(const ocsd_trc_index_t blk_st_index)
237 {
238 bool bGotData = true;
239 uint32_t start_offset = m_data_in_used; // record the offset into the buffer at start of this fn.
240
241 // input conditions:
242 // out of sync - either at start of input stream, or due to bad packet.
243 // m_data_in_used -> bytes already processed
244 // m_sync_start -> seen potential start of sync in current stream
245
246 // set a packet index for the start of the data
247 m_packet_index = blk_st_index + m_data_in_used;
248 m_num_nibbles = m_is_sync ? m_num_F_nibbles + 1 : m_num_F_nibbles; // sending unsync data may have cleared down num_nibbles.
249
250 m_bWaitSyncSaveSuppressed = true; // no need to save bytes until we want to send data.
251
252 while(bGotData && !m_is_sync)
253 {
254 bGotData = readNibble(); // read until we have a sync or run out of data
255 }
256
257 m_bWaitSyncSaveSuppressed = false;
258
259 // no data from first attempt to read
260 if(m_num_nibbles == 0)
261 return;
262
263 // we have found a sync or run out of data
264 // five possible scenarios
265 // a) all data none sync data.
266 // b) some none sync data + start of sync sequence
267 // c) some none sync data + full sync sequence in this frame
268 // d) full sync sequence @ start of this frame followed by ???
269 // e) completion of sync sequence in this frame (from b)).
270
271 if(!bGotData || m_num_nibbles > 22)
272 {
273 // for a), b), c) send the none sync data then re-enter
274 // if out of data, or sync with some previous data, this is sent as unsynced.
275
276 m_curr_packet.setPacketType(STM_PKT_NOTSYNC,false);
277 if(mon_in_use.usingMonitor())
278 {
279 uint8_t nibbles_to_send = m_num_nibbles - (m_is_sync ? 22 : m_num_F_nibbles);
280 uint8_t bytes_to_send = (nibbles_to_send / 2) + (nibbles_to_send % 2);
281 for(uint8_t i = 0; i < bytes_to_send; i++)
282 savePacketByte(m_p_data_in[start_offset+i]);
283 }
284
285 // if we have found a sync then we will re-enter this function with no pre data,
286 // but the found flags set.
287 }
288 else
289 {
290 // send the async packet
291 m_curr_packet.setPacketType(STM_PKT_ASYNC,false);
292 m_bStreamSync = true; // mark the stream as synchronised
293 clearSyncCount();
294 m_packet_index = m_sync_index;
295 if(mon_in_use.usingMonitor())
296 {
297 // we may not have the full sync packet still in the local buffer so synthesise it.
298 for(int i = 0; i < 10; i++)
299 savePacketByte(0xFF);
300 savePacketByte(0x0F);
301 }
302 }
303 sendPacket(); // mark packet for sending
304 }
305
306 // packet processing routines
307 // ************************
308 // 1 nibble opcodes
stmPktReserved()309 void TrcPktProcStm::stmPktReserved()
310 {
311 uint16_t bad_opcode = (uint16_t)m_nibble;
312 m_curr_packet.setD16Payload(bad_opcode);
313 throwReservedHdrError("STM: Unsupported or Reserved STPv2 Header");
314 }
315
stmPktNull()316 void TrcPktProcStm::stmPktNull()
317 {
318 m_curr_packet.setPacketType(STM_PKT_NULL,false);
319 if(m_bNeedsTS)
320 {
321 m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
322 (this->*m_pCurrPktFn)();
323 }
324 else
325 {
326 sendPacket();
327 }
328 }
329
stmPktNullTS()330 void TrcPktProcStm::stmPktNullTS()
331 {
332 pktNeedsTS();
333 m_pCurrPktFn = &TrcPktProcStm::stmPktNull;
334 (this->*m_pCurrPktFn)();
335 }
336
stmPktM8()337 void TrcPktProcStm::stmPktM8()
338 {
339 if(m_num_nibbles == 1) // 1st nibble - header - set type
340 m_curr_packet.setPacketType(STM_PKT_M8,false);
341
342 stmExtractVal8(3);
343 if(m_num_nibbles == 3)
344 {
345 m_curr_packet.setMaster(m_val8);
346 sendPacket();
347 }
348 }
349
stmPktMERR()350 void TrcPktProcStm::stmPktMERR()
351 {
352 if(m_num_nibbles == 1) // 1st nibble - header - set type
353 m_curr_packet.setPacketType(STM_PKT_MERR,false);
354
355 stmExtractVal8(3);
356 if(m_num_nibbles == 3)
357 {
358 m_curr_packet.setChannel(0,false); // MERR resets channel for current master to 0.
359 m_curr_packet.setD8Payload(m_val8);
360 sendPacket();
361 }
362
363 }
364
stmPktC8()365 void TrcPktProcStm::stmPktC8()
366 {
367 if(m_num_nibbles == 1) // 1st nibble - header - set type
368 m_curr_packet.setPacketType(STM_PKT_C8,false);
369 stmExtractVal8(3);
370 if(m_num_nibbles == 3)
371 {
372 m_curr_packet.setChannel((uint16_t)m_val8,true);
373 sendPacket();
374 }
375 }
376
stmPktD4()377 void TrcPktProcStm::stmPktD4()
378 {
379 if(m_num_nibbles == 1) // 1st nibble - header - set type
380 {
381 m_curr_packet.setPacketType(STM_PKT_D4,m_bIsMarker);
382 m_num_data_nibbles = 2; // need 2 nibbles to complete data
383 }
384
385 if(m_num_nibbles != m_num_data_nibbles)
386 {
387 if(readNibble())
388 {
389 m_curr_packet.setD4Payload(m_nibble);
390 if(m_bNeedsTS)
391 {
392 m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
393 (this->*m_pCurrPktFn)();
394 }
395 else
396 sendPacket();
397 }
398 }
399 }
400
stmPktD8()401 void TrcPktProcStm::stmPktD8()
402 {
403 if(m_num_nibbles == 1) // 1st nibble - header - set type
404 {
405 m_curr_packet.setPacketType(STM_PKT_D8,m_bIsMarker);
406 m_num_data_nibbles = 3; // need 3 nibbles in total to complete data
407 }
408
409 stmExtractVal8(m_num_data_nibbles);
410 if(m_num_nibbles == m_num_data_nibbles)
411 {
412 m_curr_packet.setD8Payload(m_val8);
413 if(m_bNeedsTS)
414 {
415 m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
416 (this->*m_pCurrPktFn)();
417 }
418 else
419 {
420 sendPacket();
421 }
422 }
423 }
424
stmPktD16()425 void TrcPktProcStm::stmPktD16()
426 {
427 if(m_num_nibbles == 1) // 1st nibble - header - set type
428 {
429 m_curr_packet.setPacketType(STM_PKT_D16,m_bIsMarker);
430 m_num_data_nibbles = 5;
431 }
432
433 stmExtractVal16(m_num_data_nibbles);
434 if(m_num_nibbles == m_num_data_nibbles)
435 {
436 m_curr_packet.setD16Payload(m_val16);
437 if(m_bNeedsTS)
438 {
439 m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
440 (this->*m_pCurrPktFn)();
441 }
442 else
443 {
444 sendPacket();
445 }
446 }
447 }
448
stmPktD32()449 void TrcPktProcStm::stmPktD32()
450 {
451 if(m_num_nibbles == 1) // 1st nibble - header - set type
452 {
453 m_curr_packet.setPacketType(STM_PKT_D32,m_bIsMarker);
454 m_num_data_nibbles = 9;
455 }
456
457 stmExtractVal32(m_num_data_nibbles);
458 if(m_num_nibbles == m_num_data_nibbles)
459 {
460 m_curr_packet.setD32Payload(m_val32);
461 if(m_bNeedsTS)
462 {
463 m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
464 (this->*m_pCurrPktFn)();
465 }
466 else
467 {
468 sendPacket();
469 }
470 }
471 }
472
stmPktD64()473 void TrcPktProcStm::stmPktD64()
474 {
475 if(m_num_nibbles == 1) // 1st nibble - header - set type
476 {
477 m_curr_packet.setPacketType(STM_PKT_D64,m_bIsMarker);
478 m_num_data_nibbles = 17;
479 }
480
481 stmExtractVal64(m_num_data_nibbles);
482 if(m_num_nibbles == m_num_data_nibbles)
483 {
484 m_curr_packet.setD64Payload(m_val64);
485 if(m_bNeedsTS)
486 {
487 m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
488 (this->*m_pCurrPktFn)();
489 }
490 else
491 {
492 sendPacket();
493 }
494 }
495 }
496
stmPktD4MTS()497 void TrcPktProcStm::stmPktD4MTS()
498 {
499 pktNeedsTS();
500 m_bIsMarker = true;
501 m_pCurrPktFn = &TrcPktProcStm::stmPktD4;
502 (this->*m_pCurrPktFn)();
503 }
504
stmPktD8MTS()505 void TrcPktProcStm::stmPktD8MTS()
506 {
507 pktNeedsTS();
508 m_bIsMarker = true;
509 m_pCurrPktFn = &TrcPktProcStm::stmPktD8;
510 (this->*m_pCurrPktFn)();
511 }
512
stmPktD16MTS()513 void TrcPktProcStm::stmPktD16MTS()
514 {
515 pktNeedsTS();
516 m_bIsMarker = true;
517 m_pCurrPktFn = &TrcPktProcStm::stmPktD16;
518 (this->*m_pCurrPktFn)();
519 }
520
stmPktD32MTS()521 void TrcPktProcStm::stmPktD32MTS()
522 {
523 pktNeedsTS();
524 m_bIsMarker = true;
525 m_pCurrPktFn = &TrcPktProcStm::stmPktD32;
526 (this->*m_pCurrPktFn)();
527 }
528
stmPktD64MTS()529 void TrcPktProcStm::stmPktD64MTS()
530 {
531 pktNeedsTS();
532 m_bIsMarker = true;
533 m_pCurrPktFn = &TrcPktProcStm::stmPktD64;
534 (this->*m_pCurrPktFn)();
535 }
536
stmPktFlagTS()537 void TrcPktProcStm::stmPktFlagTS()
538 {
539 pktNeedsTS();
540 m_curr_packet.setPacketType(STM_PKT_FLAG,false);
541 m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
542 (this->*m_pCurrPktFn)();
543 }
544
stmPktFExt()545 void TrcPktProcStm::stmPktFExt()
546 {
547 // no type, look at the next nibble
548 if(readNibble())
549 {
550 // switch in 2N function
551 m_pCurrPktFn = m_2N_ops[m_nibble];
552 (this->*m_pCurrPktFn)();
553 }
554 }
555
556 // ************************
557 // 2 nibble opcodes 0xFn
stmPktReservedFn()558 void TrcPktProcStm::stmPktReservedFn()
559 {
560 uint16_t bad_opcode = 0x00F;
561 bad_opcode |= ((uint16_t)m_nibble) << 4;
562 m_curr_packet.setD16Payload(bad_opcode);
563 throwReservedHdrError("STM: Unsupported or Reserved STPv2 Header");
564 }
565
stmPktF0Ext()566 void TrcPktProcStm::stmPktF0Ext()
567 {
568 // no type yet, look at the next nibble
569 if(readNibble())
570 {
571 // switch in 3N function
572 m_pCurrPktFn = m_3N_ops[m_nibble];
573 (this->*m_pCurrPktFn)();
574 }
575 }
576
stmPktGERR()577 void TrcPktProcStm::stmPktGERR()
578 {
579 if(m_num_nibbles == 2) // 2nd nibble - header - set type
580 m_curr_packet.setPacketType(STM_PKT_GERR,false);
581 stmExtractVal8(4);
582 if(m_num_nibbles == 4)
583 {
584 m_curr_packet.setD8Payload(m_val8);
585 m_curr_packet.setMaster(0); // GERR sets current master to 0.
586 sendPacket();
587 }
588 }
589
stmPktC16()590 void TrcPktProcStm::stmPktC16()
591 {
592 if(m_num_nibbles == 2) // 2nd nibble - header - set type
593 m_curr_packet.setPacketType(STM_PKT_C16,false);
594 stmExtractVal16(6);
595 if(m_num_nibbles == 6)
596 {
597 m_curr_packet.setChannel(m_val16,false);
598 sendPacket();
599 }
600 }
601
stmPktD4TS()602 void TrcPktProcStm::stmPktD4TS()
603 {
604 pktNeedsTS();
605 m_curr_packet.setPacketType(STM_PKT_D4,false); // 2nd nibble, set type here
606 m_num_data_nibbles = 3; // one more nibble for data
607 m_pCurrPktFn = &TrcPktProcStm::stmPktD4;
608 (this->*m_pCurrPktFn)();
609 }
610
stmPktD8TS()611 void TrcPktProcStm::stmPktD8TS()
612 {
613 pktNeedsTS();
614 m_curr_packet.setPacketType(STM_PKT_D8,false); // 2nd nibble, set type here
615 m_num_data_nibbles = 4;
616 m_pCurrPktFn = &TrcPktProcStm::stmPktD8;
617 (this->*m_pCurrPktFn)();
618 }
619
stmPktD16TS()620 void TrcPktProcStm::stmPktD16TS()
621 {
622 pktNeedsTS();
623 m_curr_packet.setPacketType(STM_PKT_D16,false); // 2nd nibble, set type here
624 m_num_data_nibbles = 6;
625 m_pCurrPktFn = &TrcPktProcStm::stmPktD16;
626 (this->*m_pCurrPktFn)();
627 }
628
stmPktD32TS()629 void TrcPktProcStm::stmPktD32TS()
630 {
631 pktNeedsTS();
632 m_curr_packet.setPacketType(STM_PKT_D32,false); // 2nd nibble, set type here
633 m_num_data_nibbles = 10;
634 m_pCurrPktFn = &TrcPktProcStm::stmPktD32;
635 (this->*m_pCurrPktFn)();
636 }
637
stmPktD64TS()638 void TrcPktProcStm::stmPktD64TS()
639 {
640 pktNeedsTS();
641 m_curr_packet.setPacketType(STM_PKT_D64,false); // 2nd nibble, set type here
642 m_num_data_nibbles = 18;
643 m_pCurrPktFn = &TrcPktProcStm::stmPktD64;
644 (this->*m_pCurrPktFn)();
645 }
646
stmPktD4M()647 void TrcPktProcStm::stmPktD4M()
648 {
649 m_curr_packet.setPacketType(STM_PKT_D4,true); // 2nd nibble, set type here
650 m_num_data_nibbles = 3; // one more nibble for data
651 m_pCurrPktFn = &TrcPktProcStm::stmPktD4;
652 (this->*m_pCurrPktFn)();
653 }
654
stmPktD8M()655 void TrcPktProcStm::stmPktD8M()
656 {
657 m_curr_packet.setPacketType(STM_PKT_D8,true); // 2nd nibble, set type here
658 m_num_data_nibbles = 4;
659 m_pCurrPktFn = &TrcPktProcStm::stmPktD8;
660 (this->*m_pCurrPktFn)();
661 }
662
stmPktD16M()663 void TrcPktProcStm::stmPktD16M()
664 {
665 m_curr_packet.setPacketType(STM_PKT_D16,true);
666 m_num_data_nibbles = 6;
667 m_pCurrPktFn = &TrcPktProcStm::stmPktD16;
668 (this->*m_pCurrPktFn)();
669 }
670
stmPktD32M()671 void TrcPktProcStm::stmPktD32M()
672 {
673 m_curr_packet.setPacketType(STM_PKT_D32,true);
674 m_num_data_nibbles = 10;
675 m_pCurrPktFn = &TrcPktProcStm::stmPktD32;
676 (this->*m_pCurrPktFn)();
677 }
678
stmPktD64M()679 void TrcPktProcStm::stmPktD64M()
680 {
681 m_curr_packet.setPacketType(STM_PKT_D64,true);
682 m_num_data_nibbles = 18;
683 m_pCurrPktFn = &TrcPktProcStm::stmPktD64;
684 (this->*m_pCurrPktFn)();
685 }
686
stmPktFlag()687 void TrcPktProcStm::stmPktFlag()
688 {
689 m_curr_packet.setPacketType(STM_PKT_FLAG,false);
690 sendPacket();
691 }
692
693 // ************************
694 // 3 nibble opcodes 0xF0n
stmPktReservedF0n()695 void TrcPktProcStm::stmPktReservedF0n()
696 {
697 uint16_t bad_opcode = 0x00F;
698 bad_opcode |= ((uint16_t)m_nibble) << 8;
699 m_curr_packet.setD16Payload(bad_opcode);
700 throwReservedHdrError("STM: Unsupported or Reserved STPv2 Header");
701 }
702
stmPktVersion()703 void TrcPktProcStm::stmPktVersion()
704 {
705 if(m_num_nibbles == 3)
706 m_curr_packet.setPacketType(STM_PKT_VERSION,false);
707
708 if(readNibble())
709 {
710 m_curr_packet.setD8Payload(m_nibble); // record the version number
711 switch(m_nibble)
712 {
713 case 3:
714 m_curr_packet.onVersionPkt(STM_TS_NATBINARY); break;
715 case 4:
716 m_curr_packet.onVersionPkt(STM_TS_GREY); break;
717 default:
718 // not a version we support.
719 throwBadSequenceError("STM VERSION packet : unrecognised version number.");
720 }
721 sendPacket();
722 }
723 }
724
stmPktTrigger()725 void TrcPktProcStm::stmPktTrigger()
726 {
727 if(m_num_nibbles == 3)
728 m_curr_packet.setPacketType(STM_PKT_TRIG,false);
729 stmExtractVal8(5);
730 if(m_num_nibbles == 5)
731 {
732 m_curr_packet.setD8Payload(m_val8);
733 if(m_bNeedsTS)
734 {
735 m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
736 (this->*m_pCurrPktFn)();
737 }
738 else
739 {
740 sendPacket();
741 }
742 }
743 }
744
stmPktTriggerTS()745 void TrcPktProcStm::stmPktTriggerTS()
746 {
747 pktNeedsTS();
748 m_pCurrPktFn = &TrcPktProcStm::stmPktTrigger;
749 (this->*m_pCurrPktFn)();
750 }
751
stmPktFreq()752 void TrcPktProcStm::stmPktFreq()
753 {
754 if(m_num_nibbles == 3)
755 {
756 m_curr_packet.setPacketType(STM_PKT_FREQ,false);
757 m_val32 = 0;
758 }
759 stmExtractVal32(11);
760 if(m_num_nibbles == 11)
761 {
762 m_curr_packet.setD32Payload(m_val32);
763 sendPacket();
764 }
765 }
766
stmPktASync()767 void TrcPktProcStm::stmPktASync()
768 {
769 // 2 nibbles - 0xFF - must be an async or error.
770 bool bCont = true;
771 while(bCont)
772 {
773 bCont = readNibble();
774 if(bCont)
775 {
776 if(m_is_sync)
777 {
778 bCont = false; // stop reading nibbles
779 m_bStreamSync = true; // mark stream in sync
780 m_curr_packet.setPacketType(STM_PKT_ASYNC,false);
781 clearSyncCount();
782 sendPacket();
783 }
784 else if(!m_sync_start) // no longer valid sync packet
785 {
786 throwBadSequenceError("STM: Invalid ASYNC sequence");
787 }
788 }
789 }
790 }
791
792 // ************************
793 // general data processing
794
795 // return false if no more data
796 // in an STM byte, 3:0 is 1st nibble in protocol order, 7:4 is 2nd nibble.
readNibble()797 bool TrcPktProcStm::readNibble()
798 {
799 bool dataFound = true;
800 if(m_nibble_2nd_valid)
801 {
802 m_nibble = m_nibble_2nd;
803 m_nibble_2nd_valid = false;
804 m_num_nibbles++;
805 checkSyncNibble();
806 }
807 else if(m_data_in_used < m_data_in_size )
808 {
809 m_nibble = m_p_data_in[m_data_in_used++];
810 savePacketByte(m_nibble);
811 m_nibble_2nd = (m_nibble >> 4) & 0xF;
812 m_nibble_2nd_valid = true;
813 m_nibble &= 0xF;
814 m_num_nibbles++;
815 checkSyncNibble();
816 }
817 else
818 dataFound = false; // no data available
819 return dataFound;
820 }
821
pktNeedsTS()822 void TrcPktProcStm::pktNeedsTS()
823 {
824 m_bNeedsTS = true;
825 m_req_ts_nibbles = 0;
826 m_curr_ts_nibbles = 0;
827 m_ts_update_value = 0;
828 m_ts_req_set = false;
829 }
830
stmExtractTS()831 void TrcPktProcStm::stmExtractTS()
832 {
833 if(!m_ts_req_set)
834 {
835 if(readNibble())
836 {
837 m_req_ts_nibbles = m_nibble;
838 if(m_nibble == 0xD)
839 m_req_ts_nibbles = 14;
840 else if(m_nibble == 0xE)
841 m_req_ts_nibbles = 16;
842
843 if(m_nibble == 0xF)
844 throwBadSequenceError("STM: Invalid timestamp size 0xF");
845 m_ts_req_set = true;
846 }
847 }
848
849 if(m_ts_req_set)
850 {
851 // if we do not have all the nibbles for the TS, get some...
852 if(m_req_ts_nibbles != m_curr_ts_nibbles)
853 {
854 // extract the correct amount of nibbles for the ts value.
855 bool bCont = true;
856 while(bCont && (m_curr_ts_nibbles < m_req_ts_nibbles))
857 {
858 bCont = readNibble();
859 if(bCont)
860 {
861 m_ts_update_value <<= 4;
862 m_ts_update_value |= m_nibble;
863 m_curr_ts_nibbles++;
864 }
865 }
866 }
867
868 // at this point we have the correct amount of nibbles, or have run out of data to process.
869 if(m_req_ts_nibbles == m_curr_ts_nibbles)
870 {
871 uint8_t new_bits = m_req_ts_nibbles * 4;
872 if(m_curr_packet.getTSType() == STM_TS_GREY)
873 {
874 uint64_t gray_val = bin_to_gray(m_curr_packet.getTSVal());
875 if(new_bits == 64)
876 {
877 gray_val = m_ts_update_value;
878 }
879 else
880 {
881 uint64_t mask = (0x1ULL << new_bits) - 1;
882 gray_val &= ~mask;
883 gray_val |= m_ts_update_value & mask;
884 }
885 m_curr_packet.setTS(gray_to_bin(gray_val),new_bits);
886 }
887 else if(m_curr_packet.getTSType() == STM_TS_NATBINARY)
888 {
889 m_curr_packet.setTS(m_ts_update_value, new_bits);
890 }
891 else
892 throwBadSequenceError("STM: unknown timestamp encoding");
893
894 sendPacket();
895 }
896 }
897 }
898
899 // pass in number of nibbles needed to extract the value
stmExtractVal8(uint8_t nibbles_to_val)900 void TrcPktProcStm::stmExtractVal8(uint8_t nibbles_to_val)
901 {
902 bool bCont = true;
903 while(bCont && (m_num_nibbles < nibbles_to_val))
904 {
905 bCont = readNibble();
906 if(bCont) // got a nibble
907 {
908 m_val8 <<= 4;
909 m_val8 |= m_nibble;
910 }
911 }
912 }
913
stmExtractVal16(uint8_t nibbles_to_val)914 void TrcPktProcStm::stmExtractVal16(uint8_t nibbles_to_val)
915 {
916 bool bCont = true;
917 while(bCont && (m_num_nibbles < nibbles_to_val))
918 {
919 bCont = readNibble();
920 if(bCont) // got a nibble
921 {
922 m_val16 <<= 4;
923 m_val16 |= m_nibble;
924 }
925 }
926 }
927
stmExtractVal32(uint8_t nibbles_to_val)928 void TrcPktProcStm::stmExtractVal32(uint8_t nibbles_to_val)
929 {
930 bool bCont = true;
931 while(bCont && (m_num_nibbles < nibbles_to_val))
932 {
933 bCont = readNibble();
934 if(bCont) // got a nibble
935 {
936 m_val32 <<= 4;
937 m_val32 |= m_nibble;
938 }
939 }
940 }
941
stmExtractVal64(uint8_t nibbles_to_val)942 void TrcPktProcStm::stmExtractVal64(uint8_t nibbles_to_val)
943 {
944 bool bCont = true;
945 while(bCont && (m_num_nibbles < nibbles_to_val))
946 {
947 bCont = readNibble();
948 if(bCont) // got a nibble
949 {
950 m_val64 <<= 4;
951 m_val64 |= m_nibble;
952 }
953 }
954 }
955
bin_to_gray(uint64_t bin_value)956 uint64_t TrcPktProcStm::bin_to_gray(uint64_t bin_value)
957 {
958 uint64_t gray_value = 0;
959 gray_value = (1ull << 63) & bin_value;
960 int i = 62;
961 for (; i >= 0; i--) {
962 uint64_t gray_arg_1 = ((1ull << (i+1)) & bin_value) >> (i+1);
963 uint64_t gray_arg_2 = ((1ull << i) & bin_value) >> i;
964 gray_value |= ((gray_arg_1 ^ gray_arg_2) << i);
965 }
966 return gray_value;
967 }
968
gray_to_bin(uint64_t gray_value)969 uint64_t TrcPktProcStm::gray_to_bin(uint64_t gray_value)
970 {
971 uint64_t bin_value = 0;
972 int bin_bit = 0;
973 for (; bin_bit < 64; bin_bit++) {
974 uint8_t bit_tmp = ((1ull << bin_bit) & gray_value) >> bin_bit;
975 uint8_t gray_bit = bin_bit + 1;
976 for (; gray_bit < 64; gray_bit++)
977 bit_tmp ^= (((1ull << gray_bit) & gray_value) >> gray_bit);
978
979 bin_value |= (bit_tmp << bin_bit);
980 }
981
982 return bin_value;
983 }
984
985
buildOpTables()986 void TrcPktProcStm::buildOpTables()
987 {
988 // init all reserved
989 for(int i = 0; i < 0x10; i++)
990 {
991 m_1N_ops[i] = &TrcPktProcStm::stmPktReserved;
992 m_2N_ops[i] = &TrcPktProcStm::stmPktReservedFn;
993 m_3N_ops[i] = &TrcPktProcStm::stmPktReservedF0n;
994 }
995
996 // set the 1N operations
997 m_1N_ops[0x0] = &TrcPktProcStm::stmPktNull;
998 m_1N_ops[0x1] = &TrcPktProcStm::stmPktM8;
999 m_1N_ops[0x2] = &TrcPktProcStm::stmPktMERR;
1000 m_1N_ops[0x3] = &TrcPktProcStm::stmPktC8;
1001 m_1N_ops[0x4] = &TrcPktProcStm::stmPktD8;
1002 m_1N_ops[0x5] = &TrcPktProcStm::stmPktD16;
1003 m_1N_ops[0x6] = &TrcPktProcStm::stmPktD32;
1004 m_1N_ops[0x7] = &TrcPktProcStm::stmPktD64;
1005 m_1N_ops[0x8] = &TrcPktProcStm::stmPktD8MTS;
1006 m_1N_ops[0x9] = &TrcPktProcStm::stmPktD16MTS;
1007 m_1N_ops[0xA] = &TrcPktProcStm::stmPktD32MTS;
1008 m_1N_ops[0xB] = &TrcPktProcStm::stmPktD64MTS;
1009 m_1N_ops[0xC] = &TrcPktProcStm::stmPktD4;
1010 m_1N_ops[0xD] = &TrcPktProcStm::stmPktD4MTS;
1011 m_1N_ops[0xE] = &TrcPktProcStm::stmPktFlagTS;
1012 m_1N_ops[0xF] = &TrcPktProcStm::stmPktFExt;
1013
1014 // set the 2N operations 0xFn
1015 m_2N_ops[0x0] = &TrcPktProcStm::stmPktF0Ext;
1016 // 0x1 unused in CS STM
1017 m_2N_ops[0x2] = &TrcPktProcStm::stmPktGERR;
1018 m_2N_ops[0x3] = &TrcPktProcStm::stmPktC16;
1019 m_2N_ops[0x4] = &TrcPktProcStm::stmPktD8TS;
1020 m_2N_ops[0x5] = &TrcPktProcStm::stmPktD16TS;
1021 m_2N_ops[0x6] = &TrcPktProcStm::stmPktD32TS;
1022 m_2N_ops[0x7] = &TrcPktProcStm::stmPktD64TS;
1023 m_2N_ops[0x8] = &TrcPktProcStm::stmPktD8M;
1024 m_2N_ops[0x9] = &TrcPktProcStm::stmPktD16M;
1025 m_2N_ops[0xA] = &TrcPktProcStm::stmPktD32M;
1026 m_2N_ops[0xB] = &TrcPktProcStm::stmPktD64M;
1027 m_2N_ops[0xC] = &TrcPktProcStm::stmPktD4TS;
1028 m_2N_ops[0xD] = &TrcPktProcStm::stmPktD4M;
1029 m_2N_ops[0xE] = &TrcPktProcStm::stmPktFlag;
1030 m_2N_ops[0xF] = &TrcPktProcStm::stmPktASync;
1031
1032 // set the 3N operations 0xF0n
1033 m_3N_ops[0x0] = &TrcPktProcStm::stmPktVersion;
1034 m_3N_ops[0x1] = &TrcPktProcStm::stmPktNullTS;
1035 // 0x2 .. 0x5 not used by CS STM
1036 m_3N_ops[0x6] = &TrcPktProcStm::stmPktTrigger;
1037 m_3N_ops[0x7] = &TrcPktProcStm::stmPktTriggerTS;
1038 m_3N_ops[0x8] = &TrcPktProcStm::stmPktFreq;
1039 // 0x9 .. 0xF not used by CS STM
1040
1041 }
1042
1043 /* End of File trc_pkt_proc_stm.cpp */
1044