ASoC:SOF:trace: Add runtime filtering mechanism

Parse simple text file format 'log_level uuid_id pipe_id comp_id'
separated by ';', to allow usage from external programs
(especially sof-logger) and directly by user during debugging process.
Any unused value should be set to `-1`.
Send IPC command to FW with new trace levels for selected components
in filter elements list.

Signed-off-by: Karol Trzcinski <karolx.trzcinski@linux.intel.com>

include/sound/sof/header.h

@@ -117,6 +117,7 @@
 #define SOF_IPC_TRACE_DMA_PARAMS		SOF_CMD_TYPE(0x001)
 #define SOF_IPC_TRACE_DMA_POSITION		SOF_CMD_TYPE(0x002)
 #define SOF_IPC_TRACE_DMA_PARAMS_EXT		SOF_CMD_TYPE(0x003)
+#define SOF_IPC_TRACE_FILTER_UPDATE		SOF_CMD_TYPE(0x004)
 
 /* debug */
 #define SOF_IPC_TEST_IPC_FLOOD                  SOF_CMD_TYPE(0x001)

include/sound/sof/trace.h

@@ -43,6 +43,29 @@ struct sof_ipc_dma_trace_posn {
 	uint32_t messages;	/* total trace messages */
 }  __packed;
 
+/* Values used in sof_ipc_trace_filter_elem  */
+#define SOF_IPC_TRACE_FILTER_ELEM_LEVEL	0x01	/**< new trace level for selected components */
+#define SOF_IPC_TRACE_FILTER_ELEM_UUID	0x02	/**< filter by uuid */
+#define SOF_IPC_TRACE_FILTER_ELEM_COMP	0x03	/**< filter by component id */
+#define SOF_IPC_TRACE_FILTER_ELEM_PIPE	0x04	/**< filter by pipeline */
+#define SOF_IPC_TRACE_FILTER_ELEM_MASK	0x7F	/**< filter element type mask */
+#define SOF_IPC_TRACE_FILTER_ELEM_FIN	0x80	/**< mark last filter in set */
+
+/** part of sof_ipc_trace_filter */
+struct sof_ipc_trace_filter_elem {
+	int32_t key;		/**< SOF_IPC_TRACE_FILTER_ELEM_ {LEVEL, UUID, COMP, PIPE} */
+	int32_t value;		/**< element value */
+} __attribute__((packed));
+
+/** Runtime tracing filtration data - SOF_IPC_TRACE_FILTER_UPDATE */
+struct sof_ipc_trace_filter {
+	struct sof_ipc_cmd_hdr hdr;	/**< IPC command header */
+	uint32_t elem_cnt;		/**< number of entries in elems[] array */
+	uint32_t reserved[8];		/**< reserved for future usage */
+	/** variable size array with new filtering settings */
+	struct sof_ipc_trace_filter_elem elems[];
+} __attribute__((packed));
+
 /*
  * Commom debug
  */

sound/soc/sof/trace.c

@@ -13,6 +13,8 @@
 #include "sof-priv.h"
 #include "ops.h"
 
+#define TRACE_FILTER_ELEMENT_EXPANSION 10
+
 static size_t sof_trace_avail(struct snd_sof_dev *sdev,
 			      loff_t pos, size_t buffer_size)
 {
@@ -154,12 +156,169 @@ static int trace_debugfs_create(struct snd_sof_dev *sdev)
 	return 0;
 }
 
