1 /* $OpenBSD: src/sys/netbt/l2cap_signal.c,v 1.3 2008/02/24 21:34:48 uwe Exp $ */
2 /* $NetBSD: l2cap_signal.c,v 1.9 2007/11/10 23:12:23 plunky Exp $ */
3
4 /*-
5 * Copyright (c) 2005 Iain Hibbert.
6 * Copyright (c) 2006 Itronix Inc.
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 * 1. Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * 2. Redistributions in binary form must reproduce the above copyright
15 * notice, this list of conditions and the following disclaimer in the
16 * documentation and/or other materials provided with the distribution.
17 * 3. The name of Itronix Inc. may not be used to endorse
18 * or promote products derived from this software without specific
19 * prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY ITRONIX INC. ``AS IS'' AND
22 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
23 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
24 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL ITRONIX INC. BE LIABLE FOR ANY
25 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
26 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
28 * ON ANY THEORY OF LIABILITY, WHETHER IN
29 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 * POSSIBILITY OF SUCH DAMAGE.
32 */
33
34 #include <sys/param.h>
35 #include <sys/kernel.h>
36 #include <sys/malloc.h> /* for M_NOWAIT */
37 #include <sys/mbuf.h>
38 #include <sys/proc.h>
39 #include <sys/queue.h>
40 #include <sys/systm.h>
41 #include <sys/endian.h>
42
43 #include <netbt/bluetooth.h>
44 #include <netbt/hci.h>
45 #include <netbt/l2cap.h>
46
47 /*******************************************************************************
48 *
49 * L2CAP Signal processing
50 */
51
52 static void l2cap_recv_command_rej(struct mbuf *, struct hci_link *);
53 static void l2cap_recv_connect_req(struct mbuf *, struct hci_link *);
54 static void l2cap_recv_connect_rsp(struct mbuf *, struct hci_link *);
55 static void l2cap_recv_config_req(struct mbuf *, struct hci_link *);
56 static void l2cap_recv_config_rsp(struct mbuf *, struct hci_link *);
57 static void l2cap_recv_disconnect_req(struct mbuf *, struct hci_link *);
58 static void l2cap_recv_disconnect_rsp(struct mbuf *, struct hci_link *);
59 static void l2cap_recv_info_req(struct mbuf *, struct hci_link *);
60 static int l2cap_send_signal(struct hci_link *, uint8_t, uint8_t, uint16_t, void *);
61 static int l2cap_send_command_rej(struct hci_link *, uint8_t, uint16_t, ...);
62
63 /*
64 * process incoming signal packets (CID 0x0001). Can contain multiple
65 * requests/responses.
66 */
67 void
l2cap_recv_signal(struct mbuf * m,struct hci_link * link)68 l2cap_recv_signal(struct mbuf *m, struct hci_link *link)
69 {
70 l2cap_cmd_hdr_t cmd;
71
72 for(;;) {
73 if (m->m_pkthdr.len == 0)
74 goto finish;
75
76 if (m->m_pkthdr.len < sizeof(cmd))
77 goto reject;
78
79 m_copydata(m, 0, sizeof(cmd), &cmd);
80 cmd.length = letoh16(cmd.length);
81
82 if (m->m_pkthdr.len < sizeof(cmd) + cmd.length)
83 goto reject;
84
85 DPRINTFN(2, "(%s) code %d, ident %d, len %d\n",
86 device_get_nameunit(link->hl_unit->hci_dev),
87 cmd.code, cmd.ident, cmd.length);
88
89 switch (cmd.code) {
90 case L2CAP_COMMAND_REJ:
91 if (cmd.length > sizeof(l2cap_cmd_rej_cp))
92 goto finish;
93
94 l2cap_recv_command_rej(m, link);
95 break;
96
97 case L2CAP_CONNECT_REQ:
98 if (cmd.length != sizeof(l2cap_con_req_cp))
99 goto reject;
100
101 l2cap_recv_connect_req(m, link);
102 break;
103
104 case L2CAP_CONNECT_RSP:
105 if (cmd.length != sizeof(l2cap_con_rsp_cp))
106 goto finish;
107
108 l2cap_recv_connect_rsp(m, link);
109 break;
110
111 case L2CAP_CONFIG_REQ:
112 l2cap_recv_config_req(m, link);
113 break;
114
115 case L2CAP_CONFIG_RSP:
116 l2cap_recv_config_rsp(m, link);
117 break;
118
119 case L2CAP_DISCONNECT_REQ:
120 if (cmd.length != sizeof(l2cap_discon_req_cp))
121 goto reject;
122
123 l2cap_recv_disconnect_req(m, link);
124 break;
125
126 case L2CAP_DISCONNECT_RSP:
127 if (cmd.length != sizeof(l2cap_discon_rsp_cp))
128 goto finish;
129
130 l2cap_recv_disconnect_rsp(m, link);
131 break;
132
133 case L2CAP_ECHO_REQ:
134 m_adj(m, sizeof(cmd) + cmd.length);
135 l2cap_send_signal(link, L2CAP_ECHO_RSP, cmd.ident,
136 0, NULL);
137 break;
138
139 case L2CAP_ECHO_RSP:
140 m_adj(m, sizeof(cmd) + cmd.length);
141 break;
142
143 case L2CAP_INFO_REQ:
144 if (cmd.length != sizeof(l2cap_info_req_cp))
145 goto reject;
146
147 l2cap_recv_info_req(m, link);
148 break;
149
150 case L2CAP_INFO_RSP:
151 m_adj(m, sizeof(cmd) + cmd.length);
152 break;
153
154 default:
155 goto reject;
156 }
157 }
158
159 #ifdef DIAGNOSTIC
160 panic("impossible!");
161 #endif
162
163 reject:
164 l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_NOT_UNDERSTOOD);
165 finish:
166 m_freem(m);
167 }
168
169 /*
170 * Process Received Command Reject. For now we dont try to recover gracefully
171 * from this, it probably means that the link is garbled or the other end is
172 * insufficiently capable of handling normal traffic. (not *my* fault, no way!)
173 */
174 static void
l2cap_recv_command_rej(struct mbuf * m,struct hci_link * link)175 l2cap_recv_command_rej(struct mbuf *m, struct hci_link *link)
176 {
177 struct l2cap_req *req;
178 struct l2cap_channel *chan;
179 l2cap_cmd_hdr_t cmd;
180 l2cap_cmd_rej_cp cp;
181
182 m_copydata(m, 0, sizeof(cmd), &cmd);
183 m_adj(m, sizeof(cmd));
184
185 cmd.length = letoh16(cmd.length);
186
187 m_copydata(m, 0, cmd.length, &cp);
188 m_adj(m, cmd.length);
189
190 req = l2cap_request_lookup(link, cmd.ident);
191 if (req == NULL)
192 return;
193
194 switch (letoh16(cp.reason)) {
195 case L2CAP_REJ_NOT_UNDERSTOOD:
196 /*
197 * I dont know what to do, just move up the timeout
198 */
199 callout_reset(&req->lr_rtx,0,l2cap_rtx,req);
200 break;
201
202 case L2CAP_REJ_MTU_EXCEEDED:
203 /*
204 * I didnt send any commands over L2CAP_MTU_MINIMUM size, but..
205 *
206 * XXX maybe we should resend this, instead?
207 */
208 link->hl_mtu = letoh16(cp.data[0]);
209 callout_reset(&req->lr_rtx,0,l2cap_rtx,req);
210 break;
211
212 case L2CAP_REJ_INVALID_CID:
213 /*
214 * Well, if they dont have such a channel then our channel is
215 * most likely closed. Make it so.
216 */
217 chan = req->lr_chan;
218 l2cap_request_free(req);
219 if (chan != NULL && chan->lc_state != L2CAP_CLOSED)
220 l2cap_close(chan, ECONNABORTED);
221
222 break;
223
224 default:
225 UNKNOWN(letoh16(cp.reason));
226 break;
227 }
228 }
229
230 /*
231 * Process Received Connect Request. Find listening channel matching
232 * psm & addr and ask upper layer for a new channel.
233 */
234 static void
l2cap_recv_connect_req(struct mbuf * m,struct hci_link * link)235 l2cap_recv_connect_req(struct mbuf *m, struct hci_link *link)
236 {
237 struct sockaddr_bt laddr, raddr;
238 struct l2cap_channel *chan, *new;
239 l2cap_cmd_hdr_t cmd;
240 l2cap_con_req_cp cp;
241 int err;
242
243 /* extract cmd */
244 m_copydata(m, 0, sizeof(cmd), &cmd);
245 m_adj(m, sizeof(cmd));
246
247 /* extract request */
248 m_copydata(m, 0, sizeof(cp), &cp);
249 m_adj(m, sizeof(cp));
250
251 cp.scid = letoh16(cp.scid);
252 cp.psm = letoh16(cp.psm);
253
254 memset(&laddr, 0, sizeof(struct sockaddr_bt));
255 laddr.bt_len = sizeof(struct sockaddr_bt);
256 laddr.bt_family = AF_BLUETOOTH;
257 laddr.bt_psm = cp.psm;
258 bdaddr_copy(&laddr.bt_bdaddr, &link->hl_unit->hci_bdaddr);
259
260 memset(&raddr, 0, sizeof(struct sockaddr_bt));
261 raddr.bt_len = sizeof(struct sockaddr_bt);
262 raddr.bt_family = AF_BLUETOOTH;
263 raddr.bt_psm = cp.psm;
264 bdaddr_copy(&raddr.bt_bdaddr, &link->hl_bdaddr);
265
266 LIST_FOREACH(chan, &l2cap_listen_list, lc_ncid) {
267 if (chan->lc_laddr.bt_psm != laddr.bt_psm
268 && chan->lc_laddr.bt_psm != L2CAP_PSM_ANY)
269 continue;
270
271 if (!bdaddr_same(&laddr.bt_bdaddr, &chan->lc_laddr.bt_bdaddr)
272 && bdaddr_any(&chan->lc_laddr.bt_bdaddr) == 0)
273 continue;
274
275 new= (*chan->lc_proto->newconn)(chan->lc_upper, &laddr, &raddr);
276 if (new == NULL)
277 continue;
278
279 err = l2cap_cid_alloc(new);
280 if (err) {
281 l2cap_send_connect_rsp(link, cmd.ident,
282 0, cp.scid,
283 L2CAP_NO_RESOURCES);
284
285 (*new->lc_proto->disconnected)(new->lc_upper, err);
286 return;
287 }
288
289 new->lc_link = hci_acl_open(link->hl_unit, &link->hl_bdaddr);
290 KKASSERT(new->lc_link == link);
291
292 new->lc_rcid = cp.scid;
293 new->lc_ident = cmd.ident;
294
295 memcpy(&new->lc_laddr, &laddr, sizeof(struct sockaddr_bt));
296 memcpy(&new->lc_raddr, &raddr, sizeof(struct sockaddr_bt));
297
298 new->lc_mode = chan->lc_mode;
299
300 err = l2cap_setmode(new);
301 if (err == EINPROGRESS) {
302 new->lc_state = L2CAP_WAIT_SEND_CONNECT_RSP;
303 (*new->lc_proto->connecting)(new->lc_upper);
304 return;
305 }
306 if (err) {
307 new->lc_state = L2CAP_CLOSED;
308 hci_acl_close(link, err);
309 new->lc_link = NULL;
310
311 l2cap_send_connect_rsp(link, cmd.ident,
312 0, cp.scid,
313 L2CAP_NO_RESOURCES);
314
315 (*new->lc_proto->disconnected)(new->lc_upper, err);
316 return;
317 }
318
319 err = l2cap_send_connect_rsp(link, cmd.ident,
320 new->lc_lcid, new->lc_rcid,
321 L2CAP_SUCCESS);
322 if (err) {
323 l2cap_close(new, err);
324 return;
325 }
326
327 new->lc_state = L2CAP_WAIT_CONFIG;
328 new->lc_flags |= (L2CAP_WAIT_CONFIG_REQ | L2CAP_WAIT_CONFIG_RSP);
329 err = l2cap_send_config_req(new);
330 if (err)
331 l2cap_close(new, err);
332
333 return;
334 }
335
336 l2cap_send_connect_rsp(link, cmd.ident,
337 0, cp.scid,
338 L2CAP_PSM_NOT_SUPPORTED);
339 }
340
341 /*
342 * Process Received Connect Response.
343 */
344 static void
l2cap_recv_connect_rsp(struct mbuf * m,struct hci_link * link)345 l2cap_recv_connect_rsp(struct mbuf *m, struct hci_link *link)
346 {
347 l2cap_cmd_hdr_t cmd;
348 l2cap_con_rsp_cp cp;
349 struct l2cap_req *req;
350 struct l2cap_channel *chan;
351
352 m_copydata(m, 0, sizeof(cmd), &cmd);
353 m_adj(m, sizeof(cmd));
354
355 m_copydata(m, 0, sizeof(cp), &cp);
356 m_adj(m, sizeof(cp));
357
358 cp.scid = letoh16(cp.scid);
359 cp.dcid = letoh16(cp.dcid);
360 cp.result = letoh16(cp.result);
361
362 req = l2cap_request_lookup(link, cmd.ident);
363 if (req == NULL || req->lr_code != L2CAP_CONNECT_REQ)
364 return;
365
366 chan = req->lr_chan;
367 if (chan != NULL && chan->lc_lcid != cp.scid)
368 return;
369
370 if (chan == NULL || chan->lc_state != L2CAP_WAIT_RECV_CONNECT_RSP) {
371 l2cap_request_free(req);
372 return;
373 }
374
375 switch (cp.result) {
376 case L2CAP_SUCCESS:
377 /*
378 * Ok, at this point we have a connection to the other party. We
379 * could indicate upstream that we are ready for business and
380 * wait for a "Configure Channel Request" but I'm not so sure
381 * that is required in our case - we will proceed directly to
382 * sending our config request. We set two state bits because in
383 * the config state we are waiting for requests and responses.
384 */
385 l2cap_request_free(req);
386 chan->lc_rcid = cp.dcid;
387 chan->lc_state = L2CAP_WAIT_CONFIG;
388 chan->lc_flags |= (L2CAP_WAIT_CONFIG_REQ | L2CAP_WAIT_CONFIG_RSP);
389 l2cap_send_config_req(chan);
390 break;
391
392 case L2CAP_PENDING:
393 /* XXX dont release request, should start eRTX timeout? */
394 (*chan->lc_proto->connecting)(chan->lc_upper);
395 break;
396
397 case L2CAP_PSM_NOT_SUPPORTED:
398 case L2CAP_SECURITY_BLOCK:
399 case L2CAP_NO_RESOURCES:
400 default:
401 l2cap_request_free(req);
402 l2cap_close(chan, ECONNREFUSED);
403 break;
404 }
405 }
406
407 /*
408 * Process Received Config Request.
409 */
410 static void
l2cap_recv_config_req(struct mbuf * m,struct hci_link * link)411 l2cap_recv_config_req(struct mbuf *m, struct hci_link *link)
412 {
413 uint8_t buf[L2CAP_MTU_MINIMUM];
414 l2cap_cmd_hdr_t cmd;
415 l2cap_cfg_req_cp cp;
416 l2cap_cfg_opt_t opt;
417 l2cap_cfg_opt_val_t val;
418 l2cap_cfg_rsp_cp rp;
419 struct l2cap_channel *chan;
420 int left, len;
421
422 m_copydata(m, 0, sizeof(cmd), &cmd);
423 m_adj(m, sizeof(cmd));
424 left = letoh16(cmd.length);
425
426 if (left < sizeof(cp))
427 goto reject;
428
429 m_copydata(m, 0, sizeof(cp), &cp);
430 m_adj(m, sizeof(cp));
431 left -= sizeof(cp);
432
433 cp.dcid = letoh16(cp.dcid);
434 cp.flags = letoh16(cp.flags);
435
436 chan = l2cap_cid_lookup(cp.dcid);
437 if (chan == NULL || chan->lc_link != link
438 || chan->lc_state != L2CAP_WAIT_CONFIG
439 || (chan->lc_flags & L2CAP_WAIT_CONFIG_REQ) == 0) {
440 /* XXX we should really accept reconfiguration requests */
441 l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_INVALID_CID,
442 L2CAP_NULL_CID, cp.dcid);
443 goto out;
444 }
445
446 /* ready our response packet */
447 rp.scid = htole16(chan->lc_rcid);
448 rp.flags = 0; /* "No Continuation" */
449 rp.result = L2CAP_SUCCESS;
450 len = sizeof(rp);
451
452 /*
453 * Process the packet. We build the return packet on the fly adding any
454 * unacceptable parameters as we go. As we can only return one result,
455 * unknown option takes precedence so we start our return packet anew
456 * and ignore option values thereafter as they will be re-sent.
457 *
458 * Since we do not support enough options to make overflowing the min
459 * MTU size an issue in normal use, we just reject config requests that
460 * make that happen. This could be because options are repeated or the
461 * packet is corrupted in some way.
462 *
463 * If unknown option types threaten to overflow the packet, we just
464 * ignore them. We can deny them next time.
465 */
466 while (left > 0) {
467 if (left < sizeof(opt))
468 goto reject;
469
470 m_copydata(m, 0, sizeof(opt), &opt);
471 m_adj(m, sizeof(opt));
472 left -= sizeof(opt);
473
474 if (left < opt.length)
475 goto reject;
476
477 switch(opt.type & L2CAP_OPT_HINT_MASK) {
478 case L2CAP_OPT_MTU:
479 if (rp.result == L2CAP_UNKNOWN_OPTION)
480 break;
481
482 if (opt.length != L2CAP_OPT_MTU_SIZE)
483 goto reject;
484
485 m_copydata(m, 0, L2CAP_OPT_MTU_SIZE, &val);
486 val.mtu = letoh16(val.mtu);
487
488 /*
489 * XXX how do we know what the minimum acceptable MTU is
490 * for a channel? Spec says some profiles have a higher
491 * minimum but I have no way to find that out at this
492 * juncture..
493 */
494 if (val.mtu < L2CAP_MTU_MINIMUM) {
495 if (len + sizeof(opt) + L2CAP_OPT_MTU_SIZE > sizeof(buf))
496 goto reject;
497
498 rp.result = L2CAP_UNACCEPTABLE_PARAMS;
499 memcpy(buf + len, &opt, sizeof(opt));
500 len += sizeof(opt);
501 val.mtu = htole16(L2CAP_MTU_MINIMUM);
502 memcpy(buf + len, &val, L2CAP_OPT_MTU_SIZE);
503 len += L2CAP_OPT_MTU_SIZE;
504 } else
505 chan->lc_omtu = val.mtu;
506
507 break;
508
509 case L2CAP_OPT_FLUSH_TIMO:
510 if (rp.result == L2CAP_UNKNOWN_OPTION)
511 break;
512
513 if (opt.length != L2CAP_OPT_FLUSH_TIMO_SIZE)
514 goto reject;
515
516 /*
517 * I think that this is informational only - he is
518 * informing us of the flush timeout he will be using.
519 * I dont think this affects us in any significant way,
520 * so just ignore this value for now.
521 */
522 break;
523
524 case L2CAP_OPT_QOS:
525 if (rp.result == L2CAP_UNKNOWN_OPTION)
526 break;
527
528 if (opt.length != L2CAP_OPT_QOS_SIZE)
529 goto reject;
530
531 m_copydata(m, 0, L2CAP_OPT_QOS_SIZE, &val);
532 if (val.qos.service_type == L2CAP_QOS_NO_TRAFFIC ||
533 val.qos.service_type == L2CAP_QOS_BEST_EFFORT)
534 /*
535 * In accordance with the spec, we choose to
536 * ignore the fields an provide no response.
537 */
538 break;
539
540 if (len + sizeof(opt) + L2CAP_OPT_QOS_SIZE > sizeof(buf))
541 goto reject;
542
543 if (val.qos.service_type != L2CAP_QOS_GUARANTEED) {
544 /*
545 * Instead of sending an "unacceptable
546 * parameters" response, treat this as an
547 * unknown option and include the option
548 * value in the response.
549 */
550 rp.result = L2CAP_UNKNOWN_OPTION;
551 } else {
552 /*
553 * According to the spec, we must return
554 * specific values for wild card parameters.
555 * I don't know what to return without lying,
556 * so return "unacceptable parameters" and
557 * specify the preferred service type as
558 * "Best Effort".
559 */
560 rp.result = L2CAP_UNACCEPTABLE_PARAMS;
561 val.qos.service_type = L2CAP_QOS_BEST_EFFORT;
562 }
563
564 memcpy(buf + len, &opt, sizeof(opt));
565 len += sizeof(opt);
566 memcpy(buf + len, &val, L2CAP_OPT_QOS_SIZE);
567 len += L2CAP_OPT_QOS_SIZE;
568 break;
569
570 default:
571 /* ignore hints */
572 if (opt.type & L2CAP_OPT_HINT_BIT)
573 break;
574
575 /* unknown options supersede all else */
576 if (rp.result != L2CAP_UNKNOWN_OPTION) {
577 rp.result = L2CAP_UNKNOWN_OPTION;
578 len = sizeof(rp);
579 }
580
581 /* ignore if it doesn't fit */
582 if (len + sizeof(opt) > sizeof(buf))
583 break;
584
585 /* return unknown option type, but no data */
586 buf[len++] = opt.type;
587 buf[len++] = 0;
588 break;
589 }
590
591 m_adj(m, opt.length);
592 left -= opt.length;
593 }
594
595 rp.result = htole16(rp.result);
596 memcpy(buf, &rp, sizeof(rp));
597 l2cap_send_signal(link, L2CAP_CONFIG_RSP, cmd.ident, len, buf);
598
599 if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0
600 && rp.result == letoh16(L2CAP_SUCCESS)) {
601
602 chan->lc_flags &= ~L2CAP_WAIT_CONFIG_REQ;
603
604 if ((chan->lc_flags & L2CAP_WAIT_CONFIG_RSP) == 0) {
605 chan->lc_state = L2CAP_OPEN;
606 /* XXX how to distinguish REconfiguration? */
607 (*chan->lc_proto->connected)(chan->lc_upper);
608 }
609 }
610 return;
611
612 reject:
613 l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_NOT_UNDERSTOOD);
614 out:
615 m_adj(m, left);
616 }
617
618 /*
619 * Process Received Config Response.
620 */
621 static void
l2cap_recv_config_rsp(struct mbuf * m,struct hci_link * link)622 l2cap_recv_config_rsp(struct mbuf *m, struct hci_link *link)
623 {
624 l2cap_cmd_hdr_t cmd;
625 l2cap_cfg_rsp_cp cp;
626 l2cap_cfg_opt_t opt;
627 l2cap_cfg_opt_val_t val;
628 struct l2cap_req *req;
629 struct l2cap_channel *chan;
630 int left;
631
632 m_copydata(m, 0, sizeof(cmd), &cmd);
633 m_adj(m, sizeof(cmd));
634 left = letoh16(cmd.length);
635
636 if (left < sizeof(cp))
637 goto out;
638
639 m_copydata(m, 0, sizeof(cp), &cp);
640 m_adj(m, sizeof(cp));
641 left -= sizeof(cp);
642
643 cp.scid = letoh16(cp.scid);
644 cp.flags = letoh16(cp.flags);
645 cp.result = letoh16(cp.result);
646
647 req = l2cap_request_lookup(link, cmd.ident);
648 if (req == NULL || req->lr_code != L2CAP_CONFIG_REQ)
649 goto out;
650
651 chan = req->lr_chan;
652 if (chan != NULL && chan->lc_lcid != cp.scid)
653 goto out;
654
655 l2cap_request_free(req);
656
657 if (chan == NULL || chan->lc_state != L2CAP_WAIT_CONFIG
658 || (chan->lc_flags & L2CAP_WAIT_CONFIG_RSP) == 0)
659 goto out;
660
661 if ((cp.flags & L2CAP_OPT_CFLAG_BIT)) {
662 l2cap_cfg_req_cp rp;
663
664 /*
665 * They have more to tell us and want another ID to
666 * use, so send an empty config request
667 */
668 if (l2cap_request_alloc(chan, L2CAP_CONFIG_REQ))
669 goto discon;
670
671 rp.dcid = htole16(cp.scid);
672 rp.flags = 0;
673
674 if (l2cap_send_signal(link, L2CAP_CONFIG_REQ, link->hl_lastid,
675 sizeof(rp), &rp))
676 goto discon;
677 }
678
679 switch(cp.result) {
680 case L2CAP_SUCCESS:
681 /*
682 * If continuation flag was not set, our config request was
683 * accepted. We may have to wait for their config request to
684 * complete, so check that but otherwise we are open
685 *
686 * There may be 'advisory' values in the packet but we just
687 * ignore those..
688 */
689 if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0) {
690 chan->lc_flags &= ~L2CAP_WAIT_CONFIG_RSP;
691
692 if ((chan->lc_flags & L2CAP_WAIT_CONFIG_REQ) == 0) {
693 chan->lc_state = L2CAP_OPEN;
694 /* XXX how to distinguish REconfiguration? */
695 (*chan->lc_proto->connected)(chan->lc_upper);
696 }
697 }
698 goto out;
699
700 case L2CAP_UNACCEPTABLE_PARAMS:
701 /*
702 * Packet contains unacceptable parameters with preferred values
703 */
704 while (left > 0) {
705 if (left < sizeof(opt))
706 goto discon;
707
708 m_copydata(m, 0, sizeof(opt), &opt);
709 m_adj(m, sizeof(opt));
710 left -= sizeof(opt);
711
712 if (left < opt.length)
713 goto discon;
714
715 switch (opt.type) {
716 case L2CAP_OPT_MTU:
717 if (opt.length != L2CAP_OPT_MTU_SIZE)
718 goto discon;
719
720 m_copydata(m, 0, L2CAP_OPT_MTU_SIZE, &val);
721 chan->lc_imtu = letoh16(val.mtu);
722 if (chan->lc_imtu < L2CAP_MTU_MINIMUM)
723 chan->lc_imtu = L2CAP_MTU_DEFAULT;
724 break;
725
726 case L2CAP_OPT_FLUSH_TIMO:
727 if (opt.length != L2CAP_OPT_FLUSH_TIMO_SIZE)
728 goto discon;
729
730 /*
731 * Spec says: If we cannot honor proposed value,
732 * either disconnect or try again with original
733 * value. I can't really see why they want to
734 * interfere with OUR flush timeout in any case
735 * so we just punt for now.
736 */
737 goto discon;
738
739 case L2CAP_OPT_QOS:
740 break;
741
742 default:
743 UNKNOWN(opt.type);
744 goto discon;
745 }
746
747 m_adj(m, opt.length);
748 left -= opt.length;
749 }
750
751 if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0)
752 l2cap_send_config_req(chan); /* no state change */
753
754 goto out;
755
756 case L2CAP_REJECT:
757 goto discon;
758
759 case L2CAP_UNKNOWN_OPTION:
760 /*
761 * Packet contains options not understood. Turn off unknown
762 * options by setting them to default values (means they will
763 * not be requested again).
764 *
765 * If our option was already off then fail (paranoia?)
766 *
767 * XXX Should we consider that options were set for a reason?
768 */
769 while (left > 0) {
770 if (left < sizeof(opt))
771 goto discon;
772
773 m_copydata(m, 0, sizeof(opt), &opt);
774 m_adj(m, sizeof(opt));
775 left -= sizeof(opt);
776
777 if (left < opt.length)
778 goto discon;
779
780 m_adj(m, opt.length);
781 left -= opt.length;
782
783 switch(opt.type) {
784 case L2CAP_OPT_MTU:
785 if (chan->lc_imtu == L2CAP_MTU_DEFAULT)
786 goto discon;
787
788 chan->lc_imtu = L2CAP_MTU_DEFAULT;
789 break;
790
791 case L2CAP_OPT_FLUSH_TIMO:
792 if (chan->lc_flush == L2CAP_FLUSH_TIMO_DEFAULT)
793 goto discon;
794
795 chan->lc_flush = L2CAP_FLUSH_TIMO_DEFAULT;
796 break;
797
798 case L2CAP_OPT_QOS:
799 break;
800
801 default:
802 UNKNOWN(opt.type);
803 goto discon;
804 }
805 }
806
807 if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0)
808 l2cap_send_config_req(chan); /* no state change */
809
810 goto out;
811
812 default:
813 UNKNOWN(cp.result);
814 goto discon;
815 }
816
817 DPRINTF("how did I get here!?\n");
818
819 discon:
820 l2cap_send_disconnect_req(chan);
821 l2cap_close(chan, ECONNABORTED);
822
823 out:
824 m_adj(m, left);
825 }
826
827 /*
828 * Process Received Disconnect Request. We must validate scid and dcid
829 * just in case but otherwise this connection is finished.
830 */
831 static void
l2cap_recv_disconnect_req(struct mbuf * m,struct hci_link * link)832 l2cap_recv_disconnect_req(struct mbuf *m, struct hci_link *link)
833 {
834 l2cap_cmd_hdr_t cmd;
835 l2cap_discon_req_cp cp;
836 l2cap_discon_rsp_cp rp;
837 struct l2cap_channel *chan;
838
839 m_copydata(m, 0, sizeof(cmd), &cmd);
840 m_adj(m, sizeof(cmd));
841
842 m_copydata(m, 0, sizeof(cp), &cp);
843 m_adj(m, sizeof(cp));
844
845 cp.scid = letoh16(cp.scid);
846 cp.dcid = letoh16(cp.dcid);
847
848 chan = l2cap_cid_lookup(cp.dcid);
849 if (chan == NULL || chan->lc_link != link || chan->lc_rcid != cp.scid) {
850 l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_INVALID_CID,
851 cp.dcid, cp.scid);
852 return;
853 }
854
855 rp.dcid = htole16(chan->lc_lcid);
856 rp.scid = htole16(chan->lc_rcid);
857 l2cap_send_signal(link, L2CAP_DISCONNECT_RSP, cmd.ident,
858 sizeof(rp), &rp);
859
860 if (chan->lc_state != L2CAP_CLOSED)
861 l2cap_close(chan, ECONNRESET);
862 }
863
864 /*
865 * Process Received Disconnect Response. We must validate scid and dcid but
866 * unless we were waiting for this signal, ignore it.
867 */
868 static void
l2cap_recv_disconnect_rsp(struct mbuf * m,struct hci_link * link)869 l2cap_recv_disconnect_rsp(struct mbuf *m, struct hci_link *link)
870 {
871 l2cap_cmd_hdr_t cmd;
872 l2cap_discon_rsp_cp cp;
873 struct l2cap_req *req;
874 struct l2cap_channel *chan;
875
876 m_copydata(m, 0, sizeof(cmd), &cmd);
877 m_adj(m, sizeof(cmd));
878
879 m_copydata(m, 0, sizeof(cp), &cp);
880 m_adj(m, sizeof(cp));
881
882 cp.scid = letoh16(cp.scid);
883 cp.dcid = letoh16(cp.dcid);
884
885 req = l2cap_request_lookup(link, cmd.ident);
886 if (req == NULL || req->lr_code != L2CAP_DISCONNECT_REQ)
887 return;
888
889 chan = req->lr_chan;
890 if (chan == NULL
891 || chan->lc_lcid != cp.scid
892 || chan->lc_rcid != cp.dcid)
893 return;
894
895 l2cap_request_free(req);
896
897 if (chan->lc_state != L2CAP_WAIT_DISCONNECT)
898 return;
899
900 l2cap_close(chan, 0);
901 }
902
903 /*
904 * Process Received Info Request. We must respond but alas dont
905 * support anything as yet so thats easy.
906 */
907 static void
l2cap_recv_info_req(struct mbuf * m,struct hci_link * link)908 l2cap_recv_info_req(struct mbuf *m, struct hci_link *link)
909 {
910 l2cap_cmd_hdr_t cmd;
911 l2cap_info_req_cp cp;
912 l2cap_info_rsp_cp rp;
913
914 m_copydata(m, 0, sizeof(cmd), &cmd);
915 m_adj(m, sizeof(cmd));
916
917 m_copydata(m, 0, sizeof(cp), &cp);
918 m_adj(m, sizeof(cp));
919
920 switch(letoh16(cp.type)) {
921 case L2CAP_CONNLESS_MTU:
922 case L2CAP_EXTENDED_FEATURES:
923 default:
924 rp.type = cp.type;
925 rp.result = htole16(L2CAP_NOT_SUPPORTED);
926
927 l2cap_send_signal(link, L2CAP_INFO_RSP, cmd.ident,
928 sizeof(rp), &rp);
929 break;
930 }
931 }
932
933 /*
934 * Construct signal and wrap in C-Frame for link.
935 */
936 static int
l2cap_send_signal(struct hci_link * link,uint8_t code,uint8_t ident,uint16_t length,void * data)937 l2cap_send_signal(struct hci_link *link, uint8_t code, uint8_t ident,
938 uint16_t length, void *data)
939 {
940 struct mbuf *m;
941 l2cap_hdr_t *hdr;
942 l2cap_cmd_hdr_t *cmd;
943 int hlen;
944
945 #ifdef DIAGNOSTIC
946 if (link == NULL)
947 return ENETDOWN;
948
949 if (sizeof(l2cap_cmd_hdr_t) + length > link->hl_mtu)
950 kprintf("(%s) exceeding L2CAP Signal MTU for link!\n",
951 device_get_nameunit(link->hl_unit->hci_dev));
952 #endif
953
954 m = m_gethdr(M_NOWAIT, MT_DATA);
955 if (m == NULL)
956 return ENOMEM;
957
958 hdr = mtod(m, l2cap_hdr_t *);
959 cmd = (l2cap_cmd_hdr_t *)(hdr + 1);
960 hlen = (int)(sizeof(*hdr) + sizeof(*cmd));
961
962 m->m_len = m->m_pkthdr.len = hlen; /* in case length == 0 */
963
964 /* Command Data */
965 if (length > 0 && m_copyback2(m, hlen, length, data, M_NOWAIT) != 0) {
966 m_freem(m);
967 return ENOMEM;
968 }
969
970 /* Command Header */
971 cmd->code = code;
972 cmd->ident = ident;
973 cmd->length = htole16(length);
974 length += sizeof(*cmd);
975
976 /* C-Frame Header */
977 hdr->length = htole16(length);
978 hdr->dcid = htole16(L2CAP_SIGNAL_CID);
979 length += sizeof(*hdr);
980
981 DPRINTFN(2, "(%s) code %d, ident %d, len %d\n",
982 device_get_nameunit(link->hl_unit->hci_dev), code, ident,
983 length);
984
985 return hci_acl_send(m, link, NULL);
986 }
987
988 /*
989 * Send Command Reject packet.
990 */
991 static int
l2cap_send_command_rej(struct hci_link * link,uint8_t ident,uint16_t reason,...)992 l2cap_send_command_rej(struct hci_link *link, uint8_t ident,
993 uint16_t reason, ...)
994 {
995 l2cap_cmd_rej_cp cp;
996 int len = 0;
997 __va_list ap;
998
999 __va_start(ap, reason);
1000
1001 cp.reason = htole16(reason);
1002
1003 switch (reason) {
1004 case L2CAP_REJ_NOT_UNDERSTOOD:
1005 len = 2;
1006 break;
1007
1008 case L2CAP_REJ_MTU_EXCEEDED:
1009 len = 4;
1010 cp.data[0] = __va_arg(ap, int); /* SigMTU */
1011 cp.data[0] = htole16(cp.data[0]);
1012 break;
1013
1014 case L2CAP_REJ_INVALID_CID:
1015 len = 6;
1016 cp.data[0] = __va_arg(ap, int); /* dcid */
1017 cp.data[0] = htole16(cp.data[0]);
1018 cp.data[1] = __va_arg(ap, int); /* scid */
1019 cp.data[1] = htole16(cp.data[1]);
1020 break;
1021
1022 default:
1023 UNKNOWN(reason);
1024 return EINVAL;
1025 }
1026
1027 __va_end(ap);
1028
1029 return l2cap_send_signal(link, L2CAP_COMMAND_REJ, ident, len, &cp);
1030 }
1031
1032 /*
1033 * Send Connect Request
1034 */
1035 int
l2cap_send_connect_req(struct l2cap_channel * chan)1036 l2cap_send_connect_req(struct l2cap_channel *chan)
1037 {
1038 l2cap_con_req_cp cp;
1039 int err;
1040
1041 err = l2cap_request_alloc(chan, L2CAP_CONNECT_REQ);
1042 if (err)
1043 return err;
1044
1045 cp.psm = htole16(chan->lc_raddr.bt_psm);
1046 cp.scid = htole16(chan->lc_lcid);
1047
1048 return l2cap_send_signal(chan->lc_link, L2CAP_CONNECT_REQ,
1049 chan->lc_link->hl_lastid, sizeof(cp), &cp);
1050 }
1051
1052 /*
1053 * Send Config Request
1054 *
1055 * For outgoing config request, we only put options in the packet if they
1056 * differ from the default and would have to be actioned. We dont support
1057 * enough option types to make overflowing SigMTU an issue so it can all
1058 * go in one packet.
1059 */
1060 int
l2cap_send_config_req(struct l2cap_channel * chan)1061 l2cap_send_config_req(struct l2cap_channel *chan)
1062 {
1063 l2cap_cfg_req_cp *cp;
1064 l2cap_cfg_opt_t *opt;
1065 l2cap_cfg_opt_val_t *val;
1066 uint8_t *next, buf[L2CAP_MTU_MINIMUM];
1067 int err;
1068
1069 err = l2cap_request_alloc(chan, L2CAP_CONFIG_REQ);
1070 if (err)
1071 return err;
1072
1073 /* Config Header (4 octets) */
1074 cp = (l2cap_cfg_req_cp *)buf;
1075 cp->dcid = htole16(chan->lc_rcid);
1076 cp->flags = 0; /* "No Continuation" */
1077
1078 next = buf + sizeof(l2cap_cfg_req_cp);
1079
1080 /* Incoming MTU (4 octets) */
1081 if (chan->lc_imtu != L2CAP_MTU_DEFAULT) {
1082 opt = (l2cap_cfg_opt_t *)next;
1083 opt->type = L2CAP_OPT_MTU;
1084 opt->length = L2CAP_OPT_MTU_SIZE;
1085
1086 val = (l2cap_cfg_opt_val_t *)(opt + 1);
1087 val->mtu = htole16(chan->lc_imtu);
1088
1089 next += sizeof(l2cap_cfg_opt_t) + L2CAP_OPT_MTU_SIZE;
1090 }
1091
1092 /* Flush Timeout (4 octets) */
1093 if (chan->lc_flush != L2CAP_FLUSH_TIMO_DEFAULT) {
1094 opt = (l2cap_cfg_opt_t *)next;
1095 opt->type = L2CAP_OPT_FLUSH_TIMO;
1096 opt->length = L2CAP_OPT_FLUSH_TIMO_SIZE;
1097
1098 val = (l2cap_cfg_opt_val_t *)(opt + 1);
1099 val->flush_timo = htole16(chan->lc_flush);
1100
1101 next += sizeof(l2cap_cfg_opt_t) + L2CAP_OPT_FLUSH_TIMO_SIZE;
1102 }
1103
1104 /* Outgoing QoS Flow (24 octets) */
1105 /* Retransmission & Flow Control (11 octets) */
1106 /*
1107 * From here we need to start paying attention to SigMTU as we have
1108 * possibly overflowed the minimum supported..
1109 */
1110
1111 return l2cap_send_signal(chan->lc_link, L2CAP_CONFIG_REQ,
1112 chan->lc_link->hl_lastid, (int)(next - buf), buf);
1113 }
1114
1115 /*
1116 * Send Disconnect Request
1117 */
1118 int
l2cap_send_disconnect_req(struct l2cap_channel * chan)1119 l2cap_send_disconnect_req(struct l2cap_channel *chan)
1120 {
1121 l2cap_discon_req_cp cp;
1122 int err;
1123
1124 err = l2cap_request_alloc(chan, L2CAP_DISCONNECT_REQ);
1125 if (err)
1126 return err;
1127
1128 cp.dcid = htole16(chan->lc_rcid);
1129 cp.scid = htole16(chan->lc_lcid);
1130
1131 return l2cap_send_signal(chan->lc_link, L2CAP_DISCONNECT_REQ,
1132 chan->lc_link->hl_lastid, sizeof(cp), &cp);
1133 }
1134
1135 /*
1136 * Send Connect Response
1137 */
1138 int
l2cap_send_connect_rsp(struct hci_link * link,uint8_t ident,uint16_t dcid,uint16_t scid,uint16_t result)1139 l2cap_send_connect_rsp(struct hci_link *link, uint8_t ident, uint16_t dcid,
1140 uint16_t scid, uint16_t result)
1141 {
1142 l2cap_con_rsp_cp cp;
1143
1144 memset(&cp, 0, sizeof(cp));
1145 cp.dcid = htole16(dcid);
1146 cp.scid = htole16(scid);
1147 cp.result = htole16(result);
1148
1149 return l2cap_send_signal(link, L2CAP_CONNECT_RSP, ident, sizeof(cp), &cp);
1150 }
1151