1 /*
2    CMTP implementation for Linux Bluetooth stack (BlueZ).
3    Copyright (C) 2002-2003 Marcel Holtmann <marcel@holtmann.org>
4 
5    This program is free software; you can redistribute it and/or modify
6    it under the terms of the GNU General Public License version 2 as
7    published by the Free Software Foundation;
8 
9    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
10    OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
11    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
12    IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
13    CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
14    WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
15    ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
16    OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
17 
18    ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
19    COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
20    SOFTWARE IS DISCLAIMED.
21 */
22 
23 #include <linux/config.h>
24 #include <linux/module.h>
25 
26 #include <linux/types.h>
27 #include <linux/errno.h>
28 #include <linux/kernel.h>
29 #include <linux/major.h>
30 #include <linux/sched.h>
31 #include <linux/slab.h>
32 #include <linux/poll.h>
33 #include <linux/fcntl.h>
34 #include <linux/skbuff.h>
35 #include <linux/socket.h>
36 #include <linux/ioctl.h>
37 #include <linux/file.h>
38 #include <linux/init.h>
39 #include <net/sock.h>
40 
41 #include <net/bluetooth/bluetooth.h>
42 #include <net/bluetooth/l2cap.h>
43 
44 #include "cmtp.h"
45 
46 #ifndef CONFIG_BLUEZ_CMTP_DEBUG
47 #undef  BT_DBG
48 #define BT_DBG(D...)
49 #endif
50 
51 #define VERSION "1.0"
52 
53 static DECLARE_RWSEM(cmtp_session_sem);
54 static LIST_HEAD(cmtp_session_list);
55 
__cmtp_get_session(bdaddr_t * bdaddr)56 static struct cmtp_session *__cmtp_get_session(bdaddr_t *bdaddr)
57 {
58 	struct cmtp_session *session;
59 	struct list_head *p;
60 
61 	BT_DBG("");
62 
63 	list_for_each(p, &cmtp_session_list) {
64 		session = list_entry(p, struct cmtp_session, list);
65 		if (!bacmp(bdaddr, &session->bdaddr))
66 			return session;
67 	}
68 	return NULL;
69 }
70 
__cmtp_link_session(struct cmtp_session * session)71 static void __cmtp_link_session(struct cmtp_session *session)
72 {
73 	MOD_INC_USE_COUNT;
74 	list_add(&session->list, &cmtp_session_list);
75 }
76 
__cmtp_unlink_session(struct cmtp_session * session)77 static void __cmtp_unlink_session(struct cmtp_session *session)
78 {
79 	list_del(&session->list);
80 	MOD_DEC_USE_COUNT;
81 }
82 
__cmtp_copy_session(struct cmtp_session * session,struct cmtp_conninfo * ci)83 static void __cmtp_copy_session(struct cmtp_session *session, struct cmtp_conninfo *ci)
84 {
85 	bacpy(&ci->bdaddr, &session->bdaddr);
86 
87 	ci->flags = session->flags;
88 	ci->state = session->state;
89 
90 	ci->num = session->num;
91 }
92 
93 
cmtp_alloc_block_id(struct cmtp_session * session)94 static inline int cmtp_alloc_block_id(struct cmtp_session *session)
95 {
96 	int i, id = -1;
97 
98 	for (i = 0; i < 16; i++)
99 		if (!test_and_set_bit(i, &session->blockids)) {
100 			id = i;
101 			break;
102 		}
103 
104 	return id;
105 }
106 
cmtp_free_block_id(struct cmtp_session * session,int id)107 static inline void cmtp_free_block_id(struct cmtp_session *session, int id)
108 {
109 	clear_bit(id, &session->blockids);
110 }
111 
cmtp_add_msgpart(struct cmtp_session * session,int id,const unsigned char * buf,int count)112 static inline void cmtp_add_msgpart(struct cmtp_session *session, int id, const unsigned char *buf, int count)
113 {
114 	struct sk_buff *skb = session->reassembly[id], *nskb;
115 	int size;
116 
117 	BT_DBG("session %p buf %p count %d", session, buf, count);
118 
119 	size = (skb) ? skb->len + count : count;
120 
121 	if (!(nskb = alloc_skb(size, GFP_ATOMIC))) {
122 		BT_ERR("Can't allocate memory for CAPI message");
123 		return;
124 	}
125 
126 	if (skb && (skb->len > 0))
127 		memcpy(skb_put(nskb, skb->len), skb->data, skb->len);
128 
129 	memcpy(skb_put(nskb, count), buf, count);
130 
131 	session->reassembly[id] = nskb;
132 
133 	if (skb)
134 		kfree_skb(skb);
135 }
136 
cmtp_recv_frame(struct cmtp_session * session,struct sk_buff * skb)137 static inline int cmtp_recv_frame(struct cmtp_session *session, struct sk_buff *skb)
138 {
139 	__u8 hdr, hdrlen, id;
140 	__u16 len;
141 
142 	BT_DBG("session %p skb %p len %d", session, skb, skb->len);
143 
144 	while (skb->len > 0) {
145 		hdr = skb->data[0];
146 
147 		switch (hdr & 0xc0) {
148 		case 0x40:
149 			hdrlen = 2;
150 			len = skb->data[1];
151 			break;
152 		case 0x80:
153 			hdrlen = 3;
154 			len = skb->data[1] | (skb->data[2] << 8);
155 			break;
156 		default:
157 			hdrlen = 1;
158 			len = 0;
159 			break;
160 		}
161 
162 		id = (hdr & 0x3c) >> 2;
163 
164 		BT_DBG("hdr 0x%02x hdrlen %d len %d id %d", hdr, hdrlen, len, id);
165 
166 		if (hdrlen + len > skb->len) {
167 			BT_ERR("Wrong size or header information in CMTP frame");
168 			break;
169 		}
170 
171 		if (len == 0) {
172 			skb_pull(skb, hdrlen);
173 			continue;
174 		}
175 
176 		switch (hdr & 0x03) {
177 		case 0x00:
178 			cmtp_add_msgpart(session, id, skb->data + hdrlen, len);
179 			cmtp_recv_capimsg(session, session->reassembly[id]);
180 			session->reassembly[id] = NULL;
181 			break;
182 		case 0x01:
183 			cmtp_add_msgpart(session, id, skb->data + hdrlen, len);
184 			break;
185 		default:
186 			if (session->reassembly[id] != NULL)
187 				kfree_skb(session->reassembly[id]);
188 			session->reassembly[id] = NULL;
189 			break;
190 		}
191 
192 		skb_pull(skb, hdrlen + len);
193 	}
194 
195 	kfree_skb(skb);
196 	return 0;
197 }
198 
cmtp_send_frame(struct cmtp_session * session,unsigned char * data,int len)199 static int cmtp_send_frame(struct cmtp_session *session, unsigned char *data, int len)
200 {
201 	struct socket *sock = session->sock;
202 	struct iovec iv = { data, len };
203 	struct msghdr msg;
204 	int err;
205 
206 	BT_DBG("session %p data %p len %d", session, data, len);
207 
208 	if (!len)
209 		return 0;
210 
211 	memset(&msg, 0, sizeof(msg));
212 	msg.msg_iovlen = 1;
213 	msg.msg_iov = &iv;
214 
215 	err = sock->ops->sendmsg(sock, &msg, len, 0);
216 	return err;
217 }
218 
cmtp_process_transmit(struct cmtp_session * session)219 static int cmtp_process_transmit(struct cmtp_session *session)
220 {
221 	struct sk_buff *skb, *nskb;
222 	unsigned char *hdr;
223 	unsigned int size, tail;
224 
225 	BT_DBG("session %p", session);
226 
227 	if (!(nskb = alloc_skb(session->mtu, GFP_ATOMIC))) {
228 		BT_ERR("Can't allocate memory for new frame");
229 		return -ENOMEM;
230 	}
231 
232 	while ((skb = skb_dequeue(&session->transmit))) {
233 		struct cmtp_scb *scb = (void *) skb->cb;
234 
235 		if ((tail = (session->mtu - nskb->len)) < 5) {
236 			cmtp_send_frame(session, nskb->data, nskb->len);
237 			skb_trim(nskb, 0);
238 			tail = session->mtu;
239 		}
240 
241 		size = min_t(uint, ((tail < 258) ? (tail - 2) : (tail - 3)), skb->len);
242 
243 		if ((scb->id < 0) && ((scb->id = cmtp_alloc_block_id(session)) < 0)) {
244 			skb_queue_head(&session->transmit, skb);
245 			break;
246 		}
247 
248 		if (size < 256) {
249 			hdr = skb_put(nskb, 2);
250 			hdr[0] = 0x40
251 				| ((scb->id << 2) & 0x3c)
252 				| ((skb->len == size) ? 0x00 : 0x01);
253 			hdr[1] = size;
254 		} else {
255 			hdr = skb_put(nskb, 3);
256 			hdr[0] = 0x80
257 				| ((scb->id << 2) & 0x3c)
258 				| ((skb->len == size) ? 0x00 : 0x01);
259 			hdr[1] = size & 0xff;
260 			hdr[2] = size >> 8;
261 		}
262 
263 		memcpy(skb_put(nskb, size), skb->data, size);
264 		skb_pull(skb, size);
265 
266 		if (skb->len > 0) {
267 			skb_queue_head(&session->transmit, skb);
268 		} else {
269 			cmtp_free_block_id(session, scb->id);
270 			if (scb->data) {
271 				cmtp_send_frame(session, nskb->data, nskb->len);
272 				skb_trim(nskb, 0);
273 			}
274 			kfree_skb(skb);
275 		}
276 	}
277 
278 	cmtp_send_frame(session, nskb->data, nskb->len);
279 
280 	kfree_skb(nskb);
281 
282 	return skb_queue_len(&session->transmit);
283 }
284 
cmtp_session(void * arg)285 static int cmtp_session(void *arg)
286 {
287 	struct cmtp_session *session = arg;
288 	struct sock *sk = session->sock->sk;
289 	struct sk_buff *skb;
290 	wait_queue_t wait;
291 
292 	BT_DBG("session %p", session);
293 
294 	daemonize(); reparent_to_init();
295 
296 	sprintf(current->comm, "kcmtpd_ctr_%d", session->num);
297 
298 	sigfillset(&current->blocked);
299 	flush_signals(current);
300 
301 	current->nice = -15;
302 
303 	set_fs(KERNEL_DS);
304 
305 	init_waitqueue_entry(&wait, current);
306 	add_wait_queue(sk->sleep, &wait);
307 	while (!atomic_read(&session->terminate)) {
308 		set_current_state(TASK_INTERRUPTIBLE);
309 
310 		if (sk->state != BT_CONNECTED)
311 			break;
312 
313 		while ((skb = skb_dequeue(&sk->receive_queue))) {
314 			skb_orphan(skb);
315 			cmtp_recv_frame(session, skb);
316 		}
317 
318 		cmtp_process_transmit(session);
319 
320 		schedule();
321 	}
322 	set_current_state(TASK_RUNNING);
323 	remove_wait_queue(sk->sleep, &wait);
324 
325 	down_write(&cmtp_session_sem);
326 
327 	if (!(session->flags & (1 << CMTP_LOOPBACK)))
328 		cmtp_detach_device(session);
329 
330 	fput(session->sock->file);
331 
332 	__cmtp_unlink_session(session);
333 
334 	up_write(&cmtp_session_sem);
335 
336 	kfree(session);
337 	return 0;
338 }
339 
cmtp_add_connection(struct cmtp_connadd_req * req,struct socket * sock)340 int cmtp_add_connection(struct cmtp_connadd_req *req, struct socket *sock)
341 {
342 	struct cmtp_session *session, *s;
343 	bdaddr_t src, dst;
344 	int i, err;
345 
346 	BT_DBG("");
347 
348 	baswap(&src, &bluez_pi(sock->sk)->src);
349 	baswap(&dst, &bluez_pi(sock->sk)->dst);
350 
351 	session = kmalloc(sizeof(struct cmtp_session), GFP_KERNEL);
352 	if (!session)
353 		return -ENOMEM;
354 	memset(session, 0, sizeof(struct cmtp_session));
355 
356 	down_write(&cmtp_session_sem);
357 
358 	s = __cmtp_get_session(&bluez_pi(sock->sk)->dst);
359 	if (s && s->state == BT_CONNECTED) {
360 		err = -EEXIST;
361 		goto failed;
362 	}
363 
364 	bacpy(&session->bdaddr, &bluez_pi(sock->sk)->dst);
365 
366 	session->mtu = min_t(uint, l2cap_pi(sock->sk)->omtu, l2cap_pi(sock->sk)->imtu);
367 
368 	BT_DBG("mtu %d", session->mtu);
369 
370 	sprintf(session->name, "%s", batostr(&dst));
371 
372 	session->sock  = sock;
373 	session->state = BT_CONFIG;
374 
375 	init_waitqueue_head(&session->wait);
376 
377 	session->ctrl   = NULL;
378 	session->msgnum = CMTP_INITIAL_MSGNUM;
379 
380 	INIT_LIST_HEAD(&session->applications);
381 
382 	skb_queue_head_init(&session->transmit);
383 
384 	for (i = 0; i < 16; i++)
385 		session->reassembly[i] = NULL;
386 
387 	session->flags = req->flags;
388 
389 	__cmtp_link_session(session);
390 
391 	err = kernel_thread(cmtp_session, session, CLONE_FS | CLONE_FILES | CLONE_SIGHAND);
392 	if (err < 0)
393 		goto unlink;
394 
395 	if (!(session->flags & (1 << CMTP_LOOPBACK))) {
396 		err = cmtp_attach_device(session);
397 		if (err < 0)
398 			goto detach;
399 	}
400 
401 	up_write(&cmtp_session_sem);
402 	return 0;
403 
404 detach:
405 	cmtp_detach_device(session);
406 
407 unlink:
408 	__cmtp_unlink_session(session);
409 
410 failed:
411 	up_write(&cmtp_session_sem);
412 	kfree(session);
413 	return err;
414 }
415 
cmtp_del_connection(struct cmtp_conndel_req * req)416 int cmtp_del_connection(struct cmtp_conndel_req *req)
417 {
418 	struct cmtp_session *session;
419 	int err = 0;
420 
421 	BT_DBG("");
422 
423 	down_read(&cmtp_session_sem);
424 
425 	session = __cmtp_get_session(&req->bdaddr);
426 	if (session) {
427 		/* Flush the transmit queue */
428 		skb_queue_purge(&session->transmit);
429 
430 		/* Kill session thread */
431 		atomic_inc(&session->terminate);
432 		cmtp_schedule(session);
433 	} else
434 		err = -ENOENT;
435 
436 	up_read(&cmtp_session_sem);
437 	return err;
438 }
439 
cmtp_get_connlist(struct cmtp_connlist_req * req)440 int cmtp_get_connlist(struct cmtp_connlist_req *req)
441 {
442 	struct list_head *p;
443 	int err = 0, n = 0;
444 
445 	BT_DBG("");
446 
447 	down_read(&cmtp_session_sem);
448 
449 	list_for_each(p, &cmtp_session_list) {
450 		struct cmtp_session *session;
451 		struct cmtp_conninfo ci;
452 
453 		session = list_entry(p, struct cmtp_session, list);
454 
455 		__cmtp_copy_session(session, &ci);
456 
457 		if (copy_to_user(req->ci, &ci, sizeof(ci))) {
458 			err = -EFAULT;
459 			break;
460 		}
461 
462 		if (++n >= req->cnum)
463 			break;
464 
465 		req->ci++;
466 	}
467 	req->cnum = n;
468 
469 	up_read(&cmtp_session_sem);
470 	return err;
471 }
472 
cmtp_get_conninfo(struct cmtp_conninfo * ci)473 int cmtp_get_conninfo(struct cmtp_conninfo *ci)
474 {
475 	struct cmtp_session *session;
476 	int err = 0;
477 
478 	down_read(&cmtp_session_sem);
479 
480 	session = __cmtp_get_session(&ci->bdaddr);
481 	if (session)
482 		__cmtp_copy_session(session, ci);
483 	else
484 		err = -ENOENT;
485 
486 	up_read(&cmtp_session_sem);
487 	return err;
488 }
489 
490 
init_cmtp(void)491 int __init init_cmtp(void)
492 {
493 	l2cap_load();
494 
495 	cmtp_init_capi();
496 	cmtp_init_sockets();
497 
498 	BT_INFO("BlueZ CMTP ver %s", VERSION);
499 	BT_INFO("Copyright (C) 2002-2003 Marcel Holtmann <marcel@holtmann.org>");
500 
501 	return 0;
502 }
503 
exit_cmtp(void)504 void __exit exit_cmtp(void)
505 {
506 	cmtp_cleanup_sockets();
507 	cmtp_cleanup_capi();
508 }
509 
510 module_init(init_cmtp);
511 module_exit(exit_cmtp);
512 
513 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
514 MODULE_DESCRIPTION("BlueZ CMTP ver " VERSION);
515 MODULE_LICENSE("GPL");
516