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