1 /*
2 * \file trc_pkt_proc_etmv3_impl.cpp
3 * \brief OpenCSD :
4 *
5 * \copyright Copyright (c) 2015, ARM Limited. All Rights Reserved.
6 */
7
8 /*
9 * Redistribution and use in source and binary forms, with or without modification,
10 * are permitted provided that the following conditions are met:
11 *
12 * 1. Redistributions of source code must retain the above copyright notice,
13 * this list of conditions and the following disclaimer.
14 *
15 * 2. Redistributions in binary form must reproduce the above copyright notice,
16 * this list of conditions and the following disclaimer in the documentation
17 * and/or other materials provided with the distribution.
18 *
19 * 3. Neither the name of the copyright holder nor the names of its contributors
20 * may be used to endorse or promote products derived from this software without
21 * specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND
24 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
25 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
26 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
27 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
28 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
30 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
31 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
32 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33 */
34
35 #include "trc_pkt_proc_etmv3_impl.h"
36
EtmV3PktProcImpl()37 EtmV3PktProcImpl::EtmV3PktProcImpl() :
38 m_isInit(false),
39 m_interface(0)
40 {
41 }
42
~EtmV3PktProcImpl()43 EtmV3PktProcImpl::~EtmV3PktProcImpl()
44 {
45 }
46
Configure(const EtmV3Config * p_config)47 ocsd_err_t EtmV3PktProcImpl::Configure(const EtmV3Config *p_config)
48 {
49 ocsd_err_t err = OCSD_OK;
50 if(p_config != 0)
51 {
52 m_config = *p_config;
53 m_chanIDCopy = m_config.getTraceID();
54 }
55 else
56 {
57 err = OCSD_ERR_INVALID_PARAM_VAL;
58 if(m_isInit)
59 m_interface->LogError(ocsdError(OCSD_ERR_SEV_ERROR,err));
60 }
61 return err;
62 }
63
processData(const ocsd_trc_index_t index,const uint32_t dataBlockSize,const uint8_t * pDataBlock,uint32_t * numBytesProcessed)64 ocsd_datapath_resp_t EtmV3PktProcImpl::processData(const ocsd_trc_index_t index,
65 const uint32_t dataBlockSize,
66 const uint8_t *pDataBlock,
67 uint32_t *numBytesProcessed)
68 {
69 ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
70 m_bytesProcessed = 0;
71
72 while( ( (m_bytesProcessed < dataBlockSize) ||
73 ((m_bytesProcessed == dataBlockSize) && (m_process_state == SEND_PKT)) )
74 && OCSD_DATA_RESP_IS_CONT(resp))
75 {
76 try
77 {
78 switch(m_process_state)
79 {
80 case WAIT_SYNC:
81 if(!m_bStartOfSync)
82 m_packet_index = index + m_bytesProcessed;
83 m_bytesProcessed += waitForSync(dataBlockSize-m_bytesProcessed,pDataBlock+m_bytesProcessed);
84 break;
85
86 case PROC_HDR:
87 m_packet_index = index + m_bytesProcessed;
88 processHeaderByte(pDataBlock[m_bytesProcessed++]);
89 break;
90
91 case PROC_DATA:
92 processPayloadByte(pDataBlock [m_bytesProcessed++]);
93 break;
94
95 case SEND_PKT:
96 resp = outputPacket();
97 break;
98 }
99 }
100 catch(ocsdError &err)
101 {
102 m_interface->LogError(err);
103 if( (err.getErrorCode() == OCSD_ERR_BAD_PACKET_SEQ) ||
104 (err.getErrorCode() == OCSD_ERR_INVALID_PCKT_HDR))
105 {
106 // send invalid packets up the pipe to let the next stage decide what to do.
107 m_process_state = SEND_PKT;
108 }
109 else
110 {
111 // bail out on any other error.
112 resp = OCSD_RESP_FATAL_INVALID_DATA;
113 }
114 }
115 catch(...)
116 {
117 /// vv bad at this point.
118 resp = OCSD_RESP_FATAL_SYS_ERR;
119 ocsdError fatal = ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_FAIL,m_packet_index,m_chanIDCopy);
120 fatal.setMessage("Unknown System Error decoding trace.");
121 m_interface->LogError(fatal);
122 }
123 }
124
125 *numBytesProcessed = m_bytesProcessed;
126 return resp;
127 }
128
onEOT()129 ocsd_datapath_resp_t EtmV3PktProcImpl::onEOT()
130 {
131 ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
132 // if we have a partial packet then send to attached sinks
133 if(m_currPacketData.size() != 0)
134 {
135 // TBD: m_curr_packet.updateErrType(ETM4_ETM3_PKT_I_INCOMPLETE_EOT);
136 resp = outputPacket();
137 InitPacketState();
138 }
139 return resp;
140 }
141
onReset()142 ocsd_datapath_resp_t EtmV3PktProcImpl::onReset()
143 {
144 InitProcessorState();
145 return OCSD_RESP_CONT;
146 }
147
onFlush()148 ocsd_datapath_resp_t EtmV3PktProcImpl::onFlush()
149 {
150 // packet processor never holds on to flushable data (may have partial packet,
151 // but any full packets are immediately sent)
152 return OCSD_RESP_CONT;
153 }
154
Initialise(TrcPktProcEtmV3 * p_interface)155 void EtmV3PktProcImpl::Initialise(TrcPktProcEtmV3 *p_interface)
156 {
157 if(p_interface)
158 {
159 m_interface = p_interface;
160 m_isInit = true;
161
162 }
163 InitProcessorState();
164 /* not using pattern matcher for sync at present
165 static const uint8_t a_sync[] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x80 };
166 m_syncMatch.setPattern(a_sync, sizeof(a_sync));*/
167 }
168
InitProcessorState()169 void EtmV3PktProcImpl::InitProcessorState()
170 {
171 m_bStreamSync = false; // not synced
172 m_process_state = WAIT_SYNC; // waiting for sync
173 m_bStartOfSync = false; // not seen start of sync packet
174 m_curr_packet.ResetState(); // reset intra packet state
175 InitPacketState(); // set curr packet state
176 m_bSendPartPkt = false;
177 }
178
InitPacketState()179 void EtmV3PktProcImpl::InitPacketState()
180 {
181 m_bytesExpectedThisPkt = 0;
182 m_BranchPktNeedsException = false;
183 m_bIsync_got_cycle_cnt = false;
184 m_bIsync_get_LSiP_addr = false;
185 m_IsyncInfoIdx = false;
186 m_bExpectingDataAddress = false;
187 m_bFoundDataAddress = false;
188 m_currPacketData.clear();
189 m_currPktIdx = 0; // index into processed bytes in current packet
190 m_curr_packet.Clear();
191
192 }
193
outputPacket()194 ocsd_datapath_resp_t EtmV3PktProcImpl::outputPacket()
195 {
196 ocsd_datapath_resp_t dp_resp = OCSD_RESP_FATAL_NOT_INIT;
197 if(m_isInit)
198 {
199 ocsd_etmv3_pkt_type type = m_curr_packet.getType();
200 if(!m_bSendPartPkt)
201 {
202 dp_resp = m_interface->outputOnAllInterfaces(m_packet_index,&m_curr_packet,&type,m_currPacketData);
203 m_process_state = m_bStreamSync ? PROC_HDR : WAIT_SYNC; // need a header next time, or still waiting to sync.
204 m_currPacketData.clear();
205 }
206 else
207 {
208 // sending part packet, still some data in the main packet
209 dp_resp = m_interface->outputOnAllInterfaces(m_packet_index,&m_curr_packet,&type,m_partPktData);
210 m_process_state = m_post_part_pkt_state;
211 m_packet_index += m_partPktData.size();
212 m_bSendPartPkt = false;
213 m_curr_packet.SetType(m_post_part_pkt_type);
214 }
215 }
216 return dp_resp;
217 }
218
setBytesPartPkt(int numBytes,process_state nextState,const ocsd_etmv3_pkt_type nextType)219 void EtmV3PktProcImpl::setBytesPartPkt(int numBytes, process_state nextState, const ocsd_etmv3_pkt_type nextType)
220 {
221 m_partPktData.clear();
222 for(int i=0; i < numBytes; i++)
223 {
224 m_partPktData.push_back(m_currPacketData[i]);
225 }
226 m_currPacketData.erase(m_currPacketData.begin(), m_currPacketData.begin()+numBytes);
227 m_bSendPartPkt = true;
228 m_post_part_pkt_state = nextState;
229 m_post_part_pkt_type = nextType;
230 }
231
waitForSync(const uint32_t dataBlockSize,const uint8_t * pDataBlock)232 uint32_t EtmV3PktProcImpl::waitForSync(const uint32_t dataBlockSize, const uint8_t *pDataBlock)
233 {
234 uint8_t currByte;
235 uint32_t bytesProcessed = 0;
236 bool bSendBlock = false;
237
238 // need to wait for the first sync packet
239 while(!bSendBlock && (bytesProcessed < dataBlockSize))
240 {
241 currByte = pDataBlock[bytesProcessed++];
242 // TBD: forced sync point
243
244 if(m_bStartOfSync)
245 {
246 // need to handle consecutive 0 bytes followed by genuine A-SYNC.
247
248 m_currPacketData.push_back(currByte);
249 if((currByte == 0x80) && (m_currPacketData.size() >= 6))
250 {
251 // it is a sync packet possibly with leading zeros
252 bSendBlock = true;
253 if(m_currPacketData.size() > 6)
254 {
255 m_currPacketData.pop_back();
256 bytesProcessed--; // return 0x80 to the input buffer to re-process next pass after stripping 0's
257 setBytesPartPkt(m_currPacketData.size()-5,WAIT_SYNC,ETM3_PKT_NOTSYNC);
258 }
259 else
260 {
261 m_bStreamSync = true;
262 m_curr_packet.SetType(ETM3_PKT_A_SYNC);
263 }
264 }
265 else if(currByte != 0x00)
266 {
267 m_bStartOfSync = false; // not a sync packet
268 }
269 else if(m_currPacketData.size() >= 13) // 13 0's, strip 8 of them...
270 {
271 setBytesPartPkt(8,WAIT_SYNC,ETM3_PKT_NOTSYNC);
272 bSendBlock = true;
273 }
274 }
275 else // not seen a start of sync candidate yet
276 {
277 if(currByte == 0x00) // could be the start of a-sync
278 {
279 if(m_currPacketData.size() == 0)
280 {
281 m_currPacketData.push_back(currByte);
282 m_bStartOfSync = true;
283 }
284 else
285 {
286 bytesProcessed--;
287 bSendBlock = true; // send none sync packet data, re-process this byte next time.
288 m_curr_packet.SetType(ETM3_PKT_NOTSYNC); // send unsynced data packet.
289 }
290 }
291 else
292 {
293 //save a byte - not start of a-sync
294 m_currPacketData.push_back(currByte);
295
296 // done all data in this block, or got 16 unsynced bytes
297 if((bytesProcessed == dataBlockSize) || (m_currPacketData.size() == 16))
298 {
299 bSendBlock = true; // send none sync packet block
300 m_curr_packet.SetType(ETM3_PKT_NOTSYNC); // send unsynced data packet.
301 }
302 }
303 }
304 }
305 if(bSendBlock)
306 SendPacket();
307 return bytesProcessed;
308 }
309
processHeaderByte(uint8_t by)310 ocsd_err_t EtmV3PktProcImpl::processHeaderByte(uint8_t by)
311 {
312 InitPacketState(); // new packet, clear old single packet state (retains intra packet state).
313
314 // save byte
315 m_currPacketData.push_back(by);
316
317 m_process_state = PROC_DATA; // assume next is data packet
318
319 // check for branch address 0bCxxxxxxx1
320 if((by & 0x01) == 0x01 ) {
321 m_curr_packet.SetType(ETM3_PKT_BRANCH_ADDRESS);
322 m_BranchPktNeedsException = false;
323 if((by & 0x80) != 0x80) {
324 // no continuation - 1 byte branch same in alt and std...
325 if((by == 0x01) && (m_interface->getComponentOpMode() & ETMV3_OPFLG_UNFORMATTED_SOURCE))
326 {
327 // TBD: need to fix up for handling bypassed ETM stream at some point.
328 throwUnsupportedErr("Bypassed ETM stream not supported in this version of the decoder.");
329 // could be EOTrace marker from bypassed formatter
330 m_curr_packet.SetType(ETM3_PKT_BRANCH_OR_BYPASS_EOT);
331 }
332 else
333 {
334 OnBranchAddress();
335 SendPacket(); // mark ready to send.
336 }
337 }
338 }
339 // check for p-header - 0b1xxxxxx0
340 else if((by & 0x81) == 0x80) {
341 m_curr_packet.SetType(ETM3_PKT_P_HDR);
342 if(m_curr_packet.UpdateAtomFromPHdr(by,m_config.isCycleAcc()))
343 SendPacket();
344 else
345 throwPacketHeaderErr("Invalid P-Header.");
346 }
347 // check 0b0000xx00 group
348 else if((by & 0xF3) == 0x00) {
349
350 // A-Sync
351 if(by == 0x00) {
352 m_curr_packet.SetType(ETM3_PKT_A_SYNC);
353 }
354 // cycle count
355 else if(by == 0x04) {
356 m_curr_packet.SetType(ETM3_PKT_CYCLE_COUNT);
357 }
358 // I-Sync
359 else if(by == 0x08) {
360 m_curr_packet.SetType(ETM3_PKT_I_SYNC);
361 m_bIsync_got_cycle_cnt = false;
362 m_bIsync_get_LSiP_addr = false;
363 }
364 // trigger
365 else if(by == 0x0C) {
366 m_curr_packet.SetType(ETM3_PKT_TRIGGER);
367 // no payload - just send it.
368 SendPacket();
369 }
370 }
371 // check remaining 0bxxxxxx00 codes
372 else if((by & 0x03 )== 0x00) {
373 // OoO data 0b0xx0xx00
374 if((by & 0x93 )== 0x00) {
375 if(!m_config.isDataValTrace()) {
376 m_curr_packet.SetErrType(ETM3_PKT_BAD_TRACEMODE);
377 throwPacketHeaderErr("Invalid data trace header (out of order data) - not tracing data values.");
378 }
379 m_curr_packet.SetType(ETM3_PKT_OOO_DATA);
380 uint8_t size = ((by & 0x0C) >> 2);
381 // header contains a count of the data to follow
382 // size 3 == 4 bytes, other sizes == size bytes
383 if(size == 0)
384 {
385 m_curr_packet.SetDataOOOTag((by >> 5) & 0x3);
386 m_curr_packet.SetDataValue(0);
387 SendPacket();
388 }
389 else
390 m_bytesExpectedThisPkt = (short)(1 + ((size == 3) ? 4 : size));
391 }
392 // I-Sync + cycle count
393 else if(by == 0x70) {
394 m_curr_packet.SetType(ETM3_PKT_I_SYNC_CYCLE);
395 m_bIsync_got_cycle_cnt = false;
396 m_bIsync_get_LSiP_addr = false;
397 }
398 // store failed
399 else if(by == 0x50) {
400 if(!m_config.isDataValTrace())
401 {
402 m_curr_packet.SetErrType(ETM3_PKT_BAD_TRACEMODE);
403 throwPacketHeaderErr("Invalid data trace header (store failed) - not tracing data values.");
404 }
405 m_curr_packet.SetType(ETM3_PKT_STORE_FAIL);
406 SendPacket();
407 }
408 // OoO placeholder 0b01x1xx00
409 else if((by & 0xD3 )== 0x50) {
410 m_curr_packet.SetType(ETM3_PKT_OOO_ADDR_PLC);
411 if(!m_config.isDataTrace())
412 {
413 m_curr_packet.SetErrType(ETM3_PKT_BAD_TRACEMODE);
414 throwPacketHeaderErr("Invalid data trace header (out of order placeholder) - not tracing data.");
415 }
416 // expecting data address if flagged and address tracing enabled (flag can be set even if address tracing disabled)
417 m_bExpectingDataAddress = ((by & DATA_ADDR_EXPECTED_FLAG) == DATA_ADDR_EXPECTED_FLAG) && m_config.isDataAddrTrace();
418 m_bFoundDataAddress = false;
419 m_curr_packet.SetDataOOOTag((by >> 2) & 0x3);
420 if(!m_bExpectingDataAddress) {
421 SendPacket();
422 }
423 }
424 // vmid 0b00111100
425 else if(by == 0x3c) {
426 m_curr_packet.SetType(ETM3_PKT_VMID);
427 }
428 else
429 {
430 m_curr_packet.SetErrType(ETM3_PKT_RESERVED);
431 throwPacketHeaderErr("Packet header reserved encoding");
432 }
433 }
434 // normal data 0b00x0xx10
435 else if((by & 0xD3 )== 0x02) {
436 uint8_t size = ((by & 0x0C) >> 2);
437 if(!m_config.isDataTrace()) {
438 m_curr_packet.SetErrType(ETM3_PKT_BAD_TRACEMODE);
439 throwPacketHeaderErr("Invalid data trace header (normal data) - not tracing data.");
440 }
441 m_curr_packet.SetType(ETM3_PKT_NORM_DATA);
442 m_bExpectingDataAddress = ((by & DATA_ADDR_EXPECTED_FLAG) == DATA_ADDR_EXPECTED_FLAG) && m_config.isDataAddrTrace();
443 m_bFoundDataAddress = false;
444
445 // set this with the data bytes expected this packet, plus the header byte.
446 m_bytesExpectedThisPkt = (short)( 1 + ((size == 3) ? 4 : size));
447 if(!m_bExpectingDataAddress && (m_bytesExpectedThisPkt == 1)) {
448 // single byte data packet, value = 0;
449 m_curr_packet.SetDataValue(0);
450 SendPacket();
451 }
452
453 }
454 // data suppressed 0b01100010
455 else if(by == 0x62) {
456 if(!m_config.isDataTrace())
457 {
458 m_curr_packet.SetErrType(ETM3_PKT_BAD_TRACEMODE);
459 throwPacketHeaderErr("Invalid data trace header (data suppressed) - not tracing data.");
460 }
461 m_curr_packet.SetType(ETM3_PKT_DATA_SUPPRESSED);
462 SendPacket();
463 }
464 // value not traced 0b011x1010
465 else if((by & 0xEF )== 0x6A) {
466 if(!m_config.isDataTrace()) {
467 m_curr_packet.SetErrType(ETM3_PKT_BAD_TRACEMODE);
468 throwPacketHeaderErr("Invalid data trace header (value not traced) - not tracing data.");
469 }
470 m_curr_packet.SetType(ETM3_PKT_VAL_NOT_TRACED);
471 m_bExpectingDataAddress = ((by & DATA_ADDR_EXPECTED_FLAG) == DATA_ADDR_EXPECTED_FLAG) && m_config.isDataAddrTrace();
472 m_bFoundDataAddress = false;
473 if(!m_bExpectingDataAddress) {
474 SendPacket();
475 }
476 }
477 // ignore 0b01100110
478 else if(by == 0x66) {
479 m_curr_packet.SetType(ETM3_PKT_IGNORE);
480 SendPacket();
481 }
482 // context ID 0b01101110
483 else if(by == 0x6E) {
484 m_curr_packet.SetType(ETM3_PKT_CONTEXT_ID);
485 m_bytesExpectedThisPkt = (short)(1 + m_config.CtxtIDBytes());
486 }
487 // exception return 0b01110110
488 else if(by == 0x76) {
489 m_curr_packet.SetType(ETM3_PKT_EXCEPTION_EXIT);
490 SendPacket();
491 }
492 // exception entry 0b01111110
493 else if(by == 0x7E) {
494 m_curr_packet.SetType(ETM3_PKT_EXCEPTION_ENTRY);
495 SendPacket();
496 }
497 // timestamp packet 0b01000x10
498 else if((by & 0xFB )== 0x42)
499 {
500 m_curr_packet.SetType(ETM3_PKT_TIMESTAMP);
501 }
502 else
503 {
504 m_curr_packet.SetErrType(ETM3_PKT_RESERVED);
505 throwPacketHeaderErr("Packet header reserved encoding.");
506 }
507 return OCSD_OK;
508 }
509
processPayloadByte(uint8_t by)510 ocsd_err_t EtmV3PktProcImpl::processPayloadByte(uint8_t by)
511 {
512 bool bTopBitSet = false;
513 bool packetDone = false;
514
515 // pop byte into buffer
516 m_currPacketData.push_back(by);
517
518 switch(m_curr_packet.getType()) {
519 default:
520 throw ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_PKT_INTERP_FAIL,m_packet_index,m_chanIDCopy,"Interpreter failed - cannot process payload for unexpected or unsupported packet.");
521 break;
522
523 case ETM3_PKT_BRANCH_ADDRESS:
524 bTopBitSet = (bool)((by & 0x80) == 0x80);
525 if(m_config.isAltBranch()) // etm implements the alternative branch encoding
526 {
527 if(!bTopBitSet) // no continuation
528 {
529 if(!m_BranchPktNeedsException)
530 {
531 if((by & 0xC0) == 0x40)
532 m_BranchPktNeedsException = true;
533 else
534 packetDone = true;
535 }
536 else
537 packetDone = true;
538 }
539 }
540 else
541 {
542 // standard encoding < 5 bytes cannot be exception branch
543 // 5 byte packet
544 if(m_currPacketData.size() == 5) {
545 if((by & 0xC0) == 0x40)
546 // expecting follow up byte(s)
547 m_BranchPktNeedsException = true;
548 else
549 packetDone = true;
550 }
551 // waiting for exception packet
552 else if(m_BranchPktNeedsException){
553 if(!bTopBitSet)
554 packetDone = true;
555 }
556 else {
557 // not exception - end of packets
558 if(!bTopBitSet)
559 packetDone = true;
560 }
561 }
562
563 if(packetDone)
564 {
565 OnBranchAddress();
566 SendPacket();
567 }
568 break;
569
570 case ETM3_PKT_BRANCH_OR_BYPASS_EOT:
571 /*
572 if((by != 0x00) || ( m_currPacketData.size() == ETM3_PKT_BUFF_SIZE)) {
573 if(by == 0x80 && ( m_currPacketData.size() == 7)) {
574 // branch 0 followed by A-sync!
575 m_currPacketData.size() = 1;
576 m_curr_packet.SetType(ETM3_PKT_BRANCH_ADDRESS;
577 SendPacket();
578 memcpy(m_currPacketData, &m_currPacketData[1],6);
579 m_currPacketData.size() = 6;
580 m_curr_packet.SetType(ETM3_PKT_A_SYNC;
581 SendPacket();
582 }
583 else if( m_currPacketData.size() == 2) {
584 // branch followed by another byte
585 m_currPacketData.size() = 1;
586 m_curr_packet.SetType(ETM3_PKT_BRANCH_ADDRESS;
587 SendPacket();
588 ProcessHeaderByte(by);
589 }
590 else if(by == 0x00) {
591 // end of buffer...output something - incomplete / unknown.
592 SendPacket();
593 }
594 else if(by == 0x01) {
595 // 0x01 - 0x00 x N - 0x1
596 // end of buffer...output something
597 m_currPacketData.size()--;
598 SendPacket();
599 ProcessHeaderByte(by);
600 }
601 else {
602 // branch followed by unknown sequence
603 int oldidx = m_currPacketData.size();
604 m_currPacketData.size() = 1;
605 m_curr_packet.SetType(ETM3_PKT_BRANCH_ADDRESS;
606 SendPacket();
607 oldidx--;
608 memcpy(m_currPacketData, &m_currPacketData[1],oldidx);
609 m_currPacketData.size() = oldidx;
610 SendBadPacket("ERROR : unknown sequence");
611 }
612 }*/
613 // just ignore zeros
614 break;
615
616
617
618 case ETM3_PKT_A_SYNC:
619 if(by == 0x00) {
620 if( m_currPacketData.size() > 5) {
621 // extra 0, need to lose one
622
623 // set error type
624 m_curr_packet.SetErrType(ETM3_PKT_BAD_SEQUENCE);
625 // mark extra 0 for sending, retain remaining, restart in A-SYNC processing mode.
626 setBytesPartPkt(1,PROC_DATA,ETM3_PKT_A_SYNC);
627 throwMalformedPacketErr("A-Sync ?: Extra 0x00 in sequence");
628 }
629 }
630 else if((by == 0x80) && ( m_currPacketData.size() == 6)) {
631 SendPacket();
632 m_bStreamSync = true;
633 }
634 else
635 {
636 m_curr_packet.SetErrType(ETM3_PKT_BAD_SEQUENCE);
637 m_bytesProcessed--; // remove the last byte from the number processed to re-try
638 m_currPacketData.pop_back(); // remove the last byte processed from the packet
639 throwMalformedPacketErr("A-Sync ? : Unexpected byte in sequence");
640 }
641 break;
642
643 case ETM3_PKT_CYCLE_COUNT:
644 bTopBitSet = ((by & 0x80) == 0x80);
645 if(!bTopBitSet || ( m_currPacketData.size() >= 6)) {
646 m_currPktIdx = 1;
647 m_curr_packet.SetCycleCount(extractCycleCount());
648 SendPacket();
649 }
650 break;
651
652 case ETM3_PKT_I_SYNC_CYCLE:
653 if(!m_bIsync_got_cycle_cnt) {
654 if(((by & 0x80) != 0x80) || ( m_currPacketData.size() >= 6)) {
655 m_bIsync_got_cycle_cnt = true;
656 }
657 break;
658 }
659 // fall through when we have the first non-cycle count byte
660 case ETM3_PKT_I_SYNC:
661 if(m_bytesExpectedThisPkt == 0) {
662 int cycCountBytes = m_currPacketData.size() - 2;
663 int ctxtIDBytes = m_config.CtxtIDBytes();
664 // bytes expected = header + n x ctxt id + info byte + 4 x addr;
665 if(m_config.isInstrTrace())
666 m_bytesExpectedThisPkt = cycCountBytes + 6 + ctxtIDBytes;
667 else
668 m_bytesExpectedThisPkt = 2 + ctxtIDBytes;
669 m_IsyncInfoIdx = 1 + cycCountBytes + ctxtIDBytes;
670 }
671 if(( m_currPacketData.size() - 1) == (unsigned)m_IsyncInfoIdx) {
672 m_bIsync_get_LSiP_addr = ((m_currPacketData[m_IsyncInfoIdx] & 0x80) == 0x80);
673 }
674
675 // if bytes collected >= bytes expected
676 if( m_currPacketData.size() >= m_bytesExpectedThisPkt) {
677 // if we still need the LSip Addr, then this is not part of the expected
678 // count as we have no idea how long it is
679 if(m_bIsync_get_LSiP_addr) {
680 if((by & 0x80) != 0x80) {
681 OnISyncPacket();
682 }
683 }
684 else {
685 // otherwise, output now
686 OnISyncPacket();
687 }
688 }
689 break;
690
691 case ETM3_PKT_NORM_DATA:
692 if(m_bExpectingDataAddress && !m_bFoundDataAddress) {
693 // look for end of continuation bits
694 if((by & 0x80) != 0x80) {
695 m_bFoundDataAddress = true;
696 // add on the bytes we have found for the address to the expected data bytes
697 m_bytesExpectedThisPkt += ( m_currPacketData.size() - 1);
698 }
699 else
700 break;
701 }
702 // found any data address we were expecting
703 else if(m_bytesExpectedThisPkt == m_currPacketData.size()) {
704 m_currPktIdx = 1;
705 if(m_bExpectingDataAddress)
706 {
707 uint8_t bits = 0, beVal = 0;
708 bool updateBE = false;
709 uint32_t dataAddress = extractDataAddress(bits,updateBE,beVal);
710 m_curr_packet.UpdateDataAddress(dataAddress, bits);
711 if(updateBE)
712 m_curr_packet.UpdateDataEndian(beVal);
713 }
714 m_curr_packet.SetDataValue(extractDataValue((m_currPacketData[0] >> 2) & 0x3));
715 SendPacket();
716 }
717 break;
718
719 case ETM3_PKT_OOO_DATA:
720 if(m_bytesExpectedThisPkt == m_currPacketData.size())
721 {
722 m_currPktIdx = 1;
723 m_curr_packet.SetDataValue(extractDataValue((m_currPacketData[0] >> 2) & 0x3));
724 m_curr_packet.SetDataOOOTag((m_currPacketData[0] >> 5) & 0x3);
725 SendPacket();
726 }
727 if(m_bytesExpectedThisPkt < m_currPacketData.size())
728 throwMalformedPacketErr("Malformed out of order data packet.");
729 break;
730
731 // both these expect an address only.
732 case ETM3_PKT_VAL_NOT_TRACED:
733 case ETM3_PKT_OOO_ADDR_PLC: // we set the tag earlier.
734 if(m_bExpectingDataAddress) {
735 // look for end of continuation bits
736 if((by & 0x80) != 0x80) {
737 uint8_t bits = 0, beVal = 0;
738 bool updateBE = false;
739 m_currPktIdx = 1;
740 uint32_t dataAddress = extractDataAddress(bits,updateBE,beVal);
741 m_curr_packet.UpdateDataAddress(dataAddress, bits);
742 if(updateBE)
743 m_curr_packet.UpdateDataEndian(beVal);
744 SendPacket();
745 }
746 }
747 break;
748
749 case ETM3_PKT_CONTEXT_ID:
750 if(m_bytesExpectedThisPkt == m_currPacketData.size()) {
751 m_currPktIdx = 1;
752 m_curr_packet.UpdateContextID(extractCtxtID());
753 SendPacket();
754 }
755 if(m_bytesExpectedThisPkt < m_currPacketData.size())
756 throwMalformedPacketErr("Malformed context id packet.");
757 break;
758
759 case ETM3_PKT_TIMESTAMP:
760 if((by & 0x80) != 0x80) {
761 uint8_t tsBits = 0;
762 m_currPktIdx = 1;
763 uint64_t tsVal = extractTimestamp(tsBits);
764 m_curr_packet.UpdateTimestamp(tsVal,tsBits);
765 SendPacket();
766 }
767 break;
768
769 case ETM3_PKT_VMID:
770 // single byte payload
771 m_curr_packet.UpdateVMID(by);
772 SendPacket();
773 break;
774 }
775
776 return OCSD_OK;
777 }
778
779 // extract branch address packet at current location in packet data.
OnBranchAddress()780 void EtmV3PktProcImpl::OnBranchAddress()
781 {
782 int validBits = 0;
783 ocsd_vaddr_t partAddr = 0;
784
785 partAddr = extractBrAddrPkt(validBits);
786 m_curr_packet.UpdateAddress(partAddr,validBits);
787 }
788
extractBrAddrPkt(int & nBitsOut)789 uint32_t EtmV3PktProcImpl::extractBrAddrPkt(int &nBitsOut)
790 {
791 static int addrshift[] = {
792 2, // ARM_ISA
793 1, // thumb
794 1, // thumb EE
795 0 // jazelle
796 };
797
798 static uint8_t addrMask[] = { // byte 5 masks
799 0x7, // ARM_ISA
800 0xF, // thumb
801 0xF, // thumb EE
802 0x1F // jazelle
803 };
804
805 static int addrBits[] = { // address bits in byte 5
806 3, // ARM_ISA
807 4, // thumb
808 4, // thumb EE
809 5 // jazelle
810 };
811
812 static ocsd_armv7_exception exceptionTypeARMdeprecated[] = {
813 Excp_Reset,
814 Excp_IRQ,
815 Excp_Reserved,
816 Excp_Reserved,
817 Excp_Jazelle,
818 Excp_FIQ,
819 Excp_AsyncDAbort,
820 Excp_DebugHalt
821 };
822
823 bool CBit = true;
824 int bytecount = 0;
825 int bitcount = 0;
826 int shift = 0;
827 int isa_idx = 0;
828 uint32_t value = 0;
829 uint8_t addrbyte;
830 bool byte5AddrUpdate = false;
831
832 while(CBit && bytecount < 4)
833 {
834 checkPktLimits();
835 addrbyte = m_currPacketData[m_currPktIdx++];
836 CBit = (bool)((addrbyte & 0x80) != 0);
837 shift = bitcount;
838 if(bytecount == 0)
839 {
840 addrbyte &= ~0x81;
841 bitcount+=6;
842 addrbyte >>= 1;
843 }
844 else
845 {
846 // bytes 2-4, no continuation, alt format uses bit 6 to indicate following exception bytes
847 if(m_config.isAltBranch() && !CBit)
848 {
849 // last compressed address byte with exception
850 if((addrbyte & 0x40) == 0x40)
851 extractExceptionData();
852 addrbyte &= 0x3F;
853 bitcount+=6;
854 }
855 else
856 {
857 addrbyte &= 0x7F;
858 bitcount+=7;
859 }
860 }
861 value |= ((uint32_t)addrbyte) << shift;
862 bytecount++;
863 }
864
865 // byte 5 - indicates following exception bytes (or not!)
866 if(CBit)
867 {
868 checkPktLimits();
869 addrbyte = m_currPacketData[m_currPktIdx++];
870
871 // deprecated original byte 5 encoding - ARM state exception only
872 if(addrbyte & 0x80)
873 {
874 uint8_t excep_num = (addrbyte >> 3) & 0x7;
875 m_curr_packet.UpdateISA(ocsd_isa_arm);
876 m_curr_packet.SetException(exceptionTypeARMdeprecated[excep_num], excep_num, (addrbyte & 0x40) ? true : false,m_config.isV7MArch());
877 }
878 else
879 // normal 5 byte branch, or uses exception bytes.
880 {
881 // go grab the exception bits to correctly interpret the ISA state
882 if((addrbyte & 0x40) == 0x40)
883 extractExceptionData();
884
885 if((addrbyte & 0xB8) == 0x08)
886 m_curr_packet.UpdateISA(ocsd_isa_arm);
887 else if ((addrbyte & 0xB0) == 0x10)
888 m_curr_packet.UpdateISA(m_curr_packet.AltISA() ? ocsd_isa_tee : ocsd_isa_thumb2);
889 else if ((addrbyte & 0xA0) == 0x20)
890 m_curr_packet.UpdateISA(ocsd_isa_jazelle);
891 else
892 throwMalformedPacketErr("Malformed Packet - Unknown ISA.");
893 }
894
895 byte5AddrUpdate = true; // need to update the address value from byte 5
896 }
897
898 // figure out the correct ISA shifts for the address bits
899 switch(m_curr_packet.ISA())
900 {
901 case ocsd_isa_thumb2: isa_idx = 1; break;
902 case ocsd_isa_tee: isa_idx = 2; break;
903 case ocsd_isa_jazelle: isa_idx = 3; break;
904 default: break;
905 }
906
907 if(byte5AddrUpdate)
908 {
909 value |= ((uint32_t)(addrbyte & addrMask[isa_idx])) << bitcount;
910 bitcount += addrBits[isa_idx];
911 }
912
913 // finally align according to ISA
914 shift = addrshift[isa_idx];
915 value <<= shift;
916 bitcount += shift;
917
918 nBitsOut = bitcount;
919 return value;
920 }
921
922 // extract exception data from bytes after address.
extractExceptionData()923 void EtmV3PktProcImpl::extractExceptionData()
924 {
925 static const ocsd_armv7_exception exceptionTypesStd[] = {
926 Excp_NoException, Excp_DebugHalt, Excp_SMC, Excp_Hyp,
927 Excp_AsyncDAbort, Excp_Jazelle, Excp_Reserved, Excp_Reserved,
928 Excp_Reset, Excp_Undef, Excp_SVC, Excp_PrefAbort,
929 Excp_SyncDataAbort, Excp_Generic, Excp_IRQ, Excp_FIQ
930 };
931
932 static const ocsd_armv7_exception exceptionTypesCM[] = {
933 Excp_NoException, Excp_CMIRQn, Excp_CMIRQn, Excp_CMIRQn,
934 Excp_CMIRQn, Excp_CMIRQn, Excp_CMIRQn, Excp_CMIRQn,
935 Excp_CMIRQn, Excp_CMUsageFault, Excp_CMNMI, Excp_SVC,
936 Excp_CMDebugMonitor, Excp_CMMemManage, Excp_CMPendSV, Excp_CMSysTick,
937 Excp_Reserved, Excp_Reset, Excp_Reserved, Excp_CMHardFault,
938 Excp_Reserved, Excp_CMBusFault, Excp_Reserved, Excp_Reserved
939 };
940
941 uint16_t exceptionNum = 0;
942 ocsd_armv7_exception excep_type = Excp_Reserved;
943 int resume = 0;
944 int irq_n = 0;
945 bool cancel_prev_instr = 0;
946 bool Byte2 = false;
947
948 checkPktLimits();
949
950 //**** exception info Byte 0
951 uint8_t dataByte = m_currPacketData[m_currPktIdx++];
952
953 m_curr_packet.UpdateNS(dataByte & 0x1);
954 exceptionNum |= (dataByte >> 1) & 0xF;
955 cancel_prev_instr = (dataByte & 0x20) ? true : false;
956 m_curr_packet.UpdateAltISA(((dataByte & 0x40) != 0) ? 1 : 0);
957
958 //** another byte?
959 if(dataByte & 0x80)
960 {
961 checkPktLimits();
962 dataByte = m_currPacketData[m_currPktIdx++];
963
964 if(dataByte & 0x40)
965 Byte2 = true; //** immediate info byte 2, skipping 1
966 else
967 {
968 //**** exception info Byte 1
969 if(m_config.isV7MArch())
970 {
971 exceptionNum |= ((uint16_t)(dataByte & 0x1F)) << 4;
972 }
973 m_curr_packet.UpdateHyp(dataByte & 0x20 ? 1 : 0);
974
975 if(dataByte & 0x80)
976 {
977 checkPktLimits();
978 dataByte = m_currPacketData[m_currPktIdx++];
979 Byte2 = true;
980 }
981 }
982 //**** exception info Byte 2
983 if(Byte2)
984 {
985 resume = dataByte & 0xF;
986 }
987 }
988
989 // set the exception type - according to the number and core profile
990 if(m_config.isV7MArch())
991 {
992 exceptionNum &= 0x1FF;
993 if(exceptionNum < 0x018)
994 excep_type= exceptionTypesCM[exceptionNum];
995 else
996 excep_type = Excp_CMIRQn;
997
998 if(excep_type == Excp_CMIRQn)
999 {
1000 if(exceptionNum > 0x018)
1001 irq_n = exceptionNum - 0x10;
1002 else if(exceptionNum == 0x008)
1003 irq_n = 0;
1004 else
1005 irq_n = exceptionNum;
1006 }
1007 }
1008 else
1009 {
1010 exceptionNum &= 0xF;
1011 excep_type = exceptionTypesStd[exceptionNum];
1012 }
1013 m_curr_packet.SetException(excep_type, exceptionNum, cancel_prev_instr,m_config.isV7MArch(), irq_n,resume);
1014 }
1015
checkPktLimits()1016 void EtmV3PktProcImpl::checkPktLimits()
1017 {
1018 // index running off the end of the packet means a malformed packet.
1019 if(m_currPktIdx >= m_currPacketData.size())
1020 throwMalformedPacketErr("Malformed Packet - oversized packet.");
1021 }
1022
extractCtxtID()1023 uint32_t EtmV3PktProcImpl::extractCtxtID()
1024 {
1025 uint32_t ctxtID = 0;
1026 int size = m_config.CtxtIDBytes();
1027
1028 // check we have enough data
1029 if((m_currPktIdx + size) > m_currPacketData.size())
1030 throwMalformedPacketErr("Too few bytes to extract context ID.");
1031
1032 switch(size)
1033 {
1034 case 1:
1035 ctxtID = (uint32_t)m_currPacketData[m_currPktIdx];
1036 m_currPktIdx++;
1037 break;
1038
1039 case 2:
1040 ctxtID = (uint32_t)m_currPacketData[m_currPktIdx]
1041 | ((uint32_t)m_currPacketData[m_currPktIdx+1]) << 8;
1042 m_currPktIdx+=2;
1043 break;
1044
1045 case 4:
1046 ctxtID = (uint32_t)m_currPacketData[m_currPktIdx]
1047 | ((uint32_t)m_currPacketData[m_currPktIdx+1]) << 8
1048 | ((uint32_t)m_currPacketData[m_currPktIdx+2]) << 16
1049 | ((uint32_t)m_currPacketData[m_currPktIdx+3]) << 24;
1050 m_currPktIdx+=4;
1051 break;
1052 }
1053 return ctxtID;
1054 }
1055
extractTimestamp(uint8_t & tsBits)1056 uint64_t EtmV3PktProcImpl::extractTimestamp(uint8_t &tsBits)
1057 {
1058 uint64_t ts = 0;
1059 unsigned tsMaxBytes = m_config.TSPkt64() ? 9 : 7;
1060 unsigned tsCurrBytes = 0;
1061 bool bCont = true;
1062 uint8_t mask = 0x7F;
1063 uint8_t last_mask = m_config.TSPkt64() ? 0xFF : 0x3F;
1064 uint8_t ts_iter_bits = 7;
1065 uint8_t ts_last_iter_bits = m_config.TSPkt64() ? 8 : 6;
1066 uint8_t currByte;
1067 tsBits = 0;
1068
1069 while((tsCurrBytes < tsMaxBytes) && bCont)
1070 {
1071 if(m_currPacketData.size() < (m_currPktIdx + tsCurrBytes + 1))
1072 throwMalformedPacketErr("Insufficient bytes to extract timestamp.");
1073
1074 currByte = m_currPacketData[m_currPktIdx+tsCurrBytes];
1075 ts |= ((uint64_t)(currByte & mask)) << (7 * tsCurrBytes);
1076 tsCurrBytes++;
1077 tsBits += ts_iter_bits;
1078 bCont = ((0x80 & currByte) == 0x80);
1079 if(tsCurrBytes == (tsMaxBytes - 1))
1080 {
1081 mask = last_mask;
1082 ts_iter_bits = ts_last_iter_bits;
1083 }
1084 }
1085 m_currPktIdx += tsCurrBytes;
1086 return ts;
1087 }
1088
1089
extractDataAddress(uint8_t & bits,bool & updateBE,uint8_t & beVal)1090 uint32_t EtmV3PktProcImpl::extractDataAddress(uint8_t &bits, bool &updateBE, uint8_t &beVal)
1091 {
1092 uint32_t dataAddr = 0;
1093 int bytesIdx = 0;
1094 bool bCont = true;
1095 uint8_t currByte = 0;
1096
1097 updateBE = false;
1098 bits = 0;
1099
1100 while(bCont)
1101 {
1102 checkPktLimits();
1103 currByte = m_currPacketData[m_currPktIdx++] & ((bytesIdx == 4) ? 0x0F : 0x7F);
1104 dataAddr |= (((uint32_t)currByte) << (bytesIdx * 7));
1105 bCont = ((currByte & 0x80) == 0x80);
1106 if(bytesIdx == 4)
1107 {
1108 bits += 4;
1109 updateBE = true;
1110 beVal = ((currByte >> 4) & 0x1);
1111 bCont = false;
1112 }
1113 else
1114 bits+=7;
1115 bytesIdx++;
1116 }
1117 return dataAddr;
1118 }
1119
extractDataValue(const int dataByteSize)1120 uint32_t EtmV3PktProcImpl::extractDataValue(const int dataByteSize)
1121 {
1122 static int bytesReqTable[] = { 0,1,2,4 };
1123
1124 uint32_t dataVal = 0;
1125 int bytesUsed = 0;
1126 int bytesReq = bytesReqTable[dataByteSize & 0x3];
1127 while(bytesUsed < bytesReq)
1128 {
1129 checkPktLimits();
1130 dataVal |= (((uint32_t)m_currPacketData[m_currPktIdx++]) << (bytesUsed * 8));
1131 bytesUsed++;
1132 }
1133 return dataVal;
1134 }
1135
1136
extractCycleCount()1137 uint32_t EtmV3PktProcImpl::extractCycleCount()
1138 {
1139 uint32_t cycleCount = 0;
1140 int byteIdx = 0;
1141 uint8_t mask = 0x7F;
1142 bool bCond = true;
1143 uint8_t currByte = 0;
1144
1145 while(bCond)
1146 {
1147 checkPktLimits();
1148 currByte = m_currPacketData[m_currPktIdx++];
1149 cycleCount |= ((uint32_t)(currByte & mask)) << (7 * byteIdx);
1150 bCond = ((currByte & 0x80) == 0x80);
1151 byteIdx++;
1152
1153 if(byteIdx == 4)
1154 mask = 0x0F;
1155
1156 if(byteIdx == 5)
1157 bCond = false;
1158 }
1159 return cycleCount;
1160 }
1161
OnISyncPacket()1162 void EtmV3PktProcImpl::OnISyncPacket()
1163 {
1164 uint8_t iSyncInfoByte = 0;
1165 uint32_t instrAddr = 0, LSiPAddr = 0;
1166 int LSiPBits = 0;
1167 uint8_t T = 0, J = 0, AltISA = 0;
1168
1169 m_currPktIdx = 1;
1170 if(m_bIsync_got_cycle_cnt)
1171 {
1172 m_curr_packet.SetCycleCount(extractCycleCount());
1173 m_curr_packet.SetISyncHasCC();
1174 }
1175
1176 if(m_config.CtxtIDBytes() != 0)
1177 {
1178 m_curr_packet.UpdateContextID(extractCtxtID());
1179 }
1180
1181 // extract context info
1182 iSyncInfoByte = m_currPacketData[m_currPktIdx++];
1183 m_curr_packet.SetISyncReason((ocsd_iSync_reason)((iSyncInfoByte >> 5) & 0x3));
1184 J = (iSyncInfoByte >> 4) & 0x1;
1185 AltISA = m_config.MinorRev() >= 3 ? (iSyncInfoByte >> 2) & 0x1 : 0;
1186 m_curr_packet.UpdateNS((iSyncInfoByte >> 3) & 0x1);
1187 if(m_config.hasVirtExt())
1188 m_curr_packet.UpdateHyp((iSyncInfoByte >> 1) & 0x1);
1189
1190 // main address value - full 32 bit address value
1191 if(m_config.isInstrTrace())
1192 {
1193 for(int i = 0; i < 4; i++)
1194 instrAddr |= ((uint32_t)m_currPacketData[m_currPktIdx++]) << (8*i);
1195 T = instrAddr & 0x1; // get the T bit.
1196 instrAddr &= ~0x1; // remove from address.
1197 m_curr_packet.UpdateAddress(instrAddr,32);
1198
1199 // enough data now to set the instruction set.
1200 ocsd_isa currISA = ocsd_isa_arm;
1201 if(J)
1202 currISA = ocsd_isa_jazelle;
1203 else if(T)
1204 currISA = AltISA ? ocsd_isa_tee : ocsd_isa_thumb2;
1205 m_curr_packet.UpdateISA(currISA);
1206
1207 // possible follow up address value - rarely uses unless trace enabled during
1208 // load and store instruction executing on top of other instruction.
1209 if(m_bIsync_get_LSiP_addr)
1210 {
1211 LSiPAddr = extractBrAddrPkt(LSiPBits);
1212 // follow up address value is compressed relative to the main value
1213 // we store this in the data address value temporarily.
1214 m_curr_packet.UpdateDataAddress(instrAddr,32);
1215 m_curr_packet.UpdateDataAddress(LSiPAddr,LSiPBits);
1216 }
1217 }
1218 else
1219 m_curr_packet.SetISyncNoAddr();
1220
1221 SendPacket(); // mark ready to send
1222 }
1223
1224 /* End of File trc_pkt_proc_etmv3_impl.cpp */
1225