xref: /freebsd-src/contrib/opencsd/decoder/source/etmv3/trc_pkt_proc_etmv3_impl.cpp (revision c120c5646da1a1d2c4d90fd069a7e2a8d559eb46)

/*
 * \file       trc_pkt_proc_etmv3_impl.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 "trc_pkt_proc_etmv3_impl.h"

EtmV3PktProcImpl::EtmV3PktProcImpl() :
    m_isInit(false),
    m_interface(0)
{
}

EtmV3PktProcImpl::~EtmV3PktProcImpl()
{
}

ocsd_err_t EtmV3PktProcImpl::Configure(const EtmV3Config *p_config)
{
    ocsd_err_t err = OCSD_OK;
    if(p_config != 0)
    {
        m_config = *p_config;
        m_chanIDCopy = m_config.getTraceID();
    }
    else
    {
        err = OCSD_ERR_INVALID_PARAM_VAL;
        if(m_isInit)
            m_interface->LogError(ocsdError(OCSD_ERR_SEV_ERROR,err));
    }
    return err;
}

ocsd_datapath_resp_t EtmV3PktProcImpl::processData(const ocsd_trc_index_t index,
                                                    const uint32_t dataBlockSize,
                                                    const uint8_t *pDataBlock,
                                                    uint32_t *numBytesProcessed)
{
    ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
    m_bytesProcessed = 0;

    while( ( (m_bytesProcessed < dataBlockSize) ||
             ((m_bytesProcessed == dataBlockSize) && (m_process_state == SEND_PKT)) )
        && OCSD_DATA_RESP_IS_CONT(resp))
    {
        try
        {
            switch(m_process_state)
            {
            case WAIT_SYNC:
                if(!m_bStartOfSync)
                    m_packet_index = index +  m_bytesProcessed;
                m_bytesProcessed += waitForSync(dataBlockSize-m_bytesProcessed,pDataBlock+m_bytesProcessed);
                break;

            case PROC_HDR:
                m_packet_index = index +  m_bytesProcessed;
                processHeaderByte(pDataBlock[m_bytesProcessed++]);
                break;

            case PROC_DATA:
                processPayloadByte(pDataBlock [m_bytesProcessed++]);
                break;

            case SEND_PKT:
                resp =  outputPacket();
                break;
            }
        }
        catch(ocsdError &err)
        {
            m_interface->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;
            ocsdError fatal = ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_FAIL,m_packet_index,m_chanIDCopy);
            fatal.setMessage("Unknown System Error decoding trace.");
            m_interface->LogError(fatal);
        }
    }

    *numBytesProcessed = m_bytesProcessed;
    return resp;
}

ocsd_datapath_resp_t EtmV3PktProcImpl::onEOT()
{
    ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
    // if we have a partial packet then send to attached sinks
    if(m_currPacketData.size() != 0)
    {
        // TBD: m_curr_packet.updateErrType(ETM4_ETM3_PKT_I_INCOMPLETE_EOT);
        resp = outputPacket();
        InitPacketState();
    }
    return resp;
}

ocsd_datapath_resp_t EtmV3PktProcImpl::onReset()
{
    InitProcessorState();
    return OCSD_RESP_CONT;
}

ocsd_datapath_resp_t EtmV3PktProcImpl::onFlush()
{
    // packet processor never holds on to flushable data (may have partial packet,
    // but any full packets are immediately sent)
    return OCSD_RESP_CONT;
}

void EtmV3PktProcImpl::Initialise(TrcPktProcEtmV3 *p_interface)
{
    if(p_interface)
    {
        m_interface = p_interface;
        m_isInit = true;

    }
    InitProcessorState();
    /* not using pattern matcher for sync at present
    static const uint8_t a_sync[] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x80 };
    m_syncMatch.setPattern(a_sync, sizeof(a_sync));*/
}

void EtmV3PktProcImpl::InitProcessorState()
{
    m_bStreamSync = false;  // not synced
    m_process_state = WAIT_SYNC; // waiting for sync
    m_bStartOfSync = false; // not seen start of sync packet
    m_curr_packet.ResetState(); // reset intra packet state
    InitPacketState();  // set curr packet state
    m_bSendPartPkt = false;
}

void EtmV3PktProcImpl::InitPacketState()
{
    m_bytesExpectedThisPkt = 0;
	m_BranchPktNeedsException = false;
	m_bIsync_got_cycle_cnt = false;
	m_bIsync_get_LSiP_addr = false;
	m_IsyncInfoIdx = false;
	m_bExpectingDataAddress = false;
	m_bFoundDataAddress = false;
    m_currPacketData.clear();
    m_currPktIdx = 0;       // index into processed bytes in current packet
    m_curr_packet.Clear();

}

ocsd_datapath_resp_t EtmV3PktProcImpl::outputPacket()
{
    ocsd_datapath_resp_t dp_resp = OCSD_RESP_FATAL_NOT_INIT;
    if(m_isInit)
    {
        ocsd_etmv3_pkt_type type = m_curr_packet.getType();
        if(!m_bSendPartPkt)
        {
            dp_resp = m_interface->outputOnAllInterfaces(m_packet_index,&m_curr_packet,&type,m_currPacketData);
            m_process_state = m_bStreamSync ? PROC_HDR : WAIT_SYNC; // need a header next time, or still waiting to sync.
            m_currPacketData.clear();
        }
        else
        {
            // sending part packet, still some data in the main packet
            dp_resp = m_interface->outputOnAllInterfaces(m_packet_index,&m_curr_packet,&type,m_partPktData);
            m_process_state = m_post_part_pkt_state;
            m_packet_index += m_partPktData.size();
            m_bSendPartPkt = false;
            m_curr_packet.SetType(m_post_part_pkt_type);
        }
    }
    return dp_resp;
}

