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/sched.h>
  30 #include <linux/slab.h>
  31 #include <linux/poll.h>
  32 #include <linux/fcntl.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 #ifndef CONFIG_BT_CMTP_DEBUG
  48 #undef  BT_DBG
  49 #define BT_DBG(D...)
  50 #endif
  51
  52 #define VERSION "1.0"
  53
  54 static DECLARE_RWSEM(cmtp_session_sem);
  55 static LIST_HEAD(cmtp_session_list);
  56
  57 static struct cmtp_session *__cmtp_get_session(bdaddr_t *bdaddr)
  58 {
  59         struct cmtp_session *session;
  60         struct list_head *p;
  61
  62         BT_DBG("");
  63
  64         list_for_each(p, &cmtp_session_list) {
  65                 session = list_entry(p, struct cmtp_session, list);
  66                 if (!bacmp(bdaddr, &session->bdaddr))
  67                         return session;
  68         }
  69         return NULL;
  70 }
  71
  72 static void __cmtp_link_session(struct cmtp_session *session)
  73 {
  74         __module_get(THIS_MODULE);
  75         list_add(&session->list, &cmtp_session_list);
  76 }
  77
  78 static void __cmtp_unlink_session(struct cmtp_session *session)
  79 {
  80         list_del(&session->list);
  81         module_put(THIS_MODULE);
  82 }
  83
  84 static void __cmtp_copy_session(struct cmtp_session *session, struct cmtp_conninfo *ci)
  85 {
  86         bacpy(&ci->bdaddr, &session->bdaddr);
  87
  88         ci->flags = session->flags;
  89         ci->state = session->state;
  90
  91         ci->num = session->num;
  92 }
  93
  94
  95 static inline int cmtp_alloc_block_id(struct cmtp_session *session)
  96 {
  97         int i, id = -1;
  98
  99         for (i = 0; i < 16; i++)
 100                 if (!test_and_set_bit(i, &session->blockids)) {
 101                         id = i;
 102                         break;
 103                 }
 104
 105         return id;
 106 }
 107
 108 static inline void cmtp_free_block_id(struct cmtp_session *session, int id)
 109 {
 110         clear_bit(id, &session->blockids);
 111 }
 112
 113 static inline void cmtp_add_msgpart(struct cmtp_session *session, int id, const unsigned char *buf, int count)
 114 {
 115         struct sk_buff *skb = session->reassembly[id], *nskb;
 116         int size;
 117
 118         BT_DBG("session %p buf %p count %d", session, buf, count);
 119
 120         size = (skb) ? skb->len + count : count;
 121
 122         if (!(nskb = alloc_skb(size, GFP_ATOMIC))) {
 123                 BT_ERR("Can't allocate memory for CAPI message");
 124                 return;
 125         }
 126
 127         if (skb && (skb->len > 0))
 128                 memcpy(skb_put(nskb, skb->len), skb->data, skb->len);
 129
 130         memcpy(skb_put(nskb, count), buf, count);
 131
 132         session->reassembly[id] = nskb;
 133
 134         if (skb)
 135                 kfree_skb(skb);
 136 }
 137
 138 static inline int cmtp_recv_frame(struct cmtp_session *session, struct sk_buff *skb)
 139 {
 140         __u8 hdr, hdrlen, id;
 141         __u16 len;
 142
 143         BT_DBG("session %p skb %p len %d", session, skb, skb->len);
 144
 145         while (skb->len > 0) {
 146                 hdr = skb->data[0];
 147
 148                 switch (hdr & 0xc0) {
 149                 case 0x40:
 150                         hdrlen = 2;
 151                         len = skb->data[1];
 152                         break;
 153                 case 0x80:
 154                         hdrlen = 3;
 155                         len = skb->data[1] | (skb->data[2] << 8);
 156                         break;
 157                 default:
 158                         hdrlen = 1;
 159                         len = 0;
 160                         break;
 161                 }
 162
 163                 id = (hdr & 0x3c) >> 2;
 164
 165                 BT_DBG("hdr 0x%02x hdrlen %d len %d id %d", hdr, hdrlen, len, id);
 166
 167                 if (hdrlen + len > skb->len) {
 168                         BT_ERR("Wrong size or header information in CMTP frame");
 169                         break;
 170                 }
 171
 172                 if (len == 0) {
 173                         skb_pull(skb, hdrlen);
 174                         continue;
 175                 }
 176
 177                 switch (hdr & 0x03) {
 178                 case 0x00:
 179                         cmtp_add_msgpart(session, id, skb->data + hdrlen, len);
 180                         cmtp_recv_capimsg(session, session->reassembly[id]);
 181                         session->reassembly[id] = NULL;
 182                         break;
 183                 case 0x01:
 184                         cmtp_add_msgpart(session, id, skb->data + hdrlen, len);
 185                         break;
 186                 default:
 187                         if (session->reassembly[id] != NULL)
 188                                 kfree_skb(session->reassembly[id]);
 189                         session->reassembly[id] = NULL;
 190                         break;
 191                 }
 192
 193                 skb_pull(skb, hdrlen + len);
 194         }
 195
 196         kfree_skb(skb);
 197         return 0;
 198 }
 199
 200 static int cmtp_send_frame(struct cmtp_session *session, unsigned char *data, int len)
 201 {
 202         struct socket *sock = session->sock;
 203         struct kvec iv = { data, len };
 204         struct msghdr msg;
 205
 206         BT_DBG("session %p data %p len %d", session, data, len);
 207
 208         if (!len)
 209                 return 0;
 210
 211         memset(&msg, 0, sizeof(msg));
 212
 213         return kernel_sendmsg(sock, &msg, &iv, 1, len);
 214 }
 215
 216 static int cmtp_process_transmit(struct cmtp_session *session)
 217 {
 218         struct sk_buff *skb, *nskb;
 219         unsigned char *hdr;
 220         unsigned int size, tail;
 221
 222         BT_DBG("session %p", session);
 223
 224         if (!(nskb = alloc_skb(session->mtu, GFP_ATOMIC))) {
 225                 BT_ERR("Can't allocate memory for new frame");
 226                 return -ENOMEM;
 227         }
 228
 229         while ((skb = skb_dequeue(&session->transmit))) {
 230                 struct cmtp_scb *scb = (void *) skb->cb;
 231
 232                 if ((tail = (session->mtu - nskb->len)) < 5) {
 233                         cmtp_send_frame(session, nskb->data, nskb->len);
 234                         skb_trim(nskb, 0);
 235                         tail = session->mtu;
 236                 }
 237
 238                 size = min_t(uint, ((tail < 258) ? (tail - 2) : (tail - 3)), skb->len);
 239
 240                 if ((scb->id < 0) && ((scb->id = cmtp_alloc_block_id(session)) < 0)) {
 241                         skb_queue_head(&session->transmit, skb);
 242                         break;
 243                 }
 244
 245                 if (size < 256) {
 246                         hdr = skb_put(nskb, 2);
 247                         hdr[0] = 0x40
 248                                 | ((scb->id << 2) & 0x3c)
 249                                 | ((skb->len == size) ? 0x00 : 0x01);
 250                         hdr[1] = size;
 251                 } else {
 252                         hdr = skb_put(nskb, 3);
 253                         hdr[0] = 0x80
 254                                 | ((scb->id << 2) & 0x3c)
 255                                 | ((skb->len == size) ? 0x00 : 0x01);
 256                         hdr[1] = size & 0xff;
 257                         hdr[2] = size >> 8;
 258                 }
 259
 260                 memcpy(skb_put(nskb, size), skb->data, size);
 261                 skb_pull(skb, size);
 262
 263                 if (skb->len > 0) {
 264                         skb_queue_head(&session->transmit, skb);
 265                 } else {
 266                         cmtp_free_block_id(session, scb->id);
 267                         if (scb->data) {
 268                                 cmtp_send_frame(session, nskb->data, nskb->len);
 269                                 skb_trim(nskb, 0);
 270                         }
 271                         kfree_skb(skb);
 272                 }
 273         }
 274
 275         cmtp_send_frame(session, nskb->data, nskb->len);
 276
 277         kfree_skb(nskb);
 278
 279         return skb_queue_len(&session->transmit);
 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         daemonize("kcmtpd_ctr_%d", session->num);
 292         set_user_nice(current, -15);
 293         current->flags |= PF_NOFREEZE;
 294
 295         init_waitqueue_entry(&wait, current);
 296         add_wait_queue(sk->sk_sleep, &wait);
 297         while (!atomic_read(&session->terminate)) {
 298                 set_current_state(TASK_INTERRUPTIBLE);
 299
 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->sk_sleep, &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         bdaddr_t src, dst;
 334         int i, err;
 335
 336         BT_DBG("");
 337
 338         baswap(&src, &bt_sk(sock->sk)->src);
 339         baswap(&dst, &bt_sk(sock->sk)->dst);
 340
 341         session = kmalloc(sizeof(struct cmtp_session), GFP_KERNEL);
 342         if (!session)
 343                 return -ENOMEM;
 344         memset(session, 0, sizeof(struct cmtp_session));
 345
 346         down_write(&cmtp_session_sem);
 347
 348         s = __cmtp_get_session(&bt_sk(sock->sk)->dst);
 349         if (s && s->state == BT_CONNECTED) {
 350                 err = -EEXIST;
 351                 goto failed;
 352         }
 353
 354         bacpy(&session->bdaddr, &bt_sk(sock->sk)->dst);
 355
 356         session->mtu = min_t(uint, l2cap_pi(sock->sk)->omtu, l2cap_pi(sock->sk)->imtu);
 357
 358         BT_DBG("mtu %d", session->mtu);
 359
 360         sprintf(session->name, "%s", batostr(&dst));
 361
 362         session->sock  = sock;
 363         session->state = BT_CONFIG;
 364
 365         init_waitqueue_head(&session->wait);
 366
 367         session->msgnum = CMTP_INITIAL_MSGNUM;
 368
 369         INIT_LIST_HEAD(&session->applications);
 370
 371         skb_queue_head_init(&session->transmit);
 372
 373         for (i = 0; i < 16; i++)
 374                 session->reassembly[i] = NULL;
 375
 376         session->flags = req->flags;
 377
 378         __cmtp_link_session(session);
 379
 380         err = kernel_thread(cmtp_session, session, CLONE_KERNEL);
 381         if (err < 0)
 382                 goto unlink;
 383
 384         if (!(session->flags & (1 << CMTP_LOOPBACK))) {
 385                 err = cmtp_attach_device(session);
 386                 if (err < 0)
 387                         goto detach;
 388         }
 389
 390         up_write(&cmtp_session_sem);
 391         return 0;
 392
 393 detach:
 394         cmtp_detach_device(session);
 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                 /* Kill session thread */
 420                 atomic_inc(&session->terminate);
 421                 cmtp_schedule(session);
 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         l2cap_load();
 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");