contrib/opencsd/decoder/source/ptm/trc_pkt_proc_ptm.cpp

   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 */