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 | |
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 | |
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, attrtype: NCSI_CHANNEL_ATTR_ID, value: nc->id); |
67 | m = &nc->modes[NCSI_MODE_LINK]; |
68 | nla_put_u32(skb, attrtype: NCSI_CHANNEL_ATTR_LINK_STATE, value: m->data[2]); |
69 | if (nc->state == NCSI_CHANNEL_ACTIVE) |
70 | nla_put_flag(skb, attrtype: NCSI_CHANNEL_ATTR_ACTIVE); |
71 | if (nc == nc->package->preferred_channel) |
72 | nla_put_flag(skb, attrtype: NCSI_CHANNEL_ATTR_FORCED); |
73 | |
74 | nla_put_u32(skb, attrtype: NCSI_CHANNEL_ATTR_VERSION_MAJOR, value: nc->version.version); |
75 | nla_put_u32(skb, attrtype: NCSI_CHANNEL_ATTR_VERSION_MINOR, value: nc->version.alpha2); |
76 | nla_put_string(skb, attrtype: NCSI_CHANNEL_ATTR_VERSION_STR, str: nc->version.fw_name); |
77 | |
78 | vid_nest = nla_nest_start_noflag(skb, attrtype: 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(addr: (void *)&ncf->bitmap, size: ncf->n_vids, |
84 | offset: i + 1)) < ncf->n_vids) { |
85 | if (ncf->vids[i]) |
86 | nla_put_u16(skb, attrtype: NCSI_CHANNEL_ATTR_VLAN_ID, |
87 | value: ncf->vids[i]); |
88 | } |
89 | nla_nest_end(skb, start: vid_nest); |
90 | |
91 | return 0; |
92 | } |
93 | |
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(dev: ndp->ndev.dev, format: "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, attrtype: NCSI_PKG_ATTR); |
113 | if (!pnest) |
114 | return -ENOMEM; |
115 | rc = nla_put_u32(skb, attrtype: NCSI_PKG_ATTR_ID, value: np->id); |
116 | if (rc) { |
117 | nla_nest_cancel(skb, start: pnest); |
118 | return rc; |
119 | } |
120 | if ((0x1 << np->id) == ndp->package_whitelist) |
121 | nla_put_flag(skb, attrtype: NCSI_PKG_ATTR_FORCED); |
122 | cnest = nla_nest_start_noflag(skb, attrtype: NCSI_PKG_ATTR_CHANNEL_LIST); |
123 | if (!cnest) { |
124 | nla_nest_cancel(skb, start: pnest); |
125 | return -ENOMEM; |
126 | } |
127 | NCSI_FOR_EACH_CHANNEL(np, nc) { |
128 | nest = nla_nest_start_noflag(skb, attrtype: NCSI_CHANNEL_ATTR); |
129 | if (!nest) { |
130 | nla_nest_cancel(skb, start: cnest); |
131 | nla_nest_cancel(skb, start: pnest); |
132 | return -ENOMEM; |
133 | } |
134 | rc = ncsi_write_channel_info(skb, ndp, nc); |
135 | if (rc) { |
136 | nla_nest_cancel(skb, start: nest); |
137 | nla_nest_cancel(skb, start: cnest); |
138 | nla_nest_cancel(skb, start: pnest); |
139 | return rc; |
140 | } |
141 | nla_nest_end(skb, start: nest); |
142 | } |
143 | nla_nest_end(skb, start: cnest); |
144 | nla_nest_end(skb, start: 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(net: genl_info_net(info), |
173 | ifindex: nla_get_u32(nla: 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, portid: info->snd_portid, seq: info->snd_seq, |
182 | family: &ncsi_genl_family, flags: 0, cmd: NCSI_CMD_PKG_INFO); |
183 | if (!hdr) { |
184 | kfree_skb(skb); |
185 | return -EMSGSIZE; |
186 | } |
187 | |
188 | package_id = nla_get_u32(nla: info->attrs[NCSI_ATTR_PACKAGE_ID]); |
189 | |
190 | attr = nla_nest_start_noflag(skb, attrtype: NCSI_ATTR_PACKAGE_LIST); |
191 | if (!attr) { |
192 | kfree_skb(skb); |
193 | return -EMSGSIZE; |
194 | } |
195 | rc = ncsi_write_package_info(skb, ndp, id: package_id); |
196 | |
197 | if (rc) { |
198 | nla_nest_cancel(skb, start: attr); |
199 | goto err; |
200 | } |
201 | |
202 | nla_nest_end(skb, start: 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_deprecated(nlh: cb->nlh, family: &ncsi_genl_family, tb: attrs, maxtype: NCSI_ATTR_MAX, |
224 | policy: 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(net: get_net(net: sock_net(sk: skb->sk)), |
232 | ifindex: nla_get_u32(nla: 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, seq: cb->nlh->nlmsg_seq, |
247 | family: &ncsi_genl_family, NLM_F_MULTI, cmd: NCSI_CMD_PKG_INFO); |
248 | if (!hdr) { |
249 | rc = -EMSGSIZE; |
250 | goto err; |
251 | } |
252 | |
253 | attr = nla_nest_start_noflag(skb, attrtype: NCSI_ATTR_PACKAGE_LIST); |
254 | if (!attr) { |
255 | rc = -EMSGSIZE; |
256 | goto err; |
257 | } |
258 | rc = ncsi_write_package_info(skb, ndp, id: package->id); |
259 | if (rc) { |
260 | nla_nest_cancel(skb, start: attr); |
261 | goto err; |
262 | } |
263 | |
264 | nla_nest_end(skb, start: 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 | |
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(net: get_net(net: sock_net(sk: msg->sk)), |
293 | ifindex: nla_get_u32(nla: info->attrs[NCSI_ATTR_IFINDEX])); |
294 | if (!ndp) |
295 | return -ENODEV; |
296 | |
297 | package_id = nla_get_u32(nla: 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(nla: 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(dev: ndp->ndev.dev, |
318 | format: "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(lock: &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(lock: &package->lock, flags); |
340 | |
341 | if (channel) |
342 | netdev_info(dev: ndp->ndev.dev, |
343 | format: "Set package 0x%x, channel 0x%x as preferred\n" , |
344 | package_id, channel_id); |
345 | else |
346 | netdev_info(dev: ndp->ndev.dev, format: "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(nd: &ndp->ndev); |
352 | |
353 | return 0; |
354 | } |
355 | |
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(net: get_net(net: sock_net(sk: msg->sk)), |
369 | ifindex: nla_get_u32(nla: 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(lock: &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(lock: &np->lock, flags); |
385 | } |
386 | netdev_info(dev: ndp->ndev.dev, format: "NCSI: Cleared preferred package/channel\n" ); |
387 | |
388 | /* Update channel configuration */ |
389 | if (!(ndp->flags & NCSI_DEV_RESET)) |
390 | ncsi_reset_dev(nd: &ndp->ndev); |
391 | |
392 | return 0; |
393 | } |
394 | |
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(net: get_net(net: sock_net(sk: msg->sk)), |
431 | ifindex: nla_get_u32(nla: info->attrs[NCSI_ATTR_IFINDEX])); |
432 | if (!ndp) { |
433 | ret = -ENODEV; |
434 | goto out; |
435 | } |
436 | |
437 | package_id = nla_get_u32(nla: info->attrs[NCSI_ATTR_PACKAGE_ID]); |
438 | channel_id = nla_get_u32(nla: 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(nla: info->attrs[NCSI_ATTR_DATA]); |
446 | if (len < sizeof(struct ncsi_pkt_hdr)) { |
447 | netdev_info(dev: ndp->ndev.dev, format: "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(nla: 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: &nca); |
467 | out_netlink: |
468 | if (ret != 0) { |
469 | netdev_err(dev: ndp->ndev.dev, |
470 | format: "NCSI: Error %d sending command\n" , |
471 | ret); |
472 | ncsi_send_netlink_err(dev: ndp->ndev.dev, |
473 | snd_seq: info->snd_seq, |
474 | snd_portid: info->snd_portid, |
475 | nlhdr: info->nlhdr, |
476 | err: ret); |
477 | } |
478 | out: |
479 | return ret; |
480 | } |
481 | |
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(dev: 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, portid: nr->snd_portid, seq: nr->snd_seq, |
498 | family: &ncsi_genl_family, flags: 0, cmd: NCSI_CMD_SEND_CMD); |
499 | if (!hdr) { |
500 | kfree_skb(skb); |
501 | return -EMSGSIZE; |
502 | } |
503 | |
504 | nla_put_u32(skb, attrtype: NCSI_ATTR_IFINDEX, value: nr->rsp->dev->ifindex); |
505 | if (np) |
506 | nla_put_u32(skb, attrtype: NCSI_ATTR_PACKAGE_ID, value: np->id); |
507 | if (nc) |
508 | nla_put_u32(skb, attrtype: NCSI_ATTR_CHANNEL_ID, value: nc->id); |
509 | else |
510 | nla_put_u32(skb, attrtype: NCSI_ATTR_CHANNEL_ID, NCSI_RESERVED_CHANNEL); |
511 | |
512 | rc = nla_put(skb, attrtype: NCSI_ATTR_DATA, attrlen: nr->rsp->len, data: (void *)nr->rsp->data); |
513 | if (rc) |
514 | goto err; |
515 | |
516 | genlmsg_end(skb, hdr); |
517 | return genlmsg_unicast(net, skb, portid: nr->snd_portid); |
518 | |
519 | err: |
520 | kfree_skb(skb); |
521 | return rc; |
522 | } |
523 | |
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, portid: nr->snd_portid, seq: nr->snd_seq, |
537 | family: &ncsi_genl_family, flags: 0, cmd: NCSI_CMD_SEND_CMD); |
538 | if (!hdr) { |
539 | kfree_skb(skb); |
540 | return -EMSGSIZE; |
541 | } |
542 | |
543 | net = dev_net(dev: nr->cmd->dev); |
544 | |
545 | nla_put_u32(skb, attrtype: NCSI_ATTR_IFINDEX, value: nr->cmd->dev->ifindex); |
546 | |
547 | if (np) |
548 | nla_put_u32(skb, attrtype: NCSI_ATTR_PACKAGE_ID, value: np->id); |
549 | else |
550 | nla_put_u32(skb, attrtype: 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, attrtype: NCSI_ATTR_CHANNEL_ID, value: nc->id); |
556 | else |
557 | nla_put_u32(skb, attrtype: NCSI_ATTR_CHANNEL_ID, NCSI_RESERVED_CHANNEL); |
558 | |
559 | genlmsg_end(skb, hdr); |
560 | return genlmsg_unicast(net, skb, portid: nr->snd_portid); |
561 | } |
562 | |
563 | int ncsi_send_netlink_err(struct net_device *dev, |
564 | u32 snd_seq, |
565 | u32 snd_portid, |
566 | const 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, portid: snd_portid, seq: snd_seq, |
581 | NLMSG_ERROR, payload: sizeof(*nle), flags: 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(sk: net->genl_sock, skb, portid: snd_portid); |
589 | } |
590 | |
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(net: get_net(net: sock_net(sk: msg->sk)), |
608 | ifindex: nla_get_u32(nla: info->attrs[NCSI_ATTR_IFINDEX])); |
609 | if (!ndp) |
610 | return -ENODEV; |
611 | |
612 | spin_lock_irqsave(&ndp->lock, flags); |
613 | if (nla_get_flag(nla: 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(dev: ndp->ndev.dev, |
619 | format: "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(nla: info->attrs[NCSI_ATTR_PACKAGE_MASK]); |
630 | spin_unlock_irqrestore(lock: &ndp->lock, flags); |
631 | |
632 | if (!rc) { |
633 | /* Update channel configuration */ |
634 | if (!(ndp->flags & NCSI_DEV_RESET)) |
635 | ncsi_reset_dev(nd: &ndp->ndev); |
636 | } |
637 | |
638 | return rc; |
639 | } |
640 | |
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(net: get_net(net: sock_net(sk: msg->sk)), |
663 | ifindex: nla_get_u32(nla: info->attrs[NCSI_ATTR_IFINDEX])); |
664 | if (!ndp) |
665 | return -ENODEV; |
666 | |
667 | package_id = nla_get_u32(nla: 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(nla: 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(lock: &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(nla: 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(nla: info->attrs[NCSI_ATTR_MULTI_FLAG])) { |
706 | package->multi_channel = true; |
707 | netdev_info(dev: ndp->ndev.dev, |
708 | format: "NCSI: Multi-channel enabled on package %u\n" , |
709 | package_id); |
710 | } else { |
711 | package->multi_channel = false; |
712 | } |
713 | |
714 | spin_unlock_irqrestore(lock: &package->lock, flags); |
715 | |
716 | /* Update channel configuration */ |
717 | if (!(ndp->flags & NCSI_DEV_RESET)) |
718 | ncsi_reset_dev(nd: &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 | .resv_start_op = NCSI_CMD_SET_CHANNEL_MASK + 1, |
772 | }; |
773 | |
774 | static int __init ncsi_init_netlink(void) |
775 | { |
776 | return genl_register_family(family: &ncsi_genl_family); |
777 | } |
778 | subsys_initcall(ncsi_init_netlink); |
779 | |