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 kvec 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
 214         return kernel_sendmsg(sock, &msg, &iv, 1, len);
 215 }
 216
 217 static int cmtp_process_transmit(struct cmtp_session *session)
 218 {
 219         struct sk_buff *skb, *nskb;
 220         unsigned char *hdr;
 221         unsigned int size, tail;
 222
 223         BT_DBG("session %p", session);
 224
 225         if (!(nskb = alloc_skb(session->mtu, GFP_ATOMIC))) {
 226                 BT_ERR("Can't allocate memory for new frame");
 227                 return -ENOMEM;
 228         }
 229
 230         while ((skb = skb_dequeue(&session->transmit))) {
 231                 struct cmtp_scb *scb = (void *) skb->cb;
 232
 233                 if ((tail = (session->mtu - nskb->len)) < 5) {
 234                         cmtp_send_frame(session, nskb->data, nskb->len);
 235                         skb_trim(nskb, 0);
 236                         tail = session->mtu;
 237                 }
 238
 239                 size = min_t(uint, ((tail < 258) ? (tail - 2) : (tail - 3)), skb->len);
 240
 241                 if ((scb->id < 0) && ((scb->id = cmtp_alloc_block_id(session)) < 0)) {
 242                         skb_queue_head(&session->transmit, skb);
 243                         break;
 244                 }
 245
 246                 if (size < 256) {
 247                         hdr = skb_put(nskb, 2);
 248                         hdr[0] = 0x40
 249                                 | ((scb->id << 2) & 0x3c)
 250                                 | ((skb->len == size) ? 0x00 : 0x01);
 251                         hdr[1] = size;
 252                 } else {
 253                         hdr = skb_put(nskb, 3);
 254                         hdr[0] = 0x80
 255                                 | ((scb->id << 2) & 0x3c)
 256                                 | ((skb->len == size) ? 0x00 : 0x01);
 257                         hdr[1] = size & 0xff;
 258                         hdr[2] = size >> 8;
 259                 }
 260
 261                 memcpy(skb_put(nskb, size), skb->data, size);
 262                 skb_pull(skb, size);
 263
 264                 if (skb->len > 0) {
 265                         skb_queue_head(&session->transmit, skb);
 266                 } else {
 267                         cmtp_free_block_id(session, scb->id);
 268                         if (scb->data) {
 269                                 cmtp_send_frame(session, nskb->data, nskb->len);
 270                                 skb_trim(nskb, 0);
 271                         }
 272                         kfree_skb(skb);
 273                 }
 274         }
 275
 276         cmtp_send_frame(session, nskb->data, nskb->len);
 277
 278         kfree_skb(nskb);
 279
 280         return skb_queue_len(&session->transmit);
 281 }
 282
 283 static int cmtp_session(void *arg)
 284 {
 285         struct cmtp_session *session = arg;
 286         struct sock *sk = session->sock->sk;
 287         struct sk_buff *skb;
 288         wait_queue_t wait;
 289
 290         BT_DBG("session %p", session);
 291
 292         daemonize("kcmtpd_ctr_%d", session->num);
 293         set_user_nice(current, -15);
 294         current->flags |= PF_NOFREEZE;
 295
 296         init_waitqueue_entry(&wait, current);
 297         add_wait_queue(sk->sk_sleep, &wait);
 298         while (!atomic_read(&session->terminate)) {
 299                 set_current_state(TASK_INTERRUPTIBLE);
 300
 301                 if (sk->sk_state != BT_CONNECTED)
 302                         break;
 303
 304                 while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
 305                         skb_orphan(skb);
 306                         cmtp_recv_frame(session, skb);
 307                 }
 308
 309                 cmtp_process_transmit(session);
 310
 311                 schedule();
 312         }
 313         set_current_state(TASK_RUNNING);
 314         remove_wait_queue(sk->sk_sleep, &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         bdaddr_t src, dst;
 335         int i, err;
 336
 337         BT_DBG("");
 338
 339         baswap(&src, &bt_sk(sock->sk)->src);
 340         baswap(&dst, &bt_sk(sock->sk)->dst);
 341
 342         session = kmalloc(sizeof(struct cmtp_session), GFP_KERNEL);
 343         if (!session)
 344                 return -ENOMEM;
 345         memset(session, 0, sizeof(struct cmtp_session));
 346
 347         down_write(&cmtp_session_sem);
 348
 349         s = __cmtp_get_session(&bt_sk(sock->sk)->dst);
 350         if (s && s->state == BT_CONNECTED) {
 351                 err = -EEXIST;
 352                 goto failed;
 353         }
 354
 355         bacpy(&session->bdaddr, &bt_sk(sock->sk)->dst);
 356
 357         session->mtu = min_t(uint, l2cap_pi(sock->sk)->omtu, l2cap_pi(sock->sk)->imtu);
 358
 359         BT_DBG("mtu %d", session->mtu);
 360
 361         sprintf(session->name, "%s", batostr(&dst));
 362
 363         session->sock  = sock;
 364         session->state = BT_CONFIG;
 365
 366         init_waitqueue_head(&session->wait);
 367
 368         session->msgnum = CMTP_INITIAL_MSGNUM;
 369
 370         INIT_LIST_HEAD(&session->applications);
 371
 372         skb_queue_head_init(&session->transmit);
 373
 374         for (i = 0; i < 16; i++)
 375                 session->reassembly[i] = NULL;
 376
 377         session->flags = req->flags;
 378
 379         __cmtp_link_session(session);
 380
 381         err = kernel_thread(cmtp_session, session, CLONE_KERNEL);
 382         if (err < 0)
 383                 goto unlink;
 384
 385         if (!(session->flags & (1 << CMTP_LOOPBACK))) {
 386                 err = cmtp_attach_device(session);
 387                 if (err < 0)
 388                         goto detach;
 389         }
 390
 391         up_write(&cmtp_session_sem);
 392         return 0;
 393
 394 detach:
 395         cmtp_detach_device(session);
 396
 397 unlink:
 398         __cmtp_unlink_session(session);
 399
 400 failed:
 401         up_write(&cmtp_session_sem);
 402         kfree(session);
 403         return err;
 404 }
 405
 406 int cmtp_del_connection(struct cmtp_conndel_req *req)
 407 {
 408         struct cmtp_session *session;
 409         int err = 0;
 410
 411         BT_DBG("");
 412
 413         down_read(&cmtp_session_sem);
 414
 415         session = __cmtp_get_session(&req->bdaddr);
 416         if (session) {
 417                 /* Flush the transmit queue */
 418                 skb_queue_purge(&session->transmit);
 419
 420                 /* Kill session thread */
 421                 atomic_inc(&session->terminate);
 422                 cmtp_schedule(session);
 423         } else
 424                 err = -ENOENT;
 425
 426         up_read(&cmtp_session_sem);
 427         return err;
 428 }
 429
 430 int cmtp_get_connlist(struct cmtp_connlist_req *req)
 431 {
 432         struct list_head *p;
 433         int err = 0, n = 0;
 434
 435         BT_DBG("");
 436
 437         down_read(&cmtp_session_sem);
 438
 439         list_for_each(p, &cmtp_session_list) {
 440                 struct cmtp_session *session;
 441                 struct cmtp_conninfo ci;
 442
 443                 session = list_entry(p, struct cmtp_session, list);
 444
 445                 __cmtp_copy_session(session, &ci);
 446
 447                 if (copy_to_user(req->ci, &ci, sizeof(ci))) {
 448                         err = -EFAULT;
 449                         break;
 450                 }
 451
 452                 if (++n >= req->cnum)
 453                         break;
 454
 455                 req->ci++;
 456         }
 457         req->cnum = n;
 458
 459         up_read(&cmtp_session_sem);
 460         return err;
 461 }
 462
 463 int cmtp_get_conninfo(struct cmtp_conninfo *ci)
 464 {
 465         struct cmtp_session *session;
 466         int err = 0;
 467
 468         down_read(&cmtp_session_sem);
 469
 470         session = __cmtp_get_session(&ci->bdaddr);
 471         if (session)
 472                 __cmtp_copy_session(session, ci);
 473         else
 474                 err = -ENOENT;
 475
 476         up_read(&cmtp_session_sem);
 477         return err;
 478 }
 479
 480
 481 static int __init cmtp_init(void)
 482 {
 483         l2cap_load();
 484
 485         BT_INFO("CMTP (CAPI Emulation) ver %s", VERSION);
 486
 487         cmtp_init_sockets();
 488
 489         return 0;
 490 }
 491
 492 static void __exit cmtp_exit(void)
 493 {
 494         cmtp_cleanup_sockets();
 495 }
 496
 497 module_init(cmtp_init);
 498 module_exit(cmtp_exit);
 499
 500 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
 501 MODULE_DESCRIPTION("Bluetooth CMTP ver " VERSION);
 502 MODULE_VERSION(VERSION);
 503 MODULE_LICENSE("GPL");
 504 MODULE_ALIAS("bt-proto-5");