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