1 /*
2 * ng_btsocket_l2cap.c
3 */
4
5 /*-
6 * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
7 *
8 * Copyright (c) 2001-2002 Maksim Yevmenkin <[email protected]>
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_btsocket_l2cap.c,v 1.16 2003/09/14 23:29:06 max Exp $
33 * $FreeBSD$
34 */
35
36 #include <sys/param.h>
37 #include <sys/systm.h>
38 #include <sys/bitstring.h>
39 #include <sys/domain.h>
40 #include <sys/endian.h>
41 #include <sys/errno.h>
42 #include <sys/filedesc.h>
43 #include <sys/ioccom.h>
44 #include <sys/kernel.h>
45 #include <sys/lock.h>
46 #include <sys/malloc.h>
47 #include <sys/mbuf.h>
48 #include <sys/mutex.h>
49 #include <sys/protosw.h>
50 #include <sys/queue.h>
51 #include <sys/socket.h>
52 #include <sys/socketvar.h>
53 #include <sys/sysctl.h>
54 #include <sys/taskqueue.h>
55
56 #include <net/vnet.h>
57
58 #include <netgraph/ng_message.h>
59 #include <netgraph/netgraph.h>
60 #include <netgraph/bluetooth/include/ng_bluetooth.h>
61 #include <netgraph/bluetooth/include/ng_hci.h>
62 #include <netgraph/bluetooth/include/ng_l2cap.h>
63 #include <netgraph/bluetooth/include/ng_btsocket.h>
64 #include <netgraph/bluetooth/include/ng_btsocket_l2cap.h>
65
66 /* MALLOC define */
67 #ifdef NG_SEPARATE_MALLOC
68 static MALLOC_DEFINE(M_NETGRAPH_BTSOCKET_L2CAP, "netgraph_btsocks_l2cap",
69 "Netgraph Bluetooth L2CAP sockets");
70 #else
71 #define M_NETGRAPH_BTSOCKET_L2CAP M_NETGRAPH
72 #endif /* NG_SEPARATE_MALLOC */
73
74 /* Netgraph node methods */
75 static ng_constructor_t ng_btsocket_l2cap_node_constructor;
76 static ng_rcvmsg_t ng_btsocket_l2cap_node_rcvmsg;
77 static ng_shutdown_t ng_btsocket_l2cap_node_shutdown;
78 static ng_newhook_t ng_btsocket_l2cap_node_newhook;
79 static ng_connect_t ng_btsocket_l2cap_node_connect;
80 static ng_rcvdata_t ng_btsocket_l2cap_node_rcvdata;
81 static ng_disconnect_t ng_btsocket_l2cap_node_disconnect;
82
83 static void ng_btsocket_l2cap_input (void *, int);
84 static void ng_btsocket_l2cap_rtclean (void *, int);
85
86 /* Netgraph type descriptor */
87 static struct ng_type typestruct = {
88 .version = NG_ABI_VERSION,
89 .name = NG_BTSOCKET_L2CAP_NODE_TYPE,
90 .constructor = ng_btsocket_l2cap_node_constructor,
91 .rcvmsg = ng_btsocket_l2cap_node_rcvmsg,
92 .shutdown = ng_btsocket_l2cap_node_shutdown,
93 .newhook = ng_btsocket_l2cap_node_newhook,
94 .connect = ng_btsocket_l2cap_node_connect,
95 .rcvdata = ng_btsocket_l2cap_node_rcvdata,
96 .disconnect = ng_btsocket_l2cap_node_disconnect,
97 };
98
99 /* Globals */
100 extern int ifqmaxlen;
101 static u_int32_t ng_btsocket_l2cap_debug_level;
102 static node_p ng_btsocket_l2cap_node;
103 static struct ng_bt_itemq ng_btsocket_l2cap_queue;
104 static struct mtx ng_btsocket_l2cap_queue_mtx;
105 static struct task ng_btsocket_l2cap_queue_task;
106 static LIST_HEAD(, ng_btsocket_l2cap_pcb) ng_btsocket_l2cap_sockets;
107 static struct mtx ng_btsocket_l2cap_sockets_mtx;
108 static LIST_HEAD(, ng_btsocket_l2cap_rtentry) ng_btsocket_l2cap_rt;
109 static struct mtx ng_btsocket_l2cap_rt_mtx;
110 static struct task ng_btsocket_l2cap_rt_task;
111 static struct timeval ng_btsocket_l2cap_lasttime;
112 static int ng_btsocket_l2cap_curpps;
113
114 /* Sysctl tree */
115 SYSCTL_DECL(_net_bluetooth_l2cap_sockets);
116 static SYSCTL_NODE(_net_bluetooth_l2cap_sockets, OID_AUTO, seq, CTLFLAG_RW,
117 0, "Bluetooth SEQPACKET L2CAP sockets family");
118 SYSCTL_UINT(_net_bluetooth_l2cap_sockets_seq, OID_AUTO, debug_level,
119 CTLFLAG_RW,
120 &ng_btsocket_l2cap_debug_level, NG_BTSOCKET_WARN_LEVEL,
121 "Bluetooth SEQPACKET L2CAP sockets debug level");
122 SYSCTL_UINT(_net_bluetooth_l2cap_sockets_seq, OID_AUTO, queue_len,
123 CTLFLAG_RD,
124 &ng_btsocket_l2cap_queue.len, 0,
125 "Bluetooth SEQPACKET L2CAP sockets input queue length");
126 SYSCTL_UINT(_net_bluetooth_l2cap_sockets_seq, OID_AUTO, queue_maxlen,
127 CTLFLAG_RD,
128 &ng_btsocket_l2cap_queue.maxlen, 0,
129 "Bluetooth SEQPACKET L2CAP sockets input queue max. length");
130 SYSCTL_UINT(_net_bluetooth_l2cap_sockets_seq, OID_AUTO, queue_drops,
131 CTLFLAG_RD,
132 &ng_btsocket_l2cap_queue.drops, 0,
133 "Bluetooth SEQPACKET L2CAP sockets input queue drops");
134
135 /* Debug */
136 #define NG_BTSOCKET_L2CAP_INFO \
137 if (ng_btsocket_l2cap_debug_level >= NG_BTSOCKET_INFO_LEVEL && \
138 ppsratecheck(&ng_btsocket_l2cap_lasttime, &ng_btsocket_l2cap_curpps, 1)) \
139 printf
140
141 #define NG_BTSOCKET_L2CAP_WARN \
142 if (ng_btsocket_l2cap_debug_level >= NG_BTSOCKET_WARN_LEVEL && \
143 ppsratecheck(&ng_btsocket_l2cap_lasttime, &ng_btsocket_l2cap_curpps, 1)) \
144 printf
145
146 #define NG_BTSOCKET_L2CAP_ERR \
147 if (ng_btsocket_l2cap_debug_level >= NG_BTSOCKET_ERR_LEVEL && \
148 ppsratecheck(&ng_btsocket_l2cap_lasttime, &ng_btsocket_l2cap_curpps, 1)) \
149 printf
150
151 #define NG_BTSOCKET_L2CAP_ALERT \
152 if (ng_btsocket_l2cap_debug_level >= NG_BTSOCKET_ALERT_LEVEL && \
153 ppsratecheck(&ng_btsocket_l2cap_lasttime, &ng_btsocket_l2cap_curpps, 1)) \
154 printf
155
156 /*
157 * Netgraph message processing routines
158 */
159
160 static int ng_btsocket_l2cap_process_l2ca_con_req_rsp
161 (struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
162 static int ng_btsocket_l2cap_process_l2ca_con_rsp_rsp
163 (struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
164 static int ng_btsocket_l2cap_process_l2ca_con_ind
165 (struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
166
167 static int ng_btsocket_l2cap_process_l2ca_cfg_req_rsp
168 (struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
169 static int ng_btsocket_l2cap_process_l2ca_cfg_rsp_rsp
170 (struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
171 static int ng_btsocket_l2cap_process_l2ca_cfg_ind
172 (struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
173
174 static int ng_btsocket_l2cap_process_l2ca_discon_rsp
175 (struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
176 static int ng_btsocket_l2cap_process_l2ca_discon_ind
177 (struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
178
179 static int ng_btsocket_l2cap_process_l2ca_write_rsp
180 (struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
181
182 /*
183 * Send L2CA_xxx messages to the lower layer
184 */
185
186 static int ng_btsocket_l2cap_send_l2ca_con_req
187 (ng_btsocket_l2cap_pcb_p);
188 static int ng_btsocket_l2cap_send_l2ca_con_rsp_req
189 (u_int32_t, ng_btsocket_l2cap_rtentry_p, bdaddr_p, int, int, int, int);
190 static int ng_btsocket_l2cap_send_l2ca_cfg_req
191 (ng_btsocket_l2cap_pcb_p);
192 static int ng_btsocket_l2cap_send_l2ca_cfg_rsp
193 (ng_btsocket_l2cap_pcb_p);
194 static int ng_btsocket_l2cap_send_l2ca_discon_req
195 (u_int32_t, ng_btsocket_l2cap_pcb_p);
196
197 static int ng_btsocket_l2cap_send2
198 (ng_btsocket_l2cap_pcb_p);
199
200 /*
201 * Timeout processing routines
202 */
203
204 static void ng_btsocket_l2cap_timeout (ng_btsocket_l2cap_pcb_p);
205 static void ng_btsocket_l2cap_untimeout (ng_btsocket_l2cap_pcb_p);
206 static void ng_btsocket_l2cap_process_timeout (void *);
207
208 /*
209 * Other stuff
210 */
211
212 static ng_btsocket_l2cap_pcb_p ng_btsocket_l2cap_pcb_by_addr(bdaddr_p, int);
213 static ng_btsocket_l2cap_pcb_p ng_btsocket_l2cap_pcb_by_token(u_int32_t);
214 static ng_btsocket_l2cap_pcb_p ng_btsocket_l2cap_pcb_by_cid (bdaddr_p, int,int);
215 static int ng_btsocket_l2cap_result2errno(int);
216
217 static int ng_btsock_l2cap_addrtype_to_linktype(int addrtype);
218
219 #define ng_btsocket_l2cap_wakeup_input_task() \
220 taskqueue_enqueue(taskqueue_swi_giant, &ng_btsocket_l2cap_queue_task)
221
222 #define ng_btsocket_l2cap_wakeup_route_task() \
223 taskqueue_enqueue(taskqueue_swi_giant, &ng_btsocket_l2cap_rt_task)
224
225
226
ng_btsock_l2cap_addrtype_to_linktype(int addrtype)227 int ng_btsock_l2cap_addrtype_to_linktype(int addrtype)
228 {
229 switch(addrtype){
230 case BDADDR_LE_PUBLIC:
231 return NG_HCI_LINK_LE_PUBLIC;
232 case BDADDR_LE_RANDOM:
233 return NG_HCI_LINK_LE_RANDOM;
234 default:
235 return NG_HCI_LINK_ACL;
236 }
237 }
238
239
240 /*****************************************************************************
241 *****************************************************************************
242 ** Netgraph node interface
243 *****************************************************************************
244 *****************************************************************************/
245
246 /*
247 * Netgraph node constructor. Do not allow to create node of this type.
248 */
249
250 static int
ng_btsocket_l2cap_node_constructor(node_p node)251 ng_btsocket_l2cap_node_constructor(node_p node)
252 {
253 return (EINVAL);
254 } /* ng_btsocket_l2cap_node_constructor */
255
256 /*
257 * Do local shutdown processing. Let old node go and create new fresh one.
258 */
259
260 static int
ng_btsocket_l2cap_node_shutdown(node_p node)261 ng_btsocket_l2cap_node_shutdown(node_p node)
262 {
263 int error = 0;
264
265 NG_NODE_UNREF(node);
266
267 /* Create new node */
268 error = ng_make_node_common(&typestruct, &ng_btsocket_l2cap_node);
269 if (error != 0) {
270 NG_BTSOCKET_L2CAP_ALERT(
271 "%s: Could not create Netgraph node, error=%d\n", __func__, error);
272
273 ng_btsocket_l2cap_node = NULL;
274
275 return (error);
276 }
277
278 error = ng_name_node(ng_btsocket_l2cap_node,
279 NG_BTSOCKET_L2CAP_NODE_TYPE);
280 if (error != 0) {
281 NG_BTSOCKET_L2CAP_ALERT(
282 "%s: Could not name Netgraph node, error=%d\n", __func__, error);
283
284 NG_NODE_UNREF(ng_btsocket_l2cap_node);
285 ng_btsocket_l2cap_node = NULL;
286
287 return (error);
288 }
289
290 return (0);
291 } /* ng_btsocket_l2cap_node_shutdown */
292
293 /*
294 * We allow any hook to be connected to the node.
295 */
296
297 static int
ng_btsocket_l2cap_node_newhook(node_p node,hook_p hook,char const * name)298 ng_btsocket_l2cap_node_newhook(node_p node, hook_p hook, char const *name)
299 {
300 return (0);
301 } /* ng_btsocket_l2cap_node_newhook */
302
303 /*
304 * Just say "YEP, that's OK by me!"
305 */
306
307 static int
ng_btsocket_l2cap_node_connect(hook_p hook)308 ng_btsocket_l2cap_node_connect(hook_p hook)
309 {
310 NG_HOOK_SET_PRIVATE(hook, NULL);
311 NG_HOOK_REF(hook); /* Keep extra reference to the hook */
312
313 #if 0
314 NG_HOOK_FORCE_QUEUE(NG_HOOK_PEER(hook));
315 NG_HOOK_FORCE_QUEUE(hook);
316 #endif
317
318 return (0);
319 } /* ng_btsocket_l2cap_node_connect */
320
321 /*
322 * Hook disconnection. Schedule route cleanup task
323 */
324
325 static int
ng_btsocket_l2cap_node_disconnect(hook_p hook)326 ng_btsocket_l2cap_node_disconnect(hook_p hook)
327 {
328 /*
329 * If hook has private information than we must have this hook in
330 * the routing table and must schedule cleaning for the routing table.
331 * Otherwise hook was connected but we never got "hook_info" message,
332 * so we have never added this hook to the routing table and it save
333 * to just delete it.
334 */
335
336 if (NG_HOOK_PRIVATE(hook) != NULL)
337 return (ng_btsocket_l2cap_wakeup_route_task());
338
339 NG_HOOK_UNREF(hook); /* Remove extra reference */
340
341 return (0);
342 } /* ng_btsocket_l2cap_node_disconnect */
343
344 /*
345 * Process incoming messages
346 */
347
348 static int
ng_btsocket_l2cap_node_rcvmsg(node_p node,item_p item,hook_p hook)349 ng_btsocket_l2cap_node_rcvmsg(node_p node, item_p item, hook_p hook)
350 {
351 struct ng_mesg *msg = NGI_MSG(item); /* item still has message */
352 int error = 0;
353
354 if (msg != NULL && msg->header.typecookie == NGM_L2CAP_COOKIE) {
355 mtx_lock(&ng_btsocket_l2cap_queue_mtx);
356 if (NG_BT_ITEMQ_FULL(&ng_btsocket_l2cap_queue)) {
357 NG_BTSOCKET_L2CAP_ERR(
358 "%s: Input queue is full (msg)\n", __func__);
359
360 NG_BT_ITEMQ_DROP(&ng_btsocket_l2cap_queue);
361 NG_FREE_ITEM(item);
362 error = ENOBUFS;
363 } else {
364 if (hook != NULL) {
365 NG_HOOK_REF(hook);
366 NGI_SET_HOOK(item, hook);
367 }
368
369 NG_BT_ITEMQ_ENQUEUE(&ng_btsocket_l2cap_queue, item);
370 error = ng_btsocket_l2cap_wakeup_input_task();
371 }
372 mtx_unlock(&ng_btsocket_l2cap_queue_mtx);
373 } else {
374 NG_FREE_ITEM(item);
375 error = EINVAL;
376 }
377
378 return (error);
379 } /* ng_btsocket_l2cap_node_rcvmsg */
380
381 /*
382 * Receive data on a hook
383 */
384
385 static int
ng_btsocket_l2cap_node_rcvdata(hook_p hook,item_p item)386 ng_btsocket_l2cap_node_rcvdata(hook_p hook, item_p item)
387 {
388 int error = 0;
389
390 mtx_lock(&ng_btsocket_l2cap_queue_mtx);
391 if (NG_BT_ITEMQ_FULL(&ng_btsocket_l2cap_queue)) {
392 NG_BTSOCKET_L2CAP_ERR(
393 "%s: Input queue is full (data)\n", __func__);
394
395 NG_BT_ITEMQ_DROP(&ng_btsocket_l2cap_queue);
396 NG_FREE_ITEM(item);
397 error = ENOBUFS;
398 } else {
399 NG_HOOK_REF(hook);
400 NGI_SET_HOOK(item, hook);
401
402 NG_BT_ITEMQ_ENQUEUE(&ng_btsocket_l2cap_queue, item);
403 error = ng_btsocket_l2cap_wakeup_input_task();
404 }
405 mtx_unlock(&ng_btsocket_l2cap_queue_mtx);
406
407 return (error);
408 } /* ng_btsocket_l2cap_node_rcvdata */
409
410 /*
411 * Process L2CA_Connect respose. Socket layer must have initiated connection,
412 * so we have to have a socket associated with message token.
413 */
414
415 static int
ng_btsocket_l2cap_process_l2ca_con_req_rsp(struct ng_mesg * msg,ng_btsocket_l2cap_rtentry_p rt)416 ng_btsocket_l2cap_process_l2ca_con_req_rsp(struct ng_mesg *msg,
417 ng_btsocket_l2cap_rtentry_p rt)
418 {
419 ng_l2cap_l2ca_con_op *op = NULL;
420 ng_btsocket_l2cap_pcb_t *pcb = NULL;
421 int error = 0;
422
423 if (msg->header.arglen != sizeof(*op))
424 return (EMSGSIZE);
425
426 op = (ng_l2cap_l2ca_con_op *)(msg->data);
427
428 mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
429
430 /* Look for the socket with the token */
431 pcb = ng_btsocket_l2cap_pcb_by_token(msg->header.token);
432 if (pcb == NULL) {
433 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
434 return (ENOENT);
435 }
436
437 mtx_lock(&pcb->pcb_mtx);
438
439 NG_BTSOCKET_L2CAP_INFO(
440 "%s: Got L2CA_Connect response, token=%d, src bdaddr=%x:%x:%x:%x:%x:%x, " \
441 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, result=%d, status=%d, " \
442 "state=%d\n", __func__, msg->header.token,
443 pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
444 pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
445 pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
446 pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
447 pcb->psm, op->lcid, op->result, op->status,
448 pcb->state);
449
450 if (pcb->state != NG_BTSOCKET_L2CAP_CONNECTING) {
451 mtx_unlock(&pcb->pcb_mtx);
452 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
453
454 return (ENOENT);
455 }
456
457 ng_btsocket_l2cap_untimeout(pcb);
458
459 if (op->result == NG_L2CAP_PENDING) {
460 ng_btsocket_l2cap_timeout(pcb);
461 mtx_unlock(&pcb->pcb_mtx);
462 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
463
464 return (0);
465 }
466
467 if (op->result == NG_L2CAP_SUCCESS){
468 if((pcb->idtype == NG_L2CAP_L2CA_IDTYPE_ATT)||
469 (pcb->idtype == NG_L2CAP_L2CA_IDTYPE_SMP)){
470 pcb->encryption = op->encryption; pcb->cid = op->lcid;
471 if(pcb->need_encrypt && !(pcb->encryption)){
472 ng_btsocket_l2cap_timeout(pcb);
473 pcb->state = NG_BTSOCKET_L2CAP_W4_ENC_CHANGE;
474 }else{
475 pcb->state = NG_BTSOCKET_L2CAP_OPEN;
476 soisconnected(pcb->so);
477 }
478 }else{
479 /*
480 * Channel is now open, so update local channel ID and
481 * start configuration process. Source and destination
482 * addresses as well as route must be already set.
483 */
484
485 pcb->cid = op->lcid;
486 pcb->encryption = op->encryption;
487 error = ng_btsocket_l2cap_send_l2ca_cfg_req(pcb);
488 if (error != 0) {
489 /* Send disconnect request with "zero" token */
490 ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);
491
492 /* ... and close the socket */
493 pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
494 soisdisconnected(pcb->so);
495 } else {
496 pcb->cfg_state = NG_BTSOCKET_L2CAP_CFG_IN_SENT;
497 pcb->state = NG_BTSOCKET_L2CAP_CONFIGURING;
498
499 ng_btsocket_l2cap_timeout(pcb);
500 }
501 }
502 } else {
503 /*
504 * We have failed to open connection, so convert result
505 * code to "errno" code and disconnect the socket. Channel
506 * already has been closed.
507 */
508
509 pcb->so->so_error = ng_btsocket_l2cap_result2errno(op->result);
510 pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
511 soisdisconnected(pcb->so);
512 }
513 mtx_unlock(&pcb->pcb_mtx);
514 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
515
516 return (error);
517 } /* ng_btsocket_l2cap_process_l2ca_con_req_rsp */
518
519 /*
520 * Process L2CA_ConnectRsp response
521 */
522
523 static int
ng_btsocket_l2cap_process_l2ca_con_rsp_rsp(struct ng_mesg * msg,ng_btsocket_l2cap_rtentry_p rt)524 ng_btsocket_l2cap_process_l2ca_con_rsp_rsp(struct ng_mesg *msg,
525 ng_btsocket_l2cap_rtentry_p rt)
526 {
527 ng_l2cap_l2ca_con_rsp_op *op = NULL;
528 ng_btsocket_l2cap_pcb_t *pcb = NULL;
529
530 if (msg->header.arglen != sizeof(*op))
531 return (EMSGSIZE);
532
533 op = (ng_l2cap_l2ca_con_rsp_op *)(msg->data);
534
535 mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
536
537 /* Look for the socket with the token */
538 pcb = ng_btsocket_l2cap_pcb_by_token(msg->header.token);
539 if (pcb == NULL) {
540 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
541 return (ENOENT);
542 }
543
544 mtx_lock(&pcb->pcb_mtx);
545
546 NG_BTSOCKET_L2CAP_INFO(
547 "%s: Got L2CA_ConnectRsp response, token=%d, src bdaddr=%x:%x:%x:%x:%x:%x, " \
548 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, result=%d, state=%d\n",
549 __func__, msg->header.token,
550 pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
551 pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
552 pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
553 pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
554 pcb->psm, pcb->cid, op->result, pcb->state);
555
556 if (pcb->state != NG_BTSOCKET_L2CAP_CONNECTING) {
557 mtx_unlock(&pcb->pcb_mtx);
558 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
559
560 return (ENOENT);
561 }
562
563 ng_btsocket_l2cap_untimeout(pcb);
564
565 /* Check the result and disconnect the socket on failure */
566 if (op->result != NG_L2CAP_SUCCESS) {
567 /* Close the socket - channel already closed */
568 pcb->so->so_error = ng_btsocket_l2cap_result2errno(op->result);
569 pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
570 soisdisconnected(pcb->so);
571 } else {
572 /* Move to CONFIGURING state and wait for CONFIG_IND */
573 pcb->cfg_state = 0;
574 pcb->state = NG_BTSOCKET_L2CAP_CONFIGURING;
575 ng_btsocket_l2cap_timeout(pcb);
576 }
577
578 mtx_unlock(&pcb->pcb_mtx);
579 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
580
581 return (0);
582 } /* ng_btsocket_process_l2ca_con_rsp_rsp */
583
584 /*
585 * Process L2CA_Connect indicator. Find socket that listens on address
586 * and PSM. Find exact or closest match. Create new socket and initiate
587 * connection.
588 */
589
590 static int
ng_btsocket_l2cap_process_l2ca_con_ind(struct ng_mesg * msg,ng_btsocket_l2cap_rtentry_p rt)591 ng_btsocket_l2cap_process_l2ca_con_ind(struct ng_mesg *msg,
592 ng_btsocket_l2cap_rtentry_p rt)
593 {
594 ng_l2cap_l2ca_con_ind_ip *ip = NULL;
595 ng_btsocket_l2cap_pcb_t *pcb = NULL, *pcb1 = NULL;
596 int error = 0;
597 u_int32_t token = 0;
598 u_int16_t result = 0;
599
600 if (msg->header.arglen != sizeof(*ip))
601 return (EMSGSIZE);
602
603 ip = (ng_l2cap_l2ca_con_ind_ip *)(msg->data);
604
605 NG_BTSOCKET_L2CAP_INFO(
606 "%s: Got L2CA_Connect indicator, src bdaddr=%x:%x:%x:%x:%x:%x, " \
607 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, ident=%d\n",
608 __func__,
609 rt->src.b[5], rt->src.b[4], rt->src.b[3],
610 rt->src.b[2], rt->src.b[1], rt->src.b[0],
611 ip->bdaddr.b[5], ip->bdaddr.b[4], ip->bdaddr.b[3],
612 ip->bdaddr.b[2], ip->bdaddr.b[1], ip->bdaddr.b[0],
613 ip->psm, ip->lcid, ip->ident);
614
615 mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
616
617 pcb = ng_btsocket_l2cap_pcb_by_addr(&rt->src, ip->psm);
618 if (pcb != NULL) {
619 struct socket *so1;
620
621 mtx_lock(&pcb->pcb_mtx);
622
623 CURVNET_SET(pcb->so->so_vnet);
624 so1 = sonewconn(pcb->so, 0);
625 CURVNET_RESTORE();
626 if (so1 == NULL) {
627 result = NG_L2CAP_NO_RESOURCES;
628 goto respond;
629 }
630
631 /*
632 * If we got here than we have created new socket. So complete
633 * connection. If we we listening on specific address then copy
634 * source address from listening socket, otherwise copy source
635 * address from hook's routing information.
636 */
637
638 pcb1 = so2l2cap_pcb(so1);
639 KASSERT((pcb1 != NULL),
640 ("%s: pcb1 == NULL\n", __func__));
641
642 mtx_lock(&pcb1->pcb_mtx);
643
644 if (bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(pcb->src)) != 0)
645 bcopy(&pcb->src, &pcb1->src, sizeof(pcb1->src));
646 else
647 bcopy(&rt->src, &pcb1->src, sizeof(pcb1->src));
648
649 pcb1->flags &= ~NG_BTSOCKET_L2CAP_CLIENT;
650
651 bcopy(&ip->bdaddr, &pcb1->dst, sizeof(pcb1->dst));
652 pcb1->psm = ip->psm;
653 pcb1->cid = ip->lcid;
654 pcb1->rt = rt;
655
656 /* Copy socket settings */
657 pcb1->imtu = pcb->imtu;
658 bcopy(&pcb->oflow, &pcb1->oflow, sizeof(pcb1->oflow));
659 pcb1->flush_timo = pcb->flush_timo;
660
661 token = pcb1->token;
662 } else
663 /* Nobody listens on requested BDADDR/PSM */
664 result = NG_L2CAP_PSM_NOT_SUPPORTED;
665
666 respond:
667 error = ng_btsocket_l2cap_send_l2ca_con_rsp_req(token, rt,
668 &ip->bdaddr,
669 ip->ident, ip->lcid,
670 result,ip->linktype);
671 if (pcb1 != NULL) {
672 if (error != 0) {
673 pcb1->so->so_error = error;
674 pcb1->state = NG_BTSOCKET_L2CAP_CLOSED;
675 soisdisconnected(pcb1->so);
676 } else {
677 pcb1->state = NG_BTSOCKET_L2CAP_CONNECTING;
678 soisconnecting(pcb1->so);
679
680 ng_btsocket_l2cap_timeout(pcb1);
681 }
682
683 mtx_unlock(&pcb1->pcb_mtx);
684 }
685
686 if (pcb != NULL)
687 mtx_unlock(&pcb->pcb_mtx);
688
689 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
690
691 return (error);
692 } /* ng_btsocket_l2cap_process_l2ca_con_ind */
693 /*Encryption Change*/
ng_btsocket_l2cap_process_l2ca_enc_change(struct ng_mesg * msg,ng_btsocket_l2cap_rtentry_p rt)694 static int ng_btsocket_l2cap_process_l2ca_enc_change(struct ng_mesg *msg, ng_btsocket_l2cap_rtentry_p rt)
695 {
696 ng_l2cap_l2ca_enc_chg_op *op = NULL;
697 ng_btsocket_l2cap_pcb_t *pcb = NULL;
698
699
700 if (msg->header.arglen != sizeof(*op))
701 return (EMSGSIZE);
702
703 op = (ng_l2cap_l2ca_enc_chg_op *)(msg->data);
704
705 mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
706
707 pcb = ng_btsocket_l2cap_pcb_by_cid(&rt->src, op->lcid,
708 op->idtype);
709 if (pcb == NULL) {
710 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
711 return (ENOENT);
712 }
713
714 mtx_lock(&pcb->pcb_mtx);
715 pcb->encryption = op->result;
716
717 if(pcb->need_encrypt){
718 ng_btsocket_l2cap_untimeout(pcb);
719 if(pcb->state != NG_BTSOCKET_L2CAP_W4_ENC_CHANGE){
720 NG_BTSOCKET_L2CAP_WARN("%s: Invalid pcb status %d",
721 __func__, pcb->state);
722 }else if(pcb->encryption){
723 pcb->state = NG_BTSOCKET_L2CAP_OPEN;
724 soisconnected(pcb->so);
725 }else{
726 pcb->so->so_error = EPERM;
727 ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);
728 pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
729 soisdisconnected(pcb->so);
730 }
731 }
732 mtx_unlock(&pcb->pcb_mtx);
733 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
734
735 return 0;
736 }
737 /*
738 * Process L2CA_Config response
739 */
740
741 static int
ng_btsocket_l2cap_process_l2ca_cfg_req_rsp(struct ng_mesg * msg,ng_btsocket_l2cap_rtentry_p rt)742 ng_btsocket_l2cap_process_l2ca_cfg_req_rsp(struct ng_mesg *msg,
743 ng_btsocket_l2cap_rtentry_p rt)
744 {
745 ng_l2cap_l2ca_cfg_op *op = NULL;
746 ng_btsocket_l2cap_pcb_p pcb = NULL;
747
748 if (msg->header.arglen != sizeof(*op))
749 return (EMSGSIZE);
750
751 op = (ng_l2cap_l2ca_cfg_op *)(msg->data);
752
753 mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
754
755 /*
756 * Socket must have issued a Configure request, so we must have a
757 * socket that wants to be configured. Use Netgraph message token
758 * to find it
759 */
760
761 pcb = ng_btsocket_l2cap_pcb_by_token(msg->header.token);
762 if (pcb == NULL) {
763 /*
764 * XXX FIXME what to do here? We could not find a
765 * socket with requested token. We even can not send
766 * Disconnect, because we do not know channel ID
767 */
768
769 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
770 return (ENOENT);
771 }
772
773 mtx_lock(&pcb->pcb_mtx);
774
775 NG_BTSOCKET_L2CAP_INFO(
776 "%s: Got L2CA_Config response, token=%d, src bdaddr=%x:%x:%x:%x:%x:%x, " \
777 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, result=%d, state=%d, " \
778 "cfg_state=%x\n",
779 __func__, msg->header.token,
780 pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
781 pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
782 pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
783 pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
784 pcb->psm, pcb->cid, op->result, pcb->state, pcb->cfg_state);
785
786 if (pcb->state != NG_BTSOCKET_L2CAP_CONFIGURING) {
787 mtx_unlock(&pcb->pcb_mtx);
788 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
789
790 return (ENOENT);
791 }
792
793 if (op->result == NG_L2CAP_SUCCESS) {
794 /*
795 * XXX FIXME Actually set flush and link timeout.
796 * Set QoS here if required. Resolve conficts (flush_timo).
797 * Save incoming MTU (peer's outgoing MTU) and outgoing flow
798 * spec.
799 */
800
801 pcb->imtu = op->imtu;
802 bcopy(&op->oflow, &pcb->oflow, sizeof(pcb->oflow));
803 pcb->flush_timo = op->flush_timo;
804
805 /*
806 * We have configured incoming side, so record it and check
807 * if configuration is complete. If complete then mark socket
808 * as connected, otherwise wait for the peer.
809 */
810
811 pcb->cfg_state &= ~NG_BTSOCKET_L2CAP_CFG_IN_SENT;
812 pcb->cfg_state |= NG_BTSOCKET_L2CAP_CFG_IN;
813
814 if (pcb->cfg_state == NG_BTSOCKET_L2CAP_CFG_BOTH) {
815 /* Configuration complete - mark socket as open */
816 ng_btsocket_l2cap_untimeout(pcb);
817 pcb->state = NG_BTSOCKET_L2CAP_OPEN;
818 soisconnected(pcb->so);
819 }
820 } else {
821 /*
822 * Something went wrong. Could be unacceptable parameters,
823 * reject or unknown option. That's too bad, but we will
824 * not negotiate. Send Disconnect and close the channel.
825 */
826
827 ng_btsocket_l2cap_untimeout(pcb);
828
829 switch (op->result) {
830 case NG_L2CAP_UNACCEPTABLE_PARAMS:
831 case NG_L2CAP_UNKNOWN_OPTION:
832 pcb->so->so_error = EINVAL;
833 break;
834
835 default:
836 pcb->so->so_error = ECONNRESET;
837 break;
838 }
839
840 /* Send disconnect with "zero" token */
841 ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);
842
843 /* ... and close the socket */
844 pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
845 soisdisconnected(pcb->so);
846 }
847
848 mtx_unlock(&pcb->pcb_mtx);
849 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
850
851 return (0);
852 } /* ng_btsocket_l2cap_process_l2ca_cfg_req_rsp */
853
854 /*
855 * Process L2CA_ConfigRsp response
856 */
857
858 static int
ng_btsocket_l2cap_process_l2ca_cfg_rsp_rsp(struct ng_mesg * msg,ng_btsocket_l2cap_rtentry_p rt)859 ng_btsocket_l2cap_process_l2ca_cfg_rsp_rsp(struct ng_mesg *msg,
860 ng_btsocket_l2cap_rtentry_p rt)
861 {
862 ng_l2cap_l2ca_cfg_rsp_op *op = NULL;
863 ng_btsocket_l2cap_pcb_t *pcb = NULL;
864 int error = 0;
865
866 if (msg->header.arglen != sizeof(*op))
867 return (EMSGSIZE);
868
869 op = (ng_l2cap_l2ca_cfg_rsp_op *)(msg->data);
870
871 mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
872
873 /* Look for the socket with the token */
874 pcb = ng_btsocket_l2cap_pcb_by_token(msg->header.token);
875 if (pcb == NULL) {
876 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
877 return (ENOENT);
878 }
879
880 mtx_lock(&pcb->pcb_mtx);
881
882 NG_BTSOCKET_L2CAP_INFO(
883 "%s: Got L2CA_ConfigRsp response, token=%d, src bdaddr=%x:%x:%x:%x:%x:%x, " \
884 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, result=%d, state=%d, " \
885 "cfg_state=%x\n",
886 __func__, msg->header.token,
887 pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
888 pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
889 pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
890 pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
891 pcb->psm, pcb->cid, op->result, pcb->state, pcb->cfg_state);
892
893 if (pcb->state != NG_BTSOCKET_L2CAP_CONFIGURING) {
894 mtx_unlock(&pcb->pcb_mtx);
895 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
896
897 return (ENOENT);
898 }
899
900 /* Check the result and disconnect socket of failure */
901 if (op->result != NG_L2CAP_SUCCESS)
902 goto disconnect;
903
904 /*
905 * Now we done with remote side configuration. Configure local
906 * side if we have not done it yet.
907 */
908
909 pcb->cfg_state &= ~NG_BTSOCKET_L2CAP_CFG_OUT_SENT;
910 pcb->cfg_state |= NG_BTSOCKET_L2CAP_CFG_OUT;
911
912 if (pcb->cfg_state == NG_BTSOCKET_L2CAP_CFG_BOTH) {
913 /* Configuration complete - mask socket as open */
914 ng_btsocket_l2cap_untimeout(pcb);
915 pcb->state = NG_BTSOCKET_L2CAP_OPEN;
916 soisconnected(pcb->so);
917 } else {
918 if (!(pcb->cfg_state & NG_BTSOCKET_L2CAP_CFG_IN_SENT)) {
919 /* Send L2CA_Config request - incoming path */
920 error = ng_btsocket_l2cap_send_l2ca_cfg_req(pcb);
921 if (error != 0)
922 goto disconnect;
923
924 pcb->cfg_state |= NG_BTSOCKET_L2CAP_CFG_IN_SENT;
925 }
926 }
927
928 mtx_unlock(&pcb->pcb_mtx);
929 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
930
931 return (error);
932
933 disconnect:
934 ng_btsocket_l2cap_untimeout(pcb);
935
936 /* Send disconnect with "zero" token */
937 ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);
938
939 /* ... and close the socket */
940 pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
941 soisdisconnected(pcb->so);
942
943 mtx_unlock(&pcb->pcb_mtx);
944 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
945
946 return (error);
947 } /* ng_btsocket_l2cap_process_l2ca_cfg_rsp_rsp */
948
949 /*
950 * Process L2CA_Config indicator
951 */
952
953 static int
ng_btsocket_l2cap_process_l2ca_cfg_ind(struct ng_mesg * msg,ng_btsocket_l2cap_rtentry_p rt)954 ng_btsocket_l2cap_process_l2ca_cfg_ind(struct ng_mesg *msg,
955 ng_btsocket_l2cap_rtentry_p rt)
956 {
957 ng_l2cap_l2ca_cfg_ind_ip *ip = NULL;
958 ng_btsocket_l2cap_pcb_t *pcb = NULL;
959 int error = 0;
960
961 if (msg->header.arglen != sizeof(*ip))
962 return (EMSGSIZE);
963
964 ip = (ng_l2cap_l2ca_cfg_ind_ip *)(msg->data);
965
966 mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
967
968 /* Check for the open socket that has given channel ID */
969 pcb = ng_btsocket_l2cap_pcb_by_cid(&rt->src, ip->lcid,
970 NG_L2CAP_L2CA_IDTYPE_BREDR);
971 if (pcb == NULL) {
972 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
973 return (ENOENT);
974 }
975
976 mtx_lock(&pcb->pcb_mtx);
977
978 NG_BTSOCKET_L2CAP_INFO(
979 "%s: Got L2CA_Config indicator, src bdaddr=%x:%x:%x:%x:%x:%x, " \
980 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, state=%d, cfg_state=%x\n",
981 __func__,
982 pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
983 pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
984 pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
985 pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
986 pcb->psm, pcb->cid, pcb->state, pcb->cfg_state);
987
988 /* XXX FIXME re-configuration on open socket */
989 if (pcb->state != NG_BTSOCKET_L2CAP_CONFIGURING) {
990 mtx_unlock(&pcb->pcb_mtx);
991 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
992
993 return (ENOENT);
994 }
995
996 /*
997 * XXX FIXME Actually set flush and link timeout. Set QoS here if
998 * required. Resolve conficts (flush_timo). Note outgoing MTU (peer's
999 * incoming MTU) and incoming flow spec.
1000 */
1001
1002 pcb->omtu = ip->omtu;
1003 bcopy(&ip->iflow, &pcb->iflow, sizeof(pcb->iflow));
1004 pcb->flush_timo = ip->flush_timo;
1005
1006 /*
1007 * Send L2CA_Config response to our peer and check for the errors,
1008 * if any send disconnect to close the channel.
1009 */
1010
1011 if (!(pcb->cfg_state & NG_BTSOCKET_L2CAP_CFG_OUT_SENT)) {
1012 error = ng_btsocket_l2cap_send_l2ca_cfg_rsp(pcb);
1013 if (error != 0) {
1014 ng_btsocket_l2cap_untimeout(pcb);
1015
1016 pcb->so->so_error = error;
1017
1018 /* Send disconnect with "zero" token */
1019 ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);
1020
1021 /* ... and close the socket */
1022 pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
1023 soisdisconnected(pcb->so);
1024 } else
1025 pcb->cfg_state |= NG_BTSOCKET_L2CAP_CFG_OUT_SENT;
1026 }
1027
1028 mtx_unlock(&pcb->pcb_mtx);
1029 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1030
1031 return (error);
1032 } /* ng_btsocket_l2cap_process_l2cap_cfg_ind */
1033
1034 /*
1035 * Process L2CA_Disconnect response
1036 */
1037
1038 static int
ng_btsocket_l2cap_process_l2ca_discon_rsp(struct ng_mesg * msg,ng_btsocket_l2cap_rtentry_p rt)1039 ng_btsocket_l2cap_process_l2ca_discon_rsp(struct ng_mesg *msg,
1040 ng_btsocket_l2cap_rtentry_p rt)
1041 {
1042 ng_l2cap_l2ca_discon_op *op = NULL;
1043 ng_btsocket_l2cap_pcb_t *pcb = NULL;
1044
1045 /* Check message */
1046 if (msg->header.arglen != sizeof(*op))
1047 return (EMSGSIZE);
1048
1049 op = (ng_l2cap_l2ca_discon_op *)(msg->data);
1050
1051 mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
1052
1053 /*
1054 * Socket layer must have issued L2CA_Disconnect request, so there
1055 * must be a socket that wants to be disconnected. Use Netgraph
1056 * message token to find it.
1057 */
1058
1059 pcb = ng_btsocket_l2cap_pcb_by_token(msg->header.token);
1060 if (pcb == NULL) {
1061 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1062 return (0);
1063 }
1064
1065 mtx_lock(&pcb->pcb_mtx);
1066
1067 /* XXX Close socket no matter what op->result says */
1068 if (pcb->state != NG_BTSOCKET_L2CAP_CLOSED) {
1069 NG_BTSOCKET_L2CAP_INFO(
1070 "%s: Got L2CA_Disconnect response, token=%d, src bdaddr=%x:%x:%x:%x:%x:%x, " \
1071 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, result=%d, state=%d\n",
1072 __func__, msg->header.token,
1073 pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
1074 pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
1075 pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
1076 pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
1077 pcb->psm, pcb->cid, op->result, pcb->state);
1078
1079 ng_btsocket_l2cap_untimeout(pcb);
1080
1081 pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
1082 soisdisconnected(pcb->so);
1083 }
1084
1085 mtx_unlock(&pcb->pcb_mtx);
1086 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1087
1088 return (0);
1089 } /* ng_btsocket_l2cap_process_l2ca_discon_rsp */
1090
1091 /*
1092 * Process L2CA_Disconnect indicator
1093 */
1094
1095 static int
ng_btsocket_l2cap_process_l2ca_discon_ind(struct ng_mesg * msg,ng_btsocket_l2cap_rtentry_p rt)1096 ng_btsocket_l2cap_process_l2ca_discon_ind(struct ng_mesg *msg,
1097 ng_btsocket_l2cap_rtentry_p rt)
1098 {
1099 ng_l2cap_l2ca_discon_ind_ip *ip = NULL;
1100 ng_btsocket_l2cap_pcb_t *pcb = NULL;
1101
1102 /* Check message */
1103 if (msg->header.arglen != sizeof(*ip))
1104 return (EMSGSIZE);
1105
1106 ip = (ng_l2cap_l2ca_discon_ind_ip *)(msg->data);
1107
1108 mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
1109
1110 /* Look for the socket with given channel ID */
1111 pcb = ng_btsocket_l2cap_pcb_by_cid(&rt->src, ip->lcid,
1112 ip->idtype);
1113 if (pcb == NULL) {
1114 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1115 return (0);
1116 }
1117
1118 /*
1119 * Channel has already been destroyed, so disconnect the socket
1120 * and be done with it. If there was any pending request we can
1121 * not do anything here anyway.
1122 */
1123
1124 mtx_lock(&pcb->pcb_mtx);
1125
1126 NG_BTSOCKET_L2CAP_INFO(
1127 "%s: Got L2CA_Disconnect indicator, src bdaddr=%x:%x:%x:%x:%x:%x, " \
1128 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, state=%d\n",
1129 __func__,
1130 pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
1131 pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
1132 pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
1133 pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
1134 pcb->psm, pcb->cid, pcb->state);
1135
1136 if (pcb->flags & NG_BTSOCKET_L2CAP_TIMO)
1137 ng_btsocket_l2cap_untimeout(pcb);
1138
1139 pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
1140 soisdisconnected(pcb->so);
1141
1142 mtx_unlock(&pcb->pcb_mtx);
1143 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1144
1145 return (0);
1146 } /* ng_btsocket_l2cap_process_l2ca_discon_ind */
1147
1148 /*
1149 * Process L2CA_Write response
1150 */
1151
1152 static int
ng_btsocket_l2cap_process_l2ca_write_rsp(struct ng_mesg * msg,ng_btsocket_l2cap_rtentry_p rt)1153 ng_btsocket_l2cap_process_l2ca_write_rsp(struct ng_mesg *msg,
1154 ng_btsocket_l2cap_rtentry_p rt)
1155 {
1156 ng_l2cap_l2ca_write_op *op = NULL;
1157 ng_btsocket_l2cap_pcb_t *pcb = NULL;
1158
1159 /* Check message */
1160 if (msg->header.arglen != sizeof(*op))
1161 return (EMSGSIZE);
1162
1163 op = (ng_l2cap_l2ca_write_op *)(msg->data);
1164
1165 mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
1166
1167 /* Look for the socket with given token */
1168 pcb = ng_btsocket_l2cap_pcb_by_token(msg->header.token);
1169 if (pcb == NULL) {
1170 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1171 return (ENOENT);
1172 }
1173
1174 mtx_lock(&pcb->pcb_mtx);
1175
1176 NG_BTSOCKET_L2CAP_INFO(
1177 "%s: Got L2CA_Write response, src bdaddr=%x:%x:%x:%x:%x:%x, " \
1178 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, result=%d, length=%d, " \
1179 "state=%d\n", __func__,
1180 pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
1181 pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
1182 pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
1183 pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
1184 pcb->psm, pcb->cid, op->result, op->length,
1185 pcb->state);
1186
1187 if (pcb->state != NG_BTSOCKET_L2CAP_OPEN) {
1188 mtx_unlock(&pcb->pcb_mtx);
1189 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1190
1191 return (ENOENT);
1192 }
1193
1194 ng_btsocket_l2cap_untimeout(pcb);
1195
1196 /*
1197 * Check if we have more data to send
1198 */
1199 sbdroprecord(&pcb->so->so_snd);
1200 if (sbavail(&pcb->so->so_snd) > 0) {
1201 if (ng_btsocket_l2cap_send2(pcb) == 0)
1202 ng_btsocket_l2cap_timeout(pcb);
1203 else
1204 sbdroprecord(&pcb->so->so_snd); /* XXX */
1205 }
1206
1207 /*
1208 * Now set the result, drop packet from the socket send queue and
1209 * ask for more (wakeup sender)
1210 */
1211
1212 pcb->so->so_error = ng_btsocket_l2cap_result2errno(op->result);
1213 sowwakeup(pcb->so);
1214
1215 mtx_unlock(&pcb->pcb_mtx);
1216 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1217
1218 return (0);
1219 } /* ng_btsocket_l2cap_process_l2ca_write_rsp */
1220
1221 /*
1222 * Send L2CA_Connect request
1223 */
1224
1225 static int
ng_btsocket_l2cap_send_l2ca_con_req(ng_btsocket_l2cap_pcb_p pcb)1226 ng_btsocket_l2cap_send_l2ca_con_req(ng_btsocket_l2cap_pcb_p pcb)
1227 {
1228 struct ng_mesg *msg = NULL;
1229 ng_l2cap_l2ca_con_ip *ip = NULL;
1230 int error = 0;
1231
1232 mtx_assert(&pcb->pcb_mtx, MA_OWNED);
1233
1234 if (pcb->rt == NULL ||
1235 pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook))
1236 return (ENETDOWN);
1237
1238 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CON,
1239 sizeof(*ip), M_NOWAIT);
1240 if (msg == NULL)
1241 return (ENOMEM);
1242
1243 msg->header.token = pcb->token;
1244
1245 ip = (ng_l2cap_l2ca_con_ip *)(msg->data);
1246 bcopy(&pcb->dst, &ip->bdaddr, sizeof(ip->bdaddr));
1247 ip->psm = pcb->psm;
1248 ip->linktype = ng_btsock_l2cap_addrtype_to_linktype(pcb->dsttype);
1249 ip->idtype = pcb->idtype;
1250 NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_node, msg,pcb->rt->hook, 0);
1251
1252 return (error);
1253 } /* ng_btsocket_l2cap_send_l2ca_con_req */
1254
1255 /*
1256 * Send L2CA_Connect response
1257 */
1258
1259 static int
ng_btsocket_l2cap_send_l2ca_con_rsp_req(u_int32_t token,ng_btsocket_l2cap_rtentry_p rt,bdaddr_p dst,int ident,int lcid,int result,int linktype)1260 ng_btsocket_l2cap_send_l2ca_con_rsp_req(u_int32_t token,
1261 ng_btsocket_l2cap_rtentry_p rt, bdaddr_p dst, int ident,
1262 int lcid, int result, int linktype)
1263 {
1264 struct ng_mesg *msg = NULL;
1265 ng_l2cap_l2ca_con_rsp_ip *ip = NULL;
1266 int error = 0;
1267
1268 if (rt == NULL || rt->hook == NULL || NG_HOOK_NOT_VALID(rt->hook))
1269 return (ENETDOWN);
1270
1271 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CON_RSP,
1272 sizeof(*ip), M_NOWAIT);
1273 if (msg == NULL)
1274 return (ENOMEM);
1275
1276 msg->header.token = token;
1277
1278 ip = (ng_l2cap_l2ca_con_rsp_ip *)(msg->data);
1279 bcopy(dst, &ip->bdaddr, sizeof(ip->bdaddr));
1280 ip->ident = ident;
1281 ip->lcid = lcid;
1282 ip->linktype = linktype;
1283 ip->result = result;
1284 ip->status = 0;
1285
1286 NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_node, msg, rt->hook, 0);
1287
1288 return (error);
1289 } /* ng_btsocket_l2cap_send_l2ca_con_rsp_req */
1290
1291 /*
1292 * Send L2CA_Config request
1293 */
1294
1295 static int
ng_btsocket_l2cap_send_l2ca_cfg_req(ng_btsocket_l2cap_pcb_p pcb)1296 ng_btsocket_l2cap_send_l2ca_cfg_req(ng_btsocket_l2cap_pcb_p pcb)
1297 {
1298 struct ng_mesg *msg = NULL;
1299 ng_l2cap_l2ca_cfg_ip *ip = NULL;
1300 int error = 0;
1301
1302 mtx_assert(&pcb->pcb_mtx, MA_OWNED);
1303
1304 if (pcb->rt == NULL ||
1305 pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook))
1306 return (ENETDOWN);
1307
1308 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CFG,
1309 sizeof(*ip), M_NOWAIT);
1310 if (msg == NULL)
1311 return (ENOMEM);
1312
1313 msg->header.token = pcb->token;
1314
1315 ip = (ng_l2cap_l2ca_cfg_ip *)(msg->data);
1316 ip->lcid = pcb->cid;
1317 ip->imtu = pcb->imtu;
1318 bcopy(&pcb->oflow, &ip->oflow, sizeof(ip->oflow));
1319 ip->flush_timo = pcb->flush_timo;
1320 ip->link_timo = pcb->link_timo;
1321
1322 NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_node, msg,pcb->rt->hook, 0);
1323
1324 return (error);
1325 } /* ng_btsocket_l2cap_send_l2ca_cfg_req */
1326
1327 /*
1328 * Send L2CA_Config response
1329 */
1330
1331 static int
ng_btsocket_l2cap_send_l2ca_cfg_rsp(ng_btsocket_l2cap_pcb_p pcb)1332 ng_btsocket_l2cap_send_l2ca_cfg_rsp(ng_btsocket_l2cap_pcb_p pcb)
1333 {
1334 struct ng_mesg *msg = NULL;
1335 ng_l2cap_l2ca_cfg_rsp_ip *ip = NULL;
1336 int error = 0;
1337
1338 mtx_assert(&pcb->pcb_mtx, MA_OWNED);
1339
1340 if (pcb->rt == NULL ||
1341 pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook))
1342 return (ENETDOWN);
1343
1344 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CFG_RSP,
1345 sizeof(*ip), M_NOWAIT);
1346 if (msg == NULL)
1347 return (ENOMEM);
1348
1349 msg->header.token = pcb->token;
1350
1351 ip = (ng_l2cap_l2ca_cfg_rsp_ip *)(msg->data);
1352 ip->lcid = pcb->cid;
1353 ip->omtu = pcb->omtu;
1354 bcopy(&pcb->iflow, &ip->iflow, sizeof(ip->iflow));
1355
1356 NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_node, msg, pcb->rt->hook, 0);
1357
1358 return (error);
1359 } /* ng_btsocket_l2cap_send_l2ca_cfg_rsp */
1360
1361 /*
1362 * Send L2CA_Disconnect request
1363 */
1364
1365 static int
ng_btsocket_l2cap_send_l2ca_discon_req(u_int32_t token,ng_btsocket_l2cap_pcb_p pcb)1366 ng_btsocket_l2cap_send_l2ca_discon_req(u_int32_t token,
1367 ng_btsocket_l2cap_pcb_p pcb)
1368 {
1369 struct ng_mesg *msg = NULL;
1370 ng_l2cap_l2ca_discon_ip *ip = NULL;
1371 int error = 0;
1372
1373 mtx_assert(&pcb->pcb_mtx, MA_OWNED);
1374
1375 if (pcb->rt == NULL ||
1376 pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook))
1377 return (ENETDOWN);
1378
1379 NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_DISCON,
1380 sizeof(*ip), M_NOWAIT);
1381 if (msg == NULL)
1382 return (ENOMEM);
1383
1384 msg->header.token = token;
1385
1386 ip = (ng_l2cap_l2ca_discon_ip *)(msg->data);
1387 ip->lcid = pcb->cid;
1388 ip->idtype = pcb->idtype;
1389
1390 NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_node, msg,pcb->rt->hook, 0);
1391
1392 return (error);
1393 } /* ng_btsocket_l2cap_send_l2ca_discon_req */
1394
1395 /*****************************************************************************
1396 *****************************************************************************
1397 ** Socket interface
1398 *****************************************************************************
1399 *****************************************************************************/
1400
1401 /*
1402 * L2CAP sockets data input routine
1403 */
1404
1405 static void
ng_btsocket_l2cap_data_input(struct mbuf * m,hook_p hook)1406 ng_btsocket_l2cap_data_input(struct mbuf *m, hook_p hook)
1407 {
1408 ng_l2cap_hdr_t *hdr = NULL;
1409 ng_l2cap_clt_hdr_t *clt_hdr = NULL;
1410 ng_btsocket_l2cap_pcb_t *pcb = NULL;
1411 ng_btsocket_l2cap_rtentry_t *rt = NULL;
1412 uint16_t idtype;
1413
1414 if (hook == NULL) {
1415 NG_BTSOCKET_L2CAP_ALERT(
1416 "%s: Invalid source hook for L2CAP data packet\n", __func__);
1417 goto drop;
1418 }
1419
1420 rt = (ng_btsocket_l2cap_rtentry_t *) NG_HOOK_PRIVATE(hook);
1421 if (rt == NULL) {
1422 NG_BTSOCKET_L2CAP_ALERT(
1423 "%s: Could not find out source bdaddr for L2CAP data packet\n", __func__);
1424 goto drop;
1425 }
1426
1427 m = m_pullup(m, sizeof(uint16_t));
1428 idtype = *mtod(m, uint16_t *);
1429 m_adj(m, sizeof(uint16_t));
1430
1431 /* Make sure we can access header */
1432 if (m->m_pkthdr.len < sizeof(*hdr)) {
1433 NG_BTSOCKET_L2CAP_ERR(
1434 "%s: L2CAP data packet too small, len=%d\n", __func__, m->m_pkthdr.len);
1435 goto drop;
1436 }
1437
1438 if (m->m_len < sizeof(*hdr)) {
1439 m = m_pullup(m, sizeof(*hdr));
1440 if (m == NULL)
1441 goto drop;
1442 }
1443
1444 /* Strip L2CAP packet header and verify packet length */
1445 hdr = mtod(m, ng_l2cap_hdr_t *);
1446 m_adj(m, sizeof(*hdr));
1447
1448 if (hdr->length != m->m_pkthdr.len) {
1449 NG_BTSOCKET_L2CAP_ERR(
1450 "%s: Bad L2CAP data packet length, len=%d, length=%d\n",
1451 __func__, m->m_pkthdr.len, hdr->length);
1452 goto drop;
1453 }
1454
1455 /*
1456 * Now process packet. Two cases:
1457 *
1458 * 1) Normal packet (cid != 2) then find connected socket and append
1459 * mbuf to the socket queue. Wakeup socket.
1460 *
1461 * 2) Broadcast packet (cid == 2) then find all sockets that connected
1462 * to the given PSM and have SO_BROADCAST bit set and append mbuf
1463 * to the socket queue. Wakeup socket.
1464 */
1465
1466 NG_BTSOCKET_L2CAP_INFO(
1467 "%s: Received L2CAP data packet: src bdaddr=%x:%x:%x:%x:%x:%x, " \
1468 "dcid=%d, length=%d\n",
1469 __func__,
1470 rt->src.b[5], rt->src.b[4], rt->src.b[3],
1471 rt->src.b[2], rt->src.b[1], rt->src.b[0],
1472 hdr->dcid, hdr->length);
1473
1474 if ((hdr->dcid >= NG_L2CAP_FIRST_CID) ||
1475 (idtype == NG_L2CAP_L2CA_IDTYPE_ATT)||
1476 (idtype == NG_L2CAP_L2CA_IDTYPE_SMP)
1477 ){
1478
1479 mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
1480
1481 /* Normal packet: find connected socket */
1482 pcb = ng_btsocket_l2cap_pcb_by_cid(&rt->src, hdr->dcid,idtype);
1483 if (pcb == NULL) {
1484 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1485 goto drop;
1486 }
1487
1488 mtx_lock(&pcb->pcb_mtx);
1489
1490 if (pcb->state != NG_BTSOCKET_L2CAP_OPEN) {
1491 NG_BTSOCKET_L2CAP_ERR(
1492 "%s: No connected socket found, src bdaddr=%x:%x:%x:%x:%x:%x, dcid=%d, " \
1493 "state=%d\n", __func__,
1494 rt->src.b[5], rt->src.b[4], rt->src.b[3],
1495 rt->src.b[2], rt->src.b[1], rt->src.b[0],
1496 hdr->dcid, pcb->state);
1497
1498 mtx_unlock(&pcb->pcb_mtx);
1499 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1500 goto drop;
1501 }
1502
1503 /* Check packet size against socket's incoming MTU */
1504 if (hdr->length > pcb->imtu) {
1505 NG_BTSOCKET_L2CAP_ERR(
1506 "%s: L2CAP data packet too big, src bdaddr=%x:%x:%x:%x:%x:%x, " \
1507 "dcid=%d, length=%d, imtu=%d\n",
1508 __func__,
1509 rt->src.b[5], rt->src.b[4], rt->src.b[3],
1510 rt->src.b[2], rt->src.b[1], rt->src.b[0],
1511 hdr->dcid, hdr->length, pcb->imtu);
1512
1513 mtx_unlock(&pcb->pcb_mtx);
1514 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1515 goto drop;
1516 }
1517
1518 /* Check if we have enough space in socket receive queue */
1519 if (m->m_pkthdr.len > sbspace(&pcb->so->so_rcv)) {
1520
1521 /*
1522 * This is really bad. Receive queue on socket does
1523 * not have enough space for the packet. We do not
1524 * have any other choice but drop the packet. L2CAP
1525 * does not provide any flow control.
1526 */
1527
1528 NG_BTSOCKET_L2CAP_ERR(
1529 "%s: Not enough space in socket receive queue. Dropping L2CAP data packet, " \
1530 "src bdaddr=%x:%x:%x:%x:%x:%x, dcid=%d, len=%d, space=%ld\n",
1531 __func__,
1532 rt->src.b[5], rt->src.b[4], rt->src.b[3],
1533 rt->src.b[2], rt->src.b[1], rt->src.b[0],
1534 hdr->dcid, m->m_pkthdr.len,
1535 sbspace(&pcb->so->so_rcv));
1536
1537 mtx_unlock(&pcb->pcb_mtx);
1538 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1539 goto drop;
1540 }
1541
1542 /* Append packet to the socket receive queue and wakeup */
1543 sbappendrecord(&pcb->so->so_rcv, m);
1544 m = NULL;
1545
1546 sorwakeup(pcb->so);
1547
1548 mtx_unlock(&pcb->pcb_mtx);
1549 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1550 } else if (hdr->dcid == NG_L2CAP_CLT_CID) {
1551 /* Broadcast packet: give packet to all sockets */
1552
1553 /* Check packet size against connectionless MTU */
1554 if (hdr->length > NG_L2CAP_MTU_DEFAULT) {
1555 NG_BTSOCKET_L2CAP_ERR(
1556 "%s: Connectionless L2CAP data packet too big, " \
1557 "src bdaddr=%x:%x:%x:%x:%x:%x, length=%d\n",
1558 __func__,
1559 rt->src.b[5], rt->src.b[4], rt->src.b[3],
1560 rt->src.b[2], rt->src.b[1], rt->src.b[0],
1561 hdr->length);
1562 goto drop;
1563 }
1564
1565 /* Make sure we can access connectionless header */
1566 if (m->m_pkthdr.len < sizeof(*clt_hdr)) {
1567 NG_BTSOCKET_L2CAP_ERR(
1568 "%s: Can not get L2CAP connectionless packet header, " \
1569 "src bdaddr=%x:%x:%x:%x:%x:%x, length=%d\n",
1570 __func__,
1571 rt->src.b[5], rt->src.b[4], rt->src.b[3],
1572 rt->src.b[2], rt->src.b[1], rt->src.b[0],
1573 hdr->length);
1574 goto drop;
1575 }
1576
1577 if (m->m_len < sizeof(*clt_hdr)) {
1578 m = m_pullup(m, sizeof(*clt_hdr));
1579 if (m == NULL)
1580 goto drop;
1581 }
1582
1583 /* Strip connectionless header and deliver packet */
1584 clt_hdr = mtod(m, ng_l2cap_clt_hdr_t *);
1585 m_adj(m, sizeof(*clt_hdr));
1586
1587 NG_BTSOCKET_L2CAP_INFO(
1588 "%s: Got L2CAP connectionless data packet, " \
1589 "src bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, length=%d\n",
1590 __func__,
1591 rt->src.b[5], rt->src.b[4], rt->src.b[3],
1592 rt->src.b[2], rt->src.b[1], rt->src.b[0],
1593 clt_hdr->psm, hdr->length);
1594
1595 mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
1596
1597 LIST_FOREACH(pcb, &ng_btsocket_l2cap_sockets, next) {
1598 struct mbuf *copy = NULL;
1599
1600 mtx_lock(&pcb->pcb_mtx);
1601
1602 if (bcmp(&rt->src, &pcb->src, sizeof(pcb->src)) != 0 ||
1603 pcb->psm != clt_hdr->psm ||
1604 pcb->state != NG_BTSOCKET_L2CAP_OPEN ||
1605 (pcb->so->so_options & SO_BROADCAST) == 0 ||
1606 m->m_pkthdr.len > sbspace(&pcb->so->so_rcv))
1607 goto next;
1608
1609 /*
1610 * Create a copy of the packet and append it to the
1611 * socket's queue. If m_dup() failed - no big deal
1612 * it is a broadcast traffic after all
1613 */
1614
1615 copy = m_dup(m, M_NOWAIT);
1616 if (copy != NULL) {
1617 sbappendrecord(&pcb->so->so_rcv, copy);
1618 sorwakeup(pcb->so);
1619 }
1620 next:
1621 mtx_unlock(&pcb->pcb_mtx);
1622 }
1623
1624 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1625 }
1626 drop:
1627 NG_FREE_M(m); /* checks for m != NULL */
1628 } /* ng_btsocket_l2cap_data_input */
1629
1630 /*
1631 * L2CAP sockets default message input routine
1632 */
1633
1634 static void
ng_btsocket_l2cap_default_msg_input(struct ng_mesg * msg,hook_p hook)1635 ng_btsocket_l2cap_default_msg_input(struct ng_mesg *msg, hook_p hook)
1636 {
1637 switch (msg->header.cmd) {
1638 case NGM_L2CAP_NODE_HOOK_INFO: {
1639 ng_btsocket_l2cap_rtentry_t *rt = NULL;
1640 ng_l2cap_node_hook_info_ep *ep =
1641 (ng_l2cap_node_hook_info_ep *)msg->data;
1642 if (hook == NULL || msg->header.arglen != sizeof(*ep))
1643 break;
1644
1645 if (bcmp(&ep->addr, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0)
1646 break;
1647
1648 mtx_lock(&ng_btsocket_l2cap_rt_mtx);
1649
1650 rt = (ng_btsocket_l2cap_rtentry_t *) NG_HOOK_PRIVATE(hook);
1651 if (rt == NULL) {
1652 rt = malloc(sizeof(*rt),
1653 M_NETGRAPH_BTSOCKET_L2CAP, M_NOWAIT|M_ZERO);
1654 if (rt == NULL) {
1655 mtx_unlock(&ng_btsocket_l2cap_rt_mtx);
1656 break;
1657 }
1658
1659 LIST_INSERT_HEAD(&ng_btsocket_l2cap_rt, rt, next);
1660
1661 NG_HOOK_SET_PRIVATE(hook, rt);
1662 }
1663
1664 bcopy(&ep->addr, &rt->src, sizeof(rt->src));
1665 rt->hook = hook;
1666
1667 mtx_unlock(&ng_btsocket_l2cap_rt_mtx);
1668
1669 NG_BTSOCKET_L2CAP_INFO(
1670 "%s: Updating hook \"%s\", src bdaddr=%x:%x:%x:%x:%x:%x\n",
1671 __func__, NG_HOOK_NAME(hook),
1672 rt->src.b[5], rt->src.b[4], rt->src.b[3],
1673 rt->src.b[2], rt->src.b[1], rt->src.b[0]);
1674 } break;
1675
1676 default:
1677 NG_BTSOCKET_L2CAP_WARN(
1678 "%s: Unknown message, cmd=%d\n", __func__, msg->header.cmd);
1679 break;
1680 }
1681
1682 NG_FREE_MSG(msg); /* Checks for msg != NULL */
1683 } /* ng_btsocket_l2cap_default_msg_input */
1684
1685 /*
1686 * L2CAP sockets L2CA message input routine
1687 */
1688
1689 static void
ng_btsocket_l2cap_l2ca_msg_input(struct ng_mesg * msg,hook_p hook)1690 ng_btsocket_l2cap_l2ca_msg_input(struct ng_mesg *msg, hook_p hook)
1691 {
1692 ng_btsocket_l2cap_rtentry_p rt = NULL;
1693
1694 if (hook == NULL) {
1695 NG_BTSOCKET_L2CAP_ALERT(
1696 "%s: Invalid source hook for L2CA message\n", __func__);
1697 goto drop;
1698 }
1699
1700 rt = (ng_btsocket_l2cap_rtentry_p) NG_HOOK_PRIVATE(hook);
1701 if (rt == NULL) {
1702 NG_BTSOCKET_L2CAP_ALERT(
1703 "%s: Could not find out source bdaddr for L2CA message\n", __func__);
1704 goto drop;
1705 }
1706
1707 switch (msg->header.cmd) {
1708 case NGM_L2CAP_L2CA_CON: /* L2CA_Connect response */
1709 ng_btsocket_l2cap_process_l2ca_con_req_rsp(msg, rt);
1710 break;
1711
1712 case NGM_L2CAP_L2CA_CON_RSP: /* L2CA_ConnectRsp response */
1713 ng_btsocket_l2cap_process_l2ca_con_rsp_rsp(msg, rt);
1714 break;
1715
1716 case NGM_L2CAP_L2CA_CON_IND: /* L2CA_Connect indicator */
1717 ng_btsocket_l2cap_process_l2ca_con_ind(msg, rt);
1718 break;
1719
1720 case NGM_L2CAP_L2CA_CFG: /* L2CA_Config response */
1721 ng_btsocket_l2cap_process_l2ca_cfg_req_rsp(msg, rt);
1722 break;
1723
1724 case NGM_L2CAP_L2CA_CFG_RSP: /* L2CA_ConfigRsp response */
1725 ng_btsocket_l2cap_process_l2ca_cfg_rsp_rsp(msg, rt);
1726 break;
1727
1728 case NGM_L2CAP_L2CA_CFG_IND: /* L2CA_Config indicator */
1729 ng_btsocket_l2cap_process_l2ca_cfg_ind(msg, rt);
1730 break;
1731
1732 case NGM_L2CAP_L2CA_DISCON: /* L2CA_Disconnect response */
1733 ng_btsocket_l2cap_process_l2ca_discon_rsp(msg, rt);
1734 break;
1735
1736 case NGM_L2CAP_L2CA_DISCON_IND: /* L2CA_Disconnect indicator */
1737 ng_btsocket_l2cap_process_l2ca_discon_ind(msg, rt);
1738 break;
1739
1740 case NGM_L2CAP_L2CA_WRITE: /* L2CA_Write response */
1741 ng_btsocket_l2cap_process_l2ca_write_rsp(msg, rt);
1742 break;
1743 case NGM_L2CAP_L2CA_ENC_CHANGE:
1744 ng_btsocket_l2cap_process_l2ca_enc_change(msg, rt);
1745
1746 break;
1747 /* XXX FIXME add other L2CA messages */
1748
1749 default:
1750 NG_BTSOCKET_L2CAP_WARN(
1751 "%s: Unknown L2CA message, cmd=%d\n", __func__, msg->header.cmd);
1752 break;
1753 }
1754 drop:
1755 NG_FREE_MSG(msg);
1756 } /* ng_btsocket_l2cap_l2ca_msg_input */
1757
1758 /*
1759 * L2CAP sockets input routine
1760 */
1761
1762 static void
ng_btsocket_l2cap_input(void * context,int pending)1763 ng_btsocket_l2cap_input(void *context, int pending)
1764 {
1765 item_p item = NULL;
1766 hook_p hook = NULL;
1767
1768 for (;;) {
1769 mtx_lock(&ng_btsocket_l2cap_queue_mtx);
1770 NG_BT_ITEMQ_DEQUEUE(&ng_btsocket_l2cap_queue, item);
1771 mtx_unlock(&ng_btsocket_l2cap_queue_mtx);
1772
1773 if (item == NULL)
1774 break;
1775
1776 NGI_GET_HOOK(item, hook);
1777 if (hook != NULL && NG_HOOK_NOT_VALID(hook))
1778 goto drop;
1779
1780 switch(item->el_flags & NGQF_TYPE) {
1781 case NGQF_DATA: {
1782 struct mbuf *m = NULL;
1783
1784 NGI_GET_M(item, m);
1785 ng_btsocket_l2cap_data_input(m, hook);
1786 } break;
1787
1788 case NGQF_MESG: {
1789 struct ng_mesg *msg = NULL;
1790
1791 NGI_GET_MSG(item, msg);
1792
1793 switch (msg->header.cmd) {
1794 case NGM_L2CAP_L2CA_CON:
1795 case NGM_L2CAP_L2CA_CON_RSP:
1796 case NGM_L2CAP_L2CA_CON_IND:
1797 case NGM_L2CAP_L2CA_CFG:
1798 case NGM_L2CAP_L2CA_CFG_RSP:
1799 case NGM_L2CAP_L2CA_CFG_IND:
1800 case NGM_L2CAP_L2CA_DISCON:
1801 case NGM_L2CAP_L2CA_DISCON_IND:
1802 case NGM_L2CAP_L2CA_WRITE:
1803 case NGM_L2CAP_L2CA_ENC_CHANGE:
1804 /* XXX FIXME add other L2CA messages */
1805 ng_btsocket_l2cap_l2ca_msg_input(msg, hook);
1806 break;
1807
1808 default:
1809 ng_btsocket_l2cap_default_msg_input(msg, hook);
1810 break;
1811 }
1812 } break;
1813
1814 default:
1815 KASSERT(0,
1816 ("%s: invalid item type=%ld\n", __func__, (item->el_flags & NGQF_TYPE)));
1817 break;
1818 }
1819 drop:
1820 if (hook != NULL)
1821 NG_HOOK_UNREF(hook);
1822
1823 NG_FREE_ITEM(item);
1824 }
1825 } /* ng_btsocket_l2cap_input */
1826
1827 /*
1828 * Route cleanup task. Gets scheduled when hook is disconnected. Here we
1829 * will find all sockets that use "invalid" hook and disconnect them.
1830 */
1831
1832 static void
ng_btsocket_l2cap_rtclean(void * context,int pending)1833 ng_btsocket_l2cap_rtclean(void *context, int pending)
1834 {
1835 ng_btsocket_l2cap_pcb_p pcb = NULL, pcb_next = NULL;
1836 ng_btsocket_l2cap_rtentry_p rt = NULL;
1837
1838 mtx_lock(&ng_btsocket_l2cap_rt_mtx);
1839 mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
1840
1841 /*
1842 * First disconnect all sockets that use "invalid" hook
1843 */
1844
1845 for (pcb = LIST_FIRST(&ng_btsocket_l2cap_sockets); pcb != NULL; ) {
1846 mtx_lock(&pcb->pcb_mtx);
1847 pcb_next = LIST_NEXT(pcb, next);
1848
1849 if (pcb->rt != NULL &&
1850 pcb->rt->hook != NULL && NG_HOOK_NOT_VALID(pcb->rt->hook)) {
1851 if (pcb->flags & NG_BTSOCKET_L2CAP_TIMO)
1852 ng_btsocket_l2cap_untimeout(pcb);
1853
1854 pcb->so->so_error = ENETDOWN;
1855 pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
1856 soisdisconnected(pcb->so);
1857
1858 pcb->token = 0;
1859 pcb->cid = 0;
1860 pcb->rt = NULL;
1861 }
1862
1863 mtx_unlock(&pcb->pcb_mtx);
1864 pcb = pcb_next;
1865 }
1866
1867 /*
1868 * Now cleanup routing table
1869 */
1870
1871 for (rt = LIST_FIRST(&ng_btsocket_l2cap_rt); rt != NULL; ) {
1872 ng_btsocket_l2cap_rtentry_p rt_next = LIST_NEXT(rt, next);
1873
1874 if (rt->hook != NULL && NG_HOOK_NOT_VALID(rt->hook)) {
1875 LIST_REMOVE(rt, next);
1876
1877 NG_HOOK_SET_PRIVATE(rt->hook, NULL);
1878 NG_HOOK_UNREF(rt->hook); /* Remove extra reference */
1879
1880 bzero(rt, sizeof(*rt));
1881 free(rt, M_NETGRAPH_BTSOCKET_L2CAP);
1882 }
1883
1884 rt = rt_next;
1885 }
1886
1887 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1888 mtx_unlock(&ng_btsocket_l2cap_rt_mtx);
1889 } /* ng_btsocket_l2cap_rtclean */
1890
1891 /*
1892 * Initialize everything
1893 */
1894
1895 void
ng_btsocket_l2cap_init(void)1896 ng_btsocket_l2cap_init(void)
1897 {
1898 int error = 0;
1899
1900 /* Skip initialization of globals for non-default instances. */
1901 if (!IS_DEFAULT_VNET(curvnet))
1902 return;
1903
1904 ng_btsocket_l2cap_node = NULL;
1905 ng_btsocket_l2cap_debug_level = NG_BTSOCKET_WARN_LEVEL;
1906
1907 /* Register Netgraph node type */
1908 error = ng_newtype(&typestruct);
1909 if (error != 0) {
1910 NG_BTSOCKET_L2CAP_ALERT(
1911 "%s: Could not register Netgraph node type, error=%d\n", __func__, error);
1912
1913 return;
1914 }
1915
1916 /* Create Netgrapg node */
1917 error = ng_make_node_common(&typestruct, &ng_btsocket_l2cap_node);
1918 if (error != 0) {
1919 NG_BTSOCKET_L2CAP_ALERT(
1920 "%s: Could not create Netgraph node, error=%d\n", __func__, error);
1921
1922 ng_btsocket_l2cap_node = NULL;
1923
1924 return;
1925 }
1926
1927 error = ng_name_node(ng_btsocket_l2cap_node,
1928 NG_BTSOCKET_L2CAP_NODE_TYPE);
1929 if (error != 0) {
1930 NG_BTSOCKET_L2CAP_ALERT(
1931 "%s: Could not name Netgraph node, error=%d\n", __func__, error);
1932
1933 NG_NODE_UNREF(ng_btsocket_l2cap_node);
1934 ng_btsocket_l2cap_node = NULL;
1935
1936 return;
1937 }
1938
1939 /* Create input queue */
1940 NG_BT_ITEMQ_INIT(&ng_btsocket_l2cap_queue, ifqmaxlen);
1941 mtx_init(&ng_btsocket_l2cap_queue_mtx,
1942 "btsocks_l2cap_queue_mtx", NULL, MTX_DEF);
1943 TASK_INIT(&ng_btsocket_l2cap_queue_task, 0,
1944 ng_btsocket_l2cap_input, NULL);
1945
1946 /* Create list of sockets */
1947 LIST_INIT(&ng_btsocket_l2cap_sockets);
1948 mtx_init(&ng_btsocket_l2cap_sockets_mtx,
1949 "btsocks_l2cap_sockets_mtx", NULL, MTX_DEF);
1950
1951 /* Routing table */
1952 LIST_INIT(&ng_btsocket_l2cap_rt);
1953 mtx_init(&ng_btsocket_l2cap_rt_mtx,
1954 "btsocks_l2cap_rt_mtx", NULL, MTX_DEF);
1955 TASK_INIT(&ng_btsocket_l2cap_rt_task, 0,
1956 ng_btsocket_l2cap_rtclean, NULL);
1957 } /* ng_btsocket_l2cap_init */
1958
1959 /*
1960 * Abort connection on socket
1961 */
1962
1963 void
ng_btsocket_l2cap_abort(struct socket * so)1964 ng_btsocket_l2cap_abort(struct socket *so)
1965 {
1966 so->so_error = ECONNABORTED;
1967
1968 (void)ng_btsocket_l2cap_disconnect(so);
1969 } /* ng_btsocket_l2cap_abort */
1970
1971 void
ng_btsocket_l2cap_close(struct socket * so)1972 ng_btsocket_l2cap_close(struct socket *so)
1973 {
1974
1975 (void)ng_btsocket_l2cap_disconnect(so);
1976 } /* ng_btsocket_l2cap_close */
1977
1978 /*
1979 * Accept connection on socket. Nothing to do here, socket must be connected
1980 * and ready, so just return peer address and be done with it.
1981 */
1982
1983 int
ng_btsocket_l2cap_accept(struct socket * so,struct sockaddr ** nam)1984 ng_btsocket_l2cap_accept(struct socket *so, struct sockaddr **nam)
1985 {
1986 if (ng_btsocket_l2cap_node == NULL)
1987 return (EINVAL);
1988
1989 return (ng_btsocket_l2cap_peeraddr(so, nam));
1990 } /* ng_btsocket_l2cap_accept */
1991
1992 /*
1993 * Create and attach new socket
1994 */
1995
1996 int
ng_btsocket_l2cap_attach(struct socket * so,int proto,struct thread * td)1997 ng_btsocket_l2cap_attach(struct socket *so, int proto, struct thread *td)
1998 {
1999 static u_int32_t token = 0;
2000 ng_btsocket_l2cap_pcb_p pcb = so2l2cap_pcb(so);
2001 int error;
2002
2003 /* Check socket and protocol */
2004 if (ng_btsocket_l2cap_node == NULL)
2005 return (EPROTONOSUPPORT);
2006 if (so->so_type != SOCK_SEQPACKET)
2007 return (ESOCKTNOSUPPORT);
2008
2009 #if 0 /* XXX sonewconn() calls "pru_attach" with proto == 0 */
2010 if (proto != 0)
2011 if (proto != BLUETOOTH_PROTO_L2CAP)
2012 return (EPROTONOSUPPORT);
2013 #endif /* XXX */
2014
2015 if (pcb != NULL)
2016 return (EISCONN);
2017
2018 /* Reserve send and receive space if it is not reserved yet */
2019 if ((so->so_snd.sb_hiwat == 0) || (so->so_rcv.sb_hiwat == 0)) {
2020 error = soreserve(so, NG_BTSOCKET_L2CAP_SENDSPACE,
2021 NG_BTSOCKET_L2CAP_RECVSPACE);
2022 if (error != 0)
2023 return (error);
2024 }
2025
2026 /* Allocate the PCB */
2027 pcb = malloc(sizeof(*pcb),
2028 M_NETGRAPH_BTSOCKET_L2CAP, M_NOWAIT | M_ZERO);
2029 if (pcb == NULL)
2030 return (ENOMEM);
2031
2032 /* Link the PCB and the socket */
2033 so->so_pcb = (caddr_t) pcb;
2034 pcb->so = so;
2035 pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
2036
2037 /* Initialize PCB */
2038 pcb->imtu = pcb->omtu = NG_L2CAP_MTU_DEFAULT;
2039
2040 /* Default flow */
2041 pcb->iflow.flags = 0x0;
2042 pcb->iflow.service_type = NG_HCI_SERVICE_TYPE_BEST_EFFORT;
2043 pcb->iflow.token_rate = 0xffffffff; /* maximum */
2044 pcb->iflow.token_bucket_size = 0xffffffff; /* maximum */
2045 pcb->iflow.peak_bandwidth = 0x00000000; /* maximum */
2046 pcb->iflow.latency = 0xffffffff; /* don't care */
2047 pcb->iflow.delay_variation = 0xffffffff; /* don't care */
2048
2049 bcopy(&pcb->iflow, &pcb->oflow, sizeof(pcb->oflow));
2050
2051 pcb->flush_timo = NG_L2CAP_FLUSH_TIMO_DEFAULT;
2052 pcb->link_timo = NG_L2CAP_LINK_TIMO_DEFAULT;
2053
2054 /*
2055 * XXX Mark PCB mutex as DUPOK to prevent "duplicated lock of
2056 * the same type" message. When accepting new L2CAP connection
2057 * ng_btsocket_l2cap_process_l2ca_con_ind() holds both PCB mutexes
2058 * for "old" (accepting) PCB and "new" (created) PCB.
2059 */
2060
2061 mtx_init(&pcb->pcb_mtx, "btsocks_l2cap_pcb_mtx", NULL,
2062 MTX_DEF|MTX_DUPOK);
2063 callout_init_mtx(&pcb->timo, &pcb->pcb_mtx, 0);
2064
2065 /*
2066 * Add the PCB to the list
2067 *
2068 * XXX FIXME VERY IMPORTANT!
2069 *
2070 * This is totally FUBAR. We could get here in two cases:
2071 *
2072 * 1) When user calls socket()
2073 * 2) When we need to accept new incoming connection and call
2074 * sonewconn()
2075 *
2076 * In the first case we must acquire ng_btsocket_l2cap_sockets_mtx.
2077 * In the second case we hold ng_btsocket_l2cap_sockets_mtx already.
2078 * So we now need to distinguish between these cases. From reading
2079 * /sys/kern/uipc_socket.c we can find out that sonewconn() calls
2080 * pru_attach with proto == 0 and td == NULL. For now use this fact
2081 * to figure out if we were called from socket() or from sonewconn().
2082 */
2083
2084 if (td != NULL)
2085 mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
2086 else
2087 mtx_assert(&ng_btsocket_l2cap_sockets_mtx, MA_OWNED);
2088
2089 /* Set PCB token. Use ng_btsocket_l2cap_sockets_mtx for protection */
2090 if (++ token == 0)
2091 token ++;
2092
2093 pcb->token = token;
2094
2095 LIST_INSERT_HEAD(&ng_btsocket_l2cap_sockets, pcb, next);
2096
2097 if (td != NULL)
2098 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
2099
2100 return (0);
2101 } /* ng_btsocket_l2cap_attach */
2102
2103 /*
2104 * Bind socket
2105 */
2106
2107 int
ng_btsocket_l2cap_bind(struct socket * so,struct sockaddr * nam,struct thread * td)2108 ng_btsocket_l2cap_bind(struct socket *so, struct sockaddr *nam,
2109 struct thread *td)
2110 {
2111 ng_btsocket_l2cap_pcb_t *pcb = NULL;
2112 struct sockaddr_l2cap *sa = (struct sockaddr_l2cap *) nam;
2113 int psm, error = 0;
2114
2115 if (ng_btsocket_l2cap_node == NULL)
2116 return (EINVAL);
2117
2118 /* Verify address */
2119 if (sa == NULL)
2120 return (EINVAL);
2121 if (sa->l2cap_family != AF_BLUETOOTH)
2122 return (EAFNOSUPPORT);
2123 /*For the time being, Not support LE binding.*/
2124 if ((sa->l2cap_len != sizeof(*sa))&&
2125 (sa->l2cap_len != sizeof(struct sockaddr_l2cap_compat)))
2126 return (EINVAL);
2127
2128 psm = le16toh(sa->l2cap_psm);
2129
2130 /*
2131 * Check if other socket has this address already (look for exact
2132 * match PSM and bdaddr) and assign socket address if it's available.
2133 *
2134 * Note: socket can be bound to ANY PSM (zero) thus allowing several
2135 * channels with the same PSM between the same pair of BD_ADDR'es.
2136 */
2137
2138 mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
2139
2140 LIST_FOREACH(pcb, &ng_btsocket_l2cap_sockets, next)
2141 if (psm != 0 && psm == pcb->psm &&
2142 bcmp(&pcb->src, &sa->l2cap_bdaddr, sizeof(bdaddr_t)) == 0)
2143 break;
2144
2145 if (pcb == NULL) {
2146 /* Set socket address */
2147 pcb = so2l2cap_pcb(so);
2148 if (pcb != NULL) {
2149 bcopy(&sa->l2cap_bdaddr, &pcb->src, sizeof(pcb->src));
2150 pcb->psm = psm;
2151 } else
2152 error = EINVAL;
2153 } else
2154 error = EADDRINUSE;
2155
2156 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
2157
2158 return (error);
2159 } /* ng_btsocket_l2cap_bind */
2160
2161 /*
2162 * Connect socket
2163 */
2164
2165 int
ng_btsocket_l2cap_connect(struct socket * so,struct sockaddr * nam,struct thread * td)2166 ng_btsocket_l2cap_connect(struct socket *so, struct sockaddr *nam,
2167 struct thread *td)
2168 {
2169 ng_btsocket_l2cap_pcb_t *pcb = so2l2cap_pcb(so);
2170 struct sockaddr_l2cap_compat *sal = (struct sockaddr_l2cap_compat *) nam;
2171 struct sockaddr_l2cap *sa = (struct sockaddr_l2cap *)nam;
2172 struct sockaddr_l2cap ba;
2173 ng_btsocket_l2cap_rtentry_t *rt = NULL;
2174 int have_src, error = 0;
2175 int idtype = NG_L2CAP_L2CA_IDTYPE_BREDR;
2176 /* Check socket */
2177 if (pcb == NULL)
2178 return (EINVAL);
2179 if (ng_btsocket_l2cap_node == NULL)
2180 return (EINVAL);
2181 if (pcb->state == NG_BTSOCKET_L2CAP_CONNECTING)
2182 return (EINPROGRESS);
2183
2184 /* Verify address */
2185 if (sa == NULL)
2186 return (EINVAL);
2187 if (sa->l2cap_family != AF_BLUETOOTH)
2188 return (EAFNOSUPPORT);
2189 if (sa->l2cap_len == sizeof(*sal)){
2190 bcopy(sal, &ba, sizeof(*sal));
2191 sa = &ba;
2192 sa->l2cap_len = sizeof(*sa);
2193 sa->l2cap_bdaddr_type = BDADDR_BREDR;
2194 }
2195 if (sa->l2cap_len != sizeof(*sa))
2196 return (EINVAL);
2197 if ((sa->l2cap_psm && sa->l2cap_cid))
2198 return EINVAL;
2199 if (bcmp(&sa->l2cap_bdaddr, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0)
2200 return (EDESTADDRREQ);
2201 if((sa->l2cap_bdaddr_type == BDADDR_BREDR)&&
2202 (sa->l2cap_psm == 0))
2203 return EDESTADDRREQ;
2204 if(sa->l2cap_bdaddr_type != BDADDR_BREDR){
2205 if(sa->l2cap_cid == NG_L2CAP_ATT_CID){
2206 idtype = NG_L2CAP_L2CA_IDTYPE_ATT;
2207 }else if (sa->l2cap_cid == NG_L2CAP_SMP_CID){
2208 idtype =NG_L2CAP_L2CA_IDTYPE_SMP;
2209 }else{
2210 //if cid == 0 idtype = NG_L2CAP_L2CA_IDTYPE_LE;
2211 // Not supported yet
2212 return EINVAL;
2213 }
2214 }
2215 if (pcb->psm != 0 && pcb->psm != le16toh(sa->l2cap_psm))
2216 return (EINVAL);
2217 /*
2218 * Routing. Socket should be bound to some source address. The source
2219 * address can be ANY. Destination address must be set and it must not
2220 * be ANY. If source address is ANY then find first rtentry that has
2221 * src != dst.
2222 */
2223
2224 mtx_lock(&ng_btsocket_l2cap_rt_mtx);
2225 mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
2226 mtx_lock(&pcb->pcb_mtx);
2227
2228 /* Send destination address and PSM */
2229 bcopy(&sa->l2cap_bdaddr, &pcb->dst, sizeof(pcb->dst));
2230 pcb->psm = le16toh(sa->l2cap_psm);
2231 pcb->dsttype = sa->l2cap_bdaddr_type;
2232 pcb->cid = 0;
2233 pcb->idtype = idtype;
2234 pcb->rt = NULL;
2235 have_src = bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(pcb->src));
2236
2237 LIST_FOREACH(rt, &ng_btsocket_l2cap_rt, next) {
2238 if (rt->hook == NULL || NG_HOOK_NOT_VALID(rt->hook))
2239 continue;
2240
2241 /* Match src and dst */
2242 if (have_src) {
2243 if (bcmp(&pcb->src, &rt->src, sizeof(rt->src)) == 0)
2244 break;
2245 } else {
2246 if (bcmp(&pcb->dst, &rt->src, sizeof(rt->src)) != 0)
2247 break;
2248 }
2249 }
2250
2251 if (rt != NULL) {
2252 pcb->rt = rt;
2253
2254 if (!have_src){
2255 bcopy(&rt->src, &pcb->src, sizeof(pcb->src));
2256 pcb->srctype =
2257 (sa->l2cap_bdaddr_type == BDADDR_BREDR)?
2258 BDADDR_BREDR : BDADDR_LE_PUBLIC;
2259 }
2260 } else
2261 error = EHOSTUNREACH;
2262
2263 /*
2264 * Send L2CA_Connect request
2265 */
2266
2267 if (error == 0) {
2268 error = ng_btsocket_l2cap_send_l2ca_con_req(pcb);
2269 if (error == 0) {
2270 pcb->flags |= NG_BTSOCKET_L2CAP_CLIENT;
2271 pcb->state = NG_BTSOCKET_L2CAP_CONNECTING;
2272 soisconnecting(pcb->so);
2273
2274 ng_btsocket_l2cap_timeout(pcb);
2275 }
2276 }
2277
2278 mtx_unlock(&pcb->pcb_mtx);
2279 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
2280 mtx_unlock(&ng_btsocket_l2cap_rt_mtx);
2281
2282 return (error);
2283 } /* ng_btsocket_l2cap_connect */
2284
2285 /*
2286 * Process ioctl's calls on socket
2287 */
2288
2289 int
ng_btsocket_l2cap_control(struct socket * so,u_long cmd,caddr_t data,struct ifnet * ifp,struct thread * td)2290 ng_btsocket_l2cap_control(struct socket *so, u_long cmd, caddr_t data,
2291 struct ifnet *ifp, struct thread *td)
2292 {
2293 return (EINVAL);
2294 } /* ng_btsocket_l2cap_control */
2295
2296 /*
2297 * Process getsockopt/setsockopt system calls
2298 */
2299
2300 int
ng_btsocket_l2cap_ctloutput(struct socket * so,struct sockopt * sopt)2301 ng_btsocket_l2cap_ctloutput(struct socket *so, struct sockopt *sopt)
2302 {
2303 ng_btsocket_l2cap_pcb_p pcb = so2l2cap_pcb(so);
2304 int error = 0;
2305 ng_l2cap_cfg_opt_val_t v;
2306
2307 if (pcb == NULL)
2308 return (EINVAL);
2309 if (ng_btsocket_l2cap_node == NULL)
2310 return (EINVAL);
2311
2312 if (sopt->sopt_level != SOL_L2CAP)
2313 return (0);
2314
2315 mtx_lock(&pcb->pcb_mtx);
2316
2317 switch (sopt->sopt_dir) {
2318 case SOPT_GET:
2319 switch (sopt->sopt_name) {
2320 case SO_L2CAP_IMTU: /* get incoming MTU */
2321 error = sooptcopyout(sopt, &pcb->imtu,
2322 sizeof(pcb->imtu));
2323 break;
2324
2325 case SO_L2CAP_OMTU: /* get outgoing (peer incoming) MTU */
2326 error = sooptcopyout(sopt, &pcb->omtu,
2327 sizeof(pcb->omtu));
2328 break;
2329
2330 case SO_L2CAP_IFLOW: /* get incoming flow spec. */
2331 error = sooptcopyout(sopt, &pcb->iflow,
2332 sizeof(pcb->iflow));
2333 break;
2334
2335 case SO_L2CAP_OFLOW: /* get outgoing flow spec. */
2336 error = sooptcopyout(sopt, &pcb->oflow,
2337 sizeof(pcb->oflow));
2338 break;
2339
2340 case SO_L2CAP_FLUSH: /* get flush timeout */
2341 error = sooptcopyout(sopt, &pcb->flush_timo,
2342 sizeof(pcb->flush_timo));
2343 break;
2344 case SO_L2CAP_ENCRYPTED: /* get encrypt required */
2345 error = sooptcopyout(sopt, &pcb->need_encrypt,
2346 sizeof(pcb->need_encrypt));
2347 break;
2348
2349
2350 default:
2351 error = ENOPROTOOPT;
2352 break;
2353 }
2354 break;
2355
2356 case SOPT_SET:
2357 /*
2358 * XXX
2359 * We do not allow to change these parameters while socket is
2360 * connected or we are in the process of creating a connection.
2361 * May be this should indicate re-configuration of the open
2362 * channel?
2363 */
2364
2365 if (pcb->state != NG_BTSOCKET_L2CAP_CLOSED) {
2366 error = EACCES;
2367 break;
2368 }
2369
2370 switch (sopt->sopt_name) {
2371 case SO_L2CAP_IMTU: /* set incoming MTU */
2372 error = sooptcopyin(sopt, &v, sizeof(v), sizeof(v.mtu));
2373 if (error == 0)
2374 pcb->imtu = v.mtu;
2375 break;
2376
2377 case SO_L2CAP_OFLOW: /* set outgoing flow spec. */
2378 error = sooptcopyin(sopt, &v, sizeof(v),sizeof(v.flow));
2379 if (error == 0)
2380 bcopy(&v.flow, &pcb->oflow, sizeof(pcb->oflow));
2381 break;
2382
2383 case SO_L2CAP_FLUSH: /* set flush timeout */
2384 error = sooptcopyin(sopt, &v, sizeof(v),
2385 sizeof(v.flush_timo));
2386 if (error == 0)
2387 pcb->flush_timo = v.flush_timo;
2388 break;
2389 case SO_L2CAP_ENCRYPTED: /*set connect encryption opt*/
2390 if((pcb->state != NG_BTSOCKET_L2CAP_OPEN) &&
2391 (pcb->state != NG_BTSOCKET_L2CAP_W4_ENC_CHANGE)){
2392 error = sooptcopyin(sopt, &v, sizeof(v),
2393 sizeof(v.encryption));
2394 if(error == 0)
2395 pcb->need_encrypt = (v.encryption)?1:0;
2396 }else{
2397 error = EINVAL;
2398 }
2399 break;
2400 default:
2401 error = ENOPROTOOPT;
2402 break;
2403 }
2404 break;
2405
2406 default:
2407 error = EINVAL;
2408 break;
2409 }
2410
2411 mtx_unlock(&pcb->pcb_mtx);
2412
2413 return (error);
2414 } /* ng_btsocket_l2cap_ctloutput */
2415
2416 /*
2417 * Detach and destroy socket
2418 */
2419
2420 void
ng_btsocket_l2cap_detach(struct socket * so)2421 ng_btsocket_l2cap_detach(struct socket *so)
2422 {
2423 ng_btsocket_l2cap_pcb_p pcb = so2l2cap_pcb(so);
2424
2425 KASSERT(pcb != NULL, ("ng_btsocket_l2cap_detach: pcb == NULL"));
2426
2427 if (ng_btsocket_l2cap_node == NULL)
2428 return;
2429
2430 mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
2431 mtx_lock(&pcb->pcb_mtx);
2432
2433 /* XXX what to do with pending request? */
2434 if (pcb->flags & NG_BTSOCKET_L2CAP_TIMO)
2435 ng_btsocket_l2cap_untimeout(pcb);
2436
2437 if (pcb->state != NG_BTSOCKET_L2CAP_CLOSED &&
2438 pcb->state != NG_BTSOCKET_L2CAP_DISCONNECTING)
2439 /* Send disconnect request with "zero" token */
2440 ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);
2441
2442 pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
2443
2444 LIST_REMOVE(pcb, next);
2445
2446 mtx_unlock(&pcb->pcb_mtx);
2447 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
2448
2449 mtx_destroy(&pcb->pcb_mtx);
2450 bzero(pcb, sizeof(*pcb));
2451 free(pcb, M_NETGRAPH_BTSOCKET_L2CAP);
2452
2453 soisdisconnected(so);
2454 so->so_pcb = NULL;
2455 } /* ng_btsocket_l2cap_detach */
2456
2457 /*
2458 * Disconnect socket
2459 */
2460
2461 int
ng_btsocket_l2cap_disconnect(struct socket * so)2462 ng_btsocket_l2cap_disconnect(struct socket *so)
2463 {
2464 ng_btsocket_l2cap_pcb_p pcb = so2l2cap_pcb(so);
2465 int error = 0;
2466
2467 if (pcb == NULL)
2468 return (EINVAL);
2469 if (ng_btsocket_l2cap_node == NULL)
2470 return (EINVAL);
2471
2472 mtx_lock(&pcb->pcb_mtx);
2473
2474 if (pcb->state == NG_BTSOCKET_L2CAP_DISCONNECTING) {
2475 mtx_unlock(&pcb->pcb_mtx);
2476 return (EINPROGRESS);
2477 }
2478
2479 if (pcb->state != NG_BTSOCKET_L2CAP_CLOSED) {
2480 /* XXX FIXME what to do with pending request? */
2481 if (pcb->flags & NG_BTSOCKET_L2CAP_TIMO)
2482 ng_btsocket_l2cap_untimeout(pcb);
2483
2484 error = ng_btsocket_l2cap_send_l2ca_discon_req(pcb->token, pcb);
2485 if (error == 0) {
2486 pcb->state = NG_BTSOCKET_L2CAP_DISCONNECTING;
2487 soisdisconnecting(so);
2488
2489 ng_btsocket_l2cap_timeout(pcb);
2490 }
2491
2492 /* XXX FIXME what to do if error != 0 */
2493 }
2494
2495 mtx_unlock(&pcb->pcb_mtx);
2496
2497 return (error);
2498 } /* ng_btsocket_l2cap_disconnect */
2499
2500 /*
2501 * Listen on socket
2502 */
2503
2504 int
ng_btsocket_l2cap_listen(struct socket * so,int backlog,struct thread * td)2505 ng_btsocket_l2cap_listen(struct socket *so, int backlog, struct thread *td)
2506 {
2507 ng_btsocket_l2cap_pcb_p pcb = so2l2cap_pcb(so);
2508 int error;
2509
2510 SOCK_LOCK(so);
2511 error = solisten_proto_check(so);
2512 if (error != 0)
2513 goto out;
2514 if (pcb == NULL) {
2515 error = EINVAL;
2516 goto out;
2517 }
2518 if (ng_btsocket_l2cap_node == NULL) {
2519 error = EINVAL;
2520 goto out;
2521 }
2522 if (pcb->psm == 0) {
2523 error = EADDRNOTAVAIL;
2524 goto out;
2525 }
2526 solisten_proto(so, backlog);
2527 out:
2528 SOCK_UNLOCK(so);
2529 return (error);
2530 } /* ng_btsocket_listen */
2531
2532 /*
2533 * Get peer address
2534 */
2535
2536 int
ng_btsocket_l2cap_peeraddr(struct socket * so,struct sockaddr ** nam)2537 ng_btsocket_l2cap_peeraddr(struct socket *so, struct sockaddr **nam)
2538 {
2539 ng_btsocket_l2cap_pcb_p pcb = so2l2cap_pcb(so);
2540 struct sockaddr_l2cap sa;
2541
2542 if (pcb == NULL)
2543 return (EINVAL);
2544 if (ng_btsocket_l2cap_node == NULL)
2545 return (EINVAL);
2546
2547 bcopy(&pcb->dst, &sa.l2cap_bdaddr, sizeof(sa.l2cap_bdaddr));
2548 sa.l2cap_psm = htole16(pcb->psm);
2549 sa.l2cap_len = sizeof(sa);
2550 sa.l2cap_family = AF_BLUETOOTH;
2551 switch(pcb->idtype){
2552 case NG_L2CAP_L2CA_IDTYPE_ATT:
2553 sa.l2cap_cid = NG_L2CAP_ATT_CID;
2554 break;
2555 case NG_L2CAP_L2CA_IDTYPE_SMP:
2556 sa.l2cap_cid = NG_L2CAP_SMP_CID;
2557 break;
2558 default:
2559 sa.l2cap_cid = 0;
2560 break;
2561 }
2562 sa.l2cap_bdaddr_type = pcb->dsttype;
2563 *nam = sodupsockaddr((struct sockaddr *) &sa, M_NOWAIT);
2564
2565 return ((*nam == NULL)? ENOMEM : 0);
2566 } /* ng_btsocket_l2cap_peeraddr */
2567
2568 /*
2569 * Send data to socket
2570 */
2571
2572 int
ng_btsocket_l2cap_send(struct socket * so,int flags,struct mbuf * m,struct sockaddr * nam,struct mbuf * control,struct thread * td)2573 ng_btsocket_l2cap_send(struct socket *so, int flags, struct mbuf *m,
2574 struct sockaddr *nam, struct mbuf *control, struct thread *td)
2575 {
2576 ng_btsocket_l2cap_pcb_t *pcb = so2l2cap_pcb(so);
2577 int error = 0;
2578
2579 if (ng_btsocket_l2cap_node == NULL) {
2580 error = ENETDOWN;
2581 goto drop;
2582 }
2583
2584 /* Check socket and input */
2585 if (pcb == NULL || m == NULL || control != NULL) {
2586 error = EINVAL;
2587 goto drop;
2588 }
2589
2590 mtx_lock(&pcb->pcb_mtx);
2591
2592 /* Make sure socket is connected */
2593 if (pcb->state != NG_BTSOCKET_L2CAP_OPEN) {
2594 mtx_unlock(&pcb->pcb_mtx);
2595 error = ENOTCONN;
2596 goto drop;
2597 }
2598
2599 /* Check route */
2600 if (pcb->rt == NULL ||
2601 pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook)) {
2602 mtx_unlock(&pcb->pcb_mtx);
2603 error = ENETDOWN;
2604 goto drop;
2605 }
2606
2607 /* Check packet size against outgoing (peer's incoming) MTU) */
2608 if (m->m_pkthdr.len > pcb->omtu) {
2609 NG_BTSOCKET_L2CAP_ERR(
2610 "%s: Packet too big, len=%d, omtu=%d\n", __func__, m->m_pkthdr.len, pcb->omtu);
2611
2612 mtx_unlock(&pcb->pcb_mtx);
2613 error = EMSGSIZE;
2614 goto drop;
2615 }
2616
2617 /*
2618 * First put packet on socket send queue. Then check if we have
2619 * pending timeout. If we do not have timeout then we must send
2620 * packet and schedule timeout. Otherwise do nothing and wait for
2621 * L2CA_WRITE_RSP.
2622 */
2623
2624 sbappendrecord(&pcb->so->so_snd, m);
2625 m = NULL;
2626
2627 if (!(pcb->flags & NG_BTSOCKET_L2CAP_TIMO)) {
2628 error = ng_btsocket_l2cap_send2(pcb);
2629 if (error == 0)
2630 ng_btsocket_l2cap_timeout(pcb);
2631 else
2632 sbdroprecord(&pcb->so->so_snd); /* XXX */
2633 }
2634
2635 mtx_unlock(&pcb->pcb_mtx);
2636 drop:
2637 NG_FREE_M(m); /* checks for != NULL */
2638 NG_FREE_M(control);
2639
2640 return (error);
2641 } /* ng_btsocket_l2cap_send */
2642
2643 /*
2644 * Send first packet in the socket queue to the L2CAP layer
2645 */
2646
2647 static int
ng_btsocket_l2cap_send2(ng_btsocket_l2cap_pcb_p pcb)2648 ng_btsocket_l2cap_send2(ng_btsocket_l2cap_pcb_p pcb)
2649 {
2650 struct mbuf *m = NULL;
2651 ng_l2cap_l2ca_hdr_t *hdr = NULL;
2652 int error = 0;
2653
2654 mtx_assert(&pcb->pcb_mtx, MA_OWNED);
2655
2656 if (sbavail(&pcb->so->so_snd) == 0)
2657 return (EINVAL); /* XXX */
2658
2659 m = m_dup(pcb->so->so_snd.sb_mb, M_NOWAIT);
2660 if (m == NULL)
2661 return (ENOBUFS);
2662
2663 /* Create L2CA packet header */
2664 M_PREPEND(m, sizeof(*hdr), M_NOWAIT);
2665 if (m != NULL)
2666 if (m->m_len < sizeof(*hdr))
2667 m = m_pullup(m, sizeof(*hdr));
2668
2669 if (m == NULL) {
2670 NG_BTSOCKET_L2CAP_ERR(
2671 "%s: Failed to create L2CA packet header\n", __func__);
2672
2673 return (ENOBUFS);
2674 }
2675
2676 hdr = mtod(m, ng_l2cap_l2ca_hdr_t *);
2677 hdr->token = pcb->token;
2678 hdr->length = m->m_pkthdr.len - sizeof(*hdr);
2679 hdr->lcid = pcb->cid;
2680 hdr->idtype = pcb->idtype;
2681 NG_BTSOCKET_L2CAP_INFO(
2682 "%s: Sending packet: len=%d, length=%d, lcid=%d, token=%d, state=%d\n",
2683 __func__, m->m_pkthdr.len, hdr->length, hdr->lcid,
2684 hdr->token, pcb->state);
2685
2686 /*
2687 * If we got here than we have successfully creates new L2CAP
2688 * data packet and now we can send it to the L2CAP layer
2689 */
2690
2691 NG_SEND_DATA_ONLY(error, pcb->rt->hook, m);
2692
2693 return (error);
2694 } /* ng_btsocket_l2cap_send2 */
2695
2696 /*
2697 * Get socket address
2698 */
2699
2700 int
ng_btsocket_l2cap_sockaddr(struct socket * so,struct sockaddr ** nam)2701 ng_btsocket_l2cap_sockaddr(struct socket *so, struct sockaddr **nam)
2702 {
2703 ng_btsocket_l2cap_pcb_p pcb = so2l2cap_pcb(so);
2704 struct sockaddr_l2cap sa;
2705
2706 if (pcb == NULL)
2707 return (EINVAL);
2708 if (ng_btsocket_l2cap_node == NULL)
2709 return (EINVAL);
2710
2711 bcopy(&pcb->src, &sa.l2cap_bdaddr, sizeof(sa.l2cap_bdaddr));
2712 sa.l2cap_psm = htole16(pcb->psm);
2713 sa.l2cap_len = sizeof(sa);
2714 sa.l2cap_family = AF_BLUETOOTH;
2715 sa.l2cap_cid = 0;
2716 sa.l2cap_bdaddr_type = pcb->srctype;
2717
2718 *nam = sodupsockaddr((struct sockaddr *) &sa, M_NOWAIT);
2719
2720 return ((*nam == NULL)? ENOMEM : 0);
2721 } /* ng_btsocket_l2cap_sockaddr */
2722
2723 /*****************************************************************************
2724 *****************************************************************************
2725 ** Misc. functions
2726 *****************************************************************************
2727 *****************************************************************************/
2728
2729 /*
2730 * Look for the socket that listens on given PSM and bdaddr. Returns exact or
2731 * close match (if any). Caller must hold ng_btsocket_l2cap_sockets_mtx.
2732 */
2733
2734 static ng_btsocket_l2cap_pcb_p
ng_btsocket_l2cap_pcb_by_addr(bdaddr_p bdaddr,int psm)2735 ng_btsocket_l2cap_pcb_by_addr(bdaddr_p bdaddr, int psm)
2736 {
2737 ng_btsocket_l2cap_pcb_p p = NULL, p1 = NULL;
2738
2739 mtx_assert(&ng_btsocket_l2cap_sockets_mtx, MA_OWNED);
2740
2741 LIST_FOREACH(p, &ng_btsocket_l2cap_sockets, next) {
2742 if (p->so == NULL || !(p->so->so_options & SO_ACCEPTCONN) ||
2743 p->psm != psm)
2744 continue;
2745
2746 if (bcmp(&p->src, bdaddr, sizeof(p->src)) == 0)
2747 break;
2748
2749 if (bcmp(&p->src, NG_HCI_BDADDR_ANY, sizeof(p->src)) == 0)
2750 p1 = p;
2751 }
2752
2753 return ((p != NULL)? p : p1);
2754 } /* ng_btsocket_l2cap_pcb_by_addr */
2755
2756 /*
2757 * Look for the socket that has given token.
2758 * Caller must hold ng_btsocket_l2cap_sockets_mtx.
2759 */
2760
2761 static ng_btsocket_l2cap_pcb_p
ng_btsocket_l2cap_pcb_by_token(u_int32_t token)2762 ng_btsocket_l2cap_pcb_by_token(u_int32_t token)
2763 {
2764 ng_btsocket_l2cap_pcb_p p = NULL;
2765
2766 if (token == 0)
2767 return (NULL);
2768
2769 mtx_assert(&ng_btsocket_l2cap_sockets_mtx, MA_OWNED);
2770
2771 LIST_FOREACH(p, &ng_btsocket_l2cap_sockets, next)
2772 if (p->token == token)
2773 break;
2774
2775 return (p);
2776 } /* ng_btsocket_l2cap_pcb_by_token */
2777
2778 /*
2779 * Look for the socket that assigned to given source address and channel ID.
2780 * Caller must hold ng_btsocket_l2cap_sockets_mtx
2781 */
2782
2783 static ng_btsocket_l2cap_pcb_p
ng_btsocket_l2cap_pcb_by_cid(bdaddr_p src,int cid,int idtype)2784 ng_btsocket_l2cap_pcb_by_cid(bdaddr_p src, int cid, int idtype)
2785 {
2786 ng_btsocket_l2cap_pcb_p p = NULL;
2787
2788 mtx_assert(&ng_btsocket_l2cap_sockets_mtx, MA_OWNED);
2789
2790 LIST_FOREACH(p, &ng_btsocket_l2cap_sockets, next){
2791 if (p->cid == cid &&
2792 bcmp(src, &p->src, sizeof(p->src)) == 0&&
2793 p->idtype == idtype)
2794 break;
2795
2796 }
2797 return (p);
2798 } /* ng_btsocket_l2cap_pcb_by_cid */
2799
2800 /*
2801 * Set timeout on socket
2802 */
2803
2804 static void
ng_btsocket_l2cap_timeout(ng_btsocket_l2cap_pcb_p pcb)2805 ng_btsocket_l2cap_timeout(ng_btsocket_l2cap_pcb_p pcb)
2806 {
2807 mtx_assert(&pcb->pcb_mtx, MA_OWNED);
2808
2809 if (!(pcb->flags & NG_BTSOCKET_L2CAP_TIMO)) {
2810 pcb->flags |= NG_BTSOCKET_L2CAP_TIMO;
2811 callout_reset(&pcb->timo, bluetooth_l2cap_ertx_timeout(),
2812 ng_btsocket_l2cap_process_timeout, pcb);
2813 } else
2814 KASSERT(0,
2815 ("%s: Duplicated socket timeout?!\n", __func__));
2816 } /* ng_btsocket_l2cap_timeout */
2817
2818 /*
2819 * Unset timeout on socket
2820 */
2821
2822 static void
ng_btsocket_l2cap_untimeout(ng_btsocket_l2cap_pcb_p pcb)2823 ng_btsocket_l2cap_untimeout(ng_btsocket_l2cap_pcb_p pcb)
2824 {
2825 mtx_assert(&pcb->pcb_mtx, MA_OWNED);
2826
2827 if (pcb->flags & NG_BTSOCKET_L2CAP_TIMO) {
2828 callout_stop(&pcb->timo);
2829 pcb->flags &= ~NG_BTSOCKET_L2CAP_TIMO;
2830 } else
2831 KASSERT(0,
2832 ("%s: No socket timeout?!\n", __func__));
2833 } /* ng_btsocket_l2cap_untimeout */
2834
2835 /*
2836 * Process timeout on socket
2837 */
2838
2839 static void
ng_btsocket_l2cap_process_timeout(void * xpcb)2840 ng_btsocket_l2cap_process_timeout(void *xpcb)
2841 {
2842 ng_btsocket_l2cap_pcb_p pcb = (ng_btsocket_l2cap_pcb_p) xpcb;
2843
2844 mtx_assert(&pcb->pcb_mtx, MA_OWNED);
2845
2846 pcb->flags &= ~NG_BTSOCKET_L2CAP_TIMO;
2847 pcb->so->so_error = ETIMEDOUT;
2848
2849 switch (pcb->state) {
2850 case NG_BTSOCKET_L2CAP_CONNECTING:
2851 case NG_BTSOCKET_L2CAP_CONFIGURING:
2852 case NG_BTSOCKET_L2CAP_W4_ENC_CHANGE:
2853 /* Send disconnect request with "zero" token */
2854 if (pcb->cid != 0)
2855 ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);
2856
2857 /* ... and close the socket */
2858 pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
2859 soisdisconnected(pcb->so);
2860 break;
2861
2862 case NG_BTSOCKET_L2CAP_OPEN:
2863 /* Send timeout - drop packet and wakeup sender */
2864 sbdroprecord(&pcb->so->so_snd);
2865 sowwakeup(pcb->so);
2866 break;
2867
2868 case NG_BTSOCKET_L2CAP_DISCONNECTING:
2869 /* Disconnect timeout - disconnect the socket anyway */
2870 pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
2871 soisdisconnected(pcb->so);
2872 break;
2873
2874 default:
2875 NG_BTSOCKET_L2CAP_ERR(
2876 "%s: Invalid socket state=%d\n", __func__, pcb->state);
2877 break;
2878 }
2879 } /* ng_btsocket_l2cap_process_timeout */
2880
2881 /*
2882 * Translate HCI/L2CAP error code into "errno" code
2883 * XXX Note: Some L2CAP and HCI error codes have the same value, but
2884 * different meaning
2885 */
2886
2887 static int
ng_btsocket_l2cap_result2errno(int result)2888 ng_btsocket_l2cap_result2errno(int result)
2889 {
2890 switch (result) {
2891 case 0x00: /* No error */
2892 return (0);
2893
2894 case 0x01: /* Unknown HCI command */
2895 return (ENODEV);
2896
2897 case 0x02: /* No connection */
2898 return (ENOTCONN);
2899
2900 case 0x03: /* Hardware failure */
2901 return (EIO);
2902
2903 case 0x04: /* Page timeout */
2904 return (EHOSTDOWN);
2905
2906 case 0x05: /* Authentication failure */
2907 case 0x06: /* Key missing */
2908 case 0x18: /* Pairing not allowed */
2909 case 0x21: /* Role change not allowed */
2910 case 0x24: /* LMP PSU not allowed */
2911 case 0x25: /* Encryption mode not acceptable */
2912 case 0x26: /* Unit key used */
2913 return (EACCES);
2914
2915 case 0x07: /* Memory full */
2916 return (ENOMEM);
2917
2918 case 0x08: /* Connection timeout */
2919 case 0x10: /* Host timeout */
2920 case 0x22: /* LMP response timeout */
2921 case 0xee: /* HCI timeout */
2922 case 0xeeee: /* L2CAP timeout */
2923 return (ETIMEDOUT);
2924
2925 case 0x09: /* Max number of connections */
2926 case 0x0a: /* Max number of SCO connections to a unit */
2927 return (EMLINK);
2928
2929 case 0x0b: /* ACL connection already exists */
2930 return (EEXIST);
2931
2932 case 0x0c: /* Command disallowed */
2933 return (EBUSY);
2934
2935 case 0x0d: /* Host rejected due to limited resources */
2936 case 0x0e: /* Host rejected due to securiity reasons */
2937 case 0x0f: /* Host rejected due to remote unit is a personal unit */
2938 case 0x1b: /* SCO offset rejected */
2939 case 0x1c: /* SCO interval rejected */
2940 case 0x1d: /* SCO air mode rejected */
2941 return (ECONNREFUSED);
2942
2943 case 0x11: /* Unsupported feature or parameter value */
2944 case 0x19: /* Unknown LMP PDU */
2945 case 0x1a: /* Unsupported remote feature */
2946 case 0x20: /* Unsupported LMP parameter value */
2947 case 0x27: /* QoS is not supported */
2948 case 0x29: /* Paring with unit key not supported */
2949 return (EOPNOTSUPP);
2950
2951 case 0x12: /* Invalid HCI command parameter */
2952 case 0x1e: /* Invalid LMP parameters */
2953 return (EINVAL);
2954
2955 case 0x13: /* Other end terminated connection: User ended connection */
2956 case 0x14: /* Other end terminated connection: Low resources */
2957 case 0x15: /* Other end terminated connection: About to power off */
2958 return (ECONNRESET);
2959
2960 case 0x16: /* Connection terminated by local host */
2961 return (ECONNABORTED);
2962
2963 #if 0 /* XXX not yet */
2964 case 0x17: /* Repeated attempts */
2965 case 0x1f: /* Unspecified error */
2966 case 0x23: /* LMP error transaction collision */
2967 case 0x28: /* Instant passed */
2968 #endif
2969 }
2970
2971 return (ENOSYS);
2972 } /* ng_btsocket_l2cap_result2errno */
2973
2974