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