void EtmV3PktProcImpl::setBytesPartPkt(int numBytes, process_state nextState, const ocsd_etmv3_pkt_type nextType)
{
    m_partPktData.clear();
    for(int i=0; i < numBytes; i++)
    {
        m_partPktData.push_back(m_currPacketData[i]);
    }
    m_currPacketData.erase(m_currPacketData.begin(), m_currPacketData.begin()+numBytes);
    m_bSendPartPkt = true;
    m_post_part_pkt_state = nextState;
    m_post_part_pkt_type = nextType;
}

uint32_t EtmV3PktProcImpl::waitForSync(const uint32_t dataBlockSize, const uint8_t *pDataBlock)
{
    uint8_t currByte;
    uint32_t bytesProcessed = 0;
    bool bSendBlock = false;

    // need to wait for the first sync packet
    while(!bSendBlock && (bytesProcessed < dataBlockSize))
    {
        currByte = pDataBlock[bytesProcessed++];
        // TBD: forced sync point

        if(m_bStartOfSync)
        {
            // need to handle consecutive 0 bytes followed by genuine A-SYNC.

            m_currPacketData.push_back(currByte);
            if((currByte == 0x80) && (m_currPacketData.size() >= 6))
            {
                // it is a sync packet possibly with leading zeros
                bSendBlock = true;
                if(m_currPacketData.size() > 6)
                {
                    m_currPacketData.pop_back();
                    bytesProcessed--;   // return 0x80 to the input buffer to re-process next pass after stripping 0's
                    setBytesPartPkt(m_currPacketData.size()-5,WAIT_SYNC,ETM3_PKT_NOTSYNC);
                }
                else
                {
                    m_bStreamSync = true;
                    m_curr_packet.SetType(ETM3_PKT_A_SYNC);
                }
            }
            else if(currByte != 0x00)
            {
                m_bStartOfSync = false; // not a sync packet
            }
            else if(m_currPacketData.size() >= 13)  // 13 0's, strip 8 of them...
            {
                setBytesPartPkt(8,WAIT_SYNC,ETM3_PKT_NOTSYNC);
                bSendBlock = true;
            }
        }
        else    // not seen a start of sync candidate yet
        {
            if(currByte == 0x00)  // could be the start of a-sync
            {
                if(m_currPacketData.size() == 0)
                {
                    m_currPacketData.push_back(currByte);
                    m_bStartOfSync = true;
                }
                else
                {
                    bytesProcessed--;
                    bSendBlock = true;  // send none sync packet data, re-process this byte next time.
                    m_curr_packet.SetType(ETM3_PKT_NOTSYNC);    // send unsynced data packet.
                }
            }
            else
            {
                //save a byte - not start of a-sync
                m_currPacketData.push_back(currByte);

                // done all data in this block, or got 16 unsynced bytes
                if((bytesProcessed == dataBlockSize) || (m_currPacketData.size() == 16))
                {
                    bSendBlock = true;  // send none sync packet block
                    m_curr_packet.SetType(ETM3_PKT_NOTSYNC);    // send unsynced data packet.
                }
            }
        }
    }
    if(bSendBlock)
        SendPacket();
    return bytesProcessed;
}

