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