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 37EtmV4IPktProcImpl::EtmV4IPktProcImpl() : 38 m_isInit(false), 39 m_interface(0), 40 m_first_trace_info(false) 41{ 42 BuildIPacketTable(); 43} 44 45EtmV4IPktProcImpl::~EtmV4IPktProcImpl() 46{ 47} 48 49void 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 59ocsd_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 75ocsd_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 159ocsd_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 172ocsd_datapath_resp_t EtmV4IPktProcImpl::onReset() 173{ 174 // prepare for new decoding session 175 InitProcessorState(); 176 return OCSD_RESP_CONT; 177} 178 179ocsd_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 186void 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 193void 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 205ocsd_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 212ocsd_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 233void 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 261void 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 289void 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 295void 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 334void 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 375void 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 448void 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 501void 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 533void 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 609void 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 653void 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 694void 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 766void 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 808void 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 843void 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 913void 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 937int 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 957void 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 1001void 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 1115void 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. 1188void 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 1477unsigned 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 1535int 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 1557int 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 1575void 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 */ 1583