From 3e75ca66195dec023ca2e837ff748c317fd7ac26 Mon Sep 17 00:00:00 2001
From: Sam Shih <sam.shih@mediatek.com>
Date: Fri, 2 Jun 2023 13:06:00 +0800
Subject: [PATCH] 
 [backport-networking-drivers][999-1705-add-netlink-support-for-dsa.patch]

---
 drivers/net/dsa/Makefile    |   2 +-
 drivers/net/dsa/mt7530.c    |  24 ++-
 drivers/net/dsa/mt7530.h    |   8 +
 drivers/net/dsa/mt7530_nl.c | 311 ++++++++++++++++++++++++++++++++++++
 drivers/net/dsa/mt7530_nl.h |  49 ++++++
 5 files changed, 386 insertions(+), 8 deletions(-)
 create mode 100644 drivers/net/dsa/mt7530_nl.c
 create mode 100644 drivers/net/dsa/mt7530_nl.h

diff --git a/drivers/net/dsa/Makefile b/drivers/net/dsa/Makefile
index 0aa10bc3d..ef563c6c1 100644
--- a/drivers/net/dsa/Makefile
+++ b/drivers/net/dsa/Makefile
@@ -7,7 +7,7 @@ obj-$(CONFIG_FIXED_PHY)		+= dsa_loop_bdinfo.o
 endif
 obj-$(CONFIG_NET_DSA_LANTIQ_GSWIP) += lantiq_gswip.o
 obj-$(CONFIG_NET_DSA_MT7530)	+= mt7530-dsa.o
-mt7530-dsa-objs			:= mt7530.o mt7531_phy.o
+mt7530-dsa-objs			:= mt7530.o mt7530_nl.o mt7531_phy.o
 obj-$(CONFIG_NET_DSA_MV88E6060) += mv88e6060.o
 obj-$(CONFIG_NET_DSA_QCA8K)	+= qca8k.o
 obj-$(CONFIG_NET_DSA_REALTEK_SMI) += realtek-smi.o
diff --git a/drivers/net/dsa/mt7530.c b/drivers/net/dsa/mt7530.c
index e4c021eeb..63f8a632b 100644
--- a/drivers/net/dsa/mt7530.c
+++ b/drivers/net/dsa/mt7530.c
@@ -21,6 +21,7 @@
 #include <net/dsa.h>
 
 #include "mt7530.h"
+#include "mt7530_nl.h"
 
 /* String, offset, and register size in bytes if different from 4 bytes */
 static const struct mt7530_mib_desc mt7530_mib[] = {
@@ -222,7 +223,7 @@ mt7530_mii_read(struct mt7530_priv *priv, u32 reg)
 	return (hi << 16) | (lo & 0xffff);
 }
 
-static void
+void
 mt7530_write(struct mt7530_priv *priv, u32 reg, u32 val)
 {
 	struct mii_bus *bus = priv->bus;
@@ -255,7 +256,7 @@ _mt7530_read(struct mt7530_dummy_poll *p)
 	return val;
 }
 
