diff options
| -rw-r--r-- | input/device.c | 295 | ||||
| -rw-r--r-- | input/device.h | 7 | ||||
| -rw-r--r-- | input/manager.c | 5 | ||||
| -rw-r--r-- | input/server.c | 200 |
4 files changed, 271 insertions, 236 deletions
diff --git a/input/device.c b/input/device.c index 1310b7fb..d3f07cf5 100644 --- a/input/device.c +++ b/input/device.c @@ -77,11 +77,18 @@ struct device { char *name; uint8_t major; uint8_t minor; - struct hidp_connadd_req hidp; /* FIXME: Use dynamic alloc? */ + uint16_t product; + uint16_t vendor; struct fake_input *fake; - struct pending_connect *pending_connect; + struct pending_connect *pending_connect; /* FIXME: use only the msg */ + DBusConnection *conn; + char *path; + int ctrl_sk; + int intr_sk; }; +GSList *devices = NULL; + static struct device *device_new(bdaddr_t *src, bdaddr_t *dst) { struct device *idev; @@ -95,11 +102,10 @@ static struct device *device_new(bdaddr_t *src, bdaddr_t *dst) read_device_name(src, dst, &idev->name); read_device_class(src, dst, &cls); - idev->major = (cls >> 8) & 0x1f; - idev->minor = (cls >> 2) & 0x3f; - - /* FIXME: hidp could be alloc dynamically */ - snprintf(idev->hidp.name, 128, "%s", idev->name); + idev->major = (cls >> 8) & 0x1f; + idev->minor = (cls >> 2) & 0x3f; + idev->ctrl_sk = -1; + idev->intr_sk = -1; return idev; } @@ -121,12 +127,13 @@ static void device_free(struct device *idev) return; if (idev->name) g_free(idev->name); - if (idev->hidp.rd_data) - g_free(idev->hidp.rd_data); if (idev->fake) g_free(idev->fake); - if (idev->pending_connect) + if (idev->pending_connect) pending_connect_free(idev->pending_connect); + if (idev->path) + g_free(idev->path); + dbus_connection_unref(idev->conn); g_free(idev); } @@ -509,11 +516,95 @@ failed: return -err; } +static gboolean connection_event(GIOChannel *chan, GIOCondition cond, gpointer data) +{ + struct device *idev = data; + gboolean ret = TRUE; + + if (cond & G_IO_NVAL) + ret = FALSE; + + if (cond & (G_IO_HUP | G_IO_ERR)) { + g_io_channel_close(chan); + ret = FALSE; + } + + if (ret == FALSE) + dbus_connection_emit_signal(idev->conn, + idev->path, + INPUT_DEVICE_INTERFACE, + "Disconnected", + DBUS_TYPE_INVALID); + + return ret; +} + +static void create_watch(int sk, struct device *idev) +{ + GIOChannel *io; + + io = g_io_channel_unix_new(sk); + g_io_add_watch(io, G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL, + connection_event, idev); + g_io_channel_unref(io); +} + +static int hidp_connadd(bdaddr_t *src, bdaddr_t *dst, int ctrl_sk, int intr_sk, const char *name) +{ + struct hidp_connadd_req req; + char addr[18]; + int ctl, err, timeout = 30; + + ctl = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HIDP); + if (ctl < 0) { + error("Can't open HIDP interface"); + return -errno; + } + + ba2str(dst, addr); + + memset(&req, 0, sizeof(req)); + req.ctrl_sock = ctrl_sk; + req.intr_sock = intr_sk; + req.flags = 0; + req.idle_to = timeout * 60; + + err = get_stored_device_info(src, dst, &req); + if (err < 0) { + error("Rejected connection from unknown device %s", addr); + goto cleanup; + } + + if (req.subclass & 0x40) { + err = encrypt_link(src, dst); + if (err < 0 && err != -ENOKEY) + goto cleanup; + } + + if (name) + strncpy(req.name, name, 128); + + info("New input device %s (%s)", addr, req.name); + + if (req.vendor == 0x054c && req.product == 0x0268) { + unsigned char buf[] = { 0x53, 0xf4, 0x42, 0x03, 0x00, 0x00 }; + err = write(ctrl_sk, buf, sizeof(buf)); + } + + err = ioctl(ctl, HIDPCONNADD, &req); +cleanup: + close(ctl); + + if (req.rd_data) + free(req.rd_data); + + return err; +} + static gboolean interrupt_connect_cb(GIOChannel *chan, GIOCondition cond, struct device *idev) { - int ctl, isk, ret, err; - const char *path; + int isk, ret, err; socklen_t len; isk = g_io_channel_unix_get_fd(chan); @@ -531,9 +622,6 @@ static gboolean interrupt_connect_cb(GIOChannel *chan, } - idev->hidp.intr_sock = isk; - idev->hidp.idle_to = 30 * 60; /* 30 minutes */ - len = sizeof(ret); if (getsockopt(isk, SOL_SOCKET, SO_ERROR, &ret, &len) < 0) { err = errno; @@ -547,54 +635,33 @@ static gboolean interrupt_connect_cb(GIOChannel *chan, goto failed; } - ctl = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HIDP); - if (ctl < 0) { - err = errno; - error("Can't open HIDP control socket"); + idev->intr_sk = isk; + err = hidp_connadd(&idev->src, &idev->dst, idev->ctrl_sk, idev->intr_sk, idev->name); + if (err < 0) goto failed; - } - - if (idev->hidp.subclass & 0x40) { - int ret; - ret = encrypt_link(&idev->src, &idev->dst); - if (ret < 0 && ret != -ENOKEY) { - err = -ret; - close(ctl); - goto failed; - } - } - if (ioctl(ctl, HIDPCONNADD, &idev->hidp) < 0) { - err = errno; - close(ctl); - goto failed; - } + create_watch(idev->ctrl_sk, idev); + dbus_connection_emit_signal(idev->conn, + idev->path, + INPUT_DEVICE_INTERFACE, + "Connected", + DBUS_TYPE_INVALID); /* Replying to the requestor */ send_message_and_unref(idev->pending_connect->conn, dbus_message_new_method_return(idev->pending_connect->msg)); - /* Sending the Connected signal */ - path = dbus_message_get_path(idev->pending_connect->msg); - dbus_connection_emit_signal(idev->pending_connect->conn, path, - INPUT_DEVICE_INTERFACE, "Connected" , - DBUS_TYPE_INVALID); - - close (ctl); goto cleanup; failed: err_connection_failed(idev->pending_connect->conn, idev->pending_connect->msg, strerror(err)); - -cleanup: if (isk > 0) close(isk); + close(idev->ctrl_sk); + idev->intr_sk = -1; + idev->ctrl_sk = -1; - close(idev->hidp.ctrl_sock); - - idev->hidp.intr_sock = -1; - idev->hidp.ctrl_sock = -1; - +cleanup: pending_connect_free(idev->pending_connect); idev->pending_connect = NULL; @@ -622,7 +689,7 @@ static gboolean control_connect_cb(GIOChannel *chan, } /* Set HID control channel */ - idev->hidp.ctrl_sock = csk; + idev->ctrl_sk = csk; len = sizeof(ret); if (getsockopt(csk, SOL_SOCKET, SO_ERROR, &ret, &len) < 0) { @@ -652,7 +719,7 @@ failed: if (csk > 0) close(csk); - idev->hidp.ctrl_sock = -1; + idev->ctrl_sk = -1; err_connection_failed(idev->pending_connect->conn, idev->pending_connect->msg, strerror(err)); pending_connect_free(idev->pending_connect); @@ -687,6 +754,14 @@ static int disconnect(struct device *idev, uint32_t flags) } /* Standard HID disconnect */ + if (idev->ctrl_sk >= 0) { + close(idev->ctrl_sk); + idev->ctrl_sk = -1; + } + if (idev->intr_sk >= 0) { + close(idev->intr_sk); + idev->intr_sk = -1; + } ctl = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HIDP); if (ctl < 0) { @@ -719,8 +794,8 @@ fail: close(ctl); errno = err; - idev->hidp.intr_sock = -1; - idev->hidp.ctrl_sock = -1; + idev->intr_sk = -1; + idev->ctrl_sk = -1; return -err; } @@ -812,22 +887,13 @@ static DBusHandlerResult device_disconnect(DBusConnection *conn, DBusMessage *msg, void *data) { struct device *idev = data; - const char *path; if (disconnect(idev, 0) < 0) return err_failed(conn, msg, strerror(errno)); /* Replying to the requestor */ - send_message_and_unref(conn, + return send_message_and_unref(conn, dbus_message_new_method_return(msg)); - - /* Sending the Disconnect signal */ - path = dbus_message_get_path(msg); - dbus_connection_emit_signal(conn, path, - INPUT_DEVICE_INTERFACE, "Disconnected", - DBUS_TYPE_INVALID); - - return DBUS_HANDLER_RESULT_HANDLED; } static DBusHandlerResult device_is_connected(DBusConnection *conn, @@ -896,7 +962,7 @@ static DBusHandlerResult device_get_name(DBusConnection *conn, { struct device *idev = data; DBusMessage *reply; - const char *pname = idev->hidp.name; + const char *pname = idev->name; reply = dbus_message_new_method_return(msg); if (!reply) @@ -920,7 +986,7 @@ static DBusHandlerResult device_get_product_id(DBusConnection *conn, return DBUS_HANDLER_RESULT_NEED_MEMORY; dbus_message_append_args(reply, - DBUS_TYPE_UINT16, &idev->hidp.product, + DBUS_TYPE_UINT16, &idev->product, DBUS_TYPE_INVALID); return send_message_and_unref(conn, reply); @@ -937,7 +1003,7 @@ static DBusHandlerResult device_get_vendor_id(DBusConnection *conn, return DBUS_HANDLER_RESULT_NEED_MEMORY; dbus_message_append_args(reply, - DBUS_TYPE_UINT16, &idev->hidp.vendor, + DBUS_TYPE_UINT16, &idev->vendor, DBUS_TYPE_INVALID); return send_message_and_unref(conn, reply); @@ -994,7 +1060,9 @@ static int register_path(DBusConnection *conn, const char *path, struct device * dbus_connection_emit_signal(conn, INPUT_PATH, INPUT_MANAGER_INTERFACE, "DeviceCreated", DBUS_TYPE_STRING, &path, - DBUS_TYPE_INVALID); + DBUS_TYPE_INVALID); + + devices = g_slist_append(devices, idev); info("Created input device: %s", path); @@ -1010,9 +1078,10 @@ int input_device_register(DBusConnection *conn, bdaddr_t *src, bdaddr_t *dst, idev = device_new(src, dst); path = create_input_path(idev->major, idev->minor); - - /* rd_data must not be deallocated since the memory address is copied */ - memcpy(&idev->hidp, hid, sizeof(struct hidp_connadd_req)); + idev->path = g_strdup(path); + idev->product = hid->product; + idev->vendor = hid->vendor; + idev->conn = dbus_connection_ref(conn); err = register_path(conn, path, idev); @@ -1031,6 +1100,10 @@ int fake_input_register(DBusConnection *conn, bdaddr_t *src, idev = device_new(src, dst); path = create_input_path(idev->major, idev->minor); + idev->path = g_strdup(path); + idev->conn = dbus_connection_ref(conn); + + /* FIXME: Missing set product and vendor */ idev->fake = g_new0(struct fake_input, 1); idev->fake->ch = ch; @@ -1058,6 +1131,8 @@ int input_device_unregister(DBusConnection *conn, const char *path) del_stored_device_info(&idev->src, &idev->dst); + devices = g_slist_remove(devices, idev); + dbus_connection_destroy_object_path(conn, path); dbus_connection_emit_signal(conn, INPUT_PATH, @@ -1148,3 +1223,83 @@ failed: return -1; } + +static struct device *find_device(bdaddr_t *src, bdaddr_t *dst) +{ + struct device *idev; + GSList *list; + + for (list = devices; list != NULL; list = list->next) { + idev = list->data; + + if (!bacmp(&idev->src, src) && !bacmp(&idev->dst, dst)) + return idev; + } + + return NULL; +} + +int input_device_set_channel(bdaddr_t *src, bdaddr_t *dst, int psm, int nsk) +{ + struct device *idev = find_device(src, dst); + if (!idev) + return -ENOENT; + + switch (psm) { + case L2CAP_PSM_HIDP_CTRL: + idev->ctrl_sk = nsk; + break; + case L2CAP_PSM_HIDP_INTR: + idev->intr_sk = nsk; + break; + } + + return 0; +} + +int input_device_close_channels(bdaddr_t *src, bdaddr_t *dst) +{ + struct device *idev = find_device(src, dst); + if (!idev) + return -ENOENT; + + if (idev->ctrl_sk >= 0) { + close(idev->ctrl_sk); + idev->ctrl_sk = -1; + } + + if (idev->intr_sk >= 0) { + close(idev->intr_sk); + idev->intr_sk = -1; + } + + return 0; +} + +int input_device_connadd(bdaddr_t *src, bdaddr_t *dst) +{ + struct device *idev; + int err; + + idev = find_device(src, dst); + if (!idev) + return -ENOENT; + + err = hidp_connadd(src, dst, idev->ctrl_sk, idev->intr_sk, idev->name); + if (err < 0) { + close(idev->ctrl_sk); + close(idev->intr_sk); + idev->ctrl_sk = -1; + idev->intr_sk = -1; + + return err; + } + + create_watch(idev->ctrl_sk, idev); + dbus_connection_emit_signal(idev->conn, + idev->path, + INPUT_DEVICE_INTERFACE, + "Connected", + DBUS_TYPE_INVALID); + return 0; +} diff --git a/input/device.h b/input/device.h index 5501d3b9..fdf7f90f 100644 --- a/input/device.h +++ b/input/device.h @@ -32,5 +32,12 @@ 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 input_device_set_channel(bdaddr_t *src, bdaddr_t *dst, int psm, int nsk); + +int input_device_close_channels(bdaddr_t *src, bdaddr_t *dst); + +int input_device_connadd(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 bd87c5fb..03c914ee 100644 --- a/input/manager.c +++ b/input/manager.c @@ -954,9 +954,12 @@ static void stored_input(char *key, char *value, void *data) * acceptable since the source is different. */ if (input_device_register(connection, src, &dst, &hidp, &path) < 0) - return; + goto cleanup; device_paths = g_slist_append(device_paths, g_strdup(path)); +cleanup: + if (hidp.rd_data) + g_free(hidp.rd_data); } static void register_stored_inputs(void) diff --git a/input/server.c b/input/server.c index 2a9e08d4..0f04b06a 100644 --- a/input/server.c +++ b/input/server.c @@ -42,103 +42,12 @@ #include "logging.h" #include "dbus.h" +#include "device.h" #include "server.h" #include "storage.h" -struct session_data { - bdaddr_t src; - bdaddr_t dst; - int ctrl_sk; - int intr_sk; -}; - -static GSList *sessions = NULL; static DBusConnection *connection = NULL; -static struct session_data *find_session(bdaddr_t *src, bdaddr_t *dst) -{ - GSList *list; - - for (list = sessions; list != NULL; list = list->next) { - struct session_data *session = list->data; - - if (!bacmp(&session->src, src) && !bacmp(&session->dst, dst)) - return session; - } - - return NULL; -} - -static gboolean session_event(GIOChannel *chan, GIOCondition cond, gpointer data) -{ - if (cond & G_IO_NVAL) - return FALSE; - - if (cond & (G_IO_HUP | G_IO_ERR)) { - g_io_channel_close(chan); - return FALSE; - } - - return TRUE; -} - -static void create_device(struct session_data *session) -{ - struct hidp_connadd_req req; - char addr[18]; - int ctl, err, timeout = 30; - - ctl = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HIDP); - if (ctl < 0) { - error("Can't open HIDP interface"); - goto cleanup; - } - - ba2str(&session->dst, addr); - - memset(&req, 0, sizeof(req)); - req.ctrl_sock = session->ctrl_sk; - req.intr_sock = session->intr_sk; - req.flags = 0; - req.idle_to = timeout * 60; - - if (get_stored_device_info(&session->src, &session->dst, &req) < 0) { - error("Rejected connection from unknown device %s", addr); - goto cleanup; - } - - if (req.subclass & 0x40) { - err = encrypt_link(&session->src, &session->dst); - if (err < 0 && err != -ENOKEY) { - if (req.rd_data) - free(req.rd_data); - goto cleanup; - } - } - - info("New input device %s (%s)", addr, req.name); - - if (req.vendor == 0x054c && req.product == 0x0268) { - unsigned char buf[] = { 0x53, 0xf4, 0x42, 0x03, 0x00, 0x00 }; - err = write(session->ctrl_sk, buf, sizeof(buf)); - } - - err = ioctl(ctl, HIDPCONNADD, &req); - - close(ctl); - - if (req.rd_data) - free(req.rd_data); - -cleanup: - sessions = g_slist_remove(sessions, session); - - close(session->intr_sk); - close(session->ctrl_sk); - - g_free(session); -} - static void cancel_authorization(const char *addr) { DBusMessage *msg; @@ -160,38 +69,39 @@ static void cancel_authorization(const char *addr) send_message_and_unref(connection, msg); } +struct authorization_data { + bdaddr_t src; + bdaddr_t dst; +}; + static void authorization_callback(DBusPendingCall *pcall, void *data) { - struct session_data *session = data; + struct authorization_data *auth = data; DBusMessage *reply = dbus_pending_call_steal_reply(pcall); DBusError derr; dbus_error_init(&derr); - if (dbus_set_error_from_message(&derr, reply)) { - error("Access denied: %s", derr.message); - if (dbus_error_has_name(&derr, DBUS_ERROR_NO_REPLY)) { - char addr[18]; - memset(addr, 0, sizeof(addr)); - ba2str(&session->dst, addr); - cancel_authorization(addr); - } - dbus_error_free(&derr); - - sessions = g_slist_remove(sessions, session); - - close(session->intr_sk); - close(session->ctrl_sk); - - g_free(session); - } else { - create_device(session); + if (dbus_set_error_from_message(&derr, reply) != TRUE) { + dbus_message_unref(reply); + input_device_connadd(&auth->src, &auth->dst); + return; + } + + error("Authorization denied: %s", derr.message); + if (dbus_error_has_name(&derr, DBUS_ERROR_NO_REPLY)) { + char addr[18]; + memset(addr, 0, sizeof(addr)); + ba2str(&auth->dst, addr); + cancel_authorization(addr); } + dbus_error_free(&derr); dbus_message_unref(reply); } -static int authorize_device(struct session_data *session) +static int authorize_device(bdaddr_t *src, bdaddr_t *dst) { + struct authorization_data *auth; DBusMessage *msg; DBusPendingCall *pending; char addr[18]; @@ -206,7 +116,7 @@ static int authorize_device(struct session_data *session) } memset(addr, 0, sizeof(addr)); - ba2str(&session->dst, addr); + ba2str(dst, addr); dbus_message_append_args(msg, DBUS_TYPE_STRING, &paddr, DBUS_TYPE_STRING, &uuid, @@ -216,28 +126,18 @@ static int authorize_device(struct session_data *session) msg, &pending, -1) == FALSE) return -EACCES; - dbus_pending_call_set_notify(pending, authorization_callback, session, NULL); + auth = g_new0(struct authorization_data, 1); + bacpy(&auth->src, src); + bacpy(&auth->dst, dst); + dbus_pending_call_set_notify(pending, authorization_callback, auth, g_free); dbus_pending_call_unref(pending); dbus_message_unref(msg); return 0; } -static void create_watch(int sk, struct session_data *session) -{ - GIOChannel *io; - - io = g_io_channel_unix_new(sk); - - g_io_add_watch(io, G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL, - session_event, session); - - g_io_channel_unref(io); -} - static gboolean connect_event(GIOChannel *chan, GIOCondition cond, gpointer data) { - struct session_data *session; struct sockaddr_l2 addr; socklen_t addrlen; bdaddr_t src, dst; @@ -268,44 +168,14 @@ static gboolean connect_event(GIOChannel *chan, GIOCondition cond, gpointer data debug("Incoming connection on PSM %d", psm); - session = find_session(&src, &dst); - if (session) { - if (psm == 19) { - session->intr_sk = nsk; - if (authorize_device(session) < 0) { - error("Authorization request failed"); - sessions = g_slist_remove(sessions, session); - - close(session->intr_sk); - close(session->ctrl_sk); - - g_free(session); - - return TRUE; - } - debug("Waiting authorization ..."); - } else { - error("Control channel already established"); - close(nsk); - } - } else { - if (psm == 17) { - session = g_new0(struct session_data, 1); - - bacpy(&session->src, &src); - bacpy(&session->dst, &dst); - session->ctrl_sk = nsk; - session->intr_sk = -1; - - sessions = g_slist_append(sessions, session); - - create_watch(nsk, session); - } else { - error("No control channel available"); - close(nsk); - } + if (input_device_set_channel(&src, &dst, psm, nsk) < 0) { + input_device_close_channels(&src, &dst); + return TRUE; } + if ((psm == L2CAP_PSM_HIDP_INTR) && (authorize_device(&src, &dst) < 0)) + input_device_close_channels(&src, &dst); + return TRUE; } @@ -347,14 +217,14 @@ static GIOChannel *intr_io = NULL; int server_start(DBusConnection *conn) { - ctrl_io = setup_l2cap(17); + ctrl_io = setup_l2cap(L2CAP_PSM_HIDP_CTRL); if (!ctrl_io) { error("Failed to listen on control channel"); return -1; } g_io_channel_set_close_on_unref(ctrl_io, TRUE); - intr_io = setup_l2cap(19); + intr_io = setup_l2cap(L2CAP_PSM_HIDP_INTR); if (!intr_io) { error("Failed to listen on interrupt channel"); g_io_channel_unref(ctrl_io); |