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 (1) {
 296                 set_current_state(TASK_INTERRUPTIBLE);
 297
 298                 if (atomic_read(&session->terminate))
 299                         break;
 300                 if (sk->sk_state != BT_CONNECTED)
 301                         break;
 302
 303                 while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
 304                         skb_orphan(skb);
 305                         if (!skb_linearize(skb))
 306                                 cmtp_recv_frame(session, skb);
 307                         else
 308                                 kfree_skb(skb);
 309                 }
 310
 311                 cmtp_process_transmit(session);
 312
 313                 schedule();
 314         }
 315         __set_current_state(TASK_RUNNING);
 316         remove_wait_queue(sk_sleep(sk), &wait);
 317
 318         down_write(&cmtp_session_sem);
 319
 320         if (!(session->flags & (1 << CMTP_LOOPBACK)))
 321                 cmtp_detach_device(session);
 322
 323         fput(session->sock->file);
 324
 325         __cmtp_unlink_session(session);
 326
 327         up_write(&cmtp_session_sem);
 328
 329         kfree(session);
 330         return 0;
 331 }
 332
 333 int cmtp_add_connection(struct cmtp_connadd_req *req, struct socket *sock)
 334 {
 335         struct cmtp_session *session, *s;
 336         int i, err;
 337
 338         BT_DBG("");
 339
 340         session = kzalloc(sizeof(struct cmtp_session), GFP_KERNEL);
 341         if (!session)
 342                 return -ENOMEM;
 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)->chan->omtu,
 355                                         l2cap_pi(sock->sk)->chan->imtu);
 356
 357         BT_DBG("mtu %d", session->mtu);
 358
 359         sprintf(session->name, "%s", batostr(&bt_sk(sock->sk)->dst));
 360
 361         session->sock  = sock;
 362         session->state = BT_CONFIG;
 363
 364         init_waitqueue_head(&session->wait);
 365
 366         session->msgnum = CMTP_INITIAL_MSGNUM;
 367
 368         INIT_LIST_HEAD(&session->applications);
 369
 370         skb_queue_head_init(&session->transmit);
 371
 372         for (i = 0; i < 16; i++)
 373                 session->reassembly[i] = NULL;
 374
 375         session->flags = req->flags;
 376
 377         __cmtp_link_session(session);
 378
 379         session->task = kthread_run(cmtp_session, session, "kcmtpd_ctr_%d",
 380                                                                 session->num);
 381         if (IS_ERR(session->task)) {
 382                 err = PTR_ERR(session->task);
 383                 goto unlink;
 384         }
 385
 386         if (!(session->flags & (1 << CMTP_LOOPBACK))) {
 387                 err = cmtp_attach_device(session);
 388                 if (err < 0) {
 389                         atomic_inc(&session->terminate);
 390                         wake_up_process(session->task);
 391                         up_write(&cmtp_session_sem);
 392                         return err;
 393                 }
 394         }
 395
 396         up_write(&cmtp_session_sem);
 397         return 0;
 398
 399 unlink:
 400         __cmtp_unlink_session(session);
 401
 402 failed:
 403         up_write(&cmtp_session_sem);
 404         kfree(session);
 405         return err;
 406 }
 407
 408 int cmtp_del_connection(struct cmtp_conndel_req *req)
 409 {
 410         struct cmtp_session *session;
 411         int err = 0;
 412
 413         BT_DBG("");
 414
 415         down_read(&cmtp_session_sem);
 416
 417         session = __cmtp_get_session(&req->bdaddr);
 418         if (session) {
 419                 /* Flush the transmit queue */
 420                 skb_queue_purge(&session->transmit);
 421
 422                 /* Stop session thread */
 423                 atomic_inc(&session->terminate);
 424                 wake_up_process(session->task);
 425         } else
 426                 err = -ENOENT;
 427
 428         up_read(&cmtp_session_sem);
 429         return err;
 430 }
 431
 432 int cmtp_get_connlist(struct cmtp_connlist_req *req)
 433 {
 434         struct list_head *p;
 435         int err = 0, n = 0;
 436
 437         BT_DBG("");
 438
 439         down_read(&cmtp_session_sem);
 440
 441         list_for_each(p, &cmtp_session_list) {
 442                 struct cmtp_session *session;
 443                 struct cmtp_conninfo ci;
 444
 445                 session = list_entry(p, struct cmtp_session, list);
 446
 447                 __cmtp_copy_session(session, &ci);
 448
 449                 if (copy_to_user(req->ci, &ci, sizeof(ci))) {
 450                         err = -EFAULT;
 451                         break;
 452                 }
 453
 454                 if (++n >= req->cnum)
 455                         break;
 456
 457                 req->ci++;
 458         }
 459         req->cnum = n;
 460
 461         up_read(&cmtp_session_sem);
 462         return err;
 463 }
 464
 465 int cmtp_get_conninfo(struct cmtp_conninfo *ci)
 466 {
 467         struct cmtp_session *session;
 468         int err = 0;
 469
 470         down_read(&cmtp_session_sem);
 471
 472         session = __cmtp_get_session(&ci->bdaddr);
 473         if (session)
 474                 __cmtp_copy_session(session, ci);
 475         else
 476                 err = -ENOENT;
 477
 478         up_read(&cmtp_session_sem);
 479         return err;
 480 }
 481
 482
 483 static int __init cmtp_init(void)
 484 {
 485         BT_INFO("CMTP (CAPI Emulation) ver %s", VERSION);
 486
 487         cmtp_init_sockets();
 488
 489         return 0;
 490 }
 491
 492 static void __exit cmtp_exit(void)
 493 {
 494         cmtp_cleanup_sockets();
 495 }
 496
 497 module_init(cmtp_init);
 498 module_exit(cmtp_exit);
 499
 500 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
 501 MODULE_DESCRIPTION("Bluetooth CMTP ver " VERSION);
 502 MODULE_VERSION(VERSION);
 503 MODULE_LICENSE("GPL");
 504 MODULE_ALIAS("bt-proto-5");