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         skb_put_data(nskb, 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                         kfree_skb(session->reassembly[id]);
 182                         session->reassembly[id] = NULL;
 183                         break;
 184                 }
 185
 186                 skb_pull(skb, hdrlen + len);
 187         }
 188
 189         kfree_skb(skb);
 190         return 0;
 191 }
 192
 193 static int cmtp_send_frame(struct cmtp_session *session, unsigned char *data, int len)
 194 {
 195         struct socket *sock = session->sock;
 196         struct kvec iv = { data, len };
 197         struct msghdr msg;
 198
 199         BT_DBG("session %p data %p len %d", session, data, len);
 200
 201         if (!len)
 202                 return 0;
 203
 204         memset(&msg, 0, sizeof(msg));
 205
 206         return kernel_sendmsg(sock, &msg, &iv, 1, len);
 207 }
 208
 209 static void cmtp_process_transmit(struct cmtp_session *session)
 210 {
 211         struct sk_buff *skb, *nskb;
 212         unsigned char *hdr;
 213         unsigned int size, tail;
 214
 215         BT_DBG("session %p", session);
 216
 217         nskb = alloc_skb(session->mtu, GFP_ATOMIC);
 218         if (!nskb) {
 219                 BT_ERR("Can't allocate memory for new frame");
 220                 return;
 221         }
 222
 223         while ((skb = skb_dequeue(&session->transmit))) {
 224                 struct cmtp_scb *scb = (void *) skb->cb;
 225
 226                 tail = session->mtu - nskb->len;
 227                 if (tail < 5) {
 228                         cmtp_send_frame(session, nskb->data, nskb->len);
 229                         skb_trim(nskb, 0);
 230                         tail = session->mtu;
 231                 }
 232
 233                 size = min_t(uint, ((tail < 258) ? (tail - 2) : (tail - 3)), skb->len);
 234
 235                 if (scb->id < 0) {
 236                         scb->id = cmtp_alloc_block_id(session);
 237                         if (scb->id < 0) {
 238                                 skb_queue_head(&session->transmit, skb);
 239                                 break;
 240                         }
 241                 }
 242
 243                 if (size < 256) {
 244                         hdr = skb_put(nskb, 2);
 245                         hdr[0] = 0x40
 246                                 | ((scb->id << 2) & 0x3c)
 247                                 | ((skb->len == size) ? 0x00 : 0x01);
 248                         hdr[1] = size;
 249                 } else {
 250                         hdr = skb_put(nskb, 3);
 251                         hdr[0] = 0x80
 252                                 | ((scb->id << 2) & 0x3c)
 253                                 | ((skb->len == size) ? 0x00 : 0x01);
 254                         hdr[1] = size & 0xff;
 255                         hdr[2] = size >> 8;
 256                 }
 257
 258                 skb_copy_from_linear_data(skb, skb_put(nskb, size), size);
 259                 skb_pull(skb, size);
 260
 261                 if (skb->len > 0) {
 262                         skb_queue_head(&session->transmit, skb);
 263                 } else {
 264                         cmtp_free_block_id(session, scb->id);
 265                         if (scb->data) {
 266                                 cmtp_send_frame(session, nskb->data, nskb->len);
 267                                 skb_trim(nskb, 0);
 268                         }
 269                         kfree_skb(skb);
 270                 }
 271         }
 272
 273         cmtp_send_frame(session, nskb->data, nskb->len);
 274
 275         kfree_skb(nskb);
 276 }
 277
 278 static int cmtp_session(void *arg)
 279 {
 280         struct cmtp_session *session = arg;
 281         struct sock *sk = session->sock->sk;
 282         struct sk_buff *skb;
 283         DEFINE_WAIT_FUNC(wait, woken_wake_function);
 284
 285         BT_DBG("session %p", session);
 286
 287         set_user_nice(current, -15);
 288
 289         add_wait_queue(sk_sleep(sk), &wait);
 290         while (1) {
 291                 /* Ensure session->terminate is updated */
 292                 smp_mb__before_atomic();
 293
 294                 if (atomic_read(&session->terminate))
 295                         break;
 296                 if (sk->sk_state != BT_CONNECTED)
 297                         break;
 298
 299                 while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
 300                         skb_orphan(skb);
 301                         if (!skb_linearize(skb))
 302                                 cmtp_recv_frame(session, skb);
 303                         else
 304                                 kfree_skb(skb);
 305                 }
 306
 307                 cmtp_process_transmit(session);
 308
 309                 wait_woken(&wait, TASK_INTERRUPTIBLE, MAX_SCHEDULE_TIMEOUT);
 310         }
 311         remove_wait_queue(sk_sleep(sk), &wait);
 312
 313         down_write(&cmtp_session_sem);
 314
 315         if (!(session->flags & BIT(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         module_put_and_exit(0);
 326         return 0;
 327 }
 328
 329 int cmtp_add_connection(struct cmtp_connadd_req *req, struct socket *sock)
 330 {
 331         u32 valid_flags = BIT(CMTP_LOOPBACK);
 332         struct cmtp_session *session, *s;
 333         int i, err;
 334
 335         BT_DBG("");
 336
 337         if (!l2cap_is_socket(sock))
 338                 return -EBADFD;
 339
 340         if (req->flags & ~valid_flags)
 341                 return -EINVAL;
 342
 343         session = kzalloc(sizeof(struct cmtp_session), GFP_KERNEL);
 344         if (!session)
 345                 return -ENOMEM;
 346
 347         down_write(&cmtp_session_sem);
 348
 349         s = __cmtp_get_session(&l2cap_pi(sock->sk)->chan->dst);
 350         if (s && s->state == BT_CONNECTED) {
 351                 err = -EEXIST;
 352                 goto failed;
 353         }
 354
 355         bacpy(&session->bdaddr, &l2cap_pi(sock->sk)->chan->dst);
 356
 357         session->mtu = min_t(uint, l2cap_pi(sock->sk)->chan->omtu,
 358                                         l2cap_pi(sock->sk)->chan->imtu);
 359
 360         BT_DBG("mtu %d", session->mtu);
 361
 362         sprintf(session->name, "%pMR", &session->bdaddr);
 363
 364         session->sock  = sock;
 365         session->state = BT_CONFIG;
 366
 367         init_waitqueue_head(&session->wait);
 368
 369         session->msgnum = CMTP_INITIAL_MSGNUM;
 370
 371         INIT_LIST_HEAD(&session->applications);
 372
 373         skb_queue_head_init(&session->transmit);
 374
 375         for (i = 0; i < 16; i++)
 376                 session->reassembly[i] = NULL;
 377
 378         session->flags = req->flags;
 379
 380         __cmtp_link_session(session);
 381
 382         __module_get(THIS_MODULE);
 383         session->task = kthread_run(cmtp_session, session, "kcmtpd_ctr_%d",
 384                                                                 session->num);
 385         if (IS_ERR(session->task)) {
 386                 module_put(THIS_MODULE);
 387                 err = PTR_ERR(session->task);
 388                 goto unlink;
 389         }
 390
 391         if (!(session->flags & BIT(CMTP_LOOPBACK))) {
 392                 err = cmtp_attach_device(session);
 393                 if (err < 0) {
 394                         atomic_inc(&session->terminate);
 395                         wake_up_interruptible(sk_sleep(session->sock->sk));
 396                         up_write(&cmtp_session_sem);
 397                         return err;
 398                 }
 399         }
 400
 401         up_write(&cmtp_session_sem);
 402         return 0;
 403
 404 unlink:
 405         __cmtp_unlink_session(session);
 406
 407 failed:
 408         up_write(&cmtp_session_sem);
 409         kfree(session);
 410         return err;
 411 }
 412
 413 int cmtp_del_connection(struct cmtp_conndel_req *req)
 414 {
 415         u32 valid_flags = 0;
 416         struct cmtp_session *session;
 417         int err = 0;
 418
 419         BT_DBG("");
 420
 421         if (req->flags & ~valid_flags)
 422                 return -EINVAL;
 423
 424         down_read(&cmtp_session_sem);
 425
 426         session = __cmtp_get_session(&req->bdaddr);
 427         if (session) {
 428                 /* Flush the transmit queue */
 429                 skb_queue_purge(&session->transmit);
 430
 431                 /* Stop session thread */
 432                 atomic_inc(&session->terminate);
 433
 434                 /* Ensure session->terminate is updated */
 435                 smp_mb__after_atomic();
 436
 437                 wake_up_interruptible(sk_sleep(session->sock->sk));
 438         } else
 439                 err = -ENOENT;
 440
 441         up_read(&cmtp_session_sem);
 442         return err;
 443 }
 444
 445 int cmtp_get_connlist(struct cmtp_connlist_req *req)
 446 {
 447         struct cmtp_session *session;
 448         int err = 0, n = 0;
 449
 450         BT_DBG("");
 451
 452         down_read(&cmtp_session_sem);
 453
 454         list_for_each_entry(session, &cmtp_session_list, list) {
 455                 struct cmtp_conninfo ci;
 456
 457                 __cmtp_copy_session(session, &ci);
 458
 459                 if (copy_to_user(req->ci, &ci, sizeof(ci))) {
 460                         err = -EFAULT;
 461                         break;
 462                 }
 463
 464                 if (++n >= req->cnum)
 465                         break;
 466
 467                 req->ci++;
 468         }
 469         req->cnum = n;
 470
 471         up_read(&cmtp_session_sem);
 472         return err;
 473 }
 474
 475 int cmtp_get_conninfo(struct cmtp_conninfo *ci)
 476 {
 477         struct cmtp_session *session;
 478         int err = 0;
 479
 480         down_read(&cmtp_session_sem);
 481
 482         session = __cmtp_get_session(&ci->bdaddr);
 483         if (session)
 484                 __cmtp_copy_session(session, ci);
 485         else
 486                 err = -ENOENT;
 487
 488         up_read(&cmtp_session_sem);
 489         return err;
 490 }
 491
 492
 493 static int __init cmtp_init(void)
 494 {
 495         BT_INFO("CMTP (CAPI Emulation) ver %s", VERSION);
 496
 497         cmtp_init_sockets();
 498
 499         return 0;
 500 }
 501
 502 static void __exit cmtp_exit(void)
 503 {
 504         cmtp_cleanup_sockets();
 505 }
 506
 507 module_init(cmtp_init);
 508 module_exit(cmtp_exit);
 509
 510 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
 511 MODULE_DESCRIPTION("Bluetooth CMTP ver " VERSION);
 512 MODULE_VERSION(VERSION);
 513 MODULE_LICENSE("GPL");
 514 MODULE_ALIAS("bt-proto-5");