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