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/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