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/*
29 * $Id: core.c,v 1.1.1.1 2007/08/03 18:53:50 Exp $
30 */
31
32#include <linux/module.h>
33
34#include <linux/kernel.h>
35#include <linux/sched.h>
36#include <linux/signal.h>
37#include <linux/init.h>
38#include <linux/wait.h>
39#include <linux/errno.h>
40#include <linux/net.h>
41#include <net/sock.h>
42
43#include <linux/socket.h>
44#include <linux/file.h>
45
46#include <linux/netdevice.h>
47#include <linux/etherdevice.h>
48#include <linux/skbuff.h>
49
50#include <asm/unaligned.h>
51
52#include <net/bluetooth/bluetooth.h>
53#include <net/bluetooth/hci_core.h>
54#include <net/bluetooth/l2cap.h>
55
56#include "bnep.h"
57
58#ifndef CONFIG_BT_BNEP_DEBUG
59#undef  BT_DBG
60#define BT_DBG(D...)
61#endif
62
63#define VERSION "1.2"
64
65static LIST_HEAD(bnep_session_list);
66static DECLARE_RWSEM(bnep_session_sem);
67
68static struct bnep_session *__bnep_get_session(u8 *dst)
69{
70	struct bnep_session *s;
71	struct list_head *p;
72
73	BT_DBG("");
74
75	list_for_each(p, &bnep_session_list) {
76		s = list_entry(p, struct bnep_session, list);
77		if (!compare_ether_addr(dst, s->eh.h_source))
78			return s;
79	}
80	return NULL;
81}
82
83static void __bnep_link_session(struct bnep_session *s)
84{
85	/* It's safe to call __module_get() here because sessions are added
86	   by the socket layer which has to hold the refference to this module.
87	 */
88	__module_get(THIS_MODULE);
89	list_add(&s->list, &bnep_session_list);
90}
91
92static void __bnep_unlink_session(struct bnep_session *s)
93{
94	list_del(&s->list);
95	module_put(THIS_MODULE);
96}
97
98static int bnep_send(struct bnep_session *s, void *data, size_t len)
99{
100	struct socket *sock = s->sock;
101	struct kvec iv = { data, len };
102
103	return kernel_sendmsg(sock, &s->msg, &iv, 1, len);
104}
105
106static int bnep_send_rsp(struct bnep_session *s, u8 ctrl, u16 resp)
107{
108	struct bnep_control_rsp rsp;
109	rsp.type = BNEP_CONTROL;
110	rsp.ctrl = ctrl;
111	rsp.resp = htons(resp);
112	return bnep_send(s, &rsp, sizeof(rsp));
113}
114
115#ifdef CONFIG_BT_BNEP_PROTO_FILTER
116static inline void bnep_set_default_proto_filter(struct bnep_session *s)
117{
118	/* (IPv4, ARP)  */
119	s->proto_filter[0].start = ETH_P_IP;
120	s->proto_filter[0].end   = ETH_P_ARP;
121	/* (RARP, AppleTalk) */
122	s->proto_filter[1].start = ETH_P_RARP;
123	s->proto_filter[1].end   = ETH_P_AARP;
124	/* (IPX, IPv6) */
125	s->proto_filter[2].start = ETH_P_IPX;
126	s->proto_filter[2].end   = ETH_P_IPV6;
127}
128#endif
129
130static int bnep_ctrl_set_netfilter(struct bnep_session *s, __be16 *data, int len)
131{
132	int n;
133
134	if (len < 2)
135		return -EILSEQ;
136
137	n = ntohs(get_unaligned(data));
138	data++; len -= 2;
139
140	if (len < n)
141		return -EILSEQ;
142
143	BT_DBG("filter len %d", n);
144
145#ifdef CONFIG_BT_BNEP_PROTO_FILTER
146	n /= 4;
147	if (n <= BNEP_MAX_PROTO_FILTERS) {
148		struct bnep_proto_filter *f = s->proto_filter;
149		int i;
150
151		for (i = 0; i < n; i++) {
152			f[i].start = ntohs(get_unaligned(data++));
153			f[i].end   = ntohs(get_unaligned(data++));
154
155			BT_DBG("proto filter start %d end %d",
156				f[i].start, f[i].end);
157		}
158
159		if (i < BNEP_MAX_PROTO_FILTERS)
160			memset(f + i, 0, sizeof(*f));
161
162		if (n == 0)
163			bnep_set_default_proto_filter(s);
164
165		bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_SUCCESS);
166	} else {
167		bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_LIMIT_REACHED);
168	}
169#else
170	bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_UNSUPPORTED_REQ);
171#endif
172	return 0;
173}
174
175static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len)
176{
177	int n;
178
179	if (len < 2)
180		return -EILSEQ;
181
182	n = ntohs(get_unaligned((__be16 *) data));
183	data += 2; len -= 2;
184
185	if (len < n)
186		return -EILSEQ;
187
188	BT_DBG("filter len %d", n);
189
190#ifdef CONFIG_BT_BNEP_MC_FILTER
191	n /= (ETH_ALEN * 2);
192
193	if (n > 0) {
194		s->mc_filter = 0;
195
196		/* Always send broadcast */
197		set_bit(bnep_mc_hash(s->dev->broadcast), (ulong *) &s->mc_filter);
198
199		/* Add address ranges to the multicast hash */
200		for (; n > 0; n--) {
201			u8 a1[6], *a2;
202
203			memcpy(a1, data, ETH_ALEN); data += ETH_ALEN;
204			a2 = data; data += ETH_ALEN;
205
206			BT_DBG("mc filter %s -> %s",
207				batostr((void *) a1), batostr((void *) a2));
208
209			#define INCA(a) { int i = 5; while (i >=0 && ++a[i--] == 0); }
210
211			/* Iterate from a1 to a2 */
212			set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
213			while (memcmp(a1, a2, 6) < 0 && s->mc_filter != ~0LL) {
214				INCA(a1);
215				set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
216			}
217		}
218	}
219
220	BT_DBG("mc filter hash 0x%llx", s->mc_filter);
221
222	bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_SUCCESS);
223#else
224	bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_FILTER_UNSUPPORTED_REQ);
225#endif
226	return 0;
227}
228
229static int bnep_rx_control(struct bnep_session *s, void *data, int len)
230{
231	u8  cmd = *(u8 *)data;
232	int err = 0;
233
234	data++; len--;
235
236	switch (cmd) {
237	case BNEP_CMD_NOT_UNDERSTOOD:
238	case BNEP_SETUP_CONN_REQ:
239	case BNEP_SETUP_CONN_RSP:
240	case BNEP_FILTER_NET_TYPE_RSP:
241	case BNEP_FILTER_MULTI_ADDR_RSP:
242		/* Ignore these for now */
243		break;
244
245	case BNEP_FILTER_NET_TYPE_SET:
246		err = bnep_ctrl_set_netfilter(s, data, len);
247		break;
248
249	case BNEP_FILTER_MULTI_ADDR_SET:
250		err = bnep_ctrl_set_mcfilter(s, data, len);
251		break;
252
253	default: {
254			u8 pkt[3];
255			pkt[0] = BNEP_CONTROL;
256			pkt[1] = BNEP_CMD_NOT_UNDERSTOOD;
257			pkt[2] = cmd;
258			bnep_send(s, pkt, sizeof(pkt));
259		}
260		break;
261	}
262
263	return err;
264}
265
266static int bnep_rx_extension(struct bnep_session *s, struct sk_buff *skb)
267{
268	struct bnep_ext_hdr *h;
269	int err = 0;
270
271	do {
272		h = (void *) skb->data;
273		if (!skb_pull(skb, sizeof(*h))) {
274			err = -EILSEQ;
275			break;
276		}
277
278		BT_DBG("type 0x%x len %d", h->type, h->len);
279
280		switch (h->type & BNEP_TYPE_MASK) {
281		case BNEP_EXT_CONTROL:
282			bnep_rx_control(s, skb->data, skb->len);
283			break;
284
285		default:
286			/* Unknown extension, skip it. */
287			break;
288		}
289
290		if (!skb_pull(skb, h->len)) {
291			err = -EILSEQ;
292			break;
293		}
294	} while (!err && (h->type & BNEP_EXT_HEADER));
295
296	return err;
297}
298
299static u8 __bnep_rx_hlen[] = {
300	ETH_HLEN,     /* BNEP_GENERAL */
301	0,            /* BNEP_CONTROL */
302	2,            /* BNEP_COMPRESSED */
303	ETH_ALEN + 2, /* BNEP_COMPRESSED_SRC_ONLY */
304	ETH_ALEN + 2  /* BNEP_COMPRESSED_DST_ONLY */
305};
306#define BNEP_RX_TYPES	(sizeof(__bnep_rx_hlen) - 1)
307
308static inline int bnep_rx_frame(struct bnep_session *s, struct sk_buff *skb)
309{
310	struct net_device *dev = s->dev;
311	struct sk_buff *nskb;
312	u8 type;
313
314	dev->last_rx = jiffies;
315	s->stats.rx_bytes += skb->len;
316
317	type = *(u8 *) skb->data; skb_pull(skb, 1);
318
319	if ((type & BNEP_TYPE_MASK) > BNEP_RX_TYPES)
320		goto badframe;
321
322	if ((type & BNEP_TYPE_MASK) == BNEP_CONTROL) {
323		bnep_rx_control(s, skb->data, skb->len);
324		kfree_skb(skb);
325		return 0;
326	}
327
328	skb_reset_mac_header(skb);
329
330	/* Verify and pull out header */
331	if (!skb_pull(skb, __bnep_rx_hlen[type & BNEP_TYPE_MASK]))
332		goto badframe;
333
334	s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2));
335
336	if (type & BNEP_EXT_HEADER) {
337		if (bnep_rx_extension(s, skb) < 0)
338			goto badframe;
339	}
340
341	/* Strip 802.1p header */
342	if (ntohs(s->eh.h_proto) == 0x8100) {
343		if (!skb_pull(skb, 4))
344			goto badframe;
345		s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2));
346	}
347
348	/* We have to alloc new skb and copy data here :(. Because original skb
349	 * may not be modified and because of the alignment requirements. */
350	nskb = alloc_skb(2 + ETH_HLEN + skb->len, GFP_KERNEL);
351	if (!nskb) {
352		s->stats.rx_dropped++;
353		kfree_skb(skb);
354		return -ENOMEM;
355	}
356	skb_reserve(nskb, 2);
357
358	/* Decompress header and construct ether frame */
359	switch (type & BNEP_TYPE_MASK) {
360	case BNEP_COMPRESSED:
361		memcpy(__skb_put(nskb, ETH_HLEN), &s->eh, ETH_HLEN);
362		break;
363
364	case BNEP_COMPRESSED_SRC_ONLY:
365		memcpy(__skb_put(nskb, ETH_ALEN), s->eh.h_dest, ETH_ALEN);
366		memcpy(__skb_put(nskb, ETH_ALEN), skb_mac_header(skb), ETH_ALEN);
367		put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2));
368		break;
369
370	case BNEP_COMPRESSED_DST_ONLY:
371		memcpy(__skb_put(nskb, ETH_ALEN), skb_mac_header(skb),
372		       ETH_ALEN);
373		memcpy(__skb_put(nskb, ETH_ALEN + 2), s->eh.h_source,
374		       ETH_ALEN + 2);
375		break;
376
377	case BNEP_GENERAL:
378		memcpy(__skb_put(nskb, ETH_ALEN * 2), skb_mac_header(skb),
379		       ETH_ALEN * 2);
380		put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2));
381		break;
382	}
383
384	skb_copy_from_linear_data(skb, __skb_put(nskb, skb->len), skb->len);
385	kfree_skb(skb);
386
387	s->stats.rx_packets++;
388	nskb->ip_summed = CHECKSUM_NONE;
389	nskb->protocol  = eth_type_trans(nskb, dev);
390	netif_rx_ni(nskb);
391	return 0;
392
393badframe:
394	s->stats.rx_errors++;
395	kfree_skb(skb);
396	return 0;
397}
398
399static u8 __bnep_tx_types[] = {
400	BNEP_GENERAL,
401	BNEP_COMPRESSED_SRC_ONLY,
402	BNEP_COMPRESSED_DST_ONLY,
403	BNEP_COMPRESSED
404};
405
406static inline int bnep_tx_frame(struct bnep_session *s, struct sk_buff *skb)
407{
408	struct ethhdr *eh = (void *) skb->data;
409	struct socket *sock = s->sock;
410	struct kvec iv[3];
411	int len = 0, il = 0;
412	u8 type = 0;
413
414	BT_DBG("skb %p dev %p type %d", skb, skb->dev, skb->pkt_type);
415
416	if (!skb->dev) {
417		/* Control frame sent by us */
418		goto send;
419	}
420
421	iv[il++] = (struct kvec) { &type, 1 };
422	len++;
423
424	if (!compare_ether_addr(eh->h_dest, s->eh.h_source))
425		type |= 0x01;
426
427	if (!compare_ether_addr(eh->h_source, s->eh.h_dest))
428		type |= 0x02;
429
430	if (type)
431		skb_pull(skb, ETH_ALEN * 2);
432
433	type = __bnep_tx_types[type];
434	switch (type) {
435	case BNEP_COMPRESSED_SRC_ONLY:
436		iv[il++] = (struct kvec) { eh->h_source, ETH_ALEN };
437		len += ETH_ALEN;
438		break;
439
440	case BNEP_COMPRESSED_DST_ONLY:
441		iv[il++] = (struct kvec) { eh->h_dest, ETH_ALEN };
442		len += ETH_ALEN;
443		break;
444	}
445
446send:
447	iv[il++] = (struct kvec) { skb->data, skb->len };
448	len += skb->len;
449
450	{
451		len = kernel_sendmsg(sock, &s->msg, iv, il, len);
452	}
453	kfree_skb(skb);
454
455	if (len > 0) {
456		s->stats.tx_bytes += len;
457		s->stats.tx_packets++;
458		return 0;
459	}
460
461	return len;
462}
463
464static int bnep_session(void *arg)
465{
466	struct bnep_session *s = arg;
467	struct net_device *dev = s->dev;
468	struct sock *sk = s->sock->sk;
469	struct sk_buff *skb;
470	wait_queue_t wait;
471
472	BT_DBG("");
473
474	daemonize("kbnepd %s", dev->name);
475	set_user_nice(current, -15);
476	current->flags |= PF_NOFREEZE;
477
478	init_waitqueue_entry(&wait, current);
479	add_wait_queue(sk->sk_sleep, &wait);
480	while (!atomic_read(&s->killed)) {
481		set_current_state(TASK_INTERRUPTIBLE);
482
483		// RX
484		while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
485			skb_orphan(skb);
486			bnep_rx_frame(s, skb);
487		}
488
489		if (sk->sk_state != BT_CONNECTED)
490			break;
491
492		// TX
493		while ((skb = skb_dequeue(&sk->sk_write_queue)))
494			if (bnep_tx_frame(s, skb))
495				break;
496		netif_wake_queue(dev);
497
498		schedule();
499	}
500	set_current_state(TASK_RUNNING);
501	remove_wait_queue(sk->sk_sleep, &wait);
502
503	/* Cleanup session */
504	down_write(&bnep_session_sem);
505
506	/* Delete network device */
507	unregister_netdev(dev);
508
509	/* Release the socket */
510	fput(s->sock->file);
511
512	__bnep_unlink_session(s);
513
514	up_write(&bnep_session_sem);
515	free_netdev(dev);
516	return 0;
517}
518
519static struct device *bnep_get_device(struct bnep_session *session)
520{
521	bdaddr_t *src = &bt_sk(session->sock->sk)->src;
522	bdaddr_t *dst = &bt_sk(session->sock->sk)->dst;
523	struct hci_dev *hdev;
524	struct hci_conn *conn;
525
526	hdev = hci_get_route(dst, src);
527	if (!hdev)
528		return NULL;
529
530	conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, dst);
531
532	hci_dev_put(hdev);
533
534	return conn ? &conn->dev : NULL;
535}
536
537int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock)
538{
539	struct net_device *dev;
540	struct bnep_session *s, *ss;
541	u8 dst[ETH_ALEN], src[ETH_ALEN];
542	int err;
543
544	BT_DBG("");
545
546	baswap((void *) dst, &bt_sk(sock->sk)->dst);
547	baswap((void *) src, &bt_sk(sock->sk)->src);
548
549	/* session struct allocated as private part of net_device */
550	dev = alloc_netdev(sizeof(struct bnep_session),
551			   (*req->device) ? req->device : "bnep%d",
552			   bnep_net_setup);
553	if (!dev)
554		return -ENOMEM;
555
556	down_write(&bnep_session_sem);
557
558	ss = __bnep_get_session(dst);
559	if (ss && ss->state == BT_CONNECTED) {
560		err = -EEXIST;
561		goto failed;
562	}
563
564	s = dev->priv;
565
566	/* This is rx header therefore addresses are swapped.
567	 * ie eh.h_dest is our local address. */
568	memcpy(s->eh.h_dest,   &src, ETH_ALEN);
569	memcpy(s->eh.h_source, &dst, ETH_ALEN);
570	memcpy(dev->dev_addr, s->eh.h_dest, ETH_ALEN);
571
572	s->dev   = dev;
573	s->sock  = sock;
574	s->role  = req->role;
575	s->state = BT_CONNECTED;
576
577	s->msg.msg_flags = MSG_NOSIGNAL;
578
579#ifdef CONFIG_BT_BNEP_MC_FILTER
580	/* Set default mc filter */
581	set_bit(bnep_mc_hash(dev->broadcast), (ulong *) &s->mc_filter);
582#endif
583
584#ifdef CONFIG_BT_BNEP_PROTO_FILTER
585	/* Set default protocol filter */
586	bnep_set_default_proto_filter(s);
587#endif
588
589	SET_NETDEV_DEV(dev, bnep_get_device(s));
590
591	err = register_netdev(dev);
592	if (err) {
593		goto failed;
594	}
595
596	__bnep_link_session(s);
597
598	err = kernel_thread(bnep_session, s, CLONE_KERNEL);
599	if (err < 0) {
600		/* Session thread start failed, gotta cleanup. */
601		unregister_netdev(dev);
602		__bnep_unlink_session(s);
603		goto failed;
604	}
605
606	up_write(&bnep_session_sem);
607	strcpy(req->device, dev->name);
608	return 0;
609
610failed:
611	up_write(&bnep_session_sem);
612	free_netdev(dev);
613	return err;
614}
615
616int bnep_del_connection(struct bnep_conndel_req *req)
617{
618	struct bnep_session *s;
619	int  err = 0;
620
621	BT_DBG("");
622
623	down_read(&bnep_session_sem);
624
625	s = __bnep_get_session(req->dst);
626	if (s) {
627		/* Wakeup user-space which is polling for socket errors.
628		 * This is temporary hack untill we have shutdown in L2CAP */
629		s->sock->sk->sk_err = EUNATCH;
630
631		/* Kill session thread */
632		atomic_inc(&s->killed);
633		wake_up_interruptible(s->sock->sk->sk_sleep);
634	} else
635		err = -ENOENT;
636
637	up_read(&bnep_session_sem);
638	return err;
639}
640
641static void __bnep_copy_ci(struct bnep_conninfo *ci, struct bnep_session *s)
642{
643	memcpy(ci->dst, s->eh.h_source, ETH_ALEN);
644	strcpy(ci->device, s->dev->name);
645	ci->flags = s->flags;
646	ci->state = s->state;
647	ci->role  = s->role;
648}
649
650int bnep_get_connlist(struct bnep_connlist_req *req)
651{
652	struct list_head *p;
653	int err = 0, n = 0;
654
655	down_read(&bnep_session_sem);
656
657	list_for_each(p, &bnep_session_list) {
658		struct bnep_session *s;
659		struct bnep_conninfo ci;
660
661		s = list_entry(p, struct bnep_session, list);
662
663		__bnep_copy_ci(&ci, s);
664
665		if (copy_to_user(req->ci, &ci, sizeof(ci))) {
666			err = -EFAULT;
667			break;
668		}
669
670		if (++n >= req->cnum)
671			break;
672
673		req->ci++;
674	}
675	req->cnum = n;
676
677	up_read(&bnep_session_sem);
678	return err;
679}
680
681int bnep_get_conninfo(struct bnep_conninfo *ci)
682{
683	struct bnep_session *s;
684	int err = 0;
685
686	down_read(&bnep_session_sem);
687
688	s = __bnep_get_session(ci->dst);
689	if (s)
690		__bnep_copy_ci(ci, s);
691	else
692		err = -ENOENT;
693
694	up_read(&bnep_session_sem);
695	return err;
696}
697
698static int __init bnep_init(void)
699{
700	char flt[50] = "";
701
702	l2cap_load();
703
704#ifdef CONFIG_BT_BNEP_PROTO_FILTER
705	strcat(flt, "protocol ");
706#endif
707
708#ifdef CONFIG_BT_BNEP_MC_FILTER
709	strcat(flt, "multicast");
710#endif
711
712	BT_INFO("BNEP (Ethernet Emulation) ver %s", VERSION);
713	if (flt[0])
714		BT_INFO("BNEP filters: %s", flt);
715
716	bnep_sock_init();
717	return 0;
718}
719
720static void __exit bnep_exit(void)
721{
722	bnep_sock_cleanup();
723}
724
725module_init(bnep_init);
726module_exit(bnep_exit);
727
728MODULE_AUTHOR("David Libault <david.libault@inventel.fr>, Maxim Krasnyansky <maxk@qualcomm.com>");
729MODULE_DESCRIPTION("Bluetooth BNEP ver " VERSION);
730MODULE_VERSION(VERSION);
731MODULE_LICENSE("GPL");
732MODULE_ALIAS("bt-proto-4");
733