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