ocsd_err_t EtmV3PktProcImpl::processHeaderByte(uint8_t by)
{
    InitPacketState();  // new packet, clear old single packet state (retains intra packet state).

    // save byte
    m_currPacketData.push_back(by);

    m_process_state = PROC_DATA;    // assume next is data packet

	// check for branch address 0bCxxxxxxx1
	if((by & 0x01) == 0x01 ) {
		m_curr_packet.SetType(ETM3_PKT_BRANCH_ADDRESS);
		m_BranchPktNeedsException = false;
		if((by & 0x80) != 0x80) {
			// no continuation - 1 byte branch same in alt and std...
            if((by == 0x01) && (m_interface->getComponentOpMode() & ETMV3_OPFLG_UNFORMATTED_SOURCE))
			{
                // TBD: need to fix up for handling bypassed ETM stream at some point.
                throwUnsupportedErr("Bypassed ETM stream not supported in this version of the decoder.");
                // could be EOTrace marker from bypassed formatter
				m_curr_packet.SetType(ETM3_PKT_BRANCH_OR_BYPASS_EOT);
			}
			else
            {
                OnBranchAddress();
				SendPacket();  // mark ready to send.
            }
		}
	}
	// check for p-header - 0b1xxxxxx0
	else if((by & 0x81) == 0x80) {
		m_curr_packet.SetType(ETM3_PKT_P_HDR);
        if(m_curr_packet.UpdateAtomFromPHdr(by,m_config.isCycleAcc()))
		    SendPacket();
        else
            throwPacketHeaderErr("Invalid P-Header.");
	}
	// check 0b0000xx00 group
	else if((by & 0xF3) == 0x00) {

		// 	A-Sync
		if(by == 0x00) {
			m_curr_packet.SetType(ETM3_PKT_A_SYNC);
		}
		// cycle count
		else if(by == 0x04) {
			m_curr_packet.SetType(ETM3_PKT_CYCLE_COUNT);
		}
		// I-Sync
		else if(by == 0x08) {
			m_curr_packet.SetType(ETM3_PKT_I_SYNC);
			m_bIsync_got_cycle_cnt = false;
			m_bIsync_get_LSiP_addr = false;
		}
		// trigger
		else if(by == 0x0C) {
			m_curr_packet.SetType(ETM3_PKT_TRIGGER);
            // no payload - just send it.
			SendPacket();
		}
	}
	// check remaining 0bxxxxxx00 codes
	else if((by & 0x03 )== 0x00) {
		// OoO data 0b0xx0xx00
		if((by & 0x93 )== 0x00) {
            if(!m_config.isDataValTrace()) {
                m_curr_packet.SetErrType(ETM3_PKT_BAD_TRACEMODE);
                throwPacketHeaderErr("Invalid data trace header (out of order data) - not tracing data values.");
			}
			m_curr_packet.SetType(ETM3_PKT_OOO_DATA);
			uint8_t size = ((by & 0x0C) >> 2);
			// header contains a count of the data to follow
			// size 3 == 4 bytes, other sizes == size bytes
			if(size == 0)
            {
                m_curr_packet.SetDataOOOTag((by >> 5)  & 0x3);
                m_curr_packet.SetDataValue(0);
				SendPacket();
            }
			else
				m_bytesExpectedThisPkt = (short)(1 + ((size == 3) ? 4 : size));
		}
		// I-Sync + cycle count
		else if(by == 0x70) {
			m_curr_packet.SetType(ETM3_PKT_I_SYNC_CYCLE);
			m_bIsync_got_cycle_cnt = false;
			m_bIsync_get_LSiP_addr = false;
		}
		// store failed
		else if(by == 0x50) {
            if(!m_config.isDataValTrace())
            {
                m_curr_packet.SetErrType(ETM3_PKT_BAD_TRACEMODE);
                throwPacketHeaderErr("Invalid data trace header (store failed) - not tracing data values.");
            }
            m_curr_packet.SetType(ETM3_PKT_STORE_FAIL);
            SendPacket();
		}
		// OoO placeholder 0b01x1xx00
		else if((by & 0xD3 )== 0x50) {
			m_curr_packet.SetType(ETM3_PKT_OOO_ADDR_PLC);
            if(!m_config.isDataTrace())
            {
                m_curr_packet.SetErrType(ETM3_PKT_BAD_TRACEMODE);
                throwPacketHeaderErr("Invalid data trace header (out of order placeholder) - not tracing data.");
            }
            // expecting data address if flagged and address tracing enabled (flag can be set even if address tracing disabled)
			m_bExpectingDataAddress = ((by & DATA_ADDR_EXPECTED_FLAG) == DATA_ADDR_EXPECTED_FLAG) && m_config.isDataAddrTrace();
			m_bFoundDataAddress = false;
            m_curr_packet.SetDataOOOTag((by >> 2) & 0x3);
			if(!m_bExpectingDataAddress) {
				SendPacket();
			}
		}
        // vmid 0b00111100
        else if(by == 0x3c) {
            m_curr_packet.SetType(ETM3_PKT_VMID);
        }
		else
		{
			m_curr_packet.SetErrType(ETM3_PKT_RESERVED);
            throwPacketHeaderErr("Packet header reserved encoding");
		}
	}
	// normal data 0b00x0xx10
	else if((by & 0xD3 )== 0x02) {
		uint8_t size = ((by & 0x0C) >> 2);
		if(!m_config.isDataTrace()) {
            m_curr_packet.SetErrType(ETM3_PKT_BAD_TRACEMODE);
            throwPacketHeaderErr("Invalid data trace header (normal data) - not tracing data.");
		}
		m_curr_packet.SetType(ETM3_PKT_NORM_DATA);
		m_bExpectingDataAddress = ((by & DATA_ADDR_EXPECTED_FLAG) == DATA_ADDR_EXPECTED_FLAG) && m_config.isDataAddrTrace();
		m_bFoundDataAddress = false;

        // set this with the data bytes expected this packet, plus the header byte.
		m_bytesExpectedThisPkt = (short)( 1 + ((size == 3) ? 4 : size));
		if(!m_bExpectingDataAddress && (m_bytesExpectedThisPkt == 1)) {
			// single byte data packet, value = 0;
            m_curr_packet.SetDataValue(0);
			SendPacket();
		}

	}
	// data suppressed 0b01100010
	else if(by == 0x62) {
		if(!m_config.isDataTrace())
        {
            m_curr_packet.SetErrType(ETM3_PKT_BAD_TRACEMODE);
            throwPacketHeaderErr("Invalid data trace header (data suppressed) - not tracing data.");
        }
        m_curr_packet.SetType(ETM3_PKT_DATA_SUPPRESSED);
        SendPacket();
	}
	// value not traced 0b011x1010
	else if((by & 0xEF )== 0x6A) {
		if(!m_config.isDataTrace()) {
            m_curr_packet.SetErrType(ETM3_PKT_BAD_TRACEMODE);
            throwPacketHeaderErr("Invalid data trace header (value not traced) - not tracing data.");
		}
		m_curr_packet.SetType(ETM3_PKT_VAL_NOT_TRACED);
		m_bExpectingDataAddress = ((by & DATA_ADDR_EXPECTED_FLAG) == DATA_ADDR_EXPECTED_FLAG) && m_config.isDataAddrTrace();
		m_bFoundDataAddress = false;
		if(!m_bExpectingDataAddress) {
			SendPacket();
        }
	}
	// ignore 0b01100110
	else if(by == 0x66) {
		m_curr_packet.SetType(ETM3_PKT_IGNORE);
        SendPacket();
	}
	// context ID 0b01101110
	else if(by == 0x6E) {
		m_curr_packet.SetType(ETM3_PKT_CONTEXT_ID);
        m_bytesExpectedThisPkt = (short)(1 + m_config.CtxtIDBytes());
	}
	// exception return 0b01110110
	else if(by == 0x76) {
		m_curr_packet.SetType(ETM3_PKT_EXCEPTION_EXIT);
        SendPacket();
	}
	// exception entry 0b01111110
	else if(by == 0x7E) {
		m_curr_packet.SetType(ETM3_PKT_EXCEPTION_ENTRY);
        SendPacket();
	}
	// timestamp packet 0b01000x10
	else if((by & 0xFB )== 0x42)
	{
		m_curr_packet.SetType(ETM3_PKT_TIMESTAMP);
	}
	else
	{
		m_curr_packet.SetErrType(ETM3_PKT_RESERVED);
        throwPacketHeaderErr("Packet header reserved encoding.");
	}
    return OCSD_OK;
}

