[PATCH v3 02/10] coresight: Use bitmap to assign trace id to the sources

[Mao Jinlong <quic_jinlmao@xxxxxxxxxxx>]

Except from STM and ETM/ETE, there could be other sources. Each
source needs a unique trace id. Define a bitmap for the trace ids.
The position of each bit represents trace id of the source.

Signed-off-by: Mao Jinlong <quic_jinlmao@xxxxxxxxxxx>
---
 drivers/hwtracing/coresight/coresight-core.c | 48 ++++++++++++++++++++
 include/linux/coresight-pmu.h                | 11 +++++
 2 files changed, 59 insertions(+)

diff --git a/drivers/hwtracing/coresight/coresight-core.c b/drivers/hwtracing/coresight/coresight-core.c
index a90097f88425..6cb55c3f41d5 100644
--- a/drivers/hwtracing/coresight/coresight-core.c
+++ b/drivers/hwtracing/coresight/coresight-core.c
@@ -16,6 +16,7 @@
 #include <linux/mutex.h>
 #include <linux/clk.h>
 #include <linux/coresight.h>
+#include <linux/coresight-pmu.h>
 #include <linux/of_platform.h>
 #include <linux/delay.h>
 #include <linux/pm_runtime.h>
@@ -25,8 +26,11 @@
 #include "coresight-syscfg.h"
 
 static DEFINE_MUTEX(coresight_mutex);
+static DEFINE_MUTEX(coresight_id_mutex);
 static DEFINE_PER_CPU(struct coresight_device *, csdev_sink);
 
+static DECLARE_BITMAP(coresight_trace_id, CORESIGHT_TRACE_ID_NUM);
+
 /*
  * Use IDR to map the hash length of the source's device name
  * to the pointer of path for the source
@@ -51,6 +55,48 @@ struct coresight_node {
 const u32 coresight_barrier_pkt[4] = {0x7fffffff, 0x7fffffff, 0x7fffffff, 0x7fffffff};
 EXPORT_SYMBOL_GPL(coresight_barrier_pkt);
 
+/* Init the coresight_trace_id bit map. */
+static void coresight_init_trace_id(void)
+{
+	int i;
+
+	/* Trace id 0 is invalid. */
+	set_bit(CORESIGHT_TRACE_ID_0, coresight_trace_id);
+	/* Trace id 1 is fixed for STM. */
+	set_bit(CORESIGHT_TRACE_ID_1, coresight_trace_id);
+	/* Trace id from 112 to 127 are reserved. */
+	for (i = CORESIGHT_TRACE_ID_112; i <= CORESIGHT_TRACE_ID_127; i++)
+		set_bit(i, coresight_trace_id);
+	/* Skip the trace ids of ETM/ETE. */
+	for (i = 0; i <= cpumask_last(cpu_possible_mask); i++)
+		set_bit(coresight_get_trace_id(i), coresight_trace_id);
+
+}
+
+/*
+ * Return the first zero bit position of bitmap coresight_trace_id
+ * as source's trace id.
+ *
+ */
+int coresight_get_system_trace_id(void)
+{
+	int id;
+
+	mutex_lock(&coresight_id_mutex);
+	id = find_first_zero_bit(coresight_trace_id, CORESIGHT_TRACE_ID_NUM);
+	/* If no zero bit is found, return error value. */
+	if (id == CORESIGHT_TRACE_ID_NUM) {
+		mutex_unlock(&coresight_id_mutex);
+		return -EINVAL;
+	}
+
+	set_bit(id, coresight_trace_id);
+	mutex_unlock(&coresight_id_mutex);
+
+	return id;
+}
+EXPORT_SYMBOL(coresight_get_system_trace_id);
+
 static const struct cti_assoc_op *cti_assoc_ops;
 
 void coresight_set_cti_ops(const struct cti_assoc_op *cti_op)
@@ -1750,6 +1796,8 @@ static int __init coresight_init(void)
 		return 0;
 
 	etm_perf_exit();
+
+	coresight_init_trace_id();
 exit_bus_unregister:
 	bus_unregister(&coresight_bustype);
 	return ret;
diff --git a/include/linux/coresight-pmu.h b/include/linux/coresight-pmu.h
index 4ac5c081af93..1e2c5ca4c6e6 100644
--- a/include/linux/coresight-pmu.h
+++ b/include/linux/coresight-pmu.h
@@ -32,6 +32,14 @@
 #define ETM4_CFG_BIT_RETSTK	12
 #define ETM4_CFG_BIT_VMID_OPT	15
 
+/* Coresight component supports 7 bits trace id. */
+#define CORESIGHT_TRACE_ID_NUM	128
+
+#define CORESIGHT_TRACE_ID_0	0
+#define CORESIGHT_TRACE_ID_1	1
+#define CORESIGHT_TRACE_ID_112	112
+#define CORESIGHT_TRACE_ID_127	127
+
 static inline int coresight_get_trace_id(int cpu)
 {
 	/*
@@ -43,4 +51,7 @@ static inline int coresight_get_trace_id(int cpu)
 	return (CORESIGHT_ETM_PMU_SEED + (cpu * 2));
 }
 
+/* Get the trace id for the sources except from STM, ETM/ETE. */
+extern int coresight_get_system_trace_id(void);
+
 #endif
-- 
2.17.1