recipes-kernel/linux/5.15/1023-drivers-i2c-add-slave-mqueue.patch

From e9f4254fd17b010cf78b57e3f5a6fe2202edbd4d Mon Sep 17 00:00:00 2001
From: David Wang <davidwang@quantatw.com>
Date: Wed, 8 Nov 2023 09:37:12 +0800
Subject: [PATCH] drivers: i2c: add slave-mqueue

---
 drivers/i2c/Kconfig            |  25 ++++
 drivers/i2c/Makefile           |   1 +
 drivers/i2c/i2c-slave-mqueue.c | 215 +++++++++++++++++++++++++++++++++
 3 files changed, 241 insertions(+)
 create mode 100644 drivers/i2c/i2c-slave-mqueue.c

diff --git a/drivers/i2c/Kconfig b/drivers/i2c/Kconfig
index 438905e2a1d0..5ffe13438b6f 100644
--- a/drivers/i2c/Kconfig
+++ b/drivers/i2c/Kconfig
@@ -133,6 +133,31 @@ config I2C_SLAVE_TESTUNIT
 	  multi-master, SMBus Host Notify, etc. Please read
 	  Documentation/i2c/slave-testunit-backend.rst for further details.
 
+config I2C_SLAVE_MQUEUE
+	tristate "I2C mqueue (message queue) slave driver"
+	help
+	  Some protocols over I2C are designed for bi-directional transferring
+	  messages by using I2C Master Write protocol. This driver is used to
+	  receive and queue messages from the remote I2C device.
+
+	  Userspace can get the messages by reading sysfs file that this driver
+	  exposes.
+
+	  This support is also available as a module. If so, the module will be
+	  called i2c-slave-mqueue.
+
+config I2C_SLAVE_MQUEUE_MESSAGE_SIZE
+	int "The message size of I2C mqueue slave"
+	depends on I2C_SLAVE_MQUEUE
+	default 120
+
+config I2C_SLAVE_MQUEUE_QUEUE_SIZE
+	int "The queue size of I2C mqueue slave"
+	depends on I2C_SLAVE_MQUEUE
+	default 32
+	help
+	  This number MUST be power of 2.
+
 endif
 
 config I2C_DEBUG_CORE
diff --git a/drivers/i2c/Makefile b/drivers/i2c/Makefile
index c1d493dc9bac..0442e5cf8587 100644
--- a/drivers/i2c/Makefile
+++ b/drivers/i2c/Makefile
@@ -17,5 +17,6 @@ obj-y				+= algos/ busses/ muxes/
 obj-$(CONFIG_I2C_STUB)		+= i2c-stub.o
 obj-$(CONFIG_I2C_SLAVE_EEPROM)	+= i2c-slave-eeprom.o
 obj-$(CONFIG_I2C_SLAVE_TESTUNIT)	+= i2c-slave-testunit.o
+obj-$(CONFIG_I2C_SLAVE_MQUEUE)	+= i2c-slave-mqueue.o
 
 ccflags-$(CONFIG_I2C_DEBUG_CORE) := -DDEBUG
