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 Krasnyanskiy <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 2008/10/15 03:27:33 james26_jang Exp $
30 */
31
32#define __KERNEL_SYSCALLS__
33
34#include <linux/config.h>
35#include <linux/module.h>
36
37#include <linux/kernel.h>
38#include <linux/sched.h>
39#include <linux/signal.h>
40#include <linux/init.h>
41#include <linux/wait.h>
42#include <linux/errno.h>
43#include <linux/smp_lock.h>
44#include <linux/net.h>
45#include <net/sock.h>
46
47#include <linux/socket.h>
48#include <linux/file.h>
49
50#include <linux/netdevice.h>
51#include <linux/etherdevice.h>
52#include <linux/skbuff.h>
53
54#include <asm/unaligned.h>
55
56#include <net/bluetooth/bluetooth.h>
57#include <net/bluetooth/l2cap.h>
58
59#include "bnep.h"
60
61#ifndef CONFIG_BNEP_DEBUG
62#undef  BT_DBG
63#define BT_DBG(D...)
64#endif
65
66#define VERSION "1.0"
67
68static LIST_HEAD(bnep_session_list);
69static DECLARE_RWSEM(bnep_session_sem);
70
71static struct bnep_session *__bnep_get_session(__u8 *dst)
72{
73	struct bnep_session *s;
74	struct list_head *p;
75
76	BT_DBG("");
77
78	list_for_each(p, &bnep_session_list) {
79		s = list_entry(p, struct bnep_session, list);
80		if (!memcmp(dst, s->eh.h_source, ETH_ALEN))
81			return s;
82	}
83	return NULL;
84}
85
86static void __bnep_link_session(struct bnep_session *s)
87{
88	MOD_INC_USE_COUNT;
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	MOD_DEC_USE_COUNT;
96}
97
98static int bnep_send(struct bnep_session *s, void *data, size_t len)
99{
100	struct socket *sock = s->sock;
101	struct iovec iv = { data, len };
102	s->msg.msg_iov    = &iv;
103	s->msg.msg_iovlen = 1;
104	return sock->ops->sendmsg(sock, &s->msg, len, NULL);
105}
106
107static int bnep_send_rsp(struct bnep_session *s, __u8 ctrl, __u16 resp)
108{
109	struct bnep_control_rsp rsp;
110	rsp.type = BNEP_CONTROL;
111	rsp.ctrl = ctrl;
112	rsp.resp = htons(resp);
113	return bnep_send(s, &rsp, sizeof(rsp));
114}
115
116static int bnep_ctrl_set_netfilter(struct bnep_session *s, struct sk_buff *skb)
117{
118	__u16 *data;
119	int n;
120
121	data = (void *) skb->data;
122	if (!skb_pull(skb, 2))
123		return -EILSEQ;
124	n = ntohs(get_unaligned(data));
125
126	data = (void *) skb->data;
127	if (!skb_pull(skb, n))
128		return -EILSEQ;
129
130	BT_DBG("filter len %d", n);
131
132#ifdef CONFIG_BNEP_PROTO_FILTER
133	n /= 4;
134	if (n <= BNEP_MAX_PROTO_FILTERS) {
135		struct bnep_proto_filter *f = s->proto_filter;
136		int i;
137
138		for (i = 0; i < n; i++) {
139			f[i].start = get_unaligned(data++);
140			f[i].end   = get_unaligned(data++);
141
142			BT_DBG("proto filter start %d end %d",
143				f[i].start, f[i].end);
144		}
145		if (i < BNEP_MAX_PROTO_FILTERS)
146			memset(f + i, 0, sizeof(*f));
147
148		bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_SUCCESS);
149	} else {
150		bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_LIMIT_REACHED);
151	}
152#else
153	bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_UNSUPPORTED_REQ);
154#endif
155	return 0;
156}
157
158static int bnep_ctrl_set_mcfilter(struct bnep_session *s, struct sk_buff *skb)
159{
160	u8 *data;
161	int n;
162
163	data = (void *) skb->data;
164	if (!skb_pull(skb, 2))
165		return -EILSEQ;
166	n = ntohs(get_unaligned((u16 *) data));
167
168	data = (void *) skb->data;
169	if (!skb_pull(skb, n))
170		return -EILSEQ;
171
172	BT_DBG("filter len %d", n);
173
174#ifdef CONFIG_BNEP_MC_FILTER
175	n /= (ETH_ALEN * 2);
176
177	if (n > 0) {
178		s->mc_filter = 0;
179
180		/* Always send broadcast */
181		set_bit(bnep_mc_hash(s->dev.broadcast), &s->mc_filter);
182
183		/* Add address ranges to the multicast hash */
184		for (; n > 0; n--) {
185			u8 a1[6], *a2;
186
187			memcpy(a1, data, ETH_ALEN); data += ETH_ALEN;
188			a2 = data; data += ETH_ALEN;
189
190			BT_DBG("mc filter %s -> %s",
191				batostr((void *) a1), batostr((void *) a2));
192
193			#define INCA(a) { int i = 5; while (i >=0 && ++a[i--] == 0); }
194
195			/* Iterate from a1 to a2 */
196			set_bit(bnep_mc_hash(a1), &s->mc_filter);
197			while (memcmp(a1, a2, 6) < 0 && s->mc_filter != ~0LL) {
198				INCA(a1);
199				set_bit(bnep_mc_hash(a1), &s->mc_filter);
200			}
201		}
202	}
203
204	BT_DBG("mc filter hash 0x%llx", s->mc_filter);
205
206	bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_SUCCESS);
207#else
208	bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_FILTER_UNSUPPORTED_REQ);
209#endif
210	return 0;
211}
212
213static int bnep_rx_control(struct bnep_session *s, struct sk_buff *skb)
214{
215	int err = 0;
216	__u8 cmd = *(__u8 *) skb->data;
217	skb_pull(skb, 1);
218
219	switch (cmd) {
220	case BNEP_CMD_NOT_UNDERSTOOD:
221	case BNEP_SETUP_CONN_REQ:
222	case BNEP_SETUP_CONN_RSP:
223	case BNEP_FILTER_NET_TYPE_RSP:
224	case BNEP_FILTER_MULTI_ADDR_RSP:
225		/* Ignore these for now */
226		break;
227
228	case BNEP_FILTER_NET_TYPE_SET:
229		err = bnep_ctrl_set_netfilter(s, skb);
230		break;
231
232	case BNEP_FILTER_MULTI_ADDR_SET:
233		err = bnep_ctrl_set_mcfilter(s, skb);
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			err = bnep_rx_control(s, skb);
266			break;
267
268		default:
269			/* Unknown extension */
270			if (!skb_pull(skb, h->len))
271				err = -EILSEQ;
272			break;
273		}
274	} while (!err && (h->type & BNEP_EXT_HEADER));
275
276	return err;
277}
278
279static __u8 __bnep_rx_hlen[] = {
280	ETH_HLEN,     /* BNEP_GENERAL */
281	0,            /* BNEP_CONTROL */
282	2,            /* BNEP_COMPRESSED */
283	ETH_ALEN + 2, /* BNEP_COMPRESSED_SRC_ONLY */
284	ETH_ALEN + 2  /* BNEP_COMPRESSED_DST_ONLY */
285};
286#define BNEP_RX_TYPES	(sizeof(__bnep_rx_hlen) - 1)
287
288static inline int bnep_rx_frame(struct bnep_session *s, struct sk_buff *skb)
289{
290	struct net_device *dev = &s->dev;
291	struct sk_buff *nskb;
292	__u8 type;
293
294	dev->last_rx = jiffies;
295	s->stats.rx_bytes += skb->len;
296
297	type = *(__u8 *) skb->data; skb_pull(skb, 1);
298
299	if ((type & BNEP_TYPE_MASK) > BNEP_RX_TYPES)
300		goto badframe;
301
302	if ((type & BNEP_TYPE_MASK) == BNEP_CONTROL) {
303		bnep_rx_control(s, skb);
304		kfree_skb(skb);
305		return 0;
306	}
307
308	skb->mac.raw = skb->data;
309
310	/* Verify and pull out header */
311	if (!skb_pull(skb, __bnep_rx_hlen[type & BNEP_TYPE_MASK]))
312		goto badframe;
313
314	s->eh.h_proto = get_unaligned((__u16 *) (skb->data - 2));
315
316	if (type & BNEP_EXT_HEADER) {
317		if (bnep_rx_extension(s, skb) < 0)
318			goto badframe;
319	}
320
321	/* Strip 802.1p header */
322	if (ntohs(s->eh.h_proto) == 0x8100) {
323		if (!skb_pull(skb, 4))
324			goto badframe;
325		s->eh.h_proto = get_unaligned((__u16 *) (skb->data - 2));
326	}
327
328	/* We have to alloc new skb and copy data here :(. Because original skb
329	 * may not be modified and because of the alignment requirements. */
330	nskb = alloc_skb(2 + ETH_HLEN + skb->len, GFP_KERNEL);
331	if (!nskb) {
332		s->stats.rx_dropped++;
333		kfree_skb(skb);
334		return -ENOMEM;
335	}
336	skb_reserve(nskb, 2);
337
338	/* Decompress header and construct ether frame */
339	switch (type & BNEP_TYPE_MASK) {
340	case BNEP_COMPRESSED:
341		memcpy(__skb_put(nskb, ETH_HLEN), &s->eh, ETH_HLEN);
342		break;
343
344	case BNEP_COMPRESSED_SRC_ONLY:
345		memcpy(__skb_put(nskb, ETH_ALEN), s->eh.h_dest, ETH_ALEN);
346		memcpy(__skb_put(nskb, ETH_ALEN), skb->mac.raw, ETH_ALEN);
347		put_unaligned(s->eh.h_proto, (__u16 *) __skb_put(nskb, 2));
348		break;
349
350	case BNEP_COMPRESSED_DST_ONLY:
351		memcpy(__skb_put(nskb, ETH_ALEN), skb->mac.raw, ETH_ALEN);
352		memcpy(__skb_put(nskb, ETH_ALEN + 2), s->eh.h_source, ETH_ALEN + 2);
353		break;
354
355	case BNEP_GENERAL:
356		memcpy(__skb_put(nskb, ETH_ALEN * 2), skb->mac.raw, ETH_ALEN * 2);
357		put_unaligned(s->eh.h_proto, (__u16 *) __skb_put(nskb, 2));
358		break;
359	}
360
361	memcpy(__skb_put(nskb, skb->len), skb->data, skb->len);
362	kfree_skb(skb);
363
364	s->stats.rx_packets++;
365	nskb->dev       = dev;
366	nskb->ip_summed = CHECKSUM_UNNECESSARY;
367	nskb->protocol  = eth_type_trans(nskb, dev);
368	netif_rx_ni(nskb);
369	return 0;
370
371badframe:
372	s->stats.rx_errors++;
373	kfree_skb(skb);
374	return 0;
375}
376
377static __u8 __bnep_tx_types[] = {
378	BNEP_GENERAL,
379	BNEP_COMPRESSED_SRC_ONLY,
380	BNEP_COMPRESSED_DST_ONLY,
381	BNEP_COMPRESSED
382};
383
384static inline int bnep_tx_frame(struct bnep_session *s, struct sk_buff *skb)
385{
386	struct ethhdr *eh = (void *) skb->data;
387	struct socket *sock = s->sock;
388	struct iovec iv[3];
389	int len = 0, il = 0;
390	__u8 type = 0;
391
392	BT_DBG("skb %p dev %p type %d", skb, skb->dev, skb->pkt_type);
393
394	if (!skb->dev) {
395		/* Control frame sent by us */
396		goto send;
397	}
398
399	iv[il++] = (struct iovec) { &type, 1 };
400	len++;
401
402	if (!memcmp(eh->h_dest, s->eh.h_source, ETH_ALEN))
403		type |= 0x01;
404
405	if (!memcmp(eh->h_source, s->eh.h_dest, ETH_ALEN))
406		type |= 0x02;
407
408	if (type)
409		skb_pull(skb, ETH_ALEN * 2);
410
411	type = __bnep_tx_types[type];
412	switch (type) {
413	case BNEP_COMPRESSED_SRC_ONLY:
414		iv[il++] = (struct iovec) { eh->h_source, ETH_ALEN };
415		len += ETH_ALEN;
416		break;
417
418	case BNEP_COMPRESSED_DST_ONLY:
419		iv[il++] = (struct iovec) { eh->h_dest, ETH_ALEN };
420		len += ETH_ALEN;
421		break;
422	}
423
424send:
425	iv[il++] = (struct iovec) { skb->data, skb->len };
426	len += skb->len;
427
428
429	s->msg.msg_iov    = iv;
430	s->msg.msg_iovlen = il;
431	len = sock->ops->sendmsg(sock, &s->msg, len, NULL);
432	kfree_skb(skb);
433
434	if (len > 0) {
435		s->stats.tx_bytes += len;
436		s->stats.tx_packets++;
437		return 0;
438	}
439
440	return len;
441}
442
443static int bnep_session(void *arg)
444{
445	struct bnep_session *s = arg;
446	struct net_device *dev = &s->dev;
447	struct sock *sk = s->sock->sk;
448	struct sk_buff *skb;
449	wait_queue_t wait;
450
451	BT_DBG("");
452
453        daemonize(); reparent_to_init();
454
455        sprintf(current->comm, "kbnepd %s", dev->name);
456
457        sigfillset(&current->blocked);
458	flush_signals(current);
459
460	current->nice = -15;
461
462        set_fs(KERNEL_DS);
463
464	init_waitqueue_entry(&wait, current);
465	add_wait_queue(sk->sleep, &wait);
466	while (!atomic_read(&s->killed)) {
467		set_current_state(TASK_INTERRUPTIBLE);
468
469		// RX
470		while ((skb = skb_dequeue(&sk->receive_queue))) {
471			skb_orphan(skb);
472			bnep_rx_frame(s, skb);
473		}
474
475		if (sk->state != BT_CONNECTED)
476			break;
477
478		// TX
479		while ((skb = skb_dequeue(&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, &wait);
488
489	/* Cleanup session */
490	down_write(&bnep_session_sem);
491
492	/* Delete network device */
493	unregister_netdev(dev);
494
495	/* Release the socket */
496	fput(s->sock->file);
497
498	__bnep_unlink_session(s);
499
500	up_write(&bnep_session_sem);
501	kfree(s);
502	return 0;
503}
504
505int bnep_add_connection(struct bnep_conadd_req *req, struct socket *sock)
506{
507	struct net_device *dev;
508	struct bnep_session *s, *ss;
509	__u8 dst[ETH_ALEN], src[ETH_ALEN];
510	int err;
511
512	BT_DBG("");
513
514	baswap((void *) dst, &bluez_pi(sock->sk)->dst);
515	baswap((void *) src, &bluez_pi(sock->sk)->src);
516
517	s = kmalloc(sizeof(struct bnep_session), GFP_KERNEL);
518	if (!s)
519		return -ENOMEM;
520	memset(s, 0, sizeof(struct bnep_session));
521
522	down_write(&bnep_session_sem);
523
524	ss = __bnep_get_session(dst);
525	if (ss && ss->state == BT_CONNECTED) {
526		err = -EEXIST;
527		goto failed;
528	}
529
530	dev = &s->dev;
531
532	if (*req->device)
533		strcpy(dev->name, req->device);
534	else
535		strcpy(dev->name, "bnep%d");
536
537	/* This is rx header therefor addresses are swaped.
538	 * ie eh.h_dest is our local address. */
539	memcpy(s->eh.h_dest,   &src, ETH_ALEN);
540	memcpy(s->eh.h_source, &dst, ETH_ALEN);
541
542	s->sock  = sock;
543	s->role  = req->role;
544	s->state = BT_CONNECTED;
545
546	s->msg.msg_flags = MSG_NOSIGNAL;
547
548#ifdef CONFIG_BNEP_MC_FILTER
549	/* Set default mc filter */
550	set_bit(bnep_mc_hash(dev->broadcast), &s->mc_filter);
551#endif
552
553#ifdef CONFIG_BNEP_PROTO_FILTER
554	/* Set default protocol filter */
555
556	/* (IPv4, ARP)  */
557	s->proto_filter[0].start = htons(0x0800);
558	s->proto_filter[0].end   = htons(0x0806);
559	/* (RARP, AppleTalk) */
560	s->proto_filter[1].start = htons(0x8035);
561	s->proto_filter[1].end   = htons(0x80F3);
562	/* (IPX, IPv6) */
563	s->proto_filter[2].start = htons(0x8137);
564	s->proto_filter[2].end   = htons(0x86DD);
565#endif
566
567	dev->init = bnep_net_init;
568	dev->priv = s;
569	err = register_netdev(dev);
570	if (err) {
571		goto failed;
572	}
573
574	__bnep_link_session(s);
575
576	err = kernel_thread(bnep_session, s, CLONE_FS | CLONE_FILES | CLONE_SIGHAND);
577	if (err < 0) {
578		/* Session thread start failed, gotta cleanup. */
579		unregister_netdev(dev);
580		__bnep_unlink_session(s);
581		goto failed;
582	}
583
584	up_write(&bnep_session_sem);
585	strcpy(req->device, dev->name);
586	return 0;
587
588failed:
589	up_write(&bnep_session_sem);
590	kfree(s);
591	return err;
592}
593
594int bnep_del_connection(struct bnep_condel_req *req)
595{
596	struct bnep_session *s;
597	int  err = 0;
598
599	BT_DBG("");
600
601	down_read(&bnep_session_sem);
602
603	s = __bnep_get_session(req->dst);
604	if (s) {
605		/* Wakeup user-space which is polling for socket errors.
606		 * This is temporary hack untill we have shutdown in L2CAP */
607		s->sock->sk->err = EUNATCH;
608
609		/* Kill session thread */
610		atomic_inc(&s->killed);
611		wake_up_interruptible(s->sock->sk->sleep);
612	} else
613		err = -ENOENT;
614
615	up_read(&bnep_session_sem);
616	return err;
617}
618
619static void __bnep_copy_ci(struct bnep_coninfo *ci, struct bnep_session *s)
620{
621	memcpy(ci->dst, s->eh.h_source, ETH_ALEN);
622	strcpy(ci->device, s->dev.name);
623	ci->flags = s->flags;
624	ci->state = s->state;
625	ci->role  = s->role;
626}
627
628int bnep_get_conlist(struct bnep_conlist_req *req)
629{
630	struct list_head *p;
631	int err = 0, n = 0;
632
633	down_read(&bnep_session_sem);
634
635	list_for_each(p, &bnep_session_list) {
636		struct bnep_session *s;
637		struct bnep_coninfo ci;
638
639		s = list_entry(p, struct bnep_session, list);
640
641		__bnep_copy_ci(&ci, s);
642
643		if (copy_to_user(req->ci, &ci, sizeof(ci))) {
644			err = -EFAULT;
645			break;
646		}
647
648		if (++n >= req->cnum)
649			break;
650
651		req->ci++;
652	}
653	req->cnum = n;
654
655	up_read(&bnep_session_sem);
656	return err;
657}
658
659int bnep_get_coninfo(struct bnep_coninfo *ci)
660{
661	struct bnep_session *s;
662	int err = 0;
663
664	down_read(&bnep_session_sem);
665
666	s = __bnep_get_session(ci->dst);
667	if (s)
668		__bnep_copy_ci(ci, s);
669	else
670		err = -ENOENT;
671
672	up_read(&bnep_session_sem);
673	return err;
674}
675
676static int __init bnep_init_module(void)
677{
678	BT_INFO("BNEP: BNEP2 ver %s\n"
679		"BNEP: Copyright (C) 2002 Inventel\n"
680		"BNEP: Written 2001,2002 by\n"
681		"BNEP: \tClement Moreau <clement.moreau@inventel.fr> "
682			"David Libault <david.libault@inventel.fr>\n"
683		"BNEP: Copyright (C) 2002 Maxim Krasnyanskiy <maxk@qualcomm.com>",
684			VERSION);
685
686	bnep_sock_init();
687
688	bnep_crc32_init();
689
690	return 0;
691}
692
693static void __exit bnep_cleanup_module(void)
694{
695	bnep_sock_cleanup();
696
697	bnep_crc32_cleanup();
698}
699
700module_init(bnep_init_module);
701module_exit(bnep_cleanup_module);
702
703MODULE_DESCRIPTION("BNEP2 ver " VERSION);
704MODULE_AUTHOR("David Libault <david.libault@inventel.fr> Maxim Krasnyanskiy <maxk@qualcomm.com>");
705MODULE_LICENSE("GPL");
706