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 processed = 0;
 397     const uint32_t FSYNC_PATTERN = 0x7FFFFFFF;    // LE host pattern for FSYNC
 398     const uint8_t *dataPtr = m_in_block_base;
 399
 400     while (processed < (m_in_block_size - 3))
 401     {
 402         if (*((uint32_t *)(dataPtr)) == FSYNC_PATTERN)
 403         {
 404             m_frame_synced = true;
 405             break;
 406         }
 407         processed++;
 408         dataPtr++;
 409     }
 410     return processed;
 411 }
 412
 413 void TraceFmtDcdImpl::outputUnsyncedBytes(uint32_t /*num_bytes*/)
 414 {
 415     //**TBD:
 416 }
 417
 418 int TraceFmtDcdImpl::checkForResetFSyncPatterns()
 419 {
 420         const uint32_t FSYNC_PATTERN = 0x7FFFFFFF;    // LE host pattern for FSYNC
 421         bool check_for_fsync = true;
 422         int num_fsyncs = 0;
 423         const uint8_t *dataPtr = m_in_block_base + m_in_block_processed;
 424
 425         while (check_for_fsync && (m_in_block_processed < m_in_block_size))
 426         {
 427                 // look for consecutive fsyncs as padding or for reset downstream - both cases will reset downstream....
 428                 if (*((uint32_t *)(dataPtr)) == FSYNC_PATTERN)
 429                 {
 430                         dataPtr += sizeof(uint32_t);
 431                         num_fsyncs++;
 432                 }
 433                 else
 434                         check_for_fsync = false;
 435         }
 436
 437         if (num_fsyncs)
 438         {
 439         printf("Frame deformatter: Found %d FSYNCS\n",num_fsyncs);
 440                 if ((num_fsyncs % 4) == 0)
 441         {
 442             // reset the upstream decoders
 443                         executeNoneDataOpAllIDs(OCSD_OP_RESET,m_trc_curr_idx);
 444
 445             // reset the intra frame parameters
 446             m_curr_src_ID = OCSD_BAD_CS_SRC_ID;
 447             m_ex_frm_n_bytes = 0;
 448             m_trc_curr_idx_sof = OCSD_BAD_TRC_INDEX;
 449         }
 450                 else
 451                 {
 452                         // TBD: throw processing error, none frame size block of fsyncs
 453                 }
 454         }
 455     return num_fsyncs * 4;
 456 }
 457
 458
 459 bool TraceFmtDcdImpl::extractFrame()
 460 {
 461         const uint32_t FSYNC_PATTERN = 0x7FFFFFFF;    // LE host pattern for FSYNC
 462         const uint16_t HSYNC_PATTERN = 0x7FFF;        // LE host pattern for HSYNC
 463
 464
 465         bool cont_process = true;   // continue processing after extraction.
 466     uint32_t f_sync_bytes = 0; // skipped f sync bytes
 467     uint32_t h_sync_bytes = 0; // skipped h sync bytes
 468     uint32_t ex_bytes = 0;  // extracted this pass (may be filling out part frame)
 469
 470     // memory aligned sources are always multiples of frames, aligned to start.
 471     if( m_cfgFlags & OCSD_DFRMTR_FRAME_MEM_ALIGN)
 472     {
 473                 // some linux drivers (e.g. for perf) will insert FSYNCS to pad or differentiate
 474         // between blocks of aligned data, always in frame aligned complete 16 byte frames.
 475         if (m_cfgFlags & OCSD_DFRMTR_RESET_ON_4X_FSYNC)
 476         {
 477             f_sync_bytes = checkForResetFSyncPatterns();
 478
 479             /* in this case the FSYNC pattern is output on both packed and unpacked cases */
 480             if (f_sync_bytes && (m_b_output_packed_raw || m_b_output_unpacked_raw))
 481             {
 482                 outputRawMonBytes(OCSD_OP_DATA,
 483                     m_trc_curr_idx,
 484                     OCSD_FRM_FSYNC,
 485                     f_sync_bytes,
 486                     m_in_block_base + m_in_block_processed,
 487                     0);
 488             }
 489         }
 490
 491         if((m_in_block_processed+f_sync_bytes) == m_in_block_size)
 492         {
 493             m_ex_frm_n_bytes = 0;
 494             cont_process = false;   // end of input data.
 495         }
 496                 else
 497                 {
 498                         // always a complete frame.
 499                         m_ex_frm_n_bytes = OCSD_DFRMTR_FRAME_SIZE;
 500                         memcpy(m_ex_frm_data, m_in_block_base + m_in_block_processed + f_sync_bytes, m_ex_frm_n_bytes);
 501                         m_trc_curr_idx_sof = m_trc_curr_idx + f_sync_bytes;
 502                         ex_bytes = OCSD_DFRMTR_FRAME_SIZE;
 503         }
 504     }
 505     else
 506     {
 507         // extract data accounting for frame syncs and hsyncs if present.
 508         // we know we are aligned at this point - could be FSYNC or HSYNCs here.
 509
 510         // check what we a looking for
 511         bool hasFSyncs =  ((m_cfgFlags & OCSD_DFRMTR_HAS_FSYNCS) == OCSD_DFRMTR_HAS_FSYNCS);
 512         bool hasHSyncs =  ((m_cfgFlags & OCSD_DFRMTR_HAS_HSYNCS) == OCSD_DFRMTR_HAS_HSYNCS);
 513
 514         const uint8_t *dataPtr = m_in_block_base+m_in_block_processed;
 515         const uint8_t *eodPtr = m_in_block_base+m_in_block_size;
 516
 517         cont_process = (bool)(dataPtr < eodPtr);
 518
 519         // can have FSYNCS at start of frame (in middle is an error).
 520         if(hasFSyncs && cont_process && (m_ex_frm_n_bytes == 0))
 521         {
 522             while((*((uint32_t *)(dataPtr)) == FSYNC_PATTERN) && cont_process)
 523             {
 524                 f_sync_bytes += 4;
 525                 dataPtr += 4;
 526                 cont_process = (bool)(dataPtr < eodPtr);
 527             }
 528         }
 529
 530         // not an FSYNC
 531         while((m_ex_frm_n_bytes < OCSD_DFRMTR_FRAME_SIZE) && cont_process)
 532         {
 533             // check for illegal out of sequence FSYNC
 534             if((m_ex_frm_n_bytes % 4) == 0)
 535             {
 536                 if(*((uint32_t *)(dataPtr)) == FSYNC_PATTERN)
 537                 {
 538                     // throw an illegal FSYNC error
 539                     throw ocsdError(OCSD_ERR_SEV_ERROR, OCSD_ERR_DFMTR_BAD_FHSYNC, m_trc_curr_idx, "Bad FSYNC in frame.");
 540                 }
 541             }
 542
 543             // mark start of frame after FSyncs
 544             if(m_ex_frm_n_bytes == 0)
 545                 m_trc_curr_idx_sof = m_trc_curr_idx + f_sync_bytes;
 546
 547             m_ex_frm_data[m_ex_frm_n_bytes] = dataPtr[0];
 548             m_ex_frm_data[m_ex_frm_n_bytes+1] = dataPtr[1];
 549             m_ex_frm_n_bytes+=2;
 550             ex_bytes +=2;
 551
 552             // check pair is not HSYNC
 553             if(*((uint16_t *)(dataPtr)) == HSYNC_PATTERN)
 554             {
 555                 if(hasHSyncs)
 556                 {
 557                     m_ex_frm_n_bytes-=2;
 558                     ex_bytes -= 2;
 559                     h_sync_bytes+=2;
 560                 }
 561                 else
 562                 {
 563                     // throw illegal HSYNC error.
 564                     throw ocsdError(OCSD_ERR_SEV_ERROR, OCSD_ERR_DFMTR_BAD_FHSYNC, m_trc_curr_idx, "Bad HSYNC in frame.");
 565                 }
 566             }
 567
 568             dataPtr += 2;
 569             cont_process = (bool)(dataPtr < eodPtr);
 570         }
 571
 572         // if we hit the end of data but still have a complete frame waiting,
 573         // need to continue processing to allow it to be used.
 574         if(!cont_process && (m_ex_frm_n_bytes == OCSD_DFRMTR_FRAME_SIZE))
 575             cont_process = true;
 576     }
 577
 578     // total bytes processed this pass
 579     uint32_t total_processed = ex_bytes + f_sync_bytes + h_sync_bytes;
 580
 581     // output raw data on raw frame channel - packed raw.
 582     if (((m_ex_frm_n_bytes == OCSD_DFRMTR_FRAME_SIZE) || !cont_process) && m_b_output_packed_raw)
 583     {
 584         outputRawMonBytes(  OCSD_OP_DATA,
 585                             m_trc_curr_idx,
 586                             OCSD_FRM_PACKED,
 587                             total_processed,
 588                             m_in_block_base+m_in_block_processed,
 589                             0);
 590     }
 591
 592     // update the processed count for the buffer
 593     m_in_block_processed += total_processed;
 594
 595     // update index past the processed data
 596     m_trc_curr_idx += total_processed;
 597
 598     return cont_process;
 599 }
 600
 601 bool TraceFmtDcdImpl::unpackFrame()
 602 {
 603     // unpack cannot fail as never called on incomplete frame.
 604     uint8_t frameFlagBit = 0x1;
 605     uint8_t newSrcID = OCSD_BAD_CS_SRC_ID;
 606     bool PrevIDandIDChange = false;
 607
 608     // init output processing
 609     m_out_data_idx = 0;
 610     m_out_processed = 0;
 611
 612     // set up first out data packet...
 613     m_out_data[m_out_data_idx].id = m_curr_src_ID;
 614     m_out_data[m_out_data_idx].valid = 0;
 615     m_out_data[m_out_data_idx].index =  m_trc_curr_idx_sof;
 616     m_out_data[m_out_data_idx].used = 0;
 617
 618     // work on byte pairs - bytes 0 - 13.
 619     for(int i = 0; i < 14; i+=2)
 620     {
 621         PrevIDandIDChange = false;
 622
 623         // it's an ID + data
 624         if(m_ex_frm_data[i] & 0x1)
 625         {
 626             newSrcID = (m_ex_frm_data[i] >> 1) & 0x7f;
 627             if(newSrcID != m_curr_src_ID)   // ID change
 628             {
 629                 PrevIDandIDChange = ((frameFlagBit & m_ex_frm_data[15]) != 0);
 630
 631                 // following byte for old id?
 632                 if(PrevIDandIDChange)
 633                     // 2nd byte always data
 634                     m_out_data[m_out_data_idx].data[m_out_data[m_out_data_idx].valid++] = m_ex_frm_data[i+1];
 635
 636                 // change ID
 637                 m_curr_src_ID = newSrcID;
 638
 639                 // if we already have data in this buffer
 640                 if(m_out_data[m_out_data_idx].valid > 0)
 641                 {
 642                     m_out_data_idx++; // move to next buffer
 643                     m_out_data[m_out_data_idx].valid = 0;
 644                     m_out_data[m_out_data_idx].used = 0;
 645                     m_out_data[m_out_data_idx].index = m_trc_curr_idx_sof + i;
 646                 }
 647
 648                 // set new ID on buffer
 649                 m_out_data[m_out_data_idx].id = m_curr_src_ID;
 650
 651                 /// TBD - ID indexing in here.
 652             }
 653         }
 654         else
 655         // it's just data
 656         {
 657             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);
 658         }
 659
 660         // 2nd byte always data
 661         if(!PrevIDandIDChange) // output only if we didn't for an ID change + prev ID.
 662             m_out_data[m_out_data_idx].data[m_out_data[m_out_data_idx].valid++] = m_ex_frm_data[i+1];
 663
 664         frameFlagBit <<= 1;
 665     }
 666
 667     // unpack byte 14;
 668
 669     // it's an ID
 670     if(m_ex_frm_data[14] & 0x1)
 671     {
 672         // no matter if change or not, no associated data in byte 15 anyway so just set.
 673         m_curr_src_ID = (m_ex_frm_data[14] >> 1) & 0x7f;
 674     }
 675     // it's data
 676     else
 677     {
 678         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);
 679     }
 680     m_ex_frm_n_bytes = 0;   // mark frame as empty;
 681     return true;
 682 }
 683
 684 // output data to channels.
 685 bool TraceFmtDcdImpl::outputFrame()
 686 {
 687     bool cont_processing = true;
 688     ITrcDataIn *pDataIn = 0;
 689     uint32_t bytes_used;
 690
 691     // output each valid ID within the frame - stopping if we get a wait or error
 692     while((m_out_processed < (m_out_data_idx + 1)) && cont_processing)
 693     {
 694
 695         // may have data prior to a valid ID appearing
 696         if(m_out_data[m_out_processed].id != OCSD_BAD_CS_SRC_ID)
 697         {
 698             if((pDataIn = m_IDStreams[m_out_data[m_out_processed].id].first()) != 0)
 699             {
 700                 // log the stuff we are about to put out early so as to make it visible before interpretation
 701                 // however, don't re-output if only part used first time round.
 702                 if(m_b_output_unpacked_raw && (m_out_data[m_out_processed].used == 0) && rawChanEnabled( m_out_data[m_out_processed].id))
 703                 {
 704                     outputRawMonBytes( OCSD_OP_DATA,
 705                             m_out_data[m_out_processed].index,
 706                             OCSD_FRM_ID_DATA,
 707                             m_out_data[m_out_processed].valid,
 708                             m_out_data[m_out_processed].data,
 709                             m_out_data[m_out_processed].id);
 710                 }
 711
 712                 // output to the connected packet process
 713                 CollateDataPathResp(pDataIn->TraceDataIn(OCSD_OP_DATA,
 714                     m_out_data[m_out_processed].index + m_out_data[m_out_processed].used,
 715                     m_out_data[m_out_processed].valid - m_out_data[m_out_processed].used,
 716                     m_out_data[m_out_processed].data + m_out_data[m_out_processed].used,
 717                     &bytes_used));
 718
 719                 if(!dataPathCont())
 720                 {
 721                     cont_processing = false;
 722                     m_out_data[m_out_processed].used += bytes_used;
 723                     if(m_out_data[m_out_processed].used == m_out_data[m_out_processed].valid)
 724                         m_out_processed++; // we have used up all this data.
 725                 }
 726                 else
 727                 {
 728                     m_out_processed++; // we have sent this data;
 729                 }
 730             }
 731             else
 732             {
 733                 // optional raw output for debugging / monitor tools
 734                 if(m_b_output_unpacked_raw && rawChanEnabled( m_out_data[m_out_processed].id))
 735                 {
 736                     outputRawMonBytes(  OCSD_OP_DATA,
 737                         m_out_data[m_out_processed].index,
 738                         OCSD_FRM_ID_DATA,
 739                         m_out_data[m_out_processed].valid,
 740                         m_out_data[m_out_processed].data,
 741                         m_out_data[m_out_processed].id);
 742                 }
 743                 m_out_processed++; // skip past this data.
 744             }
 745         }
 746         else
 747         {
 748             // optional raw output for debugging / monitor tools of unknown src ID data
 749             if(m_b_output_unpacked_raw)
 750             {
 751                 outputRawMonBytes(  OCSD_OP_DATA,
 752                     m_out_data[m_out_processed].index,
 753                     OCSD_FRM_ID_DATA,
 754                     m_out_data[m_out_processed].valid,
 755                     m_out_data[m_out_processed].data,
 756                     m_out_data[m_out_processed].id);
 757             }
 758             m_out_processed++; // skip past this data.
 759         }
 760     }
 761     return cont_processing;
 762 }
 763
 764 /***************************************************************/
 765 /* interface */
 766 /***************************************************************/
 767 TraceFormatterFrameDecoder::TraceFormatterFrameDecoder() : m_pDecoder(0)
 768 {
 769     m_instNum = -1;
 770 }
 771
 772 TraceFormatterFrameDecoder::TraceFormatterFrameDecoder(int instNum) : m_pDecoder(0)
 773 {
 774     m_instNum = instNum;
 775 }
 776
 777 TraceFormatterFrameDecoder::~TraceFormatterFrameDecoder()
 778 {
 779     if(m_pDecoder)
 780     {
 781         delete m_pDecoder;
 782         m_pDecoder = 0;
 783     }
 784 }
 785
 786     /* the data input interface from the reader / source */
 787 ocsd_datapath_resp_t TraceFormatterFrameDecoder::TraceDataIn(  const ocsd_datapath_op_t op,
 788                                                                 const ocsd_trc_index_t index,
 789                                                                 const uint32_t dataBlockSize,
 790                                                                 const uint8_t *pDataBlock,
 791                                                                 uint32_t *numBytesProcessed)
 792 {
 793     return (m_pDecoder == 0) ? OCSD_RESP_FATAL_NOT_INIT : m_pDecoder->TraceDataIn(op,index,dataBlockSize,pDataBlock,numBytesProcessed);
 794 }
 795
 796 /* attach a data processor to a stream ID output */
 797 componentAttachPt<ITrcDataIn> *TraceFormatterFrameDecoder::getIDStreamAttachPt(uint8_t ID)
 798 {
 799     componentAttachPt<ITrcDataIn> *pAttachPt = 0;
 800     if((ID < 128) && (m_pDecoder != 0))
 801         pAttachPt = &(m_pDecoder->m_IDStreams[ID]);
 802     return pAttachPt;
 803 }
 804
 805 /* attach a data processor to the raw frame output */
 806 componentAttachPt<ITrcRawFrameIn> *TraceFormatterFrameDecoder::getTrcRawFrameAttachPt()
 807 {
 808     return (m_pDecoder != 0) ? &m_pDecoder->m_RawTraceFrame : 0;
 809 }
 810
 811
 812 componentAttachPt<ITrcSrcIndexCreator> *TraceFormatterFrameDecoder::getTrcSrcIndexAttachPt()
 813 {
 814     return (m_pDecoder != 0) ? &m_pDecoder->m_SrcIndexer : 0;
 815 }
 816
 817 componentAttachPt<ITraceErrorLog> *TraceFormatterFrameDecoder::getErrLogAttachPt()
 818 {
 819     return (m_pDecoder != 0) ? m_pDecoder->getErrorLogAttachPt() : 0;
 820 }
 821
 822 /* configuration - set operational mode for incoming stream (has FSYNCS etc) */
 823 ocsd_err_t TraceFormatterFrameDecoder::Configure(uint32_t cfg_flags)
 824 {
 825     if(!m_pDecoder)
 826     {
 827         if(m_instNum >= 0)
 828             m_pDecoder = new (std::nothrow) TraceFmtDcdImpl(m_instNum);
 829         else
 830             m_pDecoder = new (std::nothrow) TraceFmtDcdImpl();
 831         if(!m_pDecoder) return OCSD_ERR_MEM;
 832     }
 833     m_pDecoder->DecodeConfigure(cfg_flags);
 834     return OCSD_OK;
 835 }
 836
 837 const uint32_t TraceFormatterFrameDecoder::getConfigFlags() const
 838 {
 839     uint32_t flags = 0;
 840     if(m_pDecoder)
 841         flags = m_pDecoder->m_cfgFlags;
 842     return flags;
 843 }
 844
 845
 846 /* enable / disable ID streams - default as all enabled */
 847 ocsd_err_t TraceFormatterFrameDecoder::OutputFilterIDs(std::vector<uint8_t> &id_list, bool bEnable)
 848 {
 849     return (m_pDecoder == 0) ? OCSD_ERR_NOT_INIT : m_pDecoder->OutputFilterIDs(id_list,bEnable);
 850 }
 851
 852 ocsd_err_t TraceFormatterFrameDecoder::OutputFilterAllIDs(bool bEnable)
 853 {
 854     return (m_pDecoder == 0) ? OCSD_ERR_NOT_INIT : m_pDecoder->OutputFilterAllIDs(bEnable);
 855 }
 856
 857 /* decode control */
 858 ocsd_datapath_resp_t TraceFormatterFrameDecoder::Reset()
 859 {
 860     return (m_pDecoder == 0) ? OCSD_RESP_FATAL_NOT_INIT : m_pDecoder->Reset();
 861 }
 862
 863 ocsd_datapath_resp_t TraceFormatterFrameDecoder::Flush()
 864 {
 865     return (m_pDecoder == 0) ? OCSD_RESP_FATAL_NOT_INIT : m_pDecoder->Flush();
 866 }
 867
 868
 869 /* End of File trc_frame_deformatter.cpp */