diff options
| -rw-r--r-- | input/server.c | 192 |
1 files changed, 181 insertions, 11 deletions
diff --git a/input/server.c b/input/server.c index 0725d0de..4f553d7e 100644 --- a/input/server.c +++ b/input/server.c @@ -25,50 +25,220 @@ #include <config.h> #endif +#include <errno.h> #include <unistd.h> +#include <stdlib.h> +#include <sys/ioctl.h> #include <sys/socket.h> #include <bluetooth/bluetooth.h> #include <bluetooth/l2cap.h> +#include <bluetooth/hidp.h> #include <glib.h> #include "logging.h" +#include "textfile.h" #include "server.h" +struct session_data { + bdaddr_t src; + bdaddr_t dst; + int ctrl_sk; + int intr_sk; +}; + +static GSList *sessions = 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_HUP | G_IO_ERR)) return FALSE; - debug("Incoming data session"); + return TRUE; +} + +static int get_stored_device_info(const bdaddr_t *src, const bdaddr_t *dst, + struct hidp_connadd_req *req) +{ + char filename[PATH_MAX + 1], addr[18], tmp[3], *str, *desc; + unsigned int vendor, product, version, subclass, country, parser, pos; + int i; + + desc = malloc(4096); + if (!desc) + return -ENOMEM; + + memset(desc, 0, 4096); + + ba2str(src, addr); + create_name(filename, PATH_MAX, STORAGEDIR, addr, "hidd"); + + ba2str(dst, addr); + str = textfile_get(filename, addr); + if (!str) { + free(desc); + return -EIO; + } + + sscanf(str, "%04X:%04X:%04X %02X %02X %04X %4095s %08X %n", + &vendor, &product, &version, &subclass, &country, + &parser, desc, &req->flags, &pos); + + free(str); + + req->vendor = vendor; + req->product = product; + req->version = version; + req->subclass = subclass; + req->country = country; + req->parser = parser; + + snprintf(req->name, 128, str + pos); - return FALSE; + req->rd_size = strlen(desc) / 2; + req->rd_data = malloc(req->rd_size); + if (!req->rd_data) + return -ENOMEM; + + memset(tmp, 0, sizeof(tmp)); + for (i = 0; i < req->rd_size; i++) { + memcpy(tmp, desc + (i * 2), 2); + req->rd_data[i] = (uint8_t) strtol(tmp, NULL, 16); + } + + return 0; } -static gboolean connect_event(GIOChannel *chan, GIOCondition cond, gpointer data) +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; + } + + info("New input device %s (%s)", addr, req.name); + + 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 create_watch(int sk, struct session_data *session) { GIOChannel *io; + + io = g_io_channel_unix_new(sk); + g_io_channel_set_close_on_unref(io, TRUE); + + g_io_add_watch(io, G_IO_IN | G_IO_HUP | G_IO_ERR, + 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 optlen; + socklen_t addrlen; + bdaddr_t src, dst; + unsigned char psm; int sk, nsk; sk = g_io_channel_unix_get_fd(chan); memset(&addr, 0, sizeof(addr)); - optlen = sizeof(addr); + addrlen = sizeof(addr); - nsk = accept(sk, (struct sockaddr *) &addr, &optlen); + nsk = accept(sk, (struct sockaddr *) &addr, &addrlen); if (nsk < 0) return TRUE; - io = g_io_channel_unix_new(nsk); - g_io_channel_set_close_on_unref(io, TRUE); + bacpy(&dst, &addr.l2_bdaddr); + psm = btohs(addr.l2_psm); - g_io_add_watch(io, G_IO_IN | G_IO_HUP | G_IO_ERR, - session_event, NULL); + memset(&addr, 0, sizeof(addr)); + addrlen = sizeof(addr); - g_io_channel_unref(io); + if (getsockname(nsk, (struct sockaddr *) &addr, &addrlen) < 0) { + close(nsk); + return TRUE; + } + + bacpy(&src, &addr.l2_bdaddr); + + debug("Incoming connection on PSM %d", psm); + + session = find_session(&src, &dst); + if (session) { + if (psm == 19) { + session->intr_sk = nsk; + create_device(session); + } 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); + } + } return TRUE; } |