ocsd_err_t EtmV3PktProcImpl::processPayloadByte(uint8_t by)
{
    bool bTopBitSet = false;
    bool packetDone = false;

	// pop byte into buffer
    m_currPacketData.push_back(by);

    switch(m_curr_packet.getType()) {
	default:
        throw ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_PKT_INTERP_FAIL,m_packet_index,m_chanIDCopy,"Interpreter failed - cannot process payload for unexpected or unsupported packet.");
		break;

	case ETM3_PKT_BRANCH_ADDRESS:
		bTopBitSet = (bool)((by & 0x80) == 0x80);
        if(m_config.isAltBranch())  // etm implements the alternative branch encoding
        {
			if(!bTopBitSet)     // no continuation
            {
				if(!m_BranchPktNeedsException)
				{
					if((by & 0xC0) == 0x40)
						m_BranchPktNeedsException = true;
					else
                        packetDone = true;
				}
				else
                    packetDone = true;
			}
		}
		else
        {
			// standard encoding  < 5 bytes cannot be exception branch
			// 5 byte packet
            if(m_currPacketData.size() == 5) {
				if((by & 0xC0) == 0x40)
					// expecting follow up byte(s)
					m_BranchPktNeedsException = true;
				else
                    packetDone = true;
			}
			// waiting for exception packet
			else if(m_BranchPktNeedsException){
				if(!bTopBitSet)
					packetDone = true;
			}
			else {
				// not exception - end of packets
				if(!bTopBitSet)
					packetDone = true;
			}
		}

        if(packetDone)
        {
            OnBranchAddress();
			SendPacket();
        }
		break;

	case ETM3_PKT_BRANCH_OR_BYPASS_EOT:
        /*
		if((by != 0x00) || ( m_currPacketData.size() == ETM3_PKT_BUFF_SIZE)) {
			if(by == 0x80 && ( m_currPacketData.size() == 7)) {
				// branch 0 followed by A-sync!
				m_currPacketData.size() = 1;
                m_curr_packet.SetType(ETM3_PKT_BRANCH_ADDRESS;
				SendPacket();
                memcpy(m_currPacketData, &m_currPacketData[1],6);
				m_currPacketData.size() = 6;
                m_curr_packet.SetType(ETM3_PKT_A_SYNC;
				SendPacket();
			}
			else if( m_currPacketData.size() == 2) {
				// branch followed by another byte
                m_currPacketData.size() = 1;
                m_curr_packet.SetType(ETM3_PKT_BRANCH_ADDRESS;
                SendPacket();
                ProcessHeaderByte(by);
			}
			else if(by == 0x00) {
				// end of buffer...output something - incomplete / unknown.
				SendPacket();
			}
			else if(by == 0x01) {
				// 0x01 - 0x00 x N - 0x1
				// end of buffer...output something
                m_currPacketData.size()--;
                SendPacket();
				ProcessHeaderByte(by);
			}
			else {
				// branch followed by unknown sequence
				int oldidx =  m_currPacketData.size();
                m_currPacketData.size() = 1;
                m_curr_packet.SetType(ETM3_PKT_BRANCH_ADDRESS;
				SendPacket();
                oldidx--;
                memcpy(m_currPacketData, &m_currPacketData[1],oldidx);
                m_currPacketData.size() = oldidx;
                SendBadPacket("ERROR : unknown sequence");
			}
		}*/
		// just ignore zeros
		break;



	case ETM3_PKT_A_SYNC:
		if(by == 0x00) {
			if( m_currPacketData.size() > 5) {
				// extra 0, need to lose one

                // set error type
                m_curr_packet.SetErrType(ETM3_PKT_BAD_SEQUENCE);
                // mark extra 0 for sending, retain remaining, restart in A-SYNC processing mode.
                setBytesPartPkt(1,PROC_DATA,ETM3_PKT_A_SYNC);
                throwMalformedPacketErr("A-Sync ?: Extra 0x00 in sequence");
			}
		}
		else if((by == 0x80) && ( m_currPacketData.size() == 6)) {
			SendPacket();
			m_bStreamSync = true;
		}
		else
		{
            m_curr_packet.SetErrType(ETM3_PKT_BAD_SEQUENCE);
            m_bytesProcessed--; // remove the last byte from the number processed to re-try
            m_currPacketData.pop_back();  // remove the last byte processed from the packet
            throwMalformedPacketErr("A-Sync ? : Unexpected byte in sequence");
		}
		break;

	case ETM3_PKT_CYCLE_COUNT:
		bTopBitSet = ((by & 0x80) == 0x80);
        if(!bTopBitSet || ( m_currPacketData.size() >= 6)) {
            m_currPktIdx = 1;
            m_curr_packet.SetCycleCount(extractCycleCount());
			SendPacket();
		}
		break;

	case ETM3_PKT_I_SYNC_CYCLE:
		if(!m_bIsync_got_cycle_cnt) {
			if(((by & 0x80) != 0x80) || ( m_currPacketData.size() >= 6)) {
				m_bIsync_got_cycle_cnt = true;
			}
			break;
		}
		// fall through when we have the first non-cycle count byte
	case ETM3_PKT_I_SYNC:
		if(m_bytesExpectedThisPkt == 0) {
			int cycCountBytes = m_currPacketData.size() - 2;
            int ctxtIDBytes = m_config.CtxtIDBytes();
			// bytes expected = header + n x ctxt id + info byte + 4 x addr;
            if(m_config.isInstrTrace())
				m_bytesExpectedThisPkt = cycCountBytes + 6 + ctxtIDBytes;
			else
				m_bytesExpectedThisPkt = 2 + ctxtIDBytes;
			m_IsyncInfoIdx = 1 + cycCountBytes + ctxtIDBytes;
		}
		if(( m_currPacketData.size() - 1) == (unsigned)m_IsyncInfoIdx) {
			m_bIsync_get_LSiP_addr = ((m_currPacketData[m_IsyncInfoIdx] & 0x80) == 0x80);
		}

		// if bytes collected >= bytes expected
		if( m_currPacketData.size() >= m_bytesExpectedThisPkt) {
			// if we still need the LSip Addr, then this is not part of the expected
			// count as we have no idea how long it is
			if(m_bIsync_get_LSiP_addr) {
				if((by & 0x80) != 0x80) {
                    OnISyncPacket();
				}
			}
			else {
				// otherwise, output now
                OnISyncPacket();
			}
		}
		break;

	case ETM3_PKT_NORM_DATA:
		if(m_bExpectingDataAddress && !m_bFoundDataAddress) {
			// look for end of continuation bits
			if((by & 0x80) != 0x80) {
				m_bFoundDataAddress = true;
				// add on the bytes we have found for the address to the expected data bytes
				m_bytesExpectedThisPkt += ( m_currPacketData.size() - 1);
			}
			else
				break;
		}
		// found any data address we were expecting
		else if(m_bytesExpectedThisPkt == m_currPacketData.size()) {
            m_currPktIdx = 1;
            if(m_bExpectingDataAddress)
            {
                uint8_t bits = 0, beVal = 0;
                bool updateBE = false;
                uint32_t dataAddress = extractDataAddress(bits,updateBE,beVal);
                m_curr_packet.UpdateDataAddress(dataAddress, bits);
                if(updateBE)
                    m_curr_packet.UpdateDataEndian(beVal);
            }
            m_curr_packet.SetDataValue(extractDataValue((m_currPacketData[0] >> 2) & 0x3));
			SendPacket();
		}
		break;

	case ETM3_PKT_OOO_DATA:
		if(m_bytesExpectedThisPkt ==  m_currPacketData.size())
        {
            m_currPktIdx = 1;
            m_curr_packet.SetDataValue(extractDataValue((m_currPacketData[0] >> 2) & 0x3));
            m_curr_packet.SetDataOOOTag((m_currPacketData[0] >> 5) & 0x3);
			SendPacket();
        }
		if(m_bytesExpectedThisPkt <  m_currPacketData.size())
			throwMalformedPacketErr("Malformed out of order data packet.");
		break;

        // both these expect an address only.
	case ETM3_PKT_VAL_NOT_TRACED:
    case ETM3_PKT_OOO_ADDR_PLC: // we set the tag earlier.
		if(m_bExpectingDataAddress) {
			// look for end of continuation bits
			if((by & 0x80) != 0x80) {
                uint8_t bits = 0, beVal = 0;
                bool updateBE = false;
                m_currPktIdx = 1;
                uint32_t dataAddress = extractDataAddress(bits,updateBE,beVal);
                m_curr_packet.UpdateDataAddress(dataAddress, bits);
                if(updateBE)
                    m_curr_packet.UpdateDataEndian(beVal);
				SendPacket();
			}
		}
		break;

	case ETM3_PKT_CONTEXT_ID:
		if(m_bytesExpectedThisPkt == m_currPacketData.size()) {
            m_currPktIdx = 1;
            m_curr_packet.UpdateContextID(extractCtxtID());
			SendPacket();
		}
		if(m_bytesExpectedThisPkt < m_currPacketData.size())
			throwMalformedPacketErr("Malformed context id packet.");
		break;

	case ETM3_PKT_TIMESTAMP:
		if((by & 0x80) != 0x80) {
            uint8_t tsBits = 0;
            m_currPktIdx = 1;
            uint64_t tsVal = extractTimestamp(tsBits);
            m_curr_packet.UpdateTimestamp(tsVal,tsBits);
			SendPacket();
		}
        break;

    case ETM3_PKT_VMID:
        // single byte payload
        m_curr_packet.UpdateVMID(by);
        SendPacket();
        break;
	}

    return OCSD_OK;
}

