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