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