From b3f285d83c7dfe446bd7f2460e6f795e22a72d32 Mon Sep 17 00:00:00 2001 From: Johan Hedberg Date: Tue, 19 Feb 2008 08:20:22 +0000 Subject: Implement proper HFP SLC handling --- audio/headset.c | 512 ++++++++++++++++++++++++++++++++------------------------ audio/headset.h | 1 + audio/manager.c | 6 +- 3 files changed, 297 insertions(+), 222 deletions(-) diff --git a/audio/headset.c b/audio/headset.c index c1d7ea24..a42b5131 100644 --- a/audio/headset.c +++ b/audio/headset.c @@ -109,6 +109,7 @@ struct headset { int rfcomm_ch; GIOChannel *rfcomm; + GIOChannel *tmp_rfcomm; GIOChannel *sco; guint sco_id; @@ -210,22 +211,273 @@ static int report_indicators(struct device *device, const char *buf) return headset_send(hs, "\r\nOK\r\n"); } -static int event_reporting(struct device *device, const char *buf) +static void pending_connect_complete(struct connect_cb *cb, struct device *dev) { - struct headset *hs = device->headset; - return headset_send(hs, "\r\nOK\r\n"); + struct headset *hs = dev->headset; + + if (hs->pending->err) + cb->cb(NULL, cb->cb_data); + else + cb->cb(dev, cb->cb_data); } -static int call_hold(struct device *device, const char *buf) +static void pending_connect_finalize(struct device *dev) { - struct headset *hs = device->headset; + struct headset *hs = dev->headset; + struct pending_connect *p = hs->pending; + + g_slist_foreach(p->callbacks, (GFunc) pending_connect_complete, dev); + + g_slist_foreach(p->callbacks, (GFunc) g_free, NULL); + g_slist_free(p->callbacks); + + if (p->io) { + g_io_channel_close(p->io); + g_io_channel_unref(p->io); + } + + if (p->msg) + dbus_message_unref(p->msg); + + if (p->call) { + dbus_pending_call_cancel(p->call); + dbus_pending_call_unref(p->call); + } + + g_free(p); + + hs->pending = NULL; +} + +static void pending_connect_init(struct headset *hs, headset_state_t target_state) +{ + if (hs->pending) { + if (hs->pending->target_state < target_state) + hs->pending->target_state = target_state; + return; + } + + hs->pending = g_new0(struct pending_connect, 1); + hs->pending->target_state = target_state; +} + +static unsigned int connect_cb_new(struct headset *hs, + headset_state_t target_state, + headset_stream_cb_t func, + void *user_data) +{ + struct connect_cb *cb; + unsigned int free_cb_id = 1; + + pending_connect_init(hs, target_state); + + cb = g_new(struct connect_cb, 1); + + cb->cb = func; + cb->cb_data = user_data; + cb->id = free_cb_id++; + + hs->pending->callbacks = g_slist_append(hs->pending->callbacks, + cb); + + return cb->id; +} + +static gboolean sco_connect_cb(GIOChannel *chan, GIOCondition cond, + struct device *device) +{ + struct headset *hs; + int ret, sk; + socklen_t len; + struct pending_connect *p; + + if (cond & G_IO_NVAL) + return FALSE; + + hs = device->headset; + p = hs->pending; + + sk = g_io_channel_unix_get_fd(chan); + + len = sizeof(ret); + if (getsockopt(sk, SOL_SOCKET, SO_ERROR, &ret, &len) < 0) { + p->err = errno; + error("getsockopt(SO_ERROR): %s (%d)", strerror(p->err), + p->err); + goto failed; + } + + if (ret != 0) { + p->err = ret; + error("connect(): %s (%d)", strerror(ret), ret); + goto failed; + } + + debug("SCO socket opened for headset %s", device->path); + + info("SCO fd=%d", sk); + hs->sco = chan; + p->io = NULL; + + pending_connect_finalize(device); + + fcntl(sk, F_SETFL, 0); + + headset_set_state(device, HEADSET_STATE_PLAYING); + + return FALSE; + +failed: + pending_connect_finalize(device); + if (hs->rfcomm) + headset_set_state(device, HEADSET_STATE_CONNECTED); + else + headset_set_state(device, HEADSET_STATE_DISCONNECTED); + + return FALSE; +} + +static int sco_connect(struct device *dev, headset_stream_cb_t cb, + void *user_data, unsigned int *cb_id) +{ + struct headset *hs = dev->headset; + struct sockaddr_sco addr; + GIOChannel *io; + int sk, err; + + if (hs->state != HEADSET_STATE_CONNECTED) + return -EINVAL; + + sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO); + if (sk < 0) { + err = errno; + error("socket(BTPROTO_SCO): %s (%d)", strerror(err), err); + return -err; + } + + io = g_io_channel_unix_new(sk); + if (!io) { + close(sk); + return -ENOMEM; + } + + memset(&addr, 0, sizeof(addr)); + addr.sco_family = AF_BLUETOOTH; + bacpy(&addr.sco_bdaddr, BDADDR_ANY); + + if (bind(sk, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + err = errno; + error("socket(BTPROTO_SCO): %s (%d)", strerror(err), err); + goto failed; + } + + if (set_nonblocking(sk) < 0) { + err = errno; + goto failed; + } + + memset(&addr, 0, sizeof(addr)); + addr.sco_family = AF_BLUETOOTH; + bacpy(&addr.sco_bdaddr, &dev->dst); + + err = connect(sk, (struct sockaddr *) &addr, sizeof(addr)); + + if (err < 0 && !(errno == EAGAIN || errno == EINPROGRESS)) { + err = errno; + error("connect: %s (%d)", strerror(errno), errno); + goto failed; + } + + headset_set_state(dev, HEADSET_STATE_PLAY_IN_PROGRESS); + + pending_connect_init(hs, HEADSET_STATE_PLAYING); + + if (cb) { + unsigned int id = connect_cb_new(hs, HEADSET_STATE_PLAYING, + cb, user_data); + if (cb_id) + *cb_id = id; + } + + g_io_add_watch(io, G_IO_OUT | G_IO_NVAL | G_IO_ERR | G_IO_HUP, + (GIOFunc) sco_connect_cb, dev); + + hs->pending->io = io; + + return 0; + +failed: + g_io_channel_close(io); + g_io_channel_unref(io); + return -err; +} + +static void hfp_slc_complete(struct device *dev) +{ + struct headset *hs = dev->headset; + struct pending_connect *p = hs->pending; + + debug("HFP Service Level Connection established"); + + headset_set_state(dev, HEADSET_STATE_CONNECTED); + + if (p == NULL) + return; + + if (p->msg) { + DBusMessage *reply = dbus_message_new_method_return(p->msg); + send_message_and_unref(dev->conn, reply); + } + + if (p->target_state == HEADSET_STATE_CONNECTED) { + pending_connect_finalize(dev); + return; + } + + p->err = sco_connect(dev, NULL, NULL, NULL); + if (p->err < 0) + pending_connect_finalize(dev); +} + +static int event_reporting(struct device *dev, const char *buf) +{ + struct headset *hs = dev->headset; + int ret; + + ret = headset_send(hs, "\r\nOK\r\n"); + if (ret < 0) + return ret; + + if (hs->state != HEADSET_STATE_CONNECT_IN_PROGRESS) + return 0; + + if (ag_features & AG_FEATURE_THREE_WAY_CALLING) + return 0; + + hfp_slc_complete(dev); + + return 0; +} + +static int call_hold(struct device *dev, const char *buf) +{ + struct headset *hs = dev->headset; int err; err = headset_send(hs, "\r\n+CHLD:(0,1,1x,2,2x,3,4)\r\n"); if (err < 0) return err; - return headset_send(hs, "\r\nOK\r\n"); + err = headset_send(hs, "\r\nOK\r\n"); + if (err < 0) + return err; + + if (hs->state != HEADSET_STATE_CONNECT_IN_PROGRESS) + return 0; + + hfp_slc_complete(dev); + + return 0; } static int answer_call(struct device *device, const char *buf) @@ -475,207 +727,6 @@ static gboolean sco_cb(GIOChannel *chan, GIOCondition cond, return FALSE; } -static void pending_connect_complete(struct connect_cb *cb, struct device *dev) -{ - struct headset *hs = dev->headset; - - if (hs->pending->err) - cb->cb(NULL, cb->cb_data); - else - cb->cb(dev, cb->cb_data); -} - -static void pending_connect_finalize(struct device *dev) -{ - struct headset *hs = dev->headset; - struct pending_connect *p = hs->pending; - - g_slist_foreach(p->callbacks, (GFunc) pending_connect_complete, dev); - - g_slist_foreach(p->callbacks, (GFunc) g_free, NULL); - g_slist_free(p->callbacks); - - if (p->io) { - g_io_channel_close(p->io); - g_io_channel_unref(p->io); - } - - if (p->msg) - dbus_message_unref(p->msg); - - if (p->call) { - dbus_pending_call_cancel(p->call); - dbus_pending_call_unref(p->call); - } - - g_free(p); - - hs->pending = NULL; -} - -static void pending_connect_init(struct headset *hs, headset_state_t target_state) -{ - if (hs->pending) { - if (hs->pending->target_state < target_state) - hs->pending->target_state = target_state; - return; - } - - hs->pending = g_new0(struct pending_connect, 1); - hs->pending->target_state = target_state; -} - -static unsigned int connect_cb_new(struct headset *hs, - headset_state_t target_state, - headset_stream_cb_t func, - void *user_data) -{ - struct connect_cb *cb; - unsigned int free_cb_id = 1; - - pending_connect_init(hs, target_state); - - cb = g_new(struct connect_cb, 1); - - cb->cb = func; - cb->cb_data = user_data; - cb->id = free_cb_id++; - - hs->pending->callbacks = g_slist_append(hs->pending->callbacks, - cb); - - return cb->id; -} - -static gboolean sco_connect_cb(GIOChannel *chan, GIOCondition cond, - struct device *device) -{ - struct headset *hs; - int ret, sk; - socklen_t len; - struct pending_connect *p; - - if (cond & G_IO_NVAL) - return FALSE; - - hs = device->headset; - p = hs->pending; - - sk = g_io_channel_unix_get_fd(chan); - - len = sizeof(ret); - if (getsockopt(sk, SOL_SOCKET, SO_ERROR, &ret, &len) < 0) { - p->err = errno; - error("getsockopt(SO_ERROR): %s (%d)", strerror(p->err), - p->err); - goto failed; - } - - if (ret != 0) { - p->err = ret; - error("connect(): %s (%d)", strerror(ret), ret); - goto failed; - } - - debug("SCO socket opened for headset %s", device->path); - - info("SCO fd=%d", sk); - hs->sco = chan; - p->io = NULL; - - pending_connect_finalize(device); - - fcntl(sk, F_SETFL, 0); - - headset_set_state(device, HEADSET_STATE_PLAYING); - - return FALSE; - -failed: - pending_connect_finalize(device); - if (hs->rfcomm) - headset_set_state(device, HEADSET_STATE_CONNECTED); - else - headset_set_state(device, HEADSET_STATE_DISCONNECTED); - - return FALSE; -} - -static int sco_connect(struct device *dev, headset_stream_cb_t cb, - void *user_data, unsigned int *cb_id) -{ - struct headset *hs = dev->headset; - struct sockaddr_sco addr; - GIOChannel *io; - int sk, err; - - if (hs->state != HEADSET_STATE_CONNECTED) - return -EINVAL; - - sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO); - if (sk < 0) { - err = errno; - error("socket(BTPROTO_SCO): %s (%d)", strerror(err), err); - return -err; - } - - io = g_io_channel_unix_new(sk); - if (!io) { - close(sk); - return -ENOMEM; - } - - memset(&addr, 0, sizeof(addr)); - addr.sco_family = AF_BLUETOOTH; - bacpy(&addr.sco_bdaddr, BDADDR_ANY); - - if (bind(sk, (struct sockaddr *)&addr, sizeof(addr)) < 0) { - err = errno; - error("socket(BTPROTO_SCO): %s (%d)", strerror(err), err); - goto failed; - } - - if (set_nonblocking(sk) < 0) { - err = errno; - goto failed; - } - - memset(&addr, 0, sizeof(addr)); - addr.sco_family = AF_BLUETOOTH; - bacpy(&addr.sco_bdaddr, &dev->dst); - - err = connect(sk, (struct sockaddr *) &addr, sizeof(addr)); - - if (err < 0 && !(errno == EAGAIN || errno == EINPROGRESS)) { - err = errno; - error("connect: %s (%d)", strerror(errno), errno); - goto failed; - } - - headset_set_state(dev, HEADSET_STATE_PLAY_IN_PROGRESS); - - pending_connect_init(hs, HEADSET_STATE_PLAYING); - - if (cb) { - unsigned int id = connect_cb_new(hs, HEADSET_STATE_PLAYING, - cb, user_data); - if (cb_id) - *cb_id = id; - } - - g_io_add_watch(io, G_IO_OUT | G_IO_NVAL | G_IO_ERR | G_IO_HUP, - (GIOFunc) sco_connect_cb, dev); - - hs->pending->io = io; - - return 0; - -failed: - g_io_channel_close(io); - g_io_channel_unref(io); - return -err; -} - static gboolean rfcomm_connect_cb(GIOChannel *chan, GIOCondition cond, struct device *dev) { @@ -715,12 +766,16 @@ static gboolean rfcomm_connect_cb(GIOChannel *chan, GIOCondition cond, else hs->hfp_active = FALSE; - headset_set_state(dev, HEADSET_STATE_CONNECTED); + g_io_add_watch(chan, G_IO_IN | G_IO_ERR | G_IO_HUP| G_IO_NVAL, + (GIOFunc) rfcomm_io_cb, dev); debug("%s: Connected to %s", dev->path, hs_address); - g_io_add_watch(chan, G_IO_IN | G_IO_ERR | G_IO_HUP| G_IO_NVAL, - (GIOFunc) rfcomm_io_cb, dev); + /* In HFP mode wait for Service Level Connection */ + if (hs->hfp_active) + return FALSE; + + headset_set_state(dev, HEADSET_STATE_CONNECTED); if (p->target_state == HEADSET_STATE_PLAYING) { p->err = sco_connect(dev, NULL, NULL, NULL); @@ -729,6 +784,11 @@ static gboolean rfcomm_connect_cb(GIOChannel *chan, GIOCondition cond, return FALSE; } + if (p->msg) { + DBusMessage *reply = dbus_message_new_method_return(p->msg); + send_message_and_unref(dev->conn, reply); + } + pending_connect_finalize(dev); return FALSE; @@ -1939,22 +1999,25 @@ int headset_connect_rfcomm(struct device *dev, int sock) { struct headset *hs = dev->headset; - hs->rfcomm = g_io_channel_unix_new(sock); + hs->tmp_rfcomm = g_io_channel_unix_new(sock); - return hs->rfcomm ? 0 : -EINVAL; + return hs->tmp_rfcomm ? 0 : -EINVAL; } int headset_close_rfcomm(struct device *dev) { struct headset *hs = dev->headset; + GIOChannel *rfcomm = hs->tmp_rfcomm ? hs->tmp_rfcomm : hs->rfcomm; if (hs->ring_timer) { g_source_remove(hs->ring_timer); hs->ring_timer = 0; } - if (hs->rfcomm) { - g_io_channel_close(hs->rfcomm); - g_io_channel_unref(hs->rfcomm); + + if (rfcomm) { + g_io_channel_close(rfcomm); + g_io_channel_unref(rfcomm); + hs->tmp_rfcomm = NULL; hs->rfcomm = NULL; } @@ -1964,6 +2027,21 @@ int headset_close_rfcomm(struct device *dev) return 0; } +void headset_set_authorized(struct device *dev) +{ + struct headset *hs = dev->headset; + + hs->rfcomm = hs->tmp_rfcomm; + hs->tmp_rfcomm = NULL; + + g_io_add_watch(hs->rfcomm, + G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL, + (GIOFunc) rfcomm_io_cb, dev); + + if (!hs->hfp_active) + headset_set_state(dev, HEADSET_STATE_CONNECTED); +} + void headset_set_state(struct device *dev, headset_state_t state) { struct headset *hs = dev->headset; @@ -1985,10 +2063,6 @@ void headset_set_state(struct device *dev, headset_state_t state) case HEADSET_STATE_CONNECTED: close_sco(dev); if (hs->state < state) { - g_io_add_watch(hs->rfcomm, - G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL, - (GIOFunc) rfcomm_io_cb, dev); - dbus_connection_emit_signal(dev->conn, dev->path, AUDIO_HEADSET_INTERFACE, "Connected", diff --git a/audio/headset.h b/audio/headset.h index 5a61bd56..b218b46e 100644 --- a/audio/headset.h +++ b/audio/headset.h @@ -58,6 +58,7 @@ gboolean headset_cancel_stream(struct device *dev, unsigned int id); gboolean get_hfp_active(struct device *dev); void set_hfp_active(struct device *dev, gboolean active); +void headset_set_authorized(struct device *dev); int headset_connect_rfcomm(struct device *dev, int sock); int headset_close_rfcomm(struct device *dev); diff --git a/audio/manager.c b/audio/manager.c index c45e45db..69f9237f 100644 --- a/audio/manager.c +++ b/audio/manager.c @@ -1405,7 +1405,7 @@ static void auth_cb(DBusPendingCall *call, void *data) } else { char hs_address[18]; - headset_set_state(device, HEADSET_STATE_CONNECTED); + headset_set_authorized(device); ba2str(&device->dst, hs_address); @@ -1464,14 +1464,14 @@ static gboolean ag_io_cb(GIOChannel *chan, GIOCondition cond, void *data) return TRUE; } + set_hfp_active(device, hfp_active); + if (headset_connect_rfcomm(device, cli_sk) < 0) { error("Allocating new GIOChannel failed!"); close(cli_sk); return TRUE; } - set_hfp_active(device, hfp_active); - if (!manager_authorize(&device->dst, uuid, auth_cb, device, NULL)) goto failed; -- cgit