1044 lines
29 KiB
C++
1044 lines
29 KiB
C++
/*
|
|
* \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 */
|