From 3e75ca66195dec023ca2e837ff748c317fd7ac26 Mon Sep 17 00:00:00 2001 From: Sam Shih 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 #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 + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#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 + */ + +#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