1
2
3
4
5
6
7
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 <linux/module.h>
16#include <net/genetlink.h>
17#include <net/ncsi.h>
18#include <linux/skbuff.h>
19#include <net/sock.h>
20#include <uapi/linux/ncsi.h>
21
22#include "internal.h"
23#include "ncsi-netlink.h"
24
25static struct genl_family ncsi_genl_family;
26
27static 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};
33
34static 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
57static 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 (ndp->force_channel == nc)
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
94static 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) {
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 nla_put_u32(skb, NCSI_PKG_ATTR_ID, np->id);
116 if (ndp->force_package == np)
117 nla_put_flag(skb, NCSI_PKG_ATTR_FORCED);
118 cnest = nla_nest_start_noflag(skb, NCSI_PKG_ATTR_CHANNEL_LIST);
119 if (!cnest) {
120 nla_nest_cancel(skb, pnest);
121 return -ENOMEM;
122 }
123 NCSI_FOR_EACH_CHANNEL(np, nc) {
124 nest = nla_nest_start_noflag(skb, NCSI_CHANNEL_ATTR);
125 if (!nest) {
126 nla_nest_cancel(skb, cnest);
127 nla_nest_cancel(skb, pnest);
128 return -ENOMEM;
129 }
130 rc = ncsi_write_channel_info(skb, ndp, nc);
131 if (rc) {
132 nla_nest_cancel(skb, nest);
133 nla_nest_cancel(skb, cnest);
134 nla_nest_cancel(skb, pnest);
135 return rc;
136 }
137 nla_nest_end(skb, nest);
138 }
139 nla_nest_end(skb, cnest);
140 nla_nest_end(skb, pnest);
141 found = true;
142 }
143
144 if (!found)
145 return -ENODEV;
146
147 return 0;
148}
149
150static int ncsi_pkg_info_nl(struct sk_buff *msg, struct genl_info *info)
151{
152 struct ncsi_dev_priv *ndp;
153 unsigned int package_id;
154 struct sk_buff *skb;
155 struct nlattr *attr;
156 void *hdr;
157 int rc;
158
159 if (!info || !info->attrs)
160 return -EINVAL;
161
162 if (!info->attrs[NCSI_ATTR_IFINDEX])
163 return -EINVAL;
164
165 if (!info->attrs[NCSI_ATTR_PACKAGE_ID])
166 return -EINVAL;
167
168 ndp = ndp_from_ifindex(genl_info_net(info),
169 nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
170 if (!ndp)
171 return -ENODEV;
172
173 skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
174 if (!skb)
175 return -ENOMEM;
176
177 hdr = genlmsg_put(skb, info->snd_portid, info->snd_seq,
178 &ncsi_genl_family, 0, NCSI_CMD_PKG_INFO);
179 if (!hdr) {
180 kfree_skb(skb);
181 return -EMSGSIZE;
182 }
183
184 package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]);
185
186 attr = nla_nest_start_noflag(skb, NCSI_ATTR_PACKAGE_LIST);
187 if (!attr) {
188 kfree_skb(skb);
189 return -EMSGSIZE;
190 }
191 rc = ncsi_write_package_info(skb, ndp, package_id);
192
193 if (rc) {
194 nla_nest_cancel(skb, attr);
195 goto err;
196 }
197
198 nla_nest_end(skb, attr);
199
200 genlmsg_end(skb, hdr);
201 return genlmsg_reply(skb, info);
202
203err:
204 kfree_skb(skb);
205 return rc;
206}
207
208static int ncsi_pkg_info_all_nl(struct sk_buff *skb,
209 struct netlink_callback *cb)
210{
211 struct nlattr *attrs[NCSI_ATTR_MAX + 1];
212 struct ncsi_package *np, *package;
213 struct ncsi_dev_priv *ndp;
214 unsigned int package_id;
215 struct nlattr *attr;
216 void *hdr;
217 int rc;
218
219 rc = genlmsg_parse_deprecated(cb->nlh, &ncsi_genl_family, attrs, NCSI_ATTR_MAX,
220 ncsi_genl_policy, NULL);
221 if (rc)
222 return rc;
223
224 if (!attrs[NCSI_ATTR_IFINDEX])
225 return -EINVAL;
226
227 ndp = ndp_from_ifindex(get_net(sock_net(skb->sk)),
228 nla_get_u32(attrs[NCSI_ATTR_IFINDEX]));
229
230 if (!ndp)
231 return -ENODEV;
232
233 package_id = cb->args[0];
234 package = NULL;
235 NCSI_FOR_EACH_PACKAGE(ndp, np)
236 if (np->id == package_id)
237 package = np;
238
239 if (!package)
240 return 0;
241
242 hdr = genlmsg_put(skb, NETLINK_CB(cb->skb).portid, cb->nlh->nlmsg_seq,
243 &ncsi_genl_family, 0, NCSI_CMD_PKG_INFO);
244 if (!hdr) {
245 rc = -EMSGSIZE;
246 goto err;
247 }
248
249 attr = nla_nest_start_noflag(skb, NCSI_ATTR_PACKAGE_LIST);
250 if (!attr) {
251 rc = -EMSGSIZE;
252 goto err;
253 }
254 rc = ncsi_write_package_info(skb, ndp, package->id);
255 if (rc) {
256 nla_nest_cancel(skb, attr);
257 goto err;
258 }
259
260 nla_nest_end(skb, attr);
261 genlmsg_end(skb, hdr);
262
263 cb->args[0] = package_id + 1;
264
265 return skb->len;
266err:
267 genlmsg_cancel(skb, hdr);
268 return rc;
269}
270
271static int ncsi_set_interface_nl(struct sk_buff *msg, struct genl_info *info)
272{
273 struct ncsi_package *np, *package;
274 struct ncsi_channel *nc, *channel;
275 u32 package_id, channel_id;
276 struct ncsi_dev_priv *ndp;
277 unsigned long flags;
278
279 if (!info || !info->attrs)
280 return -EINVAL;
281
282 if (!info->attrs[NCSI_ATTR_IFINDEX])
283 return -EINVAL;
284
285 if (!info->attrs[NCSI_ATTR_PACKAGE_ID])
286 return -EINVAL;
287
288 ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
289 nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
290 if (!ndp)
291 return -ENODEV;
292
293 package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]);
294 package = NULL;
295
296 spin_lock_irqsave(&ndp->lock, flags);
297
298 NCSI_FOR_EACH_PACKAGE(ndp, np)
299 if (np->id == package_id)
300 package = np;
301 if (!package) {
302
303 spin_unlock_irqrestore(&ndp->lock, flags);
304 return -ERANGE;
305 }
306
307 channel = NULL;
308 if (!info->attrs[NCSI_ATTR_CHANNEL_ID]) {
309
310 channel_id = NCSI_RESERVED_CHANNEL;
311 } else {
312 channel_id = nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_ID]);
313 NCSI_FOR_EACH_CHANNEL(package, nc)
314 if (nc->id == channel_id)
315 channel = nc;
316 }
317
318 if (channel_id != NCSI_RESERVED_CHANNEL && !channel) {
319
320
321
322 spin_unlock_irqrestore(&ndp->lock, flags);
323 netdev_info(ndp->ndev.dev, "NCSI: Channel %u does not exist!\n",
324 channel_id);
325 return -ERANGE;
326 }
327
328 ndp->force_package = package;
329 ndp->force_channel = channel;
330 spin_unlock_irqrestore(&ndp->lock, flags);
331
332 netdev_info(ndp->ndev.dev, "Set package 0x%x, channel 0x%x%s as preferred\n",
333 package_id, channel_id,
334 channel_id == NCSI_RESERVED_CHANNEL ? " (any)" : "");
335
336
337 ncsi_stop_dev(&ndp->ndev);
338 ncsi_start_dev(&ndp->ndev);
339
340 return 0;
341}
342
343static int ncsi_clear_interface_nl(struct sk_buff *msg, struct genl_info *info)
344{
345 struct ncsi_dev_priv *ndp;
346 unsigned long flags;
347
348 if (!info || !info->attrs)
349 return -EINVAL;
350
351 if (!info->attrs[NCSI_ATTR_IFINDEX])
352 return -EINVAL;
353
354 ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)),
355 nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX]));
356 if (!ndp)
357 return -ENODEV;
358
359
360 spin_lock_irqsave(&ndp->lock, flags);
361 ndp->force_package = NULL;
362 ndp->force_channel = NULL;
363 spin_unlock_irqrestore(&ndp->lock, flags);
364 netdev_info(ndp->ndev.dev, "NCSI: Cleared preferred package/channel\n");
365
366
367 ncsi_stop_dev(&ndp->ndev);
368 ncsi_start_dev(&ndp->ndev);
369
370 return 0;
371}
372
373static const struct genl_small_ops ncsi_ops[] = {
374 {
375 .cmd = NCSI_CMD_PKG_INFO,
376 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
377 .doit = ncsi_pkg_info_nl,
378 .dumpit = ncsi_pkg_info_all_nl,
379 .flags = 0,
380 },
381 {
382 .cmd = NCSI_CMD_SET_INTERFACE,
383 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
384 .doit = ncsi_set_interface_nl,
385 .flags = GENL_ADMIN_PERM,
386 },
387 {
388 .cmd = NCSI_CMD_CLEAR_INTERFACE,
389 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
390 .doit = ncsi_clear_interface_nl,
391 .flags = GENL_ADMIN_PERM,
392 },
393};
394
395static struct genl_family ncsi_genl_family __ro_after_init = {
396 .name = "NCSI",
397 .version = 0,
398 .maxattr = NCSI_ATTR_MAX,
399 .policy = ncsi_genl_policy,
400 .module = THIS_MODULE,
401 .small_ops = ncsi_ops,
402 .n_small_ops = ARRAY_SIZE(ncsi_ops),
403};
404
405int ncsi_init_netlink(struct net_device *dev)
406{
407 int rc;
408
409 rc = genl_register_family(&ncsi_genl_family);
410 if (rc)
411 netdev_err(dev, "ncsi: failed to register netlink family\n");
412
413 return rc;
414}
415
416int ncsi_unregister_netlink(struct net_device *dev)
417{
418 int rc;
419
420 rc = genl_unregister_family(&ncsi_genl_family);
421 if (rc)
422 netdev_err(dev, "ncsi: failed to unregister netlink family\n");
423
424 return rc;
425}
426
