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                         atomic_inc(&session->terminate);
 396                         wake_up_interruptible(sk_sleep(session->sock->sk));
 397                         up_write(&cmtp_session_sem);
 398                         return err;
 399                 }
 400         }
 401
 402         up_write(&cmtp_session_sem);
 403         return 0;
 404
 405 unlink:
 406         __cmtp_unlink_session(session);
 407
 408 failed:
 409         up_write(&cmtp_session_sem);
 410         kfree(session);
 411         return err;
 412 }
 413
 414 int cmtp_del_connection(struct cmtp_conndel_req *req)
 415 {
 416         u32 valid_flags = 0;
 417         struct cmtp_session *session;
 418         int err = 0;
 419
 420         BT_DBG("");
 421
 422         if (req->flags & ~valid_flags)
 423                 return -EINVAL;
 424
 425         down_read(&cmtp_session_sem);
 426
 427         session = __cmtp_get_session(&req->bdaddr);
 428         if (session) {
 429                 /* Flush the transmit queue */
 430                 skb_queue_purge(&session->transmit);
 431
 432                 /* Stop session thread */
 433                 atomic_inc(&session->terminate);
 434
 435                 /* Ensure session->terminate is updated */
 436                 smp_mb__after_atomic();
 437
 438                 wake_up_interruptible(sk_sleep(session->sock->sk));
 439         } else
 440                 err = -ENOENT;
 441
 442         up_read(&cmtp_session_sem);
 443         return err;
 444 }
 445
 446 int cmtp_get_connlist(struct cmtp_connlist_req *req)
 447 {
 448         struct cmtp_session *session;
 449         int err = 0, n = 0;
 450
 451         BT_DBG("");
 452
 453         down_read(&cmtp_session_sem);
 454
 455         list_for_each_entry(session, &cmtp_session_list, list) {
 456                 struct cmtp_conninfo ci;
 457
 458                 __cmtp_copy_session(session, &ci);
 459
 460                 if (copy_to_user(req->ci, &ci, sizeof(ci))) {
 461                         err = -EFAULT;
 462                         break;
 463                 }
 464
 465                 if (++n >= req->cnum)
 466                         break;
 467
 468                 req->ci++;
 469         }
 470         req->cnum = n;
 471
 472         up_read(&cmtp_session_sem);
 473         return err;
 474 }
 475
 476 int cmtp_get_conninfo(struct cmtp_conninfo *ci)
 477 {
 478         struct cmtp_session *session;
 479         int err = 0;
 480
 481         down_read(&cmtp_session_sem);
 482
 483         session = __cmtp_get_session(&ci->bdaddr);
 484         if (session)
 485                 __cmtp_copy_session(session, ci);
 486         else
 487                 err = -ENOENT;
 488
 489         up_read(&cmtp_session_sem);
 490         return err;
 491 }
 492
 493
 494 static int __init cmtp_init(void)
 495 {
 496         BT_INFO("CMTP (CAPI Emulation) ver %s", VERSION);
 497
 498         cmtp_init_sockets();
 499
 500         return 0;
 501 }
 502
 503 static void __exit cmtp_exit(void)
 504 {
 505         cmtp_cleanup_sockets();
 506 }
 507
 508 module_init(cmtp_init);
 509 module_exit(cmtp_exit);
 510
 511 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
 512 MODULE_DESCRIPTION("Bluetooth CMTP ver " VERSION);
 513 MODULE_VERSION(VERSION);
 514 MODULE_LICENSE("GPL");
 515 MODULE_ALIAS("bt-proto-5");