btio: Add L2CAP fixed channels support
[platform/upstream/ofono.git] / btio / btio.c
1 /*
2  *
3  *  BlueZ - Bluetooth protocol stack for Linux
4  *
5  *  Copyright (C) 2009-2010  Marcel Holtmann <marcel at holtmann.org>
6  *  Copyright (C) 2009-2010  Nokia Corporation
7  *
8  *
9  *  This program is free software; you can redistribute it and/or modify
10  *  it under the terms of the GNU General Public License as published by
11  *  the Free Software Foundation; either version 2 of the License, or
12  *  (at your option) any later version.
13  *
14  *  This program is distributed in the hope that it will be useful,
15  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
16  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
17  *  GNU General Public License for more details.
18  *
19  *  You should have received a copy of the GNU General Public License
20  *  along with this program; if not, write to the Free Software
21  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
22  *
23  */
24 #include <stdarg.h>
25 #include <stdlib.h>
26 #include <unistd.h>
27 #include <errno.h>
28 #include <poll.h>
29 #include <sys/types.h>
30 #include <sys/socket.h>
31
32 #include <bluetooth/bluetooth.h>
33 #include <bluetooth/l2cap.h>
34 #include <bluetooth/rfcomm.h>
35 #include <bluetooth/sco.h>
36 #include <bluetooth/hci.h>
37 #include <bluetooth/hci_lib.h>
38
39 #include <glib.h>
40
41 #include "btio.h"
42
43 #define ERROR_FAILED(gerr, str, err) \
44                 g_set_error(gerr, BT_IO_ERROR, BT_IO_ERROR_FAILED, \
45                                 str ": %s (%d)", strerror(err), err)
46
47 #define DEFAULT_DEFER_TIMEOUT 30
48
49 struct set_opts {
50         bdaddr_t src;
51         bdaddr_t dst;
52         int defer;
53         int sec_level;
54         uint8_t channel;
55         uint16_t psm;
56         uint16_t cid;
57         uint16_t mtu;
58         uint16_t imtu;
59         uint16_t omtu;
60         int master;
61         uint8_t mode;
62 };
63
64 struct connect {
65         BtIOConnect connect;
66         gpointer user_data;
67         GDestroyNotify destroy;
68 };
69
70 struct accept {
71         BtIOConnect connect;
72         gpointer user_data;
73         GDestroyNotify destroy;
74 };
75
76 struct server {
77         BtIOConnect connect;
78         BtIOConfirm confirm;
79         gpointer user_data;
80         GDestroyNotify destroy;
81 };
82
83 static void server_remove(struct server *server)
84 {
85         if (server->destroy)
86                 server->destroy(server->user_data);
87         g_free(server);
88 }
89
90 static void connect_remove(struct connect *conn)
91 {
92         if (conn->destroy)
93                 conn->destroy(conn->user_data);
94         g_free(conn);
95 }
96
97 static void accept_remove(struct accept *accept)
98 {
99         if (accept->destroy)
100                 accept->destroy(accept->user_data);
101         g_free(accept);
102 }
103
104 static gboolean check_nval(GIOChannel *io)
105 {
106         struct pollfd fds;
107
108         memset(&fds, 0, sizeof(fds));
109         fds.fd = g_io_channel_unix_get_fd(io);
110         fds.events = POLLNVAL;
111
112         if (poll(&fds, 1, 0) > 0 && (fds.revents & POLLNVAL))
113                 return TRUE;
114
115         return FALSE;
116 }
117
118 static gboolean accept_cb(GIOChannel *io, GIOCondition cond,
119                                                         gpointer user_data)
120 {
121         struct accept *accept = user_data;
122         GError *err = NULL;
123
124         /* If the user aborted this accept attempt */
125         if ((cond & G_IO_NVAL) || check_nval(io))
126                 return FALSE;
127
128         if (cond & (G_IO_HUP | G_IO_ERR))
129                 g_set_error(&err, BT_IO_ERROR, BT_IO_ERROR_DISCONNECTED,
130                                 "HUP or ERR on socket");
131
132         accept->connect(io, err, accept->user_data);
133
134         g_clear_error(&err);
135
136         return FALSE;
137 }
138
139 static gboolean connect_cb(GIOChannel *io, GIOCondition cond,
140                                                         gpointer user_data)
141 {
142         struct connect *conn = user_data;
143         GError *gerr = NULL;
144
145         /* If the user aborted this connect attempt */
146         if ((cond & G_IO_NVAL) || check_nval(io))
147                 return FALSE;
148
149         if (cond & G_IO_OUT) {
150                 int err = 0, sock = g_io_channel_unix_get_fd(io);
151                 socklen_t len = sizeof(err);
152
153                 if (getsockopt(sock, SOL_SOCKET, SO_ERROR, &err, &len) < 0)
154                         err = errno;
155
156                 if (err)
157                         g_set_error(&gerr, BT_IO_ERROR,
158                                         BT_IO_ERROR_CONNECT_FAILED, "%s (%d)",
159                                         strerror(err), err);
160         } else if (cond & (G_IO_HUP | G_IO_ERR))
161                 g_set_error(&gerr, BT_IO_ERROR, BT_IO_ERROR_CONNECT_FAILED,
162                                 "HUP or ERR on socket");
163
164         conn->connect(io, gerr, conn->user_data);
165
166         if (gerr)
167                 g_error_free(gerr);
168
169         return FALSE;
170 }
171
172 static gboolean server_cb(GIOChannel *io, GIOCondition cond,
173                                                         gpointer user_data)
174 {
175         struct server *server = user_data;
176         int srv_sock, cli_sock;
177         GIOChannel *cli_io;
178
179         /* If the user closed the server */
180         if ((cond & G_IO_NVAL) || check_nval(io))
181                 return FALSE;
182
183         srv_sock = g_io_channel_unix_get_fd(io);
184
185         cli_sock = accept(srv_sock, NULL, NULL);
186         if (cli_sock < 0)
187                 return TRUE;
188
189         cli_io = g_io_channel_unix_new(cli_sock);
190
191         g_io_channel_set_close_on_unref(cli_io, TRUE);
192         g_io_channel_set_flags(cli_io, G_IO_FLAG_NONBLOCK, NULL);
193
194         if (server->confirm)
195                 server->confirm(cli_io, server->user_data);
196         else
197                 server->connect(cli_io, NULL, server->user_data);
198
199         g_io_channel_unref(cli_io);
200
201         return TRUE;
202 }
203
204 static void server_add(GIOChannel *io, BtIOConnect connect,
205                                 BtIOConfirm confirm, gpointer user_data,
206                                 GDestroyNotify destroy)
207 {
208         struct server *server;
209         GIOCondition cond;
210
211         server = g_new0(struct server, 1);
212         server->connect = connect;
213         server->confirm = confirm;
214         server->user_data = user_data;
215         server->destroy = destroy;
216
217         cond = G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL;
218         g_io_add_watch_full(io, G_PRIORITY_DEFAULT, cond, server_cb, server,
219                                         (GDestroyNotify) server_remove);
220 }
221
222 static void connect_add(GIOChannel *io, BtIOConnect connect,
223                                 gpointer user_data, GDestroyNotify destroy)
224 {
225         struct connect *conn;
226         GIOCondition cond;
227
228         conn = g_new0(struct connect, 1);
229         conn->connect = connect;
230         conn->user_data = user_data;
231         conn->destroy = destroy;
232
233         cond = G_IO_OUT | G_IO_ERR | G_IO_HUP | G_IO_NVAL;
234         g_io_add_watch_full(io, G_PRIORITY_DEFAULT, cond, connect_cb, conn,
235                                         (GDestroyNotify) connect_remove);
236 }
237
238 static void accept_add(GIOChannel *io, BtIOConnect connect, gpointer user_data,
239                                                         GDestroyNotify destroy)
240 {
241         struct accept *accept;
242         GIOCondition cond;
243
244         accept = g_new0(struct accept, 1);
245         accept->connect = connect;
246         accept->user_data = user_data;
247         accept->destroy = destroy;
248
249         cond = G_IO_OUT | G_IO_ERR | G_IO_HUP | G_IO_NVAL;
250         g_io_add_watch_full(io, G_PRIORITY_DEFAULT, cond, accept_cb, accept,
251                                         (GDestroyNotify) accept_remove);
252 }
253
254 static int l2cap_bind(int sock, const bdaddr_t *src, uint16_t psm,
255                                                 uint16_t cid, GError **err)
256 {
257         struct sockaddr_l2 addr;
258
259         memset(&addr, 0, sizeof(addr));
260         addr.l2_family = AF_BLUETOOTH;
261         bacpy(&addr.l2_bdaddr, src);
262
263         if (cid)
264                 addr.l2_cid = htobs(cid);
265         else
266                 addr.l2_psm = htobs(psm);
267
268         if (bind(sock, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
269                 ERROR_FAILED(err, "l2cap_bind", errno);
270                 return -1;
271         }
272
273         return 0;
274 }
275
276 static int l2cap_connect(int sock, const bdaddr_t *dst,
277                                         uint16_t psm, uint16_t cid)
278 {
279         int err;
280         struct sockaddr_l2 addr;
281
282         memset(&addr, 0, sizeof(addr));
283         addr.l2_family = AF_BLUETOOTH;
284         bacpy(&addr.l2_bdaddr, dst);
285         if (cid)
286                 addr.l2_cid = htobs(cid);
287         else
288                 addr.l2_psm = htobs(psm);
289
290         err = connect(sock, (struct sockaddr *) &addr, sizeof(addr));
291         if (err < 0 && !(errno == EAGAIN || errno == EINPROGRESS))
292                 return err;
293
294         return 0;
295 }
296
297 static int l2cap_set_master(int sock, int master)
298 {
299         int flags;
300         socklen_t len;
301
302         len = sizeof(flags);
303         if (getsockopt(sock, SOL_L2CAP, L2CAP_LM, &flags, &len) < 0)
304                 return -errno;
305
306         if (master) {
307                 if (flags & L2CAP_LM_MASTER)
308                         return 0;
309                 flags |= L2CAP_LM_MASTER;
310         } else {
311                 if (!(flags & L2CAP_LM_MASTER))
312                         return 0;
313                 flags &= ~L2CAP_LM_MASTER;
314         }
315
316         if (setsockopt(sock, SOL_L2CAP, L2CAP_LM, &flags, sizeof(flags)) < 0)
317                 return -errno;
318
319         return 0;
320 }
321
322 static int rfcomm_set_master(int sock, int master)
323 {
324         int flags;
325         socklen_t len;
326
327         len = sizeof(flags);
328         if (getsockopt(sock, SOL_RFCOMM, RFCOMM_LM, &flags, &len) < 0)
329                 return -errno;
330
331         if (master) {
332                 if (flags & RFCOMM_LM_MASTER)
333                         return 0;
334                 flags |= RFCOMM_LM_MASTER;
335         } else {
336                 if (!(flags & RFCOMM_LM_MASTER))
337                         return 0;
338                 flags &= ~RFCOMM_LM_MASTER;
339         }
340
341         if (setsockopt(sock, SOL_RFCOMM, RFCOMM_LM, &flags, sizeof(flags)) < 0)
342                 return -errno;
343
344         return 0;
345 }
346
347 static int l2cap_set_lm(int sock, int level)
348 {
349         int lm_map[] = {
350                 0,
351                 L2CAP_LM_AUTH,
352                 L2CAP_LM_AUTH | L2CAP_LM_ENCRYPT,
353                 L2CAP_LM_AUTH | L2CAP_LM_ENCRYPT | L2CAP_LM_SECURE,
354         }, opt = lm_map[level];
355
356         if (setsockopt(sock, SOL_L2CAP, L2CAP_LM, &opt, sizeof(opt)) < 0)
357                 return -errno;
358
359         return 0;
360 }
361
362 static int rfcomm_set_lm(int sock, int level)
363 {
364         int lm_map[] = {
365                 0,
366                 RFCOMM_LM_AUTH,
367                 RFCOMM_LM_AUTH | RFCOMM_LM_ENCRYPT,
368                 RFCOMM_LM_AUTH | RFCOMM_LM_ENCRYPT | RFCOMM_LM_SECURE,
369         }, opt = lm_map[level];
370
371         if (setsockopt(sock, SOL_RFCOMM, RFCOMM_LM, &opt, sizeof(opt)) < 0)
372                 return -errno;
373
374         return 0;
375 }
376
377 static gboolean set_sec_level(int sock, BtIOType type, int level, GError **err)
378 {
379         struct bt_security sec;
380         int ret;
381
382         if (level < BT_SECURITY_LOW || level > BT_SECURITY_HIGH) {
383                 g_set_error(err, BT_IO_ERROR, BT_IO_ERROR_INVALID_ARGS,
384                                 "Valid security level range is %d-%d",
385                                 BT_SECURITY_LOW, BT_SECURITY_HIGH);
386                 return FALSE;
387         }
388
389         memset(&sec, 0, sizeof(sec));
390         sec.level = level;
391
392         if (setsockopt(sock, SOL_BLUETOOTH, BT_SECURITY, &sec,
393                                                         sizeof(sec)) == 0)
394                 return TRUE;
395
396         if (errno != ENOPROTOOPT) {
397                 ERROR_FAILED(err, "setsockopt(BT_SECURITY)", errno);
398                 return FALSE;
399         }
400
401         if (type == BT_IO_L2CAP)
402                 ret = l2cap_set_lm(sock, level);
403         else
404                 ret = rfcomm_set_lm(sock, level);
405
406         if (ret < 0) {
407                 ERROR_FAILED(err, "setsockopt(LM)", -ret);
408                 return FALSE;
409         }
410
411         return TRUE;
412 }
413
414 static int l2cap_get_lm(int sock, int *sec_level)
415 {
416         int opt;
417         socklen_t len;
418
419         len = sizeof(opt);
420         if (getsockopt(sock, SOL_L2CAP, L2CAP_LM, &opt, &len) < 0)
421                 return -errno;
422
423         *sec_level = 0;
424
425         if (opt & L2CAP_LM_AUTH)
426                 *sec_level = BT_SECURITY_LOW;
427         if (opt & L2CAP_LM_ENCRYPT)
428                 *sec_level = BT_SECURITY_MEDIUM;
429         if (opt & L2CAP_LM_SECURE)
430                 *sec_level = BT_SECURITY_HIGH;
431
432         return 0;
433 }
434
435 static int rfcomm_get_lm(int sock, int *sec_level)
436 {
437         int opt;
438         socklen_t len;
439
440         len = sizeof(opt);
441         if (getsockopt(sock, SOL_RFCOMM, RFCOMM_LM, &opt, &len) < 0)
442                 return -errno;
443
444         *sec_level = 0;
445
446         if (opt & RFCOMM_LM_AUTH)
447                 *sec_level = BT_SECURITY_LOW;
448         if (opt & RFCOMM_LM_ENCRYPT)
449                 *sec_level = BT_SECURITY_MEDIUM;
450         if (opt & RFCOMM_LM_SECURE)
451                 *sec_level = BT_SECURITY_HIGH;
452
453         return 0;
454 }
455
456 static gboolean get_sec_level(int sock, BtIOType type, int *level,
457                                                                 GError **err)
458 {
459         struct bt_security sec;
460         socklen_t len;
461         int ret;
462
463         memset(&sec, 0, sizeof(sec));
464         len = sizeof(sec);
465         if (getsockopt(sock, SOL_BLUETOOTH, BT_SECURITY, &sec, &len) == 0) {
466                 *level = sec.level;
467                 return TRUE;
468         }
469
470         if (errno != ENOPROTOOPT) {
471                 ERROR_FAILED(err, "getsockopt(BT_SECURITY)", errno);
472                 return FALSE;
473         }
474
475         if (type == BT_IO_L2CAP)
476                 ret = l2cap_get_lm(sock, level);
477         else
478                 ret = rfcomm_get_lm(sock, level);
479
480         if (ret < 0) {
481                 ERROR_FAILED(err, "getsockopt(LM)", -ret);
482                 return FALSE;
483         }
484
485         return TRUE;
486 }
487
488 static gboolean l2cap_set(int sock, int sec_level, uint16_t imtu, uint16_t omtu,
489                                         uint8_t mode, int master, GError **err)
490 {
491         if (imtu || omtu || mode) {
492                 struct l2cap_options l2o;
493                 socklen_t len;
494
495                 memset(&l2o, 0, sizeof(l2o));
496                 len = sizeof(l2o);
497                 if (getsockopt(sock, SOL_L2CAP, L2CAP_OPTIONS, &l2o,
498                                                                 &len) < 0) {
499                         ERROR_FAILED(err, "getsockopt(L2CAP_OPTIONS)", errno);
500                         return FALSE;
501                 }
502
503                 if (imtu)
504                         l2o.imtu = imtu;
505                 if (omtu)
506                         l2o.omtu = omtu;
507                 if (mode)
508                         l2o.mode = mode;
509
510                 if (setsockopt(sock, SOL_L2CAP, L2CAP_OPTIONS, &l2o,
511                                                         sizeof(l2o)) < 0) {
512                         ERROR_FAILED(err, "setsockopt(L2CAP_OPTIONS)", errno);
513                         return FALSE;
514                 }
515         }
516
517         if (master >= 0 && l2cap_set_master(sock, master) < 0) {
518                 ERROR_FAILED(err, "l2cap_set_master", errno);
519                 return FALSE;
520         }
521
522         if (sec_level && !set_sec_level(sock, BT_IO_L2CAP, sec_level, err))
523                 return FALSE;
524
525         return TRUE;
526 }
527
528 static int rfcomm_bind(int sock,
529                 const bdaddr_t *src, uint8_t channel, GError **err)
530 {
531         struct sockaddr_rc addr;
532
533         memset(&addr, 0, sizeof(addr));
534         addr.rc_family = AF_BLUETOOTH;
535         bacpy(&addr.rc_bdaddr, src);
536         addr.rc_channel = channel;
537
538         if (bind(sock, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
539                 ERROR_FAILED(err, "rfcomm_bind", errno);
540                 return -1;
541         }
542
543         return 0;
544 }
545
546 static int rfcomm_connect(int sock, const bdaddr_t *dst, uint8_t channel)
547 {
548         int err;
549         struct sockaddr_rc addr;
550
551         memset(&addr, 0, sizeof(addr));
552         addr.rc_family = AF_BLUETOOTH;
553         bacpy(&addr.rc_bdaddr, dst);
554         addr.rc_channel = channel;
555
556         err = connect(sock, (struct sockaddr *) &addr, sizeof(addr));
557         if (err < 0 && !(errno == EAGAIN || errno == EINPROGRESS))
558                 return err;
559
560         return 0;
561 }
562
563 static gboolean rfcomm_set(int sock, int sec_level, int master, GError **err)
564 {
565         if (sec_level && !set_sec_level(sock, BT_IO_RFCOMM, sec_level, err))
566                 return FALSE;
567
568         if (master >= 0 && rfcomm_set_master(sock, master) < 0) {
569                 ERROR_FAILED(err, "rfcomm_set_master", errno);
570                 return FALSE;
571         }
572
573         return TRUE;
574 }
575
576 static int sco_bind(int sock, const bdaddr_t *src, GError **err)
577 {
578         struct sockaddr_sco addr;
579
580         memset(&addr, 0, sizeof(addr));
581         addr.sco_family = AF_BLUETOOTH;
582         bacpy(&addr.sco_bdaddr, src);
583
584         if (bind(sock, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
585                 ERROR_FAILED(err, "sco_bind", errno);
586                 return -1;
587         }
588
589         return 0;
590 }
591
592 static int sco_connect(int sock, const bdaddr_t *dst)
593 {
594         struct sockaddr_sco addr;
595         int err;
596
597         memset(&addr, 0, sizeof(addr));
598         addr.sco_family = AF_BLUETOOTH;
599         bacpy(&addr.sco_bdaddr, dst);
600
601         err = connect(sock, (struct sockaddr *) &addr, sizeof(addr));
602         if (err < 0 && !(errno == EAGAIN || errno == EINPROGRESS))
603                 return err;
604
605         return 0;
606 }
607
608 static gboolean sco_set(int sock, uint16_t mtu, GError **err)
609 {
610         struct sco_options sco_opt;
611         socklen_t len;
612
613         if (!mtu)
614                 return TRUE;
615
616         len = sizeof(sco_opt);
617         memset(&sco_opt, 0, len);
618         if (getsockopt(sock, SOL_SCO, SCO_OPTIONS, &sco_opt, &len) < 0) {
619                 ERROR_FAILED(err, "getsockopt(SCO_OPTIONS)", errno);
620                 return FALSE;
621         }
622
623         sco_opt.mtu = mtu;
624         if (setsockopt(sock, SOL_SCO, SCO_OPTIONS, &sco_opt,
625                                                 sizeof(sco_opt)) < 0) {
626                 ERROR_FAILED(err, "setsockopt(SCO_OPTIONS)", errno);
627                 return FALSE;
628         }
629
630         return TRUE;
631 }
632
633 static gboolean parse_set_opts(struct set_opts *opts, GError **err,
634                                                 BtIOOption opt1, va_list args)
635 {
636         BtIOOption opt = opt1;
637         const char *str;
638
639         memset(opts, 0, sizeof(*opts));
640
641         /* Set defaults */
642         opts->defer = DEFAULT_DEFER_TIMEOUT;
643         opts->master = -1;
644         opts->sec_level = BT_IO_SEC_MEDIUM;
645         opts->mode = L2CAP_MODE_BASIC;
646
647         while (opt != BT_IO_OPT_INVALID) {
648                 switch (opt) {
649                 case BT_IO_OPT_SOURCE:
650                         str = va_arg(args, const char *);
651                         if (strncasecmp(str, "hci", 3) == 0)
652                                 hci_devba(atoi(str + 3), &opts->src);
653                         else
654                                 str2ba(str, &opts->src);
655                         break;
656                 case BT_IO_OPT_SOURCE_BDADDR:
657                         bacpy(&opts->src, va_arg(args, const bdaddr_t *));
658                         break;
659                 case BT_IO_OPT_DEST:
660                         str2ba(va_arg(args, const char *), &opts->dst);
661                         break;
662                 case BT_IO_OPT_DEST_BDADDR:
663                         bacpy(&opts->dst, va_arg(args, const bdaddr_t *));
664                         break;
665                 case BT_IO_OPT_DEFER_TIMEOUT:
666                         opts->defer = va_arg(args, int);
667                         break;
668                 case BT_IO_OPT_SEC_LEVEL:
669                         opts->sec_level = va_arg(args, int);
670                         break;
671                 case BT_IO_OPT_CHANNEL:
672                         opts->channel = va_arg(args, int);
673                         break;
674                 case BT_IO_OPT_PSM:
675                         opts->psm = va_arg(args, int);
676                         break;
677                 case BT_IO_OPT_CID:
678                         opts->cid = va_arg(args, int);
679                         break;
680                 case BT_IO_OPT_MTU:
681                         opts->mtu = va_arg(args, int);
682                         opts->imtu = opts->mtu;
683                         opts->omtu = opts->mtu;
684                         break;
685                 case BT_IO_OPT_OMTU:
686                         opts->omtu = va_arg(args, int);
687                         if (!opts->mtu)
688                                 opts->mtu = opts->omtu;
689                         break;
690                 case BT_IO_OPT_IMTU:
691                         opts->imtu = va_arg(args, int);
692                         if (!opts->mtu)
693                                 opts->mtu = opts->imtu;
694                         break;
695                 case BT_IO_OPT_MASTER:
696                         opts->master = va_arg(args, gboolean);
697                         break;
698                 case BT_IO_OPT_MODE:
699                         opts->mode = va_arg(args, int);
700                         break;
701                 default:
702                         g_set_error(err, BT_IO_ERROR, BT_IO_ERROR_INVALID_ARGS,
703                                         "Unknown option %d", opt);
704                         return FALSE;
705                 }
706
707                 opt = va_arg(args, int);
708         }
709
710         return TRUE;
711 }
712
713 static gboolean get_peers(int sock, struct sockaddr *src, struct sockaddr *dst,
714                                 socklen_t len, GError **err)
715 {
716         socklen_t olen;
717
718         memset(src, 0, len);
719         olen = len;
720         if (getsockname(sock, src, &olen) < 0) {
721                 ERROR_FAILED(err, "getsockname", errno);
722                 return FALSE;
723         }
724
725         memset(dst, 0, len);
726         olen = len;
727         if (getpeername(sock, dst, &olen) < 0) {
728                 ERROR_FAILED(err, "getpeername", errno);
729                 return FALSE;
730         }
731
732         return TRUE;
733 }
734
735 static int l2cap_get_info(int sock, uint16_t *handle, uint8_t *dev_class)
736 {
737         struct l2cap_conninfo info;
738         socklen_t len;
739
740         len = sizeof(info);
741         if (getsockopt(sock, SOL_L2CAP, L2CAP_CONNINFO, &info, &len) < 0)
742                 return -errno;
743
744         if (handle)
745                 *handle = info.hci_handle;
746
747         if (dev_class)
748                 memcpy(dev_class, info.dev_class, 3);
749
750         return 0;
751 }
752
753 static gboolean l2cap_get(int sock, GError **err, BtIOOption opt1,
754                                                                 va_list args)
755 {
756         BtIOOption opt = opt1;
757         struct sockaddr_l2 src, dst;
758         struct l2cap_options l2o;
759         int flags;
760         uint8_t dev_class[3];
761         uint16_t handle;
762         socklen_t len;
763
764         len = sizeof(l2o);
765         memset(&l2o, 0, len);
766         if (getsockopt(sock, SOL_L2CAP, L2CAP_OPTIONS, &l2o, &len) < 0) {
767                 ERROR_FAILED(err, "getsockopt(L2CAP_OPTIONS)", errno);
768                 return FALSE;
769         }
770
771         if (!get_peers(sock, (struct sockaddr *) &src,
772                                 (struct sockaddr *) &dst, sizeof(src), err))
773                 return FALSE;
774
775         while (opt != BT_IO_OPT_INVALID) {
776                 switch (opt) {
777                 case BT_IO_OPT_SOURCE:
778                         ba2str(&src.l2_bdaddr, va_arg(args, char *));
779                         break;
780                 case BT_IO_OPT_SOURCE_BDADDR:
781                         bacpy(va_arg(args, bdaddr_t *), &src.l2_bdaddr);
782                         break;
783                 case BT_IO_OPT_DEST:
784                         ba2str(&dst.l2_bdaddr, va_arg(args, char *));
785                         break;
786                 case BT_IO_OPT_DEST_BDADDR:
787                         bacpy(va_arg(args, bdaddr_t *), &dst.l2_bdaddr);
788                         break;
789                 case BT_IO_OPT_DEFER_TIMEOUT:
790                         len = sizeof(int);
791                         if (getsockopt(sock, SOL_BLUETOOTH, BT_DEFER_SETUP,
792                                         va_arg(args, int *), &len) < 0) {
793                                 ERROR_FAILED(err, "getsockopt(DEFER_SETUP)",
794                                                                         errno);
795                                 return FALSE;
796                         }
797                         break;
798                 case BT_IO_OPT_SEC_LEVEL:
799                         if (!get_sec_level(sock, BT_IO_L2CAP,
800                                                 va_arg(args, int *), err))
801                                 return FALSE;
802                         break;
803                 case BT_IO_OPT_PSM:
804                         *(va_arg(args, uint16_t *)) = src.l2_psm ?
805                                                 src.l2_psm : dst.l2_psm;
806                         break;
807                 case BT_IO_OPT_CID:
808                         *(va_arg(args, uint16_t *)) = src.l2_cid ?
809                                                 src.l2_cid : dst.l2_cid;
810                         break;
811                 case BT_IO_OPT_OMTU:
812                         *(va_arg(args, uint16_t *)) = l2o.omtu;
813                         break;
814                 case BT_IO_OPT_IMTU:
815                         *(va_arg(args, uint16_t *)) = l2o.imtu;
816                         break;
817                 case BT_IO_OPT_MASTER:
818                         len = sizeof(flags);
819                         if (getsockopt(sock, SOL_L2CAP, L2CAP_LM, &flags,
820                                                                 &len) < 0) {
821                                 ERROR_FAILED(err, "getsockopt(L2CAP_LM)",
822                                                                         errno);
823                                 return FALSE;
824                         }
825                         *(va_arg(args, gboolean *)) =
826                                 (flags & L2CAP_LM_MASTER) ? TRUE : FALSE;
827                         break;
828                 case BT_IO_OPT_HANDLE:
829                         if (l2cap_get_info(sock, &handle, dev_class) < 0) {
830                                 ERROR_FAILED(err, "L2CAP_CONNINFO", errno);
831                                 return FALSE;
832                         }
833                         *(va_arg(args, uint16_t *)) = handle;
834                         break;
835                 case BT_IO_OPT_CLASS:
836                         if (l2cap_get_info(sock, &handle, dev_class) < 0) {
837                                 ERROR_FAILED(err, "L2CAP_CONNINFO", errno);
838                                 return FALSE;
839                         }
840                         memcpy(va_arg(args, uint8_t *), dev_class, 3);
841                         break;
842                 default:
843                         g_set_error(err, BT_IO_ERROR, BT_IO_ERROR_INVALID_ARGS,
844                                         "Unknown option %d", opt);
845                         return FALSE;
846                 }
847
848                 opt = va_arg(args, int);
849         }
850
851         return TRUE;
852 }
853
854 static int rfcomm_get_info(int sock, uint16_t *handle, uint8_t *dev_class)
855 {
856         struct rfcomm_conninfo info;
857         socklen_t len;
858
859         len = sizeof(info);
860         if (getsockopt(sock, SOL_RFCOMM, RFCOMM_CONNINFO, &info, &len) < 0)
861                 return -errno;
862
863         if (handle)
864                 *handle = info.hci_handle;
865
866         if (dev_class)
867                 memcpy(dev_class, info.dev_class, 3);
868
869         return 0;
870 }
871
872 static gboolean rfcomm_get(int sock, GError **err, BtIOOption opt1,
873                                                                 va_list args)
874 {
875         BtIOOption opt = opt1;
876         struct sockaddr_rc src, dst;
877         int flags;
878         socklen_t len;
879         uint8_t dev_class[3];
880         uint16_t handle;
881
882         if (!get_peers(sock, (struct sockaddr *) &src,
883                                 (struct sockaddr *) &dst, sizeof(src), err))
884                 return FALSE;
885
886         while (opt != BT_IO_OPT_INVALID) {
887                 switch (opt) {
888                 case BT_IO_OPT_SOURCE:
889                         ba2str(&src.rc_bdaddr, va_arg(args, char *));
890                         break;
891                 case BT_IO_OPT_SOURCE_BDADDR:
892                         bacpy(va_arg(args, bdaddr_t *), &src.rc_bdaddr);
893                         break;
894                 case BT_IO_OPT_DEST:
895                         ba2str(&dst.rc_bdaddr, va_arg(args, char *));
896                         break;
897                 case BT_IO_OPT_DEST_BDADDR:
898                         bacpy(va_arg(args, bdaddr_t *), &dst.rc_bdaddr);
899                         break;
900                 case BT_IO_OPT_DEFER_TIMEOUT:
901                         len = sizeof(int);
902                         if (getsockopt(sock, SOL_BLUETOOTH, BT_DEFER_SETUP,
903                                         va_arg(args, int *), &len) < 0) {
904                                 ERROR_FAILED(err, "getsockopt(DEFER_SETUP)",
905                                                                         errno);
906                                 return FALSE;
907                         }
908                         break;
909                 case BT_IO_OPT_SEC_LEVEL:
910                         if (!get_sec_level(sock, BT_IO_RFCOMM,
911                                                 va_arg(args, int *), err))
912                                 return FALSE;
913                         break;
914                 case BT_IO_OPT_CHANNEL:
915                         *(va_arg(args, uint8_t *)) = src.rc_channel ?
916                                         src.rc_channel : dst.rc_channel;
917                         break;
918                 case BT_IO_OPT_SOURCE_CHANNEL:
919                         *(va_arg(args, uint8_t *)) = src.rc_channel;
920                         break;
921                 case BT_IO_OPT_DEST_CHANNEL:
922                         *(va_arg(args, uint8_t *)) = dst.rc_channel;
923                         break;
924                 case BT_IO_OPT_MASTER:
925                         len = sizeof(flags);
926                         if (getsockopt(sock, SOL_RFCOMM, RFCOMM_LM, &flags,
927                                                                 &len) < 0) {
928                                 ERROR_FAILED(err, "getsockopt(RFCOMM_LM)",
929                                                                         errno);
930                                 return FALSE;
931                         }
932                         *(va_arg(args, gboolean *)) =
933                                 (flags & RFCOMM_LM_MASTER) ? TRUE : FALSE;
934                         break;
935                 case BT_IO_OPT_HANDLE:
936                         if (rfcomm_get_info(sock, &handle, dev_class) < 0) {
937                                 ERROR_FAILED(err, "RFCOMM_CONNINFO", errno);
938                                 return FALSE;
939                         }
940                         *(va_arg(args, uint16_t *)) = handle;
941                         break;
942                 case BT_IO_OPT_CLASS:
943                         if (rfcomm_get_info(sock, &handle, dev_class) < 0) {
944                                 ERROR_FAILED(err, "RFCOMM_CONNINFO", errno);
945                                 return FALSE;
946                         }
947                         memcpy(va_arg(args, uint8_t *), dev_class, 3);
948                         break;
949                 default:
950                         g_set_error(err, BT_IO_ERROR, BT_IO_ERROR_INVALID_ARGS,
951                                         "Unknown option %d", opt);
952                         return FALSE;
953                 }
954
955                 opt = va_arg(args, int);
956         }
957
958         return TRUE;
959 }
960
961 static int sco_get_info(int sock, uint16_t *handle, uint8_t *dev_class)
962 {
963         struct sco_conninfo info;
964         socklen_t len;
965
966         len = sizeof(info);
967         if (getsockopt(sock, SOL_SCO, SCO_CONNINFO, &info, &len) < 0)
968                 return -errno;
969
970         if (handle)
971                 *handle = info.hci_handle;
972
973         if (dev_class)
974                 memcpy(dev_class, info.dev_class, 3);
975
976         return 0;
977 }
978
979 static gboolean sco_get(int sock, GError **err, BtIOOption opt1, va_list args)
980 {
981         BtIOOption opt = opt1;
982         struct sockaddr_sco src, dst;
983         struct sco_options sco_opt;
984         socklen_t len;
985         uint8_t dev_class[3];
986         uint16_t handle;
987
988         len = sizeof(sco_opt);
989         memset(&sco_opt, 0, len);
990         if (getsockopt(sock, SOL_SCO, SCO_OPTIONS, &sco_opt, &len) < 0) {
991                 ERROR_FAILED(err, "getsockopt(SCO_OPTIONS)", errno);
992                 return FALSE;
993         }
994
995         if (!get_peers(sock, (struct sockaddr *) &src,
996                                 (struct sockaddr *) &dst, sizeof(src), err))
997                 return FALSE;
998
999         while (opt != BT_IO_OPT_INVALID) {
1000                 switch (opt) {
1001                 case BT_IO_OPT_SOURCE:
1002                         ba2str(&src.sco_bdaddr, va_arg(args, char *));
1003                         break;
1004                 case BT_IO_OPT_SOURCE_BDADDR:
1005                         bacpy(va_arg(args, bdaddr_t *), &src.sco_bdaddr);
1006                         break;
1007                 case BT_IO_OPT_DEST:
1008                         ba2str(&dst.sco_bdaddr, va_arg(args, char *));
1009                         break;
1010                 case BT_IO_OPT_DEST_BDADDR:
1011                         bacpy(va_arg(args, bdaddr_t *), &dst.sco_bdaddr);
1012                         break;
1013                 case BT_IO_OPT_MTU:
1014                 case BT_IO_OPT_IMTU:
1015                 case BT_IO_OPT_OMTU:
1016                         *(va_arg(args, uint16_t *)) = sco_opt.mtu;
1017                         break;
1018                 case BT_IO_OPT_HANDLE:
1019                         if (sco_get_info(sock, &handle, dev_class) < 0) {
1020                                 ERROR_FAILED(err, "RFCOMM_CONNINFO", errno);
1021                                 return FALSE;
1022                         }
1023                         *(va_arg(args, uint16_t *)) = handle;
1024                         break;
1025                 case BT_IO_OPT_CLASS:
1026                         if (sco_get_info(sock, &handle, dev_class) < 0) {
1027                                 ERROR_FAILED(err, "RFCOMM_CONNINFO", errno);
1028                                 return FALSE;
1029                         }
1030                         memcpy(va_arg(args, uint8_t *), dev_class, 3);
1031                         break;
1032                 default:
1033                         g_set_error(err, BT_IO_ERROR, BT_IO_ERROR_INVALID_ARGS,
1034                                         "Unknown option %d", opt);
1035                         return FALSE;
1036                 }
1037
1038                 opt = va_arg(args, int);
1039         }
1040
1041         return TRUE;
1042 }
1043
1044 static gboolean get_valist(GIOChannel *io, BtIOType type, GError **err,
1045                                                 BtIOOption opt1, va_list args)
1046 {
1047         int sock;
1048
1049         sock = g_io_channel_unix_get_fd(io);
1050
1051         switch (type) {
1052         case BT_IO_L2RAW:
1053         case BT_IO_L2CAP:
1054                 return l2cap_get(sock, err, opt1, args);
1055         case BT_IO_RFCOMM:
1056                 return rfcomm_get(sock, err, opt1, args);
1057         case BT_IO_SCO:
1058                 return sco_get(sock, err, opt1, args);
1059         }
1060
1061         g_set_error(err, BT_IO_ERROR, BT_IO_ERROR_INVALID_ARGS,
1062                         "Unknown BtIO type %d", type);
1063         return FALSE;
1064 }
1065
1066 gboolean bt_io_accept(GIOChannel *io, BtIOConnect connect, gpointer user_data,
1067                                         GDestroyNotify destroy, GError **err)
1068 {
1069         int sock;
1070         char c;
1071         struct pollfd pfd;
1072
1073         sock = g_io_channel_unix_get_fd(io);
1074
1075         memset(&pfd, 0, sizeof(pfd));
1076         pfd.fd = sock;
1077         pfd.events = POLLOUT;
1078
1079         if (poll(&pfd, 1, 0) < 0) {
1080                 ERROR_FAILED(err, "poll", errno);
1081                 return FALSE;
1082         }
1083
1084         if (!(pfd.revents & POLLOUT)) {
1085                 int ret;
1086                 ret = read(sock, &c, 1);
1087         }
1088
1089         accept_add(io, connect, user_data, destroy);
1090
1091         return TRUE;
1092 }
1093
1094 gboolean bt_io_set(GIOChannel *io, BtIOType type, GError **err,
1095                                                         BtIOOption opt1, ...)
1096 {
1097         va_list args;
1098         gboolean ret;
1099         struct set_opts opts;
1100         int sock;
1101
1102         va_start(args, opt1);
1103         ret = parse_set_opts(&opts, err, opt1, args);
1104         va_end(args);
1105
1106         if (!ret)
1107                 return ret;
1108
1109         sock = g_io_channel_unix_get_fd(io);
1110
1111         switch (type) {
1112         case BT_IO_L2RAW:
1113         case BT_IO_L2CAP:
1114                 return l2cap_set(sock, opts.sec_level, opts.imtu, opts.omtu,
1115                                                 opts.mode, opts.master, err);
1116         case BT_IO_RFCOMM:
1117                 return rfcomm_set(sock, opts.sec_level, opts.master, err);
1118         case BT_IO_SCO:
1119                 return sco_set(sock, opts.mtu, err);
1120         }
1121
1122         g_set_error(err, BT_IO_ERROR, BT_IO_ERROR_INVALID_ARGS,
1123                         "Unknown BtIO type %d", type);
1124         return FALSE;
1125 }
1126
1127 gboolean bt_io_get(GIOChannel *io, BtIOType type, GError **err,
1128                                                         BtIOOption opt1, ...)
1129 {
1130         va_list args;
1131         gboolean ret;
1132
1133         va_start(args, opt1);
1134         ret = get_valist(io, type, err, opt1, args);
1135         va_end(args);
1136
1137         return ret;
1138 }
1139
1140 static GIOChannel *create_io(BtIOType type, gboolean server,
1141                                         struct set_opts *opts, GError **err)
1142 {
1143         int sock;
1144         GIOChannel *io;
1145
1146         switch (type) {
1147         case BT_IO_L2RAW:
1148                 sock = socket(PF_BLUETOOTH, SOCK_RAW, BTPROTO_L2CAP);
1149                 if (sock < 0) {
1150                         ERROR_FAILED(err, "socket(RAW, L2CAP)", errno);
1151                         return NULL;
1152                 }
1153                 if (l2cap_bind(sock, &opts->src, server ? opts->psm : 0,
1154                                                         opts->cid, err) < 0)
1155                         goto failed;
1156                 if (!l2cap_set(sock, opts->sec_level, 0, 0, 0, -1, err))
1157                         goto failed;
1158                 break;
1159         case BT_IO_L2CAP:
1160                 sock = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
1161                 if (sock < 0) {
1162                         ERROR_FAILED(err, "socket(SEQPACKET, L2CAP)", errno);
1163                         return NULL;
1164                 }
1165                 if (l2cap_bind(sock, &opts->src, server ? opts->psm : 0,
1166                                                         opts->cid, err) < 0)
1167                         goto failed;
1168                 if (!l2cap_set(sock, opts->sec_level, opts->imtu, opts->omtu,
1169                                                 opts->mode, opts->master, err))
1170                         goto failed;
1171                 break;
1172         case BT_IO_RFCOMM:
1173                 sock = socket(PF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
1174                 if (sock < 0) {
1175                         ERROR_FAILED(err, "socket(STREAM, RFCOMM)", errno);
1176                         return NULL;
1177                 }
1178                 if (rfcomm_bind(sock, &opts->src,
1179                                         server ? opts->channel : 0, err) < 0)
1180                         goto failed;
1181                 if (!rfcomm_set(sock, opts->sec_level, opts->master, err))
1182                         goto failed;
1183                 break;
1184         case BT_IO_SCO:
1185                 sock = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO);
1186                 if (sock < 0) {
1187                         ERROR_FAILED(err, "socket(SEQPACKET, SCO)", errno);
1188                         return NULL;
1189                 }
1190                 if (sco_bind(sock, &opts->src, err) < 0)
1191                         goto failed;
1192                 if (!sco_set(sock, opts->mtu, err))
1193                         goto failed;
1194                 break;
1195         default:
1196                 g_set_error(err, BT_IO_ERROR, BT_IO_ERROR_INVALID_ARGS,
1197                                 "Unknown BtIO type %d", type);
1198                 return NULL;
1199         }
1200
1201         io = g_io_channel_unix_new(sock);
1202
1203         g_io_channel_set_close_on_unref(io, TRUE);
1204         g_io_channel_set_flags(io, G_IO_FLAG_NONBLOCK, NULL);
1205
1206         return io;
1207
1208 failed:
1209         close(sock);
1210
1211         return NULL;
1212 }
1213
1214 GIOChannel *bt_io_connect(BtIOType type, BtIOConnect connect,
1215                                 gpointer user_data, GDestroyNotify destroy,
1216                                 GError **gerr, BtIOOption opt1, ...)
1217 {
1218         GIOChannel *io;
1219         va_list args;
1220         struct set_opts opts;
1221         int err, sock;
1222         gboolean ret;
1223
1224         va_start(args, opt1);
1225         ret = parse_set_opts(&opts, gerr, opt1, args);
1226         va_end(args);
1227
1228         if (ret == FALSE)
1229                 return NULL;
1230
1231         io = create_io(type, FALSE, &opts, gerr);
1232         if (io == NULL)
1233                 return NULL;
1234
1235         sock = g_io_channel_unix_get_fd(io);
1236
1237         switch (type) {
1238         case BT_IO_L2RAW:
1239                 err = l2cap_connect(sock, &opts.dst, 0, opts.cid);
1240                 break;
1241         case BT_IO_L2CAP:
1242                 err = l2cap_connect(sock, &opts.dst, opts.psm, opts.cid);
1243                 break;
1244         case BT_IO_RFCOMM:
1245                 err = rfcomm_connect(sock, &opts.dst, opts.channel);
1246                 break;
1247         case BT_IO_SCO:
1248                 err = sco_connect(sock, &opts.dst);
1249                 break;
1250         default:
1251                 g_set_error(gerr, BT_IO_ERROR, BT_IO_ERROR_INVALID_ARGS,
1252                                                 "Unknown BtIO type %d", type);
1253                 return NULL;
1254         }
1255
1256         if (err < 0) {
1257                 g_set_error(gerr, BT_IO_ERROR, BT_IO_ERROR_CONNECT_FAILED,
1258                                 "connect: %s (%d)", strerror(-err), -err);
1259                 g_io_channel_unref(io);
1260                 return NULL;
1261         }
1262
1263         connect_add(io, connect, user_data, destroy);
1264
1265         return io;
1266 }
1267
1268 GIOChannel *bt_io_listen(BtIOType type, BtIOConnect connect,
1269                                 BtIOConfirm confirm, gpointer user_data,
1270                                 GDestroyNotify destroy, GError **err,
1271                                 BtIOOption opt1, ...)
1272 {
1273         GIOChannel *io;
1274         va_list args;
1275         struct set_opts opts;
1276         int sock;
1277         gboolean ret;
1278
1279         if (type == BT_IO_L2RAW) {
1280                 g_set_error(err, BT_IO_ERROR, BT_IO_ERROR_INVALID_ARGS,
1281                                 "Server L2CAP RAW sockets not supported");
1282                 return NULL;
1283         }
1284
1285         va_start(args, opt1);
1286         ret = parse_set_opts(&opts, err, opt1, args);
1287         va_end(args);
1288
1289         if (ret == FALSE)
1290                 return NULL;
1291
1292         io = create_io(type, TRUE, &opts, err);
1293         if (io == NULL)
1294                 return NULL;
1295
1296         sock = g_io_channel_unix_get_fd(io);
1297
1298         if (confirm)
1299                 setsockopt(sock, SOL_BLUETOOTH, BT_DEFER_SETUP, &opts.defer,
1300                                                         sizeof(opts.defer));
1301
1302         if (listen(sock, 5) < 0) {
1303                 ERROR_FAILED(err, "listen", errno);
1304                 g_io_channel_unref(io);
1305                 return NULL;
1306         }
1307
1308         server_add(io, connect, confirm, user_data, destroy);
1309
1310         return io;
1311 }
1312
1313 GQuark bt_io_error_quark(void)
1314 {
1315         return g_quark_from_static_string("bt-io-error-quark");
1316 }