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