// extract branch address packet at current location in packet data.
void EtmV3PktProcImpl::OnBranchAddress()
{
    int validBits = 0;
    ocsd_vaddr_t partAddr = 0;

    partAddr = extractBrAddrPkt(validBits);
    m_curr_packet.UpdateAddress(partAddr,validBits);
}

uint32_t EtmV3PktProcImpl::extractBrAddrPkt(int &nBitsOut)
{
    static int addrshift[] = {
        2, // ARM_ISA
        1, // thumb
        1, // thumb EE
        0  // jazelle
    };

    static uint8_t addrMask[] = {  // byte 5 masks
        0x7, // ARM_ISA
        0xF, // thumb
        0xF, // thumb EE
        0x1F  // jazelle
    };

    static int addrBits[] = { // address bits in byte 5
        3, // ARM_ISA
        4, // thumb
        4, // thumb EE
        5  // jazelle
    };

    static ocsd_armv7_exception exceptionTypeARMdeprecated[] = {
        Excp_Reset,
        Excp_IRQ,
        Excp_Reserved,
        Excp_Reserved,
        Excp_Jazelle,
        Excp_FIQ,
        Excp_AsyncDAbort,
        Excp_DebugHalt
    };

    bool CBit = true;
    int bytecount = 0;
    int bitcount = 0;
    int shift = 0;
    int isa_idx = 0;
    uint32_t value = 0;
    uint8_t addrbyte;
    bool byte5AddrUpdate = false;

    while(CBit && bytecount < 4)
    {
        checkPktLimits();
        addrbyte = m_currPacketData[m_currPktIdx++];
        CBit = (bool)((addrbyte & 0x80) != 0);
        shift = bitcount;
        if(bytecount == 0)
        {
            addrbyte &= ~0x81;
            bitcount+=6;
            addrbyte >>= 1;
        }
        else
        {
            // bytes 2-4, no continuation, alt format uses bit 6 to indicate following exception bytes
            if(m_config.isAltBranch() && !CBit)
            {
                // last compressed address byte with exception
                if((addrbyte & 0x40) == 0x40)
                    extractExceptionData();
                addrbyte &= 0x3F;
                bitcount+=6;
            }
            else
            {
                addrbyte &= 0x7F;
                bitcount+=7;
            }
        }
        value |= ((uint32_t)addrbyte) << shift;
        bytecount++;
    }

    // byte 5 - indicates following exception bytes (or not!)
    if(CBit)
    {
        checkPktLimits();
        addrbyte = m_currPacketData[m_currPktIdx++];

        // deprecated original byte 5 encoding - ARM state exception only
        if(addrbyte & 0x80)
        {
            uint8_t excep_num = (addrbyte >> 3) & 0x7;
            m_curr_packet.UpdateISA(ocsd_isa_arm);
            m_curr_packet.SetException(exceptionTypeARMdeprecated[excep_num], excep_num, (addrbyte & 0x40) ? true : false,m_config.isV7MArch());
        }
        else
        // normal 5 byte branch, or uses exception bytes.
        {
            // go grab the exception bits to correctly interpret the ISA state
            if((addrbyte & 0x40) == 0x40)
                extractExceptionData();

            if((addrbyte & 0xB8) == 0x08)
                m_curr_packet.UpdateISA(ocsd_isa_arm);
            else if ((addrbyte & 0xB0) == 0x10)
                m_curr_packet.UpdateISA(m_curr_packet.AltISA() ? ocsd_isa_tee : ocsd_isa_thumb2);
            else if ((addrbyte & 0xA0) == 0x20)
                m_curr_packet.UpdateISA(ocsd_isa_jazelle);
            else
                throwMalformedPacketErr("Malformed Packet - Unknown ISA.");
        }

        byte5AddrUpdate = true; // need to update the address value from byte 5
    }

    // figure out the correct ISA shifts for the address bits
    switch(m_curr_packet.ISA())
    {
    case ocsd_isa_thumb2: isa_idx = 1; break;
    case ocsd_isa_tee: isa_idx = 2; break;
    case ocsd_isa_jazelle: isa_idx = 3; break;
    default: break;
    }

    if(byte5AddrUpdate)
    {
        value |= ((uint32_t)(addrbyte & addrMask[isa_idx])) << bitcount;
        bitcount += addrBits[isa_idx];
    }

    // finally align according to ISA
    shift = addrshift[isa_idx];
    value <<= shift;
    bitcount += shift;

    nBitsOut = bitcount;
    return value;
}

