1 /*
2 * \file trc_pkt_proc_ptm.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 "opencsd/ptm/trc_pkt_proc_ptm.h"
36 #include "opencsd/ptm/trc_cmp_cfg_ptm.h"
37 #include "common/ocsd_error.h"
38
39
40 #ifdef __GNUC__
41 // G++ doesn't like the ## pasting
42 #define PTM_PKTS_NAME "PKTP_PTM"
43 #else
44 // VC++ is OK
45 #define PTM_PKTS_NAME OCSD_CMPNAME_PREFIX_PKTPROC##"_PTM"
46 #endif
47
TrcPktProcPtm()48 TrcPktProcPtm::TrcPktProcPtm() : TrcPktProcBase(PTM_PKTS_NAME)
49 {
50 InitProcessorState();
51 BuildIPacketTable();
52 }
53
TrcPktProcPtm(int instIDNum)54 TrcPktProcPtm::TrcPktProcPtm(int instIDNum) : TrcPktProcBase(PTM_PKTS_NAME, instIDNum)
55 {
56 InitProcessorState();
57 BuildIPacketTable();
58 }
59
~TrcPktProcPtm()60 TrcPktProcPtm::~TrcPktProcPtm()
61 {
62
63 }
64
onProtocolConfig()65 ocsd_err_t TrcPktProcPtm::onProtocolConfig()
66 {
67 ocsd_err_t err = OCSD_ERR_NOT_INIT;
68
69 if(m_config != 0)
70 {
71 m_chanIDCopy = m_config->getTraceID();
72 err = OCSD_OK;
73 }
74 return err;
75 }
76
processData(const ocsd_trc_index_t index,const uint32_t dataBlockSize,const uint8_t * pDataBlock,uint32_t * numBytesProcessed)77 ocsd_datapath_resp_t TrcPktProcPtm::processData( const ocsd_trc_index_t index,
78 const uint32_t dataBlockSize,
79 const uint8_t *pDataBlock,
80 uint32_t *numBytesProcessed)
81 {
82 ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
83 uint8_t currByte = 0;
84
85 m_dataInProcessed = 0;
86
87 if(!checkInit())
88 {
89 resp = OCSD_RESP_FATAL_NOT_INIT;
90 }
91 else
92 {
93 m_pDataIn = pDataBlock;
94 m_dataInLen = dataBlockSize;
95 m_block_idx = index; // index start for current block
96 }
97
98 while( ( ( m_dataInProcessed < dataBlockSize) ||
99 (( m_dataInProcessed == dataBlockSize) && (m_process_state == SEND_PKT)) ) &&
100 OCSD_DATA_RESP_IS_CONT(resp))
101 {
102 try
103 {
104 switch(m_process_state)
105 {
106 case WAIT_SYNC:
107 if(!m_waitASyncSOPkt)
108 {
109 m_curr_pkt_index = m_block_idx + m_dataInProcessed;
110 m_curr_packet.type = PTM_PKT_NOTSYNC;
111 m_bAsyncRawOp = hasRawMon();
112 }
113 resp = waitASync();
114 break;
115
116 case PROC_HDR:
117 m_curr_pkt_index = m_block_idx + m_dataInProcessed;
118 if(readByte(currByte))
119 {
120 m_pIPktFn = m_i_table[currByte].pptkFn;
121 m_curr_packet.type = m_i_table[currByte].pkt_type;
122 }
123 else
124 {
125 // sequencing error - should not get to the point where readByte
126 // fails and m_DataInProcessed < dataBlockSize
127 // throw data overflow error
128 throw ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_PKT_INTERP_FAIL,m_curr_pkt_index,this->m_chanIDCopy,"Data Buffer Overrun");
129 }
130 m_process_state = PROC_DATA;
131
132 case PROC_DATA:
133 (this->*m_pIPktFn)();
134 break;
135
136 case SEND_PKT:
137 resp = outputPacket();
138 InitPacketState();
139 m_process_state = PROC_HDR;
140 break;
141 }
142 }
143 catch(ocsdError &err)
144 {
145 LogError(err);
146 if( (err.getErrorCode() == OCSD_ERR_BAD_PACKET_SEQ) ||
147 (err.getErrorCode() == OCSD_ERR_INVALID_PCKT_HDR))
148 {
149 // send invalid packets up the pipe to let the next stage decide what to do.
150 m_process_state = SEND_PKT;
151 }
152 else
153 {
154 // bail out on any other error.
155 resp = OCSD_RESP_FATAL_INVALID_DATA;
156 }
157 }
158 catch(...)
159 {
160 /// vv bad at this point.
161 resp = OCSD_RESP_FATAL_SYS_ERR;
162 const ocsdError &fatal = ocsdError(OCSD_ERR_SEV_ERROR,OCSD_ERR_FAIL,m_curr_pkt_index,m_chanIDCopy,"Unknown System Error decoding trace.");
163 LogError(fatal);
164 }
165
166 }
167 *numBytesProcessed = m_dataInProcessed;
168 return resp;
169 }
170
onEOT()171 ocsd_datapath_resp_t TrcPktProcPtm::onEOT()
172 {
173 ocsd_datapath_resp_t err = OCSD_RESP_FATAL_NOT_INIT;
174 if(checkInit())
175 {
176 err = OCSD_RESP_CONT;
177 if(m_currPacketData.size() > 0)
178 {
179 m_curr_packet.SetErrType(PTM_PKT_INCOMPLETE_EOT);
180 err = outputPacket();
181 }
182 }
183 return err;
184 }
185
onReset()186 ocsd_datapath_resp_t TrcPktProcPtm::onReset()
187 {
188 ocsd_datapath_resp_t err = OCSD_RESP_FATAL_NOT_INIT;
189 if(checkInit())
190 {
191 InitProcessorState();
192 err = OCSD_RESP_CONT;
193 }
194 return err;
195 }
196
onFlush()197 ocsd_datapath_resp_t TrcPktProcPtm::onFlush()
198 {
199 ocsd_datapath_resp_t err = OCSD_RESP_FATAL_NOT_INIT;
200 if(checkInit())
201 {
202 err = OCSD_RESP_CONT;
203 }
204 return err;
205 }
206
isBadPacket() const207 const bool TrcPktProcPtm::isBadPacket() const
208 {
209 return m_curr_packet.isBadPacket();
210 }
211
InitPacketState()212 void TrcPktProcPtm::InitPacketState()
213 {
214 m_curr_packet.Clear();
215
216 }
217
InitProcessorState()218 void TrcPktProcPtm::InitProcessorState()
219 {
220 m_curr_packet.SetType(PTM_PKT_NOTSYNC);
221 m_pIPktFn = &TrcPktProcPtm::pktReserved;
222 m_process_state = WAIT_SYNC;
223 m_async_0 = 0;
224 m_waitASyncSOPkt = false;
225 m_bAsyncRawOp = false;
226 m_bOPNotSyncPkt = false;
227 m_excepAltISA = 0;
228
229 m_curr_packet.ResetState();
230 InitPacketState();
231 }
232
readByte(uint8_t & currByte)233 const bool TrcPktProcPtm::readByte(uint8_t &currByte)
234 {
235 bool bValidByte = false;
236
237 if(m_dataInProcessed < m_dataInLen)
238 {
239 currByte = m_pDataIn[m_dataInProcessed++];
240 m_currPacketData.push_back(currByte);
241 bValidByte = true;
242 }
243 return bValidByte;
244 }
245
unReadByte()246 void TrcPktProcPtm::unReadByte()
247 {
248 m_dataInProcessed--;
249 m_currPacketData.pop_back();
250 }
251
outputPacket()252 ocsd_datapath_resp_t TrcPktProcPtm::outputPacket()
253 {
254 ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
255 resp = outputOnAllInterfaces(m_curr_pkt_index,&m_curr_packet,&m_curr_packet.type,m_currPacketData);
256 m_currPacketData.clear();
257 return resp;
258 }
259
260 /*** sync and packet functions ***/
waitASync()261 ocsd_datapath_resp_t TrcPktProcPtm::waitASync()
262 {
263 ocsd_datapath_resp_t resp = OCSD_RESP_CONT;
264
265 // looking for possible patterns in input buffer:-
266 // a) ASYNC @ start : 00 00 00 00 00 80
267 // b) unsync then async: xx xx xx xx xx xx xx xx 00 00 00 00 00 80
268 // c) unsync (may have 00) xx xx xx xx 00 xx xx 00 00 00 xx xx xx xx
269 // d) unsync then part async: xx xx xx xx xx xx xx xx xx xx xx 00 00 00
270 // e) unsync with prev part async [00 00 00] 00 xx xx xx xx xx xx xx xx [] = byte in previous input buffer
271
272 // bytes to read before throwing an unsynced packet
273 #define UNSYNC_PKT_MAX 16
274 static const uint8_t spare_zeros[] = { 0,0,0,0,0,0,0,0,
275 0,0,0,0,0,0,0,0 };
276
277 bool doScan = true;
278 bool bSendUnsyncedData = false;
279 bool bHaveASync = false;
280 int unsynced_bytes = 0;
281 int unsync_scan_block_start = 0;
282 int pktBytesOnEntry = m_currPacketData.size(); // did we have part of a potential async last time?
283
284 while(doScan && OCSD_DATA_RESP_IS_CONT(resp))
285 {
286 // may have spotted the start of an async
287 if(m_waitASyncSOPkt == true)
288 {
289 switch(findAsync())
290 {
291 case ASYNC:
292 case ASYNC_EXTRA_0:
293 m_process_state = SEND_PKT;
294 m_waitASyncSOPkt = false;
295 bSendUnsyncedData = true;
296 bHaveASync = true;
297 doScan = false;
298 break;
299
300 case THROW_0:
301 // remove a bunch of 0s
302 unsynced_bytes += ASYNC_PAD_0_LIMIT;
303 m_waitASyncSOPkt = false;
304 m_currPacketData.erase( m_currPacketData.begin(), m_currPacketData.begin()+ASYNC_PAD_0_LIMIT);
305 break;
306
307 case NOT_ASYNC:
308 unsynced_bytes += m_currPacketData.size();
309 m_waitASyncSOPkt = false;
310 m_currPacketData.clear();
311 break;
312
313 case ASYNC_INCOMPLETE:
314 bSendUnsyncedData = true;
315 doScan = false;
316 break;
317 }
318 }
319 else
320 {
321 if(m_pDataIn[m_dataInProcessed++] == 0x00)
322 {
323 m_waitASyncSOPkt = true;
324 m_currPacketData.push_back(0);
325 m_async_0 = 1;
326 }
327 else
328 {
329 unsynced_bytes++;
330 }
331 }
332
333 // may need to send some unsynced data here, either if we have enought to make it worthwhile,
334 // or are at the end of the buffer.
335 if(unsynced_bytes >= UNSYNC_PKT_MAX)
336 bSendUnsyncedData = true;
337
338 if(m_dataInProcessed == m_dataInLen)
339 {
340 bSendUnsyncedData = true;
341 doScan = false; // no more data available - stop the scan
342 }
343
344 // will send any unsynced data
345 if(bSendUnsyncedData && (unsynced_bytes > 0))
346 {
347 if(m_bAsyncRawOp)
348 {
349 // there were some 0's in the packet buyffer from the last pass that are no longer in the raw buffer,
350 // and these turned out not to be an async
351 if(pktBytesOnEntry)
352 {
353 outputRawPacketToMonitor(m_curr_pkt_index,&m_curr_packet,pktBytesOnEntry,spare_zeros);
354 m_curr_pkt_index += pktBytesOnEntry;
355 }
356 outputRawPacketToMonitor(m_curr_pkt_index,&m_curr_packet,unsynced_bytes,m_pDataIn+unsync_scan_block_start);
357 }
358 if (!m_bOPNotSyncPkt)
359 {
360 resp = outputDecodedPacket(m_curr_pkt_index, &m_curr_packet);
361 m_bOPNotSyncPkt = true;
362 }
363 unsync_scan_block_start += unsynced_bytes;
364 m_curr_pkt_index+= unsynced_bytes;
365 unsynced_bytes = 0;
366 bSendUnsyncedData = false;
367 }
368
369 // mark next packet as the ASYNC we are looking for.
370 if(bHaveASync)
371 m_curr_packet.SetType(PTM_PKT_A_SYNC);
372 }
373
374 return resp;
375 }
376
pktASync()377 void TrcPktProcPtm::pktASync()
378 {
379 if(m_currPacketData.size() == 1) // header byte
380 {
381 m_async_0 = 1;
382 }
383
384 switch(findAsync())
385 {
386 case ASYNC:
387 case ASYNC_EXTRA_0:
388 m_process_state = SEND_PKT;
389 break;
390
391 case THROW_0:
392 case NOT_ASYNC:
393 throwMalformedPacketErr("Bad Async packet");
394 break;
395
396 case ASYNC_INCOMPLETE:
397 break;
398
399 }
400 }
401
findAsync()402 TrcPktProcPtm::async_result_t TrcPktProcPtm::findAsync()
403 {
404 async_result_t async_res = NOT_ASYNC;
405 bool bFound = false; // found non-zero byte in sequence
406 bool bByteAvail = true;
407 uint8_t currByte;
408
409 while(!bFound && bByteAvail)
410 {
411 if(readByte(currByte))
412 {
413 if(currByte == 0x00)
414 {
415 m_async_0++;
416 if(m_async_0 >= (ASYNC_PAD_0_LIMIT + ASYNC_REQ_0))
417 {
418 bFound = true;
419 async_res = THROW_0;
420 }
421 }
422 else
423 {
424 if(currByte == 0x80)
425 {
426 if(m_async_0 == 5)
427 async_res = ASYNC;
428 else if(m_async_0 > 5)
429 async_res = ASYNC_EXTRA_0;
430 }
431 bFound = true;
432 }
433 }
434 else
435 {
436 bByteAvail = false;
437 async_res = ASYNC_INCOMPLETE;
438 }
439 }
440 return async_res;
441 }
442
pktISync()443 void TrcPktProcPtm::pktISync()
444 {
445 uint8_t currByte = 0;
446 int pktIndex = m_currPacketData.size() - 1;
447 bool bGotBytes = false, validByte = true;
448
449 if(pktIndex == 0)
450 {
451 m_numCtxtIDBytes = m_config->CtxtIDBytes();
452 m_gotCtxtIDBytes = 0;
453
454 // total bytes = 6 + ctxtID; (perhaps more later)
455 m_numPktBytesReq = 6 + m_numCtxtIDBytes;
456 }
457
458 while(validByte && !bGotBytes)
459 {
460 if(readByte(currByte))
461 {
462 pktIndex = m_currPacketData.size() - 1;
463 if(pktIndex == 5)
464 {
465 // got the info byte
466 int altISA = (currByte >> 2) & 0x1;
467 int reason = (currByte >> 5) & 0x3;
468 m_curr_packet.SetISyncReason((ocsd_iSync_reason)(reason));
469 m_curr_packet.UpdateNS((currByte >> 3) & 0x1);
470 m_curr_packet.UpdateAltISA((currByte >> 2) & 0x1);
471 m_curr_packet.UpdateHyp((currByte >> 1) & 0x1);
472
473 ocsd_isa isa = ocsd_isa_arm;
474 if(m_currPacketData[1] & 0x1)
475 isa = altISA ? ocsd_isa_tee : ocsd_isa_thumb2;
476 m_curr_packet.UpdateISA(isa);
477
478 // check cycle count required - not if reason == 0;
479 m_needCycleCount = (reason != 0) ? m_config->enaCycleAcc() : false;
480 m_gotCycleCount = false;
481 m_numPktBytesReq += (m_needCycleCount ? 1 : 0);
482 m_gotCCBytes = 0;
483
484 }
485 else if(pktIndex > 5)
486 {
487 // cycle count appears first if present
488 if(m_needCycleCount && !m_gotCycleCount)
489 {
490 if(pktIndex == 6)
491 m_gotCycleCount = (bool)((currByte & 0x40) == 0); // no cont bit, got cycle count
492 else
493 m_gotCycleCount = ((currByte & 0x80) == 0) || (pktIndex == 10);
494
495 m_gotCCBytes++; // count the cycle count bytes for later use.
496 if(!m_gotCycleCount) // need more cycle count bytes
497 m_numPktBytesReq++;
498 }
499 // then context ID if present.
500 else if( m_numCtxtIDBytes > m_gotCtxtIDBytes)
501 {
502 m_gotCtxtIDBytes++;
503 }
504 }
505
506 // check if we have enough bytes
507 bGotBytes = (bool)((unsigned)m_numPktBytesReq == m_currPacketData.size());
508 }
509 else
510 validByte = false; // no byte available, exit.
511 }
512
513 if(bGotBytes)
514 {
515 // extract address value, cycle count and ctxt id.
516 uint32_t cycleCount = 0;
517 uint32_t ctxtID = 0;
518 int optIdx = 6; // start index for optional elements.
519
520 // address is always full fixed 32 bit value
521 uint32_t address = ((uint32_t)m_currPacketData[1]) & 0xFE;
522 address |= ((uint32_t)m_currPacketData[2]) << 8;
523 address |= ((uint32_t)m_currPacketData[3]) << 16;
524 address |= ((uint32_t)m_currPacketData[4]) << 24;
525 m_curr_packet.UpdateAddress(address,32);
526
527 if(m_needCycleCount)
528 {
529 extractCycleCount(optIdx,cycleCount);
530 m_curr_packet.SetCycleCount(cycleCount);
531 optIdx+=m_gotCCBytes;
532 }
533
534 if(m_numCtxtIDBytes)
535 {
536 extractCtxtID(optIdx,ctxtID);
537 m_curr_packet.UpdateContextID(ctxtID);
538 }
539 m_process_state = SEND_PKT;
540 }
541 }
542
pktTrigger()543 void TrcPktProcPtm::pktTrigger()
544 {
545 m_process_state = SEND_PKT; // no payload
546 }
547
pktWPointUpdate()548 void TrcPktProcPtm::pktWPointUpdate()
549 {
550 bool bDone = false;
551 bool bBytesAvail = true;
552 uint8_t currByte = 0;
553 int byteIdx = 0;
554
555 if(m_currPacketData.size() == 1)
556 {
557 m_gotAddrBytes = false; // flag to indicate got all needed address bytes
558 m_numAddrBytes = 0; // number of address bytes so far - in this case header is not part of the address
559
560 m_gotExcepBytes = false; // mark as not got all required exception bytes thus far
561 m_numExcepBytes = 0; // 0 read in
562
563 m_addrPktIsa = ocsd_isa_unknown; // not set by this packet as yet
564 }
565
566 // collect all the bytes needed
567 while(!bDone && bBytesAvail)
568 {
569 if(readByte(currByte))
570 {
571 byteIdx = m_currPacketData.size() - 1;
572 if(!m_gotAddrBytes)
573 {
574 if(byteIdx < 4)
575 {
576 // address bytes 1 - 4;
577 // ISA stays the same
578 if((currByte & 0x80) == 0x00)
579 {
580 // no further bytes
581 m_gotAddrBytes = true;
582 bDone = true;
583 m_gotExcepBytes = true;
584 }
585 }
586 else
587 {
588 // 5th address byte - determine ISA from this.
589 if((currByte & 0x40) == 0x00)
590 m_gotExcepBytes = true; // no exception bytes - mark as done
591 m_gotAddrBytes = true;
592 bDone = m_gotExcepBytes;
593
594 m_addrPktIsa = ocsd_isa_arm; // assume ARM, but then check
595 if((currByte & 0x20) == 0x20) // bit 5 == 1'b1 - jazelle, bits 4 & 3 part of address.
596 m_addrPktIsa = ocsd_isa_jazelle;
597 else if((currByte & 0x30) == 0x10) // bit [5:4] == 2'b01 - thumb, bit 3 part of address.
598 m_addrPktIsa = ocsd_isa_thumb2;
599 }
600 m_numAddrBytes++;
601 }
602 else if(!m_gotExcepBytes)
603 {
604 // excep byte is actually a WP update byte.
605 m_excepAltISA = ((currByte & 0x40) == 0x40) ? 1 : 0;
606 m_gotExcepBytes = true;
607 m_numExcepBytes++;
608 bDone = true;
609 }
610 }
611 else
612 bBytesAvail = false;
613 }
614
615 // analyse the bytes to create the packet
616 if(bDone)
617 {
618 // ISA for the packet
619 if(m_addrPktIsa == ocsd_isa_unknown) // unchanged by trace packet
620 m_addrPktIsa = m_curr_packet.getISA(); // same as prev
621
622 if(m_gotExcepBytes) // may adjust according to alt ISA in exception packet
623 {
624 if((m_addrPktIsa == ocsd_isa_tee) && (m_excepAltISA == 0))
625 m_addrPktIsa = ocsd_isa_thumb2;
626 else if((m_addrPktIsa == ocsd_isa_thumb2) && (m_excepAltISA == 1))
627 m_addrPktIsa = ocsd_isa_tee;
628 }
629 m_curr_packet.UpdateISA(m_addrPktIsa); // mark ISA in packet (update changes current and prev to dectect an ISA change).
630
631 uint8_t total_bits = 0;
632 uint32_t addr_val = extractAddress(1,total_bits);
633 m_curr_packet.UpdateAddress(addr_val,total_bits);
634 m_process_state = SEND_PKT;
635 }
636 }
637
pktIgnore()638 void TrcPktProcPtm::pktIgnore()
639 {
640 m_process_state = SEND_PKT; // no payload
641 }
642
pktCtxtID()643 void TrcPktProcPtm::pktCtxtID()
644 {
645 int pktIndex = m_currPacketData.size() - 1;
646
647 // if at the header, determine how many more bytes we need.
648 if(pktIndex == 0)
649 {
650 m_numCtxtIDBytes = m_config->CtxtIDBytes();
651 m_gotCtxtIDBytes = 0;
652 }
653
654 // read the necessary ctxtID bytes from the stream
655 bool bGotBytes = false, bytesAvail = true;
656 uint32_t ctxtID = 0;
657
658 bGotBytes = m_numCtxtIDBytes == m_gotCtxtIDBytes;
659 while(!bGotBytes & bytesAvail)
660 {
661 bytesAvail = readByte();
662 if(bytesAvail)
663 m_gotCtxtIDBytes++;
664 bGotBytes = m_numCtxtIDBytes == m_gotCtxtIDBytes;
665 }
666
667 if(bGotBytes)
668 {
669 if(m_numCtxtIDBytes)
670 {
671 extractCtxtID(1,ctxtID);
672 }
673 m_curr_packet.UpdateContextID(ctxtID);
674 m_process_state = SEND_PKT;
675 }
676 }
677
pktVMID()678 void TrcPktProcPtm::pktVMID()
679 {
680 uint8_t currByte;
681
682 // just need a single payload byte...
683 if(readByte(currByte))
684 {
685 m_curr_packet.UpdateVMID(currByte);
686 m_process_state = SEND_PKT;
687 }
688 }
689
pktAtom()690 void TrcPktProcPtm::pktAtom()
691 {
692 uint8_t pHdr = m_currPacketData[0];
693
694 if(!m_config->enaCycleAcc())
695 {
696 m_curr_packet.SetAtomFromPHdr(pHdr);
697 m_process_state = SEND_PKT;
698 }
699 else
700 {
701 bool bGotAllPktBytes = false, byteAvail = true;
702 uint8_t currByte = 0; // cycle accurate tracing -> atom + cycle count
703
704 if(!(pHdr & 0x40))
705 {
706 // only the header byte present
707 bGotAllPktBytes = true;
708 }
709 else
710 {
711 // up to 4 additional bytes of count data.
712 while(byteAvail && !bGotAllPktBytes)
713 {
714 if(readByte(currByte))
715 {
716 if(!(currByte & 0x80) || (m_currPacketData.size() == 5))
717 bGotAllPktBytes = true;
718 }
719 else
720 byteAvail = false;
721 }
722 }
723
724 // we have all the bytes for a cycle accurate packet.
725 if(bGotAllPktBytes)
726 {
727 uint32_t cycleCount = 0;
728 extractCycleCount(0,cycleCount);
729 m_curr_packet.SetCycleCount(cycleCount);
730 m_curr_packet.SetCycleAccAtomFromPHdr(pHdr);
731 m_process_state = SEND_PKT;
732 }
733 }
734 }
735
pktTimeStamp()736 void TrcPktProcPtm::pktTimeStamp()
737 {
738 uint8_t currByte = 0;
739 int pktIndex = m_currPacketData.size() - 1;
740 bool bGotBytes = false, byteAvail = true;
741
742 if(pktIndex == 0)
743 {
744 m_gotTSBytes = false;
745 m_needCycleCount = m_config->enaCycleAcc();
746 m_gotCCBytes = 0;
747
748 // max byte buffer size for full ts packet
749 m_tsByteMax = m_config->TSPkt64() ? 10 : 8;
750 }
751
752 while(byteAvail && !bGotBytes)
753 {
754 if(readByte(currByte))
755 {
756 if(!m_gotTSBytes)
757 {
758 if(((currByte & 0x80) == 0) || (m_currPacketData.size() == (unsigned)m_tsByteMax))
759 {
760 m_gotTSBytes = true;
761 if(!m_needCycleCount)
762 bGotBytes = true;
763 }
764 }
765 else
766 {
767 uint8_t cc_cont_mask = 0x80;
768 // got TS bytes, collect cycle count
769 if(m_gotCCBytes == 0)
770 cc_cont_mask = 0x40;
771 if((currByte & cc_cont_mask) == 0)
772 bGotBytes = true;
773 m_gotCCBytes++;
774 if(m_gotCCBytes == 5)
775 bGotBytes = true;
776 }
777 }
778 else
779 byteAvail = false;
780 }
781
782 if(bGotBytes)
783 {
784 uint64_t tsVal = 0;
785 uint32_t cycleCount = 0;
786 uint8_t tsUpdateBits = 0;
787 int ts_end_idx = extractTS(tsVal,tsUpdateBits);
788 if(m_needCycleCount)
789 {
790 extractCycleCount(ts_end_idx,cycleCount);
791 m_curr_packet.SetCycleCount(cycleCount);
792 }
793 m_curr_packet.UpdateTimestamp(tsVal,tsUpdateBits);
794 m_process_state = SEND_PKT;
795 }
796 }
797
pktExceptionRet()798 void TrcPktProcPtm::pktExceptionRet()
799 {
800 m_process_state = SEND_PKT; // no payload
801 }
802
pktBranchAddr()803 void TrcPktProcPtm::pktBranchAddr()
804 {
805 uint8_t currByte = m_currPacketData[0];
806 bool bDone = false;
807 bool bBytesAvail = true;
808 int byteIdx = 0;
809
810 if(m_currPacketData.size() == 1)
811 {
812 m_gotAddrBytes = false; // flag to indicate got all needed address bytes
813 m_numAddrBytes = 1; // number of address bytes so far
814
815 m_needCycleCount = m_config->enaCycleAcc(); // check if we have a cycle count
816 m_gotCCBytes = 0; // number of cc bytes read in so far.
817
818 m_gotExcepBytes = false; // mark as not got all required exception bytes thus far
819 m_numExcepBytes = 0; // 0 read in
820
821 m_addrPktIsa = ocsd_isa_unknown; // not set by this packet as yet
822
823 // header is also 1st address byte
824 if((currByte & 0x80) == 0) // could be single byte packet
825 {
826 m_gotAddrBytes = true;
827 if(!m_needCycleCount)
828 bDone = true; // all done if no cycle count
829 m_gotExcepBytes = true; // cannot have exception bytes following single byte packet
830 }
831
832 }
833
834 // collect all the bytes needed
835 while(!bDone && bBytesAvail)
836 {
837 if(readByte(currByte))
838 {
839 byteIdx = m_currPacketData.size() - 1;
840 if(!m_gotAddrBytes)
841 {
842 if(byteIdx < 4)
843 {
844 // address bytes 2 - 4;
845 // ISA stays the same
846 if((currByte & 0x80) == 0x00)
847 {
848 // no further bytes
849 if((currByte & 0x40) == 0x00)
850 m_gotExcepBytes = true; // no exception bytes - mark as done
851 m_gotAddrBytes = true;
852 bDone = m_gotExcepBytes && !m_needCycleCount;
853 }
854 }
855 else
856 {
857 // 5th address byte - determine ISA from this.
858 if((currByte & 0x40) == 0x00)
859 m_gotExcepBytes = true; // no exception bytes - mark as done
860 m_gotAddrBytes = true;
861 bDone = m_gotExcepBytes && !m_needCycleCount;
862
863 m_addrPktIsa = ocsd_isa_arm; // assume ARM, but then check
864 if((currByte & 0x20) == 0x20) // bit 5 == 1'b1 - jazelle, bits 4 & 3 part of address.
865 m_addrPktIsa = ocsd_isa_jazelle;
866 else if((currByte & 0x30) == 0x10) // bit [5:4] == 2'b01 - thumb, bit 3 part of address.
867 m_addrPktIsa = ocsd_isa_thumb2;
868 }
869 m_numAddrBytes++;
870 }
871 else if(!m_gotExcepBytes)
872 {
873 // may need exception bytes
874 if(m_numExcepBytes == 0)
875 {
876 if((currByte & 0x80) == 0x00)
877 m_gotExcepBytes = true;
878 m_excepAltISA = ((currByte & 0x40) == 0x40) ? 1 : 0;
879 }
880 else
881 m_gotExcepBytes = true;
882 m_numExcepBytes++;
883
884 if(m_gotExcepBytes && !m_needCycleCount)
885 bDone = true;
886
887 }
888 else if(m_needCycleCount)
889 {
890 // not done after exception bytes, collect cycle count
891 if(m_gotCCBytes == 0)
892 {
893 bDone = ((currByte & 0x40) == 0x00 );
894 }
895 else
896 {
897 // done if no more or 5th byte
898 bDone = (((currByte & 0x80) == 0x00 ) || (m_gotCCBytes == 4));
899 }
900 m_gotCCBytes++;
901 }
902 else
903 // this should never be reached.
904 throwMalformedPacketErr("sequencing error analysing branch packet");
905 }
906 else
907 bBytesAvail = false;
908 }
909
910 // analyse the bytes to create the packet
911 if(bDone)
912 {
913 // ISA for the packet
914 if(m_addrPktIsa == ocsd_isa_unknown) // unchanged by trace packet
915 m_addrPktIsa = m_curr_packet.getISA(); // same as prev
916
917 if(m_gotExcepBytes) // may adjust according to alt ISA in exception packet
918 {
919 if((m_addrPktIsa == ocsd_isa_tee) && (m_excepAltISA == 0))
920 m_addrPktIsa = ocsd_isa_thumb2;
921 else if((m_addrPktIsa == ocsd_isa_thumb2) && (m_excepAltISA == 1))
922 m_addrPktIsa = ocsd_isa_tee;
923 }
924 m_curr_packet.UpdateISA(m_addrPktIsa); // mark ISA in packet (update changes current and prev to dectect an ISA change).
925
926
927
928 // we know the ISA, we can correctly interpret the address.
929 uint8_t total_bits = 0;
930 uint32_t addr_val = extractAddress(0,total_bits);
931 m_curr_packet.UpdateAddress(addr_val,total_bits);
932
933 if(m_numExcepBytes > 0)
934 {
935 uint8_t E1 = m_currPacketData[m_numAddrBytes];
936 uint16_t ENum = (E1 >> 1) & 0xF;
937 ocsd_armv7_exception excep = Excp_Reserved;
938
939 m_curr_packet.UpdateNS(E1 & 0x1);
940 if(m_numExcepBytes > 1)
941 {
942 uint8_t E2 = m_currPacketData[m_numAddrBytes+1];
943 m_curr_packet.UpdateHyp((E2 >> 5) & 0x1);
944 ENum |= ((uint16_t)(E2 & 0x1F) << 4);
945 }
946
947 if(ENum <= 0xF)
948 {
949 static ocsd_armv7_exception v7ARExceptions[16] = {
950 Excp_NoException, Excp_DebugHalt, Excp_SMC, Excp_Hyp,
951 Excp_AsyncDAbort, Excp_ThumbEECheckFail, Excp_Reserved, Excp_Reserved,
952 Excp_Reset, Excp_Undef, Excp_SVC, Excp_PrefAbort,
953 Excp_SyncDataAbort, Excp_Generic, Excp_IRQ, Excp_FIQ
954 };
955 excep = v7ARExceptions[ENum];
956 }
957 m_curr_packet.SetException(excep,ENum);
958 }
959
960 if(m_needCycleCount)
961 {
962 int countIdx = m_numAddrBytes + m_numExcepBytes;
963 uint32_t cycleCount = 0;
964 extractCycleCount(countIdx,cycleCount);
965 m_curr_packet.SetCycleCount(cycleCount);
966 }
967 m_process_state = SEND_PKT;
968 }
969 }
970
pktReserved()971 void TrcPktProcPtm::pktReserved()
972 {
973 m_process_state = SEND_PKT; // no payload
974 }
975
extractCtxtID(int idx,uint32_t & ctxtID)976 void TrcPktProcPtm::extractCtxtID(int idx, uint32_t &ctxtID)
977 {
978 ctxtID = 0;
979 int shift = 0;
980 for(int i=0; i < m_numCtxtIDBytes; i++)
981 {
982 if((size_t)idx+i >= m_currPacketData.size())
983 throwMalformedPacketErr("Insufficient packet bytes for Context ID value.");
984 ctxtID |= ((uint32_t)m_currPacketData[idx+i]) << shift;
985 shift+=8;
986 }
987 }
988
extractCycleCount(int offset,uint32_t & cycleCount)989 void TrcPktProcPtm::extractCycleCount(int offset, uint32_t &cycleCount)
990 {
991 bool bCont = true;
992 cycleCount = 0;
993 int by_idx = 0;
994 uint8_t currByte;
995 int shift = 4;
996
997 while(bCont)
998 {
999 if((size_t)by_idx+offset >= m_currPacketData.size())
1000 throwMalformedPacketErr("Insufficient packet bytes for Cycle Count value.");
1001
1002 currByte = m_currPacketData[offset+by_idx];
1003 if(by_idx == 0)
1004 {
1005 bCont = (currByte & 0x40) != 0;
1006 cycleCount = (currByte >> 2) & 0xF;
1007 }
1008 else
1009 {
1010
1011 bCont = (currByte & 0x80) != 0;
1012 if(by_idx == 4)
1013 bCont = false;
1014 cycleCount |= (((uint32_t)(currByte & 0x7F)) << shift);
1015 shift += 7;
1016 }
1017 by_idx++;
1018 }
1019 }
1020
extractTS(uint64_t & tsVal,uint8_t & tsUpdateBits)1021 int TrcPktProcPtm::extractTS(uint64_t &tsVal,uint8_t &tsUpdateBits)
1022 {
1023 bool bCont = true;
1024 int tsIdx = 1; // start index;
1025 uint8_t byteVal;
1026 bool b64BitVal = m_config->TSPkt64();
1027 int shift = 0;
1028
1029 tsVal = 0;
1030 tsUpdateBits = 0;
1031
1032 while(bCont)
1033 {
1034 if((size_t)tsIdx >= m_currPacketData.size())
1035 throwMalformedPacketErr("Insufficient packet bytes for Timestamp value.");
1036
1037 byteVal = m_currPacketData[tsIdx];
1038
1039 if(b64BitVal)
1040 {
1041 if(tsIdx < 9)
1042 {
1043 bCont = ((byteVal & 0x80) == 0x80);
1044 byteVal &= 0x7F;
1045 tsUpdateBits += 7;
1046 }
1047 else
1048 {
1049 bCont = false;
1050 tsUpdateBits += 8;
1051 }
1052 }
1053 else
1054 {
1055 if(tsIdx < 7)
1056 {
1057 bCont = ((byteVal & 0x80) == 0x80);
1058 byteVal &= 0x7F;
1059 tsUpdateBits += 7;
1060 }
1061 else
1062 {
1063 byteVal &=0x3F;
1064 bCont = false;
1065 tsUpdateBits += 6;
1066 }
1067 }
1068 tsVal |= (((uint64_t)byteVal) << shift);
1069 tsIdx++;
1070 shift += 7;
1071 }
1072 return tsIdx; // return next byte index in packet.
1073 }
1074
extractAddress(const int offset,uint8_t & total_bits)1075 uint32_t TrcPktProcPtm::extractAddress(const int offset, uint8_t &total_bits)
1076 {
1077 // we know the ISA, we can correctly interpret the address.
1078 uint32_t addr_val = 0;
1079 uint8_t mask = 0x7E; // first byte mask (always);
1080 uint8_t num_bits = 0x7; // number of bits in the 1st byte (thumb);
1081 int shift = 0;
1082 int next_shift = 0;
1083
1084 total_bits = 0;
1085
1086 for(int i = 0; i < m_numAddrBytes; i++)
1087 {
1088 if(i == 4)
1089 {
1090 // 5th byte mask
1091 mask = 0x0f; // thumb mask;
1092 num_bits = 4;
1093 if(m_addrPktIsa == ocsd_isa_jazelle)
1094 {
1095 mask = 0x1F;
1096 num_bits = 5;
1097 }
1098 else if(m_addrPktIsa == ocsd_isa_arm)
1099 {
1100 mask = 0x07;
1101 num_bits = 3;
1102 }
1103 }
1104 else if(i > 0)
1105 {
1106 mask = 0x7F;
1107 num_bits = 7;
1108 // check for last byte but not 1st or 5th byte mask
1109 if(i == m_numAddrBytes-1)
1110 {
1111 mask = 0x3F;
1112 num_bits = 6;
1113 }
1114 }
1115
1116 // extract data
1117 shift = next_shift;
1118 addr_val |= ((uint32_t)(m_currPacketData[i+offset] & mask) << shift);
1119 total_bits += num_bits;
1120
1121 // how much we shift the next value
1122 if(i == 0)
1123 {
1124 if(m_addrPktIsa == ocsd_isa_jazelle)
1125 {
1126 addr_val >>= 1;
1127 next_shift = 6;
1128 total_bits--; // adjust bits for jazelle offset
1129 }
1130 else
1131 {
1132 next_shift = 7;
1133 }
1134 }
1135 else
1136 {
1137 next_shift += 7;
1138 }
1139 }
1140
1141 if(m_addrPktIsa == ocsd_isa_arm)
1142 {
1143 addr_val <<= 1; // shift one extra bit for ARM address alignment.
1144 total_bits++;
1145 }
1146 return addr_val;
1147 }
1148
1149
BuildIPacketTable()1150 void TrcPktProcPtm::BuildIPacketTable()
1151 {
1152 // initialise all to branch, atom or reserved packet header
1153 for(unsigned i = 0; i < 256; i++)
1154 {
1155 // branch address packets all end in 8'bxxxxxxx1
1156 if((i & 0x01) == 0x01)
1157 {
1158 m_i_table[i].pkt_type = PTM_PKT_BRANCH_ADDRESS;
1159 m_i_table[i].pptkFn = &TrcPktProcPtm::pktBranchAddr;
1160 }
1161 // atom packets are 8'b1xxxxxx0
1162 else if((i & 0x81) == 0x80)
1163 {
1164 m_i_table[i].pkt_type = PTM_PKT_ATOM;
1165 m_i_table[i].pptkFn = &TrcPktProcPtm::pktAtom;
1166 }
1167 else
1168 {
1169 // set all the others to reserved for now
1170 m_i_table[i].pkt_type = PTM_PKT_RESERVED;
1171 m_i_table[i].pptkFn = &TrcPktProcPtm::pktReserved;
1172 }
1173 }
1174
1175 // pick out the other packet types by individual codes.
1176
1177 // A-sync 8'b00000000
1178 m_i_table[0x00].pkt_type = PTM_PKT_A_SYNC;
1179 m_i_table[0x00].pptkFn = &TrcPktProcPtm::pktASync;
1180
1181 // I-sync 8'b00001000
1182 m_i_table[0x08].pkt_type = PTM_PKT_I_SYNC;
1183 m_i_table[0x08].pptkFn = &TrcPktProcPtm::pktISync;
1184
1185 // waypoint update 8'b01110010
1186 m_i_table[0x72].pkt_type = PTM_PKT_WPOINT_UPDATE;
1187 m_i_table[0x72].pptkFn = &TrcPktProcPtm::pktWPointUpdate;
1188
1189 // trigger 8'b00001100
1190 m_i_table[0x0C].pkt_type = PTM_PKT_TRIGGER;
1191 m_i_table[0x0C].pptkFn = &TrcPktProcPtm::pktTrigger;
1192
1193 // context ID 8'b01101110
1194 m_i_table[0x6E].pkt_type = PTM_PKT_CONTEXT_ID;
1195 m_i_table[0x6E].pptkFn = &TrcPktProcPtm::pktCtxtID;
1196
1197 // VMID 8'b00111100
1198 m_i_table[0x3C].pkt_type = PTM_PKT_VMID;
1199 m_i_table[0x3C].pptkFn = &TrcPktProcPtm::pktVMID;
1200
1201 // Timestamp 8'b01000x10
1202 m_i_table[0x42].pkt_type = PTM_PKT_TIMESTAMP;
1203 m_i_table[0x42].pptkFn = &TrcPktProcPtm::pktTimeStamp;
1204 m_i_table[0x46].pkt_type = PTM_PKT_TIMESTAMP;
1205 m_i_table[0x46].pptkFn = &TrcPktProcPtm::pktTimeStamp;
1206
1207 // Exception return 8'b01110110
1208 m_i_table[0x76].pkt_type = PTM_PKT_EXCEPTION_RET;
1209 m_i_table[0x76].pptkFn = &TrcPktProcPtm::pktExceptionRet;
1210
1211 // Ignore 8'b01100110
1212 m_i_table[0x66].pkt_type = PTM_PKT_IGNORE;
1213 m_i_table[0x66].pptkFn = &TrcPktProcPtm::pktIgnore;
1214 }
1215
1216 /* End of File trc_pkt_proc_ptm.cpp */
1217