[PATCH v2 14/16] rpmsg: glink: add create and release rpmsg channel ops

[Arnaud Pouliquen]

Add the new ops introduced by the rpmsg_ns series and used
by the rpmsg_ctrl driver to instantiate a new rpmsg channel.

Signed-off-by: Arnaud Pouliquen <arnaud.pouliquen@xxxxxxxxxxx>
---
 drivers/rpmsg/qcom_glink_native.c | 94 ++++++++++++++++++++++++-------
 1 file changed, 75 insertions(+), 19 deletions(-)

diff --git a/drivers/rpmsg/qcom_glink_native.c b/drivers/rpmsg/qcom_glink_native.c
index 27a05167c18c..d74c338de077 100644
--- a/drivers/rpmsg/qcom_glink_native.c
+++ b/drivers/rpmsg/qcom_glink_native.c
@@ -205,6 +205,9 @@ static const struct rpmsg_endpoint_ops glink_endpoint_ops;
 #define GLINK_FEATURE_INTENTLESS	BIT(1)
 
 static void qcom_glink_rx_done_work(struct work_struct *work);
+static struct rpmsg_device *
+qcom_glink_create_rpdev(struct qcom_glink *glink,
+			struct rpmsg_channel_info *chinfo);
 
 static struct glink_channel *qcom_glink_alloc_channel(struct qcom_glink *glink,
 						      const char *name)
@@ -1203,6 +1206,37 @@ static int qcom_glink_announce_create(struct rpmsg_device *rpdev)
 	return 0;
 }
 
+static struct rpmsg_device *
+qcom_glink_create_channel(struct rpmsg_device *rp_parent,
+			  struct rpmsg_channel_info *chinfo)
+{
+	struct glink_channel *channel = to_glink_channel(rp_parent->ept);
+	struct qcom_glink *glink = channel->glink;
+	struct rpmsg_device *rpdev;
+	const char *name = chinfo->name;
+
+	channel = qcom_glink_alloc_channel(glink, name);
+	if (IS_ERR(channel))
+		return ERR_PTR(PTR_ERR(channel));
+
+	rpdev = qcom_glink_create_rpdev(glink, chinfo);
+	if (!IS_ERR(rpdev)) {
+		rpdev->ept = &channel->ept;
+		channel->rpdev = rpdev;
+	}
+
+	return rpdev;
+}
+
+static int qcom_glink_release_channel(struct rpmsg_device *rpdev,
+				      struct rpmsg_channel_info *chinfo)
+{
+	struct glink_channel *channel = to_glink_channel(rpdev->ept);
+	struct qcom_glink *glink = channel->glink;
+
+	return rpmsg_unregister_device(glink->dev, chinfo);
+}
+
 static void qcom_glink_destroy_ept(struct rpmsg_endpoint *ept)
 {
 	struct glink_channel *channel = to_glink_channel(ept);
@@ -1359,6 +1393,8 @@ static struct device_node *qcom_glink_match_channel(struct device_node *node,
 static const struct rpmsg_device_ops glink_device_ops = {
 	.create_ept = qcom_glink_create_ept,
 	.announce_create = qcom_glink_announce_create,
+	.create_channel = qcom_glink_create_channel,
+	.release_channel = qcom_glink_release_channel,
 };
 
 static const struct rpmsg_endpoint_ops glink_endpoint_ops = {
@@ -1376,13 +1412,45 @@ static void qcom_glink_rpdev_release(struct device *dev)
 	kfree(rpdev);
 }
 
+static struct rpmsg_device *
+qcom_glink_create_rpdev(struct qcom_glink *glink,
+			struct rpmsg_channel_info *chinfo)
+{
+	struct rpmsg_device *rpdev;
+	struct device_node *node;
+	int ret;
+
+	rpdev = kzalloc(sizeof(*rpdev), GFP_KERNEL);
+	if (!rpdev)
+		return ERR_PTR(-ENOMEM);
+
+	strncpy(rpdev->id.name, chinfo->name, RPMSG_NAME_SIZE);
+	rpdev->src = chinfo->src;
+	rpdev->dst = chinfo->dst;
+	rpdev->ops = &glink_device_ops;
+
+	node = qcom_glink_match_channel(glink->dev->of_node, chinfo->name);
+	rpdev->dev.of_node = node;
+	rpdev->dev.parent = glink->dev;
+	rpdev->dev.release = qcom_glink_rpdev_release;
+	rpdev->driver_override = (char *)chinfo->driver_override;
+
+	ret = rpmsg_register_device(rpdev);
+	if (ret) {
+		kfree(rpdev);
+		return ERR_PTR(ret);
+	}
+
+	return 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;
+	struct rpmsg_channel_info chinfo;
 	int lcid;
 	int ret;
 	unsigned long flags;
@@ -1416,27 +1484,15 @@ static int qcom_glink_rx_open(struct qcom_glink *glink, unsigned int rcid,
 	complete_all(&channel->open_req);
 
 	if (create_device) {
-		rpdev = kzalloc(sizeof(*rpdev), GFP_KERNEL);
-		if (!rpdev) {
-			ret = -ENOMEM;
+		strncpy(chinfo.name, channel->name, sizeof(chinfo.name));
+		chinfo.src = RPMSG_ADDR_ANY;
+		chinfo.dst = RPMSG_ADDR_ANY;
+		rpdev = qcom_glink_create_rpdev(glink, &chinfo);
+		if (IS_ERR(rpdev)) {
+			ret = PTR_ERR(rpdev);
 			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 rcid_remove;
-
 		channel->rpdev = rpdev;
 	}
 
-- 
2.17.1