contrib/opencsd/decoder/source/trc_frame_deformatter.cpp

   1 /*
   2  * \file       trc_frame_deformatter.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 #include <cstring>
  35
  36 #include "common/trc_frame_deformatter.h"
  37 #include "trc_frame_deformatter_impl.h"
  38
  39 /***************************************************************/
  40 /* Implementation */
  41 /***************************************************************/
  42
  43 #ifdef __GNUC__
  44 // G++ doesn't like the ## pasting
  45 #define DEFORMATTER_NAME "DFMT_CSFRAMES"
  46 #else
  47 // VC is fine
  48 #define DEFORMATTER_NAME OCSD_CMPNAME_PREFIX_FRAMEDEFORMATTER##"_CSFRAMES"
  49 #endif
  50
  51 TraceFmtDcdImpl::TraceFmtDcdImpl() : TraceComponent(DEFORMATTER_NAME),
  52     m_cfgFlags(0),
  53     m_force_sync_idx(0),
  54     m_use_force_sync(false),
  55     m_alignment(16), // assume frame aligned data as default.
  56     m_b_output_packed_raw(false),
  57     m_b_output_unpacked_raw(false)
  58
  59 {
  60     resetStateParams();
  61     setRawChanFilterAll(true);
  62 }
  63
  64 TraceFmtDcdImpl::TraceFmtDcdImpl(int instNum) : TraceComponent(DEFORMATTER_NAME, instNum),
  65     m_cfgFlags(0),
  66     m_force_sync_idx(0),
  67     m_use_force_sync(false),
  68     m_alignment(16)
  69 {
  70     resetStateParams();
  71     setRawChanFilterAll(true);
  72 }
  73
  74 TraceFmtDcdImpl::~TraceFmtDcdImpl()
  75 {
  76 }
  77
  78 ocsd_datapath_resp_t TraceFmtDcdImpl::TraceDataIn(
  79     const ocsd_datapath_op_t op,
  80     const ocsd_trc_index_t index,
  81     const uint32_t dataBlockSize,
  82     const uint8_t *pDataBlock,
  83     uint32_t *numBytesProcessed)
  84 {
  85     ocsd_datapath_resp_t resp = OCSD_RESP_FATAL_INVALID_OP;
  86     InitCollateDataPathResp();
  87
  88     m_b_output_packed_raw = m_RawTraceFrame.num_attached() && ((m_cfgFlags & OCSD_DFRMTR_PACKED_RAW_OUT) != 0);
  89     m_b_output_unpacked_raw = m_RawTraceFrame.num_attached() && ((m_cfgFlags & OCSD_DFRMTR_UNPACKED_RAW_OUT) != 0);
  90
  91     switch(op)
  92     {
  93     case OCSD_OP_RESET:
  94         resp = Reset();
  95         break;
  96
  97     case OCSD_OP_FLUSH:
  98         resp = Flush();
  99         break;
 100
 101     case OCSD_OP_EOT:
 102         // local 'flush' here?
 103         // pass on EOT to connected ID streams
 104         resp = executeNoneDataOpAllIDs(OCSD_OP_EOT);
 105         break;
 106
 107     case OCSD_OP_DATA:
 108         if((dataBlockSize <= 0) || ( pDataBlock == 0) || (numBytesProcessed == 0))
 109             resp = OCSD_RESP_FATAL_INVALID_PARAM;
 110         else
 111             resp = processTraceData(index,dataBlockSize, pDataBlock, numBytesProcessed);
 112         break;
 113
 114     default:
 115         break;
 116     }
 117
 118     return resp;
 119 }
 120
 121 /* enable / disable ID streams - default as all enabled */
 122 ocsd_err_t TraceFmtDcdImpl::OutputFilterIDs(std::vector<uint8_t> &id_list, bool bEnable)
 123 {
 124     ocsd_err_t err =  OCSD_OK;
 125     std::vector<uint8_t>::iterator iter = id_list.begin();
 126     uint8_t id = 0;
 127
 128     while((iter < id_list.end()) && (err == OCSD_OK))
 129     {
 130         id = *iter;
 131         if(id > 128)
 132             err = OCSD_ERR_INVALID_ID;
 133         else
 134         {
 135             m_IDStreams[id].set_enabled(bEnable);
 136             m_raw_chan_enable[id] = bEnable;
 137         }
 138         iter++;
 139     }
 140     return err;
 141 }
 142
 143 ocsd_err_t TraceFmtDcdImpl::OutputFilterAllIDs(bool bEnable)
 144 {
 145     for(uint8_t id = 0; id < 128; id++)
 146     {
 147         m_IDStreams[id].set_enabled(bEnable);
 148     }
 149     setRawChanFilterAll(bEnable);
 150     return OCSD_OK;
 151 }
 152
 153 void TraceFmtDcdImpl::setRawChanFilterAll(bool bEnable)
 154 {
 155     for(int i=0; i<128; i++)
 156     {
 157         m_raw_chan_enable[i] = bEnable;
 158     }
 159 }
 160
 161 const bool TraceFmtDcdImpl::rawChanEnabled(const uint8_t id) const
 162 {
 163     if(id < 128)
 164         return m_raw_chan_enable[id];
 165     return false;
 166 }
 167
 168 /* decode control */
 169 ocsd_datapath_resp_t TraceFmtDcdImpl::Reset()
 170 {
 171     resetStateParams();
 172     InitCollateDataPathResp();
 173     return executeNoneDataOpAllIDs(OCSD_OP_RESET);
 174 }
 175
 176 ocsd_datapath_resp_t TraceFmtDcdImpl::Flush()
 177 {
 178     executeNoneDataOpAllIDs(OCSD_OP_FLUSH);    // flush any upstream data.
 179     if(dataPathCont())
 180         outputFrame();  // try to flush any partial frame data remaining
 181     return highestDataPathResp();
 182 }
 183
 184 ocsd_datapath_resp_t TraceFmtDcdImpl::executeNoneDataOpAllIDs(ocsd_datapath_op_t op,
 185                                                               const ocsd_trc_index_t index /* = 0*/)
 186 {
 187     ITrcDataIn *pTrcComp = 0;
 188     for(uint8_t id = 0; id < 128; id++)
 189     {
 190         if(m_IDStreams[id].num_attached())
 191         {
 192             pTrcComp = m_IDStreams[id].first();
 193             while(pTrcComp)
 194             {
 195                 CollateDataPathResp(pTrcComp->TraceDataIn(op,index,0,0,0));
 196                 pTrcComp = m_IDStreams[id].next();
 197             }
 198         }
 199     }
 200
 201     if( m_RawTraceFrame.num_attached())
 202     {
 203         if(m_RawTraceFrame.first())
 204             m_RawTraceFrame.first()->TraceRawFrameIn(op,0,OCSD_FRM_NONE,0,0,0);
 205     }
 206     return highestDataPathResp();
 207 }
 208
 209 void TraceFmtDcdImpl::outputRawMonBytes(const ocsd_datapath_op_t op,
 210                            const ocsd_trc_index_t index,
 211                            const ocsd_rawframe_elem_t frame_element,
 212                            const int dataBlockSize,
 213                            const uint8_t *pDataBlock,
 214                            const uint8_t traceID)
 215 {
 216     if( m_RawTraceFrame.num_attached())
 217     {
 218         if(m_RawTraceFrame.first())
 219             m_RawTraceFrame.first()->TraceRawFrameIn(op,index,frame_element,dataBlockSize, pDataBlock,traceID);
 220     }
 221 }
 222
 223 void TraceFmtDcdImpl::CollateDataPathResp(const ocsd_datapath_resp_t resp)
 224 {
 225     // simple most severe error across multiple IDs.
 226     if(resp > m_highestResp) m_highestResp = resp;
 227 }
 228
 229 ocsd_datapath_resp_t TraceFmtDcdImpl::processTraceData(
 230                                         const ocsd_trc_index_t index,
 231                                         const uint32_t dataBlockSize,
 232                                         const uint8_t *pDataBlock,
 233                                         uint32_t *numBytesProcessed
 234                                         )
 235 {
 236     try {
 237
 238         if(!m_first_data)  // is this the initial data block?
 239         {
 240             m_trc_curr_idx = index;
 241         }
 242         else
 243         {
 244             if(m_trc_curr_idx != index) // none continuous trace data - throw an error.
 245                 throw ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_DFMTR_NOTCONTTRACE,index);
 246         }
 247
 248         if(dataBlockSize % m_alignment) // must be correctly aligned data
 249         {
 250             ocsdError err(OCSD_ERR_SEV_ERROR, OCSD_ERR_INVALID_PARAM_VAL);
 251             char msg_buffer[64];
 252             sprintf(msg_buffer,"Input block incorrect size, must be %d byte multiple", m_alignment);
 253             err.setMessage(msg_buffer);
 254             throw ocsdError(&err);
 255         }
 256
 257         // record the incoming block for extraction routines to use.
 258         m_in_block_base = pDataBlock;
 259         m_in_block_size = dataBlockSize;
 260         m_in_block_processed = 0;
 261
 262         // processing loop...
 263         if(checkForSync())
 264         {
 265             bool bProcessing = true;
 266             while(bProcessing)
 267             {
 268                 bProcessing = extractFrame();   // will stop on end of input data.
 269                 if(bProcessing)
 270                     bProcessing = unpackFrame();
 271                 if(bProcessing)
 272                     bProcessing = outputFrame(); // will stop on data path halt.
 273             }
 274         }
 275     }
 276     catch(const ocsdError &err) {
 277         LogError(err);
 278         CollateDataPathResp(OCSD_RESP_FATAL_INVALID_DATA);
 279     }
 280     catch(...) {
 281         LogError(ocsdError(OCSD_ERR_SEV_ERROR, OCSD_ERR_FAIL));
 282         CollateDataPathResp(OCSD_RESP_FATAL_SYS_ERR);
 283     }
 284
 285     if(!m_first_data)
 286         m_first_data = true;
 287
 288     // update the outputs.
 289     *numBytesProcessed = m_in_block_processed;
 290
 291     return highestDataPathResp();
 292 }
 293
 294 ocsd_err_t  TraceFmtDcdImpl::DecodeConfigure(uint32_t flags)
 295 {
 296     const char *pszErrMsg = "";
 297     ocsd_err_t err = OCSD_OK;
 298
 299     if((flags & ~OCSD_DFRMTR_VALID_MASK) != 0)
 300     {
 301         err = OCSD_ERR_INVALID_PARAM_VAL;
 302         pszErrMsg = "Unknown Config Flags";
 303     }
 304
 305     if((flags & OCSD_DFRMTR_VALID_MASK) == 0)
 306     {
 307         err = OCSD_ERR_INVALID_PARAM_VAL;
 308         pszErrMsg = "No Config Flags Set";
 309     }
 310
 311     if((flags & (OCSD_DFRMTR_HAS_FSYNCS | OCSD_DFRMTR_HAS_HSYNCS)) &&
 312        (flags & OCSD_DFRMTR_FRAME_MEM_ALIGN)
 313        )
 314     {
 315         err = OCSD_ERR_INVALID_PARAM_VAL;
 316         pszErrMsg = "Invalid Config Flag Combination Set";
 317     }
 318
 319     if(err != OCSD_OK)
 320     {
 321         ocsdError errObj(OCSD_ERR_SEV_ERROR,OCSD_ERR_INVALID_PARAM_VAL);
 322         errObj.setMessage(pszErrMsg);
 323         LogError(errObj);
 324     }
 325     else
 326     {
 327         m_cfgFlags = flags;
 328         m_alignment = 16;
 329         if(flags & OCSD_DFRMTR_HAS_FSYNCS)
 330             m_alignment = 4;
 331         else if(flags & OCSD_DFRMTR_HAS_HSYNCS)
 332             m_alignment = 2;
 333     }
 334     return err;
 335 }
 336
 337 void TraceFmtDcdImpl::resetStateParams()
 338 {
 339     // overall dynamic state - intra frame
 340     m_trc_curr_idx = OCSD_BAD_TRC_INDEX;    /* source index of current trace data */
 341     m_frame_synced = false;
 342     m_first_data = false;
 343     m_curr_src_ID = OCSD_BAD_CS_SRC_ID;
 344
 345     // current frame processing
 346     m_ex_frm_n_bytes = 0;
 347     m_trc_curr_idx_sof = OCSD_BAD_TRC_INDEX;
 348 }
 349
 350 bool TraceFmtDcdImpl::checkForSync()
 351 {
 352     // we can sync on:-
 353     // 16 byte alignment - standard input buffers such as ETB
 354     // FSYNC packets in the stream
 355     // forced index programmed into the object.
 356     uint32_t unsynced_bytes = 0;
 357
 358     if(!m_frame_synced)
 359     {
 360         if(m_use_force_sync)
 361         {
 362             // is the force sync point in this block?
 363             if((m_force_sync_idx >= m_trc_curr_idx) && (m_force_sync_idx < (m_trc_curr_idx + m_in_block_size)))
 364             {
 365                 unsynced_bytes = m_force_sync_idx - m_trc_curr_idx;
 366                 m_frame_synced = true;
 367             }
 368             else
 369             {
 370                 unsynced_bytes = m_in_block_size;
 371             }
 372         }
 373         else if( m_cfgFlags & OCSD_DFRMTR_HAS_FSYNCS)   // memory aligned data
 374         {
 375              unsynced_bytes = findfirstFSync();
 376
 377         }
 378         else
 379         {
 380             // OCSD_DFRMTR_FRAME_MEM_ALIGN - this has guaranteed 16 byte frame size and alignment.
 381             m_frame_synced = true;
 382         }
 383
 384         if(unsynced_bytes)
 385         {
 386             outputUnsyncedBytes(unsynced_bytes);
 387             m_in_block_processed = unsynced_bytes;
 388             m_trc_curr_idx += unsynced_bytes;
 389         }
 390     }
 391     return m_frame_synced;
 392 }
 393
 394 uint32_t TraceFmtDcdImpl::findfirstFSync()
 395 {
 396     uint32_t unsynced = m_in_block_size;  // consider entire block as unsynced at present.
 397     //**TBD - handle fsync patterns in TPIU captured code
 398     return unsynced;
 399 }
 400
 401 void TraceFmtDcdImpl::outputUnsyncedBytes(uint32_t /*num_bytes*/)
 402 {
 403     //**TBD:
 404 }
 405
 406 int TraceFmtDcdImpl::checkForResetFSyncPatterns()
 407 {
 408         const uint32_t FSYNC_PATTERN = 0x7FFFFFFF;    // LE host pattern for FSYNC
 409         bool check_for_fsync = true;
 410         int num_fsyncs = 0;
 411         const uint8_t *dataPtr = m_in_block_base + m_in_block_processed;
 412
 413         while (check_for_fsync && (m_in_block_processed < m_in_block_size))
 414         {
 415                 // look for consecutive fsyncs as padding or for reset downstream - both cases will reset downstream....
 416                 if (*((uint32_t *)(dataPtr)) == FSYNC_PATTERN)
 417                 {
 418                         dataPtr += sizeof(uint32_t);
 419                         num_fsyncs++;
 420                 }
 421                 else
 422                         check_for_fsync = false;
 423         }
 424
 425         if (num_fsyncs)
 426         {
 427         printf("Frame deformatter: Found %d FSYNCS\n",num_fsyncs);
 428                 if ((num_fsyncs % 4) == 0)
 429         {
 430             // reset the upstream decoders
 431                         executeNoneDataOpAllIDs(OCSD_OP_RESET,m_trc_curr_idx);
 432
 433             // reset the intra frame parameters
 434             m_curr_src_ID = OCSD_BAD_CS_SRC_ID;
 435             m_ex_frm_n_bytes = 0;
 436             m_trc_curr_idx_sof = OCSD_BAD_TRC_INDEX;
 437         }
 438                 else
 439                 {
 440                         // TBD: throw processing error, none frame size block of fsyncs
 441                 }
 442         }
 443     return num_fsyncs * 4;
 444 }
 445
 446
 447 bool TraceFmtDcdImpl::extractFrame()
 448 {
 449         const uint32_t FSYNC_PATTERN = 0x7FFFFFFF;    // LE host pattern for FSYNC
 450         const uint16_t HSYNC_PATTERN = 0x7FFF;        // LE host pattern for HSYNC
 451
 452
 453         bool cont_process = true;   // continue processing after extraction.
 454     uint32_t f_sync_bytes = 0; // skipped f sync bytes
 455     uint32_t h_sync_bytes = 0; // skipped h sync bytes
 456     uint32_t ex_bytes = 0;  // extracted bytes
 457
 458     // memory aligned sources are always multiples of frames, aligned to start.
 459     if( m_cfgFlags & OCSD_DFRMTR_FRAME_MEM_ALIGN)
 460     {
 461                 // some linux drivers (e.g. for perf) will insert FSYNCS to pad or differentiate
 462         // between blocks of aligned data, always in frame aligned complete 16 byte frames.
 463         if (m_cfgFlags & OCSD_DFRMTR_RESET_ON_4X_FSYNC)
 464         {
 465             f_sync_bytes = checkForResetFSyncPatterns();
 466
 467             /* in this case the FSYNC pattern is output on both packed and unpacked cases */
 468             if (f_sync_bytes && (m_b_output_packed_raw || m_b_output_unpacked_raw))
 469             {
 470                 outputRawMonBytes(OCSD_OP_DATA,
 471                     m_trc_curr_idx,
 472                     OCSD_FRM_FSYNC,
 473                     f_sync_bytes,
 474                     m_in_block_base + m_in_block_processed,
 475                     0);
 476             }
 477         }
 478
 479         if((m_in_block_processed+f_sync_bytes) == m_in_block_size)
 480         {
 481             m_ex_frm_n_bytes = 0;
 482             cont_process = false;   // end of input data.
 483         }
 484                 else
 485                 {
 486                         // always a complete frame.
 487                         m_ex_frm_n_bytes = OCSD_DFRMTR_FRAME_SIZE;
 488                         memcpy(m_ex_frm_data, m_in_block_base + m_in_block_processed + f_sync_bytes, m_ex_frm_n_bytes);
 489                         m_trc_curr_idx_sof = m_trc_curr_idx + f_sync_bytes;
 490                         ex_bytes = OCSD_DFRMTR_FRAME_SIZE;
 491         }
 492     }
 493     else
 494     {
 495         // extract data accounting for frame syncs and hsyncs if present.
 496         // we know we are aligned at this point - could be FSYNC or HSYNCs here.
 497
 498         // check what we a looking for
 499         bool hasFSyncs =  ((m_cfgFlags & OCSD_DFRMTR_HAS_FSYNCS) == OCSD_DFRMTR_HAS_FSYNCS);
 500         bool hasHSyncs =  ((m_cfgFlags & OCSD_DFRMTR_HAS_HSYNCS) == OCSD_DFRMTR_HAS_HSYNCS);
 501
 502         const uint8_t *dataPtr = m_in_block_base+m_in_block_processed;
 503         const uint8_t *eodPtr = m_in_block_base+m_in_block_size;
 504
 505         cont_process = (bool)(dataPtr < eodPtr);
 506
 507         // can have FSYNCS at start of frame (in middle is an error).
 508         if(hasFSyncs && cont_process && (m_ex_frm_n_bytes == 0))
 509         {
 510             while((*((uint32_t *)(dataPtr)) == FSYNC_PATTERN) && cont_process)
 511             {
 512                 f_sync_bytes += 4;
 513                 dataPtr += 4;
 514                 cont_process = (bool)(dataPtr < eodPtr);
 515
 516                 // TBD:  output raw FSYNC data on raw frame channel.
 517             }
 518         }
 519
 520         // not an FSYNC
 521         while((m_ex_frm_n_bytes < OCSD_DFRMTR_FRAME_SIZE) && cont_process)
 522         {
 523             // check for illegal out of sequence FSYNC
 524             if((m_ex_frm_n_bytes % 4) == 0)
 525             {
 526                 if(*((uint32_t *)(dataPtr)) == FSYNC_PATTERN)
 527                 {
 528                     // throw an illegal FSYNC error
 529                 }
 530             }
 531
 532             // mark start of frame after FSyncs
 533             if(m_ex_frm_n_bytes == 0)
 534                 m_trc_curr_idx_sof = m_trc_curr_idx + f_sync_bytes;
 535
 536             m_ex_frm_data[m_ex_frm_n_bytes] = dataPtr[0];
 537             m_ex_frm_data[m_ex_frm_n_bytes+1] = dataPtr[1];
 538             m_ex_frm_n_bytes+=2;
 539             ex_bytes +=2;
 540
 541             // check pair is not HSYNC
 542             if(*((uint16_t *)(dataPtr)) == HSYNC_PATTERN)
 543             {
 544                 if(hasHSyncs)
 545                 {
 546                     m_ex_frm_n_bytes-=2;
 547                     ex_bytes -= 2;
 548                     h_sync_bytes+=2;
 549
 550                     // TBD:  output raw HSYNC data on raw frame channel.
 551                 }
 552                 else
 553                 {
 554                     // throw illegal HSYNC error.
 555                 }
 556             }
 557
 558             dataPtr += 2;
 559             cont_process = (bool)(dataPtr < eodPtr);
 560         }
 561
 562         // if we hit the end of data but still have a complete frame waiting,
 563         // need to continue processing to allow it to be used.
 564         if(!cont_process && (m_ex_frm_n_bytes == OCSD_DFRMTR_FRAME_SIZE))
 565             cont_process = true;
 566     }
 567
 568     // output raw data on raw frame channel - packed raw.
 569     if ((m_ex_frm_n_bytes == OCSD_DFRMTR_FRAME_SIZE) && m_b_output_packed_raw)
 570     {
 571         outputRawMonBytes(  OCSD_OP_DATA,
 572                             m_trc_curr_idx,
 573                             OCSD_FRM_PACKED,
 574                             ex_bytes + f_sync_bytes + h_sync_bytes,
 575                             m_in_block_base+m_in_block_processed,
 576                             0);
 577     }
 578
 579     // update the processed count for the buffer
 580     m_in_block_processed += m_ex_frm_n_bytes + f_sync_bytes + h_sync_bytes;
 581
 582     // update index past the processed data
 583     m_trc_curr_idx += m_ex_frm_n_bytes + f_sync_bytes + h_sync_bytes;
 584
 585     return cont_process;
 586 }
 587
 588 bool TraceFmtDcdImpl::unpackFrame()
 589 {
 590     // unpack cannot fail as never called on incomplete frame.
 591     uint8_t frameFlagBit = 0x1;
 592     uint8_t newSrcID = OCSD_BAD_CS_SRC_ID;
 593     bool PrevIDandIDChange = false;
 594
 595     // init output processing
 596     m_out_data_idx = 0;
 597     m_out_processed = 0;
 598
 599     // set up first out data packet...
 600     m_out_data[m_out_data_idx].id = m_curr_src_ID;
 601     m_out_data[m_out_data_idx].valid = 0;
 602     m_out_data[m_out_data_idx].index =  m_trc_curr_idx_sof;
 603     m_out_data[m_out_data_idx].used = 0;
 604
 605     // work on byte pairs - bytes 0 - 13.
 606     for(int i = 0; i < 14; i+=2)
 607     {
 608         PrevIDandIDChange = false;
 609
 610         // it's an ID + data
 611         if(m_ex_frm_data[i] & 0x1)
 612         {
 613             newSrcID = (m_ex_frm_data[i] >> 1) & 0x7f;
 614             if(newSrcID != m_curr_src_ID)   // ID change
 615             {
 616                 PrevIDandIDChange = ((frameFlagBit & m_ex_frm_data[15]) != 0);
 617
 618                 // following byte for old id?
 619                 if(PrevIDandIDChange)
 620                     // 2nd byte always data
 621                     m_out_data[m_out_data_idx].data[m_out_data[m_out_data_idx].valid++] = m_ex_frm_data[i+1];
 622
 623                 // change ID
 624                 m_curr_src_ID = newSrcID;
 625
 626                 // if we already have data in this buffer
 627                 if(m_out_data[m_out_data_idx].valid > 0)
 628                 {
 629                     m_out_data_idx++; // move to next buffer
 630                     m_out_data[m_out_data_idx].valid = 0;
 631                     m_out_data[m_out_data_idx].used = 0;
 632                     m_out_data[m_out_data_idx].index = m_trc_curr_idx_sof + i;
 633                 }
 634
 635                 // set new ID on buffer
 636                 m_out_data[m_out_data_idx].id = m_curr_src_ID;
 637
 638                 /// TBD - ID indexing in here.
 639             }
 640         }
 641         else
 642         // it's just data
 643         {
 644             m_out_data[m_out_data_idx].data[m_out_data[m_out_data_idx].valid++] = m_ex_frm_data[i] | ((frameFlagBit & m_ex_frm_data[15]) ? 0x1 : 0x0);
 645         }
 646
 647         // 2nd byte always data
 648         if(!PrevIDandIDChange) // output only if we didn't for an ID change + prev ID.
 649             m_out_data[m_out_data_idx].data[m_out_data[m_out_data_idx].valid++] = m_ex_frm_data[i+1];
 650
 651         frameFlagBit <<= 1;
 652     }
 653
 654     // unpack byte 14;
 655
 656     // it's an ID
 657     if(m_ex_frm_data[14] & 0x1)
 658     {
 659         // no matter if change or not, no associated data in byte 15 anyway so just set.
 660         m_curr_src_ID = (m_ex_frm_data[14] >> 1) & 0x7f;
 661     }
 662     // it's data
 663     else
 664     {
 665         m_out_data[m_out_data_idx].data[m_out_data[m_out_data_idx].valid++] = m_ex_frm_data[14] | ((frameFlagBit & m_ex_frm_data[15]) ? 0x1 : 0x0);
 666     }
 667     m_ex_frm_n_bytes = 0;   // mark frame as empty;
 668     return true;
 669 }
 670
 671 // output data to channels.
 672 bool TraceFmtDcdImpl::outputFrame()
 673 {
 674     bool cont_processing = true;
 675     ITrcDataIn *pDataIn = 0;
 676     uint32_t bytes_used;
 677
 678     // output each valid ID within the frame - stopping if we get a wait or error
 679     while((m_out_processed < (m_out_data_idx + 1)) && cont_processing)
 680     {
 681
 682         // may have data prior to a valid ID appearing
 683         if(m_out_data[m_out_processed].id != OCSD_BAD_CS_SRC_ID)
 684         {
 685             if((pDataIn = m_IDStreams[m_out_data[m_out_processed].id].first()) != 0)
 686             {
 687                 // log the stuff we are about to put out early so as to make it visible before interpretation
 688                 // however, don't re-output if only part used first time round.
 689                 if(m_b_output_unpacked_raw && (m_out_data[m_out_processed].used == 0) && rawChanEnabled( m_out_data[m_out_processed].id))
 690                 {
 691                     outputRawMonBytes( OCSD_OP_DATA,
 692                             m_out_data[m_out_processed].index,
 693                             OCSD_FRM_ID_DATA,
 694                             m_out_data[m_out_processed].valid,
 695                             m_out_data[m_out_processed].data,
 696                             m_out_data[m_out_processed].id);
 697                 }
 698
 699                 // output to the connected packet process
 700                 CollateDataPathResp(pDataIn->TraceDataIn(OCSD_OP_DATA,
 701                     m_out_data[m_out_processed].index + m_out_data[m_out_processed].used,
 702                     m_out_data[m_out_processed].valid - m_out_data[m_out_processed].used,
 703                     m_out_data[m_out_processed].data + m_out_data[m_out_processed].used,
 704                     &bytes_used));
 705
 706                 if(!dataPathCont())
 707                 {
 708                     cont_processing = false;
 709                     m_out_data[m_out_processed].used += bytes_used;
 710                     if(m_out_data[m_out_processed].used == m_out_data[m_out_processed].valid)
 711                         m_out_processed++; // we have used up all this data.
 712                 }
 713                 else
 714                 {
 715                     m_out_processed++; // we have sent this data;
 716                 }
 717             }
 718             else
 719             {
 720                 // optional raw output for debugging / monitor tools
 721                 if(m_b_output_unpacked_raw && rawChanEnabled( m_out_data[m_out_processed].id))
 722                 {
 723                     outputRawMonBytes(  OCSD_OP_DATA,
 724                         m_out_data[m_out_processed].index,
 725                         OCSD_FRM_ID_DATA,
 726                         m_out_data[m_out_processed].valid,
 727                         m_out_data[m_out_processed].data,
 728                         m_out_data[m_out_processed].id);
 729                 }
 730                 m_out_processed++; // skip past this data.
 731             }
 732         }
 733         else
 734         {
 735             // optional raw output for debugging / monitor tools of unknown src ID data
 736             if(m_b_output_unpacked_raw)
 737             {
 738                 outputRawMonBytes(  OCSD_OP_DATA,
 739                     m_out_data[m_out_processed].index,
 740                     OCSD_FRM_ID_DATA,
 741                     m_out_data[m_out_processed].valid,
 742                     m_out_data[m_out_processed].data,
 743                     m_out_data[m_out_processed].id);
 744             }
 745             m_out_processed++; // skip past this data.
 746         }
 747     }
 748     return cont_processing;
 749 }
 750
 751 /***************************************************************/
 752 /* interface */
 753 /***************************************************************/
 754 TraceFormatterFrameDecoder::TraceFormatterFrameDecoder() : m_pDecoder(0)
 755 {
 756     m_instNum = -1;
 757 }
 758
 759 TraceFormatterFrameDecoder::TraceFormatterFrameDecoder(int instNum) : m_pDecoder(0)
 760 {
 761     m_instNum = instNum;
 762 }
 763
 764 TraceFormatterFrameDecoder::~TraceFormatterFrameDecoder()
 765 {
 766     if(m_pDecoder)
 767     {
 768         delete m_pDecoder;
 769         m_pDecoder = 0;
 770     }
 771 }
 772
 773     /* the data input interface from the reader / source */
 774 ocsd_datapath_resp_t TraceFormatterFrameDecoder::TraceDataIn(  const ocsd_datapath_op_t op,
 775                                                                 const ocsd_trc_index_t index,
 776                                                                 const uint32_t dataBlockSize,
 777                                                                 const uint8_t *pDataBlock,
 778                                                                 uint32_t *numBytesProcessed)
 779 {
 780     return (m_pDecoder == 0) ? OCSD_RESP_FATAL_NOT_INIT : m_pDecoder->TraceDataIn(op,index,dataBlockSize,pDataBlock,numBytesProcessed);
 781 }
 782
 783 /* attach a data processor to a stream ID output */
 784 componentAttachPt<ITrcDataIn> *TraceFormatterFrameDecoder::getIDStreamAttachPt(uint8_t ID)
 785 {
 786     componentAttachPt<ITrcDataIn> *pAttachPt = 0;
 787     if((ID < 128) && (m_pDecoder != 0))
 788         pAttachPt = &(m_pDecoder->m_IDStreams[ID]);
 789     return pAttachPt;
 790 }
 791
 792 /* attach a data processor to the raw frame output */
 793 componentAttachPt<ITrcRawFrameIn> *TraceFormatterFrameDecoder::getTrcRawFrameAttachPt()
 794 {
 795     return (m_pDecoder != 0) ? &m_pDecoder->m_RawTraceFrame : 0;
 796 }
 797
 798
 799 componentAttachPt<ITrcSrcIndexCreator> *TraceFormatterFrameDecoder::getTrcSrcIndexAttachPt()
 800 {
 801     return (m_pDecoder != 0) ? &m_pDecoder->m_SrcIndexer : 0;
 802 }
 803
 804 componentAttachPt<ITraceErrorLog> *TraceFormatterFrameDecoder::getErrLogAttachPt()
 805 {
 806     return (m_pDecoder != 0) ? m_pDecoder->getErrorLogAttachPt() : 0;
 807 }
 808
 809 /* configuration - set operational mode for incoming stream (has FSYNCS etc) */
 810 ocsd_err_t TraceFormatterFrameDecoder::Configure(uint32_t cfg_flags)
 811 {
 812     if(!m_pDecoder)
 813     {
 814         if(m_instNum >= 0)
 815             m_pDecoder = new (std::nothrow) TraceFmtDcdImpl(m_instNum);
 816         else
 817             m_pDecoder = new (std::nothrow) TraceFmtDcdImpl();
 818         if(!m_pDecoder) return OCSD_ERR_MEM;
 819     }
 820     m_pDecoder->m_cfgFlags = cfg_flags;
 821     return OCSD_OK;
 822 }
 823
 824 const uint32_t TraceFormatterFrameDecoder::getConfigFlags() const
 825 {
 826     uint32_t flags = 0;
 827     if(m_pDecoder)
 828         flags = m_pDecoder->m_cfgFlags;
 829     return flags;
 830 }
 831
 832
 833 /* enable / disable ID streams - default as all enabled */
 834 ocsd_err_t TraceFormatterFrameDecoder::OutputFilterIDs(std::vector<uint8_t> &id_list, bool bEnable)
 835 {
 836     return (m_pDecoder == 0) ? OCSD_ERR_NOT_INIT : m_pDecoder->OutputFilterIDs(id_list,bEnable);
 837 }
 838
 839 ocsd_err_t TraceFormatterFrameDecoder::OutputFilterAllIDs(bool bEnable)
 840 {
 841     return (m_pDecoder == 0) ? OCSD_ERR_NOT_INIT : m_pDecoder->OutputFilterAllIDs(bEnable);
 842 }
 843
 844 /* decode control */
 845 ocsd_datapath_resp_t TraceFormatterFrameDecoder::Reset()
 846 {
 847     return (m_pDecoder == 0) ? OCSD_RESP_FATAL_NOT_INIT : m_pDecoder->Reset();
 848 }
 849
 850 ocsd_datapath_resp_t TraceFormatterFrameDecoder::Flush()
 851 {
 852     return (m_pDecoder == 0) ? OCSD_RESP_FATAL_NOT_INIT : m_pDecoder->Flush();
 853 }
 854
 855
 856 /* End of File trc_frame_deformatter.cpp */