+static int trace_filter_append_elem(int32_t key, int32_t value,
+				    struct sof_ipc_trace_filter_elem **elem_list,
+				    size_t *capacity, size_t *counter)
+{
+	struct sof_ipc_trace_filter_elem *elem_ptr;
+	size_t new_capacity;
+
+	if (*capacity <= *counter) {
+		new_capacity = *capacity + TRACE_FILTER_ELEMENT_EXPANSION;
+		elem_ptr = krealloc(*elem_list, new_capacity * sizeof(*elem_ptr), GFP_KERNEL);
+		if (!elem_ptr)
+			return -ENOMEM;
+		*elem_list = elem_ptr;
+		*capacity = new_capacity;
+	}
+
+	elem_ptr = elem_list[*counter];
+	elem_ptr->key = key;
+	elem_ptr->value = value;
+	*counter += 1;
+
+	return 0;
+}
+
+static int trace_filter_parse_entry(struct snd_sof_dev *sdev, const char *line,
+				    struct sof_ipc_trace_filter_elem **elem,
+				    size_t *capacity, size_t *counter)
+{
+	int len = strlen(line);
+	size_t cnt = *counter;
+	int log_level;
+	int uuid_id;
+	int pipe_id;
+	int comp_id;
+	int read;
+	int ret;
+
+	ret = sscanf(line, " %d %x %d %d %n",
+		     &log_level, &uuid_id, &pipe_id, &comp_id, &read);
+	if (ret != 4 || read != len) {
+		dev_err(sdev->dev, "error: invalid trace filter entry '%s'\n",
+			line);
+		return -EINVAL;
+	}
+
+	if (uuid_id > 0) {
+		ret = trace_filter_append_elem(SOF_IPC_TRACE_FILTER_ELEM_UUID,
+					       uuid_id, elem, capacity, &cnt);
+		if (ret)
+			return ret;
+	}
+	if (pipe_id > 0) {
+		ret = trace_filter_append_elem(SOF_IPC_TRACE_FILTER_ELEM_PIPE,
+					       pipe_id, elem, capacity, &cnt);
+		if (ret)
+			return ret;
+	}
+	if (comp_id > 0) {
+		ret = trace_filter_append_elem(SOF_IPC_TRACE_FILTER_ELEM_PIPE,
+					       comp_id, elem, capacity, &cnt);
+		if (ret)
+			return ret;
+	}
+
+	ret = trace_filter_append_elem(SOF_IPC_TRACE_FILTER_ELEM_LEVEL |
+				       SOF_IPC_TRACE_FILTER_ELEM_FIN,
+				       log_level, elem, capacity, &cnt);
+	if (ret)
+		return ret;
+
+	/* update counter only when parsing whole entry passed */
+	*counter = cnt;
+
+	return len;
+}
+
+static int trace_filter_parse(struct snd_sof_dev *sdev, char* string,
+			      size_t len, size_t *out_elem_cnt,
+			      struct sof_ipc_trace_filter_elem **out)
+{
+	struct sof_ipc_trace_filter_elem *elems;
+	char *entry = string;
+	char *entry_end;
+	size_t capacity;
+	size_t cnt = 0;
+	int entry_len;
+
+	if (!string)
+		return -EINVAL;
+
+	/*
+	 * calc capacity, default entry length: len('0 1FFFA000 0 0;') == 15,
+	 * each entry consist up to 4 elements.
+	 */
+	capacity = len * 4 / 15 + 1;
+	elems = kzalloc(capacity * sizeof(*elems), GFP_KERNEL);
+	if (!elems)
+		return -ENOMEM;
+
+	while (entry < string + len) {
+		entry_end = strchrnul(entry, ';');
+		*entry_end = '\0';
+
+		if(strcmp(entry, "\n") == 0) {
+			entry = entry_end + 1;
+			continue;
+		}
+
+		entry_len = trace_filter_parse_entry(sdev, entry, &elems,
+						     &capacity, &cnt);
+		if (entry_len <= 0) {
+			dev_err(sdev->dev, "error: trace_filter_parse_entry for '%s' failed, '%d'\n",
+				entry, line);
+			kfree(elems);
+			return -EINVAL;
+		}
+		entry = entry_end + 1;
+	}
+
+	*out = elems;
+	*out_elem_cnt = cnt;
+	return 0;
+}
+
+int sof_ipc_trace_update_filter(struct snd_sof_dev *sdev, size_t num_elems,
+			       struct sof_ipc_trace_filter_elem *elems)
+{
+	struct sof_ipc_trace_filter *msg;
+	struct sof_ipc_reply reply;
+	size_t size;
+	int ret;
+
+	size = struct_size(msg, elems, num_elems);
+	if (size > SOF_IPC_MSG_MAX_SIZE)
+		return -ENOMEM;
+
+	msg = kmalloc(size, GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
+
+	msg->hdr.size = size;
+	msg->hdr.cmd = SOF_IPC_GLB_TRACE_MSG | SOF_IPC_TRACE_FILTER_UPDATE;
+	msg->elem_cnt = num_elems;
+	memcpy(&msg->elems[0], elems, num_elems * sizeof(*elems));
+
+	pm_runtime_get_sync(sdev->dev);
+	ret = sof_ipc_tx_message(sdev->ipc, msg->hdr.cmd, msg, msg->hdr.size,
+				 &reply, sizeof(reply));
+	pm_runtime_put_sync(sdev->dev);
+
+	kfree(msg);
+	return ret ? ret : reply.error;
+}
+
 static ssize_t sof_dfsentry_trace_filter_write(struct file *file,
 		const char __user *from, size_t count, loff_t *ppos)
 {
 	struct snd_sof_dfsentry *dfse = file->private_data;
+	struct sof_ipc_trace_filter_elem *elems;
 	struct snd_sof_dev *sdev = dfse->sdev;
+	size_t num_elems;
 	char *string;
+	int ret;
 
 	string = kzalloc(count, GFP_KERNEL);
 	if (!string)
@@ -169,6 +328,19 @@ static ssize_t sof_dfsentry_trace_filter_write(struct file *file,
 	if (ret != count)
 		return ret >= 0 ? -EIO : ret;
 
+	ret = trace_filter_parse(sdev, string, count, &num_elems, &elems);
+	if (ret < 0) {
+		dev_err(sdev->dev,
+			"error: fail in trace_filter_parse, %d\n", ret);
+		return ret;
+	}
+
+	ret = sof_ipc_trace_update_filter(sdev, num_elems, elems);
+	if (ret < 0)
+		dev_err(sdev->dev,
+			"error: fail in sof_ipc_trace_update_filter %d\n", ret);
+
+	kfree(elems);
 	return ret;
 }