1 /*
2 * ng_l2cap_evnt.c
3 */
4
5 /*-
6 * SPDX-License-Identifier: BSD-2-Clause
7 *
8 * Copyright (c) Maksim Yevmenkin <m_evmenkin@yahoo.com>
9 * All rights reserved.
10 *
11 * Redistribution and use in source and binary forms, with or without
12 * modification, are permitted provided that the following conditions
13 * are met:
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.
19 *
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
30 * SUCH DAMAGE.
31 *
32 * $Id: ng_l2cap_evnt.c,v 1.5 2003/09/08 19:11:45 max Exp $
33 */
34
35 #include <sys/param.h>
36 #include <sys/systm.h>
37 #include <sys/kernel.h>
38 #include <sys/endian.h>
39 #include <sys/malloc.h>
40 #include <sys/mbuf.h>
41 #include <sys/queue.h>
42 #include <netgraph/ng_message.h>
43 #include <netgraph/netgraph.h>
44 #include <netgraph/bluetooth/include/ng_bluetooth.h>
45 #include <netgraph/bluetooth/include/ng_hci.h>
46 #include <netgraph/bluetooth/include/ng_l2cap.h>
47 #include <netgraph/bluetooth/l2cap/ng_l2cap_var.h>
48 #include <netgraph/bluetooth/l2cap/ng_l2cap_cmds.h>
49 #include <netgraph/bluetooth/l2cap/ng_l2cap_evnt.h>
50 #include <netgraph/bluetooth/l2cap/ng_l2cap_llpi.h>
51 #include <netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h>
52 #include <netgraph/bluetooth/l2cap/ng_l2cap_misc.h>
53
54 /******************************************************************************
55 ******************************************************************************
56 ** L2CAP events processing module
57 ******************************************************************************
58 ******************************************************************************/
59
60 static int ng_l2cap_process_signal_cmd (ng_l2cap_con_p);
61 static int ng_l2cap_process_lesignal_cmd (ng_l2cap_con_p);
62 static int ng_l2cap_process_cmd_rej (ng_l2cap_con_p, u_int8_t);
63 static int ng_l2cap_process_cmd_urq (ng_l2cap_con_p, u_int8_t);
64 static int ng_l2cap_process_cmd_urs (ng_l2cap_con_p, u_int8_t);
65 static int ng_l2cap_process_con_req (ng_l2cap_con_p, u_int8_t);
66 static int ng_l2cap_process_con_rsp (ng_l2cap_con_p, u_int8_t);
67 static int ng_l2cap_process_cfg_req (ng_l2cap_con_p, u_int8_t);
68 static int ng_l2cap_process_cfg_rsp (ng_l2cap_con_p, u_int8_t);
69 static int ng_l2cap_process_discon_req (ng_l2cap_con_p, u_int8_t);
70 static int ng_l2cap_process_discon_rsp (ng_l2cap_con_p, u_int8_t);
71 static int ng_l2cap_process_echo_req (ng_l2cap_con_p, u_int8_t);
72 static int ng_l2cap_process_echo_rsp (ng_l2cap_con_p, u_int8_t);
73 static int ng_l2cap_process_info_req (ng_l2cap_con_p, u_int8_t);
74 static int ng_l2cap_process_info_rsp (ng_l2cap_con_p, u_int8_t);
75 static int send_l2cap_reject
76 (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, u_int16_t, u_int16_t);
77 static int send_l2cap_con_rej
78 (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, u_int16_t);
79 static int send_l2cap_cfg_rsp
80 (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, struct mbuf *);
81 static int send_l2cap_param_urs
82 (ng_l2cap_con_p , u_int8_t , u_int16_t);
83
84 static int get_next_l2cap_opt
85 (struct mbuf *, int *, ng_l2cap_cfg_opt_p, ng_l2cap_cfg_opt_val_p);
86
87 /*
88 * Receive L2CAP packet. First get L2CAP header and verify packet. Than
89 * get destination channel and process packet.
90 */
91
92 int
ng_l2cap_receive(ng_l2cap_con_p con)93 ng_l2cap_receive(ng_l2cap_con_p con)
94 {
95 ng_l2cap_p l2cap = con->l2cap;
96 ng_l2cap_hdr_t *hdr = NULL;
97 int error = 0;
98
99 /* Check packet */
100 if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) {
101 NG_L2CAP_ERR(
102 "%s: %s - invalid L2CAP packet. Packet too small, len=%d\n",
103 __func__, NG_NODE_NAME(l2cap->node),
104 con->rx_pkt->m_pkthdr.len);
105 error = EMSGSIZE;
106 goto drop;
107 }
108
109 /* Get L2CAP header */
110 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr));
111 if (con->rx_pkt == NULL)
112 return (ENOBUFS);
113
114 hdr = mtod(con->rx_pkt, ng_l2cap_hdr_t *);
115 hdr->length = le16toh(hdr->length);
116 hdr->dcid = le16toh(hdr->dcid);
117
118 /* Check payload size */
119 if (hdr->length != con->rx_pkt->m_pkthdr.len - sizeof(*hdr)) {
120 NG_L2CAP_ERR(
121 "%s: %s - invalid L2CAP packet. Payload length mismatch, length=%d, len=%zd\n",
122 __func__, NG_NODE_NAME(l2cap->node), hdr->length,
123 con->rx_pkt->m_pkthdr.len - sizeof(*hdr));
124 error = EMSGSIZE;
125 goto drop;
126 }
127
128 /* Process packet */
129 switch (hdr->dcid) {
130 case NG_L2CAP_SIGNAL_CID: /* L2CAP command */
131 m_adj(con->rx_pkt, sizeof(*hdr));
132 error = ng_l2cap_process_signal_cmd(con);
133 break;
134 case NG_L2CAP_LESIGNAL_CID:
135 m_adj(con->rx_pkt, sizeof(*hdr));
136 error = ng_l2cap_process_lesignal_cmd(con);
137 break;
138 case NG_L2CAP_CLT_CID: /* Connectionless packet */
139 error = ng_l2cap_l2ca_clt_receive(con);
140 break;
141
142 default: /* Data packet */
143 error = ng_l2cap_l2ca_receive(con);
144 break;
145 }
146
147 return (error);
148 drop:
149 NG_FREE_M(con->rx_pkt);
150
151 return (error);
152 } /* ng_l2cap_receive */
153
154 /*
155 * Process L2CAP signaling command. We already know that destination channel ID
156 * is 0x1 that means we have received signaling command from peer's L2CAP layer.
157 * So get command header, decode and process it.
158 *
159 * XXX do we need to check signaling MTU here?
160 */
161
162 static int
ng_l2cap_process_signal_cmd(ng_l2cap_con_p con)163 ng_l2cap_process_signal_cmd(ng_l2cap_con_p con)
164 {
165 ng_l2cap_p l2cap = con->l2cap;
166 ng_l2cap_cmd_hdr_t *hdr = NULL;
167 struct mbuf *m = NULL;
168
169 while (con->rx_pkt != NULL) {
170 /* Verify packet length */
171 if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) {
172 NG_L2CAP_ERR(
173 "%s: %s - invalid L2CAP signaling command. Packet too small, len=%d\n",
174 __func__, NG_NODE_NAME(l2cap->node),
175 con->rx_pkt->m_pkthdr.len);
176 NG_FREE_M(con->rx_pkt);
177
178 return (EMSGSIZE);
179 }
180
181 /* Get signaling command */
182 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr));
183 if (con->rx_pkt == NULL)
184 return (ENOBUFS);
185
186 hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *);
187 hdr->length = le16toh(hdr->length);
188 m_adj(con->rx_pkt, sizeof(*hdr));
189
190 /* Verify command length */
191 if (con->rx_pkt->m_pkthdr.len < hdr->length) {
192 NG_L2CAP_ERR(
193 "%s: %s - invalid L2CAP signaling command, code=%#x, ident=%d. " \
194 "Invalid command length=%d, m_pkthdr.len=%d\n",
195 __func__, NG_NODE_NAME(l2cap->node),
196 hdr->code, hdr->ident, hdr->length,
197 con->rx_pkt->m_pkthdr.len);
198 NG_FREE_M(con->rx_pkt);
199
200 return (EMSGSIZE);
201 }
202
203 /* Get the command, save the rest (if any) */
204 if (con->rx_pkt->m_pkthdr.len > hdr->length)
205 m = m_split(con->rx_pkt, hdr->length, M_NOWAIT);
206 else
207 m = NULL;
208
209 /* Process command */
210 switch (hdr->code) {
211 case NG_L2CAP_CMD_REJ:
212 ng_l2cap_process_cmd_rej(con, hdr->ident);
213 break;
214
215 case NG_L2CAP_CON_REQ:
216 ng_l2cap_process_con_req(con, hdr->ident);
217 break;
218
219 case NG_L2CAP_CON_RSP:
220 ng_l2cap_process_con_rsp(con, hdr->ident);
221 break;
222
223 case NG_L2CAP_CFG_REQ:
224 ng_l2cap_process_cfg_req(con, hdr->ident);
225 break;
226
227 case NG_L2CAP_CFG_RSP:
228 ng_l2cap_process_cfg_rsp(con, hdr->ident);
229 break;
230
231 case NG_L2CAP_DISCON_REQ:
232 ng_l2cap_process_discon_req(con, hdr->ident);
233 break;
234
235 case NG_L2CAP_DISCON_RSP:
236 ng_l2cap_process_discon_rsp(con, hdr->ident);
237 break;
238
239 case NG_L2CAP_ECHO_REQ:
240 ng_l2cap_process_echo_req(con, hdr->ident);
241 break;
242
243 case NG_L2CAP_ECHO_RSP:
244 ng_l2cap_process_echo_rsp(con, hdr->ident);
245 break;
246
247 case NG_L2CAP_INFO_REQ:
248 ng_l2cap_process_info_req(con, hdr->ident);
249 break;
250
251 case NG_L2CAP_INFO_RSP:
252 ng_l2cap_process_info_rsp(con, hdr->ident);
253 break;
254
255 default:
256 NG_L2CAP_ERR(
257 "%s: %s - unknown L2CAP signaling command, code=%#x, ident=%d, length=%d\n",
258 __func__, NG_NODE_NAME(l2cap->node),
259 hdr->code, hdr->ident, hdr->length);
260
261 /*
262 * Send L2CAP_CommandRej. Do not really care
263 * about the result
264 */
265
266 send_l2cap_reject(con, hdr->ident,
267 NG_L2CAP_REJ_NOT_UNDERSTOOD, 0, 0, 0);
268 NG_FREE_M(con->rx_pkt);
269 break;
270 }
271
272 con->rx_pkt = m;
273 }
274
275 return (0);
276 } /* ng_l2cap_process_signal_cmd */
277 static int
ng_l2cap_process_lesignal_cmd(ng_l2cap_con_p con)278 ng_l2cap_process_lesignal_cmd(ng_l2cap_con_p con)
279 {
280 ng_l2cap_p l2cap = con->l2cap;
281 ng_l2cap_cmd_hdr_t *hdr = NULL;
282 struct mbuf *m = NULL;
283
284 while (con->rx_pkt != NULL) {
285 /* Verify packet length */
286 if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) {
287 NG_L2CAP_ERR(
288 "%s: %s - invalid L2CAP signaling command. Packet too small, len=%d\n",
289 __func__, NG_NODE_NAME(l2cap->node),
290 con->rx_pkt->m_pkthdr.len);
291 NG_FREE_M(con->rx_pkt);
292
293 return (EMSGSIZE);
294 }
295
296 /* Get signaling command */
297 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr));
298 if (con->rx_pkt == NULL)
299 return (ENOBUFS);
300
301 hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *);
302 hdr->length = le16toh(hdr->length);
303 m_adj(con->rx_pkt, sizeof(*hdr));
304
305 /* Verify command length */
306 if (con->rx_pkt->m_pkthdr.len < hdr->length) {
307 NG_L2CAP_ERR(
308 "%s: %s - invalid L2CAP signaling command, code=%#x, ident=%d. " \
309 "Invalid command length=%d, m_pkthdr.len=%d\n",
310 __func__, NG_NODE_NAME(l2cap->node),
311 hdr->code, hdr->ident, hdr->length,
312 con->rx_pkt->m_pkthdr.len);
313 NG_FREE_M(con->rx_pkt);
314
315 return (EMSGSIZE);
316 }
317
318 /* Get the command, save the rest (if any) */
319 if (con->rx_pkt->m_pkthdr.len > hdr->length)
320 m = m_split(con->rx_pkt, hdr->length, M_NOWAIT);
321 else
322 m = NULL;
323
324 /* Process command */
325 switch (hdr->code) {
326 case NG_L2CAP_CMD_REJ:
327 ng_l2cap_process_cmd_rej(con, hdr->ident);
328 break;
329 case NG_L2CAP_CMD_PARAM_UPDATE_REQUEST:
330 ng_l2cap_process_cmd_urq(con, hdr->ident);
331 break;
332 case NG_L2CAP_CMD_PARAM_UPDATE_RESPONSE:
333 ng_l2cap_process_cmd_urs(con, hdr->ident);
334 break;
335
336
337 default:
338 NG_L2CAP_ERR(
339 "%s: %s - unknown L2CAP signaling command, code=%#x, ident=%d, length=%d\n",
340 __func__, NG_NODE_NAME(l2cap->node),
341 hdr->code, hdr->ident, hdr->length);
342
343 /*
344 * Send L2CAP_CommandRej. Do not really care
345 * about the result
346 */
347
348 send_l2cap_reject(con, hdr->ident,
349 NG_L2CAP_REJ_NOT_UNDERSTOOD, 0, 0, 0);
350 NG_FREE_M(con->rx_pkt);
351 break;
352 }
353
354 con->rx_pkt = m;
355 }
356
357 return (0);
358 } /* ng_l2cap_process_signal_cmd */
359 /*Update Paramater Request*/
ng_l2cap_process_cmd_urq(ng_l2cap_con_p con,uint8_t ident)360 static int ng_l2cap_process_cmd_urq(ng_l2cap_con_p con, uint8_t ident)
361 {
362 /* We do not implement parameter negotiation for now. */
363 send_l2cap_param_urs(con, ident, NG_L2CAP_UPDATE_PARAM_ACCEPT);
364 NG_FREE_M(con->rx_pkt);
365 return 0;
366 }
367
ng_l2cap_process_cmd_urs(ng_l2cap_con_p con,uint8_t ident)368 static int ng_l2cap_process_cmd_urs(ng_l2cap_con_p con, uint8_t ident)
369 {
370 /* We only support master side yet .*/
371 //send_l2cap_reject(con,ident ... );
372
373 NG_FREE_M(con->rx_pkt);
374 return 0;
375 }
376
377 /*
378 * Process L2CAP_CommandRej command
379 */
380
381 static int
ng_l2cap_process_cmd_rej(ng_l2cap_con_p con,u_int8_t ident)382 ng_l2cap_process_cmd_rej(ng_l2cap_con_p con, u_int8_t ident)
383 {
384 ng_l2cap_p l2cap = con->l2cap;
385 ng_l2cap_cmd_rej_cp *cp = NULL;
386 ng_l2cap_cmd_p cmd = NULL;
387
388 /* Get command parameters */
389 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp));
390 if (con->rx_pkt == NULL)
391 return (ENOBUFS);
392
393 cp = mtod(con->rx_pkt, ng_l2cap_cmd_rej_cp *);
394 cp->reason = le16toh(cp->reason);
395
396 /* Check if we have pending command descriptor */
397 cmd = ng_l2cap_cmd_by_ident(con, ident);
398 if (cmd != NULL) {
399 /* If command timeout already happened then ignore reject */
400 if (ng_l2cap_command_untimeout(cmd) != 0) {
401 NG_FREE_M(con->rx_pkt);
402 return (ETIMEDOUT);
403 }
404
405 ng_l2cap_unlink_cmd(cmd);
406
407 switch (cmd->code) {
408 case NG_L2CAP_CON_REQ:
409 ng_l2cap_l2ca_con_rsp(cmd->ch,cmd->token,cp->reason,0);
410 ng_l2cap_free_chan(cmd->ch);
411 break;
412
413 case NG_L2CAP_CFG_REQ:
414 ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, cp->reason);
415 break;
416
417 case NG_L2CAP_DISCON_REQ:
418 ng_l2cap_l2ca_discon_rsp(cmd->ch,cmd->token,cp->reason);
419 ng_l2cap_free_chan(cmd->ch); /* XXX free channel */
420 break;
421
422 case NG_L2CAP_ECHO_REQ:
423 ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token,
424 cp->reason, NULL);
425 break;
426
427 case NG_L2CAP_INFO_REQ:
428 ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token,
429 cp->reason, NULL);
430 break;
431
432 default:
433 NG_L2CAP_ALERT(
434 "%s: %s - unexpected L2CAP_CommandRej. Unexpected L2CAP command opcode=%d\n",
435 __func__, NG_NODE_NAME(l2cap->node), cmd->code);
436 break;
437 }
438
439 ng_l2cap_free_cmd(cmd);
440 } else
441 NG_L2CAP_ERR(
442 "%s: %s - unexpected L2CAP_CommandRej command. " \
443 "Requested ident does not exist, ident=%d\n",
444 __func__, NG_NODE_NAME(l2cap->node), ident);
445
446 NG_FREE_M(con->rx_pkt);
447
448 return (0);
449 } /* ng_l2cap_process_cmd_rej */
450
451 /*
452 * Process L2CAP_ConnectReq command
453 */
454
455 static int
ng_l2cap_process_con_req(ng_l2cap_con_p con,u_int8_t ident)456 ng_l2cap_process_con_req(ng_l2cap_con_p con, u_int8_t ident)
457 {
458 ng_l2cap_p l2cap = con->l2cap;
459 struct mbuf *m = con->rx_pkt;
460 ng_l2cap_con_req_cp *cp = NULL;
461 ng_l2cap_chan_p ch = NULL;
462 int error = 0;
463 u_int16_t dcid, psm;
464 int idtype;
465
466 /* Get command parameters */
467 NG_L2CAP_M_PULLUP(m, sizeof(*cp));
468 if (m == NULL)
469 return (ENOBUFS);
470
471 cp = mtod(m, ng_l2cap_con_req_cp *);
472 psm = le16toh(cp->psm);
473 dcid = le16toh(cp->scid);
474
475 NG_FREE_M(m);
476 con->rx_pkt = NULL;
477 if(dcid == NG_L2CAP_ATT_CID)
478 idtype = NG_L2CAP_L2CA_IDTYPE_ATT;
479 else if(dcid == NG_L2CAP_SMP_CID)
480 idtype = NG_L2CAP_L2CA_IDTYPE_SMP;
481 else if( con->linktype != NG_HCI_LINK_ACL)
482 idtype = NG_L2CAP_L2CA_IDTYPE_LE;
483 else
484 idtype = NG_L2CAP_L2CA_IDTYPE_BREDR;
485
486 /*
487 * Create new channel and send L2CA_ConnectInd notification
488 * to the upper layer protocol.
489 */
490
491 ch = ng_l2cap_new_chan(l2cap, con, psm, idtype);
492
493 if (ch == NULL)
494 return (send_l2cap_con_rej(con, ident, 0, dcid,
495 NG_L2CAP_NO_RESOURCES));
496
497 /* Update channel IDs */
498 ch->dcid = dcid;
499
500 /* Sent L2CA_ConnectInd notification to the upper layer */
501 ch->ident = ident;
502 ch->state = NG_L2CAP_W4_L2CA_CON_RSP;
503
504 error = ng_l2cap_l2ca_con_ind(ch);
505 if (error != 0) {
506 send_l2cap_con_rej(con, ident, ch->scid, dcid,
507 (error == ENOMEM)? NG_L2CAP_NO_RESOURCES :
508 NG_L2CAP_PSM_NOT_SUPPORTED);
509 ng_l2cap_free_chan(ch);
510 }
511
512 return (error);
513 } /* ng_l2cap_process_con_req */
514
515 /*
516 * Process L2CAP_ConnectRsp command
517 */
518
519 static int
ng_l2cap_process_con_rsp(ng_l2cap_con_p con,u_int8_t ident)520 ng_l2cap_process_con_rsp(ng_l2cap_con_p con, u_int8_t ident)
521 {
522 ng_l2cap_p l2cap = con->l2cap;
523 struct mbuf *m = con->rx_pkt;
524 ng_l2cap_con_rsp_cp *cp = NULL;
525 ng_l2cap_cmd_p cmd = NULL;
526 u_int16_t scid, dcid, result, status;
527 int error = 0;
528
529 /* Get command parameters */
530 NG_L2CAP_M_PULLUP(m, sizeof(*cp));
531 if (m == NULL)
532 return (ENOBUFS);
533
534 cp = mtod(m, ng_l2cap_con_rsp_cp *);
535 dcid = le16toh(cp->dcid);
536 scid = le16toh(cp->scid);
537 result = le16toh(cp->result);
538 status = le16toh(cp->status);
539
540 NG_FREE_M(m);
541 con->rx_pkt = NULL;
542
543 /* Check if we have pending command descriptor */
544 cmd = ng_l2cap_cmd_by_ident(con, ident);
545 if (cmd == NULL) {
546 NG_L2CAP_ERR(
547 "%s: %s - unexpected L2CAP_ConnectRsp command. ident=%d, con_handle=%d\n",
548 __func__, NG_NODE_NAME(l2cap->node), ident,
549 con->con_handle);
550
551 return (ENOENT);
552 }
553
554 /* Verify channel state, if invalid - do nothing */
555 if (cmd->ch->state != NG_L2CAP_W4_L2CAP_CON_RSP) {
556 NG_L2CAP_ERR(
557 "%s: %s - unexpected L2CAP_ConnectRsp. " \
558 "Invalid channel state, cid=%d, state=%d\n",
559 __func__, NG_NODE_NAME(l2cap->node), scid,
560 cmd->ch->state);
561 goto reject;
562 }
563
564 /* Verify CIDs and send reject if does not match */
565 if (cmd->ch->scid != scid) {
566 NG_L2CAP_ERR(
567 "%s: %s - unexpected L2CAP_ConnectRsp. Channel IDs do not match, scid=%d(%d)\n",
568 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid,
569 scid);
570 goto reject;
571 }
572
573 /*
574 * Looks good. We got confirmation from our peer. Now process
575 * it. First disable RTX timer. Then check the result and send
576 * notification to the upper layer. If command timeout already
577 * happened then ignore response.
578 */
579
580 if ((error = ng_l2cap_command_untimeout(cmd)) != 0)
581 return (error);
582
583 if (result == NG_L2CAP_PENDING) {
584 /*
585 * Our peer wants more time to complete connection. We shall
586 * start ERTX timer and wait. Keep command in the list.
587 */
588
589 cmd->ch->dcid = dcid;
590 ng_l2cap_command_timeout(cmd, bluetooth_l2cap_ertx_timeout());
591
592 error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token,
593 result, status);
594 if (error != 0)
595 ng_l2cap_free_chan(cmd->ch);
596 } else {
597 ng_l2cap_unlink_cmd(cmd);
598
599 if (result == NG_L2CAP_SUCCESS) {
600 /*
601 * Channel is open. Complete command and move to CONFIG
602 * state. Since we have sent positive confirmation we
603 * expect to receive L2CA_Config request from the upper
604 * layer protocol.
605 */
606
607 cmd->ch->dcid = dcid;
608 cmd->ch->state = ((cmd->ch->scid == NG_L2CAP_ATT_CID)||
609 (cmd->ch->scid == NG_L2CAP_SMP_CID))
610 ?
611 NG_L2CAP_OPEN : NG_L2CAP_CONFIG;
612 } else
613 /* There was an error, so close the channel */
614 NG_L2CAP_INFO(
615 "%s: %s - failed to open L2CAP channel, result=%d, status=%d\n",
616 __func__, NG_NODE_NAME(l2cap->node), result,
617 status);
618
619 error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token,
620 result, status);
621
622 /* XXX do we have to remove the channel on error? */
623 if (error != 0 || result != NG_L2CAP_SUCCESS)
624 ng_l2cap_free_chan(cmd->ch);
625
626 ng_l2cap_free_cmd(cmd);
627 }
628
629 return (error);
630
631 reject:
632 /* Send reject. Do not really care about the result */
633 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid);
634
635 return (0);
636 } /* ng_l2cap_process_con_rsp */
637
638 /*
639 * Process L2CAP_ConfigReq command
640 */
641
642 static int
ng_l2cap_process_cfg_req(ng_l2cap_con_p con,u_int8_t ident)643 ng_l2cap_process_cfg_req(ng_l2cap_con_p con, u_int8_t ident)
644 {
645 ng_l2cap_p l2cap = con->l2cap;
646 struct mbuf *m = con->rx_pkt;
647 ng_l2cap_cfg_req_cp *cp = NULL;
648 ng_l2cap_chan_p ch = NULL;
649 u_int16_t dcid, respond, result;
650 ng_l2cap_cfg_opt_t hdr;
651 ng_l2cap_cfg_opt_val_t val;
652 int off, error = 0;
653
654 /* Get command parameters */
655 con->rx_pkt = NULL;
656 NG_L2CAP_M_PULLUP(m, sizeof(*cp));
657 if (m == NULL)
658 return (ENOBUFS);
659
660 cp = mtod(m, ng_l2cap_cfg_req_cp *);
661 dcid = le16toh(cp->dcid);
662 respond = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags));
663 m_adj(m, sizeof(*cp));
664
665 /* Check if we have this channel and it is in valid state */
666 ch = ng_l2cap_chan_by_scid(l2cap, dcid, NG_L2CAP_L2CA_IDTYPE_BREDR);
667 if (ch == NULL) {
668 NG_L2CAP_ERR(
669 "%s: %s - unexpected L2CAP_ConfigReq command. " \
670 "Channel does not exist, cid=%d\n",
671 __func__, NG_NODE_NAME(l2cap->node), dcid);
672 goto reject;
673 }
674
675 /* Verify channel state */
676 if (ch->state != NG_L2CAP_CONFIG && ch->state != NG_L2CAP_OPEN) {
677 NG_L2CAP_ERR(
678 "%s: %s - unexpected L2CAP_ConfigReq. " \
679 "Invalid channel state, cid=%d, state=%d\n",
680 __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state);
681 goto reject;
682 }
683
684 if (ch->state == NG_L2CAP_OPEN) { /* Re-configuration */
685 ch->cfg_state = 0;
686 ch->state = NG_L2CAP_CONFIG;
687 }
688
689 for (result = 0, off = 0; ; ) {
690 error = get_next_l2cap_opt(m, &off, &hdr, &val);
691 if (error == 0) { /* We done with this packet */
692 NG_FREE_M(m);
693 break;
694 } else if (error > 0) { /* Got option */
695 switch (hdr.type) {
696 case NG_L2CAP_OPT_MTU:
697 ch->omtu = val.mtu;
698 break;
699
700 case NG_L2CAP_OPT_FLUSH_TIMO:
701 ch->flush_timo = val.flush_timo;
702 break;
703
704 case NG_L2CAP_OPT_QOS:
705 bcopy(&val.flow, &ch->iflow, sizeof(ch->iflow));
706 break;
707
708 default: /* Ignore unknown hint option */
709 break;
710 }
711 } else { /* Oops, something is wrong */
712 respond = 1;
713
714 if (error == -3) {
715 /*
716 * Adjust mbuf so we can get to the start
717 * of the first option we did not like.
718 */
719
720 m_adj(m, off - sizeof(hdr));
721 m->m_pkthdr.len = sizeof(hdr) + hdr.length;
722
723 result = NG_L2CAP_UNKNOWN_OPTION;
724 } else {
725 /* XXX FIXME Send other reject codes? */
726 NG_FREE_M(m);
727 result = NG_L2CAP_REJECT;
728 }
729
730 break;
731 }
732 }
733
734 /*
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.
738 *
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.
741 *
742 * When "respond == 0" than we have received all options and we will
743 * sent L2CA_ConfigInd event to the upper layer protocol.
744 */
745
746 if (respond) {
747 error = send_l2cap_cfg_rsp(con, ident, ch->dcid, result, m);
748 if (error != 0) {
749 ng_l2cap_l2ca_discon_ind(ch);
750 ng_l2cap_free_chan(ch);
751 }
752 } else {
753 /* Send L2CA_ConfigInd event to the upper layer protocol */
754 ch->ident = ident;
755 error = ng_l2cap_l2ca_cfg_ind(ch);
756 if (error != 0)
757 ng_l2cap_free_chan(ch);
758 }
759
760 return (error);
761
762 reject:
763 /* Send reject. Do not really care about the result */
764 NG_FREE_M(m);
765
766 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, 0, dcid);
767
768 return (0);
769 } /* ng_l2cap_process_cfg_req */
770
771 /*
772 * Process L2CAP_ConfigRsp command
773 */
774
775 static int
ng_l2cap_process_cfg_rsp(ng_l2cap_con_p con,u_int8_t ident)776 ng_l2cap_process_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident)
777 {
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;
785 int off, error = 0;
786
787 /* Get command parameters */
788 con->rx_pkt = NULL;
789 NG_L2CAP_M_PULLUP(m, sizeof(*cp));
790 if (m == NULL)
791 return (ENOBUFS);
792
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));
798
799 /* Check if we have this command */
800 cmd = ng_l2cap_cmd_by_ident(con, ident);
801 if (cmd == NULL) {
802 NG_L2CAP_ERR(
803 "%s: %s - unexpected L2CAP_ConfigRsp command. ident=%d, con_handle=%d\n",
804 __func__, NG_NODE_NAME(l2cap->node), ident,
805 con->con_handle);
806 NG_FREE_M(m);
807
808 return (ENOENT);
809 }
810
811 /* Verify CIDs and send reject if does not match */
812 if (cmd->ch->scid != scid) {
813 NG_L2CAP_ERR(
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,
817 scid);
818 goto reject;
819 }
820
821 /* Verify channel state and reject if invalid */
822 if (cmd->ch->state != NG_L2CAP_CONFIG) {
823 NG_L2CAP_ERR(
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,
827 cmd->ch->state);
828 goto reject;
829 }
830
831 /*
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
837 * ignore response.
838 */
839
840 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) {
841 NG_FREE_M(m);
842 return (error);
843 }
844
845 for (off = 0; ; ) {
846 error = get_next_l2cap_opt(m, &off, &hdr, &val);
847 if (error == 0) /* We done with this packet */
848 break;
849 else if (error > 0) { /* Got option */
850 switch (hdr.type) {
851 case NG_L2CAP_OPT_MTU:
852 cmd->ch->imtu = val.mtu;
853 break;
854
855 case NG_L2CAP_OPT_FLUSH_TIMO:
856 cmd->ch->flush_timo = val.flush_timo;
857 break;
858
859 case NG_L2CAP_OPT_QOS:
860 bcopy(&val.flow, &cmd->ch->oflow,
861 sizeof(cmd->ch->oflow));
862 break;
863
864 default: /* Ignore unknown hint option */
865 break;
866 }
867 } else {
868 /*
869 * XXX FIXME What to do here?
870 *
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.
874 */
875
876 NG_L2CAP_ALERT(
877 "%s: %s - failed to parse configuration options, error=%d\n",
878 __func__, NG_NODE_NAME(l2cap->node), error);
879
880 result = NG_L2CAP_UNKNOWN;
881 cflag = 0;
882
883 break;
884 }
885 }
886
887 NG_FREE_M(m);
888
889 if (cflag) /* Restart timer and wait for more options */
890 ng_l2cap_command_timeout(cmd, bluetooth_l2cap_rtx_timeout());
891 else {
892 ng_l2cap_unlink_cmd(cmd);
893
894 /* Send L2CA_Config response to the upper layer protocol */
895 error = ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, result);
896 if (error != 0) {
897 /*
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
901 * remote peer?
902 */
903
904 NG_L2CAP_ERR(
905 "%s: %s - failed to send L2CA_Config response, error=%d\n",
906 __func__, NG_NODE_NAME(l2cap->node), error);
907
908 ng_l2cap_free_chan(cmd->ch);
909 }
910
911 ng_l2cap_free_cmd(cmd);
912 }
913
914 return (error);
915
916 reject:
917 /* Send reject. Do not really care about the result */
918 NG_FREE_M(m);
919
920 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, 0);
921
922 return (0);
923 } /* ng_l2cap_process_cfg_rsp */
924
925 /*
926 * Process L2CAP_DisconnectReq command
927 */
928
929 static int
ng_l2cap_process_discon_req(ng_l2cap_con_p con,u_int8_t ident)930 ng_l2cap_process_discon_req(ng_l2cap_con_p con, u_int8_t ident)
931 {
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;
937
938 /* Get command parameters */
939 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp));
940 if (con->rx_pkt == NULL)
941 return (ENOBUFS);
942
943 cp = mtod(con->rx_pkt, ng_l2cap_discon_req_cp *);
944 dcid = le16toh(cp->dcid);
945 scid = le16toh(cp->scid);
946
947 NG_FREE_M(con->rx_pkt);
948
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);
951 if (ch == NULL) {
952 NG_L2CAP_ERR(
953 "%s: %s - unexpected L2CAP_DisconnectReq message. " \
954 "Channel does not exist, cid=%d\n",
955 __func__, NG_NODE_NAME(l2cap->node), dcid);
956 goto reject;
957 }
958
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) {
962 NG_L2CAP_ERR(
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);
966 goto reject;
967 }
968
969 /* Match destination channel ID */
970 if (ch->dcid != scid || ch->scid != dcid) {
971 NG_L2CAP_ERR(
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,
976 scid, dcid);
977 goto reject;
978 }
979
980 /*
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.
984 */
985
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);
989 }
990
991 /* Send L2CAP_DisconnectRsp */
992 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_DISCON_RSP, 0);
993 if (cmd == NULL)
994 return (ENOMEM);
995
996 _ng_l2cap_discon_rsp(cmd->aux, ident, dcid, scid);
997 if (cmd->aux == NULL) {
998 ng_l2cap_free_cmd(cmd);
999
1000 return (ENOBUFS);
1001 }
1002
1003 /* Link command to the queue */
1004 ng_l2cap_link_cmd(con, cmd);
1005 ng_l2cap_lp_deliver(con);
1006
1007 return (0);
1008
1009 reject:
1010 /* Send reject. Do not really care about the result */
1011 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid);
1012
1013 return (0);
1014 } /* ng_l2cap_process_discon_req */
1015
1016 /*
1017 * Process L2CAP_DisconnectRsp command
1018 */
1019
1020 static int
ng_l2cap_process_discon_rsp(ng_l2cap_con_p con,u_int8_t ident)1021 ng_l2cap_process_discon_rsp(ng_l2cap_con_p con, u_int8_t ident)
1022 {
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;
1027 int error = 0;
1028
1029 /* Get command parameters */
1030 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp));
1031 if (con->rx_pkt == NULL)
1032 return (ENOBUFS);
1033
1034 cp = mtod(con->rx_pkt, ng_l2cap_discon_rsp_cp *);
1035 dcid = le16toh(cp->dcid);
1036 scid = le16toh(cp->scid);
1037
1038 NG_FREE_M(con->rx_pkt);
1039
1040 /* Check if we have pending command descriptor */
1041 cmd = ng_l2cap_cmd_by_ident(con, ident);
1042 if (cmd == NULL) {
1043 NG_L2CAP_ERR(
1044 "%s: %s - unexpected L2CAP_DisconnectRsp command. ident=%d, con_handle=%d\n",
1045 __func__, NG_NODE_NAME(l2cap->node), ident,
1046 con->con_handle);
1047 goto out;
1048 }
1049
1050 /* Verify channel state, do nothing if invalid */
1051 if (cmd->ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) {
1052 NG_L2CAP_ERR(
1053 "%s: %s - unexpected L2CAP_DisconnectRsp. " \
1054 "Invalid channel state, cid=%d, state=%d\n",
1055 __func__, NG_NODE_NAME(l2cap->node), scid,
1056 cmd->ch->state);
1057 goto out;
1058 }
1059
1060 /* Verify CIDs and send reject if does not match */
1061 if (cmd->ch->scid != scid || cmd->ch->dcid != dcid) {
1062 NG_L2CAP_ERR(
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);
1067 goto out;
1068 }
1069
1070 /*
1071 * Looks like we have successfully disconnected channel, so notify
1072 * upper layer. If command timeout already happened then ignore
1073 * response.
1074 */
1075
1076 if ((error = ng_l2cap_command_untimeout(cmd)) != 0)
1077 goto out;
1078
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 */
1081 out:
1082 return (error);
1083 } /* ng_l2cap_process_discon_rsp */
1084
1085 /*
1086 * Process L2CAP_EchoReq command
1087 */
1088
1089 static int
ng_l2cap_process_echo_req(ng_l2cap_con_p con,u_int8_t ident)1090 ng_l2cap_process_echo_req(ng_l2cap_con_p con, u_int8_t ident)
1091 {
1092 ng_l2cap_p l2cap = con->l2cap;
1093 ng_l2cap_cmd_hdr_t *hdr = NULL;
1094 ng_l2cap_cmd_p cmd = NULL;
1095
1096 con->rx_pkt = ng_l2cap_prepend(con->rx_pkt, sizeof(*hdr));
1097 if (con->rx_pkt == NULL) {
1098 NG_L2CAP_ALERT(
1099 "%s: %s - ng_l2cap_prepend() failed, size=%zd\n",
1100 __func__, NG_NODE_NAME(l2cap->node), sizeof(*hdr));
1101
1102 return (ENOBUFS);
1103 }
1104
1105 hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *);
1106 hdr->code = NG_L2CAP_ECHO_RSP;
1107 hdr->ident = ident;
1108 hdr->length = htole16(con->rx_pkt->m_pkthdr.len - sizeof(*hdr));
1109
1110 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_ECHO_RSP, 0);
1111 if (cmd == NULL) {
1112 NG_FREE_M(con->rx_pkt);
1113
1114 return (ENOBUFS);
1115 }
1116
1117 /* Attach data and link command to the queue */
1118 cmd->aux = con->rx_pkt;
1119 con->rx_pkt = NULL;
1120 ng_l2cap_link_cmd(con, cmd);
1121 ng_l2cap_lp_deliver(con);
1122
1123 return (0);
1124 } /* ng_l2cap_process_echo_req */
1125
1126 /*
1127 * Process L2CAP_EchoRsp command
1128 */
1129
1130 static int
ng_l2cap_process_echo_rsp(ng_l2cap_con_p con,u_int8_t ident)1131 ng_l2cap_process_echo_rsp(ng_l2cap_con_p con, u_int8_t ident)
1132 {
1133 ng_l2cap_p l2cap = con->l2cap;
1134 ng_l2cap_cmd_p cmd = NULL;
1135 int error = 0;
1136
1137 /* Check if we have this command */
1138 cmd = ng_l2cap_cmd_by_ident(con, ident);
1139 if (cmd != NULL) {
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);
1143 return (error);
1144 }
1145
1146 ng_l2cap_unlink_cmd(cmd);
1147
1148 error = ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token,
1149 NG_L2CAP_SUCCESS, con->rx_pkt);
1150
1151 ng_l2cap_free_cmd(cmd);
1152 con->rx_pkt = NULL;
1153 } else {
1154 NG_L2CAP_ERR(
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);
1159 }
1160
1161 return (error);
1162 } /* ng_l2cap_process_echo_rsp */
1163
1164 /*
1165 * Process L2CAP_InfoReq command
1166 */
1167
1168 static int
ng_l2cap_process_info_req(ng_l2cap_con_p con,u_int8_t ident)1169 ng_l2cap_process_info_req(ng_l2cap_con_p con, u_int8_t ident)
1170 {
1171 ng_l2cap_p l2cap = con->l2cap;
1172 ng_l2cap_cmd_p cmd = NULL;
1173 u_int16_t type;
1174
1175 /* Get command parameters */
1176 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(ng_l2cap_info_req_cp));
1177 if (con->rx_pkt == NULL)
1178 return (ENOBUFS);
1179
1180 type = le16toh(mtod(con->rx_pkt, ng_l2cap_info_req_cp *)->type);
1181 NG_FREE_M(con->rx_pkt);
1182
1183 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_INFO_RSP, 0);
1184 if (cmd == NULL)
1185 return (ENOMEM);
1186
1187 switch (type) {
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);
1191 break;
1192
1193 default:
1194 _ng_l2cap_info_rsp(cmd->aux, ident, type,
1195 NG_L2CAP_NOT_SUPPORTED, 0);
1196 break;
1197 }
1198
1199 if (cmd->aux == NULL) {
1200 ng_l2cap_free_cmd(cmd);
1201
1202 return (ENOBUFS);
1203 }
1204
1205 /* Link command to the queue */
1206 ng_l2cap_link_cmd(con, cmd);
1207 ng_l2cap_lp_deliver(con);
1208
1209 return (0);
1210 } /* ng_l2cap_process_info_req */
1211
1212 /*
1213 * Process L2CAP_InfoRsp command
1214 */
1215
1216 static int
ng_l2cap_process_info_rsp(ng_l2cap_con_p con,u_int8_t ident)1217 ng_l2cap_process_info_rsp(ng_l2cap_con_p con, u_int8_t ident)
1218 {
1219 ng_l2cap_p l2cap = con->l2cap;
1220 ng_l2cap_info_rsp_cp *cp = NULL;
1221 ng_l2cap_cmd_p cmd = NULL;
1222 int error = 0;
1223
1224 /* Get command parameters */
1225 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp));
1226 if (con->rx_pkt == NULL)
1227 return (ENOBUFS);
1228
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));
1233
1234 /* Check if we have pending command descriptor */
1235 cmd = ng_l2cap_cmd_by_ident(con, ident);
1236 if (cmd == NULL) {
1237 NG_L2CAP_ERR(
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);
1242
1243 return (ENOENT);
1244 }
1245
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);
1249 return (error);
1250 }
1251
1252 ng_l2cap_unlink_cmd(cmd);
1253
1254 if (cp->result == NG_L2CAP_SUCCESS) {
1255 switch (cp->type) {
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 *));
1260 else {
1261 cp->result = NG_L2CAP_UNKNOWN; /* XXX */
1262
1263 NG_L2CAP_ERR(
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);
1268 }
1269 break;
1270
1271 default:
1272 NG_L2CAP_WARN(
1273 "%s: %s - invalid L2CAP_InfoRsp command. Unknown info type=%d\n",
1274 __func__, NG_NODE_NAME(l2cap->node), cp->type);
1275 break;
1276 }
1277 }
1278
1279 error = ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token,
1280 cp->result, con->rx_pkt);
1281
1282 ng_l2cap_free_cmd(cmd);
1283 con->rx_pkt = NULL;
1284
1285 return (error);
1286 } /* ng_l2cap_process_info_rsp */
1287
1288 /*
1289 * Send L2CAP reject
1290 */
1291
1292 static int
send_l2cap_reject(ng_l2cap_con_p con,u_int8_t ident,u_int16_t reason,u_int16_t mtu,u_int16_t scid,u_int16_t dcid)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)
1295 {
1296 ng_l2cap_cmd_p cmd = NULL;
1297
1298 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CMD_REJ, 0);
1299 if (cmd == NULL)
1300 return (ENOMEM);
1301
1302 _ng_l2cap_cmd_rej(cmd->aux, cmd->ident, reason, mtu, scid, dcid);
1303 if (cmd->aux == NULL) {
1304 ng_l2cap_free_cmd(cmd);
1305
1306 return (ENOBUFS);
1307 }
1308
1309 /* Link command to the queue */
1310 ng_l2cap_link_cmd(con, cmd);
1311 ng_l2cap_lp_deliver(con);
1312
1313 return (0);
1314 } /* send_l2cap_reject */
1315
1316 /*
1317 * Send L2CAP connection reject
1318 */
1319
1320 static int
send_l2cap_con_rej(ng_l2cap_con_p con,u_int8_t ident,u_int16_t scid,u_int16_t dcid,u_int16_t result)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)
1323 {
1324 ng_l2cap_cmd_p cmd = NULL;
1325
1326 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CON_RSP, 0);
1327 if (cmd == NULL)
1328 return (ENOMEM);
1329
1330 _ng_l2cap_con_rsp(cmd->aux, cmd->ident, scid, dcid, result, 0);
1331 if (cmd->aux == NULL) {
1332 ng_l2cap_free_cmd(cmd);
1333
1334 return (ENOBUFS);
1335 }
1336
1337 /* Link command to the queue */
1338 ng_l2cap_link_cmd(con, cmd);
1339 ng_l2cap_lp_deliver(con);
1340
1341 return (0);
1342 } /* send_l2cap_con_rej */
1343
1344 /*
1345 * Send L2CAP config response
1346 */
1347
1348 static int
send_l2cap_cfg_rsp(ng_l2cap_con_p con,u_int8_t ident,u_int16_t scid,u_int16_t result,struct mbuf * opt)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)
1351 {
1352 ng_l2cap_cmd_p cmd = NULL;
1353
1354 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CFG_RSP, 0);
1355 if (cmd == NULL) {
1356 NG_FREE_M(opt);
1357
1358 return (ENOMEM);
1359 }
1360
1361 _ng_l2cap_cfg_rsp(cmd->aux, cmd->ident, scid, 0, result, opt);
1362 if (cmd->aux == NULL) {
1363 ng_l2cap_free_cmd(cmd);
1364
1365 return (ENOBUFS);
1366 }
1367
1368 /* Link command to the queue */
1369 ng_l2cap_link_cmd(con, cmd);
1370 ng_l2cap_lp_deliver(con);
1371
1372 return (0);
1373 } /* send_l2cap_cfg_rsp */
1374
1375 static int
send_l2cap_param_urs(ng_l2cap_con_p con,u_int8_t ident,u_int16_t result)1376 send_l2cap_param_urs(ng_l2cap_con_p con, u_int8_t ident,
1377 u_int16_t result)
1378 {
1379 ng_l2cap_cmd_p cmd = NULL;
1380
1381 cmd = ng_l2cap_new_cmd(con, NULL, ident,
1382 NG_L2CAP_CMD_PARAM_UPDATE_RESPONSE,
1383 0);
1384 if (cmd == NULL) {
1385 return (ENOMEM);
1386 }
1387
1388 _ng_l2cap_cmd_urs(cmd->aux, cmd->ident, result);
1389 if (cmd->aux == NULL) {
1390 ng_l2cap_free_cmd(cmd);
1391
1392 return (ENOBUFS);
1393 }
1394
1395 /* Link command to the queue */
1396 ng_l2cap_link_cmd(con, cmd);
1397 ng_l2cap_lp_deliver(con);
1398
1399 return (0);
1400 } /* send_l2cap_cfg_rsp */
1401
1402 /*
1403 * Get next L2CAP configuration option
1404 *
1405 * Return codes:
1406 * 0 no option
1407 * 1 we have got option
1408 * -1 header too short
1409 * -2 bad option value or length
1410 * -3 unknown option
1411 */
1412
1413 static int
get_next_l2cap_opt(struct mbuf * m,int * off,ng_l2cap_cfg_opt_p hdr,ng_l2cap_cfg_opt_val_p val)1414 get_next_l2cap_opt(struct mbuf *m, int *off, ng_l2cap_cfg_opt_p hdr,
1415 ng_l2cap_cfg_opt_val_p val)
1416 {
1417 int hint, len = m->m_pkthdr.len - (*off);
1418
1419 if (len == 0)
1420 return (0);
1421 if (len < 0 || len < sizeof(*hdr))
1422 return (-1);
1423
1424 m_copydata(m, *off, sizeof(*hdr), (caddr_t) hdr);
1425 *off += sizeof(*hdr);
1426 len -= sizeof(*hdr);
1427
1428 hint = NG_L2CAP_OPT_HINT(hdr->type);
1429 hdr->type &= NG_L2CAP_OPT_HINT_MASK;
1430
1431 switch (hdr->type) {
1432 case NG_L2CAP_OPT_MTU:
1433 if (hdr->length != NG_L2CAP_OPT_MTU_SIZE || len < hdr->length)
1434 return (-2);
1435
1436 m_copydata(m, *off, NG_L2CAP_OPT_MTU_SIZE, (caddr_t) val);
1437 val->mtu = le16toh(val->mtu);
1438 *off += NG_L2CAP_OPT_MTU_SIZE;
1439 break;
1440
1441 case NG_L2CAP_OPT_FLUSH_TIMO:
1442 if (hdr->length != NG_L2CAP_OPT_FLUSH_TIMO_SIZE ||
1443 len < hdr->length)
1444 return (-2);
1445
1446 m_copydata(m, *off, NG_L2CAP_OPT_FLUSH_TIMO_SIZE, (caddr_t)val);
1447 val->flush_timo = le16toh(val->flush_timo);
1448 *off += NG_L2CAP_OPT_FLUSH_TIMO_SIZE;
1449 break;
1450
1451 case NG_L2CAP_OPT_QOS:
1452 if (hdr->length != NG_L2CAP_OPT_QOS_SIZE || len < hdr->length)
1453 return (-2);
1454
1455 m_copydata(m, *off, NG_L2CAP_OPT_QOS_SIZE, (caddr_t) val);
1456 val->flow.token_rate = le32toh(val->flow.token_rate);
1457 val->flow.token_bucket_size =
1458 le32toh(val->flow.token_bucket_size);
1459 val->flow.peak_bandwidth = le32toh(val->flow.peak_bandwidth);
1460 val->flow.latency = le32toh(val->flow.latency);
1461 val->flow.delay_variation = le32toh(val->flow.delay_variation);
1462 *off += NG_L2CAP_OPT_QOS_SIZE;
1463 break;
1464
1465 default:
1466 if (hint)
1467 *off += hdr->length;
1468 else
1469 return (-3);
1470 break;
1471 }
1472
1473 return (1);
1474 } /* get_next_l2cap_opt */
1475