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 48static const uint32_t STM_SUPPORTED_OP_FLAGS = OCSD_OPFLG_PKTPROC_COMMON; 49 50TrcPktProcStm::TrcPktProcStm() : TrcPktProcBase(STM_PKTS_NAME) 51{ 52 initObj(); 53} 54 55TrcPktProcStm::TrcPktProcStm(int instIDNum) : TrcPktProcBase(STM_PKTS_NAME, instIDNum) 56{ 57 initObj(); 58} 59 60TrcPktProcStm::~TrcPktProcStm() 61{ 62 getRawPacketMonAttachPt()->set_notifier(0); 63} 64 65void 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// ************************ 75ocsd_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 151ocsd_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 162ocsd_datapath_resp_t TrcPktProcStm::onReset() 163{ 164 initProcessorState(); 165 return OCSD_RESP_CONT; 166} 167 168ocsd_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 175ocsd_err_t TrcPktProcStm::onProtocolConfig() 176{ 177 return OCSD_OK; // nothing to do on config for this processor 178} 179 180const bool TrcPktProcStm::isBadPacket() const 181{ 182 return m_curr_packet.isBadPacket(); 183} 184 185ocsd_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 197void 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 203void 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 212void 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 225void 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 236void 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 309void 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 316void 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 330void TrcPktProcStm::stmPktNullTS() 331{ 332 pktNeedsTS(); 333 m_pCurrPktFn = &TrcPktProcStm::stmPktNull; 334 (this->*m_pCurrPktFn)(); 335} 336 337void 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 350void 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 365void 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 377void 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 401void 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 425void 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 449void 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 473void 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 497void TrcPktProcStm::stmPktD4MTS() 498{ 499 pktNeedsTS(); 500 m_bIsMarker = true; 501 m_pCurrPktFn = &TrcPktProcStm::stmPktD4; 502 (this->*m_pCurrPktFn)(); 503} 504 505void TrcPktProcStm::stmPktD8MTS() 506{ 507 pktNeedsTS(); 508 m_bIsMarker = true; 509 m_pCurrPktFn = &TrcPktProcStm::stmPktD8; 510 (this->*m_pCurrPktFn)(); 511} 512 513void TrcPktProcStm::stmPktD16MTS() 514{ 515 pktNeedsTS(); 516 m_bIsMarker = true; 517 m_pCurrPktFn = &TrcPktProcStm::stmPktD16; 518 (this->*m_pCurrPktFn)(); 519} 520 521void TrcPktProcStm::stmPktD32MTS() 522{ 523 pktNeedsTS(); 524 m_bIsMarker = true; 525 m_pCurrPktFn = &TrcPktProcStm::stmPktD32; 526 (this->*m_pCurrPktFn)(); 527} 528 529void TrcPktProcStm::stmPktD64MTS() 530{ 531 pktNeedsTS(); 532 m_bIsMarker = true; 533 m_pCurrPktFn = &TrcPktProcStm::stmPktD64; 534 (this->*m_pCurrPktFn)(); 535} 536 537void 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 545void 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 558void 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 566void 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 577void 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 590void 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 602void 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 611void 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 620void 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 629void 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 638void 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 647void 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 655void 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 663void 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 671void 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 679void 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 687void TrcPktProcStm::stmPktFlag() 688{ 689 m_curr_packet.setPacketType(STM_PKT_FLAG,false); 690 sendPacket(); 691} 692 693// ************************ 694// 3 nibble opcodes 0xF0n 695void 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 703void 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 725void 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 745void TrcPktProcStm::stmPktTriggerTS() 746{ 747 pktNeedsTS(); 748 m_pCurrPktFn = &TrcPktProcStm::stmPktTrigger; 749 (this->*m_pCurrPktFn)(); 750} 751 752void 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 767void 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. 797bool 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 822void 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 831void 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 900void 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 914void 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 928void 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 942void 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 956uint64_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 969uint64_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 986void 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 */ 1044