1// SPDX-License-Identifier: GPL-2.0-only
2/* Copyright (c) 2013-2018, The Linux Foundation. All rights reserved.
3 *
4 * RMNET configuration engine
5 */
6
7#include <net/sock.h>
8#include <linux/module.h>
9#include <linux/netlink.h>
10#include <linux/netdevice.h>
11#include "rmnet_config.h"
12#include "rmnet_handlers.h"
13#include "rmnet_vnd.h"
14#include "rmnet_private.h"
15#include "rmnet_map.h"
16
17/* Local Definitions and Declarations */
18
19static const struct nla_policy rmnet_policy[IFLA_RMNET_MAX + 1] = {
20 [IFLA_RMNET_MUX_ID] = { .type = NLA_U16 },
21 [IFLA_RMNET_FLAGS] = { .len = sizeof(struct ifla_rmnet_flags) },
22};
23
24static int rmnet_is_real_dev_registered(const struct net_device *real_dev)
25{
26 return rcu_access_pointer(real_dev->rx_handler) == rmnet_rx_handler;
27}
28
29/* Needs rtnl lock */
30struct rmnet_port*
31rmnet_get_port_rtnl(const struct net_device *real_dev)
32{
33 return rtnl_dereference(real_dev->rx_handler_data);
34}
35
36static int rmnet_unregister_real_device(struct net_device *real_dev)
37{
38 struct rmnet_port *port = rmnet_get_port_rtnl(real_dev);
39
40 if (port->nr_rmnet_devs)
41 return -EINVAL;
42
43 rmnet_map_tx_aggregate_exit(port);
44
45 netdev_rx_handler_unregister(dev: real_dev);
46
47 kfree(objp: port);
48
49 netdev_dbg(real_dev, "Removed from rmnet\n");
50 return 0;
51}
52
53static int rmnet_register_real_device(struct net_device *real_dev,
54 struct netlink_ext_ack *extack)
55{
56 struct rmnet_port *port;
57 int rc, entry;
58
59 ASSERT_RTNL();
60
61 if (rmnet_is_real_dev_registered(real_dev)) {
62 port = rmnet_get_port_rtnl(real_dev);
63 if (port->rmnet_mode != RMNET_EPMODE_VND) {
64 NL_SET_ERR_MSG_MOD(extack, "bridge device already exists");
65 return -EINVAL;
66 }
67
68 return 0;
69 }
70
71 port = kzalloc(sizeof(*port), GFP_KERNEL);
72 if (!port)
73 return -ENOMEM;
74
75 port->dev = real_dev;
76 rc = netdev_rx_handler_register(dev: real_dev, rx_handler: rmnet_rx_handler, rx_handler_data: port);
77 if (rc) {
78 kfree(objp: port);
79 return -EBUSY;
80 }
81
82 for (entry = 0; entry < RMNET_MAX_LOGICAL_EP; entry++)
83 INIT_HLIST_HEAD(&port->muxed_ep[entry]);
84
85 rmnet_map_tx_aggregate_init(port);
86
87 netdev_dbg(real_dev, "registered with rmnet\n");
88 return 0;
89}
90
91static void rmnet_unregister_bridge(struct rmnet_port *port)
92{
93 struct net_device *bridge_dev, *real_dev, *rmnet_dev;
94 struct rmnet_port *real_port;
95
96 if (port->rmnet_mode != RMNET_EPMODE_BRIDGE)
97 return;
98
99 rmnet_dev = port->rmnet_dev;
100 if (!port->nr_rmnet_devs) {
101 /* bridge device */
102 real_dev = port->bridge_ep;
103 bridge_dev = port->dev;
104
105 real_port = rmnet_get_port_rtnl(real_dev);
106 real_port->bridge_ep = NULL;
107 real_port->rmnet_mode = RMNET_EPMODE_VND;
108 } else {
109 /* real device */
110 bridge_dev = port->bridge_ep;
111
112 port->bridge_ep = NULL;
113 port->rmnet_mode = RMNET_EPMODE_VND;
114 }
115
116 netdev_upper_dev_unlink(dev: bridge_dev, upper_dev: rmnet_dev);
117 rmnet_unregister_real_device(real_dev: bridge_dev);
118}
119
120static int rmnet_newlink(struct net_device *dev,
121 struct rtnl_newlink_params *params,
122 struct netlink_ext_ack *extack)
123{
124 struct net *link_net = rtnl_newlink_link_net(p: params);
125 u32 data_format = RMNET_FLAGS_INGRESS_DEAGGREGATION;
126 struct nlattr **data = params->data;
127 struct nlattr **tb = params->tb;
128 struct net_device *real_dev;
129 int mode = RMNET_EPMODE_VND;
130 struct rmnet_endpoint *ep;
131 struct rmnet_port *port;
132 int err = 0;
133 u16 mux_id;
134
135 if (!tb[IFLA_LINK]) {
136 NL_SET_ERR_MSG_MOD(extack, "link not specified");
137 return -EINVAL;
138 }
139
140 real_dev = __dev_get_by_index(net: link_net, ifindex: nla_get_u32(nla: tb[IFLA_LINK]));
141 if (!real_dev) {
142 NL_SET_ERR_MSG_MOD(extack, "link does not exist");
143 return -ENODEV;
144 }
145
146 ep = kzalloc(sizeof(*ep), GFP_KERNEL);
147 if (!ep)
148 return -ENOMEM;
149
150 mux_id = nla_get_u16(nla: data[IFLA_RMNET_MUX_ID]);
151
152 err = rmnet_register_real_device(real_dev, extack);
153 if (err)
154 goto err0;
155
156 port = rmnet_get_port_rtnl(real_dev);
157 err = rmnet_vnd_newlink(id: mux_id, rmnet_dev: dev, port, real_dev, ep, extack);
158 if (err)
159 goto err1;
160
161 err = netdev_upper_dev_link(dev: real_dev, upper_dev: dev, extack);
162 if (err < 0)
163 goto err2;
164
165 port->rmnet_mode = mode;
166 port->rmnet_dev = dev;
167
168 hlist_add_head_rcu(n: &ep->hlnode, h: &port->muxed_ep[mux_id]);
169
170 if (data[IFLA_RMNET_FLAGS]) {
171 struct ifla_rmnet_flags *flags;
172
173 flags = nla_data(nla: data[IFLA_RMNET_FLAGS]);
174 data_format &= ~flags->mask;
175 data_format |= flags->flags & flags->mask;
176 }
177
178 netdev_dbg(dev, "data format [0x%08X]\n", data_format);
179 port->data_format = data_format;
180
181 return 0;
182
183err2:
184 unregister_netdevice(dev);
185 rmnet_vnd_dellink(id: mux_id, port, ep);
186err1:
187 rmnet_unregister_real_device(real_dev);
188err0:
189 kfree(objp: ep);
190 return err;
191}
192
193static void rmnet_dellink(struct net_device *dev, struct list_head *head)
194{
195 struct rmnet_priv *priv = netdev_priv(dev);
196 struct net_device *real_dev, *bridge_dev;
197 struct rmnet_port *real_port, *bridge_port;
198 struct rmnet_endpoint *ep;
199 u8 mux_id = priv->mux_id;
200
201 real_dev = priv->real_dev;
202
203 if (!rmnet_is_real_dev_registered(real_dev))
204 return;
205
206 real_port = rmnet_get_port_rtnl(real_dev);
207 bridge_dev = real_port->bridge_ep;
208 if (bridge_dev) {
209 bridge_port = rmnet_get_port_rtnl(real_dev: bridge_dev);
210 rmnet_unregister_bridge(port: bridge_port);
211 }
212
213 ep = rmnet_get_endpoint(port: real_port, mux_id);
214 if (ep) {
215 hlist_del_init_rcu(n: &ep->hlnode);
216 rmnet_vnd_dellink(id: mux_id, port: real_port, ep);
217 kfree(objp: ep);
218 }
219
220 netdev_upper_dev_unlink(dev: real_dev, upper_dev: dev);
221 rmnet_unregister_real_device(real_dev);
222 unregister_netdevice_queue(dev, head);
223}
224
225static void rmnet_force_unassociate_device(struct net_device *real_dev)
226{
227 struct hlist_node *tmp_ep;
228 struct rmnet_endpoint *ep;
229 struct rmnet_port *port;
230 unsigned long bkt_ep;
231 LIST_HEAD(list);
232
233 port = rmnet_get_port_rtnl(real_dev);
234
235 if (port->nr_rmnet_devs) {
236 /* real device */
237 rmnet_unregister_bridge(port);
238 hash_for_each_safe(port->muxed_ep, bkt_ep, tmp_ep, ep, hlnode) {
239 unregister_netdevice_queue(dev: ep->egress_dev, head: &list);
240 netdev_upper_dev_unlink(dev: real_dev, upper_dev: ep->egress_dev);
241 rmnet_vnd_dellink(id: ep->mux_id, port, ep);
242 hlist_del_init_rcu(n: &ep->hlnode);
243 kfree(objp: ep);
244 }
245 rmnet_unregister_real_device(real_dev);
246 unregister_netdevice_many(head: &list);
247 } else {
248 rmnet_unregister_bridge(port);
249 }
250}
251
252static int rmnet_config_notify_cb(struct notifier_block *nb,
253 unsigned long event, void *data)
254{
255 struct net_device *real_dev = netdev_notifier_info_to_dev(info: data);
256
257 if (!rmnet_is_real_dev_registered(real_dev))
258 return NOTIFY_DONE;
259
260 switch (event) {
261 case NETDEV_UNREGISTER:
262 netdev_dbg(real_dev, "Kernel unregister\n");
263 rmnet_force_unassociate_device(real_dev);
264 break;
265 case NETDEV_CHANGEMTU:
266 if (rmnet_vnd_validate_real_dev_mtu(real_dev))
267 return NOTIFY_BAD;
268 break;
269 default:
270 break;
271 }
272
273 return NOTIFY_DONE;
274}
275
276static struct notifier_block rmnet_dev_notifier __read_mostly = {
277 .notifier_call = rmnet_config_notify_cb,
278};
279
280static int rmnet_rtnl_validate(struct nlattr *tb[], struct nlattr *data[],
281 struct netlink_ext_ack *extack)
282{
283 u16 mux_id;
284
285 if (!data || !data[IFLA_RMNET_MUX_ID]) {
286 NL_SET_ERR_MSG_MOD(extack, "MUX ID not specified");
287 return -EINVAL;
288 }
289
290 mux_id = nla_get_u16(nla: data[IFLA_RMNET_MUX_ID]);
291 if (mux_id > (RMNET_MAX_LOGICAL_EP - 1)) {
292 NL_SET_ERR_MSG_MOD(extack, "invalid MUX ID");
293 return -ERANGE;
294 }
295
296 return 0;
297}
298
299static int rmnet_changelink(struct net_device *dev, struct nlattr *tb[],
300 struct nlattr *data[],
301 struct netlink_ext_ack *extack)
302{
303 struct rmnet_priv *priv = netdev_priv(dev);
304 struct net_device *real_dev;
305 struct rmnet_port *port;
306 u16 mux_id;
307
308 if (!dev)
309 return -ENODEV;
310
311 real_dev = priv->real_dev;
312 if (!rmnet_is_real_dev_registered(real_dev))
313 return -ENODEV;
314
315 port = rmnet_get_port_rtnl(real_dev);
316
317 if (data[IFLA_RMNET_MUX_ID]) {
318 mux_id = nla_get_u16(nla: data[IFLA_RMNET_MUX_ID]);
319
320 if (mux_id != priv->mux_id) {
321 struct rmnet_endpoint *ep;
322
323 ep = rmnet_get_endpoint(port, mux_id: priv->mux_id);
324 if (!ep)
325 return -ENODEV;
326
327 if (rmnet_get_endpoint(port, mux_id)) {
328 NL_SET_ERR_MSG_MOD(extack,
329 "MUX ID already exists");
330 return -EINVAL;
331 }
332
333 hlist_del_init_rcu(n: &ep->hlnode);
334 hlist_add_head_rcu(n: &ep->hlnode,
335 h: &port->muxed_ep[mux_id]);
336
337 ep->mux_id = mux_id;
338 priv->mux_id = mux_id;
339 }
340 }
341
342 if (data[IFLA_RMNET_FLAGS]) {
343 struct ifla_rmnet_flags *flags;
344 u32 old_data_format;
345
346 old_data_format = port->data_format;
347 flags = nla_data(nla: data[IFLA_RMNET_FLAGS]);
348 port->data_format &= ~flags->mask;
349 port->data_format |= flags->flags & flags->mask;
350
351 if (rmnet_vnd_update_dev_mtu(port, real_dev)) {
352 port->data_format = old_data_format;
353 NL_SET_ERR_MSG_MOD(extack, "Invalid MTU on real dev");
354 return -EINVAL;
355 }
356 }
357
358 return 0;
359}
360
361static size_t rmnet_get_size(const struct net_device *dev)
362{
363 return
364 /* IFLA_RMNET_MUX_ID */
365 nla_total_size(payload: 2) +
366 /* IFLA_RMNET_FLAGS */
367 nla_total_size(payload: sizeof(struct ifla_rmnet_flags));
368}
369
370static int rmnet_fill_info(struct sk_buff *skb, const struct net_device *dev)
371{
372 struct rmnet_priv *priv = netdev_priv(dev);
373 struct net_device *real_dev;
374 struct ifla_rmnet_flags f;
375 struct rmnet_port *port;
376
377 real_dev = priv->real_dev;
378
379 if (nla_put_u16(skb, attrtype: IFLA_RMNET_MUX_ID, value: priv->mux_id))
380 goto nla_put_failure;
381
382 if (rmnet_is_real_dev_registered(real_dev)) {
383 port = rmnet_get_port_rtnl(real_dev);
384 f.flags = port->data_format;
385 } else {
386 f.flags = 0;
387 }
388
389 f.mask = ~0;
390
391 if (nla_put(skb, attrtype: IFLA_RMNET_FLAGS, attrlen: sizeof(f), data: &f))
392 goto nla_put_failure;
393
394 return 0;
395
396nla_put_failure:
397 return -EMSGSIZE;
398}
399
400struct rtnl_link_ops rmnet_link_ops __read_mostly = {
401 .kind = "rmnet",
402 .maxtype = IFLA_RMNET_MAX,
403 .priv_size = sizeof(struct rmnet_priv),
404 .setup = rmnet_vnd_setup,
405 .validate = rmnet_rtnl_validate,
406 .newlink = rmnet_newlink,
407 .dellink = rmnet_dellink,
408 .get_size = rmnet_get_size,
409 .changelink = rmnet_changelink,
410 .policy = rmnet_policy,
411 .fill_info = rmnet_fill_info,
412};
413
414struct rmnet_port *rmnet_get_port_rcu(struct net_device *real_dev)
415{
416 if (rmnet_is_real_dev_registered(real_dev))
417 return rcu_dereference_bh(real_dev->rx_handler_data);
418 else
419 return NULL;
420}
421
422struct rmnet_endpoint *rmnet_get_endpoint(struct rmnet_port *port, u8 mux_id)
423{
424 struct rmnet_endpoint *ep;
425
426 hlist_for_each_entry_rcu(ep, &port->muxed_ep[mux_id], hlnode) {
427 if (ep->mux_id == mux_id)
428 return ep;
429 }
430
431 return NULL;
432}
433
434int rmnet_add_bridge(struct net_device *rmnet_dev,
435 struct net_device *slave_dev,
436 struct netlink_ext_ack *extack)
437{
438 struct rmnet_priv *priv = netdev_priv(dev: rmnet_dev);
439 struct net_device *real_dev = priv->real_dev;
440 struct rmnet_port *port, *slave_port;
441 int err;
442
443 port = rmnet_get_port_rtnl(real_dev);
444
445 /* If there is more than one rmnet dev attached, its probably being
446 * used for muxing. Skip the briding in that case
447 */
448 if (port->nr_rmnet_devs > 1) {
449 NL_SET_ERR_MSG_MOD(extack, "more than one rmnet dev attached");
450 return -EINVAL;
451 }
452
453 if (port->rmnet_mode != RMNET_EPMODE_VND) {
454 NL_SET_ERR_MSG_MOD(extack, "more than one bridge dev attached");
455 return -EINVAL;
456 }
457
458 if (rmnet_is_real_dev_registered(real_dev: slave_dev)) {
459 NL_SET_ERR_MSG_MOD(extack,
460 "slave cannot be another rmnet dev");
461
462 return -EBUSY;
463 }
464
465 err = rmnet_register_real_device(real_dev: slave_dev, extack);
466 if (err)
467 return -EBUSY;
468
469 err = netdev_master_upper_dev_link(dev: slave_dev, upper_dev: rmnet_dev, NULL, NULL,
470 extack);
471 if (err) {
472 rmnet_unregister_real_device(real_dev: slave_dev);
473 return err;
474 }
475
476 slave_port = rmnet_get_port_rtnl(real_dev: slave_dev);
477 slave_port->rmnet_mode = RMNET_EPMODE_BRIDGE;
478 slave_port->bridge_ep = real_dev;
479 slave_port->rmnet_dev = rmnet_dev;
480
481 port->rmnet_mode = RMNET_EPMODE_BRIDGE;
482 port->bridge_ep = slave_dev;
483
484 netdev_dbg(slave_dev, "registered with rmnet as slave\n");
485 return 0;
486}
487
488int rmnet_del_bridge(struct net_device *rmnet_dev,
489 struct net_device *slave_dev)
490{
491 struct rmnet_port *port = rmnet_get_port_rtnl(real_dev: slave_dev);
492
493 rmnet_unregister_bridge(port);
494
495 netdev_dbg(slave_dev, "removed from rmnet as slave\n");
496 return 0;
497}
498
499/* Startup/Shutdown */
500
501static int __init rmnet_init(void)
502{
503 int rc;
504
505 rc = register_netdevice_notifier(nb: &rmnet_dev_notifier);
506 if (rc != 0)
507 return rc;
508
509 rc = rtnl_link_register(ops: &rmnet_link_ops);
510 if (rc != 0) {
511 unregister_netdevice_notifier(nb: &rmnet_dev_notifier);
512 return rc;
513 }
514 return rc;
515}
516
517static void __exit rmnet_exit(void)
518{
519 rtnl_link_unregister(ops: &rmnet_link_ops);
520 unregister_netdevice_notifier(nb: &rmnet_dev_notifier);
521}
522
523module_init(rmnet_init)
524module_exit(rmnet_exit)
525MODULE_ALIAS_RTNL_LINK("rmnet");
526MODULE_DESCRIPTION("Qualcomm RmNet MAP driver");
527MODULE_LICENSE("GPL v2");
528

source code of linux/drivers/net/ethernet/qualcomm/rmnet/rmnet_config.c