btio: Get mode option for L2CAP sockets
[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                 case BT_IO_OPT_MODE:
843                         *(va_arg(args, uint8_t *)) = l2o.mode;
844                         break;
845                 default:
846                         g_set_error(err, BT_IO_ERROR, BT_IO_ERROR_INVALID_ARGS,
847                                         "Unknown option %d", opt);
848                         return FALSE;
849                 }
850
851                 opt = va_arg(args, int);
852         }
853
854         return TRUE;
855 }
856
857 static int rfcomm_get_info(int sock, uint16_t *handle, uint8_t *dev_class)
858 {
859         struct rfcomm_conninfo info;
860         socklen_t len;
861
862         len = sizeof(info);
863         if (getsockopt(sock, SOL_RFCOMM, RFCOMM_CONNINFO, &info, &len) < 0)
864                 return -errno;
865
866         if (handle)
867                 *handle = info.hci_handle;
868
869         if (dev_class)
870                 memcpy(dev_class, info.dev_class, 3);
871
872         return 0;
873 }
874
875 static gboolean rfcomm_get(int sock, GError **err, BtIOOption opt1,
876                                                                 va_list args)
877 {
878         BtIOOption opt = opt1;
879         struct sockaddr_rc src, dst;
880         int flags;
881         socklen_t len;
882         uint8_t dev_class[3];
883         uint16_t handle;
884
885         if (!get_peers(sock, (struct sockaddr *) &src,
886                                 (struct sockaddr *) &dst, sizeof(src), err))
887                 return FALSE;
888
889         while (opt != BT_IO_OPT_INVALID) {
890                 switch (opt) {
891                 case BT_IO_OPT_SOURCE:
892                         ba2str(&src.rc_bdaddr, va_arg(args, char *));
893                         break;
894                 case BT_IO_OPT_SOURCE_BDADDR:
895                         bacpy(va_arg(args, bdaddr_t *), &src.rc_bdaddr);
896                         break;
897                 case BT_IO_OPT_DEST:
898                         ba2str(&dst.rc_bdaddr, va_arg(args, char *));
899                         break;
900                 case BT_IO_OPT_DEST_BDADDR:
901                         bacpy(va_arg(args, bdaddr_t *), &dst.rc_bdaddr);
902                         break;
903                 case BT_IO_OPT_DEFER_TIMEOUT:
904                         len = sizeof(int);
905                         if (getsockopt(sock, SOL_BLUETOOTH, BT_DEFER_SETUP,
906                                         va_arg(args, int *), &len) < 0) {
907                                 ERROR_FAILED(err, "getsockopt(DEFER_SETUP)",
908                                                                         errno);
909                                 return FALSE;
910                         }
911                         break;
912                 case BT_IO_OPT_SEC_LEVEL:
913                         if (!get_sec_level(sock, BT_IO_RFCOMM,
914                                                 va_arg(args, int *), err))
915                                 return FALSE;
916                         break;
917                 case BT_IO_OPT_CHANNEL:
918                         *(va_arg(args, uint8_t *)) = src.rc_channel ?
919                                         src.rc_channel : dst.rc_channel;
920                         break;
921                 case BT_IO_OPT_SOURCE_CHANNEL:
922                         *(va_arg(args, uint8_t *)) = src.rc_channel;
923                         break;
924                 case BT_IO_OPT_DEST_CHANNEL:
925                         *(va_arg(args, uint8_t *)) = dst.rc_channel;
926                         break;
927                 case BT_IO_OPT_MASTER:
928                         len = sizeof(flags);
929                         if (getsockopt(sock, SOL_RFCOMM, RFCOMM_LM, &flags,
930                                                                 &len) < 0) {
931                                 ERROR_FAILED(err, "getsockopt(RFCOMM_LM)",
932                                                                         errno);
933                                 return FALSE;
934                         }
935                         *(va_arg(args, gboolean *)) =
936                                 (flags & RFCOMM_LM_MASTER) ? TRUE : FALSE;
937                         break;
938                 case BT_IO_OPT_HANDLE:
939                         if (rfcomm_get_info(sock, &handle, dev_class) < 0) {
940                                 ERROR_FAILED(err, "RFCOMM_CONNINFO", errno);
941                                 return FALSE;
942                         }
943                         *(va_arg(args, uint16_t *)) = handle;
944                         break;
945                 case BT_IO_OPT_CLASS:
946                         if (rfcomm_get_info(sock, &handle, dev_class) < 0) {
947                                 ERROR_FAILED(err, "RFCOMM_CONNINFO", errno);
948                                 return FALSE;
949                         }
950                         memcpy(va_arg(args, uint8_t *), dev_class, 3);
951                         break;
952                 default:
953                         g_set_error(err, BT_IO_ERROR, BT_IO_ERROR_INVALID_ARGS,
954                                         "Unknown option %d", opt);
955                         return FALSE;
956                 }
957
958                 opt = va_arg(args, int);
959         }
960
961         return TRUE;
962 }
963
964 static int sco_get_info(int sock, uint16_t *handle, uint8_t *dev_class)
965 {
966         struct sco_conninfo info;
967         socklen_t len;
968
969         len = sizeof(info);
970         if (getsockopt(sock, SOL_SCO, SCO_CONNINFO, &info, &len) < 0)
971                 return -errno;
972
973         if (handle)
974                 *handle = info.hci_handle;
975
976         if (dev_class)
977                 memcpy(dev_class, info.dev_class, 3);
978
979         return 0;
980 }
981
982 static gboolean sco_get(int sock, GError **err, BtIOOption opt1, va_list args)
983 {
984         BtIOOption opt = opt1;
985         struct sockaddr_sco src, dst;
986         struct sco_options sco_opt;
987         socklen_t len;
988         uint8_t dev_class[3];
989         uint16_t handle;
990
991         len = sizeof(sco_opt);
992         memset(&sco_opt, 0, len);
993         if (getsockopt(sock, SOL_SCO, SCO_OPTIONS, &sco_opt, &len) < 0) {
994                 ERROR_FAILED(err, "getsockopt(SCO_OPTIONS)", errno);
995                 return FALSE;
996         }
997
998         if (!get_peers(sock, (struct sockaddr *) &src,
999                                 (struct sockaddr *) &dst, sizeof(src), err))
1000                 return FALSE;
1001
1002         while (opt != BT_IO_OPT_INVALID) {
1003                 switch (opt) {
1004                 case BT_IO_OPT_SOURCE:
1005                         ba2str(&src.sco_bdaddr, va_arg(args, char *));
1006                         break;
1007                 case BT_IO_OPT_SOURCE_BDADDR:
1008                         bacpy(va_arg(args, bdaddr_t *), &src.sco_bdaddr);
1009                         break;
1010                 case BT_IO_OPT_DEST:
1011                         ba2str(&dst.sco_bdaddr, va_arg(args, char *));
1012                         break;
1013                 case BT_IO_OPT_DEST_BDADDR:
1014                         bacpy(va_arg(args, bdaddr_t *), &dst.sco_bdaddr);
1015                         break;
1016                 case BT_IO_OPT_MTU:
1017                 case BT_IO_OPT_IMTU:
1018                 case BT_IO_OPT_OMTU:
1019                         *(va_arg(args, uint16_t *)) = sco_opt.mtu;
1020                         break;
1021                 case BT_IO_OPT_HANDLE:
1022                         if (sco_get_info(sock, &handle, dev_class) < 0) {
1023                                 ERROR_FAILED(err, "RFCOMM_CONNINFO", errno);
1024                                 return FALSE;
1025                         }
1026                         *(va_arg(args, uint16_t *)) = handle;
1027                         break;
1028                 case BT_IO_OPT_CLASS:
1029                         if (sco_get_info(sock, &handle, dev_class) < 0) {
1030                                 ERROR_FAILED(err, "RFCOMM_CONNINFO", errno);
1031                                 return FALSE;
1032                         }
1033                         memcpy(va_arg(args, uint8_t *), dev_class, 3);
1034                         break;
1035                 default:
1036                         g_set_error(err, BT_IO_ERROR, BT_IO_ERROR_INVALID_ARGS,
1037                                         "Unknown option %d", opt);
1038                         return FALSE;
1039                 }
1040
1041                 opt = va_arg(args, int);
1042         }
1043
1044         return TRUE;
1045 }
1046
1047 static gboolean get_valist(GIOChannel *io, BtIOType type, GError **err,
1048                                                 BtIOOption opt1, va_list args)
1049 {
1050         int sock;
1051
1052         sock = g_io_channel_unix_get_fd(io);
1053
1054         switch (type) {
1055         case BT_IO_L2RAW:
1056         case BT_IO_L2CAP:
1057                 return l2cap_get(sock, err, opt1, args);
1058         case BT_IO_RFCOMM:
1059                 return rfcomm_get(sock, err, opt1, args);
1060         case BT_IO_SCO:
1061                 return sco_get(sock, err, opt1, args);
1062         }
1063
1064         g_set_error(err, BT_IO_ERROR, BT_IO_ERROR_INVALID_ARGS,
1065                         "Unknown BtIO type %d", type);
1066         return FALSE;
1067 }
1068
1069 gboolean bt_io_accept(GIOChannel *io, BtIOConnect connect, gpointer user_data,
1070                                         GDestroyNotify destroy, GError **err)
1071 {
1072         int sock;
1073         char c;
1074         struct pollfd pfd;
1075
1076         sock = g_io_channel_unix_get_fd(io);
1077
1078         memset(&pfd, 0, sizeof(pfd));
1079         pfd.fd = sock;
1080         pfd.events = POLLOUT;
1081
1082         if (poll(&pfd, 1, 0) < 0) {
1083                 ERROR_FAILED(err, "poll", errno);
1084                 return FALSE;
1085         }
1086
1087         if (!(pfd.revents & POLLOUT)) {
1088                 int ret;
1089                 ret = read(sock, &c, 1);
1090         }
1091
1092         accept_add(io, connect, user_data, destroy);
1093
1094         return TRUE;
1095 }
1096
1097 gboolean bt_io_set(GIOChannel *io, BtIOType type, GError **err,
1098                                                         BtIOOption opt1, ...)
1099 {
1100         va_list args;
1101         gboolean ret;
1102         struct set_opts opts;
1103         int sock;
1104
1105         va_start(args, opt1);
1106         ret = parse_set_opts(&opts, err, opt1, args);
1107         va_end(args);
1108
1109         if (!ret)
1110                 return ret;
1111
1112         sock = g_io_channel_unix_get_fd(io);
1113
1114         switch (type) {
1115         case BT_IO_L2RAW:
1116         case BT_IO_L2CAP:
1117                 return l2cap_set(sock, opts.sec_level, opts.imtu, opts.omtu,
1118                                                 opts.mode, opts.master, err);
1119         case BT_IO_RFCOMM:
1120                 return rfcomm_set(sock, opts.sec_level, opts.master, err);
1121         case BT_IO_SCO:
1122                 return sco_set(sock, opts.mtu, err);
1123         }
1124
1125         g_set_error(err, BT_IO_ERROR, BT_IO_ERROR_INVALID_ARGS,
1126                         "Unknown BtIO type %d", type);
1127         return FALSE;
1128 }
1129
1130 gboolean bt_io_get(GIOChannel *io, BtIOType type, GError **err,
1131                                                         BtIOOption opt1, ...)
1132 {
1133         va_list args;
1134         gboolean ret;
1135
1136         va_start(args, opt1);
1137         ret = get_valist(io, type, err, opt1, args);
1138         va_end(args);
1139
1140         return ret;
1141 }
1142
1143 static GIOChannel *create_io(BtIOType type, gboolean server,
1144                                         struct set_opts *opts, GError **err)
1145 {
1146         int sock;
1147         GIOChannel *io;
1148
1149         switch (type) {
1150         case BT_IO_L2RAW:
1151                 sock = socket(PF_BLUETOOTH, SOCK_RAW, BTPROTO_L2CAP);
1152                 if (sock < 0) {
1153                         ERROR_FAILED(err, "socket(RAW, L2CAP)", errno);
1154                         return NULL;
1155                 }
1156                 if (l2cap_bind(sock, &opts->src, server ? opts->psm : 0,
1157                                                         opts->cid, err) < 0)
1158                         goto failed;
1159                 if (!l2cap_set(sock, opts->sec_level, 0, 0, 0, -1, err))
1160                         goto failed;
1161                 break;
1162         case BT_IO_L2CAP:
1163                 sock = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
1164                 if (sock < 0) {
1165                         ERROR_FAILED(err, "socket(SEQPACKET, L2CAP)", errno);
1166                         return NULL;
1167                 }
1168                 if (l2cap_bind(sock, &opts->src, server ? opts->psm : 0,
1169                                                         opts->cid, err) < 0)
1170                         goto failed;
1171                 if (!l2cap_set(sock, opts->sec_level, opts->imtu, opts->omtu,
1172                                                 opts->mode, opts->master, err))
1173                         goto failed;
1174                 break;
1175         case BT_IO_RFCOMM:
1176                 sock = socket(PF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
1177                 if (sock < 0) {
1178                         ERROR_FAILED(err, "socket(STREAM, RFCOMM)", errno);
1179                         return NULL;
1180                 }
1181                 if (rfcomm_bind(sock, &opts->src,
1182                                         server ? opts->channel : 0, err) < 0)
1183                         goto failed;
1184                 if (!rfcomm_set(sock, opts->sec_level, opts->master, err))
1185                         goto failed;
1186                 break;
1187         case BT_IO_SCO:
1188                 sock = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO);
1189                 if (sock < 0) {
1190                         ERROR_FAILED(err, "socket(SEQPACKET, SCO)", errno);
1191                         return NULL;
1192                 }
1193                 if (sco_bind(sock, &opts->src, err) < 0)
1194                         goto failed;
1195                 if (!sco_set(sock, opts->mtu, err))
1196                         goto failed;
1197                 break;
1198         default:
1199                 g_set_error(err, BT_IO_ERROR, BT_IO_ERROR_INVALID_ARGS,
1200                                 "Unknown BtIO type %d", type);
1201                 return NULL;
1202         }
1203
1204         io = g_io_channel_unix_new(sock);
1205
1206         g_io_channel_set_close_on_unref(io, TRUE);
1207         g_io_channel_set_flags(io, G_IO_FLAG_NONBLOCK, NULL);
1208
1209         return io;
1210
1211 failed:
1212         close(sock);
1213
1214         return NULL;
1215 }
1216
1217 GIOChannel *bt_io_connect(BtIOType type, BtIOConnect connect,
1218                                 gpointer user_data, GDestroyNotify destroy,
1219                                 GError **gerr, BtIOOption opt1, ...)
1220 {
1221         GIOChannel *io;
1222         va_list args;
1223         struct set_opts opts;
1224         int err, sock;
1225         gboolean ret;
1226
1227         va_start(args, opt1);
1228         ret = parse_set_opts(&opts, gerr, opt1, args);
1229         va_end(args);
1230
1231         if (ret == FALSE)
1232                 return NULL;
1233
1234         io = create_io(type, FALSE, &opts, gerr);
1235         if (io == NULL)
1236                 return NULL;
1237
1238         sock = g_io_channel_unix_get_fd(io);
1239
1240         switch (type) {
1241         case BT_IO_L2RAW:
1242                 err = l2cap_connect(sock, &opts.dst, 0, opts.cid);
1243                 break;
1244         case BT_IO_L2CAP:
1245                 err = l2cap_connect(sock, &opts.dst, opts.psm, opts.cid);
1246                 break;
1247         case BT_IO_RFCOMM:
1248                 err = rfcomm_connect(sock, &opts.dst, opts.channel);
1249                 break;
1250         case BT_IO_SCO:
1251                 err = sco_connect(sock, &opts.dst);
1252                 break;
1253         default:
1254                 g_set_error(gerr, BT_IO_ERROR, BT_IO_ERROR_INVALID_ARGS,
1255                                                 "Unknown BtIO type %d", type);
1256                 return NULL;
1257         }
1258
1259         if (err < 0) {
1260                 g_set_error(gerr, BT_IO_ERROR, BT_IO_ERROR_CONNECT_FAILED,
1261                                 "connect: %s (%d)", strerror(-err), -err);
1262                 g_io_channel_unref(io);
1263                 return NULL;
1264         }
1265
1266         connect_add(io, connect, user_data, destroy);
1267
1268         return io;
1269 }
1270
1271 GIOChannel *bt_io_listen(BtIOType type, BtIOConnect connect,
1272                                 BtIOConfirm confirm, gpointer user_data,
1273                                 GDestroyNotify destroy, GError **err,
1274                                 BtIOOption opt1, ...)
1275 {
1276         GIOChannel *io;
1277         va_list args;
1278         struct set_opts opts;
1279         int sock;
1280         gboolean ret;
1281
1282         if (type == BT_IO_L2RAW) {
1283                 g_set_error(err, BT_IO_ERROR, BT_IO_ERROR_INVALID_ARGS,
1284                                 "Server L2CAP RAW sockets not supported");
1285                 return NULL;
1286         }
1287
1288         va_start(args, opt1);
1289         ret = parse_set_opts(&opts, err, opt1, args);
1290         va_end(args);
1291
1292         if (ret == FALSE)
1293                 return NULL;
1294
1295         io = create_io(type, TRUE, &opts, err);
1296         if (io == NULL)
1297                 return NULL;
1298
1299         sock = g_io_channel_unix_get_fd(io);
1300
1301         if (confirm)
1302                 setsockopt(sock, SOL_BLUETOOTH, BT_DEFER_SETUP, &opts.defer,
1303                                                         sizeof(opts.defer));
1304
1305         if (listen(sock, 5) < 0) {
1306                 ERROR_FAILED(err, "listen", errno);
1307                 g_io_channel_unref(io);
1308                 return NULL;
1309         }
1310
1311         server_add(io, connect, confirm, user_data, destroy);
1312
1313         return io;
1314 }
1315
1316 GQuark bt_io_error_quark(void)
1317 {
1318         return g_quark_from_static_string("bt-io-error-quark");
1319 }