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