Adding upstream version 1.3.3.upstream/1.3.3upstream

Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>

1 files changed, 1043 insertions, 0 deletions

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 */