diff --git a/drivers/i2c/i2c-slave-mqueue.c b/drivers/i2c/i2c-slave-mqueue.c
new file mode 100644
index 000000000000..c17c4911928f
--- /dev/null
+++ b/drivers/i2c/i2c-slave-mqueue.c
@@ -0,0 +1,215 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (c) 2017 - 2018, Intel Corporation.
+
+#include <linux/i2c.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/of.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/sysfs.h>
+
+#define MQ_MSGBUF_SIZE		CONFIG_I2C_SLAVE_MQUEUE_MESSAGE_SIZE
+#define MQ_QUEUE_SIZE		CONFIG_I2C_SLAVE_MQUEUE_QUEUE_SIZE
+#define MQ_QUEUE_NEXT(x)	(((x) + 1) & (MQ_QUEUE_SIZE - 1))
+
+struct mq_msg {
+	int	len;
+	u8	*buf;
+};
+
+struct mq_queue {
+	struct bin_attribute	bin;
+	struct kernfs_node	*kn;
+
+	spinlock_t		lock; /* spinlock for queue index handling */
+	int			in;
+	int			out;
+
+	struct mq_msg		*curr;
+	int			truncated; /* drop current if truncated */
+	struct mq_msg		queue[MQ_QUEUE_SIZE];
+};
+
+static int i2c_slave_mqueue_callback(struct i2c_client *client,
+				     enum i2c_slave_event event, u8 *val)
+{
+	struct mq_queue *mq = i2c_get_clientdata(client);
+	struct mq_msg *msg = mq->curr;
+	int ret = 0;
+
+	switch (event) {
+	case I2C_SLAVE_WRITE_REQUESTED:
+		mq->truncated = 0;
+
+		msg->len = 1;
+		msg->buf[0] = client->addr << 1;
+		break;
+
+	case I2C_SLAVE_WRITE_RECEIVED:
+		if (msg->len < MQ_MSGBUF_SIZE) {
+			msg->buf[msg->len++] = *val;
+		} else {
+			dev_err(&client->dev, "message is truncated!\n");
+			mq->truncated = 1;
+			ret = -EINVAL;
+		}
+		break;
+
+	case I2C_SLAVE_STOP:
+		if (unlikely(mq->truncated || msg->len < 2))
+			break;
+
+		spin_lock(&mq->lock);
+		mq->in = MQ_QUEUE_NEXT(mq->in);
+		mq->curr = &mq->queue[mq->in];
+		mq->curr->len = 0;
+
+		/* Flush the oldest message */
+		if (mq->out == mq->in)
+			mq->out = MQ_QUEUE_NEXT(mq->out);
+		spin_unlock(&mq->lock);
+
+		kernfs_notify(mq->kn);
+		break;
+
+	default:
+		*val = 0xFF;
+		break;
+	}
+
+	return ret;
+}
+
+static ssize_t i2c_slave_mqueue_bin_read(struct file *filp,
+					 struct kobject *kobj,
+					 struct bin_attribute *attr,
+					 char *buf, loff_t pos, size_t count)
+{
+	struct mq_queue *mq;
+	struct mq_msg *msg;
+	unsigned long flags;
+	bool more = false;
+	ssize_t ret = 0;
+
+	mq = dev_get_drvdata(kobj_to_dev(kobj));
+
+	spin_lock_irqsave(&mq->lock, flags);
+	if (mq->out != mq->in) {
+		msg = &mq->queue[mq->out];
+
+		if (msg->len <= count) {
+			ret = msg->len;
+			memcpy(buf, msg->buf, ret);
+		} else {
+			ret = -EOVERFLOW; /* Drop this HUGE one. */
+		}
+
+		mq->out = MQ_QUEUE_NEXT(mq->out);
+		if (mq->out != mq->in)
+			more = true;
+	}
+	spin_unlock_irqrestore(&mq->lock, flags);
+
+	if (more)
+		kernfs_notify(mq->kn);
+
+	return ret;
+}
+
+static int i2c_slave_mqueue_probe(struct i2c_client *client,
+				  const struct i2c_device_id *id)
+{
+	struct device *dev = &client->dev;
+	struct mq_queue *mq;
+	int ret, i;
+	void *buf;
+
+	BUILD_BUG_ON(!is_power_of_2(MQ_QUEUE_SIZE));
+
+	mq = devm_kzalloc(dev, sizeof(*mq), GFP_KERNEL);
+	if (!mq)
+		return -ENOMEM;
+
+	buf = devm_kmalloc_array(dev, MQ_QUEUE_SIZE, MQ_MSGBUF_SIZE,
+				 GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	for (i = 0; i < MQ_QUEUE_SIZE; i++)
+		mq->queue[i].buf = buf + i * MQ_MSGBUF_SIZE;
+
+	i2c_set_clientdata(client, mq);
+
+	spin_lock_init(&mq->lock);
+	mq->curr = &mq->queue[0];
+
+	sysfs_bin_attr_init(&mq->bin);
+	mq->bin.attr.name = "slave-mqueue";
+	mq->bin.attr.mode = 0400;
+	mq->bin.read = i2c_slave_mqueue_bin_read;
+	mq->bin.size = MQ_MSGBUF_SIZE * MQ_QUEUE_SIZE;
+
+	ret = sysfs_create_bin_file(&dev->kobj, &mq->bin);
+	if (ret)
+		return ret;
+
+	mq->kn = kernfs_find_and_get(dev->kobj.sd, mq->bin.attr.name);
+	if (!mq->kn) {
+		sysfs_remove_bin_file(&dev->kobj, &mq->bin);
+		return -EFAULT;
+	}
+
+	ret = i2c_slave_register(client, i2c_slave_mqueue_callback);
+	if (ret) {
+		kernfs_put(mq->kn);
+		sysfs_remove_bin_file(&dev->kobj, &mq->bin);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int i2c_slave_mqueue_remove(struct i2c_client *client)
+{
+	struct mq_queue *mq = i2c_get_clientdata(client);
+
+	i2c_slave_unregister(client);
+
+	kernfs_put(mq->kn);
+	sysfs_remove_bin_file(&client->dev.kobj, &mq->bin);
+
+	return 0;
+}
+
+static const struct i2c_device_id i2c_slave_mqueue_id[] = {
+	{ "slave-mqueue", 0 },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(i2c, i2c_slave_mqueue_id);
+
+#ifdef CONFIG_OF
+static const struct of_device_id i2c_slave_mqueue_of_match[] = {
+	{
+		.compatible = "i2c-slave-mqueue",
+	},
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, i2c_slave_mqueue_of_match);
+#endif
+
+static struct i2c_driver i2c_slave_mqueue_driver = {
+	.driver = {
+		.name	= "i2c-slave-mqueue",
+		.of_match_table = of_match_ptr(i2c_slave_mqueue_of_match),
+	},
+	.probe		= i2c_slave_mqueue_probe,
+	.remove		= i2c_slave_mqueue_remove,
+	.id_table	= i2c_slave_mqueue_id,
+};
+module_i2c_driver(i2c_slave_mqueue_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Haiyue Wang <haiyue.wang@linux.intel.com>");
+MODULE_DESCRIPTION("I2C slave mode for receiving and queuing messages");
-- 
2.25.1