// extract exception data from bytes after address.
void EtmV3PktProcImpl::extractExceptionData()
{
    static const ocsd_armv7_exception exceptionTypesStd[] = {
        Excp_NoException, Excp_DebugHalt, Excp_SMC, Excp_Hyp,
        Excp_AsyncDAbort, Excp_Jazelle, Excp_Reserved, Excp_Reserved,
        Excp_Reset, Excp_Undef, Excp_SVC, Excp_PrefAbort,
        Excp_SyncDataAbort, Excp_Generic, Excp_IRQ, Excp_FIQ
    };

    static const ocsd_armv7_exception exceptionTypesCM[] = {
        Excp_NoException, Excp_CMIRQn, Excp_CMIRQn, Excp_CMIRQn,
        Excp_CMIRQn, Excp_CMIRQn, Excp_CMIRQn, Excp_CMIRQn,
        Excp_CMIRQn, Excp_CMUsageFault, Excp_CMNMI, Excp_SVC,
        Excp_CMDebugMonitor, Excp_CMMemManage, Excp_CMPendSV, Excp_CMSysTick,
        Excp_Reserved, Excp_Reset,  Excp_Reserved, Excp_CMHardFault,
        Excp_Reserved, Excp_CMBusFault, Excp_Reserved, Excp_Reserved
    };

    uint16_t exceptionNum = 0;
    ocsd_armv7_exception excep_type = Excp_Reserved;
    int resume = 0;
    int irq_n = 0;
    bool cancel_prev_instr = 0;
    bool Byte2 = false;

    checkPktLimits();

    //**** exception info Byte 0
    uint8_t dataByte =  m_currPacketData[m_currPktIdx++];

    m_curr_packet.UpdateNS(dataByte & 0x1);
    exceptionNum |= (dataByte >> 1) & 0xF;
    cancel_prev_instr = (dataByte & 0x20) ? true : false;
    m_curr_packet.UpdateAltISA(((dataByte & 0x40) != 0)  ? 1 : 0);

    //** another byte?
    if(dataByte & 0x80)
    {
        checkPktLimits();
        dataByte = m_currPacketData[m_currPktIdx++];

        if(dataByte & 0x40)
            Byte2 = true;   //** immediate info byte 2, skipping 1
        else
        {
            //**** exception info Byte 1
            if(m_config.isV7MArch())
            {
                exceptionNum |= ((uint16_t)(dataByte & 0x1F)) << 4;
            }
             m_curr_packet.UpdateHyp(dataByte & 0x20 ? 1 : 0);

            if(dataByte & 0x80)
            {
                checkPktLimits();
                dataByte = m_currPacketData[m_currPktIdx++];
                Byte2 = true;
            }
        }
        //**** exception info Byte 2
        if(Byte2)
        {
            resume = dataByte & 0xF;
        }
    }

    // set the exception type - according to the number and core profile
    if(m_config.isV7MArch())
    {
       exceptionNum &= 0x1FF;
        if(exceptionNum < 0x018)
            excep_type= exceptionTypesCM[exceptionNum];
        else
            excep_type = Excp_CMIRQn;

        if(excep_type == Excp_CMIRQn)
        {
            if(exceptionNum > 0x018)
                irq_n = exceptionNum - 0x10;
            else if(exceptionNum == 0x008)
                irq_n = 0;
            else
                irq_n = exceptionNum;
        }
    }
    else
    {
        exceptionNum &= 0xF;
        excep_type = exceptionTypesStd[exceptionNum];
    }
    m_curr_packet.SetException(excep_type, exceptionNum, cancel_prev_instr,m_config.isV7MArch(), irq_n,resume);
}

