[RFC PATCH 05/10] rpmsg: Introduce flow control channel driver

[Arnaud Pouliquen <arnaud.pouliquen@foss.st.com>]

Register the channel 54 to manage flow control between endpoints.
the aim of this service is.
- to inform the local endpoint of the state of the associated remote
endpoints.
- to inform remote endpoint about the state of the local endpoint.

Signed-off-by: Arnaud Pouliquen <arnaud.pouliquen@foss.st.com>
---
 drivers/rpmsg/Kconfig    |  10 ++++
 drivers/rpmsg/Makefile   |   1 +
 drivers/rpmsg/rpmsg_fc.c | 113 +++++++++++++++++++++++++++++++++++++++
 include/linux/rpmsg/fc.h |  51 ++++++++++++++++++
 4 files changed, 175 insertions(+)
 create mode 100644 drivers/rpmsg/rpmsg_fc.c
 create mode 100644 include/linux/rpmsg/fc.h

diff --git a/drivers/rpmsg/Kconfig b/drivers/rpmsg/Kconfig
index d3795860f5c0..c6659f27c617 100644
--- a/drivers/rpmsg/Kconfig
+++ b/drivers/rpmsg/Kconfig
@@ -31,6 +31,16 @@ config RPMSG_NS
 	  channel that probes the associated RPMsg device on remote endpoint
 	  service announcement.
 
+config RPMSG_FC
+	tristate "RPMSG endpoint flow control management"
+	depends on RPMSG
+	help
+	  Say Y here to enable the support of the flow control management
+	  for the rpmsg endpoints.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called rpmsg_fc.
+
 config RPMSG_MTK_SCP
 	tristate "MediaTek SCP"
 	depends on MTK_SCP
diff --git a/drivers/rpmsg/Makefile b/drivers/rpmsg/Makefile
index 58e3b382e316..c70b9864231f 100644
--- a/drivers/rpmsg/Makefile
+++ b/drivers/rpmsg/Makefile
@@ -3,6 +3,7 @@ obj-$(CONFIG_RPMSG)		+= rpmsg_core.o
 obj-$(CONFIG_RPMSG_CHAR)	+= rpmsg_char.o
 obj-$(CONFIG_RPMSG_CTRL)	+= rpmsg_ctrl.o
 obj-$(CONFIG_RPMSG_NS)		+= rpmsg_ns.o
+obj-$(CONFIG_RPMSG_FC)		+= rpmsg_fc.o
 obj-$(CONFIG_RPMSG_MTK_SCP)	+= mtk_rpmsg.o
 qcom_glink-objs			:= qcom_glink_native.o qcom_glink_ssr.o
 obj-$(CONFIG_RPMSG_QCOM_GLINK) += qcom_glink.o
