Merge "rpmsg: qcom_smd: Read data of size greater than fifo size" (2f79bf55) · Commits · e / devices / android_kernel_xiaomi_nabu

drivers/rpmsg/qcom_smd.c

+136 −3

Original line number	Diff line number	Diff line
		@@ -28,6 +28,7 @@
		#include <linux/wait.h>
		#include <linux/rpmsg.h>
		#include <linux/rpmsg/qcom_smd.h>
		#include <linux/ipc_logging.h>

		#include "rpmsg_internal.h"

		@@ -100,6 +101,18 @@ static const struct {
		},
		};

		/* ipc logging wrapper */
		#define smd_ipc(log_ctx, print, dev, x...) do { \
		if (log_ctx) \
		ipc_log_string(log_ctx, x); \
		if (print) { \
		if (dev) \
		dev_err((dev), x); \
		else \
		pr_err(x); \
		} \
		} while (0)

		/**
		* struct qcom_smd_edge - representing a remote processor
		* @of_node: of_node handle for information related to this edge
		@@ -142,6 +155,7 @@ struct qcom_smd_edge {

		struct work_struct scan_work;
		struct work_struct state_work;
		void ipc; / ipc logging hanlder */
		};

		/*
		@@ -192,6 +206,9 @@ struct qcom_smd_endpoint {
		* @recv_lock: guard for rx info modifications and cb pointer
		* @pkt_size: size of the currently handled packet
		* @list: lite entry for @channels in qcom_smd_edge
		* @extended_buf: buffer for reading data greater than fifo size
		* @ext_buf: helper pointer for reading data greater than fifo size
		* @ext_pkt_size: size of data greater than fifo size
		*/
		struct qcom_smd_channel {
		struct qcom_smd_edge *edge;
		@@ -223,6 +240,10 @@ struct qcom_smd_channel {

		struct list_head list;
		u32 rsigs;

		void *extended_buf;
		void *ext_buf;
		int ext_pkt_size;
		};

		/*
		@@ -399,6 +420,8 @@ static void qcom_smd_channel_reset(struct qcom_smd_channel *channel)
		SET_RX_CHANNEL_INFO(channel, tail, 0);

		qcom_smd_signal_channel(channel);
		smd_ipc(channel->edge->ipc, false, NULL, "%s: ch %s ed %s\n",
		__func__, channel->name, channel->edge->name);

		channel->state = SMD_CHANNEL_CLOSED;
		channel->pkt_size = 0;
		@@ -429,6 +452,9 @@ static size_t qcom_smd_channel_get_rx_avail(struct qcom_smd_channel *channel)
		head = GET_RX_CHANNEL_INFO(channel, head);
		tail = GET_RX_CHANNEL_INFO(channel, tail);

		smd_ipc(channel->edge->ipc, false, NULL,
		"%s: h: 0x%x t: 0x%x ch %s ed %s\n",
		__func__, head, tail, channel->name, channel->edge->name);
		return (head - tail) & (channel->fifo_size - 1);
		}

		@@ -455,6 +481,8 @@ static void qcom_smd_channel_set_state(struct qcom_smd_channel *channel,

		channel->state = state;
		qcom_smd_signal_channel(channel);
		smd_ipc(channel->edge->ipc, false, NULL, "%s: ch %s ed %s\n",
		__func__, channel->name, channel->edge->name);
		}

		/*
		@@ -533,6 +561,62 @@ static void qcom_smd_channel_advance(struct qcom_smd_channel *channel,
		SET_RX_CHANNEL_INFO(channel, tail, tail);
		}

		/* Read the data of size greater than fifo size */
		static size_t qcom_smd_channel_ext_read(struct qcom_smd_channel *channel)
		{
		struct rpmsg_endpoint *ept = &channel->qsept->ept;
		size_t len;
		void *ptr;
		int ret;
		int avail;

		if (!channel->extended_buf) {
		channel->ext_pkt_size = channel->pkt_size;
		channel->extended_buf = kmalloc(channel->ext_pkt_size,
		GFP_ATOMIC);
		if (!channel->extended_buf) {
		smd_ipc(channel->edge->ipc, true, NULL,
		"%s: mem alloc failed\n", __func__);
		return 0;
		}
		ptr = channel->extended_buf;
		channel->ext_buf = ptr;
		} else {
		ptr = channel->ext_buf;
		}

		if (channel->pkt_size > channel->fifo_size) {
		avail = qcom_smd_channel_get_rx_avail(channel);
		len = qcom_smd_channel_peek(channel, ptr, avail);
		} else {
		len = qcom_smd_channel_peek(channel, ptr, channel->pkt_size);
		}

		qcom_smd_channel_advance(channel, len);
		channel->pkt_size = channel->pkt_size - len;
		channel->ext_buf = ptr + len;

		if (channel->pkt_size == 0) {
		ptr = channel->extended_buf;
		len = channel->ext_pkt_size;
		ret = ept->cb(ept->rpdev, ptr, len, ept->priv, RPMSG_ADDR_ANY);
		if (ret < 0) {
		smd_ipc(channel->edge->ipc, false, NULL,
		"%s: ret %d len %d ch %s\n", __func__, ret, len,
		channel->name);
		}
		kfree(channel->extended_buf);
		channel->extended_buf = NULL;
		channel->ext_buf = NULL;
		channel->ext_pkt_size = 0;
		}

		/* Indicate that we have seen and updated tail */
		SET_TX_CHANNEL_FLAG(channel, fTAIL, 1);

		return len;
		}

		/*
		* Read out a single packet from the rx fifo and deliver it to the device
		*/
		@@ -546,6 +630,15 @@ static int qcom_smd_channel_recv_single(struct qcom_smd_channel *channel)

		tail = GET_RX_CHANNEL_INFO(channel, tail);

		/* use extended buffer if data size is greter than fifo size */
		if ((channel->pkt_size > channel->fifo_size) \|\|
		channel->ext_pkt_size) {
		len = qcom_smd_channel_ext_read(channel);
		if (len == 0)
		return -ENOMEM;
		goto exit;
		}

		/* Use bounce buffer if the data wraps */
		if (tail + channel->pkt_size >= channel->fifo_size) {
		ptr = channel->bounce_buffer;
		@@ -556,14 +649,21 @@ static int qcom_smd_channel_recv_single(struct qcom_smd_channel *channel)
		}

		ret = ept->cb(ept->rpdev, ptr, len, ept->priv, RPMSG_ADDR_ANY);
		if (ret < 0)
		if (ret < 0) {
		smd_ipc(channel->edge->ipc, false, NULL,
		"%s: ret %d len %d ch %s\n", __func__, ret, len,
		channel->name);
		return ret;
		}

		/* Only forward the tail if the client consumed the data */
		qcom_smd_channel_advance(channel, len);

		channel->pkt_size = 0;

		exit:
		smd_ipc(channel->edge->ipc, false, NULL, "%s: len %d ch %s ed %s\n",
		__func__, len, channel->name, channel->edge->name);
		return 0;
		}

		@@ -616,10 +716,18 @@ static bool qcom_smd_channel_intr(struct qcom_smd_channel *channel)
		qcom_smd_channel_peek(channel, &pktlen, sizeof(pktlen));
		qcom_smd_channel_advance(channel, SMD_PACKET_HEADER_LEN);
		channel->pkt_size = le32_to_cpu(pktlen);
		} else if (channel->pkt_size && avail >= channel->pkt_size) {
		smd_ipc(channel->edge->ipc, false, NULL,
		"%s: pkt_size: %d ch %s ed %s\n", __func__,
		channel->pkt_size, channel->name, channel->edge->name);
		} else if (channel->pkt_size && (avail >= channel->pkt_size \|\|
		channel->pkt_size > channel->fifo_size)) {
		ret = qcom_smd_channel_recv_single(channel);
		if (ret)
		if (ret) {
		smd_ipc(channel->edge->ipc, false, NULL,
		"%s: fail ret %d ch %s ed %s\n", __func__,
		ret, channel->name, channel->edge->name);
		break;
		}
		} else {
		break;
		}
		@@ -634,6 +742,8 @@ static bool qcom_smd_channel_intr(struct qcom_smd_channel *channel)
		wmb();

		qcom_smd_signal_channel(channel);
		smd_ipc(channel->edge->ipc, false, NULL, "%s: ch %s ed %s\n",
		__func__, channel->name, channel->edge->name);
		}

		out:
		@@ -659,6 +769,9 @@ static irqreturn_t qcom_smd_edge_intr(int irq, void *data)
		list_for_each_entry(channel, &edge->channels, list) {
		spin_lock(&channel->recv_lock);
		kick_state \|= qcom_smd_channel_intr(channel);
		smd_ipc(channel->edge->ipc, false, NULL,
		"%s: to APPS ch %s ed %s\n", __func__, channel->name,
		edge->name);
		spin_unlock(&channel->recv_lock);
		}
		spin_unlock(&edge->channels_lock);
		@@ -804,6 +917,9 @@ static int __qcom_smd_send(struct qcom_smd_channel channel, const void data,
		wmb();

		qcom_smd_signal_channel(channel);
		smd_ipc(channel->edge->ipc, false, NULL,
		"%s: hdr:%d len:%d ch %s ed %s\n", __func__,
		sizeof(hdr), len, channel->name, channel->edge->name);

		out:
		spin_unlock_irqrestore(&channel->tx_lock, flags);
		@@ -994,6 +1110,8 @@ static int qcom_smd_set_sigs(struct rpmsg_endpoint *ept, u32 sigs)

		SET_TX_CHANNEL_FLAG(channel, fSTATE, 1);
		qcom_smd_signal_channel(channel);
		smd_ipc(channel->edge->ipc, false, NULL, "%s: ch %s ed %s\n",
		__func__, channel->name, channel->edge->name);

		return 0;
		}
		@@ -1091,6 +1209,8 @@ static int qcom_smd_create_device(struct qcom_smd_channel *channel)
		rpdev->dev.parent = &edge->dev;
		rpdev->dev.release = qcom_smd_release_device;

		smd_ipc(channel->edge->ipc, false, NULL,
		"%s: registering ch %s\n", __func__, channel->name);
		return rpmsg_register_device(rpdev);
		}

		@@ -1317,6 +1437,8 @@ static void qcom_channel_state_worker(struct work_struct *work)
		strncpy(chinfo.name, channel->name, sizeof(chinfo.name));
		chinfo.src = RPMSG_ADDR_ANY;
		chinfo.dst = RPMSG_ADDR_ANY;
		smd_ipc(channel->edge->ipc, false, NULL,
		"%s: unregistering ch %s\n", __func__, channel->name);
		rpmsg_unregister_device(&edge->dev, &chinfo);
		channel->registered = false;
		spin_lock_irqsave(&edge->channels_lock, flags);
		@@ -1458,6 +1580,13 @@ struct qcom_smd_edge qcom_smd_register_edge(struct device parent,
		edge->dev.of_node = node;
		edge->dev.groups = qcom_smd_edge_groups;
		dev_set_name(&edge->dev, "%s:%s", dev_name(parent), node->name);

		/* ipc logging handler */
		edge->ipc = ipc_log_context_create(4, dev_name(&edge->dev), 0);
		if (!edge->ipc)
		dev_info(&edge->dev, "%s: failed to create ipc log cntxt\n",
		__func__);

		ret = device_register(&edge->dev);
		if (ret) {
		pr_err("failed to register smd edge\n");
		@@ -1478,6 +1607,8 @@ struct qcom_smd_edge qcom_smd_register_edge(struct device parent,

		schedule_work(&edge->scan_work);

		smd_ipc(edge->ipc, false, NULL,
		"%s: %s:%s\n", __func__, dev_name(parent), node->name);
		return edge;

		unregister_dev:
		@@ -1535,6 +1666,8 @@ static int qcom_smd_remove_edge(struct device dev, void data)
		{
		struct qcom_smd_edge *edge = to_smd_edge(dev);

		ipc_log_context_destroy(edge->ipc);

		return qcom_smd_unregister_edge(edge);
		}