modem: Add M6718 IPC SPI driver queue functions

Adds the IPC driver TX queue related functionality

ST-Ericsson ID: 369397
ST-Ericsson FOSS-OUT ID: STETL-FOSS-OUT-12224
ST-Ericsson Linux next: NA

Change-Id: I09982aea887877b7793e17edb250d2c9ea9b2996
Signed-off-by: Chris Blair <chris.blair@stericsson.com>
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/36484
Reviewed-by: QATOOLS
Reviewed-by: QABUILD
Reviewed-by: Jonas ABERG <jonas.aberg@stericsson.com>

4 files changed, 224 insertions, 2 deletions

diff --git a/drivers/modem/m6718_spi/Makefile b/drivers/modem/m6718_spi/Makefile
index ea14319ad7d..60e8adbbb81 100644
--- a/drivers/modem/m6718_spi/Makefile
+++ b/drivers/modem/m6718_spi/Makefile
@@ -5,7 +5,7 @@ ifeq ($(CONFIG_MODEM_M6718_SPI_DEBUG),y)
 EXTRA_CFLAGS += -DDEBUG
 endif
 
-m6718_modem_spi-objs := modem_driver.o protocol.o util.o
+m6718_modem_spi-objs := modem_driver.o protocol.o util.o queue.o
 
 ifeq ($(CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_MODEM_STATE),y)
 m6718_modem_spi-objs += modem_state.o
diff --git a/drivers/modem/m6718_spi/modem_queue.h b/drivers/modem/m6718_spi/modem_queue.h
new file mode 100644
index 00000000000..62604129945
--- /dev/null
+++ b/drivers/modem/m6718_spi/modem_queue.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Chris Blair <chris.blair@stericsson.com> for ST-Ericsson
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * Modem IPC driver protocol interface header:
+ *   queue functionality.
+ */
+#ifndef _MODEM_QUEUE_H_
+#define _MODEM_QUEUE_H_
+
+void ipc_queue_init(struct ipc_link_context *context);
+void ipc_queue_delete_frame(struct ipc_tx_queue *frame);
+struct ipc_tx_queue *ipc_queue_new_frame(struct ipc_link_context *link_context,
+	u32 l2_length);
+bool ipc_queue_is_empty(struct ipc_link_context *context);
+int ipc_queue_push_frame(struct ipc_link_context *link_context, u8 l2_header,
+	u32 l2_length, void *l2_data);
+struct ipc_tx_queue *ipc_queue_get_frame(struct ipc_link_context *context);
+void ipc_queue_reset(struct ipc_link_context *context);
+
+#endif /* _MODEM_QUEUE_H_ */
diff --git a/drivers/modem/m6718_spi/protocol.c b/drivers/modem/m6718_spi/protocol.c
index 6879d58798b..046157dcd5e 100644
--- a/drivers/modem/m6718_spi/protocol.c
+++ b/drivers/modem/m6718_spi/protocol.c
@@ -11,6 +11,7 @@
 #include "modem_protocol.h"
 #include "modem_private.h"
 #include "modem_util.h"
+#include "modem_queue.h"
 
 #ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_MODEM_STATE
 #include <linux/workqueue.h>
