usr.sbin/bluetooth/sdpd/server.c

   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