summaryrefslogtreecommitdiffstats
path: root/decoder/source/stm
diff options
context:
space:
mode:
Diffstat (limited to 'decoder/source/stm')
-rw-r--r--decoder/source/stm/trc_pkt_decode_stm.cpp303
-rw-r--r--decoder/source/stm/trc_pkt_elem_stm.cpp314
-rw-r--r--decoder/source/stm/trc_pkt_proc_stm.cpp1043
3 files changed, 1660 insertions, 0 deletions
diff --git a/decoder/source/stm/trc_pkt_decode_stm.cpp b/decoder/source/stm/trc_pkt_decode_stm.cpp
new file mode 100644
index 0000000..1bb8d73
--- /dev/null
+++ b/decoder/source/stm/trc_pkt_decode_stm.cpp
@@ -0,0 +1,303 @@
+/*
+ * \file trc_pkt_decode_stm.cpp
+ * \brief OpenCSD : STM packet decoder - output generic SW trace packets.
+ *
+ * \copyright Copyright (c) 2016, ARM Limited. All Rights Reserved.
+ */
+
+/*
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * 3. Neither the name of the copyright holder nor the names of its contributors
+ * may be used to endorse or promote products derived from this software without
+ * specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
+ * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "opencsd/stm/trc_pkt_decode_stm.h"
+#define DCD_NAME "DCD_STM"
+
+TrcPktDecodeStm::TrcPktDecodeStm()
+ : TrcPktDecodeBase(DCD_NAME)
+{
+ initDecoder();
+}
+
+TrcPktDecodeStm::TrcPktDecodeStm(int instIDNum)
+ : TrcPktDecodeBase(DCD_NAME, instIDNum)
+{
+ initDecoder();
+}
+
+TrcPktDecodeStm::~TrcPktDecodeStm()
+{
+ if(m_payload_buffer)
+ delete [] m_payload_buffer;
+ m_payload_buffer = 0;
+}
+
+/* implementation packet decoding interface */
+ocsd_datapath_resp_t TrcPktDecodeStm::processPacket()
+{
+ ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
+ bool bPktDone = false;
+
+ m_decode_pass1 = true;
+
+ while(!bPktDone)
+ {
+ switch(m_curr_state)
+ {
+ case NO_SYNC:
+ m_output_elem.setType(OCSD_GEN_TRC_ELEM_NO_SYNC);
+ m_output_elem.setUnSyncEOTReason(m_unsync_info);
+ resp = outputTraceElement(m_output_elem);
+ m_curr_state = WAIT_SYNC;
+ break;
+
+ case WAIT_SYNC:
+ if(m_curr_packet_in->getPktType() == STM_PKT_ASYNC)
+ m_curr_state = DECODE_PKTS;
+ bPktDone = true;
+ break;
+
+ case DECODE_PKTS:
+ resp = decodePacket(bPktDone);
+ break;
+ }
+ }
+ return resp;
+}
+
+ocsd_datapath_resp_t TrcPktDecodeStm::onEOT()
+{
+ ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
+ m_output_elem.setType(OCSD_GEN_TRC_ELEM_EO_TRACE);
+ m_output_elem.setUnSyncEOTReason(UNSYNC_EOT);
+ resp = outputTraceElement(m_output_elem);
+ return resp;
+}
+
+ocsd_datapath_resp_t TrcPktDecodeStm::onReset()
+{
+ ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
+ m_unsync_info = UNSYNC_RESET_DECODER;
+ resetDecoder();
+ return resp;
+}
+
+ocsd_datapath_resp_t TrcPktDecodeStm::onFlush()
+{
+ ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
+ // don't currently save unsent packets so nothing to flush
+ return resp;
+}
+
+ocsd_err_t TrcPktDecodeStm::onProtocolConfig()
+{
+ if(m_config == 0)
+ return OCSD_ERR_NOT_INIT;
+
+ // static config - copy of CSID for easy reference
+ m_CSID = m_config->getTraceID();
+ return OCSD_OK;
+}
+
+void TrcPktDecodeStm::initDecoder()
+{
+ m_payload_buffer = 0;
+ m_num_pkt_correlation = 1; // fixed at single packet payload correlation - add feature later
+ m_CSID = 0;
+
+ // base decoder state - STM requires no memory and instruction decode.
+ setUsesMemAccess(false);
+ setUsesIDecode(false);
+ m_unsync_info = UNSYNC_INIT_DECODER;
+ resetDecoder();
+}
+
+void TrcPktDecodeStm::resetDecoder()
+{
+ m_curr_state = NO_SYNC;
+ m_payload_size = 0;
+ m_payload_used = 0;
+ m_payload_odd_nibble = false;
+ m_output_elem.init();
+ m_swt_packet_info.swt_flag_bits = 0; // zero out everything
+ initPayloadBuffer();
+}
+
+void TrcPktDecodeStm::initPayloadBuffer()
+{
+ // set up the payload buffer. If we are correlating indentical packets then
+ // need a buffer that is a multiple of 64bit packets.
+ // otherwise a single packet length will do.
+ if(m_payload_buffer)
+ delete [] m_payload_buffer;
+ m_payload_buffer = new (std::nothrow) uint8_t[m_num_pkt_correlation * sizeof(uint64_t)];
+}
+
+ocsd_datapath_resp_t TrcPktDecodeStm::decodePacket(bool &bPktDone)
+{
+ ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
+ bool bSendPacket = false; // flag to indicate output required.
+
+ bPktDone = true; // assume complete unless 2nd pass required.
+ m_output_elem.setType(OCSD_GEN_TRC_ELEM_SWTRACE);
+ clearSWTPerPcktInfo();
+
+ switch (m_curr_packet_in->getPktType())
+ {
+ case STM_PKT_BAD_SEQUENCE: /**< Incorrect protocol sequence */
+ case STM_PKT_RESERVED:
+ resp = OCSD_RESP_FATAL_INVALID_DATA;
+ m_unsync_info = UNSYNC_BAD_PACKET;
+ case STM_PKT_NOTSYNC:
+ resetDecoder();
+ break;
+
+ case STM_PKT_VERSION: /**< Version packet - not relevant to generic (versionless) o/p */
+ case STM_PKT_ASYNC: /**< Alignment synchronisation packet */
+ case STM_PKT_INCOMPLETE_EOT: /**< Incomplete packet flushed at end of trace. */
+ // no action required.
+ break;
+
+/* markers for valid packets*/
+ case STM_PKT_NULL: /**< Null packet */
+ if(m_curr_packet_in->isTSPkt())
+ bSendPacket = true; // forward NULL packet if associated timestamp.
+ break;
+
+ case STM_PKT_FREQ: /**< Frequency packet */
+ m_swt_packet_info.swt_frequency = 1;
+ updatePayload(bSendPacket);
+ break;
+
+ case STM_PKT_TRIG: /**< Trigger event packet. */
+ m_swt_packet_info.swt_trigger_event = 1;
+ updatePayload(bSendPacket);
+ break;
+
+ case STM_PKT_GERR: /**< Global error packet - protocol error but unknown which master had error */
+ m_swt_packet_info.swt_master_id = m_curr_packet_in->getMaster();
+ m_swt_packet_info.swt_channel_id = m_curr_packet_in->getChannel();
+ m_swt_packet_info.swt_global_err = 1;
+ m_swt_packet_info.swt_id_valid = 0;
+ updatePayload(bSendPacket);
+ break;
+
+ case STM_PKT_MERR: /**< Master error packet - current master detected an error (e.g. dropped trace) */
+ m_swt_packet_info.swt_channel_id = m_curr_packet_in->getChannel();
+ m_swt_packet_info.swt_master_err = 1;
+ updatePayload(bSendPacket);
+ break;
+
+ case STM_PKT_M8: /**< Set current master */
+ m_swt_packet_info.swt_master_id = m_curr_packet_in->getMaster();
+ m_swt_packet_info.swt_channel_id = m_curr_packet_in->getChannel(); // forced to 0
+ m_swt_packet_info.swt_id_valid = 1;
+ break;
+
+ case STM_PKT_C8: /**< Set lower 8 bits of current channel - packet proc hadnles this */
+ case STM_PKT_C16: /**< Set current channel */
+ m_swt_packet_info.swt_channel_id = m_curr_packet_in->getChannel();
+ break;
+
+ case STM_PKT_FLAG: /**< Flag packet */
+ m_swt_packet_info.swt_marker_packet = 1;
+ bSendPacket = true; // send 0 payload marker packet./
+ break;
+
+
+ case STM_PKT_D4: /**< 4 bit data payload packet */
+ case STM_PKT_D8: /**< 8 bit data payload packet */
+ case STM_PKT_D16: /**< 16 bit data payload packet */
+ case STM_PKT_D32: /**< 32 bit data payload packet */
+ case STM_PKT_D64: /**< 64 bit data payload packet */
+ updatePayload(bSendPacket);
+ break;
+
+ }
+
+ if(bSendPacket)
+ {
+ if(m_curr_packet_in->isTSPkt())
+ {
+ m_output_elem.setTS(m_curr_packet_in->getTSVal());
+ m_swt_packet_info.swt_has_timestamp = 1;
+ }
+ m_output_elem.setSWTInfo(m_swt_packet_info);
+ resp = outputTraceElement(m_output_elem);
+ }
+
+ return resp;
+}
+
+void TrcPktDecodeStm::clearSWTPerPcktInfo()
+{
+ m_swt_packet_info.swt_flag_bits &= (uint32_t)(0x0 | SWT_ID_VALID_MASK); // clear flags and current payload size (save id valid flag).
+}
+
+void TrcPktDecodeStm::updatePayload(bool &bSendPacket)
+{
+ // without buffering similar packets - this function is quite simple
+ bSendPacket = true;
+ m_swt_packet_info.swt_payload_num_packets = 1;
+
+ switch(m_curr_packet_in->getPktType())
+ {
+ case STM_PKT_D4: /**< 4 bit data payload packet */
+ m_swt_packet_info.swt_payload_pkt_bitsize = 4;
+ *(uint8_t *)m_payload_buffer = m_curr_packet_in->getD4Val();
+ break;
+
+ case STM_PKT_D8: /**< 8 bit data payload packet */
+ case STM_PKT_TRIG: /**< Trigger event packet - 8 bits. */
+ case STM_PKT_GERR: /**< error packet - 8 bits. */
+ case STM_PKT_MERR: /**< error packet - 8 bits. */
+ m_swt_packet_info.swt_payload_pkt_bitsize = 8;
+ *(uint8_t *)m_payload_buffer = m_curr_packet_in->getD8Val();
+ break;
+
+ case STM_PKT_D16: /**< 16 bit data payload packet */
+ m_swt_packet_info.swt_payload_pkt_bitsize = 16;
+ *(uint16_t *)m_payload_buffer = m_curr_packet_in->getD16Val();
+ break;
+
+ case STM_PKT_D32: /**< 32 bit data payload packet */
+ case STM_PKT_FREQ: /**< Frequency packet */
+ m_swt_packet_info.swt_payload_pkt_bitsize = 32;
+ *(uint32_t *)m_payload_buffer = m_curr_packet_in->getD32Val();
+ break;
+
+
+ case STM_PKT_D64: /**< 64 bit data payload packet */
+ m_swt_packet_info.swt_payload_pkt_bitsize = 64;
+ *(uint64_t *)m_payload_buffer = m_curr_packet_in->getD64Val();
+ break;
+ }
+ m_output_elem.setExtendedDataPtr(m_payload_buffer);
+ if (m_curr_packet_in->isMarkerPkt())
+ m_swt_packet_info.swt_marker_packet = 1;
+
+}
+
+/* End of File trc_pkt_decode_stm.cpp */
diff --git a/decoder/source/stm/trc_pkt_elem_stm.cpp b/decoder/source/stm/trc_pkt_elem_stm.cpp
new file mode 100644
index 0000000..d9adaf6
--- /dev/null
+++ b/decoder/source/stm/trc_pkt_elem_stm.cpp
@@ -0,0 +1,314 @@
+/*
+ * \file trc_pkt_elem_stm.cpp
+ * \brief OpenCSD : STM decode - packet class
+ *
+ * \copyright Copyright (c) 2015, ARM Limited. All Rights Reserved.
+ */
+
+/*
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * 3. Neither the name of the copyright holder nor the names of its contributors
+ * may be used to endorse or promote products derived from this software without
+ * specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
+ * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <sstream>
+#include <iomanip>
+#include "opencsd/stm/trc_pkt_elem_stm.h"
+
+StmTrcPacket::StmTrcPacket()
+{
+ initStartState();
+}
+
+StmTrcPacket &StmTrcPacket::operator =(const ocsd_stm_pkt *p_pkt)
+{
+ *dynamic_cast<ocsd_stm_pkt *>(this) = *p_pkt;
+ return *this;
+}
+
+void StmTrcPacket::initStartState()
+{
+ master = 0;
+ channel = 0;
+ timestamp = 0;
+ ts_type = STM_TS_UNKNOWN;
+ type = STM_PKT_NOTSYNC;
+ initNextPacket();
+}
+
+void StmTrcPacket::initNextPacket()
+{
+ err_type = STM_PKT_NO_ERR_TYPE;
+ pkt_ts_bits = 0;
+ pkt_has_marker = 0;
+ pkt_has_ts = 0;
+}
+
+void StmTrcPacket::setTS(const uint64_t ts_val, const uint8_t updatedBits)
+{
+ if(updatedBits == 64)
+ {
+ timestamp = ts_val;
+ }
+ else
+ {
+ uint64_t mask = (0x1ULL << updatedBits) - 1;
+ timestamp &= ~mask;
+ timestamp |= ts_val & mask;
+ }
+ pkt_ts_bits = updatedBits; // mark number of bits
+ pkt_has_ts = 1;
+}
+
+// printing
+void StmTrcPacket::toString(std::string &str) const
+{
+ std::string name, desc;
+ std::ostringstream oss;
+
+ pktTypeName(type,name, desc);
+ str = name + ":" + desc;
+
+ // extended information
+ switch(type)
+ {
+ case STM_PKT_INCOMPLETE_EOT:
+ case STM_PKT_BAD_SEQUENCE:
+ pktTypeName(err_type,name, desc);
+ str+= "[" + name + "]";
+ break;
+
+ case STM_PKT_VERSION:
+ oss << "; Ver=" << (uint16_t)payload.D8;
+ str+= oss.str();
+ break;
+
+ case STM_PKT_FREQ:
+ oss << "; Freq=" << std::dec << payload.D32 << "Hz";
+ str+= oss.str();
+ break;
+
+ case STM_PKT_TRIG:
+ oss << "; TrigData=0x" << std::hex << std::setw(2) << std::setfill('0') << (uint16_t)payload.D8;
+ str+= oss.str();
+ break;
+
+ case STM_PKT_M8:
+ oss << "; Master=0x" << std::hex << std::setw(2) << std::setfill('0') << (uint16_t)master;
+ str+= oss.str();
+ break;
+
+ case STM_PKT_C8:
+ case STM_PKT_C16:
+ oss << "; Chan=0x" << std::hex << std::setw(4) << std::setfill('0') << channel;
+ str+= oss.str();
+ break;
+
+ case STM_PKT_D4:
+ oss << "; Data=0x" << std::hex << std::setw(1) << (uint16_t)(payload.D8 & 0xF);
+ str+= oss.str();
+ break;
+
+ case STM_PKT_D8:
+ oss << "; Data=0x" << std::hex << std::setw(2) << std::setfill('0') << (uint16_t)payload.D8;
+ str+= oss.str();
+ break;
+
+ case STM_PKT_D16:
+ oss << "; Data=0x" << std::hex << std::setw(4) << std::setfill('0') << payload.D16;
+ str+= oss.str();
+ break;
+
+ case STM_PKT_D32:
+ oss << "; Data=0x" << std::hex << std::setw(8) << std::setfill('0') << payload.D32;
+ str+= oss.str();
+ break;
+
+ case STM_PKT_D64:
+ oss << "; Data=0x" << std::hex << std::setw(16) << std::setfill('0') << payload.D64;
+ str+= oss.str();
+ break;
+ }
+
+ if(isTSPkt())
+ {
+ std::string valStr;
+ trcPrintableElem::getValStr(valStr,64,64,timestamp,true,pkt_ts_bits);
+ str += "; TS=" + valStr;
+ }
+}
+
+void StmTrcPacket::toStringFmt(const uint32_t fmtFlags, std::string &str) const
+{
+ // no formatting for now.
+ toString(str);
+}
+
+void StmTrcPacket::pktTypeName(const ocsd_stm_pkt_type pkt_type, std::string &name, std::string &desc) const
+{
+ std::ostringstream oss_name;
+ std::ostringstream oss_desc;
+ bool addMarkerTS = false;
+
+
+ switch(pkt_type)
+ {
+ case STM_PKT_RESERVED:
+ oss_name << "RESERVED";
+ oss_desc << "Reserved Packet Header";
+ break;
+
+ case STM_PKT_NOTSYNC:
+ oss_name << "NOTSYNC";
+ oss_desc << "STM not synchronised";
+ break;
+
+ case STM_PKT_INCOMPLETE_EOT:
+ oss_name << "INCOMPLETE_EOT";
+ oss_desc << "Incomplete packet flushed at end of trace";
+ break;
+
+ case STM_PKT_NO_ERR_TYPE:
+ oss_name << "NO_ERR_TYPE";
+ oss_desc << "Error type not set";
+ break;
+
+ case STM_PKT_BAD_SEQUENCE:
+ oss_name << "BAD_SEQUENCE";
+ oss_desc << "Invalid sequence in packet";
+ break;
+
+ case STM_PKT_ASYNC:
+ oss_name << "ASYNC";
+ oss_desc << "Alignment synchronisation packet";
+ break;
+
+ case STM_PKT_VERSION:
+ oss_name << "VERSION";
+ oss_desc << "Version packet";
+ break;
+
+ case STM_PKT_FREQ:
+ oss_name << "FREQ";
+ oss_desc << "Frequency packet";
+ break;
+
+ case STM_PKT_NULL:
+ oss_name << "NULL";
+ oss_desc << "Null packet";
+ break;
+
+ case STM_PKT_TRIG:
+ oss_name << "TRIG";
+ oss_desc << "Trigger packet";
+ addMarkerTS = true;
+ break;
+
+ case STM_PKT_GERR:
+ oss_name << "GERR";
+ oss_desc << "Global Error";
+ break;
+
+ case STM_PKT_MERR:
+ oss_name << "MERR";
+ oss_desc << "Master Error";
+ break;
+
+ case STM_PKT_M8:
+ oss_name << "M8";
+ oss_desc << "Set current master";
+ break;
+
+ case STM_PKT_C8:
+ oss_name << "C8";
+ oss_desc << "Set current channel";
+ break;
+
+ case STM_PKT_C16:
+ oss_name << "C16";
+ oss_desc << "Set current channel";
+ break;
+
+ case STM_PKT_FLAG:
+ oss_name << "FLAG";
+ oss_desc << "Flag packet";
+ addMarkerTS = true;
+ break;
+
+ case STM_PKT_D4:
+ oss_name << "D4";
+ oss_desc << "4 bit data";
+ addMarkerTS = true;
+ break;
+
+ case STM_PKT_D8:
+ oss_name << "D8";
+ oss_desc << "8 bit data";
+ addMarkerTS = true;
+ break;
+
+ case STM_PKT_D16:
+ oss_name << "D16";
+ oss_desc << "16 bit data";
+ addMarkerTS = true;
+ break;
+
+ case STM_PKT_D32:
+ oss_name << "D32";
+ oss_desc << "32 bit data";
+ addMarkerTS = true;
+ break;
+
+ case STM_PKT_D64:
+ oss_name << "D64";
+ oss_desc << "64 bit data";
+ addMarkerTS = true;
+ break;
+
+ default:
+ oss_name << "UNKNOWN";
+ oss_desc << "ERROR: unknown packet type";
+ break;
+ }
+
+ if(addMarkerTS)
+ {
+ if(isMarkerPkt())
+ {
+ oss_name << "M";
+ oss_desc << " + marker";
+ }
+
+ if(isTSPkt())
+ {
+ oss_name << "TS";
+ oss_desc << " + timestamp";
+ }
+ }
+ desc = oss_desc.str();
+ name = oss_name.str();
+}
+
+
+/* End of File trc_pkt_elem_stm.cpp */
diff --git a/decoder/source/stm/trc_pkt_proc_stm.cpp b/decoder/source/stm/trc_pkt_proc_stm.cpp
new file mode 100644
index 0000000..b39a053
--- /dev/null
+++ b/decoder/source/stm/trc_pkt_proc_stm.cpp
@@ -0,0 +1,1043 @@
+/*
+ * \file trc_pkt_proc_stm.cpp
+ * \brief OpenCSD :
+ *
+ * \copyright Copyright (c) 2015, ARM Limited. All Rights Reserved.
+ */
+
+/*
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * 3. Neither the name of the copyright holder nor the names of its contributors
+ * may be used to endorse or promote products derived from this software without
+ * specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
+ * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "opencsd/stm/trc_pkt_proc_stm.h"
+
+
+// processor object construction
+// ************************
+
+#ifdef __GNUC__
+// G++ doesn't like the ## pasting
+#define STM_PKTS_NAME "PKTP_STM"
+#else
+#define STM_PKTS_NAME OCSD_CMPNAME_PREFIX_PKTPROC##"_STM"
+#endif
+
+static const uint32_t STM_SUPPORTED_OP_FLAGS = OCSD_OPFLG_PKTPROC_COMMON;
+
+TrcPktProcStm::TrcPktProcStm() : TrcPktProcBase(STM_PKTS_NAME)
+{
+ initObj();
+}
+
+TrcPktProcStm::TrcPktProcStm(int instIDNum) : TrcPktProcBase(STM_PKTS_NAME, instIDNum)
+{
+ initObj();
+}
+
+TrcPktProcStm::~TrcPktProcStm()
+{
+ getRawPacketMonAttachPt()->set_notifier(0);
+}
+
+void TrcPktProcStm::initObj()
+{
+ m_supported_op_flags = STM_SUPPORTED_OP_FLAGS;
+ initProcessorState();
+ getRawPacketMonAttachPt()->set_notifier(&mon_in_use);
+ buildOpTables();
+}
+
+// implementation packet processing interface overrides
+// ************************
+ocsd_datapath_resp_t TrcPktProcStm::processData( const ocsd_trc_index_t index,
+ const uint32_t dataBlockSize,
+ const uint8_t *pDataBlock,
+ uint32_t *numBytesProcessed)
+{
+ ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
+ m_p_data_in = pDataBlock;
+ m_data_in_size = dataBlockSize;
+ m_data_in_used = 0;
+
+ // while there is data and a continue response on the data path
+ while( dataToProcess() && OCSD_DATA_RESP_IS_CONT(resp) )
+ {
+ try
+ {
+ switch(m_proc_state)
+ {
+ case WAIT_SYNC:
+ waitForSync(index);
+ break;
+
+ case PROC_HDR:
+ m_packet_index = index + m_data_in_used;
+ if(readNibble())
+ {
+ m_proc_state = PROC_DATA; // read the header nibble, next if any has to be data
+ m_pCurrPktFn = m_1N_ops[m_nibble]; // set packet function and fall through
+ }
+ else
+ break;
+
+ case PROC_DATA:
+ (this->*m_pCurrPktFn)();
+
+ // if we have enough to send, fall through, otherwise stop
+ if(m_proc_state != SEND_PKT)
+ break;
+
+ case SEND_PKT:
+ resp = outputPacket();
+ break;
+ }
+ }
+ catch(ocsdError &err)
+ {
+ LogError(err);
+ if( ((err.getErrorCode() == OCSD_ERR_BAD_PACKET_SEQ) ||
+ (err.getErrorCode() == OCSD_ERR_INVALID_PCKT_HDR)) &&
+ !(getComponentOpMode() & OCSD_OPFLG_PKTPROC_ERR_BAD_PKTS))
+ {
+ // send invalid packets up the pipe to let the next stage decide what to do.
+ resp = outputPacket();
+ if(getComponentOpMode() & OCSD_OPFLG_PKTPROC_UNSYNC_ON_BAD_PKTS)
+ m_proc_state = WAIT_SYNC;
+ }
+ else
+ {
+ // bail out on any other error.
+ resp = OCSD_RESP_FATAL_INVALID_DATA;
+ }
+ }
+ catch(...)
+ {
+ /// vv bad at this point.
+ resp = OCSD_RESP_FATAL_SYS_ERR;
+ ocsdError fatal = ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_FAIL,m_packet_index,m_config->getTraceID());
+ fatal.setMessage("Unknown System Error decoding trace.");
+ LogError(fatal);
+ }
+ }
+
+ *numBytesProcessed = m_data_in_used;
+ return resp;
+
+}
+
+ocsd_datapath_resp_t TrcPktProcStm::onEOT()
+{
+ ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
+ if(m_num_nibbles > 0) // there is a partial packet in flight
+ {
+ m_curr_packet.updateErrType(STM_PKT_INCOMPLETE_EOT); // re mark as incomplete
+ resp = outputPacket();
+ }
+ return resp;
+}
+
+ocsd_datapath_resp_t TrcPktProcStm::onReset()
+{
+ initProcessorState();
+ return OCSD_RESP_CONT;
+}
+
+ocsd_datapath_resp_t TrcPktProcStm::onFlush()
+{
+ // packet processor never holds on to flushable data (may have partial packet,
+ // but any full packets are immediately sent)
+ return OCSD_RESP_CONT;
+}
+
+ocsd_err_t TrcPktProcStm::onProtocolConfig()
+{
+ return OCSD_OK; // nothing to do on config for this processor
+}
+
+const bool TrcPktProcStm::isBadPacket() const
+{
+ return m_curr_packet.isBadPacket();
+}
+
+ocsd_datapath_resp_t TrcPktProcStm::outputPacket()
+{
+ ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
+ resp = outputOnAllInterfaces(m_packet_index,&m_curr_packet,&m_curr_packet.type,m_packet_data);
+ m_packet_data.clear();
+ initNextPacket();
+ if(m_nibble_2nd_valid)
+ savePacketByte(m_nibble_2nd << 4); // put the unused nibble back on to the data stack and pad for output next time.
+ m_proc_state = m_bStreamSync ? PROC_HDR : WAIT_SYNC;
+ return resp;
+}
+
+void TrcPktProcStm::throwBadSequenceError(const char *pszMessage /*= ""*/)
+{
+ m_curr_packet.updateErrType(STM_PKT_BAD_SEQUENCE);
+ throw ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_BAD_PACKET_SEQ,m_packet_index,this->m_config->getTraceID(),pszMessage);
+}
+
+void TrcPktProcStm::throwReservedHdrError(const char *pszMessage /*= ""*/)
+{
+ m_curr_packet.setPacketType(STM_PKT_RESERVED,false);
+ throw ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_INVALID_PCKT_HDR,m_packet_index,this->m_config->getTraceID(),pszMessage);
+}
+
+// processor / packet init
+// ************************
+
+void TrcPktProcStm::initProcessorState()
+{
+ // clear any state that persists between packets
+ setProcUnsynced();
+ clearSyncCount();
+ m_curr_packet.initStartState();
+ m_nibble_2nd_valid = false;
+ initNextPacket();
+ m_bWaitSyncSaveSuppressed = false;
+
+ m_packet_data.clear();
+}
+
+void TrcPktProcStm::initNextPacket()
+{
+ // clear state that is unique to each packet
+ m_bNeedsTS = false;
+ m_bIsMarker = false;
+ m_num_nibbles = 0;
+ m_num_data_nibbles = 0;
+ m_curr_packet.initNextPacket();
+}
+
+// search remaining buffer for a start of sync or full sync packet
+void TrcPktProcStm::waitForSync(const ocsd_trc_index_t blk_st_index)
+{
+ bool bGotData = true;
+ uint32_t start_offset = m_data_in_used; // record the offset into the buffer at start of this fn.
+
+ // input conditions:
+ // out of sync - either at start of input stream, or due to bad packet.
+ // m_data_in_used -> bytes already processed
+ // m_sync_start -> seen potential start of sync in current stream
+
+ // set a packet index for the start of the data
+ m_packet_index = blk_st_index + m_data_in_used;
+ m_num_nibbles = m_is_sync ? m_num_F_nibbles + 1 : m_num_F_nibbles; // sending unsync data may have cleared down num_nibbles.
+
+ m_bWaitSyncSaveSuppressed = true; // no need to save bytes until we want to send data.
+
+ while(bGotData && !m_is_sync)
+ {
+ bGotData = readNibble(); // read until we have a sync or run out of data
+ }
+
+ m_bWaitSyncSaveSuppressed = false;
+
+ // no data from first attempt to read
+ if(m_num_nibbles == 0)
+ return;
+
+ // we have found a sync or run out of data
+ // five possible scenarios
+ // a) all data none sync data.
+ // b) some none sync data + start of sync sequence
+ // c) some none sync data + full sync sequence in this frame
+ // d) full sync sequence @ start of this frame followed by ???
+ // e) completion of sync sequence in this frame (from b)).
+
+ if(!bGotData || m_num_nibbles > 22)
+ {
+ // for a), b), c) send the none sync data then re-enter
+ // if out of data, or sync with some previous data, this is sent as unsynced.
+
+ m_curr_packet.setPacketType(STM_PKT_NOTSYNC,false);
+ if(mon_in_use.usingMonitor())
+ {
+ uint8_t nibbles_to_send = m_num_nibbles - (m_is_sync ? 22 : m_num_F_nibbles);
+ uint8_t bytes_to_send = (nibbles_to_send / 2) + (nibbles_to_send % 2);
+ for(uint8_t i = 0; i < bytes_to_send; i++)
+ savePacketByte(m_p_data_in[start_offset+i]);
+ }
+
+ // if we have found a sync then we will re-enter this function with no pre data,
+ // but the found flags set.
+ }
+ else
+ {
+ // send the async packet
+ m_curr_packet.setPacketType(STM_PKT_ASYNC,false);
+ m_bStreamSync = true; // mark the stream as synchronised
+ clearSyncCount();
+ m_packet_index = m_sync_index;
+ if(mon_in_use.usingMonitor())
+ {
+ // we may not have the full sync packet still in the local buffer so synthesise it.
+ for(int i = 0; i < 10; i++)
+ savePacketByte(0xFF);
+ savePacketByte(0x0F);
+ }
+ }
+ sendPacket(); // mark packet for sending
+}
+
+// packet processing routines
+// ************************
+// 1 nibble opcodes
+void TrcPktProcStm::stmPktReserved()
+{
+ uint16_t bad_opcode = (uint16_t)m_nibble;
+ m_curr_packet.setD16Payload(bad_opcode);
+ throwReservedHdrError("STM: Unsupported or Reserved STPv2 Header");
+}
+
+void TrcPktProcStm::stmPktNull()
+{
+ m_curr_packet.setPacketType(STM_PKT_NULL,false);
+ if(m_bNeedsTS)
+ {
+ m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
+ (this->*m_pCurrPktFn)();
+ }
+ else
+ {
+ sendPacket();
+ }
+}
+
+void TrcPktProcStm::stmPktNullTS()
+{
+ pktNeedsTS();
+ m_pCurrPktFn = &TrcPktProcStm::stmPktNull;
+ (this->*m_pCurrPktFn)();
+}
+
+void TrcPktProcStm::stmPktM8()
+{
+ if(m_num_nibbles == 1) // 1st nibble - header - set type
+ m_curr_packet.setPacketType(STM_PKT_M8,false);
+
+ stmExtractVal8(3);
+ if(m_num_nibbles == 3)
+ {
+ m_curr_packet.setMaster(m_val8);
+ sendPacket();
+ }
+}
+
+void TrcPktProcStm::stmPktMERR()
+{
+ if(m_num_nibbles == 1) // 1st nibble - header - set type
+ m_curr_packet.setPacketType(STM_PKT_MERR,false);
+
+ stmExtractVal8(3);
+ if(m_num_nibbles == 3)
+ {
+ m_curr_packet.setChannel(0,false); // MERR resets channel for current master to 0.
+ m_curr_packet.setD8Payload(m_val8);
+ sendPacket();
+ }
+
+}
+
+void TrcPktProcStm::stmPktC8()
+{
+ if(m_num_nibbles == 1) // 1st nibble - header - set type
+ m_curr_packet.setPacketType(STM_PKT_C8,false);
+ stmExtractVal8(3);
+ if(m_num_nibbles == 3)
+ {
+ m_curr_packet.setChannel((uint16_t)m_val8,true);
+ sendPacket();
+ }
+}
+
+void TrcPktProcStm::stmPktD4()
+{
+ if(m_num_nibbles == 1) // 1st nibble - header - set type
+ {
+ m_curr_packet.setPacketType(STM_PKT_D4,m_bIsMarker);
+ m_num_data_nibbles = 2; // need 2 nibbles to complete data
+ }
+
+ if(m_num_nibbles != m_num_data_nibbles)
+ {
+ if(readNibble())
+ {
+ m_curr_packet.setD4Payload(m_nibble);
+ if(m_bNeedsTS)
+ {
+ m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
+ (this->*m_pCurrPktFn)();
+ }
+ else
+ sendPacket();
+ }
+ }
+}
+
+void TrcPktProcStm::stmPktD8()
+{
+ if(m_num_nibbles == 1) // 1st nibble - header - set type
+ {
+ m_curr_packet.setPacketType(STM_PKT_D8,m_bIsMarker);
+ m_num_data_nibbles = 3; // need 3 nibbles in total to complete data
+ }
+
+ stmExtractVal8(m_num_data_nibbles);
+ if(m_num_nibbles == m_num_data_nibbles)
+ {
+ m_curr_packet.setD8Payload(m_val8);
+ if(m_bNeedsTS)
+ {
+ m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
+ (this->*m_pCurrPktFn)();
+ }
+ else
+ {
+ sendPacket();
+ }
+ }
+}
+
+void TrcPktProcStm::stmPktD16()
+{
+ if(m_num_nibbles == 1) // 1st nibble - header - set type
+ {
+ m_curr_packet.setPacketType(STM_PKT_D16,m_bIsMarker);
+ m_num_data_nibbles = 5;
+ }
+
+ stmExtractVal16(m_num_data_nibbles);
+ if(m_num_nibbles == m_num_data_nibbles)
+ {
+ m_curr_packet.setD16Payload(m_val16);
+ if(m_bNeedsTS)
+ {
+ m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
+ (this->*m_pCurrPktFn)();
+ }
+ else
+ {
+ sendPacket();
+ }
+ }
+}
+
+void TrcPktProcStm::stmPktD32()
+{
+ if(m_num_nibbles == 1) // 1st nibble - header - set type
+ {
+ m_curr_packet.setPacketType(STM_PKT_D32,m_bIsMarker);
+ m_num_data_nibbles = 9;
+ }
+
+ stmExtractVal32(m_num_data_nibbles);
+ if(m_num_nibbles == m_num_data_nibbles)
+ {
+ m_curr_packet.setD32Payload(m_val32);
+ if(m_bNeedsTS)
+ {
+ m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
+ (this->*m_pCurrPktFn)();
+ }
+ else
+ {
+ sendPacket();
+ }
+ }
+}
+
+void TrcPktProcStm::stmPktD64()
+{
+ if(m_num_nibbles == 1) // 1st nibble - header - set type
+ {
+ m_curr_packet.setPacketType(STM_PKT_D64,m_bIsMarker);
+ m_num_data_nibbles = 17;
+ }
+
+ stmExtractVal64(m_num_data_nibbles);
+ if(m_num_nibbles == m_num_data_nibbles)
+ {
+ m_curr_packet.setD64Payload(m_val64);
+ if(m_bNeedsTS)
+ {
+ m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
+ (this->*m_pCurrPktFn)();
+ }
+ else
+ {
+ sendPacket();
+ }
+ }
+}
+
+void TrcPktProcStm::stmPktD4MTS()
+{
+ pktNeedsTS();
+ m_bIsMarker = true;
+ m_pCurrPktFn = &TrcPktProcStm::stmPktD4;
+ (this->*m_pCurrPktFn)();
+}
+
+void TrcPktProcStm::stmPktD8MTS()
+{
+ pktNeedsTS();
+ m_bIsMarker = true;
+ m_pCurrPktFn = &TrcPktProcStm::stmPktD8;
+ (this->*m_pCurrPktFn)();
+}
+
+void TrcPktProcStm::stmPktD16MTS()
+{
+ pktNeedsTS();
+ m_bIsMarker = true;
+ m_pCurrPktFn = &TrcPktProcStm::stmPktD16;
+ (this->*m_pCurrPktFn)();
+}
+
+void TrcPktProcStm::stmPktD32MTS()
+{
+ pktNeedsTS();
+ m_bIsMarker = true;
+ m_pCurrPktFn = &TrcPktProcStm::stmPktD32;
+ (this->*m_pCurrPktFn)();
+}
+
+void TrcPktProcStm::stmPktD64MTS()
+{
+ pktNeedsTS();
+ m_bIsMarker = true;
+ m_pCurrPktFn = &TrcPktProcStm::stmPktD64;
+ (this->*m_pCurrPktFn)();
+}
+
+void TrcPktProcStm::stmPktFlagTS()
+{
+ pktNeedsTS();
+ m_curr_packet.setPacketType(STM_PKT_FLAG,false);
+ m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
+ (this->*m_pCurrPktFn)();
+}
+
+void TrcPktProcStm::stmPktFExt()
+{
+ // no type, look at the next nibble
+ if(readNibble())
+ {
+ // switch in 2N function
+ m_pCurrPktFn = m_2N_ops[m_nibble];
+ (this->*m_pCurrPktFn)();
+ }
+}
+
+// ************************
+// 2 nibble opcodes 0xFn
+void TrcPktProcStm::stmPktReservedFn()
+{
+ uint16_t bad_opcode = 0x00F;
+ bad_opcode |= ((uint16_t)m_nibble) << 4;
+ m_curr_packet.setD16Payload(bad_opcode);
+ throwReservedHdrError("STM: Unsupported or Reserved STPv2 Header");
+}
+
+void TrcPktProcStm::stmPktF0Ext()
+{
+ // no type yet, look at the next nibble
+ if(readNibble())
+ {
+ // switch in 3N function
+ m_pCurrPktFn = m_3N_ops[m_nibble];
+ (this->*m_pCurrPktFn)();
+ }
+}
+
+void TrcPktProcStm::stmPktGERR()
+{
+ if(m_num_nibbles == 2) // 2nd nibble - header - set type
+ m_curr_packet.setPacketType(STM_PKT_GERR,false);
+ stmExtractVal8(4);
+ if(m_num_nibbles == 4)
+ {
+ m_curr_packet.setD8Payload(m_val8);
+ m_curr_packet.setMaster(0); // GERR sets current master to 0.
+ sendPacket();
+ }
+}
+
+void TrcPktProcStm::stmPktC16()
+{
+ if(m_num_nibbles == 2) // 2nd nibble - header - set type
+ m_curr_packet.setPacketType(STM_PKT_C16,false);
+ stmExtractVal16(6);
+ if(m_num_nibbles == 6)
+ {
+ m_curr_packet.setChannel(m_val16,false);
+ sendPacket();
+ }
+}
+
+void TrcPktProcStm::stmPktD4TS()
+{
+ pktNeedsTS();
+ m_curr_packet.setPacketType(STM_PKT_D4,false); // 2nd nibble, set type here
+ m_num_data_nibbles = 3; // one more nibble for data
+ m_pCurrPktFn = &TrcPktProcStm::stmPktD4;
+ (this->*m_pCurrPktFn)();
+}
+
+void TrcPktProcStm::stmPktD8TS()
+{
+ pktNeedsTS();
+ m_curr_packet.setPacketType(STM_PKT_D8,false); // 2nd nibble, set type here
+ m_num_data_nibbles = 4;
+ m_pCurrPktFn = &TrcPktProcStm::stmPktD8;
+ (this->*m_pCurrPktFn)();
+}
+
+void TrcPktProcStm::stmPktD16TS()
+{
+ pktNeedsTS();
+ m_curr_packet.setPacketType(STM_PKT_D16,false); // 2nd nibble, set type here
+ m_num_data_nibbles = 6;
+ m_pCurrPktFn = &TrcPktProcStm::stmPktD16;
+ (this->*m_pCurrPktFn)();
+}
+
+void TrcPktProcStm::stmPktD32TS()
+{
+ pktNeedsTS();
+ m_curr_packet.setPacketType(STM_PKT_D32,false); // 2nd nibble, set type here
+ m_num_data_nibbles = 10;
+ m_pCurrPktFn = &TrcPktProcStm::stmPktD32;
+ (this->*m_pCurrPktFn)();
+}
+
+void TrcPktProcStm::stmPktD64TS()
+{
+ pktNeedsTS();
+ m_curr_packet.setPacketType(STM_PKT_D64,false); // 2nd nibble, set type here
+ m_num_data_nibbles = 18;
+ m_pCurrPktFn = &TrcPktProcStm::stmPktD64;
+ (this->*m_pCurrPktFn)();
+}
+
+void TrcPktProcStm::stmPktD4M()
+{
+ m_curr_packet.setPacketType(STM_PKT_D4,true); // 2nd nibble, set type here
+ m_num_data_nibbles = 3; // one more nibble for data
+ m_pCurrPktFn = &TrcPktProcStm::stmPktD4;
+ (this->*m_pCurrPktFn)();
+}
+
+void TrcPktProcStm::stmPktD8M()
+{
+ m_curr_packet.setPacketType(STM_PKT_D8,true); // 2nd nibble, set type here
+ m_num_data_nibbles = 4;
+ m_pCurrPktFn = &TrcPktProcStm::stmPktD8;
+ (this->*m_pCurrPktFn)();
+}
+
+void TrcPktProcStm::stmPktD16M()
+{
+ m_curr_packet.setPacketType(STM_PKT_D16,true);
+ m_num_data_nibbles = 6;
+ m_pCurrPktFn = &TrcPktProcStm::stmPktD16;
+ (this->*m_pCurrPktFn)();
+}
+
+void TrcPktProcStm::stmPktD32M()
+{
+ m_curr_packet.setPacketType(STM_PKT_D32,true);
+ m_num_data_nibbles = 10;
+ m_pCurrPktFn = &TrcPktProcStm::stmPktD32;
+ (this->*m_pCurrPktFn)();
+}
+
+void TrcPktProcStm::stmPktD64M()
+{
+ m_curr_packet.setPacketType(STM_PKT_D64,true);
+ m_num_data_nibbles = 18;
+ m_pCurrPktFn = &TrcPktProcStm::stmPktD64;
+ (this->*m_pCurrPktFn)();
+}
+
+void TrcPktProcStm::stmPktFlag()
+{
+ m_curr_packet.setPacketType(STM_PKT_FLAG,false);
+ sendPacket();
+}
+
+// ************************
+// 3 nibble opcodes 0xF0n
+void TrcPktProcStm::stmPktReservedF0n()
+{
+ uint16_t bad_opcode = 0x00F;
+ bad_opcode |= ((uint16_t)m_nibble) << 8;
+ m_curr_packet.setD16Payload(bad_opcode);
+ throwReservedHdrError("STM: Unsupported or Reserved STPv2 Header");
+}
+
+void TrcPktProcStm::stmPktVersion()
+{
+ if(m_num_nibbles == 3)
+ m_curr_packet.setPacketType(STM_PKT_VERSION,false);
+
+ if(readNibble())
+ {
+ m_curr_packet.setD8Payload(m_nibble); // record the version number
+ switch(m_nibble)
+ {
+ case 3:
+ m_curr_packet.onVersionPkt(STM_TS_NATBINARY); break;
+ case 4:
+ m_curr_packet.onVersionPkt(STM_TS_GREY); break;
+ default:
+ // not a version we support.
+ throwBadSequenceError("STM VERSION packet : unrecognised version number.");
+ }
+ sendPacket();
+ }
+}
+
+void TrcPktProcStm::stmPktTrigger()
+{
+ if(m_num_nibbles == 3)
+ m_curr_packet.setPacketType(STM_PKT_TRIG,false);
+ stmExtractVal8(5);
+ if(m_num_nibbles == 5)
+ {
+ m_curr_packet.setD8Payload(m_val8);
+ if(m_bNeedsTS)
+ {
+ m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
+ (this->*m_pCurrPktFn)();
+ }
+ else
+ {
+ sendPacket();
+ }
+ }
+}
+
+void TrcPktProcStm::stmPktTriggerTS()
+{
+ pktNeedsTS();
+ m_pCurrPktFn = &TrcPktProcStm::stmPktTrigger;
+ (this->*m_pCurrPktFn)();
+}
+
+void TrcPktProcStm::stmPktFreq()
+{
+ if(m_num_nibbles == 3)
+ {
+ m_curr_packet.setPacketType(STM_PKT_FREQ,false);
+ m_val32 = 0;
+ }
+ stmExtractVal32(11);
+ if(m_num_nibbles == 11)
+ {
+ m_curr_packet.setD32Payload(m_val32);
+ sendPacket();
+ }
+}
+
+void TrcPktProcStm::stmPktASync()
+{
+ // 2 nibbles - 0xFF - must be an async or error.
+ bool bCont = true;
+ while(bCont)
+ {
+ bCont = readNibble();
+ if(bCont)
+ {
+ if(m_is_sync)
+ {
+ bCont = false; // stop reading nibbles
+ m_bStreamSync = true; // mark stream in sync
+ m_curr_packet.setPacketType(STM_PKT_ASYNC,false);
+ clearSyncCount();
+ sendPacket();
+ }
+ else if(!m_sync_start) // no longer valid sync packet
+ {
+ throwBadSequenceError("STM: Invalid ASYNC sequence");
+ }
+ }
+ }
+}
+
+// ************************
+// general data processing
+
+// return false if no more data
+// in an STM byte, 3:0 is 1st nibble in protocol order, 7:4 is 2nd nibble.
+bool TrcPktProcStm::readNibble()
+{
+ bool dataFound = true;
+ if(m_nibble_2nd_valid)
+ {
+ m_nibble = m_nibble_2nd;
+ m_nibble_2nd_valid = false;
+ m_num_nibbles++;
+ checkSyncNibble();
+ }
+ else if(m_data_in_used < m_data_in_size )
+ {
+ m_nibble = m_p_data_in[m_data_in_used++];
+ savePacketByte(m_nibble);
+ m_nibble_2nd = (m_nibble >> 4) & 0xF;
+ m_nibble_2nd_valid = true;
+ m_nibble &= 0xF;
+ m_num_nibbles++;
+ checkSyncNibble();
+ }
+ else
+ dataFound = false; // no data available
+ return dataFound;
+}
+
+void TrcPktProcStm::pktNeedsTS()
+{
+ m_bNeedsTS = true;
+ m_req_ts_nibbles = 0;
+ m_curr_ts_nibbles = 0;
+ m_ts_update_value = 0;
+ m_ts_req_set = false;
+}
+
+void TrcPktProcStm::stmExtractTS()
+{
+ if(!m_ts_req_set)
+ {
+ if(readNibble())
+ {
+ m_req_ts_nibbles = m_nibble;
+ if(m_nibble == 0xD)
+ m_req_ts_nibbles = 14;
+ else if(m_nibble == 0xE)
+ m_req_ts_nibbles = 16;
+
+ if(m_nibble == 0xF)
+ throwBadSequenceError("STM: Invalid timestamp size 0xF");
+ m_ts_req_set = true;
+ }
+ }
+
+ if(m_ts_req_set)
+ {
+ // if we do not have all the nibbles for the TS, get some...
+ if(m_req_ts_nibbles != m_curr_ts_nibbles)
+ {
+ // extract the correct amount of nibbles for the ts value.
+ bool bCont = true;
+ while(bCont && (m_curr_ts_nibbles < m_req_ts_nibbles))
+ {
+ bCont = readNibble();
+ if(bCont)
+ {
+ m_ts_update_value <<= 4;
+ m_ts_update_value |= m_nibble;
+ m_curr_ts_nibbles++;
+ }
+ }
+ }
+
+ // at this point we have the correct amount of nibbles, or have run out of data to process.
+ if(m_req_ts_nibbles == m_curr_ts_nibbles)
+ {
+ uint8_t new_bits = m_req_ts_nibbles * 4;
+ if(m_curr_packet.getTSType() == STM_TS_GREY)
+ {
+ uint64_t gray_val = bin_to_gray(m_curr_packet.getTSVal());
+ if(new_bits == 64)
+ {
+ gray_val = m_ts_update_value;
+ }
+ else
+ {
+ uint64_t mask = (0x1ULL << new_bits) - 1;
+ gray_val &= ~mask;
+ gray_val |= m_ts_update_value & mask;
+ }
+ m_curr_packet.setTS(gray_to_bin(gray_val),new_bits);
+ }
+ else if(m_curr_packet.getTSType() == STM_TS_NATBINARY)
+ {
+ m_curr_packet.setTS(m_ts_update_value, new_bits);
+ }
+ else
+ throwBadSequenceError("STM: unknown timestamp encoding");
+
+ sendPacket();
+ }
+ }
+}
+
+// pass in number of nibbles needed to extract the value
+void TrcPktProcStm::stmExtractVal8(uint8_t nibbles_to_val)
+{
+ bool bCont = true;
+ while(bCont && (m_num_nibbles < nibbles_to_val))
+ {
+ bCont = readNibble();
+ if(bCont) // got a nibble
+ {
+ m_val8 <<= 4;
+ m_val8 |= m_nibble;
+ }
+ }
+}
+
+void TrcPktProcStm::stmExtractVal16(uint8_t nibbles_to_val)
+{
+ bool bCont = true;
+ while(bCont && (m_num_nibbles < nibbles_to_val))
+ {
+ bCont = readNibble();
+ if(bCont) // got a nibble
+ {
+ m_val16 <<= 4;
+ m_val16 |= m_nibble;
+ }
+ }
+}
+
+void TrcPktProcStm::stmExtractVal32(uint8_t nibbles_to_val)
+{
+ bool bCont = true;
+ while(bCont && (m_num_nibbles < nibbles_to_val))
+ {
+ bCont = readNibble();
+ if(bCont) // got a nibble
+ {
+ m_val32 <<= 4;
+ m_val32 |= m_nibble;
+ }
+ }
+}
+
+void TrcPktProcStm::stmExtractVal64(uint8_t nibbles_to_val)
+{
+ bool bCont = true;
+ while(bCont && (m_num_nibbles < nibbles_to_val))
+ {
+ bCont = readNibble();
+ if(bCont) // got a nibble
+ {
+ m_val64 <<= 4;
+ m_val64 |= m_nibble;
+ }
+ }
+}
+
+uint64_t TrcPktProcStm::bin_to_gray(uint64_t bin_value)
+{
+ uint64_t gray_value = 0;
+ gray_value = (1ull << 63) & bin_value;
+ int i = 62;
+ for (; i >= 0; i--) {
+ uint64_t gray_arg_1 = ((1ull << (i+1)) & bin_value) >> (i+1);
+ uint64_t gray_arg_2 = ((1ull << i) & bin_value) >> i;
+ gray_value |= ((gray_arg_1 ^ gray_arg_2) << i);
+ }
+ return gray_value;
+}
+
+uint64_t TrcPktProcStm::gray_to_bin(uint64_t gray_value)
+{
+ uint64_t bin_value = 0;
+ int bin_bit = 0;
+ for (; bin_bit < 64; bin_bit++) {
+ uint8_t bit_tmp = ((1ull << bin_bit) & gray_value) >> bin_bit;
+ uint8_t gray_bit = bin_bit + 1;
+ for (; gray_bit < 64; gray_bit++)
+ bit_tmp ^= (((1ull << gray_bit) & gray_value) >> gray_bit);
+
+ bin_value |= (bit_tmp << bin_bit);
+ }
+
+ return bin_value;
+}
+
+
+void TrcPktProcStm::buildOpTables()
+{
+ // init all reserved
+ for(int i = 0; i < 0x10; i++)
+ {
+ m_1N_ops[i] = &TrcPktProcStm::stmPktReserved;
+ m_2N_ops[i] = &TrcPktProcStm::stmPktReservedFn;
+ m_3N_ops[i] = &TrcPktProcStm::stmPktReservedF0n;
+ }
+
+ // set the 1N operations
+ m_1N_ops[0x0] = &TrcPktProcStm::stmPktNull;
+ m_1N_ops[0x1] = &TrcPktProcStm::stmPktM8;
+ m_1N_ops[0x2] = &TrcPktProcStm::stmPktMERR;
+ m_1N_ops[0x3] = &TrcPktProcStm::stmPktC8;
+ m_1N_ops[0x4] = &TrcPktProcStm::stmPktD8;
+ m_1N_ops[0x5] = &TrcPktProcStm::stmPktD16;
+ m_1N_ops[0x6] = &TrcPktProcStm::stmPktD32;
+ m_1N_ops[0x7] = &TrcPktProcStm::stmPktD64;
+ m_1N_ops[0x8] = &TrcPktProcStm::stmPktD8MTS;
+ m_1N_ops[0x9] = &TrcPktProcStm::stmPktD16MTS;
+ m_1N_ops[0xA] = &TrcPktProcStm::stmPktD32MTS;
+ m_1N_ops[0xB] = &TrcPktProcStm::stmPktD64MTS;
+ m_1N_ops[0xC] = &TrcPktProcStm::stmPktD4;
+ m_1N_ops[0xD] = &TrcPktProcStm::stmPktD4MTS;
+ m_1N_ops[0xE] = &TrcPktProcStm::stmPktFlagTS;
+ m_1N_ops[0xF] = &TrcPktProcStm::stmPktFExt;
+
+ // set the 2N operations 0xFn
+ m_2N_ops[0x0] = &TrcPktProcStm::stmPktF0Ext;
+ // 0x1 unused in CS STM
+ m_2N_ops[0x2] = &TrcPktProcStm::stmPktGERR;
+ m_2N_ops[0x3] = &TrcPktProcStm::stmPktC16;
+ m_2N_ops[0x4] = &TrcPktProcStm::stmPktD8TS;
+ m_2N_ops[0x5] = &TrcPktProcStm::stmPktD16TS;
+ m_2N_ops[0x6] = &TrcPktProcStm::stmPktD32TS;
+ m_2N_ops[0x7] = &TrcPktProcStm::stmPktD64TS;
+ m_2N_ops[0x8] = &TrcPktProcStm::stmPktD8M;
+ m_2N_ops[0x9] = &TrcPktProcStm::stmPktD16M;
+ m_2N_ops[0xA] = &TrcPktProcStm::stmPktD32M;
+ m_2N_ops[0xB] = &TrcPktProcStm::stmPktD64M;
+ m_2N_ops[0xC] = &TrcPktProcStm::stmPktD4TS;
+ m_2N_ops[0xD] = &TrcPktProcStm::stmPktD4M;
+ m_2N_ops[0xE] = &TrcPktProcStm::stmPktFlag;
+ m_2N_ops[0xF] = &TrcPktProcStm::stmPktASync;
+
+ // set the 3N operations 0xF0n
+ m_3N_ops[0x0] = &TrcPktProcStm::stmPktVersion;
+ m_3N_ops[0x1] = &TrcPktProcStm::stmPktNullTS;
+ // 0x2 .. 0x5 not used by CS STM
+ m_3N_ops[0x6] = &TrcPktProcStm::stmPktTrigger;
+ m_3N_ops[0x7] = &TrcPktProcStm::stmPktTriggerTS;
+ m_3N_ops[0x8] = &TrcPktProcStm::stmPktFreq;
+ // 0x9 .. 0xF not used by CS STM
+
+}
+
+/* End of File trc_pkt_proc_stm.cpp */
generated by cgit v1.2.3 (git 2.39.1) at 2024-12-22 09:23:14 +0000