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