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
  57         BT_DBG("");
  58
  59         list_for_each_entry(session, &cmtp_session_list, list)
  60                 if (!bacmp(bdaddr, &session->bdaddr))
  61                         return session;
  62
  63         return NULL;
  64 }
  65
  66 static void __cmtp_link_session(struct cmtp_session *session)
  67 {
  68         list_add(&session->list, &cmtp_session_list);
  69 }
  70
  71 static void __cmtp_unlink_session(struct cmtp_session *session)
  72 {
  73         list_del(&session->list);
  74 }
  75
  76 static void __cmtp_copy_session(struct cmtp_session *session, struct cmtp_conninfo *ci)
  77 {
  78         memset(ci, 0, sizeof(*ci));
  79         bacpy(&ci->bdaddr, &session->bdaddr);
  80
  81         ci->flags = session->flags;
  82         ci->state = session->state;
  83
  84         ci->num = session->num;
  85 }
  86
  87
  88 static inline int cmtp_alloc_block_id(struct cmtp_session *session)
  89 {
  90         int i, id = -1;
  91
  92         for (i = 0; i < 16; i++)
  93                 if (!test_and_set_bit(i, &session->blockids)) {
  94                         id = i;
  95                         break;
  96                 }
  97
  98         return id;
  99 }
 100
 101 static inline void cmtp_free_block_id(struct cmtp_session *session, int id)
 102 {
 103         clear_bit(id, &session->blockids);
 104 }
 105
 106 static inline void cmtp_add_msgpart(struct cmtp_session *session, int id, const unsigned char *buf, int count)
 107 {
 108         struct sk_buff *skb = session->reassembly[id], *nskb;
 109         int size;
 110
 111         BT_DBG("session %p buf %p count %d", session, buf, count);
 112
 113         size = (skb) ? skb->len + count : count;
 114
 115         nskb = alloc_skb(size, GFP_ATOMIC);
 116         if (!nskb) {
 117                 BT_ERR("Can't allocate memory for CAPI message");
 118                 return;
 119         }
 120
 121         if (skb && (skb->len > 0))
 122                 skb_copy_from_linear_data(skb, skb_put(nskb, skb->len), skb->len);
 123
 124         memcpy(skb_put(nskb, count), buf, count);
 125
 126         session->reassembly[id] = nskb;
 127
 128         kfree_skb(skb);
 129 }
 130
 131 static inline int cmtp_recv_frame(struct cmtp_session *session, struct sk_buff *skb)
 132 {
 133         __u8 hdr, hdrlen, id;
 134         __u16 len;
 135
 136         BT_DBG("session %p skb %p len %d", session, skb, skb->len);
 137
 138         while (skb->len > 0) {
 139                 hdr = skb->data[0];
 140
 141                 switch (hdr & 0xc0) {
 142                 case 0x40:
 143                         hdrlen = 2;
 144                         len = skb->data[1];
 145                         break;
 146                 case 0x80:
 147                         hdrlen = 3;
 148                         len = skb->data[1] | (skb->data[2] << 8);
 149                         break;
 150                 default:
 151                         hdrlen = 1;
 152                         len = 0;
 153                         break;
 154                 }
 155
 156                 id = (hdr & 0x3c) >> 2;
 157
 158                 BT_DBG("hdr 0x%02x hdrlen %d len %d id %d", hdr, hdrlen, len, id);
 159
 160                 if (hdrlen + len > skb->len) {
 161                         BT_ERR("Wrong size or header information in CMTP frame");
 162                         break;
 163                 }
 164
 165                 if (len == 0) {
 166                         skb_pull(skb, hdrlen);
 167                         continue;
 168                 }
 169
 170                 switch (hdr & 0x03) {
 171                 case 0x00:
 172                         cmtp_add_msgpart(session, id, skb->data + hdrlen, len);
 173                         cmtp_recv_capimsg(session, session->reassembly[id]);
 174                         session->reassembly[id] = NULL;
 175                         break;
 176                 case 0x01:
 177                         cmtp_add_msgpart(session, id, skb->data + hdrlen, len);
 178                         break;
 179                 default:
 180                         if (session->reassembly[id] != NULL)
 181                                 kfree_skb(session->reassembly[id]);
 182                         session->reassembly[id] = NULL;
 183                         break;
 184                 }
 185
 186                 skb_pull(skb, hdrlen + len);
 187         }
 188
 189         kfree_skb(skb);
 190         return 0;
 191 }
 192
 193 static int cmtp_send_frame(struct cmtp_session *session, unsigned char *data, int len)
 194 {
 195         struct socket *sock = session->sock;
 196         struct kvec iv = { data, len };
 197         struct msghdr msg;
 198
 199         BT_DBG("session %p data %p len %d", session, data, len);
 200
 201         if (!len)
 202                 return 0;
 203
 204         memset(&msg, 0, sizeof(msg));
 205
 206         return kernel_sendmsg(sock, &msg, &iv, 1, len);
 207 }
 208
 209 static void cmtp_process_transmit(struct cmtp_session *session)
 210 {
 211         struct sk_buff *skb, *nskb;
 212         unsigned char *hdr;
 213         unsigned int size, tail;
 214
 215         BT_DBG("session %p", session);
 216
 217         nskb = alloc_skb(session->mtu, GFP_ATOMIC);
 218         if (!nskb) {
 219                 BT_ERR("Can't allocate memory for new frame");
 220                 return;
 221         }
 222
 223         while ((skb = skb_dequeue(&session->transmit))) {
 224                 struct cmtp_scb *scb = (void *) skb->cb;
 225
 226                 tail = session->mtu - nskb->len;
 227                 if (tail < 5) {
 228                         cmtp_send_frame(session, nskb->data, nskb->len);
 229                         skb_trim(nskb, 0);
 230                         tail = session->mtu;
 231                 }
 232
 233                 size = min_t(uint, ((tail < 258) ? (tail - 2) : (tail - 3)), skb->len);
 234
 235                 if (scb->id < 0) {
 236                         scb->id = cmtp_alloc_block_id(session);
 237                         if (scb->id < 0) {
 238                                 skb_queue_head(&session->transmit, skb);
 239                                 break;
 240                         }
 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         set_user_nice(current, -15);
 288
 289         init_waitqueue_entry(&wait, current);
 290         add_wait_queue(sk_sleep(sk), &wait);
 291         while (1) {
 292                 set_current_state(TASK_INTERRUPTIBLE);
 293
 294                 if (atomic_read(&session->terminate))
 295                         break;
 296                 if (sk->sk_state != BT_CONNECTED)
 297                         break;
 298
 299                 while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
 300                         skb_orphan(skb);
 301                         if (!skb_linearize(skb))
 302                                 cmtp_recv_frame(session, skb);
 303                         else
 304                                 kfree_skb(skb);
 305                 }
 306
 307                 cmtp_process_transmit(session);
 308
 309                 schedule();
 310         }
 311         __set_current_state(TASK_RUNNING);
 312         remove_wait_queue(sk_sleep(sk), &wait);
 313
 314         down_write(&cmtp_session_sem);
 315
 316         if (!(session->flags & (1 << CMTP_LOOPBACK)))
 317                 cmtp_detach_device(session);
 318
 319         fput(session->sock->file);
 320
 321         __cmtp_unlink_session(session);
 322
 323         up_write(&cmtp_session_sem);
 324
 325         kfree(session);
 326         module_put_and_exit(0);
 327         return 0;
 328 }
 329
 330 int cmtp_add_connection(struct cmtp_connadd_req *req, struct socket *sock)
 331 {
 332         struct cmtp_session *session, *s;
 333         int i, err;
 334
 335         BT_DBG("");
 336
 337         if (!l2cap_is_socket(sock))
 338                 return -EBADFD;
 339
 340         session = kzalloc(sizeof(struct cmtp_session), GFP_KERNEL);
 341         if (!session)
 342                 return -ENOMEM;
 343
 344         down_write(&cmtp_session_sem);
 345
 346         s = __cmtp_get_session(&l2cap_pi(sock->sk)->chan->dst);
 347         if (s && s->state == BT_CONNECTED) {
 348                 err = -EEXIST;
 349                 goto failed;
 350         }
 351
 352         bacpy(&session->bdaddr, &l2cap_pi(sock->sk)->chan->dst);
 353
 354         session->mtu = min_t(uint, l2cap_pi(sock->sk)->chan->omtu,
 355                                         l2cap_pi(sock->sk)->chan->imtu);
 356
 357         BT_DBG("mtu %d", session->mtu);
 358
 359         sprintf(session->name, "%pMR", &session->bdaddr);
 360
 361         session->sock  = sock;
 362         session->state = BT_CONFIG;
 363
 364         init_waitqueue_head(&session->wait);
 365
 366         session->msgnum = CMTP_INITIAL_MSGNUM;
 367
 368         INIT_LIST_HEAD(&session->applications);
 369
 370         skb_queue_head_init(&session->transmit);
 371
 372         for (i = 0; i < 16; i++)
 373                 session->reassembly[i] = NULL;
 374
 375         session->flags = req->flags;
 376
 377         __cmtp_link_session(session);
 378
 379         __module_get(THIS_MODULE);
 380         session->task = kthread_run(cmtp_session, session, "kcmtpd_ctr_%d",
 381                                                                 session->num);
 382         if (IS_ERR(session->task)) {
 383                 module_put(THIS_MODULE);
 384                 err = PTR_ERR(session->task);
 385                 goto unlink;
 386         }
 387
 388         if (!(session->flags & (1 << CMTP_LOOPBACK))) {
 389                 err = cmtp_attach_device(session);
 390                 if (err < 0) {
 391                         atomic_inc(&session->terminate);
 392                         wake_up_process(session->task);
 393                         up_write(&cmtp_session_sem);
 394                         return err;
 395                 }
 396         }
 397
 398         up_write(&cmtp_session_sem);
 399         return 0;
 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                 /* Stop session thread */
 425                 atomic_inc(&session->terminate);
 426                 wake_up_process(session->task);
 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 cmtp_session *session;
 437         int err = 0, n = 0;
 438
 439         BT_DBG("");
 440
 441         down_read(&cmtp_session_sem);
 442
 443         list_for_each_entry(session, &cmtp_session_list, list) {
 444                 struct cmtp_conninfo ci;
 445
 446                 __cmtp_copy_session(session, &ci);
 447
 448                 if (copy_to_user(req->ci, &ci, sizeof(ci))) {
 449                         err = -EFAULT;
 450                         break;
 451                 }
 452
 453                 if (++n >= req->cnum)
 454                         break;
 455
 456                 req->ci++;
 457         }
 458         req->cnum = n;
 459
 460         up_read(&cmtp_session_sem);
 461         return err;
 462 }
 463
 464 int cmtp_get_conninfo(struct cmtp_conninfo *ci)
 465 {
 466         struct cmtp_session *session;
 467         int err = 0;
 468
 469         down_read(&cmtp_session_sem);
 470
 471         session = __cmtp_get_session(&ci->bdaddr);
 472         if (session)
 473                 __cmtp_copy_session(session, ci);
 474         else
 475                 err = -ENOENT;
 476
 477         up_read(&cmtp_session_sem);
 478         return err;
 479 }
 480
 481
 482 static int __init cmtp_init(void)
 483 {
 484         BT_INFO("CMTP (CAPI Emulation) ver %s", VERSION);
 485
 486         cmtp_init_sockets();
 487
 488         return 0;
 489 }
 490
 491 static void __exit cmtp_exit(void)
 492 {
 493         cmtp_cleanup_sockets();
 494 }
 495
 496 module_init(cmtp_init);
 497 module_exit(cmtp_exit);
 498
 499 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
 500 MODULE_DESCRIPTION("Bluetooth CMTP ver " VERSION);
 501 MODULE_VERSION(VERSION);
 502 MODULE_LICENSE("GPL");
 503 MODULE_ALIAS("bt-proto-5");