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