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