diff options
| -rw-r--r-- | audio/headset.c | 503 |
1 files changed, 503 insertions, 0 deletions
diff --git a/audio/headset.c b/audio/headset.c index c04bb251..307b9212 100644 --- a/audio/headset.c +++ b/audio/headset.c @@ -27,8 +27,511 @@ #include <stdio.h> #include <errno.h> +#include <getopt.h> +#include <stdlib.h> +#include <string.h> +#include <fcntl.h> +#include <unistd.h> +#include <signal.h> +#include <sys/ioctl.h> +#include <sys/socket.h> + +#include <bluetooth/bluetooth.h> +#include <bluetooth/hci.h> +#include <bluetooth/hci_lib.h> +#include <bluetooth/sco.h> +#include <bluetooth/rfcomm.h> +#include <bluetooth/sdp.h> +#include <bluetooth/sdp_lib.h> + +#include <dbus/dbus.h> + +#include "dbus.h" +#include "logging.h" +#include "glib-ectomy.c" + +#define HEADSET_PATH "/org/bluez/headset" + +struct pending_connect { + bdaddr_t bda; + DBusConnection *conn; + DBusMessage *msg; + GIOChannel *io; +}; + +struct hs_connection { + char address[18]; + GIOChannel *rfcomm; + GIOChannel *sco; +}; + +static char *on_init_bda = NULL; +static int on_init_ch = 2; + +static int started = 0; + +static DBusConnection *connection = NULL; + +static GMainLoop *main_loop = NULL; + +static struct hs_connection *connected_hs = NULL; + +static int set_nonblocking(int fd, int *err) +{ + long arg; + + arg = fcntl(fd, F_GETFL); + if (arg < 0) { + if (err) + *err = errno; + error("fcntl(F_GETFL): %s (%d)", strerror(errno), errno); + return -1; + } + + /* Return if already nonblocking */ + if (arg & O_NONBLOCK) + return 0; + + arg |= O_NONBLOCK; + if (fcntl(fd, F_SETFL, arg) < 0) { + if (err) + *err = errno; + error("fcntl(F_SETFL, O_NONBLOCK): %s (%d)", + strerror(errno), errno); + return -1; + } + + return 0; +} + +static void pending_connect_free(struct pending_connect *c, gboolean unref_io) +{ + if (unref_io && c->io) + g_io_channel_unref(c->io); + if (c->msg) + dbus_message_unref(c->msg); + if (c->conn) + dbus_connection_unref(c->conn); + free(c); +} + +static void connect_failed(DBusConnection *conn, DBusMessage *msg, int err) +{ + DBusMessage *derr; + + if (!conn) + return; + + derr = dbus_message_new_error(msg, "org.bluez.Error.ConnectFailed", + strerror(err)); + if (!derr) { + error("Unable to allocate new error return"); + return; + } + + dbus_connection_send(conn, derr, NULL); +} + +static gboolean rfcomm_io_cb(GIOChannel *chan, GIOCondition cond, struct hs_connection *hs) +{ + int sk, ret; + unsigned char buf[1024]; + + debug("rfcomm_io_cb"); + + if (cond & G_IO_NVAL) { + g_io_channel_unref(chan); + return FALSE; + } + + if (cond & (G_IO_ERR | G_IO_HUP)) + goto failed; + + sk = g_io_channel_unix_get_fd(chan); + + ret = read(sk, buf, sizeof(buf) - 1); + if (ret > 0) { + buf[ret] = '\0'; + printf("%s\n", buf); + } + + return TRUE; + +failed: + info("Disconnected from %s", hs->address); + if (hs->sco) + g_io_channel_close(hs->sco); + g_io_channel_close(chan); + free(hs); + connected_hs = NULL; + return FALSE; +} + +static gboolean rfcomm_connect_cb(GIOChannel *chan, GIOCondition cond, struct pending_connect *c) +{ + int sk, ret, err; + socklen_t len; + struct hs_connection *hs; + + if (cond & G_IO_NVAL) { + g_io_channel_unref(chan); + return FALSE; + } + + sk = g_io_channel_unix_get_fd(chan); + + len = sizeof(ret); + if (getsockopt(sk, SOL_SOCKET, SO_ERROR, &ret, &len) < 0) { + err = errno; + error("getsockopt(SO_ERROR): %s (%d)", strerror(err), err); + goto failed; + } + + if (ret != 0) { + err = ret; + error("connect(): %s (%d)", strerror(ret), ret); + goto failed; + } + + hs = malloc(sizeof(struct hs_connection)); + if (!hs) { + err = ENOMEM; + error("Allocating new hs connection struct failed!"); + goto failed; + } + + memset(hs, 0, sizeof(struct hs_connection)); + + ba2str(&c->bda, hs->address); + hs->rfcomm = chan; + + debug("rfcomm_connect_cb: connected to %s", hs->address); + + connected_hs = hs; + + g_io_add_watch(chan, G_IO_ERR | G_IO_HUP | G_IO_IN | G_IO_NVAL, + (GIOFunc) rfcomm_io_cb, hs); + + pending_connect_free(c, FALSE); + + return FALSE; + +failed: + connect_failed(c->conn, c->msg, err); + pending_connect_free(c, TRUE); + + return FALSE; +} + +static int rfcomm_connect(DBusConnection *conn, DBusMessage *msg, bdaddr_t *src, + const char *bda, uint8_t ch, int *err) +{ + struct pending_connect *c = NULL; + struct sockaddr_rc addr; + int sk; + + debug("Connecting to %s channel %d", bda, ch); + + sk = socket(PF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM); + if (sk < 0) { + if (err) + *err = errno; + error("socket: %s (%d)", strerror(errno), errno); + return -1; + } + + c = malloc(sizeof(struct pending_connect)); + if (!c) { + if (err) + *err = ENOMEM; + goto failed; + } + + memset(c, 0, sizeof(struct pending_connect)); + + memset(&addr, 0, sizeof(addr)); + addr.rc_family = AF_BLUETOOTH; + bacpy(&addr.rc_bdaddr, src); + addr.rc_channel = 0; + + if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + if (err) + *err = errno; + error("bind: %s (%d)", strerror(errno), errno); + goto failed; + } + + if (set_nonblocking(sk, err) < 0) + goto failed; + + str2ba(bda, &c->bda); + + memset(&addr, 0, sizeof(addr)); + addr.rc_family = AF_BLUETOOTH; + bacpy(&addr.rc_bdaddr, &c->bda); + addr.rc_channel = ch; + + if (conn && msg) { + c->conn = dbus_connection_ref(conn); + c->msg = dbus_message_ref(msg); + } + + c->io = g_io_channel_unix_new(sk); + g_io_channel_set_close_on_unref(c->io, TRUE); + + if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + if (!(errno == EAGAIN || errno == EINPROGRESS)) { + if (err) + *err = errno; + error("connect() failed: %s (%d)", strerror(errno), errno); + goto failed; + } + + debug("Connect in progress"); + + g_io_add_watch(c->io, G_IO_OUT, (GIOFunc) rfcomm_connect_cb, c); + } else { + debug("Connect succeeded with first try"); + rfcomm_connect_cb(c->io, G_IO_OUT, c); + } + + return 0; + +failed: + if (c) + pending_connect_free(c, TRUE); + if (sk >= 0) + close(sk); + return -1; +} + +static void sig_term(int sig) +{ + g_main_quit(main_loop); +} + +static DBusHandlerResult start_message(DBusConnection *conn, + DBusMessage *msg, void *data) +{ + DBusMessage *reply; + + info("Starting headset service"); + + reply = dbus_message_new_method_return(msg); + if (!reply) { + error("Can't create reply message"); + return DBUS_HANDLER_RESULT_NEED_MEMORY; + } + + dbus_connection_send(conn, reply, NULL); + + dbus_message_unref(reply); + + started = 1; + + return DBUS_HANDLER_RESULT_HANDLED; +} + +static DBusHandlerResult stop_message(DBusConnection *conn, + DBusMessage *msg, void *data) +{ + DBusMessage *reply; + + info("Stopping headset service"); + + reply = dbus_message_new_method_return(msg); + if (!reply) { + error("Can't create reply message"); + return DBUS_HANDLER_RESULT_NEED_MEMORY; + } + + dbus_connection_send(conn, reply, NULL); + + dbus_message_unref(reply); + + started = 0; + + return DBUS_HANDLER_RESULT_HANDLED; +} + +static DBusHandlerResult release_message(DBusConnection *conn, + DBusMessage *msg, void *data) +{ + DBusMessage *reply; + + reply = dbus_message_new_method_return(msg); + if (!reply) { + error("Can't create reply message"); + return DBUS_HANDLER_RESULT_NEED_MEMORY; + } + + dbus_connection_send(conn, reply, NULL); + + dbus_message_unref(reply); + + info("Got Release method. Exiting."); + + raise(SIGTERM); + + return DBUS_HANDLER_RESULT_HANDLED; +} + +static DBusHandlerResult hs_message(DBusConnection *conn, + DBusMessage *msg, void *data) +{ + const char *interface; + const char *member; + + interface = dbus_message_get_interface(msg); + member = dbus_message_get_member(msg); + + if (strcmp(interface, "org.bluez.ServiceAgent") == 0) { + if (strcmp(member, "Start") == 0) + return start_message(conn, msg, data); + if (strcmp(member, "Stop") == 0) + return stop_message(conn, msg, data); + if (strcmp(member, "Release") == 0) + return release_message(conn, msg, data); + return DBUS_HANDLER_RESULT_NOT_YET_HANDLED; + } + + if (strcmp(interface, "org.bluez.Headset") != 0) + return DBUS_HANDLER_RESULT_NOT_YET_HANDLED; + + /* Handle Headset interface methods here */ + + return DBUS_HANDLER_RESULT_NOT_YET_HANDLED; +} + +static const DBusObjectPathVTable hs_table = { + .message_function = hs_message, +}; + +static void register_reply(DBusPendingCall *call, void *data) +{ + DBusMessage *reply = dbus_pending_call_steal_reply(call); + DBusError derr; + int err; + + dbus_error_init(&derr); + if (dbus_set_error_from_message(&derr, reply)) { + error("Registering failed: %s", derr.message); + dbus_error_free(&derr); + raise(SIGTERM); + } + else + debug("Successfully registered"); + + dbus_message_unref(reply); + + if (!on_init_bda) + return; + + if (rfcomm_connect(NULL, NULL, BDADDR_ANY, on_init_bda, on_init_ch, &err) < 0) + exit(1); +} + + +int headset_dbus_init(char *bda) +{ + DBusMessage *msg; + DBusPendingCall *pending; + const char *name = "Headset service"; + const char *description = "A service for headsets"; + const char *hs_path = HEADSET_PATH; + + connection = init_dbus(NULL, NULL, NULL); + if (!connection) + return -1; + + if (!dbus_connection_register_object_path(connection, hs_path, + &hs_table, NULL)) { + error("D-Bus failed to register %s path", hs_path); + return -1; + } + + msg = dbus_message_new_method_call("org.bluez", "/org/bluez", + "org.bluez.Manager", "RegisterService"); + if (!msg) { + error("Can't allocate new method call"); + return -1; + } + + dbus_message_append_args(msg, DBUS_TYPE_STRING, &hs_path, + DBUS_TYPE_STRING, &name, + DBUS_TYPE_STRING, &description, + DBUS_TYPE_INVALID); + + if (!dbus_connection_send_with_reply(connection, msg, &pending, -1)) { + error("Sending Register method call failed"); + dbus_message_unref(msg); + return -1; + } + + dbus_pending_call_set_notify(pending, register_reply, NULL, NULL); + dbus_message_unref(msg); + + return 0; +} + int main(int argc, char *argv[]) { + struct sigaction sa; + int opt, daemonize = 1; + + while ((opt = getopt(argc, argv, "nc:")) != EOF) { + switch (opt) { + case 'n': + daemonize = 0; + break; + + case 'c': + on_init_ch = strtol(optarg, NULL, 0); + if (on_init_ch < 0 || on_init_ch > 255) { + error("Invalid channel"); + exit(1); + } + break; + + default: + printf("Usage: %s [-n] [-c channel] [bdaddr]\n", argv[0]); + exit(1); + } + } + + if (argv[optind]) { + on_init_bda = argv[optind]; + daemonize = 0; + } + + if (daemonize && daemon(0, 0)) { + error("Can't daemonize: %s (%d)", strerror(errno), errno); + exit(1); + } + + start_logging("bt.headsetd", "Bluetooth Headset daemon"); + + memset(&sa, 0, sizeof(sa)); + sa.sa_flags = SA_NOCLDSTOP; + sa.sa_handler = sig_term; + sigaction(SIGTERM, &sa, NULL); + sigaction(SIGINT, &sa, NULL); + + sa.sa_handler = SIG_IGN; + sigaction(SIGCHLD, &sa, NULL); + sigaction(SIGPIPE, &sa, NULL); + + enable_debug(); + + main_loop = g_main_new(FALSE); + + if (headset_dbus_init(NULL) < 0) { + error("Unable to get on D-Bus"); + exit(1); + } + + g_main_run(main_loop); + return 0; } |