1 files changed, 386 insertions, 0 deletions
diff --git a/target/linux/mediatek/files-4.19/drivers/net/phy/mtk/mt753x/mt753x_nl.c b/target/linux/mediatek/files-4.19/drivers/net/phy/mtk/mt753x/mt753x_nl.c
new file mode 100644
index 0000000000..756df4e21f
--- /dev/null
+++ b/target/linux/mediatek/files-4.19/drivers/net/phy/mtk/mt753x/mt753x_nl.c
@@ -0,0 +1,386 @@
+/*
+ * Configuration layer for MediaTek MT753x gigabit switch
+ *
+ * Copyright (C) 2018 MediaTek Inc. All Rights Reserved.
+ *
+ * Author: Sirui Zhao <Sirui.Zhao@mediatek.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <net/genetlink.h>
+
+#include "mt753x.h"
+#include "mt753x_nl.h"
+
+#define MT753X_NL_CMD_REQ_ATTRS(attr)		\
+	.required_attrs = attr,			\
+	.nr_required_attrs = ARRAY_SIZE(attr),
+
+struct mt753x_nl_cmd_item {
+	enum mt753x_cmd cmd;
+	bool require_dev;
+	int (*process)(struct genl_info *info, struct gsw_mt753x *gsw);
+	u32 nr_required_attrs;
+	const enum mt753x_attr *required_attrs;
+};
+
+static int mt753x_nl_response(struct sk_buff *skb, struct genl_info *info);
+
+static const struct nla_policy mt753x_nl_cmd_policy[] = {
+	[MT753X_ATTR_TYPE_MESG] = { .type = NLA_STRING },
+	[MT753X_ATTR_TYPE_PHY] = { .type = NLA_S32 },
+	[MT753X_ATTR_TYPE_REG] = { .type = NLA_S32 },
+	[MT753X_ATTR_TYPE_VAL] = { .type = NLA_S32 },
+	[MT753X_ATTR_TYPE_DEV_NAME] = { .type = NLA_S32 },
+	[MT753X_ATTR_TYPE_DEV_ID] = { .type = NLA_S32 },
+	[MT753X_ATTR_TYPE_DEVAD] = { .type = NLA_S32 },
+};
+
+static const struct genl_ops mt753x_nl_ops[] = {
+	{
+		.cmd = MT753X_CMD_REQUEST,
+		.doit = mt753x_nl_response,
+		.policy = mt753x_nl_cmd_policy,
+		.flags = GENL_ADMIN_PERM,
+	}, {
+		.cmd = MT753X_CMD_READ,
+		.doit = mt753x_nl_response,
+		.policy = mt753x_nl_cmd_policy,
+		.flags = GENL_ADMIN_PERM,
+	}, {
+		.cmd = MT753X_CMD_WRITE,
+		.doit = mt753x_nl_response,
+		.policy = mt753x_nl_cmd_policy,
+		.flags = GENL_ADMIN_PERM,
+	},
+};
+
+static struct genl_family mt753x_nl_family = {
+//	.id =		GENL_ID_GENERATE,
+	.name =		MT753X_GENL_NAME,
+	.version =	MT753X_GENL_VERSION,
+	.maxattr =	MT753X_NR_ATTR_TYPE,
+	.ops =		mt753x_nl_ops,
+	.n_ops =	ARRAY_SIZE(mt753x_nl_ops),
+};
+
+static int mt753x_nl_list_devs(char *buff, int size)
+{
+	struct gsw_mt753x *gsw;
+	int len, total = 0;
+	char buf[80];
+
+	memset(buff, 0, size);
+
+	mt753x_lock_gsw();
+
+	list_for_each_entry(gsw, &mt753x_devs, list) {
+		len = snprintf(buf, sizeof(buf),
+			       "id: %d, model: %s, node: %s\n",
+			       gsw->id, gsw->name, gsw->dev->of_node->name);
+		strncat(buff, buf, size - total);
+		total += len;
+	}
+
+	mt753x_put_gsw();
+
+	return total;
+}
+
+static int mt753x_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,
+			    &mt753x_nl_family, 0, cmd);
+	if (!reply) {
+		nlmsg_free(msg);
+		return -EINVAL;
+	}
+
+	*skbp = msg;
+	return 0;
+}
+
+static int mt753x_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 mt753x_nl_get_s32(struct genl_info *info, enum mt753x_attr attr,
+			     s32 defval)
+{
+	struct nlattr *na;
+
+	na = info->attrs[attr];
+	if (na)
+		return nla_get_s32(na);
+
+	return defval;
+}
+
+static int mt753x_nl_get_u32(struct genl_info *info, enum mt753x_attr attr,
+			     u32 *val)
+{
+	struct nlattr *na;
+
+	na = info->attrs[attr];
+	if (na) {
+		*val = nla_get_u32(na);
+		return 0;
+	}
+
+	return -1;
+}
+
+static struct gsw_mt753x *mt753x_nl_parse_find_gsw(struct genl_info *info)
+{
+	struct gsw_mt753x *gsw;
+	struct nlattr *na;
+	int gsw_id;
+
+	na = info->attrs[MT753X_ATTR_TYPE_DEV_ID];
+	if (na) {
+		gsw_id = nla_get_s32(na);
+		if (gsw_id >= 0)
+			gsw = mt753x_get_gsw(gsw_id);
+		else
+			gsw = mt753x_get_first_gsw();
+	} else {
+		gsw = mt753x_get_first_gsw();
+	}
+
+	return gsw;
+}
+
+static int mt753x_nl_get_swdevs(struct genl_info *info, struct gsw_mt753x *gsw)
+{
+	struct sk_buff *rep_skb = NULL;
+	char dev_info[512];
+	int ret;
+
+	ret = mt753x_nl_list_devs(dev_info, sizeof(dev_info));
+	if (!ret) {
+		pr_info("No switch registered\n");
+		return -EINVAL;
+	}
+
+	ret = mt753x_nl_prepare_reply(info, MT753X_CMD_REPLY, &rep_skb);
+	if (ret < 0)
+		goto err;
+
+	ret = nla_put_string(rep_skb, MT753X_ATTR_TYPE_MESG, dev_info);
+	if (ret < 0)
+		goto err;
+
+	return mt753x_nl_send_reply(rep_skb, info);
+
+err:
+	if (rep_skb)
+		nlmsg_free(rep_skb);
+
+	return ret;
+}
+
+static int mt753x_nl_reply_read(struct genl_info *info, struct gsw_mt753x *gsw)
+{
+	struct sk_buff *rep_skb = NULL;
+	s32 phy, devad, reg;
+	int ret, value;
+
+	phy = mt753x_nl_get_s32(info, MT753X_ATTR_TYPE_PHY, -1);
+	devad = mt753x_nl_get_s32(info, MT753X_ATTR_TYPE_DEVAD, -1);
+	reg = mt753x_nl_get_s32(info, MT753X_ATTR_TYPE_REG, -1);
+
+	if (reg < 0)
+		goto err;
+
+	ret = mt753x_nl_prepare_reply(info, MT753X_CMD_READ, &rep_skb);
+	if (ret < 0)
+		goto err;
+
+	if (phy >= 0) {
+		if (devad < 0)
+			value = gsw->mii_read(gsw, phy, reg);
+		else
+			value = gsw->mmd_read(gsw, phy, devad, reg);
+	} else {
+		value = mt753x_reg_read(gsw, reg);
+	}
+
+	ret = nla_put_s32(rep_skb, MT753X_ATTR_TYPE_REG, reg);
+	if (ret < 0)
+		goto err;
+
+	ret = nla_put_s32(rep_skb, MT753X_ATTR_TYPE_VAL, value);
+	if (ret < 0)
+		goto err;
+
+	return mt753x_nl_send_reply(rep_skb, info);
+
+err:
+	if (rep_skb)
+		nlmsg_free(rep_skb);
+
+	return ret;
+}
+
+static int mt753x_nl_reply_write(struct genl_info *info, struct gsw_mt753x *gsw)
+{
+	struct sk_buff *rep_skb = NULL;
+	s32 phy, devad, reg;
+	u32 value;
+	int ret;
+
+	phy = mt753x_nl_get_s32(info, MT753X_ATTR_TYPE_PHY, -1);
+	devad = mt753x_nl_get_s32(info, MT753X_ATTR_TYPE_DEVAD, -1);
+	reg = mt753x_nl_get_s32(info, MT753X_ATTR_TYPE_REG, -1);
+
+	if (mt753x_nl_get_u32(info, MT753X_ATTR_TYPE_VAL, &value))
+		goto err;
+
+	if (reg < 0)
+		goto err;
+
+	ret = mt753x_nl_prepare_reply(info, MT753X_CMD_WRITE, &rep_skb);
+	if (ret < 0)
+		goto err;
+
+	if (phy >= 0) {
+		if (devad < 0)
+			gsw->mii_write(gsw, phy, reg, value);
+		else
+			gsw->mmd_write(gsw, phy, devad, reg, value);
+	} else {
+		mt753x_reg_write(gsw, reg, value);
+	}
+
+	ret = nla_put_s32(rep_skb, MT753X_ATTR_TYPE_REG, reg);
+	if (ret < 0)
+		goto err;
+
+	ret = nla_put_s32(rep_skb, MT753X_ATTR_TYPE_VAL, value);
+	if (ret < 0)
+		goto err;
+
+	return mt753x_nl_send_reply(rep_skb, info);
+
+err:
+	if (rep_skb)
+		nlmsg_free(rep_skb);
+
+	return ret;
+}
+
+static const enum mt753x_attr mt753x_nl_cmd_read_attrs[] = {
+	MT753X_ATTR_TYPE_REG
+};
+
+static const enum mt753x_attr mt753x_nl_cmd_write_attrs[] = {
+	MT753X_ATTR_TYPE_REG,
+	MT753X_ATTR_TYPE_VAL
+};
+
+static const struct mt753x_nl_cmd_item mt753x_nl_cmds[] = {
+	{
+		.cmd = MT753X_CMD_REQUEST,
+		.require_dev = false,
+		.process = mt753x_nl_get_swdevs
+	}, {
+		.cmd = MT753X_CMD_READ,
+		.require_dev = true,
+		.process = mt753x_nl_reply_read,
+		MT753X_NL_CMD_REQ_ATTRS(mt753x_nl_cmd_read_attrs)
+	}, {
+		.cmd = MT753X_CMD_WRITE,
+		.require_dev = true,
+		.process = mt753x_nl_reply_write,
+		MT753X_NL_CMD_REQ_ATTRS(mt753x_nl_cmd_write_attrs)
+	}
+};
+
+static int mt753x_nl_response(struct sk_buff *skb, struct genl_info *info)
+{
+	struct genlmsghdr *hdr = nlmsg_data(info->nlhdr);
+	const struct mt753x_nl_cmd_item *cmditem = NULL;
+	struct gsw_mt753x *gsw = NULL;
+	u32 sat_req_attrs = 0;
+	int i, ret;
+
+	for (i = 0; i < ARRAY_SIZE(mt753x_nl_cmds); i++) {
+		if (hdr->cmd == mt753x_nl_cmds[i].cmd) {
+			cmditem = &mt753x_nl_cmds[i];
+			break;
+		}
+	}
+
+	if (!cmditem) {
+		pr_info("mt753x-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("mt753x-nl: missing required attr(s) for cmd %u\n",
+			hdr->cmd);
+		return -EINVAL;
+	}
+
+	if (cmditem->require_dev) {
+		gsw = mt753x_nl_parse_find_gsw(info);
+		if (!gsw) {
+			pr_info("mt753x-nl: failed to find switch dev\n");
+			return -EINVAL;
+		}
+	}
+
+	ret = cmditem->process(info, gsw);
+
+	mt753x_put_gsw();
+
+	return ret;
+}
+
+int __init mt753x_nl_init(void)
+{
+	int ret;
+
+	ret = genl_register_family(&mt753x_nl_family);
+	if (ret) {
+		pr_info("mt753x-nl: genl_register_family_with_ops failed\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+void __exit mt753x_nl_exit(void)
+{
+	genl_unregister_family(&mt753x_nl_family);
+}