debug: coresight: Add coresight_trace_deformatter

Add module which decodes data encoded using Coresight Trace
Formatter.

Signed-off-by: Krzysztof Chruściński <krzysztof.chruscinski@nordicsemi.no>

include/zephyr/debug/coresight/cs_trace_defmt.h

@@ -0,0 +1,64 @@
+/*
+ * Copyright (c) 2024 Nordic Semiconductor ASA.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef ZEPHYR_INCLUDE_DEBUG_CORESIGHT_CS_TRACE_DEFMT_H__
+#define ZEPHYR_INCLUDE_DEBUG_CORESIGHT_CS_TRACE_DEFMT_H__
+
+#include <zephyr/kernel.h>
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @defgroup coresight_apis Coresight APIs
+ * @{
+ * @}
+ * @defgroup cs_trace_defmt Coresight Trace Deformatter
+ * @ingroup coresight_apis
+ * @{
+ */
+
+/** @brief Callback signature.
+ *
+ * @param id	Stream ID.
+ * @param data	Data.
+ * @param len	Data length.
+ */
+typedef void (*cs_trace_defmt_cb)(uint32_t id, const uint8_t *data, size_t len);
+
+/** @brief Size of trace deformatter frame size in 32 bit words. */
+#define CORESIGHT_TRACE_FRAME_SIZE32 4
+
+/** @brief Size of trace deformatter frame size in bytes. */
+#define CORESIGHT_TRACE_FRAME_SIZE (CORESIGHT_TRACE_FRAME_SIZE32 * sizeof(uint32_t))
+
+/** @brief Initialize Coresight Trace Deformatter.
+ *
+ * @param cb Callback.
+ */
+int cs_trace_defmt_init(cs_trace_defmt_cb cb);
+
+/** @brief Decode data from the stream.
+ *
+ * Trace formatter puts data in the 16 byte long blocks.
+ *
+ * Callback is called with decoded data.
+ *
+ * @param data	Data.
+ * @param len	Data length. Must equal 16.
+ *
+ * @retval 0		On successful deformatting.
+ * @retval -EINVAL	If wrong length is provided.
+ */
+int cs_trace_defmt_process(const uint8_t *data, size_t len);
+
+/** @} */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* ZEPHYR_INCLUDE_DEBUG_CORESIGHT_CS_TRACE_DEFMT_H__ */

subsys/debug/CMakeLists.txt

@@ -34,3 +34,8 @@ add_subdirectory_ifdef(
   CONFIG_SYMTAB
   symtab
   )
+
+zephyr_sources_ifdef(
+  CONFIG_CS_TRACE_DEFMT
+  coresight/cs_trace_defmt.c
+)

subsys/debug/Kconfig

@@ -438,3 +438,9 @@ config MIPI_STP_DECODER
 	depends on !BIG_ENDIAN
 	help
 	  Module decodes a stream of STPv2 data.
+
+config CS_TRACE_DEFMT
+	bool "Coresight Trace Deformatter"
+	help
+	  Module is decoding data which is formatted using Coresight
+	  Trace Formatter, e.g. when data is put into ETR (Embedded Trace Router).

subsys/debug/coresight/cs_trace_defmt.c

@@ -0,0 +1,63 @@
+/*
+ * Copyright (c) 2024 Nordic Semiconductor ASA.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#include <zephyr/debug/coresight/cs_trace_defmt.h>
+
+static cs_trace_defmt_cb callback;
+static uint8_t curr_id;
+
+int cs_trace_defmt_init(cs_trace_defmt_cb cb)
+{
+	callback = cb;
+	return 0;
+}
+
+int cs_trace_defmt_process(const uint8_t *data, size_t len)
+{
+	uint8_t buf[15];
+	size_t cnt = 0;
+
+	if (len != 16) {
+		return -EINVAL;
+	}
+
+	uint8_t aux = data[15];
+	uint8_t d_id;
+	uint8_t cb_id;
+	bool do_cb = false;
+
+	for (int i = 0; i < 8; i++) {
+		d_id = data[2 * i];
+		if (d_id & 0x1) {
+			if (cnt != 0) {
+				cb_id = curr_id;
+				if ((aux >> i) & 0x1) {
+					/* next byte belongs to the old stream */
+					do_cb = true;
+				} else {
+					callback(cb_id, buf, cnt);
+					cnt = 0;
+				}
+			}
+			curr_id = d_id >> 1;
+		} else {
+			buf[cnt++] = d_id | ((aux >> i) & 0x1);
+		}
+		if (i < 7) {
+			buf[cnt++] = data[2 * i + 1];
+			if (do_cb) {
+				do_cb = false;
+				callback(cb_id, buf, cnt);
+				cnt = 0;
+			}
+		}
+	}
+
+	if (cnt) {
+		callback(curr_id, buf, cnt);
+	}
+
+	return 0;
+}