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
  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
  66 static void __cmtp_link_session(struct cmtp_session *session)
  67 {
  68         list_add(&session->list, &cmtp_session_list);
  69 }
  70
  71 static void __cmtp_unlink_session(struct cmtp_session *session)
  72 {
  73         list_del(&session->list);
  74 }
  75
  76 static 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
  89 static 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
 102 static inline void cmtp_free_block_id(struct cmtp_session *session, int id)
 103 {
 104         clear_bit(id, &session->blockids);
 105 }
 106
 107 static 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         memcpy(skb_put(nskb, count), buf, count);
 126
 127         session->reassembly[id] = nskb;
 128
 129         kfree_skb(skb);
 130 }
 131
 132 static 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                         if (session->reassembly[id] != NULL)
 182                                 kfree_skb(session->reassembly[id]);
 183                         session->reassembly[id] = NULL;
 184                         break;
 185                 }
 186
 187                 skb_pull(skb, hdrlen + len);
 188         }
 189
 190         kfree_skb(skb);
 191         return 0;
 192 }
 193
 194 static int cmtp_send_frame(struct cmtp_session *session, unsigned char *data, int len)
 195 {
 196         struct socket *sock = session->sock;
 197         struct kvec iv = { data, len };
 198         struct msghdr msg;
 199
 200         BT_DBG("session %p data %p len %d", session, data, len);
 201
 202         if (!len)
 203                 return 0;
 204
 205         memset(&msg, 0, sizeof(msg));
 206
 207         return kernel_sendmsg(sock, &msg, &iv, 1, len);
 208 }
 209
 210 static void cmtp_process_transmit(struct cmtp_session *session)
 211 {
 212         struct sk_buff *skb, *nskb;
 213         unsigned char *hdr;
 214         unsigned int size, tail;
 215
 216         BT_DBG("session %p", session);
 217
 218         nskb = alloc_skb(session->mtu, GFP_ATOMIC);
 219         if (!nskb) {
 220                 BT_ERR("Can't allocate memory for new frame");
 221                 return;
 222         }
 223
 224         while ((skb = skb_dequeue(&session->transmit))) {
 225                 struct cmtp_scb *scb = (void *) skb->cb;
 226
 227                 tail = session->mtu - nskb->len;
 228                 if (tail < 5) {
 229                         cmtp_send_frame(session, nskb->data, nskb->len);
 230                         skb_trim(nskb, 0);
 231                         tail = session->mtu;
 232                 }
 233
 234                 size = min_t(uint, ((tail < 258) ? (tail - 2) : (tail - 3)), skb->len);
 235
 236                 if (scb->id < 0) {
 237                         scb->id = cmtp_alloc_block_id(session);
 238                         if (scb->id < 0) {
 239                                 skb_queue_head(&session->transmit, skb);
 240                                 break;
 241                         }
 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         DEFINE_WAIT_FUNC(wait, woken_wake_function);
 285
 286         BT_DBG("session %p", session);
 287
 288         set_user_nice(current, -15);
 289
 290         add_wait_queue(sk_sleep(sk), &wait);
 291         while (1) {
 292                 /* Ensure session->terminate is updated */
 293                 smp_mb__before_atomic();
 294
 295                 if (atomic_read(&session->terminate))
 296                         break;
 297                 if (sk->sk_state != BT_CONNECTED)
 298                         break;
 299
 300                 while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
 301                         skb_orphan(skb);
 302                         if (!skb_linearize(skb))
 303                                 cmtp_recv_frame(session, skb);
 304                         else
 305                                 kfree_skb(skb);
 306                 }
 307
 308                 cmtp_process_transmit(session);
 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
 330 int 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                         /* Caller will call fput in case of failure, and so
 396                          * will cmtp_session kthread.
 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
 410 unlink:
 411         __cmtp_unlink_session(session);
 412
 413 failed:
 414         up_write(&cmtp_session_sem);
 415         kfree(session);
 416         return err;
 417 }
 418
 419 int 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                 /* Flush the transmit queue */
 435                 skb_queue_purge(&session->transmit);
 436
 437                 /* Stop session thread */
 438                 atomic_inc(&session->terminate);
 439
 440                 /* Ensure session->terminate is updated */
 441                 smp_mb__after_atomic();
 442
 443                 wake_up_interruptible(sk_sleep(session->sock->sk));
 444         } else
 445                 err = -ENOENT;
 446
 447         up_read(&cmtp_session_sem);
 448         return err;
 449 }
 450
 451 int cmtp_get_connlist(struct cmtp_connlist_req *req)
 452 {
 453         struct cmtp_session *session;
 454         int err = 0, n = 0;
 455
 456         BT_DBG("");
 457
 458         down_read(&cmtp_session_sem);
 459
 460         list_for_each_entry(session, &cmtp_session_list, list) {
 461                 struct cmtp_conninfo ci;
 462
 463                 __cmtp_copy_session(session, &ci);
 464
 465                 if (copy_to_user(req->ci, &ci, sizeof(ci))) {
 466                         err = -EFAULT;
 467                         break;
 468                 }
 469
 470                 if (++n >= req->cnum)
 471                         break;
 472
 473                 req->ci++;
 474         }
 475         req->cnum = n;
 476
 477         up_read(&cmtp_session_sem);
 478         return err;
 479 }
 480
 481 int cmtp_get_conninfo(struct cmtp_conninfo *ci)
 482 {
 483         struct cmtp_session *session;
 484         int err = 0;
 485
 486         down_read(&cmtp_session_sem);
 487
 488         session = __cmtp_get_session(&ci->bdaddr);
 489         if (session)
 490                 __cmtp_copy_session(session, ci);
 491         else
 492                 err = -ENOENT;
 493
 494         up_read(&cmtp_session_sem);
 495         return err;
 496 }
 497
 498
 499 static int __init cmtp_init(void)
 500 {
 501         BT_INFO("CMTP (CAPI Emulation) ver %s", VERSION);
 502
 503         cmtp_init_sockets();
 504
 505         return 0;
 506 }
 507
 508 static void __exit cmtp_exit(void)
 509 {
 510         cmtp_cleanup_sockets();
 511 }
 512
 513 module_init(cmtp_init);
 514 module_exit(cmtp_exit);
 515
 516 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
 517 MODULE_DESCRIPTION("Bluetooth CMTP ver " VERSION);
 518 MODULE_VERSION(VERSION);
 519 MODULE_LICENSE("GPL");
 520 MODULE_ALIAS("bt-proto-5");