@@ -116,6 +117,7 @@ void modem_protocol_init(void)
 int modem_m6718_spi_send(struct modem_spi_dev *modem_spi_dev, u8 channel,
 	u32 len, void *data)
 {
+	int err;
 	struct ipc_link_context *context;
 
 	if (!channels[channel].open) {
@@ -136,7 +138,18 @@ int modem_m6718_spi_send(struct modem_spi_dev *modem_spi_dev, u8 channel,
 		return -ENODEV;
 	}
 
-	/* TODO: statemachine will be kicked here */
+	err = ipc_queue_push_frame(context, channel, len, data);
+	if (err < 0)
+		return err;
+
+	if (ipc_util_link_is_idle(context)) {
+		dev_dbg(modem_spi_dev->dev,
+			"link %d is idle, kicking\n", channels[channel].link);
+		/* TODO: statemachine will be kicked here */
+	} else {
+		dev_dbg(modem_spi_dev->dev,
+			"link %d is already running\n", channels[channel].link);
+	}
 	return 0;
 }
 EXPORT_SYMBOL_GPL(modem_m6718_spi_send);
@@ -306,6 +319,8 @@ int modem_protocol_probe(struct spi_device *sdev)
 	context->last_frame = NULL;
 #endif
 
+	ipc_queue_init(context);
+
 	/*
 	 * For link0 (the handshake link) we force a state transition now so
 	 * that it prepares for boot sync.
diff --git a/drivers/modem/m6718_spi/queue.c b/drivers/modem/m6718_spi/queue.c
new file mode 100644
index 00000000000..911d538ee82
--- /dev/null
+++ b/drivers/modem/m6718_spi/queue.c
@@ -0,0 +1,183 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010,2011
+ *
+ * Author: Chris Blair <chris.blair@stericsson.com> for ST-Ericsson
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * U9500 <-> M6718 IPC protocol implementation using SPI:
+ *   TX queue functionality.
+ */
+#include <linux/modem/m6718_spi/modem_driver.h>
+#include "modem_util.h"
+
+#define FRAME_LENGTH_ALIGN (4)
+#define MAX_FRAME_COUNTER (256)
+
+/* fixed L1 frame size for audio link: 4 byte L2 header + 664 byte L2 payload */
+#define FRAME_SIZE_AUDIO (668)
+
+void ipc_queue_init(struct ipc_link_context *context)
+{
+	spin_lock_init(&context->tx_q_update_lock);
+	atomic_set(&context->tx_q_count, 0);
+	context->tx_q_free = IPC_TX_QUEUE_MAX_SIZE;
+	INIT_LIST_HEAD(&context->tx_q);
+	context->tx_frame_counter = 0;
+}
+
+void ipc_queue_delete_frame(struct ipc_tx_queue *frame)
+{
+	kfree(frame);
+}
+
+struct ipc_tx_queue *ipc_queue_new_frame(struct ipc_link_context *link_context,
+	u32 l2_length)
+{
+	struct ipc_tx_queue *frame;
+	u32 padded_len = l2_length;
+
+	/* audio link frames are always a fixed size */
+	if (link_context->link->id == IPC_LINK_AUDIO) {
+		if (l2_length > FRAME_SIZE_AUDIO) {
+			dev_err(&link_context->sdev->dev,
+				"link %d error: invalid frame size %d "
+				"requested, max is %d\n",
+				link_context->link->id,
+				l2_length,
+				FRAME_SIZE_AUDIO);
+			return NULL;
+		}
+		padded_len = FRAME_SIZE_AUDIO;
+	} else {
+		/* frame length padded to alignment boundary */
+		if (padded_len % FRAME_LENGTH_ALIGN)
+			padded_len += (FRAME_LENGTH_ALIGN -
+					(padded_len % FRAME_LENGTH_ALIGN));
+	}
+
+	dev_dbg(&link_context->sdev->dev,
+		"link %d: new frame: length %d, padded to %d\n",
+		link_context->link->id, l2_length, padded_len);
+
+	frame = kzalloc(sizeof(*frame) + padded_len, GFP_ATOMIC);
+	if (frame == NULL) {
+		dev_err(&link_context->sdev->dev,
+			"link %d error: failed to allocate frame\n",
+			link_context->link->id);
+		return NULL;
+	}
+
+	frame->actual_len = l2_length;
+	frame->len = padded_len;
+	frame->data = frame + 1;
+	return frame;
+}
+
+bool ipc_queue_is_empty(struct ipc_link_context *context)
+{
+	unsigned long flags;
+	bool empty;
+
+	spin_lock_irqsave(&context->tx_q_update_lock, flags);
+	empty = list_empty(&context->tx_q);
+	spin_unlock_irqrestore(&context->tx_q_update_lock, flags);
+
+	return empty;
+}
+
+int ipc_queue_push_frame(struct ipc_link_context *context, u8 channel,
+	u32 length, void *data)
+{
+	u32 l2_hdr;
+	unsigned long flags;
+	struct ipc_tx_queue *frame;
+	int *tx_frame_counter = &context->tx_frame_counter;
+	int qcount;
+
+	/*
+	 * Max queue size is only approximate so we allow it to go a few bytes
+	 * over the limit
+	 */
+	if (context->tx_q_free < length) {
+		dev_dbg(&context->sdev->dev,
+			"link %d: tx queue full, wanted %d free %d\n",
+			context->link->id,
+			length,
+			context->tx_q_free);
+		return -EAGAIN;
+	}
+
+	frame = ipc_queue_new_frame(context, length + IPC_L2_HDR_SIZE);
+	if (frame == NULL)
+		return -ENOMEM;
+
+	/* create l2 header and copy to pdu buffer */
+	l2_hdr = ipc_util_make_l2_header(channel, length);
+	*(u32 *)frame->data = l2_hdr;
+
+	/* copy the l2 sdu into the pdu buffer after the header */
+	memcpy(frame->data + IPC_L2_HDR_SIZE, data, length);
+
+	spin_lock_irqsave(&context->tx_q_update_lock, flags);
+	frame->counter = *tx_frame_counter;
+	*tx_frame_counter = (*tx_frame_counter + 1) % MAX_FRAME_COUNTER;
+	list_add_tail(&frame->node, &context->tx_q);
+	qcount = atomic_add_return(1, &context->tx_q_count);
+	/* tx_q_free could go negative here */
+	context->tx_q_free -= frame->len;
+#ifdef CONFIG_DEBUG_FS
+	context->tx_q_min = min(context->tx_q_free, context->tx_q_min);
+#endif
+	spin_unlock_irqrestore(&context->tx_q_update_lock, flags);
+
+	dev_dbg(&context->sdev->dev,
+		"link %d: push tx frame %d: %08x (ch %d len %d), "
+		"new count %d, new free %d\n",
+		context->link->id,
+		frame->counter,
+		l2_hdr,
+		ipc_util_get_l2_channel(l2_hdr),
+		ipc_util_get_l2_length(l2_hdr),
+		qcount,
+		context->tx_q_free);
+	return 0;
+}
+
+struct ipc_tx_queue *ipc_queue_get_frame(struct ipc_link_context *context)
+{
+	unsigned long flags;
+	struct ipc_tx_queue *frame;
+	int qcount;
+
+	spin_lock_irqsave(&context->tx_q_update_lock, flags);
+	frame = list_first_entry(&context->tx_q, struct ipc_tx_queue, node);
+	list_del(&frame->node);
+	qcount = atomic_sub_return(1, &context->tx_q_count);
+	context->tx_q_free += frame->len;
+	spin_unlock_irqrestore(&context->tx_q_update_lock, flags);
+
+	dev_dbg(&context->sdev->dev,
+		"link %d: get tx frame %d, new count %d, "
+		"new free %d\n",
+		context->link->id, frame->counter, qcount, context->tx_q_free);
+	return frame;
+}
+
+void ipc_queue_reset(struct ipc_link_context *context)
+{
+	unsigned long flags;
+	struct ipc_tx_queue *frame;
+	int qcount;
+
+	spin_lock_irqsave(&context->tx_q_update_lock, flags);
+	qcount = atomic_read(&context->tx_q_count);
+	while (qcount != 0) {
+		frame = list_first_entry(&context->tx_q,
+				struct ipc_tx_queue, node);
+		list_del(&frame->node);
+		ipc_queue_delete_frame(frame);
+		qcount = atomic_sub_return(1, &context->tx_q_count);
+	}
+	spin_unlock_irqrestore(&context->tx_q_update_lock, flags);
+}