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/skbuff.h>
  33 #include <linux/socket.h>
  34 #include <linux/ioctl.h>
  35 #include <linux/file.h>
  36 #include <linux/init.h>
  37 #include <net/sock.h>
  38
  39 #include <linux/isdn/capilli.h>
  40
  41 #include <net/bluetooth/bluetooth.h>
  42 #include <net/bluetooth/l2cap.h>
  43
  44 #include "cmtp.h"
  45
  46 #ifndef CONFIG_BT_CMTP_DEBUG
  47 #undef  BT_DBG
  48 #define BT_DBG(D...)
  49 #endif
  50
  51 #define VERSION "1.0"
  52
  53 static DECLARE_RWSEM(cmtp_session_sem);
  54 static LIST_HEAD(cmtp_session_list);
  55
  56 static struct cmtp_session *__cmtp_get_session(bdaddr_t *bdaddr)
  57 {
  58         struct cmtp_session *session;
  59         struct list_head *p;
  60
  61         BT_DBG("");
  62
  63         list_for_each(p, &cmtp_session_list) {
  64                 session = list_entry(p, struct cmtp_session, list);
  65                 if (!bacmp(bdaddr, &session->bdaddr))
  66                         return session;
  67         }
  68         return NULL;
  69 }
  70
  71 static void __cmtp_link_session(struct cmtp_session *session)
  72 {
  73         __module_get(THIS_MODULE);
  74         list_add(&session->list, &cmtp_session_list);
  75 }
  76
  77 static void __cmtp_unlink_session(struct cmtp_session *session)
  78 {
  79         list_del(&session->list);
  80         module_put(THIS_MODULE);
  81 }
  82
  83 static void __cmtp_copy_session(struct cmtp_session *session, struct cmtp_conninfo *ci)
  84 {
  85         bacpy(&ci->bdaddr, &session->bdaddr);
  86
  87         ci->flags = session->flags;
  88         ci->state = session->state;
  89
  90         ci->num = session->num;
  91 }
  92
  93
  94 static inline int cmtp_alloc_block_id(struct cmtp_session *session)
  95 {
  96         int i, id = -1;
  97
  98         for (i = 0; i < 16; i++)
  99                 if (!test_and_set_bit(i, &session->blockids)) {
 100                         id = i;
 101                         break;
 102                 }
 103
 104         return id;
 105 }
 106
 107 static inline void cmtp_free_block_id(struct cmtp_session *session, int id)
 108 {
 109         clear_bit(id, &session->blockids);
 110 }
 111
 112 static inline void cmtp_add_msgpart(struct cmtp_session *session, int id, const unsigned char *buf, int count)
 113 {
 114         struct sk_buff *skb = session->reassembly[id], *nskb;
 115         int size;
 116
 117         BT_DBG("session %p buf %p count %d", session, buf, count);
 118
 119         size = (skb) ? skb->len + count : count;
 120
 121         if (!(nskb = alloc_skb(size, GFP_ATOMIC))) {
 122                 BT_ERR("Can't allocate memory for CAPI message");
 123                 return;
 124         }
 125
 126         if (skb && (skb->len > 0))
 127                 memcpy(skb_put(nskb, skb->len), skb->data, skb->len);
 128
 129         memcpy(skb_put(nskb, count), buf, count);
 130
 131         session->reassembly[id] = nskb;
 132
 133         if (skb)
 134                 kfree_skb(skb);
 135 }
 136
 137 static inline int cmtp_recv_frame(struct cmtp_session *session, struct sk_buff *skb)
 138 {
 139         __u8 hdr, hdrlen, id;
 140         __u16 len;
 141
 142         BT_DBG("session %p skb %p len %d", session, skb, skb->len);
 143
 144         while (skb->len > 0) {
 145                 hdr = skb->data[0];
 146
 147                 switch (hdr & 0xc0) {
 148                 case 0x40:
 149                         hdrlen = 2;
 150                         len = skb->data[1];
 151                         break;
 152                 case 0x80:
 153                         hdrlen = 3;
 154                         len = skb->data[1] | (skb->data[2] << 8);
 155                         break;
 156                 default:
 157                         hdrlen = 1;
 158                         len = 0;
 159                         break;
 160                 }
 161
 162                 id = (hdr & 0x3c) >> 2;
 163
 164                 BT_DBG("hdr 0x%02x hdrlen %d len %d id %d", hdr, hdrlen, len, id);
 165
 166                 if (hdrlen + len > skb->len) {
 167                         BT_ERR("Wrong size or header information in CMTP frame");
 168                         break;
 169                 }
 170
 171                 if (len == 0) {
 172                         skb_pull(skb, hdrlen);
 173                         continue;
 174                 }
 175
 176                 switch (hdr & 0x03) {
 177                 case 0x00:
 178                         cmtp_add_msgpart(session, id, skb->data + hdrlen, len);
 179                         cmtp_recv_capimsg(session, session->reassembly[id]);
 180                         session->reassembly[id] = NULL;
 181                         break;
 182                 case 0x01:
 183                         cmtp_add_msgpart(session, id, skb->data + hdrlen, len);
 184                         break;
 185                 default:
 186                         if (session->reassembly[id] != NULL)
 187                                 kfree_skb(session->reassembly[id]);
 188                         session->reassembly[id] = NULL;
 189                         break;
 190                 }
 191
 192                 skb_pull(skb, hdrlen + len);
 193         }
 194
 195         kfree_skb(skb);
 196         return 0;
 197 }
 198
 199 static int cmtp_send_frame(struct cmtp_session *session, unsigned char *data, int len)
 200 {
 201         struct socket *sock = session->sock;
 202         struct kvec iv = { data, len };
 203         struct msghdr msg;
 204
 205         BT_DBG("session %p data %p len %d", session, data, len);
 206
 207         if (!len)
 208                 return 0;
 209
 210         memset(&msg, 0, sizeof(msg));
 211
 212         return kernel_sendmsg(sock, &msg, &iv, 1, len);
 213 }
 214
 215 static void cmtp_process_transmit(struct cmtp_session *session)
 216 {
 217         struct sk_buff *skb, *nskb;
 218         unsigned char *hdr;
 219         unsigned int size, tail;
 220
 221         BT_DBG("session %p", session);
 222
 223         if (!(nskb = alloc_skb(session->mtu, GFP_ATOMIC))) {
 224                 BT_ERR("Can't allocate memory for new frame");
 225                 return;
 226         }
 227
 228         while ((skb = skb_dequeue(&session->transmit))) {
 229                 struct cmtp_scb *scb = (void *) skb->cb;
 230
 231                 if ((tail = (session->mtu - nskb->len)) < 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) && ((scb->id = cmtp_alloc_block_id(session)) < 0)) {
 240                         skb_queue_head(&session->transmit, skb);
 241                         break;
 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                 memcpy(skb_put(nskb, size), skb->data, 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         daemonize("kcmtpd_ctr_%d", session->num);
 289         set_user_nice(current, -15);
 290         current->flags |= PF_NOFREEZE;
 291
 292         init_waitqueue_entry(&wait, current);
 293         add_wait_queue(sk->sk_sleep, &wait);
 294         while (!atomic_read(&session->terminate)) {
 295                 set_current_state(TASK_INTERRUPTIBLE);
 296
 297                 if (sk->sk_state != BT_CONNECTED)
 298                         break;
 299
 300                 while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
 301                         skb_orphan(skb);
 302                         cmtp_recv_frame(session, skb);
 303                 }
 304
 305                 cmtp_process_transmit(session);
 306
 307                 schedule();
 308         }
 309         set_current_state(TASK_RUNNING);
 310         remove_wait_queue(sk->sk_sleep, &wait);
 311
 312         down_write(&cmtp_session_sem);
 313
 314         if (!(session->flags & (1 << CMTP_LOOPBACK)))
 315                 cmtp_detach_device(session);
 316
 317         fput(session->sock->file);
 318
 319         __cmtp_unlink_session(session);
 320
 321         up_write(&cmtp_session_sem);
 322
 323         kfree(session);
 324         return 0;
 325 }
 326
 327 int cmtp_add_connection(struct cmtp_connadd_req *req, struct socket *sock)
 328 {
 329         struct cmtp_session *session, *s;
 330         bdaddr_t src, dst;
 331         int i, err;
 332
 333         BT_DBG("");
 334
 335         baswap(&src, &bt_sk(sock->sk)->src);
 336         baswap(&dst, &bt_sk(sock->sk)->dst);
 337
 338         session = kmalloc(sizeof(struct cmtp_session), GFP_KERNEL);
 339         if (!session)
 340                 return -ENOMEM;
 341         memset(session, 0, sizeof(struct cmtp_session));
 342
 343         down_write(&cmtp_session_sem);
 344
 345         s = __cmtp_get_session(&bt_sk(sock->sk)->dst);
 346         if (s && s->state == BT_CONNECTED) {
 347                 err = -EEXIST;
 348                 goto failed;
 349         }
 350
 351         bacpy(&session->bdaddr, &bt_sk(sock->sk)->dst);
 352
 353         session->mtu = min_t(uint, l2cap_pi(sock->sk)->omtu, l2cap_pi(sock->sk)->imtu);
 354
 355         BT_DBG("mtu %d", session->mtu);
 356
 357         sprintf(session->name, "%s", batostr(&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         err = kernel_thread(cmtp_session, session, CLONE_KERNEL);
 378         if (err < 0)
 379                 goto unlink;
 380
 381         if (!(session->flags & (1 << CMTP_LOOPBACK))) {
 382                 err = cmtp_attach_device(session);
 383                 if (err < 0)
 384                         goto detach;
 385         }
 386
 387         up_write(&cmtp_session_sem);
 388         return 0;
 389
 390 detach:
 391         cmtp_detach_device(session);
 392
 393 unlink:
 394         __cmtp_unlink_session(session);
 395
 396 failed:
 397         up_write(&cmtp_session_sem);
 398         kfree(session);
 399         return err;
 400 }
 401
 402 int cmtp_del_connection(struct cmtp_conndel_req *req)
 403 {
 404         struct cmtp_session *session;
 405         int err = 0;
 406
 407         BT_DBG("");
 408
 409         down_read(&cmtp_session_sem);
 410
 411         session = __cmtp_get_session(&req->bdaddr);
 412         if (session) {
 413                 /* Flush the transmit queue */
 414                 skb_queue_purge(&session->transmit);
 415
 416                 /* Kill session thread */
 417                 atomic_inc(&session->terminate);
 418                 cmtp_schedule(session);
 419         } else
 420                 err = -ENOENT;
 421
 422         up_read(&cmtp_session_sem);
 423         return err;
 424 }
 425
 426 int cmtp_get_connlist(struct cmtp_connlist_req *req)
 427 {
 428         struct list_head *p;
 429         int err = 0, n = 0;
 430
 431         BT_DBG("");
 432
 433         down_read(&cmtp_session_sem);
 434
 435         list_for_each(p, &cmtp_session_list) {
 436                 struct cmtp_session *session;
 437                 struct cmtp_conninfo ci;
 438
 439                 session = list_entry(p, struct cmtp_session, list);
 440
 441                 __cmtp_copy_session(session, &ci);
 442
 443                 if (copy_to_user(req->ci, &ci, sizeof(ci))) {
 444                         err = -EFAULT;
 445                         break;
 446                 }
 447
 448                 if (++n >= req->cnum)
 449                         break;
 450
 451                 req->ci++;
 452         }
 453         req->cnum = n;
 454
 455         up_read(&cmtp_session_sem);
 456         return err;
 457 }
 458
 459 int cmtp_get_conninfo(struct cmtp_conninfo *ci)
 460 {
 461         struct cmtp_session *session;
 462         int err = 0;
 463
 464         down_read(&cmtp_session_sem);
 465
 466         session = __cmtp_get_session(&ci->bdaddr);
 467         if (session)
 468                 __cmtp_copy_session(session, ci);
 469         else
 470                 err = -ENOENT;
 471
 472         up_read(&cmtp_session_sem);
 473         return err;
 474 }
 475
 476
 477 static int __init cmtp_init(void)
 478 {
 479         l2cap_load();
 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");