1 /* 2 * \file trc_pkt_proc_ptm.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 "opencsd/ptm/trc_pkt_proc_ptm.h" 36 #include "opencsd/ptm/trc_cmp_cfg_ptm.h" 37 #include "common/ocsd_error.h" 38 39 40 #ifdef __GNUC__ 41 // G++ doesn't like the ## pasting 42 #define PTM_PKTS_NAME "PKTP_PTM" 43 #else 44 // VC++ is OK 45 #define PTM_PKTS_NAME OCSD_CMPNAME_PREFIX_PKTPROC##"_PTM" 46 #endif 47 48 TrcPktProcPtm::TrcPktProcPtm() : TrcPktProcBase(PTM_PKTS_NAME) 49 { 50 InitProcessorState(); 51 BuildIPacketTable(); 52 } 53 54 TrcPktProcPtm::TrcPktProcPtm(int instIDNum) : TrcPktProcBase(PTM_PKTS_NAME, instIDNum) 55 { 56 InitProcessorState(); 57 BuildIPacketTable(); 58 } 59 60 TrcPktProcPtm::~TrcPktProcPtm() 61 { 62 63 } 64 65 ocsd_err_t TrcPktProcPtm::onProtocolConfig() 66 { 67 ocsd_err_t err = OCSD_ERR_NOT_INIT; 68 69 if(m_config != 0) 70 { 71 m_chanIDCopy = m_config->getTraceID(); 72 err = OCSD_OK; 73 } 74 return err; 75 } 76 77 ocsd_datapath_resp_t TrcPktProcPtm::processData( const ocsd_trc_index_t index, 78 const uint32_t dataBlockSize, 79 const uint8_t *pDataBlock, 80 uint32_t *numBytesProcessed) 81 { 82 ocsd_datapath_resp_t resp = OCSD_RESP_CONT; 83 uint8_t currByte = 0; 84 85 m_dataInProcessed = 0; 86 87 if(!checkInit()) 88 { 89 resp = OCSD_RESP_FATAL_NOT_INIT; 90 } 91 else 92 { 93 m_pDataIn = pDataBlock; 94 m_dataInLen = dataBlockSize; 95 m_block_idx = index; // index start for current block 96 } 97 98 while( ( ( m_dataInProcessed < dataBlockSize) || 99 (( m_dataInProcessed == dataBlockSize) && (m_process_state == SEND_PKT)) ) && 100 OCSD_DATA_RESP_IS_CONT(resp)) 101 { 102 try 103 { 104 switch(m_process_state) 105 { 106 case WAIT_SYNC: 107 if(!m_waitASyncSOPkt) 108 { 109 m_curr_pkt_index = m_block_idx + m_dataInProcessed; 110 m_curr_packet.type = PTM_PKT_NOTSYNC; 111 m_bAsyncRawOp = hasRawMon(); 112 } 113 resp = waitASync(); 114 break; 115 116 case PROC_HDR: 117 m_curr_pkt_index = m_block_idx + m_dataInProcessed; 118 if(readByte(currByte)) 119 { 120 m_pIPktFn = m_i_table[currByte].pptkFn; 121 m_curr_packet.type = m_i_table[currByte].pkt_type; 122 } 123 else 124 { 125 // sequencing error - should not get to the point where readByte 126 // fails and m_DataInProcessed < dataBlockSize 127 // throw data overflow error 128 throw ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_PKT_INTERP_FAIL,m_curr_pkt_index,this->m_chanIDCopy,"Data Buffer Overrun"); 129 } 130 m_process_state = PROC_DATA; 131 132 case PROC_DATA: 133 (this->*m_pIPktFn)(); 134 break; 135 136 case SEND_PKT: 137 resp = outputPacket(); 138 InitPacketState(); 139 m_process_state = PROC_HDR; 140 break; 141 } 142 } 143 catch(ocsdError &err) 144 { 145 LogError(err); 146 if( (err.getErrorCode() == OCSD_ERR_BAD_PACKET_SEQ) || 147 (err.getErrorCode() == OCSD_ERR_INVALID_PCKT_HDR)) 148 { 149 // send invalid packets up the pipe to let the next stage decide what to do. 150 m_process_state = SEND_PKT; 151 } 152 else 153 { 154 // bail out on any other error. 155 resp = OCSD_RESP_FATAL_INVALID_DATA; 156 } 157 } 158 catch(...) 159 { 160 /// vv bad at this point. 161 resp = OCSD_RESP_FATAL_SYS_ERR; 162 const ocsdError &fatal = ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_FAIL,m_curr_pkt_index,m_chanIDCopy,"Unknown System Error decoding trace."); 163 LogError(fatal); 164 } 165 166 } 167 *numBytesProcessed = m_dataInProcessed; 168 return resp; 169 } 170 171 ocsd_datapath_resp_t TrcPktProcPtm::onEOT() 172 { 173 ocsd_datapath_resp_t err = OCSD_RESP_FATAL_NOT_INIT; 174 if(checkInit()) 175 { 176 err = OCSD_RESP_CONT; 177 if(m_currPacketData.size() > 0) 178 { 179 m_curr_packet.SetErrType(PTM_PKT_INCOMPLETE_EOT); 180 err = outputPacket(); 181 } 182 } 183 return err; 184 } 185 186 ocsd_datapath_resp_t TrcPktProcPtm::onReset() 187 { 188 ocsd_datapath_resp_t err = OCSD_RESP_FATAL_NOT_INIT; 189 if(checkInit()) 190 { 191 InitProcessorState(); 192 err = OCSD_RESP_CONT; 193 } 194 return err; 195 } 196 197 ocsd_datapath_resp_t TrcPktProcPtm::onFlush() 198 { 199 ocsd_datapath_resp_t err = OCSD_RESP_FATAL_NOT_INIT; 200 if(checkInit()) 201 { 202 err = OCSD_RESP_CONT; 203 } 204 return err; 205 } 206 207 const bool TrcPktProcPtm::isBadPacket() const 208 { 209 return m_curr_packet.isBadPacket(); 210 } 211 212 void TrcPktProcPtm::InitPacketState() 213 { 214 m_curr_packet.Clear(); 215 216 } 217 218 void TrcPktProcPtm::InitProcessorState() 219 { 220 m_curr_packet.SetType(PTM_PKT_NOTSYNC); 221 m_pIPktFn = &TrcPktProcPtm::pktReserved; 222 m_process_state = WAIT_SYNC; 223 m_async_0 = 0; 224 m_waitASyncSOPkt = false; 225 m_bAsyncRawOp = false; 226 m_bOPNotSyncPkt = false; 227 228 m_curr_packet.ResetState(); 229 InitPacketState(); 230 } 231 232 const bool TrcPktProcPtm::readByte(uint8_t &currByte) 233 { 234 bool bValidByte = false; 235 236 if(m_dataInProcessed < m_dataInLen) 237 { 238 currByte = m_pDataIn[m_dataInProcessed++]; 239 m_currPacketData.push_back(currByte); 240 bValidByte = true; 241 } 242 return bValidByte; 243 } 244 245 void TrcPktProcPtm::unReadByte() 246 { 247 m_dataInProcessed--; 248 m_currPacketData.pop_back(); 249 } 250 251 ocsd_datapath_resp_t TrcPktProcPtm::outputPacket() 252 { 253 ocsd_datapath_resp_t resp = OCSD_RESP_CONT; 254 resp = outputOnAllInterfaces(m_curr_pkt_index,&m_curr_packet,&m_curr_packet.type,m_currPacketData); 255 m_currPacketData.clear(); 256 return resp; 257 } 258 259 /*** sync and packet functions ***/ 260 ocsd_datapath_resp_t TrcPktProcPtm::waitASync() 261 { 262 ocsd_datapath_resp_t resp = OCSD_RESP_CONT; 263 264 // looking for possible patterns in input buffer:- 265 // a) ASYNC @ start : 00 00 00 00 00 80 266 // b) unsync then async: xx xx xx xx xx xx xx xx 00 00 00 00 00 80 267 // c) unsync (may have 00) xx xx xx xx 00 xx xx 00 00 00 xx xx xx xx 268 // d) unsync then part async: xx xx xx xx xx xx xx xx xx xx xx 00 00 00 269 // e) unsync with prev part async [00 00 00] 00 xx xx xx xx xx xx xx xx [] = byte in previous input buffer 270 271 // bytes to read before throwing an unsynced packet 272 #define UNSYNC_PKT_MAX 16 273 static const uint8_t spare_zeros[] = { 0,0,0,0,0,0,0,0, 274 0,0,0,0,0,0,0,0 }; 275 276 bool doScan = true; 277 bool bSendUnsyncedData = false; 278 bool bHaveASync = false; 279 int unsynced_bytes = 0; 280 int unsync_scan_block_start = 0; 281 int pktBytesOnEntry = m_currPacketData.size(); // did we have part of a potential async last time? 282 283 while(doScan && OCSD_DATA_RESP_IS_CONT(resp)) 284 { 285 // may have spotted the start of an async 286 if(m_waitASyncSOPkt == true) 287 { 288 switch(findAsync()) 289 { 290 case ASYNC: 291 case ASYNC_EXTRA_0: 292 m_process_state = SEND_PKT; 293 m_waitASyncSOPkt = false; 294 bSendUnsyncedData = true; 295 bHaveASync = true; 296 doScan = false; 297 break; 298 299 case THROW_0: 300 // remove a bunch of 0s 301 unsynced_bytes += ASYNC_PAD_0_LIMIT; 302 m_waitASyncSOPkt = false; 303 m_currPacketData.erase( m_currPacketData.begin(), m_currPacketData.begin()+ASYNC_PAD_0_LIMIT); 304 break; 305 306 case NOT_ASYNC: 307 unsynced_bytes += m_currPacketData.size(); 308 m_waitASyncSOPkt = false; 309 m_currPacketData.clear(); 310 break; 311 312 case ASYNC_INCOMPLETE: 313 bSendUnsyncedData = true; 314 doScan = false; 315 break; 316 } 317 } 318 else 319 { 320 if(m_pDataIn[m_dataInProcessed++] == 0x00) 321 { 322 m_waitASyncSOPkt = true; 323 m_currPacketData.push_back(0); 324 m_async_0 = 1; 325 } 326 else 327 { 328 unsynced_bytes++; 329 } 330 } 331 332 // may need to send some unsynced data here, either if we have enought to make it worthwhile, 333 // or are at the end of the buffer. 334 if(unsynced_bytes >= UNSYNC_PKT_MAX) 335 bSendUnsyncedData = true; 336 337 if(m_dataInProcessed == m_dataInLen) 338 { 339 bSendUnsyncedData = true; 340 doScan = false; // no more data available - stop the scan 341 } 342 343 // will send any unsynced data 344 if(bSendUnsyncedData && (unsynced_bytes > 0)) 345 { 346 if(m_bAsyncRawOp) 347 { 348 // there were some 0's in the packet buyffer from the last pass that are no longer in the raw buffer, 349 // and these turned out not to be an async 350 if(pktBytesOnEntry) 351 { 352 outputRawPacketToMonitor(m_curr_pkt_index,&m_curr_packet,pktBytesOnEntry,spare_zeros); 353 m_curr_pkt_index += pktBytesOnEntry; 354 } 355 outputRawPacketToMonitor(m_curr_pkt_index,&m_curr_packet,unsynced_bytes,m_pDataIn+unsync_scan_block_start); 356 } 357 if (!m_bOPNotSyncPkt) 358 { 359 resp = outputDecodedPacket(m_curr_pkt_index, &m_curr_packet); 360 m_bOPNotSyncPkt = true; 361 } 362 unsync_scan_block_start += unsynced_bytes; 363 m_curr_pkt_index+= unsynced_bytes; 364 unsynced_bytes = 0; 365 bSendUnsyncedData = false; 366 } 367 368 // mark next packet as the ASYNC we are looking for. 369 if(bHaveASync) 370 m_curr_packet.SetType(PTM_PKT_A_SYNC); 371 } 372 373 return resp; 374 } 375 376 void TrcPktProcPtm::pktASync() 377 { 378 if(m_currPacketData.size() == 1) // header byte 379 { 380 m_async_0 = 1; 381 } 382 383 switch(findAsync()) 384 { 385 case ASYNC: 386 case ASYNC_EXTRA_0: 387 m_process_state = SEND_PKT; 388 break; 389 390 case THROW_0: 391 case NOT_ASYNC: 392 throwMalformedPacketErr("Bad Async packet"); 393 break; 394 395 case ASYNC_INCOMPLETE: 396 break; 397 398 } 399 } 400 401 TrcPktProcPtm::async_result_t TrcPktProcPtm::findAsync() 402 { 403 async_result_t async_res = NOT_ASYNC; 404 bool bFound = false; // found non-zero byte in sequence 405 bool bByteAvail = true; 406 uint8_t currByte; 407 408 while(!bFound && bByteAvail) 409 { 410 if(readByte(currByte)) 411 { 412 if(currByte == 0x00) 413 { 414 m_async_0++; 415 if(m_async_0 >= (ASYNC_PAD_0_LIMIT + ASYNC_REQ_0)) 416 { 417 bFound = true; 418 async_res = THROW_0; 419 } 420 } 421 else 422 { 423 if(currByte == 0x80) 424 { 425 if(m_async_0 == 5) 426 async_res = ASYNC; 427 else if(m_async_0 > 5) 428 async_res = ASYNC_EXTRA_0; 429 } 430 bFound = true; 431 } 432 } 433 else 434 { 435 bByteAvail = false; 436 async_res = ASYNC_INCOMPLETE; 437 } 438 } 439 return async_res; 440 } 441 442 void TrcPktProcPtm::pktISync() 443 { 444 uint8_t currByte = 0; 445 int pktIndex = m_currPacketData.size() - 1; 446 bool bGotBytes = false, validByte = true; 447 448 if(pktIndex == 0) 449 { 450 m_numCtxtIDBytes = m_config->CtxtIDBytes(); 451 m_gotCtxtIDBytes = 0; 452 453 // total bytes = 6 + ctxtID; (perhaps more later) 454 m_numPktBytesReq = 6 + m_numCtxtIDBytes; 455 } 456 457 while(validByte && !bGotBytes) 458 { 459 if(readByte(currByte)) 460 { 461 pktIndex = m_currPacketData.size() - 1; 462 if(pktIndex == 5) 463 { 464 // got the info byte 465 int altISA = (currByte >> 2) & 0x1; 466 int reason = (currByte >> 5) & 0x3; 467 m_curr_packet.SetISyncReason((ocsd_iSync_reason)(reason)); 468 m_curr_packet.UpdateNS((currByte >> 3) & 0x1); 469 m_curr_packet.UpdateAltISA((currByte >> 2) & 0x1); 470 m_curr_packet.UpdateHyp((currByte >> 1) & 0x1); 471 472 ocsd_isa isa = ocsd_isa_arm; 473 if(m_currPacketData[1] & 0x1) 474 isa = altISA ? ocsd_isa_tee : ocsd_isa_thumb2; 475 m_curr_packet.UpdateISA(isa); 476 477 // check cycle count required - not if reason == 0; 478 m_needCycleCount = (reason != 0) ? m_config->enaCycleAcc() : false; 479 m_gotCycleCount = false; 480 m_numPktBytesReq += (m_needCycleCount ? 1 : 0); 481 m_gotCCBytes = 0; 482 483 } 484 else if(pktIndex > 5) 485 { 486 // cycle count appears first if present 487 if(m_needCycleCount && !m_gotCycleCount) 488 { 489 if(pktIndex == 6) 490 m_gotCycleCount = (bool)((currByte & 0x40) == 0); // no cont bit, got cycle count 491 else 492 m_gotCycleCount = ((currByte & 0x80) == 0) || (pktIndex == 10); 493 494 m_gotCCBytes++; // count the cycle count bytes for later use. 495 if(!m_gotCycleCount) // need more cycle count bytes 496 m_numPktBytesReq++; 497 } 498 // then context ID if present. 499 else if( m_numCtxtIDBytes > m_gotCtxtIDBytes) 500 { 501 m_gotCtxtIDBytes++; 502 } 503 } 504 505 // check if we have enough bytes 506 bGotBytes = (bool)((unsigned)m_numPktBytesReq == m_currPacketData.size()); 507 } 508 else 509 validByte = false; // no byte available, exit. 510 } 511 512 if(bGotBytes) 513 { 514 // extract address value, cycle count and ctxt id. 515 uint32_t cycleCount = 0; 516 uint32_t ctxtID = 0; 517 int optIdx = 6; // start index for optional elements. 518 519 // address is always full fixed 32 bit value 520 uint32_t address = ((uint32_t)m_currPacketData[1]) & 0xFE; 521 address |= ((uint32_t)m_currPacketData[2]) << 8; 522 address |= ((uint32_t)m_currPacketData[3]) << 16; 523 address |= ((uint32_t)m_currPacketData[4]) << 24; 524 m_curr_packet.UpdateAddress(address,32); 525 526 if(m_needCycleCount) 527 { 528 extractCycleCount(optIdx,cycleCount); 529 m_curr_packet.SetCycleCount(cycleCount); 530 optIdx+=m_gotCCBytes; 531 } 532 533 if(m_numCtxtIDBytes) 534 { 535 extractCtxtID(optIdx,ctxtID); 536 m_curr_packet.UpdateContextID(ctxtID); 537 } 538 m_process_state = SEND_PKT; 539 } 540 } 541 542 void TrcPktProcPtm::pktTrigger() 543 { 544 m_process_state = SEND_PKT; // no payload 545 } 546 547 void TrcPktProcPtm::pktWPointUpdate() 548 { 549 bool bDone = false; 550 bool bBytesAvail = true; 551 uint8_t currByte = 0; 552 int byteIdx = 0; 553 554 if(m_currPacketData.size() == 1) 555 { 556 m_gotAddrBytes = false; // flag to indicate got all needed address bytes 557 m_numAddrBytes = 0; // number of address bytes so far - in this case header is not part of the address 558 559 m_gotExcepBytes = false; // mark as not got all required exception bytes thus far 560 m_numExcepBytes = 0; // 0 read in 561 562 m_addrPktIsa = ocsd_isa_unknown; // not set by this packet as yet 563 } 564 565 // collect all the bytes needed 566 while(!bDone && bBytesAvail) 567 { 568 if(readByte(currByte)) 569 { 570 byteIdx = m_currPacketData.size() - 1; 571 if(!m_gotAddrBytes) 572 { 573 if(byteIdx < 4) 574 { 575 // address bytes 1 - 4; 576 // ISA stays the same 577 if((currByte & 0x80) == 0x00) 578 { 579 // no further bytes 580 m_gotAddrBytes = true; 581 bDone = true; 582 m_gotExcepBytes = true; 583 } 584 } 585 else 586 { 587 // 5th address byte - determine ISA from this. 588 if((currByte & 0x40) == 0x00) 589 m_gotExcepBytes = true; // no exception bytes - mark as done 590 m_gotAddrBytes = true; 591 bDone = m_gotExcepBytes; 592 593 m_addrPktIsa = ocsd_isa_arm; // assume ARM, but then check 594 if((currByte & 0x20) == 0x20) // bit 5 == 1'b1 - jazelle, bits 4 & 3 part of address. 595 m_addrPktIsa = ocsd_isa_jazelle; 596 else if((currByte & 0x30) == 0x10) // bit [5:4] == 2'b01 - thumb, bit 3 part of address. 597 m_addrPktIsa = ocsd_isa_thumb2; 598 } 599 m_numAddrBytes++; 600 } 601 else if(!m_gotExcepBytes) 602 { 603 // excep byte is actually a WP update byte. 604 m_excepAltISA = ((currByte & 0x40) == 0x40) ? 1 : 0; 605 m_gotExcepBytes = true; 606 m_numExcepBytes++; 607 bDone = true; 608 } 609 } 610 else 611 bBytesAvail = false; 612 } 613 614 // analyse the bytes to create the packet 615 if(bDone) 616 { 617 // ISA for the packet 618 if(m_addrPktIsa == ocsd_isa_unknown) // unchanged by trace packet 619 m_addrPktIsa = m_curr_packet.getISA(); // same as prev 620 621 if(m_gotExcepBytes) // may adjust according to alt ISA in exception packet 622 { 623 if((m_addrPktIsa == ocsd_isa_tee) && (m_excepAltISA == 0)) 624 m_addrPktIsa = ocsd_isa_thumb2; 625 else if((m_addrPktIsa == ocsd_isa_thumb2) && (m_excepAltISA == 1)) 626 m_addrPktIsa = ocsd_isa_tee; 627 } 628 m_curr_packet.UpdateISA(m_addrPktIsa); // mark ISA in packet (update changes current and prev to dectect an ISA change). 629 630 uint8_t total_bits = 0; 631 uint32_t addr_val = extractAddress(1,total_bits); 632 m_curr_packet.UpdateAddress(addr_val,total_bits); 633 m_process_state = SEND_PKT; 634 } 635 } 636 637 void TrcPktProcPtm::pktIgnore() 638 { 639 m_process_state = SEND_PKT; // no payload 640 } 641 642 void TrcPktProcPtm::pktCtxtID() 643 { 644 int pktIndex = m_currPacketData.size() - 1; 645 646 // if at the header, determine how many more bytes we need. 647 if(pktIndex == 0) 648 { 649 m_numCtxtIDBytes = m_config->CtxtIDBytes(); 650 m_gotCtxtIDBytes = 0; 651 } 652 653 // read the necessary ctxtID bytes from the stream 654 bool bGotBytes = false, bytesAvail = true; 655 uint32_t ctxtID = 0; 656 657 bGotBytes = m_numCtxtIDBytes == m_gotCtxtIDBytes; 658 while(!bGotBytes & bytesAvail) 659 { 660 bytesAvail = readByte(); 661 if(bytesAvail) 662 m_gotCtxtIDBytes++; 663 bGotBytes = m_numCtxtIDBytes == m_gotCtxtIDBytes; 664 } 665 666 if(bGotBytes) 667 { 668 if(m_numCtxtIDBytes) 669 { 670 extractCtxtID(1,ctxtID); 671 } 672 m_curr_packet.UpdateContextID(ctxtID); 673 m_process_state = SEND_PKT; 674 } 675 } 676 677 void TrcPktProcPtm::pktVMID() 678 { 679 uint8_t currByte; 680 681 // just need a single payload byte... 682 if(readByte(currByte)) 683 { 684 m_curr_packet.UpdateVMID(currByte); 685 m_process_state = SEND_PKT; 686 } 687 } 688 689 void TrcPktProcPtm::pktAtom() 690 { 691 uint8_t pHdr = m_currPacketData[0]; 692 693 if(!m_config->enaCycleAcc()) 694 { 695 m_curr_packet.SetAtomFromPHdr(pHdr); 696 m_process_state = SEND_PKT; 697 } 698 else 699 { 700 bool bGotAllPktBytes = false, byteAvail = true; 701 uint8_t currByte = 0; // cycle accurate tracing -> atom + cycle count 702 703 if(!(pHdr & 0x40)) 704 { 705 // only the header byte present 706 bGotAllPktBytes = true; 707 } 708 else 709 { 710 // up to 4 additional bytes of count data. 711 while(byteAvail && !bGotAllPktBytes) 712 { 713 if(readByte(currByte)) 714 { 715 if(!(currByte & 0x80) || (m_currPacketData.size() == 5)) 716 bGotAllPktBytes = true; 717 } 718 else 719 byteAvail = false; 720 } 721 } 722 723 // we have all the bytes for a cycle accurate packet. 724 if(bGotAllPktBytes) 725 { 726 uint32_t cycleCount = 0; 727 extractCycleCount(0,cycleCount); 728 m_curr_packet.SetCycleCount(cycleCount); 729 m_curr_packet.SetCycleAccAtomFromPHdr(pHdr); 730 m_process_state = SEND_PKT; 731 } 732 } 733 } 734 735 void TrcPktProcPtm::pktTimeStamp() 736 { 737 uint8_t currByte = 0; 738 int pktIndex = m_currPacketData.size() - 1; 739 bool bGotBytes = false, byteAvail = true; 740 741 if(pktIndex == 0) 742 { 743 m_gotTSBytes = false; 744 m_needCycleCount = m_config->enaCycleAcc(); 745 m_gotCCBytes = 0; 746 747 // max byte buffer size for full ts packet 748 m_tsByteMax = m_config->TSPkt64() ? 10 : 8; 749 } 750 751 while(byteAvail && !bGotBytes) 752 { 753 if(readByte(currByte)) 754 { 755 if(!m_gotTSBytes) 756 { 757 if(((currByte & 0x80) == 0) || (m_currPacketData.size() == (unsigned)m_tsByteMax)) 758 { 759 m_gotTSBytes = true; 760 if(!m_needCycleCount) 761 bGotBytes = true; 762 } 763 } 764 else 765 { 766 uint8_t cc_cont_mask = 0x80; 767 // got TS bytes, collect cycle count 768 if(m_gotCCBytes == 0) 769 cc_cont_mask = 0x40; 770 if((currByte & cc_cont_mask) == 0) 771 bGotBytes = true; 772 m_gotCCBytes++; 773 if(m_gotCCBytes == 5) 774 bGotBytes = true; 775 } 776 } 777 else 778 byteAvail = false; 779 } 780 781 if(bGotBytes) 782 { 783 uint64_t tsVal = 0; 784 uint32_t cycleCount = 0; 785 uint8_t tsUpdateBits = 0; 786 int ts_end_idx = extractTS(tsVal,tsUpdateBits); 787 if(m_needCycleCount) 788 { 789 extractCycleCount(ts_end_idx,cycleCount); 790 m_curr_packet.SetCycleCount(cycleCount); 791 } 792 m_curr_packet.UpdateTimestamp(tsVal,tsUpdateBits); 793 m_process_state = SEND_PKT; 794 } 795 } 796 797 void TrcPktProcPtm::pktExceptionRet() 798 { 799 m_process_state = SEND_PKT; // no payload 800 } 801 802 void TrcPktProcPtm::pktBranchAddr() 803 { 804 uint8_t currByte = m_currPacketData[0]; 805 bool bDone = false; 806 bool bBytesAvail = true; 807 int byteIdx = 0; 808 809 if(m_currPacketData.size() == 1) 810 { 811 m_gotAddrBytes = false; // flag to indicate got all needed address bytes 812 m_numAddrBytes = 1; // number of address bytes so far 813 814 m_needCycleCount = m_config->enaCycleAcc(); // check if we have a cycle count 815 m_gotCCBytes = 0; // number of cc bytes read in so far. 816 817 m_gotExcepBytes = false; // mark as not got all required exception bytes thus far 818 m_numExcepBytes = 0; // 0 read in 819 820 m_addrPktIsa = ocsd_isa_unknown; // not set by this packet as yet 821 822 // header is also 1st address byte 823 if((currByte & 0x80) == 0) // could be single byte packet 824 { 825 m_gotAddrBytes = true; 826 if(!m_needCycleCount) 827 bDone = true; // all done if no cycle count 828 m_gotExcepBytes = true; // cannot have exception bytes following single byte packet 829 } 830 831 } 832 833 // collect all the bytes needed 834 while(!bDone && bBytesAvail) 835 { 836 if(readByte(currByte)) 837 { 838 byteIdx = m_currPacketData.size() - 1; 839 if(!m_gotAddrBytes) 840 { 841 if(byteIdx < 4) 842 { 843 // address bytes 2 - 4; 844 // ISA stays the same 845 if((currByte & 0x80) == 0x00) 846 { 847 // no further bytes 848 if((currByte & 0x40) == 0x00) 849 m_gotExcepBytes = true; // no exception bytes - mark as done 850 m_gotAddrBytes = true; 851 bDone = m_gotExcepBytes && !m_needCycleCount; 852 } 853 } 854 else 855 { 856 // 5th address byte - determine ISA from this. 857 if((currByte & 0x40) == 0x00) 858 m_gotExcepBytes = true; // no exception bytes - mark as done 859 m_gotAddrBytes = true; 860 bDone = m_gotExcepBytes && !m_needCycleCount; 861 862 m_addrPktIsa = ocsd_isa_arm; // assume ARM, but then check 863 if((currByte & 0x20) == 0x20) // bit 5 == 1'b1 - jazelle, bits 4 & 3 part of address. 864 m_addrPktIsa = ocsd_isa_jazelle; 865 else if((currByte & 0x30) == 0x10) // bit [5:4] == 2'b01 - thumb, bit 3 part of address. 866 m_addrPktIsa = ocsd_isa_thumb2; 867 } 868 m_numAddrBytes++; 869 } 870 else if(!m_gotExcepBytes) 871 { 872 // may need exception bytes 873 if(m_numExcepBytes == 0) 874 { 875 if((currByte & 0x80) == 0x00) 876 m_gotExcepBytes = true; 877 m_excepAltISA = ((currByte & 0x40) == 0x40) ? 1 : 0; 878 } 879 else 880 m_gotExcepBytes = true; 881 m_numExcepBytes++; 882 883 if(m_gotExcepBytes && !m_needCycleCount) 884 bDone = true; 885 886 } 887 else if(m_needCycleCount) 888 { 889 // not done after exception bytes, collect cycle count 890 if(m_gotCCBytes == 0) 891 { 892 bDone = ((currByte & 0x40) == 0x00 ); 893 } 894 else 895 { 896 // done if no more or 5th byte 897 bDone = (((currByte & 0x80) == 0x00 ) || (m_gotCCBytes == 4)); 898 } 899 m_gotCCBytes++; 900 } 901 else 902 // this should never be reached. 903 throwMalformedPacketErr("sequencing error analysing branch packet"); 904 } 905 else 906 bBytesAvail = false; 907 } 908 909 // analyse the bytes to create the packet 910 if(bDone) 911 { 912 // ISA for the packet 913 if(m_addrPktIsa == ocsd_isa_unknown) // unchanged by trace packet 914 m_addrPktIsa = m_curr_packet.getISA(); // same as prev 915 916 if(m_gotExcepBytes) // may adjust according to alt ISA in exception packet 917 { 918 if((m_addrPktIsa == ocsd_isa_tee) && (m_excepAltISA == 0)) 919 m_addrPktIsa = ocsd_isa_thumb2; 920 else if((m_addrPktIsa == ocsd_isa_thumb2) && (m_excepAltISA == 1)) 921 m_addrPktIsa = ocsd_isa_tee; 922 } 923 m_curr_packet.UpdateISA(m_addrPktIsa); // mark ISA in packet (update changes current and prev to dectect an ISA change). 924 925 926 927 // we know the ISA, we can correctly interpret the address. 928 uint8_t total_bits = 0; 929 uint32_t addr_val = extractAddress(0,total_bits); 930 m_curr_packet.UpdateAddress(addr_val,total_bits); 931 932 if(m_numExcepBytes > 0) 933 { 934 uint8_t E1 = m_currPacketData[m_numAddrBytes]; 935 uint16_t ENum = (E1 >> 1) & 0xF; 936 ocsd_armv7_exception excep = Excp_Reserved; 937 938 m_curr_packet.UpdateNS(E1 & 0x1); 939 if(m_numExcepBytes > 1) 940 { 941 uint8_t E2 = m_currPacketData[m_numAddrBytes+1]; 942 m_curr_packet.UpdateHyp((E2 >> 5) & 0x1); 943 ENum |= ((uint16_t)(E2 & 0x1F) << 4); 944 } 945 946 if(ENum <= 0xF) 947 { 948 static ocsd_armv7_exception v7ARExceptions[16] = { 949 Excp_NoException, Excp_DebugHalt, Excp_SMC, Excp_Hyp, 950 Excp_AsyncDAbort, Excp_ThumbEECheckFail, Excp_Reserved, Excp_Reserved, 951 Excp_Reset, Excp_Undef, Excp_SVC, Excp_PrefAbort, 952 Excp_SyncDataAbort, Excp_Generic, Excp_IRQ, Excp_FIQ 953 }; 954 excep = v7ARExceptions[ENum]; 955 } 956 m_curr_packet.SetException(excep,ENum); 957 } 958 959 if(m_needCycleCount) 960 { 961 int countIdx = m_numAddrBytes + m_numExcepBytes; 962 uint32_t cycleCount = 0; 963 extractCycleCount(countIdx,cycleCount); 964 m_curr_packet.SetCycleCount(cycleCount); 965 } 966 m_process_state = SEND_PKT; 967 } 968 } 969 970 void TrcPktProcPtm::pktReserved() 971 { 972 m_process_state = SEND_PKT; // no payload 973 } 974 975 void TrcPktProcPtm::extractCtxtID(int idx, uint32_t &ctxtID) 976 { 977 ctxtID = 0; 978 int shift = 0; 979 for(int i=0; i < m_numCtxtIDBytes; i++) 980 { 981 if((size_t)idx+i >= m_currPacketData.size()) 982 throwMalformedPacketErr("Insufficient packet bytes for Context ID value."); 983 ctxtID |= ((uint32_t)m_currPacketData[idx+i]) << shift; 984 shift+=8; 985 } 986 } 987 988 void TrcPktProcPtm::extractCycleCount(int offset, uint32_t &cycleCount) 989 { 990 bool bCont = true; 991 cycleCount = 0; 992 int by_idx = 0; 993 uint8_t currByte; 994 int shift = 4; 995 996 while(bCont) 997 { 998 if((size_t)by_idx+offset >= m_currPacketData.size()) 999 throwMalformedPacketErr("Insufficient packet bytes for Cycle Count value."); 1000 1001 currByte = m_currPacketData[offset+by_idx]; 1002 if(by_idx == 0) 1003 { 1004 bCont = (currByte & 0x40) != 0; 1005 cycleCount = (currByte >> 2) & 0xF; 1006 } 1007 else 1008 { 1009 1010 bCont = (currByte & 0x80) != 0; 1011 if(by_idx == 4) 1012 bCont = false; 1013 cycleCount |= (((uint32_t)(currByte & 0x7F)) << shift); 1014 shift += 7; 1015 } 1016 by_idx++; 1017 } 1018 } 1019 1020 int TrcPktProcPtm::extractTS(uint64_t &tsVal,uint8_t &tsUpdateBits) 1021 { 1022 bool bCont = true; 1023 int tsIdx = 1; // start index; 1024 uint8_t byteVal; 1025 bool b64BitVal = m_config->TSPkt64(); 1026 int shift = 0; 1027 1028 tsVal = 0; 1029 tsUpdateBits = 0; 1030 1031 while(bCont) 1032 { 1033 if((size_t)tsIdx >= m_currPacketData.size()) 1034 throwMalformedPacketErr("Insufficient packet bytes for Timestamp value."); 1035 1036 byteVal = m_currPacketData[tsIdx]; 1037 1038 if(b64BitVal) 1039 { 1040 if(tsIdx < 9) 1041 { 1042 bCont = ((byteVal & 0x80) == 0x80); 1043 byteVal &= 0x7F; 1044 tsUpdateBits += 7; 1045 } 1046 else 1047 { 1048 bCont = false; 1049 tsUpdateBits += 8; 1050 } 1051 } 1052 else 1053 { 1054 if(tsIdx < 7) 1055 { 1056 bCont = ((byteVal & 0x80) == 0x80); 1057 byteVal &= 0x7F; 1058 tsUpdateBits += 7; 1059 } 1060 else 1061 { 1062 byteVal &=0x3F; 1063 bCont = false; 1064 tsUpdateBits += 6; 1065 } 1066 } 1067 tsVal |= (((uint64_t)byteVal) << shift); 1068 tsIdx++; 1069 shift += 7; 1070 } 1071 return tsIdx; // return next byte index in packet. 1072 } 1073 1074 uint32_t TrcPktProcPtm::extractAddress(const int offset, uint8_t &total_bits) 1075 { 1076 // we know the ISA, we can correctly interpret the address. 1077 uint32_t addr_val = 0; 1078 uint8_t mask = 0x7E; // first byte mask (always); 1079 uint8_t num_bits = 0x7; // number of bits in the 1st byte (thumb); 1080 int shift = 0; 1081 int next_shift = 0; 1082 1083 total_bits = 0; 1084 1085 for(int i = 0; i < m_numAddrBytes; i++) 1086 { 1087 if(i == 4) 1088 { 1089 // 5th byte mask 1090 mask = 0x0f; // thumb mask; 1091 num_bits = 4; 1092 if(m_addrPktIsa == ocsd_isa_jazelle) 1093 { 1094 mask = 0x1F; 1095 num_bits = 5; 1096 } 1097 else if(m_addrPktIsa == ocsd_isa_arm) 1098 { 1099 mask = 0x07; 1100 num_bits = 3; 1101 } 1102 } 1103 else if(i > 0) 1104 { 1105 mask = 0x7F; 1106 num_bits = 7; 1107 // check for last byte but not 1st or 5th byte mask 1108 if(i == m_numAddrBytes-1) 1109 { 1110 mask = 0x3F; 1111 num_bits = 6; 1112 } 1113 } 1114 1115 // extract data 1116 shift = next_shift; 1117 addr_val |= ((uint32_t)(m_currPacketData[i+offset] & mask) << shift); 1118 total_bits += num_bits; 1119 1120 // how much we shift the next value 1121 if(i == 0) 1122 { 1123 if(m_addrPktIsa == ocsd_isa_jazelle) 1124 { 1125 addr_val >>= 1; 1126 next_shift = 6; 1127 total_bits--; // adjust bits for jazelle offset 1128 } 1129 else 1130 { 1131 next_shift = 7; 1132 } 1133 } 1134 else 1135 { 1136 next_shift += 7; 1137 } 1138 } 1139 1140 if(m_addrPktIsa == ocsd_isa_arm) 1141 { 1142 addr_val <<= 1; // shift one extra bit for ARM address alignment. 1143 total_bits++; 1144 } 1145 return addr_val; 1146 } 1147 1148 1149 void TrcPktProcPtm::BuildIPacketTable() 1150 { 1151 // initialise all to branch, atom or reserved packet header 1152 for(unsigned i = 0; i < 256; i++) 1153 { 1154 // branch address packets all end in 8'bxxxxxxx1 1155 if((i & 0x01) == 0x01) 1156 { 1157 m_i_table[i].pkt_type = PTM_PKT_BRANCH_ADDRESS; 1158 m_i_table[i].pptkFn = &TrcPktProcPtm::pktBranchAddr; 1159 } 1160 // atom packets are 8'b1xxxxxx0 1161 else if((i & 0x81) == 0x80) 1162 { 1163 m_i_table[i].pkt_type = PTM_PKT_ATOM; 1164 m_i_table[i].pptkFn = &TrcPktProcPtm::pktAtom; 1165 } 1166 else 1167 { 1168 // set all the others to reserved for now 1169 m_i_table[i].pkt_type = PTM_PKT_RESERVED; 1170 m_i_table[i].pptkFn = &TrcPktProcPtm::pktReserved; 1171 } 1172 } 1173 1174 // pick out the other packet types by individual codes. 1175 1176 // A-sync 8'b00000000 1177 m_i_table[0x00].pkt_type = PTM_PKT_A_SYNC; 1178 m_i_table[0x00].pptkFn = &TrcPktProcPtm::pktASync; 1179 1180 // I-sync 8'b00001000 1181 m_i_table[0x08].pkt_type = PTM_PKT_I_SYNC; 1182 m_i_table[0x08].pptkFn = &TrcPktProcPtm::pktISync; 1183 1184 // waypoint update 8'b01110010 1185 m_i_table[0x72].pkt_type = PTM_PKT_WPOINT_UPDATE; 1186 m_i_table[0x72].pptkFn = &TrcPktProcPtm::pktWPointUpdate; 1187 1188 // trigger 8'b00001100 1189 m_i_table[0x0C].pkt_type = PTM_PKT_TRIGGER; 1190 m_i_table[0x0C].pptkFn = &TrcPktProcPtm::pktTrigger; 1191 1192 // context ID 8'b01101110 1193 m_i_table[0x6E].pkt_type = PTM_PKT_CONTEXT_ID; 1194 m_i_table[0x6E].pptkFn = &TrcPktProcPtm::pktCtxtID; 1195 1196 // VMID 8'b00111100 1197 m_i_table[0x3C].pkt_type = PTM_PKT_VMID; 1198 m_i_table[0x3C].pptkFn = &TrcPktProcPtm::pktVMID; 1199 1200 // Timestamp 8'b01000x10 1201 m_i_table[0x42].pkt_type = PTM_PKT_TIMESTAMP; 1202 m_i_table[0x42].pptkFn = &TrcPktProcPtm::pktTimeStamp; 1203 m_i_table[0x46].pkt_type = PTM_PKT_TIMESTAMP; 1204 m_i_table[0x46].pptkFn = &TrcPktProcPtm::pktTimeStamp; 1205 1206 // Exception return 8'b01110110 1207 m_i_table[0x76].pkt_type = PTM_PKT_EXCEPTION_RET; 1208 m_i_table[0x76].pptkFn = &TrcPktProcPtm::pktExceptionRet; 1209 1210 // Ignore 8'b01100110 1211 m_i_table[0x66].pkt_type = PTM_PKT_IGNORE; 1212 m_i_table[0x66].pptkFn = &TrcPktProcPtm::pktIgnore; 1213 } 1214 1215 /* End of File trc_pkt_proc_ptm.cpp */ 1216