diff options
Diffstat (limited to 'decoder/source/stm/trc_pkt_proc_stm.cpp')
| -rw-r--r-- | decoder/source/stm/trc_pkt_proc_stm.cpp | 1043 |
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 */ |