| /* |
| * \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_excepAltISA = 0; |
| |
| 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) |
| { |
| // byteIdx for address byte will run from 1 to 5 - first 4 my have continuation or not. |
| 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 */ |
