diff options
| -rw-r--r-- | input/device.c | 127 | ||||
| -rw-r--r-- | input/device.h | 4 | ||||
| -rw-r--r-- | input/manager.c | 174 |
3 files changed, 218 insertions, 87 deletions
diff --git a/input/device.c b/input/device.c index a29cc095..76049ec4 100644 --- a/input/device.c +++ b/input/device.c @@ -54,9 +54,6 @@ #define INPUT_DEVICE_INTERFACE "org.bluez.input.Device" -#define L2CAP_PSM_HIDP_CTRL 0x11 -#define L2CAP_PSM_HIDP_INTR 0x13 - #define BUF_SIZE 16 #define UPDOWN_ENABLED 1 @@ -515,63 +512,6 @@ failed: return -err; } -static int l2cap_connect(struct device *idev, - unsigned short psm, GIOFunc cb) -{ - GIOChannel *io; - struct sockaddr_l2 addr; - struct l2cap_options opts; - int sk, err; - - if ((sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP)) < 0) - return -1; - - memset(&addr, 0, sizeof(addr)); - addr.l2_family = AF_BLUETOOTH; - bacpy(&addr.l2_bdaddr, &idev->src); - - if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) - goto failed; - - if (set_nonblocking(sk) < 0) - goto failed; - - memset(&opts, 0, sizeof(opts)); - opts.imtu = HIDP_DEFAULT_MTU; - opts.omtu = HIDP_DEFAULT_MTU; - opts.flush_to = 0xffff; - - if (setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, sizeof(opts)) < 0) - goto failed; - - memset(&addr, 0, sizeof(addr)); - addr.l2_family = AF_BLUETOOTH; - bacpy(&addr.l2_bdaddr, &idev->dst); - addr.l2_psm = htobs(psm); - - io = g_io_channel_unix_new(sk); - g_io_channel_set_close_on_unref(io, FALSE); - - if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) { - if (!(errno == EAGAIN || errno == EINPROGRESS)) - goto failed; - - g_io_add_watch(io, G_IO_OUT | G_IO_ERR | G_IO_HUP | G_IO_NVAL, - (GIOFunc) cb, idev); - } else { - cb(io, G_IO_OUT, idev); - } - - return 0; - -failed: - err = errno; - close(sk); - errno = err; - - return -1; -} - static gboolean interrupt_connect_cb(GIOChannel *chan, GIOCondition cond, struct device *idev) { @@ -704,8 +644,8 @@ static gboolean control_connect_cb(GIOChannel *chan, } /* Connect to the HID interrupt channel */ - if (l2cap_connect(idev, L2CAP_PSM_HIDP_INTR, - (GIOFunc) interrupt_connect_cb) < 0) { + if (l2cap_connect(&idev->src, &idev->dst, L2CAP_PSM_HIDP_INTR, + (GIOFunc) interrupt_connect_cb, idev) < 0) { err = errno; error("L2CAP connect failed:%s (%d)", strerror(errno), errno); @@ -861,8 +801,9 @@ static DBusHandlerResult device_connect(DBusConnection *conn, } /* HID devices */ - if (l2cap_connect(idev, L2CAP_PSM_HIDP_CTRL, - (GIOFunc) control_connect_cb) < 0) { + if (l2cap_connect(&idev->src, &idev->dst, L2CAP_PSM_HIDP_CTRL, + (GIOFunc) control_connect_cb, idev) < 0) { + error("L2CAP connect failed: %s(%d)", strerror(errno), errno); pending_connect_free(idev->pending_connect); idev->pending_connect = NULL; @@ -1153,3 +1094,61 @@ int input_device_get_bdaddr(DBusConnection *conn, const char *path, return 0; } + +int l2cap_connect(bdaddr_t *src, bdaddr_t *dst, unsigned short psm, GIOFunc cb, void *data) +{ + GIOChannel *io; + struct sockaddr_l2 addr; + struct l2cap_options opts; + int sk, err; + + if ((sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP)) < 0) + return -1; + + memset(&addr, 0, sizeof(addr)); + addr.l2_family = AF_BLUETOOTH; + bacpy(&addr.l2_bdaddr, src); + + if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) + goto failed; + + if (set_nonblocking(sk) < 0) + goto failed; + + memset(&opts, 0, sizeof(opts)); + opts.imtu = HIDP_DEFAULT_MTU; + opts.omtu = HIDP_DEFAULT_MTU; + opts.flush_to = 0xffff; + + if (setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, sizeof(opts)) < 0) + goto failed; + + memset(&addr, 0, sizeof(addr)); + addr.l2_family = AF_BLUETOOTH; + bacpy(&addr.l2_bdaddr, dst); + addr.l2_psm = htobs(psm); + + io = g_io_channel_unix_new(sk); + g_io_channel_set_close_on_unref(io, FALSE); + + if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + if (!(errno == EAGAIN || errno == EINPROGRESS)) { + g_io_channel_unref(io); + goto failed; + } + + g_io_add_watch(io, G_IO_OUT | G_IO_ERR | G_IO_HUP | G_IO_NVAL, + (GIOFunc) cb, data); + } else { + cb(io, G_IO_OUT, data); + } + + return 0; + +failed: + err = errno; + close(sk); + errno = err; + + return -1; +} diff --git a/input/device.h b/input/device.h index b156b985..b3316841 100644 --- a/input/device.h +++ b/input/device.h @@ -20,6 +20,8 @@ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA * */ +#define L2CAP_PSM_HIDP_CTRL 0x11 +#define L2CAP_PSM_HIDP_INTR 0x13 int input_device_register(DBusConnection *conn, bdaddr_t *src, bdaddr_t *dst, struct hidp_connadd_req *hidp, const char **ppath); @@ -29,3 +31,5 @@ int input_device_unregister(DBusConnection *conn, const char *path); int input_device_get_bdaddr(DBusConnection *conn, const char *path, bdaddr_t *src, bdaddr_t *dst); +int l2cap_connect(bdaddr_t *src, bdaddr_t *dst, + unsigned short psm, GIOFunc cb, void *data); diff --git a/input/manager.c b/input/manager.c index f74349df..d339c007 100644 --- a/input/manager.c +++ b/input/manager.c @@ -27,7 +27,9 @@ #include <ctype.h> #include <dirent.h> +#include <errno.h> #include <stdlib.h> +#include <unistd.h> #include <bluetooth/bluetooth.h> #include <bluetooth/hci.h> @@ -62,6 +64,7 @@ struct pending_req { DBusMessage *msg; sdp_record_t *pnp_rec; sdp_record_t *hid_rec; + int ctrl_sock; }; static GSList *device_paths = NULL; /* Input registered paths */ @@ -245,15 +248,151 @@ static void extract_pnp_record(sdp_record_t *rec, struct hidp_connadd_req *req) req->version = pdlist ? pdlist->val.uint16 : 0x0000; } +static gboolean interrupt_connect_cb(GIOChannel *chan, + GIOCondition cond, struct pending_req *pr) +{ + struct hidp_connadd_req hidp; + DBusMessage *reply; + const char *path; + int isk, ret, err; + socklen_t len; + + if (cond & G_IO_NVAL) { + err = EHOSTDOWN; + isk = -1; + goto failed; + } + + if (cond & (G_IO_HUP | G_IO_ERR)) { + err = EINTR; + isk = -1; + error("Hangup or error on HIDP interrupt socket"); + goto failed; + + } + + isk = g_io_channel_unix_get_fd(chan); + + len = sizeof(ret); + if (getsockopt(isk, 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; + } + + + memset(&hidp, 0, sizeof(struct hidp_connadd_req)); + extract_hid_record(pr->hid_rec, &hidp); + if (pr->pnp_rec) + extract_pnp_record(pr->pnp_rec, &hidp); + + store_device_info(&pr->src, &pr->dst, &hidp); + + if (input_device_register(pr->conn, &pr->src, + &pr->dst, &hidp, &path) < 0) { + err_failed(pr->conn, pr->msg, "path registration failed"); + goto cleanup; + } + + device_paths = g_slist_append(device_paths, g_strdup(path)); + + /* Replying to the requestor */ + reply = dbus_message_new_method_return(pr->msg); + + dbus_message_append_args(reply, + DBUS_TYPE_STRING, &path, + DBUS_TYPE_INVALID); + + send_message_and_unref(pr->conn, reply); + + goto cleanup; +failed: + err_connection_failed(pr->conn, pr->msg, strerror(err)); + +cleanup: + if (isk > 0) + close(isk); + + close(pr->ctrl_sock); + pending_req_free(pr); + g_io_channel_unref(chan); + + return FALSE; +} + +static gboolean control_connect_cb(GIOChannel *chan, + GIOCondition cond, struct pending_req *pr) +{ + int ret, csk, err; + socklen_t len; + + if (cond & G_IO_NVAL) { + err = EHOSTDOWN; + csk = -1; + goto failed; + } + + if (cond & (G_IO_HUP | G_IO_ERR)) { + err = EINTR; + csk = -1; + error("Hangup or error on HIDP control socket"); + goto failed; + + } + + csk = g_io_channel_unix_get_fd(chan); + /* Set HID control channel */ + pr->ctrl_sock = csk; + + len = sizeof(ret); + if (getsockopt(csk, 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; + } + + /* Connect to the HID interrupt channel */ + if (l2cap_connect(&pr->src, &pr->dst, L2CAP_PSM_HIDP_INTR, + (GIOFunc) interrupt_connect_cb, pr) < 0) { + + err = errno; + error("L2CAP connect failed:%s (%d)", strerror(errno), errno); + goto failed; + } + + g_io_channel_unref(chan); + return FALSE; + +failed: + if (csk > 0) + close(csk); + + err_connection_failed(pr->conn, pr->msg, strerror(err)); + pending_req_free(pr); + + g_io_channel_unref(chan); + + return FALSE; +} + static void hid_record_reply(DBusPendingCall *call, void *data) { DBusMessage *reply = dbus_pending_call_steal_reply(call); - DBusMessage *pr_reply; struct pending_req *pr = data; - struct hidp_connadd_req hidp; DBusError derr; uint8_t *rec_bin; - const char *path; int len, scanned; dbus_error_init(&derr); @@ -289,29 +428,18 @@ static void hid_record_reply(DBusPendingCall *call, void *data) goto fail; } - memset(&hidp, 0, sizeof(struct hidp_connadd_req)); - extract_hid_record(pr->hid_rec, &hidp); - if (pr->pnp_rec) - extract_pnp_record(pr->pnp_rec, &hidp); - - store_device_info(&pr->src, &pr->dst, &hidp); - - if (input_device_register(pr->conn, &pr->src, - &pr->dst, &hidp, &path) < 0) { - err_failed(pr->conn, pr->msg, "D-Bus path registration failed"); + if (l2cap_connect(&pr->src, &pr->dst, L2CAP_PSM_HIDP_CTRL, + (GIOFunc) control_connect_cb, pr) < 0) { + int err = errno; + error("L2CAP connect failed:%s (%d)", strerror(err), err); + err_connection_failed(pr->conn, pr->msg, strerror(err)); goto fail; - } - - device_paths = g_slist_append(device_paths, g_strdup(path)); - pr_reply = dbus_message_new_method_return(pr->msg); - - dbus_message_append_args(pr_reply, - DBUS_TYPE_STRING, &path, - DBUS_TYPE_INVALID); - - send_message_and_unref(pr->conn, pr_reply); + } + dbus_message_unref(reply); + dbus_pending_call_unref(call); + return; fail: dbus_error_free(&derr); pending_req_free(pr); |