6 * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
8 * Copyright (c) Maksim Yevmenkin <m_evmenkin@yahoo.com>
11 * Redistribution and use in source and binary forms, with or without
12 * modification, are permitted provided that the following conditions
14 * 1. Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * 2. Redistributions in binary form must reproduce the above copyright
17 * notice, this list of conditions and the following disclaimer in the
18 * documentation and/or other materials provided with the distribution.
20 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
21 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
23 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
24 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
26 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
27 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
29 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
32 * $Id: ng_l2cap_evnt.c,v 1.5 2003/09/08 19:11:45 max Exp $
36 #include <sys/param.h>
37 #include <sys/systm.h>
38 #include <sys/kernel.h>
39 #include <sys/endian.h>
40 #include <sys/malloc.h>
42 #include <sys/queue.h>
43 #include <netgraph/ng_message.h>
44 #include <netgraph/netgraph.h>
45 #include <netgraph/bluetooth/include/ng_bluetooth.h>
46 #include <netgraph/bluetooth/include/ng_hci.h>
47 #include <netgraph/bluetooth/include/ng_l2cap.h>
48 #include <netgraph/bluetooth/l2cap/ng_l2cap_var.h>
49 #include <netgraph/bluetooth/l2cap/ng_l2cap_cmds.h>
50 #include <netgraph/bluetooth/l2cap/ng_l2cap_evnt.h>
51 #include <netgraph/bluetooth/l2cap/ng_l2cap_llpi.h>
52 #include <netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h>
53 #include <netgraph/bluetooth/l2cap/ng_l2cap_misc.h>
55 /******************************************************************************
56 ******************************************************************************
57 ** L2CAP events processing module
58 ******************************************************************************
59 ******************************************************************************/
61 static int ng_l2cap_process_signal_cmd (ng_l2cap_con_p);
62 static int ng_l2cap_process_lesignal_cmd (ng_l2cap_con_p);
63 static int ng_l2cap_process_cmd_rej (ng_l2cap_con_p, u_int8_t);
64 static int ng_l2cap_process_cmd_urq (ng_l2cap_con_p, u_int8_t);
65 static int ng_l2cap_process_cmd_urs (ng_l2cap_con_p, u_int8_t);
66 static int ng_l2cap_process_con_req (ng_l2cap_con_p, u_int8_t);
67 static int ng_l2cap_process_con_rsp (ng_l2cap_con_p, u_int8_t);
68 static int ng_l2cap_process_cfg_req (ng_l2cap_con_p, u_int8_t);
69 static int ng_l2cap_process_cfg_rsp (ng_l2cap_con_p, u_int8_t);
70 static int ng_l2cap_process_discon_req (ng_l2cap_con_p, u_int8_t);
71 static int ng_l2cap_process_discon_rsp (ng_l2cap_con_p, u_int8_t);
72 static int ng_l2cap_process_echo_req (ng_l2cap_con_p, u_int8_t);
73 static int ng_l2cap_process_echo_rsp (ng_l2cap_con_p, u_int8_t);
74 static int ng_l2cap_process_info_req (ng_l2cap_con_p, u_int8_t);
75 static int ng_l2cap_process_info_rsp (ng_l2cap_con_p, u_int8_t);
76 static int send_l2cap_reject
77 (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, u_int16_t, u_int16_t);
78 static int send_l2cap_con_rej
79 (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, u_int16_t);
80 static int send_l2cap_cfg_rsp
81 (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, struct mbuf *);
82 static int send_l2cap_param_urs
83 (ng_l2cap_con_p , u_int8_t , u_int16_t);
85 static int get_next_l2cap_opt
86 (struct mbuf *, int *, ng_l2cap_cfg_opt_p, ng_l2cap_cfg_opt_val_p);
89 * Receive L2CAP packet. First get L2CAP header and verify packet. Than
90 * get destination channel and process packet.
94 ng_l2cap_receive(ng_l2cap_con_p con)
96 ng_l2cap_p l2cap = con->l2cap;
97 ng_l2cap_hdr_t *hdr = NULL;
101 if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) {
103 "%s: %s - invalid L2CAP packet. Packet too small, len=%d\n",
104 __func__, NG_NODE_NAME(l2cap->node),
105 con->rx_pkt->m_pkthdr.len);
110 /* Get L2CAP header */
111 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr));
112 if (con->rx_pkt == NULL)
115 hdr = mtod(con->rx_pkt, ng_l2cap_hdr_t *);
116 hdr->length = le16toh(hdr->length);
117 hdr->dcid = le16toh(hdr->dcid);
119 /* Check payload size */
120 if (hdr->length != con->rx_pkt->m_pkthdr.len - sizeof(*hdr)) {
122 "%s: %s - invalid L2CAP packet. Payload length mismatch, length=%d, len=%zd\n",
123 __func__, NG_NODE_NAME(l2cap->node), hdr->length,
124 con->rx_pkt->m_pkthdr.len - sizeof(*hdr));
131 case NG_L2CAP_SIGNAL_CID: /* L2CAP command */
132 m_adj(con->rx_pkt, sizeof(*hdr));
133 error = ng_l2cap_process_signal_cmd(con);
135 case NG_L2CAP_LESIGNAL_CID:
136 m_adj(con->rx_pkt, sizeof(*hdr));
137 error = ng_l2cap_process_lesignal_cmd(con);
139 case NG_L2CAP_CLT_CID: /* Connectionless packet */
140 error = ng_l2cap_l2ca_clt_receive(con);
143 default: /* Data packet */
144 error = ng_l2cap_l2ca_receive(con);
150 NG_FREE_M(con->rx_pkt);
153 } /* ng_l2cap_receive */
156 * Process L2CAP signaling command. We already know that destination channel ID
157 * is 0x1 that means we have received signaling command from peer's L2CAP layer.
158 * So get command header, decode and process it.
160 * XXX do we need to check signaling MTU here?
164 ng_l2cap_process_signal_cmd(ng_l2cap_con_p con)
166 ng_l2cap_p l2cap = con->l2cap;
167 ng_l2cap_cmd_hdr_t *hdr = NULL;
168 struct mbuf *m = NULL;
170 while (con->rx_pkt != NULL) {
171 /* Verify packet length */
172 if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) {
174 "%s: %s - invalid L2CAP signaling command. Packet too small, len=%d\n",
175 __func__, NG_NODE_NAME(l2cap->node),
176 con->rx_pkt->m_pkthdr.len);
177 NG_FREE_M(con->rx_pkt);
182 /* Get signaling command */
183 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr));
184 if (con->rx_pkt == NULL)
187 hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *);
188 hdr->length = le16toh(hdr->length);
189 m_adj(con->rx_pkt, sizeof(*hdr));
191 /* Verify command length */
192 if (con->rx_pkt->m_pkthdr.len < hdr->length) {
194 "%s: %s - invalid L2CAP signaling command, code=%#x, ident=%d. " \
195 "Invalid command length=%d, m_pkthdr.len=%d\n",
196 __func__, NG_NODE_NAME(l2cap->node),
197 hdr->code, hdr->ident, hdr->length,
198 con->rx_pkt->m_pkthdr.len);
199 NG_FREE_M(con->rx_pkt);
204 /* Get the command, save the rest (if any) */
205 if (con->rx_pkt->m_pkthdr.len > hdr->length)
206 m = m_split(con->rx_pkt, hdr->length, M_NOWAIT);
210 /* Process command */
212 case NG_L2CAP_CMD_REJ:
213 ng_l2cap_process_cmd_rej(con, hdr->ident);
216 case NG_L2CAP_CON_REQ:
217 ng_l2cap_process_con_req(con, hdr->ident);
220 case NG_L2CAP_CON_RSP:
221 ng_l2cap_process_con_rsp(con, hdr->ident);
224 case NG_L2CAP_CFG_REQ:
225 ng_l2cap_process_cfg_req(con, hdr->ident);
228 case NG_L2CAP_CFG_RSP:
229 ng_l2cap_process_cfg_rsp(con, hdr->ident);
232 case NG_L2CAP_DISCON_REQ:
233 ng_l2cap_process_discon_req(con, hdr->ident);
236 case NG_L2CAP_DISCON_RSP:
237 ng_l2cap_process_discon_rsp(con, hdr->ident);
240 case NG_L2CAP_ECHO_REQ:
241 ng_l2cap_process_echo_req(con, hdr->ident);
244 case NG_L2CAP_ECHO_RSP:
245 ng_l2cap_process_echo_rsp(con, hdr->ident);
248 case NG_L2CAP_INFO_REQ:
249 ng_l2cap_process_info_req(con, hdr->ident);
252 case NG_L2CAP_INFO_RSP:
253 ng_l2cap_process_info_rsp(con, hdr->ident);
258 "%s: %s - unknown L2CAP signaling command, code=%#x, ident=%d, length=%d\n",
259 __func__, NG_NODE_NAME(l2cap->node),
260 hdr->code, hdr->ident, hdr->length);
263 * Send L2CAP_CommandRej. Do not really care
267 send_l2cap_reject(con, hdr->ident,
268 NG_L2CAP_REJ_NOT_UNDERSTOOD, 0, 0, 0);
269 NG_FREE_M(con->rx_pkt);
277 } /* ng_l2cap_process_signal_cmd */
279 ng_l2cap_process_lesignal_cmd(ng_l2cap_con_p con)
281 ng_l2cap_p l2cap = con->l2cap;
282 ng_l2cap_cmd_hdr_t *hdr = NULL;
283 struct mbuf *m = NULL;
285 while (con->rx_pkt != NULL) {
286 /* Verify packet length */
287 if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) {
289 "%s: %s - invalid L2CAP signaling command. Packet too small, len=%d\n",
290 __func__, NG_NODE_NAME(l2cap->node),
291 con->rx_pkt->m_pkthdr.len);
292 NG_FREE_M(con->rx_pkt);
297 /* Get signaling command */
298 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr));
299 if (con->rx_pkt == NULL)
302 hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *);
303 hdr->length = le16toh(hdr->length);
304 m_adj(con->rx_pkt, sizeof(*hdr));
306 /* Verify command length */
307 if (con->rx_pkt->m_pkthdr.len < hdr->length) {
309 "%s: %s - invalid L2CAP signaling command, code=%#x, ident=%d. " \
310 "Invalid command length=%d, m_pkthdr.len=%d\n",
311 __func__, NG_NODE_NAME(l2cap->node),
312 hdr->code, hdr->ident, hdr->length,
313 con->rx_pkt->m_pkthdr.len);
314 NG_FREE_M(con->rx_pkt);
319 /* Get the command, save the rest (if any) */
320 if (con->rx_pkt->m_pkthdr.len > hdr->length)
321 m = m_split(con->rx_pkt, hdr->length, M_NOWAIT);
325 /* Process command */
327 case NG_L2CAP_CMD_REJ:
328 ng_l2cap_process_cmd_rej(con, hdr->ident);
330 case NG_L2CAP_CMD_PARAM_UPDATE_REQUEST:
331 ng_l2cap_process_cmd_urq(con, hdr->ident);
333 case NG_L2CAP_CMD_PARAM_UPDATE_RESPONSE:
334 ng_l2cap_process_cmd_urs(con, hdr->ident);
340 "%s: %s - unknown L2CAP signaling command, code=%#x, ident=%d, length=%d\n",
341 __func__, NG_NODE_NAME(l2cap->node),
342 hdr->code, hdr->ident, hdr->length);
345 * Send L2CAP_CommandRej. Do not really care
349 send_l2cap_reject(con, hdr->ident,
350 NG_L2CAP_REJ_NOT_UNDERSTOOD, 0, 0, 0);
351 NG_FREE_M(con->rx_pkt);
359 } /* ng_l2cap_process_signal_cmd */
360 /*Update Paramater Request*/
361 static int ng_l2cap_process_cmd_urq(ng_l2cap_con_p con, uint8_t ident)
363 /* We do not implement parameter negotiation for now. */
364 send_l2cap_param_urs(con, ident, NG_L2CAP_UPDATE_PARAM_ACCEPT);
365 NG_FREE_M(con->rx_pkt);
369 static int ng_l2cap_process_cmd_urs(ng_l2cap_con_p con, uint8_t ident)
371 /* We only support master side yet .*/
372 //send_l2cap_reject(con,ident ... );
374 NG_FREE_M(con->rx_pkt);
379 * Process L2CAP_CommandRej command
383 ng_l2cap_process_cmd_rej(ng_l2cap_con_p con, u_int8_t ident)
385 ng_l2cap_p l2cap = con->l2cap;
386 ng_l2cap_cmd_rej_cp *cp = NULL;
387 ng_l2cap_cmd_p cmd = NULL;
389 /* Get command parameters */
390 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp));
391 if (con->rx_pkt == NULL)
394 cp = mtod(con->rx_pkt, ng_l2cap_cmd_rej_cp *);
395 cp->reason = le16toh(cp->reason);
397 /* Check if we have pending command descriptor */
398 cmd = ng_l2cap_cmd_by_ident(con, ident);
400 /* If command timeout already happened then ignore reject */
401 if (ng_l2cap_command_untimeout(cmd) != 0) {
402 NG_FREE_M(con->rx_pkt);
406 ng_l2cap_unlink_cmd(cmd);
409 case NG_L2CAP_CON_REQ:
410 ng_l2cap_l2ca_con_rsp(cmd->ch,cmd->token,cp->reason,0);
411 ng_l2cap_free_chan(cmd->ch);
414 case NG_L2CAP_CFG_REQ:
415 ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, cp->reason);
418 case NG_L2CAP_DISCON_REQ:
419 ng_l2cap_l2ca_discon_rsp(cmd->ch,cmd->token,cp->reason);
420 ng_l2cap_free_chan(cmd->ch); /* XXX free channel */
423 case NG_L2CAP_ECHO_REQ:
424 ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token,
428 case NG_L2CAP_INFO_REQ:
429 ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token,
435 "%s: %s - unexpected L2CAP_CommandRej. Unexpected L2CAP command opcode=%d\n",
436 __func__, NG_NODE_NAME(l2cap->node), cmd->code);
440 ng_l2cap_free_cmd(cmd);
443 "%s: %s - unexpected L2CAP_CommandRej command. " \
444 "Requested ident does not exist, ident=%d\n",
445 __func__, NG_NODE_NAME(l2cap->node), ident);
447 NG_FREE_M(con->rx_pkt);
450 } /* ng_l2cap_process_cmd_rej */
453 * Process L2CAP_ConnectReq command
457 ng_l2cap_process_con_req(ng_l2cap_con_p con, u_int8_t ident)
459 ng_l2cap_p l2cap = con->l2cap;
460 struct mbuf *m = con->rx_pkt;
461 ng_l2cap_con_req_cp *cp = NULL;
462 ng_l2cap_chan_p ch = NULL;
467 /* Get command parameters */
468 NG_L2CAP_M_PULLUP(m, sizeof(*cp));
472 cp = mtod(m, ng_l2cap_con_req_cp *);
473 psm = le16toh(cp->psm);
474 dcid = le16toh(cp->scid);
478 if(dcid == NG_L2CAP_ATT_CID)
479 idtype = NG_L2CAP_L2CA_IDTYPE_ATT;
480 else if(dcid == NG_L2CAP_SMP_CID)
481 idtype = NG_L2CAP_L2CA_IDTYPE_SMP;
482 else if( con->linktype != NG_HCI_LINK_ACL)
483 idtype = NG_L2CAP_L2CA_IDTYPE_LE;
485 idtype = NG_L2CAP_L2CA_IDTYPE_BREDR;
488 * Create new channel and send L2CA_ConnectInd notification
489 * to the upper layer protocol.
492 ch = ng_l2cap_new_chan(l2cap, con, psm, idtype);
495 return (send_l2cap_con_rej(con, ident, 0, dcid,
496 NG_L2CAP_NO_RESOURCES));
498 /* Update channel IDs */
501 /* Sent L2CA_ConnectInd notification to the upper layer */
503 ch->state = NG_L2CAP_W4_L2CA_CON_RSP;
505 error = ng_l2cap_l2ca_con_ind(ch);
507 send_l2cap_con_rej(con, ident, ch->scid, dcid,
508 (error == ENOMEM)? NG_L2CAP_NO_RESOURCES :
509 NG_L2CAP_PSM_NOT_SUPPORTED);
510 ng_l2cap_free_chan(ch);
514 } /* ng_l2cap_process_con_req */
517 * Process L2CAP_ConnectRsp command
521 ng_l2cap_process_con_rsp(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_con_rsp_cp *cp = NULL;
526 ng_l2cap_cmd_p cmd = NULL;
527 u_int16_t scid, dcid, result, status;
530 /* Get command parameters */
531 NG_L2CAP_M_PULLUP(m, sizeof(*cp));
535 cp = mtod(m, ng_l2cap_con_rsp_cp *);
536 dcid = le16toh(cp->dcid);
537 scid = le16toh(cp->scid);
538 result = le16toh(cp->result);
539 status = le16toh(cp->status);
544 /* Check if we have pending command descriptor */
545 cmd = ng_l2cap_cmd_by_ident(con, ident);
548 "%s: %s - unexpected L2CAP_ConnectRsp command. ident=%d, con_handle=%d\n",
549 __func__, NG_NODE_NAME(l2cap->node), ident,
555 /* Verify channel state, if invalid - do nothing */
556 if (cmd->ch->state != NG_L2CAP_W4_L2CAP_CON_RSP) {
558 "%s: %s - unexpected L2CAP_ConnectRsp. " \
559 "Invalid channel state, cid=%d, state=%d\n",
560 __func__, NG_NODE_NAME(l2cap->node), scid,
565 /* Verify CIDs and send reject if does not match */
566 if (cmd->ch->scid != scid) {
568 "%s: %s - unexpected L2CAP_ConnectRsp. Channel IDs do not match, scid=%d(%d)\n",
569 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid,
575 * Looks good. We got confirmation from our peer. Now process
576 * it. First disable RTX timer. Then check the result and send
577 * notification to the upper layer. If command timeout already
578 * happened then ignore response.
581 if ((error = ng_l2cap_command_untimeout(cmd)) != 0)
584 if (result == NG_L2CAP_PENDING) {
586 * Our peer wants more time to complete connection. We shall
587 * start ERTX timer and wait. Keep command in the list.
590 cmd->ch->dcid = dcid;
591 ng_l2cap_command_timeout(cmd, bluetooth_l2cap_ertx_timeout());
593 error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token,
596 ng_l2cap_free_chan(cmd->ch);
598 ng_l2cap_unlink_cmd(cmd);
600 if (result == NG_L2CAP_SUCCESS) {
602 * Channel is open. Complete command and move to CONFIG
603 * state. Since we have sent positive confirmation we
604 * expect to receive L2CA_Config request from the upper
608 cmd->ch->dcid = dcid;
609 cmd->ch->state = ((cmd->ch->scid == NG_L2CAP_ATT_CID)||
610 (cmd->ch->scid == NG_L2CAP_SMP_CID))
612 NG_L2CAP_OPEN : NG_L2CAP_CONFIG;
614 /* There was an error, so close the channel */
616 "%s: %s - failed to open L2CAP channel, result=%d, status=%d\n",
617 __func__, NG_NODE_NAME(l2cap->node), result,
620 error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token,
623 /* XXX do we have to remove the channel on error? */
624 if (error != 0 || result != NG_L2CAP_SUCCESS)
625 ng_l2cap_free_chan(cmd->ch);
627 ng_l2cap_free_cmd(cmd);
633 /* Send reject. Do not really care about the result */
634 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid);
637 } /* ng_l2cap_process_con_rsp */
640 * Process L2CAP_ConfigReq command
644 ng_l2cap_process_cfg_req(ng_l2cap_con_p con, u_int8_t ident)
646 ng_l2cap_p l2cap = con->l2cap;
647 struct mbuf *m = con->rx_pkt;
648 ng_l2cap_cfg_req_cp *cp = NULL;
649 ng_l2cap_chan_p ch = NULL;
650 u_int16_t dcid, respond, result;
651 ng_l2cap_cfg_opt_t hdr;
652 ng_l2cap_cfg_opt_val_t val;
655 /* Get command parameters */
657 NG_L2CAP_M_PULLUP(m, sizeof(*cp));
661 cp = mtod(m, ng_l2cap_cfg_req_cp *);
662 dcid = le16toh(cp->dcid);
663 respond = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags));
664 m_adj(m, sizeof(*cp));
666 /* Check if we have this channel and it is in valid state */
667 ch = ng_l2cap_chan_by_scid(l2cap, dcid, NG_L2CAP_L2CA_IDTYPE_BREDR);
670 "%s: %s - unexpected L2CAP_ConfigReq command. " \
671 "Channel does not exist, cid=%d\n",
672 __func__, NG_NODE_NAME(l2cap->node), dcid);
676 /* Verify channel state */
677 if (ch->state != NG_L2CAP_CONFIG && ch->state != NG_L2CAP_OPEN) {
679 "%s: %s - unexpected L2CAP_ConfigReq. " \
680 "Invalid channel state, cid=%d, state=%d\n",
681 __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state);
685 if (ch->state == NG_L2CAP_OPEN) { /* Re-configuration */
687 ch->state = NG_L2CAP_CONFIG;
690 for (result = 0, off = 0; ; ) {
691 error = get_next_l2cap_opt(m, &off, &hdr, &val);
692 if (error == 0) { /* We done with this packet */
695 } else if (error > 0) { /* Got option */
697 case NG_L2CAP_OPT_MTU:
701 case NG_L2CAP_OPT_FLUSH_TIMO:
702 ch->flush_timo = val.flush_timo;
705 case NG_L2CAP_OPT_QOS:
706 bcopy(&val.flow, &ch->iflow, sizeof(ch->iflow));
709 default: /* Ignore unknown hint option */
712 } else { /* Oops, something is wrong */
718 * Adjust mbuf so we can get to the start
719 * of the first option we did not like.
722 m_adj(m, off - sizeof(hdr));
723 m->m_pkthdr.len = sizeof(hdr) + hdr.length;
725 result = NG_L2CAP_UNKNOWN_OPTION;
727 /* XXX FIXME Send other reject codes? */
729 result = NG_L2CAP_REJECT;
737 * Now check and see if we have to respond. If everything was OK then
738 * respond contain "C flag" and (if set) we will respond with empty
739 * packet and will wait for more options.
741 * Other case is that we did not like peer's options and will respond
742 * with L2CAP_Config response command with Reject error code.
744 * When "respond == 0" than we have received all options and we will
745 * sent L2CA_ConfigInd event to the upper layer protocol.
749 error = send_l2cap_cfg_rsp(con, ident, ch->dcid, result, m);
751 ng_l2cap_l2ca_discon_ind(ch);
752 ng_l2cap_free_chan(ch);
755 /* Send L2CA_ConfigInd event to the upper layer protocol */
757 error = ng_l2cap_l2ca_cfg_ind(ch);
759 ng_l2cap_free_chan(ch);
765 /* Send reject. Do not really care about the result */
768 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, 0, dcid);
771 } /* ng_l2cap_process_cfg_req */
774 * Process L2CAP_ConfigRsp command
778 ng_l2cap_process_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident)
780 ng_l2cap_p l2cap = con->l2cap;
781 struct mbuf *m = con->rx_pkt;
782 ng_l2cap_cfg_rsp_cp *cp = NULL;
783 ng_l2cap_cmd_p cmd = NULL;
784 u_int16_t scid, cflag, result;
785 ng_l2cap_cfg_opt_t hdr;
786 ng_l2cap_cfg_opt_val_t val;
789 /* Get command parameters */
791 NG_L2CAP_M_PULLUP(m, sizeof(*cp));
795 cp = mtod(m, ng_l2cap_cfg_rsp_cp *);
796 scid = le16toh(cp->scid);
797 cflag = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags));
798 result = le16toh(cp->result);
799 m_adj(m, sizeof(*cp));
801 /* Check if we have this command */
802 cmd = ng_l2cap_cmd_by_ident(con, ident);
805 "%s: %s - unexpected L2CAP_ConfigRsp command. ident=%d, con_handle=%d\n",
806 __func__, NG_NODE_NAME(l2cap->node), ident,
813 /* Verify CIDs and send reject if does not match */
814 if (cmd->ch->scid != scid) {
816 "%s: %s - unexpected L2CAP_ConfigRsp. " \
817 "Channel ID does not match, scid=%d(%d)\n",
818 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid,
823 /* Verify channel state and reject if invalid */
824 if (cmd->ch->state != NG_L2CAP_CONFIG) {
826 "%s: %s - unexpected L2CAP_ConfigRsp. " \
827 "Invalid channel state, scid=%d, state=%d\n",
828 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid,
834 * Looks like it is our response, so process it. First parse options,
835 * then verify C flag. If it is set then we shall expect more
836 * configuration options from the peer and we will wait. Otherwise we
837 * have received all options and we will send L2CA_ConfigRsp event to
838 * the upper layer protocol. If command timeout already happened then
842 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) {
848 error = get_next_l2cap_opt(m, &off, &hdr, &val);
849 if (error == 0) /* We done with this packet */
851 else if (error > 0) { /* Got option */
853 case NG_L2CAP_OPT_MTU:
854 cmd->ch->imtu = val.mtu;
857 case NG_L2CAP_OPT_FLUSH_TIMO:
858 cmd->ch->flush_timo = val.flush_timo;
861 case NG_L2CAP_OPT_QOS:
862 bcopy(&val.flow, &cmd->ch->oflow,
863 sizeof(cmd->ch->oflow));
866 default: /* Ignore unknown hint option */
871 * XXX FIXME What to do here?
873 * This is really BAD :( options packet was broken, or
874 * peer sent us option that we did not understand. Let
875 * upper layer know and do not wait for more options.
879 "%s: %s - failed to parse configuration options, error=%d\n",
880 __func__, NG_NODE_NAME(l2cap->node), error);
882 result = NG_L2CAP_UNKNOWN;
891 if (cflag) /* Restart timer and wait for more options */
892 ng_l2cap_command_timeout(cmd, bluetooth_l2cap_rtx_timeout());
894 ng_l2cap_unlink_cmd(cmd);
896 /* Send L2CA_Config response to the upper layer protocol */
897 error = ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, result);
900 * XXX FIXME what to do here? we were not able to send
901 * response to the upper layer protocol, so for now
902 * just close the channel. Send L2CAP_Disconnect to
907 "%s: %s - failed to send L2CA_Config response, error=%d\n",
908 __func__, NG_NODE_NAME(l2cap->node), error);
910 ng_l2cap_free_chan(cmd->ch);
913 ng_l2cap_free_cmd(cmd);
919 /* Send reject. Do not really care about the result */
922 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, 0);
925 } /* ng_l2cap_process_cfg_rsp */
928 * Process L2CAP_DisconnectReq command
932 ng_l2cap_process_discon_req(ng_l2cap_con_p con, u_int8_t ident)
934 ng_l2cap_p l2cap = con->l2cap;
935 ng_l2cap_discon_req_cp *cp = NULL;
936 ng_l2cap_chan_p ch = NULL;
937 ng_l2cap_cmd_p cmd = NULL;
938 u_int16_t scid, dcid;
940 /* Get command parameters */
941 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp));
942 if (con->rx_pkt == NULL)
945 cp = mtod(con->rx_pkt, ng_l2cap_discon_req_cp *);
946 dcid = le16toh(cp->dcid);
947 scid = le16toh(cp->scid);
949 NG_FREE_M(con->rx_pkt);
951 /* Check if we have this channel and it is in valid state */
952 ch = ng_l2cap_chan_by_scid(l2cap, dcid, NG_L2CAP_L2CA_IDTYPE_BREDR);
955 "%s: %s - unexpected L2CAP_DisconnectReq message. " \
956 "Channel does not exist, cid=%d\n",
957 __func__, NG_NODE_NAME(l2cap->node), dcid);
961 /* XXX Verify channel state and reject if invalid -- is that true? */
962 if (ch->state != NG_L2CAP_OPEN && ch->state != NG_L2CAP_CONFIG &&
963 ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) {
965 "%s: %s - unexpected L2CAP_DisconnectReq. " \
966 "Invalid channel state, cid=%d, state=%d\n",
967 __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state);
971 /* Match destination channel ID */
972 if (ch->dcid != scid || ch->scid != dcid) {
974 "%s: %s - unexpected L2CAP_DisconnectReq. " \
975 "Channel IDs does not match, channel: scid=%d, dcid=%d, " \
976 "request: scid=%d, dcid=%d\n",
977 __func__, NG_NODE_NAME(l2cap->node), ch->scid, ch->dcid,
983 * Looks good, so notify upper layer protocol that channel is about
984 * to be disconnected and send L2CA_DisconnectInd message. Then respond
985 * with L2CAP_DisconnectRsp.
988 if (ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) {
989 ng_l2cap_l2ca_discon_ind(ch); /* do not care about result */
990 ng_l2cap_free_chan(ch);
993 /* Send L2CAP_DisconnectRsp */
994 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_DISCON_RSP, 0);
998 _ng_l2cap_discon_rsp(cmd->aux, ident, dcid, scid);
999 if (cmd->aux == NULL) {
1000 ng_l2cap_free_cmd(cmd);
1005 /* Link command to the queue */
1006 ng_l2cap_link_cmd(con, cmd);
1007 ng_l2cap_lp_deliver(con);
1012 /* Send reject. Do not really care about the result */
1013 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid);
1016 } /* ng_l2cap_process_discon_req */
1019 * Process L2CAP_DisconnectRsp command
1023 ng_l2cap_process_discon_rsp(ng_l2cap_con_p con, u_int8_t ident)
1025 ng_l2cap_p l2cap = con->l2cap;
1026 ng_l2cap_discon_rsp_cp *cp = NULL;
1027 ng_l2cap_cmd_p cmd = NULL;
1028 u_int16_t scid, dcid;
1031 /* Get command parameters */
1032 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp));
1033 if (con->rx_pkt == NULL)
1036 cp = mtod(con->rx_pkt, ng_l2cap_discon_rsp_cp *);
1037 dcid = le16toh(cp->dcid);
1038 scid = le16toh(cp->scid);
1040 NG_FREE_M(con->rx_pkt);
1042 /* Check if we have pending command descriptor */
1043 cmd = ng_l2cap_cmd_by_ident(con, ident);
1046 "%s: %s - unexpected L2CAP_DisconnectRsp command. ident=%d, con_handle=%d\n",
1047 __func__, NG_NODE_NAME(l2cap->node), ident,
1052 /* Verify channel state, do nothing if invalid */
1053 if (cmd->ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) {
1055 "%s: %s - unexpected L2CAP_DisconnectRsp. " \
1056 "Invalid channel state, cid=%d, state=%d\n",
1057 __func__, NG_NODE_NAME(l2cap->node), scid,
1062 /* Verify CIDs and send reject if does not match */
1063 if (cmd->ch->scid != scid || cmd->ch->dcid != dcid) {
1065 "%s: %s - unexpected L2CAP_DisconnectRsp. " \
1066 "Channel IDs do not match, scid=%d(%d), dcid=%d(%d)\n",
1067 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid,
1068 scid, cmd->ch->dcid, dcid);
1073 * Looks like we have successfully disconnected channel, so notify
1074 * upper layer. If command timeout already happened then ignore
1078 if ((error = ng_l2cap_command_untimeout(cmd)) != 0)
1081 error = ng_l2cap_l2ca_discon_rsp(cmd->ch, cmd->token, NG_L2CAP_SUCCESS);
1082 ng_l2cap_free_chan(cmd->ch); /* this will free commands too */
1085 } /* ng_l2cap_process_discon_rsp */
1088 * Process L2CAP_EchoReq command
1092 ng_l2cap_process_echo_req(ng_l2cap_con_p con, u_int8_t ident)
1094 ng_l2cap_p l2cap = con->l2cap;
1095 ng_l2cap_cmd_hdr_t *hdr = NULL;
1096 ng_l2cap_cmd_p cmd = NULL;
1098 con->rx_pkt = ng_l2cap_prepend(con->rx_pkt, sizeof(*hdr));
1099 if (con->rx_pkt == NULL) {
1101 "%s: %s - ng_l2cap_prepend() failed, size=%zd\n",
1102 __func__, NG_NODE_NAME(l2cap->node), sizeof(*hdr));
1107 hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *);
1108 hdr->code = NG_L2CAP_ECHO_RSP;
1110 hdr->length = htole16(con->rx_pkt->m_pkthdr.len - sizeof(*hdr));
1112 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_ECHO_RSP, 0);
1114 NG_FREE_M(con->rx_pkt);
1119 /* Attach data and link command to the queue */
1120 cmd->aux = con->rx_pkt;
1122 ng_l2cap_link_cmd(con, cmd);
1123 ng_l2cap_lp_deliver(con);
1126 } /* ng_l2cap_process_echo_req */
1129 * Process L2CAP_EchoRsp command
1133 ng_l2cap_process_echo_rsp(ng_l2cap_con_p con, u_int8_t ident)
1135 ng_l2cap_p l2cap = con->l2cap;
1136 ng_l2cap_cmd_p cmd = NULL;
1139 /* Check if we have this command */
1140 cmd = ng_l2cap_cmd_by_ident(con, ident);
1142 /* If command timeout already happened then ignore response */
1143 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) {
1144 NG_FREE_M(con->rx_pkt);
1148 ng_l2cap_unlink_cmd(cmd);
1150 error = ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token,
1151 NG_L2CAP_SUCCESS, con->rx_pkt);
1153 ng_l2cap_free_cmd(cmd);
1157 "%s: %s - unexpected L2CAP_EchoRsp command. " \
1158 "Requested ident does not exist, ident=%d\n",
1159 __func__, NG_NODE_NAME(l2cap->node), ident);
1160 NG_FREE_M(con->rx_pkt);
1164 } /* ng_l2cap_process_echo_rsp */
1167 * Process L2CAP_InfoReq command
1171 ng_l2cap_process_info_req(ng_l2cap_con_p con, u_int8_t ident)
1173 ng_l2cap_p l2cap = con->l2cap;
1174 ng_l2cap_cmd_p cmd = NULL;
1177 /* Get command parameters */
1178 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(ng_l2cap_info_req_cp));
1179 if (con->rx_pkt == NULL)
1182 type = le16toh(mtod(con->rx_pkt, ng_l2cap_info_req_cp *)->type);
1183 NG_FREE_M(con->rx_pkt);
1185 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_INFO_RSP, 0);
1190 case NG_L2CAP_CONNLESS_MTU:
1191 _ng_l2cap_info_rsp(cmd->aux, ident, NG_L2CAP_CONNLESS_MTU,
1192 NG_L2CAP_SUCCESS, NG_L2CAP_MTU_DEFAULT);
1196 _ng_l2cap_info_rsp(cmd->aux, ident, type,
1197 NG_L2CAP_NOT_SUPPORTED, 0);
1201 if (cmd->aux == NULL) {
1202 ng_l2cap_free_cmd(cmd);
1207 /* Link command to the queue */
1208 ng_l2cap_link_cmd(con, cmd);
1209 ng_l2cap_lp_deliver(con);
1212 } /* ng_l2cap_process_info_req */
1215 * Process L2CAP_InfoRsp command
1219 ng_l2cap_process_info_rsp(ng_l2cap_con_p con, u_int8_t ident)
1221 ng_l2cap_p l2cap = con->l2cap;
1222 ng_l2cap_info_rsp_cp *cp = NULL;
1223 ng_l2cap_cmd_p cmd = NULL;
1226 /* Get command parameters */
1227 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp));
1228 if (con->rx_pkt == NULL)
1231 cp = mtod(con->rx_pkt, ng_l2cap_info_rsp_cp *);
1232 cp->type = le16toh(cp->type);
1233 cp->result = le16toh(cp->result);
1234 m_adj(con->rx_pkt, sizeof(*cp));
1236 /* Check if we have pending command descriptor */
1237 cmd = ng_l2cap_cmd_by_ident(con, ident);
1240 "%s: %s - unexpected L2CAP_InfoRsp command. " \
1241 "Requested ident does not exist, ident=%d\n",
1242 __func__, NG_NODE_NAME(l2cap->node), ident);
1243 NG_FREE_M(con->rx_pkt);
1248 /* If command timeout already happened then ignore response */
1249 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) {
1250 NG_FREE_M(con->rx_pkt);
1254 ng_l2cap_unlink_cmd(cmd);
1256 if (cp->result == NG_L2CAP_SUCCESS) {
1258 case NG_L2CAP_CONNLESS_MTU:
1259 if (con->rx_pkt->m_pkthdr.len == sizeof(u_int16_t))
1260 *mtod(con->rx_pkt, u_int16_t *) =
1261 le16toh(*mtod(con->rx_pkt,u_int16_t *));
1263 cp->result = NG_L2CAP_UNKNOWN; /* XXX */
1266 "%s: %s - invalid L2CAP_InfoRsp command. " \
1267 "Bad connectionless MTU parameter, len=%d\n",
1268 __func__, NG_NODE_NAME(l2cap->node),
1269 con->rx_pkt->m_pkthdr.len);
1275 "%s: %s - invalid L2CAP_InfoRsp command. Unknown info type=%d\n",
1276 __func__, NG_NODE_NAME(l2cap->node), cp->type);
1281 error = ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token,
1282 cp->result, con->rx_pkt);
1284 ng_l2cap_free_cmd(cmd);
1288 } /* ng_l2cap_process_info_rsp */
1295 send_l2cap_reject(ng_l2cap_con_p con, u_int8_t ident, u_int16_t reason,
1296 u_int16_t mtu, u_int16_t scid, u_int16_t dcid)
1298 ng_l2cap_cmd_p cmd = NULL;
1300 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CMD_REJ, 0);
1304 _ng_l2cap_cmd_rej(cmd->aux, cmd->ident, reason, mtu, scid, dcid);
1305 if (cmd->aux == NULL) {
1306 ng_l2cap_free_cmd(cmd);
1311 /* Link command to the queue */
1312 ng_l2cap_link_cmd(con, cmd);
1313 ng_l2cap_lp_deliver(con);
1316 } /* send_l2cap_reject */
1319 * Send L2CAP connection reject
1323 send_l2cap_con_rej(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid,
1324 u_int16_t dcid, u_int16_t result)
1326 ng_l2cap_cmd_p cmd = NULL;
1328 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CON_RSP, 0);
1332 _ng_l2cap_con_rsp(cmd->aux, cmd->ident, scid, dcid, result, 0);
1333 if (cmd->aux == NULL) {
1334 ng_l2cap_free_cmd(cmd);
1339 /* Link command to the queue */
1340 ng_l2cap_link_cmd(con, cmd);
1341 ng_l2cap_lp_deliver(con);
1344 } /* send_l2cap_con_rej */
1347 * Send L2CAP config response
1351 send_l2cap_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid,
1352 u_int16_t result, struct mbuf *opt)
1354 ng_l2cap_cmd_p cmd = NULL;
1356 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CFG_RSP, 0);
1363 _ng_l2cap_cfg_rsp(cmd->aux, cmd->ident, scid, 0, result, opt);
1364 if (cmd->aux == NULL) {
1365 ng_l2cap_free_cmd(cmd);
1370 /* Link command to the queue */
1371 ng_l2cap_link_cmd(con, cmd);
1372 ng_l2cap_lp_deliver(con);
1375 } /* send_l2cap_cfg_rsp */
1378 send_l2cap_param_urs(ng_l2cap_con_p con, u_int8_t ident,
1381 ng_l2cap_cmd_p cmd = NULL;
1383 cmd = ng_l2cap_new_cmd(con, NULL, ident,
1384 NG_L2CAP_CMD_PARAM_UPDATE_RESPONSE,
1391 _ng_l2cap_cmd_urs(cmd->aux, cmd->ident, result);
1392 if (cmd->aux == NULL) {
1393 ng_l2cap_free_cmd(cmd);
1398 /* Link command to the queue */
1399 ng_l2cap_link_cmd(con, cmd);
1400 ng_l2cap_lp_deliver(con);
1403 } /* send_l2cap_cfg_rsp */
1406 * Get next L2CAP configuration option
1410 * 1 we have got option
1411 * -1 header too short
1412 * -2 bad option value or length
1417 get_next_l2cap_opt(struct mbuf *m, int *off, ng_l2cap_cfg_opt_p hdr,
1418 ng_l2cap_cfg_opt_val_p val)
1420 int hint, len = m->m_pkthdr.len - (*off);
1424 if (len < 0 || len < sizeof(*hdr))
1427 m_copydata(m, *off, sizeof(*hdr), (caddr_t) hdr);
1428 *off += sizeof(*hdr);
1429 len -= sizeof(*hdr);
1431 hint = NG_L2CAP_OPT_HINT(hdr->type);
1432 hdr->type &= NG_L2CAP_OPT_HINT_MASK;
1434 switch (hdr->type) {
1435 case NG_L2CAP_OPT_MTU:
1436 if (hdr->length != NG_L2CAP_OPT_MTU_SIZE || len < hdr->length)
1439 m_copydata(m, *off, NG_L2CAP_OPT_MTU_SIZE, (caddr_t) val);
1440 val->mtu = le16toh(val->mtu);
1441 *off += NG_L2CAP_OPT_MTU_SIZE;
1444 case NG_L2CAP_OPT_FLUSH_TIMO:
1445 if (hdr->length != NG_L2CAP_OPT_FLUSH_TIMO_SIZE ||
1449 m_copydata(m, *off, NG_L2CAP_OPT_FLUSH_TIMO_SIZE, (caddr_t)val);
1450 val->flush_timo = le16toh(val->flush_timo);
1451 *off += NG_L2CAP_OPT_FLUSH_TIMO_SIZE;
1454 case NG_L2CAP_OPT_QOS:
1455 if (hdr->length != NG_L2CAP_OPT_QOS_SIZE || len < hdr->length)
1458 m_copydata(m, *off, NG_L2CAP_OPT_QOS_SIZE, (caddr_t) val);
1459 val->flow.token_rate = le32toh(val->flow.token_rate);
1460 val->flow.token_bucket_size =
1461 le32toh(val->flow.token_bucket_size);
1462 val->flow.peak_bandwidth = le32toh(val->flow.peak_bandwidth);
1463 val->flow.latency = le32toh(val->flow.latency);
1464 val->flow.delay_variation = le32toh(val->flow.delay_variation);
1465 *off += NG_L2CAP_OPT_QOS_SIZE;
1470 *off += hdr->length;
1477 } /* get_next_l2cap_opt */