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