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