1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | /* |
3 | * File: pn_dev.c |
4 | * |
5 | * Phonet network device |
6 | * |
7 | * Copyright (C) 2008 Nokia Corporation. |
8 | * |
9 | * Authors: Sakari Ailus <sakari.ailus@nokia.com> |
10 | * RĂ©mi Denis-Courmont |
11 | */ |
12 | |
13 | #include <linux/kernel.h> |
14 | #include <linux/net.h> |
15 | #include <linux/slab.h> |
16 | #include <linux/netdevice.h> |
17 | #include <linux/phonet.h> |
18 | #include <linux/proc_fs.h> |
19 | #include <linux/if_arp.h> |
20 | #include <net/sock.h> |
21 | #include <net/netns/generic.h> |
22 | #include <net/phonet/pn_dev.h> |
23 | |
24 | struct phonet_routes { |
25 | struct mutex lock; |
26 | struct net_device __rcu *table[64]; |
27 | }; |
28 | |
29 | struct phonet_net { |
30 | struct phonet_device_list pndevs; |
31 | struct phonet_routes routes; |
32 | }; |
33 | |
34 | static unsigned int phonet_net_id __read_mostly; |
35 | |
36 | static struct phonet_net *phonet_pernet(struct net *net) |
37 | { |
38 | return net_generic(net, id: phonet_net_id); |
39 | } |
40 | |
41 | struct phonet_device_list *phonet_device_list(struct net *net) |
42 | { |
43 | struct phonet_net *pnn = phonet_pernet(net); |
44 | return &pnn->pndevs; |
45 | } |
46 | |
47 | /* Allocate new Phonet device. */ |
48 | static struct phonet_device *__phonet_device_alloc(struct net_device *dev) |
49 | { |
50 | struct phonet_device_list *pndevs = phonet_device_list(net: dev_net(dev)); |
51 | struct phonet_device *pnd = kmalloc(size: sizeof(*pnd), GFP_ATOMIC); |
52 | if (pnd == NULL) |
53 | return NULL; |
54 | pnd->netdev = dev; |
55 | bitmap_zero(dst: pnd->addrs, nbits: 64); |
56 | |
57 | BUG_ON(!mutex_is_locked(&pndevs->lock)); |
58 | list_add_rcu(new: &pnd->list, head: &pndevs->list); |
59 | return pnd; |
60 | } |
61 | |
62 | static struct phonet_device *__phonet_get(struct net_device *dev) |
63 | { |
64 | struct phonet_device_list *pndevs = phonet_device_list(net: dev_net(dev)); |
65 | struct phonet_device *pnd; |
66 | |
67 | BUG_ON(!mutex_is_locked(&pndevs->lock)); |
68 | list_for_each_entry(pnd, &pndevs->list, list) { |
69 | if (pnd->netdev == dev) |
70 | return pnd; |
71 | } |
72 | return NULL; |
73 | } |
74 | |
75 | static struct phonet_device *__phonet_get_rcu(struct net_device *dev) |
76 | { |
77 | struct phonet_device_list *pndevs = phonet_device_list(net: dev_net(dev)); |
78 | struct phonet_device *pnd; |
79 | |
80 | list_for_each_entry_rcu(pnd, &pndevs->list, list) { |
81 | if (pnd->netdev == dev) |
82 | return pnd; |
83 | } |
84 | return NULL; |
85 | } |
86 | |
87 | static void phonet_device_destroy(struct net_device *dev) |
88 | { |
89 | struct phonet_device_list *pndevs = phonet_device_list(net: dev_net(dev)); |
90 | struct phonet_device *pnd; |
91 | |
92 | ASSERT_RTNL(); |
93 | |
94 | mutex_lock(&pndevs->lock); |
95 | pnd = __phonet_get(dev); |
96 | if (pnd) |
97 | list_del_rcu(entry: &pnd->list); |
98 | mutex_unlock(lock: &pndevs->lock); |
99 | |
100 | if (pnd) { |
101 | u8 addr; |
102 | |
103 | for_each_set_bit(addr, pnd->addrs, 64) |
104 | phonet_address_notify(RTM_DELADDR, dev, addr); |
105 | kfree(objp: pnd); |
106 | } |
107 | } |
108 | |
109 | struct net_device *phonet_device_get(struct net *net) |
110 | { |
111 | struct phonet_device_list *pndevs = phonet_device_list(net); |
112 | struct phonet_device *pnd; |
113 | struct net_device *dev = NULL; |
114 | |
115 | rcu_read_lock(); |
116 | list_for_each_entry_rcu(pnd, &pndevs->list, list) { |
117 | dev = pnd->netdev; |
118 | BUG_ON(!dev); |
119 | |
120 | if ((dev->reg_state == NETREG_REGISTERED) && |
121 | ((pnd->netdev->flags & IFF_UP)) == IFF_UP) |
122 | break; |
123 | dev = NULL; |
124 | } |
125 | dev_hold(dev); |
126 | rcu_read_unlock(); |
127 | return dev; |
128 | } |
129 | |
130 | int phonet_address_add(struct net_device *dev, u8 addr) |
131 | { |
132 | struct phonet_device_list *pndevs = phonet_device_list(net: dev_net(dev)); |
133 | struct phonet_device *pnd; |
134 | int err = 0; |
135 | |
136 | mutex_lock(&pndevs->lock); |
137 | /* Find or create Phonet-specific device data */ |
138 | pnd = __phonet_get(dev); |
139 | if (pnd == NULL) |
140 | pnd = __phonet_device_alloc(dev); |
141 | if (unlikely(pnd == NULL)) |
142 | err = -ENOMEM; |
143 | else if (test_and_set_bit(nr: addr >> 2, addr: pnd->addrs)) |
144 | err = -EEXIST; |
145 | mutex_unlock(lock: &pndevs->lock); |
146 | return err; |
147 | } |
148 | |
149 | int phonet_address_del(struct net_device *dev, u8 addr) |
150 | { |
151 | struct phonet_device_list *pndevs = phonet_device_list(net: dev_net(dev)); |
152 | struct phonet_device *pnd; |
153 | int err = 0; |
154 | |
155 | mutex_lock(&pndevs->lock); |
156 | pnd = __phonet_get(dev); |
157 | if (!pnd || !test_and_clear_bit(nr: addr >> 2, addr: pnd->addrs)) { |
158 | err = -EADDRNOTAVAIL; |
159 | pnd = NULL; |
160 | } else if (bitmap_empty(src: pnd->addrs, nbits: 64)) |
161 | list_del_rcu(entry: &pnd->list); |
162 | else |
163 | pnd = NULL; |
164 | mutex_unlock(lock: &pndevs->lock); |
165 | |
166 | if (pnd) |
167 | kfree_rcu(pnd, rcu); |
168 | |
169 | return err; |
170 | } |
171 | |
172 | /* Gets a source address toward a destination, through a interface. */ |
173 | u8 phonet_address_get(struct net_device *dev, u8 daddr) |
174 | { |
175 | struct phonet_device *pnd; |
176 | u8 saddr; |
177 | |
178 | rcu_read_lock(); |
179 | pnd = __phonet_get_rcu(dev); |
180 | if (pnd) { |
181 | BUG_ON(bitmap_empty(pnd->addrs, 64)); |
182 | |
183 | /* Use same source address as destination, if possible */ |
184 | if (test_bit(daddr >> 2, pnd->addrs)) |
185 | saddr = daddr; |
186 | else |
187 | saddr = find_first_bit(addr: pnd->addrs, size: 64) << 2; |
188 | } else |
189 | saddr = PN_NO_ADDR; |
190 | rcu_read_unlock(); |
191 | |
192 | if (saddr == PN_NO_ADDR) { |
193 | /* Fallback to another device */ |
194 | struct net_device *def_dev; |
195 | |
196 | def_dev = phonet_device_get(net: dev_net(dev)); |
197 | if (def_dev) { |
198 | if (def_dev != dev) |
199 | saddr = phonet_address_get(dev: def_dev, daddr); |
200 | dev_put(dev: def_dev); |
201 | } |
202 | } |
203 | return saddr; |
204 | } |
205 | |
206 | int phonet_address_lookup(struct net *net, u8 addr) |
207 | { |
208 | struct phonet_device_list *pndevs = phonet_device_list(net); |
209 | struct phonet_device *pnd; |
210 | int err = -EADDRNOTAVAIL; |
211 | |
212 | rcu_read_lock(); |
213 | list_for_each_entry_rcu(pnd, &pndevs->list, list) { |
214 | /* Don't allow unregistering devices! */ |
215 | if ((pnd->netdev->reg_state != NETREG_REGISTERED) || |
216 | ((pnd->netdev->flags & IFF_UP)) != IFF_UP) |
217 | continue; |
218 | |
219 | if (test_bit(addr >> 2, pnd->addrs)) { |
220 | err = 0; |
221 | goto found; |
222 | } |
223 | } |
224 | found: |
225 | rcu_read_unlock(); |
226 | return err; |
227 | } |
228 | |
229 | /* automatically configure a Phonet device, if supported */ |
230 | static int phonet_device_autoconf(struct net_device *dev) |
231 | { |
232 | struct if_phonet_req req; |
233 | int ret; |
234 | |
235 | if (!dev->netdev_ops->ndo_siocdevprivate) |
236 | return -EOPNOTSUPP; |
237 | |
238 | ret = dev->netdev_ops->ndo_siocdevprivate(dev, (struct ifreq *)&req, |
239 | NULL, SIOCPNGAUTOCONF); |
240 | if (ret < 0) |
241 | return ret; |
242 | |
243 | ASSERT_RTNL(); |
244 | ret = phonet_address_add(dev, addr: req.ifr_phonet_autoconf.device); |
245 | if (ret) |
246 | return ret; |
247 | phonet_address_notify(RTM_NEWADDR, dev, |
248 | addr: req.ifr_phonet_autoconf.device); |
249 | return 0; |
250 | } |
251 | |
252 | static void phonet_route_autodel(struct net_device *dev) |
253 | { |
254 | struct phonet_net *pnn = phonet_pernet(net: dev_net(dev)); |
255 | unsigned int i; |
256 | DECLARE_BITMAP(deleted, 64); |
257 | |
258 | /* Remove left-over Phonet routes */ |
259 | bitmap_zero(dst: deleted, nbits: 64); |
260 | mutex_lock(&pnn->routes.lock); |
261 | for (i = 0; i < 64; i++) |
262 | if (rcu_access_pointer(pnn->routes.table[i]) == dev) { |
263 | RCU_INIT_POINTER(pnn->routes.table[i], NULL); |
264 | set_bit(nr: i, addr: deleted); |
265 | } |
266 | mutex_unlock(lock: &pnn->routes.lock); |
267 | |
268 | if (bitmap_empty(src: deleted, nbits: 64)) |
269 | return; /* short-circuit RCU */ |
270 | synchronize_rcu(); |
271 | for_each_set_bit(i, deleted, 64) { |
272 | rtm_phonet_notify(RTM_DELROUTE, dev, dst: i); |
273 | dev_put(dev); |
274 | } |
275 | } |
276 | |
277 | /* notify Phonet of device events */ |
278 | static int phonet_device_notify(struct notifier_block *me, unsigned long what, |
279 | void *ptr) |
280 | { |
281 | struct net_device *dev = netdev_notifier_info_to_dev(info: ptr); |
282 | |
283 | switch (what) { |
284 | case NETDEV_REGISTER: |
285 | if (dev->type == ARPHRD_PHONET) |
286 | phonet_device_autoconf(dev); |
287 | break; |
288 | case NETDEV_UNREGISTER: |
289 | phonet_device_destroy(dev); |
290 | phonet_route_autodel(dev); |
291 | break; |
292 | } |
293 | return 0; |
294 | |
295 | } |
296 | |
297 | static struct notifier_block phonet_device_notifier = { |
298 | .notifier_call = phonet_device_notify, |
299 | .priority = 0, |
300 | }; |
301 | |
302 | /* Per-namespace Phonet devices handling */ |
303 | static int __net_init phonet_init_net(struct net *net) |
304 | { |
305 | struct phonet_net *pnn = phonet_pernet(net); |
306 | |
307 | if (!proc_create_net("phonet" , 0, net->proc_net, &pn_sock_seq_ops, |
308 | sizeof(struct seq_net_private))) |
309 | return -ENOMEM; |
310 | |
311 | INIT_LIST_HEAD(list: &pnn->pndevs.list); |
312 | mutex_init(&pnn->pndevs.lock); |
313 | mutex_init(&pnn->routes.lock); |
314 | return 0; |
315 | } |
316 | |
317 | static void __net_exit phonet_exit_net(struct net *net) |
318 | { |
319 | struct phonet_net *pnn = phonet_pernet(net); |
320 | |
321 | remove_proc_entry("phonet" , net->proc_net); |
322 | WARN_ON_ONCE(!list_empty(&pnn->pndevs.list)); |
323 | } |
324 | |
325 | static struct pernet_operations phonet_net_ops = { |
326 | .init = phonet_init_net, |
327 | .exit = phonet_exit_net, |
328 | .id = &phonet_net_id, |
329 | .size = sizeof(struct phonet_net), |
330 | }; |
331 | |
332 | /* Initialize Phonet devices list */ |
333 | int __init phonet_device_init(void) |
334 | { |
335 | int err = register_pernet_subsys(&phonet_net_ops); |
336 | if (err) |
337 | return err; |
338 | |
339 | proc_create_net("pnresource" , 0, init_net.proc_net, &pn_res_seq_ops, |
340 | sizeof(struct seq_net_private)); |
341 | register_netdevice_notifier(nb: &phonet_device_notifier); |
342 | err = phonet_netlink_register(); |
343 | if (err) |
344 | phonet_device_exit(); |
345 | return err; |
346 | } |
347 | |
348 | void phonet_device_exit(void) |
349 | { |
350 | rtnl_unregister_all(PF_PHONET); |
351 | unregister_netdevice_notifier(nb: &phonet_device_notifier); |
352 | unregister_pernet_subsys(&phonet_net_ops); |
353 | remove_proc_entry("pnresource" , init_net.proc_net); |
354 | } |
355 | |
356 | int phonet_route_add(struct net_device *dev, u8 daddr) |
357 | { |
358 | struct phonet_net *pnn = phonet_pernet(net: dev_net(dev)); |
359 | struct phonet_routes *routes = &pnn->routes; |
360 | int err = -EEXIST; |
361 | |
362 | daddr = daddr >> 2; |
363 | mutex_lock(&routes->lock); |
364 | if (routes->table[daddr] == NULL) { |
365 | rcu_assign_pointer(routes->table[daddr], dev); |
366 | dev_hold(dev); |
367 | err = 0; |
368 | } |
369 | mutex_unlock(lock: &routes->lock); |
370 | return err; |
371 | } |
372 | |
373 | int phonet_route_del(struct net_device *dev, u8 daddr) |
374 | { |
375 | struct phonet_net *pnn = phonet_pernet(net: dev_net(dev)); |
376 | struct phonet_routes *routes = &pnn->routes; |
377 | |
378 | daddr = daddr >> 2; |
379 | mutex_lock(&routes->lock); |
380 | if (rcu_access_pointer(routes->table[daddr]) == dev) |
381 | RCU_INIT_POINTER(routes->table[daddr], NULL); |
382 | else |
383 | dev = NULL; |
384 | mutex_unlock(lock: &routes->lock); |
385 | |
386 | if (!dev) |
387 | return -ENOENT; |
388 | synchronize_rcu(); |
389 | dev_put(dev); |
390 | return 0; |
391 | } |
392 | |
393 | struct net_device *phonet_route_get_rcu(struct net *net, u8 daddr) |
394 | { |
395 | struct phonet_net *pnn = phonet_pernet(net); |
396 | struct phonet_routes *routes = &pnn->routes; |
397 | struct net_device *dev; |
398 | |
399 | daddr >>= 2; |
400 | dev = rcu_dereference(routes->table[daddr]); |
401 | return dev; |
402 | } |
403 | |
404 | struct net_device *phonet_route_output(struct net *net, u8 daddr) |
405 | { |
406 | struct phonet_net *pnn = phonet_pernet(net); |
407 | struct phonet_routes *routes = &pnn->routes; |
408 | struct net_device *dev; |
409 | |
410 | daddr >>= 2; |
411 | rcu_read_lock(); |
412 | dev = rcu_dereference(routes->table[daddr]); |
413 | dev_hold(dev); |
414 | rcu_read_unlock(); |
415 | |
416 | if (!dev) |
417 | dev = phonet_device_get(net); /* Default route */ |
418 | return dev; |
419 | } |
420 | |