Configure timestamps to be emitted at regular intervals in the trace
stream to temporally correlate instructions executed on different CPUs.

Signed-off-by: Mathieu Poirier <mathieu.poirier@linaro.org>
---
  drivers/hwtracing/coresight/coresight-etm4x.c | 115 +++++++++++++++++-
  1 file changed, 112 insertions(+), 3 deletions(-)

diff --git a/drivers/hwtracing/coresight/coresight-etm4x.c b/drivers/hwtracing/coresight/coresight-etm4x.c
index 732ae12fca9b..45c341a5aa0b 100644
--- a/drivers/hwtracing/coresight/coresight-etm4x.c
+++ b/drivers/hwtracing/coresight/coresight-etm4x.c

@@ -138,8 +138,11 @@ static int etm4_enable_hw(struct etmv4_drvdata *drvdata)
  			       drvdata->base + TRCCNTVRn(i));
  	}
  
-	/* Resource selector pair 0 is always implemented and reserved */
-	for (i = 0; i < drvdata->nr_resource * 2; i++)
+	/*
+	 * Resource selector pair 0 is always implemented and reserved.  As
+	 * such start at 2.
+	 */
+	for (i = 2; i < drvdata->nr_resource * 2; i++)
  		writel_relaxed(config->res_ctrl[i],
  			       drvdata->base + TRCRSCTLRn(i));

@@ -201,6 +204,97 @@ static void etm4_enable_hw_smp_call(void *info)
  	arg->rc = etm4_enable_hw(arg->drvdata);
  }
  
+/*
+ * The goal of function etm4_config_timestamp_event() is to configure a
+ * counter that will tell the tracer to emit a timestamp packet when it
+ * reaches zero.  This is done in order to get a more fine grained idea
+ * of when instructions are executed so that they can be correlated
+ * with execution on other CPUs.
+ *
+ * To do this the counter itself is configured to self reload and
+ * TRCRSCTLR1 (always true) used to get the counter to decrement.  From
+ * there a resource selector is configured with the counter and the
+ * timestamp control register to use the resource selector to trigger the
+ * event that will insert a timestamp packet in the stream.
+ */
+static int etm4_config_timestamp_event(struct etmv4_drvdata *drvdata)
+{
+	int i, ctridx, ret = -EINVAL;
+	int counter, rselector;
+	u32 val = 0;
+	struct etmv4_config *config = &drvdata->config;
+
+	/* No point in trying if we don't have at least one counter */
+	if (!drvdata->nr_cntr)
+		goto out;
+
+	/* Find a counter that hasn't been initialised */
+	for (i = 0; i < drvdata->nr_cntr; i++)
+		if (config->cntr_val[i] == 0)
+			break;
+
+	/* Remember what counter we used */
+	counter = 1 << i;
+	ctridx = i;
+
+	/* All the counters have been configured already, bail out */
+	if (i == drvdata->nr_cntr) {
+		pr_err("%s: no available counter found\n", __func__);
+		goto out;
+	}

+		/* Backout what we did and exit */
+		config->cntr_ctrl[ctridx] = 0;
+		config->cntrldvr[ctridx] = 0;
+		config->cntr_val[ctridx] = 0;
+		goto out;
+	}
+
+	val = 0x2 << 16		| /* Group 0b0010 - Counter and sequencers */
+	      counter << 0;	  /* Counter to use */
+
+	config->res_ctrl[i] = val;
+
+	val = 0x0 << 7		| /* Select single resource selector */
+	      rselector;	  /* Resource selector */
+
+	config->ts_ctrl = val;
+
+	ret = 0;
+out:
+	return ret;
+}
+
  static int etm4_parse_event_config(struct etmv4_drvdata *drvdata,
  				   struct perf_event *event)
  {

@@ -236,9 +330,24 @@ static int etm4_parse_event_config(struct etmv4_drvdata *drvdata,
  		/* TRM: Must program this for cycacc to work */
  		config->ccctlr = ETM_CYC_THRESHOLD_DEFAULT;
  	}
-	if (attr->config & BIT(ETM_OPT_TS))
+	if (attr->config & BIT(ETM_OPT_TS)) {
+		/*
+		 * Configure timestamps to be emitted at regular intervals in
+		 * order to correlate instructions executed on different CPUs
+		 * (CPU-wide trace scenarios).
+		 */
+		ret = etm4_config_timestamp_event(drvdata);
+
+		/*
+		 * No need to go further if timestamp intervals can't
+		 * be configured.
+		 */
+		if (ret)
+			goto out;
+
  		/* bit[11], Global timestamp tracing bit */
  		config->cfg |= BIT(11);
+	}
  
  	if (attr->config & BIT(ETM_OPT_CTXTID))
  		/* bit[6], Context ID tracing bit */