diff --git a/usr.sbin/bluetooth/bthidd/client.c b/usr.sbin/bluetooth/bthidd/client.c index cc611bdd3b7e..252248d2e8d8 100644 --- a/usr.sbin/bluetooth/bthidd/client.c +++ b/usr.sbin/bluetooth/bthidd/client.c @@ -1,244 +1,244 @@ /* * client.c * * Copyright (c) 2004 Maksim Yevmenkin * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * SUCH DAMAGE. * * $Id: client.c,v 1.6 2004/02/26 21:57:55 max Exp $ * $FreeBSD$ */ #include #include #include #include #include #include #include #include #include #include #include #include "bthidd.h" #include "bthid_config.h" static int client_socket(bdaddr_p bdaddr, int psm); /* * Get next config entry and create outbound connection (if required) * * XXX Do only one device at a time. At least one of my devices (3COM * Bluetooth PCCARD) rejects Create_Connection command if another * Create_Connection command is still pending. Weird... */ static int connect_in_progress = 0; int client_rescan(bthid_server_p srv) { static hid_device_p d = NULL; bthid_session_p s = NULL; assert(srv != NULL); if (connect_in_progress) return (0); /* another connect is still pending */ d = get_next_hid_device(d); if (d == NULL) return (0); /* XXX should not happen? empty config? */ if ((s = session_by_bdaddr(srv, &d->bdaddr)) != NULL) return (0); /* session already active */ if (!d->new_device) { if (d->reconnect_initiate) return (0); /* device will initiate reconnect */ } syslog(LOG_NOTICE, "Opening outbound session for %s " \ "(new_device=%d, reconnect_initiate=%d)", bt_ntoa(&d->bdaddr, NULL), d->new_device, d->reconnect_initiate); if ((s = session_open(srv, &d->bdaddr)) == NULL) { syslog(LOG_CRIT, "Could not open outbound session for %s. " \ "Not enough memory", bt_ntoa(&d->bdaddr, NULL)); return (-1); } /* Open control channel */ s->ctrl = client_socket(&s->bdaddr, d->control_psm); if (s->ctrl < 0) { syslog(LOG_ERR, "Could not open control channel to %s. %s (%d)", bt_ntoa(&s->bdaddr, NULL), strerror(errno), errno); session_close(s); return (-1); } s->state = W4CTRL; FD_SET(s->ctrl, &srv->wfdset); if (s->ctrl > srv->maxfd) srv->maxfd = s->ctrl; connect_in_progress = 1; return (0); } /* * Process connect on the socket */ int client_connect(bthid_server_p srv, int fd) { bthid_session_p s = NULL; hid_device_p d = NULL; int error, len; assert(srv != NULL); assert(fd >= 0); s = session_by_fd(srv, fd); assert(s != NULL); d = get_hid_device(&s->bdaddr); assert(d != NULL); error = 0; len = sizeof(error); if (getsockopt(fd, SOL_SOCKET, SO_ERROR, &error, &len) < 0) { syslog(LOG_ERR, "Could not get socket error for %s. %s (%d)", bt_ntoa(&s->bdaddr, NULL), strerror(errno), errno); session_close(s); connect_in_progress = 0; return (-1); } if (error != 0) { syslog(LOG_ERR, "Could not connect to %s. %s (%d)", bt_ntoa(&s->bdaddr, NULL), strerror(error), error); session_close(s); connect_in_progress = 0; return (0); } switch (s->state) { case W4CTRL: /* Control channel is open */ assert(s->ctrl == fd); assert(s->intr == -1); /* Open interrupt channel */ s->intr = client_socket(&s->bdaddr, d->interrupt_psm); if (s->intr < 0) { syslog(LOG_ERR, "Could not open interrupt channel " \ "to %s. %s (%d)", bt_ntoa(&s->bdaddr, NULL), strerror(errno), errno); session_close(s); connect_in_progress = 0; return (-1); } s->state = W4INTR; FD_SET(s->intr, &srv->wfdset); if (s->intr > srv->maxfd) srv->maxfd = s->intr; d->new_device = 0; /* reset new device flag */ write_hids_file(); break; case W4INTR: /* Interrupt channel is open */ assert(s->ctrl != -1); assert(s->intr == fd); s->state = OPEN; connect_in_progress = 0; break; default: assert(0); break; } /* Move fd to from the write fd set into read fd set */ FD_CLR(fd, &srv->wfdset); FD_SET(fd, &srv->rfdset); return (0); } /* * Create bound non-blocking socket and initiate connect */ static int client_socket(bdaddr_p bdaddr, int psm) { struct sockaddr_l2cap l2addr; int s, m; s = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BLUETOOTH_PROTO_L2CAP); if (s < 0) return (-1); m = fcntl(s, F_GETFL); if (m < 0) { close(s); return (-1); } if (fcntl(s, F_SETFL, (m|O_NONBLOCK)) < 0) { close(s); return (-1); } l2addr.l2cap_len = sizeof(l2addr); l2addr.l2cap_family = AF_BLUETOOTH; memcpy(&l2addr.l2cap_bdaddr, NG_HCI_BDADDR_ANY, sizeof(l2addr.l2cap_bdaddr)); l2addr.l2cap_psm = 0; if (bind(s, (struct sockaddr *) &l2addr, sizeof(l2addr)) < 0) { close(s); return (-1); } memcpy(&l2addr.l2cap_bdaddr, bdaddr, sizeof(l2addr.l2cap_bdaddr)); - l2addr.l2cap_psm = psm; + l2addr.l2cap_psm = htole16(psm); if (connect(s, (struct sockaddr *) &l2addr, sizeof(l2addr)) < 0 && errno != EINPROGRESS) { close(s); return (-1); } return (s); } diff --git a/usr.sbin/bluetooth/bthidd/server.c b/usr.sbin/bluetooth/bthidd/server.c index ad2c82835f43..2f20715bac83 100644 --- a/usr.sbin/bluetooth/bthidd/server.c +++ b/usr.sbin/bluetooth/bthidd/server.c @@ -1,366 +1,366 @@ /* * server.c * * Copyright (c) 2004 Maksim Yevmenkin * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * SUCH DAMAGE. * * $Id: server.c,v 1.7 2004/11/17 21:59:42 max Exp $ * $FreeBSD$ */ #include #include #include #include #include #include #include #include #include #include #include #include "bthidd.h" #include "bthid_config.h" #include "kbd.h" #undef max #define max(x, y) (((x) > (y))? (x) : (y)) static int server_accept (bthid_server_p srv, int fd); static int server_process(bthid_server_p srv, int fd); /* * Initialize server */ int server_init(bthid_server_p srv) { struct sockaddr_l2cap l2addr; assert(srv != NULL); srv->ctrl = srv->intr = -1; FD_ZERO(&srv->rfdset); FD_ZERO(&srv->wfdset); LIST_INIT(&srv->sessions); /* Allocate HID keycodes buffer */ srv->keys = bit_alloc(kbd_maxkey()); if (srv->keys == NULL) { syslog(LOG_ERR, "Could not allocate HID keys buffer"); return (-1); } memset(srv->keys, 0, bitstr_size(kbd_maxkey())); /* Get wired keyboard index (if was not specified) */ if (srv->windex == -1) { srv->windex = kbd_get_index("/dev/console"); if (srv->windex < 0) { syslog(LOG_ERR, "Could not open get wired keyboard " \ "index. %s (%d)", strerror(errno), errno); free(srv->keys); return (-1); } } /* Open /dev/consolectl */ srv->cons = open("/dev/consolectl", O_RDWR); if (srv->cons < 0) { syslog(LOG_ERR, "Could not open /dev/consolectl. %s (%d)", strerror(errno), errno); return (-1); } /* Open /dev/vkbdctl */ srv->vkbd = open("/dev/vkbdctl", O_RDWR); if (srv->vkbd < 0) { syslog(LOG_ERR, "Could not open /dev/vkbdctl. %s (%d)", strerror(errno), errno); close(srv->cons); free(srv->keys); return (-1); } /* Create control socket */ srv->ctrl = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BLUETOOTH_PROTO_L2CAP); if (srv->ctrl < 0) { syslog(LOG_ERR, "Could not create control L2CAP socket. " \ "%s (%d)", strerror(errno), errno); close(srv->vkbd); close(srv->cons); free(srv->keys); return (-1); } l2addr.l2cap_len = sizeof(l2addr); l2addr.l2cap_family = AF_BLUETOOTH; memcpy(&l2addr.l2cap_bdaddr, &srv->bdaddr, sizeof(l2addr.l2cap_bdaddr)); - l2addr.l2cap_psm = 0x11; + l2addr.l2cap_psm = htole16(0x11); if (bind(srv->ctrl, (struct sockaddr *) &l2addr, sizeof(l2addr)) < 0) { syslog(LOG_ERR, "Could not bind control L2CAP socket. " \ "%s (%d)", strerror(errno), errno); close(srv->ctrl); close(srv->vkbd); close(srv->cons); free(srv->keys); return (-1); } if (listen(srv->ctrl, 10) < 0) { syslog(LOG_ERR, "Could not listen on control L2CAP socket. " \ "%s (%d)", strerror(errno), errno); close(srv->ctrl); close(srv->vkbd); close(srv->cons); free(srv->keys); return (-1); } /* Create intrrupt socket */ srv->intr = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BLUETOOTH_PROTO_L2CAP); if (srv->intr < 0) { syslog(LOG_ERR, "Could not create interrupt L2CAP socket. " \ "%s (%d)", strerror(errno), errno); close(srv->ctrl); close(srv->vkbd); close(srv->cons); free(srv->keys); return (-1); } - l2addr.l2cap_psm = 0x13; + l2addr.l2cap_psm = htole16(0x13); if (bind(srv->intr, (struct sockaddr *) &l2addr, sizeof(l2addr)) < 0) { syslog(LOG_ERR, "Could not bind interrupt L2CAP socket. " \ "%s (%d)", strerror(errno), errno); close(srv->intr); close(srv->ctrl); close(srv->vkbd); close(srv->cons); free(srv->keys); return (-1); } if (listen(srv->intr, 10) < 0) { syslog(LOG_ERR, "Could not listen on interrupt L2CAP socket. "\ "%s (%d)", strerror(errno), errno); close(srv->intr); close(srv->ctrl); close(srv->vkbd); close(srv->cons); free(srv->keys); return (-1); } FD_SET(srv->ctrl, &srv->rfdset); FD_SET(srv->intr, &srv->rfdset); srv->maxfd = max(srv->ctrl, srv->intr); return (0); } /* * Shutdown server */ void server_shutdown(bthid_server_p srv) { assert(srv != NULL); close(srv->cons); close(srv->vkbd); close(srv->ctrl); close(srv->intr); while (!LIST_EMPTY(&srv->sessions)) session_close(LIST_FIRST(&srv->sessions)); free(srv->keys); memset(srv, 0, sizeof(*srv)); } /* * Do one server iteration */ int server_do(bthid_server_p srv) { struct timeval tv; fd_set rfdset, wfdset; int n, fd; assert(srv != NULL); tv.tv_sec = 1; tv.tv_usec = 0; /* Copy cached version of the fd sets and call select */ memcpy(&rfdset, &srv->rfdset, sizeof(rfdset)); memcpy(&wfdset, &srv->wfdset, sizeof(wfdset)); n = select(srv->maxfd + 1, &rfdset, &wfdset, NULL, &tv); if (n < 0) { if (errno == EINTR) return (0); syslog(LOG_ERR, "Could not select(%d, %p, %p). %s (%d)", srv->maxfd + 1, &rfdset, &wfdset, strerror(errno), errno); return (-1); } /* Process descriptors (if any) */ for (fd = 0; fd < srv->maxfd + 1 && n > 0; fd ++) { if (FD_ISSET(fd, &rfdset)) { n --; if (fd == srv->ctrl || fd == srv->intr) server_accept(srv, fd); else server_process(srv, fd); } else if (FD_ISSET(fd, &wfdset)) { n --; client_connect(srv, fd); } } return (0); } /* * Accept new connection */ static int server_accept(bthid_server_p srv, int fd) { bthid_session_p s = NULL; hid_device_p d = NULL; struct sockaddr_l2cap l2addr; int len, new_fd; len = sizeof(l2addr); if ((new_fd = accept(fd, (struct sockaddr *) &l2addr, &len)) < 0) { syslog(LOG_ERR, "Could not accept %s connection. %s (%d)", (fd == srv->ctrl)? "control" : "interrupt", strerror(errno), errno); return (-1); } /* Check if we have session for the device */ if ((s = session_by_bdaddr(srv, &l2addr.l2cap_bdaddr)) == NULL) { /* Is device configured? */ if ((d = get_hid_device(&l2addr.l2cap_bdaddr)) == NULL) { syslog(LOG_ERR, "Rejecting %s connection from %s. " \ "Device not configured", (fd == srv->ctrl)? "control" : "interrupt", bt_ntoa(&l2addr.l2cap_bdaddr, NULL)); close(new_fd); return (-1); } d->new_device = 0; /* reset new device flag */ write_hids_file(); /* Create new inbound session */ if ((s = session_open(srv, &l2addr.l2cap_bdaddr)) == NULL) { syslog(LOG_CRIT, "Could not open inbound session " \ "for %s. Not enough memory", bt_ntoa(&l2addr.l2cap_bdaddr, NULL)); close(new_fd); return (-1); } } /* Update descriptors */ if (fd == srv->ctrl) { assert(s->ctrl == -1); s->ctrl = new_fd; s->state = (s->intr == -1)? W4INTR : OPEN; } else { assert(s->intr == -1); s->intr = new_fd; s->state = (s->ctrl == -1)? W4CTRL : OPEN; } FD_SET(new_fd, &srv->rfdset); if (new_fd > srv->maxfd) srv->maxfd = new_fd; syslog(LOG_NOTICE, "Accepted %s connection from %s", (fd == srv->ctrl)? "control" : "interrupt", bt_ntoa(&l2addr.l2cap_bdaddr, NULL)); return (0); } /* * Process data on the connection */ static int server_process(bthid_server_p srv, int fd) { bthid_session_p s = session_by_fd(srv, fd); char data[1024]; int len; if (s == NULL) return (0); /* can happen on device disconnect */ do { len = read(fd, data, sizeof(data)); } while (len < 0 && errno == EINTR); if (len < 0) { syslog(LOG_ERR, "Could not read data from %s (%s). %s (%d)", bt_ntoa(&s->bdaddr, NULL), (fd == s->ctrl)? "control" : "interrupt", strerror(errno), errno); session_close(s); return (0); } if (len == 0) { syslog(LOG_NOTICE, "Remote device %s has closed %s connection", bt_ntoa(&s->bdaddr, NULL), (fd == s->ctrl)? "control" : "interrupt"); session_close(s); return (0); } if (fd == s->ctrl) hid_control(s, data, len); else hid_interrupt(s, data, len); return (0); } diff --git a/usr.sbin/bluetooth/sdpd/server.c b/usr.sbin/bluetooth/sdpd/server.c index d07b54917fe6..9528c623b0b3 100644 --- a/usr.sbin/bluetooth/sdpd/server.c +++ b/usr.sbin/bluetooth/sdpd/server.c @@ -1,585 +1,585 @@ /* * server.c * * Copyright (c) 2004 Maksim Yevmenkin * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * SUCH DAMAGE. * * $Id: server.c,v 1.6 2004/01/13 01:54:39 max Exp $ * $FreeBSD$ */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "log.h" #include "profile.h" #include "provider.h" #include "server.h" static void server_accept_client (server_p srv, int32_t fd); static int32_t server_process_request (server_p srv, int32_t fd); static int32_t server_send_error_response (server_p srv, int32_t fd, uint16_t error); static void server_close_fd (server_p srv, int32_t fd); /* * Initialize server */ int32_t server_init(server_p srv, char const *control) { struct sockaddr_un un; struct sockaddr_l2cap l2; int32_t unsock, l2sock, size; uint16_t imtu; assert(srv != NULL); assert(control != NULL); memset(srv, 0, sizeof(srv)); /* Open control socket */ if (unlink(control) < 0 && errno != ENOENT) { log_crit("Could not unlink(%s). %s (%d)", control, strerror(errno), errno); return (-1); } unsock = socket(PF_LOCAL, SOCK_STREAM, 0); if (unsock < 0) { log_crit("Could not create control socket. %s (%d)", strerror(errno), errno); return (-1); } memset(&un, 0, sizeof(un)); un.sun_len = sizeof(un); un.sun_family = AF_LOCAL; strlcpy(un.sun_path, control, sizeof(un.sun_path)); if (bind(unsock, (struct sockaddr *) &un, sizeof(un)) < 0) { log_crit("Could not bind control socket. %s (%d)", strerror(errno), errno); close(unsock); return (-1); } if (chmod(control, S_IRUSR|S_IWUSR|S_IRGRP|S_IWGRP|S_IROTH|S_IWOTH) < 0) { log_crit("Could not change permissions on control socket. " \ "%s (%d)", strerror(errno), errno); close(unsock); return (-1); } if (listen(unsock, 10) < 0) { log_crit("Could not listen on control socket. %s (%d)", strerror(errno), errno); close(unsock); return (-1); } /* Open L2CAP socket */ l2sock = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BLUETOOTH_PROTO_L2CAP); if (l2sock < 0) { log_crit("Could not create L2CAP socket. %s (%d)", strerror(errno), errno); close(unsock); return (-1); } size = sizeof(imtu); if (getsockopt(l2sock, SOL_L2CAP, SO_L2CAP_IMTU, &imtu, &size) < 0) { log_crit("Could not get L2CAP IMTU. %s (%d)", strerror(errno), errno); close(unsock); close(l2sock); return (-1); } memset(&l2, 0, sizeof(l2)); l2.l2cap_len = sizeof(l2); l2.l2cap_family = AF_BLUETOOTH; memcpy(&l2.l2cap_bdaddr, NG_HCI_BDADDR_ANY, sizeof(l2.l2cap_bdaddr)); - l2.l2cap_psm = NG_L2CAP_PSM_SDP; + l2.l2cap_psm = htole16(NG_L2CAP_PSM_SDP); if (bind(l2sock, (struct sockaddr *) &l2, sizeof(l2)) < 0) { log_crit("Could not bind L2CAP socket. %s (%d)", strerror(errno), errno); close(unsock); close(l2sock); return (-1); } if (listen(l2sock, 10) < 0) { log_crit("Could not listen on L2CAP socket. %s (%d)", strerror(errno), errno); close(unsock); close(l2sock); return (-1); } /* Allocate incoming buffer */ srv->imtu = (imtu > SDP_LOCAL_MTU)? imtu : SDP_LOCAL_MTU; srv->req = (uint8_t *) calloc(srv->imtu, sizeof(srv->req[0])); if (srv->req == NULL) { log_crit("Could not allocate request buffer"); close(unsock); close(l2sock); return (-1); } /* Allocate memory for descriptor index */ srv->fdidx = (fd_idx_p) calloc(FD_SETSIZE, sizeof(srv->fdidx[0])); if (srv->fdidx == NULL) { log_crit("Could not allocate fd index"); free(srv->req); close(unsock); close(l2sock); return (-1); } /* Register Service Discovery profile (attach it to control socket) */ if (provider_register_sd(unsock) < 0) { log_crit("Could not register Service Discovery profile"); free(srv->fdidx); free(srv->req); close(unsock); close(l2sock); return (-1); } /* * If we got here then everything is fine. Add both control sockets * to the index. */ FD_ZERO(&srv->fdset); srv->maxfd = (unsock > l2sock)? unsock : l2sock; FD_SET(unsock, &srv->fdset); srv->fdidx[unsock].valid = 1; srv->fdidx[unsock].server = 1; srv->fdidx[unsock].control = 1; srv->fdidx[unsock].priv = 0; srv->fdidx[unsock].rsp_cs = 0; srv->fdidx[unsock].rsp_size = 0; srv->fdidx[unsock].rsp_limit = 0; srv->fdidx[unsock].omtu = SDP_LOCAL_MTU; srv->fdidx[unsock].rsp = NULL; FD_SET(l2sock, &srv->fdset); srv->fdidx[l2sock].valid = 1; srv->fdidx[l2sock].server = 1; srv->fdidx[l2sock].control = 0; srv->fdidx[l2sock].priv = 0; srv->fdidx[l2sock].rsp_cs = 0; srv->fdidx[l2sock].rsp_size = 0; srv->fdidx[l2sock].rsp_limit = 0; srv->fdidx[l2sock].omtu = 0; /* unknown */ srv->fdidx[l2sock].rsp = NULL; return (0); } /* * Shutdown server */ void server_shutdown(server_p srv) { int fd; assert(srv != NULL); for (fd = 0; fd < srv->maxfd + 1; fd ++) if (srv->fdidx[fd].valid) server_close_fd(srv, fd); free(srv->req); free(srv->fdidx); memset(srv, 0, sizeof(*srv)); } /* * Do one server iteration */ int32_t server_do(server_p srv) { fd_set fdset; int32_t n, fd; assert(srv != NULL); /* Copy cached version of the fd set and call select */ memcpy(&fdset, &srv->fdset, sizeof(fdset)); n = select(srv->maxfd + 1, &fdset, NULL, NULL, NULL); if (n < 0) { if (errno == EINTR) return (0); log_err("Could not select(%d, %p). %s (%d)", srv->maxfd + 1, &fdset, strerror(errno), errno); return (-1); } /* Process descriptors */ for (fd = 0; fd < srv->maxfd + 1 && n > 0; fd ++) { if (!FD_ISSET(fd, &fdset)) continue; assert(srv->fdidx[fd].valid); n --; if (srv->fdidx[fd].server) server_accept_client(srv, fd); else if (server_process_request(srv, fd) != 0) server_close_fd(srv, fd); } return (0); } /* * Accept new client connection and register it with index */ static void server_accept_client(server_p srv, int32_t fd) { uint8_t *rsp = NULL; int32_t cfd, size, priv; uint16_t omtu; do { cfd = accept(fd, NULL, NULL); } while (cfd < 0 && errno == EINTR); if (cfd < 0) { log_err("Could not accept connection on %s socket. %s (%d)", srv->fdidx[fd].control? "control" : "L2CAP", strerror(errno), errno); return; } assert(!FD_ISSET(cfd, &srv->fdset)); assert(!srv->fdidx[cfd].valid); priv = 0; if (!srv->fdidx[fd].control) { /* Get local BD_ADDR */ size = sizeof(srv->req_sa); if (getsockname(cfd,(struct sockaddr*)&srv->req_sa,&size) < 0) { log_err("Could not get local BD_ADDR. %s (%d)", strerror(errno), errno); close(cfd); return; } /* Get outgoing MTU */ size = sizeof(omtu); if (getsockopt(cfd,SOL_L2CAP,SO_L2CAP_OMTU,&omtu,&size) < 0) { log_err("Could not get L2CAP OMTU. %s (%d)", strerror(errno), errno); close(cfd); return; } /* * The maximum size of the L2CAP packet is 65536 bytes. * The minimum L2CAP MTU is 43 bytes. That means we need * 65536 / 43 = ~1524 chunks to transfer maximum packet * size with minimum MTU. The "rsp_cs" field in fd_idx_t * is 11 bit wide that gives us upto 2048 chunks. */ if (omtu < NG_L2CAP_MTU_MINIMUM) { log_err("L2CAP OMTU is too small (%d bytes)", omtu); close(cfd); return; } } else { struct xucred cr; struct passwd *pw; /* Get peer's credentials */ memset(&cr, 0, sizeof(cr)); size = sizeof(cr); if (getsockopt(cfd, 0, LOCAL_PEERCRED, &cr, &size) < 0) { log_err("Could not get peer's credentials. %s (%d)", strerror(errno), errno); close(cfd); return; } /* Check credentials */ pw = getpwuid(cr.cr_uid); if (pw != NULL) priv = (strcmp(pw->pw_name, "root") == 0); else log_warning("Could not verify credentials for uid %d", cr.cr_uid); memcpy(&srv->req_sa.l2cap_bdaddr, NG_HCI_BDADDR_ANY, sizeof(srv->req_sa.l2cap_bdaddr)); omtu = srv->fdidx[fd].omtu; } /* * Allocate buffer. This is an overkill, but we can not know how * big our reply is going to be. */ rsp = (uint8_t *) calloc(NG_L2CAP_MTU_MAXIMUM, sizeof(rsp[0])); if (rsp == NULL) { log_crit("Could not allocate response buffer"); close(cfd); return; } /* Add client descriptor to the index */ FD_SET(cfd, &srv->fdset); if (srv->maxfd < cfd) srv->maxfd = cfd; srv->fdidx[cfd].valid = 1; srv->fdidx[cfd].server = 0; srv->fdidx[cfd].control = srv->fdidx[fd].control; srv->fdidx[cfd].priv = priv; srv->fdidx[cfd].rsp_cs = 0; srv->fdidx[cfd].rsp_size = 0; srv->fdidx[cfd].rsp_limit = 0; srv->fdidx[cfd].omtu = omtu; srv->fdidx[cfd].rsp = rsp; } /* * Process request from the client */ static int32_t server_process_request(server_p srv, int32_t fd) { sdp_pdu_p pdu = (sdp_pdu_p) srv->req; int32_t len, error; assert(srv->imtu > 0); assert(srv->req != NULL); assert(FD_ISSET(fd, &srv->fdset)); assert(srv->fdidx[fd].valid); assert(!srv->fdidx[fd].server); assert(srv->fdidx[fd].rsp != NULL); assert(srv->fdidx[fd].omtu >= NG_L2CAP_MTU_MINIMUM); do { len = read(fd, srv->req, srv->imtu); } while (len < 0 && errno == EINTR); if (len < 0) { log_err("Could not receive SDP request from %s socket. %s (%d)", srv->fdidx[fd].control? "control" : "L2CAP", strerror(errno), errno); return (-1); } if (len == 0) { log_info("Client on %s socket has disconnected", srv->fdidx[fd].control? "control" : "L2CAP"); return (-1); } if (sizeof(*pdu) + (pdu->len = ntohs(pdu->len)) == len) { switch (pdu->pid) { case SDP_PDU_SERVICE_SEARCH_REQUEST: error = server_prepare_service_search_response(srv, fd); break; case SDP_PDU_SERVICE_ATTRIBUTE_REQUEST: error = server_prepare_service_attribute_response(srv, fd); break; case SDP_PDU_SERVICE_SEARCH_ATTRIBUTE_REQUEST: error = server_prepare_service_search_attribute_response(srv, fd); break; case SDP_PDU_SERVICE_REGISTER_REQUEST: error = server_prepare_service_register_response(srv, fd); break; case SDP_PDU_SERVICE_UNREGISTER_REQUEST: error = server_prepare_service_unregister_response(srv, fd); break; case SDP_PDU_SERVICE_CHANGE_REQUEST: error = server_prepare_service_change_response(srv, fd); break; default: error = SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX; break; } } else error = SDP_ERROR_CODE_INVALID_PDU_SIZE; if (error == 0) { switch (pdu->pid) { case SDP_PDU_SERVICE_SEARCH_REQUEST: error = server_send_service_search_response(srv, fd); break; case SDP_PDU_SERVICE_ATTRIBUTE_REQUEST: error = server_send_service_attribute_response(srv, fd); break; case SDP_PDU_SERVICE_SEARCH_ATTRIBUTE_REQUEST: error = server_send_service_search_attribute_response(srv, fd); break; case SDP_PDU_SERVICE_REGISTER_REQUEST: error = server_send_service_register_response(srv, fd); break; case SDP_PDU_SERVICE_UNREGISTER_REQUEST: error = server_send_service_unregister_response(srv, fd); break; case SDP_PDU_SERVICE_CHANGE_REQUEST: error = server_send_service_change_response(srv, fd); default: error = SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX; break; } if (error != 0) log_err("Could not send SDP response to %s socket, " \ "pdu->pid=%d, pdu->tid=%d, error=%d", srv->fdidx[fd].control? "control" : "L2CAP", pdu->pid, ntohs(pdu->tid), error); } else { log_err("Could not process SDP request from %s socket, " \ "pdu->pid=%d, pdu->tid=%d, pdu->len=%d, len=%d, " \ "error=%d", srv->fdidx[fd].control? "control" : "L2CAP", pdu->pid, ntohs(pdu->tid), pdu->len, len, error); error = server_send_error_response(srv, fd, error); if (error != 0) log_err("Could not send SDP error response to %s " \ "socket, pdu->pid=%d, pdu->tid=%d, error=%d", srv->fdidx[fd].control? "control" : "L2CAP", pdu->pid, ntohs(pdu->tid), error); } /* On error forget response (if any) */ if (error != 0) { srv->fdidx[fd].rsp_cs = 0; srv->fdidx[fd].rsp_size = 0; srv->fdidx[fd].rsp_limit = 0; } return (error); } /* * Send SDP_Error_Response PDU */ static int32_t server_send_error_response(server_p srv, int32_t fd, uint16_t error) { int32_t size; struct { sdp_pdu_t pdu; uint16_t error; } __attribute__ ((packed)) rsp; /* Prepare and send SDP error response */ rsp.pdu.pid = SDP_PDU_ERROR_RESPONSE; rsp.pdu.tid = ((sdp_pdu_p)(srv->req))->tid; rsp.pdu.len = htons(sizeof(rsp.error)); rsp.error = htons(error); do { size = write(fd, &rsp, sizeof(rsp)); } while (size < 0 && errno == EINTR); return ((size < 0)? errno : 0); } /* * Close descriptor and remove it from index */ static void server_close_fd(server_p srv, int32_t fd) { provider_p provider = NULL, provider_next = NULL; assert(FD_ISSET(fd, &srv->fdset)); assert(srv->fdidx[fd].valid); close(fd); FD_CLR(fd, &srv->fdset); if (fd == srv->maxfd) srv->maxfd --; if (srv->fdidx[fd].rsp != NULL) free(srv->fdidx[fd].rsp); memset(&srv->fdidx[fd], 0, sizeof(srv->fdidx[fd])); for (provider = provider_get_first(); provider != NULL; provider = provider_next) { provider_next = provider_get_next(provider); if (provider->fd == fd) provider_unregister(provider); } }