From mboxrd@z Thu Jan 1 00:00:00 1970 Return-Path: Received: from smtp.codeaurora.org ([198.145.29.96]:44040 "EHLO smtp.codeaurora.org" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S1750866AbdH1LqD (ORCPT ); Mon, 28 Aug 2017 07:46:03 -0400 Subject: Re: [PATCH v2 04/20] rpmsg: glink: Move the common glink protocol implementation to glink_native.c References: <1503559302-3744-1-git-send-email-sricharan@codeaurora.org> <1503559302-3744-5-git-send-email-sricharan@codeaurora.org> From: Arun Kumar Neelakantam Message-ID: Date: Mon, 28 Aug 2017 17:15:54 +0530 MIME-Version: 1.0 In-Reply-To: <1503559302-3744-5-git-send-email-sricharan@codeaurora.org> Content-Type: text/plain; charset=utf-8; format=flowed Content-Transfer-Encoding: 7bit Content-Language: en-US Sender: linux-remoteproc-owner@vger.kernel.org To: Sricharan R , ohad@wizery.com, bjorn.andersson@linaro.org, linux-remoteproc@vger.kernel.org, linux-kernel@vger.kernel.org, linux-arm-msm@vger.kernel.org, linux-arm-kernel@lists.infradead.org List-ID: On 8/24/2017 12:51 PM, Sricharan R wrote: > From: Bjorn Andersson > > Move the common part of glink core protocol implementation to > glink_native.c that can be shared with the smem based glink > transport in the later patches. > > Signed-off-by: Bjorn Andersson > Signed-off-by: Sricharan R Acked-by: Arun Kumar Neelakantam Regards, Arun N > --- > drivers/rpmsg/Kconfig | 6 +- > drivers/rpmsg/Makefile | 1 + > drivers/rpmsg/qcom_glink_native.c | 1017 +++++++++++++++++++++++++++++++++++++ > drivers/rpmsg/qcom_glink_native.h | 38 ++ > drivers/rpmsg/qcom_glink_rpm.c | 995 +----------------------------------- > 5 files changed, 1064 insertions(+), 993 deletions(-) > create mode 100644 drivers/rpmsg/qcom_glink_native.c > create mode 100644 drivers/rpmsg/qcom_glink_native.h > > diff --git a/drivers/rpmsg/Kconfig b/drivers/rpmsg/Kconfig > index 2a5d2b4..ac33688 100644 > --- a/drivers/rpmsg/Kconfig > +++ b/drivers/rpmsg/Kconfig > @@ -13,9 +13,13 @@ config RPMSG_CHAR > in /dev. They make it possible for user-space programs to send and > receive rpmsg packets. > > +config RPMSG_QCOM_GLINK_NATIVE > + tristate > + select RPMSG > + > config RPMSG_QCOM_GLINK_RPM > tristate "Qualcomm RPM Glink driver" > - select RPMSG > + select RPMSG_QCOM_GLINK_NATIVE > depends on HAS_IOMEM > depends on MAILBOX > help > diff --git a/drivers/rpmsg/Makefile b/drivers/rpmsg/Makefile > index 28cc190..09a756c 100644 > --- a/drivers/rpmsg/Makefile > +++ b/drivers/rpmsg/Makefile > @@ -1,5 +1,6 @@ > obj-$(CONFIG_RPMSG) += rpmsg_core.o > obj-$(CONFIG_RPMSG_CHAR) += rpmsg_char.o > obj-$(CONFIG_RPMSG_QCOM_GLINK_RPM) += qcom_glink_rpm.o > +obj-$(CONFIG_RPMSG_QCOM_GLINK_NATIVE) += qcom_glink_native.o > obj-$(CONFIG_RPMSG_QCOM_SMD) += qcom_smd.o > obj-$(CONFIG_RPMSG_VIRTIO) += virtio_rpmsg_bus.o > diff --git a/drivers/rpmsg/qcom_glink_native.c b/drivers/rpmsg/qcom_glink_native.c > new file mode 100644 > index 0000000..ffdf88e > --- /dev/null > +++ b/drivers/rpmsg/qcom_glink_native.c > @@ -0,0 +1,1017 @@ > +/* > + * Copyright (c) 2016-2017, Linaro Ltd > + * > + * This program is free software; you can redistribute it and/or modify > + * it under the terms of the GNU General Public License version 2 and > + * only version 2 as published by the Free Software Foundation. > + * > + * This program is distributed in the hope that it will be useful, > + * but WITHOUT ANY WARRANTY; without even the implied warranty of > + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the > + * GNU General Public License for more details. > + */ > + > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > + > +#include "rpmsg_internal.h" > +#include "qcom_glink_native.h" > + > +#define GLINK_NAME_SIZE 32 > + > +#define RPM_GLINK_CID_MIN 1 > +#define RPM_GLINK_CID_MAX 65536 > + > +struct glink_msg { > + __le16 cmd; > + __le16 param1; > + __le32 param2; > + u8 data[]; > +} __packed; > + > +/** > + * struct glink_defer_cmd - deferred incoming control message > + * @node: list node > + * @msg: message header > + * data: payload of the message > + * > + * Copy of a received control message, to be added to @rx_queue and processed > + * by @rx_work of @qcom_glink. > + */ > +struct glink_defer_cmd { > + struct list_head node; > + > + struct glink_msg msg; > + u8 data[]; > +}; > + > +/** > + * struct qcom_glink - driver context, relates to one remote subsystem > + * @dev: reference to the associated struct device > + * @mbox_client: mailbox client > + * @mbox_chan: mailbox channel > + * @rx_pipe: pipe object for receive FIFO > + * @tx_pipe: pipe object for transmit FIFO > + * @irq: IRQ for signaling incoming events > + * @rx_work: worker for handling received control messages > + * @rx_lock: protects the @rx_queue > + * @rx_queue: queue of received control messages to be processed in @rx_work > + * @tx_lock: synchronizes operations on the tx fifo > + * @idr_lock: synchronizes @lcids and @rcids modifications > + * @lcids: idr of all channels with a known local channel id > + * @rcids: idr of all channels with a known remote channel id > + */ > +struct qcom_glink { > + struct device *dev; > + > + struct mbox_client mbox_client; > + struct mbox_chan *mbox_chan; > + > + struct qcom_glink_pipe *rx_pipe; > + struct qcom_glink_pipe *tx_pipe; > + > + int irq; > + > + struct work_struct rx_work; > + spinlock_t rx_lock; > + struct list_head rx_queue; > + > + struct mutex tx_lock; > + > + struct mutex idr_lock; > + struct idr lcids; > + struct idr rcids; > +}; > + > +enum { > + GLINK_STATE_CLOSED, > + GLINK_STATE_OPENING, > + GLINK_STATE_OPEN, > + GLINK_STATE_CLOSING, > +}; > + > +/** > + * struct glink_channel - internal representation of a channel > + * @rpdev: rpdev reference, only used for primary endpoints > + * @ept: rpmsg endpoint this channel is associated with > + * @glink: qcom_glink context handle > + * @refcount: refcount for the channel object > + * @recv_lock: guard for @ept.cb > + * @name: unique channel name/identifier > + * @lcid: channel id, in local space > + * @rcid: channel id, in remote space > + * @buf: receive buffer, for gathering fragments > + * @buf_offset: write offset in @buf > + * @buf_size: size of current @buf > + * @open_ack: completed once remote has acked the open-request > + * @open_req: completed once open-request has been received > + */ > +struct glink_channel { > + struct rpmsg_endpoint ept; > + > + struct rpmsg_device *rpdev; > + struct qcom_glink *glink; > + > + struct kref refcount; > + > + spinlock_t recv_lock; > + > + char *name; > + unsigned int lcid; > + unsigned int rcid; > + > + void *buf; > + int buf_offset; > + int buf_size; > + > + struct completion open_ack; > + struct completion open_req; > +}; > + > +#define to_glink_channel(_ept) container_of(_ept, struct glink_channel, ept) > + > +static const struct rpmsg_endpoint_ops glink_endpoint_ops; > + > +#define RPM_CMD_VERSION 0 > +#define RPM_CMD_VERSION_ACK 1 > +#define RPM_CMD_OPEN 2 > +#define RPM_CMD_CLOSE 3 > +#define RPM_CMD_OPEN_ACK 4 > +#define RPM_CMD_TX_DATA 9 > +#define RPM_CMD_CLOSE_ACK 11 > +#define RPM_CMD_TX_DATA_CONT 12 > +#define RPM_CMD_READ_NOTIF 13 > + > +#define GLINK_FEATURE_INTENTLESS BIT(1) > + > +static struct glink_channel *qcom_glink_alloc_channel(struct qcom_glink *glink, > + const char *name) > +{ > + struct glink_channel *channel; > + > + channel = kzalloc(sizeof(*channel), GFP_KERNEL); > + if (!channel) > + return ERR_PTR(-ENOMEM); > + > + /* Setup glink internal glink_channel data */ > + spin_lock_init(&channel->recv_lock); > + channel->glink = glink; > + channel->name = kstrdup(name, GFP_KERNEL); > + > + init_completion(&channel->open_req); > + init_completion(&channel->open_ack); > + > + kref_init(&channel->refcount); > + > + return channel; > +} > + > +static void qcom_glink_channel_release(struct kref *ref) > +{ > + struct glink_channel *channel = container_of(ref, struct glink_channel, > + refcount); > + > + kfree(channel->name); > + kfree(channel); > +} > + > +static size_t qcom_glink_rx_avail(struct qcom_glink *glink) > +{ > + return glink->rx_pipe->avail(glink->rx_pipe); > +} > + > +static void qcom_glink_rx_peak(struct qcom_glink *glink, > + void *data, size_t count) > +{ > + glink->rx_pipe->peak(glink->rx_pipe, data, count); > +} > + > +static void qcom_glink_rx_advance(struct qcom_glink *glink, size_t count) > +{ > + glink->rx_pipe->advance(glink->rx_pipe, count); > +} > + > +static size_t qcom_glink_tx_avail(struct qcom_glink *glink) > +{ > + return glink->tx_pipe->avail(glink->tx_pipe); > +} > + > +static void qcom_glink_tx_write(struct qcom_glink *glink, > + const void *hdr, size_t hlen, > + const void *data, size_t dlen) > +{ > + glink->tx_pipe->write(glink->tx_pipe, hdr, hlen, data, dlen); > +} > + > +static int qcom_glink_tx(struct qcom_glink *glink, > + const void *hdr, size_t hlen, > + const void *data, size_t dlen, bool wait) > +{ > + unsigned int tlen = hlen + dlen; > + int ret; > + > + /* Reject packets that are too big */ > + if (tlen >= glink->tx_pipe->length) > + return -EINVAL; > + > + if (WARN(tlen % 8, "Unaligned TX request")) > + return -EINVAL; > + > + ret = mutex_lock_interruptible(&glink->tx_lock); > + if (ret) > + return ret; > + > + while (qcom_glink_tx_avail(glink) < tlen) { > + if (!wait) { > + ret = -ENOMEM; > + goto out; > + } > + > + usleep_range(10000, 15000); > + } > + > + qcom_glink_tx_write(glink, hdr, hlen, data, dlen); > + > + mbox_send_message(glink->mbox_chan, NULL); > + mbox_client_txdone(glink->mbox_chan, 0); > + > +out: > + mutex_unlock(&glink->tx_lock); > + > + return ret; > +} > + > +static int qcom_glink_send_version(struct qcom_glink *glink) > +{ > + struct glink_msg msg; > + > + msg.cmd = cpu_to_le16(RPM_CMD_VERSION); > + msg.param1 = cpu_to_le16(1); > + msg.param2 = cpu_to_le32(GLINK_FEATURE_INTENTLESS); > + > + return qcom_glink_tx(glink, &msg, sizeof(msg), NULL, 0, true); > +} > + > +static void qcom_glink_send_version_ack(struct qcom_glink *glink) > +{ > + struct glink_msg msg; > + > + msg.cmd = cpu_to_le16(RPM_CMD_VERSION_ACK); > + msg.param1 = cpu_to_le16(1); > + msg.param2 = cpu_to_le32(0); > + > + qcom_glink_tx(glink, &msg, sizeof(msg), NULL, 0, true); > +} > + > +static void qcom_glink_send_open_ack(struct qcom_glink *glink, > + struct glink_channel *channel) > +{ > + struct glink_msg msg; > + > + msg.cmd = cpu_to_le16(RPM_CMD_OPEN_ACK); > + msg.param1 = cpu_to_le16(channel->rcid); > + msg.param2 = cpu_to_le32(0); > + > + qcom_glink_tx(glink, &msg, sizeof(msg), NULL, 0, true); > +} > + > +/** > + * qcom_glink_send_open_req() - send a RPM_CMD_OPEN request to the remote > + * @glink: Ptr to the glink edge > + * @channel: Ptr to the channel that the open req is sent > + * > + * Allocates a local channel id and sends a RPM_CMD_OPEN message to the remote. > + * Will return with refcount held, regardless of outcome. > + * > + * Returns 0 on success, negative errno otherwise. > + */ > +static int qcom_glink_send_open_req(struct qcom_glink *glink, > + struct glink_channel *channel) > +{ > + struct { > + struct glink_msg msg; > + u8 name[GLINK_NAME_SIZE]; > + } __packed req; > + int name_len = strlen(channel->name) + 1; > + int req_len = ALIGN(sizeof(req.msg) + name_len, 8); > + int ret; > + > + kref_get(&channel->refcount); > + > + mutex_lock(&glink->idr_lock); > + ret = idr_alloc_cyclic(&glink->lcids, channel, > + RPM_GLINK_CID_MIN, RPM_GLINK_CID_MAX, > + GFP_KERNEL); > + mutex_unlock(&glink->idr_lock); > + if (ret < 0) > + return ret; > + > + channel->lcid = ret; > + > + req.msg.cmd = cpu_to_le16(RPM_CMD_OPEN); > + req.msg.param1 = cpu_to_le16(channel->lcid); > + req.msg.param2 = cpu_to_le32(name_len); > + strcpy(req.name, channel->name); > + > + ret = qcom_glink_tx(glink, &req, req_len, NULL, 0, true); > + if (ret) > + goto remove_idr; > + > + return 0; > + > +remove_idr: > + mutex_lock(&glink->idr_lock); > + idr_remove(&glink->lcids, channel->lcid); > + channel->lcid = 0; > + mutex_unlock(&glink->idr_lock); > + > + return ret; > +} > + > +static void qcom_glink_send_close_req(struct qcom_glink *glink, > + struct glink_channel *channel) > +{ > + struct glink_msg req; > + > + req.cmd = cpu_to_le16(RPM_CMD_CLOSE); > + req.param1 = cpu_to_le16(channel->lcid); > + req.param2 = 0; > + > + qcom_glink_tx(glink, &req, sizeof(req), NULL, 0, true); > +} > + > +static void qcom_glink_send_close_ack(struct qcom_glink *glink, > + unsigned int rcid) > +{ > + struct glink_msg req; > + > + req.cmd = cpu_to_le16(RPM_CMD_CLOSE_ACK); > + req.param1 = cpu_to_le16(rcid); > + req.param2 = 0; > + > + qcom_glink_tx(glink, &req, sizeof(req), NULL, 0, true); > +} > + > +static int qcom_glink_rx_defer(struct qcom_glink *glink, size_t extra) > +{ > + struct glink_defer_cmd *dcmd; > + > + extra = ALIGN(extra, 8); > + > + if (qcom_glink_rx_avail(glink) < sizeof(struct glink_msg) + extra) { > + dev_dbg(glink->dev, "Insufficient data in rx fifo"); > + return -ENXIO; > + } > + > + dcmd = kzalloc(sizeof(*dcmd) + extra, GFP_ATOMIC); > + if (!dcmd) > + return -ENOMEM; > + > + INIT_LIST_HEAD(&dcmd->node); > + > + qcom_glink_rx_peak(glink, &dcmd->msg, sizeof(dcmd->msg) + extra); > + > + spin_lock(&glink->rx_lock); > + list_add_tail(&dcmd->node, &glink->rx_queue); > + spin_unlock(&glink->rx_lock); > + > + schedule_work(&glink->rx_work); > + qcom_glink_rx_advance(glink, sizeof(dcmd->msg) + extra); > + > + return 0; > +} > + > +static int qcom_glink_rx_data(struct qcom_glink *glink, size_t avail) > +{ > + struct glink_channel *channel; > + struct { > + struct glink_msg msg; > + __le32 chunk_size; > + __le32 left_size; > + } __packed hdr; > + unsigned int chunk_size; > + unsigned int left_size; > + unsigned int rcid; > + > + if (avail < sizeof(hdr)) { > + dev_dbg(glink->dev, "Not enough data in fifo\n"); > + return -EAGAIN; > + } > + > + qcom_glink_rx_peak(glink, &hdr, sizeof(hdr)); > + chunk_size = le32_to_cpu(hdr.chunk_size); > + left_size = le32_to_cpu(hdr.left_size); > + > + if (avail < sizeof(hdr) + chunk_size) { > + dev_dbg(glink->dev, "Payload not yet in fifo\n"); > + return -EAGAIN; > + } > + > + if (WARN(chunk_size % 4, "Incoming data must be word aligned\n")) > + return -EINVAL; > + > + rcid = le16_to_cpu(hdr.msg.param1); > + channel = idr_find(&glink->rcids, rcid); > + if (!channel) { > + dev_dbg(glink->dev, "Data on non-existing channel\n"); > + > + /* Drop the message */ > + qcom_glink_rx_advance(glink, > + ALIGN(sizeof(hdr) + chunk_size, 8)); > + return 0; > + } > + > + /* Might have an ongoing, fragmented, message to append */ > + if (!channel->buf) { > + channel->buf = kmalloc(chunk_size + left_size, GFP_ATOMIC); > + if (!channel->buf) > + return -ENOMEM; > + > + channel->buf_size = chunk_size + left_size; > + channel->buf_offset = 0; > + } > + > + qcom_glink_rx_advance(glink, sizeof(hdr)); > + > + if (channel->buf_size - channel->buf_offset < chunk_size) { > + dev_err(glink->dev, "Insufficient space in input buffer\n"); > + > + /* The packet header lied, drop payload */ > + qcom_glink_rx_advance(glink, chunk_size); > + return -ENOMEM; > + } > + > + qcom_glink_rx_peak(glink, channel->buf + channel->buf_offset, > + chunk_size); > + channel->buf_offset += chunk_size; > + > + /* Handle message when no fragments remain to be received */ > + if (!left_size) { > + spin_lock(&channel->recv_lock); > + if (channel->ept.cb) { > + channel->ept.cb(channel->ept.rpdev, > + channel->buf, > + channel->buf_offset, > + channel->ept.priv, > + RPMSG_ADDR_ANY); > + } > + spin_unlock(&channel->recv_lock); > + > + kfree(channel->buf); > + channel->buf = NULL; > + channel->buf_size = 0; > + } > + > + /* Each message starts at 8 byte aligned address */ > + qcom_glink_rx_advance(glink, ALIGN(chunk_size, 8)); > + > + return 0; > +} > + > +static int qcom_glink_rx_open_ack(struct qcom_glink *glink, unsigned int lcid) > +{ > + struct glink_channel *channel; > + > + channel = idr_find(&glink->lcids, lcid); > + if (!channel) { > + dev_err(glink->dev, "Invalid open ack packet\n"); > + return -EINVAL; > + } > + > + complete(&channel->open_ack); > + > + return 0; > +} > + > +static irqreturn_t qcom_glink_native_intr(int irq, void *data) > +{ > + struct qcom_glink *glink = data; > + struct glink_msg msg; > + unsigned int param1; > + unsigned int param2; > + unsigned int avail; > + unsigned int cmd; > + int ret; > + > + for (;;) { > + avail = qcom_glink_rx_avail(glink); > + if (avail < sizeof(msg)) > + break; > + > + qcom_glink_rx_peak(glink, &msg, sizeof(msg)); > + > + cmd = le16_to_cpu(msg.cmd); > + param1 = le16_to_cpu(msg.param1); > + param2 = le32_to_cpu(msg.param2); > + > + switch (cmd) { > + case RPM_CMD_VERSION: > + case RPM_CMD_VERSION_ACK: > + case RPM_CMD_CLOSE: > + case RPM_CMD_CLOSE_ACK: > + ret = qcom_glink_rx_defer(glink, 0); > + break; > + case RPM_CMD_OPEN_ACK: > + ret = qcom_glink_rx_open_ack(glink, param1); > + qcom_glink_rx_advance(glink, ALIGN(sizeof(msg), 8)); > + break; > + case RPM_CMD_OPEN: > + ret = qcom_glink_rx_defer(glink, param2); > + break; > + case RPM_CMD_TX_DATA: > + case RPM_CMD_TX_DATA_CONT: > + ret = qcom_glink_rx_data(glink, avail); > + break; > + case RPM_CMD_READ_NOTIF: > + qcom_glink_rx_advance(glink, ALIGN(sizeof(msg), 8)); > + > + mbox_send_message(glink->mbox_chan, NULL); > + mbox_client_txdone(glink->mbox_chan, 0); > + > + ret = 0; > + break; > + default: > + dev_err(glink->dev, "unhandled rx cmd: %d\n", cmd); > + ret = -EINVAL; > + break; > + } > + > + if (ret) > + break; > + } > + > + return IRQ_HANDLED; > +} > + > +/* Locally initiated rpmsg_create_ept */ > +static struct glink_channel *qcom_glink_create_local(struct qcom_glink *glink, > + const char *name) > +{ > + struct glink_channel *channel; > + int ret; > + > + channel = qcom_glink_alloc_channel(glink, name); > + if (IS_ERR(channel)) > + return ERR_CAST(channel); > + > + ret = qcom_glink_send_open_req(glink, channel); > + if (ret) > + goto release_channel; > + > + ret = wait_for_completion_timeout(&channel->open_ack, 5 * HZ); > + if (!ret) > + goto err_timeout; > + > + ret = wait_for_completion_timeout(&channel->open_req, 5 * HZ); > + if (!ret) > + goto err_timeout; > + > + qcom_glink_send_open_ack(glink, channel); > + > + return channel; > + > +err_timeout: > + /* qcom_glink_send_open_req() did register the channel in lcids*/ > + mutex_lock(&glink->idr_lock); > + idr_remove(&glink->lcids, channel->lcid); > + mutex_unlock(&glink->idr_lock); > + > +release_channel: > + /* Release qcom_glink_send_open_req() reference */ > + kref_put(&channel->refcount, qcom_glink_channel_release); > + /* Release qcom_glink_alloc_channel() reference */ > + kref_put(&channel->refcount, qcom_glink_channel_release); > + > + return ERR_PTR(-ETIMEDOUT); > +} > + > +/* Remote initiated rpmsg_create_ept */ > +static int qcom_glink_create_remote(struct qcom_glink *glink, > + struct glink_channel *channel) > +{ > + int ret; > + > + qcom_glink_send_open_ack(glink, channel); > + > + ret = qcom_glink_send_open_req(glink, channel); > + if (ret) > + goto close_link; > + > + ret = wait_for_completion_timeout(&channel->open_ack, 5 * HZ); > + if (!ret) { > + ret = -ETIMEDOUT; > + goto close_link; > + } > + > + return 0; > + > +close_link: > + /* > + * Send a close request to "undo" our open-ack. The close-ack will > + * release the last reference. > + */ > + qcom_glink_send_close_req(glink, channel); > + > + /* Release qcom_glink_send_open_req() reference */ > + kref_put(&channel->refcount, qcom_glink_channel_release); > + > + return ret; > +} > + > +static struct rpmsg_endpoint *qcom_glink_create_ept(struct rpmsg_device *rpdev, > + rpmsg_rx_cb_t cb, > + void *priv, > + struct rpmsg_channel_info > + chinfo) > +{ > + struct glink_channel *parent = to_glink_channel(rpdev->ept); > + struct glink_channel *channel; > + struct qcom_glink *glink = parent->glink; > + struct rpmsg_endpoint *ept; > + const char *name = chinfo.name; > + int cid; > + int ret; > + > + idr_for_each_entry(&glink->rcids, channel, cid) { > + if (!strcmp(channel->name, name)) > + break; > + } > + > + if (!channel) { > + channel = qcom_glink_create_local(glink, name); > + if (IS_ERR(channel)) > + return NULL; > + } else { > + ret = qcom_glink_create_remote(glink, channel); > + if (ret) > + return NULL; > + } > + > + ept = &channel->ept; > + ept->rpdev = rpdev; > + ept->cb = cb; > + ept->priv = priv; > + ept->ops = &glink_endpoint_ops; > + > + return ept; > +} > + > +static void qcom_glink_destroy_ept(struct rpmsg_endpoint *ept) > +{ > + struct glink_channel *channel = to_glink_channel(ept); > + struct qcom_glink *glink = channel->glink; > + unsigned long flags; > + > + spin_lock_irqsave(&channel->recv_lock, flags); > + channel->ept.cb = NULL; > + spin_unlock_irqrestore(&channel->recv_lock, flags); > + > + /* Decouple the potential rpdev from the channel */ > + channel->rpdev = NULL; > + > + qcom_glink_send_close_req(glink, channel); > +} > + > +static int __qcom_glink_send(struct glink_channel *channel, > + void *data, int len, bool wait) > +{ > + struct qcom_glink *glink = channel->glink; > + struct { > + struct glink_msg msg; > + __le32 chunk_size; > + __le32 left_size; > + } __packed req; > + > + if (WARN(len % 8, "RPM GLINK expects 8 byte aligned messages\n")) > + return -EINVAL; > + > + req.msg.cmd = cpu_to_le16(RPM_CMD_TX_DATA); > + req.msg.param1 = cpu_to_le16(channel->lcid); > + req.msg.param2 = cpu_to_le32(channel->rcid); > + req.chunk_size = cpu_to_le32(len); > + req.left_size = cpu_to_le32(0); > + > + return qcom_glink_tx(glink, &req, sizeof(req), data, len, wait); > +} > + > +static int qcom_glink_send(struct rpmsg_endpoint *ept, void *data, int len) > +{ > + struct glink_channel *channel = to_glink_channel(ept); > + > + return __qcom_glink_send(channel, data, len, true); > +} > + > +static int qcom_glink_trysend(struct rpmsg_endpoint *ept, void *data, int len) > +{ > + struct glink_channel *channel = to_glink_channel(ept); > + > + return __qcom_glink_send(channel, data, len, false); > +} > + > +/* > + * Finds the device_node for the glink child interested in this channel. > + */ > +static struct device_node *qcom_glink_match_channel(struct device_node *node, > + const char *channel) > +{ > + struct device_node *child; > + const char *name; > + const char *key; > + int ret; > + > + for_each_available_child_of_node(node, child) { > + key = "qcom,glink-channels"; > + ret = of_property_read_string(child, key, &name); > + if (ret) > + continue; > + > + if (strcmp(name, channel) == 0) > + return child; > + } > + > + return NULL; > +} > + > +static const struct rpmsg_device_ops glink_device_ops = { > + .create_ept = qcom_glink_create_ept, > +}; > + > +static const struct rpmsg_endpoint_ops glink_endpoint_ops = { > + .destroy_ept = qcom_glink_destroy_ept, > + .send = qcom_glink_send, > + .trysend = qcom_glink_trysend, > +}; > + > +static void qcom_glink_rpdev_release(struct device *dev) > +{ > + struct rpmsg_device *rpdev = to_rpmsg_device(dev); > + struct glink_channel *channel = to_glink_channel(rpdev->ept); > + > + channel->rpdev = NULL; > + kfree(rpdev); > +} > + > +static int qcom_glink_rx_open(struct qcom_glink *glink, unsigned int rcid, > + char *name) > +{ > + struct glink_channel *channel; > + struct rpmsg_device *rpdev; > + bool create_device = false; > + struct device_node *node; > + int lcid; > + int ret; > + > + idr_for_each_entry(&glink->lcids, channel, lcid) { > + if (!strcmp(channel->name, name)) > + break; > + } > + > + if (!channel) { > + channel = qcom_glink_alloc_channel(glink, name); > + if (IS_ERR(channel)) > + return PTR_ERR(channel); > + > + /* The opening dance was initiated by the remote */ > + create_device = true; > + } > + > + mutex_lock(&glink->idr_lock); > + ret = idr_alloc(&glink->rcids, channel, rcid, rcid + 1, GFP_KERNEL); > + if (ret < 0) { > + dev_err(glink->dev, "Unable to insert channel into rcid list\n"); > + mutex_unlock(&glink->idr_lock); > + goto free_channel; > + } > + channel->rcid = ret; > + mutex_unlock(&glink->idr_lock); > + > + complete(&channel->open_req); > + > + if (create_device) { > + rpdev = kzalloc(sizeof(*rpdev), GFP_KERNEL); > + if (!rpdev) { > + ret = -ENOMEM; > + goto rcid_remove; > + } > + > + rpdev->ept = &channel->ept; > + strncpy(rpdev->id.name, name, RPMSG_NAME_SIZE); > + rpdev->src = RPMSG_ADDR_ANY; > + rpdev->dst = RPMSG_ADDR_ANY; > + rpdev->ops = &glink_device_ops; > + > + node = qcom_glink_match_channel(glink->dev->of_node, name); > + rpdev->dev.of_node = node; > + rpdev->dev.parent = glink->dev; > + rpdev->dev.release = qcom_glink_rpdev_release; > + > + ret = rpmsg_register_device(rpdev); > + if (ret) > + goto free_rpdev; > + > + channel->rpdev = rpdev; > + } > + > + return 0; > + > +free_rpdev: > + kfree(rpdev); > +rcid_remove: > + mutex_lock(&glink->idr_lock); > + idr_remove(&glink->rcids, channel->rcid); > + channel->rcid = 0; > + mutex_unlock(&glink->idr_lock); > +free_channel: > + /* Release the reference, iff we took it */ > + if (create_device) > + kref_put(&channel->refcount, qcom_glink_channel_release); > + > + return ret; > +} > + > +static void qcom_glink_rx_close(struct qcom_glink *glink, unsigned int rcid) > +{ > + struct rpmsg_channel_info chinfo; > + struct glink_channel *channel; > + > + channel = idr_find(&glink->rcids, rcid); > + if (WARN(!channel, "close request on unknown channel\n")) > + return; > + > + if (channel->rpdev) { > + strncpy(chinfo.name, channel->name, sizeof(chinfo.name)); > + chinfo.src = RPMSG_ADDR_ANY; > + chinfo.dst = RPMSG_ADDR_ANY; > + > + rpmsg_unregister_device(glink->dev, &chinfo); > + } > + > + qcom_glink_send_close_ack(glink, channel->rcid); > + > + mutex_lock(&glink->idr_lock); > + idr_remove(&glink->rcids, channel->rcid); > + channel->rcid = 0; > + mutex_unlock(&glink->idr_lock); > + > + kref_put(&channel->refcount, qcom_glink_channel_release); > +} > + > +static void qcom_glink_rx_close_ack(struct qcom_glink *glink, unsigned int lcid) > +{ > + struct glink_channel *channel; > + > + channel = idr_find(&glink->lcids, lcid); > + if (WARN(!channel, "close ack on unknown channel\n")) > + return; > + > + mutex_lock(&glink->idr_lock); > + idr_remove(&glink->lcids, channel->lcid); > + channel->lcid = 0; > + mutex_unlock(&glink->idr_lock); > + > + kref_put(&channel->refcount, qcom_glink_channel_release); > +} > + > +static void qcom_glink_work(struct work_struct *work) > +{ > + struct qcom_glink *glink = container_of(work, struct qcom_glink, > + rx_work); > + struct glink_defer_cmd *dcmd; > + struct glink_msg *msg; > + unsigned long flags; > + unsigned int param1; > + unsigned int param2; > + unsigned int cmd; > + > + for (;;) { > + spin_lock_irqsave(&glink->rx_lock, flags); > + if (list_empty(&glink->rx_queue)) { > + spin_unlock_irqrestore(&glink->rx_lock, flags); > + break; > + } > + dcmd = list_first_entry(&glink->rx_queue, > + struct glink_defer_cmd, node); > + list_del(&dcmd->node); > + spin_unlock_irqrestore(&glink->rx_lock, flags); > + > + msg = &dcmd->msg; > + cmd = le16_to_cpu(msg->cmd); > + param1 = le16_to_cpu(msg->param1); > + param2 = le32_to_cpu(msg->param2); > + > + switch (cmd) { > + case RPM_CMD_VERSION: > + qcom_glink_send_version_ack(glink); > + break; > + case RPM_CMD_VERSION_ACK: > + break; > + case RPM_CMD_OPEN: > + qcom_glink_rx_open(glink, param1, msg->data); > + break; > + case RPM_CMD_CLOSE: > + qcom_glink_rx_close(glink, param1); > + break; > + case RPM_CMD_CLOSE_ACK: > + qcom_glink_rx_close_ack(glink, param1); > + break; > + default: > + WARN(1, "Unknown defer object %d\n", cmd); > + break; > + } > + > + kfree(dcmd); > + } > +} > + > +struct qcom_glink *qcom_glink_native_probe(struct device *dev, > + struct qcom_glink_pipe *rx, > + struct qcom_glink_pipe *tx) > +{ > + int irq; > + int ret; > + struct qcom_glink *glink; > + > + glink = devm_kzalloc(dev, sizeof(*glink), GFP_KERNEL); > + if (!glink) > + return ERR_PTR(-ENOMEM); > + > + glink->dev = dev; > + glink->tx_pipe = tx; > + glink->rx_pipe = rx; > + > + mutex_init(&glink->tx_lock); > + spin_lock_init(&glink->rx_lock); > + INIT_LIST_HEAD(&glink->rx_queue); > + INIT_WORK(&glink->rx_work, qcom_glink_work); > + > + mutex_init(&glink->idr_lock); > + idr_init(&glink->lcids); > + idr_init(&glink->rcids); > + > + glink->mbox_client.dev = dev; > + glink->mbox_chan = mbox_request_channel(&glink->mbox_client, 0); > + if (IS_ERR(glink->mbox_chan)) { > + if (PTR_ERR(glink->mbox_chan) != -EPROBE_DEFER) > + dev_err(dev, "failed to acquire IPC channel\n"); > + return ERR_CAST(glink->mbox_chan); > + } > + > + irq = of_irq_get(dev->of_node, 0); > + ret = devm_request_irq(dev, irq, > + qcom_glink_native_intr, > + IRQF_NO_SUSPEND | IRQF_SHARED, > + "glink-native", glink); > + if (ret) { > + dev_err(dev, "failed to request IRQ\n"); > + return ERR_PTR(ret); > + } > + > + glink->irq = irq; > + > + ret = qcom_glink_send_version(glink); > + if (ret) > + return ERR_PTR(ret); > + > + return glink; > +} > + > +static int qcom_glink_remove_device(struct device *dev, void *data) > +{ > + device_unregister(dev); > + > + return 0; > +} > + > +void qcom_glink_native_remove(struct qcom_glink *glink) > +{ > + struct glink_channel *channel; > + int cid; > + int ret; > + > + disable_irq(glink->irq); > + cancel_work_sync(&glink->rx_work); > + > + ret = device_for_each_child(glink->dev, NULL, qcom_glink_remove_device); > + if (ret) > + dev_warn(glink->dev, "Can't remove GLINK devices: %d\n", ret); > + > + /* Release any defunct local channels, waiting for close-ack */ > + idr_for_each_entry(&glink->lcids, channel, cid) > + kref_put(&channel->refcount, qcom_glink_channel_release); > + > + idr_destroy(&glink->lcids); > + idr_destroy(&glink->rcids); > +} > diff --git a/drivers/rpmsg/qcom_glink_native.h b/drivers/rpmsg/qcom_glink_native.h > new file mode 100644 > index 0000000..d5627a4 > --- /dev/null > +++ b/drivers/rpmsg/qcom_glink_native.h > @@ -0,0 +1,38 @@ > +/* > + * Copyright (c) 2016-2017, Linaro Ltd > + * > + * This program is free software; you can redistribute it and/or modify > + * it under the terms of the GNU General Public License version 2 and > + * only version 2 as published by the Free Software Foundation. > + * > + * This program is distributed in the hope that it will be useful, > + * but WITHOUT ANY WARRANTY; without even the implied warranty of > + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the > + * GNU General Public License for more details. > + */ > + > +#ifndef __QCOM_GLINK_NATIVE_H__ > +#define __QCOM_GLINK_NATIVE_H__ > + > +struct qcom_glink_pipe { > + size_t length; > + > + size_t (*avail)(struct qcom_glink_pipe *glink_pipe); > + > + void (*peak)(struct qcom_glink_pipe *glink_pipe, void *data, > + size_t count); > + void (*advance)(struct qcom_glink_pipe *glink_pipe, size_t count); > + > + void (*write)(struct qcom_glink_pipe *glink_pipe, > + const void *hdr, size_t hlen, > + const void *data, size_t dlen); > +}; > + > +struct qcom_glink; > + > +struct qcom_glink *qcom_glink_native_probe(struct device *dev, > + struct qcom_glink_pipe *rx, > + struct qcom_glink_pipe *tx); > +void qcom_glink_native_remove(struct qcom_glink *glink); > + > +#endif > diff --git a/drivers/rpmsg/qcom_glink_rpm.c b/drivers/rpmsg/qcom_glink_rpm.c > index 5f0fa0d..33daa32 100644 > --- a/drivers/rpmsg/qcom_glink_rpm.c > +++ b/drivers/rpmsg/qcom_glink_rpm.c > @@ -19,7 +19,6 @@ > #include > #include > #include > -#include > #include > #include > #include > @@ -28,6 +27,7 @@ > #include > > #include "rpmsg_internal.h" > +#include "qcom_glink_native.h" > > #define RPM_TOC_SIZE 256 > #define RPM_TOC_MAGIC 0x67727430 /* grt0 */ > @@ -37,12 +37,7 @@ > #define RPM_TX_FIFO_ID 0x61703272 /* ap2r */ > #define RPM_RX_FIFO_ID 0x72326170 /* r2ap */ > > -#define GLINK_NAME_SIZE 32 > - > -#define RPM_GLINK_CID_MIN 1 > -#define RPM_GLINK_CID_MAX 65536 > - > -#define to_rpm_pipe(p) container_of(p, struct glink_rpm_pipe, native) > +#define to_rpm_pipe(p) container_of(p, struct glink_rpm_pipe, native) > > struct rpm_toc_entry { > __le32 id; > @@ -50,20 +45,6 @@ struct rpm_toc_entry { > __le32 size; > } __packed; > > -struct qcom_glink; > - > -struct qcom_glink_pipe { > - size_t length; > - > - size_t (*avail)(struct qcom_glink_pipe *glink_pipe); > - void (*peak)(struct qcom_glink_pipe *glink_pipe, void *data, > - size_t count); > - void (*advance)(struct qcom_glink_pipe *glink_pipe, size_t count); > - void (*write)(struct qcom_glink_pipe *glink_pipe, > - const void *hdr, size_t hlen, > - const void *data, size_t dlen); > -}; > - > struct rpm_toc { > __le32 magic; > __le32 count; > @@ -71,13 +52,6 @@ struct rpm_toc { > struct rpm_toc_entry entries[]; > } __packed; > > -struct glink_msg { > - __le16 cmd; > - __le16 param1; > - __le32 param2; > - u8 data[]; > -} __packed; > - > struct glink_rpm_pipe { > struct qcom_glink_pipe native; > > @@ -87,151 +61,6 @@ struct glink_rpm_pipe { > void __iomem *fifo; > }; > > -/** > - * struct glink_defer_cmd - deferred incoming control message > - * @node: list node > - * @msg: message header > - * data: payload of the message > - * > - * Copy of a received control message, to be added to @rx_queue and processed > - * by @rx_work of @glink_rpm. > - */ > -struct glink_defer_cmd { > - struct list_head node; > - > - struct glink_msg msg; > - u8 data[]; > -}; > - > -/** > - * struct glink_rpm - driver context, relates to one remote subsystem > - * @dev: reference to the associated struct device > - * @doorbell: "rpm_hlos" ipc doorbell > - * @rx_pipe: pipe object for receive FIFO > - * @tx_pipe: pipe object for transmit FIFO > - * @irq: IRQ for signaling incoming events > - * @rx_work: worker for handling received control messages > - * @rx_lock: protects the @rx_queue > - * @rx_queue: queue of received control messages to be processed in @rx_work > - * @tx_lock: synchronizes operations on the tx fifo > - * @idr_lock: synchronizes @lcids and @rcids modifications > - * @lcids: idr of all channels with a known local channel id > - * @rcids: idr of all channels with a known remote channel id > - */ > -struct qcom_glink { > - struct device *dev; > - > - struct mbox_client mbox_client; > - struct mbox_chan *mbox_chan; > - > - struct qcom_glink_pipe *rx_pipe; > - struct qcom_glink_pipe *tx_pipe; > - > - int irq; > - > - struct work_struct rx_work; > - spinlock_t rx_lock; > - struct list_head rx_queue; > - > - struct mutex tx_lock; > - > - struct mutex idr_lock; > - struct idr lcids; > - struct idr rcids; > -}; > - > -enum { > - GLINK_STATE_CLOSED, > - GLINK_STATE_OPENING, > - GLINK_STATE_OPEN, > - GLINK_STATE_CLOSING, > -}; > - > -/** > - * struct glink_channel - internal representation of a channel > - * @rpdev: rpdev reference, only used for primary endpoints > - * @ept: rpmsg endpoint this channel is associated with > - * @glink: qcom_glink context handle > - * @refcount: refcount for the channel object > - * @recv_lock: guard for @ept.cb > - * @name: unique channel name/identifier > - * @lcid: channel id, in local space > - * @rcid: channel id, in remote space > - * @buf: receive buffer, for gathering fragments > - * @buf_offset: write offset in @buf > - * @buf_size: size of current @buf > - * @open_ack: completed once remote has acked the open-request > - * @open_req: completed once open-request has been received > - */ > -struct glink_channel { > - struct rpmsg_endpoint ept; > - > - struct rpmsg_device *rpdev; > - struct qcom_glink *glink; > - > - struct kref refcount; > - > - spinlock_t recv_lock; > - > - char *name; > - unsigned int lcid; > - unsigned int rcid; > - > - void *buf; > - int buf_offset; > - int buf_size; > - > - struct completion open_ack; > - struct completion open_req; > -}; > - > -#define to_glink_channel(_ept) container_of(_ept, struct glink_channel, ept) > - > -static const struct rpmsg_endpoint_ops glink_endpoint_ops; > - > -#define RPM_CMD_VERSION 0 > -#define RPM_CMD_VERSION_ACK 1 > -#define RPM_CMD_OPEN 2 > -#define RPM_CMD_CLOSE 3 > -#define RPM_CMD_OPEN_ACK 4 > -#define RPM_CMD_TX_DATA 9 > -#define RPM_CMD_CLOSE_ACK 11 > -#define RPM_CMD_TX_DATA_CONT 12 > -#define RPM_CMD_READ_NOTIF 13 > - > -#define GLINK_FEATURE_INTENTLESS BIT(1) > - > -static struct glink_channel *qcom_glink_alloc_channel(struct qcom_glink *glink, > - const char *name) > -{ > - struct glink_channel *channel; > - > - channel = kzalloc(sizeof(*channel), GFP_KERNEL); > - if (!channel) > - return ERR_PTR(-ENOMEM); > - > - /* Setup glink internal glink_channel data */ > - spin_lock_init(&channel->recv_lock); > - channel->glink = glink; > - channel->name = kstrdup(name, GFP_KERNEL); > - > - init_completion(&channel->open_req); > - init_completion(&channel->open_ack); > - > - kref_init(&channel->refcount); > - > - return channel; > -} > - > -static void qcom_glink_channel_release(struct kref *ref) > -{ > - struct glink_channel *channel = container_of(ref, struct glink_channel, > - refcount); > - > - kfree(channel->name); > - kfree(channel); > -} > - > static size_t glink_rpm_rx_avail(struct qcom_glink_pipe *glink_pipe) > { > struct glink_rpm_pipe *pipe = to_rpm_pipe(glink_pipe); > @@ -247,11 +76,6 @@ static size_t glink_rpm_rx_avail(struct qcom_glink_pipe *glink_pipe) > return head - tail; > } > > -static size_t qcom_glink_rx_avail(struct qcom_glink *glink) > -{ > - return glink->rx_pipe->avail(glink->rx_pipe); > -} > - > static void glink_rpm_rx_peak(struct qcom_glink_pipe *glink_pipe, > void *data, size_t count) > { > @@ -273,12 +97,6 @@ static void glink_rpm_rx_peak(struct qcom_glink_pipe *glink_pipe, > } > } > > -static void qcom_glink_rx_peak(struct qcom_glink *glink, > - void *data, size_t count) > -{ > - glink->rx_pipe->peak(glink->rx_pipe, data, count); > -} > - > static void glink_rpm_rx_advance(struct qcom_glink_pipe *glink_pipe, > size_t count) > { > @@ -294,11 +112,6 @@ static void glink_rpm_rx_advance(struct qcom_glink_pipe *glink_pipe, > writel(tail, pipe->tail); > } > > -static void qcom_glink_rx_advance(struct qcom_glink *glink, size_t count) > -{ > - glink->rx_pipe->advance(glink->rx_pipe, count); > -} > - > static size_t glink_rpm_tx_avail(struct qcom_glink_pipe *glink_pipe) > { > struct glink_rpm_pipe *pipe = to_rpm_pipe(glink_pipe); > @@ -314,11 +127,6 @@ static size_t glink_rpm_tx_avail(struct qcom_glink_pipe *glink_pipe) > return tail - head; > } > > -static size_t qcom_glink_tx_avail(struct qcom_glink *glink) > -{ > - return glink->tx_pipe->avail(glink->tx_pipe); > -} > - > static unsigned int glink_rpm_tx_write_one(struct glink_rpm_pipe *pipe, > unsigned int head, > const void *data, size_t count) > @@ -356,731 +164,6 @@ static void glink_rpm_tx_write(struct qcom_glink_pipe *glink_pipe, > writel(head, pipe->head); > } > > -static void qcom_glink_tx_write(struct qcom_glink *glink, > - const void *hdr, size_t hlen, > - const void *data, size_t dlen) > -{ > - glink->tx_pipe->write(glink->tx_pipe, hdr, hlen, data, dlen); > -} > - > -static int qcom_glink_tx(struct qcom_glink *glink, > - const void *hdr, size_t hlen, > - const void *data, size_t dlen, bool wait) > -{ > - unsigned int tlen = hlen + dlen; > - int ret; > - > - /* Reject packets that are too big */ > - if (tlen >= glink->tx_pipe->length) > - return -EINVAL; > - > - if (WARN(tlen % 8, "Unaligned TX request")) > - return -EINVAL; > - > - ret = mutex_lock_interruptible(&glink->tx_lock); > - if (ret) > - return ret; > - > - while (qcom_glink_tx_avail(glink) < tlen) { > - if (!wait) { > - ret = -ENOMEM; > - goto out; > - } > - > - msleep(10); > - } > - > - qcom_glink_tx_write(glink, hdr, hlen, data, dlen); > - > - mbox_send_message(glink->mbox_chan, NULL); > - mbox_client_txdone(glink->mbox_chan, 0); > - > -out: > - mutex_unlock(&glink->tx_lock); > - > - return ret; > -} > - > -static int qcom_glink_send_version(struct qcom_glink *glink) > -{ > - struct glink_msg msg; > - > - msg.cmd = cpu_to_le16(RPM_CMD_VERSION); > - msg.param1 = cpu_to_le16(1); > - msg.param2 = cpu_to_le32(GLINK_FEATURE_INTENTLESS); > - > - return qcom_glink_tx(glink, &msg, sizeof(msg), NULL, 0, true); > -} > - > -static void qcom_glink_send_version_ack(struct qcom_glink *glink) > -{ > - struct glink_msg msg; > - > - msg.cmd = cpu_to_le16(RPM_CMD_VERSION_ACK); > - msg.param1 = cpu_to_le16(1); > - msg.param2 = cpu_to_le32(0); > - > - qcom_glink_tx(glink, &msg, sizeof(msg), NULL, 0, true); > -} > - > -static void qcom_glink_send_open_ack(struct qcom_glink *glink, > - struct glink_channel *channel) > -{ > - struct glink_msg msg; > - > - msg.cmd = cpu_to_le16(RPM_CMD_OPEN_ACK); > - msg.param1 = cpu_to_le16(channel->rcid); > - msg.param2 = cpu_to_le32(0); > - > - qcom_glink_tx(glink, &msg, sizeof(msg), NULL, 0, true); > -} > - > -/** > - * qcom_glink_send_open_req() - send a RPM_CMD_OPEN request to the remote > - * @glink: > - * @channel: > - * > - * Allocates a local channel id and sends a RPM_CMD_OPEN message to the remote. > - * Will return with refcount held, regardless of outcome. > - * > - * Returns 0 on success, negative errno otherwise. > - */ > -static int qcom_glink_send_open_req(struct qcom_glink *glink, > - struct glink_channel *channel) > -{ > - struct { > - struct glink_msg msg; > - u8 name[GLINK_NAME_SIZE]; > - } __packed req; > - int name_len = strlen(channel->name) + 1; > - int req_len = ALIGN(sizeof(req.msg) + name_len, 8); > - int ret; > - > - kref_get(&channel->refcount); > - > - mutex_lock(&glink->idr_lock); > - ret = idr_alloc_cyclic(&glink->lcids, channel, > - RPM_GLINK_CID_MIN, RPM_GLINK_CID_MAX, GFP_KERNEL); > - mutex_unlock(&glink->idr_lock); > - if (ret < 0) > - return ret; > - > - channel->lcid = ret; > - > - req.msg.cmd = cpu_to_le16(RPM_CMD_OPEN); > - req.msg.param1 = cpu_to_le16(channel->lcid); > - req.msg.param2 = cpu_to_le32(name_len); > - strcpy(req.name, channel->name); > - > - ret = qcom_glink_tx(glink, &req, req_len, NULL, 0, true); > - if (ret) > - goto remove_idr; > - > - return 0; > - > -remove_idr: > - mutex_lock(&glink->idr_lock); > - idr_remove(&glink->lcids, channel->lcid); > - channel->lcid = 0; > - mutex_unlock(&glink->idr_lock); > - > - return ret; > -} > - > -static void qcom_glink_send_close_req(struct qcom_glink *glink, > - struct glink_channel *channel) > -{ > - struct glink_msg req; > - > - req.cmd = cpu_to_le16(RPM_CMD_CLOSE); > - req.param1 = cpu_to_le16(channel->lcid); > - req.param2 = 0; > - > - qcom_glink_tx(glink, &req, sizeof(req), NULL, 0, true); > -} > - > -static void qcom_glink_send_close_ack(struct qcom_glink *glink, > - unsigned int rcid) > -{ > - struct glink_msg req; > - > - req.cmd = cpu_to_le16(RPM_CMD_CLOSE_ACK); > - req.param1 = cpu_to_le16(rcid); > - req.param2 = 0; > - > - qcom_glink_tx(glink, &req, sizeof(req), NULL, 0, true); > -} > - > -static int qcom_glink_rx_defer(struct qcom_glink *glink, size_t extra) > -{ > - struct glink_defer_cmd *dcmd; > - > - extra = ALIGN(extra, 8); > - > - if (qcom_glink_rx_avail(glink) < sizeof(struct glink_msg) + extra) { > - dev_dbg(glink->dev, "Insufficient data in rx fifo"); > - return -ENXIO; > - } > - > - dcmd = kzalloc(sizeof(*dcmd) + extra, GFP_ATOMIC); > - if (!dcmd) > - return -ENOMEM; > - > - INIT_LIST_HEAD(&dcmd->node); > - > - qcom_glink_rx_peak(glink, &dcmd->msg, sizeof(dcmd->msg) + extra); > - > - spin_lock(&glink->rx_lock); > - list_add_tail(&dcmd->node, &glink->rx_queue); > - spin_unlock(&glink->rx_lock); > - > - schedule_work(&glink->rx_work); > - qcom_glink_rx_advance(glink, sizeof(dcmd->msg) + extra); > - > - return 0; > -} > - > -static int qcom_glink_rx_data(struct qcom_glink *glink, size_t avail) > -{ > - struct glink_channel *channel; > - struct { > - struct glink_msg msg; > - __le32 chunk_size; > - __le32 left_size; > - } __packed hdr; > - unsigned int chunk_size; > - unsigned int left_size; > - unsigned int rcid; > - > - if (avail < sizeof(hdr)) { > - dev_dbg(glink->dev, "Not enough data in fifo\n"); > - return -EAGAIN; > - } > - > - qcom_glink_rx_peak(glink, &hdr, sizeof(hdr)); > - chunk_size = le32_to_cpu(hdr.chunk_size); > - left_size = le32_to_cpu(hdr.left_size); > - > - if (avail < sizeof(hdr) + chunk_size) { > - dev_dbg(glink->dev, "Payload not yet in fifo\n"); > - return -EAGAIN; > - } > - > - if (WARN(chunk_size % 4, "Incoming data must be word aligned\n")) > - return -EINVAL; > - > - rcid = le16_to_cpu(hdr.msg.param1); > - channel = idr_find(&glink->rcids, rcid); > - if (!channel) { > - dev_dbg(glink->dev, "Data on non-existing channel\n"); > - > - /* Drop the message */ > - qcom_glink_rx_advance(glink, > - ALIGN(sizeof(hdr) + chunk_size, 8)); > - return 0; > - } > - > - /* Might have an ongoing, fragmented, message to append */ > - if (!channel->buf) { > - channel->buf = kmalloc(chunk_size + left_size, GFP_ATOMIC); > - if (!channel->buf) > - return -ENOMEM; > - > - channel->buf_size = chunk_size + left_size; > - channel->buf_offset = 0; > - } > - > - qcom_glink_rx_advance(glink, sizeof(hdr)); > - > - if (channel->buf_size - channel->buf_offset < chunk_size) { > - dev_err(glink->dev, "Insufficient space in input buffer\n"); > - > - /* The packet header lied, drop payload */ > - qcom_glink_rx_advance(glink, chunk_size); > - return -ENOMEM; > - } > - > - qcom_glink_rx_peak(glink, channel->buf + channel->buf_offset, > - chunk_size); > - channel->buf_offset += chunk_size; > - > - /* Handle message when no fragments remain to be received */ > - if (!left_size) { > - spin_lock(&channel->recv_lock); > - if (channel->ept.cb) { > - channel->ept.cb(channel->ept.rpdev, > - channel->buf, > - channel->buf_offset, > - channel->ept.priv, > - RPMSG_ADDR_ANY); > - } > - spin_unlock(&channel->recv_lock); > - > - kfree(channel->buf); > - channel->buf = NULL; > - channel->buf_size = 0; > - } > - > - /* Each message starts at 8 byte aligned address */ > - qcom_glink_rx_advance(glink, ALIGN(chunk_size, 8)); > - > - return 0; > -} > - > -static int qcom_glink_rx_open_ack(struct qcom_glink *glink, unsigned int lcid) > -{ > - struct glink_channel *channel; > - > - channel = idr_find(&glink->lcids, lcid); > - if (!channel) { > - dev_err(glink->dev, "Invalid open ack packet\n"); > - return -EINVAL; > - } > - > - complete(&channel->open_ack); > - > - return 0; > -} > - > -static irqreturn_t qcom_glink_intr(int irq, void *data) > -{ > - struct qcom_glink *glink = data; > - struct glink_msg msg; > - unsigned int param1; > - unsigned int param2; > - unsigned int avail; > - unsigned int cmd; > - int ret; > - > - for (;;) { > - avail = qcom_glink_rx_avail(glink); > - if (avail < sizeof(msg)) > - break; > - > - qcom_glink_rx_peak(glink, &msg, sizeof(msg)); > - > - cmd = le16_to_cpu(msg.cmd); > - param1 = le16_to_cpu(msg.param1); > - param2 = le32_to_cpu(msg.param2); > - > - switch (cmd) { > - case RPM_CMD_VERSION: > - case RPM_CMD_VERSION_ACK: > - case RPM_CMD_CLOSE: > - case RPM_CMD_CLOSE_ACK: > - ret = qcom_glink_rx_defer(glink, 0); > - break; > - case RPM_CMD_OPEN_ACK: > - ret = qcom_glink_rx_open_ack(glink, param1); > - qcom_glink_rx_advance(glink, ALIGN(sizeof(msg), 8)); > - break; > - case RPM_CMD_OPEN: > - ret = qcom_glink_rx_defer(glink, param2); > - break; > - case RPM_CMD_TX_DATA: > - case RPM_CMD_TX_DATA_CONT: > - ret = qcom_glink_rx_data(glink, avail); > - break; > - case RPM_CMD_READ_NOTIF: > - qcom_glink_rx_advance(glink, ALIGN(sizeof(msg), 8)); > - > - mbox_send_message(glink->mbox_chan, NULL); > - mbox_client_txdone(glink->mbox_chan, 0); > - > - ret = 0; > - break; > - default: > - dev_err(glink->dev, "unhandled rx cmd: %d\n", cmd); > - ret = -EINVAL; > - break; > - } > - > - if (ret) > - break; > - } > - > - return IRQ_HANDLED; > -} > - > -/* Locally initiated rpmsg_create_ept */ > -static struct glink_channel *qcom_glink_create_local(struct qcom_glink *glink, > - const char *name) > -{ > - struct glink_channel *channel; > - int ret; > - > - channel = qcom_glink_alloc_channel(glink, name); > - if (IS_ERR(channel)) > - return ERR_CAST(channel); > - > - ret = qcom_glink_send_open_req(glink, channel); > - if (ret) > - goto release_channel; > - > - ret = wait_for_completion_timeout(&channel->open_ack, 5 * HZ); > - if (!ret) > - goto err_timeout; > - > - ret = wait_for_completion_timeout(&channel->open_req, 5 * HZ); > - if (!ret) > - goto err_timeout; > - > - qcom_glink_send_open_ack(glink, channel); > - > - return channel; > - > -err_timeout: > - /* qcom_glink_send_open_req() did register the channel in lcids*/ > - mutex_lock(&glink->idr_lock); > - idr_remove(&glink->lcids, channel->lcid); > - mutex_unlock(&glink->idr_lock); > - > -release_channel: > - /* Release qcom_glink_send_open_req() reference */ > - kref_put(&channel->refcount, qcom_glink_channel_release); > - /* Release qcom_glink_alloc_channel() reference */ > - kref_put(&channel->refcount, qcom_glink_channel_release); > - > - return ERR_PTR(-ETIMEDOUT); > -} > - > -/* Remote initiated rpmsg_create_ept */ > -static int qcom_glink_create_remote(struct qcom_glink *glink, > - struct glink_channel *channel) > -{ > - int ret; > - > - qcom_glink_send_open_ack(glink, channel); > - > - ret = qcom_glink_send_open_req(glink, channel); > - if (ret) > - goto close_link; > - > - ret = wait_for_completion_timeout(&channel->open_ack, 5 * HZ); > - if (!ret) { > - ret = -ETIMEDOUT; > - goto close_link; > - } > - > - return 0; > - > -close_link: > - /* > - * Send a close request to "undo" our open-ack. The close-ack will > - * release the last reference. > - */ > - qcom_glink_send_close_req(glink, channel); > - > - /* Release qcom_glink_send_open_req() reference */ > - kref_put(&channel->refcount, qcom_glink_channel_release); > - > - return ret; > -} > - > -static struct rpmsg_endpoint *qcom_glink_create_ept(struct rpmsg_device *rpdev, > - rpmsg_rx_cb_t cb, > - void *priv, > - struct rpmsg_channel_info > - chinfo) > -{ > - struct glink_channel *parent = to_glink_channel(rpdev->ept); > - struct glink_channel *channel; > - struct qcom_glink *glink = parent->glink; > - struct rpmsg_endpoint *ept; > - const char *name = chinfo.name; > - int cid; > - int ret; > - > - idr_for_each_entry(&glink->rcids, channel, cid) { > - if (!strcmp(channel->name, name)) > - break; > - } > - > - if (!channel) { > - channel = qcom_glink_create_local(glink, name); > - if (IS_ERR(channel)) > - return NULL; > - } else { > - ret = qcom_glink_create_remote(glink, channel); > - if (ret) > - return NULL; > - } > - > - ept = &channel->ept; > - ept->rpdev = rpdev; > - ept->cb = cb; > - ept->priv = priv; > - ept->ops = &glink_endpoint_ops; > - > - return ept; > -} > - > -static void qcom_glink_destroy_ept(struct rpmsg_endpoint *ept) > -{ > - struct glink_channel *channel = to_glink_channel(ept); > - struct qcom_glink *glink = channel->glink; > - unsigned long flags; > - > - spin_lock_irqsave(&channel->recv_lock, flags); > - channel->ept.cb = NULL; > - spin_unlock_irqrestore(&channel->recv_lock, flags); > - > - /* Decouple the potential rpdev from the channel */ > - channel->rpdev = NULL; > - > - qcom_glink_send_close_req(glink, channel); > -} > - > -static int __qcom_glink_send(struct glink_channel *channel, > - void *data, int len, bool wait) > -{ > - struct qcom_glink *glink = channel->glink; > - struct { > - struct glink_msg msg; > - __le32 chunk_size; > - __le32 left_size; > - } __packed req; > - > - if (WARN(len % 8, "RPM GLINK expects 8 byte aligned messages\n")) > - return -EINVAL; > - > - req.msg.cmd = cpu_to_le16(RPM_CMD_TX_DATA); > - req.msg.param1 = cpu_to_le16(channel->lcid); > - req.msg.param2 = cpu_to_le32(channel->rcid); > - req.chunk_size = cpu_to_le32(len); > - req.left_size = cpu_to_le32(0); > - > - return qcom_glink_tx(glink, &req, sizeof(req), data, len, wait); > -} > - > -static int qcom_glink_send(struct rpmsg_endpoint *ept, void *data, int len) > -{ > - struct glink_channel *channel = to_glink_channel(ept); > - > - return __qcom_glink_send(channel, data, len, true); > -} > - > -static int qcom_glink_trysend(struct rpmsg_endpoint *ept, void *data, int len) > -{ > - struct glink_channel *channel = to_glink_channel(ept); > - > - return __qcom_glink_send(channel, data, len, false); > -} > - > -/* > - * Finds the device_node for the glink child interested in this channel. > - */ > -static struct device_node *qcom_glink_match_channel(struct device_node *node, > - const char *channel) > -{ > - struct device_node *child; > - const char *name; > - const char *key; > - int ret; > - > - for_each_available_child_of_node(node, child) { > - key = "qcom,glink-channels"; > - ret = of_property_read_string(child, key, &name); > - if (ret) > - continue; > - > - if (strcmp(name, channel) == 0) > - return child; > - } > - > - return NULL; > -} > - > -static const struct rpmsg_device_ops glink_device_ops = { > - .create_ept = qcom_glink_create_ept, > -}; > - > -static const struct rpmsg_endpoint_ops glink_endpoint_ops = { > - .destroy_ept = qcom_glink_destroy_ept, > - .send = qcom_glink_send, > - .trysend = qcom_glink_trysend, > -}; > - > -static void qcom_glink_rpdev_release(struct device *dev) > -{ > - struct rpmsg_device *rpdev = to_rpmsg_device(dev); > - struct glink_channel *channel = to_glink_channel(rpdev->ept); > - > - channel->rpdev = NULL; > - kfree(rpdev); > -} > - > -static int qcom_glink_rx_open(struct qcom_glink *glink, unsigned int rcid, > - char *name) > -{ > - struct glink_channel *channel; > - struct rpmsg_device *rpdev; > - bool create_device = false; > - int lcid; > - int ret; > - struct device_node *node; > - > - idr_for_each_entry(&glink->lcids, channel, lcid) { > - if (!strcmp(channel->name, name)) > - break; > - } > - > - if (!channel) { > - channel = qcom_glink_alloc_channel(glink, name); > - if (IS_ERR(channel)) > - return PTR_ERR(channel); > - > - /* The opening dance was initiated by the remote */ > - create_device = true; > - } > - > - mutex_lock(&glink->idr_lock); > - ret = idr_alloc(&glink->rcids, channel, rcid, rcid + 1, GFP_KERNEL); > - if (ret < 0) { > - dev_err(glink->dev, "Unable to insert channel into rcid list\n"); > - mutex_unlock(&glink->idr_lock); > - goto free_channel; > - } > - channel->rcid = ret; > - mutex_unlock(&glink->idr_lock); > - > - complete(&channel->open_req); > - > - if (create_device) { > - rpdev = kzalloc(sizeof(*rpdev), GFP_KERNEL); > - if (!rpdev) { > - ret = -ENOMEM; > - goto rcid_remove; > - } > - > - rpdev->ept = &channel->ept; > - strncpy(rpdev->id.name, name, RPMSG_NAME_SIZE); > - rpdev->src = RPMSG_ADDR_ANY; > - rpdev->dst = RPMSG_ADDR_ANY; > - rpdev->ops = &glink_device_ops; > - > - node = qcom_glink_match_channel(glink->dev->of_node, name); > - rpdev->dev.of_node = node; > - rpdev->dev.parent = glink->dev; > - rpdev->dev.release = qcom_glink_rpdev_release; > - > - ret = rpmsg_register_device(rpdev); > - if (ret) > - goto free_rpdev; > - > - channel->rpdev = rpdev; > - } > - > - return 0; > - > -free_rpdev: > - kfree(rpdev); > -rcid_remove: > - mutex_lock(&glink->idr_lock); > - idr_remove(&glink->rcids, channel->rcid); > - channel->rcid = 0; > - mutex_unlock(&glink->idr_lock); > -free_channel: > - /* Release the reference, iff we took it */ > - if (create_device) > - kref_put(&channel->refcount, qcom_glink_channel_release); > - > - return ret; > -} > - > -static void qcom_glink_rx_close(struct qcom_glink *glink, unsigned int rcid) > -{ > - struct rpmsg_channel_info chinfo; > - struct glink_channel *channel; > - > - channel = idr_find(&glink->rcids, rcid); > - if (WARN(!channel, "close request on unknown channel\n")) > - return; > - > - if (channel->rpdev) { > - strncpy(chinfo.name, channel->name, sizeof(chinfo.name)); > - chinfo.src = RPMSG_ADDR_ANY; > - chinfo.dst = RPMSG_ADDR_ANY; > - > - rpmsg_unregister_device(glink->dev, &chinfo); > - } > - > - qcom_glink_send_close_ack(glink, channel->rcid); > - > - mutex_lock(&glink->idr_lock); > - idr_remove(&glink->rcids, channel->rcid); > - channel->rcid = 0; > - mutex_unlock(&glink->idr_lock); > - > - kref_put(&channel->refcount, qcom_glink_channel_release); > -} > - > -static void qcom_glink_rx_close_ack(struct qcom_glink *glink, unsigned int lcid) > -{ > - struct glink_channel *channel; > - > - channel = idr_find(&glink->lcids, lcid); > - if (WARN(!channel, "close ack on unknown channel\n")) > - return; > - > - mutex_lock(&glink->idr_lock); > - idr_remove(&glink->lcids, channel->lcid); > - channel->lcid = 0; > - mutex_unlock(&glink->idr_lock); > - > - kref_put(&channel->refcount, qcom_glink_channel_release); > -} > - > -static void qcom_glink_work(struct work_struct *work) > -{ > - struct qcom_glink *glink = container_of(work, struct qcom_glink, > - rx_work); > - struct glink_defer_cmd *dcmd; > - struct glink_msg *msg; > - unsigned long flags; > - unsigned int param1; > - unsigned int param2; > - unsigned int cmd; > - > - for (;;) { > - spin_lock_irqsave(&glink->rx_lock, flags); > - if (list_empty(&glink->rx_queue)) { > - spin_unlock_irqrestore(&glink->rx_lock, flags); > - break; > - } > - dcmd = list_first_entry(&glink->rx_queue, struct glink_defer_cmd, node); > - list_del(&dcmd->node); > - spin_unlock_irqrestore(&glink->rx_lock, flags); > - > - msg = &dcmd->msg; > - cmd = le16_to_cpu(msg->cmd); > - param1 = le16_to_cpu(msg->param1); > - param2 = le32_to_cpu(msg->param2); > - > - switch (cmd) { > - case RPM_CMD_VERSION: > - qcom_glink_send_version_ack(glink); > - break; > - case RPM_CMD_VERSION_ACK: > - break; > - case RPM_CMD_OPEN: > - qcom_glink_rx_open(glink, param1, msg->data); > - break; > - case RPM_CMD_CLOSE: > - qcom_glink_rx_close(glink, param1); > - break; > - case RPM_CMD_CLOSE_ACK: > - qcom_glink_rx_close_ack(glink, param1); > - break; > - default: > - WARN(1, "Unknown defer object %d\n", cmd); > - break; > - } > - > - kfree(dcmd); > - } > -} > - > static int glink_rpm_parse_toc(struct device *dev, > void __iomem *msg_ram, > size_t msg_ram_size, > @@ -1156,56 +239,6 @@ static int glink_rpm_parse_toc(struct device *dev, > return -EINVAL; > } > > -struct qcom_glink *qcom_glink_native_probe(struct device *dev, > - struct qcom_glink_pipe *rx, > - struct qcom_glink_pipe *tx) > -{ > - int irq; > - int ret; > - struct qcom_glink *glink; > - > - glink = devm_kzalloc(dev, sizeof(*glink), GFP_KERNEL); > - if (!glink) > - return ERR_PTR(-ENOMEM); > - > - glink->dev = dev; > - glink->tx_pipe = tx; > - glink->rx_pipe = rx; > - > - mutex_init(&glink->tx_lock); > - spin_lock_init(&glink->rx_lock); > - INIT_LIST_HEAD(&glink->rx_queue); > - INIT_WORK(&glink->rx_work, qcom_glink_work); > - > - mutex_init(&glink->idr_lock); > - idr_init(&glink->lcids); > - idr_init(&glink->rcids); > - > - glink->mbox_client.dev = dev; > - glink->mbox_chan = mbox_request_channel(&glink->mbox_client, 0); > - if (IS_ERR(glink->mbox_chan)) { > - if (PTR_ERR(glink->mbox_chan) != -EPROBE_DEFER) > - dev_err(dev, "failed to acquire IPC channel\n"); > - return ERR_CAST(glink->mbox_chan); > - } > - > - irq = of_irq_get(dev->of_node, 0); > - ret = devm_request_irq(dev, irq, > - qcom_glink_intr, > - IRQF_NO_SUSPEND | IRQF_SHARED, > - "glink-native", glink); > - if (ret) { > - dev_err(dev, "failed to request IRQ\n"); > - return ERR_PTR(ret); > - } > - > - ret = qcom_glink_send_version(glink); > - if (ret) > - return ERR_PTR(ret); > - > - return glink; > -} > - > static int glink_rpm_probe(struct platform_device *pdev) > { > struct qcom_glink *glink; > @@ -1259,33 +292,11 @@ static int glink_rpm_probe(struct platform_device *pdev) > return 0; > } > > -static int glink_rpm_remove_device(struct device *dev, void *data) > -{ > - device_unregister(dev); > - > - return 0; > -} > - > static int glink_rpm_remove(struct platform_device *pdev) > { > struct qcom_glink *glink = platform_get_drvdata(pdev); > - struct glink_channel *channel; > - int cid; > - int ret; > - > - disable_irq(glink->irq); > - cancel_work_sync(&glink->rx_work); > - > - ret = device_for_each_child(glink->dev, NULL, glink_rpm_remove_device); > - if (ret) > - dev_warn(glink->dev, "Can't remove GLINK devices: %d\n", ret); > - > - /* Release any defunct local channels, waiting for close-ack */ > - idr_for_each_entry(&glink->lcids, channel, cid) > - kref_put(&channel->refcount, qcom_glink_channel_release); > > - idr_destroy(&glink->lcids); > - idr_destroy(&glink->rcids); > + qcom_glink_native_remove(glink); > > return 0; > } From mboxrd@z Thu Jan 1 00:00:00 1970 From: aneela@codeaurora.org (Arun Kumar Neelakantam) Date: Mon, 28 Aug 2017 17:15:54 +0530 Subject: [PATCH v2 04/20] rpmsg: glink: Move the common glink protocol implementation to glink_native.c In-Reply-To: <1503559302-3744-5-git-send-email-sricharan@codeaurora.org> References: <1503559302-3744-1-git-send-email-sricharan@codeaurora.org> <1503559302-3744-5-git-send-email-sricharan@codeaurora.org> Message-ID: To: linux-arm-kernel@lists.infradead.org List-Id: linux-arm-kernel.lists.infradead.org On 8/24/2017 12:51 PM, Sricharan R wrote: > From: Bjorn Andersson > > Move the common part of glink core protocol implementation to > glink_native.c that can be shared with the smem based glink > transport in the later patches. > > Signed-off-by: Bjorn Andersson > Signed-off-by: Sricharan R Acked-by: Arun Kumar Neelakantam Regards, Arun N > --- > drivers/rpmsg/Kconfig | 6 +- > drivers/rpmsg/Makefile | 1 + > drivers/rpmsg/qcom_glink_native.c | 1017 +++++++++++++++++++++++++++++++++++++ > drivers/rpmsg/qcom_glink_native.h | 38 ++ > drivers/rpmsg/qcom_glink_rpm.c | 995 +----------------------------------- > 5 files changed, 1064 insertions(+), 993 deletions(-) > create mode 100644 drivers/rpmsg/qcom_glink_native.c > create mode 100644 drivers/rpmsg/qcom_glink_native.h > > diff --git a/drivers/rpmsg/Kconfig b/drivers/rpmsg/Kconfig > index 2a5d2b4..ac33688 100644 > --- a/drivers/rpmsg/Kconfig > +++ b/drivers/rpmsg/Kconfig > @@ -13,9 +13,13 @@ config RPMSG_CHAR > in /dev. They make it possible for user-space programs to send and > receive rpmsg packets. > > +config RPMSG_QCOM_GLINK_NATIVE > + tristate > + select RPMSG > + > config RPMSG_QCOM_GLINK_RPM > tristate "Qualcomm RPM Glink driver" > - select RPMSG > + select RPMSG_QCOM_GLINK_NATIVE > depends on HAS_IOMEM > depends on MAILBOX > help > diff --git a/drivers/rpmsg/Makefile b/drivers/rpmsg/Makefile > index 28cc190..09a756c 100644 > --- a/drivers/rpmsg/Makefile > +++ b/drivers/rpmsg/Makefile > @@ -1,5 +1,6 @@ > obj-$(CONFIG_RPMSG) += rpmsg_core.o > obj-$(CONFIG_RPMSG_CHAR) += rpmsg_char.o > obj-$(CONFIG_RPMSG_QCOM_GLINK_RPM) += qcom_glink_rpm.o > +obj-$(CONFIG_RPMSG_QCOM_GLINK_NATIVE) += qcom_glink_native.o > obj-$(CONFIG_RPMSG_QCOM_SMD) += qcom_smd.o > obj-$(CONFIG_RPMSG_VIRTIO) += virtio_rpmsg_bus.o > diff --git a/drivers/rpmsg/qcom_glink_native.c b/drivers/rpmsg/qcom_glink_native.c > new file mode 100644 > index 0000000..ffdf88e > --- /dev/null > +++ b/drivers/rpmsg/qcom_glink_native.c > @@ -0,0 +1,1017 @@ > +/* > + * Copyright (c) 2016-2017, Linaro Ltd > + * > + * This program is free software; you can redistribute it and/or modify > + * it under the terms of the GNU General Public License version 2 and > + * only version 2 as published by the Free Software Foundation. > + * > + * This program is distributed in the hope that it will be useful, > + * but WITHOUT ANY WARRANTY; without even the implied warranty of > + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the > + * GNU General Public License for more details. > + */ > + > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > + > +#include "rpmsg_internal.h" > +#include "qcom_glink_native.h" > + > +#define GLINK_NAME_SIZE 32 > + > +#define RPM_GLINK_CID_MIN 1 > +#define RPM_GLINK_CID_MAX 65536 > + > +struct glink_msg { > + __le16 cmd; > + __le16 param1; > + __le32 param2; > + u8 data[]; > +} __packed; > + > +/** > + * struct glink_defer_cmd - deferred incoming control message > + * @node: list node > + * @msg: message header > + * data: payload of the message > + * > + * Copy of a received control message, to be added to @rx_queue and processed > + * by @rx_work of @qcom_glink. > + */ > +struct glink_defer_cmd { > + struct list_head node; > + > + struct glink_msg msg; > + u8 data[]; > +}; > + > +/** > + * struct qcom_glink - driver context, relates to one remote subsystem > + * @dev: reference to the associated struct device > + * @mbox_client: mailbox client > + * @mbox_chan: mailbox channel > + * @rx_pipe: pipe object for receive FIFO > + * @tx_pipe: pipe object for transmit FIFO > + * @irq: IRQ for signaling incoming events > + * @rx_work: worker for handling received control messages > + * @rx_lock: protects the @rx_queue > + * @rx_queue: queue of received control messages to be processed in @rx_work > + * @tx_lock: synchronizes operations on the tx fifo > + * @idr_lock: synchronizes @lcids and @rcids modifications > + * @lcids: idr of all channels with a known local channel id > + * @rcids: idr of all channels with a known remote channel id > + */ > +struct qcom_glink { > + struct device *dev; > + > + struct mbox_client mbox_client; > + struct mbox_chan *mbox_chan; > + > + struct qcom_glink_pipe *rx_pipe; > + struct qcom_glink_pipe *tx_pipe; > + > + int irq; > + > + struct work_struct rx_work; > + spinlock_t rx_lock; > + struct list_head rx_queue; > + > + struct mutex tx_lock; > + > + struct mutex idr_lock; > + struct idr lcids; > + struct idr rcids; > +}; > + > +enum { > + GLINK_STATE_CLOSED, > + GLINK_STATE_OPENING, > + GLINK_STATE_OPEN, > + GLINK_STATE_CLOSING, > +}; > + > +/** > + * struct glink_channel - internal representation of a channel > + * @rpdev: rpdev reference, only used for primary endpoints > + * @ept: rpmsg endpoint this channel is associated with > + * @glink: qcom_glink context handle > + * @refcount: refcount for the channel object > + * @recv_lock: guard for @ept.cb > + * @name: unique channel name/identifier > + * @lcid: channel id, in local space > + * @rcid: channel id, in remote space > + * @buf: receive buffer, for gathering fragments > + * @buf_offset: write offset in @buf > + * @buf_size: size of current @buf > + * @open_ack: completed once remote has acked the open-request > + * @open_req: completed once open-request has been received > + */ > +struct glink_channel { > + struct rpmsg_endpoint ept; > + > + struct rpmsg_device *rpdev; > + struct qcom_glink *glink; > + > + struct kref refcount; > + > + spinlock_t recv_lock; > + > + char *name; > + unsigned int lcid; > + unsigned int rcid; > + > + void *buf; > + int buf_offset; > + int buf_size; > + > + struct completion open_ack; > + struct completion open_req; > +}; > + > +#define to_glink_channel(_ept) container_of(_ept, struct glink_channel, ept) > + > +static const struct rpmsg_endpoint_ops glink_endpoint_ops; > + > +#define RPM_CMD_VERSION 0 > +#define RPM_CMD_VERSION_ACK 1 > +#define RPM_CMD_OPEN 2 > +#define RPM_CMD_CLOSE 3 > +#define RPM_CMD_OPEN_ACK 4 > +#define RPM_CMD_TX_DATA 9 > +#define RPM_CMD_CLOSE_ACK 11 > +#define RPM_CMD_TX_DATA_CONT 12 > +#define RPM_CMD_READ_NOTIF 13 > + > +#define GLINK_FEATURE_INTENTLESS BIT(1) > + > +static struct glink_channel *qcom_glink_alloc_channel(struct qcom_glink *glink, > + const char *name) > +{ > + struct glink_channel *channel; > + > + channel = kzalloc(sizeof(*channel), GFP_KERNEL); > + if (!channel) > + return ERR_PTR(-ENOMEM); > + > + /* Setup glink internal glink_channel data */ > + spin_lock_init(&channel->recv_lock); > + channel->glink = glink; > + channel->name = kstrdup(name, GFP_KERNEL); > + > + init_completion(&channel->open_req); > + init_completion(&channel->open_ack); > + > + kref_init(&channel->refcount); > + > + return channel; > +} > + > +static void qcom_glink_channel_release(struct kref *ref) > +{ > + struct glink_channel *channel = container_of(ref, struct glink_channel, > + refcount); > + > + kfree(channel->name); > + kfree(channel); > +} > + > +static size_t qcom_glink_rx_avail(struct qcom_glink *glink) > +{ > + return glink->rx_pipe->avail(glink->rx_pipe); > +} > + > +static void qcom_glink_rx_peak(struct qcom_glink *glink, > + void *data, size_t count) > +{ > + glink->rx_pipe->peak(glink->rx_pipe, data, count); > +} > + > +static void qcom_glink_rx_advance(struct qcom_glink *glink, size_t count) > +{ > + glink->rx_pipe->advance(glink->rx_pipe, count); > +} > + > +static size_t qcom_glink_tx_avail(struct qcom_glink *glink) > +{ > + return glink->tx_pipe->avail(glink->tx_pipe); > +} > + > +static void qcom_glink_tx_write(struct qcom_glink *glink, > + const void *hdr, size_t hlen, > + const void *data, size_t dlen) > +{ > + glink->tx_pipe->write(glink->tx_pipe, hdr, hlen, data, dlen); > +} > + > +static int qcom_glink_tx(struct qcom_glink *glink, > + const void *hdr, size_t hlen, > + const void *data, size_t dlen, bool wait) > +{ > + unsigned int tlen = hlen + dlen; > + int ret; > + > + /* Reject packets that are too big */ > + if (tlen >= glink->tx_pipe->length) > + return -EINVAL; > + > + if (WARN(tlen % 8, "Unaligned TX request")) > + return -EINVAL; > + > + ret = mutex_lock_interruptible(&glink->tx_lock); > + if (ret) > + return ret; > + > + while (qcom_glink_tx_avail(glink) < tlen) { > + if (!wait) { > + ret = -ENOMEM; > + goto out; > + } > + > + usleep_range(10000, 15000); > + } > + > + qcom_glink_tx_write(glink, hdr, hlen, data, dlen); > + > + mbox_send_message(glink->mbox_chan, NULL); > + mbox_client_txdone(glink->mbox_chan, 0); > + > +out: > + mutex_unlock(&glink->tx_lock); > + > + return ret; > +} > + > +static int qcom_glink_send_version(struct qcom_glink *glink) > +{ > + struct glink_msg msg; > + > + msg.cmd = cpu_to_le16(RPM_CMD_VERSION); > + msg.param1 = cpu_to_le16(1); > + msg.param2 = cpu_to_le32(GLINK_FEATURE_INTENTLESS); > + > + return qcom_glink_tx(glink, &msg, sizeof(msg), NULL, 0, true); > +} > + > +static void qcom_glink_send_version_ack(struct qcom_glink *glink) > +{ > + struct glink_msg msg; > + > + msg.cmd = cpu_to_le16(RPM_CMD_VERSION_ACK); > + msg.param1 = cpu_to_le16(1); > + msg.param2 = cpu_to_le32(0); > + > + qcom_glink_tx(glink, &msg, sizeof(msg), NULL, 0, true); > +} > + > +static void qcom_glink_send_open_ack(struct qcom_glink *glink, > + struct glink_channel *channel) > +{ > + struct glink_msg msg; > + > + msg.cmd = cpu_to_le16(RPM_CMD_OPEN_ACK); > + msg.param1 = cpu_to_le16(channel->rcid); > + msg.param2 = cpu_to_le32(0); > + > + qcom_glink_tx(glink, &msg, sizeof(msg), NULL, 0, true); > +} > + > +/** > + * qcom_glink_send_open_req() - send a RPM_CMD_OPEN request to the remote > + * @glink: Ptr to the glink edge > + * @channel: Ptr to the channel that the open req is sent > + * > + * Allocates a local channel id and sends a RPM_CMD_OPEN message to the remote. > + * Will return with refcount held, regardless of outcome. > + * > + * Returns 0 on success, negative errno otherwise. > + */ > +static int qcom_glink_send_open_req(struct qcom_glink *glink, > + struct glink_channel *channel) > +{ > + struct { > + struct glink_msg msg; > + u8 name[GLINK_NAME_SIZE]; > + } __packed req; > + int name_len = strlen(channel->name) + 1; > + int req_len = ALIGN(sizeof(req.msg) + name_len, 8); > + int ret; > + > + kref_get(&channel->refcount); > + > + mutex_lock(&glink->idr_lock); > + ret = idr_alloc_cyclic(&glink->lcids, channel, > + RPM_GLINK_CID_MIN, RPM_GLINK_CID_MAX, > + GFP_KERNEL); > + mutex_unlock(&glink->idr_lock); > + if (ret < 0) > + return ret; > + > + channel->lcid = ret; > + > + req.msg.cmd = cpu_to_le16(RPM_CMD_OPEN); > + req.msg.param1 = cpu_to_le16(channel->lcid); > + req.msg.param2 = cpu_to_le32(name_len); > + strcpy(req.name, channel->name); > + > + ret = qcom_glink_tx(glink, &req, req_len, NULL, 0, true); > + if (ret) > + goto remove_idr; > + > + return 0; > + > +remove_idr: > + mutex_lock(&glink->idr_lock); > + idr_remove(&glink->lcids, channel->lcid); > + channel->lcid = 0; > + mutex_unlock(&glink->idr_lock); > + > + return ret; > +} > + > +static void qcom_glink_send_close_req(struct qcom_glink *glink, > + struct glink_channel *channel) > +{ > + struct glink_msg req; > + > + req.cmd = cpu_to_le16(RPM_CMD_CLOSE); > + req.param1 = cpu_to_le16(channel->lcid); > + req.param2 = 0; > + > + qcom_glink_tx(glink, &req, sizeof(req), NULL, 0, true); > +} > + > +static void qcom_glink_send_close_ack(struct qcom_glink *glink, > + unsigned int rcid) > +{ > + struct glink_msg req; > + > + req.cmd = cpu_to_le16(RPM_CMD_CLOSE_ACK); > + req.param1 = cpu_to_le16(rcid); > + req.param2 = 0; > + > + qcom_glink_tx(glink, &req, sizeof(req), NULL, 0, true); > +} > + > +static int qcom_glink_rx_defer(struct qcom_glink *glink, size_t extra) > +{ > + struct glink_defer_cmd *dcmd; > + > + extra = ALIGN(extra, 8); > + > + if (qcom_glink_rx_avail(glink) < sizeof(struct glink_msg) + extra) { > + dev_dbg(glink->dev, "Insufficient data in rx fifo"); > + return -ENXIO; > + } > + > + dcmd = kzalloc(sizeof(*dcmd) + extra, GFP_ATOMIC); > + if (!dcmd) > + return -ENOMEM; > + > + INIT_LIST_HEAD(&dcmd->node); > + > + qcom_glink_rx_peak(glink, &dcmd->msg, sizeof(dcmd->msg) + extra); > + > + spin_lock(&glink->rx_lock); > + list_add_tail(&dcmd->node, &glink->rx_queue); > + spin_unlock(&glink->rx_lock); > + > + schedule_work(&glink->rx_work); > + qcom_glink_rx_advance(glink, sizeof(dcmd->msg) + extra); > + > + return 0; > +} > + > +static int qcom_glink_rx_data(struct qcom_glink *glink, size_t avail) > +{ > + struct glink_channel *channel; > + struct { > + struct glink_msg msg; > + __le32 chunk_size; > + __le32 left_size; > + } __packed hdr; > + unsigned int chunk_size; > + unsigned int left_size; > + unsigned int rcid; > + > + if (avail < sizeof(hdr)) { > + dev_dbg(glink->dev, "Not enough data in fifo\n"); > + return -EAGAIN; > + } > + > + qcom_glink_rx_peak(glink, &hdr, sizeof(hdr)); > + chunk_size = le32_to_cpu(hdr.chunk_size); > + left_size = le32_to_cpu(hdr.left_size); > + > + if (avail < sizeof(hdr) + chunk_size) { > + dev_dbg(glink->dev, "Payload not yet in fifo\n"); > + return -EAGAIN; > + } > + > + if (WARN(chunk_size % 4, "Incoming data must be word aligned\n")) > + return -EINVAL; > + > + rcid = le16_to_cpu(hdr.msg.param1); > + channel = idr_find(&glink->rcids, rcid); > + if (!channel) { > + dev_dbg(glink->dev, "Data on non-existing channel\n"); > + > + /* Drop the message */ > + qcom_glink_rx_advance(glink, > + ALIGN(sizeof(hdr) + chunk_size, 8)); > + return 0; > + } > + > + /* Might have an ongoing, fragmented, message to append */ > + if (!channel->buf) { > + channel->buf = kmalloc(chunk_size + left_size, GFP_ATOMIC); > + if (!channel->buf) > + return -ENOMEM; > + > + channel->buf_size = chunk_size + left_size; > + channel->buf_offset = 0; > + } > + > + qcom_glink_rx_advance(glink, sizeof(hdr)); > + > + if (channel->buf_size - channel->buf_offset < chunk_size) { > + dev_err(glink->dev, "Insufficient space in input buffer\n"); > + > + /* The packet header lied, drop payload */ > + qcom_glink_rx_advance(glink, chunk_size); > + return -ENOMEM; > + } > + > + qcom_glink_rx_peak(glink, channel->buf + channel->buf_offset, > + chunk_size); > + channel->buf_offset += chunk_size; > + > + /* Handle message when no fragments remain to be received */ > + if (!left_size) { > + spin_lock(&channel->recv_lock); > + if (channel->ept.cb) { > + channel->ept.cb(channel->ept.rpdev, > + channel->buf, > + channel->buf_offset, > + channel->ept.priv, > + RPMSG_ADDR_ANY); > + } > + spin_unlock(&channel->recv_lock); > + > + kfree(channel->buf); > + channel->buf = NULL; > + channel->buf_size = 0; > + } > + > + /* Each message starts at 8 byte aligned address */ > + qcom_glink_rx_advance(glink, ALIGN(chunk_size, 8)); > + > + return 0; > +} > + > +static int qcom_glink_rx_open_ack(struct qcom_glink *glink, unsigned int lcid) > +{ > + struct glink_channel *channel; > + > + channel = idr_find(&glink->lcids, lcid); > + if (!channel) { > + dev_err(glink->dev, "Invalid open ack packet\n"); > + return -EINVAL; > + } > + > + complete(&channel->open_ack); > + > + return 0; > +} > + > +static irqreturn_t qcom_glink_native_intr(int irq, void *data) > +{ > + struct qcom_glink *glink = data; > + struct glink_msg msg; > + unsigned int param1; > + unsigned int param2; > + unsigned int avail; > + unsigned int cmd; > + int ret; > + > + for (;;) { > + avail = qcom_glink_rx_avail(glink); > + if (avail < sizeof(msg)) > + break; > + > + qcom_glink_rx_peak(glink, &msg, sizeof(msg)); > + > + cmd = le16_to_cpu(msg.cmd); > + param1 = le16_to_cpu(msg.param1); > + param2 = le32_to_cpu(msg.param2); > + > + switch (cmd) { > + case RPM_CMD_VERSION: > + case RPM_CMD_VERSION_ACK: > + case RPM_CMD_CLOSE: > + case RPM_CMD_CLOSE_ACK: > + ret = qcom_glink_rx_defer(glink, 0); > + break; > + case RPM_CMD_OPEN_ACK: > + ret = qcom_glink_rx_open_ack(glink, param1); > + qcom_glink_rx_advance(glink, ALIGN(sizeof(msg), 8)); > + break; > + case RPM_CMD_OPEN: > + ret = qcom_glink_rx_defer(glink, param2); > + break; > + case RPM_CMD_TX_DATA: > + case RPM_CMD_TX_DATA_CONT: > + ret = qcom_glink_rx_data(glink, avail); > + break; > + case RPM_CMD_READ_NOTIF: > + qcom_glink_rx_advance(glink, ALIGN(sizeof(msg), 8)); > + > + mbox_send_message(glink->mbox_chan, NULL); > + mbox_client_txdone(glink->mbox_chan, 0); > + > + ret = 0; > + break; > + default: > + dev_err(glink->dev, "unhandled rx cmd: %d\n", cmd); > + ret = -EINVAL; > + break; > + } > + > + if (ret) > + break; > + } > + > + return IRQ_HANDLED; > +} > + > +/* Locally initiated rpmsg_create_ept */ > +static struct glink_channel *qcom_glink_create_local(struct qcom_glink *glink, > + const char *name) > +{ > + struct glink_channel *channel; > + int ret; > + > + channel = qcom_glink_alloc_channel(glink, name); > + if (IS_ERR(channel)) > + return ERR_CAST(channel); > + > + ret = qcom_glink_send_open_req(glink, channel); > + if (ret) > + goto release_channel; > + > + ret = wait_for_completion_timeout(&channel->open_ack, 5 * HZ); > + if (!ret) > + goto err_timeout; > + > + ret = wait_for_completion_timeout(&channel->open_req, 5 * HZ); > + if (!ret) > + goto err_timeout; > + > + qcom_glink_send_open_ack(glink, channel); > + > + return channel; > + > +err_timeout: > + /* qcom_glink_send_open_req() did register the channel in lcids*/ > + mutex_lock(&glink->idr_lock); > + idr_remove(&glink->lcids, channel->lcid); > + mutex_unlock(&glink->idr_lock); > + > +release_channel: > + /* Release qcom_glink_send_open_req() reference */ > + kref_put(&channel->refcount, qcom_glink_channel_release); > + /* Release qcom_glink_alloc_channel() reference */ > + kref_put(&channel->refcount, qcom_glink_channel_release); > + > + return ERR_PTR(-ETIMEDOUT); > +} > + > +/* Remote initiated rpmsg_create_ept */ > +static int qcom_glink_create_remote(struct qcom_glink *glink, > + struct glink_channel *channel) > +{ > + int ret; > + > + qcom_glink_send_open_ack(glink, channel); > + > + ret = qcom_glink_send_open_req(glink, channel); > + if (ret) > + goto close_link; > + > + ret = wait_for_completion_timeout(&channel->open_ack, 5 * HZ); > + if (!ret) { > + ret = -ETIMEDOUT; > + goto close_link; > + } > + > + return 0; > + > +close_link: > + /* > + * Send a close request to "undo" our open-ack. The close-ack will > + * release the last reference. > + */ > + qcom_glink_send_close_req(glink, channel); > + > + /* Release qcom_glink_send_open_req() reference */ > + kref_put(&channel->refcount, qcom_glink_channel_release); > + > + return ret; > +} > + > +static struct rpmsg_endpoint *qcom_glink_create_ept(struct rpmsg_device *rpdev, > + rpmsg_rx_cb_t cb, > + void *priv, > + struct rpmsg_channel_info > + chinfo) > +{ > + struct glink_channel *parent = to_glink_channel(rpdev->ept); > + struct glink_channel *channel; > + struct qcom_glink *glink = parent->glink; > + struct rpmsg_endpoint *ept; > + const char *name = chinfo.name; > + int cid; > + int ret; > + > + idr_for_each_entry(&glink->rcids, channel, cid) { > + if (!strcmp(channel->name, name)) > + break; > + } > + > + if (!channel) { > + channel = qcom_glink_create_local(glink, name); > + if (IS_ERR(channel)) > + return NULL; > + } else { > + ret = qcom_glink_create_remote(glink, channel); > + if (ret) > + return NULL; > + } > + > + ept = &channel->ept; > + ept->rpdev = rpdev; > + ept->cb = cb; > + ept->priv = priv; > + ept->ops = &glink_endpoint_ops; > + > + return ept; > +} > + > +static void qcom_glink_destroy_ept(struct rpmsg_endpoint *ept) > +{ > + struct glink_channel *channel = to_glink_channel(ept); > + struct qcom_glink *glink = channel->glink; > + unsigned long flags; > + > + spin_lock_irqsave(&channel->recv_lock, flags); > + channel->ept.cb = NULL; > + spin_unlock_irqrestore(&channel->recv_lock, flags); > + > + /* Decouple the potential rpdev from the channel */ > + channel->rpdev = NULL; > + > + qcom_glink_send_close_req(glink, channel); > +} > + > +static int __qcom_glink_send(struct glink_channel *channel, > + void *data, int len, bool wait) > +{ > + struct qcom_glink *glink = channel->glink; > + struct { > + struct glink_msg msg; > + __le32 chunk_size; > + __le32 left_size; > + } __packed req; > + > + if (WARN(len % 8, "RPM GLINK expects 8 byte aligned messages\n")) > + return -EINVAL; > + > + req.msg.cmd = cpu_to_le16(RPM_CMD_TX_DATA); > + req.msg.param1 = cpu_to_le16(channel->lcid); > + req.msg.param2 = cpu_to_le32(channel->rcid); > + req.chunk_size = cpu_to_le32(len); > + req.left_size = cpu_to_le32(0); > + > + return qcom_glink_tx(glink, &req, sizeof(req), data, len, wait); > +} > + > +static int qcom_glink_send(struct rpmsg_endpoint *ept, void *data, int len) > +{ > + struct glink_channel *channel = to_glink_channel(ept); > + > + return __qcom_glink_send(channel, data, len, true); > +} > + > +static int qcom_glink_trysend(struct rpmsg_endpoint *ept, void *data, int len) > +{ > + struct glink_channel *channel = to_glink_channel(ept); > + > + return __qcom_glink_send(channel, data, len, false); > +} > + > +/* > + * Finds the device_node for the glink child interested in this channel. > + */ > +static struct device_node *qcom_glink_match_channel(struct device_node *node, > + const char *channel) > +{ > + struct device_node *child; > + const char *name; > + const char *key; > + int ret; > + > + for_each_available_child_of_node(node, child) { > + key = "qcom,glink-channels"; > + ret = of_property_read_string(child, key, &name); > + if (ret) > + continue; > + > + if (strcmp(name, channel) == 0) > + return child; > + } > + > + return NULL; > +} > + > +static const struct rpmsg_device_ops glink_device_ops = { > + .create_ept = qcom_glink_create_ept, > +}; > + > +static const struct rpmsg_endpoint_ops glink_endpoint_ops = { > + .destroy_ept = qcom_glink_destroy_ept, > + .send = qcom_glink_send, > + .trysend = qcom_glink_trysend, > +}; > + > +static void qcom_glink_rpdev_release(struct device *dev) > +{ > + struct rpmsg_device *rpdev = to_rpmsg_device(dev); > + struct glink_channel *channel = to_glink_channel(rpdev->ept); > + > + channel->rpdev = NULL; > + kfree(rpdev); > +} > + > +static int qcom_glink_rx_open(struct qcom_glink *glink, unsigned int rcid, > + char *name) > +{ > + struct glink_channel *channel; > + struct rpmsg_device *rpdev; > + bool create_device = false; > + struct device_node *node; > + int lcid; > + int ret; > + > + idr_for_each_entry(&glink->lcids, channel, lcid) { > + if (!strcmp(channel->name, name)) > + break; > + } > + > + if (!channel) { > + channel = qcom_glink_alloc_channel(glink, name); > + if (IS_ERR(channel)) > + return PTR_ERR(channel); > + > + /* The opening dance was initiated by the remote */ > + create_device = true; > + } > + > + mutex_lock(&glink->idr_lock); > + ret = idr_alloc(&glink->rcids, channel, rcid, rcid + 1, GFP_KERNEL); > + if (ret < 0) { > + dev_err(glink->dev, "Unable to insert channel into rcid list\n"); > + mutex_unlock(&glink->idr_lock); > + goto free_channel; > + } > + channel->rcid = ret; > + mutex_unlock(&glink->idr_lock); > + > + complete(&channel->open_req); > + > + if (create_device) { > + rpdev = kzalloc(sizeof(*rpdev), GFP_KERNEL); > + if (!rpdev) { > + ret = -ENOMEM; > + goto rcid_remove; > + } > + > + rpdev->ept = &channel->ept; > + strncpy(rpdev->id.name, name, RPMSG_NAME_SIZE); > + rpdev->src = RPMSG_ADDR_ANY; > + rpdev->dst = RPMSG_ADDR_ANY; > + rpdev->ops = &glink_device_ops; > + > + node = qcom_glink_match_channel(glink->dev->of_node, name); > + rpdev->dev.of_node = node; > + rpdev->dev.parent = glink->dev; > + rpdev->dev.release = qcom_glink_rpdev_release; > + > + ret = rpmsg_register_device(rpdev); > + if (ret) > + goto free_rpdev; > + > + channel->rpdev = rpdev; > + } > + > + return 0; > + > +free_rpdev: > + kfree(rpdev); > +rcid_remove: > + mutex_lock(&glink->idr_lock); > + idr_remove(&glink->rcids, channel->rcid); > + channel->rcid = 0; > + mutex_unlock(&glink->idr_lock); > +free_channel: > + /* Release the reference, iff we took it */ > + if (create_device) > + kref_put(&channel->refcount, qcom_glink_channel_release); > + > + return ret; > +} > + > +static void qcom_glink_rx_close(struct qcom_glink *glink, unsigned int rcid) > +{ > + struct rpmsg_channel_info chinfo; > + struct glink_channel *channel; > + > + channel = idr_find(&glink->rcids, rcid); > + if (WARN(!channel, "close request on unknown channel\n")) > + return; > + > + if (channel->rpdev) { > + strncpy(chinfo.name, channel->name, sizeof(chinfo.name)); > + chinfo.src = RPMSG_ADDR_ANY; > + chinfo.dst = RPMSG_ADDR_ANY; > + > + rpmsg_unregister_device(glink->dev, &chinfo); > + } > + > + qcom_glink_send_close_ack(glink, channel->rcid); > + > + mutex_lock(&glink->idr_lock); > + idr_remove(&glink->rcids, channel->rcid); > + channel->rcid = 0; > + mutex_unlock(&glink->idr_lock); > + > + kref_put(&channel->refcount, qcom_glink_channel_release); > +} > + > +static void qcom_glink_rx_close_ack(struct qcom_glink *glink, unsigned int lcid) > +{ > + struct glink_channel *channel; > + > + channel = idr_find(&glink->lcids, lcid); > + if (WARN(!channel, "close ack on unknown channel\n")) > + return; > + > + mutex_lock(&glink->idr_lock); > + idr_remove(&glink->lcids, channel->lcid); > + channel->lcid = 0; > + mutex_unlock(&glink->idr_lock); > + > + kref_put(&channel->refcount, qcom_glink_channel_release); > +} > + > +static void qcom_glink_work(struct work_struct *work) > +{ > + struct qcom_glink *glink = container_of(work, struct qcom_glink, > + rx_work); > + struct glink_defer_cmd *dcmd; > + struct glink_msg *msg; > + unsigned long flags; > + unsigned int param1; > + unsigned int param2; > + unsigned int cmd; > + > + for (;;) { > + spin_lock_irqsave(&glink->rx_lock, flags); > + if (list_empty(&glink->rx_queue)) { > + spin_unlock_irqrestore(&glink->rx_lock, flags); > + break; > + } > + dcmd = list_first_entry(&glink->rx_queue, > + struct glink_defer_cmd, node); > + list_del(&dcmd->node); > + spin_unlock_irqrestore(&glink->rx_lock, flags); > + > + msg = &dcmd->msg; > + cmd = le16_to_cpu(msg->cmd); > + param1 = le16_to_cpu(msg->param1); > + param2 = le32_to_cpu(msg->param2); > + > + switch (cmd) { > + case RPM_CMD_VERSION: > + qcom_glink_send_version_ack(glink); > + break; > + case RPM_CMD_VERSION_ACK: > + break; > + case RPM_CMD_OPEN: > + qcom_glink_rx_open(glink, param1, msg->data); > + break; > + case RPM_CMD_CLOSE: > + qcom_glink_rx_close(glink, param1); > + break; > + case RPM_CMD_CLOSE_ACK: > + qcom_glink_rx_close_ack(glink, param1); > + break; > + default: > + WARN(1, "Unknown defer object %d\n", cmd); > + break; > + } > + > + kfree(dcmd); > + } > +} > + > +struct qcom_glink *qcom_glink_native_probe(struct device *dev, > + struct qcom_glink_pipe *rx, > + struct qcom_glink_pipe *tx) > +{ > + int irq; > + int ret; > + struct qcom_glink *glink; > + > + glink = devm_kzalloc(dev, sizeof(*glink), GFP_KERNEL); > + if (!glink) > + return ERR_PTR(-ENOMEM); > + > + glink->dev = dev; > + glink->tx_pipe = tx; > + glink->rx_pipe = rx; > + > + mutex_init(&glink->tx_lock); > + spin_lock_init(&glink->rx_lock); > + INIT_LIST_HEAD(&glink->rx_queue); > + INIT_WORK(&glink->rx_work, qcom_glink_work); > + > + mutex_init(&glink->idr_lock); > + idr_init(&glink->lcids); > + idr_init(&glink->rcids); > + > + glink->mbox_client.dev = dev; > + glink->mbox_chan = mbox_request_channel(&glink->mbox_client, 0); > + if (IS_ERR(glink->mbox_chan)) { > + if (PTR_ERR(glink->mbox_chan) != -EPROBE_DEFER) > + dev_err(dev, "failed to acquire IPC channel\n"); > + return ERR_CAST(glink->mbox_chan); > + } > + > + irq = of_irq_get(dev->of_node, 0); > + ret = devm_request_irq(dev, irq, > + qcom_glink_native_intr, > + IRQF_NO_SUSPEND | IRQF_SHARED, > + "glink-native", glink); > + if (ret) { > + dev_err(dev, "failed to request IRQ\n"); > + return ERR_PTR(ret); > + } > + > + glink->irq = irq; > + > + ret = qcom_glink_send_version(glink); > + if (ret) > + return ERR_PTR(ret); > + > + return glink; > +} > + > +static int qcom_glink_remove_device(struct device *dev, void *data) > +{ > + device_unregister(dev); > + > + return 0; > +} > + > +void qcom_glink_native_remove(struct qcom_glink *glink) > +{ > + struct glink_channel *channel; > + int cid; > + int ret; > + > + disable_irq(glink->irq); > + cancel_work_sync(&glink->rx_work); > + > + ret = device_for_each_child(glink->dev, NULL, qcom_glink_remove_device); > + if (ret) > + dev_warn(glink->dev, "Can't remove GLINK devices: %d\n", ret); > + > + /* Release any defunct local channels, waiting for close-ack */ > + idr_for_each_entry(&glink->lcids, channel, cid) > + kref_put(&channel->refcount, qcom_glink_channel_release); > + > + idr_destroy(&glink->lcids); > + idr_destroy(&glink->rcids); > +} > diff --git a/drivers/rpmsg/qcom_glink_native.h b/drivers/rpmsg/qcom_glink_native.h > new file mode 100644 > index 0000000..d5627a4 > --- /dev/null > +++ b/drivers/rpmsg/qcom_glink_native.h > @@ -0,0 +1,38 @@ > +/* > + * Copyright (c) 2016-2017, Linaro Ltd > + * > + * This program is free software; you can redistribute it and/or modify > + * it under the terms of the GNU General Public License version 2 and > + * only version 2 as published by the Free Software Foundation. > + * > + * This program is distributed in the hope that it will be useful, > + * but WITHOUT ANY WARRANTY; without even the implied warranty of > + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the > + * GNU General Public License for more details. > + */ > + > +#ifndef __QCOM_GLINK_NATIVE_H__ > +#define __QCOM_GLINK_NATIVE_H__ > + > +struct qcom_glink_pipe { > + size_t length; > + > + size_t (*avail)(struct qcom_glink_pipe *glink_pipe); > + > + void (*peak)(struct qcom_glink_pipe *glink_pipe, void *data, > + size_t count); > + void (*advance)(struct qcom_glink_pipe *glink_pipe, size_t count); > + > + void (*write)(struct qcom_glink_pipe *glink_pipe, > + const void *hdr, size_t hlen, > + const void *data, size_t dlen); > +}; > + > +struct qcom_glink; > + > +struct qcom_glink *qcom_glink_native_probe(struct device *dev, > + struct qcom_glink_pipe *rx, > + struct qcom_glink_pipe *tx); > +void qcom_glink_native_remove(struct qcom_glink *glink); > + > +#endif > diff --git a/drivers/rpmsg/qcom_glink_rpm.c b/drivers/rpmsg/qcom_glink_rpm.c > index 5f0fa0d..33daa32 100644 > --- a/drivers/rpmsg/qcom_glink_rpm.c > +++ b/drivers/rpmsg/qcom_glink_rpm.c > @@ -19,7 +19,6 @@ > #include > #include > #include > -#include > #include > #include > #include > @@ -28,6 +27,7 @@ > #include > > #include "rpmsg_internal.h" > +#include "qcom_glink_native.h" > > #define RPM_TOC_SIZE 256 > #define RPM_TOC_MAGIC 0x67727430 /* grt0 */ > @@ -37,12 +37,7 @@ > #define RPM_TX_FIFO_ID 0x61703272 /* ap2r */ > #define RPM_RX_FIFO_ID 0x72326170 /* r2ap */ > > -#define GLINK_NAME_SIZE 32 > - > -#define RPM_GLINK_CID_MIN 1 > -#define RPM_GLINK_CID_MAX 65536 > - > -#define to_rpm_pipe(p) container_of(p, struct glink_rpm_pipe, native) > +#define to_rpm_pipe(p) container_of(p, struct glink_rpm_pipe, native) > > struct rpm_toc_entry { > __le32 id; > @@ -50,20 +45,6 @@ struct rpm_toc_entry { > __le32 size; > } __packed; > > -struct qcom_glink; > - > -struct qcom_glink_pipe { > - size_t length; > - > - size_t (*avail)(struct qcom_glink_pipe *glink_pipe); > - void (*peak)(struct qcom_glink_pipe *glink_pipe, void *data, > - size_t count); > - void (*advance)(struct qcom_glink_pipe *glink_pipe, size_t count); > - void (*write)(struct qcom_glink_pipe *glink_pipe, > - const void *hdr, size_t hlen, > - const void *data, size_t dlen); > -}; > - > struct rpm_toc { > __le32 magic; > __le32 count; > @@ -71,13 +52,6 @@ struct rpm_toc { > struct rpm_toc_entry entries[]; > } __packed; > > -struct glink_msg { > - __le16 cmd; > - __le16 param1; > - __le32 param2; > - u8 data[]; > -} __packed; > - > struct glink_rpm_pipe { > struct qcom_glink_pipe native; > > @@ -87,151 +61,6 @@ struct glink_rpm_pipe { > void __iomem *fifo; > }; > > -/** > - * struct glink_defer_cmd - deferred incoming control message > - * @node: list node > - * @msg: message header > - * data: payload of the message > - * > - * Copy of a received control message, to be added to @rx_queue and processed > - * by @rx_work of @glink_rpm. > - */ > -struct glink_defer_cmd { > - struct list_head node; > - > - struct glink_msg msg; > - u8 data[]; > -}; > - > -/** > - * struct glink_rpm - driver context, relates to one remote subsystem > - * @dev: reference to the associated struct device > - * @doorbell: "rpm_hlos" ipc doorbell > - * @rx_pipe: pipe object for receive FIFO > - * @tx_pipe: pipe object for transmit FIFO > - * @irq: IRQ for signaling incoming events > - * @rx_work: worker for handling received control messages > - * @rx_lock: protects the @rx_queue > - * @rx_queue: queue of received control messages to be processed in @rx_work > - * @tx_lock: synchronizes operations on the tx fifo > - * @idr_lock: synchronizes @lcids and @rcids modifications > - * @lcids: idr of all channels with a known local channel id > - * @rcids: idr of all channels with a known remote channel id > - */ > -struct qcom_glink { > - struct device *dev; > - > - struct mbox_client mbox_client; > - struct mbox_chan *mbox_chan; > - > - struct qcom_glink_pipe *rx_pipe; > - struct qcom_glink_pipe *tx_pipe; > - > - int irq; > - > - struct work_struct rx_work; > - spinlock_t rx_lock; > - struct list_head rx_queue; > - > - struct mutex tx_lock; > - > - struct mutex idr_lock; > - struct idr lcids; > - struct idr rcids; > -}; > - > -enum { > - GLINK_STATE_CLOSED, > - GLINK_STATE_OPENING, > - GLINK_STATE_OPEN, > - GLINK_STATE_CLOSING, > -}; > - > -/** > - * struct glink_channel - internal representation of a channel > - * @rpdev: rpdev reference, only used for primary endpoints > - * @ept: rpmsg endpoint this channel is associated with > - * @glink: qcom_glink context handle > - * @refcount: refcount for the channel object > - * @recv_lock: guard for @ept.cb > - * @name: unique channel name/identifier > - * @lcid: channel id, in local space > - * @rcid: channel id, in remote space > - * @buf: receive buffer, for gathering fragments > - * @buf_offset: write offset in @buf > - * @buf_size: size of current @buf > - * @open_ack: completed once remote has acked the open-request > - * @open_req: completed once open-request has been received > - */ > -struct glink_channel { > - struct rpmsg_endpoint ept; > - > - struct rpmsg_device *rpdev; > - struct qcom_glink *glink; > - > - struct kref refcount; > - > - spinlock_t recv_lock; > - > - char *name; > - unsigned int lcid; > - unsigned int rcid; > - > - void *buf; > - int buf_offset; > - int buf_size; > - > - struct completion open_ack; > - struct completion open_req; > -}; > - > -#define to_glink_channel(_ept) container_of(_ept, struct glink_channel, ept) > - > -static const struct rpmsg_endpoint_ops glink_endpoint_ops; > - > -#define RPM_CMD_VERSION 0 > -#define RPM_CMD_VERSION_ACK 1 > -#define RPM_CMD_OPEN 2 > -#define RPM_CMD_CLOSE 3 > -#define RPM_CMD_OPEN_ACK 4 > -#define RPM_CMD_TX_DATA 9 > -#define RPM_CMD_CLOSE_ACK 11 > -#define RPM_CMD_TX_DATA_CONT 12 > -#define RPM_CMD_READ_NOTIF 13 > - > -#define GLINK_FEATURE_INTENTLESS BIT(1) > - > -static struct glink_channel *qcom_glink_alloc_channel(struct qcom_glink *glink, > - const char *name) > -{ > - struct glink_channel *channel; > - > - channel = kzalloc(sizeof(*channel), GFP_KERNEL); > - if (!channel) > - return ERR_PTR(-ENOMEM); > - > - /* Setup glink internal glink_channel data */ > - spin_lock_init(&channel->recv_lock); > - channel->glink = glink; > - channel->name = kstrdup(name, GFP_KERNEL); > - > - init_completion(&channel->open_req); > - init_completion(&channel->open_ack); > - > - kref_init(&channel->refcount); > - > - return channel; > -} > - > -static void qcom_glink_channel_release(struct kref *ref) > -{ > - struct glink_channel *channel = container_of(ref, struct glink_channel, > - refcount); > - > - kfree(channel->name); > - kfree(channel); > -} > - > static size_t glink_rpm_rx_avail(struct qcom_glink_pipe *glink_pipe) > { > struct glink_rpm_pipe *pipe = to_rpm_pipe(glink_pipe); > @@ -247,11 +76,6 @@ static size_t glink_rpm_rx_avail(struct qcom_glink_pipe *glink_pipe) > return head - tail; > } > > -static size_t qcom_glink_rx_avail(struct qcom_glink *glink) > -{ > - return glink->rx_pipe->avail(glink->rx_pipe); > -} > - > static void glink_rpm_rx_peak(struct qcom_glink_pipe *glink_pipe, > void *data, size_t count) > { > @@ -273,12 +97,6 @@ static void glink_rpm_rx_peak(struct qcom_glink_pipe *glink_pipe, > } > } > > -static void qcom_glink_rx_peak(struct qcom_glink *glink, > - void *data, size_t count) > -{ > - glink->rx_pipe->peak(glink->rx_pipe, data, count); > -} > - > static void glink_rpm_rx_advance(struct qcom_glink_pipe *glink_pipe, > size_t count) > { > @@ -294,11 +112,6 @@ static void glink_rpm_rx_advance(struct qcom_glink_pipe *glink_pipe, > writel(tail, pipe->tail); > } > > -static void qcom_glink_rx_advance(struct qcom_glink *glink, size_t count) > -{ > - glink->rx_pipe->advance(glink->rx_pipe, count); > -} > - > static size_t glink_rpm_tx_avail(struct qcom_glink_pipe *glink_pipe) > { > struct glink_rpm_pipe *pipe = to_rpm_pipe(glink_pipe); > @@ -314,11 +127,6 @@ static size_t glink_rpm_tx_avail(struct qcom_glink_pipe *glink_pipe) > return tail - head; > } > > -static size_t qcom_glink_tx_avail(struct qcom_glink *glink) > -{ > - return glink->tx_pipe->avail(glink->tx_pipe); > -} > - > static unsigned int glink_rpm_tx_write_one(struct glink_rpm_pipe *pipe, > unsigned int head, > const void *data, size_t count) > @@ -356,731 +164,6 @@ static void glink_rpm_tx_write(struct qcom_glink_pipe *glink_pipe, > writel(head, pipe->head); > } > > -static void qcom_glink_tx_write(struct qcom_glink *glink, > - const void *hdr, size_t hlen, > - const void *data, size_t dlen) > -{ > - glink->tx_pipe->write(glink->tx_pipe, hdr, hlen, data, dlen); > -} > - > -static int qcom_glink_tx(struct qcom_glink *glink, > - const void *hdr, size_t hlen, > - const void *data, size_t dlen, bool wait) > -{ > - unsigned int tlen = hlen + dlen; > - int ret; > - > - /* Reject packets that are too big */ > - if (tlen >= glink->tx_pipe->length) > - return -EINVAL; > - > - if (WARN(tlen % 8, "Unaligned TX request")) > - return -EINVAL; > - > - ret = mutex_lock_interruptible(&glink->tx_lock); > - if (ret) > - return ret; > - > - while (qcom_glink_tx_avail(glink) < tlen) { > - if (!wait) { > - ret = -ENOMEM; > - goto out; > - } > - > - msleep(10); > - } > - > - qcom_glink_tx_write(glink, hdr, hlen, data, dlen); > - > - mbox_send_message(glink->mbox_chan, NULL); > - mbox_client_txdone(glink->mbox_chan, 0); > - > -out: > - mutex_unlock(&glink->tx_lock); > - > - return ret; > -} > - > -static int qcom_glink_send_version(struct qcom_glink *glink) > -{ > - struct glink_msg msg; > - > - msg.cmd = cpu_to_le16(RPM_CMD_VERSION); > - msg.param1 = cpu_to_le16(1); > - msg.param2 = cpu_to_le32(GLINK_FEATURE_INTENTLESS); > - > - return qcom_glink_tx(glink, &msg, sizeof(msg), NULL, 0, true); > -} > - > -static void qcom_glink_send_version_ack(struct qcom_glink *glink) > -{ > - struct glink_msg msg; > - > - msg.cmd = cpu_to_le16(RPM_CMD_VERSION_ACK); > - msg.param1 = cpu_to_le16(1); > - msg.param2 = cpu_to_le32(0); > - > - qcom_glink_tx(glink, &msg, sizeof(msg), NULL, 0, true); > -} > - > -static void qcom_glink_send_open_ack(struct qcom_glink *glink, > - struct glink_channel *channel) > -{ > - struct glink_msg msg; > - > - msg.cmd = cpu_to_le16(RPM_CMD_OPEN_ACK); > - msg.param1 = cpu_to_le16(channel->rcid); > - msg.param2 = cpu_to_le32(0); > - > - qcom_glink_tx(glink, &msg, sizeof(msg), NULL, 0, true); > -} > - > -/** > - * qcom_glink_send_open_req() - send a RPM_CMD_OPEN request to the remote > - * @glink: > - * @channel: > - * > - * Allocates a local channel id and sends a RPM_CMD_OPEN message to the remote. > - * Will return with refcount held, regardless of outcome. > - * > - * Returns 0 on success, negative errno otherwise. > - */ > -static int qcom_glink_send_open_req(struct qcom_glink *glink, > - struct glink_channel *channel) > -{ > - struct { > - struct glink_msg msg; > - u8 name[GLINK_NAME_SIZE]; > - } __packed req; > - int name_len = strlen(channel->name) + 1; > - int req_len = ALIGN(sizeof(req.msg) + name_len, 8); > - int ret; > - > - kref_get(&channel->refcount); > - > - mutex_lock(&glink->idr_lock); > - ret = idr_alloc_cyclic(&glink->lcids, channel, > - RPM_GLINK_CID_MIN, RPM_GLINK_CID_MAX, GFP_KERNEL); > - mutex_unlock(&glink->idr_lock); > - if (ret < 0) > - return ret; > - > - channel->lcid = ret; > - > - req.msg.cmd = cpu_to_le16(RPM_CMD_OPEN); > - req.msg.param1 = cpu_to_le16(channel->lcid); > - req.msg.param2 = cpu_to_le32(name_len); > - strcpy(req.name, channel->name); > - > - ret = qcom_glink_tx(glink, &req, req_len, NULL, 0, true); > - if (ret) > - goto remove_idr; > - > - return 0; > - > -remove_idr: > - mutex_lock(&glink->idr_lock); > - idr_remove(&glink->lcids, channel->lcid); > - channel->lcid = 0; > - mutex_unlock(&glink->idr_lock); > - > - return ret; > -} > - > -static void qcom_glink_send_close_req(struct qcom_glink *glink, > - struct glink_channel *channel) > -{ > - struct glink_msg req; > - > - req.cmd = cpu_to_le16(RPM_CMD_CLOSE); > - req.param1 = cpu_to_le16(channel->lcid); > - req.param2 = 0; > - > - qcom_glink_tx(glink, &req, sizeof(req), NULL, 0, true); > -} > - > -static void qcom_glink_send_close_ack(struct qcom_glink *glink, > - unsigned int rcid) > -{ > - struct glink_msg req; > - > - req.cmd = cpu_to_le16(RPM_CMD_CLOSE_ACK); > - req.param1 = cpu_to_le16(rcid); > - req.param2 = 0; > - > - qcom_glink_tx(glink, &req, sizeof(req), NULL, 0, true); > -} > - > -static int qcom_glink_rx_defer(struct qcom_glink *glink, size_t extra) > -{ > - struct glink_defer_cmd *dcmd; > - > - extra = ALIGN(extra, 8); > - > - if (qcom_glink_rx_avail(glink) < sizeof(struct glink_msg) + extra) { > - dev_dbg(glink->dev, "Insufficient data in rx fifo"); > - return -ENXIO; > - } > - > - dcmd = kzalloc(sizeof(*dcmd) + extra, GFP_ATOMIC); > - if (!dcmd) > - return -ENOMEM; > - > - INIT_LIST_HEAD(&dcmd->node); > - > - qcom_glink_rx_peak(glink, &dcmd->msg, sizeof(dcmd->msg) + extra); > - > - spin_lock(&glink->rx_lock); > - list_add_tail(&dcmd->node, &glink->rx_queue); > - spin_unlock(&glink->rx_lock); > - > - schedule_work(&glink->rx_work); > - qcom_glink_rx_advance(glink, sizeof(dcmd->msg) + extra); > - > - return 0; > -} > - > -static int qcom_glink_rx_data(struct qcom_glink *glink, size_t avail) > -{ > - struct glink_channel *channel; > - struct { > - struct glink_msg msg; > - __le32 chunk_size; > - __le32 left_size; > - } __packed hdr; > - unsigned int chunk_size; > - unsigned int left_size; > - unsigned int rcid; > - > - if (avail < sizeof(hdr)) { > - dev_dbg(glink->dev, "Not enough data in fifo\n"); > - return -EAGAIN; > - } > - > - qcom_glink_rx_peak(glink, &hdr, sizeof(hdr)); > - chunk_size = le32_to_cpu(hdr.chunk_size); > - left_size = le32_to_cpu(hdr.left_size); > - > - if (avail < sizeof(hdr) + chunk_size) { > - dev_dbg(glink->dev, "Payload not yet in fifo\n"); > - return -EAGAIN; > - } > - > - if (WARN(chunk_size % 4, "Incoming data must be word aligned\n")) > - return -EINVAL; > - > - rcid = le16_to_cpu(hdr.msg.param1); > - channel = idr_find(&glink->rcids, rcid); > - if (!channel) { > - dev_dbg(glink->dev, "Data on non-existing channel\n"); > - > - /* Drop the message */ > - qcom_glink_rx_advance(glink, > - ALIGN(sizeof(hdr) + chunk_size, 8)); > - return 0; > - } > - > - /* Might have an ongoing, fragmented, message to append */ > - if (!channel->buf) { > - channel->buf = kmalloc(chunk_size + left_size, GFP_ATOMIC); > - if (!channel->buf) > - return -ENOMEM; > - > - channel->buf_size = chunk_size + left_size; > - channel->buf_offset = 0; > - } > - > - qcom_glink_rx_advance(glink, sizeof(hdr)); > - > - if (channel->buf_size - channel->buf_offset < chunk_size) { > - dev_err(glink->dev, "Insufficient space in input buffer\n"); > - > - /* The packet header lied, drop payload */ > - qcom_glink_rx_advance(glink, chunk_size); > - return -ENOMEM; > - } > - > - qcom_glink_rx_peak(glink, channel->buf + channel->buf_offset, > - chunk_size); > - channel->buf_offset += chunk_size; > - > - /* Handle message when no fragments remain to be received */ > - if (!left_size) { > - spin_lock(&channel->recv_lock); > - if (channel->ept.cb) { > - channel->ept.cb(channel->ept.rpdev, > - channel->buf, > - channel->buf_offset, > - channel->ept.priv, > - RPMSG_ADDR_ANY); > - } > - spin_unlock(&channel->recv_lock); > - > - kfree(channel->buf); > - channel->buf = NULL; > - channel->buf_size = 0; > - } > - > - /* Each message starts at 8 byte aligned address */ > - qcom_glink_rx_advance(glink, ALIGN(chunk_size, 8)); > - > - return 0; > -} > - > -static int qcom_glink_rx_open_ack(struct qcom_glink *glink, unsigned int lcid) > -{ > - struct glink_channel *channel; > - > - channel = idr_find(&glink->lcids, lcid); > - if (!channel) { > - dev_err(glink->dev, "Invalid open ack packet\n"); > - return -EINVAL; > - } > - > - complete(&channel->open_ack); > - > - return 0; > -} > - > -static irqreturn_t qcom_glink_intr(int irq, void *data) > -{ > - struct qcom_glink *glink = data; > - struct glink_msg msg; > - unsigned int param1; > - unsigned int param2; > - unsigned int avail; > - unsigned int cmd; > - int ret; > - > - for (;;) { > - avail = qcom_glink_rx_avail(glink); > - if (avail < sizeof(msg)) > - break; > - > - qcom_glink_rx_peak(glink, &msg, sizeof(msg)); > - > - cmd = le16_to_cpu(msg.cmd); > - param1 = le16_to_cpu(msg.param1); > - param2 = le32_to_cpu(msg.param2); > - > - switch (cmd) { > - case RPM_CMD_VERSION: > - case RPM_CMD_VERSION_ACK: > - case RPM_CMD_CLOSE: > - case RPM_CMD_CLOSE_ACK: > - ret = qcom_glink_rx_defer(glink, 0); > - break; > - case RPM_CMD_OPEN_ACK: > - ret = qcom_glink_rx_open_ack(glink, param1); > - qcom_glink_rx_advance(glink, ALIGN(sizeof(msg), 8)); > - break; > - case RPM_CMD_OPEN: > - ret = qcom_glink_rx_defer(glink, param2); > - break; > - case RPM_CMD_TX_DATA: > - case RPM_CMD_TX_DATA_CONT: > - ret = qcom_glink_rx_data(glink, avail); > - break; > - case RPM_CMD_READ_NOTIF: > - qcom_glink_rx_advance(glink, ALIGN(sizeof(msg), 8)); > - > - mbox_send_message(glink->mbox_chan, NULL); > - mbox_client_txdone(glink->mbox_chan, 0); > - > - ret = 0; > - break; > - default: > - dev_err(glink->dev, "unhandled rx cmd: %d\n", cmd); > - ret = -EINVAL; > - break; > - } > - > - if (ret) > - break; > - } > - > - return IRQ_HANDLED; > -} > - > -/* Locally initiated rpmsg_create_ept */ > -static struct glink_channel *qcom_glink_create_local(struct qcom_glink *glink, > - const char *name) > -{ > - struct glink_channel *channel; > - int ret; > - > - channel = qcom_glink_alloc_channel(glink, name); > - if (IS_ERR(channel)) > - return ERR_CAST(channel); > - > - ret = qcom_glink_send_open_req(glink, channel); > - if (ret) > - goto release_channel; > - > - ret = wait_for_completion_timeout(&channel->open_ack, 5 * HZ); > - if (!ret) > - goto err_timeout; > - > - ret = wait_for_completion_timeout(&channel->open_req, 5 * HZ); > - if (!ret) > - goto err_timeout; > - > - qcom_glink_send_open_ack(glink, channel); > - > - return channel; > - > -err_timeout: > - /* qcom_glink_send_open_req() did register the channel in lcids*/ > - mutex_lock(&glink->idr_lock); > - idr_remove(&glink->lcids, channel->lcid); > - mutex_unlock(&glink->idr_lock); > - > -release_channel: > - /* Release qcom_glink_send_open_req() reference */ > - kref_put(&channel->refcount, qcom_glink_channel_release); > - /* Release qcom_glink_alloc_channel() reference */ > - kref_put(&channel->refcount, qcom_glink_channel_release); > - > - return ERR_PTR(-ETIMEDOUT); > -} > - > -/* Remote initiated rpmsg_create_ept */ > -static int qcom_glink_create_remote(struct qcom_glink *glink, > - struct glink_channel *channel) > -{ > - int ret; > - > - qcom_glink_send_open_ack(glink, channel); > - > - ret = qcom_glink_send_open_req(glink, channel); > - if (ret) > - goto close_link; > - > - ret = wait_for_completion_timeout(&channel->open_ack, 5 * HZ); > - if (!ret) { > - ret = -ETIMEDOUT; > - goto close_link; > - } > - > - return 0; > - > -close_link: > - /* > - * Send a close request to "undo" our open-ack. The close-ack will > - * release the last reference. > - */ > - qcom_glink_send_close_req(glink, channel); > - > - /* Release qcom_glink_send_open_req() reference */ > - kref_put(&channel->refcount, qcom_glink_channel_release); > - > - return ret; > -} > - > -static struct rpmsg_endpoint *qcom_glink_create_ept(struct rpmsg_device *rpdev, > - rpmsg_rx_cb_t cb, > - void *priv, > - struct rpmsg_channel_info > - chinfo) > -{ > - struct glink_channel *parent = to_glink_channel(rpdev->ept); > - struct glink_channel *channel; > - struct qcom_glink *glink = parent->glink; > - struct rpmsg_endpoint *ept; > - const char *name = chinfo.name; > - int cid; > - int ret; > - > - idr_for_each_entry(&glink->rcids, channel, cid) { > - if (!strcmp(channel->name, name)) > - break; > - } > - > - if (!channel) { > - channel = qcom_glink_create_local(glink, name); > - if (IS_ERR(channel)) > - return NULL; > - } else { > - ret = qcom_glink_create_remote(glink, channel); > - if (ret) > - return NULL; > - } > - > - ept = &channel->ept; > - ept->rpdev = rpdev; > - ept->cb = cb; > - ept->priv = priv; > - ept->ops = &glink_endpoint_ops; > - > - return ept; > -} > - > -static void qcom_glink_destroy_ept(struct rpmsg_endpoint *ept) > -{ > - struct glink_channel *channel = to_glink_channel(ept); > - struct qcom_glink *glink = channel->glink; > - unsigned long flags; > - > - spin_lock_irqsave(&channel->recv_lock, flags); > - channel->ept.cb = NULL; > - spin_unlock_irqrestore(&channel->recv_lock, flags); > - > - /* Decouple the potential rpdev from the channel */ > - channel->rpdev = NULL; > - > - qcom_glink_send_close_req(glink, channel); > -} > - > -static int __qcom_glink_send(struct glink_channel *channel, > - void *data, int len, bool wait) > -{ > - struct qcom_glink *glink = channel->glink; > - struct { > - struct glink_msg msg; > - __le32 chunk_size; > - __le32 left_size; > - } __packed req; > - > - if (WARN(len % 8, "RPM GLINK expects 8 byte aligned messages\n")) > - return -EINVAL; > - > - req.msg.cmd = cpu_to_le16(RPM_CMD_TX_DATA); > - req.msg.param1 = cpu_to_le16(channel->lcid); > - req.msg.param2 = cpu_to_le32(channel->rcid); > - req.chunk_size = cpu_to_le32(len); > - req.left_size = cpu_to_le32(0); > - > - return qcom_glink_tx(glink, &req, sizeof(req), data, len, wait); > -} > - > -static int qcom_glink_send(struct rpmsg_endpoint *ept, void *data, int len) > -{ > - struct glink_channel *channel = to_glink_channel(ept); > - > - return __qcom_glink_send(channel, data, len, true); > -} > - > -static int qcom_glink_trysend(struct rpmsg_endpoint *ept, void *data, int len) > -{ > - struct glink_channel *channel = to_glink_channel(ept); > - > - return __qcom_glink_send(channel, data, len, false); > -} > - > -/* > - * Finds the device_node for the glink child interested in this channel. > - */ > -static struct device_node *qcom_glink_match_channel(struct device_node *node, > - const char *channel) > -{ > - struct device_node *child; > - const char *name; > - const char *key; > - int ret; > - > - for_each_available_child_of_node(node, child) { > - key = "qcom,glink-channels"; > - ret = of_property_read_string(child, key, &name); > - if (ret) > - continue; > - > - if (strcmp(name, channel) == 0) > - return child; > - } > - > - return NULL; > -} > - > -static const struct rpmsg_device_ops glink_device_ops = { > - .create_ept = qcom_glink_create_ept, > -}; > - > -static const struct rpmsg_endpoint_ops glink_endpoint_ops = { > - .destroy_ept = qcom_glink_destroy_ept, > - .send = qcom_glink_send, > - .trysend = qcom_glink_trysend, > -}; > - > -static void qcom_glink_rpdev_release(struct device *dev) > -{ > - struct rpmsg_device *rpdev = to_rpmsg_device(dev); > - struct glink_channel *channel = to_glink_channel(rpdev->ept); > - > - channel->rpdev = NULL; > - kfree(rpdev); > -} > - > -static int qcom_glink_rx_open(struct qcom_glink *glink, unsigned int rcid, > - char *name) > -{ > - struct glink_channel *channel; > - struct rpmsg_device *rpdev; > - bool create_device = false; > - int lcid; > - int ret; > - struct device_node *node; > - > - idr_for_each_entry(&glink->lcids, channel, lcid) { > - if (!strcmp(channel->name, name)) > - break; > - } > - > - if (!channel) { > - channel = qcom_glink_alloc_channel(glink, name); > - if (IS_ERR(channel)) > - return PTR_ERR(channel); > - > - /* The opening dance was initiated by the remote */ > - create_device = true; > - } > - > - mutex_lock(&glink->idr_lock); > - ret = idr_alloc(&glink->rcids, channel, rcid, rcid + 1, GFP_KERNEL); > - if (ret < 0) { > - dev_err(glink->dev, "Unable to insert channel into rcid list\n"); > - mutex_unlock(&glink->idr_lock); > - goto free_channel; > - } > - channel->rcid = ret; > - mutex_unlock(&glink->idr_lock); > - > - complete(&channel->open_req); > - > - if (create_device) { > - rpdev = kzalloc(sizeof(*rpdev), GFP_KERNEL); > - if (!rpdev) { > - ret = -ENOMEM; > - goto rcid_remove; > - } > - > - rpdev->ept = &channel->ept; > - strncpy(rpdev->id.name, name, RPMSG_NAME_SIZE); > - rpdev->src = RPMSG_ADDR_ANY; > - rpdev->dst = RPMSG_ADDR_ANY; > - rpdev->ops = &glink_device_ops; > - > - node = qcom_glink_match_channel(glink->dev->of_node, name); > - rpdev->dev.of_node = node; > - rpdev->dev.parent = glink->dev; > - rpdev->dev.release = qcom_glink_rpdev_release; > - > - ret = rpmsg_register_device(rpdev); > - if (ret) > - goto free_rpdev; > - > - channel->rpdev = rpdev; > - } > - > - return 0; > - > -free_rpdev: > - kfree(rpdev); > -rcid_remove: > - mutex_lock(&glink->idr_lock); > - idr_remove(&glink->rcids, channel->rcid); > - channel->rcid = 0; > - mutex_unlock(&glink->idr_lock); > -free_channel: > - /* Release the reference, iff we took it */ > - if (create_device) > - kref_put(&channel->refcount, qcom_glink_channel_release); > - > - return ret; > -} > - > -static void qcom_glink_rx_close(struct qcom_glink *glink, unsigned int rcid) > -{ > - struct rpmsg_channel_info chinfo; > - struct glink_channel *channel; > - > - channel = idr_find(&glink->rcids, rcid); > - if (WARN(!channel, "close request on unknown channel\n")) > - return; > - > - if (channel->rpdev) { > - strncpy(chinfo.name, channel->name, sizeof(chinfo.name)); > - chinfo.src = RPMSG_ADDR_ANY; > - chinfo.dst = RPMSG_ADDR_ANY; > - > - rpmsg_unregister_device(glink->dev, &chinfo); > - } > - > - qcom_glink_send_close_ack(glink, channel->rcid); > - > - mutex_lock(&glink->idr_lock); > - idr_remove(&glink->rcids, channel->rcid); > - channel->rcid = 0; > - mutex_unlock(&glink->idr_lock); > - > - kref_put(&channel->refcount, qcom_glink_channel_release); > -} > - > -static void qcom_glink_rx_close_ack(struct qcom_glink *glink, unsigned int lcid) > -{ > - struct glink_channel *channel; > - > - channel = idr_find(&glink->lcids, lcid); > - if (WARN(!channel, "close ack on unknown channel\n")) > - return; > - > - mutex_lock(&glink->idr_lock); > - idr_remove(&glink->lcids, channel->lcid); > - channel->lcid = 0; > - mutex_unlock(&glink->idr_lock); > - > - kref_put(&channel->refcount, qcom_glink_channel_release); > -} > - > -static void qcom_glink_work(struct work_struct *work) > -{ > - struct qcom_glink *glink = container_of(work, struct qcom_glink, > - rx_work); > - struct glink_defer_cmd *dcmd; > - struct glink_msg *msg; > - unsigned long flags; > - unsigned int param1; > - unsigned int param2; > - unsigned int cmd; > - > - for (;;) { > - spin_lock_irqsave(&glink->rx_lock, flags); > - if (list_empty(&glink->rx_queue)) { > - spin_unlock_irqrestore(&glink->rx_lock, flags); > - break; > - } > - dcmd = list_first_entry(&glink->rx_queue, struct glink_defer_cmd, node); > - list_del(&dcmd->node); > - spin_unlock_irqrestore(&glink->rx_lock, flags); > - > - msg = &dcmd->msg; > - cmd = le16_to_cpu(msg->cmd); > - param1 = le16_to_cpu(msg->param1); > - param2 = le32_to_cpu(msg->param2); > - > - switch (cmd) { > - case RPM_CMD_VERSION: > - qcom_glink_send_version_ack(glink); > - break; > - case RPM_CMD_VERSION_ACK: > - break; > - case RPM_CMD_OPEN: > - qcom_glink_rx_open(glink, param1, msg->data); > - break; > - case RPM_CMD_CLOSE: > - qcom_glink_rx_close(glink, param1); > - break; > - case RPM_CMD_CLOSE_ACK: > - qcom_glink_rx_close_ack(glink, param1); > - break; > - default: > - WARN(1, "Unknown defer object %d\n", cmd); > - break; > - } > - > - kfree(dcmd); > - } > -} > - > static int glink_rpm_parse_toc(struct device *dev, > void __iomem *msg_ram, > size_t msg_ram_size, > @@ -1156,56 +239,6 @@ static int glink_rpm_parse_toc(struct device *dev, > return -EINVAL; > } > > -struct qcom_glink *qcom_glink_native_probe(struct device *dev, > - struct qcom_glink_pipe *rx, > - struct qcom_glink_pipe *tx) > -{ > - int irq; > - int ret; > - struct qcom_glink *glink; > - > - glink = devm_kzalloc(dev, sizeof(*glink), GFP_KERNEL); > - if (!glink) > - return ERR_PTR(-ENOMEM); > - > - glink->dev = dev; > - glink->tx_pipe = tx; > - glink->rx_pipe = rx; > - > - mutex_init(&glink->tx_lock); > - spin_lock_init(&glink->rx_lock); > - INIT_LIST_HEAD(&glink->rx_queue); > - INIT_WORK(&glink->rx_work, qcom_glink_work); > - > - mutex_init(&glink->idr_lock); > - idr_init(&glink->lcids); > - idr_init(&glink->rcids); > - > - glink->mbox_client.dev = dev; > - glink->mbox_chan = mbox_request_channel(&glink->mbox_client, 0); > - if (IS_ERR(glink->mbox_chan)) { > - if (PTR_ERR(glink->mbox_chan) != -EPROBE_DEFER) > - dev_err(dev, "failed to acquire IPC channel\n"); > - return ERR_CAST(glink->mbox_chan); > - } > - > - irq = of_irq_get(dev->of_node, 0); > - ret = devm_request_irq(dev, irq, > - qcom_glink_intr, > - IRQF_NO_SUSPEND | IRQF_SHARED, > - "glink-native", glink); > - if (ret) { > - dev_err(dev, "failed to request IRQ\n"); > - return ERR_PTR(ret); > - } > - > - ret = qcom_glink_send_version(glink); > - if (ret) > - return ERR_PTR(ret); > - > - return glink; > -} > - > static int glink_rpm_probe(struct platform_device *pdev) > { > struct qcom_glink *glink; > @@ -1259,33 +292,11 @@ static int glink_rpm_probe(struct platform_device *pdev) > return 0; > } > > -static int glink_rpm_remove_device(struct device *dev, void *data) > -{ > - device_unregister(dev); > - > - return 0; > -} > - > static int glink_rpm_remove(struct platform_device *pdev) > { > struct qcom_glink *glink = platform_get_drvdata(pdev); > - struct glink_channel *channel; > - int cid; > - int ret; > - > - disable_irq(glink->irq); > - cancel_work_sync(&glink->rx_work); > - > - ret = device_for_each_child(glink->dev, NULL, glink_rpm_remove_device); > - if (ret) > - dev_warn(glink->dev, "Can't remove GLINK devices: %d\n", ret); > - > - /* Release any defunct local channels, waiting for close-ack */ > - idr_for_each_entry(&glink->lcids, channel, cid) > - kref_put(&channel->refcount, qcom_glink_channel_release); > > - idr_destroy(&glink->lcids); > - idr_destroy(&glink->rcids); > + qcom_glink_native_remove(glink); > > return 0; > }