[PATCH 5/5] i2c: rtl9300: Add multiplexing support

[Chris Packham <chris.packham@alliedtelesis.co.nz>]

The RTL9300 I2C controller can support multiplexing by choosing the SDA
pin to be used dynamically. Add mutliplexing support to the rtl9300
driver.

Signed-off-by: Chris Packham <chris.packham@alliedtelesis.co.nz>
---
 drivers/i2c/busses/i2c-rtl9300.c | 168 ++++++++++++++++++++++++++++++-
 1 file changed, 167 insertions(+), 1 deletion(-)

diff --git a/drivers/i2c/busses/i2c-rtl9300.c b/drivers/i2c/busses/i2c-rtl9300.c
index f16e9b6343bf..a934a2526c92 100644
--- a/drivers/i2c/busses/i2c-rtl9300.c
+++ b/drivers/i2c/busses/i2c-rtl9300.c
@@ -1,6 +1,7 @@
 // SPDX-License-Identifier: GPL-2.0-only
 
 #include <linux/i2c.h>
+#include <linux/i2c-mux.h>
 #include <linux/mod_devicetable.h>
 #include <linux/mfd/syscon.h>
 #include <linux/mutex.h>
@@ -44,6 +45,14 @@ struct rtl9300_i2c {
 #define RTL9300_I2C_STD_FREQ		0
 #define RTL9300_I2C_FAST_FREQ		1
 
+struct rtl9300_i2c_chan {
+	int parent;
+	const unsigned int *chans;
+	int n_chan;
+};
+
+#define RTL9300_I2C_MUX_NCHAN	8
+
 DEFINE_MUTEX(i2c_lock);
 
 static int rtl9300_i2c_reg_addr_set(struct rtl9300_i2c *i2c, u32 reg, u16 len)
@@ -370,7 +379,164 @@ static struct platform_driver rtl9300_i2c_driver = {
 	},
 };
 
-module_platform_driver(rtl9300_i2c_driver);
+static int rtl9300_i2c_select_chan(struct i2c_mux_core *muxc, u32 chan)
+{
+	struct i2c_adapter *adap = muxc->parent;
+	struct rtl9300_i2c *i2c = i2c_get_adapdata(adap);
+	int ret;
+
+	ret = rtl9300_i2c_config_io(i2c, chan);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int rtl9300_i2c_deselect_mux(struct i2c_mux_core *muxc, u32 chan)
+{
+	struct i2c_adapter *adap = muxc->parent;
+	struct rtl9300_i2c *i2c = i2c_get_adapdata(adap);
+	int ret;
+
+	ret = rtl9300_i2c_config_io(i2c, i2c->sda_pin);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int rtl9300_i2c_mux_probe_fw(struct rtl9300_i2c_chan *mux, struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct fwnode_handle *fwnode = dev_fwnode(dev);
+	struct device_node *np = dev->of_node;
+	struct device_node *adap_np;
+	struct i2c_adapter *adap = NULL;
+	struct fwnode_handle *child;
+	unsigned int *chans;
+	int i = 0;
+
+	if (!is_of_node(fwnode))
+		return -EOPNOTSUPP;
+
+	if (!np)
+		return -ENODEV;
+
+	adap_np = of_parse_phandle(np, "i2c-parent", 0);
+	if (!adap_np) {
+		dev_err(&pdev->dev, "Cannot parse i2c-parent\n");
+		return -ENODEV;
+	}
+	adap = of_find_i2c_adapter_by_node(adap_np);
+	of_node_put(adap_np);
+
+	if (!adap)
+		return -EPROBE_DEFER;
+
+	mux->parent = i2c_adapter_id(adap);
+	put_device(&adap->dev);
+
+	mux->n_chan = device_get_child_node_count(dev);
+	if (mux->n_chan >= RTL9300_I2C_MUX_NCHAN)
+		return -EINVAL;
+
+	chans = devm_kcalloc(dev, mux->n_chan, sizeof(*mux->chans), GFP_KERNEL);
+	if (!chans)
+		return -ENOMEM;
+
+	device_for_each_child_node(dev, child) {
+		fwnode_property_read_u32(child, "reg", chans + i);
+		i++;
+	}
+	mux->chans = chans;
+
+	return 0;
+}
+
+static int rtl9300_i2c_mux_probe(struct platform_device *pdev)
+{
+	struct i2c_mux_core *muxc;
+	struct i2c_adapter *adap;
+	struct rtl9300_i2c_chan *mux;
+	int ret, i;
+
+	mux = devm_kzalloc(&pdev->dev, sizeof(*mux), GFP_KERNEL);
+	if (!mux)
+		return -ENOMEM;
+
+	ret = rtl9300_i2c_mux_probe_fw(mux, pdev);
+	if (ret)
+		return ret;
+
+	adap = i2c_get_adapter(mux->parent);
+	if (!adap)
+		return -EPROBE_DEFER;
+
+	muxc = i2c_mux_alloc(adap, &pdev->dev, mux->n_chan, 0, 0,
+			     rtl9300_i2c_select_chan, rtl9300_i2c_deselect_mux);
+	if (!muxc) {
+		ret = -ENOMEM;
+		goto err_alloc;
+	}
+	muxc->priv = mux;
+
+	platform_set_drvdata(pdev, muxc);
+
+	for (i = 0; i < mux->n_chan; i++) {
+		ret = i2c_mux_add_adapter(muxc, 0, mux->chans[i]);
+		if (ret)
+			goto err_del_adapters;
+	}
+
+	return 0;
+
+err_del_adapters:
+	i2c_mux_del_adapters(muxc);
+err_alloc:
+	i2c_put_adapter(adap);
+
+	return ret;
+}
+
+static void rtl9300_i2c_mux_remove(struct platform_device *pdev)
+{
+	struct i2c_mux_core *muxc = platform_get_drvdata(pdev);
+
+	i2c_mux_del_adapters(muxc);
+	i2c_put_adapter(muxc->parent);
+}
+
+static const struct of_device_id i2c_rtl9300_mux_dt_ids[] = {
+	{ .compatible = "realtek,rtl9300-i2c-mux" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, i2c_rtl9300_mux_dt_ids);
+
+static struct platform_driver rtl9300_i2c_mux_driver = {
+	.probe = rtl9300_i2c_mux_probe,
+	.remove = rtl9300_i2c_mux_remove,
+	.driver = {
+		.name = "i2c-mux-rtl9300",
+		.of_match_table = i2c_rtl9300_mux_dt_ids,
+	},
+};
+
+static struct platform_driver * const drivers[] = {
+	&rtl9300_i2c_driver,
+	&rtl9300_i2c_mux_driver,
+};
+
+static int __init rtl9300_i2c_init(void)
+{
+	return platform_register_drivers(drivers, ARRAY_SIZE(drivers));
+}
+module_init(rtl9300_i2c_init);
+
+static void __exit rtl9300_i2c_exit(void)
+{
+	platform_unregister_drivers(drivers, ARRAY_SIZE(drivers));
+}
+module_exit(rtl9300_i2c_exit);
 
 MODULE_DESCRIPTION("RTL9300 I2C controller driver");
 MODULE_LICENSE("GPL");
-- 
2.46.1