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 <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#define VERSION "1.0"
48
49static DECLARE_RWSEM(cmtp_session_sem);
50static LIST_HEAD(cmtp_session_list);
51
52static struct cmtp_session *__cmtp_get_session(bdaddr_t *bdaddr)
53{
54 struct cmtp_session *session;
55 struct list_head *p;
56
57 BT_DBG("");
58
59 list_for_each(p, &cmtp_session_list) {
60 session = list_entry(p, struct cmtp_session, list);
61 if (!bacmp(bdaddr, &session->bdaddr))
62 return session;
63 }
64 return NULL;
65}
66
67static void __cmtp_link_session(struct cmtp_session *session)
68{
69 __module_get(THIS_MODULE);
70 list_add(&session->list, &cmtp_session_list);
71}
72
73static void __cmtp_unlink_session(struct cmtp_session *session)
74{
75 list_del(&session->list);
76 module_put(THIS_MODULE);
77}
78
79static void __cmtp_copy_session(struct cmtp_session *session, struct cmtp_conninfo *ci)
80{
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
90static 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
103static inline void cmtp_free_block_id(struct cmtp_session *session, int id)
104{
105 clear_bit(id, &session->blockids);
106}
107
108static 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 if (!(nskb = alloc_skb(size, GFP_ATOMIC))) {
118 BT_ERR("Can't allocate memory for CAPI message");
119 return;
120 }
121
122 if (skb && (skb->len > 0))
123 skb_copy_from_linear_data(skb, skb_put(nskb, skb->len), skb->len);
124
125 memcpy(skb_put(nskb, count), buf, count);
126
127 session->reassembly[id] = nskb;
128
129 kfree_skb(skb);
130}
131
132static inline int cmtp_recv_frame(struct cmtp_session *session, struct sk_buff *skb)
133{
134 __u8 hdr, hdrlen, id;
135 __u16 len;
136
137 BT_DBG("session %p skb %p len %d", session, skb, skb->len);
138
139 while (skb->len > 0) {
140 hdr = skb->data[0];
141
142 switch (hdr & 0xc0) {
143 case 0x40:
144 hdrlen = 2;
145 len = skb->data[1];
146 break;
147 case 0x80:
148 hdrlen = 3;
149 len = skb->data[1] | (skb->data[2] << 8);
150 break;
151 default:
152 hdrlen = 1;
153 len = 0;
154 break;
155 }
156
157 id = (hdr & 0x3c) >> 2;
158
159 BT_DBG("hdr 0x%02x hdrlen %d len %d id %d", hdr, hdrlen, len, id);
160
161 if (hdrlen + len > skb->len) {
162 BT_ERR("Wrong size or header information in CMTP frame");
163 break;
164 }
165
166 if (len == 0) {
167 skb_pull(skb, hdrlen);
168 continue;
169 }
170
171 switch (hdr & 0x03) {
172 case 0x00:
173 cmtp_add_msgpart(session, id, skb->data + hdrlen, len);
174 cmtp_recv_capimsg(session, session->reassembly[id]);
175 session->reassembly[id] = NULL;
176 break;
177 case 0x01:
178 cmtp_add_msgpart(session, id, skb->data + hdrlen, len);
179 break;
180 default:
181 if (session->reassembly[id] != NULL)
182 kfree_skb(session->reassembly[id]);
183 session->reassembly[id] = NULL;
184 break;
185 }
186
187 skb_pull(skb, hdrlen + len);
188 }
189
190 kfree_skb(skb);
191 return 0;
192}
193
194static int cmtp_send_frame(struct cmtp_session *session, unsigned char *data, int len)
195{
196 struct socket *sock = session->sock;
197 struct kvec iv = { data, len };
198 struct msghdr msg;
199
200 BT_DBG("session %p data %p len %d", session, data, len);
201
202 if (!len)
203 return 0;
204
205 memset(&msg, 0, sizeof(msg));
206
207 return kernel_sendmsg(sock, &msg, &iv, 1, len);
208}
209
210static void cmtp_process_transmit(struct cmtp_session *session)
211{
212 struct sk_buff *skb, *nskb;
213 unsigned char *hdr;
214 unsigned int size, tail;
215
216 BT_DBG("session %p", session);
217
218 if (!(nskb = alloc_skb(session->mtu, GFP_ATOMIC))) {
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 if ((tail = (session->mtu - nskb->len)) < 5) {
227 cmtp_send_frame(session, nskb->data, nskb->len);
228 skb_trim(nskb, 0);
229 tail = session->mtu;
230 }
231
232 size = min_t(uint, ((tail < 258) ? (tail - 2) : (tail - 3)), skb->len);
233
234 if ((scb->id < 0) && ((scb->id = cmtp_alloc_block_id(session)) < 0)) {
235 skb_queue_head(&session->transmit, skb);
236 break;
237 }
238
239 if (size < 256) {
240 hdr = skb_put(nskb, 2);
241 hdr[0] = 0x40
242 | ((scb->id << 2) & 0x3c)
243 | ((skb->len == size) ? 0x00 : 0x01);
244 hdr[1] = size;
245 } else {
246 hdr = skb_put(nskb, 3);
247 hdr[0] = 0x80
248 | ((scb->id << 2) & 0x3c)
249 | ((skb->len == size) ? 0x00 : 0x01);
250 hdr[1] = size & 0xff;
251 hdr[2] = size >> 8;
252 }
253
254 skb_copy_from_linear_data(skb, skb_put(nskb, size), size);
255 skb_pull(skb, size);
256
257 if (skb->len > 0) {
258 skb_queue_head(&session->transmit, skb);
259 } else {
260 cmtp_free_block_id(session, scb->id);
261 if (scb->data) {
262 cmtp_send_frame(session, nskb->data, nskb->len);
263 skb_trim(nskb, 0);
264 }
265 kfree_skb(skb);
266 }
267 }
268
269 cmtp_send_frame(session, nskb->data, nskb->len);
270
271 kfree_skb(nskb);
272}
273
274static int cmtp_session(void *arg)
275{
276 struct cmtp_session *session = arg;
277 struct sock *sk = session->sock->sk;
278 struct sk_buff *skb;
279 wait_queue_t wait;
280
281 BT_DBG("session %p", session);
282
283 daemonize("kcmtpd_ctr_%d", session->num);
284 set_user_nice(current, -15);
285
286 init_waitqueue_entry(&wait, current);
287 add_wait_queue(sk->sk_sleep, &wait);
288 while (!atomic_read(&session->terminate)) {
289 set_current_state(TASK_INTERRUPTIBLE);
290
291 if (sk->sk_state != BT_CONNECTED)
292 break;
293
294 while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
295 skb_orphan(skb);
296 cmtp_recv_frame(session, skb);
297 }
298
299 cmtp_process_transmit(session);
300
301 schedule();
302 }
303 set_current_state(TASK_RUNNING);
304 remove_wait_queue(sk->sk_sleep, &wait);
305
306 down_write(&cmtp_session_sem);
307
308 if (!(session->flags & (1 << CMTP_LOOPBACK)))
309 cmtp_detach_device(session);
310
311 fput(session->sock->file);
312
313 __cmtp_unlink_session(session);
314
315 up_write(&cmtp_session_sem);
316
317 kfree(session);
318 return 0;
319}
320
321int cmtp_add_connection(struct cmtp_connadd_req *req, struct socket *sock)
322{
323 struct cmtp_session *session, *s;
324 bdaddr_t src, dst;
325 int i, err;
326
327 BT_DBG("");
328
329 baswap(&src, &bt_sk(sock->sk)->src);
330 baswap(&dst, &bt_sk(sock->sk)->dst);
331
332 session = kzalloc(sizeof(struct cmtp_session), GFP_KERNEL);
333 if (!session)
334 return -ENOMEM;
335
336 down_write(&cmtp_session_sem);
337
338 s = __cmtp_get_session(&bt_sk(sock->sk)->dst);
339 if (s && s->state == BT_CONNECTED) {
340 err = -EEXIST;
341 goto failed;
342 }
343
344 bacpy(&session->bdaddr, &bt_sk(sock->sk)->dst);
345
346 session->mtu = min_t(uint, l2cap_pi(sock->sk)->omtu, l2cap_pi(sock->sk)->imtu);
347
348 BT_DBG("mtu %d", session->mtu);
349
350 sprintf(session->name, "%s", batostr(&dst));
351
352 session->sock = sock;
353 session->state = BT_CONFIG;
354
355 init_waitqueue_head(&session->wait);
356
357 session->msgnum = CMTP_INITIAL_MSGNUM;
358
359 INIT_LIST_HEAD(&session->applications);
360
361 skb_queue_head_init(&session->transmit);
362
363 for (i = 0; i < 16; i++)
364 session->reassembly[i] = NULL;
365
366 session->flags = req->flags;
367
368 __cmtp_link_session(session);
369
370 err = kernel_thread(cmtp_session, session, CLONE_KERNEL);
371 if (err < 0)
372 goto unlink;
373
374 if (!(session->flags & (1 << CMTP_LOOPBACK))) {
375 err = cmtp_attach_device(session);
376 if (err < 0)
377 goto detach;
378 }
379
380 up_write(&cmtp_session_sem);
381 return 0;
382
383detach:
384 cmtp_detach_device(session);
385
386unlink:
387 __cmtp_unlink_session(session);
388
389failed:
390 up_write(&cmtp_session_sem);
391 kfree(session);
392 return err;
393}
394
395int cmtp_del_connection(struct cmtp_conndel_req *req)
396{
397 struct cmtp_session *session;
398 int err = 0;
399
400 BT_DBG("");
401
402 down_read(&cmtp_session_sem);
403
404 session = __cmtp_get_session(&req->bdaddr);
405 if (session) {
406
407 skb_queue_purge(&session->transmit);
408
409
410 atomic_inc(&session->terminate);
411 cmtp_schedule(session);
412 } else
413 err = -ENOENT;
414
415 up_read(&cmtp_session_sem);
416 return err;
417}
418
419int cmtp_get_connlist(struct cmtp_connlist_req *req)
420{
421 struct list_head *p;
422 int err = 0, n = 0;
423
424 BT_DBG("");
425
426 down_read(&cmtp_session_sem);
427
428 list_for_each(p, &cmtp_session_list) {
429 struct cmtp_session *session;
430 struct cmtp_conninfo ci;
431
432 session = list_entry(p, struct cmtp_session, list);
433
434 __cmtp_copy_session(session, &ci);
435
436 if (copy_to_user(req->ci, &ci, sizeof(ci))) {
437 err = -EFAULT;
438 break;
439 }
440
441 if (++n >= req->cnum)
442 break;
443
444 req->ci++;
445 }
446 req->cnum = n;
447
448 up_read(&cmtp_session_sem);
449 return err;
450}
451
452int cmtp_get_conninfo(struct cmtp_conninfo *ci)
453{
454 struct cmtp_session *session;
455 int err = 0;
456
457 down_read(&cmtp_session_sem);
458
459 session = __cmtp_get_session(&ci->bdaddr);
460 if (session)
461 __cmtp_copy_session(session, ci);
462 else
463 err = -ENOENT;
464
465 up_read(&cmtp_session_sem);
466 return err;
467}
468
469
470static int __init cmtp_init(void)
471{
472 l2cap_load();
473
474 BT_INFO("CMTP (CAPI Emulation) ver %s", VERSION);
475
476 cmtp_init_sockets();
477
478 return 0;
479}
480
481static void __exit cmtp_exit(void)
482{
483 cmtp_cleanup_sockets();
484}
485
486module_init(cmtp_init);
487module_exit(cmtp_exit);
488
489MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
490MODULE_DESCRIPTION("Bluetooth CMTP ver " VERSION);
491MODULE_VERSION(VERSION);
492MODULE_LICENSE("GPL");
493MODULE_ALIAS("bt-proto-5");
494
