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