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