1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
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
50static DECLARE_RWSEM(cmtp_session_sem);
51static LIST_HEAD(cmtp_session_list);
52
53static 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
66static void __cmtp_link_session(struct cmtp_session *session)
67{
68 list_add(&session->list, &cmtp_session_list);
69}
70
71static void __cmtp_unlink_session(struct cmtp_session *session)
72{
73 list_del(&session->list);
74}
75
76static 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
88static 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
101static inline void cmtp_free_block_id(struct cmtp_session *session, int id)
102{
103 clear_bit(id, &session->blockids);
104}
105
106static 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
131static 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
193static 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
209static 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
278static 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
330int 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, "%pMR", &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 __module_get(THIS_MODULE);
377 session->task = kthread_run(cmtp_session, session, "kcmtpd_ctr_%d",
378 session->num);
379 if (IS_ERR(session->task)) {
380 module_put(THIS_MODULE);
381 err = PTR_ERR(session->task);
382 goto unlink;
383 }
384
385 if (!(session->flags & (1 << CMTP_LOOPBACK))) {
386 err = cmtp_attach_device(session);
387 if (err < 0) {
388 atomic_inc(&session->terminate);
389 wake_up_process(session->task);
390 up_write(&cmtp_session_sem);
391 return err;
392 }
393 }
394
395 up_write(&cmtp_session_sem);
396 return 0;
397
398unlink:
399 __cmtp_unlink_session(session);
400
401failed:
402 up_write(&cmtp_session_sem);
403 kfree(session);
404 return err;
405}
406
407int cmtp_del_connection(struct cmtp_conndel_req *req)
408{
409 struct cmtp_session *session;
410 int err = 0;
411
412 BT_DBG("");
413
414 down_read(&cmtp_session_sem);
415
416 session = __cmtp_get_session(&req->bdaddr);
417 if (session) {
418
419 skb_queue_purge(&session->transmit);
420
421
422 atomic_inc(&session->terminate);
423 wake_up_process(session->task);
424 } else
425 err = -ENOENT;
426
427 up_read(&cmtp_session_sem);
428 return err;
429}
430
431int cmtp_get_connlist(struct cmtp_connlist_req *req)
432{
433 struct cmtp_session *session;
434 int err = 0, n = 0;
435
436 BT_DBG("");
437
438 down_read(&cmtp_session_sem);
439
440 list_for_each_entry(session, &cmtp_session_list, list) {
441 struct cmtp_conninfo ci;
442
443 __cmtp_copy_session(session, &ci);
444
445 if (copy_to_user(req->ci, &ci, sizeof(ci))) {
446 err = -EFAULT;
447 break;
448 }
449
450 if (++n >= req->cnum)
451 break;
452
453 req->ci++;
454 }
455 req->cnum = n;
456
457 up_read(&cmtp_session_sem);
458 return err;
459}
460
461int cmtp_get_conninfo(struct cmtp_conninfo *ci)
462{
463 struct cmtp_session *session;
464 int err = 0;
465
466 down_read(&cmtp_session_sem);
467
468 session = __cmtp_get_session(&ci->bdaddr);
469 if (session)
470 __cmtp_copy_session(session, ci);
471 else
472 err = -ENOENT;
473
474 up_read(&cmtp_session_sem);
475 return err;
476}
477
478
479static int __init cmtp_init(void)
480{
481 BT_INFO("CMTP (CAPI Emulation) ver %s", VERSION);
482
483 cmtp_init_sockets();
484
485 return 0;
486}
487
488static void __exit cmtp_exit(void)
489{
490 cmtp_cleanup_sockets();
491}
492
493module_init(cmtp_init);
494module_exit(cmtp_exit);
495
496MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
497MODULE_DESCRIPTION("Bluetooth CMTP ver " VERSION);
498MODULE_VERSION(VERSION);
499MODULE_LICENSE("GPL");
500MODULE_ALIAS("bt-proto-5");
501
