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/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
  53 static struct cmtp_session *__cmtp_get_session(bdaddr_t *bdaddr)
  54 {
  55         struct cmtp_session *session;
  56         struct list_head *p;
  57
  58         BT_DBG("");
  59
  60         list_for_each(p, &cmtp_session_list) {
  61                 session = list_entry(p, struct cmtp_session, list);
  62                 if (!bacmp(bdaddr, &session->bdaddr))
  63                         return session;
  64         }
  65         return NULL;
  66 }
  67
  68 static void __cmtp_link_session(struct cmtp_session *session)
  69 {
  70         __module_get(THIS_MODULE);
  71         list_add(&session->list, &cmtp_session_list);
  72 }
  73
  74 static void __cmtp_unlink_session(struct cmtp_session *session)
  75 {
  76         list_del(&session->list);
  77         module_put(THIS_MODULE);
  78 }
  79
  80 static void __cmtp_copy_session(struct cmtp_session *session, struct cmtp_conninfo *ci)
  81 {
  82         memset(ci, 0, sizeof(*ci));
  83         bacpy(&ci->bdaddr, &session->bdaddr);
  84
  85         ci->flags = session->flags;
  86         ci->state = session->state;
  87
  88         ci->num = session->num;
  89 }
  90
  91
  92 static inline int cmtp_alloc_block_id(struct cmtp_session *session)
  93 {
  94         int i, id = -1;
  95
  96         for (i = 0; i < 16; i++)
  97                 if (!test_and_set_bit(i, &session->blockids)) {
  98                         id = i;
  99                         break;
 100                 }
 101
 102         return id;
 103 }
 104
 105 static inline void cmtp_free_block_id(struct cmtp_session *session, int id)
 106 {
 107         clear_bit(id, &session->blockids);
 108 }
 109
 110 static inline void cmtp_add_msgpart(struct cmtp_session *session, int id, const unsigned char *buf, int count)
 111 {
 112         struct sk_buff *skb = session->reassembly[id], *nskb;
 113         int size;
 114
 115         BT_DBG("session %p buf %p count %d", session, buf, count);
 116
 117         size = (skb) ? skb->len + count : count;
 118
 119         nskb = alloc_skb(size, GFP_ATOMIC);
 120         if (!nskb) {
 121                 BT_ERR("Can't allocate memory for CAPI message");
 122                 return;
 123         }
 124
 125         if (skb && (skb->len > 0))
 126                 skb_copy_from_linear_data(skb, skb_put(nskb, skb->len), skb->len);
 127
 128         memcpy(skb_put(nskb, count), buf, count);
 129
 130         session->reassembly[id] = nskb;
 131
 132         kfree_skb(skb);
 133 }
 134
 135 static inline int cmtp_recv_frame(struct cmtp_session *session, struct sk_buff *skb)
 136 {
 137         __u8 hdr, hdrlen, id;
 138         __u16 len;
 139
 140         BT_DBG("session %p skb %p len %d", session, skb, skb->len);
 141
 142         while (skb->len > 0) {
 143                 hdr = skb->data[0];
 144
 145                 switch (hdr & 0xc0) {
 146                 case 0x40:
 147                         hdrlen = 2;
 148                         len = skb->data[1];
 149                         break;
 150                 case 0x80:
 151                         hdrlen = 3;
 152                         len = skb->data[1] | (skb->data[2] << 8);
 153                         break;
 154                 default:
 155                         hdrlen = 1;
 156                         len = 0;
 157                         break;
 158                 }
 159
 160                 id = (hdr & 0x3c) >> 2;
 161
 162                 BT_DBG("hdr 0x%02x hdrlen %d len %d id %d", hdr, hdrlen, len, id);
 163
 164                 if (hdrlen + len > skb->len) {
 165                         BT_ERR("Wrong size or header information in CMTP frame");
 166                         break;
 167                 }
 168
 169                 if (len == 0) {
 170                         skb_pull(skb, hdrlen);
 171                         continue;
 172                 }
 173
 174                 switch (hdr & 0x03) {
 175                 case 0x00:
 176                         cmtp_add_msgpart(session, id, skb->data + hdrlen, len);
 177                         cmtp_recv_capimsg(session, session->reassembly[id]);
 178                         session->reassembly[id] = NULL;
 179                         break;
 180                 case 0x01:
 181                         cmtp_add_msgpart(session, id, skb->data + hdrlen, len);
 182                         break;
 183                 default:
 184                         if (session->reassembly[id] != NULL)
 185                                 kfree_skb(session->reassembly[id]);
 186                         session->reassembly[id] = NULL;
 187                         break;
 188                 }
 189
 190                 skb_pull(skb, hdrlen + len);
 191         }
 192
 193         kfree_skb(skb);
 194         return 0;
 195 }
 196
 197 static int cmtp_send_frame(struct cmtp_session *session, unsigned char *data, int len)
 198 {
 199         struct socket *sock = session->sock;
 200         struct kvec iv = { data, len };
 201         struct msghdr msg;
 202
 203         BT_DBG("session %p data %p len %d", session, data, len);
 204
 205         if (!len)
 206                 return 0;
 207
 208         memset(&msg, 0, sizeof(msg));
 209
 210         return kernel_sendmsg(sock, &msg, &iv, 1, len);
 211 }
 212
 213 static void cmtp_process_transmit(struct cmtp_session *session)
 214 {
 215         struct sk_buff *skb, *nskb;
 216         unsigned char *hdr;
 217         unsigned int size, tail;
 218
 219         BT_DBG("session %p", session);
 220
 221         nskb = alloc_skb(session->mtu, GFP_ATOMIC);
 222         if (!nskb) {
 223                 BT_ERR("Can't allocate memory for new frame");
 224                 return;
 225         }
 226
 227         while ((skb = skb_dequeue(&session->transmit))) {
 228                 struct cmtp_scb *scb = (void *) skb->cb;
 229
 230                 tail = session->mtu - nskb->len;
 231                 if (tail < 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) {
 240                         scb->id = cmtp_alloc_block_id(session);
 241                         if (scb->id < 0) {
 242                                 skb_queue_head(&session->transmit, skb);
 243                                 break;
 244                         }
 245                 }
 246
 247                 if (size < 256) {
 248                         hdr = skb_put(nskb, 2);
 249                         hdr[0] = 0x40
 250                                 | ((scb->id << 2) & 0x3c)
 251                                 | ((skb->len == size) ? 0x00 : 0x01);
 252                         hdr[1] = size;
 253                 } else {
 254                         hdr = skb_put(nskb, 3);
 255                         hdr[0] = 0x80
 256                                 | ((scb->id << 2) & 0x3c)
 257                                 | ((skb->len == size) ? 0x00 : 0x01);
 258                         hdr[1] = size & 0xff;
 259                         hdr[2] = size >> 8;
 260                 }
 261
 262                 skb_copy_from_linear_data(skb, skb_put(nskb, size), size);
 263                 skb_pull(skb, size);
 264
 265                 if (skb->len > 0) {
 266                         skb_queue_head(&session->transmit, skb);
 267                 } else {
 268                         cmtp_free_block_id(session, scb->id);
 269                         if (scb->data) {
 270                                 cmtp_send_frame(session, nskb->data, nskb->len);
 271                                 skb_trim(nskb, 0);
 272                         }
 273                         kfree_skb(skb);
 274                 }
 275         }
 276
 277         cmtp_send_frame(session, nskb->data, nskb->len);
 278
 279         kfree_skb(nskb);
 280 }
 281
 282 static int cmtp_session(void *arg)
 283 {
 284         struct cmtp_session *session = arg;
 285         struct sock *sk = session->sock->sk;
 286         struct sk_buff *skb;
 287         wait_queue_t wait;
 288
 289         BT_DBG("session %p", session);
 290
 291         set_user_nice(current, -15);
 292
 293         init_waitqueue_entry(&wait, current);
 294         add_wait_queue(sk_sleep(sk), &wait);
 295         while (!kthread_should_stop()) {
 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_sleep(sk), &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         int i, err;
 332
 333         BT_DBG("");
 334
 335         session = kzalloc(sizeof(struct cmtp_session), GFP_KERNEL);
 336         if (!session)
 337                 return -ENOMEM;
 338
 339         down_write(&cmtp_session_sem);
 340
 341         s = __cmtp_get_session(&bt_sk(sock->sk)->dst);
 342         if (s && s->state == BT_CONNECTED) {
 343                 err = -EEXIST;
 344                 goto failed;
 345         }
 346
 347         bacpy(&session->bdaddr, &bt_sk(sock->sk)->dst);
 348
 349         session->mtu = min_t(uint, l2cap_pi(sock->sk)->chan->omtu,
 350                                         l2cap_pi(sock->sk)->chan->imtu);
 351
 352         BT_DBG("mtu %d", session->mtu);
 353
 354         sprintf(session->name, "%s", batostr(&bt_sk(sock->sk)->dst));
 355
 356         session->sock  = sock;
 357         session->state = BT_CONFIG;
 358
 359         init_waitqueue_head(&session->wait);
 360
 361         session->msgnum = CMTP_INITIAL_MSGNUM;
 362
 363         INIT_LIST_HEAD(&session->applications);
 364
 365         skb_queue_head_init(&session->transmit);
 366
 367         for (i = 0; i < 16; i++)
 368                 session->reassembly[i] = NULL;
 369
 370         session->flags = req->flags;
 371
 372         __cmtp_link_session(session);
 373
 374         session->task = kthread_run(cmtp_session, session, "kcmtpd_ctr_%d",
 375                                                                 session->num);
 376         if (IS_ERR(session->task)) {
 377                 err = PTR_ERR(session->task);
 378                 goto unlink;
 379         }
 380
 381         if (!(session->flags & (1 << CMTP_LOOPBACK))) {
 382                 err = cmtp_attach_device(session);
 383                 if (err < 0)
 384                         goto detach;
 385         }
 386
 387         up_write(&cmtp_session_sem);
 388         return 0;
 389
 390 detach:
 391         cmtp_detach_device(session);
 392
 393 unlink:
 394         __cmtp_unlink_session(session);
 395
 396 failed:
 397         up_write(&cmtp_session_sem);
 398         kfree(session);
 399         return err;
 400 }
 401
 402 int cmtp_del_connection(struct cmtp_conndel_req *req)
 403 {
 404         struct cmtp_session *session;
 405         int err = 0;
 406
 407         BT_DBG("");
 408
 409         down_read(&cmtp_session_sem);
 410
 411         session = __cmtp_get_session(&req->bdaddr);
 412         if (session) {
 413                 /* Flush the transmit queue */
 414                 skb_queue_purge(&session->transmit);
 415
 416                 /* Stop session thread */
 417                 kthread_stop(session->task);
 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         BT_INFO("CMTP (CAPI Emulation) ver %s", VERSION);
 479
 480         cmtp_init_sockets();
 481
 482         return 0;
 483 }
 484
 485 static void __exit cmtp_exit(void)
 486 {
 487         cmtp_cleanup_sockets();
 488 }
 489
 490 module_init(cmtp_init);
 491 module_exit(cmtp_exit);
 492
 493 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
 494 MODULE_DESCRIPTION("Bluetooth CMTP ver " VERSION);
 495 MODULE_VERSION(VERSION);
 496 MODULE_LICENSE("GPL");
 497 MODULE_ALIAS("bt-proto-5");