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