2 * Copyright Samuel Mendoza-Jonas, IBM Corporation 2018.
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.
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>
17 #include <linux/skbuff.h>
19 #include <uapi/linux/ncsi.h>
23 #include "ncsi-netlink.h"
25 static struct genl_family ncsi_genl_family;
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 },
38 static struct ncsi_dev_priv *ndp_from_ifindex(struct net *net, u32 ifindex)
40 struct ncsi_dev_priv *ndp;
41 struct net_device *dev;
48 dev = dev_get_by_index(net, ifindex);
50 pr_err("NCSI netlink: No device for ifindex %u\n", ifindex);
54 nd = ncsi_find_dev(dev);
55 ndp = nd ? TO_NCSI_DEV_PRIV(nd) : NULL;
61 static int ncsi_write_channel_info(struct sk_buff *skb,
62 struct ncsi_dev_priv *ndp,
63 struct ncsi_channel *nc)
65 struct ncsi_channel_vlan_filter *ncf;
66 struct ncsi_channel_mode *m;
67 struct nlattr *vid_nest;
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);
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);
82 vid_nest = nla_nest_start(skb, NCSI_CHANNEL_ATTR_VLAN_LIST);
85 ncf = &nc->vlan_filter;
87 while ((i = find_next_bit((void *)&ncf->bitmap, ncf->n_vids,
88 i + 1)) < ncf->n_vids) {
90 nla_put_u16(skb, NCSI_CHANNEL_ATTR_VLAN_ID,
93 nla_nest_end(skb, vid_nest);
98 static int ncsi_write_package_info(struct sk_buff *skb,
99 struct ncsi_dev_priv *ndp, unsigned int id)
101 struct nlattr *pnest, *cnest, *nest;
102 struct ncsi_package *np;
103 struct ncsi_channel *nc;
107 if (id > ndp->package_num - 1) {
108 netdev_info(ndp->ndev.dev, "NCSI: No package with id %u\n", id);
113 NCSI_FOR_EACH_PACKAGE(ndp, np) {
116 pnest = nla_nest_start(skb, NCSI_PKG_ATTR);
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);
124 nla_nest_cancel(skb, pnest);
127 NCSI_FOR_EACH_CHANNEL(np, nc) {
128 nest = nla_nest_start(skb, NCSI_CHANNEL_ATTR);
130 nla_nest_cancel(skb, cnest);
131 nla_nest_cancel(skb, pnest);
134 rc = ncsi_write_channel_info(skb, ndp, nc);
136 nla_nest_cancel(skb, nest);
137 nla_nest_cancel(skb, cnest);
138 nla_nest_cancel(skb, pnest);
141 nla_nest_end(skb, nest);
143 nla_nest_end(skb, cnest);
144 nla_nest_end(skb, pnest);
154 static int ncsi_pkg_info_nl(struct sk_buff *msg, struct genl_info *info)
156 struct ncsi_dev_priv *ndp;
157 unsigned int package_id;
163 if (!info || !info->attrs)
166 if (!info->attrs[NCSI_ATTR_IFINDEX])
169 if (!info->attrs[NCSI_ATTR_PACKAGE_ID])
172 ndp = ndp_from_ifindex(genl_info_net(info),
173 nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
177 skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
181 hdr = genlmsg_put(skb, info->snd_portid, info->snd_seq,
182 &ncsi_genl_family, 0, NCSI_CMD_PKG_INFO);
188 package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]);
190 attr = nla_nest_start(skb, NCSI_ATTR_PACKAGE_LIST);
195 rc = ncsi_write_package_info(skb, ndp, package_id);
198 nla_nest_cancel(skb, attr);
202 nla_nest_end(skb, attr);
204 genlmsg_end(skb, hdr);
205 return genlmsg_reply(skb, info);
212 static int ncsi_pkg_info_all_nl(struct sk_buff *skb,
213 struct netlink_callback *cb)
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;
223 rc = genlmsg_parse(cb->nlh, &ncsi_genl_family, attrs, NCSI_ATTR_MAX,
224 ncsi_genl_policy, NULL);
228 if (!attrs[NCSI_ATTR_IFINDEX])
231 ndp = ndp_from_ifindex(get_net(sock_net(skb->sk)),
232 nla_get_u32(attrs[NCSI_ATTR_IFINDEX]));
237 package_id = cb->args[0];
239 NCSI_FOR_EACH_PACKAGE(ndp, np)
240 if (np->id == package_id)
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);
253 attr = nla_nest_start(skb, NCSI_ATTR_PACKAGE_LIST);
254 rc = ncsi_write_package_info(skb, ndp, package->id);
256 nla_nest_cancel(skb, attr);
260 nla_nest_end(skb, attr);
261 genlmsg_end(skb, hdr);
263 cb->args[0] = package_id + 1;
267 genlmsg_cancel(skb, hdr);
271 static int ncsi_set_interface_nl(struct sk_buff *msg, struct genl_info *info)
273 struct ncsi_package *np, *package;
274 struct ncsi_channel *nc, *channel;
275 u32 package_id, channel_id;
276 struct ncsi_dev_priv *ndp;
279 if (!info || !info->attrs)
282 if (!info->attrs[NCSI_ATTR_IFINDEX])
285 if (!info->attrs[NCSI_ATTR_PACKAGE_ID])
288 ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
289 nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
293 package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]);
296 NCSI_FOR_EACH_PACKAGE(ndp, np)
297 if (np->id == package_id)
300 /* The user has set a package that does not exist */
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) {
313 netdev_info(ndp->ndev.dev,
314 "NCSI: Channel %u does not exist!\n",
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);
325 spin_lock_irqsave(&package->lock, flags);
326 package->multi_channel = false;
328 package->channel_whitelist = 0x1 << channel->id;
329 package->preferred_channel = channel;
331 /* Allow any channel */
332 package->channel_whitelist = UINT_MAX;
333 package->preferred_channel = NULL;
335 spin_unlock_irqrestore(&package->lock, flags);
338 netdev_info(ndp->ndev.dev,
339 "Set package 0x%x, channel 0x%x as preferred\n",
340 package_id, channel_id);
342 netdev_info(ndp->ndev.dev, "Set package 0x%x as preferred\n",
345 /* Update channel configuration */
346 if (!(ndp->flags & NCSI_DEV_RESET))
347 ncsi_reset_dev(&ndp->ndev);
352 static int ncsi_clear_interface_nl(struct sk_buff *msg, struct genl_info *info)
354 struct ncsi_dev_priv *ndp;
355 struct ncsi_package *np;
358 if (!info || !info->attrs)
361 if (!info->attrs[NCSI_ATTR_IFINDEX])
364 ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
365 nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
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);
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);
382 netdev_info(ndp->ndev.dev, "NCSI: Cleared preferred package/channel\n");
384 /* Update channel configuration */
385 if (!(ndp->flags & NCSI_DEV_RESET))
386 ncsi_reset_dev(&ndp->ndev);
391 static int ncsi_send_cmd_nl(struct sk_buff *msg, struct genl_info *info)
393 struct ncsi_dev_priv *ndp;
394 struct ncsi_pkt_hdr *hdr;
395 struct ncsi_cmd_arg nca;
401 if (!info || !info->attrs) {
406 if (!info->attrs[NCSI_ATTR_IFINDEX]) {
411 if (!info->attrs[NCSI_ATTR_PACKAGE_ID]) {
416 if (!info->attrs[NCSI_ATTR_CHANNEL_ID]) {
421 if (!info->attrs[NCSI_ATTR_DATA]) {
426 ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
427 nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
433 package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]);
434 channel_id = nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_ID]);
436 if (package_id >= NCSI_MAX_PACKAGE || channel_id >= NCSI_MAX_CHANNEL) {
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",
448 data = (unsigned char *)nla_data(info->attrs[NCSI_ATTR_DATA]);
451 hdr = (struct ncsi_pkt_hdr *)data;
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;
459 nca.payload = ntohs(hdr->length);
460 nca.data = data + sizeof(*hdr);
462 ret = ncsi_xmit_cmd(&nca);
465 netdev_err(ndp->ndev.dev,
466 "NCSI: Error %d sending command\n",
468 ncsi_send_netlink_err(ndp->ndev.dev,
478 int ncsi_send_netlink_rsp(struct ncsi_request *nr,
479 struct ncsi_package *np,
480 struct ncsi_channel *nc)
487 net = dev_net(nr->rsp->dev);
489 skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
493 hdr = genlmsg_put(skb, nr->snd_portid, nr->snd_seq,
494 &ncsi_genl_family, 0, NCSI_CMD_SEND_CMD);
500 nla_put_u32(skb, NCSI_ATTR_IFINDEX, nr->rsp->dev->ifindex);
502 nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID, np->id);
504 nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, nc->id);
506 nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, NCSI_RESERVED_CHANNEL);
508 rc = nla_put(skb, NCSI_ATTR_DATA, nr->rsp->len, (void *)nr->rsp->data);
512 genlmsg_end(skb, hdr);
513 return genlmsg_unicast(net, skb, nr->snd_portid);
520 int ncsi_send_netlink_timeout(struct ncsi_request *nr,
521 struct ncsi_package *np,
522 struct ncsi_channel *nc)
528 skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
532 hdr = genlmsg_put(skb, nr->snd_portid, nr->snd_seq,
533 &ncsi_genl_family, 0, NCSI_CMD_SEND_CMD);
539 net = dev_net(nr->cmd->dev);
541 nla_put_u32(skb, NCSI_ATTR_IFINDEX, nr->cmd->dev->ifindex);
544 nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID, np->id);
546 nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID,
547 NCSI_PACKAGE_INDEX((((struct ncsi_pkt_hdr *)
548 nr->cmd->data)->channel)));
551 nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, nc->id);
553 nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, NCSI_RESERVED_CHANNEL);
555 genlmsg_end(skb, hdr);
556 return genlmsg_unicast(net, skb, nr->snd_portid);
559 int ncsi_send_netlink_err(struct net_device *dev,
562 struct nlmsghdr *nlhdr,
565 struct nlmsghdr *nlh;
566 struct nlmsgerr *nle;
570 skb = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
576 nlh = nlmsg_put(skb, snd_portid, snd_seq,
577 NLMSG_ERROR, sizeof(*nle), 0);
578 nle = (struct nlmsgerr *)nlmsg_data(nlh);
580 memcpy(&nle->msg, nlhdr, sizeof(*nlh));
584 return nlmsg_unicast(net->genl_sock, skb, snd_portid);
587 static int ncsi_set_package_mask_nl(struct sk_buff *msg,
588 struct genl_info *info)
590 struct ncsi_dev_priv *ndp;
594 if (!info || !info->attrs)
597 if (!info->attrs[NCSI_ATTR_IFINDEX])
600 if (!info->attrs[NCSI_ATTR_PACKAGE_MASK])
603 ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
604 nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
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;
614 netdev_err(ndp->ndev.dev,
615 "NCSI: Can't use multiple packages without HWA\n");
619 ndp->multi_package = false;
624 ndp->package_whitelist =
625 nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_MASK]);
626 spin_unlock_irqrestore(&ndp->lock, flags);
629 /* Update channel configuration */
630 if (!(ndp->flags & NCSI_DEV_RESET))
631 ncsi_reset_dev(&ndp->ndev);
637 static int ncsi_set_channel_mask_nl(struct sk_buff *msg,
638 struct genl_info *info)
640 struct ncsi_package *np, *package;
641 struct ncsi_channel *nc, *channel;
642 u32 package_id, channel_id;
643 struct ncsi_dev_priv *ndp;
646 if (!info || !info->attrs)
649 if (!info->attrs[NCSI_ATTR_IFINDEX])
652 if (!info->attrs[NCSI_ATTR_PACKAGE_ID])
655 if (!info->attrs[NCSI_ATTR_CHANNEL_MASK])
658 ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
659 nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
663 package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]);
665 NCSI_FOR_EACH_PACKAGE(ndp, np)
666 if (np->id == package_id) {
673 spin_lock_irqsave(&package->lock, flags);
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) {
684 spin_unlock_irqrestore(&package->lock, flags);
687 netdev_dbg(ndp->ndev.dev,
688 "NCSI: Channel %u set as preferred channel\n",
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",
699 package->preferred_channel = channel;
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",
707 package->multi_channel = false;
710 spin_unlock_irqrestore(&package->lock, flags);
712 /* Update channel configuration */
713 if (!(ndp->flags & NCSI_DEV_RESET))
714 ncsi_reset_dev(&ndp->ndev);
719 static const struct genl_ops ncsi_ops[] = {
721 .cmd = NCSI_CMD_PKG_INFO,
722 .policy = ncsi_genl_policy,
723 .doit = ncsi_pkg_info_nl,
724 .dumpit = ncsi_pkg_info_all_nl,
728 .cmd = NCSI_CMD_SET_INTERFACE,
729 .policy = ncsi_genl_policy,
730 .doit = ncsi_set_interface_nl,
731 .flags = GENL_ADMIN_PERM,
734 .cmd = NCSI_CMD_CLEAR_INTERFACE,
735 .policy = ncsi_genl_policy,
736 .doit = ncsi_clear_interface_nl,
737 .flags = GENL_ADMIN_PERM,
740 .cmd = NCSI_CMD_SEND_CMD,
741 .policy = ncsi_genl_policy,
742 .doit = ncsi_send_cmd_nl,
743 .flags = GENL_ADMIN_PERM,
746 .cmd = NCSI_CMD_SET_PACKAGE_MASK,
747 .policy = ncsi_genl_policy,
748 .doit = ncsi_set_package_mask_nl,
749 .flags = GENL_ADMIN_PERM,
752 .cmd = NCSI_CMD_SET_CHANNEL_MASK,
753 .policy = ncsi_genl_policy,
754 .doit = ncsi_set_channel_mask_nl,
755 .flags = GENL_ADMIN_PERM,
759 static struct genl_family ncsi_genl_family __ro_after_init = {
762 .maxattr = NCSI_ATTR_MAX,
763 .module = THIS_MODULE,
765 .n_ops = ARRAY_SIZE(ncsi_ops),
768 int ncsi_init_netlink(struct net_device *dev)
772 rc = genl_register_family(&ncsi_genl_family);
774 netdev_err(dev, "ncsi: failed to register netlink family\n");
779 int ncsi_unregister_netlink(struct net_device *dev)
783 rc = genl_unregister_family(&ncsi_genl_family);
785 netdev_err(dev, "ncsi: failed to unregister netlink family\n");