* Author: Mathieu Poirier <mathieu.poirier@linaro.org>
  */
 
+#include <asm/bug.h>
 #include <linux/coresight-pmu.h>
 #include <linux/err.h>
 #include <linux/list.h>
 
 #include "cs-etm.h"
 #include "cs-etm-decoder.h"
+#include "debug.h"
 #include "intlist.h"
 
 /* use raw logging */
 static ocsd_datapath_resp_t
 cs_etm_decoder__do_hard_timestamp(struct cs_etm_queue *etmq,
                                  const ocsd_generic_trace_elem *elem,
-                                 const uint8_t trace_chan_id)
+                                 const uint8_t trace_chan_id,
+                                 const ocsd_trc_index_t indx)
 {
        struct cs_etm_packet_queue *packet_queue;
 
                return OCSD_RESP_CONT;
        }
 
-       /*
-        * This is the first timestamp we've seen since the beginning of traces
-        * or a discontinuity.  Since timestamps packets are generated *after*
-        * range packets have been generated, we need to estimate the time at
-        * which instructions started by subtracting the number of instructions
-        * executed to the timestamp.
-        */
-       packet_queue->cs_timestamp = elem->timestamp - packet_queue->instr_count;
+
+       if (!elem->timestamp) {
+               /*
+                * Zero timestamps can be seen due to misconfiguration or hardware bugs.
+                * Warn once, and don't try to subtract instr_count as it would result in an
+                * underflow.
+                */
+               packet_queue->cs_timestamp = 0;
+               WARN_ONCE(true, "Zero Coresight timestamp found at Idx:%" OCSD_TRC_IDX_STR
+                               ". Decoding may be improved with --itrace=Z...\n", indx);
+       } else if (packet_queue->instr_count > elem->timestamp) {
+               /*
+                * Sanity check that the elem->timestamp - packet_queue->instr_count would not
+                * result in an underflow. Warn and clamp at 0 if it would.
+                */
+               packet_queue->cs_timestamp = 0;
+               pr_err("Timestamp calculation underflow at Idx:%" OCSD_TRC_IDX_STR "\n", indx);
+       } else {
+               /*
+                * This is the first timestamp we've seen since the beginning of traces
+                * or a discontinuity.  Since timestamps packets are generated *after*
+                * range packets have been generated, we need to estimate the time at
+                * which instructions started by subtracting the number of instructions
+                * executed to the timestamp.
+                */
+               packet_queue->cs_timestamp = elem->timestamp - packet_queue->instr_count;
+       }
        packet_queue->next_cs_timestamp = elem->timestamp;
        packet_queue->instr_count = 0;
 
 
 static ocsd_datapath_resp_t cs_etm_decoder__gen_trace_elem_printer(
                                const void *context,
-                               const ocsd_trc_index_t indx __maybe_unused,
+                               const ocsd_trc_index_t indx,
                                const u8 trace_chan_id __maybe_unused,
                                const ocsd_generic_trace_elem *elem)
 {
                break;
        case OCSD_GEN_TRC_ELEM_TIMESTAMP:
                resp = cs_etm_decoder__do_hard_timestamp(etmq, elem,
-                                                        trace_chan_id);
+                                                        trace_chan_id,
+                                                        indx);
                break;
        case OCSD_GEN_TRC_ELEM_PE_CONTEXT:
                resp = cs_etm_decoder__set_tid(etmq, packet_queue,