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 },
35 static struct ncsi_dev_priv *ndp_from_ifindex(struct net *net, u32 ifindex)
37 struct ncsi_dev_priv *ndp;
38 struct net_device *dev;
45 dev = dev_get_by_index(net, ifindex);
47 pr_err("NCSI netlink: No device for ifindex %u\n", ifindex);
51 nd = ncsi_find_dev(dev);
52 ndp = nd ? TO_NCSI_DEV_PRIV(nd) : NULL;
58 static int ncsi_write_channel_info(struct sk_buff *skb,
59 struct ncsi_dev_priv *ndp,
60 struct ncsi_channel *nc)
62 struct ncsi_channel_vlan_filter *ncf;
63 struct ncsi_channel_mode *m;
64 struct nlattr *vid_nest;
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);
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);
79 vid_nest = nla_nest_start(skb, NCSI_CHANNEL_ATTR_VLAN_LIST);
82 ncf = &nc->vlan_filter;
84 while ((i = find_next_bit((void *)&ncf->bitmap, ncf->n_vids,
85 i + 1)) < ncf->n_vids) {
87 nla_put_u16(skb, NCSI_CHANNEL_ATTR_VLAN_ID,
90 nla_nest_end(skb, vid_nest);
95 static int ncsi_write_package_info(struct sk_buff *skb,
96 struct ncsi_dev_priv *ndp, unsigned int id)
98 struct nlattr *pnest, *cnest, *nest;
99 struct ncsi_package *np;
100 struct ncsi_channel *nc;
104 if (id > ndp->package_num - 1) {
105 netdev_info(ndp->ndev.dev, "NCSI: No package with id %u\n", id);
110 NCSI_FOR_EACH_PACKAGE(ndp, np) {
113 pnest = nla_nest_start(skb, NCSI_PKG_ATTR);
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);
121 nla_nest_cancel(skb, pnest);
124 NCSI_FOR_EACH_CHANNEL(np, nc) {
125 nest = nla_nest_start(skb, NCSI_CHANNEL_ATTR);
127 nla_nest_cancel(skb, cnest);
128 nla_nest_cancel(skb, pnest);
131 rc = ncsi_write_channel_info(skb, ndp, nc);
133 nla_nest_cancel(skb, nest);
134 nla_nest_cancel(skb, cnest);
135 nla_nest_cancel(skb, pnest);
138 nla_nest_end(skb, nest);
140 nla_nest_end(skb, cnest);
141 nla_nest_end(skb, pnest);
151 static int ncsi_pkg_info_nl(struct sk_buff *msg, struct genl_info *info)
153 struct ncsi_dev_priv *ndp;
154 unsigned int package_id;
160 if (!info || !info->attrs)
163 if (!info->attrs[NCSI_ATTR_IFINDEX])
166 if (!info->attrs[NCSI_ATTR_PACKAGE_ID])
169 ndp = ndp_from_ifindex(genl_info_net(info),
170 nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
174 skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
178 hdr = genlmsg_put(skb, info->snd_portid, info->snd_seq,
179 &ncsi_genl_family, 0, NCSI_CMD_PKG_INFO);
185 package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]);
187 attr = nla_nest_start(skb, NCSI_ATTR_PACKAGE_LIST);
192 rc = ncsi_write_package_info(skb, ndp, package_id);
195 nla_nest_cancel(skb, attr);
199 nla_nest_end(skb, attr);
201 genlmsg_end(skb, hdr);
202 return genlmsg_reply(skb, info);
209 static int ncsi_pkg_info_all_nl(struct sk_buff *skb,
210 struct netlink_callback *cb)
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;
220 rc = genlmsg_parse(cb->nlh, &ncsi_genl_family, attrs, NCSI_ATTR_MAX,
221 ncsi_genl_policy, NULL);
225 if (!attrs[NCSI_ATTR_IFINDEX])
228 ndp = ndp_from_ifindex(get_net(sock_net(skb->sk)),
229 nla_get_u32(attrs[NCSI_ATTR_IFINDEX]));
234 package_id = cb->args[0];
236 NCSI_FOR_EACH_PACKAGE(ndp, np)
237 if (np->id == package_id)
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);
250 attr = nla_nest_start(skb, NCSI_ATTR_PACKAGE_LIST);
251 rc = ncsi_write_package_info(skb, ndp, package->id);
253 nla_nest_cancel(skb, attr);
257 nla_nest_end(skb, attr);
258 genlmsg_end(skb, hdr);
260 cb->args[0] = package_id + 1;
264 genlmsg_cancel(skb, hdr);
268 static int ncsi_set_interface_nl(struct sk_buff *msg, struct genl_info *info)
270 struct ncsi_package *np, *package;
271 struct ncsi_channel *nc, *channel;
272 u32 package_id, channel_id;
273 struct ncsi_dev_priv *ndp;
276 if (!info || !info->attrs)
279 if (!info->attrs[NCSI_ATTR_IFINDEX])
282 if (!info->attrs[NCSI_ATTR_PACKAGE_ID])
285 ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
286 nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
290 package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]);
293 spin_lock_irqsave(&ndp->lock, flags);
295 NCSI_FOR_EACH_PACKAGE(ndp, np)
296 if (np->id == package_id)
299 /* The user has set a package that does not exist */
300 spin_unlock_irqrestore(&ndp->lock, flags);
305 if (!info->attrs[NCSI_ATTR_CHANNEL_ID]) {
306 /* Allow any channel */
307 channel_id = NCSI_RESERVED_CHANNEL;
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)
315 if (channel_id != NCSI_RESERVED_CHANNEL && !channel) {
316 /* The user has set a channel that does not exist on this
319 spin_unlock_irqrestore(&ndp->lock, flags);
320 netdev_info(ndp->ndev.dev, "NCSI: Channel %u does not exist!\n",
325 ndp->force_package = package;
326 ndp->force_channel = channel;
327 spin_unlock_irqrestore(&ndp->lock, flags);
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)" : "");
333 /* Bounce the NCSI channel to set changes */
334 ncsi_stop_dev(&ndp->ndev);
335 ncsi_start_dev(&ndp->ndev);
340 static int ncsi_clear_interface_nl(struct sk_buff *msg, struct genl_info *info)
342 struct ncsi_dev_priv *ndp;
345 if (!info || !info->attrs)
348 if (!info->attrs[NCSI_ATTR_IFINDEX])
351 ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
352 nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
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");
363 /* Bounce the NCSI channel to set changes */
364 ncsi_stop_dev(&ndp->ndev);
365 ncsi_start_dev(&ndp->ndev);
370 static int ncsi_send_cmd_nl(struct sk_buff *msg, struct genl_info *info)
372 struct ncsi_dev_priv *ndp;
373 struct ncsi_pkt_hdr *hdr;
374 struct ncsi_cmd_arg nca;
380 if (!info || !info->attrs) {
385 if (!info->attrs[NCSI_ATTR_IFINDEX]) {
390 if (!info->attrs[NCSI_ATTR_PACKAGE_ID]) {
395 if (!info->attrs[NCSI_ATTR_CHANNEL_ID]) {
400 if (!info->attrs[NCSI_ATTR_DATA]) {
405 ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
406 nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
412 package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]);
413 channel_id = nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_ID]);
415 if (package_id >= NCSI_MAX_PACKAGE || channel_id >= NCSI_MAX_CHANNEL) {
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",
427 data = (unsigned char *)nla_data(info->attrs[NCSI_ATTR_DATA]);
430 hdr = (struct ncsi_pkt_hdr *)data;
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;
438 nca.payload = ntohs(hdr->length);
439 nca.data = data + sizeof(*hdr);
441 ret = ncsi_xmit_cmd(&nca);
444 netdev_err(ndp->ndev.dev,
445 "NCSI: Error %d sending command\n",
447 ncsi_send_netlink_err(ndp->ndev.dev,
457 int ncsi_send_netlink_rsp(struct ncsi_request *nr,
458 struct ncsi_package *np,
459 struct ncsi_channel *nc)
466 net = dev_net(nr->rsp->dev);
468 skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
472 hdr = genlmsg_put(skb, nr->snd_portid, nr->snd_seq,
473 &ncsi_genl_family, 0, NCSI_CMD_SEND_CMD);
479 nla_put_u32(skb, NCSI_ATTR_IFINDEX, nr->rsp->dev->ifindex);
481 nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID, np->id);
483 nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, nc->id);
485 nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, NCSI_RESERVED_CHANNEL);
487 rc = nla_put(skb, NCSI_ATTR_DATA, nr->rsp->len, (void *)nr->rsp->data);
491 genlmsg_end(skb, hdr);
492 return genlmsg_unicast(net, skb, nr->snd_portid);
499 int ncsi_send_netlink_timeout(struct ncsi_request *nr,
500 struct ncsi_package *np,
501 struct ncsi_channel *nc)
507 skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
511 hdr = genlmsg_put(skb, nr->snd_portid, nr->snd_seq,
512 &ncsi_genl_family, 0, NCSI_CMD_SEND_CMD);
518 net = dev_net(nr->cmd->dev);
520 nla_put_u32(skb, NCSI_ATTR_IFINDEX, nr->cmd->dev->ifindex);
523 nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID, np->id);
525 nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID,
526 NCSI_PACKAGE_INDEX((((struct ncsi_pkt_hdr *)
527 nr->cmd->data)->channel)));
530 nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, nc->id);
532 nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, NCSI_RESERVED_CHANNEL);
534 genlmsg_end(skb, hdr);
535 return genlmsg_unicast(net, skb, nr->snd_portid);
538 int ncsi_send_netlink_err(struct net_device *dev,
541 struct nlmsghdr *nlhdr,
544 struct nlmsghdr *nlh;
545 struct nlmsgerr *nle;
549 skb = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
555 nlh = nlmsg_put(skb, snd_portid, snd_seq,
556 NLMSG_ERROR, sizeof(*nle), 0);
557 nle = (struct nlmsgerr *)nlmsg_data(nlh);
559 memcpy(&nle->msg, nlhdr, sizeof(*nlh));
563 return nlmsg_unicast(net->genl_sock, skb, snd_portid);
566 static const struct genl_ops ncsi_ops[] = {
568 .cmd = NCSI_CMD_PKG_INFO,
569 .policy = ncsi_genl_policy,
570 .doit = ncsi_pkg_info_nl,
571 .dumpit = ncsi_pkg_info_all_nl,
575 .cmd = NCSI_CMD_SET_INTERFACE,
576 .policy = ncsi_genl_policy,
577 .doit = ncsi_set_interface_nl,
578 .flags = GENL_ADMIN_PERM,
581 .cmd = NCSI_CMD_CLEAR_INTERFACE,
582 .policy = ncsi_genl_policy,
583 .doit = ncsi_clear_interface_nl,
584 .flags = GENL_ADMIN_PERM,
587 .cmd = NCSI_CMD_SEND_CMD,
588 .policy = ncsi_genl_policy,
589 .doit = ncsi_send_cmd_nl,
590 .flags = GENL_ADMIN_PERM,
594 static struct genl_family ncsi_genl_family __ro_after_init = {
597 .maxattr = NCSI_ATTR_MAX,
598 .module = THIS_MODULE,
600 .n_ops = ARRAY_SIZE(ncsi_ops),
603 int ncsi_init_netlink(struct net_device *dev)
607 rc = genl_register_family(&ncsi_genl_family);
609 netdev_err(dev, "ncsi: failed to register netlink family\n");
614 int ncsi_unregister_netlink(struct net_device *dev)
618 rc = genl_unregister_family(&ncsi_genl_family);
620 netdev_err(dev, "ncsi: failed to unregister netlink family\n");