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
42static bool compress_src = true;
43static bool compress_dst = true;
44
45static LIST_HEAD(bnep_session_list);
46static DECLARE_RWSEM(bnep_session_sem);
47
48static 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
61static void __bnep_link_session(struct bnep_session *s)
62{
63	list_add(&s->list, &bnep_session_list);
64}
65
66static void __bnep_unlink_session(struct bnep_session *s)
67{
68	list_del(&s->list);
69}
70
71static 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
79static 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
89static 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
103static 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 %d end %d",
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
149static 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
209static 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		err = bnep_send_rsp(s, BNEP_SETUP_CONN_RSP, BNEP_CONN_NOT_ALLOWED);
235		break;
236
237	default: {
238			u8 pkt[3];
239			pkt[0] = BNEP_CONTROL;
240			pkt[1] = BNEP_CMD_NOT_UNDERSTOOD;
241			pkt[2] = cmd;
242			bnep_send(s, pkt, sizeof(pkt));
243		}
244		break;
245	}
246
247	return err;
248}
249
250static int bnep_rx_extension(struct bnep_session *s, struct sk_buff *skb)
251{
252	struct bnep_ext_hdr *h;
253	int err = 0;
254
255	do {
256		h = (void *) skb->data;
257		if (!skb_pull(skb, sizeof(*h))) {
258			err = -EILSEQ;
259			break;
260		}
261
262		BT_DBG("type 0x%x len %d", h->type, h->len);
263
264		switch (h->type & BNEP_TYPE_MASK) {
265		case BNEP_EXT_CONTROL:
266			bnep_rx_control(s, skb->data, skb->len);
267			break;
268
269		default:
270			/* Unknown extension, skip it. */
271			break;
272		}
273
274		if (!skb_pull(skb, h->len)) {
275			err = -EILSEQ;
276			break;
277		}
278	} while (!err && (h->type & BNEP_EXT_HEADER));
279
280	return err;
281}
282
283static u8 __bnep_rx_hlen[] = {
284	ETH_HLEN,     /* BNEP_GENERAL */
285	0,            /* BNEP_CONTROL */
286	2,            /* BNEP_COMPRESSED */
287	ETH_ALEN + 2, /* BNEP_COMPRESSED_SRC_ONLY */
288	ETH_ALEN + 2  /* BNEP_COMPRESSED_DST_ONLY */
289};
290
291static int bnep_rx_frame(struct bnep_session *s, struct sk_buff *skb)
292{
293	struct net_device *dev = s->dev;
294	struct sk_buff *nskb;
295	u8 type;
296
297	dev->stats.rx_bytes += skb->len;
298
299	type = *(u8 *) skb->data;
300	skb_pull(skb, 1);
301
302	if ((type & BNEP_TYPE_MASK) >= sizeof(__bnep_rx_hlen))
303		goto badframe;
304
305	if ((type & BNEP_TYPE_MASK) == BNEP_CONTROL) {
306		bnep_rx_control(s, skb->data, skb->len);
307		kfree_skb(skb);
308		return 0;
309	}
310
311	skb_reset_mac_header(skb);
312
313	/* Verify and pull out header */
314	if (!skb_pull(skb, __bnep_rx_hlen[type & BNEP_TYPE_MASK]))
315		goto badframe;
316
317	s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2));
318
319	if (type & BNEP_EXT_HEADER) {
320		if (bnep_rx_extension(s, skb) < 0)
321			goto badframe;
322	}
323
324	/* Strip 802.1p header */
325	if (ntohs(s->eh.h_proto) == ETH_P_8021Q) {
326		if (!skb_pull(skb, 4))
327			goto badframe;
328		s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2));
329	}
330
331	/* We have to alloc new skb and copy data here :(. Because original skb
332	 * may not be modified and because of the alignment requirements. */
333	nskb = alloc_skb(2 + ETH_HLEN + skb->len, GFP_KERNEL);
334	if (!nskb) {
335		dev->stats.rx_dropped++;
336		kfree_skb(skb);
337		return -ENOMEM;
338	}
339	skb_reserve(nskb, 2);
340
341	/* Decompress header and construct ether frame */
342	switch (type & BNEP_TYPE_MASK) {
343	case BNEP_COMPRESSED:
344		memcpy(__skb_put(nskb, ETH_HLEN), &s->eh, ETH_HLEN);
345		break;
346
347	case BNEP_COMPRESSED_SRC_ONLY:
348		memcpy(__skb_put(nskb, ETH_ALEN), s->eh.h_dest, ETH_ALEN);
349		memcpy(__skb_put(nskb, ETH_ALEN), skb_mac_header(skb), ETH_ALEN);
350		put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2));
351		break;
352
353	case BNEP_COMPRESSED_DST_ONLY:
354		memcpy(__skb_put(nskb, ETH_ALEN), skb_mac_header(skb),
355								ETH_ALEN);
356		memcpy(__skb_put(nskb, ETH_ALEN + 2), s->eh.h_source,
357								ETH_ALEN + 2);
358		break;
359
360	case BNEP_GENERAL:
361		memcpy(__skb_put(nskb, ETH_ALEN * 2), skb_mac_header(skb),
362								ETH_ALEN * 2);
363		put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2));
364		break;
365	}
366
367	skb_copy_from_linear_data(skb, __skb_put(nskb, skb->len), skb->len);
368	kfree_skb(skb);
369
370	dev->stats.rx_packets++;
371	nskb->ip_summed = CHECKSUM_NONE;
372	nskb->protocol  = eth_type_trans(nskb, dev);
373	netif_rx_ni(nskb);
374	return 0;
375
376badframe:
377	dev->stats.rx_errors++;
378	kfree_skb(skb);
379	return 0;
380}
381
382static u8 __bnep_tx_types[] = {
383	BNEP_GENERAL,
384	BNEP_COMPRESSED_SRC_ONLY,
385	BNEP_COMPRESSED_DST_ONLY,
386	BNEP_COMPRESSED
387};
388
389static int bnep_tx_frame(struct bnep_session *s, struct sk_buff *skb)
390{
391	struct ethhdr *eh = (void *) skb->data;
392	struct socket *sock = s->sock;
393	struct kvec iv[3];
394	int len = 0, il = 0;
395	u8 type = 0;
396
397	BT_DBG("skb %p dev %p type %d", skb, skb->dev, skb->pkt_type);
398
399	if (!skb->dev) {
400		/* Control frame sent by us */
401		goto send;
402	}
403
404	iv[il++] = (struct kvec) { &type, 1 };
405	len++;
406
407	if (compress_src && ether_addr_equal(eh->h_dest, s->eh.h_source))
408		type |= 0x01;
409
410	if (compress_dst && ether_addr_equal(eh->h_source, s->eh.h_dest))
411		type |= 0x02;
412
413	if (type)
414		skb_pull(skb, ETH_ALEN * 2);
415
416	type = __bnep_tx_types[type];
417	switch (type) {
418	case BNEP_COMPRESSED_SRC_ONLY:
419		iv[il++] = (struct kvec) { eh->h_source, ETH_ALEN };
420		len += ETH_ALEN;
421		break;
422
423	case BNEP_COMPRESSED_DST_ONLY:
424		iv[il++] = (struct kvec) { eh->h_dest, ETH_ALEN };
425		len += ETH_ALEN;
426		break;
427	}
428
429send:
430	iv[il++] = (struct kvec) { skb->data, skb->len };
431	len += skb->len;
432
433	/* FIXME: linearize skb */
434	{
435		len = kernel_sendmsg(sock, &s->msg, iv, il, len);
436	}
437	kfree_skb(skb);
438
439	if (len > 0) {
440		s->dev->stats.tx_bytes += len;
441		s->dev->stats.tx_packets++;
442		return 0;
443	}
444
445	return len;
446}
447
448static int bnep_session(void *arg)
449{
450	struct bnep_session *s = arg;
451	struct net_device *dev = s->dev;
452	struct sock *sk = s->sock->sk;
453	struct sk_buff *skb;
454	wait_queue_t wait;
455
456	BT_DBG("");
457
458	set_user_nice(current, -15);
459
460	init_waitqueue_entry(&wait, current);
461	add_wait_queue(sk_sleep(sk), &wait);
462	while (1) {
463		set_current_state(TASK_INTERRUPTIBLE);
464
465		if (atomic_read(&s->terminate))
466			break;
467		/* RX */
468		while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
469			skb_orphan(skb);
470			if (!skb_linearize(skb))
471				bnep_rx_frame(s, skb);
472			else
473				kfree_skb(skb);
474		}
475
476		if (sk->sk_state != BT_CONNECTED)
477			break;
478
479		/* TX */
480		while ((skb = skb_dequeue(&sk->sk_write_queue)))
481			if (bnep_tx_frame(s, skb))
482				break;
483		netif_wake_queue(dev);
484
485		schedule();
486	}
487	__set_current_state(TASK_RUNNING);
488	remove_wait_queue(sk_sleep(sk), &wait);
489
490	/* Cleanup session */
491	down_write(&bnep_session_sem);
492
493	/* Delete network device */
494	unregister_netdev(dev);
495
496	/* Wakeup user-space polling for socket errors */
497	s->sock->sk->sk_err = EUNATCH;
498
499	wake_up_interruptible(sk_sleep(s->sock->sk));
500
501	/* Release the socket */
502	fput(s->sock->file);
503
504	__bnep_unlink_session(s);
505
506	up_write(&bnep_session_sem);
507	free_netdev(dev);
508	module_put_and_exit(0);
509	return 0;
510}
511
512static struct device *bnep_get_device(struct bnep_session *session)
513{
514	struct hci_conn *conn;
515
516	conn = l2cap_pi(session->sock->sk)->chan->conn->hcon;
517	if (!conn)
518		return NULL;
519
520	return &conn->dev;
521}
522
523static struct device_type bnep_type = {
524	.name	= "bluetooth",
525};
526
527int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock)
528{
529	struct net_device *dev;
530	struct bnep_session *s, *ss;
531	u8 dst[ETH_ALEN], src[ETH_ALEN];
532	int err;
533
534	BT_DBG("");
535
536	baswap((void *) dst, &l2cap_pi(sock->sk)->chan->dst);
537	baswap((void *) src, &l2cap_pi(sock->sk)->chan->src);
538
539	/* session struct allocated as private part of net_device */
540	dev = alloc_netdev(sizeof(struct bnep_session),
541			   (*req->device) ? req->device : "bnep%d",
542			   NET_NAME_UNKNOWN,
543			   bnep_net_setup);
544	if (!dev)
545		return -ENOMEM;
546
547	down_write(&bnep_session_sem);
548
549	ss = __bnep_get_session(dst);
550	if (ss && ss->state == BT_CONNECTED) {
551		err = -EEXIST;
552		goto failed;
553	}
554
555	s = netdev_priv(dev);
556
557	/* This is rx header therefore addresses are swapped.
558	 * ie. eh.h_dest is our local address. */
559	memcpy(s->eh.h_dest,   &src, ETH_ALEN);
560	memcpy(s->eh.h_source, &dst, ETH_ALEN);
561	memcpy(dev->dev_addr, s->eh.h_dest, ETH_ALEN);
562
563	s->dev   = dev;
564	s->sock  = sock;
565	s->role  = req->role;
566	s->state = BT_CONNECTED;
567
568	s->msg.msg_flags = MSG_NOSIGNAL;
569
570#ifdef CONFIG_BT_BNEP_MC_FILTER
571	/* Set default mc filter */
572	set_bit(bnep_mc_hash(dev->broadcast), (ulong *) &s->mc_filter);
573#endif
574
575#ifdef CONFIG_BT_BNEP_PROTO_FILTER
576	/* Set default protocol filter */
577	bnep_set_default_proto_filter(s);
578#endif
579
580	SET_NETDEV_DEV(dev, bnep_get_device(s));
581	SET_NETDEV_DEVTYPE(dev, &bnep_type);
582
583	err = register_netdev(dev);
584	if (err)
585		goto failed;
586
587	__bnep_link_session(s);
588
589	__module_get(THIS_MODULE);
590	s->task = kthread_run(bnep_session, s, "kbnepd %s", dev->name);
591	if (IS_ERR(s->task)) {
592		/* Session thread start failed, gotta cleanup. */
593		module_put(THIS_MODULE);
594		unregister_netdev(dev);
595		__bnep_unlink_session(s);
596		err = PTR_ERR(s->task);
597		goto failed;
598	}
599
600	up_write(&bnep_session_sem);
601	strcpy(req->device, dev->name);
602	return 0;
603
604failed:
605	up_write(&bnep_session_sem);
606	free_netdev(dev);
607	return err;
608}
609
610int bnep_del_connection(struct bnep_conndel_req *req)
611{
612	struct bnep_session *s;
613	int  err = 0;
614
615	BT_DBG("");
616
617	down_read(&bnep_session_sem);
618
619	s = __bnep_get_session(req->dst);
620	if (s) {
621		atomic_inc(&s->terminate);
622		wake_up_process(s->task);
623	} else
624		err = -ENOENT;
625
626	up_read(&bnep_session_sem);
627	return err;
628}
629
630static void __bnep_copy_ci(struct bnep_conninfo *ci, struct bnep_session *s)
631{
632	memset(ci, 0, sizeof(*ci));
633	memcpy(ci->dst, s->eh.h_source, ETH_ALEN);
634	strcpy(ci->device, s->dev->name);
635	ci->flags = s->flags;
636	ci->state = s->state;
637	ci->role  = s->role;
638}
639
640int bnep_get_connlist(struct bnep_connlist_req *req)
641{
642	struct bnep_session *s;
643	int err = 0, n = 0;
644
645	down_read(&bnep_session_sem);
646
647	list_for_each_entry(s, &bnep_session_list, list) {
648		struct bnep_conninfo ci;
649
650		__bnep_copy_ci(&ci, s);
651
652		if (copy_to_user(req->ci, &ci, sizeof(ci))) {
653			err = -EFAULT;
654			break;
655		}
656
657		if (++n >= req->cnum)
658			break;
659
660		req->ci++;
661	}
662	req->cnum = n;
663
664	up_read(&bnep_session_sem);
665	return err;
666}
667
668int bnep_get_conninfo(struct bnep_conninfo *ci)
669{
670	struct bnep_session *s;
671	int err = 0;
672
673	down_read(&bnep_session_sem);
674
675	s = __bnep_get_session(ci->dst);
676	if (s)
677		__bnep_copy_ci(ci, s);
678	else
679		err = -ENOENT;
680
681	up_read(&bnep_session_sem);
682	return err;
683}
684
685static int __init bnep_init(void)
686{
687	char flt[50] = "";
688
689#ifdef CONFIG_BT_BNEP_PROTO_FILTER
690	strcat(flt, "protocol ");
691#endif
692
693#ifdef CONFIG_BT_BNEP_MC_FILTER
694	strcat(flt, "multicast");
695#endif
696
697	BT_INFO("BNEP (Ethernet Emulation) ver %s", VERSION);
698	if (flt[0])
699		BT_INFO("BNEP filters: %s", flt);
700
701	bnep_sock_init();
702	return 0;
703}
704
705static void __exit bnep_exit(void)
706{
707	bnep_sock_cleanup();
708}
709
710module_init(bnep_init);
711module_exit(bnep_exit);
712
713module_param(compress_src, bool, 0644);
714MODULE_PARM_DESC(compress_src, "Compress sources headers");
715
716module_param(compress_dst, bool, 0644);
717MODULE_PARM_DESC(compress_dst, "Compress destination headers");
718
719MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
720MODULE_DESCRIPTION("Bluetooth BNEP ver " VERSION);
721MODULE_VERSION(VERSION);
722MODULE_LICENSE("GPL");
723MODULE_ALIAS("bt-proto-4");
724