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