6 * Copyright (c) Maksim Yevmenkin <m_evmenkin@yahoo.com>
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
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.
18 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
19 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
22 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
24 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
25 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
26 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
27 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
30 * $Id: ng_l2cap_evnt.c,v 1.5 2003/09/08 19:11:45 max Exp $
31 * $FreeBSD: src/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c,v 1.8 2005/01/07 01:45:43 imp Exp $
34 #include <sys/param.h>
35 #include <sys/systm.h>
36 #include <sys/kernel.h>
37 #include <sys/endian.h>
38 #include <sys/malloc.h>
40 #include <sys/queue.h>
41 #include <netgraph/ng_message.h>
42 #include <netgraph/netgraph.h>
43 #include <netgraph/bluetooth/include/ng_bluetooth.h>
44 #include <netgraph/bluetooth/include/ng_hci.h>
45 #include <netgraph/bluetooth/include/ng_l2cap.h>
46 #include <netgraph/bluetooth/l2cap/ng_l2cap_var.h>
47 #include <netgraph/bluetooth/l2cap/ng_l2cap_cmds.h>
48 #include <netgraph/bluetooth/l2cap/ng_l2cap_evnt.h>
49 #include <netgraph/bluetooth/l2cap/ng_l2cap_llpi.h>
50 #include <netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h>
51 #include <netgraph/bluetooth/l2cap/ng_l2cap_misc.h>
53 /******************************************************************************
54 ******************************************************************************
55 ** L2CAP events processing module
56 ******************************************************************************
57 ******************************************************************************/
59 static int ng_l2cap_process_signal_cmd (ng_l2cap_con_p
);
60 static int ng_l2cap_process_cmd_rej (ng_l2cap_con_p
, u_int8_t
);
61 static int ng_l2cap_process_con_req (ng_l2cap_con_p
, u_int8_t
);
62 static int ng_l2cap_process_con_rsp (ng_l2cap_con_p
, u_int8_t
);
63 static int ng_l2cap_process_cfg_req (ng_l2cap_con_p
, u_int8_t
);
64 static int ng_l2cap_process_cfg_rsp (ng_l2cap_con_p
, u_int8_t
);
65 static int ng_l2cap_process_discon_req (ng_l2cap_con_p
, u_int8_t
);
66 static int ng_l2cap_process_discon_rsp (ng_l2cap_con_p
, u_int8_t
);
67 static int ng_l2cap_process_echo_req (ng_l2cap_con_p
, u_int8_t
);
68 static int ng_l2cap_process_echo_rsp (ng_l2cap_con_p
, u_int8_t
);
69 static int ng_l2cap_process_info_req (ng_l2cap_con_p
, u_int8_t
);
70 static int ng_l2cap_process_info_rsp (ng_l2cap_con_p
, u_int8_t
);
71 static int send_l2cap_reject
72 (ng_l2cap_con_p
, u_int8_t
, u_int16_t
, u_int16_t
, u_int16_t
, u_int16_t
);
73 static int send_l2cap_con_rej
74 (ng_l2cap_con_p
, u_int8_t
, u_int16_t
, u_int16_t
, u_int16_t
);
75 static int send_l2cap_cfg_rsp
76 (ng_l2cap_con_p
, u_int8_t
, u_int16_t
, u_int16_t
, struct mbuf
*);
77 static int get_next_l2cap_opt
78 (struct mbuf
*, int *, ng_l2cap_cfg_opt_p
, ng_l2cap_cfg_opt_val_p
);
81 * Receive L2CAP packet. First get L2CAP header and verify packet. Than
82 * get destination channel and process packet.
86 ng_l2cap_receive(ng_l2cap_con_p con
)
88 ng_l2cap_p l2cap
= con
->l2cap
;
89 ng_l2cap_hdr_t
*hdr
= NULL
;
93 if (con
->rx_pkt
->m_pkthdr
.len
< sizeof(*hdr
)) {
95 "%s: %s - invalid L2CAP packet. Packet too small, len=%d\n",
96 __func__
, NG_NODE_NAME(l2cap
->node
),
97 con
->rx_pkt
->m_pkthdr
.len
);
102 /* Get L2CAP header */
103 NG_L2CAP_M_PULLUP(con
->rx_pkt
, sizeof(*hdr
));
104 if (con
->rx_pkt
== NULL
)
107 hdr
= mtod(con
->rx_pkt
, ng_l2cap_hdr_t
*);
108 hdr
->length
= le16toh(hdr
->length
);
109 hdr
->dcid
= le16toh(hdr
->dcid
);
111 /* Check payload size */
112 if (hdr
->length
!= con
->rx_pkt
->m_pkthdr
.len
- sizeof(*hdr
)) {
114 "%s: %s - invalid L2CAP packet. Payload length mismatch, length=%d, len=%zd\n",
115 __func__
, NG_NODE_NAME(l2cap
->node
), hdr
->length
,
116 con
->rx_pkt
->m_pkthdr
.len
- sizeof(*hdr
));
123 case NG_L2CAP_SIGNAL_CID
: /* L2CAP command */
124 m_adj(con
->rx_pkt
, sizeof(*hdr
));
125 error
= ng_l2cap_process_signal_cmd(con
);
128 case NG_L2CAP_CLT_CID
: /* Connectionless packet */
129 error
= ng_l2cap_l2ca_clt_receive(con
);
132 default: /* Data packet */
133 error
= ng_l2cap_l2ca_receive(con
);
139 NG_FREE_M(con
->rx_pkt
);
142 } /* ng_l2cap_receive */
145 * Process L2CAP signaling command. We already know that destination channel ID
146 * is 0x1 that means we have received signaling command from peer's L2CAP layer.
147 * So get command header, decode and process it.
149 * XXX do we need to check signaling MTU here?
153 ng_l2cap_process_signal_cmd(ng_l2cap_con_p con
)
155 ng_l2cap_p l2cap
= con
->l2cap
;
156 ng_l2cap_cmd_hdr_t
*hdr
= NULL
;
157 struct mbuf
*m
= NULL
;
159 while (con
->rx_pkt
!= NULL
) {
160 /* Verify packet length */
161 if (con
->rx_pkt
->m_pkthdr
.len
< sizeof(*hdr
)) {
163 "%s: %s - invalid L2CAP signaling command. Packet too small, len=%d\n",
164 __func__
, NG_NODE_NAME(l2cap
->node
),
165 con
->rx_pkt
->m_pkthdr
.len
);
166 NG_FREE_M(con
->rx_pkt
);
171 /* Get signaling command */
172 NG_L2CAP_M_PULLUP(con
->rx_pkt
, sizeof(*hdr
));
173 if (con
->rx_pkt
== NULL
)
176 hdr
= mtod(con
->rx_pkt
, ng_l2cap_cmd_hdr_t
*);
177 hdr
->length
= le16toh(hdr
->length
);
178 m_adj(con
->rx_pkt
, sizeof(*hdr
));
180 /* Verify command length */
181 if (con
->rx_pkt
->m_pkthdr
.len
< hdr
->length
) {
183 "%s: %s - invalid L2CAP signaling command, code=%#x, ident=%d. " \
184 "Invalid command length=%d, m_pkthdr.len=%d\n",
185 __func__
, NG_NODE_NAME(l2cap
->node
),
186 hdr
->code
, hdr
->ident
, hdr
->length
,
187 con
->rx_pkt
->m_pkthdr
.len
);
188 NG_FREE_M(con
->rx_pkt
);
193 /* Get the command, save the rest (if any) */
194 if (con
->rx_pkt
->m_pkthdr
.len
> hdr
->length
)
195 m
= m_split(con
->rx_pkt
, hdr
->length
, M_DONTWAIT
);
199 /* Process command */
201 case NG_L2CAP_CMD_REJ
:
202 ng_l2cap_process_cmd_rej(con
, hdr
->ident
);
205 case NG_L2CAP_CON_REQ
:
206 ng_l2cap_process_con_req(con
, hdr
->ident
);
209 case NG_L2CAP_CON_RSP
:
210 ng_l2cap_process_con_rsp(con
, hdr
->ident
);
213 case NG_L2CAP_CFG_REQ
:
214 ng_l2cap_process_cfg_req(con
, hdr
->ident
);
217 case NG_L2CAP_CFG_RSP
:
218 ng_l2cap_process_cfg_rsp(con
, hdr
->ident
);
221 case NG_L2CAP_DISCON_REQ
:
222 ng_l2cap_process_discon_req(con
, hdr
->ident
);
225 case NG_L2CAP_DISCON_RSP
:
226 ng_l2cap_process_discon_rsp(con
, hdr
->ident
);
229 case NG_L2CAP_ECHO_REQ
:
230 ng_l2cap_process_echo_req(con
, hdr
->ident
);
233 case NG_L2CAP_ECHO_RSP
:
234 ng_l2cap_process_echo_rsp(con
, hdr
->ident
);
237 case NG_L2CAP_INFO_REQ
:
238 ng_l2cap_process_info_req(con
, hdr
->ident
);
241 case NG_L2CAP_INFO_RSP
:
242 ng_l2cap_process_info_rsp(con
, hdr
->ident
);
247 "%s: %s - unknown L2CAP signaling command, code=%#x, ident=%d, length=%d\n",
248 __func__
, NG_NODE_NAME(l2cap
->node
),
249 hdr
->code
, hdr
->ident
, hdr
->length
);
252 * Send L2CAP_CommandRej. Do not really care
256 send_l2cap_reject(con
, hdr
->ident
,
257 NG_L2CAP_REJ_NOT_UNDERSTOOD
, 0, 0, 0);
258 NG_FREE_M(con
->rx_pkt
);
266 } /* ng_l2cap_process_signal_cmd */
269 * Process L2CAP_CommandRej command
273 ng_l2cap_process_cmd_rej(ng_l2cap_con_p con
, u_int8_t ident
)
275 ng_l2cap_p l2cap
= con
->l2cap
;
276 ng_l2cap_cmd_rej_cp
*cp
= NULL
;
277 ng_l2cap_cmd_p cmd
= NULL
;
279 /* Get command parameters */
280 NG_L2CAP_M_PULLUP(con
->rx_pkt
, sizeof(*cp
));
281 if (con
->rx_pkt
== NULL
)
284 cp
= mtod(con
->rx_pkt
, ng_l2cap_cmd_rej_cp
*);
285 cp
->reason
= le16toh(cp
->reason
);
287 /* Check if we have pending command descriptor */
288 cmd
= ng_l2cap_cmd_by_ident(con
, ident
);
290 /* If command timeout already happened then ignore reject */
291 if (ng_l2cap_command_untimeout(cmd
) != 0) {
292 NG_FREE_M(con
->rx_pkt
);
296 ng_l2cap_unlink_cmd(cmd
);
299 case NG_L2CAP_CON_REQ
:
300 ng_l2cap_l2ca_con_rsp(cmd
->ch
,cmd
->token
,cp
->reason
,0);
301 ng_l2cap_free_chan(cmd
->ch
);
304 case NG_L2CAP_CFG_REQ
:
305 ng_l2cap_l2ca_cfg_rsp(cmd
->ch
, cmd
->token
, cp
->reason
);
308 case NG_L2CAP_DISCON_REQ
:
309 ng_l2cap_l2ca_discon_rsp(cmd
->ch
,cmd
->token
,cp
->reason
);
310 ng_l2cap_free_chan(cmd
->ch
); /* XXX free channel */
313 case NG_L2CAP_ECHO_REQ
:
314 ng_l2cap_l2ca_ping_rsp(cmd
->con
, cmd
->token
,
318 case NG_L2CAP_INFO_REQ
:
319 ng_l2cap_l2ca_get_info_rsp(cmd
->con
, cmd
->token
,
325 "%s: %s - unexpected L2CAP_CommandRej. Unexpected L2CAP command opcode=%d\n",
326 __func__
, NG_NODE_NAME(l2cap
->node
), cmd
->code
);
330 ng_l2cap_free_cmd(cmd
);
333 "%s: %s - unexpected L2CAP_CommandRej command. " \
334 "Requested ident does not exist, ident=%d\n",
335 __func__
, NG_NODE_NAME(l2cap
->node
), ident
);
337 NG_FREE_M(con
->rx_pkt
);
340 } /* ng_l2cap_process_cmd_rej */
343 * Process L2CAP_ConnectReq command
347 ng_l2cap_process_con_req(ng_l2cap_con_p con
, u_int8_t ident
)
349 ng_l2cap_p l2cap
= con
->l2cap
;
350 struct mbuf
*m
= con
->rx_pkt
;
351 ng_l2cap_con_req_cp
*cp
= NULL
;
352 ng_l2cap_chan_p ch
= NULL
;
356 /* Get command parameters */
357 NG_L2CAP_M_PULLUP(m
, sizeof(*cp
));
361 cp
= mtod(m
, ng_l2cap_con_req_cp
*);
362 psm
= le16toh(cp
->psm
);
363 dcid
= le16toh(cp
->scid
);
369 * Create new channel and send L2CA_ConnectInd notification
370 * to the upper layer protocol.
373 ch
= ng_l2cap_new_chan(l2cap
, con
, psm
);
375 return (send_l2cap_con_rej(con
, ident
, 0, dcid
,
376 NG_L2CAP_NO_RESOURCES
));
378 /* Update channel IDs */
381 /* Sent L2CA_ConnectInd notification to the upper layer */
383 ch
->state
= NG_L2CAP_W4_L2CA_CON_RSP
;
385 error
= ng_l2cap_l2ca_con_ind(ch
);
387 send_l2cap_con_rej(con
, ident
, ch
->scid
, dcid
,
388 (error
== ENOMEM
)? NG_L2CAP_NO_RESOURCES
:
389 NG_L2CAP_PSM_NOT_SUPPORTED
);
390 ng_l2cap_free_chan(ch
);
394 } /* ng_l2cap_process_con_req */
397 * Process L2CAP_ConnectRsp command
401 ng_l2cap_process_con_rsp(ng_l2cap_con_p con
, u_int8_t ident
)
403 ng_l2cap_p l2cap
= con
->l2cap
;
404 struct mbuf
*m
= con
->rx_pkt
;
405 ng_l2cap_con_rsp_cp
*cp
= NULL
;
406 ng_l2cap_cmd_p cmd
= NULL
;
407 u_int16_t scid
, dcid
, result
, status
;
410 /* Get command parameters */
411 NG_L2CAP_M_PULLUP(m
, sizeof(*cp
));
415 cp
= mtod(m
, ng_l2cap_con_rsp_cp
*);
416 dcid
= le16toh(cp
->dcid
);
417 scid
= le16toh(cp
->scid
);
418 result
= le16toh(cp
->result
);
419 status
= le16toh(cp
->status
);
424 /* Check if we have pending command descriptor */
425 cmd
= ng_l2cap_cmd_by_ident(con
, ident
);
428 "%s: %s - unexpected L2CAP_ConnectRsp command. ident=%d, con_handle=%d\n",
429 __func__
, NG_NODE_NAME(l2cap
->node
), ident
,
435 /* Verify channel state, if invalid - do nothing */
436 if (cmd
->ch
->state
!= NG_L2CAP_W4_L2CAP_CON_RSP
) {
438 "%s: %s - unexpected L2CAP_ConnectRsp. " \
439 "Invalid channel state, cid=%d, state=%d\n",
440 __func__
, NG_NODE_NAME(l2cap
->node
), scid
,
445 /* Verify CIDs and send reject if does not match */
446 if (cmd
->ch
->scid
!= scid
) {
448 "%s: %s - unexpected L2CAP_ConnectRsp. Channel IDs do not match, scid=%d(%d)\n",
449 __func__
, NG_NODE_NAME(l2cap
->node
), cmd
->ch
->scid
,
455 * Looks good. We got confirmation from our peer. Now process
456 * it. First disable RTX timer. Then check the result and send
457 * notification to the upper layer. If command timeout already
458 * happened then ignore response.
461 if ((error
= ng_l2cap_command_untimeout(cmd
)) != 0)
464 if (result
== NG_L2CAP_PENDING
) {
466 * Our peer wants more time to complete connection. We shall
467 * start ERTX timer and wait. Keep command in the list.
470 cmd
->ch
->dcid
= dcid
;
471 ng_l2cap_command_timeout(cmd
, bluetooth_l2cap_ertx_timeout());
473 error
= ng_l2cap_l2ca_con_rsp(cmd
->ch
, cmd
->token
,
476 ng_l2cap_free_chan(cmd
->ch
);
478 ng_l2cap_unlink_cmd(cmd
);
480 if (result
== NG_L2CAP_SUCCESS
) {
482 * Channel is open. Complete command and move to CONFIG
483 * state. Since we have sent positive confirmation we
484 * expect to receive L2CA_Config request from the upper
488 cmd
->ch
->dcid
= dcid
;
489 cmd
->ch
->state
= NG_L2CAP_CONFIG
;
491 /* There was an error, so close the channel */
493 "%s: %s - failed to open L2CAP channel, result=%d, status=%d\n",
494 __func__
, NG_NODE_NAME(l2cap
->node
), result
,
497 error
= ng_l2cap_l2ca_con_rsp(cmd
->ch
, cmd
->token
,
500 /* XXX do we have to remove the channel on error? */
501 if (error
!= 0 || result
!= NG_L2CAP_SUCCESS
)
502 ng_l2cap_free_chan(cmd
->ch
);
504 ng_l2cap_free_cmd(cmd
);
510 /* Send reject. Do not really care about the result */
511 send_l2cap_reject(con
, ident
, NG_L2CAP_REJ_INVALID_CID
, 0, scid
, dcid
);
514 } /* ng_l2cap_process_con_rsp */
517 * Process L2CAP_ConfigReq command
521 ng_l2cap_process_cfg_req(ng_l2cap_con_p con
, u_int8_t ident
)
523 ng_l2cap_p l2cap
= con
->l2cap
;
524 struct mbuf
*m
= con
->rx_pkt
;
525 ng_l2cap_cfg_req_cp
*cp
= NULL
;
526 ng_l2cap_chan_p ch
= NULL
;
527 u_int16_t dcid
, respond
, result
;
528 ng_l2cap_cfg_opt_t hdr
;
529 ng_l2cap_cfg_opt_val_t val
;
532 /* Get command parameters */
534 NG_L2CAP_M_PULLUP(m
, sizeof(*cp
));
538 cp
= mtod(m
, ng_l2cap_cfg_req_cp
*);
539 dcid
= le16toh(cp
->dcid
);
540 respond
= NG_L2CAP_OPT_CFLAG(le16toh(cp
->flags
));
541 m_adj(m
, sizeof(*cp
));
543 /* Check if we have this channel and it is in valid state */
544 ch
= ng_l2cap_chan_by_scid(l2cap
, dcid
);
547 "%s: %s - unexpected L2CAP_ConfigReq command. " \
548 "Channel does not exist, cid=%d\n",
549 __func__
, NG_NODE_NAME(l2cap
->node
), dcid
);
553 /* Verify channel state */
554 if (ch
->state
!= NG_L2CAP_CONFIG
&& ch
->state
!= NG_L2CAP_OPEN
) {
556 "%s: %s - unexpected L2CAP_ConfigReq. " \
557 "Invalid channel state, cid=%d, state=%d\n",
558 __func__
, NG_NODE_NAME(l2cap
->node
), dcid
, ch
->state
);
562 if (ch
->state
== NG_L2CAP_OPEN
) { /* Re-configuration */
564 ch
->state
= NG_L2CAP_CONFIG
;
567 for (result
= 0, off
= 0; ; ) {
568 error
= get_next_l2cap_opt(m
, &off
, &hdr
, &val
);
569 if (error
== 0) { /* We done with this packet */
572 } else if (error
> 0) { /* Got option */
574 case NG_L2CAP_OPT_MTU
:
578 case NG_L2CAP_OPT_FLUSH_TIMO
:
579 ch
->flush_timo
= val
.flush_timo
;
582 case NG_L2CAP_OPT_QOS
:
583 bcopy(&val
.flow
, &ch
->iflow
, sizeof(ch
->iflow
));
586 default: /* Ignore unknown hint option */
589 } else { /* Oops, something is wrong */
595 * Adjust mbuf so we can get to the start
596 * of the first option we did not like.
599 m_adj(m
, off
- sizeof(hdr
));
600 m
->m_pkthdr
.len
= sizeof(hdr
) + hdr
.length
;
602 result
= NG_L2CAP_UNKNOWN_OPTION
;
604 /* XXX FIXME Send other reject codes? */
606 result
= NG_L2CAP_REJECT
;
614 * Now check and see if we have to respond. If everything was OK then
615 * respond contain "C flag" and (if set) we will respond with empty
616 * packet and will wait for more options.
618 * Other case is that we did not like peer's options and will respond
619 * with L2CAP_Config response command with Reject error code.
621 * When "respond == 0" than we have received all options and we will
622 * sent L2CA_ConfigInd event to the upper layer protocol.
626 error
= send_l2cap_cfg_rsp(con
, ident
, ch
->dcid
, result
, m
);
628 ng_l2cap_l2ca_discon_ind(ch
);
629 ng_l2cap_free_chan(ch
);
632 /* Send L2CA_ConfigInd event to the upper layer protocol */
634 error
= ng_l2cap_l2ca_cfg_ind(ch
);
636 ng_l2cap_free_chan(ch
);
642 /* Send reject. Do not really care about the result */
645 send_l2cap_reject(con
, ident
, NG_L2CAP_REJ_INVALID_CID
, 0, 0, dcid
);
648 } /* ng_l2cap_process_cfg_req */
651 * Process L2CAP_ConfigRsp command
655 ng_l2cap_process_cfg_rsp(ng_l2cap_con_p con
, u_int8_t ident
)
657 ng_l2cap_p l2cap
= con
->l2cap
;
658 struct mbuf
*m
= con
->rx_pkt
;
659 ng_l2cap_cfg_rsp_cp
*cp
= NULL
;
660 ng_l2cap_cmd_p cmd
= NULL
;
661 u_int16_t scid
, cflag
, result
;
662 ng_l2cap_cfg_opt_t hdr
;
663 ng_l2cap_cfg_opt_val_t val
;
666 /* Get command parameters */
668 NG_L2CAP_M_PULLUP(m
, sizeof(*cp
));
672 cp
= mtod(m
, ng_l2cap_cfg_rsp_cp
*);
673 scid
= le16toh(cp
->scid
);
674 cflag
= NG_L2CAP_OPT_CFLAG(le16toh(cp
->flags
));
675 result
= le16toh(cp
->result
);
676 m_adj(m
, sizeof(*cp
));
678 /* Check if we have this command */
679 cmd
= ng_l2cap_cmd_by_ident(con
, ident
);
682 "%s: %s - unexpected L2CAP_ConfigRsp command. ident=%d, con_handle=%d\n",
683 __func__
, NG_NODE_NAME(l2cap
->node
), ident
,
690 /* Verify CIDs and send reject if does not match */
691 if (cmd
->ch
->scid
!= scid
) {
693 "%s: %s - unexpected L2CAP_ConfigRsp. " \
694 "Channel ID does not match, scid=%d(%d)\n",
695 __func__
, NG_NODE_NAME(l2cap
->node
), cmd
->ch
->scid
,
700 /* Verify channel state and reject if invalid */
701 if (cmd
->ch
->state
!= NG_L2CAP_CONFIG
) {
703 "%s: %s - unexpected L2CAP_ConfigRsp. " \
704 "Invalid channel state, scid=%d, state=%d\n",
705 __func__
, NG_NODE_NAME(l2cap
->node
), cmd
->ch
->scid
,
711 * Looks like it is our response, so process it. First parse options,
712 * then verify C flag. If it is set then we shall expect more
713 * configuration options from the peer and we will wait. Otherwise we
714 * have received all options and we will send L2CA_ConfigRsp event to
715 * the upper layer protocol. If command timeout already happened then
719 if ((error
= ng_l2cap_command_untimeout(cmd
)) != 0) {
725 error
= get_next_l2cap_opt(m
, &off
, &hdr
, &val
);
726 if (error
== 0) /* We done with this packet */
728 else if (error
> 0) { /* Got option */
730 case NG_L2CAP_OPT_MTU
:
731 cmd
->ch
->imtu
= val
.mtu
;
734 case NG_L2CAP_OPT_FLUSH_TIMO
:
735 cmd
->ch
->flush_timo
= val
.flush_timo
;
738 case NG_L2CAP_OPT_QOS
:
739 bcopy(&val
.flow
, &cmd
->ch
->oflow
,
740 sizeof(cmd
->ch
->oflow
));
743 default: /* Ignore unknown hint option */
748 * XXX FIXME What to do here?
750 * This is really BAD :( options packet was broken, or
751 * peer sent us option that we did not understand. Let
752 * upper layer know and do not wait for more options.
756 "%s: %s - failed to parse configuration options, error=%d\n",
757 __func__
, NG_NODE_NAME(l2cap
->node
), error
);
759 result
= NG_L2CAP_UNKNOWN
;
768 if (cflag
) /* Restart timer and wait for more options */
769 ng_l2cap_command_timeout(cmd
, bluetooth_l2cap_rtx_timeout());
771 ng_l2cap_unlink_cmd(cmd
);
773 /* Send L2CA_Config response to the upper layer protocol */
774 error
= ng_l2cap_l2ca_cfg_rsp(cmd
->ch
, cmd
->token
, result
);
777 * XXX FIXME what to do here? we were not able to send
778 * response to the upper layer protocol, so for now
779 * just close the channel. Send L2CAP_Disconnect to
784 "%s: %s - failed to send L2CA_Config response, error=%d\n",
785 __func__
, NG_NODE_NAME(l2cap
->node
), error
);
787 ng_l2cap_free_chan(cmd
->ch
);
790 ng_l2cap_free_cmd(cmd
);
796 /* Send reject. Do not really care about the result */
799 send_l2cap_reject(con
, ident
, NG_L2CAP_REJ_INVALID_CID
, 0, scid
, 0);
802 } /* ng_l2cap_process_cfg_rsp */
805 * Process L2CAP_DisconnectReq command
809 ng_l2cap_process_discon_req(ng_l2cap_con_p con
, u_int8_t ident
)
811 ng_l2cap_p l2cap
= con
->l2cap
;
812 ng_l2cap_discon_req_cp
*cp
= NULL
;
813 ng_l2cap_chan_p ch
= NULL
;
814 ng_l2cap_cmd_p cmd
= NULL
;
815 u_int16_t scid
, dcid
;
817 /* Get command parameters */
818 NG_L2CAP_M_PULLUP(con
->rx_pkt
, sizeof(*cp
));
819 if (con
->rx_pkt
== NULL
)
822 cp
= mtod(con
->rx_pkt
, ng_l2cap_discon_req_cp
*);
823 dcid
= le16toh(cp
->dcid
);
824 scid
= le16toh(cp
->scid
);
826 NG_FREE_M(con
->rx_pkt
);
828 /* Check if we have this channel and it is in valid state */
829 ch
= ng_l2cap_chan_by_scid(l2cap
, dcid
);
832 "%s: %s - unexpected L2CAP_DisconnectReq message. " \
833 "Channel does not exist, cid=%d\n",
834 __func__
, NG_NODE_NAME(l2cap
->node
), dcid
);
838 /* XXX Verify channel state and reject if invalid -- is that true? */
839 if (ch
->state
!= NG_L2CAP_OPEN
&& ch
->state
!= NG_L2CAP_CONFIG
&&
840 ch
->state
!= NG_L2CAP_W4_L2CAP_DISCON_RSP
) {
842 "%s: %s - unexpected L2CAP_DisconnectReq. " \
843 "Invalid channel state, cid=%d, state=%d\n",
844 __func__
, NG_NODE_NAME(l2cap
->node
), dcid
, ch
->state
);
848 /* Match destination channel ID */
849 if (ch
->dcid
!= scid
|| ch
->scid
!= dcid
) {
851 "%s: %s - unexpected L2CAP_DisconnectReq. " \
852 "Channel IDs does not match, channel: scid=%d, dcid=%d, " \
853 "request: scid=%d, dcid=%d\n",
854 __func__
, NG_NODE_NAME(l2cap
->node
), ch
->scid
, ch
->dcid
,
860 * Looks good, so notify upper layer protocol that channel is about
861 * to be disconnected and send L2CA_DisconnectInd message. Then respond
862 * with L2CAP_DisconnectRsp.
865 if (ch
->state
!= NG_L2CAP_W4_L2CAP_DISCON_RSP
) {
866 ng_l2cap_l2ca_discon_ind(ch
); /* do not care about result */
867 ng_l2cap_free_chan(ch
);
870 /* Send L2CAP_DisconnectRsp */
871 cmd
= ng_l2cap_new_cmd(con
, NULL
, ident
, NG_L2CAP_DISCON_RSP
, 0);
875 _ng_l2cap_discon_rsp(cmd
->aux
, ident
, dcid
, scid
);
876 if (cmd
->aux
== NULL
) {
877 ng_l2cap_free_cmd(cmd
);
882 /* Link command to the queue */
883 ng_l2cap_link_cmd(con
, cmd
);
884 ng_l2cap_lp_deliver(con
);
889 /* Send reject. Do not really care about the result */
890 send_l2cap_reject(con
, ident
, NG_L2CAP_REJ_INVALID_CID
, 0, scid
, dcid
);
893 } /* ng_l2cap_process_discon_req */
896 * Process L2CAP_DisconnectRsp command
900 ng_l2cap_process_discon_rsp(ng_l2cap_con_p con
, u_int8_t ident
)
902 ng_l2cap_p l2cap
= con
->l2cap
;
903 ng_l2cap_discon_rsp_cp
*cp
= NULL
;
904 ng_l2cap_cmd_p cmd
= NULL
;
905 u_int16_t scid
, dcid
;
908 /* Get command parameters */
909 NG_L2CAP_M_PULLUP(con
->rx_pkt
, sizeof(*cp
));
910 if (con
->rx_pkt
== NULL
)
913 cp
= mtod(con
->rx_pkt
, ng_l2cap_discon_rsp_cp
*);
914 dcid
= le16toh(cp
->dcid
);
915 scid
= le16toh(cp
->scid
);
917 NG_FREE_M(con
->rx_pkt
);
919 /* Check if we have pending command descriptor */
920 cmd
= ng_l2cap_cmd_by_ident(con
, ident
);
923 "%s: %s - unexpected L2CAP_DisconnectRsp command. ident=%d, con_handle=%d\n",
924 __func__
, NG_NODE_NAME(l2cap
->node
), ident
,
929 /* Verify channel state, do nothing if invalid */
930 if (cmd
->ch
->state
!= NG_L2CAP_W4_L2CAP_DISCON_RSP
) {
932 "%s: %s - unexpected L2CAP_DisconnectRsp. " \
933 "Invalid channel state, cid=%d, state=%d\n",
934 __func__
, NG_NODE_NAME(l2cap
->node
), scid
,
939 /* Verify CIDs and send reject if does not match */
940 if (cmd
->ch
->scid
!= scid
|| cmd
->ch
->dcid
!= dcid
) {
942 "%s: %s - unexpected L2CAP_DisconnectRsp. " \
943 "Channel IDs do not match, scid=%d(%d), dcid=%d(%d)\n",
944 __func__
, NG_NODE_NAME(l2cap
->node
), cmd
->ch
->scid
,
945 scid
, cmd
->ch
->dcid
, dcid
);
950 * Looks like we have successfuly disconnected channel, so notify
951 * upper layer. If command timeout already happened then ignore
955 if ((error
= ng_l2cap_command_untimeout(cmd
)) != 0)
958 error
= ng_l2cap_l2ca_discon_rsp(cmd
->ch
, cmd
->token
, NG_L2CAP_SUCCESS
);
959 ng_l2cap_free_chan(cmd
->ch
); /* this will free commands too */
962 } /* ng_l2cap_process_discon_rsp */
965 * Process L2CAP_EchoReq command
969 ng_l2cap_process_echo_req(ng_l2cap_con_p con
, u_int8_t ident
)
971 ng_l2cap_p l2cap
= con
->l2cap
;
972 ng_l2cap_cmd_hdr_t
*hdr
= NULL
;
973 ng_l2cap_cmd_p cmd
= NULL
;
975 con
->rx_pkt
= ng_l2cap_prepend(con
->rx_pkt
, sizeof(*hdr
));
976 if (con
->rx_pkt
== NULL
) {
978 "%s: %s - ng_l2cap_prepend() failed, size=%zd\n",
979 __func__
, NG_NODE_NAME(l2cap
->node
), sizeof(*hdr
));
984 hdr
= mtod(con
->rx_pkt
, ng_l2cap_cmd_hdr_t
*);
985 hdr
->code
= NG_L2CAP_ECHO_RSP
;
987 hdr
->length
= htole16(con
->rx_pkt
->m_pkthdr
.len
- sizeof(*hdr
));
989 cmd
= ng_l2cap_new_cmd(con
, NULL
, ident
, NG_L2CAP_ECHO_RSP
, 0);
991 NG_FREE_M(con
->rx_pkt
);
996 /* Attach data and link command to the queue */
997 cmd
->aux
= con
->rx_pkt
;
999 ng_l2cap_link_cmd(con
, cmd
);
1000 ng_l2cap_lp_deliver(con
);
1003 } /* ng_l2cap_process_echo_req */
1006 * Process L2CAP_EchoRsp command
1010 ng_l2cap_process_echo_rsp(ng_l2cap_con_p con
, u_int8_t ident
)
1012 ng_l2cap_p l2cap
= con
->l2cap
;
1013 ng_l2cap_cmd_p cmd
= NULL
;
1016 /* Check if we have this command */
1017 cmd
= ng_l2cap_cmd_by_ident(con
, ident
);
1019 /* If command timeout already happened then ignore response */
1020 if ((error
= ng_l2cap_command_untimeout(cmd
)) != 0) {
1021 NG_FREE_M(con
->rx_pkt
);
1025 ng_l2cap_unlink_cmd(cmd
);
1027 error
= ng_l2cap_l2ca_ping_rsp(cmd
->con
, cmd
->token
,
1028 NG_L2CAP_SUCCESS
, con
->rx_pkt
);
1030 ng_l2cap_free_cmd(cmd
);
1034 "%s: %s - unexpected L2CAP_EchoRsp command. " \
1035 "Requested ident does not exist, ident=%d\n",
1036 __func__
, NG_NODE_NAME(l2cap
->node
), ident
);
1037 NG_FREE_M(con
->rx_pkt
);
1041 } /* ng_l2cap_process_echo_rsp */
1044 * Process L2CAP_InfoReq command
1048 ng_l2cap_process_info_req(ng_l2cap_con_p con
, u_int8_t ident
)
1050 ng_l2cap_p l2cap
= con
->l2cap
;
1051 ng_l2cap_cmd_p cmd
= NULL
;
1054 /* Get command parameters */
1055 NG_L2CAP_M_PULLUP(con
->rx_pkt
, sizeof(ng_l2cap_info_req_cp
));
1056 if (con
->rx_pkt
== NULL
)
1059 type
= le16toh(mtod(con
->rx_pkt
, ng_l2cap_info_req_cp
*)->type
);
1060 NG_FREE_M(con
->rx_pkt
);
1062 cmd
= ng_l2cap_new_cmd(con
, NULL
, ident
, NG_L2CAP_INFO_RSP
, 0);
1067 case NG_L2CAP_CONNLESS_MTU
:
1068 _ng_l2cap_info_rsp(cmd
->aux
, ident
, NG_L2CAP_CONNLESS_MTU
,
1069 NG_L2CAP_SUCCESS
, NG_L2CAP_MTU_DEFAULT
);
1073 _ng_l2cap_info_rsp(cmd
->aux
, ident
, type
,
1074 NG_L2CAP_NOT_SUPPORTED
, 0);
1078 if (cmd
->aux
== NULL
) {
1079 ng_l2cap_free_cmd(cmd
);
1084 /* Link command to the queue */
1085 ng_l2cap_link_cmd(con
, cmd
);
1086 ng_l2cap_lp_deliver(con
);
1089 } /* ng_l2cap_process_info_req */
1092 * Process L2CAP_InfoRsp command
1096 ng_l2cap_process_info_rsp(ng_l2cap_con_p con
, u_int8_t ident
)
1098 ng_l2cap_p l2cap
= con
->l2cap
;
1099 ng_l2cap_info_rsp_cp
*cp
= NULL
;
1100 ng_l2cap_cmd_p cmd
= NULL
;
1103 /* Get command parameters */
1104 NG_L2CAP_M_PULLUP(con
->rx_pkt
, sizeof(*cp
));
1105 if (con
->rx_pkt
== NULL
)
1108 cp
= mtod(con
->rx_pkt
, ng_l2cap_info_rsp_cp
*);
1109 cp
->type
= le16toh(cp
->type
);
1110 cp
->result
= le16toh(cp
->result
);
1111 m_adj(con
->rx_pkt
, sizeof(*cp
));
1113 /* Check if we have pending command descriptor */
1114 cmd
= ng_l2cap_cmd_by_ident(con
, ident
);
1117 "%s: %s - unexpected L2CAP_InfoRsp command. " \
1118 "Requested ident does not exist, ident=%d\n",
1119 __func__
, NG_NODE_NAME(l2cap
->node
), ident
);
1120 NG_FREE_M(con
->rx_pkt
);
1125 /* If command timeout already happened then ignore response */
1126 if ((error
= ng_l2cap_command_untimeout(cmd
)) != 0) {
1127 NG_FREE_M(con
->rx_pkt
);
1131 ng_l2cap_unlink_cmd(cmd
);
1133 if (cp
->result
== NG_L2CAP_SUCCESS
) {
1135 case NG_L2CAP_CONNLESS_MTU
:
1136 if (con
->rx_pkt
->m_pkthdr
.len
== sizeof(u_int16_t
))
1137 *mtod(con
->rx_pkt
, u_int16_t
*) =
1138 le16toh(*mtod(con
->rx_pkt
,u_int16_t
*));
1140 cp
->result
= NG_L2CAP_UNKNOWN
; /* XXX */
1143 "%s: %s - invalid L2CAP_InfoRsp command. " \
1144 "Bad connectionless MTU parameter, len=%d\n",
1145 __func__
, NG_NODE_NAME(l2cap
->node
),
1146 con
->rx_pkt
->m_pkthdr
.len
);
1152 "%s: %s - invalid L2CAP_InfoRsp command. Unknown info type=%d\n",
1153 __func__
, NG_NODE_NAME(l2cap
->node
), cp
->type
);
1158 error
= ng_l2cap_l2ca_get_info_rsp(cmd
->con
, cmd
->token
,
1159 cp
->result
, con
->rx_pkt
);
1161 ng_l2cap_free_cmd(cmd
);
1165 } /* ng_l2cap_process_info_rsp */
1172 send_l2cap_reject(ng_l2cap_con_p con
, u_int8_t ident
, u_int16_t reason
,
1173 u_int16_t mtu
, u_int16_t scid
, u_int16_t dcid
)
1175 ng_l2cap_cmd_p cmd
= NULL
;
1177 cmd
= ng_l2cap_new_cmd(con
, NULL
, ident
, NG_L2CAP_CMD_REJ
, 0);
1181 _ng_l2cap_cmd_rej(cmd
->aux
, cmd
->ident
, reason
, mtu
, scid
, dcid
);
1182 if (cmd
->aux
== NULL
) {
1183 ng_l2cap_free_cmd(cmd
);
1188 /* Link command to the queue */
1189 ng_l2cap_link_cmd(con
, cmd
);
1190 ng_l2cap_lp_deliver(con
);
1193 } /* send_l2cap_reject */
1196 * Send L2CAP connection reject
1200 send_l2cap_con_rej(ng_l2cap_con_p con
, u_int8_t ident
, u_int16_t scid
,
1201 u_int16_t dcid
, u_int16_t result
)
1203 ng_l2cap_cmd_p cmd
= NULL
;
1205 cmd
= ng_l2cap_new_cmd(con
, NULL
, ident
, NG_L2CAP_CON_RSP
, 0);
1209 _ng_l2cap_con_rsp(cmd
->aux
, cmd
->ident
, scid
, dcid
, result
, 0);
1210 if (cmd
->aux
== NULL
) {
1211 ng_l2cap_free_cmd(cmd
);
1216 /* Link command to the queue */
1217 ng_l2cap_link_cmd(con
, cmd
);
1218 ng_l2cap_lp_deliver(con
);
1221 } /* send_l2cap_con_rej */
1224 * Send L2CAP config response
1228 send_l2cap_cfg_rsp(ng_l2cap_con_p con
, u_int8_t ident
, u_int16_t scid
,
1229 u_int16_t result
, struct mbuf
*opt
)
1231 ng_l2cap_cmd_p cmd
= NULL
;
1233 cmd
= ng_l2cap_new_cmd(con
, NULL
, ident
, NG_L2CAP_CFG_RSP
, 0);
1240 _ng_l2cap_cfg_rsp(cmd
->aux
, cmd
->ident
, scid
, 0, result
, opt
);
1241 if (cmd
->aux
== NULL
) {
1242 ng_l2cap_free_cmd(cmd
);
1247 /* Link command to the queue */
1248 ng_l2cap_link_cmd(con
, cmd
);
1249 ng_l2cap_lp_deliver(con
);
1252 } /* send_l2cap_cfg_rsp */
1255 * Get next L2CAP configuration option
1259 * 1 we have got option
1260 * -1 header too short
1261 * -2 bad option value or length
1266 get_next_l2cap_opt(struct mbuf
*m
, int *off
, ng_l2cap_cfg_opt_p hdr
,
1267 ng_l2cap_cfg_opt_val_p val
)
1269 int hint
, len
= m
->m_pkthdr
.len
- (*off
);
1273 if (len
< 0 || len
< sizeof(*hdr
))
1276 m_copydata(m
, *off
, sizeof(*hdr
), (caddr_t
) hdr
);
1277 *off
+= sizeof(*hdr
);
1278 len
-= sizeof(*hdr
);
1280 hint
= NG_L2CAP_OPT_HINT(hdr
->type
);
1281 hdr
->type
&= NG_L2CAP_OPT_HINT_MASK
;
1283 switch (hdr
->type
) {
1284 case NG_L2CAP_OPT_MTU
:
1285 if (hdr
->length
!= NG_L2CAP_OPT_MTU_SIZE
|| len
< hdr
->length
)
1288 m_copydata(m
, *off
, NG_L2CAP_OPT_MTU_SIZE
, (caddr_t
) val
);
1289 val
->mtu
= le16toh(val
->mtu
);
1290 *off
+= NG_L2CAP_OPT_MTU_SIZE
;
1293 case NG_L2CAP_OPT_FLUSH_TIMO
:
1294 if (hdr
->length
!= NG_L2CAP_OPT_FLUSH_TIMO_SIZE
||
1298 m_copydata(m
, *off
, NG_L2CAP_OPT_FLUSH_TIMO_SIZE
, (caddr_t
)val
);
1299 val
->flush_timo
= le16toh(val
->flush_timo
);
1300 *off
+= NG_L2CAP_OPT_FLUSH_TIMO_SIZE
;
1303 case NG_L2CAP_OPT_QOS
:
1304 if (hdr
->length
!= NG_L2CAP_OPT_QOS_SIZE
|| len
< hdr
->length
)
1307 m_copydata(m
, *off
, NG_L2CAP_OPT_QOS_SIZE
, (caddr_t
) val
);
1308 val
->flow
.token_rate
= le32toh(val
->flow
.token_rate
);
1309 val
->flow
.token_bucket_size
=
1310 le32toh(val
->flow
.token_bucket_size
);
1311 val
->flow
.peak_bandwidth
= le32toh(val
->flow
.peak_bandwidth
);
1312 val
->flow
.latency
= le32toh(val
->flow
.latency
);
1313 val
->flow
.delay_variation
= le32toh(val
->flow
.delay_variation
);
1314 *off
+= NG_L2CAP_OPT_QOS_SIZE
;
1319 *off
+= hdr
->length
;
1326 } /* get_next_l2cap_opt */