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