void EtmV3PktProcImpl::checkPktLimits()
{
    // index running off the end of the packet means a malformed packet.
    if(m_currPktIdx >= m_currPacketData.size())
        throwMalformedPacketErr("Malformed Packet - oversized packet.");
}

uint32_t EtmV3PktProcImpl::extractCtxtID()
{
    uint32_t ctxtID = 0;
    int size = m_config.CtxtIDBytes();

    // check we have enough data
    if((m_currPktIdx + size) > m_currPacketData.size())
        throwMalformedPacketErr("Too few bytes to extract context ID.");

    switch(size)
    {
    case 1:
        ctxtID = (uint32_t)m_currPacketData[m_currPktIdx];
        m_currPktIdx++;
        break;

    case 2:
        ctxtID = (uint32_t)m_currPacketData[m_currPktIdx]
                    | ((uint32_t)m_currPacketData[m_currPktIdx+1]) << 8;
        m_currPktIdx+=2;
        break;

    case 4:
        ctxtID = (uint32_t)m_currPacketData[m_currPktIdx]
                    | ((uint32_t)m_currPacketData[m_currPktIdx+1]) << 8
                    | ((uint32_t)m_currPacketData[m_currPktIdx+2]) << 16
                    | ((uint32_t)m_currPacketData[m_currPktIdx+3]) << 24;
        m_currPktIdx+=4;
        break;
    }
    return ctxtID;
}

uint64_t EtmV3PktProcImpl::extractTimestamp(uint8_t &tsBits)
{
    uint64_t ts = 0;
    unsigned tsMaxBytes = m_config.TSPkt64() ? 9 : 7;
    unsigned tsCurrBytes = 0;
    bool bCont = true;
    uint8_t mask = 0x7F;
    uint8_t last_mask = m_config.TSPkt64() ? 0xFF : 0x3F;
    uint8_t ts_iter_bits = 7;
    uint8_t ts_last_iter_bits = m_config.TSPkt64() ? 8 : 6;
    uint8_t currByte;
    tsBits = 0;

    while((tsCurrBytes < tsMaxBytes) && bCont)
    {
        if(m_currPacketData.size() < (m_currPktIdx + tsCurrBytes + 1))
            throwMalformedPacketErr("Insufficient bytes to extract timestamp.");

        currByte = m_currPacketData[m_currPktIdx+tsCurrBytes];
        ts |= ((uint64_t)(currByte & mask)) << (7 * tsCurrBytes);
        tsCurrBytes++;
        tsBits += ts_iter_bits;
        bCont = ((0x80 & currByte) == 0x80);
        if(tsCurrBytes == (tsMaxBytes - 1))
        {
            mask = last_mask;
            ts_iter_bits = ts_last_iter_bits;
        }
    }
    m_currPktIdx += tsCurrBytes;
    return ts;
}


