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