1216 lines
37 KiB
C++
1216 lines
37 KiB
C++
/*
|
|
* \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 */
|