target/linux/mediatek/files-4.19/drivers/net/phy/mtk/mt753x/mt753x_nl.c

   1 /*
   2  * Configuration layer for MediaTek MT753x gigabit switch
   3  *
   4  * Copyright (C) 2018 MediaTek Inc. All Rights Reserved.
   5  *
   6  * Author: Sirui Zhao <Sirui.Zhao@mediatek.com>
   7  *
   8  * SPDX-License-Identifier:     GPL-2.0+
   9  */
  10
  11 #include <linux/types.h>
  12 #include <linux/kernel.h>
  13 #include <linux/module.h>
  14 #include <linux/init.h>
  15 #include <net/genetlink.h>
  16
  17 #include "mt753x.h"
  18 #include "mt753x_nl.h"
  19
  20 #define MT753X_NL_CMD_REQ_ATTRS(attr)           \
  21         .required_attrs = attr,                 \
  22         .nr_required_attrs = ARRAY_SIZE(attr),
  23
  24 struct mt753x_nl_cmd_item {
  25         enum mt753x_cmd cmd;
  26         bool require_dev;
  27         int (*process)(struct genl_info *info, struct gsw_mt753x *gsw);
  28         u32 nr_required_attrs;
  29         const enum mt753x_attr *required_attrs;
  30 };
  31
  32 static int mt753x_nl_response(struct sk_buff *skb, struct genl_info *info);
  33
  34 static const struct nla_policy mt753x_nl_cmd_policy[] = {
  35         [MT753X_ATTR_TYPE_MESG] = { .type = NLA_STRING },
  36         [MT753X_ATTR_TYPE_PHY] = { .type = NLA_S32 },
  37         [MT753X_ATTR_TYPE_REG] = { .type = NLA_S32 },
  38         [MT753X_ATTR_TYPE_VAL] = { .type = NLA_S32 },
  39         [MT753X_ATTR_TYPE_DEV_NAME] = { .type = NLA_S32 },
  40         [MT753X_ATTR_TYPE_DEV_ID] = { .type = NLA_S32 },
  41         [MT753X_ATTR_TYPE_DEVAD] = { .type = NLA_S32 },
  42 };
  43
  44 static const struct genl_ops mt753x_nl_ops[] = {
  45         {
  46                 .cmd = MT753X_CMD_REQUEST,
  47                 .doit = mt753x_nl_response,
  48                 .policy = mt753x_nl_cmd_policy,
  49                 .flags = GENL_ADMIN_PERM,
  50         }, {
  51                 .cmd = MT753X_CMD_READ,
  52                 .doit = mt753x_nl_response,
  53                 .policy = mt753x_nl_cmd_policy,
  54                 .flags = GENL_ADMIN_PERM,
  55         }, {
  56                 .cmd = MT753X_CMD_WRITE,
  57                 .doit = mt753x_nl_response,
  58                 .policy = mt753x_nl_cmd_policy,
  59                 .flags = GENL_ADMIN_PERM,
  60         },
  61 };
  62
  63 static struct genl_family mt753x_nl_family = {
  64 //      .id =           GENL_ID_GENERATE,
  65         .name =         MT753X_GENL_NAME,
  66         .version =      MT753X_GENL_VERSION,
  67         .maxattr =      MT753X_NR_ATTR_TYPE,
  68         .ops =          mt753x_nl_ops,
  69         .n_ops =        ARRAY_SIZE(mt753x_nl_ops),
  70 };
  71
  72 static int mt753x_nl_list_devs(char *buff, int size)
  73 {
  74         struct gsw_mt753x *gsw;
  75         int len, total = 0;
  76         char buf[80];
  77
  78         memset(buff, 0, size);
  79
  80         mt753x_lock_gsw();
  81
  82         list_for_each_entry(gsw, &mt753x_devs, list) {
  83                 len = snprintf(buf, sizeof(buf),
  84                                "id: %d, model: %s, node: %s\n",
  85                                gsw->id, gsw->name, gsw->dev->of_node->name);
  86                 strncat(buff, buf, size - total);
  87                 total += len;
  88         }
  89
  90         mt753x_put_gsw();
  91
  92         return total;
  93 }
  94
  95 static int mt753x_nl_prepare_reply(struct genl_info *info, u8 cmd,
  96                                    struct sk_buff **skbp)
  97 {
  98         struct sk_buff *msg;
  99         void *reply;
 100
 101         if (!info)
 102                 return -EINVAL;
 103
 104         msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
 105         if (!msg)
 106                 return -ENOMEM;
 107
 108         /* Construct send-back message header */
 109         reply = genlmsg_put(msg, info->snd_portid, info->snd_seq,
 110                             &mt753x_nl_family, 0, cmd);
 111         if (!reply) {
 112                 nlmsg_free(msg);
 113                 return -EINVAL;
 114         }
 115
 116         *skbp = msg;
 117         return 0;
 118 }
 119
 120 static int mt753x_nl_send_reply(struct sk_buff *skb, struct genl_info *info)
 121 {
 122         struct genlmsghdr *genlhdr = nlmsg_data(nlmsg_hdr(skb));
 123         void *reply = genlmsg_data(genlhdr);
 124
 125         /* Finalize a generic netlink message (update message header) */
 126         genlmsg_end(skb, reply);
 127
 128         /* reply to a request */
 129         return genlmsg_reply(skb, info);
 130 }
 131
 132 static s32 mt753x_nl_get_s32(struct genl_info *info, enum mt753x_attr attr,
 133                              s32 defval)
 134 {
 135         struct nlattr *na;
 136
 137         na = info->attrs[attr];
 138         if (na)
 139                 return nla_get_s32(na);
 140
 141         return defval;
 142 }
 143
 144 static int mt753x_nl_get_u32(struct genl_info *info, enum mt753x_attr attr,
 145                              u32 *val)
 146 {
 147         struct nlattr *na;
 148
 149         na = info->attrs[attr];
 150         if (na) {
 151                 *val = nla_get_u32(na);
 152                 return 0;
 153         }
 154
 155         return -1;
 156 }
 157
 158 static struct gsw_mt753x *mt753x_nl_parse_find_gsw(struct genl_info *info)
 159 {
 160         struct gsw_mt753x *gsw;
 161         struct nlattr *na;
 162         int gsw_id;
 163
 164         na = info->attrs[MT753X_ATTR_TYPE_DEV_ID];
 165         if (na) {
 166                 gsw_id = nla_get_s32(na);
 167                 if (gsw_id >= 0)
 168                         gsw = mt753x_get_gsw(gsw_id);
 169                 else
 170                         gsw = mt753x_get_first_gsw();
 171         } else {
 172                 gsw = mt753x_get_first_gsw();
 173         }
 174
 175         return gsw;
 176 }
 177
 178 static int mt753x_nl_get_swdevs(struct genl_info *info, struct gsw_mt753x *gsw)
 179 {
 180         struct sk_buff *rep_skb = NULL;
 181         char dev_info[512];
 182         int ret;
 183
 184         ret = mt753x_nl_list_devs(dev_info, sizeof(dev_info));
 185         if (!ret) {
 186                 pr_info("No switch registered\n");
 187                 return -EINVAL;
 188         }
 189
 190         ret = mt753x_nl_prepare_reply(info, MT753X_CMD_REPLY, &rep_skb);
 191         if (ret < 0)
 192                 goto err;
 193
 194         ret = nla_put_string(rep_skb, MT753X_ATTR_TYPE_MESG, dev_info);
 195         if (ret < 0)
 196                 goto err;
 197
 198         return mt753x_nl_send_reply(rep_skb, info);
 199
 200 err:
 201         if (rep_skb)
 202                 nlmsg_free(rep_skb);
 203
 204         return ret;
 205 }
 206
 207 static int mt753x_nl_reply_read(struct genl_info *info, struct gsw_mt753x *gsw)
 208 {
 209         struct sk_buff *rep_skb = NULL;
 210         s32 phy, devad, reg;
 211         int ret, value;
 212
 213         phy = mt753x_nl_get_s32(info, MT753X_ATTR_TYPE_PHY, -1);
 214         devad = mt753x_nl_get_s32(info, MT753X_ATTR_TYPE_DEVAD, -1);
 215         reg = mt753x_nl_get_s32(info, MT753X_ATTR_TYPE_REG, -1);
 216
 217         if (reg < 0)
 218                 goto err;
 219
 220         ret = mt753x_nl_prepare_reply(info, MT753X_CMD_READ, &rep_skb);
 221         if (ret < 0)
 222                 goto err;
 223
 224         if (phy >= 0) {
 225                 if (devad < 0)
 226                         value = gsw->mii_read(gsw, phy, reg);
 227                 else
 228                         value = gsw->mmd_read(gsw, phy, devad, reg);
 229         } else {
 230                 value = mt753x_reg_read(gsw, reg);
 231         }
 232
 233         ret = nla_put_s32(rep_skb, MT753X_ATTR_TYPE_REG, reg);
 234         if (ret < 0)
 235                 goto err;
 236
 237         ret = nla_put_s32(rep_skb, MT753X_ATTR_TYPE_VAL, value);
 238         if (ret < 0)
 239                 goto err;
 240
 241         return mt753x_nl_send_reply(rep_skb, info);
 242
 243 err:
 244         if (rep_skb)
 245                 nlmsg_free(rep_skb);
 246
 247         return ret;
 248 }
 249
 250 static int mt753x_nl_reply_write(struct genl_info *info, struct gsw_mt753x *gsw)
 251 {
 252         struct sk_buff *rep_skb = NULL;
 253         s32 phy, devad, reg;
 254         u32 value;
 255         int ret;
 256
 257         phy = mt753x_nl_get_s32(info, MT753X_ATTR_TYPE_PHY, -1);
 258         devad = mt753x_nl_get_s32(info, MT753X_ATTR_TYPE_DEVAD, -1);
 259         reg = mt753x_nl_get_s32(info, MT753X_ATTR_TYPE_REG, -1);
 260
 261         if (mt753x_nl_get_u32(info, MT753X_ATTR_TYPE_VAL, &value))
 262                 goto err;
 263
 264         if (reg < 0)
 265                 goto err;
 266
 267         ret = mt753x_nl_prepare_reply(info, MT753X_CMD_WRITE, &rep_skb);
 268         if (ret < 0)
 269                 goto err;
 270
 271         if (phy >= 0) {
 272                 if (devad < 0)
 273                         gsw->mii_write(gsw, phy, reg, value);
 274                 else
 275                         gsw->mmd_write(gsw, phy, devad, reg, value);
 276         } else {
 277                 mt753x_reg_write(gsw, reg, value);
 278         }
 279
 280         ret = nla_put_s32(rep_skb, MT753X_ATTR_TYPE_REG, reg);
 281         if (ret < 0)
 282                 goto err;
 283
 284         ret = nla_put_s32(rep_skb, MT753X_ATTR_TYPE_VAL, value);
 285         if (ret < 0)
 286                 goto err;
 287
 288         return mt753x_nl_send_reply(rep_skb, info);
 289
 290 err:
 291         if (rep_skb)
 292                 nlmsg_free(rep_skb);
 293
 294         return ret;
 295 }
 296
 297 static const enum mt753x_attr mt753x_nl_cmd_read_attrs[] = {
 298         MT753X_ATTR_TYPE_REG
 299 };
 300
 301 static const enum mt753x_attr mt753x_nl_cmd_write_attrs[] = {
 302         MT753X_ATTR_TYPE_REG,
 303         MT753X_ATTR_TYPE_VAL
 304 };
 305
 306 static const struct mt753x_nl_cmd_item mt753x_nl_cmds[] = {
 307         {
 308                 .cmd = MT753X_CMD_REQUEST,
 309                 .require_dev = false,
 310                 .process = mt753x_nl_get_swdevs
 311         }, {
 312                 .cmd = MT753X_CMD_READ,
 313                 .require_dev = true,
 314                 .process = mt753x_nl_reply_read,
 315                 MT753X_NL_CMD_REQ_ATTRS(mt753x_nl_cmd_read_attrs)
 316         }, {
 317                 .cmd = MT753X_CMD_WRITE,
 318                 .require_dev = true,
 319                 .process = mt753x_nl_reply_write,
 320                 MT753X_NL_CMD_REQ_ATTRS(mt753x_nl_cmd_write_attrs)
 321         }
 322 };
 323
 324 static int mt753x_nl_response(struct sk_buff *skb, struct genl_info *info)
 325 {
 326         struct genlmsghdr *hdr = nlmsg_data(info->nlhdr);
 327         const struct mt753x_nl_cmd_item *cmditem = NULL;
 328         struct gsw_mt753x *gsw = NULL;
 329         u32 sat_req_attrs = 0;
 330         int i, ret;
 331
 332         for (i = 0; i < ARRAY_SIZE(mt753x_nl_cmds); i++) {
 333                 if (hdr->cmd == mt753x_nl_cmds[i].cmd) {
 334                         cmditem = &mt753x_nl_cmds[i];
 335                         break;
 336                 }
 337         }
 338
 339         if (!cmditem) {
 340                 pr_info("mt753x-nl: unknown cmd %u\n", hdr->cmd);
 341                 return -EINVAL;
 342         }
 343
 344         for (i = 0; i < cmditem->nr_required_attrs; i++) {
 345                 if (info->attrs[cmditem->required_attrs[i]])
 346                         sat_req_attrs++;
 347         }
 348
 349         if (sat_req_attrs != cmditem->nr_required_attrs) {
 350                 pr_info("mt753x-nl: missing required attr(s) for cmd %u\n",
 351                         hdr->cmd);
 352                 return -EINVAL;
 353         }
 354
 355         if (cmditem->require_dev) {
 356                 gsw = mt753x_nl_parse_find_gsw(info);
 357                 if (!gsw) {
 358                         pr_info("mt753x-nl: failed to find switch dev\n");
 359                         return -EINVAL;
 360                 }
 361         }
 362
 363         ret = cmditem->process(info, gsw);
 364
 365         mt753x_put_gsw();
 366
 367         return ret;
 368 }
 369
 370 int __init mt753x_nl_init(void)
 371 {
 372         int ret;
 373
 374         ret = genl_register_family(&mt753x_nl_family);
 375         if (ret) {
 376                 pr_info("mt753x-nl: genl_register_family_with_ops failed\n");
 377                 return ret;
 378         }
 379
 380         return 0;
 381 }
 382
 383 void __exit mt753x_nl_exit(void)
 384 {
 385         genl_unregister_family(&mt753x_nl_family);
 386 }