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/module.h>
24 
25 #include <linux/types.h>
26 #include <linux/errno.h>
27 #include <linux/kernel.h>
28 #include <linux/sched.h>
29 #include <linux/slab.h>
30 #include <linux/poll.h>
31 #include <linux/fcntl.h>
32 #include <linux/freezer.h>
33 #include <linux/skbuff.h>
34 #include <linux/socket.h>
35 #include <linux/ioctl.h>
36 #include <linux/file.h>
37 #include <linux/init.h>
38 #include <net/sock.h>
39 
40 #include <linux/isdn/capilli.h>
41 
42 #include <net/bluetooth/bluetooth.h>
43 #include <net/bluetooth/l2cap.h>
44 
45 #include "cmtp.h"
46 
47 #define VERSION "1.0"
48 
49 static DECLARE_RWSEM(cmtp_session_sem);
50 static LIST_HEAD(cmtp_session_list);
51 
__cmtp_get_session(bdaddr_t * bdaddr)52 static struct cmtp_session *__cmtp_get_session(bdaddr_t *bdaddr)
53 {
54 	struct cmtp_session *session;
55 	struct list_head *p;
56 
57 	BT_DBG("");
58 
59 	list_for_each(p, &cmtp_session_list) {
60 		session = list_entry(p, struct cmtp_session, list);
61 		if (!bacmp(bdaddr, &session->bdaddr))
62 			return session;
63 	}
64 	return NULL;
65 }
66 
__cmtp_link_session(struct cmtp_session * session)67 static void __cmtp_link_session(struct cmtp_session *session)
68 {
69 	__module_get(THIS_MODULE);
70 	list_add(&session->list, &cmtp_session_list);
71 }
72 
__cmtp_unlink_session(struct cmtp_session * session)73 static void __cmtp_unlink_session(struct cmtp_session *session)
74 {
75 	list_del(&session->list);
76 	module_put(THIS_MODULE);
77 }
78 
__cmtp_copy_session(struct cmtp_session * session,struct cmtp_conninfo * ci)79 static void __cmtp_copy_session(struct cmtp_session *session, struct cmtp_conninfo *ci)
80 {
81 	memset(ci, 0, sizeof(*ci));
82 	bacpy(&ci->bdaddr, &session->bdaddr);
83 
84 	ci->flags = session->flags;
85 	ci->state = session->state;
86 
87 	ci->num = session->num;
88 }
89 
90 
cmtp_alloc_block_id(struct cmtp_session * session)91 static inline int cmtp_alloc_block_id(struct cmtp_session *session)
92 {
93 	int i, id = -1;
94 
95 	for (i = 0; i < 16; i++)
96 		if (!test_and_set_bit(i, &session->blockids)) {
97 			id = i;
98 			break;
99 		}
100 
101 	return id;
102 }
103 
cmtp_free_block_id(struct cmtp_session * session,int id)104 static inline void cmtp_free_block_id(struct cmtp_session *session, int id)
105 {
106 	clear_bit(id, &session->blockids);
107 }
108 
cmtp_add_msgpart(struct cmtp_session * session,int id,const unsigned char * buf,int count)109 static inline void cmtp_add_msgpart(struct cmtp_session *session, int id, const unsigned char *buf, int count)
110 {
111 	struct sk_buff *skb = session->reassembly[id], *nskb;
112 	int size;
113 
114 	BT_DBG("session %p buf %p count %d", session, buf, count);
115 
116 	size = (skb) ? skb->len + count : count;
117 
118 	nskb = alloc_skb(size, GFP_ATOMIC);
119 	if (!nskb) {
120 		BT_ERR("Can't allocate memory for CAPI message");
121 		return;
122 	}
123 
124 	if (skb && (skb->len > 0))
125 		skb_copy_from_linear_data(skb, skb_put(nskb, skb->len), skb->len);
126 
127 	memcpy(skb_put(nskb, count), buf, count);
128 
129 	session->reassembly[id] = nskb;
130 
131 	kfree_skb(skb);
132 }
133 
cmtp_recv_frame(struct cmtp_session * session,struct sk_buff * skb)134 static inline int cmtp_recv_frame(struct cmtp_session *session, struct sk_buff *skb)
135 {
136 	__u8 hdr, hdrlen, id;
137 	__u16 len;
138 
139 	BT_DBG("session %p skb %p len %d", session, skb, skb->len);
140 
141 	while (skb->len > 0) {
142 		hdr = skb->data[0];
143 
144 		switch (hdr & 0xc0) {
145 		case 0x40:
146 			hdrlen = 2;
147 			len = skb->data[1];
148 			break;
149 		case 0x80:
150 			hdrlen = 3;
151 			len = skb->data[1] | (skb->data[2] << 8);
152 			break;
153 		default:
154 			hdrlen = 1;
155 			len = 0;
156 			break;
157 		}
158 
159 		id = (hdr & 0x3c) >> 2;
160 
161 		BT_DBG("hdr 0x%02x hdrlen %d len %d id %d", hdr, hdrlen, len, id);
162 
163 		if (hdrlen + len > skb->len) {
164 			BT_ERR("Wrong size or header information in CMTP frame");
165 			break;
166 		}
167 
168 		if (len == 0) {
169 			skb_pull(skb, hdrlen);
170 			continue;
171 		}
172 
173 		switch (hdr & 0x03) {
174 		case 0x00:
175 			cmtp_add_msgpart(session, id, skb->data + hdrlen, len);
176 			cmtp_recv_capimsg(session, session->reassembly[id]);
177 			session->reassembly[id] = NULL;
178 			break;
179 		case 0x01:
180 			cmtp_add_msgpart(session, id, skb->data + hdrlen, len);
181 			break;
182 		default:
183 			if (session->reassembly[id] != NULL)
184 				kfree_skb(session->reassembly[id]);
185 			session->reassembly[id] = NULL;
186 			break;
187 		}
188 
189 		skb_pull(skb, hdrlen + len);
190 	}
191 
192 	kfree_skb(skb);
193 	return 0;
194 }
195 
cmtp_send_frame(struct cmtp_session * session,unsigned char * data,int len)196 static int cmtp_send_frame(struct cmtp_session *session, unsigned char *data, int len)
197 {
198 	struct socket *sock = session->sock;
199 	struct kvec iv = { data, len };
200 	struct msghdr msg;
201 
202 	BT_DBG("session %p data %p len %d", session, data, len);
203 
204 	if (!len)
205 		return 0;
206 
207 	memset(&msg, 0, sizeof(msg));
208 
209 	return kernel_sendmsg(sock, &msg, &iv, 1, len);
210 }
211 
cmtp_process_transmit(struct cmtp_session * session)212 static void cmtp_process_transmit(struct cmtp_session *session)
213 {
214 	struct sk_buff *skb, *nskb;
215 	unsigned char *hdr;
216 	unsigned int size, tail;
217 
218 	BT_DBG("session %p", session);
219 
220 	nskb = alloc_skb(session->mtu, GFP_ATOMIC);
221 	if (!nskb) {
222 		BT_ERR("Can't allocate memory for new frame");
223 		return;
224 	}
225 
226 	while ((skb = skb_dequeue(&session->transmit))) {
227 		struct cmtp_scb *scb = (void *) skb->cb;
228 
229 		tail = session->mtu - nskb->len;
230 		if (tail < 5) {
231 			cmtp_send_frame(session, nskb->data, nskb->len);
232 			skb_trim(nskb, 0);
233 			tail = session->mtu;
234 		}
235 
236 		size = min_t(uint, ((tail < 258) ? (tail - 2) : (tail - 3)), skb->len);
237 
238 		if ((scb->id < 0) && ((scb->id = cmtp_alloc_block_id(session)) < 0)) {
239 			skb_queue_head(&session->transmit, skb);
240 			break;
241 		}
242 
243 		if (size < 256) {
244 			hdr = skb_put(nskb, 2);
245 			hdr[0] = 0x40
246 				| ((scb->id << 2) & 0x3c)
247 				| ((skb->len == size) ? 0x00 : 0x01);
248 			hdr[1] = size;
249 		} else {
250 			hdr = skb_put(nskb, 3);
251 			hdr[0] = 0x80
252 				| ((scb->id << 2) & 0x3c)
253 				| ((skb->len == size) ? 0x00 : 0x01);
254 			hdr[1] = size & 0xff;
255 			hdr[2] = size >> 8;
256 		}
257 
258 		skb_copy_from_linear_data(skb, skb_put(nskb, size), size);
259 		skb_pull(skb, size);
260 
261 		if (skb->len > 0) {
262 			skb_queue_head(&session->transmit, skb);
263 		} else {
264 			cmtp_free_block_id(session, scb->id);
265 			if (scb->data) {
266 				cmtp_send_frame(session, nskb->data, nskb->len);
267 				skb_trim(nskb, 0);
268 			}
269 			kfree_skb(skb);
270 		}
271 	}
272 
273 	cmtp_send_frame(session, nskb->data, nskb->len);
274 
275 	kfree_skb(nskb);
276 }
277 
cmtp_session(void * arg)278 static int cmtp_session(void *arg)
279 {
280 	struct cmtp_session *session = arg;
281 	struct sock *sk = session->sock->sk;
282 	struct sk_buff *skb;
283 	wait_queue_t wait;
284 
285 	BT_DBG("session %p", session);
286 
287 	daemonize("kcmtpd_ctr_%d", session->num);
288 	set_user_nice(current, -15);
289 
290 	init_waitqueue_entry(&wait, current);
291 	add_wait_queue(sk_sleep(sk), &wait);
292 	while (!atomic_read(&session->terminate)) {
293 		set_current_state(TASK_INTERRUPTIBLE);
294 
295 		if (sk->sk_state != BT_CONNECTED)
296 			break;
297 
298 		while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
299 			skb_orphan(skb);
300 			cmtp_recv_frame(session, skb);
301 		}
302 
303 		cmtp_process_transmit(session);
304 
305 		schedule();
306 	}
307 	set_current_state(TASK_RUNNING);
308 	remove_wait_queue(sk_sleep(sk), &wait);
309 
310 	down_write(&cmtp_session_sem);
311 
312 	if (!(session->flags & (1 << CMTP_LOOPBACK)))
313 		cmtp_detach_device(session);
314 
315 	fput(session->sock->file);
316 
317 	__cmtp_unlink_session(session);
318 
319 	up_write(&cmtp_session_sem);
320 
321 	kfree(session);
322 	return 0;
323 }
324 
cmtp_add_connection(struct cmtp_connadd_req * req,struct socket * sock)325 int cmtp_add_connection(struct cmtp_connadd_req *req, struct socket *sock)
326 {
327 	struct cmtp_session *session, *s;
328 	int i, err;
329 
330 	BT_DBG("");
331 
332 	session = kzalloc(sizeof(struct cmtp_session), GFP_KERNEL);
333 	if (!session)
334 		return -ENOMEM;
335 
336 	down_write(&cmtp_session_sem);
337 
338 	s = __cmtp_get_session(&bt_sk(sock->sk)->dst);
339 	if (s && s->state == BT_CONNECTED) {
340 		err = -EEXIST;
341 		goto failed;
342 	}
343 
344 	bacpy(&session->bdaddr, &bt_sk(sock->sk)->dst);
345 
346 	session->mtu = min_t(uint, l2cap_pi(sock->sk)->omtu, l2cap_pi(sock->sk)->imtu);
347 
348 	BT_DBG("mtu %d", session->mtu);
349 
350 	sprintf(session->name, "%s", batostr(&bt_sk(sock->sk)->dst));
351 
352 	session->sock  = sock;
353 	session->state = BT_CONFIG;
354 
355 	init_waitqueue_head(&session->wait);
356 
357 	session->msgnum = CMTP_INITIAL_MSGNUM;
358 
359 	INIT_LIST_HEAD(&session->applications);
360 
361 	skb_queue_head_init(&session->transmit);
362 
363 	for (i = 0; i < 16; i++)
364 		session->reassembly[i] = NULL;
365 
366 	session->flags = req->flags;
367 
368 	__cmtp_link_session(session);
369 
370 	err = kernel_thread(cmtp_session, session, CLONE_KERNEL);
371 	if (err < 0)
372 		goto unlink;
373 
374 	if (!(session->flags & (1 << CMTP_LOOPBACK))) {
375 		err = cmtp_attach_device(session);
376 		if (err < 0)
377 			goto detach;
378 	}
379 
380 	up_write(&cmtp_session_sem);
381 	return 0;
382 
383 detach:
384 	cmtp_detach_device(session);
385 
386 unlink:
387 	__cmtp_unlink_session(session);
388 
389 failed:
390 	up_write(&cmtp_session_sem);
391 	kfree(session);
392 	return err;
393 }
394 
cmtp_del_connection(struct cmtp_conndel_req * req)395 int cmtp_del_connection(struct cmtp_conndel_req *req)
396 {
397 	struct cmtp_session *session;
398 	int err = 0;
399 
400 	BT_DBG("");
401 
402 	down_read(&cmtp_session_sem);
403 
404 	session = __cmtp_get_session(&req->bdaddr);
405 	if (session) {
406 		/* Flush the transmit queue */
407 		skb_queue_purge(&session->transmit);
408 
409 		/* Kill session thread */
410 		atomic_inc(&session->terminate);
411 		cmtp_schedule(session);
412 	} else
413 		err = -ENOENT;
414 
415 	up_read(&cmtp_session_sem);
416 	return err;
417 }
418 
cmtp_get_connlist(struct cmtp_connlist_req * req)419 int cmtp_get_connlist(struct cmtp_connlist_req *req)
420 {
421 	struct list_head *p;
422 	int err = 0, n = 0;
423 
424 	BT_DBG("");
425 
426 	down_read(&cmtp_session_sem);
427 
428 	list_for_each(p, &cmtp_session_list) {
429 		struct cmtp_session *session;
430 		struct cmtp_conninfo ci;
431 
432 		session = list_entry(p, struct cmtp_session, list);
433 
434 		__cmtp_copy_session(session, &ci);
435 
436 		if (copy_to_user(req->ci, &ci, sizeof(ci))) {
437 			err = -EFAULT;
438 			break;
439 		}
440 
441 		if (++n >= req->cnum)
442 			break;
443 
444 		req->ci++;
445 	}
446 	req->cnum = n;
447 
448 	up_read(&cmtp_session_sem);
449 	return err;
450 }
451 
cmtp_get_conninfo(struct cmtp_conninfo * ci)452 int cmtp_get_conninfo(struct cmtp_conninfo *ci)
453 {
454 	struct cmtp_session *session;
455 	int err = 0;
456 
457 	down_read(&cmtp_session_sem);
458 
459 	session = __cmtp_get_session(&ci->bdaddr);
460 	if (session)
461 		__cmtp_copy_session(session, ci);
462 	else
463 		err = -ENOENT;
464 
465 	up_read(&cmtp_session_sem);
466 	return err;
467 }
468 
469 
cmtp_init(void)470 static int __init cmtp_init(void)
471 {
472 	BT_INFO("CMTP (CAPI Emulation) ver %s", VERSION);
473 
474 	cmtp_init_sockets();
475 
476 	return 0;
477 }
478 
cmtp_exit(void)479 static void __exit cmtp_exit(void)
480 {
481 	cmtp_cleanup_sockets();
482 }
483 
484 module_init(cmtp_init);
485 module_exit(cmtp_exit);
486 
487 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
488 MODULE_DESCRIPTION("Bluetooth CMTP ver " VERSION);
489 MODULE_VERSION(VERSION);
490 MODULE_LICENSE("GPL");
491 MODULE_ALIAS("bt-proto-5");
492