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