diff options
Diffstat (limited to 'decoder/source/ptm/trc_pkt_proc_ptm.cpp')
| -rw-r--r-- | decoder/source/ptm/trc_pkt_proc_ptm.cpp | 1215 |
1 files changed, 1215 insertions, 0 deletions
diff --git a/decoder/source/ptm/trc_pkt_proc_ptm.cpp b/decoder/source/ptm/trc_pkt_proc_ptm.cpp new file mode 100644 index 000000000000..7c90b62e6413 --- /dev/null +++ b/decoder/source/ptm/trc_pkt_proc_ptm.cpp @@ -0,0 +1,1215 @@ +/* + * \file trc_pkt_proc_ptm.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/ptm/trc_pkt_proc_ptm.h" +#include "opencsd/ptm/trc_cmp_cfg_ptm.h" +#include "common/ocsd_error.h" + + +#ifdef __GNUC__ +// G++ doesn't like the ## pasting +#define PTM_PKTS_NAME "PKTP_PTM" +#else +// VC++ is OK +#define PTM_PKTS_NAME OCSD_CMPNAME_PREFIX_PKTPROC##"_PTM" +#endif + +TrcPktProcPtm::TrcPktProcPtm() : TrcPktProcBase(PTM_PKTS_NAME) +{ + InitProcessorState(); + BuildIPacketTable(); +} + +TrcPktProcPtm::TrcPktProcPtm(int instIDNum) : TrcPktProcBase(PTM_PKTS_NAME, instIDNum) +{ + InitProcessorState(); + BuildIPacketTable(); +} + +TrcPktProcPtm::~TrcPktProcPtm() +{ + +} + +ocsd_err_t TrcPktProcPtm::onProtocolConfig() +{ + ocsd_err_t err = OCSD_ERR_NOT_INIT; + + if(m_config != 0) + { + m_chanIDCopy = m_config->getTraceID(); + err = OCSD_OK; + } + return err; +} + +ocsd_datapath_resp_t TrcPktProcPtm::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; + uint8_t currByte = 0; + + m_dataInProcessed = 0; + + if(!checkInit()) + { + resp = OCSD_RESP_FATAL_NOT_INIT; + } + else + { + m_pDataIn = pDataBlock; + m_dataInLen = dataBlockSize; + m_block_idx = index; // index start for current block + } + + while( ( ( m_dataInProcessed < dataBlockSize) || + (( m_dataInProcessed == dataBlockSize) && (m_process_state == SEND_PKT)) ) && + OCSD_DATA_RESP_IS_CONT(resp)) + { + try + { + switch(m_process_state) + { + case WAIT_SYNC: + if(!m_waitASyncSOPkt) + { + m_curr_pkt_index = m_block_idx + m_dataInProcessed; + m_curr_packet.type = PTM_PKT_NOTSYNC; + m_bAsyncRawOp = hasRawMon(); + } + resp = waitASync(); + break; + + case PROC_HDR: + m_curr_pkt_index = m_block_idx + m_dataInProcessed; + if(readByte(currByte)) + { + m_pIPktFn = m_i_table[currByte].pptkFn; + m_curr_packet.type = m_i_table[currByte].pkt_type; + } + else + { + // sequencing error - should not get to the point where readByte + // fails and m_DataInProcessed < dataBlockSize + // throw data overflow error + throw ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_PKT_INTERP_FAIL,m_curr_pkt_index,this->m_chanIDCopy,"Data Buffer Overrun"); + } + m_process_state = PROC_DATA; + + case PROC_DATA: + (this->*m_pIPktFn)(); + break; + + case SEND_PKT: + resp = outputPacket(); + InitPacketState(); + m_process_state = PROC_HDR; + break; + } + } + catch(ocsdError &err) + { + LogError(err); + if( (err.getErrorCode() == OCSD_ERR_BAD_PACKET_SEQ) || + (err.getErrorCode() == OCSD_ERR_INVALID_PCKT_HDR)) + { + // send invalid packets up the pipe to let the next stage decide what to do. + m_process_state = SEND_PKT; + } + 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; + const ocsdError &fatal = ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_FAIL,m_curr_pkt_index,m_chanIDCopy,"Unknown System Error decoding trace."); + LogError(fatal); + } + + } + *numBytesProcessed = m_dataInProcessed; + return resp; +} + +ocsd_datapath_resp_t TrcPktProcPtm::onEOT() +{ + ocsd_datapath_resp_t err = OCSD_RESP_FATAL_NOT_INIT; + if(checkInit()) + { + err = OCSD_RESP_CONT; + if(m_currPacketData.size() > 0) + { + m_curr_packet.SetErrType(PTM_PKT_INCOMPLETE_EOT); + err = outputPacket(); + } + } + return err; +} + +ocsd_datapath_resp_t TrcPktProcPtm::onReset() +{ + ocsd_datapath_resp_t err = OCSD_RESP_FATAL_NOT_INIT; + if(checkInit()) + { + InitProcessorState(); + err = OCSD_RESP_CONT; + } + return err; +} + +ocsd_datapath_resp_t TrcPktProcPtm::onFlush() +{ + ocsd_datapath_resp_t err = OCSD_RESP_FATAL_NOT_INIT; + if(checkInit()) + { + err = OCSD_RESP_CONT; + } + return err; +} + +const bool TrcPktProcPtm::isBadPacket() const +{ + return m_curr_packet.isBadPacket(); +} + +void TrcPktProcPtm::InitPacketState() +{ + m_curr_packet.Clear(); + +} + +void TrcPktProcPtm::InitProcessorState() +{ + m_curr_packet.SetType(PTM_PKT_NOTSYNC); + m_pIPktFn = &TrcPktProcPtm::pktReserved; + m_process_state = WAIT_SYNC; + m_async_0 = 0; + m_waitASyncSOPkt = false; + m_bAsyncRawOp = false; + m_bOPNotSyncPkt = false; + + m_curr_packet.ResetState(); + InitPacketState(); +} + +const bool TrcPktProcPtm::readByte(uint8_t &currByte) +{ + bool bValidByte = false; + + if(m_dataInProcessed < m_dataInLen) + { + currByte = m_pDataIn[m_dataInProcessed++]; + m_currPacketData.push_back(currByte); + bValidByte = true; + } + return bValidByte; +} + +void TrcPktProcPtm::unReadByte() +{ + m_dataInProcessed--; + m_currPacketData.pop_back(); +} + +ocsd_datapath_resp_t TrcPktProcPtm::outputPacket() +{ + ocsd_datapath_resp_t resp = OCSD_RESP_CONT; + resp = outputOnAllInterfaces(m_curr_pkt_index,&m_curr_packet,&m_curr_packet.type,m_currPacketData); + m_currPacketData.clear(); + return resp; +} + +/*** sync and packet functions ***/ +ocsd_datapath_resp_t TrcPktProcPtm::waitASync() +{ + ocsd_datapath_resp_t resp = OCSD_RESP_CONT; + + // looking for possible patterns in input buffer:- + // a) ASYNC @ start : 00 00 00 00 00 80 + // b) unsync then async: xx xx xx xx xx xx xx xx 00 00 00 00 00 80 + // c) unsync (may have 00) xx xx xx xx 00 xx xx 00 00 00 xx xx xx xx + // d) unsync then part async: xx xx xx xx xx xx xx xx xx xx xx 00 00 00 + // e) unsync with prev part async [00 00 00] 00 xx xx xx xx xx xx xx xx [] = byte in previous input buffer + + // bytes to read before throwing an unsynced packet + #define UNSYNC_PKT_MAX 16 + static const uint8_t spare_zeros[] = { 0,0,0,0,0,0,0,0, + 0,0,0,0,0,0,0,0 }; + + bool doScan = true; + bool bSendUnsyncedData = false; + bool bHaveASync = false; + int unsynced_bytes = 0; + int unsync_scan_block_start = 0; + int pktBytesOnEntry = m_currPacketData.size(); // did we have part of a potential async last time? + + while(doScan && OCSD_DATA_RESP_IS_CONT(resp)) + { + // may have spotted the start of an async + if(m_waitASyncSOPkt == true) + { + switch(findAsync()) + { + case ASYNC: + case ASYNC_EXTRA_0: + m_process_state = SEND_PKT; + m_waitASyncSOPkt = false; + bSendUnsyncedData = true; + bHaveASync = true; + doScan = false; + break; + + case THROW_0: + // remove a bunch of 0s + unsynced_bytes += ASYNC_PAD_0_LIMIT; + m_waitASyncSOPkt = false; + m_currPacketData.erase( m_currPacketData.begin(), m_currPacketData.begin()+ASYNC_PAD_0_LIMIT); + break; + + case NOT_ASYNC: + unsynced_bytes += m_currPacketData.size(); + m_waitASyncSOPkt = false; + m_currPacketData.clear(); + break; + + case ASYNC_INCOMPLETE: + bSendUnsyncedData = true; + doScan = false; + break; + } + } + else + { + if(m_pDataIn[m_dataInProcessed++] == 0x00) + { + m_waitASyncSOPkt = true; + m_currPacketData.push_back(0); + m_async_0 = 1; + } + else + { + unsynced_bytes++; + } + } + + // may need to send some unsynced data here, either if we have enought to make it worthwhile, + // or are at the end of the buffer. + if(unsynced_bytes >= UNSYNC_PKT_MAX) + bSendUnsyncedData = true; + + if(m_dataInProcessed == m_dataInLen) + { + bSendUnsyncedData = true; + doScan = false; // no more data available - stop the scan + } + + // will send any unsynced data + if(bSendUnsyncedData && (unsynced_bytes > 0)) + { + if(m_bAsyncRawOp) + { + // there were some 0's in the packet buyffer from the last pass that are no longer in the raw buffer, + // and these turned out not to be an async + if(pktBytesOnEntry) + { + outputRawPacketToMonitor(m_curr_pkt_index,&m_curr_packet,pktBytesOnEntry,spare_zeros); + m_curr_pkt_index += pktBytesOnEntry; + } + outputRawPacketToMonitor(m_curr_pkt_index,&m_curr_packet,unsynced_bytes,m_pDataIn+unsync_scan_block_start); + } + if (!m_bOPNotSyncPkt) + { + resp = outputDecodedPacket(m_curr_pkt_index, &m_curr_packet); + m_bOPNotSyncPkt = true; + } + unsync_scan_block_start += unsynced_bytes; + m_curr_pkt_index+= unsynced_bytes; + unsynced_bytes = 0; + bSendUnsyncedData = false; + } + + // mark next packet as the ASYNC we are looking for. + if(bHaveASync) + m_curr_packet.SetType(PTM_PKT_A_SYNC); + } + + return resp; +} + +void TrcPktProcPtm::pktASync() +{ + if(m_currPacketData.size() == 1) // header byte + { + m_async_0 = 1; + } + + switch(findAsync()) + { + case ASYNC: + case ASYNC_EXTRA_0: + m_process_state = SEND_PKT; + break; + + case THROW_0: + case NOT_ASYNC: + throwMalformedPacketErr("Bad Async packet"); + break; + + case ASYNC_INCOMPLETE: + break; + + } +} + +TrcPktProcPtm::async_result_t TrcPktProcPtm::findAsync() +{ + async_result_t async_res = NOT_ASYNC; + bool bFound = false; // found non-zero byte in sequence + bool bByteAvail = true; + uint8_t currByte; + + while(!bFound && bByteAvail) + { + if(readByte(currByte)) + { + if(currByte == 0x00) + { + m_async_0++; + if(m_async_0 >= (ASYNC_PAD_0_LIMIT + ASYNC_REQ_0)) + { + bFound = true; + async_res = THROW_0; + } + } + else + { + if(currByte == 0x80) + { + if(m_async_0 == 5) + async_res = ASYNC; + else if(m_async_0 > 5) + async_res = ASYNC_EXTRA_0; + } + bFound = true; + } + } + else + { + bByteAvail = false; + async_res = ASYNC_INCOMPLETE; + } + } + return async_res; +} + +void TrcPktProcPtm::pktISync() +{ + uint8_t currByte = 0; + int pktIndex = m_currPacketData.size() - 1; + bool bGotBytes = false, validByte = true; + + if(pktIndex == 0) + { + m_numCtxtIDBytes = m_config->CtxtIDBytes(); + m_gotCtxtIDBytes = 0; + + // total bytes = 6 + ctxtID; (perhaps more later) + m_numPktBytesReq = 6 + m_numCtxtIDBytes; + } + + while(validByte && !bGotBytes) + { + if(readByte(currByte)) + { + pktIndex = m_currPacketData.size() - 1; + if(pktIndex == 5) + { + // got the info byte + int altISA = (currByte >> 2) & 0x1; + int reason = (currByte >> 5) & 0x3; + m_curr_packet.SetISyncReason((ocsd_iSync_reason)(reason)); + m_curr_packet.UpdateNS((currByte >> 3) & 0x1); + m_curr_packet.UpdateAltISA((currByte >> 2) & 0x1); + m_curr_packet.UpdateHyp((currByte >> 1) & 0x1); + + ocsd_isa isa = ocsd_isa_arm; + if(m_currPacketData[1] & 0x1) + isa = altISA ? ocsd_isa_tee : ocsd_isa_thumb2; + m_curr_packet.UpdateISA(isa); + + // check cycle count required - not if reason == 0; + m_needCycleCount = (reason != 0) ? m_config->enaCycleAcc() : false; + m_gotCycleCount = false; + m_numPktBytesReq += (m_needCycleCount ? 1 : 0); + m_gotCCBytes = 0; + + } + else if(pktIndex > 5) + { + // cycle count appears first if present + if(m_needCycleCount && !m_gotCycleCount) + { + if(pktIndex == 6) + m_gotCycleCount = (bool)((currByte & 0x40) == 0); // no cont bit, got cycle count + else + m_gotCycleCount = ((currByte & 0x80) == 0) || (pktIndex == 10); + + m_gotCCBytes++; // count the cycle count bytes for later use. + if(!m_gotCycleCount) // need more cycle count bytes + m_numPktBytesReq++; + } + // then context ID if present. + else if( m_numCtxtIDBytes > m_gotCtxtIDBytes) + { + m_gotCtxtIDBytes++; + } + } + + // check if we have enough bytes + bGotBytes = (bool)((unsigned)m_numPktBytesReq == m_currPacketData.size()); + } + else + validByte = false; // no byte available, exit. + } + + if(bGotBytes) + { + // extract address value, cycle count and ctxt id. + uint32_t cycleCount = 0; + uint32_t ctxtID = 0; + int optIdx = 6; // start index for optional elements. + + // address is always full fixed 32 bit value + uint32_t address = ((uint32_t)m_currPacketData[1]) & 0xFE; + address |= ((uint32_t)m_currPacketData[2]) << 8; + address |= ((uint32_t)m_currPacketData[3]) << 16; + address |= ((uint32_t)m_currPacketData[4]) << 24; + m_curr_packet.UpdateAddress(address,32); + + if(m_needCycleCount) + { + extractCycleCount(optIdx,cycleCount); + m_curr_packet.SetCycleCount(cycleCount); + optIdx+=m_gotCCBytes; + } + + if(m_numCtxtIDBytes) + { + extractCtxtID(optIdx,ctxtID); + m_curr_packet.UpdateContextID(ctxtID); + } + m_process_state = SEND_PKT; + } +} + +void TrcPktProcPtm::pktTrigger() +{ + m_process_state = SEND_PKT; // no payload +} + +void TrcPktProcPtm::pktWPointUpdate() +{ + bool bDone = false; + bool bBytesAvail = true; + uint8_t currByte = 0; + int byteIdx = 0; + + if(m_currPacketData.size() == 1) + { + m_gotAddrBytes = false; // flag to indicate got all needed address bytes + m_numAddrBytes = 0; // number of address bytes so far - in this case header is not part of the address + + m_gotExcepBytes = false; // mark as not got all required exception bytes thus far + m_numExcepBytes = 0; // 0 read in + + m_addrPktIsa = ocsd_isa_unknown; // not set by this packet as yet + } + + // collect all the bytes needed + while(!bDone && bBytesAvail) + { + if(readByte(currByte)) + { + byteIdx = m_currPacketData.size() - 1; + if(!m_gotAddrBytes) + { + if(byteIdx < 4) + { + // address bytes 1 - 4; + // ISA stays the same + if((currByte & 0x80) == 0x00) + { + // no further bytes + m_gotAddrBytes = true; + bDone = true; + m_gotExcepBytes = true; + } + } + else + { + // 5th address byte - determine ISA from this. + if((currByte & 0x40) == 0x00) + m_gotExcepBytes = true; // no exception bytes - mark as done + m_gotAddrBytes = true; + bDone = m_gotExcepBytes; + + m_addrPktIsa = ocsd_isa_arm; // assume ARM, but then check + if((currByte & 0x20) == 0x20) // bit 5 == 1'b1 - jazelle, bits 4 & 3 part of address. + m_addrPktIsa = ocsd_isa_jazelle; + else if((currByte & 0x30) == 0x10) // bit [5:4] == 2'b01 - thumb, bit 3 part of address. + m_addrPktIsa = ocsd_isa_thumb2; + } + m_numAddrBytes++; + } + else if(!m_gotExcepBytes) + { + // excep byte is actually a WP update byte. + m_excepAltISA = ((currByte & 0x40) == 0x40) ? 1 : 0; + m_gotExcepBytes = true; + m_numExcepBytes++; + bDone = true; + } + } + else + bBytesAvail = false; + } + + // analyse the bytes to create the packet + if(bDone) + { + // ISA for the packet + if(m_addrPktIsa == ocsd_isa_unknown) // unchanged by trace packet + m_addrPktIsa = m_curr_packet.getISA(); // same as prev + + if(m_gotExcepBytes) // may adjust according to alt ISA in exception packet + { + if((m_addrPktIsa == ocsd_isa_tee) && (m_excepAltISA == 0)) + m_addrPktIsa = ocsd_isa_thumb2; + else if((m_addrPktIsa == ocsd_isa_thumb2) && (m_excepAltISA == 1)) + m_addrPktIsa = ocsd_isa_tee; + } + m_curr_packet.UpdateISA(m_addrPktIsa); // mark ISA in packet (update changes current and prev to dectect an ISA change). + + uint8_t total_bits = 0; + uint32_t addr_val = extractAddress(1,total_bits); + m_curr_packet.UpdateAddress(addr_val,total_bits); + m_process_state = SEND_PKT; + } +} + +void TrcPktProcPtm::pktIgnore() +{ + m_process_state = SEND_PKT; // no payload +} + +void TrcPktProcPtm::pktCtxtID() +{ + int pktIndex = m_currPacketData.size() - 1; + + // if at the header, determine how many more bytes we need. + if(pktIndex == 0) + { + m_numCtxtIDBytes = m_config->CtxtIDBytes(); + m_gotCtxtIDBytes = 0; + } + + // read the necessary ctxtID bytes from the stream + bool bGotBytes = false, bytesAvail = true; + uint32_t ctxtID = 0; + + bGotBytes = m_numCtxtIDBytes == m_gotCtxtIDBytes; + while(!bGotBytes & bytesAvail) + { + bytesAvail = readByte(); + if(bytesAvail) + m_gotCtxtIDBytes++; + bGotBytes = m_numCtxtIDBytes == m_gotCtxtIDBytes; + } + + if(bGotBytes) + { + if(m_numCtxtIDBytes) + { + extractCtxtID(1,ctxtID); + } + m_curr_packet.UpdateContextID(ctxtID); + m_process_state = SEND_PKT; + } +} + +void TrcPktProcPtm::pktVMID() +{ + uint8_t currByte; + + // just need a single payload byte... + if(readByte(currByte)) + { + m_curr_packet.UpdateVMID(currByte); + m_process_state = SEND_PKT; + } +} + +void TrcPktProcPtm::pktAtom() +{ + uint8_t pHdr = m_currPacketData[0]; + + if(!m_config->enaCycleAcc()) + { + m_curr_packet.SetAtomFromPHdr(pHdr); + m_process_state = SEND_PKT; + } + else + { + bool bGotAllPktBytes = false, byteAvail = true; + uint8_t currByte = 0; // cycle accurate tracing -> atom + cycle count + + if(!(pHdr & 0x40)) + { + // only the header byte present + bGotAllPktBytes = true; + } + else + { + // up to 4 additional bytes of count data. + while(byteAvail && !bGotAllPktBytes) + { + if(readByte(currByte)) + { + if(!(currByte & 0x80) || (m_currPacketData.size() == 5)) + bGotAllPktBytes = true; + } + else + byteAvail = false; + } + } + + // we have all the bytes for a cycle accurate packet. + if(bGotAllPktBytes) + { + uint32_t cycleCount = 0; + extractCycleCount(0,cycleCount); + m_curr_packet.SetCycleCount(cycleCount); + m_curr_packet.SetCycleAccAtomFromPHdr(pHdr); + m_process_state = SEND_PKT; + } + } +} + +void TrcPktProcPtm::pktTimeStamp() +{ + uint8_t currByte = 0; + int pktIndex = m_currPacketData.size() - 1; + bool bGotBytes = false, byteAvail = true; + + if(pktIndex == 0) + { + m_gotTSBytes = false; + m_needCycleCount = m_config->enaCycleAcc(); + m_gotCCBytes = 0; + + // max byte buffer size for full ts packet + m_tsByteMax = m_config->TSPkt64() ? 10 : 8; + } + + while(byteAvail && !bGotBytes) + { + if(readByte(currByte)) + { + if(!m_gotTSBytes) + { + if(((currByte & 0x80) == 0) || (m_currPacketData.size() == (unsigned)m_tsByteMax)) + { + m_gotTSBytes = true; + if(!m_needCycleCount) + bGotBytes = true; + } + } + else + { + uint8_t cc_cont_mask = 0x80; + // got TS bytes, collect cycle count + if(m_gotCCBytes == 0) + cc_cont_mask = 0x40; + if((currByte & cc_cont_mask) == 0) + bGotBytes = true; + m_gotCCBytes++; + if(m_gotCCBytes == 5) + bGotBytes = true; + } + } + else + byteAvail = false; + } + + if(bGotBytes) + { + uint64_t tsVal = 0; + uint32_t cycleCount = 0; + uint8_t tsUpdateBits = 0; + int ts_end_idx = extractTS(tsVal,tsUpdateBits); + if(m_needCycleCount) + { + extractCycleCount(ts_end_idx,cycleCount); + m_curr_packet.SetCycleCount(cycleCount); + } + m_curr_packet.UpdateTimestamp(tsVal,tsUpdateBits); + m_process_state = SEND_PKT; + } +} + +void TrcPktProcPtm::pktExceptionRet() +{ + m_process_state = SEND_PKT; // no payload +} + +void TrcPktProcPtm::pktBranchAddr() +{ + uint8_t currByte = m_currPacketData[0]; + bool bDone = false; + bool bBytesAvail = true; + int byteIdx = 0; + + if(m_currPacketData.size() == 1) + { + m_gotAddrBytes = false; // flag to indicate got all needed address bytes + m_numAddrBytes = 1; // number of address bytes so far + + m_needCycleCount = m_config->enaCycleAcc(); // check if we have a cycle count + m_gotCCBytes = 0; // number of cc bytes read in so far. + + m_gotExcepBytes = false; // mark as not got all required exception bytes thus far + m_numExcepBytes = 0; // 0 read in + + m_addrPktIsa = ocsd_isa_unknown; // not set by this packet as yet + + // header is also 1st address byte + if((currByte & 0x80) == 0) // could be single byte packet + { + m_gotAddrBytes = true; + if(!m_needCycleCount) + bDone = true; // all done if no cycle count + m_gotExcepBytes = true; // cannot have exception bytes following single byte packet + } + + } + + // collect all the bytes needed + while(!bDone && bBytesAvail) + { + if(readByte(currByte)) + { + byteIdx = m_currPacketData.size() - 1; + if(!m_gotAddrBytes) + { + if(byteIdx < 4) + { + // address bytes 2 - 4; + // ISA stays the same + if((currByte & 0x80) == 0x00) + { + // no further bytes + if((currByte & 0x40) == 0x00) + m_gotExcepBytes = true; // no exception bytes - mark as done + m_gotAddrBytes = true; + bDone = m_gotExcepBytes && !m_needCycleCount; + } + } + else + { + // 5th address byte - determine ISA from this. + if((currByte & 0x40) == 0x00) + m_gotExcepBytes = true; // no exception bytes - mark as done + m_gotAddrBytes = true; + bDone = m_gotExcepBytes && !m_needCycleCount; + + m_addrPktIsa = ocsd_isa_arm; // assume ARM, but then check + if((currByte & 0x20) == 0x20) // bit 5 == 1'b1 - jazelle, bits 4 & 3 part of address. + m_addrPktIsa = ocsd_isa_jazelle; + else if((currByte & 0x30) == 0x10) // bit [5:4] == 2'b01 - thumb, bit 3 part of address. + m_addrPktIsa = ocsd_isa_thumb2; + } + m_numAddrBytes++; + } + else if(!m_gotExcepBytes) + { + // may need exception bytes + if(m_numExcepBytes == 0) + { + if((currByte & 0x80) == 0x00) + m_gotExcepBytes = true; + m_excepAltISA = ((currByte & 0x40) == 0x40) ? 1 : 0; + } + else + m_gotExcepBytes = true; + m_numExcepBytes++; + + if(m_gotExcepBytes && !m_needCycleCount) + bDone = true; + + } + else if(m_needCycleCount) + { + // not done after exception bytes, collect cycle count + if(m_gotCCBytes == 0) + { + bDone = ((currByte & 0x40) == 0x00 ); + } + else + { + // done if no more or 5th byte + bDone = (((currByte & 0x80) == 0x00 ) || (m_gotCCBytes == 4)); + } + m_gotCCBytes++; + } + else + // this should never be reached. + throwMalformedPacketErr("sequencing error analysing branch packet"); + } + else + bBytesAvail = false; + } + + // analyse the bytes to create the packet + if(bDone) + { + // ISA for the packet + if(m_addrPktIsa == ocsd_isa_unknown) // unchanged by trace packet + m_addrPktIsa = m_curr_packet.getISA(); // same as prev + + if(m_gotExcepBytes) // may adjust according to alt ISA in exception packet + { + if((m_addrPktIsa == ocsd_isa_tee) && (m_excepAltISA == 0)) + m_addrPktIsa = ocsd_isa_thumb2; + else if((m_addrPktIsa == ocsd_isa_thumb2) && (m_excepAltISA == 1)) + m_addrPktIsa = ocsd_isa_tee; + } + m_curr_packet.UpdateISA(m_addrPktIsa); // mark ISA in packet (update changes current and prev to dectect an ISA change). + + + + // we know the ISA, we can correctly interpret the address. + uint8_t total_bits = 0; + uint32_t addr_val = extractAddress(0,total_bits); + m_curr_packet.UpdateAddress(addr_val,total_bits); + + if(m_numExcepBytes > 0) + { + uint8_t E1 = m_currPacketData[m_numAddrBytes]; + uint16_t ENum = (E1 >> 1) & 0xF; + ocsd_armv7_exception excep = Excp_Reserved; + + m_curr_packet.UpdateNS(E1 & 0x1); + if(m_numExcepBytes > 1) + { + uint8_t E2 = m_currPacketData[m_numAddrBytes+1]; + m_curr_packet.UpdateHyp((E2 >> 5) & 0x1); + ENum |= ((uint16_t)(E2 & 0x1F) << 4); + } + + if(ENum <= 0xF) + { + static ocsd_armv7_exception v7ARExceptions[16] = { + Excp_NoException, Excp_DebugHalt, Excp_SMC, Excp_Hyp, + Excp_AsyncDAbort, Excp_ThumbEECheckFail, Excp_Reserved, Excp_Reserved, + Excp_Reset, Excp_Undef, Excp_SVC, Excp_PrefAbort, + Excp_SyncDataAbort, Excp_Generic, Excp_IRQ, Excp_FIQ + }; + excep = v7ARExceptions[ENum]; + } + m_curr_packet.SetException(excep,ENum); + } + + if(m_needCycleCount) + { + int countIdx = m_numAddrBytes + m_numExcepBytes; + uint32_t cycleCount = 0; + extractCycleCount(countIdx,cycleCount); + m_curr_packet.SetCycleCount(cycleCount); + } + m_process_state = SEND_PKT; + } +} + +void TrcPktProcPtm::pktReserved() +{ + m_process_state = SEND_PKT; // no payload +} + +void TrcPktProcPtm::extractCtxtID(int idx, uint32_t &ctxtID) +{ + ctxtID = 0; + int shift = 0; + for(int i=0; i < m_numCtxtIDBytes; i++) + { + if((size_t)idx+i >= m_currPacketData.size()) + throwMalformedPacketErr("Insufficient packet bytes for Context ID value."); + ctxtID |= ((uint32_t)m_currPacketData[idx+i]) << shift; + shift+=8; + } +} + +void TrcPktProcPtm::extractCycleCount(int offset, uint32_t &cycleCount) +{ + bool bCont = true; + cycleCount = 0; + int by_idx = 0; + uint8_t currByte; + int shift = 4; + + while(bCont) + { + if((size_t)by_idx+offset >= m_currPacketData.size()) + throwMalformedPacketErr("Insufficient packet bytes for Cycle Count value."); + + currByte = m_currPacketData[offset+by_idx]; + if(by_idx == 0) + { + bCont = (currByte & 0x40) != 0; + cycleCount = (currByte >> 2) & 0xF; + } + else + { + + bCont = (currByte & 0x80) != 0; + if(by_idx == 4) + bCont = false; + cycleCount |= (((uint32_t)(currByte & 0x7F)) << shift); + shift += 7; + } + by_idx++; + } +} + +int TrcPktProcPtm::extractTS(uint64_t &tsVal,uint8_t &tsUpdateBits) +{ + bool bCont = true; + int tsIdx = 1; // start index; + uint8_t byteVal; + bool b64BitVal = m_config->TSPkt64(); + int shift = 0; + + tsVal = 0; + tsUpdateBits = 0; + + while(bCont) + { + if((size_t)tsIdx >= m_currPacketData.size()) + throwMalformedPacketErr("Insufficient packet bytes for Timestamp value."); + + byteVal = m_currPacketData[tsIdx]; + + if(b64BitVal) + { + if(tsIdx < 9) + { + bCont = ((byteVal & 0x80) == 0x80); + byteVal &= 0x7F; + tsUpdateBits += 7; + } + else + { + bCont = false; + tsUpdateBits += 8; + } + } + else + { + if(tsIdx < 7) + { + bCont = ((byteVal & 0x80) == 0x80); + byteVal &= 0x7F; + tsUpdateBits += 7; + } + else + { + byteVal &=0x3F; + bCont = false; + tsUpdateBits += 6; + } + } + tsVal |= (((uint64_t)byteVal) << shift); + tsIdx++; + shift += 7; + } + return tsIdx; // return next byte index in packet. +} + +uint32_t TrcPktProcPtm::extractAddress(const int offset, uint8_t &total_bits) +{ + // we know the ISA, we can correctly interpret the address. + uint32_t addr_val = 0; + uint8_t mask = 0x7E; // first byte mask (always); + uint8_t num_bits = 0x7; // number of bits in the 1st byte (thumb); + int shift = 0; + int next_shift = 0; + + total_bits = 0; + + for(int i = 0; i < m_numAddrBytes; i++) + { + if(i == 4) + { + // 5th byte mask + mask = 0x0f; // thumb mask; + num_bits = 4; + if(m_addrPktIsa == ocsd_isa_jazelle) + { + mask = 0x1F; + num_bits = 5; + } + else if(m_addrPktIsa == ocsd_isa_arm) + { + mask = 0x07; + num_bits = 3; + } + } + else if(i > 0) + { + mask = 0x7F; + num_bits = 7; + // check for last byte but not 1st or 5th byte mask + if(i == m_numAddrBytes-1) + { + mask = 0x3F; + num_bits = 6; + } + } + + // extract data + shift = next_shift; + addr_val |= ((uint32_t)(m_currPacketData[i+offset] & mask) << shift); + total_bits += num_bits; + + // how much we shift the next value + if(i == 0) + { + if(m_addrPktIsa == ocsd_isa_jazelle) + { + addr_val >>= 1; + next_shift = 6; + total_bits--; // adjust bits for jazelle offset + } + else + { + next_shift = 7; + } + } + else + { + next_shift += 7; + } + } + + if(m_addrPktIsa == ocsd_isa_arm) + { + addr_val <<= 1; // shift one extra bit for ARM address alignment. + total_bits++; + } + return addr_val; +} + + +void TrcPktProcPtm::BuildIPacketTable() +{ + // initialise all to branch, atom or reserved packet header + for(unsigned i = 0; i < 256; i++) + { + // branch address packets all end in 8'bxxxxxxx1 + if((i & 0x01) == 0x01) + { + m_i_table[i].pkt_type = PTM_PKT_BRANCH_ADDRESS; + m_i_table[i].pptkFn = &TrcPktProcPtm::pktBranchAddr; + } + // atom packets are 8'b1xxxxxx0 + else if((i & 0x81) == 0x80) + { + m_i_table[i].pkt_type = PTM_PKT_ATOM; + m_i_table[i].pptkFn = &TrcPktProcPtm::pktAtom; + } + else + { + // set all the others to reserved for now + m_i_table[i].pkt_type = PTM_PKT_RESERVED; + m_i_table[i].pptkFn = &TrcPktProcPtm::pktReserved; + } + } + + // pick out the other packet types by individual codes. + + // A-sync 8'b00000000 + m_i_table[0x00].pkt_type = PTM_PKT_A_SYNC; + m_i_table[0x00].pptkFn = &TrcPktProcPtm::pktASync; + + // I-sync 8'b00001000 + m_i_table[0x08].pkt_type = PTM_PKT_I_SYNC; + m_i_table[0x08].pptkFn = &TrcPktProcPtm::pktISync; + + // waypoint update 8'b01110010 + m_i_table[0x72].pkt_type = PTM_PKT_WPOINT_UPDATE; + m_i_table[0x72].pptkFn = &TrcPktProcPtm::pktWPointUpdate; + + // trigger 8'b00001100 + m_i_table[0x0C].pkt_type = PTM_PKT_TRIGGER; + m_i_table[0x0C].pptkFn = &TrcPktProcPtm::pktTrigger; + + // context ID 8'b01101110 + m_i_table[0x6E].pkt_type = PTM_PKT_CONTEXT_ID; + m_i_table[0x6E].pptkFn = &TrcPktProcPtm::pktCtxtID; + + // VMID 8'b00111100 + m_i_table[0x3C].pkt_type = PTM_PKT_VMID; + m_i_table[0x3C].pptkFn = &TrcPktProcPtm::pktVMID; + + // Timestamp 8'b01000x10 + m_i_table[0x42].pkt_type = PTM_PKT_TIMESTAMP; + m_i_table[0x42].pptkFn = &TrcPktProcPtm::pktTimeStamp; + m_i_table[0x46].pkt_type = PTM_PKT_TIMESTAMP; + m_i_table[0x46].pptkFn = &TrcPktProcPtm::pktTimeStamp; + + // Exception return 8'b01110110 + m_i_table[0x76].pkt_type = PTM_PKT_EXCEPTION_RET; + m_i_table[0x76].pptkFn = &TrcPktProcPtm::pktExceptionRet; + + // Ignore 8'b01100110 + m_i_table[0x66].pkt_type = PTM_PKT_IGNORE; + m_i_table[0x66].pptkFn = &TrcPktProcPtm::pktIgnore; +} + +/* End of File trc_pkt_proc_ptm.cpp */ |