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