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