net/ncsi/ncsi-netlink.c

   1 /*
   2  * Copyright Samuel Mendoza-Jonas, IBM Corporation 2018.
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; either version 2 of the License, or
   7  * (at your option) any later version.
   8  */
   9
  10 #include <linux/module.h>
  11 #include <linux/kernel.h>
  12 #include <linux/if_arp.h>
  13 #include <linux/rtnetlink.h>
  14 #include <linux/etherdevice.h>
  15 #include <net/genetlink.h>
  16 #include <net/ncsi.h>
  17 #include <linux/skbuff.h>
  18 #include <net/sock.h>
  19 #include <uapi/linux/ncsi.h>
  20
  21 #include "internal.h"
  22 #include "ncsi-pkt.h"
  23 #include "ncsi-netlink.h"
  24
  25 static struct genl_family ncsi_genl_family;
  26
  27 static const struct nla_policy ncsi_genl_policy[NCSI_ATTR_MAX + 1] = {
  28         [NCSI_ATTR_IFINDEX] =           { .type = NLA_U32 },
  29         [NCSI_ATTR_PACKAGE_LIST] =      { .type = NLA_NESTED },
  30         [NCSI_ATTR_PACKAGE_ID] =        { .type = NLA_U32 },
  31         [NCSI_ATTR_CHANNEL_ID] =        { .type = NLA_U32 },
  32         [NCSI_ATTR_DATA] =              { .type = NLA_BINARY, .len = 2048 },
  33         [NCSI_ATTR_MULTI_FLAG] =        { .type = NLA_FLAG },
  34         [NCSI_ATTR_PACKAGE_MASK] =      { .type = NLA_U32 },
  35         [NCSI_ATTR_CHANNEL_MASK] =      { .type = NLA_U32 },
  36 };
  37
  38 static struct ncsi_dev_priv *ndp_from_ifindex(struct net *net, u32 ifindex)
  39 {
  40         struct ncsi_dev_priv *ndp;
  41         struct net_device *dev;
  42         struct ncsi_dev *nd;
  43         struct ncsi_dev;
  44
  45         if (!net)
  46                 return NULL;
  47
  48         dev = dev_get_by_index(net, ifindex);
  49         if (!dev) {
  50                 pr_err("NCSI netlink: No device for ifindex %u\n", ifindex);
  51                 return NULL;
  52         }
  53
  54         nd = ncsi_find_dev(dev);
  55         ndp = nd ? TO_NCSI_DEV_PRIV(nd) : NULL;
  56
  57         dev_put(dev);
  58         return ndp;
  59 }
  60
  61 static int ncsi_write_channel_info(struct sk_buff *skb,
  62                                    struct ncsi_dev_priv *ndp,
  63                                    struct ncsi_channel *nc)
  64 {
  65         struct ncsi_channel_vlan_filter *ncf;
  66         struct ncsi_channel_mode *m;
  67         struct nlattr *vid_nest;
  68         int i;
  69
  70         nla_put_u32(skb, NCSI_CHANNEL_ATTR_ID, nc->id);
  71         m = &nc->modes[NCSI_MODE_LINK];
  72         nla_put_u32(skb, NCSI_CHANNEL_ATTR_LINK_STATE, m->data[2]);
  73         if (nc->state == NCSI_CHANNEL_ACTIVE)
  74                 nla_put_flag(skb, NCSI_CHANNEL_ATTR_ACTIVE);
  75         if (nc == nc->package->preferred_channel)
  76                 nla_put_flag(skb, NCSI_CHANNEL_ATTR_FORCED);
  77
  78         nla_put_u32(skb, NCSI_CHANNEL_ATTR_VERSION_MAJOR, nc->version.version);
  79         nla_put_u32(skb, NCSI_CHANNEL_ATTR_VERSION_MINOR, nc->version.alpha2);
  80         nla_put_string(skb, NCSI_CHANNEL_ATTR_VERSION_STR, nc->version.fw_name);
  81
  82         vid_nest = nla_nest_start(skb, NCSI_CHANNEL_ATTR_VLAN_LIST);
  83         if (!vid_nest)
  84                 return -ENOMEM;
  85         ncf = &nc->vlan_filter;
  86         i = -1;
  87         while ((i = find_next_bit((void *)&ncf->bitmap, ncf->n_vids,
  88                                   i + 1)) < ncf->n_vids) {
  89                 if (ncf->vids[i])
  90                         nla_put_u16(skb, NCSI_CHANNEL_ATTR_VLAN_ID,
  91                                     ncf->vids[i]);
  92         }
  93         nla_nest_end(skb, vid_nest);
  94
  95         return 0;
  96 }
  97
  98 static int ncsi_write_package_info(struct sk_buff *skb,
  99                                    struct ncsi_dev_priv *ndp, unsigned int id)
 100 {
 101         struct nlattr *pnest, *cnest, *nest;
 102         struct ncsi_package *np;
 103         struct ncsi_channel *nc;
 104         bool found;
 105         int rc;
 106
 107         if (id > ndp->package_num - 1) {
 108                 netdev_info(ndp->ndev.dev, "NCSI: No package with id %u\n", id);
 109                 return -ENODEV;
 110         }
 111
 112         found = false;
 113         NCSI_FOR_EACH_PACKAGE(ndp, np) {
 114                 if (np->id != id)
 115                         continue;
 116                 pnest = nla_nest_start(skb, NCSI_PKG_ATTR);
 117                 if (!pnest)
 118                         return -ENOMEM;
 119                 nla_put_u32(skb, NCSI_PKG_ATTR_ID, np->id);
 120                 if ((0x1 << np->id) == ndp->package_whitelist)
 121                         nla_put_flag(skb, NCSI_PKG_ATTR_FORCED);
 122                 cnest = nla_nest_start(skb, NCSI_PKG_ATTR_CHANNEL_LIST);
 123                 if (!cnest) {
 124                         nla_nest_cancel(skb, pnest);
 125                         return -ENOMEM;
 126                 }
 127                 NCSI_FOR_EACH_CHANNEL(np, nc) {
 128                         nest = nla_nest_start(skb, NCSI_CHANNEL_ATTR);
 129                         if (!nest) {
 130                                 nla_nest_cancel(skb, cnest);
 131                                 nla_nest_cancel(skb, pnest);
 132                                 return -ENOMEM;
 133                         }
 134                         rc = ncsi_write_channel_info(skb, ndp, nc);
 135                         if (rc) {
 136                                 nla_nest_cancel(skb, nest);
 137                                 nla_nest_cancel(skb, cnest);
 138                                 nla_nest_cancel(skb, pnest);
 139                                 return rc;
 140                         }
 141                         nla_nest_end(skb, nest);
 142                 }
 143                 nla_nest_end(skb, cnest);
 144                 nla_nest_end(skb, pnest);
 145                 found = true;
 146         }
 147
 148         if (!found)
 149                 return -ENODEV;
 150
 151         return 0;
 152 }
 153
 154 static int ncsi_pkg_info_nl(struct sk_buff *msg, struct genl_info *info)
 155 {
 156         struct ncsi_dev_priv *ndp;
 157         unsigned int package_id;
 158         struct sk_buff *skb;
 159         struct nlattr *attr;
 160         void *hdr;
 161         int rc;
 162
 163         if (!info || !info->attrs)
 164                 return -EINVAL;
 165
 166         if (!info->attrs[NCSI_ATTR_IFINDEX])
 167                 return -EINVAL;
 168
 169         if (!info->attrs[NCSI_ATTR_PACKAGE_ID])
 170                 return -EINVAL;
 171
 172         ndp = ndp_from_ifindex(genl_info_net(info),
 173                                nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
 174         if (!ndp)
 175                 return -ENODEV;
 176
 177         skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
 178         if (!skb)
 179                 return -ENOMEM;
 180
 181         hdr = genlmsg_put(skb, info->snd_portid, info->snd_seq,
 182                           &ncsi_genl_family, 0, NCSI_CMD_PKG_INFO);
 183         if (!hdr) {
 184                 kfree_skb(skb);
 185                 return -EMSGSIZE;
 186         }
 187
 188         package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]);
 189
 190         attr = nla_nest_start(skb, NCSI_ATTR_PACKAGE_LIST);
 191         if (!attr) {
 192                 kfree_skb(skb);
 193                 return -EMSGSIZE;
 194         }
 195         rc = ncsi_write_package_info(skb, ndp, package_id);
 196
 197         if (rc) {
 198                 nla_nest_cancel(skb, attr);
 199                 goto err;
 200         }
 201
 202         nla_nest_end(skb, attr);
 203
 204         genlmsg_end(skb, hdr);
 205         return genlmsg_reply(skb, info);
 206
 207 err:
 208         kfree_skb(skb);
 209         return rc;
 210 }
 211
 212 static int ncsi_pkg_info_all_nl(struct sk_buff *skb,
 213                                 struct netlink_callback *cb)
 214 {
 215         struct nlattr *attrs[NCSI_ATTR_MAX + 1];
 216         struct ncsi_package *np, *package;
 217         struct ncsi_dev_priv *ndp;
 218         unsigned int package_id;
 219         struct nlattr *attr;
 220         void *hdr;
 221         int rc;
 222
 223         rc = genlmsg_parse(cb->nlh, &ncsi_genl_family, attrs, NCSI_ATTR_MAX,
 224                            ncsi_genl_policy, NULL);
 225         if (rc)
 226                 return rc;
 227
 228         if (!attrs[NCSI_ATTR_IFINDEX])
 229                 return -EINVAL;
 230
 231         ndp = ndp_from_ifindex(get_net(sock_net(skb->sk)),
 232                                nla_get_u32(attrs[NCSI_ATTR_IFINDEX]));
 233
 234         if (!ndp)
 235                 return -ENODEV;
 236
 237         package_id = cb->args[0];
 238         package = NULL;
 239         NCSI_FOR_EACH_PACKAGE(ndp, np)
 240                 if (np->id == package_id)
 241                         package = np;
 242
 243         if (!package)
 244                 return 0; /* done */
 245
 246         hdr = genlmsg_put(skb, NETLINK_CB(cb->skb).portid, cb->nlh->nlmsg_seq,
 247                           &ncsi_genl_family, NLM_F_MULTI,  NCSI_CMD_PKG_INFO);
 248         if (!hdr) {
 249                 rc = -EMSGSIZE;
 250                 goto err;
 251         }
 252
 253         attr = nla_nest_start(skb, NCSI_ATTR_PACKAGE_LIST);
 254         rc = ncsi_write_package_info(skb, ndp, package->id);
 255         if (rc) {
 256                 nla_nest_cancel(skb, attr);
 257                 goto err;
 258         }
 259
 260         nla_nest_end(skb, attr);
 261         genlmsg_end(skb, hdr);
 262
 263         cb->args[0] = package_id + 1;
 264
 265         return skb->len;
 266 err:
 267         genlmsg_cancel(skb, hdr);
 268         return rc;
 269 }
 270
 271 static int ncsi_set_interface_nl(struct sk_buff *msg, struct genl_info *info)
 272 {
 273         struct ncsi_package *np, *package;
 274         struct ncsi_channel *nc, *channel;
 275         u32 package_id, channel_id;
 276         struct ncsi_dev_priv *ndp;
 277         unsigned long flags;
 278
 279         if (!info || !info->attrs)
 280                 return -EINVAL;
 281
 282         if (!info->attrs[NCSI_ATTR_IFINDEX])
 283                 return -EINVAL;
 284
 285         if (!info->attrs[NCSI_ATTR_PACKAGE_ID])
 286                 return -EINVAL;
 287
 288         ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
 289                                nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
 290         if (!ndp)
 291                 return -ENODEV;
 292
 293         package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]);
 294         package = NULL;
 295
 296         NCSI_FOR_EACH_PACKAGE(ndp, np)
 297                 if (np->id == package_id)
 298                         package = np;
 299         if (!package) {
 300                 /* The user has set a package that does not exist */
 301                 return -ERANGE;
 302         }
 303
 304         channel = NULL;
 305         if (info->attrs[NCSI_ATTR_CHANNEL_ID]) {
 306                 channel_id = nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_ID]);
 307                 NCSI_FOR_EACH_CHANNEL(package, nc)
 308                         if (nc->id == channel_id) {
 309                                 channel = nc;
 310                                 break;
 311                         }
 312                 if (!channel) {
 313                         netdev_info(ndp->ndev.dev,
 314                                     "NCSI: Channel %u does not exist!\n",
 315                                     channel_id);
 316                         return -ERANGE;
 317                 }
 318         }
 319
 320         spin_lock_irqsave(&ndp->lock, flags);
 321         ndp->package_whitelist = 0x1 << package->id;
 322         ndp->multi_package = false;
 323         spin_unlock_irqrestore(&ndp->lock, flags);
 324
 325         spin_lock_irqsave(&package->lock, flags);
 326         package->multi_channel = false;
 327         if (channel) {
 328                 package->channel_whitelist = 0x1 << channel->id;
 329                 package->preferred_channel = channel;
 330         } else {
 331                 /* Allow any channel */
 332                 package->channel_whitelist = UINT_MAX;
 333                 package->preferred_channel = NULL;
 334         }
 335         spin_unlock_irqrestore(&package->lock, flags);
 336
 337         if (channel)
 338                 netdev_info(ndp->ndev.dev,
 339                             "Set package 0x%x, channel 0x%x as preferred\n",
 340                             package_id, channel_id);
 341         else
 342                 netdev_info(ndp->ndev.dev, "Set package 0x%x as preferred\n",
 343                             package_id);
 344
 345         /* Update channel configuration */
 346         if (!(ndp->flags & NCSI_DEV_RESET))
 347                 ncsi_reset_dev(&ndp->ndev);
 348
 349         return 0;
 350 }
 351
 352 static int ncsi_clear_interface_nl(struct sk_buff *msg, struct genl_info *info)
 353 {
 354         struct ncsi_dev_priv *ndp;
 355         struct ncsi_package *np;
 356         unsigned long flags;
 357
 358         if (!info || !info->attrs)
 359                 return -EINVAL;
 360
 361         if (!info->attrs[NCSI_ATTR_IFINDEX])
 362                 return -EINVAL;
 363
 364         ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
 365                                nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
 366         if (!ndp)
 367                 return -ENODEV;
 368
 369         /* Reset any whitelists and disable multi mode */
 370         spin_lock_irqsave(&ndp->lock, flags);
 371         ndp->package_whitelist = UINT_MAX;
 372         ndp->multi_package = false;
 373         spin_unlock_irqrestore(&ndp->lock, flags);
 374
 375         NCSI_FOR_EACH_PACKAGE(ndp, np) {
 376                 spin_lock_irqsave(&np->lock, flags);
 377                 np->multi_channel = false;
 378                 np->channel_whitelist = UINT_MAX;
 379                 np->preferred_channel = NULL;
 380                 spin_unlock_irqrestore(&np->lock, flags);
 381         }
 382         netdev_info(ndp->ndev.dev, "NCSI: Cleared preferred package/channel\n");
 383
 384         /* Update channel configuration */
 385         if (!(ndp->flags & NCSI_DEV_RESET))
 386                 ncsi_reset_dev(&ndp->ndev);
 387
 388         return 0;
 389 }
 390
 391 static int ncsi_send_cmd_nl(struct sk_buff *msg, struct genl_info *info)
 392 {
 393         struct ncsi_dev_priv *ndp;
 394         struct ncsi_pkt_hdr *hdr;
 395         struct ncsi_cmd_arg nca;
 396         unsigned char *data;
 397         u32 package_id;
 398         u32 channel_id;
 399         int len, ret;
 400
 401         if (!info || !info->attrs) {
 402                 ret = -EINVAL;
 403                 goto out;
 404         }
 405
 406         if (!info->attrs[NCSI_ATTR_IFINDEX]) {
 407                 ret = -EINVAL;
 408                 goto out;
 409         }
 410
 411         if (!info->attrs[NCSI_ATTR_PACKAGE_ID]) {
 412                 ret = -EINVAL;
 413                 goto out;
 414         }
 415
 416         if (!info->attrs[NCSI_ATTR_CHANNEL_ID]) {
 417                 ret = -EINVAL;
 418                 goto out;
 419         }
 420
 421         if (!info->attrs[NCSI_ATTR_DATA]) {
 422                 ret = -EINVAL;
 423                 goto out;
 424         }
 425
 426         ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
 427                                nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
 428         if (!ndp) {
 429                 ret = -ENODEV;
 430                 goto out;
 431         }
 432
 433         package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]);
 434         channel_id = nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_ID]);
 435
 436         if (package_id >= NCSI_MAX_PACKAGE || channel_id >= NCSI_MAX_CHANNEL) {
 437                 ret = -ERANGE;
 438                 goto out_netlink;
 439         }
 440
 441         len = nla_len(info->attrs[NCSI_ATTR_DATA]);
 442         if (len < sizeof(struct ncsi_pkt_hdr)) {
 443                 netdev_info(ndp->ndev.dev, "NCSI: no command to send %u\n",
 444                             package_id);
 445                 ret = -EINVAL;
 446                 goto out_netlink;
 447         } else {
 448                 data = (unsigned char *)nla_data(info->attrs[NCSI_ATTR_DATA]);
 449         }
 450
 451         hdr = (struct ncsi_pkt_hdr *)data;
 452
 453         nca.ndp = ndp;
 454         nca.package = (unsigned char)package_id;
 455         nca.channel = (unsigned char)channel_id;
 456         nca.type = hdr->type;
 457         nca.req_flags = NCSI_REQ_FLAG_NETLINK_DRIVEN;
 458         nca.info = info;
 459         nca.payload = ntohs(hdr->length);
 460         nca.data = data + sizeof(*hdr);
 461
 462         ret = ncsi_xmit_cmd(&nca);
 463 out_netlink:
 464         if (ret != 0) {
 465                 netdev_err(ndp->ndev.dev,
 466                            "NCSI: Error %d sending command\n",
 467                            ret);
 468                 ncsi_send_netlink_err(ndp->ndev.dev,
 469                                       info->snd_seq,
 470                                       info->snd_portid,
 471                                       info->nlhdr,
 472                                       ret);
 473         }
 474 out:
 475         return ret;
 476 }
 477
 478 int ncsi_send_netlink_rsp(struct ncsi_request *nr,
 479                           struct ncsi_package *np,
 480                           struct ncsi_channel *nc)
 481 {
 482         struct sk_buff *skb;
 483         struct net *net;
 484         void *hdr;
 485         int rc;
 486
 487         net = dev_net(nr->rsp->dev);
 488
 489         skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
 490         if (!skb)
 491                 return -ENOMEM;
 492
 493         hdr = genlmsg_put(skb, nr->snd_portid, nr->snd_seq,
 494                           &ncsi_genl_family, 0, NCSI_CMD_SEND_CMD);
 495         if (!hdr) {
 496                 kfree_skb(skb);
 497                 return -EMSGSIZE;
 498         }
 499
 500         nla_put_u32(skb, NCSI_ATTR_IFINDEX, nr->rsp->dev->ifindex);
 501         if (np)
 502                 nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID, np->id);
 503         if (nc)
 504                 nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, nc->id);
 505         else
 506                 nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, NCSI_RESERVED_CHANNEL);
 507
 508         rc = nla_put(skb, NCSI_ATTR_DATA, nr->rsp->len, (void *)nr->rsp->data);
 509         if (rc)
 510                 goto err;
 511
 512         genlmsg_end(skb, hdr);
 513         return genlmsg_unicast(net, skb, nr->snd_portid);
 514
 515 err:
 516         kfree_skb(skb);
 517         return rc;
 518 }
 519
 520 int ncsi_send_netlink_timeout(struct ncsi_request *nr,
 521                               struct ncsi_package *np,
 522                               struct ncsi_channel *nc)
 523 {
 524         struct sk_buff *skb;
 525         struct net *net;
 526         void *hdr;
 527
 528         skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
 529         if (!skb)
 530                 return -ENOMEM;
 531
 532         hdr = genlmsg_put(skb, nr->snd_portid, nr->snd_seq,
 533                           &ncsi_genl_family, 0, NCSI_CMD_SEND_CMD);
 534         if (!hdr) {
 535                 kfree_skb(skb);
 536                 return -EMSGSIZE;
 537         }
 538
 539         net = dev_net(nr->cmd->dev);
 540
 541         nla_put_u32(skb, NCSI_ATTR_IFINDEX, nr->cmd->dev->ifindex);
 542
 543         if (np)
 544                 nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID, np->id);
 545         else
 546                 nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID,
 547                             NCSI_PACKAGE_INDEX((((struct ncsi_pkt_hdr *)
 548                                                  nr->cmd->data)->channel)));
 549
 550         if (nc)
 551                 nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, nc->id);
 552         else
 553                 nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, NCSI_RESERVED_CHANNEL);
 554
 555         genlmsg_end(skb, hdr);
 556         return genlmsg_unicast(net, skb, nr->snd_portid);
 557 }
 558
 559 int ncsi_send_netlink_err(struct net_device *dev,
 560                           u32 snd_seq,
 561                           u32 snd_portid,
 562                           struct nlmsghdr *nlhdr,
 563                           int err)
 564 {
 565         struct nlmsghdr *nlh;
 566         struct nlmsgerr *nle;
 567         struct sk_buff *skb;
 568         struct net *net;
 569
 570         skb = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
 571         if (!skb)
 572                 return -ENOMEM;
 573
 574         net = dev_net(dev);
 575
 576         nlh = nlmsg_put(skb, snd_portid, snd_seq,
 577                         NLMSG_ERROR, sizeof(*nle), 0);
 578         nle = (struct nlmsgerr *)nlmsg_data(nlh);
 579         nle->error = err;
 580         memcpy(&nle->msg, nlhdr, sizeof(*nlh));
 581
 582         nlmsg_end(skb, nlh);
 583
 584         return nlmsg_unicast(net->genl_sock, skb, snd_portid);
 585 }
 586
 587 static int ncsi_set_package_mask_nl(struct sk_buff *msg,
 588                                     struct genl_info *info)
 589 {
 590         struct ncsi_dev_priv *ndp;
 591         unsigned long flags;
 592         int rc;
 593
 594         if (!info || !info->attrs)
 595                 return -EINVAL;
 596
 597         if (!info->attrs[NCSI_ATTR_IFINDEX])
 598                 return -EINVAL;
 599
 600         if (!info->attrs[NCSI_ATTR_PACKAGE_MASK])
 601                 return -EINVAL;
 602
 603         ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
 604                                nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
 605         if (!ndp)
 606                 return -ENODEV;
 607
 608         spin_lock_irqsave(&ndp->lock, flags);
 609         if (nla_get_flag(info->attrs[NCSI_ATTR_MULTI_FLAG])) {
 610                 if (ndp->flags & NCSI_DEV_HWA) {
 611                         ndp->multi_package = true;
 612                         rc = 0;
 613                 } else {
 614                         netdev_err(ndp->ndev.dev,
 615                                    "NCSI: Can't use multiple packages without HWA\n");
 616                         rc = -EPERM;
 617                 }
 618         } else {
 619                 ndp->multi_package = false;
 620                 rc = 0;
 621         }
 622
 623         if (!rc)
 624                 ndp->package_whitelist =
 625                         nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_MASK]);
 626         spin_unlock_irqrestore(&ndp->lock, flags);
 627
 628         if (!rc) {
 629                 /* Update channel configuration */
 630                 if (!(ndp->flags & NCSI_DEV_RESET))
 631                         ncsi_reset_dev(&ndp->ndev);
 632         }
 633
 634         return rc;
 635 }
 636
 637 static int ncsi_set_channel_mask_nl(struct sk_buff *msg,
 638                                     struct genl_info *info)
 639 {
 640         struct ncsi_package *np, *package;
 641         struct ncsi_channel *nc, *channel;
 642         u32 package_id, channel_id;
 643         struct ncsi_dev_priv *ndp;
 644         unsigned long flags;
 645
 646         if (!info || !info->attrs)
 647                 return -EINVAL;
 648
 649         if (!info->attrs[NCSI_ATTR_IFINDEX])
 650                 return -EINVAL;
 651
 652         if (!info->attrs[NCSI_ATTR_PACKAGE_ID])
 653                 return -EINVAL;
 654
 655         if (!info->attrs[NCSI_ATTR_CHANNEL_MASK])
 656                 return -EINVAL;
 657
 658         ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
 659                                nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
 660         if (!ndp)
 661                 return -ENODEV;
 662
 663         package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]);
 664         package = NULL;
 665         NCSI_FOR_EACH_PACKAGE(ndp, np)
 666                 if (np->id == package_id) {
 667                         package = np;
 668                         break;
 669                 }
 670         if (!package)
 671                 return -ERANGE;
 672
 673         spin_lock_irqsave(&package->lock, flags);
 674
 675         channel = NULL;
 676         if (info->attrs[NCSI_ATTR_CHANNEL_ID]) {
 677                 channel_id = nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_ID]);
 678                 NCSI_FOR_EACH_CHANNEL(np, nc)
 679                         if (nc->id == channel_id) {
 680                                 channel = nc;
 681                                 break;
 682                         }
 683                 if (!channel) {
 684                         spin_unlock_irqrestore(&package->lock, flags);
 685                         return -ERANGE;
 686                 }
 687                 netdev_dbg(ndp->ndev.dev,
 688                            "NCSI: Channel %u set as preferred channel\n",
 689                            channel->id);
 690         }
 691
 692         package->channel_whitelist =
 693                 nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_MASK]);
 694         if (package->channel_whitelist == 0)
 695                 netdev_dbg(ndp->ndev.dev,
 696                            "NCSI: Package %u set to all channels disabled\n",
 697                            package->id);
 698
 699         package->preferred_channel = channel;
 700
 701         if (nla_get_flag(info->attrs[NCSI_ATTR_MULTI_FLAG])) {
 702                 package->multi_channel = true;
 703                 netdev_info(ndp->ndev.dev,
 704                             "NCSI: Multi-channel enabled on package %u\n",
 705                             package_id);
 706         } else {
 707                 package->multi_channel = false;
 708         }
 709
 710         spin_unlock_irqrestore(&package->lock, flags);
 711
 712         /* Update channel configuration */
 713         if (!(ndp->flags & NCSI_DEV_RESET))
 714                 ncsi_reset_dev(&ndp->ndev);
 715
 716         return 0;
 717 }
 718
 719 static const struct genl_ops ncsi_ops[] = {
 720         {
 721                 .cmd = NCSI_CMD_PKG_INFO,
 722                 .policy = ncsi_genl_policy,
 723                 .doit = ncsi_pkg_info_nl,
 724                 .dumpit = ncsi_pkg_info_all_nl,
 725                 .flags = 0,
 726         },
 727         {
 728                 .cmd = NCSI_CMD_SET_INTERFACE,
 729                 .policy = ncsi_genl_policy,
 730                 .doit = ncsi_set_interface_nl,
 731                 .flags = GENL_ADMIN_PERM,
 732         },
 733         {
 734                 .cmd = NCSI_CMD_CLEAR_INTERFACE,
 735                 .policy = ncsi_genl_policy,
 736                 .doit = ncsi_clear_interface_nl,
 737                 .flags = GENL_ADMIN_PERM,
 738         },
 739         {
 740                 .cmd = NCSI_CMD_SEND_CMD,
 741                 .policy = ncsi_genl_policy,
 742                 .doit = ncsi_send_cmd_nl,
 743                 .flags = GENL_ADMIN_PERM,
 744         },
 745         {
 746                 .cmd = NCSI_CMD_SET_PACKAGE_MASK,
 747                 .policy = ncsi_genl_policy,
 748                 .doit = ncsi_set_package_mask_nl,
 749                 .flags = GENL_ADMIN_PERM,
 750         },
 751         {
 752                 .cmd = NCSI_CMD_SET_CHANNEL_MASK,
 753                 .policy = ncsi_genl_policy,
 754                 .doit = ncsi_set_channel_mask_nl,
 755                 .flags = GENL_ADMIN_PERM,
 756         },
 757 };
 758
 759 static struct genl_family ncsi_genl_family __ro_after_init = {
 760         .name = "NCSI",
 761         .version = 0,
 762         .maxattr = NCSI_ATTR_MAX,
 763         .module = THIS_MODULE,
 764         .ops = ncsi_ops,
 765         .n_ops = ARRAY_SIZE(ncsi_ops),
 766 };
 767
 768 int ncsi_init_netlink(struct net_device *dev)
 769 {
 770         int rc;
 771
 772         rc = genl_register_family(&ncsi_genl_family);
 773         if (rc)
 774                 netdev_err(dev, "ncsi: failed to register netlink family\n");
 775
 776         return rc;
 777 }
 778
 779 int ncsi_unregister_netlink(struct net_device *dev)
 780 {
 781         int rc;
 782
 783         rc = genl_unregister_family(&ncsi_genl_family);
 784         if (rc)
 785                 netdev_err(dev, "ncsi: failed to unregister netlink family\n");
 786
 787         return rc;
 788 }