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