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