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/config.h>
  24 #include <linux/module.h>
  25
  26 #include <linux/types.h>
  27 #include <linux/errno.h>
  28 #include <linux/kernel.h>
  29 #include <linux/major.h>
  30 #include <linux/sched.h>
  31 #include <linux/slab.h>
  32 #include <linux/poll.h>
  33 #include <linux/fcntl.h>
  34 #include <linux/skbuff.h>
  35 #include <linux/socket.h>
  36 #include <linux/ioctl.h>
  37 #include <linux/file.h>
  38 #include <linux/init.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 #ifndef CONFIG_BT_CMTP_DEBUG
  49 #undef  BT_DBG
  50 #define BT_DBG(D...)
  51 #endif
  52
  53 #define VERSION "1.0"
  54
  55 static DECLARE_RWSEM(cmtp_session_sem);
  56 static LIST_HEAD(cmtp_session_list);
  57
  58 static struct cmtp_session *__cmtp_get_session(bdaddr_t *bdaddr)
  59 {
  60         struct cmtp_session *session;
  61         struct list_head *p;
  62
  63         BT_DBG("");
  64
  65         list_for_each(p, &cmtp_session_list) {
  66                 session = list_entry(p, struct cmtp_session, list);
  67                 if (!bacmp(bdaddr, &session->bdaddr))
  68                         return session;
  69         }
  70         return NULL;
  71 }
  72
  73 static void __cmtp_link_session(struct cmtp_session *session)
  74 {
  75         __module_get(THIS_MODULE);
  76         list_add(&session->list, &cmtp_session_list);
  77 }
  78
  79 static void __cmtp_unlink_session(struct cmtp_session *session)
  80 {
  81         list_del(&session->list);
  82         module_put(THIS_MODULE);
  83 }
  84
  85 static void __cmtp_copy_session(struct cmtp_session *session, struct cmtp_conninfo *ci)
  86 {
  87         bacpy(&ci->bdaddr, &session->bdaddr);
  88
  89         ci->flags = session->flags;
  90         ci->state = session->state;
  91
  92         ci->num = session->num;
  93 }
  94
  95
  96 static inline int cmtp_alloc_block_id(struct cmtp_session *session)
  97 {
  98         int i, id = -1;
  99
 100         for (i = 0; i < 16; i++)
 101                 if (!test_and_set_bit(i, &session->blockids)) {
 102                         id = i;
 103                         break;
 104                 }
 105
 106         return id;
 107 }
 108
 109 static inline void cmtp_free_block_id(struct cmtp_session *session, int id)
 110 {
 111         clear_bit(id, &session->blockids);
 112 }
 113
 114 static inline void cmtp_add_msgpart(struct cmtp_session *session, int id, const unsigned char *buf, int count)
 115 {
 116         struct sk_buff *skb = session->reassembly[id], *nskb;
 117         int size;
 118
 119         BT_DBG("session %p buf %p count %d", session, buf, count);
 120
 121         size = (skb) ? skb->len + count : count;
 122
 123         if (!(nskb = alloc_skb(size, GFP_ATOMIC))) {
 124                 BT_ERR("Can't allocate memory for CAPI message");
 125                 return;
 126         }
 127
 128         if (skb && (skb->len > 0))
 129                 memcpy(skb_put(nskb, skb->len), skb->data, skb->len);
 130
 131         memcpy(skb_put(nskb, count), buf, count);
 132
 133         session->reassembly[id] = nskb;
 134
 135         if (skb)
 136                 kfree_skb(skb);
 137 }
 138
 139 static inline int cmtp_recv_frame(struct cmtp_session *session, struct sk_buff *skb)
 140 {
 141         __u8 hdr, hdrlen, id;
 142         __u16 len;
 143
 144         BT_DBG("session %p skb %p len %d", session, skb, skb->len);
 145
 146         while (skb->len > 0) {
 147                 hdr = skb->data[0];
 148
 149                 switch (hdr & 0xc0) {
 150                 case 0x40:
 151                         hdrlen = 2;
 152                         len = skb->data[1];
 153                         break;
 154                 case 0x80:
 155                         hdrlen = 3;
 156                         len = skb->data[1] | (skb->data[2] << 8);
 157                         break;
 158                 default:
 159                         hdrlen = 1;
 160                         len = 0;
 161                         break;
 162                 }
 163
 164                 id = (hdr & 0x3c) >> 2;
 165
 166                 BT_DBG("hdr 0x%02x hdrlen %d len %d id %d", hdr, hdrlen, len, id);
 167
 168                 if (hdrlen + len > skb->len) {
 169                         BT_ERR("Wrong size or header information in CMTP frame");
 170                         break;
 171                 }
 172
 173                 if (len == 0) {
 174                         skb_pull(skb, hdrlen);
 175                         continue;
 176                 }
 177
 178                 switch (hdr & 0x03) {
 179                 case 0x00:
 180                         cmtp_add_msgpart(session, id, skb->data + hdrlen, len);
 181                         cmtp_recv_capimsg(session, session->reassembly[id]);
 182                         session->reassembly[id] = NULL;
 183                         break;
 184                 case 0x01:
 185                         cmtp_add_msgpart(session, id, skb->data + hdrlen, len);
 186                         break;
 187                 default:
 188                         if (session->reassembly[id] != NULL)
 189                                 kfree_skb(session->reassembly[id]);
 190                         session->reassembly[id] = NULL;
 191                         break;
 192                 }
 193
 194                 skb_pull(skb, hdrlen + len);
 195         }
 196
 197         kfree_skb(skb);
 198         return 0;
 199 }
 200
 201 static int cmtp_send_frame(struct cmtp_session *session, unsigned char *data, int len)
 202 {
 203         struct socket *sock = session->sock;
 204         struct iovec iv = { data, len };
 205         struct msghdr msg;
 206
 207         BT_DBG("session %p data %p len %d", session, data, len);
 208
 209         if (!len)
 210                 return 0;
 211
 212         memset(&msg, 0, sizeof(msg));
 213         msg.msg_iovlen = 1;
 214         msg.msg_iov = &iv;
 215
 216         return sock_sendmsg(sock, &msg, len);
 217 }
 218
 219 static int cmtp_process_transmit(struct cmtp_session *session)
 220 {
 221         struct sk_buff *skb, *nskb;
 222         unsigned char *hdr;
 223         unsigned int size, tail;
 224
 225         BT_DBG("session %p", session);
 226
 227         if (!(nskb = alloc_skb(session->mtu, GFP_ATOMIC))) {
 228                 BT_ERR("Can't allocate memory for new frame");
 229                 return -ENOMEM;
 230         }
 231
 232         while ((skb = skb_dequeue(&session->transmit))) {
 233                 struct cmtp_scb *scb = (void *) skb->cb;
 234
 235                 if ((tail = (session->mtu - nskb->len)) < 5) {
 236                         cmtp_send_frame(session, nskb->data, nskb->len);
 237                         skb_trim(nskb, 0);
 238                         tail = session->mtu;
 239                 }
 240
 241                 size = min_t(uint, ((tail < 258) ? (tail - 2) : (tail - 3)), skb->len);
 242
 243                 if ((scb->id < 0) && ((scb->id = cmtp_alloc_block_id(session)) < 0)) {
 244                         skb_queue_head(&session->transmit, skb);
 245                         break;
 246                 }
 247
 248                 if (size < 256) {
 249                         hdr = skb_put(nskb, 2);
 250                         hdr[0] = 0x40
 251                                 | ((scb->id << 2) & 0x3c)
 252                                 | ((skb->len == size) ? 0x00 : 0x01);
 253                         hdr[1] = size;
 254                 } else {
 255                         hdr = skb_put(nskb, 3);
 256                         hdr[0] = 0x80
 257                                 | ((scb->id << 2) & 0x3c)
 258                                 | ((skb->len == size) ? 0x00 : 0x01);
 259                         hdr[1] = size & 0xff;
 260                         hdr[2] = size >> 8;
 261                 }
 262
 263                 memcpy(skb_put(nskb, size), skb->data, size);
 264                 skb_pull(skb, size);
 265
 266                 if (skb->len > 0) {
 267                         skb_queue_head(&session->transmit, skb);
 268                 } else {
 269                         cmtp_free_block_id(session, scb->id);
 270                         if (scb->data) {
 271                                 cmtp_send_frame(session, nskb->data, nskb->len);
 272                                 skb_trim(nskb, 0);
 273                         }
 274                         kfree_skb(skb);
 275                 }
 276         }
 277
 278         cmtp_send_frame(session, nskb->data, nskb->len);
 279
 280         kfree_skb(nskb);
 281
 282         return skb_queue_len(&session->transmit);
 283 }
 284
 285 static int cmtp_session(void *arg)
 286 {
 287         struct cmtp_session *session = arg;
 288         struct sock *sk = session->sock->sk;
 289         struct sk_buff *skb;
 290         wait_queue_t wait;
 291
 292         BT_DBG("session %p", session);
 293
 294         daemonize("kcmtpd_ctr_%d", session->num);
 295         set_user_nice(current, -15);
 296         current->flags |= PF_NOFREEZE;
 297
 298         set_fs(KERNEL_DS);
 299
 300         init_waitqueue_entry(&wait, current);
 301         add_wait_queue(sk->sk_sleep, &wait);
 302         while (!atomic_read(&session->terminate)) {
 303                 set_current_state(TASK_INTERRUPTIBLE);
 304
 305                 if (sk->sk_state != BT_CONNECTED)
 306                         break;
 307
 308                 while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
 309                         skb_orphan(skb);
 310                         cmtp_recv_frame(session, skb);
 311                 }
 312
 313                 cmtp_process_transmit(session);
 314
 315                 schedule();
 316         }
 317         set_current_state(TASK_RUNNING);
 318         remove_wait_queue(sk->sk_sleep, &wait);
 319
 320         down_write(&cmtp_session_sem);
 321
 322         if (!(session->flags & (1 << CMTP_LOOPBACK)))
 323                 cmtp_detach_device(session);
 324
 325         fput(session->sock->file);
 326
 327         __cmtp_unlink_session(session);
 328
 329         up_write(&cmtp_session_sem);
 330
 331         kfree(session);
 332         return 0;
 333 }
 334
 335 int cmtp_add_connection(struct cmtp_connadd_req *req, struct socket *sock)
 336 {
 337         struct cmtp_session *session, *s;
 338         bdaddr_t src, dst;
 339         int i, err;
 340
 341         BT_DBG("");
 342
 343         baswap(&src, &bt_sk(sock->sk)->src);
 344         baswap(&dst, &bt_sk(sock->sk)->dst);
 345
 346         session = kmalloc(sizeof(struct cmtp_session), GFP_KERNEL);
 347         if (!session)
 348                 return -ENOMEM;
 349         memset(session, 0, sizeof(struct cmtp_session));
 350
 351         down_write(&cmtp_session_sem);
 352
 353         s = __cmtp_get_session(&bt_sk(sock->sk)->dst);
 354         if (s && s->state == BT_CONNECTED) {
 355                 err = -EEXIST;
 356                 goto failed;
 357         }
 358
 359         bacpy(&session->bdaddr, &bt_sk(sock->sk)->dst);
 360
 361         session->mtu = min_t(uint, l2cap_pi(sock->sk)->omtu, l2cap_pi(sock->sk)->imtu);
 362
 363         BT_DBG("mtu %d", session->mtu);
 364
 365         sprintf(session->name, "%s", batostr(&dst));
 366
 367         session->sock  = sock;
 368         session->state = BT_CONFIG;
 369
 370         init_waitqueue_head(&session->wait);
 371
 372         session->msgnum = CMTP_INITIAL_MSGNUM;
 373
 374         INIT_LIST_HEAD(&session->applications);
 375
 376         skb_queue_head_init(&session->transmit);
 377
 378         for (i = 0; i < 16; i++)
 379                 session->reassembly[i] = NULL;
 380
 381         session->flags = req->flags;
 382
 383         __cmtp_link_session(session);
 384
 385         err = kernel_thread(cmtp_session, session, CLONE_KERNEL);
 386         if (err < 0)
 387                 goto unlink;
 388
 389         if (!(session->flags & (1 << CMTP_LOOPBACK))) {
 390                 err = cmtp_attach_device(session);
 391                 if (err < 0)
 392                         goto detach;
 393         }
 394
 395         up_write(&cmtp_session_sem);
 396         return 0;
 397
 398 detach:
 399         cmtp_detach_device(session);
 400
 401 unlink:
 402         __cmtp_unlink_session(session);
 403
 404 failed:
 405         up_write(&cmtp_session_sem);
 406         kfree(session);
 407         return err;
 408 }
 409
 410 int cmtp_del_connection(struct cmtp_conndel_req *req)
 411 {
 412         struct cmtp_session *session;
 413         int err = 0;
 414
 415         BT_DBG("");
 416
 417         down_read(&cmtp_session_sem);
 418
 419         session = __cmtp_get_session(&req->bdaddr);
 420         if (session) {
 421                 /* Flush the transmit queue */
 422                 skb_queue_purge(&session->transmit);
 423
 424                 /* Kill session thread */
 425                 atomic_inc(&session->terminate);
 426                 cmtp_schedule(session);
 427         } else
 428                 err = -ENOENT;
 429
 430         up_read(&cmtp_session_sem);
 431         return err;
 432 }
 433
 434 int cmtp_get_connlist(struct cmtp_connlist_req *req)
 435 {
 436         struct list_head *p;
 437         int err = 0, n = 0;
 438
 439         BT_DBG("");
 440
 441         down_read(&cmtp_session_sem);
 442
 443         list_for_each(p, &cmtp_session_list) {
 444                 struct cmtp_session *session;
 445                 struct cmtp_conninfo ci;
 446
 447                 session = list_entry(p, struct cmtp_session, list);
 448
 449                 __cmtp_copy_session(session, &ci);
 450
 451                 if (copy_to_user(req->ci, &ci, sizeof(ci))) {
 452                         err = -EFAULT;
 453                         break;
 454                 }
 455
 456                 if (++n >= req->cnum)
 457                         break;
 458
 459                 req->ci++;
 460         }
 461         req->cnum = n;
 462
 463         up_read(&cmtp_session_sem);
 464         return err;
 465 }
 466
 467 int cmtp_get_conninfo(struct cmtp_conninfo *ci)
 468 {
 469         struct cmtp_session *session;
 470         int err = 0;
 471
 472         down_read(&cmtp_session_sem);
 473
 474         session = __cmtp_get_session(&ci->bdaddr);
 475         if (session)
 476                 __cmtp_copy_session(session, ci);
 477         else
 478                 err = -ENOENT;
 479
 480         up_read(&cmtp_session_sem);
 481         return err;
 482 }
 483
 484
 485 static int __init cmtp_init(void)
 486 {
 487         l2cap_load();
 488
 489         BT_INFO("CMTP (CAPI Emulation) ver %s", VERSION);
 490
 491         cmtp_init_sockets();
 492
 493         return 0;
 494 }
 495
 496 static void __exit cmtp_exit(void)
 497 {
 498         cmtp_cleanup_sockets();
 499 }
 500
 501 module_init(cmtp_init);
 502 module_exit(cmtp_exit);
 503
 504 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
 505 MODULE_DESCRIPTION("Bluetooth CMTP ver " VERSION);
 506 MODULE_VERSION(VERSION);
 507 MODULE_LICENSE("GPL");
 508 MODULE_ALIAS("bt-proto-5");