1 /*
2 * \file trc_pkt_proc_etmv4i.cpp
3 * \brief OpenCSD : Packet processor for ETMv4
4 *
5 * \copyright Copyright (c) 2015, 2019, ARM Limited. All Rights Reserved.
6 */
7
8 /*
9 * Redistribution and use in source and binary forms, with or without modification,
10 * are permitted provided that the following conditions are met:
11 *
12 * 1. Redistributions of source code must retain the above copyright notice,
13 * this list of conditions and the following disclaimer.
14 *
15 * 2. Redistributions in binary form must reproduce the above copyright notice,
16 * this list of conditions and the following disclaimer in the documentation
17 * and/or other materials provided with the distribution.
18 *
19 * 3. Neither the name of the copyright holder nor the names of its contributors
20 * may be used to endorse or promote products derived from this software without
21 * specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND
24 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
25 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
26 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
27 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
28 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
30 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
31 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
32 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33 */
34
35 #include "opencsd/etmv4/trc_pkt_proc_etmv4.h"
36 #include "common/ocsd_error.h"
37
38 #ifdef __GNUC__
39 // G++ doesn't like the ## pasting
40 #define ETMV4I_PKTS_NAME "PKTP_ETMV4I"
41 #else
42 // VC++ is fine
43 #define ETMV4I_PKTS_NAME OCSD_CMPNAME_PREFIX_PKTPROC##"_ETMV4I"
44 #endif
45
46 static const uint32_t ETMV4_SUPPORTED_OP_FLAGS = OCSD_OPFLG_PKTPROC_COMMON;
47
48 // test defines - if testing with ETMv4 sources, disable error on ERET.
49 // #define ETE_TRACE_ERET_AS_IGNORE
50
51 /* trace etmv4 packet processing class */
TrcPktProcEtmV4I()52 TrcPktProcEtmV4I::TrcPktProcEtmV4I() : TrcPktProcBase(ETMV4I_PKTS_NAME),
53 m_isInit(false),
54 m_first_trace_info(false)
55 {
56 m_supported_op_flags = ETMV4_SUPPORTED_OP_FLAGS;
57 }
58
TrcPktProcEtmV4I(int instIDNum)59 TrcPktProcEtmV4I::TrcPktProcEtmV4I(int instIDNum) : TrcPktProcBase(ETMV4I_PKTS_NAME, instIDNum),
60 m_isInit(false),
61 m_first_trace_info(false)
62 {
63 m_supported_op_flags = ETMV4_SUPPORTED_OP_FLAGS;
64 }
65
66
~TrcPktProcEtmV4I()67 TrcPktProcEtmV4I::~TrcPktProcEtmV4I()
68 {
69 }
70
onProtocolConfig()71 ocsd_err_t TrcPktProcEtmV4I::onProtocolConfig()
72 {
73 InitProcessorState();
74 m_config = *TrcPktProcBase::getProtocolConfig();
75 BuildIPacketTable(); // packet table based on config
76 m_curr_packet.setProtocolVersion(m_config.FullVersion());
77 m_isInit = true;
78 statsInit();
79 return OCSD_OK;
80 }
81
processData(const ocsd_trc_index_t index,const uint32_t dataBlockSize,const uint8_t * pDataBlock,uint32_t * numBytesProcessed)82 ocsd_datapath_resp_t TrcPktProcEtmV4I::processData( const ocsd_trc_index_t index,
83 const uint32_t dataBlockSize,
84 const uint8_t *pDataBlock,
85 uint32_t *numBytesProcessed)
86 {
87 ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
88
89 if (!m_isInit)
90 return OCSD_RESP_FATAL_NOT_INIT;
91
92 m_trcIn.init(dataBlockSize, pDataBlock, &m_currPacketData);
93 m_blockIndex = index;
94 bool done = false;
95 uint8_t nextByte;
96
97 do
98 {
99 try
100 {
101 while ( (!m_trcIn.empty() || (m_process_state == SEND_PKT)) &&
102 OCSD_DATA_RESP_IS_CONT(resp)
103 )
104 {
105 switch (m_process_state)
106 {
107 case PROC_HDR:
108 m_packet_index = m_blockIndex + m_trcIn.processed();
109 if (m_is_sync)
110 {
111 nextByte = m_trcIn.peekNextByte();
112 m_pIPktFn = m_i_table[nextByte].pptkFn;
113 m_curr_packet.type = m_i_table[nextByte].pkt_type;
114 }
115 else
116 {
117 // unsynced - process data until we see a sync point
118 m_pIPktFn = &TrcPktProcEtmV4I::iNotSync;
119 m_curr_packet.type = ETM4_PKT_I_NOTSYNC;
120 }
121 m_process_state = PROC_DATA;
122
123 case PROC_DATA:
124 // loop till full packet or no more data...
125 while (!m_trcIn.empty() && (m_process_state == PROC_DATA))
126 {
127 nextByte = m_trcIn.peekNextByte();
128 m_trcIn.copyByteToPkt(); // move next byte into the packet
129 (this->*m_pIPktFn)(nextByte);
130 }
131 break;
132
133 case SEND_PKT:
134 resp = outputPacket();
135 InitPacketState();
136 m_process_state = PROC_HDR;
137 break;
138
139 case SEND_UNSYNCED:
140 resp = outputUnsyncedRawPacket();
141 if (m_update_on_unsync_packet_index != 0)
142 {
143 m_packet_index = m_update_on_unsync_packet_index;
144 m_update_on_unsync_packet_index = 0;
145 }
146 m_process_state = PROC_DATA; // after dumping unsynced data, still in data mode.
147 break;
148 }
149 }
150 done = true;
151 }
152 catch(ocsdError &err)
153 {
154 done = true;
155 LogError(err);
156 if( (err.getErrorCode() == OCSD_ERR_BAD_PACKET_SEQ) ||
157 (err.getErrorCode() == OCSD_ERR_INVALID_PCKT_HDR))
158 {
159 // send invalid packets up the pipe to let the next stage decide what to do.
160 if (err.getErrorCode() == OCSD_ERR_INVALID_PCKT_HDR)
161 statsAddBadHdrCount(1);
162 else
163 statsAddBadSeqCount(1);
164 m_process_state = SEND_PKT;
165 done = false;
166 }
167 else
168 {
169 // bail out on any other error.
170 resp = OCSD_RESP_FATAL_INVALID_DATA;
171 }
172 }
173 catch(...)
174 {
175 done = true;
176 /// vv bad at this point.
177 resp = OCSD_RESP_FATAL_SYS_ERR;
178 const ocsdError &fatal = ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_FAIL,m_packet_index,m_config.getTraceID(),"Unknown System Error decoding trace.");
179 LogError(fatal);
180 }
181 } while (!done);
182
183 statsAddTotalCount(m_trcIn.processed());
184 *numBytesProcessed = m_trcIn.processed();
185 return resp;
186 }
187
onEOT()188 ocsd_datapath_resp_t TrcPktProcEtmV4I::onEOT()
189 {
190 ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
191 if (!m_isInit)
192 return OCSD_RESP_FATAL_NOT_INIT;
193
194 // if we have a partial packet then send to attached sinks
195 if(m_currPacketData.size() != 0)
196 {
197 m_curr_packet.updateErrType(ETM4_PKT_I_INCOMPLETE_EOT);
198 resp = outputPacket();
199 InitPacketState();
200 }
201 return resp;
202 }
203
onReset()204 ocsd_datapath_resp_t TrcPktProcEtmV4I::onReset()
205 {
206 if (!m_isInit)
207 return OCSD_RESP_FATAL_NOT_INIT;
208
209 // prepare for new decoding session
210 InitProcessorState();
211 return OCSD_RESP_CONT;
212 }
213
onFlush()214 ocsd_datapath_resp_t TrcPktProcEtmV4I::onFlush()
215 {
216 if (!m_isInit)
217 return OCSD_RESP_FATAL_NOT_INIT;
218
219 // packet processor never holds on to flushable data (may have partial packet,
220 // but any full packets are immediately sent)
221 return OCSD_RESP_CONT;
222 }
223
InitPacketState()224 void TrcPktProcEtmV4I::InitPacketState()
225 {
226 m_currPacketData.clear();
227 m_curr_packet.initNextPacket(); // clear for next packet.
228 m_update_on_unsync_packet_index = 0;
229 }
230
InitProcessorState()231 void TrcPktProcEtmV4I::InitProcessorState()
232 {
233 InitPacketState();
234 m_pIPktFn = &TrcPktProcEtmV4I::iNotSync;
235 m_packet_index = 0;
236 m_is_sync = false;
237 m_first_trace_info = false;
238 m_sent_notsync_packet = false;
239 m_process_state = PROC_HDR;
240 m_curr_packet.initStartState();
241 }
242
outputPacket()243 ocsd_datapath_resp_t TrcPktProcEtmV4I::outputPacket()
244 {
245 ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
246 resp = outputOnAllInterfaces(m_packet_index,&m_curr_packet,&m_curr_packet.type,m_currPacketData);
247 return resp;
248 }
249
outputUnsyncedRawPacket()250 ocsd_datapath_resp_t TrcPktProcEtmV4I::outputUnsyncedRawPacket()
251 {
252 ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
253
254 statsAddUnsyncCount(m_dump_unsynced_bytes);
255 outputRawPacketToMonitor(m_packet_index,&m_curr_packet,m_dump_unsynced_bytes,&m_currPacketData[0]);
256
257 if(!m_sent_notsync_packet)
258 {
259 resp = outputDecodedPacket(m_packet_index,&m_curr_packet);
260 m_sent_notsync_packet = true;
261 }
262
263 if(m_currPacketData.size() <= m_dump_unsynced_bytes)
264 m_currPacketData.clear();
265 else
266 m_currPacketData.erase(m_currPacketData.begin(),m_currPacketData.begin()+m_dump_unsynced_bytes);
267
268 return resp;
269 }
270
iNotSync(const uint8_t lastByte)271 void TrcPktProcEtmV4I::iNotSync(const uint8_t lastByte)
272 {
273 // is it an extension byte?
274 if (lastByte == 0x00) // TBD : add check for forced sync in here?
275 {
276 if (m_currPacketData.size() > 1)
277 {
278 m_dump_unsynced_bytes = m_currPacketData.size() - 1;
279 m_process_state = SEND_UNSYNCED;
280 // outputting some data then update packet index after so output indexes accurate
281 m_update_on_unsync_packet_index = m_blockIndex + m_trcIn.processed() - 1;
282 }
283 else
284 m_packet_index = m_blockIndex + m_trcIn.processed() - 1; // set it up now otherwise.
285
286 m_pIPktFn = m_i_table[lastByte].pptkFn;
287 }
288 else if (m_currPacketData.size() >= 8)
289 {
290 m_dump_unsynced_bytes = m_currPacketData.size();
291 m_process_state = SEND_UNSYNCED;
292 // outputting some data then update packet index after so output indexes accurate
293 m_update_on_unsync_packet_index = m_blockIndex + m_trcIn.processed();
294 }
295 }
296
iPktNoPayload(const uint8_t lastByte)297 void TrcPktProcEtmV4I::iPktNoPayload(const uint8_t lastByte)
298 {
299 // some expansion may be required...
300 switch(m_curr_packet.type)
301 {
302 case ETM4_PKT_I_ADDR_MATCH:
303 case ETE_PKT_I_SRC_ADDR_MATCH:
304 m_curr_packet.setAddressExactMatch(lastByte & 0x3);
305 break;
306
307 case ETM4_PKT_I_EVENT:
308 m_curr_packet.setEvent(lastByte & 0xF);
309 break;
310
311 case ETM4_PKT_I_NUM_DS_MKR:
312 case ETM4_PKT_I_UNNUM_DS_MKR:
313 m_curr_packet.setDataSyncMarker(lastByte & 0x7);
314 break;
315
316 // these just need the packet type - no processing required.
317 case ETM4_PKT_I_COND_FLUSH:
318 case ETM4_PKT_I_EXCEPT_RTN:
319 case ETM4_PKT_I_TRACE_ON:
320 case ETM4_PKT_I_FUNC_RET:
321 case ETE_PKT_I_TRANS_ST:
322 case ETE_PKT_I_TRANS_COMMIT:
323 case ETM4_PKT_I_IGNORE:
324 default: break;
325 }
326 m_process_state = SEND_PKT; // now just send it....
327 }
328
iPktReserved(const uint8_t lastByte)329 void TrcPktProcEtmV4I::iPktReserved(const uint8_t lastByte)
330 {
331 m_curr_packet.updateErrType(ETM4_PKT_I_RESERVED, lastByte); // swap type for err type
332 throw ocsdError(OCSD_ERR_SEV_ERROR, OCSD_ERR_INVALID_PCKT_HDR,m_packet_index,m_config.getTraceID());
333 }
334
iPktInvalidCfg(const uint8_t lastByte)335 void TrcPktProcEtmV4I::iPktInvalidCfg(const uint8_t lastByte)
336 {
337 m_curr_packet.updateErrType(ETM4_PKT_I_RESERVED_CFG, lastByte); // swap type for err type
338 throw ocsdError(OCSD_ERR_SEV_ERROR, OCSD_ERR_INVALID_PCKT_HDR, m_packet_index, m_config.getTraceID());
339 }
340
iPktExtension(const uint8_t lastByte)341 void TrcPktProcEtmV4I::iPktExtension(const uint8_t lastByte)
342 {
343 if(m_currPacketData.size() == 2)
344 {
345 // not sync and not next by 0x00 - not sync sequence
346 if(!m_is_sync && (lastByte != 0x00))
347 {
348 m_pIPktFn = &TrcPktProcEtmV4I::iNotSync;
349 m_curr_packet.type = ETM4_PKT_I_NOTSYNC;
350 return;
351 }
352
353 switch(lastByte)
354 {
355 case 0x03: // discard packet.
356 m_curr_packet.type = ETM4_PKT_I_DISCARD;
357 m_process_state = SEND_PKT;
358 break;
359
360 case 0x05:
361 m_curr_packet.type = ETM4_PKT_I_OVERFLOW;
362 m_process_state = SEND_PKT;
363 break;
364
365 case 0x00:
366 m_curr_packet.type = ETM4_PKT_I_ASYNC;
367 m_pIPktFn = &TrcPktProcEtmV4I::iPktASync; // handle subsequent bytes as async
368 break;
369
370 default:
371 m_curr_packet.err_type = m_curr_packet.type;
372 m_curr_packet.type = ETM4_PKT_I_BAD_SEQUENCE;
373 m_process_state = SEND_PKT;
374 break;
375 }
376 }
377 }
378
iPktASync(const uint8_t lastByte)379 void TrcPktProcEtmV4I::iPktASync(const uint8_t lastByte)
380 {
381 if(lastByte != 0x00)
382 {
383 // not sync and not next by 0x00 - not sync sequence if < 12
384 if(!m_is_sync && m_currPacketData.size() != 12)
385 {
386 m_pIPktFn = &TrcPktProcEtmV4I::iNotSync;
387 m_curr_packet.type = ETM4_PKT_I_NOTSYNC;
388 return;
389 }
390
391 // 12 bytes and not valid sync sequence - not possible even if not synced
392 m_process_state = SEND_PKT;
393 if((m_currPacketData.size() != 12) || (lastByte != 0x80))
394 {
395 m_curr_packet.type = ETM4_PKT_I_BAD_SEQUENCE;
396 m_curr_packet.err_type = ETM4_PKT_I_ASYNC;
397 }
398 else
399 m_is_sync = true; // found a sync packet, mark decoder as synchronised.
400 }
401 else if(m_currPacketData.size() == 12)
402 {
403 if(!m_is_sync)
404 {
405 // if we are not yet synced then ignore extra leading 0x00.
406 m_dump_unsynced_bytes = 1;
407 m_process_state = SEND_UNSYNCED;
408 }
409 else
410 {
411 // bad periodic ASYNC sequence.
412 m_curr_packet.type = ETM4_PKT_I_BAD_SEQUENCE;
413 m_curr_packet.err_type = ETM4_PKT_I_ASYNC;
414 m_process_state = SEND_PKT;
415 }
416 }
417 }
418
iPktTraceInfo(const uint8_t lastByte)419 void TrcPktProcEtmV4I::iPktTraceInfo(const uint8_t lastByte)
420 {
421 if(m_currPacketData.size() == 1) // header
422 {
423 //clear flags
424 m_tinfo_sections.sectFlags = 0; // mark all sections as incomplete.
425 m_tinfo_sections.ctrlBytes = 1; // assume only a single control section byte for now
426
427 }
428 else if(m_currPacketData.size() == 2) // first payload control byte
429 {
430 // figure out which sections are absent and set to true - opposite of bitfeild in byte;
431 m_tinfo_sections.sectFlags = (~lastByte) & TINFO_ALL_SECT;
432
433 // see if there is an extended control section, otherwise this byte is it.
434 if((lastByte & 0x80) == 0x0)
435 m_tinfo_sections.sectFlags |= TINFO_CTRL;
436
437 }
438 else
439 {
440 if(!(m_tinfo_sections.sectFlags & TINFO_CTRL))
441 {
442 m_tinfo_sections.sectFlags |= (lastByte & 0x80) ? 0 : TINFO_CTRL;
443 m_tinfo_sections.ctrlBytes++;
444 }
445 else if(!(m_tinfo_sections.sectFlags & TINFO_INFO_SECT))
446 m_tinfo_sections.sectFlags |= (lastByte & 0x80) ? 0 : TINFO_INFO_SECT;
447 else if(!(m_tinfo_sections.sectFlags & TINFO_KEY_SECT))
448 m_tinfo_sections.sectFlags |= (lastByte & 0x80) ? 0 : TINFO_KEY_SECT;
449 else if(!(m_tinfo_sections.sectFlags & TINFO_SPEC_SECT))
450 m_tinfo_sections.sectFlags |= (lastByte & 0x80) ? 0 : TINFO_SPEC_SECT;
451 else if(!(m_tinfo_sections.sectFlags & TINFO_CYCT_SECT))
452 m_tinfo_sections.sectFlags |= (lastByte & 0x80) ? 0 : TINFO_CYCT_SECT;
453 else if (!(m_tinfo_sections.sectFlags & TINFO_WNDW_SECT))
454 m_tinfo_sections.sectFlags |= (lastByte & 0x80) ? 0 : TINFO_WNDW_SECT;
455 }
456
457 // all sections accounted for?
458 if(m_tinfo_sections.sectFlags == TINFO_ALL)
459 {
460 // index of first section is number of payload control bytes + 1 for header byte
461 unsigned idx = m_tinfo_sections.ctrlBytes + 1;
462 uint32_t fieldVal = 0;
463 uint8_t presSect = m_currPacketData[1] & TINFO_ALL_SECT; // first payload control byte
464
465 m_curr_packet.clearTraceInfo();
466
467 if((presSect & TINFO_INFO_SECT) && (idx < m_currPacketData.size()))
468 {
469 idx += extractContField(m_currPacketData,idx,fieldVal);
470 m_curr_packet.setTraceInfo(fieldVal);
471 }
472 if((presSect & TINFO_KEY_SECT) && (idx < m_currPacketData.size()))
473 {
474 idx += extractContField(m_currPacketData,idx,fieldVal);
475 m_curr_packet.setTraceInfoKey(fieldVal);
476 }
477 if((presSect & TINFO_SPEC_SECT) && (idx < m_currPacketData.size()))
478 {
479 idx += extractContField(m_currPacketData,idx,fieldVal);
480 m_curr_packet.setTraceInfoSpec(fieldVal);
481 }
482 if((presSect & TINFO_CYCT_SECT) && (idx < m_currPacketData.size()))
483 {
484 idx += extractContField(m_currPacketData,idx,fieldVal);
485 m_curr_packet.setTraceInfoCyct(fieldVal);
486 }
487 if ((presSect & TINFO_WNDW_SECT) && (idx < m_currPacketData.size()))
488 {
489 idx += extractContField(m_currPacketData, idx, fieldVal);
490 /* Trace commit window unsupported in current ETE versions */
491 }
492 m_process_state = SEND_PKT;
493 m_first_trace_info = true;
494 }
495
496 }
497
iPktTimestamp(const uint8_t lastByte)498 void TrcPktProcEtmV4I::iPktTimestamp(const uint8_t lastByte)
499 {
500 // process the header byte
501 if(m_currPacketData.size() == 1)
502 {
503 m_ccount_done = (bool)((lastByte & 0x1) == 0); // 0 = not present
504 m_ts_done = false;
505 m_ts_bytes = 0;
506 }
507 else
508 {
509 if(!m_ts_done)
510 {
511 m_ts_bytes++;
512 m_ts_done = (m_ts_bytes == 9) || ((lastByte & 0x80) == 0);
513 }
514 else if(!m_ccount_done)
515 {
516 m_ccount_done = (bool)((lastByte & 0x80) == 0);
517 // TBD: check for oorange ccount - bad packet.
518 }
519 }
520
521 if(m_ts_done && m_ccount_done)
522 {
523 int idx = 1;
524 uint64_t tsVal;
525 int ts_bytes = extractTSField64(m_currPacketData, idx, tsVal);
526 int ts_bits;
527
528 // if ts_bytes 8 or less, then cont bits on each byte, otherwise full 64 bit value for 9 bytes
529 ts_bits = ts_bytes < 9 ? ts_bytes * 7 : 64;
530
531 if(!m_curr_packet.pkt_valid.bits.ts_valid && m_first_trace_info)
532 ts_bits = 64; // after trace info, missing bits are all 0.
533
534 m_curr_packet.setTS(tsVal,(uint8_t)ts_bits);
535
536 if((m_currPacketData[0] & 0x1) == 0x1)
537 {
538 uint32_t countVal, countMask;
539
540 idx += ts_bytes;
541 extractContField(m_currPacketData, idx, countVal, 3); // only 3 possible count bytes.
542 countMask = (((uint32_t)1UL << m_config.ccSize()) - 1); // mask of the CC size
543 countVal &= countMask;
544 m_curr_packet.setCycleCount(countVal);
545 }
546
547 m_process_state = SEND_PKT;
548 }
549 }
550
iPktException(const uint8_t lastByte)551 void TrcPktProcEtmV4I::iPktException(const uint8_t lastByte)
552 {
553 uint16_t excep_type = 0;
554
555 switch(m_currPacketData.size())
556 {
557 case 1: m_excep_size = 3; break;
558 case 2: if((lastByte & 0x80) == 0x00)
559 m_excep_size = 2;
560 // ETE exception reset or trans failed
561 if (m_config.MajVersion() >= 0x5)
562 {
563 excep_type = (m_currPacketData[1] >> 1) & 0x1F;
564 if ((excep_type == 0x0) || (excep_type == 0x18))
565 m_excep_size = 3;
566 }
567 break;
568 }
569
570 if(m_currPacketData.size() == (unsigned)m_excep_size)
571 {
572 excep_type = (m_currPacketData[1] >> 1) & 0x1F;
573 uint8_t addr_interp = (m_currPacketData[1] & 0x40) >> 5 | (m_currPacketData[1] & 0x1);
574 uint8_t m_fault_pending = 0;
575 uint8_t m_type = (m_config.coreProfile() == profile_CortexM) ? 1 : 0;
576
577 // extended exception packet (probably M class);
578 if(m_currPacketData[1] & 0x80)
579 {
580 excep_type |= ((uint16_t)m_currPacketData[2] & 0x1F) << 5;
581 m_fault_pending = (m_currPacketData[2] >> 5) & 0x1;
582 }
583 m_curr_packet.setExceptionInfo(excep_type,addr_interp,m_fault_pending, m_type);
584 m_process_state = SEND_PKT;
585
586 // ETE exception reset or trans failed
587 if (m_config.MajVersion() >= 0x5)
588 {
589 if ((excep_type == 0x0) || (excep_type == 0x18))
590 {
591 m_curr_packet.set64BitAddress(0, 0);
592 if (excep_type == 0x18)
593 m_curr_packet.setType(ETE_PKT_I_TRANS_FAIL);
594 else
595 m_curr_packet.setType(ETE_PKT_I_PE_RESET);
596 }
597 }
598 // allow the standard address packet handlers to process the address packet field for the exception.
599 }
600 }
601
iPktCycleCntF123(const uint8_t lastByte)602 void TrcPktProcEtmV4I::iPktCycleCntF123(const uint8_t lastByte)
603 {
604 ocsd_etmv4_i_pkt_type format = m_curr_packet.type;
605
606 if( m_currPacketData.size() == 1)
607 {
608 m_count_done = m_commit_done = false;
609 m_has_count = true;
610
611 if(format == ETM4_PKT_I_CCNT_F3)
612 {
613 // no commit section for TRCIDR0.COMMOPT == 1
614 if(!m_config.commitOpt1())
615 {
616 m_curr_packet.setCommitElements(((lastByte >> 2) & 0x3) + 1);
617 }
618 // TBD: warning of non-valid CC threshold here?
619 m_curr_packet.setCycleCount(m_curr_packet.getCCThreshold() + (lastByte & 0x3));
620 m_process_state = SEND_PKT;
621 }
622 else if(format == ETM4_PKT_I_CCNT_F1)
623 {
624 if((lastByte & 0x1) == 0x1)
625 {
626 m_has_count = false;
627 m_count_done = true;
628 }
629
630 // no commit section for TRCIDR0.COMMOPT == 1
631 if(m_config.commitOpt1())
632 m_commit_done = true;
633 }
634 }
635 else if((format == ETM4_PKT_I_CCNT_F2) && ( m_currPacketData.size() == 2))
636 {
637 int commit_offset = ((lastByte & 0x1) == 0x1) ? ((int)m_config.MaxSpecDepth() - 15) : 1;
638 int commit_elements = ((lastByte >> 4) & 0xF);
639 commit_elements += commit_offset;
640
641 // TBD: warning if commit elements < 0?
642
643 m_curr_packet.setCycleCount(m_curr_packet.getCCThreshold() + (lastByte & 0xF));
644 m_curr_packet.setCommitElements(commit_elements);
645 m_process_state = SEND_PKT;
646 }
647 else
648 {
649 // F1 and size 2 or more
650 if(!m_commit_done)
651 m_commit_done = ((lastByte & 0x80) == 0x00);
652 else if(!m_count_done)
653 m_count_done = ((lastByte & 0x80) == 0x00);
654 }
655
656 if((format == ETM4_PKT_I_CCNT_F1) && m_commit_done && m_count_done)
657 {
658 int idx = 1; // index into buffer for payload data.
659 uint32_t field_value = 0;
660 // no commit section for TRCIDR0.COMMOPT == 1
661 if(!m_config.commitOpt1())
662 {
663 idx += extractContField(m_currPacketData,idx,field_value);
664 m_curr_packet.setCommitElements(field_value);
665 }
666 if (m_has_count)
667 {
668 extractContField(m_currPacketData, idx, field_value, 3);
669 m_curr_packet.setCycleCount(field_value + m_curr_packet.getCCThreshold());
670 }
671 else
672 m_curr_packet.setCycleCount(0); /* unknown CC marked as 0 after overflow */
673 m_process_state = SEND_PKT;
674 }
675 }
676
iPktSpeclRes(const uint8_t lastByte)677 void TrcPktProcEtmV4I::iPktSpeclRes(const uint8_t lastByte)
678 {
679 if(m_currPacketData.size() == 1)
680 {
681 switch(m_curr_packet.getType())
682 {
683 case ETM4_PKT_I_MISPREDICT:
684 case ETM4_PKT_I_CANCEL_F2:
685 switch(lastByte & 0x3)
686 {
687 case 0x1: m_curr_packet.setAtomPacket(ATOM_PATTERN, 0x1, 1); break; // E
688 case 0x2: m_curr_packet.setAtomPacket(ATOM_PATTERN, 0x3, 2); break; // EE
689 case 0x3: m_curr_packet.setAtomPacket(ATOM_PATTERN, 0x0, 1); break; // N
690 }
691 if (m_curr_packet.getType() == ETM4_PKT_I_CANCEL_F2)
692 m_curr_packet.setCancelElements(1);
693 else
694 m_curr_packet.setCancelElements(0);
695 m_process_state = SEND_PKT;
696 break;
697
698 case ETM4_PKT_I_CANCEL_F3:
699 if(lastByte & 0x1)
700 m_curr_packet.setAtomPacket(ATOM_PATTERN, 0x1, 1); // E
701 m_curr_packet.setCancelElements(((lastByte >> 1) & 0x3) + 2);
702 m_process_state = SEND_PKT;
703 break;
704 }
705 }
706 else
707 {
708 if((lastByte & 0x80) == 0x00)
709 {
710 uint32_t field_val = 0;
711 extractContField(m_currPacketData,1,field_val);
712 if(m_curr_packet.getType() == ETM4_PKT_I_COMMIT)
713 m_curr_packet.setCommitElements(field_val);
714 else
715 m_curr_packet.setCancelElements(field_val);
716 m_process_state = SEND_PKT;
717 }
718 }
719 }
720
iPktCondInstr(const uint8_t lastByte)721 void TrcPktProcEtmV4I::iPktCondInstr(const uint8_t lastByte)
722 {
723 bool bF1Done = false;
724
725 if(m_currPacketData.size() == 1)
726 {
727 if(m_curr_packet.getType() == ETM4_PKT_I_COND_I_F2)
728 {
729 m_curr_packet.setCondIF2(lastByte & 0x3);
730 m_process_state = SEND_PKT;
731 }
732
733 }
734 else if(m_currPacketData.size() == 2)
735 {
736 if(m_curr_packet.getType() == ETM4_PKT_I_COND_I_F3) // f3 two bytes long
737 {
738 uint8_t num_c_elem = ((lastByte >> 1) & 0x3F) + (lastByte & 0x1);
739 m_curr_packet.setCondIF3(num_c_elem,(bool)((lastByte & 0x1) == 0x1));
740 // TBD: check for 0 num_c_elem in here.
741 m_process_state = SEND_PKT;
742 }
743 else
744 {
745 bF1Done = ((lastByte & 0x80) == 0x00);
746 }
747 }
748 else
749 {
750 bF1Done = ((lastByte & 0x80) == 0x00);
751 }
752
753 if(bF1Done)
754 {
755 uint32_t cond_key = 0;
756 extractContField(m_currPacketData, 1, cond_key);
757 m_process_state = SEND_PKT;
758 }
759 }
760
iPktCondResult(const uint8_t lastByte)761 void TrcPktProcEtmV4I::iPktCondResult(const uint8_t lastByte)
762 {
763 if(m_currPacketData.size() == 1)
764 {
765 m_F1P1_done = false; // F1 payload 1 done
766 m_F1P2_done = false; // F1 payload 2 done
767 m_F1has_P2 = false; // F1 has a payload 2
768
769 switch(m_curr_packet.getType())
770 {
771 case ETM4_PKT_I_COND_RES_F1:
772
773 m_F1has_P2 = true;
774 if((lastByte & 0xFC) == 0x6C)// only one payload set
775 {
776 m_F1P2_done = true;
777 m_F1has_P2 = false;
778 }
779 break;
780
781 case ETM4_PKT_I_COND_RES_F2:
782 m_curr_packet.setCondRF2((lastByte & 0x4) ? 2 : 1, lastByte & 0x3);
783 m_process_state = SEND_PKT;
784 break;
785
786 case ETM4_PKT_I_COND_RES_F3:
787 break;
788
789 case ETM4_PKT_I_COND_RES_F4:
790 m_curr_packet.setCondRF4(lastByte & 0x3);
791 m_process_state = SEND_PKT;
792 break;
793 }
794 }
795 else if((m_curr_packet.getType() == ETM4_PKT_I_COND_RES_F3) && (m_currPacketData.size() == 2))
796 {
797 // 2nd F3 packet
798 uint16_t f3_tokens = 0;
799 f3_tokens = (uint16_t)m_currPacketData[1];
800 f3_tokens |= ((uint16_t)m_currPacketData[0] & 0xf) << 8;
801 m_curr_packet.setCondRF3(f3_tokens);
802 m_process_state = SEND_PKT;
803 }
804 else // !first packet - F1
805 {
806 if(!m_F1P1_done)
807 m_F1P1_done = ((lastByte & 0x80) == 0x00);
808 else if(!m_F1P2_done)
809 m_F1P2_done = ((lastByte & 0x80) == 0x00);
810
811 if(m_F1P1_done && m_F1P2_done)
812 {
813 int st_idx = 1;
814 uint32_t key[2];
815 uint8_t result[2];
816 uint8_t CI[2];
817
818 st_idx+= extractCondResult(m_currPacketData,st_idx,key[0],result[0]);
819 CI[0] = m_currPacketData[0] & 0x1;
820 if(m_F1has_P2) // 2nd payload?
821 {
822 extractCondResult(m_currPacketData,st_idx,key[1],result[1]);
823 CI[1] = (m_currPacketData[0] >> 1) & 0x1;
824 }
825 m_curr_packet.setCondRF1(key,result,CI,m_F1has_P2);
826 m_process_state = SEND_PKT;
827 }
828 }
829 }
830
iPktContext(const uint8_t lastByte)831 void TrcPktProcEtmV4I::iPktContext(const uint8_t lastByte)
832 {
833 bool bSendPacket = false;
834
835 if(m_currPacketData.size() == 1)
836 {
837 if((lastByte & 0x1) == 0)
838 {
839 m_curr_packet.setContextInfo(false); // no update context packet (ctxt same as last time).
840 m_process_state = SEND_PKT;
841 }
842 }
843 else if(m_currPacketData.size() == 2)
844 {
845 if((lastByte & 0xC0) == 0) // no VMID or CID
846 {
847 bSendPacket = true;
848 }
849 else
850 {
851 m_vmidBytes = ((lastByte & 0x40) == 0x40) ? (m_config.vmidSize()/8) : 0;
852 m_ctxtidBytes = ((lastByte & 0x80) == 0x80) ? (m_config.cidSize()/8) : 0;
853 }
854 }
855 else // 3rd byte onwards
856 {
857 if(m_vmidBytes > 0)
858 m_vmidBytes--;
859 else if(m_ctxtidBytes > 0)
860 m_ctxtidBytes--;
861
862 if((m_ctxtidBytes == 0) && (m_vmidBytes == 0))
863 bSendPacket = true;
864 }
865
866 if(bSendPacket)
867 {
868 extractAndSetContextInfo(m_currPacketData,1);
869 m_process_state = SEND_PKT;
870 }
871 }
872
extractAndSetContextInfo(const std::vector<uint8_t> & buffer,const int st_idx)873 void TrcPktProcEtmV4I::extractAndSetContextInfo(const std::vector<uint8_t> &buffer, const int st_idx)
874 {
875 // on input, buffer index points at the info byte - always present
876 uint8_t infoByte = m_currPacketData[st_idx];
877
878 m_curr_packet.setContextInfo(true, (infoByte & 0x3), (infoByte >> 5) & 0x1, (infoByte >> 4) & 0x1, (infoByte >> 3) & 0x1);
879
880 // see if there are VMID and CID bytes, and how many.
881 int nVMID_bytes = ((infoByte & 0x40) == 0x40) ? (m_config.vmidSize()/8) : 0;
882 int nCtxtID_bytes = ((infoByte & 0x80) == 0x80) ? (m_config.cidSize()/8) : 0;
883
884 // extract any VMID and CID
885 int payload_idx = st_idx+1;
886 if(nVMID_bytes)
887 {
888 uint32_t VMID = 0;
889 for(int i = 0; i < nVMID_bytes; i++)
890 {
891 VMID |= ((uint32_t)m_currPacketData[i+payload_idx] << i*8);
892 }
893 payload_idx += nVMID_bytes;
894 m_curr_packet.setContextVMID(VMID);
895 }
896
897 if(nCtxtID_bytes)
898 {
899 uint32_t CID = 0;
900 for(int i = 0; i < nCtxtID_bytes; i++)
901 {
902 CID |= ((uint32_t)m_currPacketData[i+payload_idx] << i*8);
903 }
904 m_curr_packet.setContextCID(CID);
905 }
906 }
907
iPktAddrCtxt(const uint8_t lastByte)908 void TrcPktProcEtmV4I::iPktAddrCtxt(const uint8_t lastByte)
909 {
910 if( m_currPacketData.size() == 1)
911 {
912 m_addrIS = 0;
913 m_addrBytes = 4;
914 m_bAddr64bit = false;
915 m_vmidBytes = 0;
916 m_ctxtidBytes = 0;
917 m_bCtxtInfoDone = false;
918
919 switch(m_curr_packet.type)
920 {
921 case ETM4_PKT_I_ADDR_CTXT_L_32IS1:
922 m_addrIS = 1;
923 case ETM4_PKT_I_ADDR_CTXT_L_32IS0:
924 break;
925
926 case ETM4_PKT_I_ADDR_CTXT_L_64IS1:
927 m_addrIS = 1;
928 case ETM4_PKT_I_ADDR_CTXT_L_64IS0:
929 m_addrBytes = 8;
930 m_bAddr64bit = true;
931 break;
932 }
933 }
934 else
935 {
936 if(m_addrBytes == 0)
937 {
938 if(m_bCtxtInfoDone == false)
939 {
940 m_bCtxtInfoDone = true;
941 m_vmidBytes = ((lastByte & 0x40) == 0x40) ? (m_config.vmidSize()/8) : 0;
942 m_ctxtidBytes = ((lastByte & 0x80) == 0x80) ? (m_config.cidSize()/8) : 0;
943 }
944 else
945 {
946 if( m_vmidBytes > 0)
947 m_vmidBytes--;
948 else if(m_ctxtidBytes > 0)
949 m_ctxtidBytes--;
950 }
951 }
952 else
953 m_addrBytes--;
954
955 if((m_addrBytes == 0) && m_bCtxtInfoDone && (m_vmidBytes == 0) && (m_ctxtidBytes == 0))
956 {
957 int st_idx = 1;
958 if(m_bAddr64bit)
959 {
960 uint64_t val64;
961 st_idx+=extract64BitLongAddr(m_currPacketData,st_idx,m_addrIS,val64);
962 m_curr_packet.set64BitAddress(val64,m_addrIS);
963 }
964 else
965 {
966 uint32_t val32;
967 st_idx+=extract32BitLongAddr(m_currPacketData,st_idx,m_addrIS,val32);
968 m_curr_packet.set32BitAddress(val32,m_addrIS);
969 }
970 extractAndSetContextInfo(m_currPacketData,st_idx);
971 m_process_state = SEND_PKT;
972 }
973 }
974 }
975
iPktShortAddr(const uint8_t lastByte)976 void TrcPktProcEtmV4I::iPktShortAddr(const uint8_t lastByte)
977 {
978 if (m_currPacketData.size() == 1)
979 {
980 m_addr_done = false;
981 m_addrIS = 0;
982 if ((lastByte == ETM4_PKT_I_ADDR_S_IS1) ||
983 (lastByte == ETE_PKT_I_SRC_ADDR_S_IS1))
984 m_addrIS = 1;
985 }
986 else if(!m_addr_done)
987 {
988 m_addr_done = (m_currPacketData.size() == 3) || ((lastByte & 0x80) == 0x00);
989 }
990
991 if(m_addr_done)
992 {
993 uint32_t addr_val = 0;
994 int bits = 0;
995
996 extractShortAddr(m_currPacketData,1,m_addrIS,addr_val,bits);
997 m_curr_packet.updateShortAddress(addr_val,m_addrIS,(uint8_t)bits);
998 m_process_state = SEND_PKT;
999 }
1000 }
1001
extractShortAddr(const std::vector<uint8_t> & buffer,const int st_idx,const uint8_t IS,uint32_t & value,int & bits)1002 int TrcPktProcEtmV4I::extractShortAddr(const std::vector<uint8_t> &buffer, const int st_idx, const uint8_t IS, uint32_t &value, int &bits)
1003 {
1004 int IS_shift = (IS == 0) ? 2 : 1;
1005 int idx = 0;
1006
1007 bits = 7; // at least 7 bits
1008 value = 0;
1009 value |= ((uint32_t)(buffer[st_idx+idx] & 0x7F)) << IS_shift;
1010
1011 if(m_currPacketData[st_idx+idx] & 0x80)
1012 {
1013 idx++;
1014 value |= ((uint32_t)m_currPacketData[st_idx+idx]) << (7 + IS_shift);
1015 bits += 8;
1016 }
1017 idx++;
1018 bits += IS_shift;
1019 return idx;
1020 }
1021
iPktLongAddr(const uint8_t lastByte)1022 void TrcPktProcEtmV4I::iPktLongAddr(const uint8_t lastByte)
1023 {
1024 if(m_currPacketData.size() == 1)
1025 {
1026 // init the intra-byte data
1027 m_addrIS = 0;
1028 m_bAddr64bit = false;
1029 m_addrBytes = 4;
1030
1031 switch(m_curr_packet.type)
1032 {
1033 case ETM4_PKT_I_ADDR_L_32IS1:
1034 case ETE_PKT_I_SRC_ADDR_L_32IS1:
1035 m_addrIS = 1;
1036 case ETM4_PKT_I_ADDR_L_32IS0:
1037 case ETE_PKT_I_SRC_ADDR_L_32IS0:
1038 m_addrBytes = 4;
1039 break;
1040
1041 case ETM4_PKT_I_ADDR_L_64IS1:
1042 case ETE_PKT_I_SRC_ADDR_L_64IS1:
1043 m_addrIS = 1;
1044 case ETM4_PKT_I_ADDR_L_64IS0:
1045 case ETE_PKT_I_SRC_ADDR_L_64IS0:
1046 m_addrBytes = 8;
1047 m_bAddr64bit = true;
1048 break;
1049 }
1050 }
1051 if(m_currPacketData.size() == (unsigned)(1+m_addrBytes))
1052 {
1053 int st_idx = 1;
1054 if(m_bAddr64bit)
1055 {
1056 uint64_t val64;
1057 st_idx+=extract64BitLongAddr(m_currPacketData,st_idx,m_addrIS,val64);
1058 m_curr_packet.set64BitAddress(val64,m_addrIS);
1059 }
1060 else
1061 {
1062 uint32_t val32;
1063 st_idx+=extract32BitLongAddr(m_currPacketData,st_idx,m_addrIS,val32);
1064 m_curr_packet.set32BitAddress(val32,m_addrIS);
1065 }
1066 m_process_state = SEND_PKT;
1067 }
1068 }
1069
iPktQ(const uint8_t lastByte)1070 void TrcPktProcEtmV4I::iPktQ(const uint8_t lastByte)
1071 {
1072 if(m_currPacketData.size() == 1)
1073 {
1074 m_Q_type = lastByte & 0xF;
1075
1076 m_addrBytes = 0;
1077 m_count_done = false;
1078 m_has_addr = false;
1079 m_addr_short = true;
1080 m_addr_match = false;
1081 m_addrIS = 1;
1082 m_QE = 0;
1083
1084 switch(m_Q_type)
1085 {
1086 // count only - implied address.
1087 case 0x0:
1088 case 0x1:
1089 case 0x2:
1090 m_addr_match = true;
1091 m_has_addr = true;
1092 m_QE = m_Q_type & 0x3;
1093 case 0xC:
1094 break;
1095
1096 // count + short address
1097 case 0x5:
1098 m_addrIS = 0;
1099 case 0x6:
1100 m_has_addr = true;
1101 m_addrBytes = 2; // short IS0/1
1102 break;
1103
1104 // count + long address
1105 case 0xA:
1106 m_addrIS = 0;
1107 case 0xB:
1108 m_has_addr = true;
1109 m_addr_short = false;
1110 m_addrBytes = 4; // long IS0/1
1111 break;
1112
1113 // no count, no address
1114 case 0xF:
1115 m_count_done = true;
1116 break;
1117
1118 // reserved values 0x3, 0x4, 0x7, 0x8, 0x9, 0xD, 0xE
1119 default:
1120 m_curr_packet.err_type = m_curr_packet.type;
1121 m_curr_packet.type = ETM4_PKT_I_BAD_SEQUENCE;
1122 m_process_state = SEND_PKT;
1123 break;
1124 }
1125 }
1126 else
1127 {
1128 if(m_addrBytes > 0)
1129 {
1130 if(m_addr_short && m_addrBytes == 2) // short
1131 {
1132 if((lastByte & 0x80) == 0x00)
1133 m_addrBytes--; // short version can have just single byte.
1134 }
1135 m_addrBytes--;
1136 }
1137 else if(!m_count_done)
1138 {
1139 m_count_done = ((lastByte & 0x80) == 0x00);
1140 }
1141 }
1142
1143 if(((m_addrBytes == 0) && m_count_done))
1144 {
1145 int idx = 1; // move past the header
1146 int bits = 0;
1147 uint32_t q_addr;
1148 uint32_t q_count;
1149
1150 if(m_has_addr)
1151 {
1152 if(m_addr_match)
1153 {
1154 m_curr_packet.setAddressExactMatch(m_QE);
1155 }
1156 else if(m_addr_short)
1157 {
1158 idx+=extractShortAddr(m_currPacketData,idx,m_addrIS,q_addr,bits);
1159 m_curr_packet.updateShortAddress(q_addr,m_addrIS,(uint8_t)bits);
1160 }
1161 else
1162 {
1163 idx+=extract32BitLongAddr(m_currPacketData,idx,m_addrIS,q_addr);
1164 m_curr_packet.set32BitAddress(q_addr,m_addrIS);
1165 }
1166 }
1167
1168 if(m_Q_type != 0xF)
1169 {
1170 extractContField(m_currPacketData,idx,q_count);
1171 m_curr_packet.setQType(true,q_count,m_has_addr,m_addr_match,m_Q_type);
1172 }
1173 else
1174 {
1175 m_curr_packet.setQType(false,0,false,false,0xF);
1176 }
1177 m_process_state = SEND_PKT;
1178 }
1179
1180 }
1181
iAtom(const uint8_t lastByte)1182 void TrcPktProcEtmV4I::iAtom(const uint8_t lastByte)
1183 {
1184 // patterns lsbit = oldest atom, ms bit = newest.
1185 static const uint32_t f4_patterns[] = {
1186 0xE, // EEEN
1187 0x0, // NNNN
1188 0xA, // ENEN
1189 0x5 // NENE
1190 };
1191
1192 uint8_t pattIdx = 0, pattCount = 0;
1193 uint32_t pattern;
1194
1195 // atom packets are single byte, no payload.
1196 switch(m_curr_packet.type)
1197 {
1198 case ETM4_PKT_I_ATOM_F1:
1199 m_curr_packet.setAtomPacket(ATOM_PATTERN,(lastByte & 0x1), 1); // 1xE or N
1200 break;
1201
1202 case ETM4_PKT_I_ATOM_F2:
1203 m_curr_packet.setAtomPacket(ATOM_PATTERN,(lastByte & 0x3), 2); // 2x (E or N)
1204 break;
1205
1206 case ETM4_PKT_I_ATOM_F3:
1207 m_curr_packet.setAtomPacket(ATOM_PATTERN,(lastByte & 0x7), 3); // 3x (E or N)
1208 break;
1209
1210 case ETM4_PKT_I_ATOM_F4:
1211 m_curr_packet.setAtomPacket(ATOM_PATTERN,f4_patterns[(lastByte & 0x3)], 4); // 4 atom pattern
1212 break;
1213
1214 case ETM4_PKT_I_ATOM_F5:
1215 pattIdx = ((lastByte & 0x20) >> 3) | (lastByte & 0x3);
1216 switch(pattIdx)
1217 {
1218 case 5: // 0b101
1219 m_curr_packet.setAtomPacket(ATOM_PATTERN,0x1E, 5); // 5 atom pattern EEEEN
1220 break;
1221
1222 case 1: // 0b001
1223 m_curr_packet.setAtomPacket(ATOM_PATTERN,0x00, 5); // 5 atom pattern NNNNN
1224 break;
1225
1226 case 2: //0b010
1227 m_curr_packet.setAtomPacket(ATOM_PATTERN,0x0A, 5); // 5 atom pattern NENEN
1228 break;
1229
1230 case 3: //0b011
1231 m_curr_packet.setAtomPacket(ATOM_PATTERN,0x15, 5); // 5 atom pattern ENENE
1232 break;
1233
1234 default:
1235 // TBD: warn about invalid pattern in here.
1236 break;
1237 }
1238 break;
1239
1240 case ETM4_PKT_I_ATOM_F6:
1241 pattCount = (lastByte & 0x1F) + 3; // count of E's
1242 // TBD: check 23 or less at this point?
1243 pattern = ((uint32_t)0x1 << pattCount) - 1; // set pattern to string of E's
1244 if((lastByte & 0x20) == 0x00) // last atom is E?
1245 pattern |= ((uint32_t)0x1 << pattCount);
1246 m_curr_packet.setAtomPacket(ATOM_PATTERN,pattern, pattCount+1);
1247 break;
1248 }
1249
1250 m_process_state = SEND_PKT;
1251 }
1252
iPktITE(const uint8_t)1253 void TrcPktProcEtmV4I::iPktITE(const uint8_t /* lastByte */)
1254 {
1255 uint64_t value;
1256 int shift = 0;
1257
1258 /* packet is always 10 bytes, Header, EL info byte, 8 bytes payload */
1259 if (m_currPacketData.size() == 10) {
1260 value = 0;
1261 for (int i = 2; i < 10; i++) {
1262 value |= ((uint64_t)m_currPacketData[i]) << shift;
1263 shift += 8;
1264 }
1265 m_curr_packet.setITE(m_currPacketData[1], value);
1266 m_process_state = SEND_PKT;
1267 }
1268 }
1269
1270 // header byte processing is table driven.
BuildIPacketTable()1271 void TrcPktProcEtmV4I::BuildIPacketTable()
1272 {
1273 // initialise everything as reserved.
1274 for(int i = 0; i < 256; i++)
1275 {
1276 m_i_table[i].pkt_type = ETM4_PKT_I_RESERVED;
1277 m_i_table[i].pptkFn = &TrcPktProcEtmV4I::iPktReserved;
1278 }
1279
1280 // 0x00 - extension
1281 m_i_table[0x00].pkt_type = ETM4_PKT_I_EXTENSION;
1282 m_i_table[0x00].pptkFn = &TrcPktProcEtmV4I::iPktExtension;
1283
1284 // 0x01 - Trace info
1285 m_i_table[0x01].pkt_type = ETM4_PKT_I_TRACE_INFO;
1286 m_i_table[0x01].pptkFn = &TrcPktProcEtmV4I::iPktTraceInfo;
1287
1288 // b0000001x - timestamp
1289 m_i_table[0x02].pkt_type = ETM4_PKT_I_TIMESTAMP;
1290 m_i_table[0x02].pptkFn = &TrcPktProcEtmV4I::iPktTimestamp;
1291 m_i_table[0x03].pkt_type = ETM4_PKT_I_TIMESTAMP;
1292 m_i_table[0x03].pptkFn = &TrcPktProcEtmV4I::iPktTimestamp;
1293
1294 // b0000 0100 - trace on
1295 m_i_table[0x04].pkt_type = ETM4_PKT_I_TRACE_ON;
1296 m_i_table[0x04].pptkFn = &TrcPktProcEtmV4I::iPktNoPayload;
1297
1298
1299 // b0000 0101 - Funct ret V8M
1300 m_i_table[0x05].pkt_type = ETM4_PKT_I_FUNC_RET;
1301 if ((m_config.coreProfile() == profile_CortexM) &&
1302 (OCSD_IS_V8_ARCH(m_config.archVersion())) &&
1303 (m_config.FullVersion() >= 0x42))
1304 {
1305 m_i_table[0x05].pptkFn = &TrcPktProcEtmV4I::iPktNoPayload;
1306 }
1307
1308 // b0000 0110 - exception
1309 m_i_table[0x06].pkt_type = ETM4_PKT_I_EXCEPT;
1310 m_i_table[0x06].pptkFn = &TrcPktProcEtmV4I::iPktException;
1311
1312 // b0000 0111 - exception return
1313 m_i_table[0x07].pkt_type = ETM4_PKT_I_EXCEPT_RTN;
1314 if (m_config.MajVersion() >= 0x5) // not valid for ETE
1315 {
1316 #ifdef ETE_TRACE_ERET_AS_IGNORE
1317 m_i_table[0x07].pkt_type = ETM4_PKT_I_IGNORE;
1318 m_i_table[0x07].pptkFn = &EtmV4IPktProcImpl::iPktNoPayload;
1319 #else
1320 m_i_table[0x07].pptkFn = &TrcPktProcEtmV4I::iPktInvalidCfg;
1321 #endif
1322 }
1323 else
1324 m_i_table[0x07].pptkFn = &TrcPktProcEtmV4I::iPktNoPayload;
1325
1326 // b00001010, b00001011 ETE TRANS packets
1327 // b00001001 - ETE sw instrumentation packet
1328 if (m_config.MajVersion() >= 0x5)
1329 {
1330 m_i_table[0x0A].pkt_type = ETE_PKT_I_TRANS_ST;
1331 m_i_table[0x0A].pptkFn = &TrcPktProcEtmV4I::iPktNoPayload;
1332
1333 m_i_table[0x0B].pkt_type = ETE_PKT_I_TRANS_COMMIT;
1334 m_i_table[0x0B].pptkFn = &TrcPktProcEtmV4I::iPktNoPayload;
1335
1336 // FEAT_ITE - sw instrumentation packet
1337 if (m_config.MinVersion() >= 0x3)
1338 {
1339 m_i_table[0x09].pkt_type = ETE_PKT_I_ITE;
1340 m_i_table[0x09].pptkFn = &TrcPktProcEtmV4I::iPktITE;
1341 }
1342 }
1343
1344 // b0000 110x - cycle count f2
1345 // b0000 111x - cycle count f1
1346 for(int i = 0; i < 4; i++)
1347 {
1348 m_i_table[0x0C+i].pkt_type = (i >= 2) ? ETM4_PKT_I_CCNT_F1 : ETM4_PKT_I_CCNT_F2;
1349 m_i_table[0x0C+i].pptkFn = &TrcPktProcEtmV4I::iPktCycleCntF123;
1350 }
1351
1352 // b0001 xxxx - cycle count f3
1353 for(int i = 0; i < 16; i++)
1354 {
1355 m_i_table[0x10+i].pkt_type = ETM4_PKT_I_CCNT_F3;
1356 m_i_table[0x10+i].pptkFn = &TrcPktProcEtmV4I::iPktCycleCntF123;
1357 }
1358
1359 // b0010 0xxx - NDSM
1360 for(int i = 0; i < 8; i++)
1361 {
1362 m_i_table[0x20 + i].pkt_type = ETM4_PKT_I_NUM_DS_MKR;
1363 if (m_config.enabledDataTrace())
1364 m_i_table[0x20+i].pptkFn = &TrcPktProcEtmV4I::iPktNoPayload;
1365 else
1366 m_i_table[0x20+i].pptkFn = &TrcPktProcEtmV4I::iPktInvalidCfg;
1367 }
1368
1369 // b0010 10xx, b0010 1100 - UDSM
1370 for(int i = 0; i < 5; i++)
1371 {
1372 m_i_table[0x28+i].pkt_type = ETM4_PKT_I_UNNUM_DS_MKR;
1373 if (m_config.enabledDataTrace())
1374 m_i_table[0x28+i].pptkFn = &TrcPktProcEtmV4I::iPktNoPayload;
1375 else
1376 m_i_table[0x28+i].pptkFn = &TrcPktProcEtmV4I::iPktInvalidCfg;
1377 }
1378
1379 // b0010 1101 - commit
1380 m_i_table[0x2D].pkt_type = ETM4_PKT_I_COMMIT;
1381 m_i_table[0x2D].pptkFn = &TrcPktProcEtmV4I::iPktSpeclRes;
1382
1383 // b0010 111x - cancel f1 (mis pred)
1384 m_i_table[0x2E].pkt_type = ETM4_PKT_I_CANCEL_F1;
1385 m_i_table[0x2E].pptkFn = &TrcPktProcEtmV4I::iPktSpeclRes;
1386 m_i_table[0x2F].pkt_type = ETM4_PKT_I_CANCEL_F1_MISPRED;
1387 m_i_table[0x2F].pptkFn = &TrcPktProcEtmV4I::iPktSpeclRes;
1388
1389 // b0011 00xx - mis predict
1390 for(int i = 0; i < 4; i++)
1391 {
1392 m_i_table[0x30+i].pkt_type = ETM4_PKT_I_MISPREDICT;
1393 m_i_table[0x30+i].pptkFn = &TrcPktProcEtmV4I::iPktSpeclRes;
1394 }
1395
1396 // b0011 01xx - cancel f2
1397 for(int i = 0; i < 4; i++)
1398 {
1399 m_i_table[0x34+i].pkt_type = ETM4_PKT_I_CANCEL_F2;
1400 m_i_table[0x34+i].pptkFn = &TrcPktProcEtmV4I::iPktSpeclRes;
1401 }
1402
1403 // b0011 1xxx - cancel f3
1404 for(int i = 0; i < 8; i++)
1405 {
1406 m_i_table[0x38+i].pkt_type = ETM4_PKT_I_CANCEL_F3;
1407 m_i_table[0x38+i].pptkFn = &TrcPktProcEtmV4I::iPktSpeclRes;
1408 }
1409
1410 bool bCondValid = m_config.hasCondTrace() && m_config.enabledCondITrace();
1411
1412 // b0100 000x, b0100 0010 - cond I f2
1413 for (int i = 0; i < 3; i++)
1414 {
1415 m_i_table[0x40 + i].pkt_type = ETM4_PKT_I_COND_I_F2;
1416 if (bCondValid)
1417 m_i_table[0x40 + i].pptkFn = &TrcPktProcEtmV4I::iPktCondInstr;
1418 else
1419 m_i_table[0x40 + i].pptkFn = &TrcPktProcEtmV4I::iPktInvalidCfg;
1420 }
1421
1422 // b0100 0011 - cond flush
1423 m_i_table[0x43].pkt_type = ETM4_PKT_I_COND_FLUSH;
1424 if (bCondValid)
1425 m_i_table[0x43].pptkFn = &TrcPktProcEtmV4I::iPktNoPayload;
1426 else
1427 m_i_table[0x43].pptkFn = &TrcPktProcEtmV4I::iPktInvalidCfg;
1428
1429 // b0100 010x, b0100 0110 - cond res f4
1430 for (int i = 0; i < 3; i++)
1431 {
1432 m_i_table[0x44 + i].pkt_type = ETM4_PKT_I_COND_RES_F4;
1433 if (bCondValid)
1434 m_i_table[0x44 + i].pptkFn = &TrcPktProcEtmV4I::iPktCondResult;
1435 else
1436 m_i_table[0x44 + i].pptkFn = &TrcPktProcEtmV4I::iPktInvalidCfg;
1437 }
1438
1439 // b0100 100x, b0100 0110 - cond res f2
1440 // b0100 110x, b0100 1110 - cond res f2
1441 for (int i = 0; i < 3; i++)
1442 {
1443 m_i_table[0x48 + i].pkt_type = ETM4_PKT_I_COND_RES_F2;
1444 if (bCondValid)
1445 m_i_table[0x48 + i].pptkFn = &TrcPktProcEtmV4I::iPktCondResult;
1446 else
1447 m_i_table[0x48 + i].pptkFn = &TrcPktProcEtmV4I::iPktInvalidCfg;
1448 }
1449 for (int i = 0; i < 3; i++)
1450 {
1451 m_i_table[0x4C + i].pkt_type = ETM4_PKT_I_COND_RES_F2;
1452 if (bCondValid)
1453 m_i_table[0x4C + i].pptkFn = &TrcPktProcEtmV4I::iPktCondResult;
1454 else
1455 m_i_table[0x4C + i].pptkFn = &TrcPktProcEtmV4I::iPktInvalidCfg;
1456 }
1457
1458 // b0101xxxx - cond res f3
1459 for (int i = 0; i < 16; i++)
1460 {
1461 m_i_table[0x50 + i].pkt_type = ETM4_PKT_I_COND_RES_F3;
1462 if (bCondValid)
1463 m_i_table[0x50 + i].pptkFn = &TrcPktProcEtmV4I::iPktCondResult;
1464 else
1465 m_i_table[0x50 + i].pptkFn = &TrcPktProcEtmV4I::iPktInvalidCfg;
1466 }
1467
1468 // b011010xx - cond res f1
1469 for (int i = 0; i < 4; i++)
1470 {
1471 m_i_table[0x68 + i].pkt_type = ETM4_PKT_I_COND_RES_F1;
1472 if (bCondValid)
1473 m_i_table[0x68 + i].pptkFn = &TrcPktProcEtmV4I::iPktCondResult;
1474 else
1475 m_i_table[0x68 + i].pptkFn = &TrcPktProcEtmV4I::iPktInvalidCfg;
1476 }
1477
1478 // b0110 1100 - cond instr f1
1479 m_i_table[0x6C].pkt_type = ETM4_PKT_I_COND_I_F1;
1480 if (bCondValid)
1481 m_i_table[0x6C].pptkFn = &TrcPktProcEtmV4I::iPktCondInstr;
1482 else
1483 m_i_table[0x6C].pptkFn = &TrcPktProcEtmV4I::iPktInvalidCfg;
1484
1485 // b0110 1101 - cond instr f3
1486 m_i_table[0x6D].pkt_type = ETM4_PKT_I_COND_I_F3;
1487 if (bCondValid)
1488 m_i_table[0x6D].pptkFn = &TrcPktProcEtmV4I::iPktCondInstr;
1489 else
1490 m_i_table[0x6D].pptkFn = &TrcPktProcEtmV4I::iPktInvalidCfg;
1491
1492 // b0110111x - cond res f1
1493 for (int i = 0; i < 2; i++)
1494 {
1495 // G++ cannot understand [0x6E+i] so change these round
1496 m_i_table[i + 0x6E].pkt_type = ETM4_PKT_I_COND_RES_F1;
1497 if (bCondValid)
1498 m_i_table[i + 0x6E].pptkFn = &TrcPktProcEtmV4I::iPktCondResult;
1499 else
1500 m_i_table[i + 0x6E].pptkFn = &TrcPktProcEtmV4I::iPktInvalidCfg;
1501 }
1502
1503 // ETM 4.3 introduces ignore packets
1504 if (m_config.FullVersion() >= 0x43)
1505 {
1506 m_i_table[0x70].pkt_type = ETM4_PKT_I_IGNORE;
1507 m_i_table[0x70].pptkFn = &TrcPktProcEtmV4I::iPktNoPayload;
1508 }
1509
1510 // b01110001 - b01111111 - event trace
1511 for(int i = 0; i < 15; i++)
1512 {
1513 m_i_table[0x71+i].pkt_type = ETM4_PKT_I_EVENT;
1514 m_i_table[0x71+i].pptkFn = &TrcPktProcEtmV4I::iPktNoPayload;
1515 }
1516
1517 // 0b1000 000x - context
1518 for(int i = 0; i < 2; i++)
1519 {
1520 m_i_table[0x80+i].pkt_type = ETM4_PKT_I_CTXT;
1521 m_i_table[0x80+i].pptkFn = &TrcPktProcEtmV4I::iPktContext;
1522 }
1523
1524 // 0b1000 0010 to b1000 0011 - addr with ctxt
1525 // 0b1000 0101 to b1000 0110 - addr with ctxt
1526 for(int i = 0; i < 2; i++)
1527 {
1528 m_i_table[0x82+i].pkt_type = (i == 0) ? ETM4_PKT_I_ADDR_CTXT_L_32IS0 : ETM4_PKT_I_ADDR_CTXT_L_32IS1;
1529 m_i_table[0x82+i].pptkFn = &TrcPktProcEtmV4I::iPktAddrCtxt;
1530 }
1531
1532 for(int i = 0; i < 2; i++)
1533 {
1534 m_i_table[0x85+i].pkt_type = (i == 0) ? ETM4_PKT_I_ADDR_CTXT_L_64IS0 : ETM4_PKT_I_ADDR_CTXT_L_64IS1;
1535 m_i_table[0x85+i].pptkFn = &TrcPktProcEtmV4I::iPktAddrCtxt;
1536 }
1537
1538 // 0b1000 1000 - ETE 1.1 TS Marker. also ETMv4.6
1539 if(m_config.FullVersion() >= 0x46)
1540 {
1541 m_i_table[0x88].pkt_type = ETE_PKT_I_TS_MARKER;
1542 m_i_table[0x88].pptkFn = &TrcPktProcEtmV4I::iPktNoPayload;
1543 }
1544 // 0b1001 0000 to b1001 0010 - exact match addr
1545 for(int i = 0; i < 3; i++)
1546 {
1547 m_i_table[0x90+i].pkt_type = ETM4_PKT_I_ADDR_MATCH;
1548 m_i_table[0x90+i].pptkFn = &TrcPktProcEtmV4I::iPktNoPayload;
1549 }
1550
1551 // b1001 0101 - b1001 0110 - addr short address
1552 for(int i = 0; i < 2; i++)
1553 {
1554 m_i_table[0x95+i].pkt_type = (i == 0) ? ETM4_PKT_I_ADDR_S_IS0 : ETM4_PKT_I_ADDR_S_IS1;
1555 m_i_table[0x95+i].pptkFn = &TrcPktProcEtmV4I::iPktShortAddr;
1556 }
1557
1558 // b10011010 - b10011011 - addr long address
1559 // b10011101 - b10011110 - addr long address
1560 for(int i = 0; i < 2; i++)
1561 {
1562 m_i_table[0x9A+i].pkt_type = (i == 0) ? ETM4_PKT_I_ADDR_L_32IS0 : ETM4_PKT_I_ADDR_L_32IS1;
1563 m_i_table[0x9A+i].pptkFn = &TrcPktProcEtmV4I::iPktLongAddr;
1564 }
1565 for(int i = 0; i < 2; i++)
1566 {
1567 m_i_table[0x9D+i].pkt_type = (i == 0) ? ETM4_PKT_I_ADDR_L_64IS0 : ETM4_PKT_I_ADDR_L_64IS1;
1568 m_i_table[0x9D+i].pptkFn = &TrcPktProcEtmV4I::iPktLongAddr;
1569 }
1570
1571 // b1010xxxx - Q packet
1572 for (int i = 0; i < 16; i++)
1573 {
1574 m_i_table[0xA0 + i].pkt_type = ETM4_PKT_I_Q;
1575 // certain Q type codes are reserved.
1576 switch (i) {
1577 case 0x3:
1578 case 0x4:
1579 case 0x7:
1580 case 0x8:
1581 case 0x9:
1582 case 0xD:
1583 case 0xE:
1584 // don't update pkt fn - leave at default reserved.
1585 break;
1586 default:
1587 // if this config supports Q elem - otherwise reserved again.
1588 if (m_config.hasQElem())
1589 m_i_table[0xA0 + i].pptkFn = &TrcPktProcEtmV4I::iPktQ;
1590 }
1591 }
1592
1593 // b10110000 - b10111001 - ETE src address packets
1594 if (m_config.FullVersion() >= 0x50)
1595 {
1596 for (int i = 0; i < 3; i++)
1597 {
1598 m_i_table[0xB0 + i].pkt_type = ETE_PKT_I_SRC_ADDR_MATCH;
1599 m_i_table[0xB0 + i].pptkFn = &TrcPktProcEtmV4I::iPktNoPayload;
1600 }
1601
1602 m_i_table[0xB4].pkt_type = ETE_PKT_I_SRC_ADDR_S_IS0;
1603 m_i_table[0xB4].pptkFn = &TrcPktProcEtmV4I::iPktShortAddr;
1604 m_i_table[0xB5].pkt_type = ETE_PKT_I_SRC_ADDR_S_IS1;
1605 m_i_table[0xB5].pptkFn = &TrcPktProcEtmV4I::iPktShortAddr;
1606
1607 m_i_table[0xB6].pkt_type = ETE_PKT_I_SRC_ADDR_L_32IS0;
1608 m_i_table[0xB6].pptkFn = &TrcPktProcEtmV4I::iPktLongAddr;
1609 m_i_table[0xB7].pkt_type = ETE_PKT_I_SRC_ADDR_L_32IS1;
1610 m_i_table[0xB7].pptkFn = &TrcPktProcEtmV4I::iPktLongAddr;
1611 m_i_table[0xB8].pkt_type = ETE_PKT_I_SRC_ADDR_L_64IS0;
1612 m_i_table[0xB8].pptkFn = &TrcPktProcEtmV4I::iPktLongAddr;
1613 m_i_table[0xB9].pkt_type = ETE_PKT_I_SRC_ADDR_L_64IS1;
1614 m_i_table[0xB9].pptkFn = &TrcPktProcEtmV4I::iPktLongAddr;
1615 }
1616
1617 // Atom Packets - all no payload but have specific pattern generation fn
1618 for(int i = 0xC0; i <= 0xD4; i++) // atom f6
1619 {
1620 m_i_table[i].pkt_type = ETM4_PKT_I_ATOM_F6;
1621 m_i_table[i].pptkFn = &TrcPktProcEtmV4I::iAtom;
1622 }
1623 for(int i = 0xD5; i <= 0xD7; i++) // atom f5
1624 {
1625 m_i_table[i].pkt_type = ETM4_PKT_I_ATOM_F5;
1626 m_i_table[i].pptkFn = &TrcPktProcEtmV4I::iAtom;
1627 }
1628 for(int i = 0xD8; i <= 0xDB; i++) // atom f2
1629 {
1630 m_i_table[i].pkt_type = ETM4_PKT_I_ATOM_F2;
1631 m_i_table[i].pptkFn = &TrcPktProcEtmV4I::iAtom;
1632 }
1633 for(int i = 0xDC; i <= 0xDF; i++) // atom f4
1634 {
1635 m_i_table[i].pkt_type = ETM4_PKT_I_ATOM_F4;
1636 m_i_table[i].pptkFn = &TrcPktProcEtmV4I::iAtom;
1637 }
1638 for(int i = 0xE0; i <= 0xF4; i++) // atom f6
1639 {
1640 m_i_table[i].pkt_type = ETM4_PKT_I_ATOM_F6;
1641 m_i_table[i].pptkFn = &TrcPktProcEtmV4I::iAtom;
1642 }
1643
1644 // atom f5
1645 m_i_table[0xF5].pkt_type = ETM4_PKT_I_ATOM_F5;
1646 m_i_table[0xF5].pptkFn = &TrcPktProcEtmV4I::iAtom;
1647
1648 for(int i = 0xF6; i <= 0xF7; i++) // atom f1
1649 {
1650 m_i_table[i].pkt_type = ETM4_PKT_I_ATOM_F1;
1651 m_i_table[i].pptkFn = &TrcPktProcEtmV4I::iAtom;
1652 }
1653 for(int i = 0xF8; i <= 0xFF; i++) // atom f3
1654 {
1655 m_i_table[i].pkt_type = ETM4_PKT_I_ATOM_F3;
1656 m_i_table[i].pptkFn = &TrcPktProcEtmV4I::iAtom;
1657 }
1658 }
1659
extractContField(const std::vector<uint8_t> & buffer,const unsigned st_idx,uint32_t & value,const unsigned byte_limit)1660 unsigned TrcPktProcEtmV4I::extractContField(const std::vector<uint8_t> &buffer, const unsigned st_idx, uint32_t &value, const unsigned byte_limit /*= 5*/)
1661 {
1662 unsigned idx = 0;
1663 bool lastByte = false;
1664 uint8_t byteVal;
1665 value = 0;
1666 while(!lastByte && (idx < byte_limit)) // max 5 bytes for 32 bit value;
1667 {
1668 if(buffer.size() > (st_idx + idx))
1669 {
1670 // each byte has seven bits + cont bit
1671 byteVal = buffer[(st_idx + idx)];
1672 lastByte = (byteVal & 0x80) != 0x80;
1673 value |= ((uint32_t)(byteVal & 0x7F)) << (idx * 7);
1674 idx++;
1675 }
1676 else
1677 {
1678 throwBadSequenceError("Invalid 32 bit continuation fields in packet");
1679 }
1680 }
1681 return idx;
1682 }
1683
extractTSField64(const std::vector<uint8_t> & buffer,const unsigned st_idx,uint64_t & value)1684 unsigned TrcPktProcEtmV4I::extractTSField64(const std::vector<uint8_t> &buffer, const unsigned st_idx, uint64_t &value)
1685 {
1686 const unsigned max_byte_idx = 8; /* the 9th byte, index 8, will use full 8 bits for value */
1687 unsigned idx = 0;
1688 bool lastByte = false;
1689 uint8_t byteVal;
1690 uint8_t byteValMask = 0x7f;
1691
1692 /* init value */
1693 value = 0;
1694 while(!lastByte) // max 9 bytes for 64 bit value;
1695 {
1696 if(buffer.size() > (st_idx + idx))
1697 {
1698 // each byte has seven bits + cont bit
1699 byteVal = buffer[(st_idx + idx)];
1700
1701 /* detect the final byte - which uses full 8 bits as value */
1702 if (idx == max_byte_idx)
1703 {
1704 byteValMask = 0xFF; /* last byte of 9, no cont bit */
1705 lastByte = true;
1706 }
1707 else
1708 lastByte = (byteVal & 0x80) != 0x80;
1709
1710 value |= ((uint64_t)(byteVal & byteValMask)) << (idx * 7);
1711 idx++;
1712 }
1713 else
1714 {
1715 throwBadSequenceError("Invalid 64 bit continuation fields in packet");
1716 }
1717 }
1718 // index is the count of bytes used here.
1719 return idx;
1720 }
1721
extractCondResult(const std::vector<uint8_t> & buffer,const unsigned st_idx,uint32_t & key,uint8_t & result)1722 unsigned TrcPktProcEtmV4I::extractCondResult(const std::vector<uint8_t> &buffer, const unsigned st_idx, uint32_t& key, uint8_t &result)
1723 {
1724 unsigned idx = 0;
1725 bool lastByte = false;
1726 int incr = 0;
1727
1728 key = 0;
1729
1730 while(!lastByte && (idx < 6)) // cannot be more than 6 bytes for res + 32 bit key
1731 {
1732 if(buffer.size() > (st_idx + idx))
1733 {
1734 if(idx == 0)
1735 {
1736 result = buffer[st_idx+idx];
1737 key = (buffer[st_idx+idx] >> 4) & 0x7;
1738 incr+=3;
1739 }
1740 else
1741 {
1742 key |= ((uint32_t)(buffer[st_idx+idx] & 0x7F)) << incr;
1743 incr+=7;
1744 }
1745 lastByte = (bool)((buffer[st_idx+idx] & 0x80) == 0);
1746 idx++;
1747 }
1748 else
1749 {
1750 throwBadSequenceError("Invalid continuation fields in packet");
1751 }
1752 }
1753 return idx;
1754 }
1755
extract64BitLongAddr(const std::vector<uint8_t> & buffer,const int st_idx,const uint8_t IS,uint64_t & value)1756 int TrcPktProcEtmV4I::extract64BitLongAddr(const std::vector<uint8_t> &buffer, const int st_idx, const uint8_t IS, uint64_t &value)
1757 {
1758 value = 0;
1759 if(IS == 0)
1760 {
1761 value |= ((uint64_t)(buffer[st_idx+0] & 0x7F)) << 2;
1762 value |= ((uint64_t)(buffer[st_idx+1] & 0x7F)) << 9;
1763 }
1764 else
1765 {
1766 value |= ((uint64_t)(buffer[st_idx+0] & 0x7F)) << 1;
1767 value |= ((uint64_t)buffer[st_idx+1]) << 8;
1768 }
1769 value |= ((uint64_t)buffer[st_idx+2]) << 16;
1770 value |= ((uint64_t)buffer[st_idx+3]) << 24;
1771 value |= ((uint64_t)buffer[st_idx+4]) << 32;
1772 value |= ((uint64_t)buffer[st_idx+5]) << 40;
1773 value |= ((uint64_t)buffer[st_idx+6]) << 48;
1774 value |= ((uint64_t)buffer[st_idx+7]) << 56;
1775 return 8;
1776 }
1777
extract32BitLongAddr(const std::vector<uint8_t> & buffer,const int st_idx,const uint8_t IS,uint32_t & value)1778 int TrcPktProcEtmV4I::extract32BitLongAddr(const std::vector<uint8_t> &buffer, const int st_idx, const uint8_t IS, uint32_t &value)
1779 {
1780 value = 0;
1781 if(IS == 0)
1782 {
1783 value |= ((uint32_t)(buffer[st_idx+0] & 0x7F)) << 2;
1784 value |= ((uint32_t)(buffer[st_idx+1] & 0x7F)) << 9;
1785 }
1786 else
1787 {
1788 value |= ((uint32_t)(buffer[st_idx+0] & 0x7F)) << 1;
1789 value |= ((uint32_t)buffer[st_idx+1]) << 8;
1790 }
1791 value |= ((uint32_t)buffer[st_idx+2]) << 16;
1792 value |= ((uint32_t)buffer[st_idx+3]) << 24;
1793 return 4;
1794 }
1795
throwBadSequenceError(const char * pszExtMsg)1796 void TrcPktProcEtmV4I::throwBadSequenceError(const char *pszExtMsg)
1797 {
1798 m_curr_packet.updateErrType(ETM4_PKT_I_BAD_SEQUENCE); // swap type for err type
1799 throw ocsdError(OCSD_ERR_SEV_ERROR, OCSD_ERR_BAD_PACKET_SEQ,m_packet_index,m_config.getTraceID(),pszExtMsg);
1800 }
1801
1802
1803 /* End of File trc_pkt_proc_etmv4i.cpp */
1804