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