diff options
Diffstat (limited to 'target/linux/mediatek/files-4.19/drivers/net/phy/mtk/mt753x/mt753x_nl.c')
-rw-r--r-- | target/linux/mediatek/files-4.19/drivers/net/phy/mtk/mt753x/mt753x_nl.c | 386 |
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); +} |