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

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