[QUEUED v20180920 05/16] stm class: Add a helper for writing data packets

[Alexander Shishkin <alexander.shishkin@linux.intel.com>]

Add a helper to write a sequence of bytes as STP data packets. This
is used by protocol drivers to output their metadata, as well as the
actual data payload.

Signed-off-by: Alexander Shishkin <alexander.shishkin@linux.intel.com>
---
 drivers/hwtracing/stm/core.c | 50 ++++++++++++++++++++++++++----------
 drivers/hwtracing/stm/stm.h  |  3 +++
 2 files changed, 40 insertions(+), 13 deletions(-)

diff --git a/drivers/hwtracing/stm/core.c b/drivers/hwtracing/stm/core.c
index c869a30661ac..9ed2f06deb47 100644
--- a/drivers/hwtracing/stm/core.c
+++ b/drivers/hwtracing/stm/core.c
@@ -564,27 +564,51 @@ stm_assign_first_policy(struct stm_device *stm, struct stm_output *output,
 	return err;
 }
 
-static ssize_t notrace stm_write(struct stm_data *data, unsigned int master,
-			  unsigned int channel, const char *buf, size_t count)
+/**
+ * stm_data_write() - send the given payload as data packets
+ * @data:	stm driver's data
+ * @m:		STP master
+ * @c:		STP channel
+ * @ts_first:	timestamp the first packet
+ * @buf:	data payload buffer
+ * @count:	data payload size
+ */
+ssize_t notrace stm_data_write(struct stm_data *data, unsigned int m,
+			       unsigned int c, bool ts_first, const void *buf,
+			       size_t count)
 {
-	unsigned int flags = STP_PACKET_TIMESTAMPED;
-	const unsigned char *p = buf, nil = 0;
-	size_t pos;
+	unsigned int flags = ts_first ? STP_PACKET_TIMESTAMPED : 0;
 	ssize_t sz;
+	size_t pos;
 
-	for (pos = 0, p = buf; count > pos; pos += sz, p += sz) {
+	for (pos = 0, sz = 0; pos < count; pos += sz) {
 		sz = min_t(unsigned int, count - pos, 8);
-		sz = data->packet(data, master, channel, STP_PACKET_DATA, flags,
-				  sz, p);
-		flags = 0;
-
-		if (sz < 0)
+		sz = data->packet(data, m, c, STP_PACKET_DATA, flags, sz,
+				  &((u8 *)buf)[pos]);
+		if (sz <= 0)
 			break;
+
+		if (ts_first) {
+			flags = 0;
+			ts_first = false;
+		}
 	}
 
-	data->packet(data, master, channel, STP_PACKET_FLAG, 0, 0, &nil);
+	return sz < 0 ? sz : pos;
+}
+EXPORT_SYMBOL_GPL(stm_data_write);
+
+static ssize_t notrace stm_write(struct stm_data *data, unsigned int master,
+			  unsigned int channel, const char *buf, size_t count)
+{
+	ssize_t sz;
+
+	sz = stm_data_write(data, master, channel, true, buf, count);
+	if (sz > 0)
+		data->packet(data, master, channel, STP_PACKET_FLAG, 0, 0,
+			     buf);
 
-	return pos;
+	return sz;
 }
 
 static ssize_t stm_char_write(struct file *file, const char __user *buf,
diff --git a/drivers/hwtracing/stm/stm.h b/drivers/hwtracing/stm/stm.h
index 921ebd9fd3bd..753d83acb7ae 100644
--- a/drivers/hwtracing/stm/stm.h
+++ b/drivers/hwtracing/stm/stm.h
@@ -111,5 +111,8 @@ int stm_lookup_protocol(const char *name,
 			const struct stm_protocol_driver **pdrv,
 			const struct config_item_type **type);
 void stm_put_protocol(const struct stm_protocol_driver *pdrv);
+ssize_t stm_data_write(struct stm_data *data, unsigned int m,
+		       unsigned int c, bool ts_first, const void *buf,
+		       size_t count);
 
 #endif /* _STM_STM_H_ */
-- 
2.18.0