usr.sbin/bluetooth/sdpd/server.c

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