diff options
| -rw-r--r-- | hidd/main.c | 205 | ||||
| -rw-r--r-- | hidd/sdp.c | 94 |
2 files changed, 255 insertions, 44 deletions
diff --git a/hidd/main.c b/hidd/main.c index 46b25ec4..aa2a7b1f 100644 --- a/hidd/main.c +++ b/hidd/main.c @@ -63,6 +63,42 @@ static void sig_term(int sig) __io_canceled = 1; } +static int l2cap_connect(bdaddr_t *src, bdaddr_t *dst, unsigned short psm) +{ + struct sockaddr_l2 addr; + struct l2cap_options opts; + int sk; + + if ((sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP)) < 0) + return -1; + + addr.l2_family = AF_BLUETOOTH; + bacpy(&addr.l2_bdaddr, src); + addr.l2_psm = 0; + + if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + close(sk); + return -1; + } + + opts.imtu = HIDP_DEFAULT_MTU; + opts.omtu = HIDP_DEFAULT_MTU; + opts.flush_to = 0xffff; + + setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, sizeof(opts)); + + addr.l2_family = AF_BLUETOOTH; + bacpy(&addr.l2_bdaddr, dst); + addr.l2_psm = htobs(psm); + + if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + close(sk); + return -1; + } + + return sk; +} + static int l2cap_listen(const bdaddr_t *bdaddr, unsigned short psm, int backlog) { struct sockaddr_l2 addr; @@ -115,7 +151,7 @@ static int l2cap_accept(int sk, bdaddr_t *bdaddr) return nsk; } -static int create(int ctl, int csk, int isk, int timeout) +static int create_device(int ctl, int csk, int isk, int timeout) { struct hidp_connadd_req req; struct sockaddr_l2 addr; @@ -140,16 +176,18 @@ static int create(int ctl, int csk, int isk, int timeout) bacpy(&dst, &addr.l2_bdaddr); - ba2str(&dst, bda); - syslog(LOG_INFO, "New HID connection from %s", bda); - memset(&req, 0, sizeof(req)); req.ctrl_sock = csk; req.intr_sock = isk; req.flags = 0; req.idle_to = timeout * 60; - get_hid_device_info(&src, &dst, &req); + err = get_hid_device_info(&src, &dst, &req); + if (err < 0) + return err; + + ba2str(&dst, bda); + syslog(LOG_INFO, "New HID device %s (%s)", bda, req.name); err = ioctl(ctl, HIDPCONNADD, &req); @@ -159,7 +197,7 @@ static int create(int ctl, int csk, int isk, int timeout) return err; } -static void run(int ctl, int csk, int isk, int timeout) +static void run_server(int ctl, int csk, int isk, int timeout) { struct pollfd p[2]; short events; @@ -185,7 +223,7 @@ static void run(int ctl, int csk, int isk, int timeout) ncsk = l2cap_accept(csk, NULL); nisk = l2cap_accept(isk, NULL); - err = create(ctl, ncsk, nisk, timeout); + err = create_device(ctl, ncsk, nisk, timeout); if (err < 0) syslog(LOG_ERR, "HID create error %d (%s)", errno, strerror(errno)); @@ -196,12 +234,87 @@ static void run(int ctl, int csk, int isk, int timeout) } } +static void do_connect(int ctl, bdaddr_t *src, bdaddr_t *dst, int timeout) +{ + int csk, isk, err; + + csk = l2cap_connect(src, dst, L2CAP_PSM_HIDP_CTRL); + if (csk < 0) { + perror("Can't create HID control channel"); + close(ctl); + exit(1); + } + + isk = l2cap_connect(src, dst, L2CAP_PSM_HIDP_INTR); + if (isk < 0) { + perror("Can't create HID interrupt channel"); + close(csk); + close(ctl); + exit(1); + } + + err = create_device(ctl, csk, isk, timeout); + if (err < 0) { + fprintf(stderr, "HID create error %d (%s)\n", + errno, strerror(errno)); + close(isk); + sleep(1); + close(csk); + close(ctl); + exit(1); + } +} + +static void do_search(int ctl, bdaddr_t *bdaddr, int timeout) +{ + inquiry_info *info = NULL; + bdaddr_t src, dst; + int i, dev_id, num_rsp, length, flags; + char addr[18]; + uint8_t class[3]; + + ba2str(bdaddr, addr); + dev_id = hci_devid(addr); + if (dev_id < 0) { + dev_id = hci_get_route(NULL); + hci_devba(dev_id, &src); + } else + bacpy(&src, bdaddr); + + length = 8; /* ~10 seconds */ + num_rsp = 0; + flags = 0; + + printf("Searching ...\n"); + + num_rsp = hci_inquiry(dev_id, length, num_rsp, NULL, &info, flags); + + for (i = 0; i < num_rsp; i++) { + memcpy(class, (info+i)->dev_class, 3); + if (class[1] == 0x25 && class[2] == 0x00) { + bacpy(&dst, &(info+i)->bdaddr); + ba2str(&dst, addr); + + printf("\tConnecting to device %s\n", addr); + do_connect(ctl, &src, &dst, timeout); + } + } + + free(info); + + if (!num_rsp) { + fprintf(stderr, "\tNo devices in range or visible\n"); + close(ctl); + exit(1); + } +} + static void usage(void) { printf("hidd - Bluetooth HID daemon\n\n"); printf("Usage:\n" - "\thidd [options]\n" + "\thidd [options] [commands]\n" "\n"); printf("Options:\n" @@ -210,6 +323,12 @@ static void usage(void) "\t-n, --nodaemon Don't fork daemon to background\n" "\t-h, --help Display help\n" "\n"); + + printf("Commands:\n" + "\t--server Start HID server\n" + "\t--search Search for HID devices\n" + "\t--connect <bdaddr> Connect remote HID device\n" + "\n"); } static struct option main_options[] = { @@ -217,21 +336,25 @@ static struct option main_options[] = { { "nodaemon", 0, 0, 'n' }, { "timeout", 1, 0, 't' }, { "device", 1, 0, 'i' }, + { "server", 0, 0, 'd' }, + { "listen", 0, 0, 'd' }, + { "search", 0, 0, 's' }, + { "connect", 1, 0, 'c' }, { 0, 0, 0, 0 } }; int main(int argc, char *argv[]) { struct sigaction sa; - bdaddr_t bdaddr; + bdaddr_t bdaddr, dev; char addr[18]; int log_option = LOG_NDELAY | LOG_PID; int opt, fd, ctl, csk, isk; - int daemon = 1, timeout = 30; + int mode = 0, daemon = 1, timeout = 30; bacpy(&bdaddr, BDADDR_ANY); - while ((opt = getopt_long(argc, argv, "+i:nt:h", main_options, NULL)) != -1) { + while ((opt = getopt_long(argc, argv, "+i:nt:dsc:h", main_options, NULL)) != -1) { switch(opt) { case 'i': if (!strncasecmp(optarg, "hci", 3)) @@ -245,6 +368,16 @@ int main(int argc, char *argv[]) case 't': timeout = atoi(optarg); break; + case 'd': + mode = 1; + break; + case 's': + mode = 2; + break; + case 'c': + str2ba(optarg, &dev); + mode = 3; + break; case 'h': usage(); exit(0); @@ -255,24 +388,42 @@ int main(int argc, char *argv[]) ba2str(&bdaddr, addr); - csk = l2cap_listen(&bdaddr, L2CAP_PSM_HIDP_CTRL, 10); - if (csk < 0) { - perror("Can't listen on HID control channel"); + ctl = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HIDP); + if (ctl < 0) { + perror("Can't open HIDP control socket"); exit(1); } - isk = l2cap_listen(&bdaddr, L2CAP_PSM_HIDP_INTR, 10); - if (isk < 0) { - perror("Can't listen on HID interrupt channel"); - close(csk); - exit(1); - } + switch (mode) { + case 1: + csk = l2cap_listen(&bdaddr, L2CAP_PSM_HIDP_CTRL, 10); + if (csk < 0) { + perror("Can't listen on HID control channel"); + close(ctl); + exit(1); + } - ctl = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HIDP); - if (ctl < 0) { - perror("Can't open HIDP control socket"); - close(csk); - close(isk); + isk = l2cap_listen(&bdaddr, L2CAP_PSM_HIDP_INTR, 10); + if (isk < 0) { + perror("Can't listen on HID interrupt channel"); + close(ctl); + close(csk); + exit(1); + } + break; + + case 2: + do_search(ctl, &bdaddr, timeout); + close(ctl); + exit(0); + + case 3: + do_connect(ctl, &bdaddr, &dev, timeout); + close(ctl); + exit(0); + + default: + usage(); exit(1); } @@ -309,13 +460,13 @@ int main(int argc, char *argv[]) sigaction(SIGCHLD, &sa, NULL); sigaction(SIGPIPE, &sa, NULL); - run(ctl, csk, isk, timeout); + run_server(ctl, csk, isk, timeout); syslog(LOG_INFO, "Exit"); - close(ctl); close(csk); close(isk); + close(ctl); return 0; } @@ -51,43 +51,103 @@ int get_hid_device_info(bdaddr_t *src, bdaddr_t *dst, struct hidp_connadd_req *req) { + uint32_t range = 0x0000ffff; sdp_session_t *s; - sdp_list_t *srch, *attrs, *rsp; + sdp_list_t *search, *attrid, *pnp_rsp, *hid_rsp; sdp_record_t *rec; - sdp_data_t *pdlist; + sdp_data_t *pdlist, *pdlist2; uuid_t svclass; - uint16_t attr; int err; s = sdp_connect(src, dst, 0); if (!s) return -1; + sdp_uuid16_create(&svclass, PNP_INFO_SVCLASS_ID); + search = sdp_list_append(NULL, &svclass); + attrid = sdp_list_append(NULL, &range); + + err = sdp_service_search_attr_req(s, search, SDP_ATTR_REQ_RANGE, attrid, &pnp_rsp); + + sdp_list_free(search, 0); + sdp_list_free(attrid, 0); + sdp_uuid16_create(&svclass, HID_SVCLASS_ID); - srch = sdp_list_append(NULL, &svclass); + search = sdp_list_append(NULL, &svclass); + attrid = sdp_list_append(NULL, &range); - attr = 0x0206; - attrs = sdp_list_append(NULL, &attr); + err = sdp_service_search_attr_req(s, search, SDP_ATTR_REQ_RANGE, attrid, &hid_rsp); - err = sdp_service_search_attr_req(s, srch, SDP_ATTR_REQ_INDIVIDUAL, attrs, &rsp); + sdp_list_free(search, 0); + sdp_list_free(attrid, 0); sdp_close(s); - if (err || !rsp) + if (err || !hid_rsp) return -1; - rec = (sdp_record_t *) rsp->data; + if (pnp_rsp) { + rec = (sdp_record_t *) pnp_rsp->data; + + pdlist = sdp_data_get(rec, 0x0201); + if (pdlist) + req->vendor = pdlist->val.uint16; + + pdlist = sdp_data_get(rec, 0x0202); + if (pdlist) + req->product = pdlist->val.uint16; + + pdlist = sdp_data_get(rec, 0x0203); + if (pdlist) + req->version = pdlist->val.uint16; + + sdp_record_free(rec); + } + + rec = (sdp_record_t *) hid_rsp->data; + + pdlist = sdp_data_get(rec, 0x0101); + pdlist2 = sdp_data_get(rec, 0x0102); + if (pdlist) { + if (pdlist2) { + if (strncmp(pdlist->val.str, pdlist2->val.str, 5)) { + strncpy(req->name, pdlist2->val.str, sizeof(req->name) - 1); + strcat(req->name, " "); + } + strncat(req->name, pdlist->val.str, + sizeof(req->name) - strlen(req->name)); + } else + strncpy(req->name, pdlist->val.str, sizeof(req->name)); + } else { + pdlist2 = sdp_data_get(rec, 0x0100); + if (pdlist2) + strncpy(req->name, pdlist2->val.str, sizeof(req->name)); + } + + pdlist = sdp_data_get(rec, 0x0201); + if (pdlist) + req->parser = pdlist->val.uint16; + else + req->parser = 0x0100; + + pdlist = sdp_data_get(rec, 0x0203); + if (pdlist) + req->country = pdlist->val.uint8; pdlist = sdp_data_get(rec, 0x0206); - pdlist = pdlist->val.dataseq; - pdlist = pdlist->val.dataseq; - pdlist = pdlist->next; - - req->rd_data = malloc(pdlist->unitSize); - if (req->rd_data) { - memcpy(req->rd_data, (unsigned char *) pdlist->val.str, pdlist->unitSize); - req->rd_size = pdlist->unitSize; + if (pdlist) { + pdlist = pdlist->val.dataseq; + pdlist = pdlist->val.dataseq; + pdlist = pdlist->next; + + req->rd_data = malloc(pdlist->unitSize); + if (req->rd_data) { + memcpy(req->rd_data, (unsigned char *) pdlist->val.str, pdlist->unitSize); + req->rd_size = pdlist->unitSize; + } } + sdp_record_free(rec); + return 0; } |