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
__cmtp_get_session(bdaddr_t * bdaddr)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
__cmtp_link_session(struct cmtp_session * session)66 static void __cmtp_link_session(struct cmtp_session *session)
67 {
68 list_add(&session->list, &cmtp_session_list);
69 }
70
__cmtp_unlink_session(struct cmtp_session * session)71 static void __cmtp_unlink_session(struct cmtp_session *session)
72 {
73 list_del(&session->list);
74 }
75
__cmtp_copy_session(struct cmtp_session * session,struct cmtp_conninfo * ci)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
cmtp_alloc_block_id(struct cmtp_session * session)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
cmtp_free_block_id(struct cmtp_session * session,int id)102 static inline void cmtp_free_block_id(struct cmtp_session *session, int id)
103 {
104 clear_bit(id, &session->blockids);
105 }
106
cmtp_add_msgpart(struct cmtp_session * session,int id,const unsigned char * buf,int count)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 memcpy(skb_put(nskb, count), buf, count);
126
127 session->reassembly[id] = nskb;
128
129 kfree_skb(skb);
130 }
131
cmtp_recv_frame(struct cmtp_session * session,struct sk_buff * skb)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 if (session->reassembly[id] != NULL)
182 kfree_skb(session->reassembly[id]);
183 session->reassembly[id] = NULL;
184 break;
185 }
186
187 skb_pull(skb, hdrlen + len);
188 }
189
190 kfree_skb(skb);
191 return 0;
192 }
193
cmtp_send_frame(struct cmtp_session * session,unsigned char * data,int len)194 static int cmtp_send_frame(struct cmtp_session *session, unsigned char *data, int len)
195 {
196 struct socket *sock = session->sock;
197 struct kvec iv = { data, len };
198 struct msghdr msg;
199
200 BT_DBG("session %p data %p len %d", session, data, len);
201
202 if (!len)
203 return 0;
204
205 memset(&msg, 0, sizeof(msg));
206
207 return kernel_sendmsg(sock, &msg, &iv, 1, len);
208 }
209
cmtp_process_transmit(struct cmtp_session * session)210 static void cmtp_process_transmit(struct cmtp_session *session)
211 {
212 struct sk_buff *skb, *nskb;
213 unsigned char *hdr;
214 unsigned int size, tail;
215
216 BT_DBG("session %p", session);
217
218 nskb = alloc_skb(session->mtu, GFP_ATOMIC);
219 if (!nskb) {
220 BT_ERR("Can't allocate memory for new frame");
221 return;
222 }
223
224 while ((skb = skb_dequeue(&session->transmit))) {
225 struct cmtp_scb *scb = (void *) skb->cb;
226
227 tail = session->mtu - nskb->len;
228 if (tail < 5) {
229 cmtp_send_frame(session, nskb->data, nskb->len);
230 skb_trim(nskb, 0);
231 tail = session->mtu;
232 }
233
234 size = min_t(uint, ((tail < 258) ? (tail - 2) : (tail - 3)), skb->len);
235
236 if (scb->id < 0) {
237 scb->id = cmtp_alloc_block_id(session);
238 if (scb->id < 0) {
239 skb_queue_head(&session->transmit, skb);
240 break;
241 }
242 }
243
244 if (size < 256) {
245 hdr = skb_put(nskb, 2);
246 hdr[0] = 0x40
247 | ((scb->id << 2) & 0x3c)
248 | ((skb->len == size) ? 0x00 : 0x01);
249 hdr[1] = size;
250 } else {
251 hdr = skb_put(nskb, 3);
252 hdr[0] = 0x80
253 | ((scb->id << 2) & 0x3c)
254 | ((skb->len == size) ? 0x00 : 0x01);
255 hdr[1] = size & 0xff;
256 hdr[2] = size >> 8;
257 }
258
259 skb_copy_from_linear_data(skb, skb_put(nskb, size), size);
260 skb_pull(skb, size);
261
262 if (skb->len > 0) {
263 skb_queue_head(&session->transmit, skb);
264 } else {
265 cmtp_free_block_id(session, scb->id);
266 if (scb->data) {
267 cmtp_send_frame(session, nskb->data, nskb->len);
268 skb_trim(nskb, 0);
269 }
270 kfree_skb(skb);
271 }
272 }
273
274 cmtp_send_frame(session, nskb->data, nskb->len);
275
276 kfree_skb(nskb);
277 }
278
cmtp_session(void * arg)279 static int cmtp_session(void *arg)
280 {
281 struct cmtp_session *session = arg;
282 struct sock *sk = session->sock->sk;
283 struct sk_buff *skb;
284 wait_queue_t wait;
285
286 BT_DBG("session %p", session);
287
288 set_user_nice(current, -15);
289
290 init_waitqueue_entry(&wait, current);
291 add_wait_queue(sk_sleep(sk), &wait);
292 while (1) {
293 set_current_state(TASK_INTERRUPTIBLE);
294
295 if (atomic_read(&session->terminate))
296 break;
297 if (sk->sk_state != BT_CONNECTED)
298 break;
299
300 while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
301 skb_orphan(skb);
302 if (!skb_linearize(skb))
303 cmtp_recv_frame(session, skb);
304 else
305 kfree_skb(skb);
306 }
307
308 cmtp_process_transmit(session);
309
310 schedule();
311 }
312 __set_current_state(TASK_RUNNING);
313 remove_wait_queue(sk_sleep(sk), &wait);
314
315 down_write(&cmtp_session_sem);
316
317 if (!(session->flags & BIT(CMTP_LOOPBACK)))
318 cmtp_detach_device(session);
319
320 fput(session->sock->file);
321
322 __cmtp_unlink_session(session);
323
324 up_write(&cmtp_session_sem);
325
326 kfree(session);
327 module_put_and_exit(0);
328 return 0;
329 }
330
cmtp_add_connection(struct cmtp_connadd_req * req,struct socket * sock)331 int cmtp_add_connection(struct cmtp_connadd_req *req, struct socket *sock)
332 {
333 u32 valid_flags = BIT(CMTP_LOOPBACK);
334 struct cmtp_session *session, *s;
335 int i, err;
336
337 BT_DBG("");
338
339 if (!l2cap_is_socket(sock))
340 return -EBADFD;
341
342 if (req->flags & ~valid_flags)
343 return -EINVAL;
344
345 session = kzalloc(sizeof(struct cmtp_session), GFP_KERNEL);
346 if (!session)
347 return -ENOMEM;
348
349 down_write(&cmtp_session_sem);
350
351 s = __cmtp_get_session(&l2cap_pi(sock->sk)->chan->dst);
352 if (s && s->state == BT_CONNECTED) {
353 err = -EEXIST;
354 goto failed;
355 }
356
357 bacpy(&session->bdaddr, &l2cap_pi(sock->sk)->chan->dst);
358
359 session->mtu = min_t(uint, l2cap_pi(sock->sk)->chan->omtu,
360 l2cap_pi(sock->sk)->chan->imtu);
361
362 BT_DBG("mtu %d", session->mtu);
363
364 sprintf(session->name, "%pMR", &session->bdaddr);
365
366 session->sock = sock;
367 session->state = BT_CONFIG;
368
369 init_waitqueue_head(&session->wait);
370
371 session->msgnum = CMTP_INITIAL_MSGNUM;
372
373 INIT_LIST_HEAD(&session->applications);
374
375 skb_queue_head_init(&session->transmit);
376
377 for (i = 0; i < 16; i++)
378 session->reassembly[i] = NULL;
379
380 session->flags = req->flags;
381
382 __cmtp_link_session(session);
383
384 __module_get(THIS_MODULE);
385 session->task = kthread_run(cmtp_session, session, "kcmtpd_ctr_%d",
386 session->num);
387 if (IS_ERR(session->task)) {
388 module_put(THIS_MODULE);
389 err = PTR_ERR(session->task);
390 goto unlink;
391 }
392
393 if (!(session->flags & BIT(CMTP_LOOPBACK))) {
394 err = cmtp_attach_device(session);
395 if (err < 0) {
396 atomic_inc(&session->terminate);
397 wake_up_process(session->task);
398 up_write(&cmtp_session_sem);
399 return err;
400 }
401 }
402
403 up_write(&cmtp_session_sem);
404 return 0;
405
406 unlink:
407 __cmtp_unlink_session(session);
408
409 failed:
410 up_write(&cmtp_session_sem);
411 kfree(session);
412 return err;
413 }
414
cmtp_del_connection(struct cmtp_conndel_req * req)415 int cmtp_del_connection(struct cmtp_conndel_req *req)
416 {
417 u32 valid_flags = 0;
418 struct cmtp_session *session;
419 int err = 0;
420
421 BT_DBG("");
422
423 if (req->flags & ~valid_flags)
424 return -EINVAL;
425
426 down_read(&cmtp_session_sem);
427
428 session = __cmtp_get_session(&req->bdaddr);
429 if (session) {
430 /* Flush the transmit queue */
431 skb_queue_purge(&session->transmit);
432
433 /* Stop session thread */
434 atomic_inc(&session->terminate);
435 wake_up_process(session->task);
436 } else
437 err = -ENOENT;
438
439 up_read(&cmtp_session_sem);
440 return err;
441 }
442
cmtp_get_connlist(struct cmtp_connlist_req * req)443 int cmtp_get_connlist(struct cmtp_connlist_req *req)
444 {
445 struct cmtp_session *session;
446 int err = 0, n = 0;
447
448 BT_DBG("");
449
450 down_read(&cmtp_session_sem);
451
452 list_for_each_entry(session, &cmtp_session_list, list) {
453 struct cmtp_conninfo ci;
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
cmtp_init(void)491 static int __init cmtp_init(void)
492 {
493 BT_INFO("CMTP (CAPI Emulation) ver %s", VERSION);
494
495 cmtp_init_sockets();
496
497 return 0;
498 }
499
cmtp_exit(void)500 static void __exit cmtp_exit(void)
501 {
502 cmtp_cleanup_sockets();
503 }
504
505 module_init(cmtp_init);
506 module_exit(cmtp_exit);
507
508 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
509 MODULE_DESCRIPTION("Bluetooth CMTP ver " VERSION);
510 MODULE_VERSION(VERSION);
511 MODULE_LICENSE("GPL");
512 MODULE_ALIAS("bt-proto-5");
513