1 /*
2 * ng_l2cap_ulpi.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_ulpi.c,v 1.1 2002/11/24 19:47:06 max Exp $
31 * $FreeBSD: src/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.c,v 1.5 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_hci.h>
44 #include <netgraph7/bluetooth/include/ng_l2cap.h>
45 #include <netgraph7/bluetooth/l2cap/ng_l2cap_var.h>
46 #include <netgraph7/bluetooth/l2cap/ng_l2cap_cmds.h>
47 #include <netgraph7/bluetooth/l2cap/ng_l2cap_evnt.h>
48 #include <netgraph7/bluetooth/l2cap/ng_l2cap_llpi.h>
49 #include <netgraph7/bluetooth/l2cap/ng_l2cap_ulpi.h>
50 #include <netgraph7/bluetooth/l2cap/ng_l2cap_misc.h>
51
52 /******************************************************************************
53 ******************************************************************************
54 ** Upper Layer Protocol Interface module
55 ******************************************************************************
56 ******************************************************************************/
57
58 /*
59 * Process L2CA_Connect request from the upper layer protocol.
60 */
61
62 int
ng_l2cap_l2ca_con_req(ng_l2cap_p l2cap,struct ng_mesg * msg)63 ng_l2cap_l2ca_con_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
64 {
65 ng_l2cap_l2ca_con_ip *ip = NULL;
66 ng_l2cap_con_p con = NULL;
67 ng_l2cap_chan_p ch = NULL;
68 ng_l2cap_cmd_p cmd = NULL;
69 int error = 0;
70
71 /* Check message */
72 if (msg->header.arglen != sizeof(*ip)) {
73 NG_L2CAP_ALERT(
74 "%s: %s - invalid L2CA_Connect request message size, size=%d\n",
75 __func__, NG_NODE_NAME(l2cap->node),
76 msg->header.arglen);
77 error = EMSGSIZE;
78 goto out;
79 }
80
81 ip = (ng_l2cap_l2ca_con_ip *)(msg->data);
82
83 /* Check if we have connection to the remote unit */
84 con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr);
85 if (con == NULL) {
86 /* Submit LP_ConnectReq to the lower layer */
87 error = ng_l2cap_lp_con_req(l2cap, &ip->bdaddr);
88 if (error != 0) {
89 NG_L2CAP_ERR(
90 "%s: %s - unable to send LP_ConnectReq message, error=%d\n",
91 __func__, NG_NODE_NAME(l2cap->node), error);
92 goto out;
93 }
94
95 /* This should not fail */
96 con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr);
97 KASSERT((con != NULL),
98 ("%s: %s - could not find connection!\n", __func__, NG_NODE_NAME(l2cap->node)));
99 }
100
101 /*
102 * Create new empty channel descriptor. In case of any failure do
103 * not touch connection descriptor.
104 */
105
106 ch = ng_l2cap_new_chan(l2cap, con, ip->psm);
107 if (ch == NULL) {
108 error = ENOMEM;
109 goto out;
110 }
111
112 /* Now create L2CAP_ConnectReq command */
113 cmd = ng_l2cap_new_cmd(ch->con, ch, ng_l2cap_get_ident(con),
114 NG_L2CAP_CON_REQ, msg->header.token);
115 if (cmd == NULL) {
116 ng_l2cap_free_chan(ch);
117 error = ENOMEM;
118 goto out;
119 }
120
121 if (cmd->ident == NG_L2CAP_NULL_IDENT) {
122 ng_l2cap_free_cmd(cmd);
123 ng_l2cap_free_chan(ch);
124 error = EIO;
125 goto out;
126 }
127
128 /* Create L2CAP command packet */
129 _ng_l2cap_con_req(cmd->aux, cmd->ident, ch->psm, ch->scid);
130 if (cmd->aux == NULL) {
131 ng_l2cap_free_cmd(cmd);
132 ng_l2cap_free_chan(ch);
133 error = ENOBUFS;
134 goto out;
135 }
136
137 ch->state = NG_L2CAP_W4_L2CAP_CON_RSP;
138
139 /* Link command to the queue */
140 ng_l2cap_link_cmd(ch->con, cmd);
141 ng_l2cap_lp_deliver(ch->con);
142 out:
143 return (error);
144 } /* ng_l2cap_l2ca_con_req */
145
146 /*
147 * Send L2CA_Connect response to the upper layer protocol.
148 */
149
150 int
ng_l2cap_l2ca_con_rsp(ng_l2cap_chan_p ch,u_int32_t token,u_int16_t result,u_int16_t status)151 ng_l2cap_l2ca_con_rsp(ng_l2cap_chan_p ch, u_int32_t token, u_int16_t result,
152 u_int16_t status)
153 {
154 ng_l2cap_p l2cap = ch->con->l2cap;
155 struct ng_mesg *msg = NULL;
156 ng_l2cap_l2ca_con_op *op = NULL;
157 int error = 0;
158
159 /* Check if upstream hook is connected and valid */
160 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
161 NG_L2CAP_ERR(
162 "%s: %s - unable to send L2CA_Connect response message. " \
163 "Hook is not connected or valid, psm=%d\n",
164 __func__, NG_NODE_NAME(l2cap->node), ch->psm);
165
166 return (ENOTCONN);
167 }
168
169 /* Create and send L2CA_Connect response message */
170 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CON,
171 sizeof(*op), M_WAITOK | M_NULLOK);
172 if (msg == NULL)
173 error = ENOMEM;
174 else {
175 msg->header.token = token;
176 msg->header.flags |= NGF_RESP;
177
178 op = (ng_l2cap_l2ca_con_op *)(msg->data);
179
180 /*
181 * XXX Spec. says we should only populate LCID when result == 0
182 * What about PENDING? What the heck, for now always populate
183 * LCID :)
184 */
185
186 op->lcid = ch->scid;
187 op->result = result;
188 op->status = status;
189
190 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0);
191 }
192
193 return (error);
194 } /* ng_l2cap_l2ca_con_rsp */
195
196 /*
197 * Process L2CA_ConnectRsp request from the upper layer protocol.
198 */
199
200 int
ng_l2cap_l2ca_con_rsp_req(ng_l2cap_p l2cap,struct ng_mesg * msg)201 ng_l2cap_l2ca_con_rsp_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
202 {
203 ng_l2cap_l2ca_con_rsp_ip *ip = NULL;
204 ng_l2cap_con_p con = NULL;
205 ng_l2cap_chan_p ch = NULL;
206 ng_l2cap_cmd_p cmd = NULL;
207 u_int16_t dcid;
208 int error = 0;
209
210 /* Check message */
211 if (msg->header.arglen != sizeof(*ip)) {
212 NG_L2CAP_ALERT(
213 "%s: %s - invalid L2CA_ConnectRsp request message size, size=%d\n",
214 __func__, NG_NODE_NAME(l2cap->node),
215 msg->header.arglen);
216 error = EMSGSIZE;
217 goto out;
218 }
219
220 ip = (ng_l2cap_l2ca_con_rsp_ip *)(msg->data);
221
222 /* Check if we have this channel */
223 ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid);
224 if (ch == NULL) {
225 NG_L2CAP_ALERT(
226 "%s: %s - unexpected L2CA_ConnectRsp request message. " \
227 "Channel does not exist, lcid=%d\n",
228 __func__, NG_NODE_NAME(l2cap->node), ip->lcid);
229 error = ENOENT;
230 goto out;
231 }
232
233 /* Check channel state */
234 if (ch->state != NG_L2CAP_W4_L2CA_CON_RSP) {
235 NG_L2CAP_ERR(
236 "%s: %s - unexpected L2CA_ConnectRsp request message. " \
237 "Invalid channel state, state=%d, lcid=%d\n",
238 __func__, NG_NODE_NAME(l2cap->node), ch->state,
239 ip->lcid);
240 error = EINVAL;
241 goto out;
242 }
243
244 dcid = ch->dcid;
245 con = ch->con;
246
247 /*
248 * Now we are pretty much sure it is our response. So create and send
249 * L2CAP_ConnectRsp message to our peer.
250 */
251
252 if (ch->ident != ip->ident)
253 NG_L2CAP_WARN(
254 "%s: %s - channel ident and response ident do not match, scid=%d, ident=%d. " \
255 "Will use response ident=%d\n",
256 __func__, NG_NODE_NAME(l2cap->node), ch->scid,
257 ch->ident, ip->ident);
258
259 /* Check result */
260 switch (ip->result) {
261 case NG_L2CAP_SUCCESS:
262 ch->state = NG_L2CAP_CONFIG;
263 ch->cfg_state = 0;
264 break;
265
266 case NG_L2CAP_PENDING:
267 break;
268
269 default:
270 ng_l2cap_free_chan(ch);
271 ch = NULL;
272 break;
273 }
274
275 /* Create L2CAP command */
276 cmd = ng_l2cap_new_cmd(con, ch, ip->ident, NG_L2CAP_CON_RSP,
277 msg->header.token);
278 if (cmd == NULL) {
279 if (ch != NULL)
280 ng_l2cap_free_chan(ch);
281
282 error = ENOMEM;
283 goto out;
284 }
285
286 _ng_l2cap_con_rsp(cmd->aux, cmd->ident, ip->lcid, dcid,
287 ip->result, ip->status);
288 if (cmd->aux == NULL) {
289 if (ch != NULL)
290 ng_l2cap_free_chan(ch);
291
292 ng_l2cap_free_cmd(cmd);
293 error = ENOBUFS;
294 goto out;
295 }
296
297 /* Link command to the queue */
298 ng_l2cap_link_cmd(con, cmd);
299 ng_l2cap_lp_deliver(con);
300 out:
301 return (error);
302 } /* ng_l2cap_l2ca_con_rsp_req */
303
304 /*
305 * Send L2CAP_ConnectRsp response to the upper layer
306 */
307
308 int
ng_l2cap_l2ca_con_rsp_rsp(ng_l2cap_chan_p ch,u_int32_t token,u_int16_t result)309 ng_l2cap_l2ca_con_rsp_rsp(ng_l2cap_chan_p ch, u_int32_t token, u_int16_t result)
310 {
311 ng_l2cap_p l2cap = ch->con->l2cap;
312 struct ng_mesg *msg = NULL;
313 ng_l2cap_l2ca_con_rsp_op *op = NULL;
314 int error = 0;
315
316 /* Check if upstream hook is connected and valid */
317 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
318 NG_L2CAP_ERR(
319 "%s: %s - unable to send L2CA_ConnectRsp response message. " \
320 "Hook is not connected or valid, psm=%d\n",
321 __func__, NG_NODE_NAME(l2cap->node), ch->psm);
322
323 return (ENOTCONN);
324 }
325
326 /* Create and send L2CA_ConnectRsp response message */
327 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CON_RSP,
328 sizeof(*op), M_WAITOK | M_NULLOK);
329 if (msg == NULL)
330 error = ENOMEM;
331 else {
332 msg->header.token = token;
333 msg->header.flags |= NGF_RESP;
334
335 op = (ng_l2cap_l2ca_con_rsp_op *)(msg->data);
336 op->result = result;
337
338 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0);
339 }
340
341 return (error);
342 } /* ng_l2cap_l2ca_con_rsp_rsp */
343
344 /*
345 * Send L2CA_ConnectInd message to the upper layer protocol.
346 */
347
348 int
ng_l2cap_l2ca_con_ind(ng_l2cap_chan_p ch)349 ng_l2cap_l2ca_con_ind(ng_l2cap_chan_p ch)
350 {
351 ng_l2cap_p l2cap = ch->con->l2cap;
352 struct ng_mesg *msg = NULL;
353 ng_l2cap_l2ca_con_ind_ip *ip = NULL;
354 int error = 0;
355
356 /* Check if upstream hook is connected and valid */
357 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
358 NG_L2CAP_ERR(
359 "%s: %s - unable to send L2CA_ConnectInd message. " \
360 "Hook is not connected or valid, psm=%d\n",
361 __func__, NG_NODE_NAME(l2cap->node), ch->psm);
362
363 return (ENOTCONN);
364 }
365
366 /* Create and send L2CA_ConnectInd message */
367 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CON_IND,
368 sizeof(*ip), M_WAITOK | M_NULLOK);
369 if (msg == NULL)
370 error = ENOMEM;
371 else {
372 ip = (ng_l2cap_l2ca_con_ind_ip *)(msg->data);
373
374 bcopy(&ch->con->remote, &ip->bdaddr, sizeof(ip->bdaddr));
375 ip->lcid = ch->scid;
376 ip->psm = ch->psm;
377 ip->ident = ch->ident;
378
379 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0);
380 }
381
382 return (error);
383 } /* ng_l2cap_l2ca_con_ind */
384
385 /*
386 * Process L2CA_Config request from the upper layer protocol
387 */
388
389 int
ng_l2cap_l2ca_cfg_req(ng_l2cap_p l2cap,struct ng_mesg * msg)390 ng_l2cap_l2ca_cfg_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
391 {
392 ng_l2cap_l2ca_cfg_ip *ip = NULL;
393 ng_l2cap_chan_p ch = NULL;
394 ng_l2cap_cmd_p cmd = NULL;
395 struct mbuf *opt = NULL;
396 u_int16_t *mtu = NULL, *flush_timo = NULL;
397 ng_l2cap_flow_p flow = NULL;
398 int error = 0;
399
400 /* Check message */
401 if (msg->header.arglen != sizeof(*ip)) {
402 NG_L2CAP_ALERT(
403 "%s: %s - Invalid L2CA_Config request message size, size=%d\n",
404 __func__, NG_NODE_NAME(l2cap->node),
405 msg->header.arglen);
406 error = EMSGSIZE;
407 goto out;
408 }
409
410 ip = (ng_l2cap_l2ca_cfg_ip *)(msg->data);
411
412 /* Check if we have this channel */
413 ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid);
414 if (ch == NULL) {
415 NG_L2CAP_ERR(
416 "%s: %s - unexpected L2CA_Config request message. " \
417 "Channel does not exist, lcid=%d\n",
418 __func__, NG_NODE_NAME(l2cap->node), ip->lcid);
419 error = ENOENT;
420 goto out;
421 }
422
423 /* Check channel state */
424 if (ch->state != NG_L2CAP_OPEN && ch->state != NG_L2CAP_CONFIG) {
425 NG_L2CAP_ERR(
426 "%s: %s - unexpected L2CA_Config request message. " \
427 "Invalid channel state, state=%d, lcid=%d\n",
428 __func__, NG_NODE_NAME(l2cap->node), ch->state,
429 ch->scid);
430 error = EINVAL;
431 goto out;
432 }
433
434 /* Set requested channel configuration options */
435 ch->imtu = ip->imtu;
436 bcopy(&ip->oflow, &ch->oflow, sizeof(ch->oflow));
437 ch->flush_timo = ip->flush_timo;
438 ch->link_timo = ip->link_timo;
439
440 /* Compare channel settings with defaults */
441 if (ch->imtu != NG_L2CAP_MTU_DEFAULT)
442 mtu = &ch->imtu;
443 if (ch->flush_timo != NG_L2CAP_FLUSH_TIMO_DEFAULT)
444 flush_timo = &ch->flush_timo;
445 if (bcmp(ng_l2cap_default_flow(), &ch->oflow, sizeof(ch->oflow)) != 0)
446 flow = &ch->oflow;
447
448 /* Create configuration options */
449 _ng_l2cap_build_cfg_options(opt, mtu, flush_timo, flow);
450 if (opt == NULL) {
451 error = ENOBUFS;
452 goto out;
453 }
454
455 /* Create L2CAP command descriptor */
456 cmd = ng_l2cap_new_cmd(ch->con, ch, ng_l2cap_get_ident(ch->con),
457 NG_L2CAP_CFG_REQ, msg->header.token);
458 if (cmd == NULL) {
459 NG_FREE_M(opt);
460 error = ENOMEM;
461 goto out;
462 }
463
464 if (cmd->ident == NG_L2CAP_NULL_IDENT) {
465 ng_l2cap_free_cmd(cmd);
466 NG_FREE_M(opt);
467 error = EIO;
468 goto out;
469 }
470
471 /* Create L2CAP command packet */
472 _ng_l2cap_cfg_req(cmd->aux, cmd->ident, ch->dcid, 0, opt);
473 if (cmd->aux == NULL) {
474 ng_l2cap_free_cmd(cmd);
475 error = ENOBUFS;
476 goto out;
477 }
478
479 /* Adjust channel state for re-configuration */
480 if (ch->state == NG_L2CAP_OPEN) {
481 ch->state = NG_L2CAP_CONFIG;
482 ch->cfg_state = 0;
483 }
484
485 /* Link command to the queue */
486 ng_l2cap_link_cmd(ch->con, cmd);
487 ng_l2cap_lp_deliver(ch->con);
488 out:
489 return (error);
490 } /* ng_l2cap_l2ca_cfg_req */
491
492 /*
493 * Send L2CA_Config response to the upper layer protocol
494 */
495
496 int
ng_l2cap_l2ca_cfg_rsp(ng_l2cap_chan_p ch,u_int32_t token,u_int16_t result)497 ng_l2cap_l2ca_cfg_rsp(ng_l2cap_chan_p ch, u_int32_t token, u_int16_t result)
498 {
499 ng_l2cap_p l2cap = ch->con->l2cap;
500 struct ng_mesg *msg = NULL;
501 ng_l2cap_l2ca_cfg_op *op = NULL;
502 int error = 0;
503
504 /* Check if upstream hook is connected and valid */
505 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
506 NG_L2CAP_ERR(
507 "%s: %s - unable to send L2CA_Config response message. " \
508 "Hook is not connected or valid, psm=%d\n",
509 __func__, NG_NODE_NAME(l2cap->node), ch->psm);
510
511 return (ENOTCONN);
512 }
513
514 /* Create and send L2CA_Config response message */
515 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CFG,
516 sizeof(*op), M_WAITOK | M_NULLOK);
517 if (msg == NULL)
518 error = ENOMEM;
519 else {
520 msg->header.token = token;
521 msg->header.flags |= NGF_RESP;
522
523 op = (ng_l2cap_l2ca_cfg_op *)(msg->data);
524 op->result = result;
525 op->imtu = ch->imtu;
526 bcopy(&ch->oflow, &op->oflow, sizeof(op->oflow));
527 op->flush_timo = ch->flush_timo;
528
529 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0);
530
531 if (error == 0 && result == NG_L2CAP_SUCCESS) {
532 ch->cfg_state |= NG_L2CAP_CFG_IN;
533
534 if (ch->cfg_state == NG_L2CAP_CFG_BOTH)
535 ch->state = NG_L2CAP_OPEN;
536 }
537 }
538
539 return (error);
540 } /* ng_l2cap_l2ca_cfg_rsp */
541
542 /*
543 * Process L2CA_ConfigRsp request from the upper layer protocol
544 *
545 * XXX XXX XXX
546 *
547 * NOTE: The Bluetooth specification says that Configuration_Response
548 * (L2CA_ConfigRsp) should be used to issue response to configuration request
549 * indication. The minor problem here is L2CAP command ident. We should use
550 * ident from original L2CAP request to make sure our peer can match request
551 * and response. For some reason Bluetooth specification does not include
552 * ident field into L2CA_ConfigInd and L2CA_ConfigRsp messages. This seems
553 * strange to me, because L2CA_ConnectInd and L2CA_ConnectRsp do have ident
554 * field. So we should store last known L2CAP request command ident in channel.
555 * Also it seems that upper layer can not reject configuration request, as
556 * Configuration_Response message does not have status/reason field.
557 */
558
559 int
ng_l2cap_l2ca_cfg_rsp_req(ng_l2cap_p l2cap,struct ng_mesg * msg)560 ng_l2cap_l2ca_cfg_rsp_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
561 {
562 ng_l2cap_l2ca_cfg_rsp_ip *ip = NULL;
563 ng_l2cap_chan_p ch = NULL;
564 ng_l2cap_cmd_p cmd = NULL;
565 struct mbuf *opt = NULL;
566 u_int16_t *mtu = NULL;
567 ng_l2cap_flow_p flow = NULL;
568 int error = 0;
569
570 /* Check message */
571 if (msg->header.arglen != sizeof(*ip)) {
572 NG_L2CAP_ALERT(
573 "%s: %s - invalid L2CA_ConfigRsp request message size, size=%d\n",
574 __func__, NG_NODE_NAME(l2cap->node),
575 msg->header.arglen);
576 error = EMSGSIZE;
577 goto out;
578 }
579
580 ip = (ng_l2cap_l2ca_cfg_rsp_ip *)(msg->data);
581
582 /* Check if we have this channel */
583 ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid);
584 if (ch == NULL) {
585 NG_L2CAP_ERR(
586 "%s: %s - unexpected L2CA_ConfigRsp request message. " \
587 "Channel does not exist, lcid=%d\n",
588 __func__, NG_NODE_NAME(l2cap->node), ip->lcid);
589 error = ENOENT;
590 goto out;
591 }
592
593 /* Check channel state */
594 if (ch->state != NG_L2CAP_CONFIG) {
595 NG_L2CAP_ERR(
596 "%s: %s - unexpected L2CA_ConfigRsp request message. " \
597 "Invalid channel state, state=%d, lcid=%d\n",
598 __func__, NG_NODE_NAME(l2cap->node), ch->state,
599 ch->scid);
600 error = EINVAL;
601 goto out;
602 }
603
604 /* Set channel settings */
605 if (ip->omtu != ch->omtu) {
606 ch->omtu = ip->omtu;
607 mtu = &ch->omtu;
608 }
609
610 if (bcmp(&ip->iflow, &ch->iflow, sizeof(ch->iflow)) != 0) {
611 bcopy(&ip->iflow, &ch->iflow, sizeof(ch->iflow));
612 flow = &ch->iflow;
613 }
614
615 if (mtu != NULL || flow != NULL) {
616 _ng_l2cap_build_cfg_options(opt, mtu, NULL, flow);
617 if (opt == NULL) {
618 error = ENOBUFS;
619 goto out;
620 }
621 }
622
623 /* Create L2CAP command */
624 cmd = ng_l2cap_new_cmd(ch->con, ch, ch->ident, NG_L2CAP_CFG_RSP,
625 msg->header.token);
626 if (cmd == NULL) {
627 NG_FREE_M(opt);
628 error = ENOMEM;
629 goto out;
630 }
631
632 _ng_l2cap_cfg_rsp(cmd->aux,cmd->ident,ch->dcid,0,NG_L2CAP_SUCCESS,opt);
633 if (cmd->aux == NULL) {
634 ng_l2cap_free_cmd(cmd);
635 error = ENOBUFS;
636 goto out;
637 }
638
639 /* XXX FIXME - not here ??? */
640 ch->cfg_state |= NG_L2CAP_CFG_OUT;
641 if (ch->cfg_state == NG_L2CAP_CFG_BOTH)
642 ch->state = NG_L2CAP_OPEN;
643
644 /* Link command to the queue */
645 ng_l2cap_link_cmd(ch->con, cmd);
646 ng_l2cap_lp_deliver(ch->con);
647 out:
648 return (error);
649 } /* ng_l2cap_l2ca_cfg_rsp_req */
650
651 /*
652 * Send L2CA_ConfigRsp response to the upper layer protocol
653 */
654
655 int
ng_l2cap_l2ca_cfg_rsp_rsp(ng_l2cap_chan_p ch,u_int32_t token,u_int16_t result)656 ng_l2cap_l2ca_cfg_rsp_rsp(ng_l2cap_chan_p ch, u_int32_t token, u_int16_t result)
657 {
658 ng_l2cap_p l2cap = ch->con->l2cap;
659 struct ng_mesg *msg = NULL;
660 ng_l2cap_l2ca_cfg_rsp_op *op = NULL;
661 int error = 0;
662
663 /* Check if upstream hook is connected and valid */
664 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
665 NG_L2CAP_ERR(
666 "%s: %s - unable to send L2CA_ConfigRsp response message. " \
667 "Hook is not connected or valid, psm=%d\n",
668 __func__, NG_NODE_NAME(l2cap->node), ch->psm);
669
670 return (ENOTCONN);
671 }
672
673 /* Create and send L2CA_ConfigRsp response message */
674 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CFG_RSP,
675 sizeof(*op), M_WAITOK | M_NULLOK);
676 if (msg == NULL)
677 error = ENOMEM;
678 else {
679 msg->header.token = token;
680 msg->header.flags |= NGF_RESP;
681
682 op = (ng_l2cap_l2ca_cfg_rsp_op *)(msg->data);
683 op->result = result;
684
685 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0);
686 }
687
688 return (error);
689 } /* ng_l2cap_l2ca_cfg_rsp_rsp */
690
691 /*
692 * Send L2CA_ConfigInd message to the upper layer protocol
693 *
694 * XXX XXX XXX
695 *
696 * NOTE: The Bluetooth specification says that Configuration_Response
697 * (L2CA_ConfigRsp) should be used to issue response to configuration request
698 * indication. The minor problem here is L2CAP command ident. We should use
699 * ident from original L2CAP request to make sure our peer can match request
700 * and response. For some reason Bluetooth specification does not include
701 * ident field into L2CA_ConfigInd and L2CA_ConfigRsp messages. This seems
702 * strange to me, because L2CA_ConnectInd and L2CA_ConnectRsp do have ident
703 * field. So we should store last known L2CAP request command ident in channel.
704 * Also it seems that upper layer can not reject configuration request, as
705 * Configuration_Response message does not have status/reason field.
706 */
707
708 int
ng_l2cap_l2ca_cfg_ind(ng_l2cap_chan_p ch)709 ng_l2cap_l2ca_cfg_ind(ng_l2cap_chan_p ch)
710 {
711 ng_l2cap_p l2cap = ch->con->l2cap;
712 struct ng_mesg *msg = NULL;
713 ng_l2cap_l2ca_cfg_ind_ip *ip = NULL;
714 int error = 0;
715
716 /* Check if upstream hook is connected and valid */
717 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
718 NG_L2CAP_ERR(
719 "%s: %s - Unable to send L2CA_ConfigInd message. " \
720 "Hook is not connected or valid, psm=%d\n",
721 __func__, NG_NODE_NAME(l2cap->node), ch->psm);
722
723 return (ENOTCONN);
724 }
725
726 /* Create and send L2CA_ConnectInd message */
727 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CFG_IND,
728 sizeof(*ip), M_WAITOK | M_NULLOK);
729 if (msg == NULL)
730 error = ENOMEM;
731 else {
732 ip = (ng_l2cap_l2ca_cfg_ind_ip *)(msg->data);
733 ip->lcid = ch->scid;
734 ip->omtu = ch->omtu;
735 bcopy(&ch->iflow, &ip->iflow, sizeof(ip->iflow));
736 ip->flush_timo = ch->flush_timo;
737
738 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0);
739 }
740
741 return (error);
742 } /* ng_l2cap_l2ca_cfg_ind */
743
744 /*
745 * Process L2CA_Write event
746 */
747
748 int
ng_l2cap_l2ca_write_req(ng_l2cap_p l2cap,struct mbuf * m)749 ng_l2cap_l2ca_write_req(ng_l2cap_p l2cap, struct mbuf *m)
750 {
751 ng_l2cap_l2ca_hdr_t *l2ca_hdr = NULL;
752 ng_l2cap_chan_p ch = NULL;
753 ng_l2cap_cmd_p cmd = NULL;
754 int error = 0;
755 u_int32_t token = 0;
756
757 /* Make sure we can access L2CA data packet header */
758 if (m->m_pkthdr.len < sizeof(*l2ca_hdr)) {
759 NG_L2CAP_ERR(
760 "%s: %s - L2CA Data packet too small, len=%d\n",
761 __func__,NG_NODE_NAME(l2cap->node),m->m_pkthdr.len);
762 error = EMSGSIZE;
763 goto drop;
764 }
765
766 /* Get L2CA data packet header */
767 NG_L2CAP_M_PULLUP(m, sizeof(*l2ca_hdr));
768 if (m == NULL)
769 return (ENOBUFS);
770
771 l2ca_hdr = mtod(m, ng_l2cap_l2ca_hdr_t *);
772 token = l2ca_hdr->token;
773 m_adj(m, sizeof(*l2ca_hdr));
774
775 /* Verify payload size */
776 if (l2ca_hdr->length != m->m_pkthdr.len) {
777 NG_L2CAP_ERR(
778 "%s: %s - invalid L2CA Data packet. " \
779 "Payload length does not match, length=%d, len=%d\n",
780 __func__, NG_NODE_NAME(l2cap->node), l2ca_hdr->length,
781 m->m_pkthdr.len);
782 error = EMSGSIZE;
783 goto drop;
784 }
785
786 /* Check channel ID */
787 if (l2ca_hdr->lcid < NG_L2CAP_FIRST_CID) {
788 NG_L2CAP_ERR(
789 "%s: %s - invalid L2CA Data packet. Invalid channel ID, cid=%d\n",
790 __func__, NG_NODE_NAME(l2cap->node), l2ca_hdr->lcid);
791 error = EINVAL;
792 goto drop;
793 }
794
795 /* Verify that we have the channel and make sure it is open */
796 ch = ng_l2cap_chan_by_scid(l2cap, l2ca_hdr->lcid);
797 if (ch == NULL) {
798 NG_L2CAP_ERR(
799 "%s: %s - invalid L2CA Data packet. Channel does not exist, cid=%d\n",
800 __func__, NG_NODE_NAME(l2cap->node), l2ca_hdr->lcid);
801 error = ENOENT;
802 goto drop;
803 }
804
805 if (ch->state != NG_L2CAP_OPEN) {
806 NG_L2CAP_ERR(
807 "%s: %s - invalid L2CA Data packet. Invalid channel state, scid=%d, state=%d\n",
808 __func__, NG_NODE_NAME(l2cap->node), ch->scid,
809 ch->state);
810 error = EHOSTDOWN;
811 goto drop; /* XXX not always - re-configure */
812 }
813
814 /* Create L2CAP command descriptor */
815 cmd = ng_l2cap_new_cmd(ch->con, ch, 0, NGM_L2CAP_L2CA_WRITE, token);
816 if (cmd == NULL) {
817 error = ENOMEM;
818 goto drop;
819 }
820
821 /* Attach data packet and link command to the queue */
822 cmd->aux = m;
823 ng_l2cap_link_cmd(ch->con, cmd);
824 ng_l2cap_lp_deliver(ch->con);
825
826 return (error);
827 drop:
828 NG_FREE_M(m);
829
830 return (error);
831 } /* ng_l2cap_l2ca_write_req */
832
833 /*
834 * Send L2CA_Write response
835 */
836
837 int
ng_l2cap_l2ca_write_rsp(ng_l2cap_chan_p ch,u_int32_t token,u_int16_t result,u_int16_t length)838 ng_l2cap_l2ca_write_rsp(ng_l2cap_chan_p ch, u_int32_t token, u_int16_t result,
839 u_int16_t length)
840 {
841 ng_l2cap_p l2cap = ch->con->l2cap;
842 struct ng_mesg *msg = NULL;
843 ng_l2cap_l2ca_write_op *op = NULL;
844 int error = 0;
845
846 /* Check if upstream hook is connected and valid */
847 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
848 NG_L2CAP_ERR(
849 "%s: %s - unable to send L2CA_WriteRsp message. " \
850 "Hook is not connected or valid, psm=%d\n",
851 __func__, NG_NODE_NAME(l2cap->node), ch->psm);
852
853 return (ENOTCONN);
854 }
855
856 /* Create and send L2CA_WriteRsp message */
857 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_WRITE,
858 sizeof(*op), M_WAITOK | M_NULLOK);
859 if (msg == NULL)
860 error = ENOMEM;
861 else {
862 msg->header.token = token;
863 msg->header.flags |= NGF_RESP;
864
865 op = (ng_l2cap_l2ca_write_op *)(msg->data);
866 op->result = result;
867 op->length = length;
868 op->lcid = ch->scid;
869
870 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0);
871 }
872
873 return (error);
874 } /* ng_l2cap_l2ca_write_rsp */
875
876 /*
877 * Receive packet from the lower layer protocol and send it to the upper
878 * layer protocol (L2CAP_Read)
879 */
880
881 int
ng_l2cap_l2ca_receive(ng_l2cap_con_p con)882 ng_l2cap_l2ca_receive(ng_l2cap_con_p con)
883 {
884 ng_l2cap_p l2cap = con->l2cap;
885 ng_l2cap_hdr_t *hdr = NULL;
886 ng_l2cap_chan_p ch = NULL;
887 int error = 0;
888
889 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr));
890 if (con->rx_pkt == NULL)
891 return (ENOBUFS);
892
893 hdr = mtod(con->rx_pkt, ng_l2cap_hdr_t *);
894
895 /* Check channel */
896 ch = ng_l2cap_chan_by_scid(l2cap, hdr->dcid);
897 if (ch == NULL) {
898 NG_L2CAP_ERR(
899 "%s: %s - unexpected L2CAP data packet. Channel does not exist, cid=%d\n",
900 __func__, NG_NODE_NAME(l2cap->node), hdr->dcid);
901 error = ENOENT;
902 goto drop;
903 }
904
905 /* Check channel state */
906 if (ch->state != NG_L2CAP_OPEN) {
907 NG_L2CAP_WARN(
908 "%s: %s - unexpected L2CAP data packet. " \
909 "Invalid channel state, cid=%d, state=%d\n",
910 __func__, NG_NODE_NAME(l2cap->node), ch->scid,
911 ch->state);
912 error = EHOSTDOWN; /* XXX not always - re-configuration */
913 goto drop;
914 }
915
916 /* Check payload size and channel's MTU */
917 if (hdr->length > ch->imtu) {
918 NG_L2CAP_ERR(
919 "%s: %s - invalid L2CAP data packet. " \
920 "Packet too big, length=%d, imtu=%d, cid=%d\n",
921 __func__, NG_NODE_NAME(l2cap->node), hdr->length,
922 ch->imtu, ch->scid);
923 error = EMSGSIZE;
924 goto drop;
925 }
926
927 /*
928 * If we got here then everything looks good and we can sent packet
929 * to the upper layer protocol.
930 */
931
932 /* Check if upstream hook is connected and valid */
933 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
934 NG_L2CAP_ERR(
935 "%s: %s - unable to send L2CAP data packet. " \
936 "Hook is not connected or valid, psm=%d\n",
937 __func__, NG_NODE_NAME(l2cap->node), ch->psm);
938 error = ENOTCONN;
939 goto drop;
940 }
941
942 NG_SEND_DATA_ONLY(error, l2cap->l2c, con->rx_pkt);
943 con->rx_pkt = NULL;
944 drop:
945 NG_FREE_M(con->rx_pkt); /* checks for != NULL */
946
947 return (error);
948 } /* ng_l2cap_receive */
949
950 /*
951 * Receive connectioless (multicast) packet from the lower layer protocol and
952 * send it to the upper layer protocol
953 */
954
955 int
ng_l2cap_l2ca_clt_receive(ng_l2cap_con_p con)956 ng_l2cap_l2ca_clt_receive(ng_l2cap_con_p con)
957 {
958 struct _clt_pkt {
959 ng_l2cap_hdr_t h;
960 ng_l2cap_clt_hdr_t c_h;
961 } __attribute__ ((packed)) *hdr = NULL;
962 ng_l2cap_p l2cap = con->l2cap;
963 int length, error = 0;
964
965 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr));
966 if (con->rx_pkt == NULL)
967 return (ENOBUFS);
968
969 hdr = mtod(con->rx_pkt, struct _clt_pkt *);
970
971 /* Check packet */
972 length = con->rx_pkt->m_pkthdr.len - sizeof(*hdr);
973 if (length < 0) {
974 NG_L2CAP_ERR(
975 "%s: %s - invalid L2CAP CLT data packet. Packet too small, length=%d\n",
976 __func__, NG_NODE_NAME(l2cap->node), length);
977 error = EMSGSIZE;
978 goto drop;
979 }
980
981 /* Check payload size against CLT MTU */
982 if (length > NG_L2CAP_MTU_DEFAULT) {
983 NG_L2CAP_ERR(
984 "%s: %s - invalid L2CAP CLT data packet. Packet too big, length=%d, mtu=%d\n",
985 __func__, NG_NODE_NAME(l2cap->node), length,
986 NG_L2CAP_MTU_DEFAULT);
987 error = EMSGSIZE;
988 goto drop;
989 }
990
991 hdr->c_h.psm = le16toh(hdr->c_h.psm);
992
993 /*
994 * If we got here then everything looks good and we can sent packet
995 * to the upper layer protocol.
996 */
997
998 /* Select upstream hook based on PSM */
999 switch (hdr->c_h.psm) {
1000 case NG_L2CAP_PSM_SDP:
1001 if (l2cap->flags & NG_L2CAP_CLT_SDP_DISABLED)
1002 goto drop;
1003 break;
1004
1005 case NG_L2CAP_PSM_RFCOMM:
1006 if (l2cap->flags & NG_L2CAP_CLT_RFCOMM_DISABLED)
1007 goto drop;
1008 break;
1009
1010 case NG_L2CAP_PSM_TCP:
1011 if (l2cap->flags & NG_L2CAP_CLT_TCP_DISABLED)
1012 goto drop;
1013 break;
1014 }
1015
1016 /* Check if upstream hook is connected and valid */
1017 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
1018 NG_L2CAP_ERR(
1019 "%s: %s - unable to send L2CAP CLT data packet. " \
1020 "Hook is not connected or valid, psm=%d\n",
1021 __func__, NG_NODE_NAME(l2cap->node), hdr->c_h.psm);
1022 error = ENOTCONN;
1023 goto drop;
1024 }
1025
1026 NG_SEND_DATA_ONLY(error, l2cap->l2c, con->rx_pkt);
1027 con->rx_pkt = NULL;
1028 drop:
1029 NG_FREE_M(con->rx_pkt); /* checks for != NULL */
1030
1031 return (error);
1032 } /* ng_l2cap_l2ca_clt_receive */
1033
1034 /*
1035 * Send L2CA_QoSViolationInd to the upper layer protocol
1036 */
1037
1038 int
ng_l2cap_l2ca_qos_ind(ng_l2cap_chan_p ch)1039 ng_l2cap_l2ca_qos_ind(ng_l2cap_chan_p ch)
1040 {
1041 ng_l2cap_p l2cap = ch->con->l2cap;
1042 struct ng_mesg *msg = NULL;
1043 ng_l2cap_l2ca_qos_ind_ip *ip = NULL;
1044 int error = 0;
1045
1046 /* Check if upstream hook is connected and valid */
1047 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
1048 NG_L2CAP_ERR(
1049 "%s: %s - unable to send L2CA_QoSViolationInd message. " \
1050 "Hook is not connected or valid, psm=%d\n",
1051 __func__, NG_NODE_NAME(l2cap->node), ch->psm);
1052
1053 return (ENOTCONN);
1054 }
1055
1056 /* Create and send L2CA_QoSViolationInd message */
1057 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_QOS_IND,
1058 sizeof(*ip), M_WAITOK | M_NULLOK);
1059 if (msg == NULL)
1060 error = ENOMEM;
1061 else {
1062 ip = (ng_l2cap_l2ca_qos_ind_ip *)(msg->data);
1063 bcopy(&ch->con->remote, &ip->bdaddr, sizeof(ip->bdaddr));
1064 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0);
1065 }
1066
1067 return (error);
1068 } /* ng_l2cap_l2ca_qos_ind */
1069
1070 /*
1071 * Process L2CA_Disconnect request from the upper layer protocol.
1072 */
1073
1074 int
ng_l2cap_l2ca_discon_req(ng_l2cap_p l2cap,struct ng_mesg * msg)1075 ng_l2cap_l2ca_discon_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
1076 {
1077 ng_l2cap_l2ca_discon_ip *ip = NULL;
1078 ng_l2cap_chan_p ch = NULL;
1079 ng_l2cap_cmd_p cmd = NULL;
1080 int error = 0;
1081
1082 /* Check message */
1083 if (msg->header.arglen != sizeof(*ip)) {
1084 NG_L2CAP_ALERT(
1085 "%s: %s - invalid L2CA_Disconnect request message size, size=%d\n",
1086 __func__, NG_NODE_NAME(l2cap->node),
1087 msg->header.arglen);
1088 error = EMSGSIZE;
1089 goto out;
1090 }
1091
1092 ip = (ng_l2cap_l2ca_discon_ip *)(msg->data);
1093
1094 /* Check if we have this channel */
1095 ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid);
1096 if (ch == NULL) {
1097 NG_L2CAP_ERR(
1098 "%s: %s - unexpected L2CA_Disconnect request message. " \
1099 "Channel does not exist, lcid=%d\n",
1100 __func__, NG_NODE_NAME(l2cap->node), ip->lcid);
1101 error = ENOENT;
1102 goto out;
1103 }
1104
1105 /* Check channel state */
1106 if (ch->state != NG_L2CAP_CONFIG && ch->state != NG_L2CAP_OPEN &&
1107 ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) {
1108 NG_L2CAP_ERR(
1109 "%s: %s - unexpected L2CA_Disconnect request message. " \
1110 "Invalid channel state, state=%d, lcid=%d\n",
1111 __func__, NG_NODE_NAME(l2cap->node), ch->state,
1112 ch->scid);
1113 error = EINVAL;
1114 goto out;
1115 }
1116
1117 /* Create and send L2CAP_DisconReq message */
1118 cmd = ng_l2cap_new_cmd(ch->con, ch, ng_l2cap_get_ident(ch->con),
1119 NG_L2CAP_DISCON_REQ, msg->header.token);
1120 if (cmd == NULL) {
1121 ng_l2cap_free_chan(ch);
1122 error = ENOMEM;
1123 goto out;
1124 }
1125
1126 if (cmd->ident == NG_L2CAP_NULL_IDENT) {
1127 ng_l2cap_free_chan(ch);
1128 ng_l2cap_free_cmd(cmd);
1129 error = EIO;
1130 goto out;
1131 }
1132
1133 _ng_l2cap_discon_req(cmd->aux, cmd->ident, ch->dcid, ch->scid);
1134 if (cmd->aux == NULL) {
1135 ng_l2cap_free_chan(ch);
1136 ng_l2cap_free_cmd(cmd);
1137 error = ENOBUFS;
1138 goto out;
1139 }
1140
1141 ch->state = NG_L2CAP_W4_L2CAP_DISCON_RSP;
1142
1143 /* Link command to the queue */
1144 ng_l2cap_link_cmd(ch->con, cmd);
1145 ng_l2cap_lp_deliver(ch->con);
1146 out:
1147 return (error);
1148 } /* ng_l2cap_l2ca_discon_req */
1149
1150 /*
1151 * Send L2CA_Disconnect response to the upper layer protocol
1152 */
1153
1154 int
ng_l2cap_l2ca_discon_rsp(ng_l2cap_chan_p ch,u_int32_t token,u_int16_t result)1155 ng_l2cap_l2ca_discon_rsp(ng_l2cap_chan_p ch, u_int32_t token, u_int16_t result)
1156 {
1157 ng_l2cap_p l2cap = ch->con->l2cap;
1158 struct ng_mesg *msg = NULL;
1159 ng_l2cap_l2ca_discon_op *op = NULL;
1160 int error = 0;
1161
1162 /* Check if upstream hook is connected and valid */
1163 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
1164 NG_L2CAP_ERR(
1165 "%s: %s - unable to send L2CA_Disconnect response message. " \
1166 "Hook is not connected or valid, psm=%d\n",
1167 __func__, NG_NODE_NAME(l2cap->node), ch->psm);
1168
1169 return (ENOTCONN);
1170 }
1171
1172 /* Create and send L2CA_Disconnect response message */
1173 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_DISCON,
1174 sizeof(*op), M_WAITOK | M_NULLOK);
1175 if (msg == NULL)
1176 error = ENOMEM;
1177 else {
1178 msg->header.token = token;
1179 msg->header.flags |= NGF_RESP;
1180
1181 op = (ng_l2cap_l2ca_discon_op *)(msg->data);
1182 op->result = result;
1183
1184 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0);
1185 }
1186
1187 return (error);
1188 } /* ng_l2cap_l2ca_discon_rsp */
1189
1190 /*
1191 * Send L2CA_DisconnectInd message to the upper layer protocol.
1192 */
1193
1194 int
ng_l2cap_l2ca_discon_ind(ng_l2cap_chan_p ch)1195 ng_l2cap_l2ca_discon_ind(ng_l2cap_chan_p ch)
1196 {
1197 ng_l2cap_p l2cap = ch->con->l2cap;
1198 struct ng_mesg *msg = NULL;
1199 ng_l2cap_l2ca_discon_ind_ip *ip = NULL;
1200 int error = 0;
1201
1202 /* Check if upstream hook is connected and valid */
1203 if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
1204 NG_L2CAP_ERR(
1205 "%s: %s - unable to send L2CA_DisconnectInd message. " \
1206 "Hook is not connected or valid, psm=%d\n",
1207 __func__, NG_NODE_NAME(l2cap->node), ch->psm);
1208
1209 return (ENOTCONN);
1210 }
1211
1212 /* Create and send L2CA_DisconnectInd message */
1213 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_DISCON_IND,
1214 sizeof(*ip), M_WAITOK | M_NULLOK);
1215 if (msg == NULL)
1216 error = ENOMEM;
1217 else {
1218 ip = (ng_l2cap_l2ca_discon_ind_ip *)(msg->data);
1219 ip->lcid = ch->scid;
1220 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0);
1221 }
1222
1223 return (error);
1224 } /* ng_l2cap_l2ca_discon_ind */
1225
1226 /*
1227 * Process L2CA_GroupCreate request from the upper layer protocol.
1228 * XXX FIXME
1229 */
1230
1231 int
ng_l2cap_l2ca_grp_create(ng_l2cap_p l2cap,struct ng_mesg * msg)1232 ng_l2cap_l2ca_grp_create(ng_l2cap_p l2cap, struct ng_mesg *msg)
1233 {
1234 return (ENOTSUP);
1235 } /* ng_l2cap_l2ca_grp_create */
1236
1237 /*
1238 * Process L2CA_GroupClose request from the upper layer protocol
1239 * XXX FIXME
1240 */
1241
1242 int
ng_l2cap_l2ca_grp_close(ng_l2cap_p l2cap,struct ng_mesg * msg)1243 ng_l2cap_l2ca_grp_close(ng_l2cap_p l2cap, struct ng_mesg *msg)
1244 {
1245 return (ENOTSUP);
1246 } /* ng_l2cap_l2ca_grp_close */
1247
1248 /*
1249 * Process L2CA_GroupAddMember request from the upper layer protocol.
1250 * XXX FIXME
1251 */
1252
1253 int
ng_l2cap_l2ca_grp_add_member_req(ng_l2cap_p l2cap,struct ng_mesg * msg)1254 ng_l2cap_l2ca_grp_add_member_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
1255 {
1256 return (ENOTSUP);
1257 } /* ng_l2cap_l2ca_grp_add_member_req */
1258
1259 /*
1260 * Send L2CA_GroupAddMember response to the upper layer protocol.
1261 * XXX FIXME
1262 */
1263
1264 int
ng_l2cap_l2ca_grp_add_member_rsp(ng_l2cap_chan_p ch,u_int32_t token,u_int16_t result)1265 ng_l2cap_l2ca_grp_add_member_rsp(ng_l2cap_chan_p ch, u_int32_t token,
1266 u_int16_t result)
1267 {
1268 return (0);
1269 } /* ng_l2cap_l2ca_grp_add_member_rsp */
1270
1271 /*
1272 * Process L2CA_GroupDeleteMember request from the upper layer protocol
1273 * XXX FIXME
1274 */
1275
1276 int
ng_l2cap_l2ca_grp_rem_member(ng_l2cap_p l2cap,struct ng_mesg * msg)1277 ng_l2cap_l2ca_grp_rem_member(ng_l2cap_p l2cap, struct ng_mesg *msg)
1278 {
1279 return (ENOTSUP);
1280 } /* ng_l2cap_l2ca_grp_rem_member */
1281
1282 /*
1283 * Process L2CA_GroupGetMembers request from the upper layer protocol
1284 * XXX FIXME
1285 */
1286
1287 int
ng_l2cap_l2ca_grp_get_members(ng_l2cap_p l2cap,struct ng_mesg * msg)1288 ng_l2cap_l2ca_grp_get_members(ng_l2cap_p l2cap, struct ng_mesg *msg)
1289 {
1290 return (ENOTSUP);
1291 } /* ng_l2cap_l2ca_grp_get_members */
1292
1293 /*
1294 * Process L2CA_Ping request from the upper layer protocol
1295 */
1296
1297 int
ng_l2cap_l2ca_ping_req(ng_l2cap_p l2cap,struct ng_mesg * msg)1298 ng_l2cap_l2ca_ping_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
1299 {
1300 ng_l2cap_l2ca_ping_ip *ip = NULL;
1301 ng_l2cap_con_p con = NULL;
1302 ng_l2cap_cmd_p cmd = NULL;
1303 int error = 0;
1304
1305 /* Verify message */
1306 if (msg->header.arglen < sizeof(*ip)) {
1307 NG_L2CAP_ALERT(
1308 "%s: %s - invalid L2CA_Ping request message size, size=%d\n",
1309 __func__, NG_NODE_NAME(l2cap->node),
1310 msg->header.arglen);
1311 error = EMSGSIZE;
1312 goto out;
1313 }
1314
1315 ip = (ng_l2cap_l2ca_ping_ip *)(msg->data);
1316 if (ip->echo_size > NG_L2CAP_MAX_ECHO_SIZE) {
1317 NG_L2CAP_WARN(
1318 "%s: %s - invalid L2CA_Ping request. Echo size is too big, echo_size=%d\n",
1319 __func__, NG_NODE_NAME(l2cap->node), ip->echo_size);
1320 error = EMSGSIZE;
1321 goto out;
1322 }
1323
1324 /* Check if we have connection to the unit */
1325 con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr);
1326 if (con == NULL) {
1327 /* Submit LP_ConnectReq to the lower layer */
1328 error = ng_l2cap_lp_con_req(l2cap, &ip->bdaddr);
1329 if (error != 0) {
1330 NG_L2CAP_ERR(
1331 "%s: %s - unable to send LP_ConnectReq message, error=%d\n",
1332 __func__, NG_NODE_NAME(l2cap->node), error);
1333 goto out;
1334 }
1335
1336 /* This should not fail */
1337 con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr);
1338 KASSERT((con != NULL),
1339 ("%s: %s - could not find connection!\n", __func__, NG_NODE_NAME(l2cap->node)));
1340 }
1341
1342 /* Create L2CAP command descriptor */
1343 cmd = ng_l2cap_new_cmd(con, NULL, ng_l2cap_get_ident(con),
1344 NG_L2CAP_ECHO_REQ, msg->header.token);
1345 if (cmd == NULL) {
1346 error = ENOMEM;
1347 goto out;
1348 }
1349
1350 if (cmd->ident == NG_L2CAP_NULL_IDENT) {
1351 ng_l2cap_free_cmd(cmd);
1352 error = EIO;
1353 goto out;
1354 }
1355
1356 /* Create L2CAP command packet */
1357 _ng_l2cap_echo_req(cmd->aux, cmd->ident,
1358 msg->data + sizeof(*ip), ip->echo_size);
1359 if (cmd->aux == NULL) {
1360 ng_l2cap_free_cmd(cmd);
1361 error = ENOBUFS;
1362 goto out;
1363 }
1364
1365 /* Link command to the queue */
1366 ng_l2cap_link_cmd(con, cmd);
1367 ng_l2cap_lp_deliver(con);
1368 out:
1369 return (error);
1370 } /* ng_l2cap_l2ca_ping_req */
1371
1372 /*
1373 * Send L2CA_Ping response to the upper layer protocol
1374 */
1375
1376 int
ng_l2cap_l2ca_ping_rsp(ng_l2cap_con_p con,u_int32_t token,u_int16_t result,struct mbuf * data)1377 ng_l2cap_l2ca_ping_rsp(ng_l2cap_con_p con, u_int32_t token, u_int16_t result,
1378 struct mbuf *data)
1379 {
1380 ng_l2cap_p l2cap = con->l2cap;
1381 struct ng_mesg *msg = NULL;
1382 ng_l2cap_l2ca_ping_op *op = NULL;
1383 int error = 0, size = 0;
1384
1385 /* Check if control hook is connected and valid */
1386 if (l2cap->ctl == NULL || NG_HOOK_NOT_VALID(l2cap->ctl)) {
1387 NG_L2CAP_WARN(
1388 "%s: %s - unable to send L2CA_Ping response message. " \
1389 "Hook is not connected or valid\n",
1390 __func__, NG_NODE_NAME(l2cap->node));
1391 error = ENOTCONN;
1392 goto out;
1393 }
1394
1395 size = (data == NULL)? 0 : data->m_pkthdr.len;
1396
1397 /* Create and send L2CA_Ping response message */
1398 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_PING,
1399 sizeof(*op) + size, M_WAITOK | M_NULLOK);
1400 if (msg == NULL)
1401 error = ENOMEM;
1402 else {
1403 msg->header.token = token;
1404 msg->header.flags |= NGF_RESP;
1405
1406 op = (ng_l2cap_l2ca_ping_op *)(msg->data);
1407 op->result = result;
1408 bcopy(&con->remote, &op->bdaddr, sizeof(op->bdaddr));
1409 if (data != NULL && size > 0) {
1410 op->echo_size = size;
1411 m_copydata(data, 0, size, (caddr_t) op + sizeof(*op));
1412 }
1413
1414 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->ctl, 0);
1415 }
1416 out:
1417 NG_FREE_M(data);
1418
1419 return (error);
1420 } /* ng_l2cap_l2ca_ping_rsp */
1421
1422 /*
1423 * Process L2CA_GetInfo request from the upper layer protocol
1424 */
1425
1426 int
ng_l2cap_l2ca_get_info_req(ng_l2cap_p l2cap,struct ng_mesg * msg)1427 ng_l2cap_l2ca_get_info_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
1428 {
1429 ng_l2cap_l2ca_get_info_ip *ip = NULL;
1430 ng_l2cap_con_p con = NULL;
1431 ng_l2cap_cmd_p cmd = NULL;
1432 int error = 0;
1433
1434 /* Verify message */
1435 if (msg->header.arglen != sizeof(*ip)) {
1436 NG_L2CAP_ALERT(
1437 "%s: %s - invalid L2CA_GetInfo request message size, size=%d\n",
1438 __func__, NG_NODE_NAME(l2cap->node),
1439 msg->header.arglen);
1440 error = EMSGSIZE;
1441 goto out;
1442 }
1443
1444 ip = (ng_l2cap_l2ca_get_info_ip *)(msg->data);
1445
1446 /* Check if we have connection to the unit */
1447 con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr);
1448 if (con == NULL) {
1449 /* Submit LP_ConnectReq to the lower layer */
1450 error = ng_l2cap_lp_con_req(l2cap, &ip->bdaddr);
1451 if (error != 0) {
1452 NG_L2CAP_ERR(
1453 "%s: %s - unable to send LP_ConnectReq message, error=%d\n",
1454 __func__, NG_NODE_NAME(l2cap->node), error);
1455 goto out;
1456 }
1457
1458 /* This should not fail */
1459 con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr);
1460 KASSERT((con != NULL),
1461 ("%s: %s - could not find connection!\n", __func__, NG_NODE_NAME(l2cap->node)));
1462 }
1463
1464 /* Create L2CAP command descriptor */
1465 cmd = ng_l2cap_new_cmd(con, NULL, ng_l2cap_get_ident(con),
1466 NG_L2CAP_INFO_REQ, msg->header.token);
1467 if (cmd == NULL) {
1468 error = ENOMEM;
1469 goto out;
1470 }
1471
1472 if (cmd->ident == NG_L2CAP_NULL_IDENT) {
1473 ng_l2cap_free_cmd(cmd);
1474 error = EIO;
1475 goto out;
1476 }
1477
1478 /* Create L2CAP command packet */
1479 _ng_l2cap_info_req(cmd->aux, cmd->ident, ip->info_type);
1480 if (cmd->aux == NULL) {
1481 ng_l2cap_free_cmd(cmd);
1482 error = ENOBUFS;
1483 goto out;
1484 }
1485
1486 /* Link command to the queue */
1487 ng_l2cap_link_cmd(con, cmd);
1488 ng_l2cap_lp_deliver(con);
1489 out:
1490 return (error);
1491 } /* ng_l2cap_l2ca_get_info_req */
1492
1493 /*
1494 * Send L2CA_GetInfo response to the upper layer protocol
1495 */
1496
1497 int
ng_l2cap_l2ca_get_info_rsp(ng_l2cap_con_p con,u_int32_t token,u_int16_t result,struct mbuf * data)1498 ng_l2cap_l2ca_get_info_rsp(ng_l2cap_con_p con, u_int32_t token,
1499 u_int16_t result, struct mbuf *data)
1500 {
1501 ng_l2cap_p l2cap = con->l2cap;
1502 struct ng_mesg *msg = NULL;
1503 ng_l2cap_l2ca_get_info_op *op = NULL;
1504 int error = 0, size;
1505
1506 /* Check if control hook is connected and valid */
1507 if (l2cap->ctl == NULL || NG_HOOK_NOT_VALID(l2cap->ctl)) {
1508 NG_L2CAP_WARN(
1509 "%s: %s - unable to send L2CA_GetInfo response message. " \
1510 "Hook is not connected or valid\n",
1511 __func__, NG_NODE_NAME(l2cap->node));
1512 error = ENOTCONN;
1513 goto out;
1514 }
1515
1516 size = (data == NULL)? 0 : data->m_pkthdr.len;
1517
1518 /* Create and send L2CA_GetInfo response message */
1519 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_GET_INFO,
1520 sizeof(*op) + size, M_WAITOK | M_NULLOK);
1521 if (msg == NULL)
1522 error = ENOMEM;
1523 else {
1524 msg->header.token = token;
1525 msg->header.flags |= NGF_RESP;
1526
1527 op = (ng_l2cap_l2ca_get_info_op *)(msg->data);
1528 op->result = result;
1529 if (data != NULL && size > 0) {
1530 op->info_size = size;
1531 m_copydata(data, 0, size, (caddr_t) op + sizeof(*op));
1532 }
1533
1534 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->ctl, 0);
1535 }
1536 out:
1537 NG_FREE_M(data);
1538
1539 return (error);
1540 } /* ng_l2cap_l2ca_get_info_rsp */
1541
1542 /*
1543 * Process L2CA_EnableCLT message from the upper layer protocol
1544 * XXX convert to NGN_L2CAP_NODE_SET_FLAGS?
1545 */
1546
1547 int
ng_l2cap_l2ca_enable_clt(ng_l2cap_p l2cap,struct ng_mesg * msg)1548 ng_l2cap_l2ca_enable_clt(ng_l2cap_p l2cap, struct ng_mesg *msg)
1549 {
1550 ng_l2cap_l2ca_enable_clt_ip *ip = NULL;
1551 int error = 0;
1552 #if 0
1553 * ng_l2cap_l2ca_enable_clt_op *op = NULL;
1554 * u_int16_t result;
1555 * u_int32_t token;
1556 #endif
1557
1558 /* Check message */
1559 if (msg->header.arglen != sizeof(*ip)) {
1560 NG_L2CAP_ALERT(
1561 "%s: %s - invalid L2CA_EnableCLT message size, size=%d\n",
1562 __func__, NG_NODE_NAME(l2cap->node),
1563 msg->header.arglen);
1564
1565 return (EMSGSIZE);
1566 }
1567
1568 /* Process request */
1569 ip = (ng_l2cap_l2ca_enable_clt_ip *) (msg->data);
1570 #if 0
1571 * result = NG_L2CAP_SUCCESS;
1572 #endif
1573
1574 switch (ip->psm)
1575 {
1576 case 0:
1577 /* Special case: disable/enable all PSM */
1578 if (ip->enable)
1579 l2cap->flags &= ~(NG_L2CAP_CLT_SDP_DISABLED |
1580 NG_L2CAP_CLT_RFCOMM_DISABLED |
1581 NG_L2CAP_CLT_TCP_DISABLED);
1582 else
1583 l2cap->flags |= (NG_L2CAP_CLT_SDP_DISABLED |
1584 NG_L2CAP_CLT_RFCOMM_DISABLED |
1585 NG_L2CAP_CLT_TCP_DISABLED);
1586 break;
1587
1588 case NG_L2CAP_PSM_SDP:
1589 if (ip->enable)
1590 l2cap->flags &= ~NG_L2CAP_CLT_SDP_DISABLED;
1591 else
1592 l2cap->flags |= NG_L2CAP_CLT_SDP_DISABLED;
1593 break;
1594
1595 case NG_L2CAP_PSM_RFCOMM:
1596 if (ip->enable)
1597 l2cap->flags &= ~NG_L2CAP_CLT_RFCOMM_DISABLED;
1598 else
1599 l2cap->flags |= NG_L2CAP_CLT_RFCOMM_DISABLED;
1600 break;
1601
1602 case NG_L2CAP_PSM_TCP:
1603 if (ip->enable)
1604 l2cap->flags &= ~NG_L2CAP_CLT_TCP_DISABLED;
1605 else
1606 l2cap->flags |= NG_L2CAP_CLT_TCP_DISABLED;
1607 break;
1608
1609 default:
1610 NG_L2CAP_ERR(
1611 "%s: %s - unsupported PSM=%d\n", __func__, NG_NODE_NAME(l2cap->node), ip->psm);
1612 #if 0
1613 * result = NG_L2CAP_PSM_NOT_SUPPORTED;
1614 #endif
1615 error = ENOTSUP;
1616 break;
1617 }
1618
1619 #if 0
1620 * /* Create and send response message */
1621 * token = msg->header.token;
1622 * NG_FREE_MSG(msg);
1623 * NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_ENABLE_CLT,
1624 * sizeof(*op), M_WAITOK | M_NULLOK);
1625 * if (msg == NULL)
1626 * error = ENOMEM;
1627 * else {
1628 * msg->header.token = token;
1629 * msg->header.flags |= NGF_RESP;
1630 *
1631 * op = (ng_l2cap_l2ca_enable_clt_op *)(msg->data);
1632 * op->result = result;
1633 * }
1634 *
1635 * /* Send response to control hook */
1636 * if (l2cap->ctl != NULL && NG_HOOK_IS_VALID(l2cap->ctl))
1637 * NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->ctl, 0);
1638 #endif
1639
1640 return (error);
1641 } /* ng_l2cap_l2ca_enable_clt */
1642
1643