| /* |
| * \file trc_pkt_proc_stm.cpp |
| * \brief Reference CoreSight Trace Decoder : |
| * |
| * \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 "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 RCTDL_CMPNAME_PREFIX_PKTPROC##"_STM" |
| #endif |
| |
| static const uint32_t STM_SUPPORTED_OP_FLAGS = RCTDL_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 |
| // ************************ |
| rctdl_datapath_resp_t TrcPktProcStm::processData( const rctdl_trc_index_t index, |
| const uint32_t dataBlockSize, |
| const uint8_t *pDataBlock, |
| uint32_t *numBytesProcessed) |
| { |
| rctdl_datapath_resp_t resp = RCTDL_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() && RCTDL_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(rctdlError &err) |
| { |
| LogError(err); |
| if( ((err.getErrorCode() == RCTDL_ERR_BAD_PACKET_SEQ) || |
| (err.getErrorCode() == RCTDL_ERR_INVALID_PCKT_HDR)) && |
| !(getComponentOpMode() & RCTDL_OPFLG_PKTPROC_ERR_BAD_PKTS)) |
| { |
| // send invalid packets up the pipe to let the next stage decide what to do. |
| resp = outputPacket(); |
| if(getComponentOpMode() & RCTDL_OPFLG_PKTPROC_UNSYNC_ON_BAD_PKTS) |
| m_proc_state = WAIT_SYNC; |
| } |
| else |
| { |
| // bail out on any other error. |
| resp = RCDTL_RESP_FATAL_INVALID_DATA; |
| } |
| } |
| catch(...) |
| { |
| /// vv bad at this point. |
| resp = RCTDL_RESP_FATAL_SYS_ERR; |
| rctdlError fatal = rctdlError(RCTDL_ERR_SEV_ERROR,RCTDL_ERR_FAIL,m_packet_index,m_config->getTraceID()); |
| fatal.setMessage("Unknown System Error decoding trace."); |
| LogError(fatal); |
| } |
| } |
| |
| *numBytesProcessed = m_data_in_used; |
| return resp; |
| |
| } |
| |
| rctdl_datapath_resp_t TrcPktProcStm::onEOT() |
| { |
| rctdl_datapath_resp_t resp = RCTDL_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; |
| } |
| |
| rctdl_datapath_resp_t TrcPktProcStm::onReset() |
| { |
| initProcessorState(); |
| return RCTDL_RESP_CONT; |
| } |
| |
| rctdl_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 RCTDL_RESP_CONT; |
| } |
| |
| rctdl_err_t TrcPktProcStm::onProtocolConfig() |
| { |
| return RCTDL_OK; // nothing to do on config for this processor |
| } |
| |
| const bool TrcPktProcStm::isBadPacket() const |
| { |
| return m_curr_packet.isBadPacket(); |
| } |
| |
| rctdl_datapath_resp_t TrcPktProcStm::outputPacket() |
| { |
| rctdl_datapath_resp_t resp = RCTDL_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 rctdlError(RCTDL_ERR_SEV_ERROR,RCTDL_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 rctdlError(RCTDL_ERR_SEV_ERROR,RCTDL_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 rctdl_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); |
| sendPacket(); |
| } |
| |
| 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); |
| 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.getCurrentTSVal()); |
| 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; |
| // 0x1 .. 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 */ |