core.c revision fcb73338ed531dcc00cb17ca76fe3e05f774e4e9 
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
50static DECLARE_RWSEM(cmtp_session_sem);
51static LIST_HEAD(cmtp_session_list);
52
53static 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
66static void __cmtp_link_session(struct cmtp_session *session)
67{
68	list_add(&session->list, &cmtp_session_list);
69}
70
71static void __cmtp_unlink_session(struct cmtp_session *session)
72{
73	list_del(&session->list);
74}
75
76static void __cmtp_copy_session(struct cmtp_session *session, struct cmtp_conninfo *ci)
77{
78	memset(ci, 0, sizeof(*ci));
79	bacpy(&ci->bdaddr, &session->bdaddr);
80
81	ci->flags = session->flags;
82	ci->state = session->state;
83
84	ci->num = session->num;
85}
86
87
88static inline int cmtp_alloc_block_id(struct cmtp_session *session)
89{
90	int i, id = -1;
91
92	for (i = 0; i < 16; i++)
93		if (!test_and_set_bit(i, &session->blockids)) {
94			id = i;
95			break;
96		}
97
98	return id;
99}
100
101static inline void cmtp_free_block_id(struct cmtp_session *session, int id)
102{
103	clear_bit(id, &session->blockids);
104}
105
106static inline void cmtp_add_msgpart(struct cmtp_session *session, int id, const unsigned char *buf, int count)
107{
108	struct sk_buff *skb = session->reassembly[id], *nskb;
109	int size;
110
111	BT_DBG("session %p buf %p count %d", session, buf, count);
112
113	size = (skb) ? skb->len + count : count;
114
115	nskb = alloc_skb(size, GFP_ATOMIC);
116	if (!nskb) {
117		BT_ERR("Can't allocate memory for CAPI message");
118		return;
119	}
120
121	if (skb && (skb->len > 0))
122		skb_copy_from_linear_data(skb, skb_put(nskb, skb->len), skb->len);
123
124	memcpy(skb_put(nskb, count), buf, count);
125
126	session->reassembly[id] = nskb;
127
128	kfree_skb(skb);
129}
130
131static inline int cmtp_recv_frame(struct cmtp_session *session, struct sk_buff *skb)
132{
133	__u8 hdr, hdrlen, id;
134	__u16 len;
135
136	BT_DBG("session %p skb %p len %d", session, skb, skb->len);
137
138	while (skb->len > 0) {
139		hdr = skb->data[0];
140
141		switch (hdr & 0xc0) {
142		case 0x40:
143			hdrlen = 2;
144			len = skb->data[1];
145			break;
146		case 0x80:
147			hdrlen = 3;
148			len = skb->data[1] | (skb->data[2] << 8);
149			break;
150		default:
151			hdrlen = 1;
152			len = 0;
153			break;
154		}
155
156		id = (hdr & 0x3c) >> 2;
157
158		BT_DBG("hdr 0x%02x hdrlen %d len %d id %d", hdr, hdrlen, len, id);
159
160		if (hdrlen + len > skb->len) {
161			BT_ERR("Wrong size or header information in CMTP frame");
162			break;
163		}
164
165		if (len == 0) {
166			skb_pull(skb, hdrlen);
167			continue;
168		}
169
170		switch (hdr & 0x03) {
171		case 0x00:
172			cmtp_add_msgpart(session, id, skb->data + hdrlen, len);
173			cmtp_recv_capimsg(session, session->reassembly[id]);
174			session->reassembly[id] = NULL;
175			break;
176		case 0x01:
177			cmtp_add_msgpart(session, id, skb->data + hdrlen, len);
178			break;
179		default:
180			if (session->reassembly[id] != NULL)
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
193static 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
209static 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
278static 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	set_user_nice(current, -15);
288
289	init_waitqueue_entry(&wait, current);
290	add_wait_queue(sk_sleep(sk), &wait);
291	while (1) {
292		set_current_state(TASK_INTERRUPTIBLE);
293
294		if (atomic_read(&session->terminate))
295			break;
296		if (sk->sk_state != BT_CONNECTED)
297			break;
298
299		while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
300			skb_orphan(skb);
301			if (!skb_linearize(skb))
302				cmtp_recv_frame(session, skb);
303			else
304				kfree_skb(skb);
305		}
306
307		cmtp_process_transmit(session);
308
309		schedule();
310	}
311	__set_current_state(TASK_RUNNING);
312	remove_wait_queue(sk_sleep(sk), &wait);
313
314	down_write(&cmtp_session_sem);
315
316	if (!(session->flags & (1 << 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_exit(0);
327	return 0;
328}
329
330int cmtp_add_connection(struct cmtp_connadd_req *req, struct socket *sock)
331{
332	struct cmtp_session *session, *s;
333	int i, err;
334
335	BT_DBG("");
336
337	session = kzalloc(sizeof(struct cmtp_session), GFP_KERNEL);
338	if (!session)
339		return -ENOMEM;
340
341	down_write(&cmtp_session_sem);
342
343	s = __cmtp_get_session(&bt_sk(sock->sk)->dst);
344	if (s && s->state == BT_CONNECTED) {
345		err = -EEXIST;
346		goto failed;
347	}
348
349	bacpy(&session->bdaddr, &bt_sk(sock->sk)->dst);
350
351	session->mtu = min_t(uint, l2cap_pi(sock->sk)->chan->omtu,
352					l2cap_pi(sock->sk)->chan->imtu);
353
354	BT_DBG("mtu %d", session->mtu);
355
356	sprintf(session->name, "%pMR", &bt_sk(sock->sk)->dst);
357
358	session->sock  = sock;
359	session->state = BT_CONFIG;
360
361	init_waitqueue_head(&session->wait);
362
363	session->msgnum = CMTP_INITIAL_MSGNUM;
364
365	INIT_LIST_HEAD(&session->applications);
366
367	skb_queue_head_init(&session->transmit);
368
369	for (i = 0; i < 16; i++)
370		session->reassembly[i] = NULL;
371
372	session->flags = req->flags;
373
374	__cmtp_link_session(session);
375
376	__module_get(THIS_MODULE);
377	session->task = kthread_run(cmtp_session, session, "kcmtpd_ctr_%d",
378								session->num);
379	if (IS_ERR(session->task)) {
380		module_put(THIS_MODULE);
381		err = PTR_ERR(session->task);
382		goto unlink;
383	}
384
385	if (!(session->flags & (1 << CMTP_LOOPBACK))) {
386		err = cmtp_attach_device(session);
387		if (err < 0) {
388			atomic_inc(&session->terminate);
389			wake_up_process(session->task);
390			up_write(&cmtp_session_sem);
391			return err;
392		}
393	}
394
395	up_write(&cmtp_session_sem);
396	return 0;
397
398unlink:
399	__cmtp_unlink_session(session);
400
401failed:
402	up_write(&cmtp_session_sem);
403	kfree(session);
404	return err;
405}
406
407int cmtp_del_connection(struct cmtp_conndel_req *req)
408{
409	struct cmtp_session *session;
410	int err = 0;
411
412	BT_DBG("");
413
414	down_read(&cmtp_session_sem);
415
416	session = __cmtp_get_session(&req->bdaddr);
417	if (session) {
418		/* Flush the transmit queue */
419		skb_queue_purge(&session->transmit);
420
421		/* Stop session thread */
422		atomic_inc(&session->terminate);
423		wake_up_process(session->task);
424	} else
425		err = -ENOENT;
426
427	up_read(&cmtp_session_sem);
428	return err;
429}
430
431int cmtp_get_connlist(struct cmtp_connlist_req *req)
432{
433	struct cmtp_session *session;
434	int err = 0, n = 0;
435
436	BT_DBG("");
437
438	down_read(&cmtp_session_sem);
439
440	list_for_each_entry(session, &cmtp_session_list, list) {
441		struct cmtp_conninfo ci;
442
443		__cmtp_copy_session(session, &ci);
444
445		if (copy_to_user(req->ci, &ci, sizeof(ci))) {
446			err = -EFAULT;
447			break;
448		}
449
450		if (++n >= req->cnum)
451			break;
452
453		req->ci++;
454	}
455	req->cnum = n;
456
457	up_read(&cmtp_session_sem);
458	return err;
459}
460
461int cmtp_get_conninfo(struct cmtp_conninfo *ci)
462{
463	struct cmtp_session *session;
464	int err = 0;
465
466	down_read(&cmtp_session_sem);
467
468	session = __cmtp_get_session(&ci->bdaddr);
469	if (session)
470		__cmtp_copy_session(session, ci);
471	else
472		err = -ENOENT;
473
474	up_read(&cmtp_session_sem);
475	return err;
476}
477
478
479static int __init cmtp_init(void)
480{
481	BT_INFO("CMTP (CAPI Emulation) ver %s", VERSION);
482
483	cmtp_init_sockets();
484
485	return 0;
486}
487
488static void __exit cmtp_exit(void)
489{
490	cmtp_cleanup_sockets();
491}
492
493module_init(cmtp_init);
494module_exit(cmtp_exit);
495
496MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
497MODULE_DESCRIPTION("Bluetooth CMTP ver " VERSION);
498MODULE_VERSION(VERSION);
499MODULE_LICENSE("GPL");
500MODULE_ALIAS("bt-proto-5");
501