1 /* $NetBSD: server.c,v 1.1 2006/06/19 15:44:56 gdamore Exp $ */ 2 3 /*- 4 * Copyright (c) 2006 Itronix Inc. 5 * All rights reserved. 6 * 7 * Redistribution and use in source and binary forms, with or without 8 * modification, are permitted provided that the following conditions 9 * are met: 10 * 1. Redistributions of source code must retain the above copyright 11 * notice, this list of conditions and the following disclaimer. 12 * 2. Redistributions in binary form must reproduce the above copyright 13 * notice, this list of conditions and the following disclaimer in the 14 * documentation and/or other materials provided with the distribution. 15 * 3. The name of Itronix Inc. may not be used to endorse 16 * or promote products derived from this software without specific 17 * prior written permission. 18 * 19 * THIS SOFTWARE IS PROVIDED BY ITRONIX INC. ``AS IS'' AND 20 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 21 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 22 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL ITRONIX INC. BE LIABLE FOR ANY 23 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 26 * ON ANY THEORY OF LIABILITY, WHETHER IN 27 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 * POSSIBILITY OF SUCH DAMAGE. 30 */ 31 /* 32 * server.c 33 * 34 * Copyright (c) 2004 Maksim Yevmenkin <m_evmenkin@yahoo.com> 35 * All rights reserved. 36 * 37 * Redistribution and use in source and binary forms, with or without 38 * modification, are permitted provided that the following conditions 39 * are met: 40 * 1. Redistributions of source code must retain the above copyright 41 * notice, this list of conditions and the following disclaimer. 42 * 2. Redistributions in binary form must reproduce the above copyright 43 * notice, this list of conditions and the following disclaimer in the 44 * documentation and/or other materials provided with the distribution. 45 * 46 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 47 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 48 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 49 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 50 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 51 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 52 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 53 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 54 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 55 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 56 * SUCH DAMAGE. 57 * 58 * $Id: server.c,v 1.1 2006/06/19 15:44:56 gdamore Exp $ 59 * $FreeBSD: src/usr.sbin/bluetooth/sdpd/server.c,v 1.2 2005/12/06 17:56:36 emax Exp $ 60 */ 61 62 #include <sys/cdefs.h> 63 __RCSID("$NetBSD: server.c,v 1.1 2006/06/19 15:44:56 gdamore Exp $"); 64 65 #include <sys/param.h> 66 #include <sys/select.h> 67 #include <sys/stat.h> 68 #include <sys/queue.h> 69 #include <sys/ucred.h> 70 #include <sys/un.h> 71 #include <netinet/in.h> 72 #include <arpa/inet.h> 73 #include <assert.h> 74 #include <bluetooth.h> 75 #include <errno.h> 76 #include <pwd.h> 77 #include <sdp.h> 78 #include <stdio.h> 79 #include <stdlib.h> 80 #include <string.h> 81 #include <unistd.h> 82 #include "log.h" 83 #include "profile.h" 84 #include "provider.h" 85 #include "server.h" 86 87 static void server_accept_client (server_p srv, int32_t fd); 88 static int32_t server_process_request (server_p srv, int32_t fd); 89 static int32_t server_send_error_response (server_p srv, int32_t fd, 90 uint16_t error); 91 static void server_close_fd (server_p srv, int32_t fd); 92 93 /* 94 * Initialize server 95 */ 96 97 int32_t 98 server_init(server_p srv, char const *control) 99 { 100 struct sockaddr_un un; 101 struct sockaddr_bt l2; 102 socklen_t size; 103 int32_t unsock, l2sock; 104 uint16_t imtu; 105 int opt; 106 107 assert(srv != NULL); 108 assert(control != NULL); 109 110 memset(srv, 0, sizeof(srv)); 111 112 /* Open control socket */ 113 if (unlink(control) < 0 && errno != ENOENT) { 114 log_crit("Could not unlink(%s). %s (%d)", 115 control, strerror(errno), errno); 116 return (-1); 117 } 118 119 unsock = socket(PF_LOCAL, SOCK_STREAM, 0); 120 if (unsock < 0) { 121 log_crit("Could not create control socket. %s (%d)", 122 strerror(errno), errno); 123 return (-1); 124 } 125 126 opt = 1; 127 if (setsockopt(unsock, 0, LOCAL_CREDS, &opt, sizeof(opt)) < 0) 128 log_crit("Warning: No credential checks on control socket"); 129 130 memset(&un, 0, sizeof(un)); 131 un.sun_len = sizeof(un); 132 un.sun_family = AF_LOCAL; 133 strlcpy(un.sun_path, control, sizeof(un.sun_path)); 134 135 if (bind(unsock, (struct sockaddr *) &un, sizeof(un)) < 0) { 136 log_crit("Could not bind control socket. %s (%d)", 137 strerror(errno), errno); 138 close(unsock); 139 return (-1); 140 } 141 142 if (chmod(control, S_IRUSR|S_IWUSR|S_IRGRP|S_IWGRP|S_IROTH|S_IWOTH) < 0) { 143 log_crit("Could not change permissions on control socket. " \ 144 "%s (%d)", strerror(errno), errno); 145 close(unsock); 146 return (-1); 147 } 148 149 if (listen(unsock, 10) < 0) { 150 log_crit("Could not listen on control socket. %s (%d)", 151 strerror(errno), errno); 152 close(unsock); 153 return (-1); 154 } 155 156 /* Open L2CAP socket */ 157 l2sock = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP); 158 if (l2sock < 0) { 159 log_crit("Could not create L2CAP socket. %s (%d)", 160 strerror(errno), errno); 161 close(unsock); 162 return (-1); 163 } 164 165 size = sizeof(imtu); 166 if (getsockopt(l2sock, BTPROTO_L2CAP, SO_L2CAP_IMTU, &imtu, &size) < 0) { 167 log_crit("Could not get L2CAP IMTU. %s (%d)", 168 strerror(errno), errno); 169 close(unsock); 170 close(l2sock); 171 return (-1); 172 } 173 174 memset(&l2, 0, sizeof(l2)); 175 l2.bt_len = sizeof(l2); 176 l2.bt_family = AF_BLUETOOTH; 177 l2.bt_psm = L2CAP_PSM_SDP; 178 bdaddr_copy(&l2.bt_bdaddr, BDADDR_ANY); 179 180 if (bind(l2sock, (struct sockaddr *) &l2, sizeof(l2)) < 0) { 181 log_crit("Could not bind L2CAP socket. %s (%d)", 182 strerror(errno), errno); 183 close(unsock); 184 close(l2sock); 185 return (-1); 186 } 187 188 if (listen(l2sock, 10) < 0) { 189 log_crit("Could not listen on L2CAP socket. %s (%d)", 190 strerror(errno), errno); 191 close(unsock); 192 close(l2sock); 193 return (-1); 194 } 195 196 /* Allocate incoming buffer */ 197 srv->imtu = (imtu > SDP_LOCAL_MTU)? imtu : SDP_LOCAL_MTU; 198 srv->req = (uint8_t *) calloc(srv->imtu, sizeof(srv->req[0])); 199 if (srv->req == NULL) { 200 log_crit("Could not allocate request buffer"); 201 close(unsock); 202 close(l2sock); 203 return (-1); 204 } 205 206 /* Allocate memory for descriptor index */ 207 srv->fdidx = (fd_idx_p) calloc(FD_SETSIZE, sizeof(srv->fdidx[0])); 208 if (srv->fdidx == NULL) { 209 log_crit("Could not allocate fd index"); 210 free(srv->req); 211 close(unsock); 212 close(l2sock); 213 return (-1); 214 } 215 216 /* Register Service Discovery profile (attach it to control socket) */ 217 if (provider_register_sd(unsock) < 0) { 218 log_crit("Could not register Service Discovery profile"); 219 free(srv->fdidx); 220 free(srv->req); 221 close(unsock); 222 close(l2sock); 223 return (-1); 224 } 225 226 /* 227 * If we got here then everything is fine. Add both control sockets 228 * to the index. 229 */ 230 231 FD_ZERO(&srv->fdset); 232 srv->maxfd = (unsock > l2sock)? unsock : l2sock; 233 234 FD_SET(unsock, &srv->fdset); 235 srv->fdidx[unsock].valid = 1; 236 srv->fdidx[unsock].server = 1; 237 srv->fdidx[unsock].control = 1; 238 srv->fdidx[unsock].priv = 0; 239 srv->fdidx[unsock].rsp_cs = 0; 240 srv->fdidx[unsock].rsp_size = 0; 241 srv->fdidx[unsock].rsp_limit = 0; 242 srv->fdidx[unsock].omtu = SDP_LOCAL_MTU; 243 srv->fdidx[unsock].rsp = NULL; 244 245 FD_SET(l2sock, &srv->fdset); 246 srv->fdidx[l2sock].valid = 1; 247 srv->fdidx[l2sock].server = 1; 248 srv->fdidx[l2sock].control = 0; 249 srv->fdidx[l2sock].priv = 0; 250 srv->fdidx[l2sock].rsp_cs = 0; 251 srv->fdidx[l2sock].rsp_size = 0; 252 srv->fdidx[l2sock].rsp_limit = 0; 253 srv->fdidx[l2sock].omtu = 0; /* unknown */ 254 srv->fdidx[l2sock].rsp = NULL; 255 256 return (0); 257 } 258 259 /* 260 * Shutdown server 261 */ 262 263 void 264 server_shutdown(server_p srv) 265 { 266 int fd; 267 268 assert(srv != NULL); 269 270 for (fd = 0; fd < srv->maxfd + 1; fd ++) 271 if (srv->fdidx[fd].valid) 272 server_close_fd(srv, fd); 273 274 free(srv->req); 275 free(srv->fdidx); 276 277 memset(srv, 0, sizeof(*srv)); 278 } 279 280 /* 281 * Do one server iteration 282 */ 283 284 int32_t 285 server_do(server_p srv) 286 { 287 fd_set fdset; 288 int32_t n, fd; 289 290 assert(srv != NULL); 291 292 /* Copy cached version of the fd set and call select */ 293 memcpy(&fdset, &srv->fdset, sizeof(fdset)); 294 n = select(srv->maxfd + 1, &fdset, NULL, NULL, NULL); 295 if (n < 0) { 296 if (errno == EINTR) 297 return (0); 298 299 log_err("Could not select(%d, %p). %s (%d)", 300 srv->maxfd + 1, &fdset, strerror(errno), errno); 301 302 return (-1); 303 } 304 305 /* Process descriptors */ 306 for (fd = 0; fd < srv->maxfd + 1 && n > 0; fd ++) { 307 if (!FD_ISSET(fd, &fdset)) 308 continue; 309 310 assert(srv->fdidx[fd].valid); 311 n --; 312 313 if (srv->fdidx[fd].server) 314 server_accept_client(srv, fd); 315 else if (server_process_request(srv, fd) != 0) 316 server_close_fd(srv, fd); 317 } 318 319 return (0); 320 321 } 322 323 /* 324 * Accept new client connection and register it with index 325 */ 326 327 static void 328 server_accept_client(server_p srv, int32_t fd) 329 { 330 uint8_t *rsp = NULL; 331 socklen_t size; 332 int32_t cfd; 333 uint16_t omtu; 334 335 do { 336 cfd = accept(fd, NULL, NULL); 337 } while (cfd < 0 && errno == EINTR); 338 339 if (cfd < 0) { 340 log_err("Could not accept connection on %s socket. %s (%d)", 341 srv->fdidx[fd].control? "control" : "L2CAP", 342 strerror(errno), errno); 343 return; 344 } 345 346 assert(!FD_ISSET(cfd, &srv->fdset)); 347 assert(!srv->fdidx[cfd].valid); 348 349 if (!srv->fdidx[fd].control) { 350 /* Get local BD_ADDR */ 351 size = sizeof(srv->req_sa); 352 if (getsockname(cfd,(struct sockaddr*)&srv->req_sa, &size) < 0) { 353 log_err("Could not get local BD_ADDR. %s (%d)", 354 strerror(errno), errno); 355 close(cfd); 356 return; 357 } 358 359 /* Get outgoing MTU */ 360 size = sizeof(omtu); 361 if (getsockopt(cfd, BTPROTO_L2CAP, SO_L2CAP_OMTU, &omtu, &size) < 0) { 362 log_err("Could not get L2CAP OMTU. %s (%d)", 363 strerror(errno), errno); 364 close(cfd); 365 return; 366 } 367 368 /* 369 * The maximum size of the L2CAP packet is 65536 bytes. 370 * The minimum L2CAP MTU is 43 bytes. That means we need 371 * 65536 / 43 = ~1524 chunks to transfer maximum packet 372 * size with minimum MTU. The "rsp_cs" field in fd_idx_t 373 * is 11 bit wide that gives us upto 2048 chunks. 374 */ 375 376 if (omtu < L2CAP_MTU_MINIMUM) { 377 log_err("L2CAP OMTU is too small (%d bytes)", omtu); 378 close(cfd); 379 return; 380 } 381 } else { 382 bdaddr_copy(&srv->req_sa.bt_bdaddr, BDADDR_ANY); 383 omtu = srv->fdidx[fd].omtu; 384 } 385 386 /* 387 * Allocate buffer. This is an overkill, but we can not know how 388 * big our reply is going to be. 389 */ 390 391 rsp = (uint8_t *) calloc(L2CAP_MTU_MAXIMUM, sizeof(rsp[0])); 392 if (rsp == NULL) { 393 log_crit("Could not allocate response buffer"); 394 close(cfd); 395 return; 396 } 397 398 /* Add client descriptor to the index */ 399 FD_SET(cfd, &srv->fdset); 400 if (srv->maxfd < cfd) 401 srv->maxfd = cfd; 402 srv->fdidx[cfd].valid = 1; 403 srv->fdidx[cfd].server = 0; 404 srv->fdidx[cfd].control = srv->fdidx[fd].control; 405 srv->fdidx[cfd].priv = 0; 406 srv->fdidx[cfd].rsp_cs = 0; 407 srv->fdidx[cfd].rsp_size = 0; 408 srv->fdidx[cfd].rsp_limit = 0; 409 srv->fdidx[cfd].omtu = omtu; 410 srv->fdidx[cfd].rsp = rsp; 411 } 412 413 /* 414 * Process request from the client 415 */ 416 417 static int32_t 418 server_process_request(server_p srv, int32_t fd) 419 { 420 uint8_t ctl[128]; 421 sdp_pdu_p pdu = (sdp_pdu_p) srv->req; 422 struct msghdr msg; 423 struct iovec iov; 424 int32_t len, error; 425 struct cmsghdr *cmsg; 426 struct sockcred *cred; 427 428 assert(srv->imtu > 0); 429 assert(srv->req != NULL); 430 assert(FD_ISSET(fd, &srv->fdset)); 431 assert(srv->fdidx[fd].valid); 432 assert(!srv->fdidx[fd].server); 433 assert(srv->fdidx[fd].rsp != NULL); 434 assert(srv->fdidx[fd].omtu >= L2CAP_MTU_MINIMUM); 435 436 iov.iov_base = srv->req; 437 iov.iov_len = srv->imtu; 438 439 msg.msg_name = NULL; 440 msg.msg_namelen = 0; 441 msg.msg_iov = &iov; 442 msg.msg_iovlen = 1; 443 msg.msg_control = ctl; 444 msg.msg_controllen = sizeof(ctl); 445 msg.msg_flags = 0; 446 447 do { 448 len = recvmsg(fd, &msg, 0); 449 } while (len < 0 && errno == EINTR); 450 451 if (len < 0) { 452 log_err("Could not receive SDP request from %s socket. %s (%d)", 453 srv->fdidx[fd].control? "control" : "L2CAP", 454 strerror(errno), errno); 455 return (-1); 456 } 457 if (len == 0) { 458 log_info("Client on %s socket has disconnected", 459 srv->fdidx[fd].control? "control" : "L2CAP"); 460 return (-1); 461 } 462 463 if ((cmsg = CMSG_FIRSTHDR(&msg)) != NULL 464 && cmsg->cmsg_level == SOL_SOCKET 465 && cmsg->cmsg_type == SCM_CREDS 466 && cmsg->cmsg_len >= sizeof(*cmsg) + sizeof(*cred) 467 && (cred = (struct sockcred *)CMSG_DATA(cmsg)) != NULL 468 && (cred->sc_uid == 0 || cred->sc_euid == 0)) 469 srv->fdidx[fd].priv = 1; 470 471 if (sizeof(*pdu) + (pdu->len = ntohs(pdu->len)) == len) { 472 switch (pdu->pid) { 473 case SDP_PDU_SERVICE_SEARCH_REQUEST: 474 error = server_prepare_service_search_response(srv, fd); 475 break; 476 477 case SDP_PDU_SERVICE_ATTRIBUTE_REQUEST: 478 error = server_prepare_service_attribute_response(srv, fd); 479 break; 480 481 case SDP_PDU_SERVICE_SEARCH_ATTRIBUTE_REQUEST: 482 error = server_prepare_service_search_attribute_response(srv, fd); 483 break; 484 485 case SDP_PDU_SERVICE_REGISTER_REQUEST: 486 error = server_prepare_service_register_response(srv, fd); 487 break; 488 489 case SDP_PDU_SERVICE_UNREGISTER_REQUEST: 490 error = server_prepare_service_unregister_response(srv, fd); 491 break; 492 493 case SDP_PDU_SERVICE_CHANGE_REQUEST: 494 error = server_prepare_service_change_response(srv, fd); 495 break; 496 497 default: 498 error = SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX; 499 break; 500 } 501 } else 502 error = SDP_ERROR_CODE_INVALID_PDU_SIZE; 503 504 if (error == 0) { 505 switch (pdu->pid) { 506 case SDP_PDU_SERVICE_SEARCH_REQUEST: 507 error = server_send_service_search_response(srv, fd); 508 break; 509 510 case SDP_PDU_SERVICE_ATTRIBUTE_REQUEST: 511 error = server_send_service_attribute_response(srv, fd); 512 break; 513 514 case SDP_PDU_SERVICE_SEARCH_ATTRIBUTE_REQUEST: 515 error = server_send_service_search_attribute_response(srv, fd); 516 break; 517 518 case SDP_PDU_SERVICE_REGISTER_REQUEST: 519 error = server_send_service_register_response(srv, fd); 520 break; 521 522 case SDP_PDU_SERVICE_UNREGISTER_REQUEST: 523 error = server_send_service_unregister_response(srv, fd); 524 break; 525 526 case SDP_PDU_SERVICE_CHANGE_REQUEST: 527 error = server_send_service_change_response(srv, fd); 528 break; 529 530 default: 531 error = SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX; 532 break; 533 } 534 535 if (error != 0) 536 log_err("Could not send SDP response to %s socket, " \ 537 "pdu->pid=%d, pdu->tid=%d, error=%d", 538 srv->fdidx[fd].control? "control" : "L2CAP", 539 pdu->pid, ntohs(pdu->tid), error); 540 } else { 541 log_err("Could not process SDP request from %s socket, " \ 542 "pdu->pid=%d, pdu->tid=%d, pdu->len=%d, len=%d, " \ 543 "error=%d", 544 srv->fdidx[fd].control? "control" : "L2CAP", 545 pdu->pid, ntohs(pdu->tid), pdu->len, len, error); 546 547 error = server_send_error_response(srv, fd, error); 548 if (error != 0) 549 log_err("Could not send SDP error response to %s " \ 550 "socket, pdu->pid=%d, pdu->tid=%d, error=%d", 551 srv->fdidx[fd].control? "control" : "L2CAP", 552 pdu->pid, ntohs(pdu->tid), error); 553 } 554 555 /* On error forget response (if any) */ 556 if (error != 0) { 557 srv->fdidx[fd].rsp_cs = 0; 558 srv->fdidx[fd].rsp_size = 0; 559 srv->fdidx[fd].rsp_limit = 0; 560 } 561 562 return (error); 563 } 564 565 /* 566 * Send SDP_Error_Response PDU 567 */ 568 569 static int32_t 570 server_send_error_response(server_p srv, int32_t fd, uint16_t error) 571 { 572 int32_t size; 573 574 struct { 575 sdp_pdu_t pdu; 576 uint16_t error; 577 } __attribute__ ((packed)) rsp; 578 579 /* Prepare and send SDP error response */ 580 rsp.pdu.pid = SDP_PDU_ERROR_RESPONSE; 581 rsp.pdu.tid = ((sdp_pdu_p)(srv->req))->tid; 582 rsp.pdu.len = htons(sizeof(rsp.error)); 583 rsp.error = htons(error); 584 585 do { 586 size = write(fd, &rsp, sizeof(rsp)); 587 } while (size < 0 && errno == EINTR); 588 589 return ((size < 0)? errno : 0); 590 } 591 592 /* 593 * Close descriptor and remove it from index 594 */ 595 596 static void 597 server_close_fd(server_p srv, int32_t fd) 598 { 599 provider_p provider = NULL, provider_next = NULL; 600 601 assert(FD_ISSET(fd, &srv->fdset)); 602 assert(srv->fdidx[fd].valid); 603 604 close(fd); 605 606 FD_CLR(fd, &srv->fdset); 607 if (fd == srv->maxfd) 608 srv->maxfd --; 609 610 if (srv->fdidx[fd].rsp != NULL) 611 free(srv->fdidx[fd].rsp); 612 613 memset(&srv->fdidx[fd], 0, sizeof(srv->fdidx[fd])); 614 615 for (provider = provider_get_first(); 616 provider != NULL; 617 provider = provider_next) { 618 provider_next = provider_get_next(provider); 619 620 if (provider->fd == fd) 621 provider_unregister(provider); 622 } 623 } 624