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 memset(ci, 0, sizeof(*ci));
82 bacpy(&ci->bdaddr, &session->bdaddr);
83
84 ci->flags = session->flags;
85 ci->state = session->state;
86
87 ci->num = session->num;
88}
89
90
91static inline int cmtp_alloc_block_id(struct cmtp_session *session)
92{
93 int i, id = -1;
94
95 for (i = 0; i < 16; i++)
96 if (!test_and_set_bit(i, &session->blockids)) {
97 id = i;
98 break;
99 }
100
101 return id;
102}
103
104static inline void cmtp_free_block_id(struct cmtp_session *session, int id)
105{
106 clear_bit(id, &session->blockids);
107}
108
109static inline void cmtp_add_msgpart(struct cmtp_session *session, int id, const unsigned char *buf, int count)
110{
111 struct sk_buff *skb = session->reassembly[id], *nskb;
112 int size;
113
114 BT_DBG("session %p buf %p count %d", session, buf, count);
115
116 size = (skb) ? skb->len + count : count;
117
118 nskb = alloc_skb(size, GFP_ATOMIC);
119 if (!nskb) {
120 BT_ERR("Can't allocate memory for CAPI message");
121 return;
122 }
123
124 if (skb && (skb->len > 0))
125 skb_copy_from_linear_data(skb, skb_put(nskb, skb->len), skb->len);
126
127 memcpy(skb_put(nskb, count), buf, count);
128
129 session->reassembly[id] = nskb;
130
131 kfree_skb(skb);
132}
133
134static inline int cmtp_recv_frame(struct cmtp_session *session, struct sk_buff *skb)
135{
136 __u8 hdr, hdrlen, id;
137 __u16 len;
138
139 BT_DBG("session %p skb %p len %d", session, skb, skb->len);
140
141 while (skb->len > 0) {
142 hdr = skb->data[0];
143
144 switch (hdr & 0xc0) {
145 case 0x40:
146 hdrlen = 2;
147 len = skb->data[1];
148 break;
149 case 0x80:
150 hdrlen = 3;
151 len = skb->data[1] | (skb->data[2] << 8);
152 break;
153 default:
154 hdrlen = 1;
155 len = 0;
156 break;
157 }
158
159 id = (hdr & 0x3c) >> 2;
160
161 BT_DBG("hdr 0x%02x hdrlen %d len %d id %d", hdr, hdrlen, len, id);
162
163 if (hdrlen + len > skb->len) {
164 BT_ERR("Wrong size or header information in CMTP frame");
165 break;
166 }
167
168 if (len == 0) {
169 skb_pull(skb, hdrlen);
170 continue;
171 }
172
173 switch (hdr & 0x03) {
174 case 0x00:
175 cmtp_add_msgpart(session, id, skb->data + hdrlen, len);
176 cmtp_recv_capimsg(session, session->reassembly[id]);
177 session->reassembly[id] = NULL;
178 break;
179 case 0x01:
180 cmtp_add_msgpart(session, id, skb->data + hdrlen, len);
181 break;
182 default:
183 if (session->reassembly[id] != NULL)
184 kfree_skb(session->reassembly[id]);
185 session->reassembly[id] = NULL;
186 break;
187 }
188
189 skb_pull(skb, hdrlen + len);
190 }
191
192 kfree_skb(skb);
193 return 0;
194}
195
196static int cmtp_send_frame(struct cmtp_session *session, unsigned char *data, int len)
197{
198 struct socket *sock = session->sock;
199 struct kvec iv = { data, len };
200 struct msghdr msg;
201
202 BT_DBG("session %p data %p len %d", session, data, len);
203
204 if (!len)
205 return 0;
206
207 memset(&msg, 0, sizeof(msg));
208
209 return kernel_sendmsg(sock, &msg, &iv, 1, len);
210}
211
212static void cmtp_process_transmit(struct cmtp_session *session)
213{
214 struct sk_buff *skb, *nskb;
215 unsigned char *hdr;
216 unsigned int size, tail;
217
218 BT_DBG("session %p", session);
219
220 nskb = alloc_skb(session->mtu, GFP_ATOMIC);
221 if (!nskb) {
222 BT_ERR("Can't allocate memory for new frame");
223 return;
224 }
225
226 while ((skb = skb_dequeue(&session->transmit))) {
227 struct cmtp_scb *scb = (void *) skb->cb;
228
229 tail = session->mtu - nskb->len;
230 if (tail < 5) {
231 cmtp_send_frame(session, nskb->data, nskb->len);
232 skb_trim(nskb, 0);
233 tail = session->mtu;
234 }
235
236 size = min_t(uint, ((tail < 258) ? (tail - 2) : (tail - 3)), skb->len);
237
238 if ((scb->id < 0) && ((scb->id = cmtp_alloc_block_id(session)) < 0)) {
239 skb_queue_head(&session->transmit, skb);
240 break;
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 daemonize("kcmtpd_ctr_%d", session->num);
288 set_user_nice(current, -15);
289
290 init_waitqueue_entry(&wait, current);
291 add_wait_queue(sk_sleep(sk), &wait);
292 while (!atomic_read(&session->terminate)) {
293 set_current_state(TASK_INTERRUPTIBLE);
294
295 if (sk->sk_state != BT_CONNECTED)
296 break;
297
298 while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
299 skb_orphan(skb);
300 cmtp_recv_frame(session, skb);
301 }
302
303 cmtp_process_transmit(session);
304
305 schedule();
306 }
307 set_current_state(TASK_RUNNING);
308 remove_wait_queue(sk_sleep(sk), &wait);
309
310 down_write(&cmtp_session_sem);
311
312 if (!(session->flags & (1 << CMTP_LOOPBACK)))
313 cmtp_detach_device(session);
314
315 fput(session->sock->file);
316
317 __cmtp_unlink_session(session);
318
319 up_write(&cmtp_session_sem);
320
321 kfree(session);
322 return 0;
323}
324
325int cmtp_add_connection(struct cmtp_connadd_req *req, struct socket *sock)
326{
327 struct cmtp_session *session, *s;
328 int i, err;
329
330 BT_DBG("");
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(&bt_sk(sock->sk)->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 BT_INFO("CMTP (CAPI Emulation) ver %s", VERSION);
473
474 cmtp_init_sockets();
475
476 return 0;
477}
478
479static void __exit cmtp_exit(void)
480{
481 cmtp_cleanup_sockets();
482}
483
484module_init(cmtp_init);
485module_exit(cmtp_exit);
486
487MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
488MODULE_DESCRIPTION("Bluetooth CMTP ver " VERSION);
489MODULE_VERSION(VERSION);
490MODULE_LICENSE("GPL");
491MODULE_ALIAS("bt-proto-5");
492
