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 $
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_lesignal_cmd (ng_l2cap_con_p);
61 static int ng_l2cap_process_cmd_rej (ng_l2cap_con_p, u_int8_t);
62 static int ng_l2cap_process_cmd_urq (ng_l2cap_con_p, u_int8_t);
63 static int ng_l2cap_process_cmd_urs (ng_l2cap_con_p, u_int8_t);
64 static int ng_l2cap_process_con_req (ng_l2cap_con_p, u_int8_t);
65 static int ng_l2cap_process_con_rsp (ng_l2cap_con_p, u_int8_t);
66 static int ng_l2cap_process_cfg_req (ng_l2cap_con_p, u_int8_t);
67 static int ng_l2cap_process_cfg_rsp (ng_l2cap_con_p, u_int8_t);
68 static int ng_l2cap_process_discon_req (ng_l2cap_con_p, u_int8_t);
69 static int ng_l2cap_process_discon_rsp (ng_l2cap_con_p, u_int8_t);
70 static int ng_l2cap_process_echo_req (ng_l2cap_con_p, u_int8_t);
71 static int ng_l2cap_process_echo_rsp (ng_l2cap_con_p, u_int8_t);
72 static int ng_l2cap_process_info_req (ng_l2cap_con_p, u_int8_t);
73 static int ng_l2cap_process_info_rsp (ng_l2cap_con_p, u_int8_t);
74 static int send_l2cap_reject
75 (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, u_int16_t, u_int16_t);
76 static int send_l2cap_con_rej
77 (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, u_int16_t);
78 static int send_l2cap_cfg_rsp
79 (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, struct mbuf *);
80 static int send_l2cap_param_urs
81 (ng_l2cap_con_p , u_int8_t , u_int16_t);
83 static int get_next_l2cap_opt
84 (struct mbuf *, int *, ng_l2cap_cfg_opt_p, ng_l2cap_cfg_opt_val_p);
87 * Receive L2CAP packet. First get L2CAP header and verify packet. Than
88 * get destination channel and process packet.
92 ng_l2cap_receive(ng_l2cap_con_p con)
94 ng_l2cap_p l2cap = con->l2cap;
95 ng_l2cap_hdr_t *hdr = NULL;
99 if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) {
101 "%s: %s - invalid L2CAP packet. Packet too small, len=%d\n",
102 __func__, NG_NODE_NAME(l2cap->node),
103 con->rx_pkt->m_pkthdr.len);
108 /* Get L2CAP header */
109 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr));
110 if (con->rx_pkt == NULL)
113 hdr = mtod(con->rx_pkt, ng_l2cap_hdr_t *);
114 hdr->length = le16toh(hdr->length);
115 hdr->dcid = le16toh(hdr->dcid);
117 /* Check payload size */
118 if (hdr->length != con->rx_pkt->m_pkthdr.len - sizeof(*hdr)) {
120 "%s: %s - invalid L2CAP packet. Payload length mismatch, length=%d, len=%zd\n",
121 __func__, NG_NODE_NAME(l2cap->node), hdr->length,
122 con->rx_pkt->m_pkthdr.len - sizeof(*hdr));
129 case NG_L2CAP_SIGNAL_CID: /* L2CAP command */
130 m_adj(con->rx_pkt, sizeof(*hdr));
131 error = ng_l2cap_process_signal_cmd(con);
133 case NG_L2CAP_LESIGNAL_CID:
134 m_adj(con->rx_pkt, sizeof(*hdr));
135 error = ng_l2cap_process_lesignal_cmd(con);
137 case NG_L2CAP_CLT_CID: /* Connectionless packet */
138 error = ng_l2cap_l2ca_clt_receive(con);
141 default: /* Data packet */
142 error = ng_l2cap_l2ca_receive(con);
148 NG_FREE_M(con->rx_pkt);
151 } /* ng_l2cap_receive */
154 * Process L2CAP signaling command. We already know that destination channel ID
155 * is 0x1 that means we have received signaling command from peer's L2CAP layer.
156 * So get command header, decode and process it.
158 * XXX do we need to check signaling MTU here?
162 ng_l2cap_process_signal_cmd(ng_l2cap_con_p con)
164 ng_l2cap_p l2cap = con->l2cap;
165 ng_l2cap_cmd_hdr_t *hdr = NULL;
166 struct mbuf *m = NULL;
168 while (con->rx_pkt != NULL) {
169 /* Verify packet length */
170 if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) {
172 "%s: %s - invalid L2CAP signaling command. Packet too small, len=%d\n",
173 __func__, NG_NODE_NAME(l2cap->node),
174 con->rx_pkt->m_pkthdr.len);
175 NG_FREE_M(con->rx_pkt);
180 /* Get signaling command */
181 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr));
182 if (con->rx_pkt == NULL)
185 hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *);
186 hdr->length = le16toh(hdr->length);
187 m_adj(con->rx_pkt, sizeof(*hdr));
189 /* Verify command length */
190 if (con->rx_pkt->m_pkthdr.len < hdr->length) {
192 "%s: %s - invalid L2CAP signaling command, code=%#x, ident=%d. " \
193 "Invalid command length=%d, m_pkthdr.len=%d\n",
194 __func__, NG_NODE_NAME(l2cap->node),
195 hdr->code, hdr->ident, hdr->length,
196 con->rx_pkt->m_pkthdr.len);
197 NG_FREE_M(con->rx_pkt);
202 /* Get the command, save the rest (if any) */
203 if (con->rx_pkt->m_pkthdr.len > hdr->length)
204 m = m_split(con->rx_pkt, hdr->length, M_NOWAIT);
208 /* Process command */
210 case NG_L2CAP_CMD_REJ:
211 ng_l2cap_process_cmd_rej(con, hdr->ident);
214 case NG_L2CAP_CON_REQ:
215 ng_l2cap_process_con_req(con, hdr->ident);
218 case NG_L2CAP_CON_RSP:
219 ng_l2cap_process_con_rsp(con, hdr->ident);
222 case NG_L2CAP_CFG_REQ:
223 ng_l2cap_process_cfg_req(con, hdr->ident);
226 case NG_L2CAP_CFG_RSP:
227 ng_l2cap_process_cfg_rsp(con, hdr->ident);
230 case NG_L2CAP_DISCON_REQ:
231 ng_l2cap_process_discon_req(con, hdr->ident);
234 case NG_L2CAP_DISCON_RSP:
235 ng_l2cap_process_discon_rsp(con, hdr->ident);
238 case NG_L2CAP_ECHO_REQ:
239 ng_l2cap_process_echo_req(con, hdr->ident);
242 case NG_L2CAP_ECHO_RSP:
243 ng_l2cap_process_echo_rsp(con, hdr->ident);
246 case NG_L2CAP_INFO_REQ:
247 ng_l2cap_process_info_req(con, hdr->ident);
250 case NG_L2CAP_INFO_RSP:
251 ng_l2cap_process_info_rsp(con, hdr->ident);
256 "%s: %s - unknown L2CAP signaling command, code=%#x, ident=%d, length=%d\n",
257 __func__, NG_NODE_NAME(l2cap->node),
258 hdr->code, hdr->ident, hdr->length);
261 * Send L2CAP_CommandRej. Do not really care
265 send_l2cap_reject(con, hdr->ident,
266 NG_L2CAP_REJ_NOT_UNDERSTOOD, 0, 0, 0);
267 NG_FREE_M(con->rx_pkt);
275 } /* ng_l2cap_process_signal_cmd */
277 ng_l2cap_process_lesignal_cmd(ng_l2cap_con_p con)
279 ng_l2cap_p l2cap = con->l2cap;
280 ng_l2cap_cmd_hdr_t *hdr = NULL;
281 struct mbuf *m = NULL;
283 while (con->rx_pkt != NULL) {
284 /* Verify packet length */
285 if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) {
287 "%s: %s - invalid L2CAP signaling command. Packet too small, len=%d\n",
288 __func__, NG_NODE_NAME(l2cap->node),
289 con->rx_pkt->m_pkthdr.len);
290 NG_FREE_M(con->rx_pkt);
295 /* Get signaling command */
296 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr));
297 if (con->rx_pkt == NULL)
300 hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *);
301 hdr->length = le16toh(hdr->length);
302 m_adj(con->rx_pkt, sizeof(*hdr));
304 /* Verify command length */
305 if (con->rx_pkt->m_pkthdr.len < hdr->length) {
307 "%s: %s - invalid L2CAP signaling command, code=%#x, ident=%d. " \
308 "Invalid command length=%d, m_pkthdr.len=%d\n",
309 __func__, NG_NODE_NAME(l2cap->node),
310 hdr->code, hdr->ident, hdr->length,
311 con->rx_pkt->m_pkthdr.len);
312 NG_FREE_M(con->rx_pkt);
317 /* Get the command, save the rest (if any) */
318 if (con->rx_pkt->m_pkthdr.len > hdr->length)
319 m = m_split(con->rx_pkt, hdr->length, M_NOWAIT);
323 /* Process command */
325 case NG_L2CAP_CMD_REJ:
326 ng_l2cap_process_cmd_rej(con, hdr->ident);
328 case NG_L2CAP_CMD_PARAM_UPDATE_REQUEST:
329 ng_l2cap_process_cmd_urq(con, hdr->ident);
331 case NG_L2CAP_CMD_PARAM_UPDATE_RESPONSE:
332 ng_l2cap_process_cmd_urs(con, hdr->ident);
338 "%s: %s - unknown L2CAP signaling command, code=%#x, ident=%d, length=%d\n",
339 __func__, NG_NODE_NAME(l2cap->node),
340 hdr->code, hdr->ident, hdr->length);
343 * Send L2CAP_CommandRej. Do not really care
347 send_l2cap_reject(con, hdr->ident,
348 NG_L2CAP_REJ_NOT_UNDERSTOOD, 0, 0, 0);
349 NG_FREE_M(con->rx_pkt);
357 } /* ng_l2cap_process_signal_cmd */
358 /*Update Paramater Request*/
359 static int ng_l2cap_process_cmd_urq(ng_l2cap_con_p con, uint8_t ident)
361 /* We do not implement parameter negotiation for now. */
362 send_l2cap_param_urs(con, ident, NG_L2CAP_UPDATE_PARAM_ACCEPT);
363 NG_FREE_M(con->rx_pkt);
367 static int ng_l2cap_process_cmd_urs(ng_l2cap_con_p con, uint8_t ident)
369 /* We only support master side yet .*/
370 //send_l2cap_reject(con,ident ... );
372 NG_FREE_M(con->rx_pkt);
377 * Process L2CAP_CommandRej command
381 ng_l2cap_process_cmd_rej(ng_l2cap_con_p con, u_int8_t ident)
383 ng_l2cap_p l2cap = con->l2cap;
384 ng_l2cap_cmd_rej_cp *cp = NULL;
385 ng_l2cap_cmd_p cmd = NULL;
387 /* Get command parameters */
388 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp));
389 if (con->rx_pkt == NULL)
392 cp = mtod(con->rx_pkt, ng_l2cap_cmd_rej_cp *);
393 cp->reason = le16toh(cp->reason);
395 /* Check if we have pending command descriptor */
396 cmd = ng_l2cap_cmd_by_ident(con, ident);
398 /* If command timeout already happened then ignore reject */
399 if (ng_l2cap_command_untimeout(cmd) != 0) {
400 NG_FREE_M(con->rx_pkt);
404 ng_l2cap_unlink_cmd(cmd);
407 case NG_L2CAP_CON_REQ:
408 ng_l2cap_l2ca_con_rsp(cmd->ch,cmd->token,cp->reason,0);
409 ng_l2cap_free_chan(cmd->ch);
412 case NG_L2CAP_CFG_REQ:
413 ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, cp->reason);
416 case NG_L2CAP_DISCON_REQ:
417 ng_l2cap_l2ca_discon_rsp(cmd->ch,cmd->token,cp->reason);
418 ng_l2cap_free_chan(cmd->ch); /* XXX free channel */
421 case NG_L2CAP_ECHO_REQ:
422 ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token,
426 case NG_L2CAP_INFO_REQ:
427 ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token,
433 "%s: %s - unexpected L2CAP_CommandRej. Unexpected L2CAP command opcode=%d\n",
434 __func__, NG_NODE_NAME(l2cap->node), cmd->code);
438 ng_l2cap_free_cmd(cmd);
441 "%s: %s - unexpected L2CAP_CommandRej command. " \
442 "Requested ident does not exist, ident=%d\n",
443 __func__, NG_NODE_NAME(l2cap->node), ident);
445 NG_FREE_M(con->rx_pkt);
448 } /* ng_l2cap_process_cmd_rej */
451 * Process L2CAP_ConnectReq command
455 ng_l2cap_process_con_req(ng_l2cap_con_p con, u_int8_t ident)
457 ng_l2cap_p l2cap = con->l2cap;
458 struct mbuf *m = con->rx_pkt;
459 ng_l2cap_con_req_cp *cp = NULL;
460 ng_l2cap_chan_p ch = NULL;
465 /* Get command parameters */
466 NG_L2CAP_M_PULLUP(m, sizeof(*cp));
470 cp = mtod(m, ng_l2cap_con_req_cp *);
471 psm = le16toh(cp->psm);
472 dcid = le16toh(cp->scid);
476 if(dcid == NG_L2CAP_ATT_CID)
477 idtype = NG_L2CAP_L2CA_IDTYPE_ATT;
478 else if(dcid == NG_L2CAP_SMP_CID)
479 idtype = NG_L2CAP_L2CA_IDTYPE_SMP;
480 else if( con->linktype != NG_HCI_LINK_ACL)
481 idtype = NG_L2CAP_L2CA_IDTYPE_LE;
483 idtype = NG_L2CAP_L2CA_IDTYPE_BREDR;
486 * Create new channel and send L2CA_ConnectInd notification
487 * to the upper layer protocol.
490 ch = ng_l2cap_new_chan(l2cap, con, psm, idtype);
493 return (send_l2cap_con_rej(con, ident, 0, dcid,
494 NG_L2CAP_NO_RESOURCES));
496 /* Update channel IDs */
499 /* Sent L2CA_ConnectInd notification to the upper layer */
501 ch->state = NG_L2CAP_W4_L2CA_CON_RSP;
503 error = ng_l2cap_l2ca_con_ind(ch);
505 send_l2cap_con_rej(con, ident, ch->scid, dcid,
506 (error == ENOMEM)? NG_L2CAP_NO_RESOURCES :
507 NG_L2CAP_PSM_NOT_SUPPORTED);
508 ng_l2cap_free_chan(ch);
512 } /* ng_l2cap_process_con_req */
515 * Process L2CAP_ConnectRsp command
519 ng_l2cap_process_con_rsp(ng_l2cap_con_p con, u_int8_t ident)
521 ng_l2cap_p l2cap = con->l2cap;
522 struct mbuf *m = con->rx_pkt;
523 ng_l2cap_con_rsp_cp *cp = NULL;
524 ng_l2cap_cmd_p cmd = NULL;
525 u_int16_t scid, dcid, result, status;
528 /* Get command parameters */
529 NG_L2CAP_M_PULLUP(m, sizeof(*cp));
533 cp = mtod(m, ng_l2cap_con_rsp_cp *);
534 dcid = le16toh(cp->dcid);
535 scid = le16toh(cp->scid);
536 result = le16toh(cp->result);
537 status = le16toh(cp->status);
542 /* Check if we have pending command descriptor */
543 cmd = ng_l2cap_cmd_by_ident(con, ident);
546 "%s: %s - unexpected L2CAP_ConnectRsp command. ident=%d, con_handle=%d\n",
547 __func__, NG_NODE_NAME(l2cap->node), ident,
553 /* Verify channel state, if invalid - do nothing */
554 if (cmd->ch->state != NG_L2CAP_W4_L2CAP_CON_RSP) {
556 "%s: %s - unexpected L2CAP_ConnectRsp. " \
557 "Invalid channel state, cid=%d, state=%d\n",
558 __func__, NG_NODE_NAME(l2cap->node), scid,
563 /* Verify CIDs and send reject if does not match */
564 if (cmd->ch->scid != scid) {
566 "%s: %s - unexpected L2CAP_ConnectRsp. Channel IDs do not match, scid=%d(%d)\n",
567 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid,
573 * Looks good. We got confirmation from our peer. Now process
574 * it. First disable RTX timer. Then check the result and send
575 * notification to the upper layer. If command timeout already
576 * happened then ignore response.
579 if ((error = ng_l2cap_command_untimeout(cmd)) != 0)
582 if (result == NG_L2CAP_PENDING) {
584 * Our peer wants more time to complete connection. We shall
585 * start ERTX timer and wait. Keep command in the list.
588 cmd->ch->dcid = dcid;
589 ng_l2cap_command_timeout(cmd, bluetooth_l2cap_ertx_timeout());
591 error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token,
594 ng_l2cap_free_chan(cmd->ch);
596 ng_l2cap_unlink_cmd(cmd);
598 if (result == NG_L2CAP_SUCCESS) {
600 * Channel is open. Complete command and move to CONFIG
601 * state. Since we have sent positive confirmation we
602 * expect to receive L2CA_Config request from the upper
606 cmd->ch->dcid = dcid;
607 cmd->ch->state = ((cmd->ch->scid == NG_L2CAP_ATT_CID)||
608 (cmd->ch->scid == NG_L2CAP_SMP_CID))
610 NG_L2CAP_OPEN : NG_L2CAP_CONFIG;
612 /* There was an error, so close the channel */
614 "%s: %s - failed to open L2CAP channel, result=%d, status=%d\n",
615 __func__, NG_NODE_NAME(l2cap->node), result,
618 error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token,
621 /* XXX do we have to remove the channel on error? */
622 if (error != 0 || result != NG_L2CAP_SUCCESS)
623 ng_l2cap_free_chan(cmd->ch);
625 ng_l2cap_free_cmd(cmd);
631 /* Send reject. Do not really care about the result */
632 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid);
635 } /* ng_l2cap_process_con_rsp */
638 * Process L2CAP_ConfigReq command
642 ng_l2cap_process_cfg_req(ng_l2cap_con_p con, u_int8_t ident)
644 ng_l2cap_p l2cap = con->l2cap;
645 struct mbuf *m = con->rx_pkt;
646 ng_l2cap_cfg_req_cp *cp = NULL;
647 ng_l2cap_chan_p ch = NULL;
648 u_int16_t dcid, respond, result;
649 ng_l2cap_cfg_opt_t hdr;
650 ng_l2cap_cfg_opt_val_t val;
653 /* Get command parameters */
655 NG_L2CAP_M_PULLUP(m, sizeof(*cp));
659 cp = mtod(m, ng_l2cap_cfg_req_cp *);
660 dcid = le16toh(cp->dcid);
661 respond = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags));
662 m_adj(m, sizeof(*cp));
664 /* Check if we have this channel and it is in valid state */
665 ch = ng_l2cap_chan_by_scid(l2cap, dcid, NG_L2CAP_L2CA_IDTYPE_BREDR);
668 "%s: %s - unexpected L2CAP_ConfigReq command. " \
669 "Channel does not exist, cid=%d\n",
670 __func__, NG_NODE_NAME(l2cap->node), dcid);
674 /* Verify channel state */
675 if (ch->state != NG_L2CAP_CONFIG && ch->state != NG_L2CAP_OPEN) {
677 "%s: %s - unexpected L2CAP_ConfigReq. " \
678 "Invalid channel state, cid=%d, state=%d\n",
679 __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state);
683 if (ch->state == NG_L2CAP_OPEN) { /* Re-configuration */
685 ch->state = NG_L2CAP_CONFIG;
688 for (result = 0, off = 0; ; ) {
689 error = get_next_l2cap_opt(m, &off, &hdr, &val);
690 if (error == 0) { /* We done with this packet */
693 } else if (error > 0) { /* Got option */
695 case NG_L2CAP_OPT_MTU:
699 case NG_L2CAP_OPT_FLUSH_TIMO:
700 ch->flush_timo = val.flush_timo;
703 case NG_L2CAP_OPT_QOS:
704 bcopy(&val.flow, &ch->iflow, sizeof(ch->iflow));
707 default: /* Ignore unknown hint option */
710 } else { /* Oops, something is wrong */
716 * Adjust mbuf so we can get to the start
717 * of the first option we did not like.
720 m_adj(m, off - sizeof(hdr));
721 m->m_pkthdr.len = sizeof(hdr) + hdr.length;
723 result = NG_L2CAP_UNKNOWN_OPTION;
725 /* XXX FIXME Send other reject codes? */
727 result = NG_L2CAP_REJECT;
735 * Now check and see if we have to respond. If everything was OK then
736 * respond contain "C flag" and (if set) we will respond with empty
737 * packet and will wait for more options.
739 * Other case is that we did not like peer's options and will respond
740 * with L2CAP_Config response command with Reject error code.
742 * When "respond == 0" than we have received all options and we will
743 * sent L2CA_ConfigInd event to the upper layer protocol.
747 error = send_l2cap_cfg_rsp(con, ident, ch->dcid, result, m);
749 ng_l2cap_l2ca_discon_ind(ch);
750 ng_l2cap_free_chan(ch);
753 /* Send L2CA_ConfigInd event to the upper layer protocol */
755 error = ng_l2cap_l2ca_cfg_ind(ch);
757 ng_l2cap_free_chan(ch);
763 /* Send reject. Do not really care about the result */
766 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, 0, dcid);
769 } /* ng_l2cap_process_cfg_req */
772 * Process L2CAP_ConfigRsp command
776 ng_l2cap_process_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident)
778 ng_l2cap_p l2cap = con->l2cap;
779 struct mbuf *m = con->rx_pkt;
780 ng_l2cap_cfg_rsp_cp *cp = NULL;
781 ng_l2cap_cmd_p cmd = NULL;
782 u_int16_t scid, cflag, result;
783 ng_l2cap_cfg_opt_t hdr;
784 ng_l2cap_cfg_opt_val_t val;
787 /* Get command parameters */
789 NG_L2CAP_M_PULLUP(m, sizeof(*cp));
793 cp = mtod(m, ng_l2cap_cfg_rsp_cp *);
794 scid = le16toh(cp->scid);
795 cflag = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags));
796 result = le16toh(cp->result);
797 m_adj(m, sizeof(*cp));
799 /* Check if we have this command */
800 cmd = ng_l2cap_cmd_by_ident(con, ident);
803 "%s: %s - unexpected L2CAP_ConfigRsp command. ident=%d, con_handle=%d\n",
804 __func__, NG_NODE_NAME(l2cap->node), ident,
811 /* Verify CIDs and send reject if does not match */
812 if (cmd->ch->scid != scid) {
814 "%s: %s - unexpected L2CAP_ConfigRsp. " \
815 "Channel ID does not match, scid=%d(%d)\n",
816 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid,
821 /* Verify channel state and reject if invalid */
822 if (cmd->ch->state != NG_L2CAP_CONFIG) {
824 "%s: %s - unexpected L2CAP_ConfigRsp. " \
825 "Invalid channel state, scid=%d, state=%d\n",
826 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid,
832 * Looks like it is our response, so process it. First parse options,
833 * then verify C flag. If it is set then we shall expect more
834 * configuration options from the peer and we will wait. Otherwise we
835 * have received all options and we will send L2CA_ConfigRsp event to
836 * the upper layer protocol. If command timeout already happened then
840 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) {
846 error = get_next_l2cap_opt(m, &off, &hdr, &val);
847 if (error == 0) /* We done with this packet */
849 else if (error > 0) { /* Got option */
851 case NG_L2CAP_OPT_MTU:
852 cmd->ch->imtu = val.mtu;
855 case NG_L2CAP_OPT_FLUSH_TIMO:
856 cmd->ch->flush_timo = val.flush_timo;
859 case NG_L2CAP_OPT_QOS:
860 bcopy(&val.flow, &cmd->ch->oflow,
861 sizeof(cmd->ch->oflow));
864 default: /* Ignore unknown hint option */
869 * XXX FIXME What to do here?
871 * This is really BAD :( options packet was broken, or
872 * peer sent us option that we did not understand. Let
873 * upper layer know and do not wait for more options.
877 "%s: %s - failed to parse configuration options, error=%d\n",
878 __func__, NG_NODE_NAME(l2cap->node), error);
880 result = NG_L2CAP_UNKNOWN;
889 if (cflag) /* Restart timer and wait for more options */
890 ng_l2cap_command_timeout(cmd, bluetooth_l2cap_rtx_timeout());
892 ng_l2cap_unlink_cmd(cmd);
894 /* Send L2CA_Config response to the upper layer protocol */
895 error = ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, result);
898 * XXX FIXME what to do here? we were not able to send
899 * response to the upper layer protocol, so for now
900 * just close the channel. Send L2CAP_Disconnect to
905 "%s: %s - failed to send L2CA_Config response, error=%d\n",
906 __func__, NG_NODE_NAME(l2cap->node), error);
908 ng_l2cap_free_chan(cmd->ch);
911 ng_l2cap_free_cmd(cmd);
917 /* Send reject. Do not really care about the result */
920 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, 0);
923 } /* ng_l2cap_process_cfg_rsp */
926 * Process L2CAP_DisconnectReq command
930 ng_l2cap_process_discon_req(ng_l2cap_con_p con, u_int8_t ident)
932 ng_l2cap_p l2cap = con->l2cap;
933 ng_l2cap_discon_req_cp *cp = NULL;
934 ng_l2cap_chan_p ch = NULL;
935 ng_l2cap_cmd_p cmd = NULL;
936 u_int16_t scid, dcid;
938 /* Get command parameters */
939 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp));
940 if (con->rx_pkt == NULL)
943 cp = mtod(con->rx_pkt, ng_l2cap_discon_req_cp *);
944 dcid = le16toh(cp->dcid);
945 scid = le16toh(cp->scid);
947 NG_FREE_M(con->rx_pkt);
949 /* Check if we have this channel and it is in valid state */
950 ch = ng_l2cap_chan_by_scid(l2cap, dcid, NG_L2CAP_L2CA_IDTYPE_BREDR);
953 "%s: %s - unexpected L2CAP_DisconnectReq message. " \
954 "Channel does not exist, cid=%d\n",
955 __func__, NG_NODE_NAME(l2cap->node), dcid);
959 /* XXX Verify channel state and reject if invalid -- is that true? */
960 if (ch->state != NG_L2CAP_OPEN && ch->state != NG_L2CAP_CONFIG &&
961 ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) {
963 "%s: %s - unexpected L2CAP_DisconnectReq. " \
964 "Invalid channel state, cid=%d, state=%d\n",
965 __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state);
969 /* Match destination channel ID */
970 if (ch->dcid != scid || ch->scid != dcid) {
972 "%s: %s - unexpected L2CAP_DisconnectReq. " \
973 "Channel IDs does not match, channel: scid=%d, dcid=%d, " \
974 "request: scid=%d, dcid=%d\n",
975 __func__, NG_NODE_NAME(l2cap->node), ch->scid, ch->dcid,
981 * Looks good, so notify upper layer protocol that channel is about
982 * to be disconnected and send L2CA_DisconnectInd message. Then respond
983 * with L2CAP_DisconnectRsp.
986 if (ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) {
987 ng_l2cap_l2ca_discon_ind(ch); /* do not care about result */
988 ng_l2cap_free_chan(ch);
991 /* Send L2CAP_DisconnectRsp */
992 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_DISCON_RSP, 0);
996 _ng_l2cap_discon_rsp(cmd->aux, ident, dcid, scid);
997 if (cmd->aux == NULL) {
998 ng_l2cap_free_cmd(cmd);
1003 /* Link command to the queue */
1004 ng_l2cap_link_cmd(con, cmd);
1005 ng_l2cap_lp_deliver(con);
1010 /* Send reject. Do not really care about the result */
1011 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid);
1014 } /* ng_l2cap_process_discon_req */
1017 * Process L2CAP_DisconnectRsp command
1021 ng_l2cap_process_discon_rsp(ng_l2cap_con_p con, u_int8_t ident)
1023 ng_l2cap_p l2cap = con->l2cap;
1024 ng_l2cap_discon_rsp_cp *cp = NULL;
1025 ng_l2cap_cmd_p cmd = NULL;
1026 u_int16_t scid, dcid;
1029 /* Get command parameters */
1030 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp));
1031 if (con->rx_pkt == NULL)
1034 cp = mtod(con->rx_pkt, ng_l2cap_discon_rsp_cp *);
1035 dcid = le16toh(cp->dcid);
1036 scid = le16toh(cp->scid);
1038 NG_FREE_M(con->rx_pkt);
1040 /* Check if we have pending command descriptor */
1041 cmd = ng_l2cap_cmd_by_ident(con, ident);
1044 "%s: %s - unexpected L2CAP_DisconnectRsp command. ident=%d, con_handle=%d\n",
1045 __func__, NG_NODE_NAME(l2cap->node), ident,
1050 /* Verify channel state, do nothing if invalid */
1051 if (cmd->ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) {
1053 "%s: %s - unexpected L2CAP_DisconnectRsp. " \
1054 "Invalid channel state, cid=%d, state=%d\n",
1055 __func__, NG_NODE_NAME(l2cap->node), scid,
1060 /* Verify CIDs and send reject if does not match */
1061 if (cmd->ch->scid != scid || cmd->ch->dcid != dcid) {
1063 "%s: %s - unexpected L2CAP_DisconnectRsp. " \
1064 "Channel IDs do not match, scid=%d(%d), dcid=%d(%d)\n",
1065 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid,
1066 scid, cmd->ch->dcid, dcid);
1071 * Looks like we have successfully disconnected channel, so notify
1072 * upper layer. If command timeout already happened then ignore
1076 if ((error = ng_l2cap_command_untimeout(cmd)) != 0)
1079 error = ng_l2cap_l2ca_discon_rsp(cmd->ch, cmd->token, NG_L2CAP_SUCCESS);
1080 ng_l2cap_free_chan(cmd->ch); /* this will free commands too */
1083 } /* ng_l2cap_process_discon_rsp */
1086 * Process L2CAP_EchoReq command
1090 ng_l2cap_process_echo_req(ng_l2cap_con_p con, u_int8_t ident)
1092 ng_l2cap_p l2cap = con->l2cap;
1093 ng_l2cap_cmd_hdr_t *hdr = NULL;
1094 ng_l2cap_cmd_p cmd = NULL;
1096 con->rx_pkt = ng_l2cap_prepend(con->rx_pkt, sizeof(*hdr));
1097 if (con->rx_pkt == NULL) {
1099 "%s: %s - ng_l2cap_prepend() failed, size=%zd\n",
1100 __func__, NG_NODE_NAME(l2cap->node), sizeof(*hdr));
1105 hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *);
1106 hdr->code = NG_L2CAP_ECHO_RSP;
1108 hdr->length = htole16(con->rx_pkt->m_pkthdr.len - sizeof(*hdr));
1110 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_ECHO_RSP, 0);
1112 NG_FREE_M(con->rx_pkt);
1117 /* Attach data and link command to the queue */
1118 cmd->aux = con->rx_pkt;
1120 ng_l2cap_link_cmd(con, cmd);
1121 ng_l2cap_lp_deliver(con);
1124 } /* ng_l2cap_process_echo_req */
1127 * Process L2CAP_EchoRsp command
1131 ng_l2cap_process_echo_rsp(ng_l2cap_con_p con, u_int8_t ident)
1133 ng_l2cap_p l2cap = con->l2cap;
1134 ng_l2cap_cmd_p cmd = NULL;
1137 /* Check if we have this command */
1138 cmd = ng_l2cap_cmd_by_ident(con, ident);
1140 /* If command timeout already happened then ignore response */
1141 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) {
1142 NG_FREE_M(con->rx_pkt);
1146 ng_l2cap_unlink_cmd(cmd);
1148 error = ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token,
1149 NG_L2CAP_SUCCESS, con->rx_pkt);
1151 ng_l2cap_free_cmd(cmd);
1155 "%s: %s - unexpected L2CAP_EchoRsp command. " \
1156 "Requested ident does not exist, ident=%d\n",
1157 __func__, NG_NODE_NAME(l2cap->node), ident);
1158 NG_FREE_M(con->rx_pkt);
1162 } /* ng_l2cap_process_echo_rsp */
1165 * Process L2CAP_InfoReq command
1169 ng_l2cap_process_info_req(ng_l2cap_con_p con, u_int8_t ident)
1171 ng_l2cap_p l2cap = con->l2cap;
1172 ng_l2cap_cmd_p cmd = NULL;
1175 /* Get command parameters */
1176 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(ng_l2cap_info_req_cp));
1177 if (con->rx_pkt == NULL)
1180 type = le16toh(mtod(con->rx_pkt, ng_l2cap_info_req_cp *)->type);
1181 NG_FREE_M(con->rx_pkt);
1183 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_INFO_RSP, 0);
1188 case NG_L2CAP_CONNLESS_MTU:
1189 _ng_l2cap_info_rsp(cmd->aux, ident, NG_L2CAP_CONNLESS_MTU,
1190 NG_L2CAP_SUCCESS, NG_L2CAP_MTU_DEFAULT);
1194 _ng_l2cap_info_rsp(cmd->aux, ident, type,
1195 NG_L2CAP_NOT_SUPPORTED, 0);
1199 if (cmd->aux == NULL) {
1200 ng_l2cap_free_cmd(cmd);
1205 /* Link command to the queue */
1206 ng_l2cap_link_cmd(con, cmd);
1207 ng_l2cap_lp_deliver(con);
1210 } /* ng_l2cap_process_info_req */
1213 * Process L2CAP_InfoRsp command
1217 ng_l2cap_process_info_rsp(ng_l2cap_con_p con, u_int8_t ident)
1219 ng_l2cap_p l2cap = con->l2cap;
1220 ng_l2cap_info_rsp_cp *cp = NULL;
1221 ng_l2cap_cmd_p cmd = NULL;
1224 /* Get command parameters */
1225 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp));
1226 if (con->rx_pkt == NULL)
1229 cp = mtod(con->rx_pkt, ng_l2cap_info_rsp_cp *);
1230 cp->type = le16toh(cp->type);
1231 cp->result = le16toh(cp->result);
1232 m_adj(con->rx_pkt, sizeof(*cp));
1234 /* Check if we have pending command descriptor */
1235 cmd = ng_l2cap_cmd_by_ident(con, ident);
1238 "%s: %s - unexpected L2CAP_InfoRsp command. " \
1239 "Requested ident does not exist, ident=%d\n",
1240 __func__, NG_NODE_NAME(l2cap->node), ident);
1241 NG_FREE_M(con->rx_pkt);
1246 /* If command timeout already happened then ignore response */
1247 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) {
1248 NG_FREE_M(con->rx_pkt);
1252 ng_l2cap_unlink_cmd(cmd);
1254 if (cp->result == NG_L2CAP_SUCCESS) {
1256 case NG_L2CAP_CONNLESS_MTU:
1257 if (con->rx_pkt->m_pkthdr.len == sizeof(u_int16_t))
1258 *mtod(con->rx_pkt, u_int16_t *) =
1259 le16toh(*mtod(con->rx_pkt,u_int16_t *));
1261 cp->result = NG_L2CAP_UNKNOWN; /* XXX */
1264 "%s: %s - invalid L2CAP_InfoRsp command. " \
1265 "Bad connectionless MTU parameter, len=%d\n",
1266 __func__, NG_NODE_NAME(l2cap->node),
1267 con->rx_pkt->m_pkthdr.len);
1273 "%s: %s - invalid L2CAP_InfoRsp command. Unknown info type=%d\n",
1274 __func__, NG_NODE_NAME(l2cap->node), cp->type);
1279 error = ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token,
1280 cp->result, con->rx_pkt);
1282 ng_l2cap_free_cmd(cmd);
1286 } /* ng_l2cap_process_info_rsp */
1293 send_l2cap_reject(ng_l2cap_con_p con, u_int8_t ident, u_int16_t reason,
1294 u_int16_t mtu, u_int16_t scid, u_int16_t dcid)
1296 ng_l2cap_cmd_p cmd = NULL;
1298 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CMD_REJ, 0);
1302 _ng_l2cap_cmd_rej(cmd->aux, cmd->ident, reason, mtu, scid, dcid);
1303 if (cmd->aux == NULL) {
1304 ng_l2cap_free_cmd(cmd);
1309 /* Link command to the queue */
1310 ng_l2cap_link_cmd(con, cmd);
1311 ng_l2cap_lp_deliver(con);
1314 } /* send_l2cap_reject */
1317 * Send L2CAP connection reject
1321 send_l2cap_con_rej(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid,
1322 u_int16_t dcid, u_int16_t result)
1324 ng_l2cap_cmd_p cmd = NULL;
1326 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CON_RSP, 0);
1330 _ng_l2cap_con_rsp(cmd->aux, cmd->ident, scid, dcid, result, 0);
1331 if (cmd->aux == NULL) {
1332 ng_l2cap_free_cmd(cmd);
1337 /* Link command to the queue */
1338 ng_l2cap_link_cmd(con, cmd);
1339 ng_l2cap_lp_deliver(con);
1342 } /* send_l2cap_con_rej */
1345 * Send L2CAP config response
1349 send_l2cap_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid,
1350 u_int16_t result, struct mbuf *opt)
1352 ng_l2cap_cmd_p cmd = NULL;
1354 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CFG_RSP, 0);
1361 _ng_l2cap_cfg_rsp(cmd->aux, cmd->ident, scid, 0, result, opt);
1362 if (cmd->aux == NULL) {
1363 ng_l2cap_free_cmd(cmd);
1368 /* Link command to the queue */
1369 ng_l2cap_link_cmd(con, cmd);
1370 ng_l2cap_lp_deliver(con);
1373 } /* send_l2cap_cfg_rsp */
1376 send_l2cap_param_urs(ng_l2cap_con_p con, u_int8_t ident,
1379 ng_l2cap_cmd_p cmd = NULL;
1381 cmd = ng_l2cap_new_cmd(con, NULL, ident,
1382 NG_L2CAP_CMD_PARAM_UPDATE_RESPONSE,
1389 _ng_l2cap_cmd_urs(cmd->aux, cmd->ident, result);
1390 if (cmd->aux == NULL) {
1391 ng_l2cap_free_cmd(cmd);
1396 /* Link command to the queue */
1397 ng_l2cap_link_cmd(con, cmd);
1398 ng_l2cap_lp_deliver(con);
1401 } /* send_l2cap_cfg_rsp */
1404 * Get next L2CAP configuration option
1408 * 1 we have got option
1409 * -1 header too short
1410 * -2 bad option value or length
1415 get_next_l2cap_opt(struct mbuf *m, int *off, ng_l2cap_cfg_opt_p hdr,
1416 ng_l2cap_cfg_opt_val_p val)
1418 int hint, len = m->m_pkthdr.len - (*off);
1422 if (len < 0 || len < sizeof(*hdr))
1425 m_copydata(m, *off, sizeof(*hdr), (caddr_t) hdr);
1426 *off += sizeof(*hdr);
1427 len -= sizeof(*hdr);
1429 hint = NG_L2CAP_OPT_HINT(hdr->type);
1430 hdr->type &= NG_L2CAP_OPT_HINT_MASK;
1432 switch (hdr->type) {
1433 case NG_L2CAP_OPT_MTU:
1434 if (hdr->length != NG_L2CAP_OPT_MTU_SIZE || len < hdr->length)
1437 m_copydata(m, *off, NG_L2CAP_OPT_MTU_SIZE, (caddr_t) val);
1438 val->mtu = le16toh(val->mtu);
1439 *off += NG_L2CAP_OPT_MTU_SIZE;
1442 case NG_L2CAP_OPT_FLUSH_TIMO:
1443 if (hdr->length != NG_L2CAP_OPT_FLUSH_TIMO_SIZE ||
1447 m_copydata(m, *off, NG_L2CAP_OPT_FLUSH_TIMO_SIZE, (caddr_t)val);
1448 val->flush_timo = le16toh(val->flush_timo);
1449 *off += NG_L2CAP_OPT_FLUSH_TIMO_SIZE;
1452 case NG_L2CAP_OPT_QOS:
1453 if (hdr->length != NG_L2CAP_OPT_QOS_SIZE || len < hdr->length)
1456 m_copydata(m, *off, NG_L2CAP_OPT_QOS_SIZE, (caddr_t) val);
1457 val->flow.token_rate = le32toh(val->flow.token_rate);
1458 val->flow.token_bucket_size =
1459 le32toh(val->flow.token_bucket_size);
1460 val->flow.peak_bandwidth = le32toh(val->flow.peak_bandwidth);
1461 val->flow.latency = le32toh(val->flow.latency);
1462 val->flow.delay_variation = le32toh(val->flow.delay_variation);
1463 *off += NG_L2CAP_OPT_QOS_SIZE;
1468 *off += hdr->length;
1475 } /* get_next_l2cap_opt */