bluetooth/cmtp/core.c

0001 /*
0002    CMTP implementation for Linux Bluetooth stack (BlueZ).
0003    Copyright (C) 2002-2003 Marcel Holtmann <marcel@holtmann.org>
0004
0005    This program is free software; you can redistribute it and/or modify
0006    it under the terms of the GNU General Public License version 2 as
0007    published by the Free Software Foundation;
0008
0009    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
0010    OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
0011    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
0012    IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
0013    CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
0014    WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
0015    ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
0016    OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
0017
0018    ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
0019    COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
0020    SOFTWARE IS DISCLAIMED.
0021 */
0022
0023 #include <linux/module.h>
0024
0025 #include <linux/types.h>
0026 #include <linux/errno.h>
0027 #include <linux/kernel.h>
0028 #include <linux/sched.h>
0029 #include <linux/slab.h>
0030 #include <linux/poll.h>
0031 #include <linux/fcntl.h>
0032 #include <linux/freezer.h>
0033 #include <linux/skbuff.h>
0034 #include <linux/socket.h>
0035 #include <linux/ioctl.h>
0036 #include <linux/file.h>
0037 #include <linux/init.h>
0038 #include <linux/kthread.h>
0039 #include <net/sock.h>
0040
0041 #include <linux/isdn/capilli.h>
0042
0043 #include <net/bluetooth/bluetooth.h>
0044 #include <net/bluetooth/l2cap.h>
0045
0046 #include "cmtp.h"
0047
0048 #define VERSION "1.0"
0049
0050 static DECLARE_RWSEM(cmtp_session_sem);
0051 static LIST_HEAD(cmtp_session_list);
0052
0053 static struct cmtp_session *__cmtp_get_session(bdaddr_t *bdaddr)
0054 {
0055     struct cmtp_session *session;
0056
0057     BT_DBG("");
0058
0059     list_for_each_entry(session, &cmtp_session_list, list)
0060         if (!bacmp(bdaddr, &session->bdaddr))
0061             return session;
0062
0063     return NULL;
0064 }
0065
0066 static void __cmtp_link_session(struct cmtp_session *session)
0067 {
0068     list_add(&session->list, &cmtp_session_list);
0069 }
0070
0071 static void __cmtp_unlink_session(struct cmtp_session *session)
0072 {
0073     list_del(&session->list);
0074 }
0075
0076 static void __cmtp_copy_session(struct cmtp_session *session, struct cmtp_conninfo *ci)
0077 {
0078     u32 valid_flags = BIT(CMTP_LOOPBACK);
0079     memset(ci, 0, sizeof(*ci));
0080     bacpy(&ci->bdaddr, &session->bdaddr);
0081
0082     ci->flags = session->flags & valid_flags;
0083     ci->state = session->state;
0084
0085     ci->num = session->num;
0086 }
0087
0088
0089 static inline int cmtp_alloc_block_id(struct cmtp_session *session)
0090 {
0091     int i, id = -1;
0092
0093     for (i = 0; i < 16; i++)
0094         if (!test_and_set_bit(i, &session->blockids)) {
0095             id = i;
0096             break;
0097         }
0098
0099     return id;
0100 }
0101
0102 static inline void cmtp_free_block_id(struct cmtp_session *session, int id)
0103 {
0104     clear_bit(id, &session->blockids);
0105 }
0106
0107 static inline void cmtp_add_msgpart(struct cmtp_session *session, int id, const unsigned char *buf, int count)
0108 {
0109     struct sk_buff *skb = session->reassembly[id], *nskb;
0110     int size;
0111
0112     BT_DBG("session %p buf %p count %d", session, buf, count);
0113
0114     size = (skb) ? skb->len + count : count;
0115
0116     nskb = alloc_skb(size, GFP_ATOMIC);
0117     if (!nskb) {
0118         BT_ERR("Can't allocate memory for CAPI message");
0119         return;
0120     }
0121
0122     if (skb && (skb->len > 0))
0123         skb_copy_from_linear_data(skb, skb_put(nskb, skb->len), skb->len);
0124
0125     skb_put_data(nskb, buf, count);
0126
0127     session->reassembly[id] = nskb;
0128
0129     kfree_skb(skb);
0130 }
0131
0132 static inline int cmtp_recv_frame(struct cmtp_session *session, struct sk_buff *skb)
0133 {
0134     __u8 hdr, hdrlen, id;
0135     __u16 len;
0136
0137     BT_DBG("session %p skb %p len %d", session, skb, skb->len);
0138
0139     while (skb->len > 0) {
0140         hdr = skb->data[0];
0141
0142         switch (hdr & 0xc0) {
0143         case 0x40:
0144             hdrlen = 2;
0145             len = skb->data[1];
0146             break;
0147         case 0x80:
0148             hdrlen = 3;
0149             len = skb->data[1] | (skb->data[2] << 8);
0150             break;
0151         default:
0152             hdrlen = 1;
0153             len = 0;
0154             break;
0155         }
0156
0157         id = (hdr & 0x3c) >> 2;
0158
0159         BT_DBG("hdr 0x%02x hdrlen %d len %d id %d", hdr, hdrlen, len, id);
0160
0161         if (hdrlen + len > skb->len) {
0162             BT_ERR("Wrong size or header information in CMTP frame");
0163             break;
0164         }
0165
0166         if (len == 0) {
0167             skb_pull(skb, hdrlen);
0168             continue;
0169         }
0170
0171         switch (hdr & 0x03) {
0172         case 0x00:
0173             cmtp_add_msgpart(session, id, skb->data + hdrlen, len);
0174             cmtp_recv_capimsg(session, session->reassembly[id]);
0175             session->reassembly[id] = NULL;
0176             break;
0177         case 0x01:
0178             cmtp_add_msgpart(session, id, skb->data + hdrlen, len);
0179             break;
0180         default:
0181             kfree_skb(session->reassembly[id]);
0182             session->reassembly[id] = NULL;
0183             break;
0184         }
0185
0186         skb_pull(skb, hdrlen + len);
0187     }
0188
0189     kfree_skb(skb);
0190     return 0;
0191 }
0192
0193 static int cmtp_send_frame(struct cmtp_session *session, unsigned char *data, int len)
0194 {
0195     struct socket *sock = session->sock;
0196     struct kvec iv = { data, len };
0197     struct msghdr msg;
0198
0199     BT_DBG("session %p data %p len %d", session, data, len);
0200
0201     if (!len)
0202         return 0;
0203
0204     memset(&msg, 0, sizeof(msg));
0205
0206     return kernel_sendmsg(sock, &msg, &iv, 1, len);
0207 }
0208
0209 static void cmtp_process_transmit(struct cmtp_session *session)
0210 {
0211     struct sk_buff *skb, *nskb;
0212     unsigned char *hdr;
0213     unsigned int size, tail;
0214
0215     BT_DBG("session %p", session);
0216
0217     nskb = alloc_skb(session->mtu, GFP_ATOMIC);
0218     if (!nskb) {
0219         BT_ERR("Can't allocate memory for new frame");
0220         return;
0221     }
0222
0223     while ((skb = skb_dequeue(&session->transmit))) {
0224         struct cmtp_scb *scb = (void *) skb->cb;
0225
0226         tail = session->mtu - nskb->len;
0227         if (tail < 5) {
0228             cmtp_send_frame(session, nskb->data, nskb->len);
0229             skb_trim(nskb, 0);
0230             tail = session->mtu;
0231         }
0232
0233         size = min_t(uint, ((tail < 258) ? (tail - 2) : (tail - 3)), skb->len);
0234
0235         if (scb->id < 0) {
0236             scb->id = cmtp_alloc_block_id(session);
0237             if (scb->id < 0) {
0238                 skb_queue_head(&session->transmit, skb);
0239                 break;
0240             }
0241         }
0242
0243         if (size < 256) {
0244             hdr = skb_put(nskb, 2);
0245             hdr[0] = 0x40
0246                 | ((scb->id << 2) & 0x3c)
0247                 | ((skb->len == size) ? 0x00 : 0x01);
0248             hdr[1] = size;
0249         } else {
0250             hdr = skb_put(nskb, 3);
0251             hdr[0] = 0x80
0252                 | ((scb->id << 2) & 0x3c)
0253                 | ((skb->len == size) ? 0x00 : 0x01);
0254             hdr[1] = size & 0xff;
0255             hdr[2] = size >> 8;
0256         }
0257
0258         skb_copy_from_linear_data(skb, skb_put(nskb, size), size);
0259         skb_pull(skb, size);
0260
0261         if (skb->len > 0) {
0262             skb_queue_head(&session->transmit, skb);
0263         } else {
0264             cmtp_free_block_id(session, scb->id);
0265             if (scb->data) {
0266                 cmtp_send_frame(session, nskb->data, nskb->len);
0267                 skb_trim(nskb, 0);
0268             }
0269             kfree_skb(skb);
0270         }
0271     }
0272
0273     cmtp_send_frame(session, nskb->data, nskb->len);
0274
0275     kfree_skb(nskb);
0276 }
0277
0278 static int cmtp_session(void *arg)
0279 {
0280     struct cmtp_session *session = arg;
0281     struct sock *sk = session->sock->sk;
0282     struct sk_buff *skb;
0283     DEFINE_WAIT_FUNC(wait, woken_wake_function);
0284
0285     BT_DBG("session %p", session);
0286
0287     set_user_nice(current, -15);
0288
0289     add_wait_queue(sk_sleep(sk), &wait);
0290     while (1) {
0291         if (atomic_read(&session->terminate))
0292             break;
0293         if (sk->sk_state != BT_CONNECTED)
0294             break;
0295
0296         while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
0297             skb_orphan(skb);
0298             if (!skb_linearize(skb))
0299                 cmtp_recv_frame(session, skb);
0300             else
0301                 kfree_skb(skb);
0302         }
0303
0304         cmtp_process_transmit(session);
0305
0306         /*
0307          * wait_woken() performs the necessary memory barriers
0308          * for us; see the header comment for this primitive.
0309          */
0310         wait_woken(&wait, TASK_INTERRUPTIBLE, MAX_SCHEDULE_TIMEOUT);
0311     }
0312     remove_wait_queue(sk_sleep(sk), &wait);
0313
0314     down_write(&cmtp_session_sem);
0315
0316     if (!(session->flags & BIT(CMTP_LOOPBACK)))
0317         cmtp_detach_device(session);
0318
0319     fput(session->sock->file);
0320
0321     __cmtp_unlink_session(session);
0322
0323     up_write(&cmtp_session_sem);
0324
0325     kfree(session);
0326     module_put_and_kthread_exit(0);
0327     return 0;
0328 }
0329
0330 int cmtp_add_connection(struct cmtp_connadd_req *req, struct socket *sock)
0331 {
0332     u32 valid_flags = BIT(CMTP_LOOPBACK);
0333     struct cmtp_session *session, *s;
0334     int i, err;
0335
0336     BT_DBG("");
0337
0338     if (!l2cap_is_socket(sock))
0339         return -EBADFD;
0340
0341     if (req->flags & ~valid_flags)
0342         return -EINVAL;
0343
0344     session = kzalloc(sizeof(struct cmtp_session), GFP_KERNEL);
0345     if (!session)
0346         return -ENOMEM;
0347
0348     down_write(&cmtp_session_sem);
0349
0350     s = __cmtp_get_session(&l2cap_pi(sock->sk)->chan->dst);
0351     if (s && s->state == BT_CONNECTED) {
0352         err = -EEXIST;
0353         goto failed;
0354     }
0355
0356     bacpy(&session->bdaddr, &l2cap_pi(sock->sk)->chan->dst);
0357
0358     session->mtu = min_t(uint, l2cap_pi(sock->sk)->chan->omtu,
0359                     l2cap_pi(sock->sk)->chan->imtu);
0360
0361     BT_DBG("mtu %d", session->mtu);
0362
0363     sprintf(session->name, "%pMR", &session->bdaddr);
0364
0365     session->sock  = sock;
0366     session->state = BT_CONFIG;
0367
0368     init_waitqueue_head(&session->wait);
0369
0370     session->msgnum = CMTP_INITIAL_MSGNUM;
0371
0372     INIT_LIST_HEAD(&session->applications);
0373
0374     skb_queue_head_init(&session->transmit);
0375
0376     for (i = 0; i < 16; i++)
0377         session->reassembly[i] = NULL;
0378
0379     session->flags = req->flags;
0380
0381     __cmtp_link_session(session);
0382
0383     __module_get(THIS_MODULE);
0384     session->task = kthread_run(cmtp_session, session, "kcmtpd_ctr_%d",
0385                                 session->num);
0386     if (IS_ERR(session->task)) {
0387         module_put(THIS_MODULE);
0388         err = PTR_ERR(session->task);
0389         goto unlink;
0390     }
0391
0392     if (!(session->flags & BIT(CMTP_LOOPBACK))) {
0393         err = cmtp_attach_device(session);
0394         if (err < 0) {
0395             /* Caller will call fput in case of failure, and so
0396              * will cmtp_session kthread.
0397              */
0398             get_file(session->sock->file);
0399
0400             atomic_inc(&session->terminate);
0401             wake_up_interruptible(sk_sleep(session->sock->sk));
0402             up_write(&cmtp_session_sem);
0403             return err;
0404         }
0405     }
0406
0407     up_write(&cmtp_session_sem);
0408     return 0;
0409
0410 unlink:
0411     __cmtp_unlink_session(session);
0412
0413 failed:
0414     up_write(&cmtp_session_sem);
0415     kfree(session);
0416     return err;
0417 }
0418
0419 int cmtp_del_connection(struct cmtp_conndel_req *req)
0420 {
0421     u32 valid_flags = 0;
0422     struct cmtp_session *session;
0423     int err = 0;
0424
0425     BT_DBG("");
0426
0427     if (req->flags & ~valid_flags)
0428         return -EINVAL;
0429
0430     down_read(&cmtp_session_sem);
0431
0432     session = __cmtp_get_session(&req->bdaddr);
0433     if (session) {
0434         /* Flush the transmit queue */
0435         skb_queue_purge(&session->transmit);
0436
0437         /* Stop session thread */
0438         atomic_inc(&session->terminate);
0439
0440         /*
0441          * See the comment preceding the call to wait_woken()
0442          * in cmtp_session().
0443          */
0444         wake_up_interruptible(sk_sleep(session->sock->sk));
0445     } else
0446         err = -ENOENT;
0447
0448     up_read(&cmtp_session_sem);
0449     return err;
0450 }
0451
0452 int cmtp_get_connlist(struct cmtp_connlist_req *req)
0453 {
0454     struct cmtp_session *session;
0455     int err = 0, n = 0;
0456
0457     BT_DBG("");
0458
0459     down_read(&cmtp_session_sem);
0460
0461     list_for_each_entry(session, &cmtp_session_list, list) {
0462         struct cmtp_conninfo ci;
0463
0464         __cmtp_copy_session(session, &ci);
0465
0466         if (copy_to_user(req->ci, &ci, sizeof(ci))) {
0467             err = -EFAULT;
0468             break;
0469         }
0470
0471         if (++n >= req->cnum)
0472             break;
0473
0474         req->ci++;
0475     }
0476     req->cnum = n;
0477
0478     up_read(&cmtp_session_sem);
0479     return err;
0480 }
0481
0482 int cmtp_get_conninfo(struct cmtp_conninfo *ci)
0483 {
0484     struct cmtp_session *session;
0485     int err = 0;
0486
0487     down_read(&cmtp_session_sem);
0488
0489     session = __cmtp_get_session(&ci->bdaddr);
0490     if (session)
0491         __cmtp_copy_session(session, ci);
0492     else
0493         err = -ENOENT;
0494
0495     up_read(&cmtp_session_sem);
0496     return err;
0497 }
0498
0499
0500 static int __init cmtp_init(void)
0501 {
0502     BT_INFO("CMTP (CAPI Emulation) ver %s", VERSION);
0503
0504     return cmtp_init_sockets();
0505 }
0506
0507 static void __exit cmtp_exit(void)
0508 {
0509     cmtp_cleanup_sockets();
0510 }
0511
0512 module_init(cmtp_init);
0513 module_exit(cmtp_exit);
0514
0515 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
0516 MODULE_DESCRIPTION("Bluetooth CMTP ver " VERSION);
0517 MODULE_VERSION(VERSION);
0518 MODULE_LICENSE("GPL");
0519 MODULE_ALIAS("bt-proto-5");