uint32_t EtmV3PktProcImpl::extractDataAddress(uint8_t &bits, bool &updateBE, uint8_t &beVal)
{
    uint32_t dataAddr = 0;
    int bytesIdx = 0;
    bool bCont = true;
    uint8_t currByte = 0;

    updateBE = false;
    bits = 0;

    while(bCont)
    {
        checkPktLimits();
        currByte = m_currPacketData[m_currPktIdx++] & ((bytesIdx == 4) ? 0x0F : 0x7F);
        dataAddr |= (((uint32_t)currByte)  << (bytesIdx * 7));
        bCont = ((currByte & 0x80) == 0x80);
        if(bytesIdx == 4)
        {
            bits += 4;
            updateBE = true;
            beVal = ((currByte >> 4) & 0x1);
            bCont = false;
        }
        else
            bits+=7;
        bytesIdx++;
    }
    return dataAddr;
}

uint32_t EtmV3PktProcImpl::extractDataValue(const int dataByteSize)
{
    static int bytesReqTable[] = { 0,1,2,4 };

    uint32_t dataVal = 0;
    int bytesUsed = 0;
    int bytesReq = bytesReqTable[dataByteSize & 0x3];
    while(bytesUsed < bytesReq)
    {
        checkPktLimits();
        dataVal |= (((uint32_t)m_currPacketData[m_currPktIdx++])  << (bytesUsed * 8));
        bytesUsed++;
    }
    return dataVal;
}


uint32_t EtmV3PktProcImpl::extractCycleCount()
{
    uint32_t cycleCount = 0;
    int byteIdx = 0;
    uint8_t mask = 0x7F;
    bool bCond = true;
    uint8_t currByte = 0;

    while(bCond)
    {
        checkPktLimits();
        currByte = m_currPacketData[m_currPktIdx++];
        cycleCount |= ((uint32_t)(currByte & mask)) << (7 * byteIdx);
        bCond = ((currByte & 0x80) == 0x80);
        byteIdx++;

        if(byteIdx == 4)
            mask = 0x0F;

        if(byteIdx == 5)
            bCond = false;
    }
    return cycleCount;
}

void EtmV3PktProcImpl::OnISyncPacket()
{
    uint8_t iSyncInfoByte = 0;
    uint32_t instrAddr = 0, LSiPAddr = 0;
    int LSiPBits = 0;
    uint8_t T = 0, J = 0, AltISA = 0;

    m_currPktIdx = 1;
    if(m_bIsync_got_cycle_cnt)
    {
        m_curr_packet.SetCycleCount(extractCycleCount());
        m_curr_packet.SetISyncHasCC();
    }

    if(m_config.CtxtIDBytes() != 0)
    {
        m_curr_packet.UpdateContextID(extractCtxtID());
    }

    // extract context info
    iSyncInfoByte = m_currPacketData[m_currPktIdx++];
    m_curr_packet.SetISyncReason((ocsd_iSync_reason)((iSyncInfoByte >> 5) & 0x3));
    J = (iSyncInfoByte >> 4) & 0x1;
    AltISA = m_config.MinorRev() >= 3 ? (iSyncInfoByte >> 2) & 0x1 : 0;
    m_curr_packet.UpdateNS((iSyncInfoByte >> 3) & 0x1);
    if(m_config.hasVirtExt())
        m_curr_packet.UpdateHyp((iSyncInfoByte >> 1) & 0x1);

    // main address value - full 32 bit address value
    if(m_config.isInstrTrace())
    {
        for(int i = 0; i < 4; i++)
            instrAddr |= ((uint32_t)m_currPacketData[m_currPktIdx++]) << (8*i);
        T = instrAddr & 0x1;    // get the T bit.
        instrAddr &= ~0x1;      // remove from address.
        m_curr_packet.UpdateAddress(instrAddr,32);

        // enough data now to set the instruction set.
        ocsd_isa currISA = ocsd_isa_arm;
        if(J)
            currISA = ocsd_isa_jazelle;
        else if(T)
            currISA = AltISA ? ocsd_isa_tee : ocsd_isa_thumb2;
        m_curr_packet.UpdateISA(currISA);

        // possible follow up address value - rarely uses unless trace enabled during
        // load and store instruction executing on top of other instruction.
        if(m_bIsync_get_LSiP_addr)
        {
            LSiPAddr = extractBrAddrPkt(LSiPBits);
            // follow up address value is compressed relative to the main value
            // we store this in the data address value temporarily.
            m_curr_packet.UpdateDataAddress(instrAddr,32);
            m_curr_packet.UpdateDataAddress(LSiPAddr,LSiPBits);
        }
    }
    else
        m_curr_packet.SetISyncNoAddr();

    SendPacket(); // mark ready to send
}

/* End of File trc_pkt_proc_etmv3_impl.cpp */