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