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