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