1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
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 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
89static 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
102static inline void cmtp_free_block_id(struct cmtp_session *session, int id)
103{
104 clear_bit(id, &session->blockids);
105}
106
107static 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
132static 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
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 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
308
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_exit(0);
327 return 0;
328}
329
330int 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
396
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
410unlink:
411 __cmtp_unlink_session(session);
412
413failed:
414 up_write(&cmtp_session_sem);
415 kfree(session);
416 return err;
417}
418
419int 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
435 skb_queue_purge(&session->transmit);
436
437
438 atomic_inc(&session->terminate);
439
440
441
442
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
452int 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
482int 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
500static int __init cmtp_init(void)
501{
502 BT_INFO("CMTP (CAPI Emulation) ver %s", VERSION);
503
504 cmtp_init_sockets();
505
506 return 0;
507}
508
509static void __exit cmtp_exit(void)
510{
511 cmtp_cleanup_sockets();
512}
513
514module_init(cmtp_init);
515module_exit(cmtp_exit);
516
517MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
518MODULE_DESCRIPTION("Bluetooth CMTP ver " VERSION);
519MODULE_VERSION(VERSION);
520MODULE_LICENSE("GPL");
521MODULE_ALIAS("bt-proto-5");
522
