]> CyberLeo.Net >> Repos - FreeBSD/FreeBSD.git/blob - contrib/opencsd/decoder/source/etmv4/trc_pkt_decode_etmv4i.cpp
Re-add opencsd as a vendor import from the dist directory
[FreeBSD/FreeBSD.git] / contrib / opencsd / decoder / source / etmv4 / trc_pkt_decode_etmv4i.cpp
1 /*
2  * \file       trc_pkt_decode_etmv4i.cpp
3  * \brief      OpenCSD : ETMv4 decoder
4  * 
5  * \copyright  Copyright (c) 2015, ARM Limited. All Rights Reserved.
6  */
7
8
9 /* 
10  * Redistribution and use in source and binary forms, with or without modification, 
11  * are permitted provided that the following conditions are met:
12  * 
13  * 1. Redistributions of source code must retain the above copyright notice, 
14  * this list of conditions and the following disclaimer.
15  * 
16  * 2. Redistributions in binary form must reproduce the above copyright notice, 
17  * this list of conditions and the following disclaimer in the documentation 
18  * and/or other materials provided with the distribution. 
19  * 
20  * 3. Neither the name of the copyright holder nor the names of its contributors 
21  * may be used to endorse or promote products derived from this software without 
22  * specific prior written permission. 
23  * 
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND 
25  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
26  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 
27  * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 
28  * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 
29  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 
31  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
32  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
33  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 
34  */ 
35
36 #include "opencsd/etmv4/trc_pkt_decode_etmv4i.h"
37
38 #include "common/trc_gen_elem.h"
39
40
41 #define DCD_NAME "DCD_ETMV4"
42
43 static const uint32_t ETMV4_SUPPORTED_DECODE_OP_FLAGS = OCSD_OPFLG_PKTDEC_COMMON;
44
45 TrcPktDecodeEtmV4I::TrcPktDecodeEtmV4I()
46     : TrcPktDecodeBase(DCD_NAME)
47 {
48     initDecoder();
49 }
50
51 TrcPktDecodeEtmV4I::TrcPktDecodeEtmV4I(int instIDNum)
52     : TrcPktDecodeBase(DCD_NAME,instIDNum)
53 {
54     initDecoder();
55 }
56
57 TrcPktDecodeEtmV4I::~TrcPktDecodeEtmV4I()   
58 {
59 }
60
61 /*********************** implementation packet decoding interface */
62
63 ocsd_datapath_resp_t TrcPktDecodeEtmV4I::processPacket()
64 {
65     ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
66     bool bPktDone = false;
67
68     while(!bPktDone)
69     {
70         switch (m_curr_state)
71         {
72         case NO_SYNC:
73             // output the initial not synced packet to the sink
74             m_output_elem.setType(OCSD_GEN_TRC_ELEM_NO_SYNC);
75             resp = outputTraceElement(m_output_elem);
76             m_curr_state = WAIT_SYNC;
77             // fall through to check if the current packet is the async we are waiting for.
78             break;
79
80         case WAIT_SYNC:
81             if(m_curr_packet_in->getType() == ETM4_PKT_I_ASYNC)
82                 m_curr_state = WAIT_TINFO;
83             bPktDone = true;
84             break;
85
86         case WAIT_TINFO:
87             m_need_ctxt = true;
88             m_need_addr = true;
89             if(m_curr_packet_in->getType() == ETM4_PKT_I_TRACE_INFO)
90             {
91                 doTraceInfoPacket();
92                 m_curr_state = DECODE_PKTS;
93                 m_return_stack.flush();
94             }
95             bPktDone = true;
96             break;
97
98         case DECODE_PKTS:
99             resp = decodePacket(bPktDone);  // this may change the state to commit elem;
100             break;
101
102         case COMMIT_ELEM:
103             resp = commitElements(bPktDone); // this will change the state to DECODE_PKTS once all elem committed.
104             break;
105
106         }
107     }
108     return resp;
109 }
110
111 ocsd_datapath_resp_t TrcPktDecodeEtmV4I::onEOT()
112 {
113     ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
114     m_flush_EOT = true;
115     resp = flushEOT();
116     return resp;
117 }
118
119 ocsd_datapath_resp_t TrcPktDecodeEtmV4I::onReset()
120 {
121     ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
122     resetDecoder();
123     return resp;
124 }
125
126 ocsd_datapath_resp_t TrcPktDecodeEtmV4I::onFlush()
127 {
128     ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
129
130     // continue exception processing (can't go through processPacket as elements no longer on stack)
131     if(m_excep_info.proc != EXCEP_POP)
132         resp = processException();
133     // continue ongoing output operations on comitted elements.
134     else if(m_curr_state == COMMIT_ELEM)
135         resp = processPacket(); 
136     // continue flushing at end of trace
137     else if(m_flush_EOT)
138         resp = flushEOT();
139     return resp;
140 }
141
142 ocsd_err_t TrcPktDecodeEtmV4I::onProtocolConfig()
143 {
144     ocsd_err_t err = OCSD_OK;
145
146     // set some static config elements
147     m_CSID = m_config->getTraceID();
148     m_max_spec_depth = m_config->MaxSpecDepth();
149     m_p0_key_max = m_config->P0_Key_Max();
150     m_cond_key_max_incr = m_config->CondKeyMaxIncr();
151
152     // set up static trace instruction decode elements
153     m_instr_info.dsb_dmb_waypoints = 0;
154     m_instr_info.wfi_wfe_branch = m_config->wfiwfeBranch() ? 1 : 0;
155     m_instr_info.pe_type.arch = m_config->archVersion();
156     m_instr_info.pe_type.profile = m_config->coreProfile();
157
158     m_IASize64 = (m_config->iaSizeMax() == 64);
159
160     if (m_config->enabledRetStack())
161     {
162         m_return_stack.set_active(true);
163 #ifdef TRC_RET_STACK_DEBUG
164         m_return_stack.set_dbg_logger(this);
165 #endif
166     }
167
168     // check config compatible with current decoder support level.
169     // at present no data trace, no spec depth, no return stack, no QE
170     // Remove these checks as support is added.
171     if(m_max_spec_depth != 0)
172     {
173         err = OCSD_ERR_HW_CFG_UNSUPP;
174         LogError(ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_HW_CFG_UNSUPP,"ETMv4 instruction decode : None-zero speculation depth not supported"));
175     }
176     else if(m_config->enabledDataTrace())
177     {
178         err = OCSD_ERR_HW_CFG_UNSUPP;
179         LogError(ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_HW_CFG_UNSUPP,"ETMv4 instruction decode : Data trace elements not supported"));
180     }
181     else if(m_config->enabledLSP0Trace())
182     {
183         err = OCSD_ERR_HW_CFG_UNSUPP;
184         LogError(ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_HW_CFG_UNSUPP,"ETMv4 instruction decode : LSP0 elements not supported."));
185     }
186     else if(m_config->enabledCondITrace() != EtmV4Config::COND_TR_DIS)
187     {
188         err = OCSD_ERR_HW_CFG_UNSUPP;
189         LogError(ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_HW_CFG_UNSUPP,"ETMv4 instruction decode : Trace on conditional non-branch elements not supported."));
190     }
191     else if(m_config->enabledQE())
192     {
193         err = OCSD_ERR_HW_CFG_UNSUPP;
194         LogError(ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_HW_CFG_UNSUPP,"ETMv4 instruction decode : Trace using Q elements not supported."));
195     }
196     return err;
197 }
198
199
200 /************* local decode methods */
201 void TrcPktDecodeEtmV4I::initDecoder()
202 {
203     // set the operational modes supported.
204     m_supported_op_flags = ETMV4_SUPPORTED_DECODE_OP_FLAGS;
205
206     /* init elements that get set by config */
207     m_max_spec_depth = 0;
208     m_p0_key_max = 0;
209     m_CSID = 0;
210     m_cond_key_max_incr = 0;
211     m_IASize64 = false;
212
213     // reset decoder state to unsynced
214     resetDecoder();
215 }
216
217 void TrcPktDecodeEtmV4I::resetDecoder()
218 {
219     m_curr_state = NO_SYNC;
220     m_timestamp = 0;
221     m_context_id = 0;              
222     m_vmid_id = 0;                 
223     m_is_secure = true;
224     m_is_64bit = false;
225     m_cc_threshold = 0;
226     m_curr_spec_depth = 0;
227     m_p0_key = 0;
228     m_cond_c_key = 0;
229     m_cond_r_key = 0;
230     m_need_ctxt = true;
231     m_need_addr = true;
232     m_except_pending_addr = false;
233     m_mem_nacc_pending = false;
234     m_prev_overflow = false;
235     m_P0_stack.delete_all();
236     m_output_elem.init();
237     m_excep_info.proc = EXCEP_POP;
238     m_flush_EOT = false;
239 }
240
241 // this function can output an immediate generic element if this covers the complete packet decode, 
242 // or stack P0 and other elements for later processing on commit or cancel.
243 ocsd_datapath_resp_t TrcPktDecodeEtmV4I::decodePacket(bool &Complete)
244 {
245     bool bAllocErr = false;
246     ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
247     Complete = true;
248     bool is_addr = false;
249     bool is_except = false;
250     
251     switch(m_curr_packet_in->getType())
252     {
253     case ETM4_PKT_I_ASYNC: // nothing to do with this packet.
254     case ETM4_PKT_I_IGNORE: // or this one.
255         break;
256
257     case ETM4_PKT_I_TRACE_INFO:
258         // skip subsequent TInfo packets.
259         m_return_stack.flush();
260         break;
261
262     case ETM4_PKT_I_TRACE_ON:
263         {
264             if (m_P0_stack.createParamElemNoParam(P0_TRC_ON, false, m_curr_packet_in->getType(), m_index_curr_pkt) == 0)
265                 bAllocErr = true;
266         }
267         break;
268
269     case ETM4_PKT_I_OVERFLOW:
270         {
271             if (m_P0_stack.createParamElemNoParam(P0_OVERFLOW, false, m_curr_packet_in->getType(), m_index_curr_pkt) == 0)
272                 bAllocErr = true;
273         }
274         break;
275
276     case ETM4_PKT_I_ATOM_F1:
277     case ETM4_PKT_I_ATOM_F2:
278     case ETM4_PKT_I_ATOM_F3:
279     case ETM4_PKT_I_ATOM_F4:
280     case ETM4_PKT_I_ATOM_F5:
281     case ETM4_PKT_I_ATOM_F6:
282         {
283             if (m_P0_stack.createAtomElem(m_curr_packet_in->getType(), m_index_curr_pkt, m_curr_packet_in->getAtom()) == 0)
284                 bAllocErr = true;
285             else
286                 m_curr_spec_depth += m_curr_packet_in->getAtom().num;
287         }
288         break;
289
290     case ETM4_PKT_I_CTXT:
291         {
292             if (m_P0_stack.createContextElem(m_curr_packet_in->getType(), m_index_curr_pkt, m_curr_packet_in->getContext()) == 0)
293                 bAllocErr = true;
294         }
295         break;
296
297     case ETM4_PKT_I_ADDR_MATCH:
298         {
299             etmv4_addr_val_t addr;
300
301             addr.val = m_curr_packet_in->getAddrVal();
302             addr.isa = m_curr_packet_in->getAddrIS();
303             if (m_P0_stack.createAddrElem(m_curr_packet_in->getType(), m_index_curr_pkt, addr) == 0)
304                 bAllocErr = true;
305             is_addr = true;
306         }
307         break;
308
309     case ETM4_PKT_I_ADDR_CTXT_L_64IS0:
310     case ETM4_PKT_I_ADDR_CTXT_L_64IS1:
311     case ETM4_PKT_I_ADDR_CTXT_L_32IS0:
312     case ETM4_PKT_I_ADDR_CTXT_L_32IS1:    
313         {
314             if (m_P0_stack.createContextElem(m_curr_packet_in->getType(), m_index_curr_pkt, m_curr_packet_in->getContext()) == 0)
315                 bAllocErr = true;
316         }
317     case ETM4_PKT_I_ADDR_L_32IS0:
318     case ETM4_PKT_I_ADDR_L_32IS1:
319     case ETM4_PKT_I_ADDR_L_64IS0:
320     case ETM4_PKT_I_ADDR_L_64IS1:
321     case ETM4_PKT_I_ADDR_S_IS0:
322     case ETM4_PKT_I_ADDR_S_IS1:
323         {
324             etmv4_addr_val_t addr;
325
326             addr.val = m_curr_packet_in->getAddrVal();
327             addr.isa = m_curr_packet_in->getAddrIS();
328             if (m_P0_stack.createAddrElem(m_curr_packet_in->getType(), m_index_curr_pkt, addr) == 0)
329                 bAllocErr = true;
330             is_addr = true;
331         }
332         break;
333
334     // Exceptions
335     case ETM4_PKT_I_EXCEPT:
336          {
337             if (m_P0_stack.createExceptElem(m_curr_packet_in->getType(), m_index_curr_pkt, 
338                                             (m_curr_packet_in->exception_info.addr_interp == 0x2), 
339                                             m_curr_packet_in->exception_info.exceptionType) == 0)
340                 bAllocErr = true;
341             else
342             {
343                 m_except_pending_addr = true;  // wait for following packets before marking for commit.
344                 is_except = true;
345             }
346         }
347         break;
348
349     case ETM4_PKT_I_EXCEPT_RTN:
350         {
351             // P0 element if V7M profile.
352             bool bV7MProfile = (m_config->archVersion() == ARCH_V7) && (m_config->coreProfile() == profile_CortexM);
353             if (m_P0_stack.createParamElemNoParam(P0_EXCEP_RET, bV7MProfile, m_curr_packet_in->getType(), m_index_curr_pkt) == 0)
354                 bAllocErr = true;
355             else if (bV7MProfile)
356                 m_curr_spec_depth++;
357         }
358         break;
359
360     case ETM4_PKT_I_FUNC_RET:
361         {
362             // P0 element iff V8M profile, otherwise ignore
363             if (OCSD_IS_V8_ARCH(m_config->archVersion()) && (m_config->coreProfile() == profile_CortexM))
364             {
365                 if (m_P0_stack.createParamElemNoParam(P0_FUNC_RET, true, m_curr_packet_in->getType(), m_index_curr_pkt) == 0)
366                     bAllocErr = true;
367                 else
368                     m_curr_spec_depth++;
369             }
370         }
371         break;
372
373     // event trace
374     case ETM4_PKT_I_EVENT:
375         {
376             std::vector<uint32_t> params = { 0 };
377             params[0] = (uint32_t)m_curr_packet_in->event_val;
378             if (m_P0_stack.createParamElem(P0_EVENT, false, m_curr_packet_in->getType(), m_index_curr_pkt, params) == 0)
379                 bAllocErr = true;
380
381         }
382         break;
383
384     /* cycle count packets */
385     case ETM4_PKT_I_CCNT_F1:
386     case ETM4_PKT_I_CCNT_F2:
387     case ETM4_PKT_I_CCNT_F3:
388         {
389             std::vector<uint32_t> params = { 0 };
390             params[0] = m_curr_packet_in->getCC();
391             if (m_P0_stack.createParamElem(P0_CC, false, m_curr_packet_in->getType(), m_index_curr_pkt, params) == 0)
392                 bAllocErr = true;
393
394         }
395         break;
396
397     // timestamp
398     case ETM4_PKT_I_TIMESTAMP:
399         {
400             bool bTSwithCC = m_config->enabledCCI();
401             uint64_t ts = m_curr_packet_in->getTS();
402             std::vector<uint32_t> params = { 0, 0, 0 };
403             params[0] = (uint32_t)(ts & 0xFFFFFFFF);
404             params[1] = (uint32_t)((ts >> 32) & 0xFFFFFFFF);
405             if (bTSwithCC)
406                 params[2] = m_curr_packet_in->getCC();
407             if (m_P0_stack.createParamElem(bTSwithCC ? P0_TS_CC : P0_TS, false, m_curr_packet_in->getType(), m_index_curr_pkt, params) == 0)
408                 bAllocErr = true;
409
410         }
411         break;
412
413     case ETM4_PKT_I_BAD_SEQUENCE:
414         resp = handleBadPacket("Bad byte sequence in packet.");
415         break;
416
417     case ETM4_PKT_I_BAD_TRACEMODE:
418         resp = handleBadPacket("Invalid packet type for trace mode.");
419         break;
420
421     case ETM4_PKT_I_RESERVED:
422         resp = handleBadPacket("Reserved packet header");
423         break;
424
425     /*** presently unsupported packets ***/
426     /* conditional instruction tracing */
427     case ETM4_PKT_I_COND_FLUSH:
428     case ETM4_PKT_I_COND_I_F1:
429     case ETM4_PKT_I_COND_I_F2:
430     case ETM4_PKT_I_COND_I_F3:
431     case ETM4_PKT_I_COND_RES_F1:
432     case ETM4_PKT_I_COND_RES_F2:
433     case ETM4_PKT_I_COND_RES_F3:
434     case ETM4_PKT_I_COND_RES_F4:
435     // speculation 
436     case ETM4_PKT_I_CANCEL_F1:
437     case ETM4_PKT_I_CANCEL_F2:
438     case ETM4_PKT_I_CANCEL_F3:
439     case ETM4_PKT_I_COMMIT:
440     case ETM4_PKT_I_MISPREDICT:
441     case ETM4_PKT_I_DISCARD:
442     // data synchronisation markers
443     case ETM4_PKT_I_NUM_DS_MKR:
444     case ETM4_PKT_I_UNNUM_DS_MKR:
445     /* Q packets */
446     case ETM4_PKT_I_Q:
447         resp = OCSD_RESP_FATAL_INVALID_DATA;
448         LogError(ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_BAD_DECODE_PKT,"Unsupported packet type."));
449         break;
450
451     default:
452         // any other packet - bad packet error
453         resp = OCSD_RESP_FATAL_INVALID_DATA;
454         LogError(ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_BAD_DECODE_PKT,"Unknown packet type."));
455         break;
456
457     }
458
459     // we need to wait for following address after exception 
460     // - work out if we have seen enough here...
461     if(m_except_pending_addr && !is_except)
462     {
463         m_except_pending_addr = false;  //next packet has to be an address
464         // exception packet sequence complete
465         if(is_addr)
466         {
467             m_curr_spec_depth++;   // exceptions are P0 elements so up the spec depth to commit if needed.
468         }
469         else
470         {
471             resp = OCSD_RESP_FATAL_INVALID_DATA;
472             LogError(ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_BAD_DECODE_PKT,"Expected Address packet to follow exception packet."));
473         }
474     }
475
476     if(bAllocErr)
477     {
478         resp = OCSD_RESP_FATAL_SYS_ERR;
479         LogError(ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_MEM,"Memory allocation error."));       
480     }
481     else if(m_curr_spec_depth > m_max_spec_depth)
482     {
483         // auto commit anything above max spec depth 
484         // (this will auto commit anything if spec depth not supported!)
485         m_P0_commit = m_curr_spec_depth - m_max_spec_depth;
486         m_curr_state = COMMIT_ELEM;
487         Complete = false;   // force the processing of the commit elements.        
488     }
489     return resp;
490 }
491
492 void TrcPktDecodeEtmV4I::doTraceInfoPacket()
493 {
494     m_trace_info = m_curr_packet_in->getTraceInfo();
495     m_cc_threshold = m_curr_packet_in->getCCThreshold();
496     m_p0_key = m_curr_packet_in->getP0Key();
497     m_curr_spec_depth = m_curr_packet_in->getCurrSpecDepth();
498 }
499
500 /*
501  * Walks through the element stack, processing from oldest element to the newest, 
502    according to the number of P0 elements that need committing.
503  */
504 ocsd_datapath_resp_t TrcPktDecodeEtmV4I::commitElements(bool &Complete)
505 {
506     ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
507     bool bPause = false;    // pause commit operation 
508     bool bPopElem = true;       // do we remove the element from the stack (multi atom elements may need to stay!)
509     int num_commit_req = m_P0_commit;
510     ocsd_trc_index_t err_idx = 0;
511
512     Complete = true; // assume we exit due to completion of commit operation
513
514     TrcStackElem *pElem = 0;    // stacked element pointer
515
516     while(m_P0_commit && !bPause)
517     {
518         if(m_P0_stack.size() > 0)
519         {
520             pElem = m_P0_stack.back();  // get oldest element
521             err_idx = pElem->getRootIndex(); // save index in case of error.
522
523             switch (pElem->getP0Type())
524             {
525             // indicates a trace restart - beginning of trace or discontinuiuty
526             case P0_TRC_ON:
527                 m_output_elem.setType(OCSD_GEN_TRC_ELEM_TRACE_ON);
528                 m_output_elem.trace_on_reason = m_prev_overflow ? TRACE_ON_OVERFLOW : TRACE_ON_NORMAL;
529                 m_prev_overflow = false;
530                 resp = outputTraceElementIdx(pElem->getRootIndex(),m_output_elem);
531                 m_return_stack.flush();
532                 break;
533
534             case P0_ADDR:
535                 {
536                 TrcStackElemAddr *pAddrElem = dynamic_cast<TrcStackElemAddr *>(pElem);
537                 m_return_stack.clear_pop_pending(); // address removes the need to pop the indirect address target from the stack
538                 if(pAddrElem)
539                 {
540                     SetInstrInfoInAddrISA(pAddrElem->getAddr().val, pAddrElem->getAddr().isa);
541                     m_need_addr = false;
542                 }
543                 }
544                 break;
545
546             case P0_CTXT:
547                 {
548                 TrcStackElemCtxt *pCtxtElem = dynamic_cast<TrcStackElemCtxt *>(pElem);
549                 if(pCtxtElem)
550                 {
551                     etmv4_context_t ctxt = pCtxtElem->getContext();
552                     // check this is an updated context
553                     if(ctxt.updated)
554                     {
555                         updateContext(pCtxtElem);
556
557                         m_output_elem.setType(OCSD_GEN_TRC_ELEM_PE_CONTEXT);
558                         resp = outputTraceElementIdx(pElem->getRootIndex(),m_output_elem);
559                     }
560                 }
561                 }
562                 break;
563
564             case P0_EVENT:
565                 {
566                 TrcStackElemParam *pParamElem = dynamic_cast<TrcStackElemParam *>(pElem);
567                 if(pParamElem)
568                     resp = this->outputEvent(pParamElem);
569                 }
570                 break;
571
572             case P0_TS:
573                 {
574                 TrcStackElemParam *pParamElem = dynamic_cast<TrcStackElemParam *>(pElem);
575                 if(pParamElem)
576                     resp = outputTS(pParamElem,false);
577                 }
578                 break;
579
580             case P0_CC:
581                 {
582                 TrcStackElemParam *pParamElem = dynamic_cast<TrcStackElemParam *>(pElem);
583                 if(pParamElem)
584                     resp = outputCC(pParamElem);
585                 }
586                 break;
587
588             case P0_TS_CC:
589                 {
590                 TrcStackElemParam *pParamElem = dynamic_cast<TrcStackElemParam *>(pElem);
591                 if(pParamElem)
592                     resp = outputTS(pParamElem,true);
593                 }
594                 break;
595
596             case P0_OVERFLOW:
597                 m_prev_overflow = true;
598                 break;
599
600             case P0_ATOM:
601                 {
602                 TrcStackElemAtom *pAtomElem = dynamic_cast<TrcStackElemAtom *>(pElem);
603
604                 if(pAtomElem)
605                 {
606                     bool bContProcess = true;
607                     while(!pAtomElem->isEmpty() && m_P0_commit && bContProcess)
608                     {
609                         ocsd_atm_val atom = pAtomElem->commitOldest();
610
611                         // check if prev atom left us an indirect address target on the return stack
612                         if ((resp = returnStackPop()) != OCSD_RESP_CONT)
613                             break;
614
615                         // if address and context do instruction trace follower.
616                         // otherwise skip atom and reduce committed elements
617                         if(!m_need_ctxt && !m_need_addr)
618                         {
619                             resp = processAtom(atom,bContProcess);
620                         }
621                         m_P0_commit--; // mark committed 
622                     }
623                     if(!pAtomElem->isEmpty())   
624                         bPopElem = false;   // don't remove if still atoms to process.
625                 }
626                 }
627                 break;
628
629             case P0_EXCEP:
630                 // check if prev atom left us an indirect address target on the return stack
631                 if ((resp = returnStackPop()) != OCSD_RESP_CONT)
632                     break;
633
634                 m_excep_info.proc = EXCEP_POP;   // set state in case we need to stop part way through
635                 resp = processException();  // output trace + exception elements.
636                 m_P0_commit--;
637                 break;
638
639             case P0_EXCEP_RET:
640                 m_output_elem.setType(OCSD_GEN_TRC_ELEM_EXCEPTION_RET);
641                 resp = outputTraceElementIdx(pElem->getRootIndex(),m_output_elem);
642                 if(pElem->isP0()) // are we on a core that counts ERET as P0?
643                     m_P0_commit--;
644                 break;
645
646             case P0_FUNC_RET:
647                 // func ret is V8M - data trace only - hint that data has been popped off the stack.
648                 // at this point nothing to do till the decoder starts handling data trace.
649                 if (pElem->isP0()) 
650                     m_P0_commit--;
651                 break;
652             }
653
654             if(bPopElem)
655                 m_P0_stack.delete_back();  // remove element from stack;
656
657             // if response not continue, then break out of the loop.
658             if(!OCSD_DATA_RESP_IS_CONT(resp))
659             {
660                 bPause = true;
661             }
662         }
663         else
664         {
665             // too few elements for commit operation - decode error.
666             resp = OCSD_RESP_FATAL_INVALID_DATA;
667             LogError(ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_COMMIT_PKT_OVERRUN,err_idx,m_CSID,"Not enough elements to commit"));
668             bPause = true;
669         }
670     }
671
672     // done all elements - need more packets.
673     if(m_P0_commit == 0)    
674         m_curr_state = DECODE_PKTS;
675
676     // reduce the spec depth by number of comitted elements
677     m_curr_spec_depth -= (num_commit_req-m_P0_commit);
678     return resp;
679 }
680
681 ocsd_datapath_resp_t TrcPktDecodeEtmV4I::returnStackPop()
682 {
683     ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
684     ocsd_isa nextISA;
685     
686     if (m_return_stack.pop_pending())
687     {
688         ocsd_vaddr_t popAddr = m_return_stack.pop(nextISA);
689         if (m_return_stack.overflow())
690         {
691             resp = OCSD_RESP_FATAL_INVALID_DATA;
692             LogError(ocsdError(OCSD_ERR_SEV_ERROR, OCSD_ERR_RET_STACK_OVERFLOW, "Trace Return Stack Overflow."));
693         }
694         else
695         {
696             m_instr_info.instr_addr = popAddr;
697             m_instr_info.isa = nextISA;
698             m_need_addr = false;
699         }
700     }
701     return resp;
702 }
703
704 ocsd_datapath_resp_t TrcPktDecodeEtmV4I::flushEOT()
705 {
706     ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
707     if(m_flush_EOT)
708     {
709         TrcStackElem *pElem = 0;
710         while(OCSD_DATA_RESP_IS_CONT(resp) && (m_P0_stack.size() > 0))
711         {
712             // scan for outstanding events, TS and CC, before any outstanding
713             // P0 commit elements.
714             pElem = m_P0_stack.back();
715             
716             switch(pElem->getP0Type())
717             {
718                 // clear stack and stop
719             case P0_UNKNOWN:
720             case P0_ATOM:
721             case P0_TRC_ON:
722             case P0_EXCEP:
723             case P0_EXCEP_RET:
724             case P0_OVERFLOW:
725                 m_P0_stack.delete_all();
726                 break;
727
728                 //skip
729             case P0_ADDR:
730             case P0_CTXT:
731                 break;
732
733                 // output
734             case P0_EVENT:
735                 {
736                 TrcStackElemParam *pParamElem = dynamic_cast<TrcStackElemParam *>(pElem);
737                 if(pParamElem)
738                     resp = this->outputEvent(pParamElem);
739                 }
740                 break;
741
742             case P0_TS:
743                 {
744                 TrcStackElemParam *pParamElem = dynamic_cast<TrcStackElemParam *>(pElem);
745                 if(pParamElem)
746                     resp = outputTS(pParamElem,false);
747                 }
748                 break;
749
750             case P0_CC:
751                 {
752                 TrcStackElemParam *pParamElem = dynamic_cast<TrcStackElemParam *>(pElem);
753                 if(pParamElem)
754                     resp = outputCC(pParamElem);
755                 }
756                 break;
757
758             case P0_TS_CC:
759                 {
760                 TrcStackElemParam *pParamElem = dynamic_cast<TrcStackElemParam *>(pElem);
761                 if(pParamElem)
762                     resp = outputTS(pParamElem,true);
763                 }
764                 break;
765             }
766             m_P0_stack.delete_back();
767         }
768
769         if(OCSD_DATA_RESP_IS_CONT(resp) && (m_P0_stack.size() == 0))
770         {
771             m_output_elem.setType(OCSD_GEN_TRC_ELEM_EO_TRACE);
772             resp = outputTraceElement(m_output_elem);
773             m_flush_EOT = false;
774         }
775     }
776     return resp;
777 }
778
779 ocsd_datapath_resp_t TrcPktDecodeEtmV4I::outputCC(TrcStackElemParam *pParamElem)
780 {
781     m_output_elem.setType(OCSD_GEN_TRC_ELEM_CYCLE_COUNT);
782     m_output_elem.setCycleCount(pParamElem->getParam(0));
783     return outputTraceElementIdx(pParamElem->getRootIndex(),m_output_elem);
784 }
785
786 ocsd_datapath_resp_t TrcPktDecodeEtmV4I::outputTS(TrcStackElemParam *pParamElem, bool withCC)
787 {
788     m_output_elem.setType(OCSD_GEN_TRC_ELEM_TIMESTAMP);
789     m_output_elem.timestamp = (uint64_t)(pParamElem->getParam(0)) | (((uint64_t)pParamElem->getParam(1)) << 32);
790     if(withCC)
791         m_output_elem.setCycleCount(pParamElem->getParam(2));
792     return outputTraceElementIdx(pParamElem->getRootIndex(),m_output_elem);
793 }
794
795 ocsd_datapath_resp_t TrcPktDecodeEtmV4I::outputEvent(TrcStackElemParam *pParamElem)
796 {
797     m_output_elem.setType(OCSD_GEN_TRC_ELEM_EVENT);
798     m_output_elem.trace_event.ev_type = EVENT_NUMBERED;
799     m_output_elem.trace_event.ev_number = pParamElem->getParam(0);
800     return outputTraceElementIdx(pParamElem->getRootIndex(),m_output_elem);
801 }
802
803 ocsd_datapath_resp_t TrcPktDecodeEtmV4I::outputTraceRange(const bool executed, ocsd_trc_index_t index)
804 {
805     m_output_elem.setType(OCSD_GEN_TRC_ELEM_INSTR_RANGE);
806     m_output_elem.setLastInstrInfo(executed, m_instr_info.type, m_instr_info.sub_type, m_instr_info.instr_size);
807     m_output_elem.setISA(m_instr_info.isa);
808     m_output_elem.setLastInstrCond(m_instr_info.is_conditional);
809     if (executed)
810         m_instr_info.isa = m_instr_info.next_isa;
811     return outputTraceElementIdx(index, m_output_elem);
812 }
813
814 ocsd_datapath_resp_t TrcPktDecodeEtmV4I::processAtom(const ocsd_atm_val atom, bool &bCont)
815 {
816     ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
817     TrcStackElem *pElem = m_P0_stack.back();  // get the atom element
818     bool bWPFound = false;
819     ocsd_err_t err;
820     bCont = true;
821
822     err = traceInstrToWP(bWPFound);
823     if(err != OCSD_OK)
824     {
825         if(err == OCSD_ERR_UNSUPPORTED_ISA)
826         {
827              m_need_addr = true;
828              m_need_ctxt = true;
829              LogError(ocsdError(OCSD_ERR_SEV_WARN,err,pElem->getRootIndex(),m_CSID,"Warning: unsupported instruction set processing atom packet."));  
830              // wait for next context
831              return resp;
832         }
833         else
834         {
835             bCont = false;
836             resp = OCSD_RESP_FATAL_INVALID_DATA;
837             LogError(ocsdError(OCSD_ERR_SEV_ERROR,err,pElem->getRootIndex(),m_CSID,"Error processing atom packet."));  
838             return resp;
839         }
840     }
841
842     if(bWPFound)
843     {
844         //  save recorded next instuction address
845         ocsd_vaddr_t nextAddr = m_instr_info.instr_addr;
846
847         // action according to waypoint type and atom value
848         switch(m_instr_info.type)
849         {
850         case OCSD_INSTR_BR:
851             if (atom == ATOM_E)
852             {
853                 m_instr_info.instr_addr = m_instr_info.branch_addr;
854                 if (m_instr_info.is_link)
855                     m_return_stack.push(nextAddr, m_instr_info.isa);
856
857             }
858             break;
859
860         case OCSD_INSTR_BR_INDIRECT:
861             if (atom == ATOM_E)
862             {
863                 m_need_addr = true; // indirect branch taken - need new address.
864                 if (m_instr_info.is_link)
865                     m_return_stack.push(nextAddr,m_instr_info.isa);
866                 m_return_stack.set_pop_pending();  // need to know next packet before we know what is to happen
867             }
868             break;
869         }
870         resp = outputTraceRange((atom == ATOM_E), pElem->getRootIndex());
871
872     }
873     else
874     {
875         // no waypoint - likely inaccessible memory range.
876         m_need_addr = true; // need an address update 
877
878         if(m_output_elem.st_addr != m_output_elem.en_addr)
879         {
880             // some trace before we were out of memory access range
881             resp = outputTraceRange(true, pElem->getRootIndex());
882         }
883
884         if(m_mem_nacc_pending && OCSD_DATA_RESP_IS_CONT(resp))
885         {
886             m_output_elem.setType(OCSD_GEN_TRC_ELEM_ADDR_NACC);
887             m_output_elem.st_addr = m_nacc_addr;
888             resp = outputTraceElementIdx(pElem->getRootIndex(),m_output_elem);
889             m_mem_nacc_pending = false;
890         }
891     }
892
893     if(!OCSD_DATA_RESP_IS_CONT(resp))
894         bCont = false;
895
896     return resp;
897 }
898
899 // Exception processor
900 ocsd_datapath_resp_t  TrcPktDecodeEtmV4I::processException()
901 {
902     ocsd_datapath_resp_t resp = OCSD_RESP_CONT; 
903     TrcStackElemExcept *pExceptElem;
904
905     m_excep_info.addr_b_tgt = false;
906     
907     if(m_excep_info.proc == EXCEP_POP)
908     {
909         pExceptElem = dynamic_cast<TrcStackElemExcept *>(m_P0_stack.back());  // get the exception element
910         TrcStackElemAddr *pAddressElem = 0;
911         TrcStackElemCtxt *pCtxtElem = 0;
912         TrcStackElem *pElem = 0;
913     
914         m_P0_stack.pop_back(); // remove the exception element
915         pElem = m_P0_stack.back();  // look at next element.
916         if(pElem->getP0Type() == P0_CTXT)
917         {
918             pCtxtElem = dynamic_cast<TrcStackElemCtxt *>(pElem);
919             m_P0_stack.pop_back(); // remove the context element
920             pElem = m_P0_stack.back();  // next one should be an address element
921         }
922    
923         if(pElem->getP0Type() != P0_ADDR)
924         {
925             // no following address element - indicate processing error.
926             resp = OCSD_RESP_FATAL_INVALID_DATA;
927             LogError(ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_BAD_PACKET_SEQ,pExceptElem->getRootIndex(),m_CSID,"Address missing in exception packet."));  
928         }
929         else
930         {
931             // extract address
932             pAddressElem = static_cast<TrcStackElemAddr *>(pElem);
933
934             // fill in exception info for use later
935             m_excep_info.addr = pAddressElem->getAddr();                
936             m_excep_info.number = pExceptElem->getExcepNum();
937             m_excep_info.index = pExceptElem->getRootIndex();
938             m_excep_info.addr_b_tgt = pExceptElem->getPrevSame();
939
940             // see if there is an address + optional context element implied 
941             // prior to the exception.
942             if (m_excep_info.addr_b_tgt)
943             {
944                 // this was a branch target address - update current setting
945                 bool b64bit = m_instr_info.isa == ocsd_isa_aarch64;
946                 if (pCtxtElem) {
947                     b64bit = pCtxtElem->getContext().SF;
948                 }
949                 m_instr_info.instr_addr = m_excep_info.addr.val;
950                 m_instr_info.isa = (m_excep_info.addr.isa == 0) ? 
951                     (b64bit ? ocsd_isa_aarch64 : ocsd_isa_arm) : ocsd_isa_thumb2;
952                 m_need_addr = false;
953             }
954
955             // figure out next move
956             if (pCtxtElem) {
957                 m_excep_info.proc = EXCEP_CTXT;
958                 updateContext(pCtxtElem);
959             }
960             else if(m_excep_info.addr.val == m_instr_info.instr_addr)
961                 m_excep_info.proc = EXCEP_EXCEP;
962             else
963                 m_excep_info.proc = EXCEP_RANGE;
964         }
965         m_P0_stack.delete_popped();
966     }
967
968     // output a context element
969     if (m_excep_info.proc == EXCEP_CTXT)
970     {
971         m_output_elem.setType(OCSD_GEN_TRC_ELEM_PE_CONTEXT);
972         resp = outputTraceElementIdx(m_excep_info.index, m_output_elem);
973         m_excep_info.proc = EXCEP_EXCEP;
974         if (!OCSD_DATA_RESP_IS_CONT(resp))
975             return resp;
976     }
977
978     // output a range element
979     if(m_excep_info.proc == EXCEP_RANGE) 
980     {
981         bool bWPFound = false;
982         ocsd_err_t err;
983
984         // last instr_info address is the start address
985         m_output_elem.st_addr = m_instr_info.instr_addr;
986
987         // look for match to return address.
988         err = traceInstrToWP(bWPFound,true,m_excep_info.addr.val);
989
990         if(err != OCSD_OK)
991         {
992             if(err == OCSD_ERR_UNSUPPORTED_ISA)
993             {
994                 m_need_addr = true;
995                 m_need_ctxt = true;
996                 LogError(ocsdError(OCSD_ERR_SEV_WARN,err,m_excep_info.index,m_CSID,"Warning: unsupported instruction set processing exception packet."));  
997             }
998             else
999             {
1000                 resp = OCSD_RESP_FATAL_INVALID_DATA;
1001                 LogError(ocsdError(OCSD_ERR_SEV_ERROR,err,m_excep_info.index,m_CSID,"Error processing exception packet."));  
1002                 m_excep_info.proc = EXCEP_POP;  // nothing more to do, reset to start of exception handling
1003             }
1004         }
1005
1006         if(bWPFound)
1007         {
1008             // waypoint address found - output range
1009             resp = outputTraceRange(true, m_excep_info.index);
1010             m_excep_info.proc = EXCEP_EXCEP;
1011         }
1012         else
1013         {
1014             // no waypoint - likely inaccessible memory range.
1015             m_need_addr = true; // need an address update 
1016             
1017             if(m_output_elem.st_addr != m_output_elem.en_addr)
1018             {
1019                 // some trace before we were out of memory access range
1020                 resp = outputTraceRange(true, m_excep_info.index);
1021             }
1022
1023             m_excep_info.proc = m_mem_nacc_pending ? EXCEP_NACC : EXCEP_EXCEP;
1024         }
1025     }  
1026     
1027     if((m_excep_info.proc == EXCEP_NACC) && OCSD_DATA_RESP_IS_CONT(resp))
1028     {
1029         m_output_elem.setType(OCSD_GEN_TRC_ELEM_ADDR_NACC);
1030         m_output_elem.st_addr = m_nacc_addr;
1031         resp = outputTraceElementIdx(m_excep_info.index,m_output_elem);
1032         m_excep_info.proc = EXCEP_EXCEP;
1033         m_mem_nacc_pending = false;
1034     }
1035     
1036     if((m_excep_info.proc == EXCEP_EXCEP) && OCSD_DATA_RESP_IS_CONT(resp))
1037     {
1038         // output element.
1039         m_output_elem.setType(OCSD_GEN_TRC_ELEM_EXCEPTION);
1040         // add end address as preferred return address to end addr in element
1041         m_output_elem.en_addr = m_excep_info.addr.val;
1042         m_output_elem.excep_ret_addr = 1;
1043         m_output_elem.excep_ret_addr_br_tgt = m_excep_info.addr_b_tgt;
1044         m_output_elem.exception_number = m_excep_info.number;
1045         resp = outputTraceElementIdx(m_excep_info.index,m_output_elem);  
1046         m_excep_info.proc = EXCEP_POP;
1047     }   
1048     return resp;
1049 }
1050
1051 void TrcPktDecodeEtmV4I::SetInstrInfoInAddrISA(const ocsd_vaddr_t addr_val, const uint8_t isa)
1052 {
1053     m_instr_info.instr_addr = addr_val;
1054     if(m_is_64bit)
1055         m_instr_info.isa = ocsd_isa_aarch64;
1056     else
1057         m_instr_info.isa = (isa == 0) ? ocsd_isa_arm : ocsd_isa_thumb2;
1058 }
1059
1060 // trace an instruction range to a waypoint - and set next address to restart from.
1061 ocsd_err_t TrcPktDecodeEtmV4I::traceInstrToWP(bool &bWPFound, const bool traceToAddrNext /*= false*/, const ocsd_vaddr_t nextAddrMatch /*= 0*/)
1062 {
1063     uint32_t opcode;
1064     uint32_t bytesReq;
1065     ocsd_err_t err = OCSD_OK;
1066
1067     // TBD?: update mem space to allow for EL as well.
1068     ocsd_mem_space_acc_t mem_space = m_is_secure ? OCSD_MEM_SPACE_S : OCSD_MEM_SPACE_N;
1069
1070     m_output_elem.st_addr = m_output_elem.en_addr = m_instr_info.instr_addr;
1071     m_output_elem.num_instr_range = 0;
1072
1073     bWPFound = false;
1074
1075     while(!bWPFound && !m_mem_nacc_pending)
1076     {
1077         // start off by reading next opcode;
1078         bytesReq = 4;
1079         err = accessMemory(m_instr_info.instr_addr,mem_space,&bytesReq,(uint8_t *)&opcode);
1080         if(err != OCSD_OK) break;
1081
1082         if(bytesReq == 4) // got data back
1083         {
1084             m_instr_info.opcode = opcode;
1085             err = instrDecode(&m_instr_info);
1086             if(err != OCSD_OK) break;
1087
1088             // increment address - may be adjusted by direct branch value later
1089             m_instr_info.instr_addr += m_instr_info.instr_size;
1090
1091             // update the range decoded address in the output packet.
1092             m_output_elem.en_addr = m_instr_info.instr_addr;
1093             m_output_elem.num_instr_range++;
1094
1095             // either walking to match the next instruction address or a real watchpoint
1096             if(traceToAddrNext)
1097                 bWPFound = (m_output_elem.en_addr == nextAddrMatch);
1098             else
1099                 bWPFound = (m_instr_info.type != OCSD_INSTR_OTHER);
1100         }
1101         else
1102         {
1103             // not enough memory accessible.
1104             m_mem_nacc_pending = true;
1105             m_nacc_addr = m_instr_info.instr_addr;           
1106         }
1107     }
1108     return err;
1109 }
1110
1111 void TrcPktDecodeEtmV4I::updateContext(TrcStackElemCtxt *pCtxtElem)
1112 {
1113     etmv4_context_t ctxt = pCtxtElem->getContext();
1114     // map to output element  and local saved state.
1115     m_is_64bit = (ctxt.SF != 0);
1116     m_output_elem.context.bits64 = ctxt.SF;
1117     m_is_secure = (ctxt.NS == 0);
1118     m_output_elem.context.security_level = ctxt.NS ? ocsd_sec_nonsecure : ocsd_sec_secure;
1119     m_output_elem.context.exception_level = (ocsd_ex_level)ctxt.EL;
1120     m_output_elem.context.el_valid = 1;
1121     if(ctxt.updated_c)
1122     {
1123         m_output_elem.context.ctxt_id_valid = 1;
1124         m_context_id = m_output_elem.context.context_id = ctxt.ctxtID;
1125     }
1126     if(ctxt.updated_v)
1127     {
1128         m_output_elem.context.vmid_valid = 1;
1129         m_vmid_id = m_output_elem.context.vmid = ctxt.VMID;
1130     }
1131     m_need_ctxt = false;
1132 }
1133
1134 ocsd_datapath_resp_t TrcPktDecodeEtmV4I::handleBadPacket(const char *reason)
1135 {
1136     ocsd_datapath_resp_t resp  = OCSD_RESP_CONT;   
1137
1138     if(getComponentOpMode() && OCSD_OPFLG_PKTDEC_ERROR_BAD_PKTS)
1139     {
1140         // error out - stop decoding
1141         resp = OCSD_RESP_FATAL_INVALID_DATA;
1142         LogError(ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_BAD_DECODE_PKT,reason));
1143     }
1144     else
1145     {
1146         // switch to unsync - clear decode state
1147         m_output_elem.setType(OCSD_GEN_TRC_ELEM_NO_SYNC);
1148         resp = outputTraceElement(m_output_elem);
1149         resetDecoder();
1150         m_curr_state = WAIT_SYNC;
1151     }
1152     return resp;
1153 }
1154
1155 /* End of File trc_pkt_decode_etmv4i.cpp */