-static u32
+u32
 mt7530_read(struct mt7530_priv *priv, u32 reg)
 {
 	struct mt7530_dummy_poll p;
@@ -614,7 +615,7 @@ static int mt7530_phy_write(struct dsa_switch *ds, int port, int regnum,
 	return mdiobus_write_nested(priv->bus, port, regnum, val);
 }
 
-static int
+int
 mt7531_ind_c45_phy_read(struct mt7530_priv *priv, int port, int devad,
 			int regnum)
 {
@@ -663,7 +664,7 @@ out:
 	return ret;
 }
 
-static int
+int
 mt7531_ind_c45_phy_write(struct mt7530_priv *priv, int port, int devad,
 			 int regnum, u32 data)
 {
@@ -711,7 +712,7 @@ out:
 	return ret;
 }
 
-static int
+int
 mt7531_ind_c22_phy_read(struct mt7530_priv *priv, int port, int regnum)
 {
 	struct mii_bus *bus = priv->bus;
@@ -749,7 +750,7 @@ out:
 	return ret;
 }
 
-static int
+int
 mt7531_ind_c22_phy_write(struct mt7530_priv *priv, int port, int regnum,
 			 u16 data)
 {
@@ -2691,6 +2692,7 @@ mt7530_probe(struct mdio_device *mdiodev)
 {
 	struct mt7530_priv *priv;
 	struct device_node *dn;
+	int ret;
 
 	dn = mdiodev->dev.of_node;
 
@@ -2766,7 +2768,13 @@ mt7530_probe(struct mdio_device *mdiodev)
 	mutex_init(&priv->reg_mutex);
 	dev_set_drvdata(&mdiodev->dev, priv);
 
-	return dsa_register_switch(priv->ds);
+	ret = dsa_register_switch(priv->ds);
+	if (ret)
+		return ret;
+
+	mt7530_nl_init(&priv);
+
+	return 0;
 }
 
 static void
@@ -2787,6 +2795,8 @@ mt7530_remove(struct mdio_device *mdiodev)
 
 	dsa_unregister_switch(priv->ds);
 	mutex_destroy(&priv->reg_mutex);
+
+	mt7530_nl_exit();
 }
 
 static struct mdio_driver mt7530_mdio_driver = {
diff --git a/drivers/net/dsa/mt7530.h b/drivers/net/dsa/mt7530.h
index 8f1e827ff..130d7e5ec 100644
--- a/drivers/net/dsa/mt7530.h
+++ b/drivers/net/dsa/mt7530.h
@@ -783,4 +783,12 @@ static inline void INIT_MT7530_DUMMY_POLL(struct mt7530_dummy_poll *p,
 }
 
 int mt7531_phy_setup(struct dsa_switch *ds);
+u32 mt7530_read(struct mt7530_priv *priv, u32 reg);
+void mt7530_write(struct mt7530_priv *priv, u32 reg, u32 val);
+int mt7531_ind_c45_phy_read(struct mt7530_priv *priv, int port, int devad, int regnum);
+int mt7531_ind_c45_phy_write(struct mt7530_priv *priv, int port, int devad, int regnum, u32 data);
+int mt7531_ind_c22_phy_read(struct mt7530_priv *priv, int port, int regnum);
+int mt7531_ind_c22_phy_write(struct mt7530_priv *priv, int port, int regnum, u16 data);
+
+
 #endif /* __MT7530_H */
diff --git a/drivers/net/dsa/mt7530_nl.c b/drivers/net/dsa/mt7530_nl.c
new file mode 100644
index 000000000..676adef70
--- /dev/null
+++ b/drivers/net/dsa/mt7530_nl.c
@@ -0,0 +1,311 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2018 MediaTek Inc.
+ * Author: Sirui Zhao <Sirui.Zhao@mediatek.com>
+ */
+
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <net/genetlink.h>
+#include <linux/of_mdio.h>
+#include <linux/phylink.h>
+#include <net/dsa.h>
+
+#include "mt7530.h"
+#include "mt7530_nl.h"
+
+struct mt7530_nl_cmd_item {
+	enum mt7530_cmd cmd;
+	bool require_dev;
+	int (*process)(struct genl_info *info);
+	u32 nr_required_attrs;
+	const enum mt7530_attr *required_attrs;
+};
+
+struct mt7530_priv *sw_priv;
+
+static DEFINE_MUTEX(mt7530_devs_lock);
+
+void mt7530_put(void)
+{
+	mutex_unlock(&mt7530_devs_lock);
+}
+
+void mt7530_lock(void)
+{
+	mutex_lock(&mt7530_devs_lock);
+}
+
+static int mt7530_nl_response(struct sk_buff *skb, struct genl_info *info);
+
+static const struct nla_policy mt7530_nl_cmd_policy[] = {
+	[MT7530_ATTR_TYPE_MESG] = { .type = NLA_STRING },
+	[MT7530_ATTR_TYPE_PHY] = { .type = NLA_S32 },
+	[MT7530_ATTR_TYPE_REG] = { .type = NLA_S32 },
+	[MT7530_ATTR_TYPE_VAL] = { .type = NLA_S32 },
+	[MT7530_ATTR_TYPE_DEV_NAME] = { .type = NLA_S32 },
+	[MT7530_ATTR_TYPE_DEV_ID] = { .type = NLA_S32 },
+	[MT7530_ATTR_TYPE_DEVAD] = { .type = NLA_S32 },
+};
+
+static const struct genl_ops mt7530_nl_ops[] = {
+	{
+		.cmd = MT7530_CMD_REQUEST,
+		.doit = mt7530_nl_response,
+		.flags = GENL_ADMIN_PERM,
+	}, {
+		.cmd = MT7530_CMD_READ,
+		.doit = mt7530_nl_response,
+		.flags = GENL_ADMIN_PERM,
+	}, {
+		.cmd = MT7530_CMD_WRITE,
+		.doit = mt7530_nl_response,
+		.flags = GENL_ADMIN_PERM,
+	},
+};
+
+static struct genl_family mt7530_nl_family = {
+	.name =		MT7530_DSA_GENL_NAME,
+	.version =	MT7530_GENL_VERSION,
+	.maxattr =	MT7530_NR_ATTR_TYPE,
+	.ops =		mt7530_nl_ops,
+	.n_ops =	ARRAY_SIZE(mt7530_nl_ops),
+	.policy =	mt7530_nl_cmd_policy,
+};
+
+static int mt7530_nl_prepare_reply(struct genl_info *info, u8 cmd,
+				   struct sk_buff **skbp)
+{
+	struct sk_buff *msg;
+	void *reply;
+
+	if (!info)
+		return -EINVAL;
+
+	msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
+
+	/* Construct send-back message header */
+	reply = genlmsg_put(msg, info->snd_portid, info->snd_seq,
+			    &mt7530_nl_family, 0, cmd);
+	if (!reply) {
+		nlmsg_free(msg);
+		return -EINVAL;
+	}
+
+	*skbp = msg;
+	return 0;
+}
+
+static int mt7530_nl_send_reply(struct sk_buff *skb, struct genl_info *info)
+{
+	struct genlmsghdr *genlhdr = nlmsg_data(nlmsg_hdr(skb));
+	void *reply = genlmsg_data(genlhdr);
+
+	/* Finalize a generic netlink message (update message header) */
+	genlmsg_end(skb, reply);
+
+	/* reply to a request */
+	return genlmsg_reply(skb, info);
+}
+
+static s32 mt7530_nl_get_s32(struct genl_info *info, enum mt7530_attr attr,
+			     s32 defval)
+{
+	struct nlattr *na;
+
+	na = info->attrs[attr];
+	if (na)
+		return nla_get_s32(na);
+
+	return defval;
+}
+
+static int mt7530_nl_get_u32(struct genl_info *info, enum mt7530_attr attr,
+			     u32 *val)
+{
+	struct nlattr *na;
+
+	na = info->attrs[attr];
+	if (na) {
+		*val = nla_get_u32(na);
+		return 0;
+	}
+
+	return -1;
+}
+
+static int mt7530_nl_reply_read(struct genl_info *info)
+{
+	struct sk_buff *rep_skb = NULL;
+	s32 phy, devad, reg;
+	int value;
+	int ret = 0;
+
+	phy = mt7530_nl_get_s32(info, MT7530_ATTR_TYPE_PHY, -1);
+	devad = mt7530_nl_get_s32(info, MT7530_ATTR_TYPE_DEVAD, -1);
+	reg = mt7530_nl_get_s32(info, MT7530_ATTR_TYPE_REG, -1);
+
+	if (reg < 0)
+		goto err;
+
+	ret = mt7530_nl_prepare_reply(info, MT7530_CMD_READ, &rep_skb);
+	if (ret < 0)
+		goto err;
+
+	if (phy >= 0) {
+		if (devad < 0)
+			value = mt7531_ind_c22_phy_read(sw_priv, phy, reg);
+		else
+			value = mt7531_ind_c45_phy_read(sw_priv, phy, devad, reg);
+	} else
+		value = mt7530_read(sw_priv, reg);
+
+	ret = nla_put_s32(rep_skb, MT7530_ATTR_TYPE_REG, reg);
+	if (ret < 0)
+		goto err;
+
+	ret = nla_put_s32(rep_skb, MT7530_ATTR_TYPE_VAL, value);
+	if (ret < 0)
+		goto err;
+
+	return mt7530_nl_send_reply(rep_skb, info);
+
+err:
+	if (rep_skb)
+		nlmsg_free(rep_skb);
+
+	return ret;
+}
+
+static int mt7530_nl_reply_write(struct genl_info *info)
+{
+	struct sk_buff *rep_skb = NULL;
+	s32 phy, devad, reg;
+	u32 value;
+	int ret = 0;
+
+	phy = mt7530_nl_get_s32(info, MT7530_ATTR_TYPE_PHY, -1);
+	devad = mt7530_nl_get_s32(info, MT7530_ATTR_TYPE_DEVAD, -1);
+	reg = mt7530_nl_get_s32(info, MT7530_ATTR_TYPE_REG, -1);
+
+	if (mt7530_nl_get_u32(info, MT7530_ATTR_TYPE_VAL, &value))
+		goto err;
+
+	if (reg < 0)
+		goto err;
+
+	ret = mt7530_nl_prepare_reply(info, MT7530_CMD_WRITE, &rep_skb);
+	if (ret < 0)
+		goto err;
+
+	if (phy >= 0) {
+		if (devad < 0)
+			mt7531_ind_c22_phy_write(sw_priv, phy, reg, value);
+		else
+			mt7531_ind_c45_phy_write(sw_priv, phy, devad, reg, value);
+	} else
+		mt7530_write(sw_priv, reg, value);
+
+	ret = nla_put_s32(rep_skb, MT7530_ATTR_TYPE_REG, reg);
+	if (ret < 0)
+		goto err;
+
+	ret = nla_put_s32(rep_skb, MT7530_ATTR_TYPE_VAL, value);
+	if (ret < 0)
+		goto err;
+
+	return mt7530_nl_send_reply(rep_skb, info);
+
+err:
+	if (rep_skb)
+		nlmsg_free(rep_skb);
+
+	return ret;
+}
+
+static const enum mt7530_attr mt7530_nl_cmd_read_attrs[] = {
+	MT7530_ATTR_TYPE_REG
+};
+
+static const enum mt7530_attr mt7530_nl_cmd_write_attrs[] = {
+	MT7530_ATTR_TYPE_REG,
+	MT7530_ATTR_TYPE_VAL
+};
+
+static const struct mt7530_nl_cmd_item mt7530_nl_cmds[] = {
+	{
+		.cmd = MT7530_CMD_READ,
+		.require_dev = true,
+		.process = mt7530_nl_reply_read,
+		.required_attrs = mt7530_nl_cmd_read_attrs,
+		.nr_required_attrs = ARRAY_SIZE(mt7530_nl_cmd_read_attrs),
+	}, {
+		.cmd = MT7530_CMD_WRITE,
+		.require_dev = true,
+		.process = mt7530_nl_reply_write,
+		.required_attrs = mt7530_nl_cmd_write_attrs,
+		.nr_required_attrs = ARRAY_SIZE(mt7530_nl_cmd_write_attrs),
+	}
+};
+
+static int mt7530_nl_response(struct sk_buff *skb, struct genl_info *info)
+{
+	struct genlmsghdr *hdr = nlmsg_data(info->nlhdr);
+	const struct mt7530_nl_cmd_item *cmditem = NULL;
+	u32 sat_req_attrs = 0;
+	int i, ret;
+
+	for (i = 0; i < ARRAY_SIZE(mt7530_nl_cmds); i++) {
+		if (hdr->cmd == mt7530_nl_cmds[i].cmd) {
+			cmditem = &mt7530_nl_cmds[i];
+			break;
+		}
+	}
+
+	if (!cmditem) {
+		pr_info("mt7530-nl: unknown cmd %u\n", hdr->cmd);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < cmditem->nr_required_attrs; i++) {
+		if (info->attrs[cmditem->required_attrs[i]])
+			sat_req_attrs++;
+	}
+
+	if (sat_req_attrs != cmditem->nr_required_attrs) {
+		pr_info("mt7530-nl: missing required attr(s) for cmd %u\n",
+			hdr->cmd);
+		return -EINVAL;
+	}
+
+	ret = cmditem->process(info);
+
+	mt7530_put();
+
+	return ret;
+}
+
+int mt7530_nl_init(struct mt7530_priv **priv)
+{
+	int ret;
+
+	pr_info("mt7530-nl: genl_register_family_with_ops \n");
+
+	sw_priv = *priv;
+	ret = genl_register_family(&mt7530_nl_family);
+	if (ret) {
+		return ret;
+	}
+
+	return 0;
+}
+
+void mt7530_nl_exit()
+{
+	sw_priv = NULL;
+	genl_unregister_family(&mt7530_nl_family);
+}
diff --git a/drivers/net/dsa/mt7530_nl.h b/drivers/net/dsa/mt7530_nl.h
new file mode 100644
index 000000000..4619288c2
--- /dev/null
+++ b/drivers/net/dsa/mt7530_nl.h
@@ -0,0 +1,49 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2018 MediaTek Inc.
+ * Author: Sirui Zhao <Sirui.Zhao@mediatek.com>
+ */
+
+#ifndef _MT753x_NL_H_
+#define _MT753x_NL_H_
+
+#define MT7530_DSA_GENL_NAME "mt753x_dsa"
+#define MT7530_GENL_VERSION		0x1
+
+enum mt7530_cmd {
+	MT7530_CMD_UNSPEC = 0,
+	MT7530_CMD_REQUEST,
+	MT7530_CMD_REPLY,
+	MT7530_CMD_READ,
+	MT7530_CMD_WRITE,
+
+	__MT7530_CMD_MAX,
+};
+
+enum mt7530_attr {
+	MT7530_ATTR_TYPE_UNSPEC = 0,
+	MT7530_ATTR_TYPE_MESG,
+	MT7530_ATTR_TYPE_PHY,
+	MT7530_ATTR_TYPE_DEVAD,
+	MT7530_ATTR_TYPE_REG,
+	MT7530_ATTR_TYPE_VAL,
+	MT7530_ATTR_TYPE_DEV_NAME,
+	MT7530_ATTR_TYPE_DEV_ID,
+
+	__MT7530_ATTR_TYPE_MAX,
+};
+
+#define MT7530_NR_ATTR_TYPE		(__MT7530_ATTR_TYPE_MAX - 1)
+
+struct mt7530_info {
+	struct mii_bus	*bus;
+	void __iomem *base;
+	int direct_access;
+};
+
+#ifdef __KERNEL__
+int  mt7530_nl_init(struct mt7530_priv **priv);
+void mt7530_nl_exit(void);
+#endif /* __KERNEL__ */
+
+#endif /* _MT7530_NL_H_ */
-- 
2.34.1