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
572 byteIdx = m_currPacketData.size() - 1;
573 if(!m_gotAddrBytes)
574 {
575 // byteIdx for address byte will run from 1 to 5 - first 4 my have continuation or not.
576 if(byteIdx <= 4)
577 {
578 // address bytes 1 - 4;
579 // ISA stays the same
580 if((currByte & 0x80) == 0x00)
581 {
582 // no further bytes
583 m_gotAddrBytes = true;
584 bDone = true;
585 m_gotExcepBytes = true;
586 }
587 }
588 else
589 {
590 // 5th address byte - determine ISA from this.
591 if((currByte & 0x40) == 0x00)
592 m_gotExcepBytes = true; // no exception bytes - mark as done
593 m_gotAddrBytes = true;
594 bDone = m_gotExcepBytes;
595
596 m_addrPktIsa = ocsd_isa_arm; // assume ARM, but then check
597 if((currByte & 0x20) == 0x20) // bit 5 == 1'b1 - jazelle, bits 4 & 3 part of address.
598 m_addrPktIsa = ocsd_isa_jazelle;
599 else if((currByte & 0x30) == 0x10) // bit [5:4] == 2'b01 - thumb, bit 3 part of address.
600 m_addrPktIsa = ocsd_isa_thumb2;
601 }
602 m_numAddrBytes++;
603 }
604 else if(!m_gotExcepBytes)
605 {
606 // excep byte is actually a WP update byte.
607 m_excepAltISA = ((currByte & 0x40) == 0x40) ? 1 : 0;
608 m_gotExcepBytes = true;
609 m_numExcepBytes++;
610 bDone = true;
611 }
612 }
613 else
614 bBytesAvail = false;
615 }
616
617 // analyse the bytes to create the packet
618 if(bDone)
619 {
620 // ISA for the packet
621 if(m_addrPktIsa == ocsd_isa_unknown) // unchanged by trace packet
622 m_addrPktIsa = m_curr_packet.getISA(); // same as prev
623
624 if(m_gotExcepBytes) // may adjust according to alt ISA in exception packet
625 {
626 if((m_addrPktIsa == ocsd_isa_tee) && (m_excepAltISA == 0))
627 m_addrPktIsa = ocsd_isa_thumb2;
628 else if((m_addrPktIsa == ocsd_isa_thumb2) && (m_excepAltISA == 1))
629 m_addrPktIsa = ocsd_isa_tee;
630 }
631 m_curr_packet.UpdateISA(m_addrPktIsa); // mark ISA in packet (update changes current and prev to dectect an ISA change).
632
633 uint8_t total_bits = 0;
634 uint32_t addr_val = extractAddress(1,total_bits);
635 m_curr_packet.UpdateAddress(addr_val,total_bits);
636 m_process_state = SEND_PKT;
637 }
638 }
639
pktIgnore()640 void TrcPktProcPtm::pktIgnore()
641 {
642 m_process_state = SEND_PKT; // no payload
643 }
644
pktCtxtID()645 void TrcPktProcPtm::pktCtxtID()
646 {
647 int pktIndex = m_currPacketData.size() - 1;
648
649 // if at the header, determine how many more bytes we need.
650 if(pktIndex == 0)
651 {
652 m_numCtxtIDBytes = m_config->CtxtIDBytes();
653 m_gotCtxtIDBytes = 0;
654 }
655
656 // read the necessary ctxtID bytes from the stream
657 bool bGotBytes = false, bytesAvail = true;
658 uint32_t ctxtID = 0;
659
660 bGotBytes = m_numCtxtIDBytes == m_gotCtxtIDBytes;
661 while(!bGotBytes & bytesAvail)
662 {
663 bytesAvail = readByte();
664 if(bytesAvail)
665 m_gotCtxtIDBytes++;
666 bGotBytes = m_numCtxtIDBytes == m_gotCtxtIDBytes;
667 }
668
669 if(bGotBytes)
670 {
671 if(m_numCtxtIDBytes)
672 {
673 extractCtxtID(1,ctxtID);
674 }
675 m_curr_packet.UpdateContextID(ctxtID);
676 m_process_state = SEND_PKT;
677 }
678 }
679
pktVMID()680 void TrcPktProcPtm::pktVMID()
681 {
682 uint8_t currByte;
683
684 // just need a single payload byte...
685 if(readByte(currByte))
686 {
687 m_curr_packet.UpdateVMID(currByte);
688 m_process_state = SEND_PKT;
689 }
690 }
691
pktAtom()692 void TrcPktProcPtm::pktAtom()
693 {
694 uint8_t pHdr = m_currPacketData[0];
695
696 if(!m_config->enaCycleAcc())
697 {
698 m_curr_packet.SetAtomFromPHdr(pHdr);
699 m_process_state = SEND_PKT;
700 }
701 else
702 {
703 bool bGotAllPktBytes = false, byteAvail = true;
704 uint8_t currByte = 0; // cycle accurate tracing -> atom + cycle count
705
706 if(!(pHdr & 0x40))
707 {
708 // only the header byte present
709 bGotAllPktBytes = true;
710 }
711 else
712 {
713 // up to 4 additional bytes of count data.
714 while(byteAvail && !bGotAllPktBytes)
715 {
716 if(readByte(currByte))
717 {
718 if(!(currByte & 0x80) || (m_currPacketData.size() == 5))
719 bGotAllPktBytes = true;
720 }
721 else
722 byteAvail = false;
723 }
724 }
725
726 // we have all the bytes for a cycle accurate packet.
727 if(bGotAllPktBytes)
728 {
729 uint32_t cycleCount = 0;
730 extractCycleCount(0,cycleCount);
731 m_curr_packet.SetCycleCount(cycleCount);
732 m_curr_packet.SetCycleAccAtomFromPHdr(pHdr);
733 m_process_state = SEND_PKT;
734 }
735 }
736 }
737
pktTimeStamp()738 void TrcPktProcPtm::pktTimeStamp()
739 {
740 uint8_t currByte = 0;
741 int pktIndex = m_currPacketData.size() - 1;
742 bool bGotBytes = false, byteAvail = true;
743
744 if(pktIndex == 0)
745 {
746 m_gotTSBytes = false;
747 m_needCycleCount = m_config->enaCycleAcc();
748 m_gotCCBytes = 0;
749
750 // max byte buffer size for full ts packet
751 m_tsByteMax = m_config->TSPkt64() ? 10 : 8;
752 }
753
754 while(byteAvail && !bGotBytes)
755 {
756 if(readByte(currByte))
757 {
758 if(!m_gotTSBytes)
759 {
760 if(((currByte & 0x80) == 0) || (m_currPacketData.size() == (unsigned)m_tsByteMax))
761 {
762 m_gotTSBytes = true;
763 if(!m_needCycleCount)
764 bGotBytes = true;
765 }
766 }
767 else
768 {
769 uint8_t cc_cont_mask = 0x80;
770 // got TS bytes, collect cycle count
771 if(m_gotCCBytes == 0)
772 cc_cont_mask = 0x40;
773 if((currByte & cc_cont_mask) == 0)
774 bGotBytes = true;
775 m_gotCCBytes++;
776 if(m_gotCCBytes == 5)
777 bGotBytes = true;
778 }
779 }
780 else
781 byteAvail = false;
782 }
783
784 if(bGotBytes)
785 {
786 uint64_t tsVal = 0;
787 uint32_t cycleCount = 0;
788 uint8_t tsUpdateBits = 0;
789 int ts_end_idx = extractTS(tsVal,tsUpdateBits);
790 if(m_needCycleCount)
791 {
792 extractCycleCount(ts_end_idx,cycleCount);
793 m_curr_packet.SetCycleCount(cycleCount);
794 }
795 m_curr_packet.UpdateTimestamp(tsVal,tsUpdateBits);
796 m_process_state = SEND_PKT;
797 }
798 }
799
pktExceptionRet()800 void TrcPktProcPtm::pktExceptionRet()
801 {
802 m_process_state = SEND_PKT; // no payload
803 }
804
pktBranchAddr()805 void TrcPktProcPtm::pktBranchAddr()
806 {
807 uint8_t currByte = m_currPacketData[0];
808 bool bDone = false;
809 bool bBytesAvail = true;
810 int byteIdx = 0;
811
812 if(m_currPacketData.size() == 1)
813 {
814 m_gotAddrBytes = false; // flag to indicate got all needed address bytes
815 m_numAddrBytes = 1; // number of address bytes so far
816
817 m_needCycleCount = m_config->enaCycleAcc(); // check if we have a cycle count
818 m_gotCCBytes = 0; // number of cc bytes read in so far.
819
820 m_gotExcepBytes = false; // mark as not got all required exception bytes thus far
821 m_numExcepBytes = 0; // 0 read in
822
823 m_addrPktIsa = ocsd_isa_unknown; // not set by this packet as yet
824
825 // header is also 1st address byte
826 if((currByte & 0x80) == 0) // could be single byte packet
827 {
828 m_gotAddrBytes = true;
829 if(!m_needCycleCount)
830 bDone = true; // all done if no cycle count
831 m_gotExcepBytes = true; // cannot have exception bytes following single byte packet
832 }
833
834 }
835
836 // collect all the bytes needed
837 while(!bDone && bBytesAvail)
838 {
839 if(readByte(currByte))
840 {
841 byteIdx = m_currPacketData.size() - 1;
842 if(!m_gotAddrBytes)
843 {
844 if(byteIdx < 4)
845 {
846 // address bytes 2 - 4;
847 // ISA stays the same
848 if((currByte & 0x80) == 0x00)
849 {
850 // no further bytes
851 if((currByte & 0x40) == 0x00)
852 m_gotExcepBytes = true; // no exception bytes - mark as done
853 m_gotAddrBytes = true;
854 bDone = m_gotExcepBytes && !m_needCycleCount;
855 }
856 }
857 else
858 {
859 // 5th address byte - determine ISA from this.
860 if((currByte & 0x40) == 0x00)
861 m_gotExcepBytes = true; // no exception bytes - mark as done
862 m_gotAddrBytes = true;
863 bDone = m_gotExcepBytes && !m_needCycleCount;
864
865 m_addrPktIsa = ocsd_isa_arm; // assume ARM, but then check
866 if((currByte & 0x20) == 0x20) // bit 5 == 1'b1 - jazelle, bits 4 & 3 part of address.
867 m_addrPktIsa = ocsd_isa_jazelle;
868 else if((currByte & 0x30) == 0x10) // bit [5:4] == 2'b01 - thumb, bit 3 part of address.
869 m_addrPktIsa = ocsd_isa_thumb2;
870 }
871 m_numAddrBytes++;
872 }
873 else if(!m_gotExcepBytes)
874 {
875 // may need exception bytes
876 if(m_numExcepBytes == 0)
877 {
878 if((currByte & 0x80) == 0x00)
879 m_gotExcepBytes = true;
880 m_excepAltISA = ((currByte & 0x40) == 0x40) ? 1 : 0;
881 }
882 else
883 m_gotExcepBytes = true;
884 m_numExcepBytes++;
885
886 if(m_gotExcepBytes && !m_needCycleCount)
887 bDone = true;
888
889 }
890 else if(m_needCycleCount)
891 {
892 // not done after exception bytes, collect cycle count
893 if(m_gotCCBytes == 0)
894 {
895 bDone = ((currByte & 0x40) == 0x00 );
896 }
897 else
898 {
899 // done if no more or 5th byte
900 bDone = (((currByte & 0x80) == 0x00 ) || (m_gotCCBytes == 4));
901 }
902 m_gotCCBytes++;
903 }
904 else
905 // this should never be reached.
906 throwMalformedPacketErr("sequencing error analysing branch packet");
907 }
908 else
909 bBytesAvail = false;
910 }
911
912 // analyse the bytes to create the packet
913 if(bDone)
914 {
915 // ISA for the packet
916 if(m_addrPktIsa == ocsd_isa_unknown) // unchanged by trace packet
917 m_addrPktIsa = m_curr_packet.getISA(); // same as prev
918
919 if(m_gotExcepBytes) // may adjust according to alt ISA in exception packet
920 {
921 if((m_addrPktIsa == ocsd_isa_tee) && (m_excepAltISA == 0))
922 m_addrPktIsa = ocsd_isa_thumb2;
923 else if((m_addrPktIsa == ocsd_isa_thumb2) && (m_excepAltISA == 1))
924 m_addrPktIsa = ocsd_isa_tee;
925 }
926 m_curr_packet.UpdateISA(m_addrPktIsa); // mark ISA in packet (update changes current and prev to dectect an ISA change).
927
928
929
930 // we know the ISA, we can correctly interpret the address.
931 uint8_t total_bits = 0;
932 uint32_t addr_val = extractAddress(0,total_bits);
933 m_curr_packet.UpdateAddress(addr_val,total_bits);
934
935 if(m_numExcepBytes > 0)
936 {
937 uint8_t E1 = m_currPacketData[m_numAddrBytes];
938 uint16_t ENum = (E1 >> 1) & 0xF;
939 ocsd_armv7_exception excep = Excp_Reserved;
940
941 m_curr_packet.UpdateNS(E1 & 0x1);
942 if(m_numExcepBytes > 1)
943 {
944 uint8_t E2 = m_currPacketData[m_numAddrBytes+1];
945 m_curr_packet.UpdateHyp((E2 >> 5) & 0x1);
946 ENum |= ((uint16_t)(E2 & 0x1F) << 4);
947 }
948
949 if(ENum <= 0xF)
950 {
951 static ocsd_armv7_exception v7ARExceptions[16] = {
952 Excp_NoException, Excp_DebugHalt, Excp_SMC, Excp_Hyp,
953 Excp_AsyncDAbort, Excp_ThumbEECheckFail, Excp_Reserved, Excp_Reserved,
954 Excp_Reset, Excp_Undef, Excp_SVC, Excp_PrefAbort,
955 Excp_SyncDataAbort, Excp_Generic, Excp_IRQ, Excp_FIQ
956 };
957 excep = v7ARExceptions[ENum];
958 }
959 m_curr_packet.SetException(excep,ENum);
960 }
961
962 if(m_needCycleCount)
963 {
964 int countIdx = m_numAddrBytes + m_numExcepBytes;
965 uint32_t cycleCount = 0;
966 extractCycleCount(countIdx,cycleCount);
967 m_curr_packet.SetCycleCount(cycleCount);
968 }
969 m_process_state = SEND_PKT;
970 }
971 }
972
pktReserved()973 void TrcPktProcPtm::pktReserved()
974 {
975 m_process_state = SEND_PKT; // no payload
976 }
977
extractCtxtID(int idx,uint32_t & ctxtID)978 void TrcPktProcPtm::extractCtxtID(int idx, uint32_t &ctxtID)
979 {
980 ctxtID = 0;
981 int shift = 0;
982 for(int i=0; i < m_numCtxtIDBytes; i++)
983 {
984 if((size_t)idx+i >= m_currPacketData.size())
985 throwMalformedPacketErr("Insufficient packet bytes for Context ID value.");
986 ctxtID |= ((uint32_t)m_currPacketData[idx+i]) << shift;
987 shift+=8;
988 }
989 }
990
extractCycleCount(int offset,uint32_t & cycleCount)991 void TrcPktProcPtm::extractCycleCount(int offset, uint32_t &cycleCount)
992 {
993 bool bCont = true;
994 cycleCount = 0;
995 int by_idx = 0;
996 uint8_t currByte;
997 int shift = 4;
998
999 while(bCont)
1000 {
1001 if((size_t)by_idx+offset >= m_currPacketData.size())
1002 throwMalformedPacketErr("Insufficient packet bytes for Cycle Count value.");
1003
1004 currByte = m_currPacketData[offset+by_idx];
1005 if(by_idx == 0)
1006 {
1007 bCont = (currByte & 0x40) != 0;
1008 cycleCount = (currByte >> 2) & 0xF;
1009 }
1010 else
1011 {
1012
1013 bCont = (currByte & 0x80) != 0;
1014 if(by_idx == 4)
1015 bCont = false;
1016 cycleCount |= (((uint32_t)(currByte & 0x7F)) << shift);
1017 shift += 7;
1018 }
1019 by_idx++;
1020 }
1021 }
1022
extractTS(uint64_t & tsVal,uint8_t & tsUpdateBits)1023 int TrcPktProcPtm::extractTS(uint64_t &tsVal,uint8_t &tsUpdateBits)
1024 {
1025 bool bCont = true;
1026 int tsIdx = 1; // start index;
1027 uint8_t byteVal;
1028 bool b64BitVal = m_config->TSPkt64();
1029 int shift = 0;
1030
1031 tsVal = 0;
1032 tsUpdateBits = 0;
1033
1034 while(bCont)
1035 {
1036 if((size_t)tsIdx >= m_currPacketData.size())
1037 throwMalformedPacketErr("Insufficient packet bytes for Timestamp value.");
1038
1039 byteVal = m_currPacketData[tsIdx];
1040
1041 if(b64BitVal)
1042 {
1043 if(tsIdx < 9)
1044 {
1045 bCont = ((byteVal & 0x80) == 0x80);
1046 byteVal &= 0x7F;
1047 tsUpdateBits += 7;
1048 }
1049 else
1050 {
1051 bCont = false;
1052 tsUpdateBits += 8;
1053 }
1054 }
1055 else
1056 {
1057 if(tsIdx < 7)
1058 {
1059 bCont = ((byteVal & 0x80) == 0x80);
1060 byteVal &= 0x7F;
1061 tsUpdateBits += 7;
1062 }
1063 else
1064 {
1065 byteVal &=0x3F;
1066 bCont = false;
1067 tsUpdateBits += 6;
1068 }
1069 }
1070 tsVal |= (((uint64_t)byteVal) << shift);
1071 tsIdx++;
1072 shift += 7;
1073 }
1074 return tsIdx; // return next byte index in packet.
1075 }
1076
extractAddress(const int offset,uint8_t & total_bits)1077 uint32_t TrcPktProcPtm::extractAddress(const int offset, uint8_t &total_bits)
1078 {
1079 // we know the ISA, we can correctly interpret the address.
1080 uint32_t addr_val = 0;
1081 uint8_t mask = 0x7E; // first byte mask (always);
1082 uint8_t num_bits = 0x7; // number of bits in the 1st byte (thumb);
1083 int shift = 0;
1084 int next_shift = 0;
1085
1086 total_bits = 0;
1087
1088 for(int i = 0; i < m_numAddrBytes; i++)
1089 {
1090 if(i == 4)
1091 {
1092 // 5th byte mask
1093 mask = 0x0f; // thumb mask;
1094 num_bits = 4;
1095 if(m_addrPktIsa == ocsd_isa_jazelle)
1096 {
1097 mask = 0x1F;
1098 num_bits = 5;
1099 }
1100 else if(m_addrPktIsa == ocsd_isa_arm)
1101 {
1102 mask = 0x07;
1103 num_bits = 3;
1104 }
1105 }
1106 else if(i > 0)
1107 {
1108 mask = 0x7F;
1109 num_bits = 7;
1110 // check for last byte but not 1st or 5th byte mask
1111 if(i == m_numAddrBytes-1)
1112 {
1113 mask = 0x3F;
1114 num_bits = 6;
1115 }
1116 }
1117
1118 // extract data
1119 shift = next_shift;
1120 addr_val |= ((uint32_t)(m_currPacketData[i+offset] & mask) << shift);
1121 total_bits += num_bits;
1122
1123 // how much we shift the next value
1124 if(i == 0)
1125 {
1126 if(m_addrPktIsa == ocsd_isa_jazelle)
1127 {
1128 addr_val >>= 1;
1129 next_shift = 6;
1130 total_bits--; // adjust bits for jazelle offset
1131 }
1132 else
1133 {
1134 next_shift = 7;
1135 }
1136 }
1137 else
1138 {
1139 next_shift += 7;
1140 }
1141 }
1142
1143 if(m_addrPktIsa == ocsd_isa_arm)
1144 {
1145 addr_val <<= 1; // shift one extra bit for ARM address alignment.
1146 total_bits++;
1147 }
1148 return addr_val;
1149 }
1150
1151
BuildIPacketTable()1152 void TrcPktProcPtm::BuildIPacketTable()
1153 {
1154 // initialise all to branch, atom or reserved packet header
1155 for(unsigned i = 0; i < 256; i++)
1156 {
1157 // branch address packets all end in 8'bxxxxxxx1
1158 if((i & 0x01) == 0x01)
1159 {
1160 m_i_table[i].pkt_type = PTM_PKT_BRANCH_ADDRESS;
1161 m_i_table[i].pptkFn = &TrcPktProcPtm::pktBranchAddr;
1162 }
1163 // atom packets are 8'b1xxxxxx0
1164 else if((i & 0x81) == 0x80)
1165 {
1166 m_i_table[i].pkt_type = PTM_PKT_ATOM;
1167 m_i_table[i].pptkFn = &TrcPktProcPtm::pktAtom;
1168 }
1169 else
1170 {
1171 // set all the others to reserved for now
1172 m_i_table[i].pkt_type = PTM_PKT_RESERVED;
1173 m_i_table[i].pptkFn = &TrcPktProcPtm::pktReserved;
1174 }
1175 }
1176
1177 // pick out the other packet types by individual codes.
1178
1179 // A-sync 8'b00000000
1180 m_i_table[0x00].pkt_type = PTM_PKT_A_SYNC;
1181 m_i_table[0x00].pptkFn = &TrcPktProcPtm::pktASync;
1182
1183 // I-sync 8'b00001000
1184 m_i_table[0x08].pkt_type = PTM_PKT_I_SYNC;
1185 m_i_table[0x08].pptkFn = &TrcPktProcPtm::pktISync;
1186
1187 // waypoint update 8'b01110010
1188 m_i_table[0x72].pkt_type = PTM_PKT_WPOINT_UPDATE;
1189 m_i_table[0x72].pptkFn = &TrcPktProcPtm::pktWPointUpdate;
1190
1191 // trigger 8'b00001100
1192 m_i_table[0x0C].pkt_type = PTM_PKT_TRIGGER;
1193 m_i_table[0x0C].pptkFn = &TrcPktProcPtm::pktTrigger;
1194
1195 // context ID 8'b01101110
1196 m_i_table[0x6E].pkt_type = PTM_PKT_CONTEXT_ID;
1197 m_i_table[0x6E].pptkFn = &TrcPktProcPtm::pktCtxtID;
1198
1199 // VMID 8'b00111100
1200 m_i_table[0x3C].pkt_type = PTM_PKT_VMID;
1201 m_i_table[0x3C].pptkFn = &TrcPktProcPtm::pktVMID;
1202
1203 // Timestamp 8'b01000x10
1204 m_i_table[0x42].pkt_type = PTM_PKT_TIMESTAMP;
1205 m_i_table[0x42].pptkFn = &TrcPktProcPtm::pktTimeStamp;
1206 m_i_table[0x46].pkt_type = PTM_PKT_TIMESTAMP;
1207 m_i_table[0x46].pptkFn = &TrcPktProcPtm::pktTimeStamp;
1208
1209 // Exception return 8'b01110110
1210 m_i_table[0x76].pkt_type = PTM_PKT_EXCEPTION_RET;
1211 m_i_table[0x76].pptkFn = &TrcPktProcPtm::pktExceptionRet;
1212
1213 // Ignore 8'b01100110
1214 m_i_table[0x66].pkt_type = PTM_PKT_IGNORE;
1215 m_i_table[0x66].pptkFn = &TrcPktProcPtm::pktIgnore;
1216 }
1217
1218 /* End of File trc_pkt_proc_ptm.cpp */
1219