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 }; 34 35 static struct ncsi_dev_priv *ndp_from_ifindex(struct net *net, u32 ifindex) 36 { 37 struct ncsi_dev_priv *ndp; 38 struct net_device *dev; 39 struct ncsi_dev *nd; 40 struct ncsi_dev; 41 42 if (!net) 43 return NULL; 44 45 dev = dev_get_by_index(net, ifindex); 46 if (!dev) { 47 pr_err("NCSI netlink: No device for ifindex %u\n", ifindex); 48 return NULL; 49 } 50 51 nd = ncsi_find_dev(dev); 52 ndp = nd ? TO_NCSI_DEV_PRIV(nd) : NULL; 53 54 dev_put(dev); 55 return ndp; 56 } 57 58 static int ncsi_write_channel_info(struct sk_buff *skb, 59 struct ncsi_dev_priv *ndp, 60 struct ncsi_channel *nc) 61 { 62 struct ncsi_channel_vlan_filter *ncf; 63 struct ncsi_channel_mode *m; 64 struct nlattr *vid_nest; 65 int i; 66 67 nla_put_u32(skb, NCSI_CHANNEL_ATTR_ID, nc->id); 68 m = &nc->modes[NCSI_MODE_LINK]; 69 nla_put_u32(skb, NCSI_CHANNEL_ATTR_LINK_STATE, m->data[2]); 70 if (nc->state == NCSI_CHANNEL_ACTIVE) 71 nla_put_flag(skb, NCSI_CHANNEL_ATTR_ACTIVE); 72 if (ndp->force_channel == nc) 73 nla_put_flag(skb, NCSI_CHANNEL_ATTR_FORCED); 74 75 nla_put_u32(skb, NCSI_CHANNEL_ATTR_VERSION_MAJOR, nc->version.version); 76 nla_put_u32(skb, NCSI_CHANNEL_ATTR_VERSION_MINOR, nc->version.alpha2); 77 nla_put_string(skb, NCSI_CHANNEL_ATTR_VERSION_STR, nc->version.fw_name); 78 79 vid_nest = nla_nest_start(skb, NCSI_CHANNEL_ATTR_VLAN_LIST); 80 if (!vid_nest) 81 return -ENOMEM; 82 ncf = &nc->vlan_filter; 83 i = -1; 84 while ((i = find_next_bit((void *)&ncf->bitmap, ncf->n_vids, 85 i + 1)) < ncf->n_vids) { 86 if (ncf->vids[i]) 87 nla_put_u16(skb, NCSI_CHANNEL_ATTR_VLAN_ID, 88 ncf->vids[i]); 89 } 90 nla_nest_end(skb, vid_nest); 91 92 return 0; 93 } 94 95 static int ncsi_write_package_info(struct sk_buff *skb, 96 struct ncsi_dev_priv *ndp, unsigned int id) 97 { 98 struct nlattr *pnest, *cnest, *nest; 99 struct ncsi_package *np; 100 struct ncsi_channel *nc; 101 bool found; 102 int rc; 103 104 if (id > ndp->package_num - 1) { 105 netdev_info(ndp->ndev.dev, "NCSI: No package with id %u\n", id); 106 return -ENODEV; 107 } 108 109 found = false; 110 NCSI_FOR_EACH_PACKAGE(ndp, np) { 111 if (np->id != id) 112 continue; 113 pnest = nla_nest_start(skb, NCSI_PKG_ATTR); 114 if (!pnest) 115 return -ENOMEM; 116 nla_put_u32(skb, NCSI_PKG_ATTR_ID, np->id); 117 if (ndp->force_package == np) 118 nla_put_flag(skb, NCSI_PKG_ATTR_FORCED); 119 cnest = nla_nest_start(skb, NCSI_PKG_ATTR_CHANNEL_LIST); 120 if (!cnest) { 121 nla_nest_cancel(skb, pnest); 122 return -ENOMEM; 123 } 124 NCSI_FOR_EACH_CHANNEL(np, nc) { 125 nest = nla_nest_start(skb, NCSI_CHANNEL_ATTR); 126 if (!nest) { 127 nla_nest_cancel(skb, cnest); 128 nla_nest_cancel(skb, pnest); 129 return -ENOMEM; 130 } 131 rc = ncsi_write_channel_info(skb, ndp, nc); 132 if (rc) { 133 nla_nest_cancel(skb, nest); 134 nla_nest_cancel(skb, cnest); 135 nla_nest_cancel(skb, pnest); 136 return rc; 137 } 138 nla_nest_end(skb, nest); 139 } 140 nla_nest_end(skb, cnest); 141 nla_nest_end(skb, pnest); 142 found = true; 143 } 144 145 if (!found) 146 return -ENODEV; 147 148 return 0; 149 } 150 151 static int ncsi_pkg_info_nl(struct sk_buff *msg, struct genl_info *info) 152 { 153 struct ncsi_dev_priv *ndp; 154 unsigned int package_id; 155 struct sk_buff *skb; 156 struct nlattr *attr; 157 void *hdr; 158 int rc; 159 160 if (!info || !info->attrs) 161 return -EINVAL; 162 163 if (!info->attrs[NCSI_ATTR_IFINDEX]) 164 return -EINVAL; 165 166 if (!info->attrs[NCSI_ATTR_PACKAGE_ID]) 167 return -EINVAL; 168 169 ndp = ndp_from_ifindex(genl_info_net(info), 170 nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX])); 171 if (!ndp) 172 return -ENODEV; 173 174 skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL); 175 if (!skb) 176 return -ENOMEM; 177 178 hdr = genlmsg_put(skb, info->snd_portid, info->snd_seq, 179 &ncsi_genl_family, 0, NCSI_CMD_PKG_INFO); 180 if (!hdr) { 181 kfree_skb(skb); 182 return -EMSGSIZE; 183 } 184 185 package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]); 186 187 attr = nla_nest_start(skb, NCSI_ATTR_PACKAGE_LIST); 188 if (!attr) { 189 kfree_skb(skb); 190 return -EMSGSIZE; 191 } 192 rc = ncsi_write_package_info(skb, ndp, package_id); 193 194 if (rc) { 195 nla_nest_cancel(skb, attr); 196 goto err; 197 } 198 199 nla_nest_end(skb, attr); 200 201 genlmsg_end(skb, hdr); 202 return genlmsg_reply(skb, info); 203 204 err: 205 kfree_skb(skb); 206 return rc; 207 } 208 209 static int ncsi_pkg_info_all_nl(struct sk_buff *skb, 210 struct netlink_callback *cb) 211 { 212 struct nlattr *attrs[NCSI_ATTR_MAX + 1]; 213 struct ncsi_package *np, *package; 214 struct ncsi_dev_priv *ndp; 215 unsigned int package_id; 216 struct nlattr *attr; 217 void *hdr; 218 int rc; 219 220 rc = genlmsg_parse(cb->nlh, &ncsi_genl_family, attrs, NCSI_ATTR_MAX, 221 ncsi_genl_policy, NULL); 222 if (rc) 223 return rc; 224 225 if (!attrs[NCSI_ATTR_IFINDEX]) 226 return -EINVAL; 227 228 ndp = ndp_from_ifindex(get_net(sock_net(skb->sk)), 229 nla_get_u32(attrs[NCSI_ATTR_IFINDEX])); 230 231 if (!ndp) 232 return -ENODEV; 233 234 package_id = cb->args[0]; 235 package = NULL; 236 NCSI_FOR_EACH_PACKAGE(ndp, np) 237 if (np->id == package_id) 238 package = np; 239 240 if (!package) 241 return 0; /* done */ 242 243 hdr = genlmsg_put(skb, NETLINK_CB(cb->skb).portid, cb->nlh->nlmsg_seq, 244 &ncsi_genl_family, NLM_F_MULTI, NCSI_CMD_PKG_INFO); 245 if (!hdr) { 246 rc = -EMSGSIZE; 247 goto err; 248 } 249 250 attr = nla_nest_start(skb, NCSI_ATTR_PACKAGE_LIST); 251 rc = ncsi_write_package_info(skb, ndp, package->id); 252 if (rc) { 253 nla_nest_cancel(skb, attr); 254 goto err; 255 } 256 257 nla_nest_end(skb, attr); 258 genlmsg_end(skb, hdr); 259 260 cb->args[0] = package_id + 1; 261 262 return skb->len; 263 err: 264 genlmsg_cancel(skb, hdr); 265 return rc; 266 } 267 268 static int ncsi_set_interface_nl(struct sk_buff *msg, struct genl_info *info) 269 { 270 struct ncsi_package *np, *package; 271 struct ncsi_channel *nc, *channel; 272 u32 package_id, channel_id; 273 struct ncsi_dev_priv *ndp; 274 unsigned long flags; 275 276 if (!info || !info->attrs) 277 return -EINVAL; 278 279 if (!info->attrs[NCSI_ATTR_IFINDEX]) 280 return -EINVAL; 281 282 if (!info->attrs[NCSI_ATTR_PACKAGE_ID]) 283 return -EINVAL; 284 285 ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)), 286 nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX])); 287 if (!ndp) 288 return -ENODEV; 289 290 package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]); 291 package = NULL; 292 293 spin_lock_irqsave(&ndp->lock, flags); 294 295 NCSI_FOR_EACH_PACKAGE(ndp, np) 296 if (np->id == package_id) 297 package = np; 298 if (!package) { 299 /* The user has set a package that does not exist */ 300 spin_unlock_irqrestore(&ndp->lock, flags); 301 return -ERANGE; 302 } 303 304 channel = NULL; 305 if (!info->attrs[NCSI_ATTR_CHANNEL_ID]) { 306 /* Allow any channel */ 307 channel_id = NCSI_RESERVED_CHANNEL; 308 } else { 309 channel_id = nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_ID]); 310 NCSI_FOR_EACH_CHANNEL(package, nc) 311 if (nc->id == channel_id) 312 channel = nc; 313 } 314 315 if (channel_id != NCSI_RESERVED_CHANNEL && !channel) { 316 /* The user has set a channel that does not exist on this 317 * package 318 */ 319 spin_unlock_irqrestore(&ndp->lock, flags); 320 netdev_info(ndp->ndev.dev, "NCSI: Channel %u does not exist!\n", 321 channel_id); 322 return -ERANGE; 323 } 324 325 ndp->force_package = package; 326 ndp->force_channel = channel; 327 spin_unlock_irqrestore(&ndp->lock, flags); 328 329 netdev_info(ndp->ndev.dev, "Set package 0x%x, channel 0x%x%s as preferred\n", 330 package_id, channel_id, 331 channel_id == NCSI_RESERVED_CHANNEL ? " (any)" : ""); 332 333 /* Bounce the NCSI channel to set changes */ 334 ncsi_stop_dev(&ndp->ndev); 335 ncsi_start_dev(&ndp->ndev); 336 337 return 0; 338 } 339 340 static int ncsi_clear_interface_nl(struct sk_buff *msg, struct genl_info *info) 341 { 342 struct ncsi_dev_priv *ndp; 343 unsigned long flags; 344 345 if (!info || !info->attrs) 346 return -EINVAL; 347 348 if (!info->attrs[NCSI_ATTR_IFINDEX]) 349 return -EINVAL; 350 351 ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)), 352 nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX])); 353 if (!ndp) 354 return -ENODEV; 355 356 /* Clear any override */ 357 spin_lock_irqsave(&ndp->lock, flags); 358 ndp->force_package = NULL; 359 ndp->force_channel = NULL; 360 spin_unlock_irqrestore(&ndp->lock, flags); 361 netdev_info(ndp->ndev.dev, "NCSI: Cleared preferred package/channel\n"); 362 363 /* Bounce the NCSI channel to set changes */ 364 ncsi_stop_dev(&ndp->ndev); 365 ncsi_start_dev(&ndp->ndev); 366 367 return 0; 368 } 369 370 static int ncsi_send_cmd_nl(struct sk_buff *msg, struct genl_info *info) 371 { 372 struct ncsi_dev_priv *ndp; 373 struct ncsi_pkt_hdr *hdr; 374 struct ncsi_cmd_arg nca; 375 unsigned char *data; 376 u32 package_id; 377 u32 channel_id; 378 int len, ret; 379 380 if (!info || !info->attrs) { 381 ret = -EINVAL; 382 goto out; 383 } 384 385 if (!info->attrs[NCSI_ATTR_IFINDEX]) { 386 ret = -EINVAL; 387 goto out; 388 } 389 390 if (!info->attrs[NCSI_ATTR_PACKAGE_ID]) { 391 ret = -EINVAL; 392 goto out; 393 } 394 395 if (!info->attrs[NCSI_ATTR_CHANNEL_ID]) { 396 ret = -EINVAL; 397 goto out; 398 } 399 400 if (!info->attrs[NCSI_ATTR_DATA]) { 401 ret = -EINVAL; 402 goto out; 403 } 404 405 ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)), 406 nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX])); 407 if (!ndp) { 408 ret = -ENODEV; 409 goto out; 410 } 411 412 package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]); 413 channel_id = nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_ID]); 414 415 if (package_id >= NCSI_MAX_PACKAGE || channel_id >= NCSI_MAX_CHANNEL) { 416 ret = -ERANGE; 417 goto out_netlink; 418 } 419 420 len = nla_len(info->attrs[NCSI_ATTR_DATA]); 421 if (len < sizeof(struct ncsi_pkt_hdr)) { 422 netdev_info(ndp->ndev.dev, "NCSI: no command to send %u\n", 423 package_id); 424 ret = -EINVAL; 425 goto out_netlink; 426 } else { 427 data = (unsigned char *)nla_data(info->attrs[NCSI_ATTR_DATA]); 428 } 429 430 hdr = (struct ncsi_pkt_hdr *)data; 431 432 nca.ndp = ndp; 433 nca.package = (unsigned char)package_id; 434 nca.channel = (unsigned char)channel_id; 435 nca.type = hdr->type; 436 nca.req_flags = NCSI_REQ_FLAG_NETLINK_DRIVEN; 437 nca.info = info; 438 nca.payload = ntohs(hdr->length); 439 nca.data = data + sizeof(*hdr); 440 441 ret = ncsi_xmit_cmd(&nca); 442 out_netlink: 443 if (ret != 0) { 444 netdev_err(ndp->ndev.dev, 445 "NCSI: Error %d sending command\n", 446 ret); 447 ncsi_send_netlink_err(ndp->ndev.dev, 448 info->snd_seq, 449 info->snd_portid, 450 info->nlhdr, 451 ret); 452 } 453 out: 454 return ret; 455 } 456 457 int ncsi_send_netlink_rsp(struct ncsi_request *nr, 458 struct ncsi_package *np, 459 struct ncsi_channel *nc) 460 { 461 struct sk_buff *skb; 462 struct net *net; 463 void *hdr; 464 int rc; 465 466 net = dev_net(nr->rsp->dev); 467 468 skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC); 469 if (!skb) 470 return -ENOMEM; 471 472 hdr = genlmsg_put(skb, nr->snd_portid, nr->snd_seq, 473 &ncsi_genl_family, 0, NCSI_CMD_SEND_CMD); 474 if (!hdr) { 475 kfree_skb(skb); 476 return -EMSGSIZE; 477 } 478 479 nla_put_u32(skb, NCSI_ATTR_IFINDEX, nr->rsp->dev->ifindex); 480 if (np) 481 nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID, np->id); 482 if (nc) 483 nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, nc->id); 484 else 485 nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, NCSI_RESERVED_CHANNEL); 486 487 rc = nla_put(skb, NCSI_ATTR_DATA, nr->rsp->len, (void *)nr->rsp->data); 488 if (rc) 489 goto err; 490 491 genlmsg_end(skb, hdr); 492 return genlmsg_unicast(net, skb, nr->snd_portid); 493 494 err: 495 kfree_skb(skb); 496 return rc; 497 } 498 499 int ncsi_send_netlink_timeout(struct ncsi_request *nr, 500 struct ncsi_package *np, 501 struct ncsi_channel *nc) 502 { 503 struct sk_buff *skb; 504 struct net *net; 505 void *hdr; 506 507 skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC); 508 if (!skb) 509 return -ENOMEM; 510 511 hdr = genlmsg_put(skb, nr->snd_portid, nr->snd_seq, 512 &ncsi_genl_family, 0, NCSI_CMD_SEND_CMD); 513 if (!hdr) { 514 kfree_skb(skb); 515 return -EMSGSIZE; 516 } 517 518 net = dev_net(nr->cmd->dev); 519 520 nla_put_u32(skb, NCSI_ATTR_IFINDEX, nr->cmd->dev->ifindex); 521 522 if (np) 523 nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID, np->id); 524 else 525 nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID, 526 NCSI_PACKAGE_INDEX((((struct ncsi_pkt_hdr *) 527 nr->cmd->data)->channel))); 528 529 if (nc) 530 nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, nc->id); 531 else 532 nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, NCSI_RESERVED_CHANNEL); 533 534 genlmsg_end(skb, hdr); 535 return genlmsg_unicast(net, skb, nr->snd_portid); 536 } 537 538 int ncsi_send_netlink_err(struct net_device *dev, 539 u32 snd_seq, 540 u32 snd_portid, 541 struct nlmsghdr *nlhdr, 542 int err) 543 { 544 struct nlmsghdr *nlh; 545 struct nlmsgerr *nle; 546 struct sk_buff *skb; 547 struct net *net; 548 549 skb = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC); 550 if (!skb) 551 return -ENOMEM; 552 553 net = dev_net(dev); 554 555 nlh = nlmsg_put(skb, snd_portid, snd_seq, 556 NLMSG_ERROR, sizeof(*nle), 0); 557 nle = (struct nlmsgerr *)nlmsg_data(nlh); 558 nle->error = err; 559 memcpy(&nle->msg, nlhdr, sizeof(*nlh)); 560 561 nlmsg_end(skb, nlh); 562 563 return nlmsg_unicast(net->genl_sock, skb, snd_portid); 564 } 565 566 static const struct genl_ops ncsi_ops[] = { 567 { 568 .cmd = NCSI_CMD_PKG_INFO, 569 .policy = ncsi_genl_policy, 570 .doit = ncsi_pkg_info_nl, 571 .dumpit = ncsi_pkg_info_all_nl, 572 .flags = 0, 573 }, 574 { 575 .cmd = NCSI_CMD_SET_INTERFACE, 576 .policy = ncsi_genl_policy, 577 .doit = ncsi_set_interface_nl, 578 .flags = GENL_ADMIN_PERM, 579 }, 580 { 581 .cmd = NCSI_CMD_CLEAR_INTERFACE, 582 .policy = ncsi_genl_policy, 583 .doit = ncsi_clear_interface_nl, 584 .flags = GENL_ADMIN_PERM, 585 }, 586 { 587 .cmd = NCSI_CMD_SEND_CMD, 588 .policy = ncsi_genl_policy, 589 .doit = ncsi_send_cmd_nl, 590 .flags = GENL_ADMIN_PERM, 591 }, 592 }; 593 594 static struct genl_family ncsi_genl_family __ro_after_init = { 595 .name = "NCSI", 596 .version = 0, 597 .maxattr = NCSI_ATTR_MAX, 598 .module = THIS_MODULE, 599 .ops = ncsi_ops, 600 .n_ops = ARRAY_SIZE(ncsi_ops), 601 }; 602 603 int ncsi_init_netlink(struct net_device *dev) 604 { 605 int rc; 606 607 rc = genl_register_family(&ncsi_genl_family); 608 if (rc) 609 netdev_err(dev, "ncsi: failed to register netlink family\n"); 610 611 return rc; 612 } 613 614 int ncsi_unregister_netlink(struct net_device *dev) 615 { 616 int rc; 617 618 rc = genl_unregister_family(&ncsi_genl_family); 619 if (rc) 620 netdev_err(dev, "ncsi: failed to unregister netlink family\n"); 621 622 return rc; 623 } 624