contrib/opencsd/decoder/source/etmv4/trc_pkt_proc_etmv4i_impl.cpp

   1 /*
   2  * \file       trc_pkt_proc_etmv4i_impl.cpp
   3  * \brief      OpenCSD :
   4  *
   5  * \copyright  Copyright (c) 2015, ARM Limited. All Rights Reserved.
   6  */
   7
   8 /*
   9  * Redistribution and use in source and binary forms, with or without modification,
  10  * are permitted provided that the following conditions are met:
  11  *
  12  * 1. Redistributions of source code must retain the above copyright notice,
  13  * this list of conditions and the following disclaimer.
  14  *
  15  * 2. Redistributions in binary form must reproduce the above copyright notice,
  16  * this list of conditions and the following disclaimer in the documentation
  17  * and/or other materials provided with the distribution.
  18  *
  19  * 3. Neither the name of the copyright holder nor the names of its contributors
  20  * may be used to endorse or promote products derived from this software without
  21  * specific prior written permission.
  22  *
  23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND
  24  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  25  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
  26  * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
  27  * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  28  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  30  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  31  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  32  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  33  */
  34
  35 #include "trc_pkt_proc_etmv4i_impl.h"
  36
  37 /* Trace raw input buffer class */
  38 TraceRawBuffer::TraceRawBuffer()
  39 {
  40     m_bufSize = 0;
  41     m_bufProcessed = 0;
  42     m_pBuffer = 0;
  43     pkt = 0;
  44 }
  45
  46 // init the buffer
  47 void TraceRawBuffer::init(const uint32_t size, const uint8_t *rawtrace, std::vector<uint8_t> *out_packet)
  48 {
  49     m_bufSize = size;
  50     m_bufProcessed = 0;
  51     m_pBuffer = rawtrace;
  52     pkt = out_packet;
  53 }
  54
  55 void TraceRawBuffer::copyByteToPkt()
  56 {
  57     if (!empty()) {
  58         pkt->push_back(m_pBuffer[m_bufProcessed]);
  59         m_bufProcessed++;
  60     }
  61 }
  62 uint8_t TraceRawBuffer::peekNextByte()
  63 {
  64     uint8_t val = 0;
  65     if (!empty())
  66         val = m_pBuffer[m_bufProcessed];
  67     return val;
  68 }
  69
  70 /* trace etmv4 packet processing class */
  71 EtmV4IPktProcImpl::EtmV4IPktProcImpl() :
  72     m_isInit(false),
  73     m_interface(0),
  74     m_first_trace_info(false)
  75 {
  76
  77 }
  78
  79 EtmV4IPktProcImpl::~EtmV4IPktProcImpl()
  80 {
  81 }
  82
  83 void EtmV4IPktProcImpl::Initialise(TrcPktProcEtmV4I *p_interface)
  84 {
  85      if(p_interface)
  86      {
  87          m_interface = p_interface;
  88          m_isInit = true;
  89      }
  90      InitProcessorState();
  91 }
  92
  93 ocsd_err_t EtmV4IPktProcImpl::Configure(const EtmV4Config *p_config)
  94 {
  95     ocsd_err_t err = OCSD_OK;
  96     if(p_config != 0)
  97     {
  98         m_config = *p_config;
  99         BuildIPacketTable();    // packet table based on config
 100     }
 101     else
 102     {
 103         err = OCSD_ERR_INVALID_PARAM_VAL;
 104         if(m_isInit)
 105             m_interface->LogError(ocsdError(OCSD_ERR_SEV_ERROR,err));
 106     }
 107     return err;
 108 }
 109
 110 ocsd_datapath_resp_t EtmV4IPktProcImpl::processData(  const ocsd_trc_index_t index,
 111                                     const uint32_t dataBlockSize,
 112                                     const uint8_t *pDataBlock,
 113                                     uint32_t *numBytesProcessed)
 114 {
 115     ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
 116     m_trcIn.init(dataBlockSize, pDataBlock, &m_currPacketData);
 117     m_blockIndex = index;
 118     bool done = false;
 119     uint8_t nextByte;
 120
 121     do
 122     {
 123         try
 124         {
 125            /* while (((m_blockBytesProcessed < dataBlockSize) ||
 126                 ((m_blockBytesProcessed == dataBlockSize) && (m_process_state == SEND_PKT))) &&
 127                 OCSD_DATA_RESP_IS_CONT(resp))*/
 128             while ( (!m_trcIn.empty() || (m_process_state == SEND_PKT)) &&
 129                     OCSD_DATA_RESP_IS_CONT(resp)
 130                 )
 131             {
 132                 switch (m_process_state)
 133                 {
 134                 case PROC_HDR:
 135                     m_packet_index = m_blockIndex + m_trcIn.processed();
 136                     if (m_is_sync)
 137                     {
 138                         nextByte = m_trcIn.peekNextByte();
 139                         m_pIPktFn = m_i_table[nextByte].pptkFn;
 140                         m_curr_packet.type = m_i_table[nextByte].pkt_type;
 141                     }
 142                     else
 143                     {
 144                         // unsynced - process data until we see a sync point
 145                         m_pIPktFn = &EtmV4IPktProcImpl::iNotSync;
 146                         m_curr_packet.type = ETM4_PKT_I_NOTSYNC;
 147                     }
 148                     m_process_state = PROC_DATA;
 149
 150                 case PROC_DATA:
 151                     // loop till full packet or no more data...
 152                     while (!m_trcIn.empty() && (m_process_state == PROC_DATA))
 153                     {
 154                         nextByte = m_trcIn.peekNextByte();
 155                         m_trcIn.copyByteToPkt();  // move next byte into the packet
 156     //                    m_currPacketData.push_back(pDataBlock[m_blockBytesProcessed]);
 157     //                    m_blockBytesProcessed++;
 158                         (this->*m_pIPktFn)(nextByte);
 159                     }
 160                     break;
 161
 162                 case SEND_PKT:
 163                     resp = outputPacket();
 164                     InitPacketState();
 165                     m_process_state = PROC_HDR;
 166                     break;
 167
 168                 case SEND_UNSYNCED:
 169                     resp = outputUnsyncedRawPacket();
 170                     if (m_update_on_unsync_packet_index != 0)
 171                     {
 172                         m_packet_index = m_update_on_unsync_packet_index;
 173                         m_update_on_unsync_packet_index = 0;
 174                     }
 175                     m_process_state = PROC_DATA;        // after dumping unsynced data, still in data mode.
 176                     break;
 177                 }
 178             }
 179             done = true;
 180         }
 181         catch(ocsdError &err)
 182         {
 183             done = true;
 184             m_interface->LogError(err);
 185             if( (err.getErrorCode() == OCSD_ERR_BAD_PACKET_SEQ) ||
 186                 (err.getErrorCode() == OCSD_ERR_INVALID_PCKT_HDR))
 187             {
 188                 // send invalid packets up the pipe to let the next stage decide what to do.
 189                 m_process_state = SEND_PKT;
 190                 done = false;
 191             }
 192             else
 193             {
 194                 // bail out on any other error.
 195                 resp = OCSD_RESP_FATAL_INVALID_DATA;
 196             }
 197         }
 198         catch(...)
 199         {
 200             done = true;
 201             /// vv bad at this point.
 202             resp = OCSD_RESP_FATAL_SYS_ERR;
 203             const ocsdError &fatal = ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_FAIL,m_packet_index,m_config.getTraceID(),"Unknown System Error decoding trace.");
 204             m_interface->LogError(fatal);
 205         }
 206     } while (!done);
 207
 208     *numBytesProcessed = m_trcIn.processed();
 209     return resp;
 210 }
 211
 212 ocsd_datapath_resp_t EtmV4IPktProcImpl::onEOT()
 213 {
 214     ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
 215     // if we have a partial packet then send to attached sinks
 216     if(m_currPacketData.size() != 0)
 217     {
 218         m_curr_packet.updateErrType(ETM4_PKT_I_INCOMPLETE_EOT);
 219         resp = outputPacket();
 220         InitPacketState();
 221     }
 222     return resp;
 223 }
 224
 225 ocsd_datapath_resp_t EtmV4IPktProcImpl::onReset()
 226 {
 227     // prepare for new decoding session
 228     InitProcessorState();
 229     return OCSD_RESP_CONT;
 230 }
 231
 232 ocsd_datapath_resp_t EtmV4IPktProcImpl::onFlush()
 233 {
 234     // packet processor never holds on to flushable data (may have partial packet,
 235     // but any full packets are immediately sent)
 236     return OCSD_RESP_CONT;
 237 }
 238
 239 void EtmV4IPktProcImpl::InitPacketState()
 240 {
 241     m_currPacketData.clear();
 242     m_curr_packet.initNextPacket(); // clear for next packet.
 243     m_update_on_unsync_packet_index = 0;
 244 }
 245
 246 void EtmV4IPktProcImpl::InitProcessorState()
 247 {
 248     InitPacketState();
 249     m_pIPktFn = &EtmV4IPktProcImpl::iNotSync;
 250     m_packet_index = 0;
 251     m_is_sync = false;
 252     m_first_trace_info = false;
 253     m_sent_notsync_packet = false;
 254     m_process_state = PROC_HDR;
 255     m_curr_packet.initStartState();
 256 }
 257
 258 ocsd_datapath_resp_t EtmV4IPktProcImpl::outputPacket()
 259 {
 260     ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
 261     resp = m_interface->outputOnAllInterfaces(m_packet_index,&m_curr_packet,&m_curr_packet.type,m_currPacketData);
 262     return resp;
 263 }
 264
 265 ocsd_datapath_resp_t EtmV4IPktProcImpl::outputUnsyncedRawPacket()
 266 {
 267     ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
 268
 269
 270     m_interface->outputRawPacketToMonitor(m_packet_index,&m_curr_packet,m_dump_unsynced_bytes,&m_currPacketData[0]);
 271
 272     if(!m_sent_notsync_packet)
 273     {
 274         resp = m_interface->outputDecodedPacket(m_packet_index,&m_curr_packet);
 275         m_sent_notsync_packet = true;
 276     }
 277
 278     if(m_currPacketData.size() <= m_dump_unsynced_bytes)
 279         m_currPacketData.clear();
 280     else
 281         m_currPacketData.erase(m_currPacketData.begin(),m_currPacketData.begin()+m_dump_unsynced_bytes);
 282
 283     return resp;
 284 }
 285
 286 void EtmV4IPktProcImpl::iNotSync(const uint8_t lastByte)
 287 {
 288     // is it an extension byte?
 289     if (lastByte == 0x00) // TBD : add check for forced sync in here?
 290     {
 291         if (m_currPacketData.size() > 1)
 292         {
 293             m_dump_unsynced_bytes = m_currPacketData.size() - 1;
 294             m_process_state = SEND_UNSYNCED;
 295             // outputting some data then update packet index after so output indexes accurate
 296             m_update_on_unsync_packet_index = m_blockIndex + m_trcIn.processed() - 1;
 297         }
 298         else
 299             m_packet_index = m_blockIndex + m_trcIn.processed() - 1;  // set it up now otherwise.
 300
 301         m_pIPktFn = m_i_table[lastByte].pptkFn;
 302     }
 303     else if (m_currPacketData.size() >= 8)
 304     {
 305         m_dump_unsynced_bytes = m_currPacketData.size();
 306         m_process_state = SEND_UNSYNCED;
 307         // outputting some data then update packet index after so output indexes accurate
 308         m_update_on_unsync_packet_index = m_blockIndex + m_trcIn.processed();
 309     }
 310 }
 311
 312 void EtmV4IPktProcImpl::iPktNoPayload(const uint8_t lastByte)
 313 {
 314     // some expansion may be required...
 315     switch(m_curr_packet.type)
 316     {
 317     case ETM4_PKT_I_ADDR_MATCH:
 318         m_curr_packet.setAddressExactMatch(lastByte & 0x3);
 319         break;
 320
 321     case ETM4_PKT_I_EVENT:
 322         m_curr_packet.setEvent(lastByte & 0xF);
 323         break;
 324
 325     case ETM4_PKT_I_NUM_DS_MKR:
 326     case ETM4_PKT_I_UNNUM_DS_MKR:
 327         m_curr_packet.setDataSyncMarker(lastByte & 0x7);
 328         break;
 329
 330     // these just need the packet type - no processing required.
 331     case ETM4_PKT_I_COND_FLUSH:
 332     case ETM4_PKT_I_EXCEPT_RTN:
 333     case ETM4_PKT_I_TRACE_ON:
 334     case ETM4_PKT_I_FUNC_RET:
 335     case ETM4_PKT_I_IGNORE:
 336     default: break;
 337     }
 338     m_process_state = SEND_PKT; // now just send it....
 339 }
 340
 341 void EtmV4IPktProcImpl::iPktReserved(const uint8_t lastByte)
 342 {
 343     m_curr_packet.updateErrType(ETM4_PKT_I_RESERVED, lastByte);   // swap type for err type
 344     throw ocsdError(OCSD_ERR_SEV_ERROR, OCSD_ERR_INVALID_PCKT_HDR,m_packet_index,m_config.getTraceID());
 345 }
 346
 347 void EtmV4IPktProcImpl::iPktInvalidCfg(const uint8_t lastByte)
 348 {
 349     m_curr_packet.updateErrType(ETM4_PKT_I_RESERVED_CFG, lastByte);   // swap type for err type
 350     throw ocsdError(OCSD_ERR_SEV_ERROR, OCSD_ERR_INVALID_PCKT_HDR, m_packet_index, m_config.getTraceID());
 351 }
 352
 353 void EtmV4IPktProcImpl::iPktExtension(const uint8_t lastByte)
 354 {
 355     if(m_currPacketData.size() == 2)
 356     {
 357         // not sync and not next by 0x00 - not sync sequence
 358         if(!m_is_sync && (lastByte != 0x00))
 359         {
 360             m_pIPktFn = &EtmV4IPktProcImpl::iNotSync;
 361             m_curr_packet.type = ETM4_PKT_I_NOTSYNC;
 362             return;
 363         }
 364
 365         switch(lastByte)
 366         {
 367         case 0x03: // discard packet.
 368             m_curr_packet.type = ETM4_PKT_I_DISCARD;
 369             m_process_state = SEND_PKT;
 370             break;
 371
 372         case 0x05:
 373             m_curr_packet.type = ETM4_PKT_I_OVERFLOW;
 374             m_process_state = SEND_PKT;
 375             break;
 376
 377         case 0x00:
 378             m_curr_packet.type = ETM4_PKT_I_ASYNC;
 379             m_pIPktFn = &EtmV4IPktProcImpl::iPktASync;  // handle subsequent bytes as async
 380             break;
 381
 382         default:
 383             m_curr_packet.err_type = m_curr_packet.type;
 384             m_curr_packet.type = ETM4_PKT_I_BAD_SEQUENCE;
 385             m_process_state = SEND_PKT;
 386             break;
 387         }
 388     }
 389 }
 390
 391 void EtmV4IPktProcImpl::iPktASync(const uint8_t lastByte)
 392 {
 393     if(lastByte != 0x00)
 394     {
 395         // not sync and not next by 0x00 - not sync sequence if < 12
 396         if(!m_is_sync && m_currPacketData.size() != 12)
 397         {
 398             m_pIPktFn = &EtmV4IPktProcImpl::iNotSync;
 399             m_curr_packet.type = ETM4_PKT_I_NOTSYNC;
 400             return;
 401         }
 402
 403         // 12 bytes and not valid sync sequence - not possible even if not synced
 404         m_process_state = SEND_PKT;
 405         if((m_currPacketData.size() != 12) || (lastByte != 0x80))
 406         {
 407             m_curr_packet.type = ETM4_PKT_I_BAD_SEQUENCE;
 408             m_curr_packet.err_type = ETM4_PKT_I_ASYNC;
 409         }
 410         else
 411              m_is_sync = true;  // found a sync packet, mark decoder as synchronised.
 412     }
 413     else if(m_currPacketData.size() == 12)
 414     {
 415         if(!m_is_sync)
 416         {
 417             // if we are not yet synced then ignore extra leading 0x00.
 418             m_dump_unsynced_bytes = 1;
 419             m_process_state = SEND_UNSYNCED;
 420         }
 421         else
 422         {
 423             // bad periodic ASYNC sequence.
 424             m_curr_packet.type = ETM4_PKT_I_BAD_SEQUENCE;
 425             m_curr_packet.err_type = ETM4_PKT_I_ASYNC;
 426             m_process_state = SEND_PKT;
 427         }
 428     }
 429 }
 430
 431 void EtmV4IPktProcImpl::iPktTraceInfo(const uint8_t lastByte)
 432 {
 433     if(m_currPacketData.size() == 1)    // header
 434     {
 435         //clear flags
 436         m_tinfo_sections.sectFlags = 0; // mark all sections as incomplete.
 437         m_tinfo_sections.ctrlBytes = 1; // assume only a single control section byte for now
 438
 439     }
 440     else if(m_currPacketData.size() == 2) // first payload control byte
 441     {
 442         // figure out which sections are absent and set to true - opposite of bitfeild in byte;
 443         m_tinfo_sections.sectFlags = (~lastByte) & TINFO_ALL_SECT;
 444
 445         // see if there is an extended control section, otherwise this byte is it.
 446         if((lastByte & 0x80) == 0x0)
 447             m_tinfo_sections.sectFlags |= TINFO_CTRL;
 448
 449     }
 450     else
 451     {
 452         if(!(m_tinfo_sections.sectFlags & TINFO_CTRL))
 453         {
 454             m_tinfo_sections.sectFlags |= (lastByte & 0x80) ? 0 : TINFO_CTRL;
 455             m_tinfo_sections.ctrlBytes++;
 456         }
 457         else if(!(m_tinfo_sections.sectFlags & TINFO_INFO_SECT))
 458             m_tinfo_sections.sectFlags |= (lastByte & 0x80) ? 0 : TINFO_INFO_SECT;
 459         else if(!(m_tinfo_sections.sectFlags & TINFO_KEY_SECT))
 460             m_tinfo_sections.sectFlags |= (lastByte & 0x80) ? 0 : TINFO_KEY_SECT;
 461         else if(!(m_tinfo_sections.sectFlags & TINFO_SPEC_SECT))
 462             m_tinfo_sections.sectFlags |= (lastByte & 0x80) ? 0 : TINFO_SPEC_SECT;
 463         else if(!(m_tinfo_sections.sectFlags & TINFO_CYCT_SECT))
 464             m_tinfo_sections.sectFlags |= (lastByte & 0x80) ? 0 : TINFO_CYCT_SECT;
 465     }
 466
 467     // all sections accounted for?
 468     if(m_tinfo_sections.sectFlags == TINFO_ALL)
 469     {
 470         // index of first section is number of payload control bytes + 1 for header byte
 471         unsigned idx = m_tinfo_sections.ctrlBytes + 1;
 472         uint32_t fieldVal = 0;
 473         uint8_t presSect = m_currPacketData[1] & TINFO_ALL_SECT;  // first payload control byte
 474
 475         m_curr_packet.clearTraceInfo();
 476
 477         if((presSect & TINFO_INFO_SECT) && (idx < m_currPacketData.size()))
 478         {
 479             idx += extractContField(m_currPacketData,idx,fieldVal);
 480             m_curr_packet.setTraceInfo(fieldVal);
 481         }
 482         if((presSect & TINFO_KEY_SECT) && (idx < m_currPacketData.size()))
 483         {
 484             idx += extractContField(m_currPacketData,idx,fieldVal);
 485             m_curr_packet.setTraceInfoKey(fieldVal);
 486         }
 487         if((presSect & TINFO_SPEC_SECT) && (idx < m_currPacketData.size()))
 488         {
 489             idx += extractContField(m_currPacketData,idx,fieldVal);
 490             m_curr_packet.setTraceInfoSpec(fieldVal);
 491         }
 492         if((presSect & TINFO_CYCT_SECT) && (idx < m_currPacketData.size()))
 493         {
 494             idx += extractContField(m_currPacketData,idx,fieldVal);
 495             m_curr_packet.setTraceInfoCyct(fieldVal);
 496         }
 497         m_process_state = SEND_PKT;
 498         m_first_trace_info = true;
 499     }
 500
 501 }
 502
 503 void EtmV4IPktProcImpl::iPktTimestamp(const uint8_t lastByte)
 504 {
 505     // process the header byte
 506     if(m_currPacketData.size() == 1)
 507     {
 508         m_ccount_done = (bool)((lastByte & 0x1) == 0); // 0 = not present
 509         m_ts_done = false;
 510         m_ts_bytes = 0;
 511     }
 512     else
 513     {
 514         if(!m_ts_done)
 515         {
 516             m_ts_bytes++;
 517             m_ts_done = (m_ts_bytes == 9) || ((lastByte & 0x80) == 0);
 518         }
 519         else if(!m_ccount_done)
 520         {
 521             m_ccount_done = (bool)((lastByte & 0x80) == 0);
 522             // TBD: check for oorange ccount - bad packet.
 523         }
 524     }
 525
 526     if(m_ts_done && m_ccount_done)
 527     {
 528         int idx = 1;
 529         uint64_t tsVal;
 530         int ts_bytes = extractContField64(m_currPacketData, idx, tsVal);
 531         int ts_bits = ts_bytes < 7 ? ts_bytes * 7 : 64;
 532
 533         if(!m_curr_packet.pkt_valid.bits.ts_valid && m_first_trace_info)
 534             ts_bits = 64;   // after trace info, missing bits are all 0.
 535
 536         m_curr_packet.setTS(tsVal,(uint8_t)ts_bits);
 537
 538         if((m_currPacketData[0] & 0x1) == 0x1)
 539         {
 540             uint32_t countVal, countMask;
 541
 542             idx += ts_bytes;
 543             extractContField(m_currPacketData, idx, countVal, 3);    // only 3 possible count bytes.
 544             countMask = (((uint32_t)1UL << m_config.ccSize()) - 1); // mask of the CC size
 545             countVal &= countMask;
 546             m_curr_packet.setCycleCount(countVal);
 547         }
 548
 549         m_process_state = SEND_PKT;
 550     }
 551 }
 552
 553 void EtmV4IPktProcImpl::iPktException(const uint8_t lastByte)
 554 {
 555     uint16_t excep_type = 0;
 556
 557     switch(m_currPacketData.size())
 558     {
 559     case 1: m_excep_size = 3; break;
 560     case 2: if((lastByte & 0x80) == 0x00)
 561                 m_excep_size = 2;
 562             break;
 563     }
 564
 565     if(m_currPacketData.size() ==  (unsigned)m_excep_size)
 566     {
 567         excep_type =  (m_currPacketData[1] >> 1) & 0x1F;
 568         uint8_t addr_interp = (m_currPacketData[1] & 0x40) >> 5 | (m_currPacketData[1] & 0x1);
 569         uint8_t m_fault_pending = 0;
 570         uint8_t m_type = (m_config.coreProfile() == profile_CortexM) ? 1 : 0;
 571
 572         // extended exception packet (probably M class);
 573         if(m_currPacketData[1] & 0x80)
 574         {
 575             excep_type |= ((uint16_t)m_currPacketData[2] & 0x1F) << 5;
 576             m_fault_pending = (m_currPacketData[2] >> 5)  & 0x1;
 577         }
 578         m_curr_packet.setExceptionInfo(excep_type,addr_interp,m_fault_pending, m_type);
 579         m_process_state = SEND_PKT;
 580
 581         // allow the standard address packet handlers to process the address packet field for the exception.
 582     }
 583 }
 584
 585 void EtmV4IPktProcImpl::iPktCycleCntF123(const uint8_t lastByte)
 586 {
 587     ocsd_etmv4_i_pkt_type format = m_curr_packet.type;
 588
 589     if( m_currPacketData.size() == 1)
 590     {
 591         m_count_done = m_commit_done = false;
 592         m_has_count = true;
 593
 594         if(format == ETM4_PKT_I_CCNT_F3)
 595         {
 596             // no commit section for TRCIDR0.COMMOPT == 1
 597             if(!m_config.commitOpt1())
 598             {
 599                 m_curr_packet.setCommitElements(((lastByte >> 2) & 0x3) + 1);
 600             }
 601             // TBD: warning of non-valid CC threshold here?
 602             m_curr_packet.setCycleCount(m_curr_packet.getCCThreshold() + (lastByte & 0x3));
 603             m_process_state = SEND_PKT;
 604         }
 605         else if(format == ETM4_PKT_I_CCNT_F1)
 606         {
 607             if((lastByte & 0x1) == 0x1)
 608             {
 609                 m_has_count = false;
 610                 m_count_done = true;
 611             }
 612
 613             // no commit section for TRCIDR0.COMMOPT == 1
 614             if(m_config.commitOpt1())
 615                 m_commit_done = true;
 616         }
 617     }
 618     else if((format == ETM4_PKT_I_CCNT_F2) && ( m_currPacketData.size() == 2))
 619     {
 620         int commit_offset = ((lastByte & 0x1) == 0x1) ? ((int)m_config.MaxSpecDepth() - 15) : 1;
 621         int commit_elements = ((lastByte >> 4) & 0xF);
 622         commit_elements += commit_offset;
 623
 624         // TBD: warning if commit elements < 0?
 625
 626         m_curr_packet.setCycleCount(m_curr_packet.getCCThreshold() + (lastByte & 0xF));
 627         m_curr_packet.setCommitElements(commit_elements);
 628         m_process_state = SEND_PKT;
 629     }
 630     else
 631     {
 632         // F1 and size 2 or more
 633         if(!m_commit_done)
 634             m_commit_done = ((lastByte & 0x80) == 0x00);
 635         else if(!m_count_done)
 636             m_count_done = ((lastByte & 0x80) == 0x00);
 637     }
 638
 639     if((format == ETM4_PKT_I_CCNT_F1) && m_commit_done && m_count_done)
 640     {
 641         int idx = 1; // index into buffer for payload data.
 642         uint32_t field_value = 0;
 643         // no commit section for TRCIDR0.COMMOPT == 1
 644         if(!m_config.commitOpt1())
 645         {
 646             idx += extractContField(m_currPacketData,idx,field_value);
 647             m_curr_packet.setCommitElements(field_value);
 648         }
 649                 if (m_has_count)
 650                 {
 651                         extractContField(m_currPacketData, idx, field_value, 3);
 652                         m_curr_packet.setCycleCount(field_value + m_curr_packet.getCCThreshold());
 653                 }
 654                 else
 655                         m_curr_packet.setCycleCount(0); /* unknown CC marked as 0 after overflow */
 656         m_process_state = SEND_PKT;
 657     }
 658 }
 659
 660 void EtmV4IPktProcImpl::iPktSpeclRes(const uint8_t lastByte)
 661 {
 662     if(m_currPacketData.size() == 1)
 663     {
 664         switch(m_curr_packet.getType())
 665         {
 666         case ETM4_PKT_I_MISPREDICT:
 667         case ETM4_PKT_I_CANCEL_F2:
 668             switch(lastByte & 0x3)
 669             {
 670             case 0x1: m_curr_packet.setAtomPacket(ATOM_PATTERN, 0x1, 1); break; // E
 671             case 0x2: m_curr_packet.setAtomPacket(ATOM_PATTERN, 0x3, 2); break; // EE
 672             case 0x3: m_curr_packet.setAtomPacket(ATOM_PATTERN, 0x0, 1); break; // N
 673             }
 674             if(m_curr_packet.getType() == ETM4_PKT_I_CANCEL_F2)
 675                 m_curr_packet.setCancelElements(1);
 676             m_process_state = SEND_PKT;
 677             break;
 678
 679         case ETM4_PKT_I_CANCEL_F3:
 680             if(lastByte & 0x1)
 681                 m_curr_packet.setAtomPacket(ATOM_PATTERN, 0x1, 1); // E
 682             m_curr_packet.setCancelElements(((lastByte >> 1) & 0x3) + 2);
 683             m_process_state = SEND_PKT;
 684             break;
 685         }
 686     }
 687     else
 688     {
 689         if((lastByte & 0x80) == 0x00)
 690         {
 691             uint32_t field_val = 0;
 692             extractContField(m_currPacketData,1,field_val);
 693             if(m_curr_packet.getType() == ETM4_PKT_I_COMMIT)
 694                 m_curr_packet.setCommitElements(field_val);
 695             else
 696                 m_curr_packet.setCancelElements(field_val);
 697             // TBD: sanity check with max spec depth here?
 698             m_process_state = SEND_PKT;
 699         }
 700     }
 701 }
 702
 703 void EtmV4IPktProcImpl::iPktCondInstr(const uint8_t lastByte)
 704 {
 705     bool bF1Done = false;
 706
 707     if(m_currPacketData.size() == 1)
 708     {
 709         if(m_curr_packet.getType() == ETM4_PKT_I_COND_I_F2)
 710         {
 711             m_curr_packet.setCondIF2(lastByte & 0x3);
 712             m_process_state = SEND_PKT;
 713         }
 714
 715     }
 716     else if(m_currPacketData.size() == 2)
 717     {
 718         if(m_curr_packet.getType() == ETM4_PKT_I_COND_I_F3)   // f3 two bytes long
 719         {
 720           uint8_t num_c_elem = ((lastByte >> 1) & 0x3F) + (lastByte & 0x1);
 721             m_curr_packet.setCondIF3(num_c_elem,(bool)((lastByte & 0x1) == 0x1));
 722             // TBD: check for 0 num_c_elem in here.
 723             m_process_state = SEND_PKT;
 724         }
 725         else
 726         {
 727             bF1Done = ((lastByte & 0x80) == 0x00);
 728         }
 729     }
 730     else
 731     {
 732         bF1Done = ((lastByte & 0x80) == 0x00);
 733     }
 734
 735     if(bF1Done)
 736     {
 737         uint32_t cond_key = 0;
 738         extractContField(m_currPacketData, 1, cond_key);
 739         m_process_state = SEND_PKT;
 740     }
 741 }
 742
 743 void EtmV4IPktProcImpl::iPktCondResult(const uint8_t lastByte)
 744 {
 745     if(m_currPacketData.size() == 1)
 746     {
 747         m_F1P1_done = false;  // F1 payload 1 done
 748         m_F1P2_done = false;  // F1 payload 2 done
 749         m_F1has_P2 = false;   // F1 has a payload 2
 750
 751         switch(m_curr_packet.getType())
 752         {
 753         case ETM4_PKT_I_COND_RES_F1:
 754
 755             m_F1has_P2 = true;
 756             if((lastByte & 0xFC) == 0x6C)// only one payload set
 757             {
 758                 m_F1P2_done = true;
 759                 m_F1has_P2 = false;
 760             }
 761             break;
 762
 763         case ETM4_PKT_I_COND_RES_F2:
 764             m_curr_packet.setCondRF2((lastByte & 0x4) ? 2 : 1, lastByte & 0x3);
 765             m_process_state = SEND_PKT;
 766             break;
 767
 768         case ETM4_PKT_I_COND_RES_F3:
 769             break;
 770
 771         case ETM4_PKT_I_COND_RES_F4:
 772             m_curr_packet.setCondRF4(lastByte & 0x3);
 773             m_process_state = SEND_PKT;
 774             break;
 775         }
 776     }
 777     else if((m_curr_packet.getType() == ETM4_PKT_I_COND_RES_F3) && (m_currPacketData.size() == 2))
 778     {
 779         // 2nd F3 packet
 780         uint16_t f3_tokens = 0;
 781         f3_tokens = (uint16_t)m_currPacketData[1];
 782         f3_tokens |= ((uint16_t)m_currPacketData[0] & 0xf) << 8;
 783         m_curr_packet.setCondRF3(f3_tokens);
 784         m_process_state = SEND_PKT;
 785     }
 786     else  // !first packet  - F1
 787     {
 788         if(!m_F1P1_done)
 789             m_F1P1_done = ((lastByte & 0x80) == 0x00);
 790         else if(!m_F1P2_done)
 791             m_F1P2_done = ((lastByte & 0x80) == 0x00);
 792
 793         if(m_F1P1_done && m_F1P2_done)
 794         {
 795             int st_idx = 1;
 796             uint32_t key[2];
 797             uint8_t result[2];
 798             uint8_t CI[2];
 799
 800             st_idx+= extractCondResult(m_currPacketData,st_idx,key[0],result[0]);
 801             CI[0] = m_currPacketData[0] & 0x1;
 802             if(m_F1has_P2) // 2nd payload?
 803             {
 804                 extractCondResult(m_currPacketData,st_idx,key[1],result[1]);
 805                 CI[1] = (m_currPacketData[0] >> 1) & 0x1;
 806             }
 807             m_curr_packet.setCondRF1(key,result,CI,m_F1has_P2);
 808             m_process_state = SEND_PKT;
 809         }
 810     }
 811 }
 812
 813 void EtmV4IPktProcImpl::iPktContext(const uint8_t lastByte)
 814 {
 815     bool bSendPacket = false;
 816
 817     if(m_currPacketData.size() == 1)
 818     {
 819         if((lastByte & 0x1) == 0)
 820         {
 821             m_curr_packet.setContextInfo(false);    // no update context packet (ctxt same as last time).
 822             m_process_state = SEND_PKT;
 823         }
 824     }
 825     else if(m_currPacketData.size() == 2)
 826     {
 827         if((lastByte & 0xC0) == 0) // no VMID or CID
 828         {
 829             bSendPacket = true;
 830         }
 831         else
 832         {
 833             m_vmidBytes = ((lastByte & 0x40) == 0x40) ? (m_config.vmidSize()/8) : 0;
 834             m_ctxtidBytes = ((lastByte & 0x80) == 0x80) ? (m_config.cidSize()/8) : 0;
 835         }
 836     }
 837     else    // 3rd byte onwards
 838     {
 839         if(m_vmidBytes > 0)
 840             m_vmidBytes--;
 841         else if(m_ctxtidBytes > 0)
 842             m_ctxtidBytes--;
 843
 844         if((m_ctxtidBytes == 0) && (m_vmidBytes == 0))
 845             bSendPacket = true;
 846     }
 847
 848     if(bSendPacket)
 849     {
 850         extractAndSetContextInfo(m_currPacketData,1);
 851         m_process_state = SEND_PKT;
 852     }
 853 }
 854
 855 void EtmV4IPktProcImpl::extractAndSetContextInfo(const std::vector<uint8_t> &buffer, const int st_idx)
 856 {
 857     // on input, buffer index points at the info byte - always present
 858     uint8_t infoByte = m_currPacketData[st_idx];
 859
 860     m_curr_packet.setContextInfo(true, (infoByte & 0x3), (infoByte >> 5) & 0x1, (infoByte >> 4) & 0x1);
 861
 862     // see if there are VMID and CID bytes, and how many.
 863     int nVMID_bytes = ((infoByte & 0x40) == 0x40) ? (m_config.vmidSize()/8) : 0;
 864     int nCtxtID_bytes = ((infoByte & 0x80) == 0x80) ? (m_config.cidSize()/8) : 0;
 865
 866     // extract any VMID and CID
 867     int payload_idx = st_idx+1;
 868     if(nVMID_bytes)
 869     {
 870         uint32_t VMID = 0;
 871         for(int i = 0; i < nVMID_bytes; i++)
 872         {
 873             VMID |= ((uint32_t)m_currPacketData[i+payload_idx] << i*8);
 874         }
 875         payload_idx += nVMID_bytes;
 876         m_curr_packet.setContextVMID(VMID);
 877     }
 878
 879     if(nCtxtID_bytes)
 880     {
 881         uint32_t CID = 0;
 882         for(int i = 0; i < nCtxtID_bytes; i++)
 883         {
 884             CID |= ((uint32_t)m_currPacketData[i+payload_idx] << i*8);
 885         }
 886         m_curr_packet.setContextCID(CID);
 887     }
 888 }
 889
 890 void EtmV4IPktProcImpl::iPktAddrCtxt(const uint8_t lastByte)
 891 {
 892     if( m_currPacketData.size() == 1)
 893     {
 894         m_addrIS = 0;
 895         m_addrBytes = 4;
 896         m_bAddr64bit = false;
 897         m_vmidBytes = 0;
 898         m_ctxtidBytes = 0;
 899         m_bCtxtInfoDone = false;
 900
 901         switch(m_curr_packet.type)
 902         {
 903         case ETM4_PKT_I_ADDR_CTXT_L_32IS1:
 904             m_addrIS = 1;
 905         case ETM4_PKT_I_ADDR_CTXT_L_32IS0:
 906             break;
 907
 908         case ETM4_PKT_I_ADDR_CTXT_L_64IS1:
 909             m_addrIS = 1;
 910         case ETM4_PKT_I_ADDR_CTXT_L_64IS0:
 911             m_addrBytes = 8;
 912             m_bAddr64bit = true;
 913             break;
 914         }
 915     }
 916     else
 917     {
 918         if(m_addrBytes == 0)
 919         {
 920             if(m_bCtxtInfoDone == false)
 921             {
 922                 m_bCtxtInfoDone = true;
 923                 m_vmidBytes = ((lastByte & 0x40) == 0x40) ? (m_config.vmidSize()/8) : 0;
 924                 m_ctxtidBytes = ((lastByte & 0x80) == 0x80) ? (m_config.cidSize()/8) : 0;
 925             }
 926             else
 927             {
 928                 if( m_vmidBytes > 0)
 929                      m_vmidBytes--;
 930                 else if(m_ctxtidBytes > 0)
 931                     m_ctxtidBytes--;
 932             }
 933         }
 934         else
 935             m_addrBytes--;
 936
 937         if((m_addrBytes == 0) && m_bCtxtInfoDone && (m_vmidBytes == 0) && (m_ctxtidBytes == 0))
 938         {
 939             int st_idx = 1;
 940             if(m_bAddr64bit)
 941             {
 942                 uint64_t val64;
 943                 st_idx+=extract64BitLongAddr(m_currPacketData,st_idx,m_addrIS,val64);
 944                 m_curr_packet.set64BitAddress(val64,m_addrIS);
 945             }
 946             else
 947             {
 948                 uint32_t val32;
 949                 st_idx+=extract32BitLongAddr(m_currPacketData,st_idx,m_addrIS,val32);
 950                 m_curr_packet.set32BitAddress(val32,m_addrIS);
 951             }
 952             extractAndSetContextInfo(m_currPacketData,st_idx);
 953             m_process_state = SEND_PKT;
 954         }
 955     }
 956 }
 957
 958 void EtmV4IPktProcImpl::iPktShortAddr(const uint8_t lastByte)
 959 {
 960     if (m_currPacketData.size() == 1)
 961     {
 962         m_addr_done = false;
 963         m_addrIS = 0;
 964         if (lastByte == ETM4_PKT_I_ADDR_S_IS1)
 965             m_addrIS = 1;
 966     }
 967     else if(!m_addr_done)
 968     {
 969         m_addr_done = (m_currPacketData.size() == 3) || ((lastByte & 0x80) == 0x00);
 970     }
 971
 972     if(m_addr_done)
 973     {
 974         uint32_t addr_val = 0;
 975         int bits = 0;
 976
 977         extractShortAddr(m_currPacketData,1,m_addrIS,addr_val,bits);
 978         m_curr_packet.updateShortAddress(addr_val,m_addrIS,(uint8_t)bits);
 979         m_process_state = SEND_PKT;
 980     }
 981 }
 982
 983 int EtmV4IPktProcImpl::extractShortAddr(const std::vector<uint8_t> &buffer, const int st_idx, const uint8_t IS, uint32_t &value, int &bits)
 984 {
 985     int IS_shift = (IS == 0) ? 2 : 1;
 986     int idx = 0;
 987
 988     bits = 7;   // at least 7 bits
 989     value = 0;
 990     value |= ((uint32_t)(buffer[st_idx+idx] & 0x7F)) << IS_shift;
 991
 992     if(m_currPacketData[st_idx+idx] & 0x80)
 993     {
 994         idx++;
 995         value |= ((uint32_t)m_currPacketData[st_idx+idx]) <<  (7 + IS_shift);
 996         bits += 8;
 997     }
 998     idx++;
 999     bits += IS_shift;
1000     return idx;
1001 }
1002
1003 void EtmV4IPktProcImpl::iPktLongAddr(const uint8_t lastByte)
1004 {
1005     if(m_currPacketData.size() == 1)
1006     {
1007         // init the intra-byte data
1008         m_addrIS = 0;
1009         m_bAddr64bit = false;
1010         m_addrBytes = 4;
1011
1012         switch(m_curr_packet.type)
1013         {
1014         case ETM4_PKT_I_ADDR_L_32IS1:
1015             m_addrIS = 1;
1016         case ETM4_PKT_I_ADDR_L_32IS0:
1017             m_addrBytes = 4;
1018             break;
1019
1020         case ETM4_PKT_I_ADDR_L_64IS1:
1021             m_addrIS = 1;
1022         case ETM4_PKT_I_ADDR_L_64IS0:
1023             m_addrBytes = 8;
1024             m_bAddr64bit = true;
1025             break;
1026         }
1027     }
1028     if(m_currPacketData.size() == (unsigned)(1+m_addrBytes))
1029     {
1030         int st_idx = 1;
1031         if(m_bAddr64bit)
1032         {
1033             uint64_t val64;
1034             st_idx+=extract64BitLongAddr(m_currPacketData,st_idx,m_addrIS,val64);
1035             m_curr_packet.set64BitAddress(val64,m_addrIS);
1036         }
1037         else
1038         {
1039             uint32_t val32;
1040             st_idx+=extract32BitLongAddr(m_currPacketData,st_idx,m_addrIS,val32);
1041             m_curr_packet.set32BitAddress(val32,m_addrIS);
1042         }
1043         m_process_state = SEND_PKT;
1044     }
1045 }
1046
1047 void EtmV4IPktProcImpl::iPktQ(const uint8_t lastByte)
1048 {
1049     if(m_currPacketData.size() == 1)
1050     {
1051         m_Q_type = lastByte & 0xF;
1052
1053         m_addrBytes = 0;
1054         m_count_done = false;
1055         m_has_addr = false;
1056         m_addr_short = true;
1057         m_addr_match = false;
1058         m_addrIS = 1;
1059         m_QE = 0;
1060
1061         switch(m_Q_type)
1062         {
1063             // count only - implied address.
1064         case 0x0:
1065         case 0x1:
1066         case 0x2:
1067             m_addr_match = true;
1068             m_has_addr = true;
1069             m_QE = m_Q_type & 0x3;
1070         case 0xC:
1071             break;
1072
1073             // count + short address
1074         case 0x5:
1075             m_addrIS = 0;
1076         case 0x6:
1077             m_has_addr = true;
1078             m_addrBytes = 2;  // short IS0/1
1079             break;
1080
1081             // count + long address
1082         case 0xA:
1083             m_addrIS = 0;
1084         case 0xB:
1085             m_has_addr = true;
1086             m_addr_short = false;
1087             m_addrBytes = 4; // long IS0/1
1088             break;
1089
1090             // no count, no address
1091         case 0xF:
1092             m_count_done = true;
1093             break;
1094
1095             // reserved values 0x3, 0x4, 0x7, 0x8, 0x9, 0xD, 0xE
1096         default:
1097             m_curr_packet.err_type =  m_curr_packet.type;
1098             m_curr_packet.type = ETM4_PKT_I_BAD_SEQUENCE;
1099             //SendBadIPacket( PKT_BAD_SEQUENCE, "ERROR: Bad Q packet type", PKT_Q );
1100             break;
1101         }
1102     }
1103     else
1104     {
1105         if(m_addrBytes > 0)
1106         {
1107             if(m_addr_short && m_addrBytes == 2)  // short
1108             {
1109                 if((lastByte & 0x80) == 0x00)
1110                     m_addrBytes--;        // short version can have just single byte.
1111             }
1112             m_addrBytes--;
1113         }
1114         else if(!m_count_done)
1115         {
1116             m_count_done = ((lastByte & 0x80) == 0x00);
1117         }
1118     }
1119
1120     if(((m_addrBytes == 0) && m_count_done))
1121     {
1122         int idx = 1; // move past the header
1123         int bits = 0;
1124         uint32_t q_addr;
1125         uint32_t q_count;
1126
1127         if(m_has_addr)
1128         {
1129             if(m_addr_match)
1130             {
1131                 m_curr_packet.setAddressExactMatch(m_QE);
1132             }
1133             else if(m_addr_short)
1134             {
1135                 idx+=extractShortAddr(m_currPacketData,idx,m_addrIS,q_addr,bits);
1136                 m_curr_packet.updateShortAddress(q_addr,m_addrIS,(uint8_t)bits);
1137             }
1138             else
1139             {
1140                 idx+=extract32BitLongAddr(m_currPacketData,idx,m_addrIS,q_addr);
1141                 m_curr_packet.set32BitAddress(q_addr,m_addrIS);
1142             }
1143         }
1144
1145         if(m_Q_type != 0xF)
1146         {
1147             extractContField(m_currPacketData,idx,q_count);
1148             m_curr_packet.setQType(true,q_count,m_has_addr,m_addr_match,m_Q_type);
1149         }
1150         else
1151         {
1152             m_curr_packet.setQType(false,0,false,false,0xF);
1153         }
1154         m_process_state = SEND_PKT;
1155     }
1156
1157 }
1158
1159 void EtmV4IPktProcImpl::iAtom(const uint8_t lastByte)
1160 {
1161     // patterns lsbit = oldest atom, ms bit = newest.
1162     static const uint32_t f4_patterns[] = {
1163         0xE, // EEEN
1164         0x0, // NNNN
1165         0xA, // ENEN
1166         0x5  // NENE
1167     };
1168
1169     uint8_t pattIdx = 0, pattCount = 0;
1170     uint32_t pattern;
1171
1172     // atom packets are single byte, no payload.
1173     switch(m_curr_packet.type)
1174     {
1175     case ETM4_PKT_I_ATOM_F1:
1176         m_curr_packet.setAtomPacket(ATOM_PATTERN,(lastByte & 0x1), 1); // 1xE or N
1177         break;
1178
1179     case ETM4_PKT_I_ATOM_F2:
1180         m_curr_packet.setAtomPacket(ATOM_PATTERN,(lastByte & 0x3), 2); // 2x (E or N)
1181         break;
1182
1183     case ETM4_PKT_I_ATOM_F3:
1184         m_curr_packet.setAtomPacket(ATOM_PATTERN,(lastByte & 0x7), 3); // 3x (E or N)
1185         break;
1186
1187     case ETM4_PKT_I_ATOM_F4:
1188         m_curr_packet.setAtomPacket(ATOM_PATTERN,f4_patterns[(lastByte & 0x3)], 4); // 4 atom pattern
1189         break;
1190
1191     case ETM4_PKT_I_ATOM_F5:
1192         pattIdx = ((lastByte & 0x20) >> 3) | (lastByte & 0x3);
1193         switch(pattIdx)
1194         {
1195         case 5: // 0b101
1196             m_curr_packet.setAtomPacket(ATOM_PATTERN,0x1E, 5); // 5 atom pattern EEEEN
1197             break;
1198
1199         case 1: // 0b001
1200             m_curr_packet.setAtomPacket(ATOM_PATTERN,0x00, 5); // 5 atom pattern NNNNN
1201             break;
1202
1203         case 2: //0b010
1204             m_curr_packet.setAtomPacket(ATOM_PATTERN,0x0A, 5); // 5 atom pattern NENEN
1205             break;
1206
1207         case 3: //0b011
1208             m_curr_packet.setAtomPacket(ATOM_PATTERN,0x15, 5); // 5 atom pattern ENENE
1209             break;
1210
1211         default:
1212             // TBD: warn about invalid pattern in here.
1213             break;
1214         }
1215         break;
1216
1217     case ETM4_PKT_I_ATOM_F6:
1218         pattCount = (lastByte & 0x1F) + 3;  // count of E's
1219         // TBD: check 23 or less at this point?
1220         pattern = ((uint32_t)0x1 << pattCount) - 1; // set pattern to string of E's
1221         if((lastByte & 0x20) == 0x00)   // last atom is E?
1222             pattern |= ((uint32_t)0x1 << pattCount);
1223         m_curr_packet.setAtomPacket(ATOM_PATTERN,pattern, pattCount+1);
1224         break;
1225     }
1226
1227     m_process_state = SEND_PKT;
1228 }
1229
1230 // header byte processing is table driven.
1231 void EtmV4IPktProcImpl::BuildIPacketTable()
1232 {
1233     // initialise everything as reserved.
1234     for(int i = 0; i < 256; i++)
1235     {
1236         m_i_table[i].pkt_type = ETM4_PKT_I_RESERVED;
1237         m_i_table[i].pptkFn = &EtmV4IPktProcImpl::iPktReserved;
1238     }
1239
1240     // 0x00 - extension
1241     m_i_table[0x00].pkt_type = ETM4_PKT_I_EXTENSION;
1242     m_i_table[0x00].pptkFn   = &EtmV4IPktProcImpl::iPktExtension;
1243
1244     // 0x01 - Trace info
1245     m_i_table[0x01].pkt_type = ETM4_PKT_I_TRACE_INFO;
1246     m_i_table[0x01].pptkFn   = &EtmV4IPktProcImpl::iPktTraceInfo;
1247
1248     // b0000001x - timestamp
1249     m_i_table[0x02].pkt_type = ETM4_PKT_I_TIMESTAMP;
1250     m_i_table[0x02].pptkFn   = &EtmV4IPktProcImpl::iPktTimestamp;
1251     m_i_table[0x03].pkt_type = ETM4_PKT_I_TIMESTAMP;
1252     m_i_table[0x03].pptkFn   = &EtmV4IPktProcImpl::iPktTimestamp;
1253
1254     // b0000 0100 - trace on
1255     m_i_table[0x04].pkt_type = ETM4_PKT_I_TRACE_ON;
1256     m_i_table[0x04].pptkFn   = &EtmV4IPktProcImpl::iPktNoPayload;
1257
1258
1259     // b0000 0101 - Funct ret V8M
1260     m_i_table[0x05].pkt_type = ETM4_PKT_I_FUNC_RET;
1261     if ((m_config.coreProfile() == profile_CortexM) &&
1262         (OCSD_IS_V8_ARCH(m_config.archVersion())) &&
1263         (m_config.FullVersion() >= 0x42))
1264     {
1265         m_i_table[0x05].pptkFn = &EtmV4IPktProcImpl::iPktNoPayload;
1266     }
1267
1268     // b0000 0110 - exception
1269     m_i_table[0x06].pkt_type = ETM4_PKT_I_EXCEPT;
1270     m_i_table[0x06].pptkFn   = &EtmV4IPktProcImpl::iPktException;
1271
1272     // b0000 0111 - exception return
1273     m_i_table[0x07].pkt_type = ETM4_PKT_I_EXCEPT_RTN;
1274     m_i_table[0x07].pptkFn = &EtmV4IPktProcImpl::iPktNoPayload;
1275
1276     // b0000 110x - cycle count f2
1277     // b0000 111x - cycle count f1
1278     for(int i = 0; i < 4; i++)
1279     {
1280         m_i_table[0x0C+i].pkt_type = (i >= 2) ? ETM4_PKT_I_CCNT_F1 : ETM4_PKT_I_CCNT_F2;
1281         m_i_table[0x0C+i].pptkFn   = &EtmV4IPktProcImpl::iPktCycleCntF123;
1282     }
1283
1284     // b0001 xxxx - cycle count f3
1285     for(int i = 0; i < 16; i++)
1286     {
1287         m_i_table[0x10+i].pkt_type = ETM4_PKT_I_CCNT_F3;
1288         m_i_table[0x10+i].pptkFn   = &EtmV4IPktProcImpl::iPktCycleCntF123;
1289     }
1290
1291     // b0010 0xxx - NDSM
1292     for(int i = 0; i < 8; i++)
1293     {
1294         m_i_table[0x20 + i].pkt_type = ETM4_PKT_I_NUM_DS_MKR;
1295         if (m_config.enabledDataTrace())
1296             m_i_table[0x20+i].pptkFn = &EtmV4IPktProcImpl::iPktNoPayload;
1297         else
1298             m_i_table[0x20+i].pptkFn = &EtmV4IPktProcImpl::iPktInvalidCfg;
1299     }
1300
1301     // b0010 10xx, b0010 1100 - UDSM
1302     for(int i = 0; i < 5; i++)
1303     {
1304         m_i_table[0x28+i].pkt_type = ETM4_PKT_I_UNNUM_DS_MKR;
1305         if (m_config.enabledDataTrace())
1306             m_i_table[0x28+i].pptkFn = &EtmV4IPktProcImpl::iPktNoPayload;
1307         else
1308             m_i_table[0x28+i].pptkFn = &EtmV4IPktProcImpl::iPktInvalidCfg;
1309     }
1310
1311     // b0010 1101 - commit
1312     m_i_table[0x2D].pkt_type = ETM4_PKT_I_COMMIT;
1313     m_i_table[0x2D].pptkFn   = &EtmV4IPktProcImpl::iPktSpeclRes;
1314
1315     // b0010 111x - cancel f1
1316     for(int i = 0; i < 2; i++)
1317     {
1318         // G++ doesn't understand [0x2E+i] so...
1319         int idx = i + 0x2E;
1320         m_i_table[idx].pkt_type = ETM4_PKT_I_CANCEL_F1;
1321         m_i_table[idx].pptkFn   = &EtmV4IPktProcImpl::iPktSpeclRes;
1322     }
1323
1324     // b0011 00xx - mis predict
1325     for(int i = 0; i < 4; i++)
1326     {
1327         m_i_table[0x30+i].pkt_type = ETM4_PKT_I_MISPREDICT;
1328         m_i_table[0x30+i].pptkFn   =  &EtmV4IPktProcImpl::iPktSpeclRes;
1329     }
1330
1331     // b0011 01xx - cancel f2
1332     for(int i = 0; i < 4; i++)
1333     {
1334         m_i_table[0x34+i].pkt_type = ETM4_PKT_I_CANCEL_F2;
1335         m_i_table[0x34+i].pptkFn   =  &EtmV4IPktProcImpl::iPktSpeclRes;
1336     }
1337
1338     // b0011 1xxx - cancel f3
1339     for(int i = 0; i < 8; i++)
1340     {
1341         m_i_table[0x38+i].pkt_type = ETM4_PKT_I_CANCEL_F3;
1342         m_i_table[0x38+i].pptkFn   =  &EtmV4IPktProcImpl::iPktSpeclRes;
1343     }
1344
1345     bool bCondValid = m_config.hasCondTrace() && m_config.enabledCondITrace();
1346
1347     // b0100 000x, b0100 0010 - cond I f2
1348     for (int i = 0; i < 3; i++)
1349     {
1350         m_i_table[0x40 + i].pkt_type = ETM4_PKT_I_COND_I_F2;
1351         if (bCondValid)
1352             m_i_table[0x40 + i].pptkFn = &EtmV4IPktProcImpl::iPktCondInstr;
1353         else
1354             m_i_table[0x40 + i].pptkFn = &EtmV4IPktProcImpl::iPktInvalidCfg;
1355     }
1356
1357     // b0100 0011 - cond flush
1358     m_i_table[0x43].pkt_type = ETM4_PKT_I_COND_FLUSH;
1359     if (bCondValid)
1360         m_i_table[0x43].pptkFn = &EtmV4IPktProcImpl::iPktNoPayload;
1361     else
1362         m_i_table[0x43].pptkFn = &EtmV4IPktProcImpl::iPktInvalidCfg;
1363
1364     // b0100 010x, b0100 0110 - cond res f4
1365     for (int i = 0; i < 3; i++)
1366     {
1367         m_i_table[0x44 + i].pkt_type = ETM4_PKT_I_COND_RES_F4;
1368         if (bCondValid)
1369             m_i_table[0x44 + i].pptkFn = &EtmV4IPktProcImpl::iPktCondResult;
1370         else
1371             m_i_table[0x44 + i].pptkFn = &EtmV4IPktProcImpl::iPktInvalidCfg;
1372     }
1373
1374     // b0100 100x, b0100 0110 - cond res f2
1375     // b0100 110x, b0100 1110 - cond res f2
1376     for (int i = 0; i < 3; i++)
1377     {
1378         m_i_table[0x48 + i].pkt_type = ETM4_PKT_I_COND_RES_F2;
1379         if (bCondValid)
1380             m_i_table[0x48 + i].pptkFn = &EtmV4IPktProcImpl::iPktCondResult;
1381         else
1382             m_i_table[0x48 + i].pptkFn = &EtmV4IPktProcImpl::iPktInvalidCfg;
1383     }
1384     for (int i = 0; i < 3; i++)
1385     {
1386         m_i_table[0x4C + i].pkt_type = ETM4_PKT_I_COND_RES_F2;
1387         if (bCondValid)
1388             m_i_table[0x4C + i].pptkFn = &EtmV4IPktProcImpl::iPktCondResult;
1389         else
1390             m_i_table[0x4C + i].pptkFn = &EtmV4IPktProcImpl::iPktInvalidCfg;
1391     }
1392
1393     // b0101xxxx - cond res f3
1394     for (int i = 0; i < 16; i++)
1395     {
1396         m_i_table[0x50 + i].pkt_type = ETM4_PKT_I_COND_RES_F3;
1397         if (bCondValid)
1398             m_i_table[0x50 + i].pptkFn = &EtmV4IPktProcImpl::iPktCondResult;
1399         else
1400             m_i_table[0x50 + i].pptkFn = &EtmV4IPktProcImpl::iPktInvalidCfg;
1401     }
1402
1403     // b011010xx - cond res f1
1404     for (int i = 0; i < 4; i++)
1405     {
1406         m_i_table[0x68 + i].pkt_type = ETM4_PKT_I_COND_RES_F1;
1407         if (bCondValid)
1408             m_i_table[0x68 + i].pptkFn = &EtmV4IPktProcImpl::iPktCondResult;
1409         else
1410             m_i_table[0x68 + i].pptkFn = &EtmV4IPktProcImpl::iPktInvalidCfg;
1411     }
1412
1413     // b0110 1100 - cond instr f1
1414     m_i_table[0x6C].pkt_type = ETM4_PKT_I_COND_I_F1;
1415     if (bCondValid)
1416         m_i_table[0x6C].pptkFn = &EtmV4IPktProcImpl::iPktCondInstr;
1417     else
1418         m_i_table[0x6C].pptkFn = &EtmV4IPktProcImpl::iPktInvalidCfg;
1419
1420     // b0110 1101 - cond instr f3
1421     m_i_table[0x6D].pkt_type = ETM4_PKT_I_COND_I_F3;
1422     if (bCondValid)
1423         m_i_table[0x6D].pptkFn = &EtmV4IPktProcImpl::iPktCondInstr;
1424     else
1425         m_i_table[0x6D].pptkFn = &EtmV4IPktProcImpl::iPktInvalidCfg;
1426
1427     // b0110111x - cond res f1
1428     for (int i = 0; i < 2; i++)
1429     {
1430         // G++ cannot understand [0x6E+i] so change these round
1431         m_i_table[i + 0x6E].pkt_type = ETM4_PKT_I_COND_RES_F1;
1432         if (bCondValid)
1433             m_i_table[i + 0x6E].pptkFn = &EtmV4IPktProcImpl::iPktCondResult;
1434         else
1435             m_i_table[i + 0x6E].pptkFn = &EtmV4IPktProcImpl::iPktInvalidCfg;
1436     }
1437
1438     // ETM 4.3 introduces ignore packets
1439     if (m_config.FullVersion() >= 0x43)
1440     {
1441         m_i_table[0x70].pkt_type = ETM4_PKT_I_IGNORE;
1442         m_i_table[0x70].pptkFn = &EtmV4IPktProcImpl::iPktNoPayload;
1443     }
1444
1445     // b01110001 - b01111111 - event trace
1446     for(int i = 0; i < 15; i++)
1447     {
1448         m_i_table[0x71+i].pkt_type = ETM4_PKT_I_EVENT;
1449         m_i_table[0x71+i].pptkFn   = &EtmV4IPktProcImpl::iPktNoPayload;
1450     }
1451
1452     // 0b1000 000x - context
1453     for(int i = 0; i < 2; i++)
1454     {
1455         m_i_table[0x80+i].pkt_type = ETM4_PKT_I_CTXT;
1456         m_i_table[0x80+i].pptkFn   = &EtmV4IPktProcImpl::iPktContext;
1457     }
1458
1459     // 0b1000 0010 to b1000 0011 - addr with ctxt
1460     // 0b1000 0101 to b1000 0110 - addr with ctxt
1461     for(int i = 0; i < 2; i++)
1462     {
1463         m_i_table[0x82+i].pkt_type =  (i == 0) ? ETM4_PKT_I_ADDR_CTXT_L_32IS0 : ETM4_PKT_I_ADDR_CTXT_L_32IS1;
1464         m_i_table[0x82+i].pptkFn   = &EtmV4IPktProcImpl::iPktAddrCtxt;
1465     }
1466
1467     for(int i = 0; i < 2; i++)
1468     {
1469         m_i_table[0x85+i].pkt_type = (i == 0) ? ETM4_PKT_I_ADDR_CTXT_L_64IS0 : ETM4_PKT_I_ADDR_CTXT_L_64IS1;
1470         m_i_table[0x85+i].pptkFn   = &EtmV4IPktProcImpl::iPktAddrCtxt;
1471     }
1472
1473     // 0b1001 0000 to b1001 0010 - exact match addr
1474     for(int i = 0; i < 3; i++)
1475     {
1476         m_i_table[0x90+i].pkt_type = ETM4_PKT_I_ADDR_MATCH;
1477         m_i_table[0x90+i].pptkFn   = &EtmV4IPktProcImpl::iPktNoPayload;
1478     }
1479
1480     // b1001 0101 - b1001 0110 - addr short address
1481     for(int i = 0; i < 2; i++)
1482     {
1483         m_i_table[0x95+i].pkt_type =  (i == 0) ? ETM4_PKT_I_ADDR_S_IS0 : ETM4_PKT_I_ADDR_S_IS1;
1484         m_i_table[0x95+i].pptkFn   = &EtmV4IPktProcImpl::iPktShortAddr;
1485     }
1486
1487     // b10011010 - b10011011 - addr long address
1488     // b10011101 - b10011110 - addr long address
1489     for(int i = 0; i < 2; i++)
1490     {
1491         m_i_table[0x9A+i].pkt_type =  (i == 0) ? ETM4_PKT_I_ADDR_L_32IS0 : ETM4_PKT_I_ADDR_L_32IS1;
1492         m_i_table[0x9A+i].pptkFn   = &EtmV4IPktProcImpl::iPktLongAddr;
1493     }
1494     for(int i = 0; i < 2; i++)
1495     {
1496         m_i_table[0x9D+i].pkt_type =  (i == 0) ? ETM4_PKT_I_ADDR_L_64IS0 : ETM4_PKT_I_ADDR_L_64IS1;
1497         m_i_table[0x9D+i].pptkFn   = &EtmV4IPktProcImpl::iPktLongAddr;
1498     }
1499
1500     // b1010xxxx - Q packet
1501     for (int i = 0; i < 16; i++)
1502     {
1503         m_i_table[0xA0 + i].pkt_type = ETM4_PKT_I_Q;
1504         // certain Q type codes are reserved.
1505         switch (i) {
1506         case 0x3:
1507         case 0x4:
1508         case 0x7:
1509         case 0x8:
1510         case 0x9:
1511         case 0xD:
1512         case 0xE:
1513             // don't update pkt fn - leave at default reserved.
1514             break;
1515         default:
1516             // if this config supports Q elem - otherwise reserved again.
1517             if (m_config.hasQElem())
1518                 m_i_table[0xA0 + i].pptkFn = &EtmV4IPktProcImpl::iPktQ;
1519         }
1520     }
1521
1522     // Atom Packets - all no payload but have specific pattern generation fn
1523     for(int i = 0xC0; i <= 0xD4; i++)   // atom f6
1524     {
1525         m_i_table[i].pkt_type = ETM4_PKT_I_ATOM_F6;
1526         m_i_table[i].pptkFn   = &EtmV4IPktProcImpl::iAtom;
1527     }
1528     for(int i = 0xD5; i <= 0xD7; i++)  // atom f5
1529     {
1530         m_i_table[i].pkt_type = ETM4_PKT_I_ATOM_F5;
1531         m_i_table[i].pptkFn   = &EtmV4IPktProcImpl::iAtom;
1532     }
1533     for(int i = 0xD8; i <= 0xDB; i++)  // atom f2
1534     {
1535         m_i_table[i].pkt_type = ETM4_PKT_I_ATOM_F2;
1536         m_i_table[i].pptkFn   = &EtmV4IPktProcImpl::iAtom;
1537     }
1538     for(int i = 0xDC; i <= 0xDF; i++)  // atom f4
1539     {
1540         m_i_table[i].pkt_type = ETM4_PKT_I_ATOM_F4;
1541         m_i_table[i].pptkFn   = &EtmV4IPktProcImpl::iAtom;
1542     }
1543     for(int i = 0xE0; i <= 0xF4; i++)  // atom f6
1544     {
1545         m_i_table[i].pkt_type = ETM4_PKT_I_ATOM_F6;
1546         m_i_table[i].pptkFn   = &EtmV4IPktProcImpl::iAtom;
1547     }
1548
1549     // atom f5
1550     m_i_table[0xF5].pkt_type = ETM4_PKT_I_ATOM_F5;
1551     m_i_table[0xF5].pptkFn   = &EtmV4IPktProcImpl::iAtom;
1552
1553     for(int i = 0xF6; i <= 0xF7; i++)  // atom f1
1554     {
1555         m_i_table[i].pkt_type = ETM4_PKT_I_ATOM_F1;
1556         m_i_table[i].pptkFn   = &EtmV4IPktProcImpl::iAtom;
1557     }
1558     for(int i = 0xF8; i <= 0xFF; i++)  // atom f3
1559     {
1560         m_i_table[i].pkt_type = ETM4_PKT_I_ATOM_F3;
1561         m_i_table[i].pptkFn   = &EtmV4IPktProcImpl::iAtom;
1562     }
1563 }
1564
1565  unsigned EtmV4IPktProcImpl::extractContField(const std::vector<uint8_t> &buffer, const unsigned st_idx, uint32_t &value, const unsigned byte_limit /*= 5*/)
1566 {
1567     unsigned idx = 0;
1568     bool lastByte = false;
1569     uint8_t byteVal;
1570     value = 0;
1571     while(!lastByte && (idx < byte_limit))   // max 5 bytes for 32 bit value;
1572     {
1573         if(buffer.size() > (st_idx + idx))
1574         {
1575             // each byte has seven bits + cont bit
1576             byteVal = buffer[(st_idx + idx)];
1577             lastByte = (byteVal & 0x80) != 0x80;
1578             value |= ((uint32_t)(byteVal & 0x7F)) << (idx * 7);
1579             idx++;
1580         }
1581         else
1582         {
1583             throwBadSequenceError("Invalid 32 bit continuation fields in packet");
1584         }
1585     }
1586     return idx;
1587 }
1588
1589 unsigned EtmV4IPktProcImpl::extractContField64(const std::vector<uint8_t> &buffer, const unsigned st_idx, uint64_t &value, const unsigned byte_limit /*= 9*/)
1590 {
1591     unsigned idx = 0;
1592     bool lastByte = false;
1593     uint8_t byteVal;
1594     value = 0;
1595     while(!lastByte && (idx < byte_limit))   // max 9 bytes for 64 bit value;
1596     {
1597         if(buffer.size() > (st_idx + idx))
1598         {
1599             // each byte has seven bits + cont bit
1600             byteVal = buffer[(st_idx + idx)];
1601             lastByte = (byteVal & 0x80) != 0x80;
1602             value |= ((uint64_t)(byteVal & 0x7F)) << (idx * 7);
1603             idx++;
1604         }
1605         else
1606         {
1607             throwBadSequenceError("Invalid 64 bit continuation fields in packet");
1608         }
1609     }
1610     return idx;
1611 }
1612
1613  unsigned EtmV4IPktProcImpl::extractCondResult(const std::vector<uint8_t> &buffer, const unsigned st_idx, uint32_t& key, uint8_t &result)
1614 {
1615     unsigned idx = 0;
1616     bool lastByte = false;
1617     int incr = 0;
1618
1619     key = 0;
1620
1621     while(!lastByte && (idx < 6)) // cannot be more than 6 bytes for res + 32 bit key
1622     {
1623         if(buffer.size() > (st_idx + idx))
1624         {
1625             if(idx == 0)
1626             {
1627                 result = buffer[st_idx+idx];
1628                 key = (buffer[st_idx+idx] >> 4) & 0x7;
1629                 incr+=3;
1630             }
1631             else
1632             {
1633                 key |= ((uint32_t)(buffer[st_idx+idx] & 0x7F)) << incr;
1634                 incr+=7;
1635             }
1636             lastByte = (bool)((buffer[st_idx+idx] & 0x80) == 0);
1637             idx++;
1638         }
1639         else
1640         {
1641             throwBadSequenceError("Invalid continuation fields in packet");
1642         }
1643     }
1644     return idx;
1645 }
1646
1647 int EtmV4IPktProcImpl::extract64BitLongAddr(const std::vector<uint8_t> &buffer, const int st_idx, const uint8_t IS, uint64_t &value)
1648 {
1649     value = 0;
1650     if(IS == 0)
1651     {
1652         value |= ((uint64_t)(buffer[st_idx+0] & 0x7F)) << 2;
1653         value |= ((uint64_t)(buffer[st_idx+1] & 0x7F)) << 9;
1654     }
1655     else
1656     {
1657         value |= ((uint64_t)(buffer[st_idx+0] & 0x7F)) << 1;
1658         value |= ((uint64_t)buffer[st_idx+1]) << 8;
1659     }
1660     value |= ((uint64_t)buffer[st_idx+2]) << 16;
1661     value |= ((uint64_t)buffer[st_idx+3]) << 24;
1662     value |= ((uint64_t)buffer[st_idx+4]) << 32;
1663     value |= ((uint64_t)buffer[st_idx+5]) << 40;
1664     value |= ((uint64_t)buffer[st_idx+6]) << 48;
1665     value |= ((uint64_t)buffer[st_idx+7]) << 56;
1666     return 8;
1667 }
1668
1669 int EtmV4IPktProcImpl::extract32BitLongAddr(const std::vector<uint8_t> &buffer, const int st_idx, const uint8_t IS, uint32_t &value)
1670 {
1671     value = 0;
1672     if(IS == 0)
1673     {
1674         value |= ((uint32_t)(buffer[st_idx+0] & 0x7F)) << 2;
1675         value |= ((uint32_t)(buffer[st_idx+1] & 0x7F)) << 9;
1676     }
1677     else
1678     {
1679         value |= ((uint32_t)(buffer[st_idx+0] & 0x7F)) << 1;
1680         value |= ((uint32_t)buffer[st_idx+1]) << 8;
1681     }
1682     value |= ((uint32_t)buffer[st_idx+2]) << 16;
1683     value |= ((uint32_t)buffer[st_idx+3]) << 24;
1684     return 4;
1685 }
1686
1687 void EtmV4IPktProcImpl::throwBadSequenceError(const char *pszExtMsg)
1688 {
1689     m_curr_packet.updateErrType(ETM4_PKT_I_BAD_SEQUENCE);   // swap type for err type
1690     throw ocsdError(OCSD_ERR_SEV_ERROR, OCSD_ERR_BAD_PACKET_SEQ,m_packet_index,m_config.getTraceID(),pszExtMsg);
1691 }
1692
1693
1694 /* End of File trc_pkt_proc_etmv4i_impl.cpp */