1 /*
2    BNEP implementation for Linux Bluetooth stack (BlueZ).
3    Copyright (C) 2001-2002 Inventel Systemes
4    Written 2001-2002 by
5 	Clément Moreau <clement.moreau@inventel.fr>
6 	David Libault  <david.libault@inventel.fr>
7 
8    Copyright (C) 2002 Maxim Krasnyansky <maxk@qualcomm.com>
9 
10    This program is free software; you can redistribute it and/or modify
11    it under the terms of the GNU General Public License version 2 as
12    published by the Free Software Foundation;
13 
14    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
15    OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
17    IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
18    CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
19    WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
20    ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
21    OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
22 
23    ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
24    COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
25    SOFTWARE IS DISCLAIMED.
26 */
27 
28 #include <linux/module.h>
29 #include <linux/kthread.h>
30 #include <linux/file.h>
31 #include <linux/etherdevice.h>
32 #include <asm/unaligned.h>
33 
34 #include <net/bluetooth/bluetooth.h>
35 #include <net/bluetooth/l2cap.h>
36 #include <net/bluetooth/hci_core.h>
37 
38 #include "bnep.h"
39 
40 #define VERSION "1.3"
41 
42 static bool compress_src = true;
43 static bool compress_dst = true;
44 
45 static LIST_HEAD(bnep_session_list);
46 static DECLARE_RWSEM(bnep_session_sem);
47 
__bnep_get_session(u8 * dst)48 static struct bnep_session *__bnep_get_session(u8 *dst)
49 {
50 	struct bnep_session *s;
51 
52 	BT_DBG("");
53 
54 	list_for_each_entry(s, &bnep_session_list, list)
55 		if (ether_addr_equal(dst, s->eh.h_source))
56 			return s;
57 
58 	return NULL;
59 }
60 
__bnep_link_session(struct bnep_session * s)61 static void __bnep_link_session(struct bnep_session *s)
62 {
63 	list_add(&s->list, &bnep_session_list);
64 }
65 
__bnep_unlink_session(struct bnep_session * s)66 static void __bnep_unlink_session(struct bnep_session *s)
67 {
68 	list_del(&s->list);
69 }
70 
bnep_send(struct bnep_session * s,void * data,size_t len)71 static int bnep_send(struct bnep_session *s, void *data, size_t len)
72 {
73 	struct socket *sock = s->sock;
74 	struct kvec iv = { data, len };
75 
76 	return kernel_sendmsg(sock, &s->msg, &iv, 1, len);
77 }
78 
bnep_send_rsp(struct bnep_session * s,u8 ctrl,u16 resp)79 static int bnep_send_rsp(struct bnep_session *s, u8 ctrl, u16 resp)
80 {
81 	struct bnep_control_rsp rsp;
82 	rsp.type = BNEP_CONTROL;
83 	rsp.ctrl = ctrl;
84 	rsp.resp = htons(resp);
85 	return bnep_send(s, &rsp, sizeof(rsp));
86 }
87 
88 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
bnep_set_default_proto_filter(struct bnep_session * s)89 static inline void bnep_set_default_proto_filter(struct bnep_session *s)
90 {
91 	/* (IPv4, ARP)  */
92 	s->proto_filter[0].start = ETH_P_IP;
93 	s->proto_filter[0].end   = ETH_P_ARP;
94 	/* (RARP, AppleTalk) */
95 	s->proto_filter[1].start = ETH_P_RARP;
96 	s->proto_filter[1].end   = ETH_P_AARP;
97 	/* (IPX, IPv6) */
98 	s->proto_filter[2].start = ETH_P_IPX;
99 	s->proto_filter[2].end   = ETH_P_IPV6;
100 }
101 #endif
102 
bnep_ctrl_set_netfilter(struct bnep_session * s,__be16 * data,int len)103 static int bnep_ctrl_set_netfilter(struct bnep_session *s, __be16 *data, int len)
104 {
105 	int n;
106 
107 	if (len < 2)
108 		return -EILSEQ;
109 
110 	n = get_unaligned_be16(data);
111 	data++;
112 	len -= 2;
113 
114 	if (len < n)
115 		return -EILSEQ;
116 
117 	BT_DBG("filter len %d", n);
118 
119 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
120 	n /= 4;
121 	if (n <= BNEP_MAX_PROTO_FILTERS) {
122 		struct bnep_proto_filter *f = s->proto_filter;
123 		int i;
124 
125 		for (i = 0; i < n; i++) {
126 			f[i].start = get_unaligned_be16(data++);
127 			f[i].end   = get_unaligned_be16(data++);
128 
129 			BT_DBG("proto filter start %u end %u",
130 			       f[i].start, f[i].end);
131 		}
132 
133 		if (i < BNEP_MAX_PROTO_FILTERS)
134 			memset(f + i, 0, sizeof(*f));
135 
136 		if (n == 0)
137 			bnep_set_default_proto_filter(s);
138 
139 		bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_SUCCESS);
140 	} else {
141 		bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_LIMIT_REACHED);
142 	}
143 #else
144 	bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_UNSUPPORTED_REQ);
145 #endif
146 	return 0;
147 }
148 
bnep_ctrl_set_mcfilter(struct bnep_session * s,u8 * data,int len)149 static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len)
150 {
151 	int n;
152 
153 	if (len < 2)
154 		return -EILSEQ;
155 
156 	n = get_unaligned_be16(data);
157 	data += 2;
158 	len -= 2;
159 
160 	if (len < n)
161 		return -EILSEQ;
162 
163 	BT_DBG("filter len %d", n);
164 
165 #ifdef CONFIG_BT_BNEP_MC_FILTER
166 	n /= (ETH_ALEN * 2);
167 
168 	if (n > 0) {
169 		int i;
170 
171 		s->mc_filter = 0;
172 
173 		/* Always send broadcast */
174 		set_bit(bnep_mc_hash(s->dev->broadcast), (ulong *) &s->mc_filter);
175 
176 		/* Add address ranges to the multicast hash */
177 		for (; n > 0; n--) {
178 			u8 a1[6], *a2;
179 
180 			memcpy(a1, data, ETH_ALEN);
181 			data += ETH_ALEN;
182 			a2 = data;
183 			data += ETH_ALEN;
184 
185 			BT_DBG("mc filter %pMR -> %pMR", a1, a2);
186 
187 			/* Iterate from a1 to a2 */
188 			set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
189 			while (memcmp(a1, a2, 6) < 0 && s->mc_filter != ~0LL) {
190 				/* Increment a1 */
191 				i = 5;
192 				while (i >= 0 && ++a1[i--] == 0)
193 					;
194 
195 				set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
196 			}
197 		}
198 	}
199 
200 	BT_DBG("mc filter hash 0x%llx", s->mc_filter);
201 
202 	bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_SUCCESS);
203 #else
204 	bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_FILTER_UNSUPPORTED_REQ);
205 #endif
206 	return 0;
207 }
208 
bnep_rx_control(struct bnep_session * s,void * data,int len)209 static int bnep_rx_control(struct bnep_session *s, void *data, int len)
210 {
211 	u8  cmd = *(u8 *)data;
212 	int err = 0;
213 
214 	data++;
215 	len--;
216 
217 	switch (cmd) {
218 	case BNEP_CMD_NOT_UNDERSTOOD:
219 	case BNEP_SETUP_CONN_RSP:
220 	case BNEP_FILTER_NET_TYPE_RSP:
221 	case BNEP_FILTER_MULTI_ADDR_RSP:
222 		/* Ignore these for now */
223 		break;
224 
225 	case BNEP_FILTER_NET_TYPE_SET:
226 		err = bnep_ctrl_set_netfilter(s, data, len);
227 		break;
228 
229 	case BNEP_FILTER_MULTI_ADDR_SET:
230 		err = bnep_ctrl_set_mcfilter(s, data, len);
231 		break;
232 
233 	case BNEP_SETUP_CONN_REQ:
234 		/* Successful response should be sent only once */
235 		if (test_bit(BNEP_SETUP_RESPONSE, &s->flags) &&
236 		    !test_and_set_bit(BNEP_SETUP_RSP_SENT, &s->flags))
237 			err = bnep_send_rsp(s, BNEP_SETUP_CONN_RSP,
238 					    BNEP_SUCCESS);
239 		else
240 			err = bnep_send_rsp(s, BNEP_SETUP_CONN_RSP,
241 					    BNEP_CONN_NOT_ALLOWED);
242 		break;
243 
244 	default: {
245 			u8 pkt[3];
246 			pkt[0] = BNEP_CONTROL;
247 			pkt[1] = BNEP_CMD_NOT_UNDERSTOOD;
248 			pkt[2] = cmd;
249 			err = bnep_send(s, pkt, sizeof(pkt));
250 		}
251 		break;
252 	}
253 
254 	return err;
255 }
256 
bnep_rx_extension(struct bnep_session * s,struct sk_buff * skb)257 static int bnep_rx_extension(struct bnep_session *s, struct sk_buff *skb)
258 {
259 	struct bnep_ext_hdr *h;
260 	int err = 0;
261 
262 	do {
263 		h = (void *) skb->data;
264 		if (!skb_pull(skb, sizeof(*h))) {
265 			err = -EILSEQ;
266 			break;
267 		}
268 
269 		BT_DBG("type 0x%x len %u", h->type, h->len);
270 
271 		switch (h->type & BNEP_TYPE_MASK) {
272 		case BNEP_EXT_CONTROL:
273 			bnep_rx_control(s, skb->data, skb->len);
274 			break;
275 
276 		default:
277 			/* Unknown extension, skip it. */
278 			break;
279 		}
280 
281 		if (!skb_pull(skb, h->len)) {
282 			err = -EILSEQ;
283 			break;
284 		}
285 	} while (!err && (h->type & BNEP_EXT_HEADER));
286 
287 	return err;
288 }
289 
290 static u8 __bnep_rx_hlen[] = {
291 	ETH_HLEN,     /* BNEP_GENERAL */
292 	0,            /* BNEP_CONTROL */
293 	2,            /* BNEP_COMPRESSED */
294 	ETH_ALEN + 2, /* BNEP_COMPRESSED_SRC_ONLY */
295 	ETH_ALEN + 2  /* BNEP_COMPRESSED_DST_ONLY */
296 };
297 
bnep_rx_frame(struct bnep_session * s,struct sk_buff * skb)298 static int bnep_rx_frame(struct bnep_session *s, struct sk_buff *skb)
299 {
300 	struct net_device *dev = s->dev;
301 	struct sk_buff *nskb;
302 	u8 type, ctrl_type;
303 
304 	dev->stats.rx_bytes += skb->len;
305 
306 	type = *(u8 *) skb->data;
307 	skb_pull(skb, 1);
308 	ctrl_type = *(u8 *)skb->data;
309 
310 	if ((type & BNEP_TYPE_MASK) >= sizeof(__bnep_rx_hlen))
311 		goto badframe;
312 
313 	if ((type & BNEP_TYPE_MASK) == BNEP_CONTROL) {
314 		if (bnep_rx_control(s, skb->data, skb->len) < 0) {
315 			dev->stats.tx_errors++;
316 			kfree_skb(skb);
317 			return 0;
318 		}
319 
320 		if (!(type & BNEP_EXT_HEADER)) {
321 			kfree_skb(skb);
322 			return 0;
323 		}
324 
325 		/* Verify and pull ctrl message since it's already processed */
326 		switch (ctrl_type) {
327 		case BNEP_SETUP_CONN_REQ:
328 			/* Pull: ctrl type (1 b), len (1 b), data (len bytes) */
329 			if (!skb_pull(skb, 2 + *(u8 *)(skb->data + 1) * 2))
330 				goto badframe;
331 			break;
332 		case BNEP_FILTER_MULTI_ADDR_SET:
333 		case BNEP_FILTER_NET_TYPE_SET:
334 			/* Pull: ctrl type (1 b), len (2 b), data (len bytes) */
335 			if (!skb_pull(skb, 3 + *(u16 *)(skb->data + 1) * 2))
336 				goto badframe;
337 			break;
338 		default:
339 			kfree_skb(skb);
340 			return 0;
341 		}
342 	} else {
343 		skb_reset_mac_header(skb);
344 
345 		/* Verify and pull out header */
346 		if (!skb_pull(skb, __bnep_rx_hlen[type & BNEP_TYPE_MASK]))
347 			goto badframe;
348 
349 		s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2));
350 	}
351 
352 	if (type & BNEP_EXT_HEADER) {
353 		if (bnep_rx_extension(s, skb) < 0)
354 			goto badframe;
355 	}
356 
357 	/* Strip 802.1p header */
358 	if (ntohs(s->eh.h_proto) == ETH_P_8021Q) {
359 		if (!skb_pull(skb, 4))
360 			goto badframe;
361 		s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2));
362 	}
363 
364 	/* We have to alloc new skb and copy data here :(. Because original skb
365 	 * may not be modified and because of the alignment requirements. */
366 	nskb = alloc_skb(2 + ETH_HLEN + skb->len, GFP_KERNEL);
367 	if (!nskb) {
368 		dev->stats.rx_dropped++;
369 		kfree_skb(skb);
370 		return -ENOMEM;
371 	}
372 	skb_reserve(nskb, 2);
373 
374 	/* Decompress header and construct ether frame */
375 	switch (type & BNEP_TYPE_MASK) {
376 	case BNEP_COMPRESSED:
377 		__skb_put_data(nskb, &s->eh, ETH_HLEN);
378 		break;
379 
380 	case BNEP_COMPRESSED_SRC_ONLY:
381 		__skb_put_data(nskb, s->eh.h_dest, ETH_ALEN);
382 		__skb_put_data(nskb, skb_mac_header(skb), ETH_ALEN);
383 		put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2));
384 		break;
385 
386 	case BNEP_COMPRESSED_DST_ONLY:
387 		__skb_put_data(nskb, skb_mac_header(skb), ETH_ALEN);
388 		__skb_put_data(nskb, s->eh.h_source, ETH_ALEN + 2);
389 		break;
390 
391 	case BNEP_GENERAL:
392 		__skb_put_data(nskb, skb_mac_header(skb), ETH_ALEN * 2);
393 		put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2));
394 		break;
395 	}
396 
397 	skb_copy_from_linear_data(skb, __skb_put(nskb, skb->len), skb->len);
398 	kfree_skb(skb);
399 
400 	dev->stats.rx_packets++;
401 	nskb->ip_summed = CHECKSUM_NONE;
402 	nskb->protocol  = eth_type_trans(nskb, dev);
403 	netif_rx(nskb);
404 	return 0;
405 
406 badframe:
407 	dev->stats.rx_errors++;
408 	kfree_skb(skb);
409 	return 0;
410 }
411 
412 static u8 __bnep_tx_types[] = {
413 	BNEP_GENERAL,
414 	BNEP_COMPRESSED_SRC_ONLY,
415 	BNEP_COMPRESSED_DST_ONLY,
416 	BNEP_COMPRESSED
417 };
418 
bnep_tx_frame(struct bnep_session * s,struct sk_buff * skb)419 static int bnep_tx_frame(struct bnep_session *s, struct sk_buff *skb)
420 {
421 	struct ethhdr *eh = (void *) skb->data;
422 	struct socket *sock = s->sock;
423 	struct kvec iv[3];
424 	int len = 0, il = 0;
425 	u8 type = 0;
426 
427 	BT_DBG("skb %p dev %p type %u", skb, skb->dev, skb->pkt_type);
428 
429 	if (!skb->dev) {
430 		/* Control frame sent by us */
431 		goto send;
432 	}
433 
434 	iv[il++] = (struct kvec) { &type, 1 };
435 	len++;
436 
437 	if (compress_src && ether_addr_equal(eh->h_dest, s->eh.h_source))
438 		type |= 0x01;
439 
440 	if (compress_dst && ether_addr_equal(eh->h_source, s->eh.h_dest))
441 		type |= 0x02;
442 
443 	if (type)
444 		skb_pull(skb, ETH_ALEN * 2);
445 
446 	type = __bnep_tx_types[type];
447 	switch (type) {
448 	case BNEP_COMPRESSED_SRC_ONLY:
449 		iv[il++] = (struct kvec) { eh->h_source, ETH_ALEN };
450 		len += ETH_ALEN;
451 		break;
452 
453 	case BNEP_COMPRESSED_DST_ONLY:
454 		iv[il++] = (struct kvec) { eh->h_dest, ETH_ALEN };
455 		len += ETH_ALEN;
456 		break;
457 	}
458 
459 send:
460 	iv[il++] = (struct kvec) { skb->data, skb->len };
461 	len += skb->len;
462 
463 	/* FIXME: linearize skb */
464 	{
465 		len = kernel_sendmsg(sock, &s->msg, iv, il, len);
466 	}
467 	kfree_skb(skb);
468 
469 	if (len > 0) {
470 		s->dev->stats.tx_bytes += len;
471 		s->dev->stats.tx_packets++;
472 		return 0;
473 	}
474 
475 	return len;
476 }
477 
bnep_session(void * arg)478 static int bnep_session(void *arg)
479 {
480 	struct bnep_session *s = arg;
481 	struct net_device *dev = s->dev;
482 	struct sock *sk = s->sock->sk;
483 	struct sk_buff *skb;
484 	DEFINE_WAIT_FUNC(wait, woken_wake_function);
485 
486 	BT_DBG("");
487 
488 	set_user_nice(current, -15);
489 
490 	add_wait_queue(sk_sleep(sk), &wait);
491 	while (1) {
492 		if (atomic_read(&s->terminate))
493 			break;
494 		/* RX */
495 		while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
496 			skb_orphan(skb);
497 			if (!skb_linearize(skb))
498 				bnep_rx_frame(s, skb);
499 			else
500 				kfree_skb(skb);
501 		}
502 
503 		if (sk->sk_state != BT_CONNECTED)
504 			break;
505 
506 		/* TX */
507 		while ((skb = skb_dequeue(&sk->sk_write_queue)))
508 			if (bnep_tx_frame(s, skb))
509 				break;
510 		netif_wake_queue(dev);
511 
512 		/*
513 		 * wait_woken() performs the necessary memory barriers
514 		 * for us; see the header comment for this primitive.
515 		 */
516 		wait_woken(&wait, TASK_INTERRUPTIBLE, MAX_SCHEDULE_TIMEOUT);
517 	}
518 	remove_wait_queue(sk_sleep(sk), &wait);
519 
520 	/* Cleanup session */
521 	down_write(&bnep_session_sem);
522 
523 	/* Delete network device */
524 	unregister_netdev(dev);
525 
526 	/* Wakeup user-space polling for socket errors */
527 	s->sock->sk->sk_err = EUNATCH;
528 
529 	wake_up_interruptible(sk_sleep(s->sock->sk));
530 
531 	/* Release the socket */
532 	fput(s->sock->file);
533 
534 	__bnep_unlink_session(s);
535 
536 	up_write(&bnep_session_sem);
537 	free_netdev(dev);
538 	module_put_and_kthread_exit(0);
539 	return 0;
540 }
541 
bnep_get_device(struct bnep_session * session)542 static struct device *bnep_get_device(struct bnep_session *session)
543 {
544 	struct l2cap_conn *conn = l2cap_pi(session->sock->sk)->chan->conn;
545 
546 	if (!conn || !conn->hcon)
547 		return NULL;
548 
549 	return &conn->hcon->dev;
550 }
551 
552 static struct device_type bnep_type = {
553 	.name	= "bluetooth",
554 };
555 
bnep_add_connection(struct bnep_connadd_req * req,struct socket * sock)556 int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock)
557 {
558 	u32 valid_flags = BIT(BNEP_SETUP_RESPONSE);
559 	struct net_device *dev;
560 	struct bnep_session *s, *ss;
561 	u8 dst[ETH_ALEN], src[ETH_ALEN];
562 	int err;
563 
564 	BT_DBG("");
565 
566 	if (!l2cap_is_socket(sock))
567 		return -EBADFD;
568 
569 	if (req->flags & ~valid_flags)
570 		return -EINVAL;
571 
572 	baswap((void *) dst, &l2cap_pi(sock->sk)->chan->dst);
573 	baswap((void *) src, &l2cap_pi(sock->sk)->chan->src);
574 
575 	/* session struct allocated as private part of net_device */
576 	dev = alloc_netdev(sizeof(struct bnep_session),
577 			   (*req->device) ? req->device : "bnep%d",
578 			   NET_NAME_UNKNOWN,
579 			   bnep_net_setup);
580 	if (!dev)
581 		return -ENOMEM;
582 
583 	down_write(&bnep_session_sem);
584 
585 	ss = __bnep_get_session(dst);
586 	if (ss && ss->state == BT_CONNECTED) {
587 		err = -EEXIST;
588 		goto failed;
589 	}
590 
591 	s = netdev_priv(dev);
592 
593 	/* This is rx header therefore addresses are swapped.
594 	 * ie. eh.h_dest is our local address. */
595 	memcpy(s->eh.h_dest,   &src, ETH_ALEN);
596 	memcpy(s->eh.h_source, &dst, ETH_ALEN);
597 	eth_hw_addr_set(dev, s->eh.h_dest);
598 
599 	s->dev   = dev;
600 	s->sock  = sock;
601 	s->role  = req->role;
602 	s->state = BT_CONNECTED;
603 	s->flags = req->flags;
604 
605 	s->msg.msg_flags = MSG_NOSIGNAL;
606 
607 #ifdef CONFIG_BT_BNEP_MC_FILTER
608 	/* Set default mc filter to not filter out any mc addresses
609 	 * as defined in the BNEP specification (revision 0.95a)
610 	 * http://grouper.ieee.org/groups/802/15/Bluetooth/BNEP.pdf
611 	 */
612 	s->mc_filter = ~0LL;
613 #endif
614 
615 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
616 	/* Set default protocol filter */
617 	bnep_set_default_proto_filter(s);
618 #endif
619 
620 	SET_NETDEV_DEV(dev, bnep_get_device(s));
621 	SET_NETDEV_DEVTYPE(dev, &bnep_type);
622 
623 	err = register_netdev(dev);
624 	if (err)
625 		goto failed;
626 
627 	__bnep_link_session(s);
628 
629 	__module_get(THIS_MODULE);
630 	s->task = kthread_run(bnep_session, s, "kbnepd %s", dev->name);
631 	if (IS_ERR(s->task)) {
632 		/* Session thread start failed, gotta cleanup. */
633 		module_put(THIS_MODULE);
634 		unregister_netdev(dev);
635 		__bnep_unlink_session(s);
636 		err = PTR_ERR(s->task);
637 		goto failed;
638 	}
639 
640 	up_write(&bnep_session_sem);
641 	strcpy(req->device, dev->name);
642 	return 0;
643 
644 failed:
645 	up_write(&bnep_session_sem);
646 	free_netdev(dev);
647 	return err;
648 }
649 
bnep_del_connection(struct bnep_conndel_req * req)650 int bnep_del_connection(struct bnep_conndel_req *req)
651 {
652 	u32 valid_flags = 0;
653 	struct bnep_session *s;
654 	int  err = 0;
655 
656 	BT_DBG("");
657 
658 	if (req->flags & ~valid_flags)
659 		return -EINVAL;
660 
661 	down_read(&bnep_session_sem);
662 
663 	s = __bnep_get_session(req->dst);
664 	if (s) {
665 		atomic_inc(&s->terminate);
666 		wake_up_interruptible(sk_sleep(s->sock->sk));
667 	} else
668 		err = -ENOENT;
669 
670 	up_read(&bnep_session_sem);
671 	return err;
672 }
673 
__bnep_copy_ci(struct bnep_conninfo * ci,struct bnep_session * s)674 static void __bnep_copy_ci(struct bnep_conninfo *ci, struct bnep_session *s)
675 {
676 	u32 valid_flags = BIT(BNEP_SETUP_RESPONSE);
677 
678 	memset(ci, 0, sizeof(*ci));
679 	memcpy(ci->dst, s->eh.h_source, ETH_ALEN);
680 	strcpy(ci->device, s->dev->name);
681 	ci->flags = s->flags & valid_flags;
682 	ci->state = s->state;
683 	ci->role  = s->role;
684 }
685 
bnep_get_connlist(struct bnep_connlist_req * req)686 int bnep_get_connlist(struct bnep_connlist_req *req)
687 {
688 	struct bnep_session *s;
689 	int err = 0, n = 0;
690 
691 	down_read(&bnep_session_sem);
692 
693 	list_for_each_entry(s, &bnep_session_list, list) {
694 		struct bnep_conninfo ci;
695 
696 		__bnep_copy_ci(&ci, s);
697 
698 		if (copy_to_user(req->ci, &ci, sizeof(ci))) {
699 			err = -EFAULT;
700 			break;
701 		}
702 
703 		if (++n >= req->cnum)
704 			break;
705 
706 		req->ci++;
707 	}
708 	req->cnum = n;
709 
710 	up_read(&bnep_session_sem);
711 	return err;
712 }
713 
bnep_get_conninfo(struct bnep_conninfo * ci)714 int bnep_get_conninfo(struct bnep_conninfo *ci)
715 {
716 	struct bnep_session *s;
717 	int err = 0;
718 
719 	down_read(&bnep_session_sem);
720 
721 	s = __bnep_get_session(ci->dst);
722 	if (s)
723 		__bnep_copy_ci(ci, s);
724 	else
725 		err = -ENOENT;
726 
727 	up_read(&bnep_session_sem);
728 	return err;
729 }
730 
bnep_init(void)731 static int __init bnep_init(void)
732 {
733 	char flt[50] = "";
734 
735 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
736 	strcat(flt, "protocol ");
737 #endif
738 
739 #ifdef CONFIG_BT_BNEP_MC_FILTER
740 	strcat(flt, "multicast");
741 #endif
742 
743 	BT_INFO("BNEP (Ethernet Emulation) ver %s", VERSION);
744 	if (flt[0])
745 		BT_INFO("BNEP filters: %s", flt);
746 
747 	bnep_sock_init();
748 	return 0;
749 }
750 
bnep_exit(void)751 static void __exit bnep_exit(void)
752 {
753 	bnep_sock_cleanup();
754 }
755 
756 module_init(bnep_init);
757 module_exit(bnep_exit);
758 
759 module_param(compress_src, bool, 0644);
760 MODULE_PARM_DESC(compress_src, "Compress sources headers");
761 
762 module_param(compress_dst, bool, 0644);
763 MODULE_PARM_DESC(compress_dst, "Compress destination headers");
764 
765 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
766 MODULE_DESCRIPTION("Bluetooth BNEP ver " VERSION);
767 MODULE_VERSION(VERSION);
768 MODULE_LICENSE("GPL");
769 MODULE_ALIAS("bt-proto-4");
770