diff --git a/drivers/rpmsg/rpmsg_fc.c b/drivers/rpmsg/rpmsg_fc.c
new file mode 100644
index 000000000000..d7e17c8cffb0
--- /dev/null
+++ b/drivers/rpmsg/rpmsg_fc.c
@@ -0,0 +1,113 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) STMicroelectronics 2022 - All Rights Reserved
+ */
+
+#define pr_fmt(fmt)		KBUILD_MODNAME ": " fmt
+
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/rpmsg.h>
+#include <linux/rpmsg/fc.h>
+#include <linux/slab.h>
+
+#include "rpmsg_internal.h"
+
+/**
+ * rpmsg_fc_register_device() - register name service device based on rpdev
+ * @rpdev: prepared rpdev to be used catfor creating endpoints
+ *
+ * This function wraps rpmsg_register_device() preparing the rpdev for use as
+ * basis for the rpmsg name service device.
+ */
+int rpmsg_fc_register_device(struct rpmsg_device *rpdev)
+{
+	strcpy(rpdev->id.name, "rpmsg_fc");
+	rpdev->driver_override = KBUILD_MODNAME;
+	rpdev->src = RPMSG_FC_ADDR;
+	rpdev->dst = RPMSG_FC_ADDR;
+
+	return rpmsg_register_device(rpdev);
+}
+EXPORT_SYMBOL(rpmsg_fc_register_device);
+
+/* Invoked when a name service announcement arrives */
+static int rpmsg_fc_cb(struct rpmsg_device *rpdev, void *data, int len,
+		       void *priv, u32 src)
+{
+	struct rpmsg_ept_msg *msg = data;
+	struct rpmsg_channel_info chinfo;
+	struct device *dev = rpdev->dev.parent;
+	bool enable;
+	int ret;
+
+	if (len != sizeof(*msg)) {
+		dev_err(dev, "malformed fc msg (%d)\n", len);
+		return -EINVAL;
+	}
+
+	chinfo.src = rpmsg32_to_cpu(rpdev, msg->src);
+	chinfo.dst = rpmsg32_to_cpu(rpdev, msg->dst);
+	enable = rpmsg32_to_cpu(rpdev, msg->flags) & RPMSG_EPT_FC_ON;
+
+	dev_dbg(dev, "remote endpoint 0x%x in state %sable\n", chinfo.src, enable ? "en" : "dis");
+
+	ret = rpmsg_channel_remote_fc(rpdev, &chinfo, enable);
+	if (ret)
+		dev_err(dev, "rpmsg_annouce_flow_ctrl failed: %d\n", ret);
+
+	return ret;
+}
+
+static int rpmsg_fc_probe(struct rpmsg_device *rpdev)
+{
+	struct rpmsg_endpoint *fc_ept;
+	struct rpmsg_channel_info fc_chinfo = {
+		.src = RPMSG_FC_ADDR,
+		.dst = RPMSG_FC_ADDR,
+		.name = "flow_control_service",
+	};
+
+	/*
+	 * Create the Flow control (FC) service endpoint associated to the RPMsg
+	 * device. The endpoint will be automatically destroyed when the RPMsg
+	 * device will be deleted.
+	 */
+	fc_ept = rpmsg_create_ept(rpdev, rpmsg_fc_cb, NULL, fc_chinfo);
+	if (!fc_ept) {
+		dev_err(&rpdev->dev, "failed to create the FC ept\n");
+		return -ENOMEM;
+	}
+	rpdev->ept = fc_ept;
+
+	return 0;
+}
+
+static struct rpmsg_driver rpmsg_fc_driver = {
+	.drv.name = KBUILD_MODNAME,
+	.probe = rpmsg_fc_probe,
+};
+
+static int rpmsg_fc_init(void)
+{
+	int ret;
+
+	ret = register_rpmsg_driver(&rpmsg_fc_driver);
+	if (ret < 0)
+		pr_err("%s: Failed to register FC rpmsg driver\n", __func__);
+
+	return ret;
+}
+postcore_initcall(rpmsg_fc_init);
+
+static void rpmsg_fc_exit(void)
+{
+	unregister_rpmsg_driver(&rpmsg_fc_driver);
+}
+module_exit(rpmsg_fc_exit);
+
+MODULE_DESCRIPTION("Flow control service rpmsg driver");
+MODULE_AUTHOR("Arnaud Pouliquen <arnaud.pouliquen@foss.st.com>");
+MODULE_ALIAS("rpmsg:" KBUILD_MODNAME);
+MODULE_LICENSE("GPL");
diff --git a/include/linux/rpmsg/fc.h b/include/linux/rpmsg/fc.h
new file mode 100644
index 000000000000..5284ea410673
--- /dev/null
+++ b/include/linux/rpmsg/fc.h
@@ -0,0 +1,51 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+
+#ifndef _LINUX_RPMSG_FC_H
+#define _LINUX_RPMSG_FC_H
+
+#include <linux/mod_devicetable.h>
+#include <linux/rpmsg.h>
+#include <linux/rpmsg/byteorder.h>
+#include <linux/types.h>
+
+/* The feature bitmap for the endpoint flow control flags */
+#define	RPMSG_EPT_FC_ON	 BIT(0) /* Set when endpoint is ready to communicate */
+
+/**
+ * struct rpmsg_ept_msg - dynamic endpoint announcement message
+ * @src: address of the endpoint that sends the message
+ * @dest: address of the destination endpoint.
+ * @flags: indicates the state of the endpoint based on @rpmsg_ept_flags enum.
+ *
+ * This message is sent across to inform the remote about the state of a local
+ * endpoint associated with a remote endpoint:
+ * - a RPMSG_EPT_OFF can be sent to inform that a local endpoint is suspended.
+ * - a RPMSG_EPT_ON can be sent to inform that a local endpoint is ready to communicate.
+ *
+ * When we receive these messages, the appropriate endpoint is informed.
+ */
+struct rpmsg_ept_msg {
+	__rpmsg32 src;
+	__rpmsg32 dst;
+	__rpmsg32 flags;
+} __packed;
+
+/* Address 54 is reserved for flow control advertising */
+#define RPMSG_FC_ADDR                   (54)
+
+#if IS_ENABLED(CONFIG_RPMSG_FC)
+
+int rpmsg_fc_register_device(struct rpmsg_device *rpdev);
+
+#else
+
+static inline int rpmsg_fc_register_device(struct rpmsg_device *rpdev)
+{
+	/* This shouldn't be possible */
+	WARN_ON(1);
+
+	return -ENXIO;
+}
+#endif /* IS_ENABLED(CONFIG_RPMSG_FC)*/
+
+#endif
-- 
2.25.1