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