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