contrib/opencsd/decoder/source/stm/trc_pkt_proc_stm.cpp

   1 /*
   2  * \file       trc_pkt_proc_stm.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/stm/trc_pkt_proc_stm.h"
  36
  37
  38 // processor object construction
  39 // ************************
  40
  41 #ifdef __GNUC__
  42 // G++ doesn't like the ## pasting
  43 #define STM_PKTS_NAME "PKTP_STM"
  44 #else
  45 #define STM_PKTS_NAME OCSD_CMPNAME_PREFIX_PKTPROC##"_STM"
  46 #endif
  47
  48 static const uint32_t STM_SUPPORTED_OP_FLAGS = OCSD_OPFLG_PKTPROC_COMMON;
  49
  50 TrcPktProcStm::TrcPktProcStm() : TrcPktProcBase(STM_PKTS_NAME)
  51 {
  52     initObj();
  53 }
  54
  55 TrcPktProcStm::TrcPktProcStm(int instIDNum) : TrcPktProcBase(STM_PKTS_NAME, instIDNum)
  56 {
  57     initObj();
  58 }
  59
  60 TrcPktProcStm::~TrcPktProcStm()
  61 {
  62     getRawPacketMonAttachPt()->set_notifier(0);
  63 }
  64
  65 void TrcPktProcStm::initObj()
  66 {
  67     m_supported_op_flags = STM_SUPPORTED_OP_FLAGS;
  68     initProcessorState();
  69     getRawPacketMonAttachPt()->set_notifier(&mon_in_use);
  70     buildOpTables();
  71 }
  72
  73 // implementation packet processing interface overrides
  74 // ************************
  75 ocsd_datapath_resp_t TrcPktProcStm::processData(  const ocsd_trc_index_t index,
  76                                             const uint32_t dataBlockSize,
  77                                             const uint8_t *pDataBlock,
  78                                             uint32_t *numBytesProcessed)
  79 {
  80     ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
  81     m_p_data_in = pDataBlock;
  82     m_data_in_size = dataBlockSize;
  83     m_data_in_used = 0;
  84
  85     // while there is data and a continue response on the data path
  86     while(  dataToProcess() && OCSD_DATA_RESP_IS_CONT(resp) )
  87     {
  88         try
  89         {
  90             switch(m_proc_state)
  91             {
  92             case WAIT_SYNC:
  93                 waitForSync(index);
  94                 break;
  95
  96             case PROC_HDR:
  97                 m_packet_index = index + m_data_in_used;
  98                 if(readNibble())
  99                 {
 100                     m_proc_state = PROC_DATA;   // read the header nibble, next if any has to be data
 101                     m_pCurrPktFn = m_1N_ops[m_nibble]; // set packet function and fall through
 102                 }
 103                 else
 104                     break;
 105
 106             case PROC_DATA:
 107                 (this->*m_pCurrPktFn)();
 108
 109                 // if we have enough to send, fall through, otherwise stop
 110                 if(m_proc_state != SEND_PKT)
 111                     break;
 112
 113             case SEND_PKT:
 114                 resp = outputPacket();
 115                 break;
 116             }
 117         }
 118         catch(ocsdError &err)
 119         {
 120             LogError(err);
 121             if( ((err.getErrorCode() == OCSD_ERR_BAD_PACKET_SEQ) ||
 122                  (err.getErrorCode() == OCSD_ERR_INVALID_PCKT_HDR)) &&
 123                  !(getComponentOpMode() & OCSD_OPFLG_PKTPROC_ERR_BAD_PKTS))
 124             {
 125                 // send invalid packets up the pipe to let the next stage decide what to do.
 126                 resp = outputPacket();
 127                 if(getComponentOpMode() & OCSD_OPFLG_PKTPROC_UNSYNC_ON_BAD_PKTS)
 128                     m_proc_state = WAIT_SYNC;
 129             }
 130             else
 131             {
 132                 // bail out on any other error.
 133                 resp = OCSD_RESP_FATAL_INVALID_DATA;
 134             }
 135         }
 136         catch(...)
 137         {
 138             /// vv bad at this point.
 139             resp = OCSD_RESP_FATAL_SYS_ERR;
 140             ocsdError fatal = ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_FAIL,m_packet_index,m_config->getTraceID());
 141             fatal.setMessage("Unknown System Error decoding trace.");
 142             LogError(fatal);
 143         }
 144     }
 145
 146     *numBytesProcessed = m_data_in_used;
 147     return resp;
 148
 149 }
 150
 151 ocsd_datapath_resp_t TrcPktProcStm::onEOT()
 152 {
 153     ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
 154     if(m_num_nibbles > 0)   // there is a partial packet in flight
 155     {
 156         m_curr_packet.updateErrType(STM_PKT_INCOMPLETE_EOT);    // re mark as incomplete
 157         resp = outputPacket();
 158     }
 159     return resp;
 160 }
 161
 162 ocsd_datapath_resp_t TrcPktProcStm::onReset()
 163 {
 164     initProcessorState();
 165     return OCSD_RESP_CONT;
 166 }
 167
 168 ocsd_datapath_resp_t TrcPktProcStm::onFlush()
 169 {
 170     // packet processor never holds on to flushable data (may have partial packet,
 171     // but any full packets are immediately sent)
 172     return OCSD_RESP_CONT;
 173 }
 174
 175 ocsd_err_t TrcPktProcStm::onProtocolConfig()
 176 {
 177     return OCSD_OK;  // nothing to do on config for this processor
 178 }
 179
 180 const bool TrcPktProcStm::isBadPacket() const
 181 {
 182     return m_curr_packet.isBadPacket();
 183 }
 184
 185 ocsd_datapath_resp_t TrcPktProcStm::outputPacket()
 186 {
 187     ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
 188     resp = outputOnAllInterfaces(m_packet_index,&m_curr_packet,&m_curr_packet.type,m_packet_data);
 189     m_packet_data.clear();
 190     initNextPacket();
 191     if(m_nibble_2nd_valid)
 192         savePacketByte(m_nibble_2nd << 4);     // put the unused nibble back on to the data stack and pad for output next time.
 193     m_proc_state = m_bStreamSync ? PROC_HDR : WAIT_SYNC;
 194     return resp;
 195 }
 196
 197 void TrcPktProcStm::throwBadSequenceError(const char *pszMessage /*= ""*/)
 198 {
 199     m_curr_packet.updateErrType(STM_PKT_BAD_SEQUENCE);
 200     throw ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_BAD_PACKET_SEQ,m_packet_index,this->m_config->getTraceID(),pszMessage);
 201 }
 202
 203 void TrcPktProcStm::throwReservedHdrError(const char *pszMessage /*= ""*/)
 204 {
 205     m_curr_packet.setPacketType(STM_PKT_RESERVED,false);
 206     throw ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_INVALID_PCKT_HDR,m_packet_index,this->m_config->getTraceID(),pszMessage);
 207 }
 208
 209 // processor / packet init
 210 // ************************
 211
 212 void TrcPktProcStm::initProcessorState()
 213 {
 214     // clear any state that persists between packets
 215     setProcUnsynced();
 216     clearSyncCount();
 217     m_curr_packet.initStartState();
 218     m_nibble_2nd_valid = false;
 219     initNextPacket();
 220     m_bWaitSyncSaveSuppressed = false;
 221
 222     m_packet_data.clear();
 223 }
 224
 225 void TrcPktProcStm::initNextPacket()
 226 {
 227     // clear state that is unique to each packet
 228     m_bNeedsTS = false;
 229     m_bIsMarker = false;
 230     m_num_nibbles = 0;
 231     m_num_data_nibbles = 0;
 232     m_curr_packet.initNextPacket();
 233 }
 234
 235 // search remaining buffer for a start of sync or full sync packet
 236 void TrcPktProcStm::waitForSync(const ocsd_trc_index_t blk_st_index)
 237 {
 238     bool bGotData = true;
 239     uint32_t start_offset = m_data_in_used; // record the offset into the buffer at start of this fn.
 240
 241     // input conditions:
 242     // out of sync - either at start of input stream, or due to bad packet.
 243     // m_data_in_used -> bytes already processed
 244     // m_sync_start -> seen potential start of sync in current stream
 245
 246     // set a packet index for the start of the data
 247     m_packet_index = blk_st_index + m_data_in_used;
 248     m_num_nibbles = m_is_sync ? m_num_F_nibbles + 1 : m_num_F_nibbles;    // sending unsync data may have cleared down num_nibbles.
 249
 250     m_bWaitSyncSaveSuppressed = true;   // no need to save bytes until we want to send data.
 251
 252     while(bGotData && !m_is_sync)
 253     {
 254         bGotData = readNibble();    // read until we have a sync or run out of data
 255     }
 256
 257     m_bWaitSyncSaveSuppressed = false;
 258
 259     // no data from first attempt to read
 260     if(m_num_nibbles == 0)
 261         return;
 262
 263     // we have found a sync or run out of data
 264     // five possible scenarios
 265     // a) all data none sync data.
 266     // b) some none sync data + start of sync sequence
 267     // c) some none sync data + full sync sequence in this frame
 268     // d) full sync sequence @ start of this frame followed by ???
 269     // e) completion of sync sequence in this frame (from b)).
 270
 271     if(!bGotData || m_num_nibbles > 22)
 272     {
 273         // for a), b), c) send the none sync data then re-enter
 274         // if out of data, or sync with some previous data, this is sent as unsynced.
 275
 276         m_curr_packet.setPacketType(STM_PKT_NOTSYNC,false);
 277         if(mon_in_use.usingMonitor())
 278         {
 279             uint8_t nibbles_to_send = m_num_nibbles - (m_is_sync ? 22 : m_num_F_nibbles);
 280             uint8_t bytes_to_send = (nibbles_to_send / 2) + (nibbles_to_send % 2);
 281             for(uint8_t i = 0; i < bytes_to_send; i++)
 282                 savePacketByte(m_p_data_in[start_offset+i]);
 283         }
 284
 285         // if we have found a sync then we will re-enter this function with no pre data,
 286         // but the found flags set.
 287     }
 288     else
 289     {
 290         // send the async packet
 291         m_curr_packet.setPacketType(STM_PKT_ASYNC,false);
 292         m_bStreamSync = true;   // mark the stream as synchronised
 293         clearSyncCount();
 294         m_packet_index = m_sync_index;
 295         if(mon_in_use.usingMonitor())
 296         {
 297             // we may not have the full sync packet still in the local buffer so synthesise it.
 298             for(int i = 0; i < 10; i++)
 299                 savePacketByte(0xFF);
 300             savePacketByte(0x0F);
 301         }
 302     }
 303     sendPacket();  // mark packet for sending
 304 }
 305
 306 // packet processing routines
 307 // ************************
 308 // 1 nibble opcodes
 309 void TrcPktProcStm::stmPktReserved()
 310 {
 311     uint16_t bad_opcode = (uint16_t)m_nibble;
 312     m_curr_packet.setD16Payload(bad_opcode);
 313     throwReservedHdrError("STM: Unsupported or Reserved STPv2 Header");
 314 }
 315
 316 void TrcPktProcStm::stmPktNull()
 317 {
 318     m_curr_packet.setPacketType(STM_PKT_NULL,false);
 319     if(m_bNeedsTS)
 320     {
 321         m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
 322         (this->*m_pCurrPktFn)();
 323     }
 324     else
 325     {
 326         sendPacket();
 327     }
 328 }
 329
 330 void TrcPktProcStm::stmPktNullTS()
 331 {
 332     pktNeedsTS();
 333     m_pCurrPktFn = &TrcPktProcStm::stmPktNull;
 334     (this->*m_pCurrPktFn)();
 335 }
 336
 337 void TrcPktProcStm::stmPktM8()
 338 {
 339     if(m_num_nibbles == 1)    // 1st nibble - header - set type
 340         m_curr_packet.setPacketType(STM_PKT_M8,false);
 341
 342     stmExtractVal8(3);
 343     if(m_num_nibbles == 3)
 344     {
 345         m_curr_packet.setMaster(m_val8);
 346         sendPacket();
 347     }
 348 }
 349
 350 void TrcPktProcStm::stmPktMERR()
 351 {
 352     if(m_num_nibbles == 1)    // 1st nibble - header - set type
 353         m_curr_packet.setPacketType(STM_PKT_MERR,false);
 354
 355     stmExtractVal8(3);
 356     if(m_num_nibbles == 3)
 357     {
 358         m_curr_packet.setChannel(0,false);    // MERR resets channel for current master to 0.
 359         m_curr_packet.setD8Payload(m_val8);
 360         sendPacket();
 361     }
 362
 363 }
 364
 365 void TrcPktProcStm::stmPktC8()
 366 {
 367     if(m_num_nibbles == 1)    // 1st nibble - header - set type
 368         m_curr_packet.setPacketType(STM_PKT_C8,false);
 369     stmExtractVal8(3);
 370     if(m_num_nibbles == 3)
 371     {
 372         m_curr_packet.setChannel((uint16_t)m_val8,true);
 373         sendPacket();
 374     }
 375 }
 376
 377 void TrcPktProcStm::stmPktD4()
 378 {
 379     if(m_num_nibbles == 1)    // 1st nibble - header - set type
 380     {
 381         m_curr_packet.setPacketType(STM_PKT_D4,m_bIsMarker);
 382         m_num_data_nibbles = 2;  // need 2 nibbles to complete data
 383     }
 384
 385     if(m_num_nibbles != m_num_data_nibbles)
 386     {
 387         if(readNibble())
 388         {
 389             m_curr_packet.setD4Payload(m_nibble);
 390             if(m_bNeedsTS)
 391             {
 392                 m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
 393                 (this->*m_pCurrPktFn)();
 394             }
 395             else
 396                 sendPacket();
 397         }
 398     }
 399 }
 400
 401 void TrcPktProcStm::stmPktD8()
 402 {
 403     if(m_num_nibbles == 1)    // 1st nibble - header - set type
 404     {
 405         m_curr_packet.setPacketType(STM_PKT_D8,m_bIsMarker);
 406         m_num_data_nibbles = 3; // need 3 nibbles in total to complete data
 407     }
 408
 409     stmExtractVal8(m_num_data_nibbles);
 410     if(m_num_nibbles == m_num_data_nibbles)
 411     {
 412         m_curr_packet.setD8Payload(m_val8);
 413         if(m_bNeedsTS)
 414         {
 415             m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
 416             (this->*m_pCurrPktFn)();
 417         }
 418         else
 419         {
 420             sendPacket();
 421         }
 422     }
 423 }
 424
 425 void TrcPktProcStm::stmPktD16()
 426 {
 427     if(m_num_nibbles == 1)    // 1st nibble - header - set type
 428     {
 429         m_curr_packet.setPacketType(STM_PKT_D16,m_bIsMarker);
 430         m_num_data_nibbles = 5;
 431     }
 432
 433     stmExtractVal16(m_num_data_nibbles);
 434     if(m_num_nibbles == m_num_data_nibbles)
 435     {
 436         m_curr_packet.setD16Payload(m_val16);
 437         if(m_bNeedsTS)
 438         {
 439             m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
 440             (this->*m_pCurrPktFn)();
 441         }
 442         else
 443         {
 444             sendPacket();
 445         }
 446     }
 447 }
 448
 449 void TrcPktProcStm::stmPktD32()
 450 {
 451     if(m_num_nibbles == 1)    // 1st nibble - header - set type
 452     {
 453         m_curr_packet.setPacketType(STM_PKT_D32,m_bIsMarker);
 454         m_num_data_nibbles = 9;
 455     }
 456
 457     stmExtractVal32(m_num_data_nibbles);
 458     if(m_num_nibbles == m_num_data_nibbles)
 459     {
 460         m_curr_packet.setD32Payload(m_val32);
 461         if(m_bNeedsTS)
 462         {
 463             m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
 464             (this->*m_pCurrPktFn)();
 465         }
 466         else
 467         {
 468             sendPacket();
 469         }
 470     }
 471 }
 472
 473 void TrcPktProcStm::stmPktD64()
 474 {
 475     if(m_num_nibbles == 1)    // 1st nibble - header - set type
 476     {
 477         m_curr_packet.setPacketType(STM_PKT_D64,m_bIsMarker);
 478         m_num_data_nibbles = 17;
 479     }
 480
 481     stmExtractVal64(m_num_data_nibbles);
 482     if(m_num_nibbles == m_num_data_nibbles)
 483     {
 484         m_curr_packet.setD64Payload(m_val64);
 485         if(m_bNeedsTS)
 486         {
 487             m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
 488             (this->*m_pCurrPktFn)();
 489         }
 490         else
 491         {
 492             sendPacket();
 493         }
 494     }
 495 }
 496
 497 void TrcPktProcStm::stmPktD4MTS()
 498 {
 499     pktNeedsTS();
 500     m_bIsMarker = true;
 501     m_pCurrPktFn = &TrcPktProcStm::stmPktD4;
 502     (this->*m_pCurrPktFn)();
 503 }
 504
 505 void TrcPktProcStm::stmPktD8MTS()
 506 {
 507     pktNeedsTS();
 508     m_bIsMarker = true;
 509     m_pCurrPktFn = &TrcPktProcStm::stmPktD8;
 510     (this->*m_pCurrPktFn)();
 511 }
 512
 513 void TrcPktProcStm::stmPktD16MTS()
 514 {
 515     pktNeedsTS();
 516     m_bIsMarker = true;
 517     m_pCurrPktFn = &TrcPktProcStm::stmPktD16;
 518     (this->*m_pCurrPktFn)();
 519 }
 520
 521 void TrcPktProcStm::stmPktD32MTS()
 522 {
 523     pktNeedsTS();
 524     m_bIsMarker = true;
 525     m_pCurrPktFn = &TrcPktProcStm::stmPktD32;
 526     (this->*m_pCurrPktFn)();
 527 }
 528
 529 void TrcPktProcStm::stmPktD64MTS()
 530 {
 531     pktNeedsTS();
 532     m_bIsMarker = true;
 533     m_pCurrPktFn = &TrcPktProcStm::stmPktD64;
 534     (this->*m_pCurrPktFn)();
 535 }
 536
 537 void TrcPktProcStm::stmPktFlagTS()
 538 {
 539     pktNeedsTS();
 540     m_curr_packet.setPacketType(STM_PKT_FLAG,false);
 541     m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
 542     (this->*m_pCurrPktFn)();
 543 }
 544
 545 void TrcPktProcStm::stmPktFExt()
 546 {
 547     // no type, look at the next nibble
 548     if(readNibble())
 549     {
 550         // switch in 2N function
 551         m_pCurrPktFn = m_2N_ops[m_nibble];
 552         (this->*m_pCurrPktFn)();
 553     }
 554 }
 555
 556 // ************************
 557 // 2 nibble opcodes 0xFn
 558 void TrcPktProcStm::stmPktReservedFn()
 559 {
 560     uint16_t bad_opcode = 0x00F;
 561     bad_opcode |= ((uint16_t)m_nibble) << 4;
 562     m_curr_packet.setD16Payload(bad_opcode);
 563     throwReservedHdrError("STM: Unsupported or Reserved STPv2 Header");
 564 }
 565
 566 void TrcPktProcStm::stmPktF0Ext()
 567 {
 568     // no type yet, look at the next nibble
 569     if(readNibble())
 570     {
 571         // switch in 3N function
 572         m_pCurrPktFn = m_3N_ops[m_nibble];
 573         (this->*m_pCurrPktFn)();
 574     }
 575 }
 576
 577 void TrcPktProcStm::stmPktGERR()
 578 {
 579     if(m_num_nibbles == 2)    // 2nd nibble - header - set type
 580         m_curr_packet.setPacketType(STM_PKT_GERR,false);
 581     stmExtractVal8(4);
 582     if(m_num_nibbles == 4)
 583     {
 584         m_curr_packet.setD8Payload(m_val8);
 585         m_curr_packet.setMaster(0); // GERR sets current master to 0.
 586         sendPacket();
 587     }
 588 }
 589
 590 void TrcPktProcStm::stmPktC16()
 591 {
 592     if(m_num_nibbles == 2)    // 2nd nibble - header - set type
 593         m_curr_packet.setPacketType(STM_PKT_C16,false);
 594     stmExtractVal16(6);
 595     if(m_num_nibbles == 6)
 596     {
 597         m_curr_packet.setChannel(m_val16,false);
 598         sendPacket();
 599     }
 600 }
 601
 602 void TrcPktProcStm::stmPktD4TS()
 603 {
 604     pktNeedsTS();
 605     m_curr_packet.setPacketType(STM_PKT_D4,false); // 2nd nibble, set type here
 606     m_num_data_nibbles = 3; // one more nibble for data
 607     m_pCurrPktFn = &TrcPktProcStm::stmPktD4;
 608     (this->*m_pCurrPktFn)();
 609 }
 610
 611 void TrcPktProcStm::stmPktD8TS()
 612 {
 613     pktNeedsTS();
 614     m_curr_packet.setPacketType(STM_PKT_D8,false); // 2nd nibble, set type here
 615     m_num_data_nibbles = 4;
 616     m_pCurrPktFn = &TrcPktProcStm::stmPktD8;
 617     (this->*m_pCurrPktFn)();
 618 }
 619
 620 void TrcPktProcStm::stmPktD16TS()
 621 {
 622     pktNeedsTS();
 623     m_curr_packet.setPacketType(STM_PKT_D16,false); // 2nd nibble, set type here
 624     m_num_data_nibbles = 6;
 625     m_pCurrPktFn = &TrcPktProcStm::stmPktD16;
 626     (this->*m_pCurrPktFn)();
 627 }
 628
 629 void TrcPktProcStm::stmPktD32TS()
 630 {
 631     pktNeedsTS();
 632     m_curr_packet.setPacketType(STM_PKT_D32,false); // 2nd nibble, set type here
 633     m_num_data_nibbles = 10;
 634     m_pCurrPktFn = &TrcPktProcStm::stmPktD32;
 635     (this->*m_pCurrPktFn)();
 636 }
 637
 638 void TrcPktProcStm::stmPktD64TS()
 639 {
 640     pktNeedsTS();
 641     m_curr_packet.setPacketType(STM_PKT_D64,false); // 2nd nibble, set type here
 642     m_num_data_nibbles = 18;
 643     m_pCurrPktFn = &TrcPktProcStm::stmPktD64;
 644     (this->*m_pCurrPktFn)();
 645 }
 646
 647 void TrcPktProcStm::stmPktD4M()
 648 {
 649     m_curr_packet.setPacketType(STM_PKT_D4,true); // 2nd nibble, set type here
 650     m_num_data_nibbles = 3; // one more nibble for data
 651     m_pCurrPktFn = &TrcPktProcStm::stmPktD4;
 652     (this->*m_pCurrPktFn)();
 653 }
 654
 655 void TrcPktProcStm::stmPktD8M()
 656 {
 657     m_curr_packet.setPacketType(STM_PKT_D8,true); // 2nd nibble, set type here
 658     m_num_data_nibbles = 4;
 659     m_pCurrPktFn = &TrcPktProcStm::stmPktD8;
 660     (this->*m_pCurrPktFn)();
 661 }
 662
 663 void TrcPktProcStm::stmPktD16M()
 664 {
 665     m_curr_packet.setPacketType(STM_PKT_D16,true);
 666     m_num_data_nibbles = 6;
 667     m_pCurrPktFn = &TrcPktProcStm::stmPktD16;
 668     (this->*m_pCurrPktFn)();
 669 }
 670
 671 void TrcPktProcStm::stmPktD32M()
 672 {
 673     m_curr_packet.setPacketType(STM_PKT_D32,true);
 674     m_num_data_nibbles = 10;
 675     m_pCurrPktFn = &TrcPktProcStm::stmPktD32;
 676     (this->*m_pCurrPktFn)();
 677 }
 678
 679 void TrcPktProcStm::stmPktD64M()
 680 {
 681     m_curr_packet.setPacketType(STM_PKT_D64,true);
 682     m_num_data_nibbles = 18;
 683     m_pCurrPktFn = &TrcPktProcStm::stmPktD64;
 684     (this->*m_pCurrPktFn)();
 685 }
 686
 687 void TrcPktProcStm::stmPktFlag()
 688 {
 689     m_curr_packet.setPacketType(STM_PKT_FLAG,false);
 690     sendPacket();
 691 }
 692
 693 // ************************
 694 // 3 nibble opcodes 0xF0n
 695 void TrcPktProcStm::stmPktReservedF0n()
 696 {
 697     uint16_t bad_opcode = 0x00F;
 698     bad_opcode |= ((uint16_t)m_nibble) << 8;
 699     m_curr_packet.setD16Payload(bad_opcode);
 700     throwReservedHdrError("STM: Unsupported or Reserved STPv2 Header");
 701 }
 702
 703 void TrcPktProcStm::stmPktVersion()
 704 {
 705     if(m_num_nibbles == 3)
 706         m_curr_packet.setPacketType(STM_PKT_VERSION,false);
 707
 708     if(readNibble())
 709     {
 710         m_curr_packet.setD8Payload(m_nibble);   // record the version number
 711         switch(m_nibble)
 712         {
 713         case 3:
 714             m_curr_packet.onVersionPkt(STM_TS_NATBINARY); break;
 715         case 4:
 716             m_curr_packet.onVersionPkt(STM_TS_GREY); break;
 717         default:
 718             // not a version we support.
 719             throwBadSequenceError("STM VERSION packet : unrecognised version number.");
 720         }
 721         sendPacket();
 722     }
 723 }
 724
 725 void TrcPktProcStm::stmPktTrigger()
 726 {
 727     if(m_num_nibbles == 3)
 728         m_curr_packet.setPacketType(STM_PKT_TRIG,false);
 729     stmExtractVal8(5);
 730     if(m_num_nibbles == 5)
 731     {
 732         m_curr_packet.setD8Payload(m_val8);
 733         if(m_bNeedsTS)
 734         {
 735             m_pCurrPktFn = &TrcPktProcStm::stmExtractTS;
 736             (this->*m_pCurrPktFn)();
 737         }
 738         else
 739         {
 740             sendPacket();
 741         }
 742     }
 743 }
 744
 745 void TrcPktProcStm::stmPktTriggerTS()
 746 {
 747     pktNeedsTS();
 748     m_pCurrPktFn = &TrcPktProcStm::stmPktTrigger;
 749     (this->*m_pCurrPktFn)();
 750 }
 751
 752 void TrcPktProcStm::stmPktFreq()
 753 {
 754     if(m_num_nibbles == 3)
 755     {
 756         m_curr_packet.setPacketType(STM_PKT_FREQ,false);
 757         m_val32 = 0;
 758     }
 759     stmExtractVal32(11);
 760     if(m_num_nibbles == 11)
 761     {
 762         m_curr_packet.setD32Payload(m_val32);
 763         sendPacket();
 764     }
 765 }
 766
 767 void TrcPktProcStm::stmPktASync()
 768 {
 769     // 2 nibbles - 0xFF - must be an async or error.
 770     bool bCont = true;
 771     while(bCont)
 772     {
 773         bCont = readNibble();
 774         if(bCont)
 775         {
 776             if(m_is_sync)
 777             {
 778                 bCont = false;  // stop reading nibbles
 779                 m_bStreamSync = true;   // mark stream in sync
 780                 m_curr_packet.setPacketType(STM_PKT_ASYNC,false);
 781                 clearSyncCount();
 782                 sendPacket();
 783             }
 784             else if(!m_sync_start)  // no longer valid sync packet
 785             {
 786                 throwBadSequenceError("STM: Invalid ASYNC sequence");
 787             }
 788         }
 789     }
 790 }
 791
 792 // ************************
 793 // general data processing
 794
 795 // return false if no more data
 796 // in an STM byte, 3:0 is 1st nibble in protocol order, 7:4 is 2nd nibble.
 797 bool TrcPktProcStm::readNibble()
 798 {
 799     bool dataFound = true;
 800     if(m_nibble_2nd_valid)
 801     {
 802         m_nibble = m_nibble_2nd;
 803         m_nibble_2nd_valid = false;
 804         m_num_nibbles++;
 805         checkSyncNibble();
 806     }
 807     else if(m_data_in_used < m_data_in_size )
 808     {
 809         m_nibble = m_p_data_in[m_data_in_used++];
 810         savePacketByte(m_nibble);
 811         m_nibble_2nd = (m_nibble >> 4) & 0xF;
 812         m_nibble_2nd_valid = true;
 813         m_nibble &= 0xF;
 814         m_num_nibbles++;
 815         checkSyncNibble();
 816     }
 817     else
 818         dataFound = false;  // no data available
 819     return dataFound;
 820 }
 821
 822 void TrcPktProcStm::pktNeedsTS()
 823 {
 824     m_bNeedsTS = true;
 825     m_req_ts_nibbles = 0;
 826     m_curr_ts_nibbles = 0;
 827     m_ts_update_value = 0;
 828     m_ts_req_set = false;
 829 }
 830
 831 void TrcPktProcStm::stmExtractTS()
 832 {
 833     if(!m_ts_req_set)
 834     {
 835         if(readNibble())
 836         {
 837             m_req_ts_nibbles = m_nibble;
 838             if(m_nibble == 0xD)
 839                 m_req_ts_nibbles = 14;
 840             else if(m_nibble == 0xE)
 841                 m_req_ts_nibbles = 16;
 842
 843             if(m_nibble == 0xF)
 844                 throwBadSequenceError("STM: Invalid timestamp size 0xF");
 845             m_ts_req_set = true;
 846         }
 847     }
 848
 849     if(m_ts_req_set)
 850     {
 851         // if we do not have all the nibbles for the TS, get some...
 852         if(m_req_ts_nibbles != m_curr_ts_nibbles)
 853         {
 854             // extract the correct amount of nibbles for the ts value.
 855             bool bCont = true;
 856             while(bCont && (m_curr_ts_nibbles < m_req_ts_nibbles))
 857             {
 858                 bCont = readNibble();
 859                 if(bCont)
 860                 {
 861                     m_ts_update_value <<= 4;
 862                     m_ts_update_value |= m_nibble;
 863                     m_curr_ts_nibbles++;
 864                 }
 865             }
 866         }
 867
 868         // at this point we have the correct amount of nibbles, or have run out of data to process.
 869         if(m_req_ts_nibbles == m_curr_ts_nibbles)
 870         {
 871             uint8_t new_bits = m_req_ts_nibbles * 4;
 872             if(m_curr_packet.getTSType() == STM_TS_GREY)
 873             {
 874                 uint64_t gray_val = bin_to_gray(m_curr_packet.getTSVal());
 875                 if(new_bits == 64)
 876                 {
 877                     gray_val = m_ts_update_value;
 878                 }
 879                 else
 880                 {
 881                     uint64_t mask = (0x1ULL << new_bits) - 1;
 882                     gray_val &= ~mask;
 883                     gray_val |= m_ts_update_value & mask;
 884                 }
 885                 m_curr_packet.setTS(gray_to_bin(gray_val),new_bits);
 886             }
 887             else if(m_curr_packet.getTSType() == STM_TS_NATBINARY)
 888             {
 889                 m_curr_packet.setTS(m_ts_update_value, new_bits);
 890             }
 891             else
 892                 throwBadSequenceError("STM: unknown timestamp encoding");
 893
 894             sendPacket();
 895         }
 896     }
 897 }
 898
 899 // pass in number of nibbles needed to extract the value
 900 void TrcPktProcStm::stmExtractVal8(uint8_t nibbles_to_val)
 901 {
 902     bool bCont = true;
 903     while(bCont && (m_num_nibbles < nibbles_to_val))
 904     {
 905         bCont = readNibble();
 906         if(bCont)   // got a nibble
 907         {
 908             m_val8 <<= 4;
 909             m_val8 |= m_nibble;
 910         }
 911     }
 912 }
 913
 914 void TrcPktProcStm::stmExtractVal16(uint8_t nibbles_to_val)
 915 {
 916     bool bCont = true;
 917     while(bCont && (m_num_nibbles < nibbles_to_val))
 918     {
 919         bCont = readNibble();
 920         if(bCont)   // got a nibble
 921         {
 922             m_val16 <<= 4;
 923             m_val16 |= m_nibble;
 924         }
 925     }
 926 }
 927
 928 void TrcPktProcStm::stmExtractVal32(uint8_t nibbles_to_val)
 929 {
 930     bool bCont = true;
 931     while(bCont && (m_num_nibbles < nibbles_to_val))
 932     {
 933         bCont = readNibble();
 934         if(bCont)   // got a nibble
 935         {
 936             m_val32 <<= 4;
 937             m_val32 |= m_nibble;
 938         }
 939     }
 940 }
 941
 942 void TrcPktProcStm::stmExtractVal64(uint8_t nibbles_to_val)
 943 {
 944     bool bCont = true;
 945     while(bCont && (m_num_nibbles < nibbles_to_val))
 946     {
 947         bCont = readNibble();
 948         if(bCont)   // got a nibble
 949         {
 950             m_val64 <<= 4;
 951             m_val64 |= m_nibble;
 952         }
 953     }
 954 }
 955
 956 uint64_t TrcPktProcStm::bin_to_gray(uint64_t bin_value)
 957 {
 958         uint64_t gray_value = 0;
 959         gray_value = (1ull << 63) & bin_value;
 960         int i = 62;
 961         for (; i >= 0; i--) {
 962                 uint64_t gray_arg_1 = ((1ull << (i+1)) & bin_value) >> (i+1);
 963                 uint64_t gray_arg_2 = ((1ull << i) & bin_value) >> i;
 964                 gray_value |= ((gray_arg_1 ^ gray_arg_2) << i);
 965         }
 966         return gray_value;
 967 }
 968
 969 uint64_t TrcPktProcStm::gray_to_bin(uint64_t gray_value)
 970 {
 971         uint64_t bin_value = 0;
 972         int bin_bit = 0;
 973         for (; bin_bit < 64; bin_bit++) {
 974                 uint8_t bit_tmp = ((1ull << bin_bit) & gray_value) >> bin_bit;
 975                 uint8_t gray_bit = bin_bit + 1;
 976                 for (; gray_bit < 64; gray_bit++)
 977                         bit_tmp ^= (((1ull << gray_bit) & gray_value) >> gray_bit);
 978
 979                 bin_value |= (bit_tmp << bin_bit);
 980         }
 981
 982         return bin_value;
 983 }
 984
 985
 986 void TrcPktProcStm::buildOpTables()
 987 {
 988     // init all reserved
 989     for(int i = 0; i < 0x10; i++)
 990     {
 991         m_1N_ops[i] = &TrcPktProcStm::stmPktReserved;
 992         m_2N_ops[i] = &TrcPktProcStm::stmPktReservedFn;
 993         m_3N_ops[i] = &TrcPktProcStm::stmPktReservedF0n;
 994     }
 995
 996     // set the 1N operations
 997     m_1N_ops[0x0] = &TrcPktProcStm::stmPktNull;
 998     m_1N_ops[0x1] = &TrcPktProcStm::stmPktM8;
 999     m_1N_ops[0x2] = &TrcPktProcStm::stmPktMERR;
1000     m_1N_ops[0x3] = &TrcPktProcStm::stmPktC8;
1001     m_1N_ops[0x4] = &TrcPktProcStm::stmPktD8;
1002     m_1N_ops[0x5] = &TrcPktProcStm::stmPktD16;
1003     m_1N_ops[0x6] = &TrcPktProcStm::stmPktD32;
1004     m_1N_ops[0x7] = &TrcPktProcStm::stmPktD64;
1005     m_1N_ops[0x8] = &TrcPktProcStm::stmPktD8MTS;
1006     m_1N_ops[0x9] = &TrcPktProcStm::stmPktD16MTS;
1007     m_1N_ops[0xA] = &TrcPktProcStm::stmPktD32MTS;
1008     m_1N_ops[0xB] = &TrcPktProcStm::stmPktD64MTS;
1009     m_1N_ops[0xC] = &TrcPktProcStm::stmPktD4;
1010     m_1N_ops[0xD] = &TrcPktProcStm::stmPktD4MTS;
1011     m_1N_ops[0xE] = &TrcPktProcStm::stmPktFlagTS;
1012     m_1N_ops[0xF] = &TrcPktProcStm::stmPktFExt;
1013
1014     // set the 2N operations 0xFn
1015     m_2N_ops[0x0] = &TrcPktProcStm::stmPktF0Ext;
1016     // 0x1 unused in CS STM
1017     m_2N_ops[0x2] = &TrcPktProcStm::stmPktGERR;
1018     m_2N_ops[0x3] = &TrcPktProcStm::stmPktC16;
1019     m_2N_ops[0x4] = &TrcPktProcStm::stmPktD8TS;
1020     m_2N_ops[0x5] = &TrcPktProcStm::stmPktD16TS;
1021     m_2N_ops[0x6] = &TrcPktProcStm::stmPktD32TS;
1022     m_2N_ops[0x7] = &TrcPktProcStm::stmPktD64TS;
1023     m_2N_ops[0x8] = &TrcPktProcStm::stmPktD8M;
1024     m_2N_ops[0x9] = &TrcPktProcStm::stmPktD16M;
1025     m_2N_ops[0xA] = &TrcPktProcStm::stmPktD32M;
1026     m_2N_ops[0xB] = &TrcPktProcStm::stmPktD64M;
1027     m_2N_ops[0xC] = &TrcPktProcStm::stmPktD4TS;
1028     m_2N_ops[0xD] = &TrcPktProcStm::stmPktD4M;
1029     m_2N_ops[0xE] = &TrcPktProcStm::stmPktFlag;
1030     m_2N_ops[0xF] = &TrcPktProcStm::stmPktASync;
1031
1032     // set the 3N operations 0xF0n
1033     m_3N_ops[0x0] = &TrcPktProcStm::stmPktVersion;
1034     m_3N_ops[0x1] = &TrcPktProcStm::stmPktNullTS;
1035     // 0x2 .. 0x5 not used by CS STM
1036     m_3N_ops[0x6] = &TrcPktProcStm::stmPktTrigger;
1037     m_3N_ops[0x7] = &TrcPktProcStm::stmPktTriggerTS;
1038     m_3N_ops[0x8] = &TrcPktProcStm::stmPktFreq;
1039     // 0x9 .. 0xF not used by CS STM
1040
1041 }
1042
1043 /* End of File trc_pkt_proc_stm.cpp */